player_ragdoll.h

   1 #ifndef PLAYER_RAGDOLL_H
   2 #define PLAYER_RAGDOLL_H
   3
   4 #define VG_GAME
   5 #include "vg/vg.h"
   6 #include "skeleton.h"
   7 #include "rigidbody.h"
   8 #include "player_model.h"
   9 #include "world.h"
  10 #include "audio.h"
  11
  12 struct player_ragdoll{
  13    struct ragdoll_part{
  14       u32 bone_id;
  15
  16       /* Collider transform relative to bone */
  17       m4x3f collider_mtx,
  18             inv_collider_mtx;
  19
  20       v4f prev_q;
  21       v3f prev_co;
  22
  23       u32 use_limits;
  24       v3f limits[2];
  25
  26       rb_object obj;
  27       u32 parent;
  28       u32 colour;
  29    }
  30    parts[32];
  31    u32 part_count;
  32
  33    rb_constr_pos  position_constraints[32];
  34    u32            position_constraints_count;
  35
  36    rb_constr_swingtwist cone_constraints[32];
  37    u32                  cone_constraints_count;
  38
  39    int shoes[2];
  40 };
  41
  42 VG_STATIC float k_ragdoll_floatyiness = 20.0f,
  43                 k_ragdoll_floatydrag  = 1.0f,
  44                 k_ragdoll_limit_scale = 1.0f;
  45
  46 VG_STATIC int   k_ragdoll_div = 1,
  47                 ragdoll_frame = 0,
  48                 k_ragdoll_debug_collider = 1,
  49                 k_ragdoll_debug_constraints = 0;
  50
  51 VG_STATIC void player_ragdoll_init(void)
  52 {
  53    VG_VAR_F32( k_ragdoll_limit_scale );
  54    VG_VAR_I32( k_ragdoll_div );
  55    VG_VAR_I32( k_ragdoll_debug_collider );
  56    VG_VAR_I32( k_ragdoll_debug_constraints );
  57 }
  58
  59 VG_STATIC void player_init_ragdoll_bone_collider( struct skeleton_bone *bone,
  60                                                   struct ragdoll_part *rp )
  61 {
  62    m4x3_identity( rp->collider_mtx );
  63
  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_copy( delta, rp->obj.rb.bbx[1] );
  71       v3_muls( delta, -1.0f, rp->obj.rb.bbx[0] );
  72
  73       q_identity( rp->obj.rb.q );
  74       rp->obj.type = k_rb_shape_box;
  75       rp->colour = 0xffcccccc;
  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       rb_tangent_basis( v1, tx, ty );
  94
  95       float r = (fabsf(v3_dot(tx,v0)) + fabsf(v3_dot(ty,v0))) * 0.25f,
  96             l = 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->obj.type = k_rb_shape_capsule;
 106       rp->obj.inf.capsule.height = l;
 107       rp->obj.inf.capsule.radius = r;
 108
 109       rp->colour = 0xff000000 | (0xff << (major_axis*8));
 110    }
 111    else{
 112       vg_warn( "type: %u\n", bone->collider );
 113       vg_fatal_exit_loop( "Invalid bone collider type" );
 114    }
 115
 116    m4x3_invert_affine( rp->collider_mtx, rp->inv_collider_mtx );
 117
 118    /* Position collider into rest */
 119    m3x3_q( rp->collider_mtx, rp->obj.rb.q );
 120    v3_add( rp->collider_mtx[3], bone->co, rp->obj.rb.co );
 121    v3_zero( rp->obj.rb.v );
 122    v3_zero( rp->obj.rb.w );
 123    rb_init_object( &rp->obj );
 124 }
 125
 126 /*
 127  * Get parent index in the ragdoll
 128  */
 129 VG_STATIC u32 ragdoll_bone_parent( struct player_ragdoll *rd,
 130                                    struct player_avatar *av, u32 bone_id )
 131 {
 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_exit_loop( "Referenced parent bone does not have a rigidbody" );
 137    return 0;
 138 }
 139
 140 /*
 141  * Setup ragdoll colliders
 142  */
 143 VG_STATIC void player_setup_ragdoll_from_avatar( struct player_ragdoll *rd,
 144                                                  struct player_avatar *av )
 145 {
 146    rd->part_count = 0;
 147
 148    if( !av->sk.collider_count )
 149       return;
 150
 151    rd->position_constraints_count = 0;
 152    rd->cone_constraints_count = 0;
 153
 154    for( u32 i=1; i<av->sk.bone_count; i ++ ){
 155       struct skeleton_bone *bone = &av->sk.bones[i];
 156
 157       /*
 158        * Bones with colliders
 159        */
 160       if( !(bone->collider) )
 161          continue;
 162
 163       if( rd->part_count > vg_list_size(rd->parts) )
 164          vg_fatal_exit_loop( "Playermodel has too many colliders" );
 165
 166       struct ragdoll_part *rp = &rd->parts[ rd->part_count ++ ];
 167       rp->bone_id = i;
 168       rp->parent = 0xffffffff;
 169
 170       player_init_ragdoll_bone_collider( bone, rp );
 171
 172       /*
 173        * Bones with collider and parent
 174        */
 175       if( !bone->parent )
 176          continue;
 177
 178       rp->parent = ragdoll_bone_parent( rd, av, bone->parent );
 179
 180
 181       if( bone->orig_bone->flags & k_bone_flag_cone_constraint ){
 182          struct rb_constr_pos *c =
 183             &rd->position_constraints[ rd->position_constraints_count ++ ];
 184
 185          struct skeleton_bone *bj = &av->sk.bones[rp->bone_id];
 186          struct ragdoll_part  *pp = &rd->parts[rp->parent];
 187          struct skeleton_bone *bp = &av->sk.bones[pp->bone_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 VG_STATIC void copy_ragdoll_pose_to_avatar( struct player_ragdoll *rd,
 234                                             struct player_avatar *av )
 235 {
 236    for( int i=0; i<rd->part_count; i++ ){
 237       struct ragdoll_part *part = &rd->parts[i];
 238
 239       m4x3f mtx;
 240
 241       v4f q_int;
 242       v3f co_int;
 243
 244       float substep = vg.time_fixed_extrapolate;
 245       v3_lerp( part->prev_co, part->obj.rb.co, substep, co_int );
 246       q_nlerp( part->prev_q, part->obj.rb.q, substep, q_int );
 247
 248       q_m3x3( q_int, mtx );
 249       v3_copy( co_int, mtx[3] );
 250
 251       m4x3_mul( mtx, part->inv_collider_mtx, av->sk.final_mtx[part->bone_id] );
 252    }
 253
 254    for( u32 i=1; i<av->sk.bone_count; i++ ){
 255       struct skeleton_bone *sb = &av->sk.bones[i];
 256
 257       if( sb->parent && !sb->collider ){
 258          v3f delta;
 259          v3_sub( av->sk.bones[i].co, av->sk.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( av->sk.final_mtx[sb->parent], posemtx, av->sk.final_mtx[i] );
 267       }
 268    }
 269
 270    skeleton_apply_inverses( &av->sk );
 271 }
 272
 273 /*
 274  * Make the ragdoll copy the player model
 275  */
 276 VG_STATIC void copy_avatar_pose_to_ragdoll( struct player_avatar *av,
 277                                             struct player_ragdoll *rd,
 278                                             v3f velocity )
 279 {
 280    for( int i=0; i<rd->part_count; i++ ){
 281       struct ragdoll_part *part = &rd->parts[i];
 282
 283       v3f pos, offset;
 284       u32 bone = part->bone_id;
 285
 286       m4x3_mulv( av->sk.final_mtx[bone], av->sk.bones[bone].co, pos );
 287       m3x3_mulv( av->sk.final_mtx[bone], part->collider_mtx[3], offset );
 288       v3_add( pos, offset, part->obj.rb.co );
 289
 290       m3x3f r;
 291       m3x3_mul( av->sk.final_mtx[bone], part->collider_mtx, r );
 292       m3x3_q( r, part->obj.rb.q );
 293
 294       v3_copy( velocity, part->obj.rb.v );
 295       v3_zero( part->obj.rb.w );
 296
 297       v3_copy( part->obj.rb.co, part->prev_co );
 298       v4_copy( part->obj.rb.q, part->prev_q );
 299
 300       rb_update_transform( &part->obj.rb );
 301    }
 302 }
 303
 304 /*
 305  * Draw rigidbody colliders for ragdoll
 306  */
 307 VG_STATIC void player_debug_ragdoll(void)
 308 {
 309 }
 310
 311 /*
 312  * Ragdoll physics step
 313  */
 314 VG_STATIC void player_ragdoll_iter( struct player_ragdoll *rd )
 315 {
 316    world_instance *world = get_active_world();
 317
 318    int run_sim = 0;
 319    ragdoll_frame ++;
 320
 321    if( ragdoll_frame >= k_ragdoll_div ){
 322       ragdoll_frame = 0;
 323       run_sim = 1;
 324    }
 325
 326    rb_solver_reset();
 327
 328    float contact_velocities[256];
 329
 330    for( int i=0; i<rd->part_count; i ++ ){
 331       v4_copy( rd->parts[i].obj.rb.q, rd->parts[i].prev_q );
 332       v3_copy( rd->parts[i].obj.rb.co, rd->parts[i].prev_co );
 333
 334       if( rb_global_has_space() ){
 335          rb_ct *buf = rb_global_buffer();
 336
 337          int l;
 338
 339          if( rd->parts[i].obj.type == k_rb_shape_capsule ){
 340             l = rb_capsule__scene( rd->parts[i].obj.rb.to_world,
 341                                    &rd->parts[i].obj.inf.capsule,
 342                                    NULL, &world->rb_geo.inf.scene, buf );
 343          }
 344          else if( rd->parts[i].obj.type == k_rb_shape_box ){
 345             l = rb_box__scene( rd->parts[i].obj.rb.to_world,
 346                                rd->parts[i].obj.rb.bbx,
 347                                NULL, &world->rb_geo.inf.scene, buf );
 348          }
 349          else continue;
 350
 351          for( int j=0; j<l; j++ ){
 352             buf[j].rba = &rd->parts[i].obj.rb;
 353             buf[j].rbb = &world->rb_geo.rb;
 354          }
 355
 356          rb_contact_count += l;
 357       }
 358    }
 359
 360    /*
 361     * self-collision
 362     */
 363    for( int i=0; i<rd->part_count-1; i ++ ){
 364       for( int j=i+1; j<rd->part_count; j ++ ){
 365          if( rd->parts[j].parent != i ){
 366             if( !rb_global_has_space() )
 367                break;
 368
 369             if( rd->parts[j].obj.type != k_rb_shape_capsule )
 370                continue;
 371
 372             if( rd->parts[i].obj.type != k_rb_shape_capsule )
 373                continue;
 374
 375             rb_ct *buf = rb_global_buffer();
 376
 377             int l = rb_capsule__capsule( rd->parts[i].obj.rb.to_world,
 378                                          &rd->parts[i].obj.inf.capsule,
 379                                          rd->parts[j].obj.rb.to_world,
 380                                          &rd->parts[j].obj.inf.capsule,
 381                                          buf );
 382
 383             for( int k=0; k<l; k++ ){
 384                buf[k].rba = &rd->parts[i].obj.rb;
 385                buf[k].rbb = &rd->parts[j].obj.rb;
 386             }
 387
 388             rb_contact_count += l;
 389          }
 390       }
 391    }
 392
 393    for( int j=0; j<rd->part_count; j++ ){
 394       struct ragdoll_part *pj = &rd->parts[j];
 395
 396       if( run_sim ){
 397          v4f plane = {0.0f,1.0f,0.0f,0.0f};
 398          rb_effect_simple_bouyency( &pj->obj.rb, plane,
 399                                      k_ragdoll_floatyiness,
 400                                      k_ragdoll_floatydrag );
 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.25f );
 459
 460       rb_correct_position_constraints( rd->position_constraints,
 461                                        rd->position_constraints_count, 0.5f );
 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[rand()%5], stress->co, 20.0f, 1.0f );
 494       audio_unlock();
 495    }
 496 }
 497
 498 #endif /* PLAYER_RAGDOLL_H */