walk stability

diff --git a/player_physics.h b/player_physics.h
index e08f6112281b5f0394ca10a2060da9d3b0a6095d..e49c4d83434edf3a512982014a9dc8410df6259e 100644 (file)
--- a/player_physics.h
+++ b/player_physics.h
@@ -577,7 +577,7 @@ VG_STATIC void player_physics_control_air(void)
 #endif
 }
 
-VG_STATIC void player_walk_update_collision(void)
+VG_STATIC void player_walk_collider_configuration(void)
 {
    struct player_phys *phys = &player.phys;
    float h0 = 0.3f,
@@ -597,6 +597,32 @@ VG_STATIC void player_walk_update_collision(void)
    rb_update_bounds( rbb );
 }
 
+VG_STATIC void player_regular_collider_configuration(void)
+{
+   struct player_phys *phys = &player.phys;
+   
+   /* Standard ground configuration */
+   rigidbody *rbf = &player.collide_front,
+             *rbb = &player.collide_back;
+
+   m3x3_copy( phys->rb.to_world, player.collide_front.to_world );
+   m3x3_copy( phys->rb.to_world, player.collide_back.to_world );
+   
+   player.air_blend = vg_lerpf( player.air_blend, phys->in_air, 0.1f );
+   float h = player.air_blend*0.0f;
+
+   m4x3_mulv( phys->rb.to_world, (v3f){0.0f,h,-k_board_length}, rbf->co );
+   v3_copy( rbf->co, rbf->to_world[3] );
+   m4x3_mulv( phys->rb.to_world, (v3f){0.0f,h, k_board_length}, rbb->co );
+   v3_copy( rbb->co, rbb->to_world[3] );
+
+   m4x3_invert_affine( rbf->to_world, rbf->to_local );
+   m4x3_invert_affine( rbb->to_world, rbb->to_local );
+
+   rb_update_bounds( rbf );
+   rb_update_bounds( rbb );
+}
+
 VG_STATIC void player_integrate(void);
 
 VG_STATIC int player_walk_surface_standable( v3f n )
@@ -632,10 +658,7 @@ VG_STATIC void player_walk_stepdown(void)
    }
 }
 
-/*
- * Entire Walking physics model
- * TODO: sleep when under certain velotiy
- */
+VG_STATIC int player_update_collision_manifold( rb_ct *manifold );
 VG_STATIC void player_walk_physics(void)
 {
    struct player_phys *phys = &player.phys;
@@ -665,9 +688,7 @@ VG_STATIC void player_walk_physics(void)
 
    if( phys->in_air )
    {
-      player_walk_update_collision();
-      rb_debug( rbf, 0xff0000ff );
-      rb_debug( rbb, 0xff0000ff );
+      player_walk_collider_configuration();
 
       /* allow player to accelerate a bit */
       v3f walk_3d;
@@ -683,9 +704,7 @@ VG_STATIC void player_walk_physics(void)
       v3_muladds( phys->rb.v, forward_dir, walk[1] * vel_diff, phys->rb.v );
 
 
-      len = 0;
-      len += rb_sphere_scene( rbf, &world.rb_geo, manifold+len );
-      len += rb_sphere_scene( rbb, &world.rb_geo, manifold+len );
+      len = player_update_collision_manifold( manifold );
       rb_presolve_contacts( manifold, len );
 
       for( int i=0; i<len; i++ )
@@ -741,39 +760,37 @@ VG_STATIC void player_walk_physics(void)
 
       v2_muls( walk, k_walkspeed * VG_TIMESTEP_FIXED, walk );
 
-      v3f walk_apply;
-      v3_zero( walk_apply );
-
       /* Do XY translation */
+      v3f walk_apply, walk_measured;
+      v3_zero( walk_apply );
       v3_muladds( walk_apply, right_dir,   walk[0], walk_apply );
       v3_muladds( walk_apply, forward_dir, walk[1], walk_apply );
       v3_add( walk_apply, phys->rb.co, phys->rb.co );
-      v3_divs( walk_apply, VG_TIMESTEP_FIXED, phys->rb.v );
 
       /* Directly resolve collisions */
-      player_walk_update_collision();
-      rb_debug( rbf, 0xffffff00 );
-      rb_debug( rbb, 0xffffff00 );
-
-      len = 0;
-      len += rb_sphere_scene( rbf, &world.rb_geo, manifold+len );
-      len += rb_sphere_scene( rbb, &world.rb_geo, manifold+len );
+      player_walk_collider_configuration();
+      len = player_update_collision_manifold( manifold );
 
       v3f dt;
       v3_zero( dt );
-      for( int j=0; j<3; j++ )
+      for( int j=0; j<8; j++ )
       {
          for( int i=0; i<len; i++ )
          {
             struct contact *ct = &manifold[i];
 
-            float p   = vg_maxf( 0.0f, ct->p - 0.00f ),
-                  cur = vg_clampf( v3_dot( ct->n, dt ), 0.0f, p );
-            v3_muladds( dt, ct->n, (p - cur) * 0.333333333f, dt );
+            float resolved_amt = v3_dot( ct->n, dt ),
+                  remaining    = (ct->p-k_penetration_slop) - resolved_amt,
+                  apply        = vg_maxf( remaining, 0.0f ) * 0.3f;
+
+            v3_muladds( dt, ct->n, apply, dt );
          }
       }
       v3_add( dt, phys->rb.co, phys->rb.co );
 
+      v3_add( dt, walk_apply, walk_measured );
+      v3_divs( walk_measured, VG_TIMESTEP_FIXED, phys->rb.v );
+
       if( len )
       {
          struct world_material *surface_mat = world_contact_material(manifold);
@@ -800,58 +817,6 @@ VG_STATIC void player_walk_physics(void)
       /* otherwise... */
       if( phys->in_air )
          player_walk_stepdown();
-      
-#if 0
-      /* if we've put us in the air, step down slowly */
-      phys->in_air = 1;
-      float max_dist = 0.3f,
-            start_y = phys->rb.co[1];
-
-      v3f pa, pb;
-      v3_copy( phys->rb.co, pa );
-      v3_muladds( pa, (v3f){0.0f,1.0f,0.0f}, -max_dist, pb );
-
-
-      for( int j=0; j<8; j++ )
-      {
-         for( int i=0; i<len; i++ )
-         {
-            struct contact *ct = &manifold[i];
-            if( v3_dot( ct->n, (v3f){0.0f,1.0f,0.0f} ) > 0.5f )
-            {
-               phys->in_air = 0;
-               if( j == 0 )
-                  return;
-
-               v3f dt;
-               v3_zero( dt );
-               for( int j=0; j<3; j++ )
-               {
-                  for( int i=0; i<len; i++ )
-                  {
-                     struct contact *ct = &manifold[i];
-
-                     float p   = vg_maxf( 0.0f, ct->p - 0.0025f ),
-                           cur = vg_clampf( v3_dot( ct->n, dt ), 0.0f, p );
-                     v3_muladds( dt, ct->n, (p - cur) * 0.333333333f, dt );
-                  }
-               }
-               v3_add( dt, phys->rb.co, phys->rb.co );
-               return;
-            }
-         }
-
-         phys->rb.co[1] -= max_dist * 0.125f;
-
-         player_walk_update_collision();
-         len = 0;
-         len += rb_sphere_scene( rbf, &world.rb_geo, manifold+len );
-         len += rb_sphere_scene( rbb, &world.rb_geo, manifold+len );
-      }
-      
-      /* Transitioning into air mode */
-      phys->rb.co[1] = start_y;
-#endif
    }
 }
 
@@ -939,32 +904,18 @@ VG_STATIC int player_update_collision_manifold( rb_ct *manifold )
 {
    struct player_phys *phys = &player.phys;
 
-   phys->rise = vg_lerpf( phys->rise, phys->in_air? -0.25f: 0.0f, 
-                          VG_TIMESTEP_FIXED );
-
    rigidbody *rbf = &player.collide_front,
              *rbb = &player.collide_back;
 
-   m3x3_copy( phys->rb.to_world, player.collide_front.to_world );
-   m3x3_copy( phys->rb.to_world, player.collide_back.to_world );
-   
-   player.air_blend = vg_lerpf( player.air_blend, phys->in_air, 0.1f );
-   float h = player.air_blend*0.0f;
-
-   m4x3_mulv( phys->rb.to_world, (v3f){0.0f,h,-k_board_length}, rbf->co );
-   v3_copy( rbf->co, rbf->to_world[3] );
-   m4x3_mulv( phys->rb.to_world, (v3f){0.0f,h, k_board_length}, rbb->co );
-   v3_copy( rbb->co, rbb->to_world[3] );
-
-   m4x3_invert_affine( rbf->to_world, rbf->to_local );
-   m4x3_invert_affine( rbb->to_world, rbb->to_local );
-
-   rb_update_bounds( rbf );
-   rb_update_bounds( rbb );
-
    rb_debug( rbf, 0xff00ffff );
    rb_debug( rbb, 0xffffff00 );
 
+
+#if 0
+   phys->rise = vg_lerpf( phys->rise, phys->in_air? -0.25f: 0.0f, 
+                          VG_TIMESTEP_FIXED );
+#endif
+
    int len_f = 0,
        len_b = 0; 
 
@@ -996,28 +947,6 @@ VG_STATIC int player_update_collision_manifold( rb_ct *manifold )
       len_b = 1;
    else
       len_b = new_len_b;
-#if 0
-   /* 
-    * Preprocess collision points, and create a surface picture.
-    * we want contacts that are within our 'capsule's internal line to be 
-    * clamped so that they face the line and do not oppose, to stop the
-    * player hanging up on stuff
-    */
-   for( int i=0; i<len; i++ )
-   {
-      v3f dfront, dback;
-      v3_sub( manifold[i].co, rbf->co, dfront );
-      v3_sub( manifold[i].co, rbb->co, dback );
-
-      if( (v3_dot( dfront, phys->rb.forward ) < -0.02f) &&
-          (v3_dot( dback,  phys->rb.forward ) > 0.02f))
-      {
-         float p = v3_dot( manifold[i].n, phys->rb.forward );
-         v3_muladds( manifold[i].n, phys->rb.forward, -p, manifold[i].n );
-         v3_normalize( manifold[i].n );
-      }
-   }
-#endif
 
    return len_f + len_b;
 }
@@ -1181,6 +1110,8 @@ VG_STATIC void player_do_motion(void)
    if( phys->on_board )
    {
       rb_ct manifold[72];
+      
+      player_regular_collider_configuration();
       int len = player_update_collision_manifold( manifold );
       int grind_col = player_update_grind_collision( &manifold[len] );
 

diff --git a/player_ragdoll.h b/player_ragdoll.h
index 0e48f0ce060b8d7ce06be14596ce61ffa4c57cec..0d6d74d3f7cba96b03b2b23700585e97a1b9ba2c 100644 (file)
--- a/player_ragdoll.h
+++ b/player_ragdoll.h
@@ -276,6 +276,19 @@ VG_STATIC void player_ragdoll_iter(void)
       }
    }
 
+   for( int j=0; j<mdl->ragdoll_count; j++ )
+   {
+      struct ragdoll_part *pj = &mdl->ragdoll[j];
+      struct skeleton_bone *bj = &mdl->sk.bones[pj->bone_id];
+
+      if( run_sim )
+      {
+         v4f plane = {0.0f,1.0f,0.0f,0.0f};
+         rb_effect_simple_bouyency( &pj->rb, plane, k_ragdoll_floatyiness,
+                                                    k_ragdoll_floatydrag );
+      }
+   }
+
    /*
     * PRESOLVE
     */
@@ -301,21 +314,6 @@ VG_STATIC void player_ragdoll_iter(void)
                                        mdl->cone_constraints_count );
    }
 
-#if 0
-   for( int j=0; j<mdl->ragdoll_count; j++ )
-   {
-      struct ragdoll_part *pj = &mdl->ragdoll[j];
-      struct skeleton_bone *bj = &mdl->sk.bones[pj->bone_id];
-
-      if( run_sim )
-      {
-         v4f plane = {0.0f,1.0f,0.0f,0.0f};
-         rb_effect_simple_bouyency( &pj->rb, plane, k_ragdoll_floatyiness,
-                                                    k_ragdoll_floatydrag );
-      }
-   }
-#endif
-
    /*
     * SOLVE CONSTRAINTS 
     */

diff --git a/rigidbody.h b/rigidbody.h
index e8dae45227d1045b91a755b49214dc0dac170f27..d39cf1b6012926941f63860c8e441927f160bc4b 100644 (file)
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -2350,6 +2350,21 @@ VG_STATIC void rb_correct_swingtwist_constraints( rb_constr_swingtwist *buf,
    }
 }
 
+VG_STATIC void rb_correct_contact_constraints( rb_ct *buf, int len, float amt )
+{
+   for( int i=0; i<len; i++ )
+   {
+      rb_ct *ct = &buf[i];
+      rigidbody *rba = ct->rba,
+                *rbb = ct->rbb;
+
+      float mass_total = 1.0f / (rba->inv_mass + rbb->inv_mass);
+
+      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 );
+   }
+}
+
 
 /* 
  * Effectors