X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=rigidbody.h;h=a69e4afd69cb5c2b41089b65b3dbdd31f248bc70;hb=78cc452a8343821ba47c0905d755657847dafd25;hp=7d878e9a4976146a08cf09cb9cc2a5519e978408;hpb=22f62f001f21d1b91fefd9fc495c122d9ddf205a;p=carveJwlIkooP6JGAAIwe30JlM.git

diff --git a/rigidbody.h b/rigidbody.h
index 7d878e9..a69e4af 100644
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -330,9 +330,11 @@ static void rb_iter( rigidbody *rb ){
       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
 }
 
 
@@ -1076,6 +1078,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 +1134,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 +1151,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 +1822,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 );
    }
 }