player_ragdoll.c

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