player_ragdoll.h

   1 #ifndef PLAYER_RAGDOLL_H
   2 #define PLAYER_RAGDOLL_H
   3
   4 #include "player.h"
   5
   6 VG_STATIC float k_ragdoll_floatyiness = 20.0f,
   7                 k_ragdoll_floatydrag  = 1.0f,
   8                 k_ragdoll_limit_scale = 1.0f;
   9
  10 VG_STATIC int   k_ragdoll_div = 1,
  11                 ragdoll_frame = 0,
  12                 k_ragdoll_debug_collider = 1,
  13                 k_ragdoll_debug_constraints = 0;
  14
  15 VG_STATIC void player_init_ragdoll_bone_collider( struct skeleton_bone *bone,
  16                                                   struct ragdoll_part *rp )
  17 {
  18    m4x3_identity( rp->collider_mtx );
  19
  20    if( bone->flags & k_bone_flag_collider_box )
  21    {
  22       v3f delta;
  23       v3_sub( bone->hitbox[1], bone->hitbox[0], delta );
  24       v3_muls( delta, 0.5f, delta );
  25       v3_add( bone->hitbox[0], delta, rp->collider_mtx[3] );
  26
  27       v3_copy( delta, rp->rb.bbx[1] );
  28       v3_muls( delta, -1.0f, rp->rb.bbx[0] );
  29
  30       q_identity( rp->rb.q );
  31       rp->rb.type = k_rb_shape_box;
  32       rp->colour = 0xffcccccc;
  33    }
  34    else if( bone->flags & k_bone_flag_collider_capsule )
  35    {
  36       v3f v0, v1, tx, ty;
  37       v3_sub( bone->hitbox[1], bone->hitbox[0], v0 );
  38
  39       int major_axis = 0;
  40       float largest = -1.0f;
  41
  42       for( int i=0; i<3; i ++ )
  43       {
  44          if( fabsf( v0[i] ) > largest )
  45          {
  46             largest = fabsf( v0[i] );
  47             major_axis = i;
  48          }
  49       }
  50
  51       v3_zero( v1 );
  52       v1[ major_axis ] = 1.0f;
  53       rb_tangent_basis( v1, tx, ty );
  54
  55       float r = (fabsf(v3_dot(tx,v0)) + fabsf(v3_dot(ty,v0))) * 0.25f,
  56             l = fabsf(v0[ major_axis ]);
  57
  58       /* orientation */
  59       v3_muls( tx, -1.0f, rp->collider_mtx[0] );
  60       v3_muls( v1, -1.0f, rp->collider_mtx[1] );
  61       v3_muls( ty, -1.0f, rp->collider_mtx[2] );
  62       v3_add( bone->hitbox[0], bone->hitbox[1], rp->collider_mtx[3] );
  63       v3_muls( rp->collider_mtx[3], 0.5f, rp->collider_mtx[3] );
  64
  65       rp->rb.type = k_rb_shape_capsule;
  66       rp->rb.inf.capsule.height = l;
  67       rp->rb.inf.capsule.radius = r;
  68
  69       rp->colour = 0xff000000 | (0xff << (major_axis*8));
  70    }
  71    else
  72       vg_fatal_exit_loop( "Invalid bone collider type" );
  73
  74    m4x3_invert_affine( rp->collider_mtx, rp->inv_collider_mtx );
  75
  76    /* Position collider into rest */
  77    m3x3_q( rp->collider_mtx, rp->rb.q );
  78    v3_add( rp->collider_mtx[3], bone->co, rp->rb.co );
  79    rp->rb.is_world = 0;
  80    rb_init( &rp->rb );
  81 }
  82
  83 /*
  84  * Get parent index in the ragdoll
  85  */
  86 VG_STATIC u32 ragdoll_bone_parent( struct player_model *mdl, u32 bone_id )
  87 {
  88    for( u32 j=0; j<mdl->ragdoll_count; j++ )
  89       if( mdl->ragdoll[ j ].bone_id == bone_id )
  90          return j;
  91
  92    vg_fatal_exit_loop( "Referenced parent bone does not have a rigidbody" );
  93    return 0;
  94 }
  95
  96 /*
  97  * Setup ragdoll colliders
  98  */
  99 VG_STATIC void player_init_ragdoll(void)
 100 {
 101    struct player_model *mdl = &player.mdl;
 102    mdl_context *src = &mdl->meta;
 103
 104    if( !mdl->sk.collider_count )
 105    {
 106       mdl->ragdoll_count = 0;
 107       return;
 108    }
 109
 110    mdl->ragdoll_count = 0;
 111    mdl->position_constraints_count = 0;
 112    mdl->cone_constraints_count = 0;
 113
 114    for( u32 i=0; i<mdl->sk.bone_count; i ++ )
 115    {
 116       struct skeleton_bone *bone = &mdl->sk.bones[i];
 117
 118       /*
 119        * Bones with colliders
 120        */
 121       if( !(bone->flags & k_bone_flag_collider_any) )
 122          continue;
 123
 124       if( mdl->ragdoll_count > vg_list_size(player.mdl.ragdoll) )
 125          vg_fatal_exit_loop( "Playermodel has too many colliders" );
 126
 127       struct ragdoll_part *rp = &mdl->ragdoll[ mdl->ragdoll_count ++ ];
 128       rp->bone_id = i;
 129       rp->parent = 0xffffffff;
 130
 131       player_init_ragdoll_bone_collider( bone, rp );
 132
 133       struct mdl_node *pnode = mdl_node_from_id( src, bone->orig_node );
 134       struct classtype_bone *inf = mdl_get_entdata( src, pnode );
 135
 136       /*
 137        * Bones with collider and parent
 138        */
 139       if( !bone->parent )
 140          continue;
 141
 142       rp->parent = ragdoll_bone_parent( mdl, bone->parent );
 143
 144       /* Always assign a point-to-point constraint */
 145       struct rb_constr_pos *c =
 146          &mdl->position_constraints[ mdl->position_constraints_count ++ ];
 147
 148       struct skeleton_bone *bj = &mdl->sk.bones[rp->bone_id];
 149       struct ragdoll_part  *pp = &mdl->ragdoll[rp->parent];
 150       struct skeleton_bone *bp = &mdl->sk.bones[pp->bone_id];
 151
 152       /* Convention: rba -- parent, rbb -- child */
 153       c->rba = &pp->rb;
 154       c->rbb = &rp->rb;
 155
 156       v3f delta;
 157       v3_sub( bj->co, bp->co, delta );
 158       m4x3_mulv( rp->inv_collider_mtx, (v3f){0.0f,0.0f,0.0f}, c->lcb );
 159       m4x3_mulv( pp->inv_collider_mtx, delta, c->lca );
 160
 161       if( inf->flags & k_bone_flag_cone_constraint )
 162       {
 163          struct rb_constr_swingtwist *a =
 164             &mdl->cone_constraints[ mdl->cone_constraints_count ++ ];
 165          a->rba = &pp->rb;
 166          a->rbb = &rp->rb;
 167          a->conet = cosf( inf->conet )-0.0001f;
 168
 169          /* Store constraint in local space vectors */
 170          m3x3_mulv( c->rba->to_local, inf->conevx, a->conevx );
 171          m3x3_mulv( c->rba->to_local, inf->conevy, a->conevy );
 172          m3x3_mulv( c->rbb->to_local, inf->coneva, a->coneva );
 173          v3_copy( c->lca, a->view_offset );
 174
 175          v3_cross( inf->coneva, inf->conevy, a->conevxb );
 176          m3x3_mulv( c->rbb->to_local, a->conevxb, a->conevxb );
 177
 178          v3_normalize( a->conevxb );
 179          v3_normalize( a->conevx );
 180          v3_normalize( a->conevy );
 181          v3_normalize( a->coneva );
 182
 183          a->conevx[3] = v3_length( inf->conevx );
 184          a->conevy[3] = v3_length( inf->conevy );
 185
 186          rp->use_limits = 1;
 187       }
 188    }
 189 }
 190
 191 /*
 192  * Make the player model copy the ragdoll
 193  */
 194 VG_STATIC void player_model_copy_ragdoll(void)
 195 {
 196    struct player_model *mdl = &player.mdl;
 197
 198    for( int i=0; i<mdl->ragdoll_count; i++ )
 199    {
 200       struct ragdoll_part *part = &mdl->ragdoll[i];
 201       m4x3f offset;
 202       m3x3_identity(offset);
 203       m4x3_mul( part->rb.to_world, part->inv_collider_mtx,
 204                   mdl->sk.final_mtx[part->bone_id] );
 205    }
 206
 207    skeleton_apply_inverses( &mdl->sk );
 208 }
 209
 210 /*
 211  * Make the ragdoll copy the player model
 212  */
 213 VG_STATIC void player_ragdoll_copy_model( v3f v )
 214 {
 215    struct player_model *mdl = &player.mdl;
 216
 217    for( int i=0; i<mdl->ragdoll_count; i++ )
 218    {
 219       struct ragdoll_part *part = &mdl->ragdoll[i];
 220
 221       v3f pos, offset;
 222       u32 bone = part->bone_id;
 223
 224       m4x3_mulv( mdl->sk.final_mtx[bone], mdl->sk.bones[bone].co, pos );
 225       m3x3_mulv( mdl->sk.final_mtx[bone], part->collider_mtx[3], offset );
 226       v3_add( pos, offset, part->rb.co );
 227
 228       m3x3f r;
 229       m3x3_mul( mdl->sk.final_mtx[bone], part->collider_mtx, r );
 230       m3x3_q( r, part->rb.q );
 231
 232       v3_copy( v, part->rb.v );
 233       v3_zero( part->rb.w );
 234
 235       rb_update_transform( &part->rb );
 236    }
 237 }
 238
 239 /*
 240  * Draw rigidbody colliders for ragdoll
 241  */
 242 VG_STATIC void player_debug_ragdoll(void)
 243 {
 244    struct player_model *mdl = &player.mdl;
 245 }
 246
 247 /*
 248  * Ragdoll physics step
 249  */
 250 VG_STATIC void player_ragdoll_iter(void)
 251 {
 252    struct player_model *mdl = &player.mdl;
 253
 254    int run_sim = 0;
 255    ragdoll_frame ++;
 256
 257    if( ragdoll_frame >= k_ragdoll_div )
 258    {
 259       ragdoll_frame = 0;
 260       run_sim = 1;
 261    }
 262
 263    rb_solver_reset();
 264    for( int i=0; i<mdl->ragdoll_count; i ++ )
 265       rb_collide( &mdl->ragdoll[i].rb, &world.rb_geo );
 266
 267    /*
 268     * COLLISION DETECTION
 269     */
 270    for( int i=0; i<mdl->ragdoll_count-1; i ++ )
 271    {
 272       for( int j=i+1; j<mdl->ragdoll_count; j ++ )
 273       {
 274          if( mdl->ragdoll[j].parent != i )
 275             rb_collide( &mdl->ragdoll[i].rb, &mdl->ragdoll[j].rb );
 276       }
 277    }
 278
 279    for( int j=0; j<mdl->ragdoll_count; j++ )
 280    {
 281       struct ragdoll_part *pj = &mdl->ragdoll[j];
 282       struct skeleton_bone *bj = &mdl->sk.bones[pj->bone_id];
 283
 284       if( run_sim )
 285       {
 286          v4f plane = {0.0f,1.0f,0.0f,0.0f};
 287          rb_effect_simple_bouyency( &pj->rb, plane, k_ragdoll_floatyiness,
 288                                                     k_ragdoll_floatydrag );
 289       }
 290    }
 291
 292    /*
 293     * PRESOLVE
 294     */
 295    rb_presolve_contacts( rb_contact_buffer, rb_contact_count );
 296    rb_presolve_swingtwist_constraints( mdl->cone_constraints,
 297                                        mdl->cone_constraints_count );
 298
 299    /*
 300     * DEBUG
 301     */
 302    if( k_ragdoll_debug_collider )
 303    {
 304       for( u32 i=0; i<mdl->ragdoll_count; i ++ )
 305          rb_debug( &mdl->ragdoll[i].rb, mdl->ragdoll[i].colour );
 306    }
 307
 308    if( k_ragdoll_debug_constraints )
 309    {
 310       rb_debug_position_constraints( mdl->position_constraints,
 311                                      mdl->position_constraints_count );
 312
 313       rb_debug_swingtwist_constraints( mdl->cone_constraints,
 314                                        mdl->cone_constraints_count );
 315    }
 316
 317    /*
 318     * SOLVE CONSTRAINTS
 319     */
 320    if( run_sim )
 321    {
 322       for( int i=0; i<25; i++ )
 323       {
 324          rb_solve_contacts( rb_contact_buffer, rb_contact_count );
 325          rb_solve_swingtwist_constraints( mdl->cone_constraints,
 326                                           mdl->cone_constraints_count );
 327          rb_solve_position_constraints( mdl->position_constraints,
 328                                         mdl->position_constraints_count );
 329       }
 330
 331       for( int i=0; i<mdl->ragdoll_count; i++ )
 332          rb_iter( &mdl->ragdoll[i].rb );
 333
 334       for( int i=0; i<mdl->ragdoll_count; i++ )
 335          rb_update_transform( &mdl->ragdoll[i].rb );
 336
 337       rb_correct_swingtwist_constraints( mdl->cone_constraints,
 338                                          mdl->cone_constraints_count, 0.25f );
 339
 340       rb_correct_position_constraints( mdl->position_constraints,
 341                                        mdl->position_constraints_count, 0.5f );
 342    }
 343
 344 }
 345
 346 #endif /* PLAYER_RAGDOLL_H */