q_mul( rotation, rb->q, rb->q );
}
+#if 0
/* damping */
v3_muls( rb->v, 1.0f/(1.0f+k_rb_delta*k_damp_linear), rb->v );
v3_muls( rb->w, 1.0f/(1.0f+k_rb_delta*k_damp_angular), rb->w );
+#endif
}
capsule_manifold manifold;
rb_capsule_manifold_init( &manifold );
+
+ v3f v0, v1, n;
+ v3_sub( tri[1], tri[0], v0 );
+ v3_sub( tri[2], tri[0], v1 );
+ v3_cross( v0, v1, n );
+
+ if( v3_length2( n ) <= 0.00001f ){
+#ifdef RIGIDBODY_CRY_ABOUT_EVERYTHING
+ vg_error( "Zero area triangle!\n" );
+#endif
+ return 0;
+ }
+
+ v3_normalize( n );
+
+#if 1
+ /* deep penetration recovery. for when we clip through the triangles. so its
+ * not very 'correct' */
+ f32 dist;
+ if( ray_tri( tri, p0w, mtxA[1], &dist, 1 ) ){
+ f32 l = c->height - c->radius*2.0f;
+ if( (dist >= 0.0f) && (dist < l) ){
+ v3f co;
+ v3_muladds( p0w, mtxA[1], dist, co );
+ vg_line_point( co, 0.02f, 0xffffff00 );
+
+ v3f d0, d1;
+ v3_sub( p0w, co, d0 );
+ v3_sub( p1w, co, d1 );
+
+ f32 p = vg_minf( v3_dot( n, d0 ), v3_dot( n, d1 ) ) - c->radius;
+
+ rb_ct *ct = buf;
+ ct->p = -p;
+ ct->type = k_contact_type_default;
+ v3_copy( n, ct->n );
+ v3_muladds( co, n, p, ct->co );
+
+ return 1;
+ }
+ }
+#endif
v3f c0, c1;
closest_on_triangle_1( p0w, tri, c0 );
v3_normalize(d1);
v3_normalize(da);
+ /* the two balls at the ends */
if( v3_dot( da, d0 ) <= 0.01f )
rb_capsule_manifold( p0w, c0, 0.0f, c->radius, &manifold );
-
if( v3_dot( da, d1 ) >= -0.01f )
rb_capsule_manifold( p1w, c1, 1.0f, c->radius, &manifold );
+ /* the edges to edges */
for( int i=0; i<3; i++ ){
int i0 = i,
i1 = (i+1)%3;
rb_capsule_manifold( ca, cb, ta, c->radius, &manifold );
}
- v3f v0, v1, n;
- v3_sub( tri[1], tri[0], v0 );
- v3_sub( tri[2], tri[0], v1 );
- v3_cross( v0, v1, n );
-
- if( v3_length2( n ) <= 0.00001f ){
-#ifdef RIGIDBODY_CRY_ABOUT_EVERYTHING
- vg_error( "Zero area triangle!\n" );
-#endif
- return 0;
- }
-
- v3_normalize( n );
-
int count = rb_capsule__manifold_done( mtxA, c, &manifold, buf );
for( int i=0; i<count; i++ )
v3_copy( n, buf[i].n );
rigidbody *rba = ct->rba,
*rbb = ct->rbb;
- float mass_total = 1.0f / (rba->inv_mass + rbb->inv_mass);
+ f32 mass_total = 1.0f / (rba->inv_mass + rbb->inv_mass),
+ d = ct->p*mass_total*amt;
- v3_muladds( rba->co, ct->n, -mass_total * rba->inv_mass, rba->co );
- v3_muladds( rbb->co, ct->n, mass_total * rbb->inv_mass, rbb->co );
+ v3_muladds( rba->co, ct->n, -d * rba->inv_mass, rba->co );
+ v3_muladds( rbb->co, ct->n, d * rbb->inv_mass, rbb->co );
}
}