X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=player_ragdoll.h;h=0e48f0ce060b8d7ce06be14596ce61ffa4c57cec;hb=2a238d32da833812e837cf38e16a7685c98db5c3;hp=be7506407561c4573469ef3afa196bc908abf8aa;hpb=b9dedb4dd2a1e94ae76a3986716ee3c57e568213;p=carveJwlIkooP6JGAAIwe30JlM.git

diff --git a/player_ragdoll.h b/player_ragdoll.h
index be75064..0e48f0c 100644
--- a/player_ragdoll.h
+++ b/player_ragdoll.h
@@ -3,15 +3,103 @@
 
 #include "player.h"
 
-static float k_ragdoll_floatyiness = 40.0f,
-             k_ragdoll_floatydrag  = 1.0f;
+VG_STATIC float k_ragdoll_floatyiness = 20.0f,
+                k_ragdoll_floatydrag  = 1.0f,
+                k_ragdoll_limit_scale = 1.0f;
+
+VG_STATIC int   k_ragdoll_div = 1,
+                ragdoll_frame = 0,
+                k_ragdoll_debug_collider = 1,
+                k_ragdoll_debug_constraints = 0;
+
+VG_STATIC void player_init_ragdoll_bone_collider( struct skeleton_bone *bone,
+                                                  struct ragdoll_part *rp )
+{
+   m4x3_identity( rp->collider_mtx );
+
+   if( bone->flags & k_bone_flag_collider_box )
+   {
+      v3f delta;
+      v3_sub( bone->hitbox[1], bone->hitbox[0], delta );
+      v3_muls( delta, 0.5f, delta );
+      v3_add( bone->hitbox[0], delta, rp->collider_mtx[3] );
+
+      v3_copy( delta, rp->rb.bbx[1] );
+      v3_muls( delta, -1.0f, rp->rb.bbx[0] );
+
+      q_identity( rp->rb.q );
+      rp->rb.type = k_rb_shape_box;
+      rp->colour = 0xffcccccc;
+   }
+   else if( bone->flags & k_bone_flag_collider_capsule )
+   {
+      v3f v0, v1, tx, ty;
+      v3_sub( bone->hitbox[1], bone->hitbox[0], v0 );
+
+      int major_axis = 0;
+      float largest = -1.0f;
+
+      for( int i=0; i<3; i ++ )
+      {
+         if( fabsf( v0[i] ) > largest )
+         {
+            largest = fabsf( v0[i] );
+            major_axis = i;
+         }
+      }
+      
+      v3_zero( v1 );
+      v1[ major_axis ] = 1.0f;
+      rb_tangent_basis( v1, tx, ty );
+      
+      float r = (fabsf(v3_dot(tx,v0)) + fabsf(v3_dot(ty,v0))) * 0.25f,
+            l = fabsf(v0[ major_axis ]);
+      
+      /* orientation */
+      v3_muls( tx, -1.0f, rp->collider_mtx[0] );
+      v3_muls( v1, -1.0f, rp->collider_mtx[1] );
+      v3_muls( ty, -1.0f, rp->collider_mtx[2] );
+      v3_add( bone->hitbox[0], bone->hitbox[1], rp->collider_mtx[3] );
+      v3_muls( rp->collider_mtx[3], 0.5f, rp->collider_mtx[3] );
+
+      rp->rb.type = k_rb_shape_capsule;
+      rp->rb.inf.capsule.height = l;
+      rp->rb.inf.capsule.radius = r;
+
+      rp->colour = 0xff000000 | (0xff << (major_axis*8));
+   }
+   else
+      vg_fatal_exit_loop( "Invalid bone collider type" );
+
+   m4x3_invert_affine( rp->collider_mtx, rp->inv_collider_mtx );
+
+   /* Position collider into rest */
+   m3x3_q( rp->collider_mtx, rp->rb.q );
+   v3_add( rp->collider_mtx[3], bone->co, rp->rb.co );
+   rp->rb.is_world = 0;
+   rb_init( &rp->rb );
+}
+
+/*
+ * Get parent index in the ragdoll
+ */
+VG_STATIC u32 ragdoll_bone_parent( struct player_model *mdl, u32 bone_id )
+{
+   for( u32 j=0; j<mdl->ragdoll_count; j++ )
+      if( mdl->ragdoll[ j ].bone_id == bone_id )
+         return j;
+
+   vg_fatal_exit_loop( "Referenced parent bone does not have a rigidbody" );
+   return 0;
+}
 
 /*
  * Setup ragdoll colliders 
  */
-static void player_init_ragdoll( mdl_header *src )
+VG_STATIC void player_init_ragdoll(void)
 {
    struct player_model *mdl = &player.mdl;
+   mdl_context *src = &mdl->meta;
 
    if( !mdl->sk.collider_count )
    {
@@ -19,53 +107,83 @@ static void player_init_ragdoll( mdl_header *src )
       return;
    }
 
-   mdl->ragdoll = vg_alloc(sizeof(struct ragdoll_part)*mdl->sk.collider_count);
    mdl->ragdoll_count = 0;
+   mdl->position_constraints_count = 0;
+   mdl->cone_constraints_count = 0;
 
    for( u32 i=0; i<mdl->sk.bone_count; i ++ )
    {
       struct skeleton_bone *bone = &mdl->sk.bones[i];
-      
-      if( bone->collider )
-      {
-         struct ragdoll_part *rp = &mdl->ragdoll[ mdl->ragdoll_count ++ ];
-         rp->bone_id = i;
-         
-         v3f delta;
-         v3_sub( bone->hitbox[1], bone->hitbox[0], delta );
-         v3_muls( delta, 0.5f, delta );
 
-         v3_add( bone->hitbox[0], delta, rp->offset );
+      /*
+       * Bones with colliders
+       */
+      if( !(bone->flags & k_bone_flag_collider_any) )
+         continue;
 
-         v3_copy( delta, rp->rb.bbx[1] );
-         v3_muls( delta, -1.0f, rp->rb.bbx[0] );
+      if( mdl->ragdoll_count > vg_list_size(player.mdl.ragdoll) )
+         vg_fatal_exit_loop( "Playermodel has too many colliders" );
 
-         q_identity( rp->rb.q );
-         v3_add( bone->co, rp->offset, rp->rb.co );
-         rp->rb.type = k_rb_shape_box;
-         rp->rb.is_world = 0;
-         rp->parent = 0xffffffff;
+      struct ragdoll_part *rp = &mdl->ragdoll[ mdl->ragdoll_count ++ ];
+      rp->bone_id = i;
+      rp->parent = 0xffffffff;
 
-         if( bone->parent )
-         {
-            for( u32 j=0; j<mdl->ragdoll_count; j++ )
-            {
-               if( mdl->ragdoll[ j ].bone_id == bone->parent )
-               {
-                  rp->parent = j;
-                  break;
-               }
-            }
-         }
+      player_init_ragdoll_bone_collider( bone, rp );
+      
+      struct mdl_node *pnode = mdl_node_from_id( src, bone->orig_node );
+      struct classtype_bone *inf = mdl_get_entdata( src, pnode );
+      
+      /*
+       * Bones with collider and parent
+       */
+      if( !bone->parent )
+         continue;
+
+      rp->parent = ragdoll_bone_parent( mdl, bone->parent );
+      
+      /* Always assign a point-to-point constraint */
+      struct rb_constr_pos *c = 
+         &mdl->position_constraints[ mdl->position_constraints_count ++ ];
+
+      struct skeleton_bone *bj = &mdl->sk.bones[rp->bone_id];
+      struct ragdoll_part  *pp = &mdl->ragdoll[rp->parent];
+      struct skeleton_bone *bp = &mdl->sk.bones[pp->bone_id];
+
+      /* Convention: rba -- parent, rbb -- child */
+      c->rba = &pp->rb;
+      c->rbb = &rp->rb;
+
+      v3f delta;
+      v3_sub( bj->co, bp->co, delta );
+      m4x3_mulv( rp->inv_collider_mtx, (v3f){0.0f,0.0f,0.0f}, c->lcb );
+      m4x3_mulv( pp->inv_collider_mtx, delta, c->lca );
+      
+      if( inf->flags & k_bone_flag_cone_constraint )
+      {
+         struct rb_constr_swingtwist *a = 
+            &mdl->cone_constraints[ mdl->cone_constraints_count ++ ];
+         a->rba = &pp->rb;
+         a->rbb = &rp->rb;
+         a->conet = cosf( inf->conet )-0.0001f;
          
-         struct mdl_node *pnode = mdl_node_from_id( src, bone->orig_node );
-         struct classtype_bone *bone_inf = mdl_get_entdata( src, pnode );
+         /* Store constraint in local space vectors */
+         m3x3_mulv( c->rba->to_local, inf->conevx, a->conevx );
+         m3x3_mulv( c->rba->to_local, inf->conevy, a->conevy );
+         m3x3_mulv( c->rbb->to_local, inf->coneva, a->coneva );
+         v3_copy( c->lca, a->view_offset );
+         
+         v3_cross( inf->coneva, inf->conevy, a->conevxb );
+         m3x3_mulv( c->rbb->to_local, a->conevxb, a->conevxb );
+
+         v3_normalize( a->conevxb );
+         v3_normalize( a->conevx );
+         v3_normalize( a->conevy );
+         v3_normalize( a->coneva );
 
-         rp->use_limits = bone_inf->use_limits;
-         v3_copy( bone_inf->angle_limits[0], rp->limits[0] );
-         v3_copy( bone_inf->angle_limits[1], rp->limits[1] );
+         a->conevx[3] = v3_length( inf->conevx );
+         a->conevy[3] = v3_length( inf->conevy );
 
-         rb_init( &rp->rb );
+         rp->use_limits = 1;
       }
    }
 }
@@ -73,7 +191,7 @@ static void player_init_ragdoll( mdl_header *src )
 /*
  * Make the player model copy the ragdoll
  */
-static void player_model_copy_ragdoll(void)
+VG_STATIC void player_model_copy_ragdoll(void)
 {
    struct player_model *mdl = &player.mdl;
 
@@ -82,8 +200,8 @@ static void player_model_copy_ragdoll(void)
       struct ragdoll_part *part = &mdl->ragdoll[i];
       m4x3f offset;
       m3x3_identity(offset);
-      v3_negate( part->offset, offset[3] );
-      m4x3_mul( part->rb.to_world, offset, mdl->sk.final_mtx[part->bone_id] );
+      m4x3_mul( part->rb.to_world, part->inv_collider_mtx, 
+                  mdl->sk.final_mtx[part->bone_id] );
    }
 
    skeleton_apply_inverses( &mdl->sk );
@@ -92,7 +210,7 @@ static void player_model_copy_ragdoll(void)
 /*
  * Make the ragdoll copy the player model
  */
-static void player_ragdoll_copy_model( v3f v )
+VG_STATIC void player_ragdoll_copy_model( v3f v )
 {
    struct player_model *mdl = &player.mdl;
 
@@ -102,11 +220,15 @@ static void player_ragdoll_copy_model( v3f v )
 
       v3f pos, offset;
       u32 bone = part->bone_id;
-      
+
       m4x3_mulv( mdl->sk.final_mtx[bone], mdl->sk.bones[bone].co, pos );
-      m3x3_mulv( mdl->sk.final_mtx[bone], part->offset, offset );
+      m3x3_mulv( mdl->sk.final_mtx[bone], part->collider_mtx[3], offset );
       v3_add( pos, offset, part->rb.co );
-      m3x3_q( mdl->sk.final_mtx[bone], part->rb.q );
+
+      m3x3f r;
+      m3x3_mul( mdl->sk.final_mtx[bone], part->collider_mtx, r );
+      m3x3_q( r, part->rb.q );
+
       v3_copy( v, part->rb.v );
       v3_zero( part->rb.w );
       
@@ -117,98 +239,110 @@ static void player_ragdoll_copy_model( v3f v )
 /*
  * Draw rigidbody colliders for ragdoll
  */
-static void player_debug_ragdoll(void)
+VG_STATIC void player_debug_ragdoll(void)
 {
    struct player_model *mdl = &player.mdl;
-
-   for( u32 i=0; i<mdl->ragdoll_count; i ++ )
-      rb_debug( &mdl->ragdoll[i].rb, 0xff00ff00 );
 }
 
 /*
  * Ragdoll physics step
  */
-static void player_ragdoll_iter(void)
+VG_STATIC void player_ragdoll_iter(void)
 {
    struct player_model *mdl = &player.mdl;
-   rb_solver_reset();
 
+   int run_sim = 0;
+   ragdoll_frame ++;
+
+   if( ragdoll_frame >= k_ragdoll_div )
+   {
+      ragdoll_frame = 0;
+      run_sim = 1;
+   }
+
+   rb_solver_reset();
    for( int i=0; i<mdl->ragdoll_count; i ++ )
       rb_collide( &mdl->ragdoll[i].rb, &world.rb_geo );
 
+   /* 
+    * COLLISION DETECTION
+    */
+   for( int i=0; i<mdl->ragdoll_count-1; i ++ )
+   {
+      for( int j=i+1; j<mdl->ragdoll_count; j ++ )
+      {
+         if( mdl->ragdoll[j].parent != i )
+            rb_collide( &mdl->ragdoll[i].rb, &mdl->ragdoll[j].rb );
+      }
+   }
+
+   /*
+    * PRESOLVE
+    */
    rb_presolve_contacts( rb_contact_buffer, rb_contact_count );
+   rb_presolve_swingtwist_constraints( mdl->cone_constraints,
+                                       mdl->cone_constraints_count );
 
-   v3f rv;
+   /* 
+    * DEBUG
+    */
+   if( k_ragdoll_debug_collider )
+   {
+      for( u32 i=0; i<mdl->ragdoll_count; i ++ )
+         rb_debug( &mdl->ragdoll[i].rb, mdl->ragdoll[i].colour );
+   }
+
+   if( k_ragdoll_debug_constraints )
+   {
+      rb_debug_position_constraints( mdl->position_constraints, 
+                                     mdl->position_constraints_count );
+
+      rb_debug_swingtwist_constraints( mdl->cone_constraints,
+                                       mdl->cone_constraints_count );
+   }
 
 #if 0
-   float shoe_vel[2] = {0.0f,0.0f};
-   for( int i=0; i<2; i++ )
-      if( mdl->shoes[i] )
-         shoe_vel[i] = v3_length( mdl->ragdoll[i].rb.v );
-#endif
-   
    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( pj->parent != 0xffffffff )
+      if( run_sim )
       {
-         struct ragdoll_part *pp = &mdl->ragdoll[pj->parent];
-         struct skeleton_bone *bp = &mdl->sk.bones[pp->bone_id];
-
-         v3f lca, lcb;
-         v3_negate( pj->offset, lca );
-         v3_add( bp->co, pp->offset, lcb );
-         v3_sub( bj->co, lcb, lcb );
-
-         rb_debug_constraint_position( &pj->rb, lca, &pp->rb, lcb );
-
-         if( pj->use_limits )
-         {
-            rb_debug_constraint_limits( &pj->rb, &pp->rb, lca, pj->limits );
-         }
+         v4f plane = {0.0f,1.0f,0.0f,0.0f};
+         rb_effect_simple_bouyency( &pj->rb, plane, k_ragdoll_floatyiness,
+                                                    k_ragdoll_floatydrag );
       }
-
-      v4f plane = {0.0f,1.0f,0.0f,0.0f};
-      rb_effect_simple_bouyency( &pj->rb, plane, k_ragdoll_floatyiness,
-                                                 k_ragdoll_floatydrag );
    }
+#endif
 
-   /* CONSTRAINTS */
-   for( int i=0; i<10; i++ )
+   /*
+    * SOLVE CONSTRAINTS 
+    */
+   if( run_sim )
    {
-      rb_solve_contacts( rb_contact_buffer, rb_contact_count );
-
-      for( int j=0; j<mdl->ragdoll_count; j++ )
+      for( int i=0; i<25; i++ )
       {
-         struct ragdoll_part *pj = &mdl->ragdoll[j];
-         struct skeleton_bone *bj = &mdl->sk.bones[pj->bone_id];
+         rb_solve_contacts( rb_contact_buffer, rb_contact_count );
+         rb_solve_swingtwist_constraints( mdl->cone_constraints, 
+                                          mdl->cone_constraints_count );
+         rb_solve_position_constraints( mdl->position_constraints, 
+                                        mdl->position_constraints_count );
+      }
 
-         if( (pj->parent != 0xffffffff) && pj->use_limits )
-         {
-            struct ragdoll_part *pp = &mdl->ragdoll[pj->parent];
-            struct skeleton_bone *bp = &mdl->sk.bones[pp->bone_id];
+      for( int i=0; i<mdl->ragdoll_count; i++ )
+         rb_iter( &mdl->ragdoll[i].rb );
 
-            v3f lca, lcb;
-            v3_negate( pj->offset, lca );
-            v3_add( bp->co, pp->offset, lcb );
-            v3_sub( bj->co, lcb, lcb );
+      for( int i=0; i<mdl->ragdoll_count; i++ )
+         rb_update_transform( &mdl->ragdoll[i].rb );
 
-            rb_constraint_position( &pj->rb, lca, &pp->rb, lcb );
+      rb_correct_swingtwist_constraints( mdl->cone_constraints, 
+                                         mdl->cone_constraints_count, 0.25f );
 
-            rb_constraint_limits( &pj->rb, lca, &pp->rb, lcb, pj->limits );
-         }
-      }
+      rb_correct_position_constraints( mdl->position_constraints,
+                                       mdl->position_constraints_count, 0.5f );
    }
 
-   /* INTEGRATION */
-   for( int i=0; i<mdl->ragdoll_count; i++ )
-      rb_iter( &mdl->ragdoll[i].rb );
-   
-   /* SHOES */
-   for( int i=0; i<mdl->ragdoll_count; i++ )
-      rb_update_transform( &mdl->ragdoll[i].rb );
 }
 
 #endif /* PLAYER_RAGDOLL_H */