From: hgn <hgodden00@gmail.com>
Date: Mon, 22 Jan 2024 17:52:08 +0000 (+0000)
Subject: rigidbody math corrections & ragdoll tweaks for stability
X-Git-Url: https://harrygodden.com/git/?p=carveJwlIkooP6JGAAIwe30JlM.git;a=commitdiff_plain;h=f0ba02684dbf4722afa2dcfd491d56921162182a

rigidbody math corrections & ragdoll tweaks for stability
---

diff --git a/maps_src/dev_flatworld/main.mdl b/maps_src/dev_flatworld/main.mdl
index 10619c5..f9c326c 100644
Binary files a/maps_src/dev_flatworld/main.mdl and b/maps_src/dev_flatworld/main.mdl differ
diff --git a/models_src/ch_none.mdl b/models_src/ch_none.mdl
index 5dacbe1..82927ff 100644
Binary files a/models_src/ch_none.mdl and b/models_src/ch_none.mdl differ
diff --git a/player_glide.c b/player_glide.c
index 80cb2bb..3ec1fac 100644
--- a/player_glide.c
+++ b/player_glide.c
@@ -91,7 +91,6 @@ static void player_glide_update(void){
 
    v3f balance = {0.0f,-k_glide_balance,0.0f};
    m3x3_mulv( rb->to_local, balance, balance );
-   vg_info( PRINTF_v3f( balance ) );
 
    v3f Fw = {
        steer[1]*k_glide_steer - balance[2],
diff --git a/player_ragdoll.c b/player_ragdoll.c
index e334580..e1231bc 100644
--- a/player_ragdoll.c
+++ b/player_ragdoll.c
@@ -116,7 +116,7 @@ static void player_init_ragdoll_bone_collider( struct skeleton_bone *bone,
    v3_add( rp->collider_mtx[3], bone->co, rp->obj.rb.co );
    v3_zero( rp->obj.rb.v );
    v3_zero( rp->obj.rb.w );
-   rb_init_object( &rp->obj );
+   rb_init_object( &rp->obj, 4.0f );
 }
 
 /*
@@ -453,14 +453,26 @@ static void player_ragdoll_iter( struct player_ragdoll *rd ){
     * SOLVE CONSTRAINTS  & Integrate
     */
    if( run_sim ){
-      for( int i=0; i<12; i++ ){
-         rb_solve_contacts( rb_contact_buffer, rb_contact_count );
-         rb_solve_swingtwist_constraints( rd->cone_constraints, 
-                                          rd->cone_constraints_count );
+      /* the solver is not very quickly converging so... */
+      for( int i=0; i<40; i++ ){
+         if( i<20 ){
+            rb_solve_contacts( rb_contact_buffer, rb_contact_count );
+            rb_solve_swingtwist_constraints( rd->cone_constraints, 
+                                             rd->cone_constraints_count );
+            rb_postsolve_swingtwist_constraints( rd->cone_constraints, 
+                                                 rd->cone_constraints_count );
+         }
          rb_solve_position_constraints( rd->position_constraints, 
                                         rd->position_constraints_count );
       }
 
+      rb_correct_position_constraints( rd->position_constraints,
+                                       rd->position_constraints_count, 
+                                       k_ragdoll_correction * 0.5f );
+      rb_correct_swingtwist_constraints( rd->cone_constraints, 
+                                         rd->cone_constraints_count,
+                                         k_ragdoll_correction * 0.25f );
+
       for( int i=0; i<rd->part_count; i++ ){
          rb_iter( &rd->parts[i].obj.rb );
 
@@ -475,16 +487,6 @@ static void player_ragdoll_iter( struct player_ragdoll *rd ){
 
       for( int i=0; i<rd->part_count; i++ )
          rb_update_transform( &rd->parts[i].obj.rb );
-
-      for( int i=0; i<5; i ++ ){
-         rb_correct_swingtwist_constraints( rd->cone_constraints, 
-                                            rd->cone_constraints_count,
-                                            k_ragdoll_correction * 0.25f );
-
-         rb_correct_position_constraints( rd->position_constraints,
-                                          rd->position_constraints_count, 
-                                          k_ragdoll_correction * 0.5f );
-      }
    }
 
    rb_ct *stress = NULL;
diff --git a/rigidbody.h b/rigidbody.h
index a69e4af..992bdba 100644
--- 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
       }
    }
 }
diff --git a/skaterift.c b/skaterift.c
index be4084e..a1a6157 100644
--- a/skaterift.c
+++ b/skaterift.c
@@ -57,6 +57,7 @@
 #include "particle.c"
 #include "player_effects.c"
 #include "freecam.c"
+#include "testing.c"
 
 static int k_tools_mode = 0;
 
@@ -230,6 +231,7 @@ static void vg_load(void){
    vg_loader_step( workshop_init, NULL );
    vg_loader_step( skateshop_init, NULL );
    vg_loader_step( ent_tornado_init, NULL );
+   vg_loader_step( testing_init, NULL );
   
    vg_loader_step( skaterift_load_player_content, NULL );
 
@@ -325,6 +327,7 @@ static void vg_fixed_update(void){
    world_routes_fixedupdate( world_current_instance() );
    player__update();
    vehicle_update_fixed();
+   testing_update();
 }
 
 static void vg_post_update(void){
diff --git a/testing.c b/testing.c
new file mode 100644
index 0000000..b5b4cec
--- /dev/null
+++ b/testing.c
@@ -0,0 +1,43 @@
+#include "rigidbody.h"
+
+static rb_object baller = {
+   .type = k_rb_shape_box,
+   .rb.bbx = {{ -0.1f, -0.2f, -0.1f },
+              {  0.1f,  1.0f,  0.1f }},
+   .rb.q = { 0,0,0,1 },
+};
+
+static void testing_update(void){
+   if( vg_getkey( SDLK_9 ) ){
+      v3_add( localplayer.rb.co, (v3f){0,1,0}, baller.rb.co );
+      v3_zero( baller.rb.w );
+      v3_zero( baller.rb.v );
+      q_identity( baller.rb.q );
+      rb_update_transform( &baller.rb );
+   }
+   rb_object_debug( &baller, VG__RED );
+
+   world_instance *world = world_current_instance();
+
+   rb_solver_reset();
+   rb_ct *buf = rb_global_buffer();
+   rb_contact_count += rb_box__scene( baller.rb.to_world, baller.rb.bbx,
+                          NULL, &world->rb_geo.inf.scene, buf,
+                          k_material_flag_ghosts );
+   for( u32 j=0; j<rb_contact_count; j++ ){
+      buf[j].rba = &baller.rb;
+      buf[j].rbb = &world->rb_geo.rb;
+   }
+
+   rb_presolve_contacts( rb_contact_buffer, rb_contact_count );
+
+   for( u32 i=0; i<8; i ++ )
+      rb_solve_contacts( rb_contact_buffer, rb_contact_count );
+
+   rb_iter( &baller.rb );
+   rb_update_transform( &baller.rb );
+}
+
+static void testing_init(void){
+   rb_init_object( &baller, 1.0f );
+}
diff --git a/vehicle.c b/vehicle.c
index 3d68d11..b367a1c 100644
--- a/vehicle.c
+++ b/vehicle.c
@@ -36,7 +36,7 @@ static void vehicle_init(void)
    v3_zero( gzoomer.obj.rb.w );
    v3_zero( gzoomer.obj.rb.v );
    v3_zero( gzoomer.obj.rb.co );
-   rb_init_object( &gzoomer.obj );
+   rb_init_object( &gzoomer.obj, 1.0f );
 
    VG_VAR_F32( k_car_spring,        flags=VG_VAR_PERSISTENT );
    VG_VAR_F32( k_car_spring_damp,   flags=VG_VAR_PERSISTENT );
diff --git a/world_gen.c b/world_gen.c
index e2ec14a..b86c5cf 100644
--- a/world_gen.c
+++ b/world_gen.c
@@ -277,7 +277,7 @@ static void world_gen_generate_meshes( world_instance *world ){
 
    world->rb_geo.type = k_rb_shape_scene;
    world->rb_geo.inf.scene.bh_scene = world->geo_bh;
-   rb_init_object( &world->rb_geo );
+   rb_init_object( &world->rb_geo, 0.0f );
 
    /*
     * Generate scene: non-collidable geometry
diff --git a/world_routes.c b/world_routes.c
index b80e68e..daba459 100644
--- a/world_routes.c
+++ b/world_routes.c
@@ -897,7 +897,7 @@ static void world_routes_fracture( world_instance *world, ent_gate *gate,
             particle->obj.type = k_rb_shape_sphere;
             particle->obj.inf.sphere.radius = r*0.6f;
 
-            rb_init_object( &particle->obj );
+            rb_init_object( &particle->obj, 1.0f );
          }
          offset[0] += glyph->size[0];
       }