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