X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=player_ragdoll.h;h=d6607ae89331a9ef7ec2883e37099f7665043f73;hb=8c376ed2e4021a18b0a6c6e800109d67ad09d198;hp=0d6d74d3f7cba96b03b2b23700585e97a1b9ba2c;hpb=deac3e6a1b3a8d28302bbc18c183c7fe64cbfb3e;p=carveJwlIkooP6JGAAIwe30JlM.git

diff --git a/player_ragdoll.h b/player_ragdoll.h
index 0d6d74d..d6607ae 100644
--- a/player_ragdoll.h
+++ b/player_ragdoll.h
@@ -1,7 +1,40 @@
 #ifndef PLAYER_RAGDOLL_H
 #define PLAYER_RAGDOLL_H
 
-#include "player.h"
+#define VG_GAME
+#include "vg/vg.h"
+#include "skeleton.h"
+#include "rigidbody.h"
+#include "player_model.h"
+#include "world.h"
+#include "audio.h"
+
+struct player_ragdoll{
+   struct ragdoll_part{
+      u32 bone_id;
+      
+      /* Collider transform relative to bone */
+      m4x3f collider_mtx,
+            inv_collider_mtx;
+
+      u32 use_limits;
+      v3f limits[2];
+
+      rb_object obj;
+      u32 parent;
+      u32 colour;
+   }
+   parts[32];
+   u32 part_count;
+
+   rb_constr_pos  position_constraints[32];
+   u32            position_constraints_count;
+
+   rb_constr_swingtwist cone_constraints[32];
+   u32                  cone_constraints_count;
+
+   int shoes[2];
+};
 
 VG_STATIC float k_ragdoll_floatyiness = 20.0f,
                 k_ragdoll_floatydrag  = 1.0f,
@@ -12,37 +45,41 @@ VG_STATIC int   k_ragdoll_div = 1,
                 k_ragdoll_debug_collider = 1,
                 k_ragdoll_debug_constraints = 0;
 
+VG_STATIC void player_ragdoll_init(void)
+{
+   VG_VAR_F32( k_ragdoll_limit_scale );
+   VG_VAR_I32( k_ragdoll_div );
+   VG_VAR_I32( k_ragdoll_debug_collider );
+   VG_VAR_I32( k_ragdoll_debug_constraints );
+}
+
 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 )
-   {
+   if( bone->collider == k_bone_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] );
+      v3_copy( delta, rp->obj.rb.bbx[1] );
+      v3_muls( delta, -1.0f, rp->obj.rb.bbx[0] );
 
-      q_identity( rp->rb.q );
-      rp->rb.type = k_rb_shape_box;
+      q_identity( rp->obj.rb.q );
+      rp->obj.type = k_rb_shape_box;
       rp->colour = 0xffcccccc;
    }
-   else if( bone->flags & k_bone_flag_collider_capsule )
-   {
+   else if( bone->collider == k_bone_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 )
-         {
+      for( int i=0; i<3; i ++ ){
+         if( fabsf( v0[i] ) > largest ){
             largest = fabsf( v0[i] );
             major_axis = i;
          }
@@ -62,31 +99,35 @@ VG_STATIC void player_init_ragdoll_bone_collider( struct skeleton_bone *bone,
       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->obj.type = k_rb_shape_capsule;
+      rp->obj.inf.capsule.height = l;
+      rp->obj.inf.capsule.radius = r;
 
       rp->colour = 0xff000000 | (0xff << (major_axis*8));
    }
-   else
+   else{
+      vg_warn( "type: %u\n", bone->collider );
       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 );
+   m3x3_q( rp->collider_mtx, rp->obj.rb.q );
+   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 );
 }
 
 /*
  * Get parent index in the ragdoll
  */
-VG_STATIC u32 ragdoll_bone_parent( struct player_model *mdl, u32 bone_id )
+VG_STATIC u32 ragdoll_bone_parent( struct player_ragdoll *rd,
+                                   struct player_avatar *av, u32 bone_id )
 {
-   for( u32 j=0; j<mdl->ragdoll_count; j++ )
-      if( mdl->ragdoll[ j ].bone_id == bone_id )
+   for( u32 j=0; j<rd->part_count; j++ )
+      if( rd->parts[ j ].bone_id == bone_id )
          return j;
 
    vg_fatal_exit_loop( "Referenced parent bone does not have a rigidbody" );
@@ -96,74 +137,69 @@ VG_STATIC u32 ragdoll_bone_parent( struct player_model *mdl, u32 bone_id )
 /*
  * Setup ragdoll colliders 
  */
-VG_STATIC void player_init_ragdoll(void)
+VG_STATIC void player_setup_ragdoll_from_avatar( struct player_ragdoll *rd,
+                                                 struct player_avatar *av )
 {
-   struct player_model *mdl = &player.mdl;
-   mdl_context *src = &mdl->meta;
+   rd->part_count = 0;
 
-   if( !mdl->sk.collider_count )
-   {
-      mdl->ragdoll_count = 0;
+   if( !av->sk.collider_count )
       return;
-   }
 
-   mdl->ragdoll_count = 0;
-   mdl->position_constraints_count = 0;
-   mdl->cone_constraints_count = 0;
+   rd->position_constraints_count = 0;
+   rd->cone_constraints_count = 0;
 
-   for( u32 i=0; i<mdl->sk.bone_count; i ++ )
-   {
-      struct skeleton_bone *bone = &mdl->sk.bones[i];
+   for( u32 i=1; i<av->sk.bone_count; i ++ ){
+      struct skeleton_bone *bone = &av->sk.bones[i];
 
       /*
        * Bones with colliders
        */
-      if( !(bone->flags & k_bone_flag_collider_any) )
+      if( !(bone->collider) )
          continue;
 
-      if( mdl->ragdoll_count > vg_list_size(player.mdl.ragdoll) )
+      if( rd->part_count > vg_list_size(rd->parts) )
          vg_fatal_exit_loop( "Playermodel has too many colliders" );
 
-      struct ragdoll_part *rp = &mdl->ragdoll[ mdl->ragdoll_count ++ ];
+      struct ragdoll_part *rp = &rd->parts[ rd->part_count ++ ];
       rp->bone_id = i;
       rp->parent = 0xffffffff;
 
       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 );
+      rp->parent = ragdoll_bone_parent( rd, av, bone->parent );
+      
       
-      /* Always assign a point-to-point constraint */
-      struct rb_constr_pos *c = 
-         &mdl->position_constraints[ mdl->position_constraints_count ++ ];
+      if( bone->orig_bone->flags & k_bone_flag_cone_constraint ){
+         struct rb_constr_pos *c = 
+            &rd->position_constraints[ rd->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];
+         struct skeleton_bone *bj = &av->sk.bones[rp->bone_id];
+         struct ragdoll_part  *pp = &rd->parts[rp->parent];
+         struct skeleton_bone *bp = &av->sk.bones[pp->bone_id];
 
-      /* Convention: rba -- parent, rbb -- child */
-      c->rba = &pp->rb;
-      c->rbb = &rp->rb;
+         /* Convention: rba -- parent, rbb -- child */
+         c->rba = &pp->obj.rb;
+         c->rbb = &rp->obj.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 );
+
+
+         mdl_bone *inf = bone->orig_bone;
 
-      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;
+            &rd->cone_constraints[ rd->cone_constraints_count ++ ];
+
+         a->rba = &pp->obj.rb;
+         a->rbb = &rp->obj.rb;
          a->conet = cosf( inf->conet )-0.0001f;
          
          /* Store constraint in local space vectors */
@@ -189,50 +225,63 @@ VG_STATIC void player_init_ragdoll(void)
 }
 
 /*
- * Make the player model copy the ragdoll
+ * Make avatar copy the ragdoll
  */
-VG_STATIC void player_model_copy_ragdoll(void)
+VG_STATIC void copy_ragdoll_pose_to_avatar( struct player_ragdoll *rd,
+                                            struct player_avatar *av )
 {
-   struct player_model *mdl = &player.mdl;
-
-   for( int i=0; i<mdl->ragdoll_count; i++ )
-   {
-      struct ragdoll_part *part = &mdl->ragdoll[i];
+   for( int i=0; i<rd->part_count; i++ ){
+      struct ragdoll_part *part = &rd->parts[i];
       m4x3f offset;
       m3x3_identity(offset);
-      m4x3_mul( part->rb.to_world, part->inv_collider_mtx, 
-                  mdl->sk.final_mtx[part->bone_id] );
+      m4x3_mul( part->obj.rb.to_world, part->inv_collider_mtx, 
+                  av->sk.final_mtx[part->bone_id] );
    }
 
-   skeleton_apply_inverses( &mdl->sk );
+   for( u32 i=1; i<av->sk.bone_count; i++ ){
+      struct skeleton_bone *sb = &av->sk.bones[i];
+
+      if( sb->parent && !sb->collider ){
+         v3f delta;
+         v3_sub( av->sk.bones[i].co, av->sk.bones[sb->parent].co, delta );
+
+         m4x3f posemtx;
+         m3x3_identity( posemtx );
+         v3_copy( delta, posemtx[3] );
+
+         /* final matrix */
+         m4x3_mul( av->sk.final_mtx[sb->parent], posemtx, av->sk.final_mtx[i] );
+      }
+   }
+
+   skeleton_apply_inverses( &av->sk );
 }
 
 /*
  * Make the ragdoll copy the player model
  */
-VG_STATIC void player_ragdoll_copy_model( v3f v )
+VG_STATIC void copy_avatar_pose_to_ragdoll( struct player_avatar *av,
+                                            struct player_ragdoll *rd,
+                                            v3f velocity )
 {
-   struct player_model *mdl = &player.mdl;
-
-   for( int i=0; i<mdl->ragdoll_count; i++ )
-   {
-      struct ragdoll_part *part = &mdl->ragdoll[i];
+   for( int i=0; i<rd->part_count; i++ ){
+      struct ragdoll_part *part = &rd->parts[i];
 
       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->collider_mtx[3], offset );
-      v3_add( pos, offset, part->rb.co );
+      m4x3_mulv( av->sk.final_mtx[bone], av->sk.bones[bone].co, pos );
+      m3x3_mulv( av->sk.final_mtx[bone], part->collider_mtx[3], offset );
+      v3_add( pos, offset, part->obj.rb.co );
 
       m3x3f r;
-      m3x3_mul( mdl->sk.final_mtx[bone], part->collider_mtx, r );
-      m3x3_q( r, part->rb.q );
+      m3x3_mul( av->sk.final_mtx[bone], part->collider_mtx, r );
+      m3x3_q( r, part->obj.rb.q );
 
-      v3_copy( v, part->rb.v );
-      v3_zero( part->rb.w );
+      v3_copy( velocity, part->obj.rb.v );
+      v3_zero( part->obj.rb.w );
       
-      rb_update_transform( &part->rb );
+      rb_update_transform( &part->obj.rb );
    }
 }
 
@@ -241,106 +290,190 @@ VG_STATIC void player_ragdoll_copy_model( v3f v )
  */
 VG_STATIC void player_debug_ragdoll(void)
 {
-   struct player_model *mdl = &player.mdl;
 }
 
 /*
  * Ragdoll physics step
  */
-VG_STATIC void player_ragdoll_iter(void)
+VG_STATIC void player_ragdoll_iter( struct player_ragdoll *rd )
 {
-   struct player_model *mdl = &player.mdl;
+   world_instance *world = get_active_world();
 
    int run_sim = 0;
    ragdoll_frame ++;
 
-   if( ragdoll_frame >= k_ragdoll_div )
-   {
+   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 );
+   
+   float contact_velocities[256];
+
+   for( int i=0; i<rd->part_count; i ++ ){
+      if( rb_global_has_space() ){
+         rb_ct *buf = rb_global_buffer();
+
+         int l;
+         
+         if( rd->parts[i].obj.type == k_rb_shape_capsule ){
+            l = rb_capsule__scene( rd->parts[i].obj.rb.to_world,
+                                   &rd->parts[i].obj.inf.capsule,
+                                   NULL, &world->rb_geo.inf.scene, buf );
+         }
+         else if( rd->parts[i].obj.type == k_rb_shape_box ){
+            l = rb_box__scene( rd->parts[i].obj.rb.to_world,
+                               rd->parts[i].obj.rb.bbx,
+                               NULL, &world->rb_geo.inf.scene, buf );
+         }
+         else continue;
+
+         for( int j=0; j<l; j++ ){
+            buf[j].rba = &rd->parts[i].obj.rb;
+            buf[j].rbb = &world->rb_geo.rb;
+         }
+
+         rb_contact_count += l;
+      }
+   }
 
    /* 
-    * COLLISION DETECTION
+    * self-collision
     */
-   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 );
+   for( int i=0; i<rd->part_count-1; i ++ ){
+      for( int j=i+1; j<rd->part_count; j ++ ){
+         if( rd->parts[j].parent != i ){
+            if( !rb_global_has_space() )
+               break;
+
+            if( rd->parts[j].obj.type != k_rb_shape_capsule )
+               continue;
+
+            if( rd->parts[i].obj.type != k_rb_shape_capsule )
+               continue;
+
+            rb_ct *buf = rb_global_buffer();
+
+            int l = rb_capsule__capsule( rd->parts[i].obj.rb.to_world,
+                                         &rd->parts[i].obj.inf.capsule,
+                                         rd->parts[j].obj.rb.to_world,
+                                         &rd->parts[j].obj.inf.capsule,
+                                         buf );
+
+            for( int k=0; k<l; k++ ){
+               buf[k].rba = &rd->parts[i].obj.rb;
+               buf[k].rbb = &rd->parts[j].obj.rb;
+            }
+
+            rb_contact_count += l;
+         }
       }
    }
 
-   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];
+   for( int j=0; j<rd->part_count; j++ ){
+      struct ragdoll_part *pj = &rd->parts[j];
 
-      if( run_sim )
-      {
+      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 );
+         rb_effect_simple_bouyency( &pj->obj.rb, plane, 
+                                     k_ragdoll_floatyiness,
+                                     k_ragdoll_floatydrag );
       }
    }
 
    /*
     * PRESOLVE
     */
+   for( u32 i=0; i<rb_contact_count; i++ ){
+      rb_ct *ct = &rb_contact_buffer[i];
+
+      v3f rv, ra, rb;
+      v3_sub( ct->co, ct->rba->co, ra );
+      v3_sub( ct->co, ct->rbb->co, rb );
+      rb_rcv( ct->rba, ct->rbb, ra, rb, rv );
+      float     vn = v3_dot( rv, ct->n );
+
+      contact_velocities[i] = vn;
+   }
+
    rb_presolve_contacts( rb_contact_buffer, rb_contact_count );
-   rb_presolve_swingtwist_constraints( mdl->cone_constraints,
-                                       mdl->cone_constraints_count );
+   rb_presolve_swingtwist_constraints( rd->cone_constraints,
+                                       rd->cone_constraints_count );
 
    /* 
     * 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_collider ){
+      for( u32 i=0; i<rd->part_count; i ++ )
+         rb_object_debug( &rd->parts[i].obj, rd->parts[i].colour );
    }
 
-   if( k_ragdoll_debug_constraints )
-   {
-      rb_debug_position_constraints( mdl->position_constraints, 
-                                     mdl->position_constraints_count );
+   if( k_ragdoll_debug_constraints ){
+      rb_debug_position_constraints( rd->position_constraints, 
+                                     rd->position_constraints_count );
 
-      rb_debug_swingtwist_constraints( mdl->cone_constraints,
-                                       mdl->cone_constraints_count );
+      rb_debug_swingtwist_constraints( rd->cone_constraints,
+                                       rd->cone_constraints_count );
    }
 
    /*
     * SOLVE CONSTRAINTS 
     */
-   if( run_sim )
-   {
-      for( int i=0; i<25; i++ )
-      {
+   if( run_sim ){
+      for( int i=0; i<16; i++ ){
          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 );
+         rb_solve_swingtwist_constraints( rd->cone_constraints, 
+                                          rd->cone_constraints_count );
+         rb_solve_position_constraints( rd->position_constraints, 
+                                        rd->position_constraints_count );
       }
 
-      for( int i=0; i<mdl->ragdoll_count; i++ )
-         rb_iter( &mdl->ragdoll[i].rb );
+      for( int i=0; i<rd->part_count; i++ )
+         rb_iter( &rd->parts[i].obj.rb );
 
-      for( int i=0; i<mdl->ragdoll_count; i++ )
-         rb_update_transform( &mdl->ragdoll[i].rb );
+      for( int i=0; i<rd->part_count; i++ )
+         rb_update_transform( &rd->parts[i].obj.rb );
 
-      rb_correct_swingtwist_constraints( mdl->cone_constraints, 
-                                         mdl->cone_constraints_count, 0.25f );
+      rb_correct_swingtwist_constraints( rd->cone_constraints, 
+                                         rd->cone_constraints_count, 0.25f );
 
-      rb_correct_position_constraints( mdl->position_constraints,
-                                       mdl->position_constraints_count, 0.5f );
+      rb_correct_position_constraints( rd->position_constraints,
+                                       rd->position_constraints_count, 0.5f );
    }
 
+   rb_ct *stress = NULL;
+   float max_stress = 1.0f;
+
+   for( u32 i=0; i<rb_contact_count; i++ ){
+      rb_ct *ct = &rb_contact_buffer[i];
+
+      v3f rv, ra, rb;
+      v3_sub( ct->co, ct->rba->co, ra );
+      v3_sub( ct->co, ct->rbb->co, rb );
+      rb_rcv( ct->rba, ct->rbb, ra, rb, rv );
+      float vn = v3_dot( rv, ct->n );
+
+      float s = fabsf(vn - contact_velocities[i]);
+      if( s > max_stress ){
+         stress = ct;
+         max_stress = s;
+      }
+   }
+
+   static u32 temp_filter = 0;
+
+   if( temp_filter ){
+      temp_filter --;
+      return;
+   }
+
+   if( stress ){
+      temp_filter = 20;
+      audio_lock();
+      audio_oneshot_3d( &audio_hits[rand()%5], stress->co, 20.0f, 1.0f );
+      audio_unlock();
+   }
 }
 
 #endif /* PLAYER_RAGDOLL_H */