player_ragdoll.c

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