1 #ifndef PLAYER_RAGDOLL_H
2 #define PLAYER_RAGDOLL_H
8 #include "player_model.h"
18 /* Collider transform relative to bone */
32 rb_constr_pos position_constraints
[32];
33 u32 position_constraints_count
;
35 rb_constr_swingtwist cone_constraints
[32];
36 u32 cone_constraints_count
;
41 VG_STATIC
void player_init_ragdoll_bone_collider( struct skeleton_bone
*bone
,
42 struct ragdoll_part
*rp
)
44 m4x3_identity( rp
->collider_mtx
);
46 if( bone
->collider
== k_bone_collider_box
){
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] );
52 v3_copy( delta
, rp
->rb
.bbx
[1] );
53 v3_muls( delta
, -1.0f
, rp
->rb
.bbx
[0] );
55 q_identity( rp
->rb
.q
);
56 rp
->rb
.type
= k_rb_shape_box
;
57 rp
->colour
= 0xffcccccc;
59 else if( bone
->collider
== k_bone_collider_capsule
){
61 v3_sub( bone
->hitbox
[1], bone
->hitbox
[0], v0
);
64 float largest
= -1.0f
;
66 for( int i
=0; i
<3; i
++ ){
67 if( fabsf( v0
[i
] ) > largest
){
68 largest
= fabsf( v0
[i
] );
74 v1
[ major_axis
] = 1.0f
;
75 rb_tangent_basis( v1
, tx
, ty
);
77 float r
= (fabsf(v3_dot(tx
,v0
)) + fabsf(v3_dot(ty
,v0
))) * 0.25f
,
78 l
= fabsf(v0
[ major_axis
]);
81 v3_muls( tx
, -1.0f
, rp
->collider_mtx
[0] );
82 v3_muls( v1
, -1.0f
, rp
->collider_mtx
[1] );
83 v3_muls( ty
, -1.0f
, rp
->collider_mtx
[2] );
84 v3_add( bone
->hitbox
[0], bone
->hitbox
[1], rp
->collider_mtx
[3] );
85 v3_muls( rp
->collider_mtx
[3], 0.5f
, rp
->collider_mtx
[3] );
87 rp
->rb
.type
= k_rb_shape_capsule
;
88 rp
->rb
.inf
.capsule
.height
= l
;
89 rp
->rb
.inf
.capsule
.radius
= r
;
91 rp
->colour
= 0xff000000 | (0xff << (major_axis
*8));
94 vg_warn( "type: %u\n", bone
->collider
);
95 vg_fatal_exit_loop( "Invalid bone collider type" );
98 m4x3_invert_affine( rp
->collider_mtx
, rp
->inv_collider_mtx
);
100 /* Position collider into rest */
101 m3x3_q( rp
->collider_mtx
, rp
->rb
.q
);
102 v3_add( rp
->collider_mtx
[3], bone
->co
, rp
->rb
.co
);
108 * Get parent index in the ragdoll
110 VG_STATIC u32
ragdoll_bone_parent( struct player_ragdoll
*rd
,
111 struct player_avatar
*av
, u32 bone_id
)
113 for( u32 j
=0; j
<rd
->part_count
; j
++ )
114 if( rd
->parts
[ j
].bone_id
== bone_id
)
117 vg_fatal_exit_loop( "Referenced parent bone does not have a rigidbody" );
122 * Setup ragdoll colliders
124 VG_STATIC
void player_setup_ragdoll_from_avatar( struct player_ragdoll
*rd
,
125 struct player_avatar
*av
)
129 if( !av
->sk
.collider_count
)
132 rd
->position_constraints_count
= 0;
133 rd
->cone_constraints_count
= 0;
135 for( u32 i
=1; i
<av
->sk
.bone_count
; i
++ ){
136 struct skeleton_bone
*bone
= &av
->sk
.bones
[i
];
139 * Bones with colliders
141 if( !(bone
->collider
) )
144 if( rd
->part_count
> vg_list_size(rd
->parts
) )
145 vg_fatal_exit_loop( "Playermodel has too many colliders" );
147 struct ragdoll_part
*rp
= &rd
->parts
[ rd
->part_count
++ ];
149 rp
->parent
= 0xffffffff;
151 player_init_ragdoll_bone_collider( bone
, rp
);
154 * Bones with collider and parent
159 rp
->parent
= ragdoll_bone_parent( rd
, av
, bone
->parent
);
162 if( bone
->orig_bone
->flags
& k_bone_flag_cone_constraint
){
163 struct rb_constr_pos
*c
=
164 &rd
->position_constraints
[ rd
->position_constraints_count
++ ];
166 struct skeleton_bone
*bj
= &av
->sk
.bones
[rp
->bone_id
];
167 struct ragdoll_part
*pp
= &rd
->parts
[rp
->parent
];
168 struct skeleton_bone
*bp
= &av
->sk
.bones
[pp
->bone_id
];
170 /* Convention: rba -- parent, rbb -- child */
175 v3_sub( bj
->co
, bp
->co
, delta
);
176 m4x3_mulv( rp
->inv_collider_mtx
, (v3f
){0.0f
,0.0f
,0.0f
}, c
->lcb
);
177 m4x3_mulv( pp
->inv_collider_mtx
, delta
, c
->lca
);
180 mdl_bone
*inf
= bone
->orig_bone
;
182 struct rb_constr_swingtwist
*a
=
183 &rd
->cone_constraints
[ rd
->cone_constraints_count
++ ];
187 a
->conet
= cosf( inf
->conet
)-0.0001f
;
189 /* Store constraint in local space vectors */
190 m3x3_mulv( c
->rba
->to_local
, inf
->conevx
, a
->conevx
);
191 m3x3_mulv( c
->rba
->to_local
, inf
->conevy
, a
->conevy
);
192 m3x3_mulv( c
->rbb
->to_local
, inf
->coneva
, a
->coneva
);
193 v3_copy( c
->lca
, a
->view_offset
);
195 v3_cross( inf
->coneva
, inf
->conevy
, a
->conevxb
);
196 m3x3_mulv( c
->rbb
->to_local
, a
->conevxb
, a
->conevxb
);
198 v3_normalize( a
->conevxb
);
199 v3_normalize( a
->conevx
);
200 v3_normalize( a
->conevy
);
201 v3_normalize( a
->coneva
);
203 a
->conevx
[3] = v3_length( inf
->conevx
);
204 a
->conevy
[3] = v3_length( inf
->conevy
);
212 * Make avatar copy the ragdoll
214 VG_STATIC
void copy_ragdoll_pose_to_avatar( struct player_ragdoll
*rd
,
215 struct player_avatar
*av
)
217 for( int i
=0; i
<rd
->part_count
; i
++ ){
218 struct ragdoll_part
*part
= &rd
->parts
[i
];
220 m3x3_identity(offset
);
221 m4x3_mul( part
->rb
.to_world
, part
->inv_collider_mtx
,
222 av
->sk
.final_mtx
[part
->bone_id
] );
225 for( u32 i
=1; i
<av
->sk
.bone_count
; i
++ ){
226 struct skeleton_bone
*sb
= &av
->sk
.bones
[i
];
228 if( sb
->parent
&& !sb
->collider
){
230 v3_sub( av
->sk
.bones
[i
].co
, av
->sk
.bones
[sb
->parent
].co
, delta
);
233 m3x3_identity( posemtx
);
234 v3_copy( delta
, posemtx
[3] );
237 m4x3_mul( av
->sk
.final_mtx
[sb
->parent
], posemtx
, av
->sk
.final_mtx
[i
] );
241 skeleton_apply_inverses( &av
->sk
);
245 * Make the ragdoll copy the player model
247 VG_STATIC
void copy_avatar_pose_to_ragdoll( struct player_avatar
*av
,
248 struct player_ragdoll
*rd
,
251 for( int i
=0; i
<rd
->part_count
; i
++ ){
252 struct ragdoll_part
*part
= &rd
->parts
[i
];
255 u32 bone
= part
->bone_id
;
257 m4x3_mulv( av
->sk
.final_mtx
[bone
], av
->sk
.bones
[bone
].co
, pos
);
258 m3x3_mulv( av
->sk
.final_mtx
[bone
], part
->collider_mtx
[3], offset
);
259 v3_add( pos
, offset
, part
->rb
.co
);
262 m3x3_mul( av
->sk
.final_mtx
[bone
], part
->collider_mtx
, r
);
263 m3x3_q( r
, part
->rb
.q
);
265 v3_copy( velocity
, part
->rb
.v
);
266 v3_zero( part
->rb
.w
);
268 rb_update_transform( &part
->rb
);
273 * Draw rigidbody colliders for ragdoll
275 VG_STATIC
void player_debug_ragdoll(void)
280 * Ragdoll physics step
282 VG_STATIC
void player_ragdoll_iter( struct player_ragdoll
*rd
)
284 world_instance
*world
= get_active_world();
289 if( ragdoll_frame
>= k_ragdoll_div
){
296 float contact_velocities
[256];
298 for( int i
=0; i
<rd
->part_count
; i
++ ){
299 if( rb_global_has_space() ){
300 rb_ct
*buf
= rb_global_buffer();
304 if( rd
->parts
[i
].rb
.type
== k_rb_shape_capsule
){
305 l
= rb_capsule__scene( rd
->parts
[i
].rb
.to_world
,
306 &rd
->parts
[i
].rb
.inf
.capsule
,
307 NULL
, &world
->rb_geo
.inf
.scene
, buf
);
309 else if( rd
->parts
[i
].rb
.type
== k_rb_shape_box
){
310 l
= rb_box__scene( rd
->parts
[i
].rb
.to_world
,
312 NULL
, &world
->rb_geo
.inf
.scene
, buf
);
316 for( int j
=0; j
<l
; j
++ ){
317 buf
[j
].rba
= &rd
->parts
[i
].rb
;
318 buf
[j
].rbb
= &world
->rb_geo
;
321 rb_contact_count
+= l
;
328 for( int i
=0; i
<rd
->part_count
-1; i
++ ){
329 for( int j
=i
+1; j
<rd
->part_count
; j
++ ){
330 if( rd
->parts
[j
].parent
!= i
){
331 if( !rb_global_has_space() )
334 if( rd
->parts
[j
].rb
.type
!= k_rb_shape_capsule
)
337 if( rd
->parts
[i
].rb
.type
!= k_rb_shape_capsule
)
340 rb_ct
*buf
= rb_global_buffer();
342 int l
= rb_capsule__capsule( rd
->parts
[i
].rb
.to_world
,
343 &rd
->parts
[i
].rb
.inf
.capsule
,
344 rd
->parts
[j
].rb
.to_world
,
345 &rd
->parts
[j
].rb
.inf
.capsule
,
348 for( int k
=0; k
<l
; k
++ ){
349 buf
[k
].rba
= &rd
->parts
[i
].rb
;
350 buf
[k
].rbb
= &rd
->parts
[j
].rb
;
353 rb_contact_count
+= l
;
358 for( int j
=0; j
<rd
->part_count
; j
++ ){
359 struct ragdoll_part
*pj
= &rd
->parts
[j
];
362 v4f plane
= {0.0f
,1.0f
,0.0f
,0.0f
};
363 rb_effect_simple_bouyency( &pj
->rb
, plane
, k_ragdoll_floatyiness
,
364 k_ragdoll_floatydrag
);
371 for( u32 i
=0; i
<rb_contact_count
; i
++ ){
372 rb_ct
*ct
= &rb_contact_buffer
[i
];
375 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
376 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
377 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
378 float vn
= v3_dot( rv
, ct
->n
);
380 contact_velocities
[i
] = vn
;
383 rb_presolve_contacts( rb_contact_buffer
, rb_contact_count
);
384 rb_presolve_swingtwist_constraints( rd
->cone_constraints
,
385 rd
->cone_constraints_count
);
390 if( k_ragdoll_debug_collider
){
391 for( u32 i
=0; i
<rd
->part_count
; i
++ )
392 rb_debug( &rd
->parts
[i
].rb
, rd
->parts
[i
].colour
);
395 if( k_ragdoll_debug_constraints
){
396 rb_debug_position_constraints( rd
->position_constraints
,
397 rd
->position_constraints_count
);
399 rb_debug_swingtwist_constraints( rd
->cone_constraints
,
400 rd
->cone_constraints_count
);
407 for( int i
=0; i
<16; i
++ ){
408 rb_solve_contacts( rb_contact_buffer
, rb_contact_count
);
409 rb_solve_swingtwist_constraints( rd
->cone_constraints
,
410 rd
->cone_constraints_count
);
411 rb_solve_position_constraints( rd
->position_constraints
,
412 rd
->position_constraints_count
);
415 for( int i
=0; i
<rd
->part_count
; i
++ )
416 rb_iter( &rd
->parts
[i
].rb
);
418 for( int i
=0; i
<rd
->part_count
; i
++ )
419 rb_update_transform( &rd
->parts
[i
].rb
);
421 rb_correct_swingtwist_constraints( rd
->cone_constraints
,
422 rd
->cone_constraints_count
, 0.25f
);
424 rb_correct_position_constraints( rd
->position_constraints
,
425 rd
->position_constraints_count
, 0.5f
);
428 rb_ct
*stress
= NULL
;
429 float max_stress
= 1.0f
;
431 for( u32 i
=0; i
<rb_contact_count
; i
++ ){
432 rb_ct
*ct
= &rb_contact_buffer
[i
];
435 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
436 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
437 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
438 float vn
= v3_dot( rv
, ct
->n
);
440 float s
= fabsf(vn
- contact_velocities
[i
]);
441 if( s
> max_stress
){
447 static u32 temp_filter
= 0;
457 audio_oneshot_3d( &audio_hits
[rand()%5], stress
->co
, 20.0f
, 1.0f
);
462 #endif /* PLAYER_RAGDOLL_H */