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
->flags
& k_bone_flag_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
->flags
& k_bone_flag_collider_capsule
)
62 v3_sub( bone
->hitbox
[1], bone
->hitbox
[0], v0
);
65 float largest
= -1.0f
;
67 for( int i
=0; i
<3; i
++ )
69 if( fabsf( v0
[i
] ) > largest
)
71 largest
= fabsf( v0
[i
] );
77 v1
[ major_axis
] = 1.0f
;
78 rb_tangent_basis( v1
, tx
, ty
);
80 float r
= (fabsf(v3_dot(tx
,v0
)) + fabsf(v3_dot(ty
,v0
))) * 0.25f
,
81 l
= fabsf(v0
[ major_axis
]);
84 v3_muls( tx
, -1.0f
, rp
->collider_mtx
[0] );
85 v3_muls( v1
, -1.0f
, rp
->collider_mtx
[1] );
86 v3_muls( ty
, -1.0f
, rp
->collider_mtx
[2] );
87 v3_add( bone
->hitbox
[0], bone
->hitbox
[1], rp
->collider_mtx
[3] );
88 v3_muls( rp
->collider_mtx
[3], 0.5f
, rp
->collider_mtx
[3] );
90 rp
->rb
.type
= k_rb_shape_capsule
;
91 rp
->rb
.inf
.capsule
.height
= l
;
92 rp
->rb
.inf
.capsule
.radius
= r
;
94 rp
->colour
= 0xff000000 | (0xff << (major_axis
*8));
97 vg_fatal_exit_loop( "Invalid bone collider type" );
99 m4x3_invert_affine( rp
->collider_mtx
, rp
->inv_collider_mtx
);
101 /* Position collider into rest */
102 m3x3_q( rp
->collider_mtx
, rp
->rb
.q
);
103 v3_add( rp
->collider_mtx
[3], bone
->co
, rp
->rb
.co
);
109 * Get parent index in the ragdoll
111 VG_STATIC u32
ragdoll_bone_parent( struct player_ragdoll
*rd
,
112 struct player_avatar
*av
, u32 bone_id
)
114 for( u32 j
=0; j
<rd
->part_count
; j
++ )
115 if( rd
->parts
[ j
].bone_id
== bone_id
)
118 vg_fatal_exit_loop( "Referenced parent bone does not have a rigidbody" );
123 * Setup ragdoll colliders
125 VG_STATIC
void player_setup_ragdoll_from_avatar( struct player_ragdoll
*rd
,
126 struct player_avatar
*av
)
130 if( !av
->sk
.collider_count
)
133 rd
->position_constraints_count
= 0;
134 rd
->cone_constraints_count
= 0;
136 for( u32 i
=0; i
<av
->sk
.bone_count
; i
++ )
138 struct skeleton_bone
*bone
= &av
->sk
.bones
[i
];
141 * Bones with colliders
143 if( !(bone
->flags
& k_bone_flag_collider_any
) )
146 if( rd
->part_count
> vg_list_size(rd
->parts
) )
147 vg_fatal_exit_loop( "Playermodel has too many colliders" );
149 struct ragdoll_part
*rp
= &rd
->parts
[ rd
->part_count
++ ];
151 rp
->parent
= 0xffffffff;
153 player_init_ragdoll_bone_collider( bone
, rp
);
155 struct mdl_node
*pnode
= mdl_node_from_id( &av
->meta
, bone
->orig_node
);
156 struct classtype_bone
*inf
= mdl_get_entdata( &av
->meta
, pnode
);
159 * Bones with collider and parent
164 rp
->parent
= ragdoll_bone_parent( rd
, av
, bone
->parent
);
166 /* Always assign a point-to-point constraint */
167 struct rb_constr_pos
*c
=
168 &rd
->position_constraints
[ rd
->position_constraints_count
++ ];
170 struct skeleton_bone
*bj
= &av
->sk
.bones
[rp
->bone_id
];
171 struct ragdoll_part
*pp
= &rd
->parts
[rp
->parent
];
172 struct skeleton_bone
*bp
= &av
->sk
.bones
[pp
->bone_id
];
174 /* Convention: rba -- parent, rbb -- child */
179 v3_sub( bj
->co
, bp
->co
, delta
);
180 m4x3_mulv( rp
->inv_collider_mtx
, (v3f
){0.0f
,0.0f
,0.0f
}, c
->lcb
);
181 m4x3_mulv( pp
->inv_collider_mtx
, delta
, c
->lca
);
183 if( inf
->flags
& k_bone_flag_cone_constraint
)
185 struct rb_constr_swingtwist
*a
=
186 &rd
->cone_constraints
[ rd
->cone_constraints_count
++ ];
189 a
->conet
= cosf( inf
->conet
)-0.0001f
;
191 /* Store constraint in local space vectors */
192 m3x3_mulv( c
->rba
->to_local
, inf
->conevx
, a
->conevx
);
193 m3x3_mulv( c
->rba
->to_local
, inf
->conevy
, a
->conevy
);
194 m3x3_mulv( c
->rbb
->to_local
, inf
->coneva
, a
->coneva
);
195 v3_copy( c
->lca
, a
->view_offset
);
197 v3_cross( inf
->coneva
, inf
->conevy
, a
->conevxb
);
198 m3x3_mulv( c
->rbb
->to_local
, a
->conevxb
, a
->conevxb
);
200 v3_normalize( a
->conevxb
);
201 v3_normalize( a
->conevx
);
202 v3_normalize( a
->conevy
);
203 v3_normalize( a
->coneva
);
205 a
->conevx
[3] = v3_length( inf
->conevx
);
206 a
->conevy
[3] = v3_length( inf
->conevy
);
214 * Make avatar copy the ragdoll
216 VG_STATIC
void copy_ragdoll_pose_to_avatar( struct player_ragdoll
*rd
,
217 struct player_avatar
*av
)
219 for( int i
=0; i
<rd
->part_count
; i
++ )
221 struct ragdoll_part
*part
= &rd
->parts
[i
];
223 m3x3_identity(offset
);
224 m4x3_mul( part
->rb
.to_world
, part
->inv_collider_mtx
,
225 av
->sk
.final_mtx
[part
->bone_id
] );
228 skeleton_apply_inverses( &av
->sk
);
232 * Make the ragdoll copy the player model
234 VG_STATIC
void copy_avatar_pose_to_ragdoll( struct player_avatar
*av
,
235 struct player_ragdoll
*rd
,
238 for( int i
=0; i
<rd
->part_count
; i
++ )
240 struct ragdoll_part
*part
= &rd
->parts
[i
];
243 u32 bone
= part
->bone_id
;
245 m4x3_mulv( av
->sk
.final_mtx
[bone
], av
->sk
.bones
[bone
].co
, pos
);
246 m3x3_mulv( av
->sk
.final_mtx
[bone
], part
->collider_mtx
[3], offset
);
247 v3_add( pos
, offset
, part
->rb
.co
);
250 m3x3_mul( av
->sk
.final_mtx
[bone
], part
->collider_mtx
, r
);
251 m3x3_q( r
, part
->rb
.q
);
253 v3_copy( velocity
, part
->rb
.v
);
254 v3_zero( part
->rb
.w
);
256 rb_update_transform( &part
->rb
);
261 * Draw rigidbody colliders for ragdoll
263 VG_STATIC
void player_debug_ragdoll(void)
268 * Ragdoll physics step
270 VG_STATIC
void player_ragdoll_iter( struct player_ragdoll
*rd
)
275 if( ragdoll_frame
>= k_ragdoll_div
)
282 for( int i
=0; i
<rd
->part_count
; i
++ )
283 rb_collide( &rd
->parts
[i
].rb
, &world
.rb_geo
);
286 * COLLISION DETECTION
288 for( int i
=0; i
<rd
->part_count
-1; i
++ )
290 for( int j
=i
+1; j
<rd
->part_count
; j
++ )
292 if( rd
->parts
[j
].parent
!= i
)
293 rb_collide( &rd
->parts
[i
].rb
, &rd
->parts
[j
].rb
);
297 for( int j
=0; j
<rd
->part_count
; j
++ )
299 struct ragdoll_part
*pj
= &rd
->parts
[j
];
303 v4f plane
= {0.0f
,1.0f
,0.0f
,0.0f
};
304 rb_effect_simple_bouyency( &pj
->rb
, plane
, k_ragdoll_floatyiness
,
305 k_ragdoll_floatydrag
);
312 rb_presolve_contacts( rb_contact_buffer
, rb_contact_count
);
313 rb_presolve_swingtwist_constraints( rd
->cone_constraints
,
314 rd
->cone_constraints_count
);
319 if( k_ragdoll_debug_collider
)
321 for( u32 i
=0; i
<rd
->part_count
; i
++ )
322 rb_debug( &rd
->parts
[i
].rb
, rd
->parts
[i
].colour
);
325 if( k_ragdoll_debug_constraints
)
327 rb_debug_position_constraints( rd
->position_constraints
,
328 rd
->position_constraints_count
);
330 rb_debug_swingtwist_constraints( rd
->cone_constraints
,
331 rd
->cone_constraints_count
);
339 for( int i
=0; i
<25; i
++ )
341 rb_solve_contacts( rb_contact_buffer
, rb_contact_count
);
342 rb_solve_swingtwist_constraints( rd
->cone_constraints
,
343 rd
->cone_constraints_count
);
344 rb_solve_position_constraints( rd
->position_constraints
,
345 rd
->position_constraints_count
);
348 for( int i
=0; i
<rd
->part_count
; i
++ )
349 rb_iter( &rd
->parts
[i
].rb
);
351 for( int i
=0; i
<rd
->part_count
; i
++ )
352 rb_update_transform( &rd
->parts
[i
].rb
);
354 rb_correct_swingtwist_constraints( rd
->cone_constraints
,
355 rd
->cone_constraints_count
, 0.25f
);
357 rb_correct_position_constraints( rd
->position_constraints
,
358 rd
->position_constraints_count
, 0.5f
);
363 #endif /* PLAYER_RAGDOLL_H */