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