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