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