assorted crap
[carveJwlIkooP6JGAAIwe30JlM.git] / player_physics.h
1 /*
2 * Copyright (C) 2021-2022 Mt.ZERO Software, Harry Godden - All Rights Reserved
3 */
4
5 #ifndef PLAYER_PHYSICS_H
6 #define PLAYER_PHYSICS_H
7
8 #include "player.h"
9 #include "camera.h"
10
11 VG_STATIC void apply_gravity( v3f vel, float const timestep )
12 {
13 v3f gravity = { 0.0f, -9.6f, 0.0f };
14 v3_muladds( vel, gravity, timestep, vel );
15 }
16
17 /*
18 * Called when launching into the air to predict and adjust trajectories
19 */
20 VG_STATIC void player_start_air(void)
21 {
22 struct player_phys *phys = &player.phys;
23
24 if( phys->in_air )
25 return;
26
27 phys->in_air = 1;
28
29 float pstep = VG_TIMESTEP_FIXED * 10.0f;
30 float best_velocity_delta = -9999.9f;
31 float k_bias = 0.96f;
32
33 v3f axis;
34 v3_cross( phys->rb.up, phys->rb.v, axis );
35 v3_normalize( axis );
36 player.land_log_count = 0;
37
38 m3x3_identity( phys->vr );
39
40 for( int m=-3;m<=12; m++ )
41 {
42 float vmod = ((float)m / 15.0f)*0.09f;
43
44 v3f pco, pco1, pv;
45 v3_copy( phys->rb.co, pco );
46 v3_muls( phys->rb.v, k_bias, pv );
47
48 /*
49 * Try different 'rotations' of the velocity to find the best possible
50 * landing normal. This conserves magnitude at the expense of slightly
51 * unrealistic results
52 */
53
54 m3x3f vr;
55 v4f vr_q;
56
57 q_axis_angle( vr_q, axis, vmod );
58 q_m3x3( vr_q, vr );
59
60 m3x3_mulv( vr, pv, pv );
61 v3_muladds( pco, pv, pstep, pco );
62
63 for( int i=0; i<50; i++ )
64 {
65 v3_copy( pco, pco1 );
66 apply_gravity( pv, pstep );
67
68 m3x3_mulv( vr, pv, pv );
69 v3_muladds( pco, pv, pstep, pco );
70
71 ray_hit contact;
72 v3f vdir;
73
74 v3_sub( pco, pco1, vdir );
75 contact.dist = v3_length( vdir );
76 v3_divs( vdir, contact.dist, vdir);
77
78 if( ray_world( pco1, vdir, &contact ))
79 {
80 float land_delta = v3_dot( pv, contact.normal );
81 u32 scolour = (u8)(vg_minf(-land_delta * 2.0f, 255.0f));
82
83 /* Bias prediction towords ramps */
84 if( ray_hit_material( &contact )->info.flags
85 & k_material_flag_skate_surface )
86 {
87 land_delta *= 0.1f;
88 scolour |= 0x0000a000;
89 }
90
91 if( (land_delta < 0.0f) && (land_delta > best_velocity_delta) )
92 {
93 best_velocity_delta = land_delta;
94
95 v3_copy( contact.pos, player.land_target );
96
97 m3x3_copy( vr, phys->vr_pstep );
98 q_axis_angle( vr_q, axis, vmod*0.1f );
99 q_m3x3( vr_q, phys->vr );
100 }
101
102 v3_copy( contact.pos,
103 player.land_target_log[player.land_log_count] );
104 player.land_target_colours[player.land_log_count] =
105 0xff000000 | scolour;
106
107 player.land_log_count ++;
108
109 break;
110 }
111 }
112 }
113 }
114
115 /*
116 * Main friction interface model
117 */
118 VG_STATIC void player_physics_control(void)
119 {
120 struct player_phys *phys = &player.phys;
121
122 /*
123 * Computing localized friction forces for controlling the character
124 * Friction across X is significantly more than Z
125 */
126
127 v3f vel;
128 m3x3_mulv( phys->rb.to_local, phys->rb.v, vel );
129 float slip = 0.0f;
130
131 if( fabsf(vel[2]) > 0.01f )
132 slip = fabsf(-vel[0] / vel[2]) * vg_signf(vel[0]);
133
134 if( fabsf( slip ) > 1.2f )
135 slip = vg_signf( slip ) * 1.2f;
136 phys->slip = slip;
137 phys->reverse = -vg_signf(vel[2]);
138
139 float substep = VG_TIMESTEP_FIXED * 0.2f;
140 float fwd_resistance = k_friction_resistance;
141
142 for( int i=0; i<5; i++ )
143 {
144 vel[2] = stable_force( vel[2],vg_signf(vel[2]) * -fwd_resistance*substep);
145 vel[0] = stable_force( vel[0],vg_signf(vel[0]) * -k_friction_lat*substep);
146 }
147
148 if( player.input_jump->button.value )
149 {
150 phys->jump += VG_TIMESTEP_FIXED * k_jump_charge_speed;
151
152 if( !phys->jump_charge )
153 phys->jump_dir = phys->reverse > 0.0f? 1: 0;
154
155 phys->jump_charge = 1;
156 }
157
158 static int push_thresh_last = 0;
159 float push = player.input_push->button.value;
160 int push_thresh = push>0.15f? 1: 0;
161
162 if( push_thresh && !push_thresh_last )
163 player.phys.start_push = vg.time;
164
165 push_thresh_last = push_thresh;
166
167 if( !player.input_jump->button.value && push_thresh )
168 {
169 player.phys.pushing = 1.0f;
170 player.phys.push_time = vg.time - player.phys.start_push;
171
172 float cycle_time = player.phys.push_time*k_push_cycle_rate,
173 amt = k_push_accel * (sinf(cycle_time)*0.5f+0.5f)*VG_TIMESTEP_FIXED,
174 current = v3_length( vel ),
175 new_vel = vg_minf( current + amt, k_max_push_speed );
176
177 new_vel -= vg_minf(current, k_max_push_speed);
178 vel[2] -= new_vel * phys->reverse;
179 }
180
181 m3x3_mulv( phys->rb.to_world, vel, phys->rb.v );
182
183 float input = player.input_js1h->axis.value,
184 grab = player.input_grab->axis.value,
185 steer = input * (1.0f-(phys->jump+grab)*0.4f),
186 steer_scaled = vg_signf(steer) * powf(steer,2.0f) * k_steer_ground;
187
188 phys->iY -= steer_scaled * VG_TIMESTEP_FIXED;
189 }
190
191 /*
192 * Air control, no real physics
193 */
194 VG_STATIC void player_physics_control_air(void)
195 {
196 struct player_phys *phys = &player.phys;
197
198 m3x3_mulv( phys->vr, phys->rb.v, phys->rb.v );
199 vg_line_cross( player.land_target, 0xff0000ff, 0.25f );
200
201 ray_hit hit;
202
203 /*
204 * Prediction
205 */
206 float pstep = VG_TIMESTEP_FIXED * 10.0f;
207
208 v3f pco, pco1, pv;
209 v3_copy( phys->rb.co, pco );
210 v3_copy( phys->rb.v, pv );
211
212 float time_to_impact = 0.0f;
213 float limiter = 1.0f;
214
215 for( int i=0; i<50; i++ )
216 {
217 v3_copy( pco, pco1 );
218 m3x3_mulv( phys->vr_pstep, pv, pv );
219 apply_gravity( pv, pstep );
220 v3_muladds( pco, pv, pstep, pco );
221
222 ray_hit contact;
223 v3f vdir;
224
225 v3_sub( pco, pco1, vdir );
226 contact.dist = v3_length( vdir );
227 v3_divs( vdir, contact.dist, vdir);
228
229 float orig_dist = contact.dist;
230 if( ray_world( pco1, vdir, &contact ))
231 {
232 float angle = v3_dot( phys->rb.up, contact.normal );
233 v3f axis;
234 v3_cross( phys->rb.up, contact.normal, axis );
235
236 time_to_impact += (contact.dist/orig_dist)*pstep;
237 limiter = vg_minf( 5.0f, time_to_impact )/5.0f;
238 limiter = 1.0f-limiter;
239 limiter *= limiter;
240 limiter = 1.0f-limiter;
241
242 if( angle < 0.99f )
243 {
244 v4f correction;
245 q_axis_angle( correction, axis,
246 acosf(angle)*(1.0f-limiter)*3.0f*VG_TIMESTEP_FIXED );
247 q_mul( correction, phys->rb.q, phys->rb.q );
248 }
249
250 vg_line_cross( contact.pos, 0xffff0000, 0.25f );
251 break;
252 }
253 time_to_impact += pstep;
254 }
255
256 v2f steer = { player.input_js1h->axis.value,
257 player.input_js1v->axis.value };
258
259 float l2 = v2_length2( steer );
260 if( l2 > 1.0f )
261 v2_muls( steer, 1.0f/sqrtf(l2), steer );
262
263 phys->iY -= steer[0] * k_steer_air * VG_TIMESTEP_FIXED;
264
265 float iX = steer[1] *
266 phys->reverse * k_steer_air * limiter * VG_TIMESTEP_FIXED;
267
268 static float siX = 0.0f;
269 siX = vg_lerpf( siX, iX, k_steer_air_lerp );
270
271 v4f rotate;
272 q_axis_angle( rotate, phys->rb.right, siX );
273 q_mul( rotate, phys->rb.q, phys->rb.q );
274
275 #if 0
276 v2f target = {0.0f,0.0f};
277 v2_muladds( target, (v2f){ vg_get_axis("grabh"), vg_get_axis("grabv") },
278 phys->grab, target );
279 #endif
280 }
281
282 VG_STATIC void player_walk_update_collision(void)
283 {
284 struct player_phys *phys = &player.phys;
285 float h0 = 0.3f,
286 h1 = 0.9f;
287
288 rigidbody *rbf = &player.collide_front,
289 *rbb = &player.collide_back;
290
291 v3_add( phys->rb.co, (v3f){0.0f,h0,0.0f}, rbf->co );
292 v3_add( phys->rb.co, (v3f){0.0f,h1,0.0f}, rbb->co );
293 v3_copy( rbf->co, rbf->to_world[3] );
294 v3_copy( rbb->co, rbb->to_world[3] );
295 m4x3_invert_affine( rbf->to_world, rbf->to_local );
296 m4x3_invert_affine( rbb->to_world, rbb->to_local );
297
298 rb_update_bounds( rbf );
299 rb_update_bounds( rbb );
300 }
301
302 VG_STATIC void player_integrate(void);
303 /*
304 * Entire Walking physics model
305 * TODO: sleep when under certain velotiy
306 */
307 VG_STATIC void player_walk_physics(void)
308 {
309 struct player_phys *phys = &player.phys;
310 rigidbody *rbf = &player.collide_front,
311 *rbb = &player.collide_back;
312
313 m3x3_identity( player.collide_front.to_world );
314 m3x3_identity( player.collide_back.to_world );
315
316 v3_zero( phys->rb.w );
317 q_axis_angle( phys->rb.q, (v3f){0.0f,1.0f,0.0f}, -player.angles[0] );
318
319 rb_ct manifold[64];
320 int len;
321
322 v3f forward_dir = { sinf(player.angles[0]),0.0f,-cosf(player.angles[0]) };
323 v3f right_dir = { -forward_dir[2], 0.0f, forward_dir[0] };
324
325 v2f walk = { player.input_walkh->axis.value,
326 player.input_walkv->axis.value };
327
328 if( v2_length2(walk) > 0.001f )
329 v2_normalize_clamp( walk );
330
331 if( phys->in_air )
332 {
333 player_walk_update_collision();
334 rb_debug( rbf, 0xff0000ff );
335 rb_debug( rbb, 0xff0000ff );
336
337 /* allow player to accelerate a bit */
338 v3f walk_3d;
339 v3_muls( forward_dir, walk[1], walk_3d );
340 v3_muladds( walk_3d, right_dir, walk[0], walk_3d );
341
342 float current_vel = fabsf(v3_dot( walk_3d, phys->rb.v )),
343 new_vel = current_vel + VG_TIMESTEP_FIXED*k_air_accelerate,
344 clamped_new = vg_clampf( new_vel, 0.0f, k_walkspeed ),
345 vel_diff = vg_maxf( 0.0f, clamped_new - current_vel );
346
347 v3_muladds( phys->rb.v, right_dir, walk[0] * vel_diff, phys->rb.v );
348 v3_muladds( phys->rb.v, forward_dir, walk[1] * vel_diff, phys->rb.v );
349
350
351 len = 0;
352 len += rb_sphere_scene( rbf, &world.rb_geo, manifold+len );
353 len += rb_sphere_scene( rbb, &world.rb_geo, manifold+len );
354 rb_presolve_contacts( manifold, len );
355
356 for( int i=0; i<len; i++ )
357 {
358 struct contact *ct = &manifold[i];
359 if( v3_dot( ct->n, (v3f){0.0f,1.0f,0.0f} ) > 0.5f )
360 phys->in_air = 0;
361 }
362
363 for( int j=0; j<5; j++ )
364 {
365 for( int i=0; i<len; i++ )
366 {
367 struct contact *ct = &manifold[i];
368
369 /*normal */
370 float vn = -v3_dot( phys->rb.v, ct->n );
371 vn += ct->bias;
372
373 float temp = ct->norm_impulse;
374 ct->norm_impulse = vg_maxf( temp + vn, 0.0f );
375 vn = ct->norm_impulse - temp;
376
377 v3f impulse;
378 v3_muls( ct->n, vn, impulse );
379
380 v3_add( impulse, phys->rb.v, phys->rb.v );
381
382 /* friction */
383 for( int j=0; j<2; j++ )
384 {
385 float f = k_friction * ct->norm_impulse,
386 vt = v3_dot( phys->rb.v, ct->t[j] ),
387 lambda = -vt;
388
389 float temp = ct->tangent_impulse[j];
390 ct->tangent_impulse[j] = vg_clampf( temp + lambda, -f, f );
391 lambda = ct->tangent_impulse[j] - temp;
392
393 v3_muladds( phys->rb.v, ct->t[j], lambda, phys->rb.v );
394 }
395 }
396 }
397
398 player_integrate();
399 }
400 else
401 {
402 player.walk = v2_length( walk );
403
404 if( player.input_walk->button.value )
405 v2_muls( walk, 0.5f, walk );
406
407 v2_muls( walk, k_walkspeed * VG_TIMESTEP_FIXED, walk );
408
409 v3f walk_apply;
410 v3_zero( walk_apply );
411
412 /* Do XY translation */
413 v3_muladds( walk_apply, right_dir, walk[0], walk_apply );
414 v3_muladds( walk_apply, forward_dir, walk[1], walk_apply );
415 v3_add( walk_apply, phys->rb.co, phys->rb.co );
416 v3_divs( walk_apply, VG_TIMESTEP_FIXED, phys->rb.v );
417
418 /* Directly resolve collisions */
419 player_walk_update_collision();
420 rb_debug( rbf, 0xffffff00 );
421 rb_debug( rbb, 0xffffff00 );
422
423 len = 0;
424 len += rb_sphere_scene( rbf, &world.rb_geo, manifold+len );
425 len += rb_sphere_scene( rbb, &world.rb_geo, manifold+len );
426
427 v3f dt;
428 v3_zero( dt );
429 for( int j=0; j<3; j++ )
430 {
431 for( int i=0; i<len; i++ )
432 {
433 struct contact *ct = &manifold[i];
434
435 float p = vg_maxf( 0.0f, ct->p - 0.00f ),
436 cur = vg_clampf( v3_dot( ct->n, dt ), 0.0f, p );
437 v3_muladds( dt, ct->n, (p - cur) * 0.333333333f, dt );
438 }
439 }
440 v3_add( dt, phys->rb.co, phys->rb.co );
441
442 if( len )
443 {
444 struct world_material *surface_mat = world_contact_material(manifold);
445 player.surface_prop = surface_mat->info.surface_prop;
446 }
447
448 /* jump */
449 if( player.input_jump->button.value )
450 {
451 phys->rb.v[1] = 5.0f;
452 phys->in_air = 1;
453 return;
454 }
455
456 /* if we've put us in the air, step down slowly */
457 phys->in_air = 1;
458 float max_dist = 0.3f,
459 start_y = phys->rb.co[1];
460
461 for( int j=0; j<8; j++ )
462 {
463 for( int i=0; i<len; i++ )
464 {
465 struct contact *ct = &manifold[i];
466 if( v3_dot( ct->n, (v3f){0.0f,1.0f,0.0f} ) > 0.5f )
467 {
468 phys->in_air = 0;
469 if( j == 0 )
470 return;
471
472 v3f dt;
473 v3_zero( dt );
474 for( int j=0; j<3; j++ )
475 {
476 for( int i=0; i<len; i++ )
477 {
478 struct contact *ct = &manifold[i];
479
480 float p = vg_maxf( 0.0f, ct->p - 0.0025f ),
481 cur = vg_clampf( v3_dot( ct->n, dt ), 0.0f, p );
482 v3_muladds( dt, ct->n, (p - cur) * 0.333333333f, dt );
483 }
484 }
485 v3_add( dt, phys->rb.co, phys->rb.co );
486 return;
487 }
488 }
489
490 phys->rb.co[1] -= max_dist * 0.125f;
491
492 player_walk_update_collision();
493 len = 0;
494 len += rb_sphere_scene( rbf, &world.rb_geo, manifold+len );
495 len += rb_sphere_scene( rbb, &world.rb_geo, manifold+len );
496 }
497
498 /* Transitioning into air mode */
499 phys->rb.co[1] = start_y;
500 }
501 }
502
503 VG_STATIC void player_grind(void)
504 {
505 struct player_phys *phys = &player.phys;
506
507 v3f closest;
508 int idx = bh_closest_point( world.grind_bh, phys->rb.co, closest, INFINITY );
509 if( idx == -1 )
510 return;
511
512 struct grind_edge *edge = &world.grind_edges[ idx ];
513
514 vg_line( phys->rb.co, closest, 0xff000000 );
515 vg_line_cross( closest, 0xff000000, 0.3f );
516 vg_line( edge->p0, edge->p1, 0xff000000 );
517
518 return;
519
520 idx = bh_closest_point( world.geo_bh, phys->rb.co, closest, INFINITY );
521 vg_line( phys->rb.co, closest, 0xff000000 );
522 vg_line_cross( closest, 0xff000000, 0.3f );
523
524 idx = world.scene_geo->arrindices[ idx * 3 ];
525 struct world_material *mat = world_tri_index_material( idx );
526
527 if( mat->info.flags & k_material_flag_grind_surface )
528 {
529 v3f grind_delta;
530 v3_sub( closest, phys->rb.co, grind_delta );
531
532 float p = v3_dot( phys->rb.forward, grind_delta );
533 v3_muladds( grind_delta, phys->rb.forward, -p, grind_delta );
534
535 float a = vg_maxf( 0.0f, 4.0f-v3_dist2( closest, phys->rb.co ) );
536 v3_muladds( phys->rb.v, grind_delta, a*0.2f, phys->rb.v );
537 }
538 }
539
540 /*
541 * Physics collision detection, and control
542 */
543 VG_STATIC void player_physics(void)
544 {
545 struct player_phys *phys = &player.phys;
546 /*
547 * Update collision fronts
548 */
549
550 rigidbody *rbf = &player.collide_front,
551 *rbb = &player.collide_back;
552
553 m3x3_copy( phys->rb.to_world, player.collide_front.to_world );
554 m3x3_copy( phys->rb.to_world, player.collide_back.to_world );
555
556 player.air_blend = vg_lerpf( player.air_blend, phys->in_air, 0.1f );
557 float h = player.air_blend*0.2f;
558
559 m4x3_mulv( phys->rb.to_world, (v3f){0.0f,h,-k_board_length}, rbf->co );
560 v3_copy( rbf->co, rbf->to_world[3] );
561 m4x3_mulv( phys->rb.to_world, (v3f){0.0f,h, k_board_length}, rbb->co );
562 v3_copy( rbb->co, rbb->to_world[3] );
563
564 m4x3_invert_affine( rbf->to_world, rbf->to_local );
565 m4x3_invert_affine( rbb->to_world, rbb->to_local );
566
567 rb_update_bounds( rbf );
568 rb_update_bounds( rbb );
569
570 rb_debug( rbf, 0xff00ffff );
571 rb_debug( rbb, 0xffffff00 );
572
573 rb_ct manifold[64];
574 int len_f = 0,
575 len_b = 0;
576
577 len_f = rb_sphere_scene( rbf, &world.rb_geo, manifold );
578 rb_manifold_filter_coplanar( manifold, len_f, 0.05f );
579 if( len_f > 1 )
580 {
581 rb_manifold_filter_backface( manifold, len_f );
582 rb_manifold_filter_joint_edges( manifold, len_f, 0.05f );
583 rb_manifold_filter_pairs( manifold, len_f, 0.05f );
584 }
585 len_f = rb_manifold_apply_filtered( manifold, len_f );
586
587 rb_ct *man_b = &manifold[len_f];
588 len_b = rb_sphere_scene( rbb, &world.rb_geo, man_b );
589 rb_manifold_filter_coplanar( man_b, len_b, 0.05f );
590 if( len_b > 1 )
591 {
592 rb_manifold_filter_backface( man_b, len_b );
593 rb_manifold_filter_joint_edges( man_b, len_b, 0.05f );
594 rb_manifold_filter_pairs( man_b, len_b, 0.05f );
595 }
596 len_b = rb_manifold_apply_filtered( man_b, len_b );
597
598 int len = len_f+len_b;
599
600 player_grind();
601
602 boxf bax;
603 v3_sub( phys->rb.co, (v3f){2.0f,2.0f,2.0f}, bax[0] );
604 v3_add( phys->rb.co, (v3f){2.0f,2.0f,2.0f}, bax[1] );
605
606 /*
607 * Preprocess collision points, and create a surface picture.
608 * we want contacts that are within our 'capsule's internal line to be
609 * clamped so that they face the line and do not oppose, to stop the
610 * player hanging up on stuff
611 */
612 for( int i=0; i<len; i++ )
613 {
614 v3f dfront, dback;
615 v3_sub( manifold[i].co, rbf->co, dfront );
616 v3_sub( manifold[i].co, rbb->co, dback );
617
618 if( (v3_dot( dfront, phys->rb.forward ) < -0.02f) &&
619 (v3_dot( dback, phys->rb.forward ) > 0.02f))
620 {
621 float p = v3_dot( manifold[i].n, phys->rb.forward );
622 v3_muladds( manifold[i].n, phys->rb.forward, -p, manifold[i].n );
623 v3_normalize( manifold[i].n );
624 }
625 }
626
627 rb_presolve_contacts( manifold, len );
628 v3f surface_avg = {0.0f, 0.0f, 0.0f};
629
630 if( !len )
631 {
632 player_start_air();
633 }
634 else
635 {
636 for( int i=0; i<len; i++ )
637 {
638 v3_add( manifold[i].n, surface_avg, surface_avg );
639 }
640
641 v3_normalize( surface_avg );
642
643 if( v3_dot( phys->rb.v, surface_avg ) > 0.5f )
644 {
645 player_start_air();
646 }
647 else
648 {
649 phys->in_air = 0;
650 }
651 }
652
653 for( int j=0; j<5; j++ )
654 {
655 for( int i=0; i<len; i++ )
656 {
657 struct contact *ct = &manifold[i];
658
659 v3f dv, delta;
660 v3_sub( ct->co, phys->rb.co, delta );
661 v3_cross( phys->rb.w, delta, dv );
662 v3_add( phys->rb.v, dv, dv );
663
664 float vn = -v3_dot( dv, ct->n );
665 vn += ct->bias;
666
667 float temp = ct->norm_impulse;
668 ct->norm_impulse = vg_maxf( temp + vn, 0.0f );
669 vn = ct->norm_impulse - temp;
670
671 v3f impulse;
672 v3_muls( ct->n, vn, impulse );
673
674 if( fabsf(v3_dot( impulse, phys->rb.forward )) > 10.0f ||
675 fabsf(v3_dot( impulse, phys->rb.up )) > 50.0f )
676 {
677 player_kill();
678 return;
679 }
680
681 v3_add( impulse, phys->rb.v, phys->rb.v );
682 v3_cross( delta, impulse, impulse );
683
684 /*
685 * W Impulses are limited to the Y and X axises, we don't really want
686 * roll angular velocities being included.
687 *
688 * Can also tweak the resistance of each axis here by scaling the wx,wy
689 * components.
690 */
691
692 float wy = v3_dot( phys->rb.up, impulse ),
693 wx = v3_dot( phys->rb.right, impulse )*1.5f;
694
695 v3_muladds( phys->rb.w, phys->rb.up, wy, phys->rb.w );
696 v3_muladds( phys->rb.w, phys->rb.right, wx, phys->rb.w );
697 }
698 }
699
700 float grabt = player.input_grab->axis.value;
701
702 if( grabt > 0.5f )
703 {
704 v2_muladds( phys->grab_mouse_delta, vg.mouse_delta, 0.02f,
705 phys->grab_mouse_delta );
706 v2_normalize_clamp( phys->grab_mouse_delta );
707 }
708 else
709 v2_zero( phys->grab_mouse_delta );
710
711 phys->grab = vg_lerpf( phys->grab, grabt, 0.14f );
712 player.phys.pushing = 0.0f;
713
714 if( !phys->in_air )
715 {
716 v3f projected, axis;
717
718 float d = v3_dot( phys->rb.forward, surface_avg );
719 v3_muladds( surface_avg, phys->rb.forward, -d, projected );
720 v3_normalize( projected );
721
722 float angle = v3_dot( phys->rb.up, projected );
723 v3_cross( phys->rb.up, projected, axis );
724
725 v3f p0, p1;
726 v3_add( phys->rb.co, projected, p0 );
727 v3_add( phys->rb.co, phys->rb.up, p1 );
728 vg_line( phys->rb.co, p0, 0xff00ff00 );
729 vg_line( phys->rb.co, p1, 0xff000fff );
730
731 if( fabsf(angle) < 0.999f )
732 {
733 v4f correction;
734 q_axis_angle( correction, axis, acosf(angle)*4.0f*VG_TIMESTEP_FIXED );
735 q_mul( correction, phys->rb.q, phys->rb.q );
736 }
737
738 float const DOWNFORCE = -k_downforce*VG_TIMESTEP_FIXED;
739 v3_muladds( phys->rb.v, phys->rb.up, DOWNFORCE, phys->rb.v );
740
741 player_physics_control();
742
743 if( !phys->jump_charge && phys->jump > 0.2f )
744 {
745 v3f jumpdir;
746
747 /* Launch more up if alignment is up else improve velocity */
748 float aup = fabsf(v3_dot( (v3f){0.0f,1.0f,0.0f}, phys->rb.up )),
749 mod = 0.5f,
750 dir = mod + aup*(1.0f-mod);
751
752 v3_copy( phys->rb.v, jumpdir );
753 v3_normalize( jumpdir );
754 v3_muls( jumpdir, 1.0f-dir, jumpdir );
755 v3_muladds( jumpdir, phys->rb.up, dir, jumpdir );
756 v3_normalize( jumpdir );
757
758 float force = k_jump_force*phys->jump;
759 v3_muladds( phys->rb.v, jumpdir, force, phys->rb.v );
760 phys->jump = 0.0f;
761
762 player.jump_time = vg.time;
763
764 /* TODO: Move to audio file */
765 audio_lock();
766 audio_player_set_flags( &audio_player_extra, AUDIO_FLAG_SPACIAL_3D );
767 audio_player_set_position( &audio_player_extra, phys->rb.co );
768 audio_player_set_vol( &audio_player_extra, 20.0f );
769 audio_player_playclip( &audio_player_extra, &audio_jumps[rand()%2] );
770 audio_unlock();
771 }
772 }
773 else
774 {
775 player_physics_control_air();
776 }
777
778 if( !phys->jump_charge )
779 {
780 phys->jump -= k_jump_charge_speed * VG_TIMESTEP_FIXED;
781 }
782
783 phys->jump_charge = 0;
784 phys->jump = vg_clampf( phys->jump, 0.0f, 1.0f );
785 }
786
787 VG_STATIC void player_save_frame(void)
788 {
789 player.phys_gate_frame = player.phys;
790 }
791
792 VG_STATIC void player_restore_frame(void)
793 {
794 player.phys = player.phys_gate_frame;
795 rb_update_transform( &player.phys.rb );
796 }
797
798 VG_STATIC void player_integrate(void)
799 {
800 struct player_phys *phys = &player.phys;
801 apply_gravity( phys->rb.v, VG_TIMESTEP_FIXED );
802 v3_muladds( phys->rb.co, phys->rb.v, VG_TIMESTEP_FIXED, phys->rb.co );
803 }
804
805 VG_STATIC void player_do_motion(void)
806 {
807 struct player_phys *phys = &player.phys;
808
809 if( world.water.enabled )
810 {
811 if( (phys->rb.co[1] < 0.0f) && !player.is_dead )
812 {
813 audio_lock();
814 audio_player_set_flags( &audio_player_extra, AUDIO_FLAG_SPACIAL_3D );
815 audio_player_set_position( &audio_player_extra, phys->rb.co );
816 audio_player_set_vol( &audio_player_extra, 20.0f );
817 audio_player_playclip( &audio_player_extra, &audio_splash );
818 audio_unlock();
819
820 player_kill();
821 }
822 }
823
824
825 v3f prevco;
826 v3_copy( phys->rb.co, prevco );
827
828 if( phys->on_board )
829 {
830 player_physics();
831 player_integrate();
832 }
833 else
834 player_walk_physics();
835
836
837 /* Real angular velocity integration */
838 v3_lerp( phys->rb.w, (v3f){0.0f,0.0f,0.0f}, 0.125f, phys->rb.w );
839 if( v3_length2( phys->rb.w ) > 0.0f )
840 {
841 v4f rotation;
842 v3f axis;
843 v3_copy( phys->rb.w, axis );
844
845 float mag = v3_length( axis );
846 v3_divs( axis, mag, axis );
847 q_axis_angle( rotation, axis, mag*k_rb_delta );
848 q_mul( rotation, phys->rb.q, phys->rb.q );
849 }
850
851 /* Faux angular velocity */
852 v4f rotate;
853
854 float lerpq = phys->in_air? 0.04f: 0.3f;
855 phys->siY = vg_lerpf( phys->siY, phys->iY, lerpq );
856
857 q_axis_angle( rotate, phys->rb.up, phys->siY );
858 q_mul( rotate, phys->rb.q, phys->rb.q );
859 phys->iY = 0.0f;
860
861 /*
862 * Gate intersection, by tracing a line over the gate planes
863 */
864 for( int i=0; i<world.gate_count; i++ )
865 {
866 struct route_gate *rg = &world.gates[i];
867 teleport_gate *gate = &rg->gate;
868
869 if( gate_intersect( gate, phys->rb.co, prevco ) )
870 {
871 m4x3_mulv( gate->transport, phys->rb.co, phys->rb.co );
872 m3x3_mulv( gate->transport, phys->rb.v, phys->rb.v );
873 m3x3_mulv( gate->transport, phys->vl, phys->vl );
874 m3x3_mulv( gate->transport, phys->v_last, phys->v_last );
875 m3x3_mulv( gate->transport, phys->m, phys->m );
876 m3x3_mulv( gate->transport, phys->bob, phys->bob );
877
878 v4f transport_rotation;
879 m3x3_q( gate->transport, transport_rotation );
880 q_mul( transport_rotation, phys->rb.q, phys->rb.q );
881
882 world_routes_activate_gate( i );
883
884 if( !phys->on_board )
885 {
886 v3f fwd_dir = {cosf(player.angles[0]),
887 0.0f,
888 sinf(player.angles[0])};
889 m3x3_mulv( gate->transport, fwd_dir, fwd_dir );
890
891 player.angles[0] = atan2f( fwd_dir[2], fwd_dir[0] );
892 }
893
894 player.rewind_length = 0;
895 player.rewind_total_length = 0.0f;
896 player.rewind_incrementer = 10000;
897 player_save_frame();
898
899 audio_lock();
900 audio_play_oneshot( &audio_gate_pass, 1.0f );
901 audio_unlock();
902 break;
903 }
904 }
905
906 rb_update_transform( &phys->rb );
907 }
908
909 VG_STATIC void player_freecam(void)
910 {
911 player_mouseview();
912
913 float movespeed = fc_speed;
914 v3f lookdir = { 0.0f, 0.0f, -1.0f },
915 sidedir = { 1.0f, 0.0f, 0.0f };
916
917 m3x3_mulv( camera_mtx, lookdir, lookdir );
918 m3x3_mulv( camera_mtx, sidedir, sidedir );
919
920 static v3f move_vel = { 0.0f, 0.0f, 0.0f };
921
922 /* TODO */
923 #if 0
924 if( vg_get_button( "forward" ) )
925 v3_muladds( move_vel, lookdir, VG_TIMESTEP_FIXED * movespeed, move_vel );
926 if( vg_get_button( "back" ) )
927 v3_muladds( move_vel, lookdir, VG_TIMESTEP_FIXED *-movespeed, move_vel );
928 if( vg_get_button( "left" ) )
929 v3_muladds( move_vel, sidedir, VG_TIMESTEP_FIXED *-movespeed, move_vel );
930 if( vg_get_button( "right" ) )
931 v3_muladds( move_vel, sidedir, VG_TIMESTEP_FIXED * movespeed, move_vel );
932 #endif
933
934 v3_muls( move_vel, 0.7f, move_vel );
935 v3_add( move_vel, player.camera_pos, player.camera_pos );
936 }
937
938 VG_STATIC int reset_player( int argc, char const *argv[] )
939 {
940 struct player_phys *phys = &player.phys;
941 struct respawn_point *rp = NULL, *r;
942
943 if( argc == 1 )
944 {
945 for( int i=0; i<world.spawn_count; i++ )
946 {
947 r = &world.spawns[i];
948 if( !strcmp( r->name, argv[0] ) )
949 {
950 rp = r;
951 break;
952 }
953 }
954
955 if( !rp )
956 vg_warn( "No spawn named '%s'\n", argv[0] );
957 }
958
959 if( !rp )
960 {
961 float min_dist = INFINITY;
962
963 for( int i=0; i<world.spawn_count; i++ )
964 {
965 r = &world.spawns[i];
966 float d = v3_dist2( r->co, phys->rb.co );
967
968 vg_info( "Dist %s : %f\n", r->name, d );
969 if( d < min_dist )
970 {
971 min_dist = d;
972 rp = r;
973 }
974 }
975 }
976
977 if( !rp )
978 {
979 vg_error( "No spawn found\n" );
980 if( !world.spawn_count )
981 return 0;
982
983 rp = &world.spawns[0];
984 }
985
986 player.is_dead = 0;
987
988 m3x3f the_long_way;
989 q_m3x3( rp->q, the_long_way );
990
991 v3f delta = {1.0f,0.0f,0.0f};
992 m3x3_mulv( the_long_way, delta, delta );
993
994 player.angles[0] = atan2f( delta[0], -delta[2] );
995 player.angles[1] = -asinf( delta[1] );
996
997
998 v4_copy( rp->q, phys->rb.q );
999 v3_copy( rp->co, phys->rb.co );
1000 v3_zero( phys->rb.v );
1001
1002 phys->vswitch = 1.0f;
1003 phys->slip_last = 0.0f;
1004 phys->in_air = 1;
1005 phys->on_board = 0;
1006 m3x3_identity( phys->vr );
1007
1008 player.mdl.shoes[0] = 1;
1009 player.mdl.shoes[1] = 1;
1010
1011 rb_update_transform( &phys->rb );
1012 player_save_frame();
1013 return 1;
1014 }
1015
1016 #endif /* PLAYER_PHYSICS_H */