1 #ifndef PLAYER_RAGDOLL_C
2 #define PLAYER_RAGDOLL_C
7 static int dev_ragdoll_saveload(int argc
, const char *argv
[]){
9 vg_info( "Usage: ragdoll load/save filepath\n" );
13 if( !strcmp(argv
[0],"save") ){
14 FILE *fp
= fopen( argv
[1], "wb" );
16 vg_error( "Failed to open file\n" );
19 fwrite( &localplayer
.ragdoll
.parts
,
20 sizeof(localplayer
.ragdoll
.parts
), 1, fp
);
23 else if( !strcmp(argv
[0],"load") ){
24 FILE *fp
= fopen( argv
[1], "rb" );
26 vg_error( "Failed to open file\n" );
30 fread( &localplayer
.ragdoll
.parts
,
31 sizeof(localplayer
.ragdoll
.parts
), 1, fp
);
35 vg_error( "Unknown command: %s (options are: save,load)\n", argv
[0] );
42 static void player_ragdoll_init(void){
43 VG_VAR_F32( k_ragdoll_active_threshold
);
44 VG_VAR_F32( k_ragdoll_angular_drag
);
45 VG_VAR_F32( k_ragdoll_correction
);
46 VG_VAR_F32( k_ragdoll_limit_scale
);
47 VG_VAR_F32( k_ragdoll_spring
);
48 VG_VAR_F32( k_ragdoll_dampening
);
49 VG_VAR_I32( k_ragdoll_div
);
50 VG_VAR_I32( k_ragdoll_debug_collider
);
51 VG_VAR_I32( k_ragdoll_debug_constraints
);
52 vg_console_reg_cmd( "ragdoll", dev_ragdoll_saveload
, NULL
);
55 static void player_init_ragdoll_bone_collider( struct skeleton_bone
*bone
,
56 struct ragdoll_part
*rp
)
59 k_inertia_scale
= 2.0f
;
61 m4x3_identity( rp
->collider_mtx
);
63 rp
->type
= bone
->collider
;
64 if( bone
->collider
== k_bone_collider_box
){
66 v3_sub( bone
->hitbox
[1], bone
->hitbox
[0], delta
);
67 v3_muls( delta
, 0.5f
, delta
);
68 v3_add( bone
->hitbox
[0], delta
, rp
->collider_mtx
[3] );
70 v3_muls( delta
, -1.0f
, rp
->inf
.box
[0] );
71 v3_copy( delta
, rp
->inf
.box
[1] );
73 rp
->colour
= 0xffcccccc;
75 rb_setbody_box( &rp
->rb
, rp
->inf
.box
, k_density
, k_inertia_scale
);
77 else if( bone
->collider
== k_bone_collider_capsule
){
79 v3_sub( bone
->hitbox
[1], bone
->hitbox
[0], v0
);
82 float largest
= -1.0f
;
84 for( int i
=0; i
<3; i
++ ){
85 if( fabsf( v0
[i
] ) > largest
){
86 largest
= fabsf( v0
[i
] );
92 v1
[ major_axis
] = 1.0f
;
93 v3_tangent_basis( v1
, tx
, ty
);
95 rp
->inf
.capsule
.r
= (fabsf(v3_dot(tx
,v0
)) + fabsf(v3_dot(ty
,v0
))) * 0.25f
;
96 rp
->inf
.capsule
.h
= fabsf(v0
[ major_axis
]);
99 v3_muls( tx
, -1.0f
, rp
->collider_mtx
[0] );
100 v3_muls( v1
, -1.0f
, rp
->collider_mtx
[1] );
101 v3_muls( ty
, -1.0f
, rp
->collider_mtx
[2] );
102 v3_add( bone
->hitbox
[0], bone
->hitbox
[1], rp
->collider_mtx
[3] );
103 v3_muls( rp
->collider_mtx
[3], 0.5f
, rp
->collider_mtx
[3] );
105 rp
->colour
= 0xff000000 | (0xff << (major_axis
*8));
107 rb_setbody_capsule( &rp
->rb
, rp
->inf
.capsule
.r
, rp
->inf
.capsule
.h
,
108 k_density
, k_inertia_scale
);
111 vg_warn( "type: %u\n", bone
->collider
);
112 vg_fatal_error( "Invalid bone collider type" );
115 m4x3_invert_affine( rp
->collider_mtx
, rp
->inv_collider_mtx
);
117 /* Position collider into rest */
118 m3x3_q( rp
->collider_mtx
, rp
->rb
.q
);
119 v3_add( rp
->collider_mtx
[3], bone
->co
, rp
->rb
.co
);
122 rb_update_matrices( &rp
->rb
);
126 * Get parent index in the ragdoll
128 static u32
ragdoll_bone_parent( struct player_ragdoll
*rd
, u32 bone_id
){
129 for( u32 j
=0; j
<rd
->part_count
; j
++ )
130 if( rd
->parts
[ j
].bone_id
== bone_id
)
133 vg_fatal_error( "Referenced parent bone does not have a rigidbody" );
138 * Setup ragdoll colliders from skeleton
140 static void setup_ragdoll_from_skeleton( struct skeleton
*sk
,
141 struct player_ragdoll
*rd
){
144 if( !sk
->collider_count
)
147 rd
->position_constraints_count
= 0;
148 rd
->cone_constraints_count
= 0;
150 for( u32 i
=1; i
<sk
->bone_count
; i
++ ){
151 struct skeleton_bone
*bone
= &sk
->bones
[i
];
154 * Bones with colliders
156 if( !(bone
->collider
) )
159 if( rd
->part_count
> vg_list_size(rd
->parts
) )
160 vg_fatal_error( "Playermodel has too many colliders" );
162 u32 part_id
= rd
->part_count
;
165 struct ragdoll_part
*rp
= &rd
->parts
[ part_id
];
167 rp
->parent
= 0xffffffff;
169 player_init_ragdoll_bone_collider( bone
, rp
);
172 * Bones with collider and parent
177 rp
->parent
= ragdoll_bone_parent( rd
, bone
->parent
);
179 if( bone
->orig_bone
->flags
& k_bone_flag_cone_constraint
){
180 u32 conid
= rd
->position_constraints_count
;
181 rd
->position_constraints_count
++;
183 struct rb_constr_pos
*c
= &rd
->position_constraints
[ conid
];
185 struct skeleton_bone
*bj
= &sk
->bones
[rp
->bone_id
];
186 struct ragdoll_part
*pp
= &rd
->parts
[rp
->parent
];
187 struct skeleton_bone
*bp
= &sk
->bones
[pp
->bone_id
];
189 rd
->constraint_associations
[conid
][0] = rp
->parent
;
190 rd
->constraint_associations
[conid
][1] = part_id
;
192 /* Convention: rba -- parent, rbb -- child */
197 v3_sub( bj
->co
, bp
->co
, delta
);
198 m4x3_mulv( rp
->inv_collider_mtx
, (v3f
){0.0f
,0.0f
,0.0f
}, c
->lcb
);
199 m4x3_mulv( pp
->inv_collider_mtx
, delta
, c
->lca
);
202 mdl_bone
*inf
= bone
->orig_bone
;
204 struct rb_constr_swingtwist
*a
=
205 &rd
->cone_constraints
[ rd
->cone_constraints_count
++ ];
209 a
->conet
= cosf( inf
->conet
)-0.0001f
;
211 /* Store constraint in local space vectors */
212 m3x3_mulv( c
->rba
->to_local
, inf
->conevx
, a
->conevx
);
213 m3x3_mulv( c
->rba
->to_local
, inf
->conevy
, a
->conevy
);
214 m3x3_mulv( c
->rbb
->to_local
, inf
->coneva
, a
->coneva
);
215 v3_copy( c
->lca
, a
->view_offset
);
217 v3_cross( inf
->coneva
, inf
->conevy
, a
->conevxb
);
218 m3x3_mulv( c
->rbb
->to_local
, a
->conevxb
, a
->conevxb
);
220 v3_normalize( a
->conevxb
);
221 v3_normalize( a
->conevx
);
222 v3_normalize( a
->conevy
);
223 v3_normalize( a
->coneva
);
225 a
->conevx
[3] = v3_length( inf
->conevx
);
226 a
->conevy
[3] = v3_length( inf
->conevy
);
234 * Make avatar copy the ragdoll
236 static void copy_ragdoll_pose_to_localplayer( struct player_ragdoll
*rd
){
237 for( int i
=0; i
<rd
->part_count
; i
++ ){
238 struct ragdoll_part
*part
= &rd
->parts
[i
];
245 float substep
= vg
.time_fixed_extrapolate
;
246 v3_lerp( part
->prev_co
, part
->rb
.co
, substep
, co_int
);
247 q_nlerp( part
->prev_q
, part
->rb
.q
, substep
, q_int
);
249 q_m3x3( q_int
, mtx
);
250 v3_copy( co_int
, mtx
[3] );
252 m4x3_mul( mtx
, part
->inv_collider_mtx
,
253 localplayer
.final_mtx
[part
->bone_id
] );
256 for( u32 i
=1; i
<localplayer
.skeleton
.bone_count
; i
++ ){
257 struct skeleton_bone
*sb
= &localplayer
.skeleton
.bones
[i
];
259 if( sb
->parent
&& !sb
->collider
){
261 v3_sub( localplayer
.skeleton
.bones
[i
].co
,
262 localplayer
.skeleton
.bones
[sb
->parent
].co
, delta
);
265 m3x3_identity( posemtx
);
266 v3_copy( delta
, posemtx
[3] );
269 m4x3_mul( localplayer
.final_mtx
[sb
->parent
], posemtx
,
270 localplayer
.final_mtx
[i
] );
274 skeleton_apply_inverses( &localplayer
.skeleton
, localplayer
.final_mtx
);
278 * Make the ragdoll copy the player model
280 static void copy_localplayer_to_ragdoll( struct player_ragdoll
*rd
,
281 enum player_die_type type
){
284 v3f
*bone_mtx
= localplayer
.final_mtx
[localplayer
.id_hip
];
286 localplayer
.skeleton
.bones
[localplayer
.id_hip
].co
, centroid
);
288 for( int i
=0; i
<rd
->part_count
; i
++ ){
289 struct ragdoll_part
*part
= &rd
->parts
[i
];
292 u32 bone
= part
->bone_id
;
294 v3f
*bone_mtx
= localplayer
.final_mtx
[bone
];
296 m4x3_mulv( bone_mtx
, localplayer
.skeleton
.bones
[bone
].co
, pos
);
297 m3x3_mulv( bone_mtx
, part
->collider_mtx
[3], offset
);
298 v3_add( pos
, offset
, part
->rb
.co
);
301 m3x3_mul( bone_mtx
, part
->collider_mtx
, r
);
302 m3x3_q( r
, part
->rb
.q
);
305 v3_sub( part
->rb
.co
, centroid
, ra
);
306 v3_cross( localplayer
.rb
.w
, ra
, v
);
307 v3_add( localplayer
.rb
.v
, v
, part
->rb
.v
);
309 if( type
== k_player_die_type_feet
){
310 if( (bone
== localplayer
.id_foot_l
) ||
311 (bone
== localplayer
.id_foot_r
) ){
312 v3_zero( part
->rb
.v
);
316 v3_copy( localplayer
.rb
.w
, part
->rb
.w
);
318 v3_copy( part
->rb
.co
, part
->prev_co
);
319 v4_copy( part
->rb
.q
, part
->prev_q
);
321 rb_update_matrices( &part
->rb
);
326 * Ragdoll physics step
328 static void player_ragdoll_iter( struct player_ragdoll
*rd
){
329 world_instance
*world
= world_current_instance();
334 if( ragdoll_frame
>= k_ragdoll_div
){
341 float contact_velocities
[256];
343 rigidbody _null
= {0};
344 _null
.inv_mass
= 0.0f
;
345 m3x3_zero( _null
.iI
);
347 for( int i
=0; i
<rd
->part_count
; i
++ ){
348 v4_copy( rd
->parts
[i
].rb
.q
, rd
->parts
[i
].prev_q
);
349 v3_copy( rd
->parts
[i
].rb
.co
, rd
->parts
[i
].prev_co
);
351 if( rb_global_has_space() ){
352 rb_ct
*buf
= rb_global_buffer();
356 if( rd
->parts
[i
].type
== k_bone_collider_capsule
){
357 l
= rb_capsule__scene( rd
->parts
[i
].rb
.to_world
,
358 &rd
->parts
[i
].inf
.capsule
,
359 NULL
, world
->geo_bh
, buf
,
360 k_material_flag_ghosts
);
362 else if( rd
->parts
[i
].type
== k_bone_collider_box
){
363 l
= rb_box__scene( rd
->parts
[i
].rb
.to_world
,
364 rd
->parts
[i
].inf
.box
,
365 NULL
, world
->geo_bh
, buf
,
366 k_material_flag_ghosts
);
370 for( int j
=0; j
<l
; j
++ ){
371 buf
[j
].rba
= &rd
->parts
[i
].rb
;
375 rb_contact_count
+= l
;
382 for( int i
=0; i
<rd
->part_count
-1; i
++ ){
383 for( int j
=i
+1; j
<rd
->part_count
; j
++ ){
384 if( rd
->parts
[j
].parent
!= i
){
385 if( !rb_global_has_space() )
388 if( rd
->parts
[j
].type
!= k_bone_collider_capsule
)
391 if( rd
->parts
[i
].type
!= k_bone_collider_capsule
)
394 rb_ct
*buf
= rb_global_buffer();
396 int l
= rb_capsule__capsule( rd
->parts
[i
].rb
.to_world
,
397 &rd
->parts
[i
].inf
.capsule
,
398 rd
->parts
[j
].rb
.to_world
,
399 &rd
->parts
[j
].inf
.capsule
,
402 for( int k
=0; k
<l
; k
++ ){
403 buf
[k
].rba
= &rd
->parts
[i
].rb
;
404 buf
[k
].rbb
= &rd
->parts
[j
].rb
;
407 rb_contact_count
+= l
;
412 if( world
->water
.enabled
){
413 for( int j
=0; j
<rd
->part_count
; j
++ ){
414 struct ragdoll_part
*pj
= &rd
->parts
[j
];
417 rb_effect_simple_bouyency( &pj
->rb
, world
->water
.plane
,
418 k_ragdoll_floatyiness
,
419 k_ragdoll_floatydrag
);
427 for( u32 i
=0; i
<rb_contact_count
; i
++ ){
428 rb_ct
*ct
= &rb_contact_buffer
[i
];
431 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
432 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
433 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
434 float vn
= v3_dot( rv
, ct
->n
);
436 contact_velocities
[i
] = vn
;
439 rb_presolve_contacts( rb_contact_buffer
, rb_contact_count
);
440 rb_presolve_swingtwist_constraints( rd
->cone_constraints
,
441 rd
->cone_constraints_count
);
446 if( k_ragdoll_debug_collider
){
447 for( u32 i
=0; i
<rd
->part_count
; i
++ ){
448 struct ragdoll_part
*rp
= &rd
->parts
[i
];
450 if( rp
->type
== k_bone_collider_capsule
){
451 vg_line_capsule( rp
->rb
.to_world
,
452 rp
->inf
.capsule
.r
, rp
->inf
.capsule
.h
, rp
->colour
);
454 else if( rp
->type
== k_bone_collider_box
){
455 vg_line_boxf_transformed( rp
->rb
.to_world
,
456 rp
->inf
.box
, rp
->colour
);
461 if( k_ragdoll_debug_constraints
){
462 rb_debug_position_constraints( rd
->position_constraints
,
463 rd
->position_constraints_count
);
465 rb_debug_swingtwist_constraints( rd
->cone_constraints
,
466 rd
->cone_constraints_count
);
470 * SOLVE CONSTRAINTS & Integrate
473 /* the solver is not very quickly converging so... */
474 for( int i
=0; i
<40; i
++ ){
476 rb_solve_contacts( rb_contact_buffer
, rb_contact_count
);
477 rb_solve_swingtwist_constraints( rd
->cone_constraints
,
478 rd
->cone_constraints_count
);
479 rb_postsolve_swingtwist_constraints( rd
->cone_constraints
,
480 rd
->cone_constraints_count
);
482 rb_solve_position_constraints( rd
->position_constraints
,
483 rd
->position_constraints_count
);
486 rb_correct_position_constraints( rd
->position_constraints
,
487 rd
->position_constraints_count
,
488 k_ragdoll_correction
* 0.5f
);
489 rb_correct_swingtwist_constraints( rd
->cone_constraints
,
490 rd
->cone_constraints_count
,
491 k_ragdoll_correction
* 0.25f
);
493 for( int i
=0; i
<rd
->part_count
; i
++ ){
494 rb_iter( &rd
->parts
[i
].rb
);
497 v3_copy( rd
->parts
[i
].rb
.w
, w
);
498 if( v3_length2( w
) > 0.00001f
){
500 v3_muladds( rd
->parts
[i
].rb
.w
, w
, -k_ragdoll_angular_drag
,
505 for( int i
=0; i
<rd
->part_count
; i
++ )
506 rb_update_matrices( &rd
->parts
[i
].rb
);
509 rb_ct
*stress
= NULL
;
510 float max_stress
= 1.0f
;
512 for( u32 i
=0; i
<rb_contact_count
; i
++ ){
513 rb_ct
*ct
= &rb_contact_buffer
[i
];
516 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
517 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
518 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
519 float vn
= v3_dot( rv
, ct
->n
);
521 float s
= fabsf(vn
- contact_velocities
[i
]);
522 if( s
> max_stress
){
528 static u32 temp_filter
= 0;
534 (v3_length2(player_dead
.v_lpf
)>(k_ragdoll_active_threshold
*
535 k_ragdoll_active_threshold
)) ){
536 assert( rd
->cone_constraints_count
== rd
->position_constraints_count
);
538 mdl_keyframe anim
[32];
539 skeleton_sample_anim( &localplayer
.skeleton
, player_dead
.anim_bail
,
542 for( u32 i
=0; i
<rd
->cone_constraints_count
; i
++ ){
543 rb_constr_swingtwist
*st
= &rd
->cone_constraints
[i
];
544 rb_constr_pos
*pc
= &rd
->position_constraints
[i
];
548 m3x3_mulv( st
->rbb
->to_world
, st
->coneva
, va
);
550 /* calculate va as seen in rest position, from the perspective of the
551 * parent object, mapped to pose world space using the parents
552 * transform. thats our target */
554 u32 id_p
= rd
->constraint_associations
[i
][0],
555 id_a
= rd
->constraint_associations
[i
][1];
557 struct ragdoll_part
*pa
= &rd
->parts
[ id_a
],
558 *pp
= &rd
->parts
[ id_p
];
560 mdl_keyframe
*kf
= &anim
[ pa
->bone_id
-1 ];
561 m3x3_mulv( pa
->collider_mtx
, st
->coneva
, vap
);
562 q_mulv( kf
->q
, vap
, vap
);
564 /* This could be a transfer function */
565 m3x3_mulv( pp
->inv_collider_mtx
, vap
, vap
);
566 m3x3_mulv( st
->rba
->to_world
, vap
, vap
);
568 f32 d
= v3_dot( vap
, va
),
569 a
= acosf( vg_clampf( d
, -1.0f
, 1.0f
) );
572 v3_cross( vap
, va
, axis
);
574 f32 Fs
= -a
* k_ragdoll_spring
,
575 Fd
= -v3_dot( st
->rbb
->w
, axis
) * k_ragdoll_dampening
,
579 v3_muls( axis
, F
, torque
);
580 v3_muladds( st
->rbb
->w
, torque
, k_rb_delta
, st
->rbb
->w
);
582 /* apply a adjustment to keep velocity at joint 0 */
585 m3x3_mulv( st
->rbb
->to_world
, pc
->lcb
, wcb
);
586 v3_cross( torque
, wcb
, vcb
);
587 v3_muladds( st
->rbb
->v
, vcb
, k_rb_delta
, st
->rbb
->v
);
600 audio_oneshot_3d( &audio_hits
[vg_randu32(&vg
.rand
)%5],
601 stress
->co
, 20.0f
, 1.0f
);
606 #endif /* PLAYER_RAGDOLL_C */