ragdoll quality improvements

[carveJwlIkooP6JGAAIwe30JlM.git] / rigidbody.h

diff --git a/rigidbody.h b/rigidbody.h
index 7d878e9a4976146a08cf09cb9cc2a5519e978408..2c074c47d46aea97d7b56b67d8aa70191f7c198b 100644 (file)
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -1076,6 +1076,48 @@ static int rb_capsule__triangle( m4x3f mtxA, rb_capsule *c,
 
    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 );
@@ -1090,12 +1132,13 @@ static int rb_capsule__triangle( m4x3f mtxA, rb_capsule *c,
    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;
@@ -1106,20 +1149,6 @@ static int rb_capsule__triangle( m4x3f mtxA, rb_capsule *c,
       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 );
@@ -1791,10 +1820,11 @@ static void rb_correct_contact_constraints( rb_ct *buf, int len, float amt ){
       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 );
    }
 }