player_ragdoll.c

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