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_limit_scale );
  44    VG_VAR_I32( k_ragdoll_div );
  45    VG_VAR_I32( k_ragdoll_debug_collider );
  46    VG_VAR_I32( k_ragdoll_debug_constraints );
  47    vg_console_reg_cmd( "ragdoll", dev_ragdoll_saveload, NULL );
  48 }
  49
  50 static void player_init_ragdoll_bone_collider( struct skeleton_bone *bone,
  51                                                   struct ragdoll_part *rp )
  52 {
  53    m4x3_identity( rp->collider_mtx );
  54
  55    if( bone->collider == k_bone_collider_box ){
  56       v3f delta;
  57       v3_sub( bone->hitbox[1], bone->hitbox[0], delta );
  58       v3_muls( delta, 0.5f, delta );
  59       v3_add( bone->hitbox[0], delta, rp->collider_mtx[3] );
  60
  61       v3_copy( delta, rp->obj.rb.bbx[1] );
  62       v3_muls( delta, -1.0f, rp->obj.rb.bbx[0] );
  63
  64       q_identity( rp->obj.rb.q );
  65       rp->obj.type = k_rb_shape_box;
  66       rp->colour = 0xffcccccc;
  67    }
  68    else if( bone->collider == k_bone_collider_capsule ){
  69       v3f v0, v1, tx, ty;
  70       v3_sub( bone->hitbox[1], bone->hitbox[0], v0 );
  71
  72       int major_axis = 0;
  73       float largest = -1.0f;
  74
  75       for( int i=0; i<3; i ++ ){
  76          if( fabsf( v0[i] ) > largest ){
  77             largest = fabsf( v0[i] );
  78             major_axis = i;
  79          }
  80       }
  81
  82       v3_zero( v1 );
  83       v1[ major_axis ] = 1.0f;
  84       v3_tangent_basis( v1, tx, ty );
  85
  86       float r = (fabsf(v3_dot(tx,v0)) + fabsf(v3_dot(ty,v0))) * 0.25f,
  87             l = fabsf(v0[ major_axis ]);
  88
  89       /* orientation */
  90       v3_muls( tx, -1.0f, rp->collider_mtx[0] );
  91       v3_muls( v1, -1.0f, rp->collider_mtx[1] );
  92       v3_muls( ty, -1.0f, rp->collider_mtx[2] );
  93       v3_add( bone->hitbox[0], bone->hitbox[1], rp->collider_mtx[3] );
  94       v3_muls( rp->collider_mtx[3], 0.5f, rp->collider_mtx[3] );
  95
  96       rp->obj.type = k_rb_shape_capsule;
  97       rp->obj.inf.capsule.height = l;
  98       rp->obj.inf.capsule.radius = r;
  99
 100       rp->colour = 0xff000000 | (0xff << (major_axis*8));
 101    }
 102    else{
 103       vg_warn( "type: %u\n", bone->collider );
 104       vg_fatal_error( "Invalid bone collider type" );
 105    }
 106
 107    m4x3_invert_affine( rp->collider_mtx, rp->inv_collider_mtx );
 108
 109    /* Position collider into rest */
 110    m3x3_q( rp->collider_mtx, rp->obj.rb.q );
 111    v3_add( rp->collider_mtx[3], bone->co, rp->obj.rb.co );
 112    v3_zero( rp->obj.rb.v );
 113    v3_zero( rp->obj.rb.w );
 114    rb_init_object( &rp->obj );
 115 }
 116
 117 /*
 118  * Get parent index in the ragdoll
 119  */
 120 static u32 ragdoll_bone_parent( struct player_ragdoll *rd, u32 bone_id ){
 121    for( u32 j=0; j<rd->part_count; j++ )
 122       if( rd->parts[ j ].bone_id == bone_id )
 123          return j;
 124
 125    vg_fatal_error( "Referenced parent bone does not have a rigidbody" );
 126    return 0;
 127 }
 128
 129 /*
 130  * Setup ragdoll colliders from skeleton
 131  */
 132 static void setup_ragdoll_from_skeleton( struct skeleton *sk,
 133                                          struct player_ragdoll *rd ){
 134    rd->part_count = 0;
 135
 136    if( !sk->collider_count )
 137       return;
 138
 139    rd->position_constraints_count = 0;
 140    rd->cone_constraints_count = 0;
 141
 142    for( u32 i=1; i<sk->bone_count; i ++ ){
 143       struct skeleton_bone *bone = &sk->bones[i];
 144
 145       /*
 146        * Bones with colliders
 147        */
 148       if( !(bone->collider) )
 149          continue;
 150
 151       if( rd->part_count > vg_list_size(rd->parts) )
 152          vg_fatal_error( "Playermodel has too many colliders" );
 153
 154       struct ragdoll_part *rp = &rd->parts[ rd->part_count ++ ];
 155       rp->bone_id = i;
 156       rp->parent = 0xffffffff;
 157
 158       player_init_ragdoll_bone_collider( bone, rp );
 159
 160       /*
 161        * Bones with collider and parent
 162        */
 163       if( !bone->parent )
 164          continue;
 165
 166       rp->parent = ragdoll_bone_parent( rd, bone->parent );
 167
 168       if( bone->orig_bone->flags & k_bone_flag_cone_constraint ){
 169          struct rb_constr_pos *c =
 170             &rd->position_constraints[ rd->position_constraints_count ++ ];
 171
 172          struct skeleton_bone *bj = &sk->bones[rp->bone_id];
 173          struct ragdoll_part  *pp = &rd->parts[rp->parent];
 174          struct skeleton_bone *bp = &sk->bones[pp->bone_id];
 175
 176          /* Convention: rba -- parent, rbb -- child */
 177          c->rba = &pp->obj.rb;
 178          c->rbb = &rp->obj.rb;
 179
 180          v3f delta;
 181          v3_sub( bj->co, bp->co, delta );
 182          m4x3_mulv( rp->inv_collider_mtx, (v3f){0.0f,0.0f,0.0f}, c->lcb );
 183          m4x3_mulv( pp->inv_collider_mtx, delta, c->lca );
 184
 185
 186          mdl_bone *inf = bone->orig_bone;
 187
 188          struct rb_constr_swingtwist *a =
 189             &rd->cone_constraints[ rd->cone_constraints_count ++ ];
 190
 191          a->rba = &pp->obj.rb;
 192          a->rbb = &rp->obj.rb;
 193          a->conet = cosf( inf->conet )-0.0001f;
 194
 195          /* Store constraint in local space vectors */
 196          m3x3_mulv( c->rba->to_local, inf->conevx, a->conevx );
 197          m3x3_mulv( c->rba->to_local, inf->conevy, a->conevy );
 198          m3x3_mulv( c->rbb->to_local, inf->coneva, a->coneva );
 199          v3_copy( c->lca, a->view_offset );
 200
 201          v3_cross( inf->coneva, inf->conevy, a->conevxb );
 202          m3x3_mulv( c->rbb->to_local, a->conevxb, a->conevxb );
 203
 204          v3_normalize( a->conevxb );
 205          v3_normalize( a->conevx );
 206          v3_normalize( a->conevy );
 207          v3_normalize( a->coneva );
 208
 209          a->conevx[3] = v3_length( inf->conevx );
 210          a->conevy[3] = v3_length( inf->conevy );
 211
 212          rp->use_limits = 1;
 213       }
 214    }
 215 }
 216
 217 /*
 218  * Make avatar copy the ragdoll
 219  */
 220 static void copy_ragdoll_pose_to_localplayer( struct player_ragdoll *rd ){
 221    for( int i=0; i<rd->part_count; i++ ){
 222       struct ragdoll_part *part = &rd->parts[i];
 223
 224       m4x3f mtx;
 225
 226       v4f q_int;
 227       v3f co_int;
 228
 229       float substep = vg.time_fixed_extrapolate;
 230       v3_lerp( part->prev_co, part->obj.rb.co, substep, co_int );
 231       q_nlerp( part->prev_q, part->obj.rb.q, substep, q_int );
 232
 233       q_m3x3( q_int, mtx );
 234       v3_copy( co_int, mtx[3] );
 235
 236       m4x3_mul( mtx, part->inv_collider_mtx,
 237                 localplayer.final_mtx[part->bone_id] );
 238    }
 239
 240    for( u32 i=1; i<localplayer.skeleton.bone_count; i++ ){
 241       struct skeleton_bone *sb = &localplayer.skeleton.bones[i];
 242
 243       if( sb->parent && !sb->collider ){
 244          v3f delta;
 245          v3_sub( localplayer.skeleton.bones[i].co,
 246                  localplayer.skeleton.bones[sb->parent].co, delta );
 247
 248          m4x3f posemtx;
 249          m3x3_identity( posemtx );
 250          v3_copy( delta, posemtx[3] );
 251
 252          /* final matrix */
 253          m4x3_mul( localplayer.final_mtx[sb->parent], posemtx,
 254                    localplayer.final_mtx[i] );
 255       }
 256    }
 257
 258    skeleton_apply_inverses( &localplayer.skeleton, localplayer.final_mtx );
 259 }
 260
 261 /*
 262  * Make the ragdoll copy the player model
 263  */
 264 static void copy_localplayer_to_ragdoll( struct player_ragdoll *rd,
 265                                          enum player_die_type type ){
 266    v3f centroid;
 267
 268    v3f *bone_mtx = localplayer.final_mtx[localplayer.id_hip];
 269    m4x3_mulv( bone_mtx,
 270               localplayer.skeleton.bones[localplayer.id_hip].co, centroid );
 271
 272    for( int i=0; i<rd->part_count; i++ ){
 273       struct ragdoll_part *part = &rd->parts[i];
 274
 275       v3f pos, offset;
 276       u32 bone = part->bone_id;
 277
 278       v3f *bone_mtx = localplayer.final_mtx[bone];
 279
 280       m4x3_mulv( bone_mtx, localplayer.skeleton.bones[bone].co, pos );
 281       m3x3_mulv( bone_mtx, part->collider_mtx[3], offset );
 282       v3_add( pos, offset, part->obj.rb.co );
 283
 284       m3x3f r;
 285       m3x3_mul( bone_mtx, part->collider_mtx, r );
 286       m3x3_q( r, part->obj.rb.q );
 287
 288       v3f ra, v;
 289       v3_sub( part->obj.rb.co, centroid, ra );
 290       v3_cross( localplayer.rb.w, ra, v );
 291       v3_add( localplayer.rb.v, v, part->obj.rb.v );
 292
 293       if( type == k_player_die_type_feet ){
 294          if( (bone == localplayer.id_foot_l) ||
 295              (bone == localplayer.id_foot_r) ){
 296             v3_zero( part->obj.rb.v );
 297          }
 298       }
 299
 300       v3_copy( localplayer.rb.w, part->obj.rb.w );
 301
 302       v3_copy( part->obj.rb.co, part->prev_co );
 303       v4_copy( part->obj.rb.q, part->prev_q );
 304
 305       rb_update_transform( &part->obj.rb );
 306    }
 307 }
 308
 309 /*
 310  * Ragdoll physics step
 311  */
 312 static void player_ragdoll_iter( struct player_ragdoll *rd ){
 313    world_instance *world = world_current_instance();
 314
 315    int run_sim = 0;
 316    ragdoll_frame ++;
 317
 318    if( ragdoll_frame >= k_ragdoll_div ){
 319       ragdoll_frame = 0;
 320       run_sim = 1;
 321    }
 322
 323    rb_solver_reset();
 324
 325    float contact_velocities[256];
 326
 327    for( int i=0; i<rd->part_count; i ++ ){
 328       v4_copy( rd->parts[i].obj.rb.q, rd->parts[i].prev_q );
 329       v3_copy( rd->parts[i].obj.rb.co, rd->parts[i].prev_co );
 330
 331       if( rb_global_has_space() ){
 332          rb_ct *buf = rb_global_buffer();
 333
 334          int l;
 335
 336          if( rd->parts[i].obj.type == k_rb_shape_capsule ){
 337             l = rb_capsule__scene( rd->parts[i].obj.rb.to_world,
 338                                    &rd->parts[i].obj.inf.capsule,
 339                                    NULL, &world->rb_geo.inf.scene, buf,
 340                                    k_material_flag_ghosts );
 341          }
 342          else if( rd->parts[i].obj.type == k_rb_shape_box ){
 343             l = rb_box__scene( rd->parts[i].obj.rb.to_world,
 344                                rd->parts[i].obj.rb.bbx,
 345                                NULL, &world->rb_geo.inf.scene, buf,
 346                                k_material_flag_ghosts );
 347          }
 348          else continue;
 349
 350          for( int j=0; j<l; j++ ){
 351             buf[j].rba = &rd->parts[i].obj.rb;
 352             buf[j].rbb = &world->rb_geo.rb;
 353          }
 354
 355          rb_contact_count += l;
 356       }
 357    }
 358
 359    /*
 360     * self-collision
 361     */
 362    for( int i=0; i<rd->part_count-1; i ++ ){
 363       for( int j=i+1; j<rd->part_count; j ++ ){
 364          if( rd->parts[j].parent != i ){
 365             if( !rb_global_has_space() )
 366                break;
 367
 368             if( rd->parts[j].obj.type != k_rb_shape_capsule )
 369                continue;
 370
 371             if( rd->parts[i].obj.type != k_rb_shape_capsule )
 372                continue;
 373
 374             rb_ct *buf = rb_global_buffer();
 375
 376             int l = rb_capsule__capsule( rd->parts[i].obj.rb.to_world,
 377                                          &rd->parts[i].obj.inf.capsule,
 378                                          rd->parts[j].obj.rb.to_world,
 379                                          &rd->parts[j].obj.inf.capsule,
 380                                          buf );
 381
 382             for( int k=0; k<l; k++ ){
 383                buf[k].rba = &rd->parts[i].obj.rb;
 384                buf[k].rbb = &rd->parts[j].obj.rb;
 385             }
 386
 387             rb_contact_count += l;
 388          }
 389       }
 390    }
 391
 392    if( world->water.enabled ){
 393       for( int j=0; j<rd->part_count; j++ ){
 394          struct ragdoll_part *pj = &rd->parts[j];
 395
 396          if( run_sim ){
 397             rb_effect_simple_bouyency( &pj->obj.rb, world->water.plane,
 398                                         k_ragdoll_floatyiness,
 399                                         k_ragdoll_floatydrag );
 400          }
 401       }
 402    }
 403
 404    /*
 405     * PRESOLVE
 406     */
 407    for( u32 i=0; i<rb_contact_count; i++ ){
 408       rb_ct *ct = &rb_contact_buffer[i];
 409
 410       v3f rv, ra, rb;
 411       v3_sub( ct->co, ct->rba->co, ra );
 412       v3_sub( ct->co, ct->rbb->co, rb );
 413       rb_rcv( ct->rba, ct->rbb, ra, rb, rv );
 414       float     vn = v3_dot( rv, ct->n );
 415
 416       contact_velocities[i] = vn;
 417    }
 418
 419    rb_presolve_contacts( rb_contact_buffer, rb_contact_count );
 420    rb_presolve_swingtwist_constraints( rd->cone_constraints,
 421                                        rd->cone_constraints_count );
 422
 423    /*
 424     * DEBUG
 425     */
 426    if( k_ragdoll_debug_collider ){
 427       for( u32 i=0; i<rd->part_count; i ++ )
 428          rb_object_debug( &rd->parts[i].obj, rd->parts[i].colour );
 429    }
 430
 431    if( k_ragdoll_debug_constraints ){
 432       rb_debug_position_constraints( rd->position_constraints,
 433                                      rd->position_constraints_count );
 434
 435       rb_debug_swingtwist_constraints( rd->cone_constraints,
 436                                        rd->cone_constraints_count );
 437    }
 438
 439    /*
 440     * SOLVE CONSTRAINTS
 441     */
 442    if( run_sim ){
 443       for( int i=0; i<16; i++ ){
 444          rb_solve_contacts( rb_contact_buffer, rb_contact_count );
 445          rb_solve_swingtwist_constraints( rd->cone_constraints,
 446                                           rd->cone_constraints_count );
 447          rb_solve_position_constraints( rd->position_constraints,
 448                                         rd->position_constraints_count );
 449       }
 450
 451       for( int i=0; i<rd->part_count; i++ )
 452          rb_iter( &rd->parts[i].obj.rb );
 453
 454       for( int i=0; i<rd->part_count; i++ )
 455          rb_update_transform( &rd->parts[i].obj.rb );
 456
 457       rb_correct_swingtwist_constraints( rd->cone_constraints,
 458                                          rd->cone_constraints_count, 0.125f );
 459
 460       rb_correct_position_constraints( rd->position_constraints,
 461                                        rd->position_constraints_count, 0.25f );
 462    }
 463
 464    rb_ct *stress = NULL;
 465    float max_stress = 1.0f;
 466
 467    for( u32 i=0; i<rb_contact_count; i++ ){
 468       rb_ct *ct = &rb_contact_buffer[i];
 469
 470       v3f rv, ra, rb;
 471       v3_sub( ct->co, ct->rba->co, ra );
 472       v3_sub( ct->co, ct->rbb->co, rb );
 473       rb_rcv( ct->rba, ct->rbb, ra, rb, rv );
 474       float vn = v3_dot( rv, ct->n );
 475
 476       float s = fabsf(vn - contact_velocities[i]);
 477       if( s > max_stress ){
 478          stress = ct;
 479          max_stress = s;
 480       }
 481    }
 482
 483    static u32 temp_filter = 0;
 484
 485    if( temp_filter ){
 486       temp_filter --;
 487       return;
 488    }
 489
 490    if( stress ){
 491       temp_filter = 20;
 492       audio_lock();
 493       audio_oneshot_3d( &audio_hits[vg_randu32(&vg.rand)%5],
 494                         stress->co, 20.0f, 1.0f );
 495       audio_unlock();
 496    }
 497 }
 498
 499 #endif /* PLAYER_RAGDOLL_C */