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