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