2 #include "vg/vg_rigidbody.h"
3 #include "vg/vg_rigidbody_collision.h"
4 #include "vg/vg_rigidbody_constraints.h"
5 #include "scene_rigidbody.h"
10 static int dev_ragdoll_saveload(int argc
, const char *argv
[]){
12 vg_info( "Usage: ragdoll load/save filepath\n" );
16 if( !strcmp(argv
[0],"save") ){
17 FILE *fp
= fopen( argv
[1], "wb" );
19 vg_error( "Failed to open file\n" );
22 fwrite( &localplayer
.ragdoll
.parts
,
23 sizeof(localplayer
.ragdoll
.parts
), 1, fp
);
26 else if( !strcmp(argv
[0],"load") ){
27 FILE *fp
= fopen( argv
[1], "rb" );
29 vg_error( "Failed to open file\n" );
33 fread( &localplayer
.ragdoll
.parts
,
34 sizeof(localplayer
.ragdoll
.parts
), 1, fp
);
38 vg_error( "Unknown command: %s (options are: save,load)\n", argv
[0] );
45 static void player_ragdoll_init(void){
46 VG_VAR_F32( k_ragdoll_active_threshold
);
47 VG_VAR_F32( k_ragdoll_angular_drag
);
48 VG_VAR_F32( k_ragdoll_correction
);
49 VG_VAR_F32( k_ragdoll_limit_scale
);
50 VG_VAR_F32( k_ragdoll_spring
);
51 VG_VAR_F32( k_ragdoll_dampening
);
52 VG_VAR_I32( k_ragdoll_div
);
53 VG_VAR_I32( k_ragdoll_debug_collider
);
54 VG_VAR_I32( k_ragdoll_debug_constraints
);
55 vg_console_reg_cmd( "ragdoll", dev_ragdoll_saveload
, NULL
);
58 static void player_init_ragdoll_bone_collider( struct skeleton_bone
*bone
,
59 struct ragdoll_part
*rp
)
62 k_inertia_scale
= 2.0f
;
64 m4x3_identity( rp
->collider_mtx
);
66 rp
->type
= bone
->collider
;
67 if( bone
->collider
== k_bone_collider_box
){
69 v3_sub( bone
->hitbox
[1], bone
->hitbox
[0], delta
);
70 v3_muls( delta
, 0.5f
, delta
);
71 v3_add( bone
->hitbox
[0], delta
, rp
->collider_mtx
[3] );
73 v3_muls( delta
, -1.0f
, rp
->inf
.box
[0] );
74 v3_copy( delta
, rp
->inf
.box
[1] );
76 rp
->colour
= 0xffcccccc;
78 rb_setbody_box( &rp
->rb
, rp
->inf
.box
, k_density
, k_inertia_scale
);
80 else if( bone
->collider
== k_bone_collider_capsule
){
82 v3_sub( bone
->hitbox
[1], bone
->hitbox
[0], v0
);
85 float largest
= -1.0f
;
87 for( int i
=0; i
<3; i
++ ){
88 if( fabsf( v0
[i
] ) > largest
){
89 largest
= fabsf( v0
[i
] );
95 v1
[ major_axis
] = 1.0f
;
96 v3_tangent_basis( v1
, tx
, ty
);
98 rp
->inf
.capsule
.r
= (fabsf(v3_dot(tx
,v0
)) + fabsf(v3_dot(ty
,v0
))) * 0.25f
;
99 rp
->inf
.capsule
.h
= fabsf(v0
[ major_axis
]);
102 v3_muls( tx
, -1.0f
, rp
->collider_mtx
[0] );
103 v3_muls( v1
, -1.0f
, rp
->collider_mtx
[1] );
104 v3_muls( ty
, -1.0f
, rp
->collider_mtx
[2] );
105 v3_add( bone
->hitbox
[0], bone
->hitbox
[1], rp
->collider_mtx
[3] );
106 v3_muls( rp
->collider_mtx
[3], 0.5f
, rp
->collider_mtx
[3] );
108 rp
->colour
= 0xff000000 | (0xff << (major_axis
*8));
110 rb_setbody_capsule( &rp
->rb
, rp
->inf
.capsule
.r
, rp
->inf
.capsule
.h
,
111 k_density
, k_inertia_scale
);
114 vg_warn( "type: %u\n", bone
->collider
);
115 vg_fatal_error( "Invalid bone collider type" );
118 m4x3_invert_affine( rp
->collider_mtx
, rp
->inv_collider_mtx
);
120 /* Position collider into rest */
121 m3x3_q( rp
->collider_mtx
, rp
->rb
.q
);
122 v3_add( rp
->collider_mtx
[3], bone
->co
, rp
->rb
.co
);
125 rb_update_matrices( &rp
->rb
);
129 * Get parent index in the ragdoll
131 static u32
ragdoll_bone_parent( struct player_ragdoll
*rd
, u32 bone_id
){
132 for( u32 j
=0; j
<rd
->part_count
; j
++ )
133 if( rd
->parts
[ j
].bone_id
== bone_id
)
136 vg_fatal_error( "Referenced parent bone does not have a rigidbody" );
141 * Setup ragdoll colliders from skeleton
143 static void setup_ragdoll_from_skeleton( struct skeleton
*sk
,
144 struct player_ragdoll
*rd
){
147 if( !sk
->collider_count
)
150 rd
->position_constraints_count
= 0;
151 rd
->cone_constraints_count
= 0;
153 for( u32 i
=1; i
<sk
->bone_count
; i
++ ){
154 struct skeleton_bone
*bone
= &sk
->bones
[i
];
157 * Bones with colliders
159 if( !(bone
->collider
) )
162 if( rd
->part_count
> vg_list_size(rd
->parts
) )
163 vg_fatal_error( "Playermodel has too many colliders" );
165 u32 part_id
= rd
->part_count
;
168 struct ragdoll_part
*rp
= &rd
->parts
[ part_id
];
170 rp
->parent
= 0xffffffff;
172 player_init_ragdoll_bone_collider( bone
, rp
);
175 * Bones with collider and parent
180 rp
->parent
= ragdoll_bone_parent( rd
, bone
->parent
);
182 if( bone
->orig_bone
->flags
& k_bone_flag_cone_constraint
){
183 u32 conid
= rd
->position_constraints_count
;
184 rd
->position_constraints_count
++;
186 struct rb_constr_pos
*c
= &rd
->position_constraints
[ conid
];
188 struct skeleton_bone
*bj
= &sk
->bones
[rp
->bone_id
];
189 struct ragdoll_part
*pp
= &rd
->parts
[rp
->parent
];
190 struct skeleton_bone
*bp
= &sk
->bones
[pp
->bone_id
];
192 rd
->constraint_associations
[conid
][0] = rp
->parent
;
193 rd
->constraint_associations
[conid
][1] = part_id
;
195 /* Convention: rba -- parent, rbb -- child */
200 v3_sub( bj
->co
, bp
->co
, delta
);
201 m4x3_mulv( rp
->inv_collider_mtx
, (v3f
){0.0f
,0.0f
,0.0f
}, c
->lcb
);
202 m4x3_mulv( pp
->inv_collider_mtx
, delta
, c
->lca
);
205 mdl_bone
*inf
= bone
->orig_bone
;
207 struct rb_constr_swingtwist
*a
=
208 &rd
->cone_constraints
[ rd
->cone_constraints_count
++ ];
212 a
->conet
= cosf( inf
->conet
)-0.0001f
;
214 /* Store constraint in local space vectors */
215 m3x3_mulv( c
->rba
->to_local
, inf
->conevx
, a
->conevx
);
216 m3x3_mulv( c
->rba
->to_local
, inf
->conevy
, a
->conevy
);
217 m3x3_mulv( c
->rbb
->to_local
, inf
->coneva
, a
->coneva
);
218 v3_copy( c
->lca
, a
->view_offset
);
220 v3_cross( inf
->coneva
, inf
->conevy
, a
->conevxb
);
221 m3x3_mulv( c
->rbb
->to_local
, a
->conevxb
, a
->conevxb
);
223 v3_normalize( a
->conevxb
);
224 v3_normalize( a
->conevx
);
225 v3_normalize( a
->conevy
);
226 v3_normalize( a
->coneva
);
228 a
->conevx
[3] = v3_length( inf
->conevx
);
229 a
->conevy
[3] = v3_length( inf
->conevy
);
237 * Make avatar copy the ragdoll
239 static void copy_ragdoll_pose_to_localplayer( struct player_ragdoll
*rd
){
240 for( int i
=0; i
<rd
->part_count
; i
++ ){
241 struct ragdoll_part
*part
= &rd
->parts
[i
];
248 float substep
= vg
.time_fixed_extrapolate
;
249 v3_lerp( part
->prev_co
, part
->rb
.co
, substep
, co_int
);
250 q_nlerp( part
->prev_q
, part
->rb
.q
, substep
, q_int
);
252 q_m3x3( q_int
, mtx
);
253 v3_copy( co_int
, mtx
[3] );
255 m4x3_mul( mtx
, part
->inv_collider_mtx
,
256 localplayer
.final_mtx
[part
->bone_id
] );
259 for( u32 i
=1; i
<localplayer
.skeleton
.bone_count
; i
++ ){
260 struct skeleton_bone
*sb
= &localplayer
.skeleton
.bones
[i
];
262 if( sb
->parent
&& !sb
->collider
){
264 v3_sub( localplayer
.skeleton
.bones
[i
].co
,
265 localplayer
.skeleton
.bones
[sb
->parent
].co
, delta
);
268 m3x3_identity( posemtx
);
269 v3_copy( delta
, posemtx
[3] );
272 m4x3_mul( localplayer
.final_mtx
[sb
->parent
], posemtx
,
273 localplayer
.final_mtx
[i
] );
277 skeleton_apply_inverses( &localplayer
.skeleton
, localplayer
.final_mtx
);
281 * Make the ragdoll copy the player model
283 static void copy_localplayer_to_ragdoll( struct player_ragdoll
*rd
,
284 enum player_die_type type
){
287 v3f
*bone_mtx
= localplayer
.final_mtx
[localplayer
.id_hip
];
289 localplayer
.skeleton
.bones
[localplayer
.id_hip
].co
, centroid
);
291 for( int i
=0; i
<rd
->part_count
; i
++ ){
292 struct ragdoll_part
*part
= &rd
->parts
[i
];
295 u32 bone
= part
->bone_id
;
297 v3f
*bone_mtx
= localplayer
.final_mtx
[bone
];
299 m4x3_mulv( bone_mtx
, localplayer
.skeleton
.bones
[bone
].co
, pos
);
300 m3x3_mulv( bone_mtx
, part
->collider_mtx
[3], offset
);
301 v3_add( pos
, offset
, part
->rb
.co
);
304 m3x3_mul( bone_mtx
, part
->collider_mtx
, r
);
305 m3x3_q( r
, part
->rb
.q
);
308 v3_sub( part
->rb
.co
, centroid
, ra
);
309 v3_cross( localplayer
.rb
.w
, ra
, v
);
310 v3_add( localplayer
.rb
.v
, v
, part
->rb
.v
);
312 if( type
== k_player_die_type_feet
){
313 if( (bone
== localplayer
.id_foot_l
) ||
314 (bone
== localplayer
.id_foot_r
) ){
315 v3_zero( part
->rb
.v
);
319 v3_copy( localplayer
.rb
.w
, part
->rb
.w
);
321 v3_copy( part
->rb
.co
, part
->prev_co
);
322 v4_copy( part
->rb
.q
, part
->prev_q
);
324 rb_update_matrices( &part
->rb
);
329 * Ragdoll physics step
331 static void player_ragdoll_iter( struct player_ragdoll
*rd
){
332 world_instance
*world
= world_current_instance();
337 if( ragdoll_frame
>= k_ragdoll_div
){
344 float contact_velocities
[256];
346 rigidbody _null
= {0};
347 _null
.inv_mass
= 0.0f
;
348 m3x3_zero( _null
.iI
);
350 for( int i
=0; i
<rd
->part_count
; i
++ ){
351 v4_copy( rd
->parts
[i
].rb
.q
, rd
->parts
[i
].prev_q
);
352 v3_copy( rd
->parts
[i
].rb
.co
, rd
->parts
[i
].prev_co
);
354 if( rb_global_has_space() ){
355 rb_ct
*buf
= rb_global_buffer();
359 if( rd
->parts
[i
].type
== k_bone_collider_capsule
){
360 l
= rb_capsule__scene( rd
->parts
[i
].rb
.to_world
,
361 &rd
->parts
[i
].inf
.capsule
,
362 NULL
, world
->geo_bh
, buf
,
363 k_material_flag_ghosts
);
365 else if( rd
->parts
[i
].type
== k_bone_collider_box
){
366 l
= rb_box__scene( rd
->parts
[i
].rb
.to_world
,
367 rd
->parts
[i
].inf
.box
,
368 NULL
, world
->geo_bh
, buf
,
369 k_material_flag_ghosts
);
373 for( int j
=0; j
<l
; j
++ ){
374 buf
[j
].rba
= &rd
->parts
[i
].rb
;
378 rb_contact_count
+= l
;
385 for( int i
=0; i
<rd
->part_count
-1; i
++ ){
386 for( int j
=i
+1; j
<rd
->part_count
; j
++ ){
387 if( rd
->parts
[j
].parent
!= i
){
388 if( !rb_global_has_space() )
391 if( rd
->parts
[j
].type
!= k_bone_collider_capsule
)
394 if( rd
->parts
[i
].type
!= k_bone_collider_capsule
)
397 rb_ct
*buf
= rb_global_buffer();
399 int l
= rb_capsule__capsule( rd
->parts
[i
].rb
.to_world
,
400 &rd
->parts
[i
].inf
.capsule
,
401 rd
->parts
[j
].rb
.to_world
,
402 &rd
->parts
[j
].inf
.capsule
,
405 for( int k
=0; k
<l
; k
++ ){
406 buf
[k
].rba
= &rd
->parts
[i
].rb
;
407 buf
[k
].rbb
= &rd
->parts
[j
].rb
;
410 rb_contact_count
+= l
;
415 if( world
->water
.enabled
){
416 for( int j
=0; j
<rd
->part_count
; j
++ ){
417 struct ragdoll_part
*pj
= &rd
->parts
[j
];
420 rb_effect_simple_bouyency( &pj
->rb
, world
->water
.plane
,
421 k_ragdoll_floatyiness
,
422 k_ragdoll_floatydrag
);
430 for( u32 i
=0; i
<rb_contact_count
; i
++ ){
431 rb_ct
*ct
= &rb_contact_buffer
[i
];
434 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
435 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
436 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
437 float vn
= v3_dot( rv
, ct
->n
);
439 contact_velocities
[i
] = vn
;
442 rb_presolve_contacts( rb_contact_buffer
, rb_contact_count
);
443 rb_presolve_swingtwist_constraints( rd
->cone_constraints
,
444 rd
->cone_constraints_count
);
449 if( k_ragdoll_debug_collider
){
450 for( u32 i
=0; i
<rd
->part_count
; i
++ ){
451 struct ragdoll_part
*rp
= &rd
->parts
[i
];
453 if( rp
->type
== k_bone_collider_capsule
){
454 vg_line_capsule( rp
->rb
.to_world
,
455 rp
->inf
.capsule
.r
, rp
->inf
.capsule
.h
, rp
->colour
);
457 else if( rp
->type
== k_bone_collider_box
){
458 vg_line_boxf_transformed( rp
->rb
.to_world
,
459 rp
->inf
.box
, rp
->colour
);
464 if( k_ragdoll_debug_constraints
){
465 rb_debug_position_constraints( rd
->position_constraints
,
466 rd
->position_constraints_count
);
468 rb_debug_swingtwist_constraints( rd
->cone_constraints
,
469 rd
->cone_constraints_count
);
473 * SOLVE CONSTRAINTS & Integrate
476 /* the solver is not very quickly converging so... */
477 for( int i
=0; i
<40; i
++ ){
479 rb_solve_contacts( rb_contact_buffer
, rb_contact_count
);
480 rb_solve_swingtwist_constraints( rd
->cone_constraints
,
481 rd
->cone_constraints_count
);
482 rb_postsolve_swingtwist_constraints( rd
->cone_constraints
,
483 rd
->cone_constraints_count
);
485 rb_solve_position_constraints( rd
->position_constraints
,
486 rd
->position_constraints_count
);
489 rb_correct_position_constraints( rd
->position_constraints
,
490 rd
->position_constraints_count
,
491 k_ragdoll_correction
* 0.5f
);
492 rb_correct_swingtwist_constraints( rd
->cone_constraints
,
493 rd
->cone_constraints_count
,
494 k_ragdoll_correction
* 0.25f
);
496 for( int i
=0; i
<rd
->part_count
; i
++ ){
497 rb_iter( &rd
->parts
[i
].rb
);
500 v3_copy( rd
->parts
[i
].rb
.w
, w
);
501 if( v3_length2( w
) > 0.00001f
){
503 v3_muladds( rd
->parts
[i
].rb
.w
, w
, -k_ragdoll_angular_drag
,
508 for( int i
=0; i
<rd
->part_count
; i
++ )
509 rb_update_matrices( &rd
->parts
[i
].rb
);
512 rb_ct
*stress
= NULL
;
513 float max_stress
= 1.0f
;
515 for( u32 i
=0; i
<rb_contact_count
; i
++ ){
516 rb_ct
*ct
= &rb_contact_buffer
[i
];
519 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
520 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
521 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
522 float vn
= v3_dot( rv
, ct
->n
);
524 float s
= fabsf(vn
- contact_velocities
[i
]);
525 if( s
> max_stress
){
531 static u32 temp_filter
= 0;
537 (v3_length2(player_dead
.v_lpf
)>(k_ragdoll_active_threshold
*
538 k_ragdoll_active_threshold
)) ){
539 assert( rd
->cone_constraints_count
== rd
->position_constraints_count
);
541 mdl_keyframe anim
[32];
542 skeleton_sample_anim( &localplayer
.skeleton
, player_dead
.anim_bail
,
545 for( u32 i
=0; i
<rd
->cone_constraints_count
; i
++ ){
546 rb_constr_swingtwist
*st
= &rd
->cone_constraints
[i
];
547 rb_constr_pos
*pc
= &rd
->position_constraints
[i
];
551 m3x3_mulv( st
->rbb
->to_world
, st
->coneva
, va
);
553 /* calculate va as seen in rest position, from the perspective of the
554 * parent object, mapped to pose world space using the parents
555 * transform. thats our target */
557 u32 id_p
= rd
->constraint_associations
[i
][0],
558 id_a
= rd
->constraint_associations
[i
][1];
560 struct ragdoll_part
*pa
= &rd
->parts
[ id_a
],
561 *pp
= &rd
->parts
[ id_p
];
563 mdl_keyframe
*kf
= &anim
[ pa
->bone_id
-1 ];
564 m3x3_mulv( pa
->collider_mtx
, st
->coneva
, vap
);
565 q_mulv( kf
->q
, vap
, vap
);
567 /* This could be a transfer function */
568 m3x3_mulv( pp
->inv_collider_mtx
, vap
, vap
);
569 m3x3_mulv( st
->rba
->to_world
, vap
, vap
);
571 f32 d
= v3_dot( vap
, va
),
572 a
= acosf( vg_clampf( d
, -1.0f
, 1.0f
) );
575 v3_cross( vap
, va
, axis
);
577 f32 Fs
= -a
* k_ragdoll_spring
,
578 Fd
= -v3_dot( st
->rbb
->w
, axis
) * k_ragdoll_dampening
,
582 v3_muls( axis
, F
, torque
);
583 v3_muladds( st
->rbb
->w
, torque
, k_rb_delta
, st
->rbb
->w
);
585 /* apply a adjustment to keep velocity at joint 0 */
588 m3x3_mulv( st
->rbb
->to_world
, pc
->lcb
, wcb
);
589 v3_cross( torque
, wcb
, vcb
);
590 v3_muladds( st
->rbb
->v
, vcb
, k_rb_delta
, st
->rbb
->v
);
603 audio_oneshot_3d( &audio_hits
[vg_randu32(&vg
.rand
)%5],
604 stress
->co
, 20.0f
, 1.0f
);