player_ragdoll.c

   1 #pragma once
   2 #include "vg/vg_rigidbody.h"
   3 #include "vg/vg_rigidbody_collision.h"
   4 #include "vg/vg_rigidbody_constraints.h"
   5 #include "scene_rigidbody.h"
   6
   7 #include "player.h"
   8 #include "player_dead.h"
   9 #include "audio.h"
  10
  11 static float k_ragdoll_floatyiness = 20.0f,
  12              k_ragdoll_floatydrag  = 1.0f,
  13              k_ragdoll_limit_scale = 1.0f,
  14              k_ragdoll_spring = 127.0f,
  15              k_ragdoll_dampening = 15.0f,
  16              k_ragdoll_correction = 0.5f,
  17              k_ragdoll_angular_drag = 0.08f,
  18              k_ragdoll_active_threshold = 5.0f;
  19
  20 static int   k_ragdoll_div = 1,
  21              ragdoll_frame = 0,
  22              k_ragdoll_debug_collider = 1,
  23              k_ragdoll_debug_constraints = 0;
  24
  25 static int dev_ragdoll_saveload(int argc, const char *argv[]){
  26    if( argc != 2 ){
  27       vg_info( "Usage: ragdoll load/save filepath\n" );
  28       return 1;
  29    }
  30
  31    if( !strcmp(argv[0],"save") ){
  32       FILE *fp = fopen( argv[1], "wb" );
  33       if( !fp ){
  34          vg_error( "Failed to open file\n" );
  35          return 1;
  36       }
  37       fwrite( &localplayer.ragdoll.parts,
  38                sizeof(localplayer.ragdoll.parts), 1, fp );
  39       fclose( fp );
  40    }
  41    else if( !strcmp(argv[0],"load") ){
  42       FILE *fp = fopen( argv[1], "rb" );
  43       if( !fp ){
  44          vg_error( "Failed to open file\n" );
  45          return 1;
  46       }
  47
  48       fread( &localplayer.ragdoll.parts,
  49               sizeof(localplayer.ragdoll.parts), 1, fp );
  50       fclose( fp );
  51    }
  52    else {
  53       vg_error( "Unknown command: %s (options are: save,load)\n", argv[0] );
  54       return 1;
  55    }
  56
  57    return 0;
  58 }
  59
  60 void player_ragdoll_init(void)
  61 {
  62    VG_VAR_F32( k_ragdoll_active_threshold );
  63    VG_VAR_F32( k_ragdoll_angular_drag );
  64    VG_VAR_F32( k_ragdoll_correction );
  65    VG_VAR_F32( k_ragdoll_limit_scale );
  66    VG_VAR_F32( k_ragdoll_spring );
  67    VG_VAR_F32( k_ragdoll_dampening );
  68    VG_VAR_I32( k_ragdoll_div );
  69    VG_VAR_I32( k_ragdoll_debug_collider );
  70    VG_VAR_I32( k_ragdoll_debug_constraints );
  71    vg_console_reg_cmd( "ragdoll", dev_ragdoll_saveload, NULL );
  72 }
  73
  74 void player_init_ragdoll_bone_collider( struct skeleton_bone *bone,
  75                                         struct ragdoll_part *rp )
  76 {
  77    f32 k_density = 8.0f,
  78        k_inertia_scale = 2.0f;
  79
  80    m4x3_identity( rp->collider_mtx );
  81
  82    rp->type = bone->collider;
  83    if( bone->collider == k_bone_collider_box ){
  84       v3f delta;
  85       v3_sub( bone->hitbox[1], bone->hitbox[0], delta );
  86       v3_muls( delta, 0.5f, delta );
  87       v3_add( bone->hitbox[0], delta, rp->collider_mtx[3] );
  88
  89       v3_muls( delta, -1.0f, rp->inf.box[0] );
  90       v3_copy( delta,        rp->inf.box[1] );
  91
  92       rp->colour = 0xffcccccc;
  93
  94       rb_setbody_box( &rp->rb, rp->inf.box, k_density, k_inertia_scale );
  95    }
  96    else if( bone->collider == k_bone_collider_capsule ){
  97       v3f v0, v1, tx, ty;
  98       v3_sub( bone->hitbox[1], bone->hitbox[0], v0 );
  99
 100       int major_axis = 0;
 101       float largest = -1.0f;
 102
 103       for( int i=0; i<3; i ++ ){
 104          if( fabsf( v0[i] ) > largest ){
 105             largest = fabsf( v0[i] );
 106             major_axis = i;
 107          }
 108       }
 109
 110       v3_zero( v1 );
 111       v1[ major_axis ] = 1.0f;
 112       v3_tangent_basis( v1, tx, ty );
 113
 114       rp->inf.capsule.r = (fabsf(v3_dot(tx,v0)) + fabsf(v3_dot(ty,v0))) * 0.25f;
 115       rp->inf.capsule.h = fabsf(v0[ major_axis ]);
 116
 117       /* orientation */
 118       v3_muls( tx, -1.0f, rp->collider_mtx[0] );
 119       v3_muls( v1, -1.0f, rp->collider_mtx[1] );
 120       v3_muls( ty, -1.0f, rp->collider_mtx[2] );
 121       v3_add( bone->hitbox[0], bone->hitbox[1], rp->collider_mtx[3] );
 122       v3_muls( rp->collider_mtx[3], 0.5f, rp->collider_mtx[3] );
 123
 124       rp->colour = 0xff000000 | (0xff << (major_axis*8));
 125
 126       rb_setbody_capsule( &rp->rb, rp->inf.capsule.r, rp->inf.capsule.h,
 127                            k_density, k_inertia_scale );
 128    }
 129    else{
 130       vg_warn( "type: %u\n", bone->collider );
 131       vg_fatal_error( "Invalid bone collider type" );
 132    }
 133
 134    m4x3_invert_affine( rp->collider_mtx, rp->inv_collider_mtx );
 135
 136    /* Position collider into rest */
 137    m3x3_q( rp->collider_mtx, rp->rb.q );
 138    v3_add( rp->collider_mtx[3], bone->co, rp->rb.co );
 139    v3_zero( rp->rb.v );
 140    v3_zero( rp->rb.w );
 141    rb_update_matrices( &rp->rb );
 142 }
 143
 144 /*
 145  * Get parent index in the ragdoll
 146  */
 147 u32 ragdoll_bone_parent( struct player_ragdoll *rd, u32 bone_id )
 148 {
 149    for( u32 j=0; j<rd->part_count; j++ )
 150       if( rd->parts[ j ].bone_id == bone_id )
 151          return j;
 152
 153    vg_fatal_error( "Referenced parent bone does not have a rigidbody" );
 154    return 0;
 155 }
 156
 157 /*
 158  * Setup ragdoll colliders from skeleton
 159  */
 160 void setup_ragdoll_from_skeleton( struct skeleton *sk,
 161                                   struct player_ragdoll *rd )
 162 {
 163    rd->part_count = 0;
 164
 165    if( !sk->collider_count )
 166       return;
 167
 168    rd->position_constraints_count = 0;
 169    rd->cone_constraints_count = 0;
 170
 171    for( u32 i=1; i<sk->bone_count; i ++ ){
 172       struct skeleton_bone *bone = &sk->bones[i];
 173
 174       /*
 175        * Bones with colliders
 176        */
 177       if( !(bone->collider) )
 178          continue;
 179
 180       if( rd->part_count > vg_list_size(rd->parts) )
 181          vg_fatal_error( "Playermodel has too many colliders" );
 182
 183       u32 part_id = rd->part_count;
 184       rd->part_count ++;
 185
 186       struct ragdoll_part *rp = &rd->parts[ part_id ];
 187       rp->bone_id = i;
 188       rp->parent = 0xffffffff;
 189
 190       player_init_ragdoll_bone_collider( bone, rp );
 191
 192       /*
 193        * Bones with collider and parent
 194        */
 195       if( !bone->parent )
 196          continue;
 197
 198       rp->parent = ragdoll_bone_parent( rd, bone->parent );
 199
 200       if( bone->orig_bone->flags & k_bone_flag_cone_constraint ){
 201          u32 conid = rd->position_constraints_count;
 202          rd->position_constraints_count ++;
 203
 204          struct rb_constr_pos *c = &rd->position_constraints[ conid ];
 205
 206          struct skeleton_bone *bj = &sk->bones[rp->bone_id];
 207          struct ragdoll_part  *pp = &rd->parts[rp->parent];
 208          struct skeleton_bone *bp = &sk->bones[pp->bone_id];
 209
 210          rd->constraint_associations[conid][0] = rp->parent;
 211          rd->constraint_associations[conid][1] = part_id;
 212
 213          /* Convention: rba -- parent, rbb -- child */
 214          c->rba = &pp->rb;
 215          c->rbb = &rp->rb;
 216
 217          v3f delta;
 218          v3_sub( bj->co, bp->co, delta );
 219          m4x3_mulv( rp->inv_collider_mtx, (v3f){0.0f,0.0f,0.0f}, c->lcb );
 220          m4x3_mulv( pp->inv_collider_mtx, delta, c->lca );
 221
 222
 223          mdl_bone *inf = bone->orig_bone;
 224
 225          struct rb_constr_swingtwist *a =
 226             &rd->cone_constraints[ rd->cone_constraints_count ++ ];
 227
 228          a->rba = &pp->rb;
 229          a->rbb = &rp->rb;
 230          a->conet = cosf( inf->conet )-0.0001f;
 231
 232          /* Store constraint in local space vectors */
 233          m3x3_mulv( c->rba->to_local, inf->conevx, a->conevx );
 234          m3x3_mulv( c->rba->to_local, inf->conevy, a->conevy );
 235          m3x3_mulv( c->rbb->to_local, inf->coneva, a->coneva );
 236          v3_copy( c->lca, a->view_offset );
 237
 238          v3_cross( inf->coneva, inf->conevy, a->conevxb );
 239          m3x3_mulv( c->rbb->to_local, a->conevxb, a->conevxb );
 240
 241          v3_normalize( a->conevxb );
 242          v3_normalize( a->conevx );
 243          v3_normalize( a->conevy );
 244          v3_normalize( a->coneva );
 245
 246          a->conevx[3] = v3_length( inf->conevx );
 247          a->conevy[3] = v3_length( inf->conevy );
 248
 249          rp->use_limits = 1;
 250       }
 251    }
 252 }
 253
 254 /*
 255  * Make avatar copy the ragdoll
 256  */
 257 void copy_ragdoll_pose_to_localplayer( struct player_ragdoll *rd )
 258 {
 259    for( int i=0; i<rd->part_count; i++ ){
 260       struct ragdoll_part *part = &rd->parts[i];
 261
 262       m4x3f mtx;
 263
 264       v4f q_int;
 265       v3f co_int;
 266
 267       float substep = vg.time_fixed_extrapolate;
 268       v3_lerp( part->prev_co, part->rb.co, substep, co_int );
 269       q_nlerp( part->prev_q, part->rb.q, substep, q_int );
 270
 271       q_m3x3( q_int, mtx );
 272       v3_copy( co_int, mtx[3] );
 273
 274       m4x3_mul( mtx, part->inv_collider_mtx,
 275                 localplayer.final_mtx[part->bone_id] );
 276    }
 277
 278    for( u32 i=1; i<localplayer.skeleton.bone_count; i++ ){
 279       struct skeleton_bone *sb = &localplayer.skeleton.bones[i];
 280
 281       if( sb->parent && !sb->collider ){
 282          v3f delta;
 283          v3_sub( localplayer.skeleton.bones[i].co,
 284                  localplayer.skeleton.bones[sb->parent].co, delta );
 285
 286          m4x3f posemtx;
 287          m3x3_identity( posemtx );
 288          v3_copy( delta, posemtx[3] );
 289
 290          /* final matrix */
 291          m4x3_mul( localplayer.final_mtx[sb->parent], posemtx,
 292                    localplayer.final_mtx[i] );
 293       }
 294    }
 295
 296    skeleton_apply_inverses( &localplayer.skeleton, localplayer.final_mtx );
 297 }
 298
 299 /*
 300  * Make the ragdoll copy the player model
 301  */
 302 void copy_localplayer_to_ragdoll( struct player_ragdoll *rd,
 303                                   enum player_die_type type )
 304 {
 305    v3f centroid;
 306
 307    v3f *bone_mtx = localplayer.final_mtx[localplayer.id_hip];
 308    m4x3_mulv( bone_mtx,
 309               localplayer.skeleton.bones[localplayer.id_hip].co, centroid );
 310
 311    for( int i=0; i<rd->part_count; i++ ){
 312       struct ragdoll_part *part = &rd->parts[i];
 313
 314       v3f pos, offset;
 315       u32 bone = part->bone_id;
 316
 317       v3f *bone_mtx = localplayer.final_mtx[bone];
 318
 319       m4x3_mulv( bone_mtx, localplayer.skeleton.bones[bone].co, pos );
 320       m3x3_mulv( bone_mtx, part->collider_mtx[3], offset );
 321       v3_add( pos, offset, part->rb.co );
 322
 323       m3x3f r;
 324       m3x3_mul( bone_mtx, part->collider_mtx, r );
 325       m3x3_q( r, part->rb.q );
 326
 327       v3f ra, v;
 328       v3_sub( part->rb.co, centroid, ra );
 329       v3_cross( localplayer.rb.w, ra, v );
 330       v3_add( localplayer.rb.v, v, part->rb.v );
 331
 332       if( type == k_player_die_type_feet ){
 333          if( (bone == localplayer.id_foot_l) ||
 334              (bone == localplayer.id_foot_r) ){
 335             v3_zero( part->rb.v );
 336          }
 337       }
 338
 339       v3_copy( localplayer.rb.w, part->rb.w );
 340
 341       v3_copy( part->rb.co, part->prev_co );
 342       v4_copy( part->rb.q, part->prev_q );
 343
 344       rb_update_matrices( &part->rb );
 345    }
 346 }
 347
 348 /*
 349  * Ragdoll physics step
 350  */
 351 void player_ragdoll_iter( struct player_ragdoll *rd )
 352 {
 353    world_instance *world = world_current_instance();
 354
 355    int run_sim = 0;
 356    ragdoll_frame ++;
 357
 358    if( ragdoll_frame >= k_ragdoll_div ){
 359       ragdoll_frame = 0;
 360       run_sim = 1;
 361    }
 362
 363    rb_solver_reset();
 364
 365    float contact_velocities[256];
 366
 367    rigidbody _null = {0};
 368    _null.inv_mass = 0.0f;
 369    m3x3_zero( _null.iI );
 370
 371    for( int i=0; i<rd->part_count; i ++ ){
 372       v4_copy( rd->parts[i].rb.q, rd->parts[i].prev_q );
 373       v3_copy( rd->parts[i].rb.co, rd->parts[i].prev_co );
 374
 375       if( rb_global_has_space() ){
 376          rb_ct *buf = rb_global_buffer();
 377
 378          int l;
 379
 380          if( rd->parts[i].type == k_bone_collider_capsule ){
 381             l = rb_capsule__scene( rd->parts[i].rb.to_world,
 382                                    &rd->parts[i].inf.capsule,
 383                                    NULL, world->geo_bh, buf,
 384                                    k_material_flag_ghosts );
 385          }
 386          else if( rd->parts[i].type == k_bone_collider_box ){
 387             l = rb_box__scene( rd->parts[i].rb.to_world,
 388                                rd->parts[i].inf.box,
 389                                NULL, world->geo_bh, buf,
 390                                k_material_flag_ghosts );
 391          }
 392          else continue;
 393
 394          for( int j=0; j<l; j++ ){
 395             buf[j].rba = &rd->parts[i].rb;
 396             buf[j].rbb = &_null;
 397          }
 398
 399          rb_contact_count += l;
 400       }
 401    }
 402
 403    /*
 404     * self-collision
 405     */
 406    for( int i=0; i<rd->part_count-1; i ++ ){
 407       for( int j=i+1; j<rd->part_count; j ++ ){
 408          if( rd->parts[j].parent != i ){
 409             if( !rb_global_has_space() )
 410                break;
 411
 412             if( rd->parts[j].type != k_bone_collider_capsule )
 413                continue;
 414
 415             if( rd->parts[i].type != k_bone_collider_capsule )
 416                continue;
 417
 418             rb_ct *buf = rb_global_buffer();
 419
 420             int l = rb_capsule__capsule( rd->parts[i].rb.to_world,
 421                                          &rd->parts[i].inf.capsule,
 422                                          rd->parts[j].rb.to_world,
 423                                          &rd->parts[j].inf.capsule,
 424                                          buf );
 425
 426             for( int k=0; k<l; k++ ){
 427                buf[k].rba = &rd->parts[i].rb;
 428                buf[k].rbb = &rd->parts[j].rb;
 429             }
 430
 431             rb_contact_count += l;
 432          }
 433       }
 434    }
 435
 436    if( localplayer.drowned )
 437    {
 438       for( int j=0; j<rd->part_count; j++ )
 439       {
 440          struct ragdoll_part *pj = &rd->parts[j];
 441
 442          if( run_sim )
 443          {
 444             rb_effect_simple_bouyency( &pj->rb, world->water.plane,
 445                                         k_ragdoll_floatyiness,
 446                                         k_ragdoll_floatydrag );
 447          }
 448       }
 449    }
 450
 451    /*
 452     * PRESOLVE
 453     */
 454    for( u32 i=0; i<rb_contact_count; i++ ){
 455       rb_ct *ct = &rb_contact_buffer[i];
 456
 457       v3f rv, ra, rb;
 458       v3_sub( ct->co, ct->rba->co, ra );
 459       v3_sub( ct->co, ct->rbb->co, rb );
 460       rb_rcv( ct->rba, ct->rbb, ra, rb, rv );
 461       float     vn = v3_dot( rv, ct->n );
 462
 463       contact_velocities[i] = vn;
 464    }
 465
 466    rb_presolve_contacts( rb_contact_buffer, vg.time_fixed_delta,
 467                          rb_contact_count );
 468    rb_presolve_swingtwist_constraints( rd->cone_constraints,
 469                                        rd->cone_constraints_count );
 470
 471    /*
 472     * DEBUG
 473     */
 474    if( k_ragdoll_debug_collider ){
 475       for( u32 i=0; i<rd->part_count; i ++ ){
 476          struct ragdoll_part *rp = &rd->parts[i];
 477
 478          if( rp->type == k_bone_collider_capsule ){
 479             vg_line_capsule( rp->rb.to_world,
 480                              rp->inf.capsule.r, rp->inf.capsule.h, rp->colour );
 481          }
 482          else if( rp->type == k_bone_collider_box ){
 483             vg_line_boxf_transformed( rp->rb.to_world,
 484                                       rp->inf.box, rp->colour );
 485          }
 486       }
 487    }
 488
 489    if( k_ragdoll_debug_constraints ){
 490       rb_debug_position_constraints( rd->position_constraints,
 491                                      rd->position_constraints_count );
 492
 493       rb_debug_swingtwist_constraints( rd->cone_constraints,
 494                                        rd->cone_constraints_count );
 495    }
 496
 497    /*
 498     * SOLVE CONSTRAINTS  & Integrate
 499     */
 500    if( run_sim ){
 501       /* the solver is not very quickly converging so... */
 502       for( int i=0; i<40; i++ ){
 503          if( i<20 ){
 504             rb_solve_contacts( rb_contact_buffer, rb_contact_count );
 505             rb_solve_swingtwist_constraints( rd->cone_constraints,
 506                                              rd->cone_constraints_count );
 507             rb_postsolve_swingtwist_constraints( rd->cone_constraints,
 508                                                  rd->cone_constraints_count );
 509          }
 510          rb_solve_position_constraints( rd->position_constraints,
 511                                         rd->position_constraints_count );
 512       }
 513
 514       rb_correct_position_constraints( rd->position_constraints,
 515                                        rd->position_constraints_count,
 516                                        k_ragdoll_correction * 0.5f );
 517       rb_correct_swingtwist_constraints( rd->cone_constraints,
 518                                          rd->cone_constraints_count,
 519                                          k_ragdoll_correction * 0.25f );
 520
 521       for( int i=0; i<rd->part_count; i++ ){
 522          rb_iter( &rd->parts[i].rb );
 523
 524          v3f w;
 525          v3_copy( rd->parts[i].rb.w, w );
 526          if( v3_length2( w ) > 0.00001f ){
 527             v3_normalize( w );
 528             v3_muladds( rd->parts[i].rb.w, w, -k_ragdoll_angular_drag,
 529                         rd->parts[i].rb.w );
 530          }
 531       }
 532
 533       for( int i=0; i<rd->part_count; i++ )
 534          rb_update_matrices( &rd->parts[i].rb );
 535    }
 536
 537    rb_ct *stress = NULL;
 538    float max_stress = 1.0f;
 539
 540    for( u32 i=0; i<rb_contact_count; i++ ){
 541       rb_ct *ct = &rb_contact_buffer[i];
 542
 543       v3f rv, ra, rb;
 544       v3_sub( ct->co, ct->rba->co, ra );
 545       v3_sub( ct->co, ct->rbb->co, rb );
 546       rb_rcv( ct->rba, ct->rbb, ra, rb, rv );
 547       float vn = v3_dot( rv, ct->n );
 548
 549       float s = fabsf(vn - contact_velocities[i]);
 550       if( s > max_stress ){
 551          stress = ct;
 552          max_stress = s;
 553       }
 554    }
 555
 556    static u32 temp_filter = 0;
 557
 558    /*
 559     * motorized joints
 560     */
 561    if( run_sim &&
 562          (v3_length2(player_dead.v_lpf)>(k_ragdoll_active_threshold*
 563                                          k_ragdoll_active_threshold)) ){
 564       mdl_keyframe anim[32];
 565       skeleton_sample_anim( &localplayer.skeleton, player_dead.anim_bail,
 566                             0.0f, anim );
 567
 568       for( u32 i=0; i<rd->cone_constraints_count; i ++ ){
 569          rb_constr_swingtwist *st = &rd->cone_constraints[i];
 570          rb_constr_pos *pc = &rd->position_constraints[i];
 571
 572          v3f va, vap;
 573
 574          m3x3_mulv( st->rbb->to_world, st->coneva, va );
 575
 576          /* calculate va as seen in rest position, from the perspective of the
 577           * parent object, mapped to pose world space using the parents
 578           * transform. thats our target */
 579
 580          u32 id_p = rd->constraint_associations[i][0],
 581              id_a = rd->constraint_associations[i][1];
 582
 583          struct ragdoll_part *pa = &rd->parts[ id_a ],
 584                              *pp = &rd->parts[ id_p ];
 585
 586          mdl_keyframe *kf = &anim[ pa->bone_id-1 ];
 587          m3x3_mulv( pa->collider_mtx, st->coneva, vap );
 588          q_mulv( kf->q, vap, vap );
 589
 590          /* This could be a transfer function */
 591          m3x3_mulv( pp->inv_collider_mtx, vap, vap );
 592          m3x3_mulv( st->rba->to_world, vap, vap );
 593
 594          f32 d = v3_dot( vap, va ),
 595              a = acosf( vg_clampf( d, -1.0f, 1.0f ) );
 596
 597          v3f axis;
 598          v3_cross( vap, va, axis );
 599
 600          f32 Fs = -a * k_ragdoll_spring,
 601              Fd = -v3_dot( st->rbb->w, axis ) * k_ragdoll_dampening,
 602              F  = Fs+Fd;
 603
 604          v3f torque;
 605          v3_muls( axis, F, torque );
 606          v3_muladds( st->rbb->w, torque, vg.time_fixed_delta, st->rbb->w );
 607
 608          /* apply a adjustment to keep velocity at joint 0 */
 609 #if 0
 610          v3f wcb, vcb;
 611          m3x3_mulv( st->rbb->to_world, pc->lcb, wcb );
 612          v3_cross( torque, wcb, vcb );
 613          v3_muladds( st->rbb->v, vcb, vg.time_fixed_delta, st->rbb->v );
 614 #endif
 615       }
 616    }
 617
 618    if( temp_filter ){
 619       temp_filter --;
 620       return;
 621    }
 622
 623    if( stress ){
 624       temp_filter = 20;
 625       audio_lock();
 626       audio_oneshot_3d( &audio_hits[vg_randu32(&vg.rand)%5],
 627                         stress->co, 20.0f, 1.0f );
 628       audio_unlock();
 629    }
 630 }