1 #ifndef PLAYER_RAGDOLL_H
2 #define PLAYER_RAGDOLL_H
8 #include "player_model.h"
17 /* Collider transform relative to bone */
31 rb_constr_pos position_constraints
[32];
32 u32 position_constraints_count
;
34 rb_constr_swingtwist cone_constraints
[32];
35 u32 cone_constraints_count
;
40 VG_STATIC
void player_init_ragdoll_bone_collider( struct skeleton_bone
*bone
,
41 struct ragdoll_part
*rp
)
43 m4x3_identity( rp
->collider_mtx
);
45 if( bone
->collider
== k_bone_collider_box
){
47 v3_sub( bone
->hitbox
[1], bone
->hitbox
[0], delta
);
48 v3_muls( delta
, 0.5f
, delta
);
49 v3_add( bone
->hitbox
[0], delta
, rp
->collider_mtx
[3] );
51 v3_copy( delta
, rp
->rb
.bbx
[1] );
52 v3_muls( delta
, -1.0f
, rp
->rb
.bbx
[0] );
54 q_identity( rp
->rb
.q
);
55 rp
->rb
.type
= k_rb_shape_box
;
56 rp
->colour
= 0xffcccccc;
58 else if( bone
->collider
== k_bone_collider_capsule
){
60 v3_sub( bone
->hitbox
[1], bone
->hitbox
[0], v0
);
63 float largest
= -1.0f
;
65 for( int i
=0; i
<3; i
++ ){
66 if( fabsf( v0
[i
] ) > largest
){
67 largest
= fabsf( v0
[i
] );
73 v1
[ major_axis
] = 1.0f
;
74 rb_tangent_basis( v1
, tx
, ty
);
76 float r
= (fabsf(v3_dot(tx
,v0
)) + fabsf(v3_dot(ty
,v0
))) * 0.25f
,
77 l
= fabsf(v0
[ major_axis
]);
80 v3_muls( tx
, -1.0f
, rp
->collider_mtx
[0] );
81 v3_muls( v1
, -1.0f
, rp
->collider_mtx
[1] );
82 v3_muls( ty
, -1.0f
, rp
->collider_mtx
[2] );
83 v3_add( bone
->hitbox
[0], bone
->hitbox
[1], rp
->collider_mtx
[3] );
84 v3_muls( rp
->collider_mtx
[3], 0.5f
, rp
->collider_mtx
[3] );
86 rp
->rb
.type
= k_rb_shape_capsule
;
87 rp
->rb
.inf
.capsule
.height
= l
;
88 rp
->rb
.inf
.capsule
.radius
= r
;
90 rp
->colour
= 0xff000000 | (0xff << (major_axis
*8));
93 vg_fatal_exit_loop( "Invalid bone collider type" );
95 m4x3_invert_affine( rp
->collider_mtx
, rp
->inv_collider_mtx
);
97 /* Position collider into rest */
98 m3x3_q( rp
->collider_mtx
, rp
->rb
.q
);
99 v3_add( rp
->collider_mtx
[3], bone
->co
, rp
->rb
.co
);
105 * Get parent index in the ragdoll
107 VG_STATIC u32
ragdoll_bone_parent( struct player_ragdoll
*rd
,
108 struct player_avatar
*av
, u32 bone_id
)
110 for( u32 j
=0; j
<rd
->part_count
; j
++ )
111 if( rd
->parts
[ j
].bone_id
== bone_id
)
114 vg_fatal_exit_loop( "Referenced parent bone does not have a rigidbody" );
119 * Setup ragdoll colliders
121 VG_STATIC
void player_setup_ragdoll_from_avatar( struct player_ragdoll
*rd
,
122 struct player_avatar
*av
)
126 if( !av
->sk
.collider_count
)
129 rd
->position_constraints_count
= 0;
130 rd
->cone_constraints_count
= 0;
132 for( u32 i
=0; i
<av
->sk
.bone_count
; i
++ ){
133 struct skeleton_bone
*bone
= &av
->sk
.bones
[i
];
136 * Bones with colliders
138 if( !(bone
->collider
) )
141 if( rd
->part_count
> vg_list_size(rd
->parts
) )
142 vg_fatal_exit_loop( "Playermodel has too many colliders" );
144 struct ragdoll_part
*rp
= &rd
->parts
[ rd
->part_count
++ ];
146 rp
->parent
= 0xffffffff;
148 player_init_ragdoll_bone_collider( bone
, rp
);
151 * Bones with collider and parent
156 rp
->parent
= ragdoll_bone_parent( rd
, av
, bone
->parent
);
159 if( bone
->orig_bone
->flags
& k_bone_flag_cone_constraint
){
160 struct rb_constr_pos
*c
=
161 &rd
->position_constraints
[ rd
->position_constraints_count
++ ];
163 struct skeleton_bone
*bj
= &av
->sk
.bones
[rp
->bone_id
];
164 struct ragdoll_part
*pp
= &rd
->parts
[rp
->parent
];
165 struct skeleton_bone
*bp
= &av
->sk
.bones
[pp
->bone_id
];
167 /* Convention: rba -- parent, rbb -- child */
172 v3_sub( bj
->co
, bp
->co
, delta
);
173 m4x3_mulv( rp
->inv_collider_mtx
, (v3f
){0.0f
,0.0f
,0.0f
}, c
->lcb
);
174 m4x3_mulv( pp
->inv_collider_mtx
, delta
, c
->lca
);
177 mdl_bone
*inf
= bone
->orig_bone
;
179 struct rb_constr_swingtwist
*a
=
180 &rd
->cone_constraints
[ rd
->cone_constraints_count
++ ];
184 a
->conet
= cosf( inf
->conet
)-0.0001f
;
186 /* Store constraint in local space vectors */
187 m3x3_mulv( c
->rba
->to_local
, inf
->conevx
, a
->conevx
);
188 m3x3_mulv( c
->rba
->to_local
, inf
->conevy
, a
->conevy
);
189 m3x3_mulv( c
->rbb
->to_local
, inf
->coneva
, a
->coneva
);
190 v3_copy( c
->lca
, a
->view_offset
);
192 v3_cross( inf
->coneva
, inf
->conevy
, a
->conevxb
);
193 m3x3_mulv( c
->rbb
->to_local
, a
->conevxb
, a
->conevxb
);
195 v3_normalize( a
->conevxb
);
196 v3_normalize( a
->conevx
);
197 v3_normalize( a
->conevy
);
198 v3_normalize( a
->coneva
);
200 a
->conevx
[3] = v3_length( inf
->conevx
);
201 a
->conevy
[3] = v3_length( inf
->conevy
);
209 * Make avatar copy the ragdoll
211 VG_STATIC
void copy_ragdoll_pose_to_avatar( struct player_ragdoll
*rd
,
212 struct player_avatar
*av
)
214 for( int i
=0; i
<rd
->part_count
; i
++ ){
215 struct ragdoll_part
*part
= &rd
->parts
[i
];
217 m3x3_identity(offset
);
218 m4x3_mul( part
->rb
.to_world
, part
->inv_collider_mtx
,
219 av
->sk
.final_mtx
[part
->bone_id
] );
222 skeleton_apply_inverses( &av
->sk
);
226 * Make the ragdoll copy the player model
228 VG_STATIC
void copy_avatar_pose_to_ragdoll( struct player_avatar
*av
,
229 struct player_ragdoll
*rd
,
232 for( int i
=0; i
<rd
->part_count
; i
++ ){
233 struct ragdoll_part
*part
= &rd
->parts
[i
];
236 u32 bone
= part
->bone_id
;
238 m4x3_mulv( av
->sk
.final_mtx
[bone
], av
->sk
.bones
[bone
].co
, pos
);
239 m3x3_mulv( av
->sk
.final_mtx
[bone
], part
->collider_mtx
[3], offset
);
240 v3_add( pos
, offset
, part
->rb
.co
);
243 m3x3_mul( av
->sk
.final_mtx
[bone
], part
->collider_mtx
, r
);
244 m3x3_q( r
, part
->rb
.q
);
246 v3_copy( velocity
, part
->rb
.v
);
247 v3_zero( part
->rb
.w
);
249 rb_update_transform( &part
->rb
);
254 * Draw rigidbody colliders for ragdoll
256 VG_STATIC
void player_debug_ragdoll(void)
261 * Ragdoll physics step
263 VG_STATIC
void player_ragdoll_iter( struct player_ragdoll
*rd
)
265 world_instance
*world
= get_active_world();
270 if( ragdoll_frame
>= k_ragdoll_div
){
276 for( int i
=0; i
<rd
->part_count
; i
++ ){
277 if( rb_global_has_space() ){
278 rb_ct
*buf
= rb_global_buffer();
282 if( rd
->parts
[i
].rb
.type
== k_rb_shape_capsule
){
283 l
= rb_capsule__scene( rd
->parts
[i
].rb
.to_world
,
284 &rd
->parts
[i
].rb
.inf
.capsule
,
285 NULL
, &world
->rb_geo
.inf
.scene
, buf
);
287 else if( rd
->parts
[i
].rb
.type
== k_rb_shape_box
){
288 l
= rb_box__scene( rd
->parts
[i
].rb
.to_world
,
290 NULL
, &world
->rb_geo
.inf
.scene
, buf
);
294 for( int j
=0; j
<l
; j
++ ){
295 buf
[j
].rba
= &rd
->parts
[i
].rb
;
296 buf
[j
].rbb
= &world
->rb_geo
;
299 rb_contact_count
+= l
;
306 for( int i
=0; i
<rd
->part_count
-1; i
++ ){
307 for( int j
=i
+1; j
<rd
->part_count
; j
++ ){
308 if( rd
->parts
[j
].parent
!= i
){
309 if( !rb_global_has_space() )
312 if( rd
->parts
[j
].rb
.type
!= k_rb_shape_capsule
)
315 if( rd
->parts
[i
].rb
.type
!= k_rb_shape_capsule
)
318 rb_ct
*buf
= rb_global_buffer();
320 int l
= rb_capsule__capsule( rd
->parts
[i
].rb
.to_world
,
321 &rd
->parts
[i
].rb
.inf
.capsule
,
322 rd
->parts
[j
].rb
.to_world
,
323 &rd
->parts
[j
].rb
.inf
.capsule
,
326 for( int k
=0; k
<l
; k
++ ){
327 buf
[k
].rba
= &rd
->parts
[i
].rb
;
328 buf
[k
].rbb
= &rd
->parts
[j
].rb
;
331 rb_contact_count
+= l
;
336 for( int j
=0; j
<rd
->part_count
; j
++ ){
337 struct ragdoll_part
*pj
= &rd
->parts
[j
];
340 v4f plane
= {0.0f
,1.0f
,0.0f
,0.0f
};
341 rb_effect_simple_bouyency( &pj
->rb
, plane
, k_ragdoll_floatyiness
,
342 k_ragdoll_floatydrag
);
349 rb_presolve_contacts( rb_contact_buffer
, rb_contact_count
);
350 rb_presolve_swingtwist_constraints( rd
->cone_constraints
,
351 rd
->cone_constraints_count
);
356 if( k_ragdoll_debug_collider
){
357 for( u32 i
=0; i
<rd
->part_count
; i
++ )
358 rb_debug( &rd
->parts
[i
].rb
, rd
->parts
[i
].colour
);
361 if( k_ragdoll_debug_constraints
){
362 rb_debug_position_constraints( rd
->position_constraints
,
363 rd
->position_constraints_count
);
365 rb_debug_swingtwist_constraints( rd
->cone_constraints
,
366 rd
->cone_constraints_count
);
373 for( int i
=0; i
<16; i
++ ){
374 rb_solve_contacts( rb_contact_buffer
, rb_contact_count
);
375 rb_solve_swingtwist_constraints( rd
->cone_constraints
,
376 rd
->cone_constraints_count
);
377 rb_solve_position_constraints( rd
->position_constraints
,
378 rd
->position_constraints_count
);
381 for( int i
=0; i
<rd
->part_count
; i
++ )
382 rb_iter( &rd
->parts
[i
].rb
);
384 for( int i
=0; i
<rd
->part_count
; i
++ )
385 rb_update_transform( &rd
->parts
[i
].rb
);
387 rb_correct_swingtwist_constraints( rd
->cone_constraints
,
388 rd
->cone_constraints_count
, 0.25f
);
390 rb_correct_position_constraints( rd
->position_constraints
,
391 rd
->position_constraints_count
, 0.5f
);
396 #endif /* PLAYER_RAGDOLL_H */