e1231bcbd6956ba83e47b70d7697d1059bf15ebb
[carveJwlIkooP6JGAAIwe30JlM.git] / player_ragdoll.c
1 #ifndef PLAYER_RAGDOLL_C
2 #define PLAYER_RAGDOLL_C
3
4 #include "player.h"
5 #include "audio.h"
6
7 static int dev_ragdoll_saveload(int argc, const char *argv[]){
8 if( argc != 2 ){
9 vg_info( "Usage: ragdoll load/save filepath\n" );
10 return 1;
11 }
12
13 if( !strcmp(argv[0],"save") ){
14 FILE *fp = fopen( argv[1], "wb" );
15 if( !fp ){
16 vg_error( "Failed to open file\n" );
17 return 1;
18 }
19 fwrite( &localplayer.ragdoll.parts,
20 sizeof(localplayer.ragdoll.parts), 1, fp );
21 fclose( fp );
22 }
23 else if( !strcmp(argv[0],"load") ){
24 FILE *fp = fopen( argv[1], "rb" );
25 if( !fp ){
26 vg_error( "Failed to open file\n" );
27 return 1;
28 }
29
30 fread( &localplayer.ragdoll.parts,
31 sizeof(localplayer.ragdoll.parts), 1, fp );
32 fclose( fp );
33 }
34 else {
35 vg_error( "Unknown command: %s (options are: save,load)\n", argv[0] );
36 return 1;
37 }
38
39 return 0;
40 }
41
42 static void player_ragdoll_init(void){
43 VG_VAR_F32( k_ragdoll_active_threshold );
44 VG_VAR_F32( k_ragdoll_angular_drag );
45 VG_VAR_F32( k_ragdoll_correction );
46 VG_VAR_F32( k_ragdoll_limit_scale );
47 VG_VAR_F32( k_ragdoll_spring );
48 VG_VAR_F32( k_ragdoll_dampening );
49 VG_VAR_I32( k_ragdoll_div );
50 VG_VAR_I32( k_ragdoll_debug_collider );
51 VG_VAR_I32( k_ragdoll_debug_constraints );
52 vg_console_reg_cmd( "ragdoll", dev_ragdoll_saveload, NULL );
53 }
54
55 static void player_init_ragdoll_bone_collider( struct skeleton_bone *bone,
56 struct ragdoll_part *rp )
57 {
58 m4x3_identity( rp->collider_mtx );
59
60 if( bone->collider == k_bone_collider_box ){
61 v3f delta;
62 v3_sub( bone->hitbox[1], bone->hitbox[0], delta );
63 v3_muls( delta, 0.5f, delta );
64 v3_add( bone->hitbox[0], delta, rp->collider_mtx[3] );
65
66 v3_copy( delta, rp->obj.rb.bbx[1] );
67 v3_muls( delta, -1.0f, rp->obj.rb.bbx[0] );
68
69 q_identity( rp->obj.rb.q );
70 rp->obj.type = k_rb_shape_box;
71 rp->colour = 0xffcccccc;
72 }
73 else if( bone->collider == k_bone_collider_capsule ){
74 v3f v0, v1, tx, ty;
75 v3_sub( bone->hitbox[1], bone->hitbox[0], v0 );
76
77 int major_axis = 0;
78 float largest = -1.0f;
79
80 for( int i=0; i<3; i ++ ){
81 if( fabsf( v0[i] ) > largest ){
82 largest = fabsf( v0[i] );
83 major_axis = i;
84 }
85 }
86
87 v3_zero( v1 );
88 v1[ major_axis ] = 1.0f;
89 v3_tangent_basis( v1, tx, ty );
90
91 float r = (fabsf(v3_dot(tx,v0)) + fabsf(v3_dot(ty,v0))) * 0.25f,
92 l = fabsf(v0[ major_axis ]);
93
94 /* orientation */
95 v3_muls( tx, -1.0f, rp->collider_mtx[0] );
96 v3_muls( v1, -1.0f, rp->collider_mtx[1] );
97 v3_muls( ty, -1.0f, rp->collider_mtx[2] );
98 v3_add( bone->hitbox[0], bone->hitbox[1], rp->collider_mtx[3] );
99 v3_muls( rp->collider_mtx[3], 0.5f, rp->collider_mtx[3] );
100
101 rp->obj.type = k_rb_shape_capsule;
102 rp->obj.inf.capsule.height = l;
103 rp->obj.inf.capsule.radius = r;
104
105 rp->colour = 0xff000000 | (0xff << (major_axis*8));
106 }
107 else{
108 vg_warn( "type: %u\n", bone->collider );
109 vg_fatal_error( "Invalid bone collider type" );
110 }
111
112 m4x3_invert_affine( rp->collider_mtx, rp->inv_collider_mtx );
113
114 /* Position collider into rest */
115 m3x3_q( rp->collider_mtx, rp->obj.rb.q );
116 v3_add( rp->collider_mtx[3], bone->co, rp->obj.rb.co );
117 v3_zero( rp->obj.rb.v );
118 v3_zero( rp->obj.rb.w );
119 rb_init_object( &rp->obj, 4.0f );
120 }
121
122 /*
123 * Get parent index in the ragdoll
124 */
125 static u32 ragdoll_bone_parent( struct player_ragdoll *rd, u32 bone_id ){
126 for( u32 j=0; j<rd->part_count; j++ )
127 if( rd->parts[ j ].bone_id == bone_id )
128 return j;
129
130 vg_fatal_error( "Referenced parent bone does not have a rigidbody" );
131 return 0;
132 }
133
134 /*
135 * Setup ragdoll colliders from skeleton
136 */
137 static void setup_ragdoll_from_skeleton( struct skeleton *sk,
138 struct player_ragdoll *rd ){
139 rd->part_count = 0;
140
141 if( !sk->collider_count )
142 return;
143
144 rd->position_constraints_count = 0;
145 rd->cone_constraints_count = 0;
146
147 for( u32 i=1; i<sk->bone_count; i ++ ){
148 struct skeleton_bone *bone = &sk->bones[i];
149
150 /*
151 * Bones with colliders
152 */
153 if( !(bone->collider) )
154 continue;
155
156 if( rd->part_count > vg_list_size(rd->parts) )
157 vg_fatal_error( "Playermodel has too many colliders" );
158
159 u32 part_id = rd->part_count;
160 rd->part_count ++;
161
162 struct ragdoll_part *rp = &rd->parts[ part_id ];
163 rp->bone_id = i;
164 rp->parent = 0xffffffff;
165
166 player_init_ragdoll_bone_collider( bone, rp );
167
168 /*
169 * Bones with collider and parent
170 */
171 if( !bone->parent )
172 continue;
173
174 rp->parent = ragdoll_bone_parent( rd, bone->parent );
175
176 if( bone->orig_bone->flags & k_bone_flag_cone_constraint ){
177 u32 conid = rd->position_constraints_count;
178 rd->position_constraints_count ++;
179
180 struct rb_constr_pos *c = &rd->position_constraints[ conid ];
181
182 struct skeleton_bone *bj = &sk->bones[rp->bone_id];
183 struct ragdoll_part *pp = &rd->parts[rp->parent];
184 struct skeleton_bone *bp = &sk->bones[pp->bone_id];
185
186 rd->constraint_associations[conid][0] = rp->parent;
187 rd->constraint_associations[conid][1] = part_id;
188
189 /* Convention: rba -- parent, rbb -- child */
190 c->rba = &pp->obj.rb;
191 c->rbb = &rp->obj.rb;
192
193 v3f delta;
194 v3_sub( bj->co, bp->co, delta );
195 m4x3_mulv( rp->inv_collider_mtx, (v3f){0.0f,0.0f,0.0f}, c->lcb );
196 m4x3_mulv( pp->inv_collider_mtx, delta, c->lca );
197
198
199 mdl_bone *inf = bone->orig_bone;
200
201 struct rb_constr_swingtwist *a =
202 &rd->cone_constraints[ rd->cone_constraints_count ++ ];
203
204 a->rba = &pp->obj.rb;
205 a->rbb = &rp->obj.rb;
206 a->conet = cosf( inf->conet )-0.0001f;
207
208 /* Store constraint in local space vectors */
209 m3x3_mulv( c->rba->to_local, inf->conevx, a->conevx );
210 m3x3_mulv( c->rba->to_local, inf->conevy, a->conevy );
211 m3x3_mulv( c->rbb->to_local, inf->coneva, a->coneva );
212 v3_copy( c->lca, a->view_offset );
213
214 v3_cross( inf->coneva, inf->conevy, a->conevxb );
215 m3x3_mulv( c->rbb->to_local, a->conevxb, a->conevxb );
216
217 v3_normalize( a->conevxb );
218 v3_normalize( a->conevx );
219 v3_normalize( a->conevy );
220 v3_normalize( a->coneva );
221
222 a->conevx[3] = v3_length( inf->conevx );
223 a->conevy[3] = v3_length( inf->conevy );
224
225 rp->use_limits = 1;
226 }
227 }
228 }
229
230 /*
231 * Make avatar copy the ragdoll
232 */
233 static void copy_ragdoll_pose_to_localplayer( struct player_ragdoll *rd ){
234 for( int i=0; i<rd->part_count; i++ ){
235 struct ragdoll_part *part = &rd->parts[i];
236
237 m4x3f mtx;
238
239 v4f q_int;
240 v3f co_int;
241
242 float substep = vg.time_fixed_extrapolate;
243 v3_lerp( part->prev_co, part->obj.rb.co, substep, co_int );
244 q_nlerp( part->prev_q, part->obj.rb.q, substep, q_int );
245
246 q_m3x3( q_int, mtx );
247 v3_copy( co_int, mtx[3] );
248
249 m4x3_mul( mtx, part->inv_collider_mtx,
250 localplayer.final_mtx[part->bone_id] );
251 }
252
253 for( u32 i=1; i<localplayer.skeleton.bone_count; i++ ){
254 struct skeleton_bone *sb = &localplayer.skeleton.bones[i];
255
256 if( sb->parent && !sb->collider ){
257 v3f delta;
258 v3_sub( localplayer.skeleton.bones[i].co,
259 localplayer.skeleton.bones[sb->parent].co, delta );
260
261 m4x3f posemtx;
262 m3x3_identity( posemtx );
263 v3_copy( delta, posemtx[3] );
264
265 /* final matrix */
266 m4x3_mul( localplayer.final_mtx[sb->parent], posemtx,
267 localplayer.final_mtx[i] );
268 }
269 }
270
271 skeleton_apply_inverses( &localplayer.skeleton, localplayer.final_mtx );
272 }
273
274 /*
275 * Make the ragdoll copy the player model
276 */
277 static void copy_localplayer_to_ragdoll( struct player_ragdoll *rd,
278 enum player_die_type type ){
279 v3f centroid;
280
281 v3f *bone_mtx = localplayer.final_mtx[localplayer.id_hip];
282 m4x3_mulv( bone_mtx,
283 localplayer.skeleton.bones[localplayer.id_hip].co, centroid );
284
285 for( int i=0; i<rd->part_count; i++ ){
286 struct ragdoll_part *part = &rd->parts[i];
287
288 v3f pos, offset;
289 u32 bone = part->bone_id;
290
291 v3f *bone_mtx = localplayer.final_mtx[bone];
292
293 m4x3_mulv( bone_mtx, localplayer.skeleton.bones[bone].co, pos );
294 m3x3_mulv( bone_mtx, part->collider_mtx[3], offset );
295 v3_add( pos, offset, part->obj.rb.co );
296
297 m3x3f r;
298 m3x3_mul( bone_mtx, part->collider_mtx, r );
299 m3x3_q( r, part->obj.rb.q );
300
301 v3f ra, v;
302 v3_sub( part->obj.rb.co, centroid, ra );
303 v3_cross( localplayer.rb.w, ra, v );
304 v3_add( localplayer.rb.v, v, part->obj.rb.v );
305
306 if( type == k_player_die_type_feet ){
307 if( (bone == localplayer.id_foot_l) ||
308 (bone == localplayer.id_foot_r) ){
309 v3_zero( part->obj.rb.v );
310 }
311 }
312
313 v3_copy( localplayer.rb.w, part->obj.rb.w );
314
315 v3_copy( part->obj.rb.co, part->prev_co );
316 v4_copy( part->obj.rb.q, part->prev_q );
317
318 rb_update_transform( &part->obj.rb );
319 }
320 }
321
322 /*
323 * Ragdoll physics step
324 */
325 static void player_ragdoll_iter( struct player_ragdoll *rd ){
326 world_instance *world = world_current_instance();
327
328 int run_sim = 0;
329 ragdoll_frame ++;
330
331 if( ragdoll_frame >= k_ragdoll_div ){
332 ragdoll_frame = 0;
333 run_sim = 1;
334 }
335
336 rb_solver_reset();
337
338 float contact_velocities[256];
339
340 for( int i=0; i<rd->part_count; i ++ ){
341 v4_copy( rd->parts[i].obj.rb.q, rd->parts[i].prev_q );
342 v3_copy( rd->parts[i].obj.rb.co, rd->parts[i].prev_co );
343
344 if( rb_global_has_space() ){
345 rb_ct *buf = rb_global_buffer();
346
347 int l;
348
349 if( rd->parts[i].obj.type == k_rb_shape_capsule ){
350 l = rb_capsule__scene( rd->parts[i].obj.rb.to_world,
351 &rd->parts[i].obj.inf.capsule,
352 NULL, &world->rb_geo.inf.scene, buf,
353 k_material_flag_ghosts );
354 }
355 else if( rd->parts[i].obj.type == k_rb_shape_box ){
356 l = rb_box__scene( rd->parts[i].obj.rb.to_world,
357 rd->parts[i].obj.rb.bbx,
358 NULL, &world->rb_geo.inf.scene, buf,
359 k_material_flag_ghosts );
360 }
361 else continue;
362
363 for( int j=0; j<l; j++ ){
364 buf[j].rba = &rd->parts[i].obj.rb;
365 buf[j].rbb = &world->rb_geo.rb;
366 }
367
368 rb_contact_count += l;
369 }
370 }
371
372 /*
373 * self-collision
374 */
375 for( int i=0; i<rd->part_count-1; i ++ ){
376 for( int j=i+1; j<rd->part_count; j ++ ){
377 if( rd->parts[j].parent != i ){
378 if( !rb_global_has_space() )
379 break;
380
381 if( rd->parts[j].obj.type != k_rb_shape_capsule )
382 continue;
383
384 if( rd->parts[i].obj.type != k_rb_shape_capsule )
385 continue;
386
387 rb_ct *buf = rb_global_buffer();
388
389 int l = rb_capsule__capsule( rd->parts[i].obj.rb.to_world,
390 &rd->parts[i].obj.inf.capsule,
391 rd->parts[j].obj.rb.to_world,
392 &rd->parts[j].obj.inf.capsule,
393 buf );
394
395 for( int k=0; k<l; k++ ){
396 buf[k].rba = &rd->parts[i].obj.rb;
397 buf[k].rbb = &rd->parts[j].obj.rb;
398 }
399
400 rb_contact_count += l;
401 }
402 }
403 }
404
405 if( world->water.enabled ){
406 for( int j=0; j<rd->part_count; j++ ){
407 struct ragdoll_part *pj = &rd->parts[j];
408
409 if( run_sim ){
410 rb_effect_simple_bouyency( &pj->obj.rb, world->water.plane,
411 k_ragdoll_floatyiness,
412 k_ragdoll_floatydrag );
413 }
414 }
415 }
416
417 /*
418 * PRESOLVE
419 */
420 for( u32 i=0; i<rb_contact_count; i++ ){
421 rb_ct *ct = &rb_contact_buffer[i];
422
423 v3f rv, ra, rb;
424 v3_sub( ct->co, ct->rba->co, ra );
425 v3_sub( ct->co, ct->rbb->co, rb );
426 rb_rcv( ct->rba, ct->rbb, ra, rb, rv );
427 float vn = v3_dot( rv, ct->n );
428
429 contact_velocities[i] = vn;
430 }
431
432 rb_presolve_contacts( rb_contact_buffer, rb_contact_count );
433 rb_presolve_swingtwist_constraints( rd->cone_constraints,
434 rd->cone_constraints_count );
435
436 /*
437 * DEBUG
438 */
439 if( k_ragdoll_debug_collider ){
440 for( u32 i=0; i<rd->part_count; i ++ )
441 rb_object_debug( &rd->parts[i].obj, rd->parts[i].colour );
442 }
443
444 if( k_ragdoll_debug_constraints ){
445 rb_debug_position_constraints( rd->position_constraints,
446 rd->position_constraints_count );
447
448 rb_debug_swingtwist_constraints( rd->cone_constraints,
449 rd->cone_constraints_count );
450 }
451
452 /*
453 * SOLVE CONSTRAINTS & Integrate
454 */
455 if( run_sim ){
456 /* the solver is not very quickly converging so... */
457 for( int i=0; i<40; i++ ){
458 if( i<20 ){
459 rb_solve_contacts( rb_contact_buffer, rb_contact_count );
460 rb_solve_swingtwist_constraints( rd->cone_constraints,
461 rd->cone_constraints_count );
462 rb_postsolve_swingtwist_constraints( rd->cone_constraints,
463 rd->cone_constraints_count );
464 }
465 rb_solve_position_constraints( rd->position_constraints,
466 rd->position_constraints_count );
467 }
468
469 rb_correct_position_constraints( rd->position_constraints,
470 rd->position_constraints_count,
471 k_ragdoll_correction * 0.5f );
472 rb_correct_swingtwist_constraints( rd->cone_constraints,
473 rd->cone_constraints_count,
474 k_ragdoll_correction * 0.25f );
475
476 for( int i=0; i<rd->part_count; i++ ){
477 rb_iter( &rd->parts[i].obj.rb );
478
479 v3f w;
480 v3_copy( rd->parts[i].obj.rb.w, w );
481 if( v3_length2( w ) > 0.00001f ){
482 v3_normalize( w );
483 v3_muladds( rd->parts[i].obj.rb.w, w, -k_ragdoll_angular_drag,
484 rd->parts[i].obj.rb.w );
485 }
486 }
487
488 for( int i=0; i<rd->part_count; i++ )
489 rb_update_transform( &rd->parts[i].obj.rb );
490 }
491
492 rb_ct *stress = NULL;
493 float max_stress = 1.0f;
494
495 for( u32 i=0; i<rb_contact_count; i++ ){
496 rb_ct *ct = &rb_contact_buffer[i];
497
498 v3f rv, ra, rb;
499 v3_sub( ct->co, ct->rba->co, ra );
500 v3_sub( ct->co, ct->rbb->co, rb );
501 rb_rcv( ct->rba, ct->rbb, ra, rb, rv );
502 float vn = v3_dot( rv, ct->n );
503
504 float s = fabsf(vn - contact_velocities[i]);
505 if( s > max_stress ){
506 stress = ct;
507 max_stress = s;
508 }
509 }
510
511 static u32 temp_filter = 0;
512
513 /*
514 * motorized joints
515 */
516 if( run_sim &&
517 (v3_length2(player_dead.v_lpf)>(k_ragdoll_active_threshold*
518 k_ragdoll_active_threshold)) ){
519 assert( rd->cone_constraints_count == rd->position_constraints_count );
520
521 mdl_keyframe anim[32];
522 skeleton_sample_anim( &localplayer.skeleton, player_dead.anim_bail,
523 0.0f, anim );
524
525 for( u32 i=0; i<rd->cone_constraints_count; i ++ ){
526 rb_constr_swingtwist *st = &rd->cone_constraints[i];
527 rb_constr_pos *pc = &rd->position_constraints[i];
528
529 v3f va, vap;
530
531 m3x3_mulv( st->rbb->to_world, st->coneva, va );
532
533 /* calculate va as seen in rest position, from the perspective of the
534 * parent object, mapped to pose world space using the parents
535 * transform. thats our target */
536
537 u32 id_p = rd->constraint_associations[i][0],
538 id_a = rd->constraint_associations[i][1];
539
540 struct ragdoll_part *pa = &rd->parts[ id_a ],
541 *pp = &rd->parts[ id_p ];
542
543 mdl_keyframe *kf = &anim[ pa->bone_id-1 ];
544 m3x3_mulv( pa->collider_mtx, st->coneva, vap );
545 q_mulv( kf->q, vap, vap );
546
547 /* This could be a transfer function */
548 m3x3_mulv( pp->inv_collider_mtx, vap, vap );
549 m3x3_mulv( st->rba->to_world, vap, vap );
550
551 f32 d = v3_dot( vap, va ),
552 a = acosf( vg_clampf( d, -1.0f, 1.0f ) );
553
554 v3f axis;
555 v3_cross( vap, va, axis );
556
557 f32 Fs = -a * k_ragdoll_spring,
558 Fd = -v3_dot( st->rbb->w, axis ) * k_ragdoll_dampening,
559 F = Fs+Fd;
560
561 v3f torque;
562 v3_muls( axis, F, torque );
563 v3_muladds( st->rbb->w, torque, k_rb_delta, st->rbb->w );
564
565 /* apply a adjustment to keep velocity at joint 0 */
566 #if 0
567 v3f wcb, vcb;
568 m3x3_mulv( st->rbb->to_world, pc->lcb, wcb );
569 v3_cross( torque, wcb, vcb );
570 v3_muladds( st->rbb->v, vcb, k_rb_delta, st->rbb->v );
571 #endif
572 }
573 }
574
575 if( temp_filter ){
576 temp_filter --;
577 return;
578 }
579
580 if( stress ){
581 temp_filter = 20;
582 audio_lock();
583 audio_oneshot_3d( &audio_hits[vg_randu32(&vg.rand)%5],
584 stress->co, 20.0f, 1.0f );
585 audio_unlock();
586 }
587 }
588
589 #endif /* PLAYER_RAGDOLL_C */