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