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"
8 #include "player_dead.h"
11 static float k_ragdoll_floatyiness
= 20.0f
,
12 k_ragdoll_floatydrag
= 1.0f
,
13 k_ragdoll_limit_scale
= 1.0f
,
14 k_ragdoll_spring
= 127.0f
,
15 k_ragdoll_dampening
= 15.0f
,
16 k_ragdoll_correction
= 0.5f
,
17 k_ragdoll_angular_drag
= 0.08f
,
18 k_ragdoll_active_threshold
= 5.0f
;
20 static int k_ragdoll_div
= 1,
22 k_ragdoll_debug_collider
= 1,
23 k_ragdoll_debug_constraints
= 0;
25 static int dev_ragdoll_saveload(int argc
, const char *argv
[]){
27 vg_info( "Usage: ragdoll load/save filepath\n" );
31 if( !strcmp(argv
[0],"save") ){
32 FILE *fp
= fopen( argv
[1], "wb" );
34 vg_error( "Failed to open file\n" );
37 fwrite( &localplayer
.ragdoll
.parts
,
38 sizeof(localplayer
.ragdoll
.parts
), 1, fp
);
41 else if( !strcmp(argv
[0],"load") ){
42 FILE *fp
= fopen( argv
[1], "rb" );
44 vg_error( "Failed to open file\n" );
48 fread( &localplayer
.ragdoll
.parts
,
49 sizeof(localplayer
.ragdoll
.parts
), 1, fp
);
53 vg_error( "Unknown command: %s (options are: save,load)\n", argv
[0] );
60 void player_ragdoll_init(void)
62 VG_VAR_F32( k_ragdoll_active_threshold
);
63 VG_VAR_F32( k_ragdoll_angular_drag
);
64 VG_VAR_F32( k_ragdoll_correction
);
65 VG_VAR_F32( k_ragdoll_limit_scale
);
66 VG_VAR_F32( k_ragdoll_spring
);
67 VG_VAR_F32( k_ragdoll_dampening
);
68 VG_VAR_I32( k_ragdoll_div
);
69 VG_VAR_I32( k_ragdoll_debug_collider
);
70 VG_VAR_I32( k_ragdoll_debug_constraints
);
71 vg_console_reg_cmd( "ragdoll", dev_ragdoll_saveload
, NULL
);
74 void player_init_ragdoll_bone_collider( struct skeleton_bone
*bone
,
75 struct ragdoll_part
*rp
)
78 k_inertia_scale
= 2.0f
;
80 m4x3_identity( rp
->collider_mtx
);
82 rp
->type
= bone
->collider
;
83 if( bone
->collider
== k_bone_collider_box
){
85 v3_sub( bone
->hitbox
[1], bone
->hitbox
[0], delta
);
86 v3_muls( delta
, 0.5f
, delta
);
87 v3_add( bone
->hitbox
[0], delta
, rp
->collider_mtx
[3] );
89 v3_muls( delta
, -1.0f
, rp
->inf
.box
[0] );
90 v3_copy( delta
, rp
->inf
.box
[1] );
92 rp
->colour
= 0xffcccccc;
94 rb_setbody_box( &rp
->rb
, rp
->inf
.box
, k_density
, k_inertia_scale
);
96 else if( bone
->collider
== k_bone_collider_capsule
){
98 v3_sub( bone
->hitbox
[1], bone
->hitbox
[0], v0
);
101 float largest
= -1.0f
;
103 for( int i
=0; i
<3; i
++ ){
104 if( fabsf( v0
[i
] ) > largest
){
105 largest
= fabsf( v0
[i
] );
111 v1
[ major_axis
] = 1.0f
;
112 v3_tangent_basis( v1
, tx
, ty
);
114 rp
->inf
.capsule
.r
= (fabsf(v3_dot(tx
,v0
)) + fabsf(v3_dot(ty
,v0
))) * 0.25f
;
115 rp
->inf
.capsule
.h
= fabsf(v0
[ major_axis
]);
118 v3_muls( tx
, -1.0f
, rp
->collider_mtx
[0] );
119 v3_muls( v1
, -1.0f
, rp
->collider_mtx
[1] );
120 v3_muls( ty
, -1.0f
, rp
->collider_mtx
[2] );
121 v3_add( bone
->hitbox
[0], bone
->hitbox
[1], rp
->collider_mtx
[3] );
122 v3_muls( rp
->collider_mtx
[3], 0.5f
, rp
->collider_mtx
[3] );
124 rp
->colour
= 0xff000000 | (0xff << (major_axis
*8));
126 rb_setbody_capsule( &rp
->rb
, rp
->inf
.capsule
.r
, rp
->inf
.capsule
.h
,
127 k_density
, k_inertia_scale
);
130 vg_warn( "type: %u\n", bone
->collider
);
131 vg_fatal_error( "Invalid bone collider type" );
134 m4x3_invert_affine( rp
->collider_mtx
, rp
->inv_collider_mtx
);
136 /* Position collider into rest */
137 m3x3_q( rp
->collider_mtx
, rp
->rb
.q
);
138 v3_add( rp
->collider_mtx
[3], bone
->co
, rp
->rb
.co
);
141 rb_update_matrices( &rp
->rb
);
145 * Get parent index in the ragdoll
147 u32
ragdoll_bone_parent( struct player_ragdoll
*rd
, u32 bone_id
)
149 for( u32 j
=0; j
<rd
->part_count
; j
++ )
150 if( rd
->parts
[ j
].bone_id
== bone_id
)
153 vg_fatal_error( "Referenced parent bone does not have a rigidbody" );
158 * Setup ragdoll colliders from skeleton
160 void setup_ragdoll_from_skeleton( struct skeleton
*sk
,
161 struct player_ragdoll
*rd
)
165 if( !sk
->collider_count
)
168 rd
->position_constraints_count
= 0;
169 rd
->cone_constraints_count
= 0;
171 for( u32 i
=1; i
<sk
->bone_count
; i
++ ){
172 struct skeleton_bone
*bone
= &sk
->bones
[i
];
175 * Bones with colliders
177 if( !(bone
->collider
) )
180 if( rd
->part_count
> vg_list_size(rd
->parts
) )
181 vg_fatal_error( "Playermodel has too many colliders" );
183 u32 part_id
= rd
->part_count
;
186 struct ragdoll_part
*rp
= &rd
->parts
[ part_id
];
188 rp
->parent
= 0xffffffff;
190 player_init_ragdoll_bone_collider( bone
, rp
);
193 * Bones with collider and parent
198 rp
->parent
= ragdoll_bone_parent( rd
, bone
->parent
);
200 if( bone
->orig_bone
->flags
& k_bone_flag_cone_constraint
){
201 u32 conid
= rd
->position_constraints_count
;
202 rd
->position_constraints_count
++;
204 struct rb_constr_pos
*c
= &rd
->position_constraints
[ conid
];
206 struct skeleton_bone
*bj
= &sk
->bones
[rp
->bone_id
];
207 struct ragdoll_part
*pp
= &rd
->parts
[rp
->parent
];
208 struct skeleton_bone
*bp
= &sk
->bones
[pp
->bone_id
];
210 rd
->constraint_associations
[conid
][0] = rp
->parent
;
211 rd
->constraint_associations
[conid
][1] = part_id
;
213 /* Convention: rba -- parent, rbb -- child */
218 v3_sub( bj
->co
, bp
->co
, delta
);
219 m4x3_mulv( rp
->inv_collider_mtx
, (v3f
){0.0f
,0.0f
,0.0f
}, c
->lcb
);
220 m4x3_mulv( pp
->inv_collider_mtx
, delta
, c
->lca
);
223 mdl_bone
*inf
= bone
->orig_bone
;
225 struct rb_constr_swingtwist
*a
=
226 &rd
->cone_constraints
[ rd
->cone_constraints_count
++ ];
230 a
->conet
= cosf( inf
->conet
)-0.0001f
;
232 /* Store constraint in local space vectors */
233 m3x3_mulv( c
->rba
->to_local
, inf
->conevx
, a
->conevx
);
234 m3x3_mulv( c
->rba
->to_local
, inf
->conevy
, a
->conevy
);
235 m3x3_mulv( c
->rbb
->to_local
, inf
->coneva
, a
->coneva
);
236 v3_copy( c
->lca
, a
->view_offset
);
238 v3_cross( inf
->coneva
, inf
->conevy
, a
->conevxb
);
239 m3x3_mulv( c
->rbb
->to_local
, a
->conevxb
, a
->conevxb
);
241 v3_normalize( a
->conevxb
);
242 v3_normalize( a
->conevx
);
243 v3_normalize( a
->conevy
);
244 v3_normalize( a
->coneva
);
246 a
->conevx
[3] = v3_length( inf
->conevx
);
247 a
->conevy
[3] = v3_length( inf
->conevy
);
255 * Make avatar copy the ragdoll
257 void copy_ragdoll_pose_to_localplayer( struct player_ragdoll
*rd
)
259 for( int i
=0; i
<rd
->part_count
; i
++ ){
260 struct ragdoll_part
*part
= &rd
->parts
[i
];
267 float substep
= vg
.time_fixed_extrapolate
;
268 v3_lerp( part
->prev_co
, part
->rb
.co
, substep
, co_int
);
269 q_nlerp( part
->prev_q
, part
->rb
.q
, substep
, q_int
);
271 q_m3x3( q_int
, mtx
);
272 v3_copy( co_int
, mtx
[3] );
274 m4x3_mul( mtx
, part
->inv_collider_mtx
,
275 localplayer
.final_mtx
[part
->bone_id
] );
278 for( u32 i
=1; i
<localplayer
.skeleton
.bone_count
; i
++ ){
279 struct skeleton_bone
*sb
= &localplayer
.skeleton
.bones
[i
];
281 if( sb
->parent
&& !sb
->collider
){
283 v3_sub( localplayer
.skeleton
.bones
[i
].co
,
284 localplayer
.skeleton
.bones
[sb
->parent
].co
, delta
);
287 m3x3_identity( posemtx
);
288 v3_copy( delta
, posemtx
[3] );
291 m4x3_mul( localplayer
.final_mtx
[sb
->parent
], posemtx
,
292 localplayer
.final_mtx
[i
] );
296 skeleton_apply_inverses( &localplayer
.skeleton
, localplayer
.final_mtx
);
300 * Make the ragdoll copy the player model
302 void copy_localplayer_to_ragdoll( struct player_ragdoll
*rd
,
303 enum player_die_type type
)
307 v3f
*bone_mtx
= localplayer
.final_mtx
[localplayer
.id_hip
];
309 localplayer
.skeleton
.bones
[localplayer
.id_hip
].co
, centroid
);
311 for( int i
=0; i
<rd
->part_count
; i
++ ){
312 struct ragdoll_part
*part
= &rd
->parts
[i
];
315 u32 bone
= part
->bone_id
;
317 v3f
*bone_mtx
= localplayer
.final_mtx
[bone
];
319 m4x3_mulv( bone_mtx
, localplayer
.skeleton
.bones
[bone
].co
, pos
);
320 m3x3_mulv( bone_mtx
, part
->collider_mtx
[3], offset
);
321 v3_add( pos
, offset
, part
->rb
.co
);
324 m3x3_mul( bone_mtx
, part
->collider_mtx
, r
);
325 m3x3_q( r
, part
->rb
.q
);
328 v3_sub( part
->rb
.co
, centroid
, ra
);
329 v3_cross( localplayer
.rb
.w
, ra
, v
);
330 v3_add( localplayer
.rb
.v
, v
, part
->rb
.v
);
332 if( type
== k_player_die_type_feet
){
333 if( (bone
== localplayer
.id_foot_l
) ||
334 (bone
== localplayer
.id_foot_r
) ){
335 v3_zero( part
->rb
.v
);
339 v3_copy( localplayer
.rb
.w
, part
->rb
.w
);
341 v3_copy( part
->rb
.co
, part
->prev_co
);
342 v4_copy( part
->rb
.q
, part
->prev_q
);
344 rb_update_matrices( &part
->rb
);
349 * Ragdoll physics step
351 void player_ragdoll_iter( struct player_ragdoll
*rd
)
353 world_instance
*world
= world_current_instance();
358 if( ragdoll_frame
>= k_ragdoll_div
){
365 float contact_velocities
[256];
367 rigidbody _null
= {0};
368 _null
.inv_mass
= 0.0f
;
369 m3x3_zero( _null
.iI
);
371 for( int i
=0; i
<rd
->part_count
; i
++ ){
372 v4_copy( rd
->parts
[i
].rb
.q
, rd
->parts
[i
].prev_q
);
373 v3_copy( rd
->parts
[i
].rb
.co
, rd
->parts
[i
].prev_co
);
375 if( rb_global_has_space() ){
376 rb_ct
*buf
= rb_global_buffer();
380 if( rd
->parts
[i
].type
== k_bone_collider_capsule
){
381 l
= rb_capsule__scene( rd
->parts
[i
].rb
.to_world
,
382 &rd
->parts
[i
].inf
.capsule
,
383 NULL
, world
->geo_bh
, buf
,
384 k_material_flag_ghosts
);
386 else if( rd
->parts
[i
].type
== k_bone_collider_box
){
387 l
= rb_box__scene( rd
->parts
[i
].rb
.to_world
,
388 rd
->parts
[i
].inf
.box
,
389 NULL
, world
->geo_bh
, buf
,
390 k_material_flag_ghosts
);
394 for( int j
=0; j
<l
; j
++ ){
395 buf
[j
].rba
= &rd
->parts
[i
].rb
;
399 rb_contact_count
+= l
;
406 for( int i
=0; i
<rd
->part_count
-1; i
++ ){
407 for( int j
=i
+1; j
<rd
->part_count
; j
++ ){
408 if( rd
->parts
[j
].parent
!= i
){
409 if( !rb_global_has_space() )
412 if( rd
->parts
[j
].type
!= k_bone_collider_capsule
)
415 if( rd
->parts
[i
].type
!= k_bone_collider_capsule
)
418 rb_ct
*buf
= rb_global_buffer();
420 int l
= rb_capsule__capsule( rd
->parts
[i
].rb
.to_world
,
421 &rd
->parts
[i
].inf
.capsule
,
422 rd
->parts
[j
].rb
.to_world
,
423 &rd
->parts
[j
].inf
.capsule
,
426 for( int k
=0; k
<l
; k
++ ){
427 buf
[k
].rba
= &rd
->parts
[i
].rb
;
428 buf
[k
].rbb
= &rd
->parts
[j
].rb
;
431 rb_contact_count
+= l
;
436 if( localplayer
.drowned
)
438 for( int j
=0; j
<rd
->part_count
; j
++ )
440 struct ragdoll_part
*pj
= &rd
->parts
[j
];
444 rb_effect_simple_bouyency( &pj
->rb
, world
->water
.plane
,
445 k_ragdoll_floatyiness
,
446 k_ragdoll_floatydrag
);
454 for( u32 i
=0; i
<rb_contact_count
; i
++ ){
455 rb_ct
*ct
= &rb_contact_buffer
[i
];
458 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
459 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
460 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
461 float vn
= v3_dot( rv
, ct
->n
);
463 contact_velocities
[i
] = vn
;
466 rb_presolve_contacts( rb_contact_buffer
, vg
.time_fixed_delta
,
468 rb_presolve_swingtwist_constraints( rd
->cone_constraints
,
469 rd
->cone_constraints_count
);
474 if( k_ragdoll_debug_collider
){
475 for( u32 i
=0; i
<rd
->part_count
; i
++ ){
476 struct ragdoll_part
*rp
= &rd
->parts
[i
];
478 if( rp
->type
== k_bone_collider_capsule
){
479 vg_line_capsule( rp
->rb
.to_world
,
480 rp
->inf
.capsule
.r
, rp
->inf
.capsule
.h
, rp
->colour
);
482 else if( rp
->type
== k_bone_collider_box
){
483 vg_line_boxf_transformed( rp
->rb
.to_world
,
484 rp
->inf
.box
, rp
->colour
);
489 if( k_ragdoll_debug_constraints
){
490 rb_debug_position_constraints( rd
->position_constraints
,
491 rd
->position_constraints_count
);
493 rb_debug_swingtwist_constraints( rd
->cone_constraints
,
494 rd
->cone_constraints_count
);
498 * SOLVE CONSTRAINTS & Integrate
501 /* the solver is not very quickly converging so... */
502 for( int i
=0; i
<40; i
++ ){
504 rb_solve_contacts( rb_contact_buffer
, rb_contact_count
);
505 rb_solve_swingtwist_constraints( rd
->cone_constraints
,
506 rd
->cone_constraints_count
);
507 rb_postsolve_swingtwist_constraints( rd
->cone_constraints
,
508 rd
->cone_constraints_count
);
510 rb_solve_position_constraints( rd
->position_constraints
,
511 rd
->position_constraints_count
);
514 rb_correct_position_constraints( rd
->position_constraints
,
515 rd
->position_constraints_count
,
516 k_ragdoll_correction
* 0.5f
);
517 rb_correct_swingtwist_constraints( rd
->cone_constraints
,
518 rd
->cone_constraints_count
,
519 k_ragdoll_correction
* 0.25f
);
521 for( int i
=0; i
<rd
->part_count
; i
++ ){
522 rb_iter( &rd
->parts
[i
].rb
);
525 v3_copy( rd
->parts
[i
].rb
.w
, w
);
526 if( v3_length2( w
) > 0.00001f
){
528 v3_muladds( rd
->parts
[i
].rb
.w
, w
, -k_ragdoll_angular_drag
,
533 for( int i
=0; i
<rd
->part_count
; i
++ )
534 rb_update_matrices( &rd
->parts
[i
].rb
);
537 rb_ct
*stress
= NULL
;
538 float max_stress
= 1.0f
;
540 for( u32 i
=0; i
<rb_contact_count
; i
++ ){
541 rb_ct
*ct
= &rb_contact_buffer
[i
];
544 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
545 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
546 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
547 float vn
= v3_dot( rv
, ct
->n
);
549 float s
= fabsf(vn
- contact_velocities
[i
]);
550 if( s
> max_stress
){
556 static u32 temp_filter
= 0;
562 (v3_length2(player_dead
.v_lpf
)>(k_ragdoll_active_threshold
*
563 k_ragdoll_active_threshold
)) ){
564 mdl_keyframe anim
[32];
565 skeleton_sample_anim( &localplayer
.skeleton
, player_dead
.anim_bail
,
568 for( u32 i
=0; i
<rd
->cone_constraints_count
; i
++ ){
569 rb_constr_swingtwist
*st
= &rd
->cone_constraints
[i
];
570 rb_constr_pos
*pc
= &rd
->position_constraints
[i
];
574 m3x3_mulv( st
->rbb
->to_world
, st
->coneva
, va
);
576 /* calculate va as seen in rest position, from the perspective of the
577 * parent object, mapped to pose world space using the parents
578 * transform. thats our target */
580 u32 id_p
= rd
->constraint_associations
[i
][0],
581 id_a
= rd
->constraint_associations
[i
][1];
583 struct ragdoll_part
*pa
= &rd
->parts
[ id_a
],
584 *pp
= &rd
->parts
[ id_p
];
586 mdl_keyframe
*kf
= &anim
[ pa
->bone_id
-1 ];
587 m3x3_mulv( pa
->collider_mtx
, st
->coneva
, vap
);
588 q_mulv( kf
->q
, vap
, vap
);
590 /* This could be a transfer function */
591 m3x3_mulv( pp
->inv_collider_mtx
, vap
, vap
);
592 m3x3_mulv( st
->rba
->to_world
, vap
, vap
);
594 f32 d
= v3_dot( vap
, va
),
595 a
= acosf( vg_clampf( d
, -1.0f
, 1.0f
) );
598 v3_cross( vap
, va
, axis
);
600 f32 Fs
= -a
* k_ragdoll_spring
,
601 Fd
= -v3_dot( st
->rbb
->w
, axis
) * k_ragdoll_dampening
,
605 v3_muls( axis
, F
, torque
);
606 v3_muladds( st
->rbb
->w
, torque
, vg
.time_fixed_delta
, st
->rbb
->w
);
608 /* apply a adjustment to keep velocity at joint 0 */
611 m3x3_mulv( st
->rbb
->to_world
, pc
->lcb
, wcb
);
612 v3_cross( torque
, wcb
, vcb
);
613 v3_muladds( st
->rbb
->v
, vcb
, vg
.time_fixed_delta
, st
->rbb
->v
);
626 audio_oneshot_3d( &audio_hits
[vg_randu32(&vg
.rand
)%5],
627 stress
->co
, 20.0f
, 1.0f
);