rigidbody math corrections & ragdoll tweaks for stability

[carveJwlIkooP6JGAAIwe30JlM.git] / rigidbody.h

diff --git a/rigidbody.h b/rigidbody.h
index a69e4afd69cb5c2b41089b65b3dbdd31f248bc70..992bdba981ffcc6b9b5e875f095383c9ef351dae 100644 (file)
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -31,7 +31,7 @@ static const float
    k_damp_linear      = 0.1f,               /* scale velocity 1/(1+x) */
    k_damp_angular     = 0.1f,                /* scale angular  1/(1+x) */
    k_penetration_slop = 0.01f,
-   k_inertia_scale    = 8.0f,
+   k_inertia_scale    = 4.0f,
    k_phys_baumgarte   = 0.2f,
    k_gravity          = 9.6f;
 
@@ -81,7 +81,6 @@ struct rigidbody{
    float inv_mass;
 
    /* inertia model and inverse world tensor */
-   v3f I;
    m3x3f iI, iIw;
    m4x3f to_world, to_local;
 };
@@ -139,6 +138,8 @@ struct rb_constr_swingtwist{
 
    float conet;
    float tangent_mass, axis_mass;
+
+   f32 conv_tangent, conv_axis;
 };
 
 /*
@@ -206,8 +207,10 @@ static void rb_update_transform( rigidbody *rb )
    v3_copy( rb->co, rb->to_world[3] );
 
    m4x3_invert_affine( rb->to_world, rb->to_local );
-   m3x3_mul( rb->iI, rb->to_local, rb->iIw );
-   m3x3_mul( rb->to_world, rb->iIw, rb->iIw );
+
+   /* I = R I_0 R^T */
+   m3x3_mul( rb->to_world, rb->iI, rb->iIw );
+   m3x3_mul( rb->iIw, rb->to_local, rb->iIw );
 
    rb_update_bounds( rb );
 }
@@ -240,7 +243,7 @@ static void rb_extrapolate( rigidbody *rb, v3f co, v4f q )
 /*
  * Initialize rigidbody and calculate masses, inertia
  */
-static void rb_init_object( rb_object *obj ){
+static void rb_init_object( rb_object *obj, f32 inertia_scale ){
    float volume = 1.0f;
    int inert = 0;
 
@@ -271,32 +274,62 @@ static void rb_init_object( rb_object *obj ){
 
    if( inert ){
       obj->rb.inv_mass = 0.0f;
-      v3_zero( obj->rb.I );
       m3x3_zero( obj->rb.iI );
    }
    else{
-      float mass = 2.0f*volume;
+      f32 mass = 8.0f*volume;
       obj->rb.inv_mass = 1.0f/mass;
 
-      v3f extent;
+      v3f extent, com;
       v3_sub( obj->rb.bbx[1], obj->rb.bbx[0], extent );
-      v3_muls( extent, 0.5f, extent );
+      v3_muladds( obj->rb.bbx[0], extent, 0.5f, com );
 
       /* local intertia tensor */
-      float scale = k_inertia_scale;
-      float ex2 = scale*extent[0]*extent[0],
-            ey2 = scale*extent[1]*extent[1],
-            ez2 = scale*extent[2]*extent[2];
-
-      obj->rb.I[0] = ((1.0f/12.0f) * mass * (ey2+ez2));
-      obj->rb.I[1] = ((1.0f/12.0f) * mass * (ex2+ez2));
-      obj->rb.I[2] = ((1.0f/12.0f) * mass * (ex2+ey2));
-
-      m3x3_identity( obj->rb.iI );
-      obj->rb.iI[0][0] = obj->rb.I[0];
-      obj->rb.iI[1][1] = obj->rb.I[1];
-      obj->rb.iI[2][2] = obj->rb.I[2];
-      m3x3_inv( obj->rb.iI, obj->rb.iI );
+      f32 ex2 = extent[0]*extent[0],
+          ey2 = extent[1]*extent[1],
+          ez2 = extent[2]*extent[2];
+
+      /* compute inertia tensor */
+      v3f I;
+
+      if( obj->type == k_rb_shape_box ){
+         I[0] = inertia_scale * (ey2+ez2) * mass * (1.0f/12.0f);
+         I[1] = inertia_scale * (ex2+ez2) * mass * (1.0f/12.0f);
+         I[2] = inertia_scale * (ex2+ey2) * mass * (1.0f/12.0f);
+      }
+      else if( obj->type == k_rb_shape_sphere ){
+         f32 r = obj->inf.sphere.radius;
+         v3_fill( I, inertia_scale * r*r * mass * (2.0f/5.0f) );
+      }
+      else if( obj->type == k_rb_shape_capsule ){
+         f32 r = obj->inf.capsule.radius;
+         I[1] = inertia_scale * r*r * mass * (2.0f/5.0f);
+         I[0] = inertia_scale * (ey2+ez2) * mass * (1.0f/12.0f);
+         I[2] = inertia_scale * (ey2+ex2) * mass * (1.0f/12.0f);
+      }
+      else {
+         vg_fatal_error( "" );
+      }
+
+      m3x3f i;
+      m3x3_identity( i );
+      m3x3_setdiagonalv3( i, I );
+
+      /* compute translation */
+      m3x3f i_t, i_t_outer, i_t_scale;
+      m3x3_diagonal( i_t, v3_dot(com,com) );
+      m3x3_outer_product( i_t_outer, com, com );
+      m3x3_sub( i_t, i_t_outer, i_t );
+      m3x3_diagonal( i_t_scale, mass );
+      m3x3_mul( i_t_scale, i_t, i_t );
+
+      /* TODO: compute rotation */
+
+      /* add Ic and Ict */
+      m3x3_add( i, i_t, i );
+
+      /* store as inverted */
+      m3x3_inv( i, obj->rb.iI );
    }
 
    rb_update_transform( &obj->rb );
@@ -1221,7 +1254,7 @@ static rb_ct *rb_global_ct(void){
 }
 
 static void rb_prepare_contact( rb_ct *ct, float timestep ){
-   ct->bias = -0.2f * (timestep*3600.0f) 
+   ct->bias = -k_phys_baumgarte * (timestep*3600.0f) 
                * vg_minf( 0.0f, -ct->p+k_penetration_slop );
    
    v3_tangent_basis( ct->n, ct->t[0], ct->t[1] );
@@ -1405,8 +1438,6 @@ static void rb_debug_position_constraints( rb_constr_pos *buffer, int len ){
 
 static void rb_presolve_swingtwist_constraints( rb_constr_swingtwist *buf,
                                                    int len ){
-   float size = 0.12f;
-
    for( int i=0; i<len; i++ ){
       rb_constr_swingtwist *st = &buf[ i ];
       
@@ -1656,8 +1687,6 @@ static void rb_solve_position_constraints( rb_constr_pos *buf, int len ){
 
 static void rb_solve_swingtwist_constraints( rb_constr_swingtwist *buf, 
                                                 int len ){
-   float size = 0.12f;
-
    for( int i=0; i<len; i++ ){
       rb_constr_swingtwist *st = &buf[ i ];
 
@@ -1709,6 +1738,44 @@ static void rb_solve_swingtwist_constraints( rb_constr_swingtwist *buf,
    }
 }
 
+/* debugging */
+static void rb_postsolve_swingtwist_constraints( rb_constr_swingtwist *buf, 
+                                                 u32 len ){
+   for( int i=0; i<len; i++ ){
+      rb_constr_swingtwist *st = &buf[ i ];
+
+      if( !st->axis_violation ){
+         st->conv_axis = 0.0f;
+         continue;
+      }
+
+      f32 rv = v3_dot( st->axis, st->rbb->w ) - 
+               v3_dot( st->axis, st->rba->w );
+
+      if( rv * (f32)st->axis_violation > 0.0f )
+         st->conv_axis = 0.0f;
+      else
+         st->conv_axis = rv;
+   }
+
+   for( int i=0; i<len; i++ ){
+      rb_constr_swingtwist *st = &buf[ i ];
+
+      if( !st->tangent_violation ){
+         st->conv_tangent = 0.0f;
+         continue;
+      }
+
+      f32 rv = v3_dot( st->tangent_axis, st->rbb->w ) - 
+               v3_dot( st->tangent_axis, st->rba->w );
+
+      if( rv > 0.0f )
+         st->conv_tangent = 0.0f;
+      else
+         st->conv_tangent = rv;
+   }
+}
+
 static void rb_solve_constr_angle( rigidbody *rba, rigidbody *rbb,
                                       v3f ra, v3f rb ){
    m3x3f ssra, ssrb, ssrat, ssrbt;
@@ -1764,8 +1831,16 @@ static void rb_correct_position_constraints( rb_constr_pos *buf, int len,
       v3_add( rbb->co, p1, p1 );
       v3_sub( p1, p0, d );
 
+#if 1
       v3_muladds( rbb->co, d, -1.0f * amt, rbb->co );
       rb_update_transform( rbb );
+#else
+      f32 mt = 1.0f/(rba->inv_mass+rbb->inv_mass),
+          a  = mt * (k_phys_baumgarte/k_rb_delta);
+
+      v3_muladds( rba->v, d, a* rba->inv_mass, rba->v );
+      v3_muladds( rbb->v, d, a*-rbb->inv_mass, rbb->v );
+#endif
    }
 }
 
@@ -1780,16 +1855,23 @@ static void rb_correct_swingtwist_constraints( rb_constr_swingtwist *buf,
       v3f va;
       m3x3_mulv( st->rbb->to_world, st->coneva, va );
 
-      float angle = v3_dot( va, st->tangent_target );
+      f32 angle = v3_dot( va, st->tangent_target );
 
       if( fabsf(angle) < 0.9999f ){
          v3f axis;
          v3_cross( va, st->tangent_target, axis );
-
+#if 1
+         angle = acosf(angle) * amt;
          v4f correction;
-         q_axis_angle( correction, axis, acosf(angle) * amt );
+         q_axis_angle( correction, axis, angle );
          q_mul( correction, st->rbb->q, st->rbb->q );
          rb_update_transform( st->rbb );
+#else
+         f32 mt = 1.0f/(st->rba->inv_mass+st->rbb->inv_mass),
+             wa = mt * acosf(angle) * (k_phys_baumgarte/k_rb_delta);
+         //v3_muladds( st->rba->w, axis, wa*-st->rba->inv_mass, st->rba->w );
+         v3_muladds( st->rbb->w, axis, wa* st->rbb->inv_mass, st->rbb->w );
+#endif
       }
    }
 
@@ -1802,16 +1884,24 @@ static void rb_correct_swingtwist_constraints( rb_constr_swingtwist *buf,
       v3f vxb;
       m3x3_mulv( st->rbb->to_world, st->conevxb, vxb );
 
-      float angle = v3_dot( vxb, st->axis_target );
+      f32 angle = v3_dot( vxb, st->axis_target );
 
       if( fabsf(angle) < 0.9999f ){
          v3f axis;
          v3_cross( vxb, st->axis_target, axis );
 
+#if 1
+         angle = acosf(angle) * amt;
          v4f correction;
-         q_axis_angle( correction, axis, acosf(angle) * amt );
+         q_axis_angle( correction, axis, angle );
          q_mul( correction, st->rbb->q, st->rbb->q );
          rb_update_transform( st->rbb );
+#else
+         f32 mt = 1.0f/(st->rba->inv_mass+st->rbb->inv_mass),
+             wa = mt * acosf(angle) * (k_phys_baumgarte/k_rb_delta);
+         //v3_muladds( st->rba->w, axis, wa*-0.5f, st->rba->w );
+         v3_muladds( st->rbb->w, axis, wa* st->rbb->inv_mass, st->rbb->w );
+#endif
       }
    }
 }