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