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