2 * Copyright (C) 2021-2022 Mt.ZERO Software, Harry Godden - All Rights Reserved
5 #ifndef PLAYER_PHYSICS_H
6 #define PLAYER_PHYSICS_H
11 VG_STATIC
void apply_gravity( v3f vel
, float const timestep
)
13 v3f gravity
= { 0.0f
, -9.6f
, 0.0f
};
14 v3_muladds( vel
, gravity
, timestep
, vel
);
18 grind_edge
*player_grind_collect_edge( v3f p0
, v3f p1
,
19 v3f c0
, v3f c1
, float max_dist
)
21 struct player_phys
*phys
= &player
.phys
;
24 bh_iter_init( 0, &it
);
28 box_init_inf( region
);
29 box_addpt( region
, p0
);
30 box_addpt( region
, p1
);
33 v3_add( (v3f
){ k_r
, k_r
, k_r
}, region
[1], region
[1] );
34 v3_add( (v3f
){-k_r
,-k_r
,-k_r
}, region
[0], region
[0] );
36 float closest
= k_r
*k_r
;
37 struct grind_edge
*closest_edge
= NULL
;
40 while( bh_next( world
.grind_bh
, &it
, region
, &idx
) )
42 struct grind_edge
*edge
= &world
.grind_edges
[ idx
];
48 closest_segment_segment( p0
, p1
, edge
->p0
, edge
->p1
, &s
,&t
, pa
, pb
);
63 * Called when launching into the air to predict and adjust trajectories
65 VG_STATIC
void player_start_air(void)
67 struct player_phys
*phys
= &player
.phys
;
69 float pstep
= VG_TIMESTEP_FIXED
* 10.0f
;
70 float best_velocity_delta
= -9999.9f
;
74 v3_cross( phys
->rb
.up
, phys
->rb
.v
, axis
);
76 player
.land_log_count
= 0;
78 m3x3_identity( phys
->vr
);
80 for( int m
=-3;m
<=12; m
++ )
82 struct land_log
*log
= &player
.land_log
[ player
.land_log_count
++ ];
84 log
->colour
= 0xff000000;
86 float vmod
= ((float)m
/ 15.0f
)*0.09f
;
89 v3_copy( phys
->rb
.co
, pco
);
90 v3_muls( phys
->rb
.v
, k_bias
, pv
);
93 * Try different 'rotations' of the velocity to find the best possible
94 * landing normal. This conserves magnitude at the expense of slightly
101 q_axis_angle( vr_q
, axis
, vmod
);
104 m3x3_mulv( vr
, pv
, pv
);
105 v3_muladds( pco
, pv
, pstep
, pco
);
107 struct grind_edge
*best_grind
= NULL
;
108 float closest_grind
= INFINITY
;
110 for( int i
=0; i
<50; i
++ )
112 v3_copy( pco
, pco1
);
113 apply_gravity( pv
, pstep
);
115 m3x3_mulv( vr
, pv
, pv
);
116 v3_muladds( pco
, pv
, pstep
, pco
);
121 v3_sub( pco
, pco1
, vdir
);
122 contact
.dist
= v3_length( vdir
);
123 v3_divs( vdir
, contact
.dist
, vdir
);
126 struct grind_edge
*ge
= player_grind_collect_edge( pco
, pco1
,
129 if( ge
&& (v3_dot((v3f
){0.0f
,1.0f
,0.0f
},vdir
) < -0.2f
) )
131 float d2
= v3_dist2( c0
, c1
);
132 if( d2
< closest_grind
)
139 if( ray_world( pco1
, vdir
, &contact
))
141 float land_delta
= v3_dot( pv
, contact
.normal
);
142 u32 scolour
= (u8
)(vg_minf(-land_delta
* 2.0f
, 255.0f
));
144 /* Bias prediction towords ramps */
145 if( ray_hit_material( &contact
)->info
.flags
146 & k_material_flag_skate_surface
)
149 scolour
|= 0x0000a000;
152 if( (land_delta
< 0.0f
) && (land_delta
> best_velocity_delta
) )
154 best_velocity_delta
= land_delta
;
156 v3_copy( contact
.pos
, player
.land_target
);
158 m3x3_copy( vr
, phys
->vr_pstep
);
159 q_axis_angle( vr_q
, axis
, vmod
*0.1f
);
160 q_m3x3( vr_q
, phys
->vr
);
163 v3_copy( contact
.pos
, log
->positions
[ log
->count
++ ] );
164 log
->colour
= 0xff000000 | scolour
;
168 v3_copy( pco
, log
->positions
[ log
->count
++ ] );
173 log
->colour
= 0xff0000ff;
175 float score
= -closest_grind
* 0.05f
;
177 if( score
> best_velocity_delta
)
179 best_velocity_delta
= score
;
181 m3x3_copy( vr
, phys
->vr_pstep
);
182 q_axis_angle( vr_q
, axis
, vmod
*0.1f
);
183 q_m3x3( vr_q
, phys
->vr
);
190 VG_STATIC
void player_physics_control_passive(void)
192 struct player_phys
*phys
= &player
.phys
;
193 float grabt
= player
.input_grab
->axis
.value
;
197 v2_muladds( phys
->grab_mouse_delta
, vg
.mouse_delta
, 0.02f
,
198 phys
->grab_mouse_delta
);
199 v2_normalize_clamp( phys
->grab_mouse_delta
);
202 v2_zero( phys
->grab_mouse_delta
);
204 phys
->grab
= vg_lerpf( phys
->grab
, grabt
, 0.14f
);
205 player
.phys
.pushing
= 0.0f
;
207 if( !phys
->jump_charge
|| phys
->in_air
)
209 phys
->jump
-= k_jump_charge_speed
* VG_TIMESTEP_FIXED
;
212 phys
->jump_charge
= 0;
213 phys
->jump
= vg_clampf( phys
->jump
, 0.0f
, 1.0f
);
217 * Main friction interface model
219 VG_STATIC
void player_physics_control(void)
221 struct player_phys
*phys
= &player
.phys
;
224 * Computing localized friction forces for controlling the character
225 * Friction across X is significantly more than Z
229 m3x3_mulv( phys
->rb
.to_local
, phys
->rb
.v
, vel
);
232 if( fabsf(vel
[2]) > 0.01f
)
233 slip
= fabsf(-vel
[0] / vel
[2]) * vg_signf(vel
[0]);
235 if( fabsf( slip
) > 1.2f
)
236 slip
= vg_signf( slip
) * 1.2f
;
238 phys
->reverse
= -vg_signf(vel
[2]);
240 float substep
= VG_TIMESTEP_FIXED
* 0.2f
;
241 float fwd_resistance
= k_friction_resistance
;
243 for( int i
=0; i
<5; i
++ )
245 vel
[2] = stable_force( vel
[2],vg_signf(vel
[2]) * -fwd_resistance
*substep
);
246 vel
[0] = stable_force( vel
[0],vg_signf(vel
[0]) * -k_friction_lat
*substep
);
249 if( player
.input_jump
->button
.value
)
251 phys
->jump
+= VG_TIMESTEP_FIXED
* k_jump_charge_speed
;
253 if( !phys
->jump_charge
)
254 phys
->jump_dir
= phys
->reverse
> 0.0f
? 1: 0;
256 phys
->jump_charge
= 1;
259 static int push_thresh_last
= 0;
260 float push
= player
.input_push
->button
.value
;
261 int push_thresh
= push
>0.15f
? 1: 0;
263 if( push_thresh
&& !push_thresh_last
)
264 player
.phys
.start_push
= vg
.time
;
266 push_thresh_last
= push_thresh
;
268 if( !player
.input_jump
->button
.value
&& push_thresh
)
270 player
.phys
.pushing
= 1.0f
;
271 player
.phys
.push_time
= vg
.time
- player
.phys
.start_push
;
273 float cycle_time
= player
.phys
.push_time
*k_push_cycle_rate
,
274 amt
= k_push_accel
* (sinf(cycle_time
)*0.5f
+0.5f
)*VG_TIMESTEP_FIXED
,
275 current
= v3_length( vel
),
276 new_vel
= vg_minf( current
+ amt
, k_max_push_speed
);
278 new_vel
-= vg_minf(current
, k_max_push_speed
);
279 vel
[2] -= new_vel
* phys
->reverse
;
282 m3x3_mulv( phys
->rb
.to_world
, vel
, phys
->rb
.v
);
284 float input
= player
.input_js1h
->axis
.value
,
285 grab
= player
.input_grab
->axis
.value
,
286 steer
= input
* (1.0f
-(phys
->jump
+grab
)*0.4f
),
287 steer_scaled
= vg_signf(steer
) * powf(steer
,2.0f
) * k_steer_ground
;
289 phys
->iY
-= steer_scaled
* VG_TIMESTEP_FIXED
;
291 if( !phys
->jump_charge
&& phys
->jump
> 0.2f
)
295 /* Launch more up if alignment is up else improve velocity */
296 float aup
= fabsf(v3_dot( (v3f
){0.0f
,1.0f
,0.0f
}, phys
->rb
.up
)),
298 dir
= mod
+ aup
*(1.0f
-mod
);
300 v3_copy( phys
->rb
.v
, jumpdir
);
301 v3_normalize( jumpdir
);
302 v3_muls( jumpdir
, 1.0f
-dir
, jumpdir
);
303 v3_muladds( jumpdir
, phys
->rb
.up
, dir
, jumpdir
);
304 v3_normalize( jumpdir
);
306 float force
= k_jump_force
*phys
->jump
;
307 v3_muladds( phys
->rb
.v
, jumpdir
, force
, phys
->rb
.v
);
310 player
.jump_time
= vg
.time
;
312 /* TODO: Move to audio file */
314 audio_player_set_flags( &audio_player_extra
, AUDIO_FLAG_SPACIAL_3D
);
315 audio_player_set_position( &audio_player_extra
, phys
->rb
.co
);
316 audio_player_set_vol( &audio_player_extra
, 20.0f
);
317 audio_player_playclip( &audio_player_extra
, &audio_jumps
[rand()%2] );
322 VG_STATIC
void player_physics_control_grind(void)
324 struct player_phys
*phys
= &player
.phys
;
325 v2f steer
= { player
.input_js1h
->axis
.value
,
326 player
.input_js1v
->axis
.value
};
328 float l2
= v2_length2( steer
);
330 v2_muls( steer
, 1.0f
/sqrtf(l2
), steer
);
332 phys
->iY
-= steer
[0] * k_steer_air
* VG_TIMESTEP_FIXED
;
334 float iX
= steer
[1] * phys
->reverse
* k_steer_air
* VG_TIMESTEP_FIXED
;
336 static float siX
= 0.0f
;
337 siX
= vg_lerpf( siX
, iX
, k_steer_air_lerp
);
340 q_axis_angle( rotate
, phys
->rb
.right
, siX
);
341 q_mul( rotate
, phys
->rb
.q
, phys
->rb
.q
);
347 * Air control, no real physics
349 VG_STATIC
void player_physics_control_air(void)
351 struct player_phys
*phys
= &player
.phys
;
353 m3x3_mulv( phys
->vr
, phys
->rb
.v
, phys
->rb
.v
);
354 vg_line_cross( player
.land_target
, 0xff0000ff, 0.25f
);
361 float pstep
= VG_TIMESTEP_FIXED
* 10.0f
;
364 v3_copy( phys
->rb
.co
, pco
);
365 v3_copy( phys
->rb
.v
, pv
);
367 float time_to_impact
= 0.0f
;
368 float limiter
= 1.0f
;
370 for( int i
=0; i
<50; i
++ )
372 v3_copy( pco
, pco1
);
373 m3x3_mulv( phys
->vr_pstep
, pv
, pv
);
374 apply_gravity( pv
, pstep
);
375 v3_muladds( pco
, pv
, pstep
, pco
);
380 v3_sub( pco
, pco1
, vdir
);
381 contact
.dist
= v3_length( vdir
);
382 v3_divs( vdir
, contact
.dist
, vdir
);
384 float orig_dist
= contact
.dist
;
385 if( ray_world( pco1
, vdir
, &contact
))
387 float angle
= v3_dot( phys
->rb
.up
, contact
.normal
);
389 v3_cross( phys
->rb
.up
, contact
.normal
, axis
);
391 time_to_impact
+= (contact
.dist
/orig_dist
)*pstep
;
392 limiter
= vg_minf( 5.0f
, time_to_impact
)/5.0f
;
393 limiter
= 1.0f
-limiter
;
395 limiter
= 1.0f
-limiter
;
397 if( fabsf(angle
) < 0.99f
)
400 q_axis_angle( correction
, axis
,
401 acosf(angle
)*(1.0f
-limiter
)*3.0f
*VG_TIMESTEP_FIXED
);
402 q_mul( correction
, phys
->rb
.q
, phys
->rb
.q
);
405 vg_line_cross( contact
.pos
, 0xffff0000, 0.25f
);
408 time_to_impact
+= pstep
;
411 v2f steer
= { player
.input_js1h
->axis
.value
,
412 player
.input_js1v
->axis
.value
};
414 float l2
= v2_length2( steer
);
416 v2_muls( steer
, 1.0f
/sqrtf(l2
), steer
);
418 phys
->iY
-= steer
[0] * k_steer_air
* VG_TIMESTEP_FIXED
;
420 float iX
= steer
[1] *
421 phys
->reverse
* k_steer_air
* limiter
* VG_TIMESTEP_FIXED
;
423 static float siX
= 0.0f
;
424 siX
= vg_lerpf( siX
, iX
, k_steer_air_lerp
);
427 q_axis_angle( rotate
, phys
->rb
.right
, siX
);
428 q_mul( rotate
, phys
->rb
.q
, phys
->rb
.q
);
431 v2f target
= {0.0f
,0.0f
};
432 v2_muladds( target
, (v2f
){ vg_get_axis("grabh"), vg_get_axis("grabv") },
433 phys
->grab
, target
);
437 VG_STATIC
void player_walk_update_collision(void)
439 struct player_phys
*phys
= &player
.phys
;
443 rigidbody
*rbf
= &player
.collide_front
,
444 *rbb
= &player
.collide_back
;
446 v3_add( phys
->rb
.co
, (v3f
){0.0f
,h0
,0.0f
}, rbf
->co
);
447 v3_add( phys
->rb
.co
, (v3f
){0.0f
,h1
,0.0f
}, rbb
->co
);
448 v3_copy( rbf
->co
, rbf
->to_world
[3] );
449 v3_copy( rbb
->co
, rbb
->to_world
[3] );
450 m4x3_invert_affine( rbf
->to_world
, rbf
->to_local
);
451 m4x3_invert_affine( rbb
->to_world
, rbb
->to_local
);
453 rb_update_bounds( rbf
);
454 rb_update_bounds( rbb
);
457 VG_STATIC
void player_integrate(void);
459 * Entire Walking physics model
460 * TODO: sleep when under certain velotiy
462 VG_STATIC
void player_walk_physics(void)
464 struct player_phys
*phys
= &player
.phys
;
465 rigidbody
*rbf
= &player
.collide_front
,
466 *rbb
= &player
.collide_back
;
468 m3x3_identity( player
.collide_front
.to_world
);
469 m3x3_identity( player
.collide_back
.to_world
);
471 v3_zero( phys
->rb
.w
);
472 q_axis_angle( phys
->rb
.q
, (v3f
){0.0f
,1.0f
,0.0f
}, -player
.angles
[0] );
477 v3f forward_dir
= { sinf(player
.angles
[0]),0.0f
,-cosf(player
.angles
[0]) };
478 v3f right_dir
= { -forward_dir
[2], 0.0f
, forward_dir
[0] };
480 v2f walk
= { player
.input_walkh
->axis
.value
,
481 player
.input_walkv
->axis
.value
};
483 if( v2_length2(walk
) > 0.001f
)
484 v2_normalize_clamp( walk
);
488 player_walk_update_collision();
489 rb_debug( rbf
, 0xff0000ff );
490 rb_debug( rbb
, 0xff0000ff );
492 /* allow player to accelerate a bit */
494 v3_muls( forward_dir
, walk
[1], walk_3d
);
495 v3_muladds( walk_3d
, right_dir
, walk
[0], walk_3d
);
497 float current_vel
= fabsf(v3_dot( walk_3d
, phys
->rb
.v
)),
498 new_vel
= current_vel
+ VG_TIMESTEP_FIXED
*k_air_accelerate
,
499 clamped_new
= vg_clampf( new_vel
, 0.0f
, k_walkspeed
),
500 vel_diff
= vg_maxf( 0.0f
, clamped_new
- current_vel
);
502 v3_muladds( phys
->rb
.v
, right_dir
, walk
[0] * vel_diff
, phys
->rb
.v
);
503 v3_muladds( phys
->rb
.v
, forward_dir
, walk
[1] * vel_diff
, phys
->rb
.v
);
507 len
+= rb_sphere_scene( rbf
, &world
.rb_geo
, manifold
+len
);
508 len
+= rb_sphere_scene( rbb
, &world
.rb_geo
, manifold
+len
);
509 rb_presolve_contacts( manifold
, len
);
511 for( int i
=0; i
<len
; i
++ )
513 struct contact
*ct
= &manifold
[i
];
514 if( v3_dot( ct
->n
, (v3f
){0.0f
,1.0f
,0.0f
} ) > 0.5f
)
518 for( int j
=0; j
<5; j
++ )
520 for( int i
=0; i
<len
; i
++ )
522 struct contact
*ct
= &manifold
[i
];
525 float vn
= -v3_dot( phys
->rb
.v
, ct
->n
);
528 float temp
= ct
->norm_impulse
;
529 ct
->norm_impulse
= vg_maxf( temp
+ vn
, 0.0f
);
530 vn
= ct
->norm_impulse
- temp
;
533 v3_muls( ct
->n
, vn
, impulse
);
535 v3_add( impulse
, phys
->rb
.v
, phys
->rb
.v
);
538 for( int j
=0; j
<2; j
++ )
540 float f
= k_friction
* ct
->norm_impulse
,
541 vt
= v3_dot( phys
->rb
.v
, ct
->t
[j
] ),
544 float temp
= ct
->tangent_impulse
[j
];
545 ct
->tangent_impulse
[j
] = vg_clampf( temp
+ lambda
, -f
, f
);
546 lambda
= ct
->tangent_impulse
[j
] - temp
;
548 v3_muladds( phys
->rb
.v
, ct
->t
[j
], lambda
, phys
->rb
.v
);
557 player
.walk
= v2_length( walk
);
559 if( player
.input_walk
->button
.value
)
560 v2_muls( walk
, 0.5f
, walk
);
562 v2_muls( walk
, k_walkspeed
* VG_TIMESTEP_FIXED
, walk
);
565 v3_zero( walk_apply
);
567 /* Do XY translation */
568 v3_muladds( walk_apply
, right_dir
, walk
[0], walk_apply
);
569 v3_muladds( walk_apply
, forward_dir
, walk
[1], walk_apply
);
570 v3_add( walk_apply
, phys
->rb
.co
, phys
->rb
.co
);
571 v3_divs( walk_apply
, VG_TIMESTEP_FIXED
, phys
->rb
.v
);
573 /* Directly resolve collisions */
574 player_walk_update_collision();
575 rb_debug( rbf
, 0xffffff00 );
576 rb_debug( rbb
, 0xffffff00 );
579 len
+= rb_sphere_scene( rbf
, &world
.rb_geo
, manifold
+len
);
580 len
+= rb_sphere_scene( rbb
, &world
.rb_geo
, manifold
+len
);
584 for( int j
=0; j
<3; j
++ )
586 for( int i
=0; i
<len
; i
++ )
588 struct contact
*ct
= &manifold
[i
];
590 float p
= vg_maxf( 0.0f
, ct
->p
- 0.00f
),
591 cur
= vg_clampf( v3_dot( ct
->n
, dt
), 0.0f
, p
);
592 v3_muladds( dt
, ct
->n
, (p
- cur
) * 0.333333333f
, dt
);
595 v3_add( dt
, phys
->rb
.co
, phys
->rb
.co
);
599 struct world_material
*surface_mat
= world_contact_material(manifold
);
600 player
.surface_prop
= surface_mat
->info
.surface_prop
;
604 if( player
.input_jump
->button
.value
)
606 phys
->rb
.v
[1] = 5.0f
;
611 /* if we've put us in the air, step down slowly */
613 float max_dist
= 0.3f
,
614 start_y
= phys
->rb
.co
[1];
616 for( int j
=0; j
<8; j
++ )
618 for( int i
=0; i
<len
; i
++ )
620 struct contact
*ct
= &manifold
[i
];
621 if( v3_dot( ct
->n
, (v3f
){0.0f
,1.0f
,0.0f
} ) > 0.5f
)
629 for( int j
=0; j
<3; j
++ )
631 for( int i
=0; i
<len
; i
++ )
633 struct contact
*ct
= &manifold
[i
];
635 float p
= vg_maxf( 0.0f
, ct
->p
- 0.0025f
),
636 cur
= vg_clampf( v3_dot( ct
->n
, dt
), 0.0f
, p
);
637 v3_muladds( dt
, ct
->n
, (p
- cur
) * 0.333333333f
, dt
);
640 v3_add( dt
, phys
->rb
.co
, phys
->rb
.co
);
645 phys
->rb
.co
[1] -= max_dist
* 0.125f
;
647 player_walk_update_collision();
649 len
+= rb_sphere_scene( rbf
, &world
.rb_geo
, manifold
+len
);
650 len
+= rb_sphere_scene( rbb
, &world
.rb_geo
, manifold
+len
);
653 /* Transitioning into air mode */
654 phys
->rb
.co
[1] = start_y
;
658 VG_STATIC
void player_grind(void)
660 struct player_phys
*phys
= &player
.phys
;
663 int idx
= bh_closest_point( world
.grind_bh
, phys
->rb
.co
, closest
, INFINITY
);
667 struct grind_edge
*edge
= &world
.grind_edges
[ idx
];
669 vg_line( phys
->rb
.co
, closest
, 0xff000000 );
670 vg_line_cross( closest
, 0xff000000, 0.3f
);
671 vg_line( edge
->p0
, edge
->p1
, 0xff000000 );
674 v3_sub( closest
, phys
->rb
.co
, grind_delta
);
676 float p
= v3_dot( phys
->rb
.forward
, grind_delta
);
677 v3_muladds( grind_delta
, phys
->rb
.forward
, -p
, grind_delta
);
679 float a
= vg_maxf( 0.0f
, 4.0f
-v3_dist2( closest
, phys
->rb
.co
) );
680 v3_muladds( phys
->rb
.v
, grind_delta
, a
*0.2f
, phys
->rb
.v
);
683 VG_STATIC
int player_update_grind_collision( rb_ct
*contact
)
685 struct player_phys
*phys
= &player
.phys
;
688 v3_muladds( phys
->rb
.co
, phys
->rb
.forward
, 0.5f
, p0
);
689 v3_muladds( phys
->rb
.co
, phys
->rb
.forward
, -0.5f
, p1
);
690 v3_muladds( p0
, phys
->rb
.up
, 0.125f
, p0
);
691 v3_muladds( p1
, phys
->rb
.up
, 0.125f
, p1
);
693 float const k_r
= 0.25f
;
694 struct grind_edge
*closest_edge
= player_grind_collect_edge( p0
, p1
,
698 vg_line( p0
, p1
, 0xff0000ff );
702 vg_line_cross( c0
, 0xff000000, 0.1f
);
703 vg_line_cross( c1
, 0xff000000, 0.1f
);
704 vg_line( c0
, c1
, 0xff000000 );
707 v3_sub( c1
, c0
, delta
);
709 if( v3_dot( delta
, phys
->rb
.up
) > 0.0f
)
711 v3_copy( delta
, contact
->n
);
712 float l
= v3_length( contact
->n
);
713 v3_muls( contact
->n
, 1.0f
/l
, contact
->n
);
715 contact
->type
= k_contact_type_edge
;
716 contact
->element_id
= 0;
717 v3_copy( c1
, contact
->co
);
718 contact
->rba
= &player
.phys
.rb
;
719 contact
->rbb
= &world
.rb_geo
;
721 v3f edge_dir
, axis_dir
;
722 v3_sub( closest_edge
->p1
, closest_edge
->p0
, edge_dir
);
723 v3_normalize( edge_dir
);
724 v3_cross( (v3f
){0.0f
,1.0f
,0.0f
}, edge_dir
, axis_dir
);
725 v3_cross( edge_dir
, axis_dir
, contact
->n
);
736 /* Manifold must be able to hold at least 64 elements */
737 VG_STATIC
int player_update_collision_manifold( rb_ct
*manifold
)
739 struct player_phys
*phys
= &player
.phys
;
741 rigidbody
*rbf
= &player
.collide_front
,
742 *rbb
= &player
.collide_back
;
744 m3x3_copy( phys
->rb
.to_world
, player
.collide_front
.to_world
);
745 m3x3_copy( phys
->rb
.to_world
, player
.collide_back
.to_world
);
747 player
.air_blend
= vg_lerpf( player
.air_blend
, phys
->in_air
, 0.1f
);
748 float h
= player
.air_blend
*0.2f
;
750 m4x3_mulv( phys
->rb
.to_world
, (v3f
){0.0f
,h
,-k_board_length
}, rbf
->co
);
751 v3_copy( rbf
->co
, rbf
->to_world
[3] );
752 m4x3_mulv( phys
->rb
.to_world
, (v3f
){0.0f
,h
, k_board_length
}, rbb
->co
);
753 v3_copy( rbb
->co
, rbb
->to_world
[3] );
755 m4x3_invert_affine( rbf
->to_world
, rbf
->to_local
);
756 m4x3_invert_affine( rbb
->to_world
, rbb
->to_local
);
758 rb_update_bounds( rbf
);
759 rb_update_bounds( rbb
);
761 rb_debug( rbf
, 0xff00ffff );
762 rb_debug( rbb
, 0xffffff00 );
767 len_f
= rb_sphere_scene( rbf
, &world
.rb_geo
, manifold
);
768 rb_manifold_filter_coplanar( manifold
, len_f
, 0.05f
);
771 rb_manifold_filter_backface( manifold
, len_f
);
772 rb_manifold_filter_joint_edges( manifold
, len_f
, 0.05f
);
773 rb_manifold_filter_pairs( manifold
, len_f
, 0.05f
);
775 len_f
= rb_manifold_apply_filtered( manifold
, len_f
);
777 rb_ct
*man_b
= &manifold
[len_f
];
778 len_b
= rb_sphere_scene( rbb
, &world
.rb_geo
, man_b
);
779 rb_manifold_filter_coplanar( man_b
, len_b
, 0.05f
);
782 rb_manifold_filter_backface( man_b
, len_b
);
783 rb_manifold_filter_joint_edges( man_b
, len_b
, 0.05f
);
784 rb_manifold_filter_pairs( man_b
, len_b
, 0.05f
);
786 len_b
= rb_manifold_apply_filtered( man_b
, len_b
);
790 * Preprocess collision points, and create a surface picture.
791 * we want contacts that are within our 'capsule's internal line to be
792 * clamped so that they face the line and do not oppose, to stop the
793 * player hanging up on stuff
795 for( int i
=0; i
<len
; i
++ )
798 v3_sub( manifold
[i
].co
, rbf
->co
, dfront
);
799 v3_sub( manifold
[i
].co
, rbb
->co
, dback
);
801 if( (v3_dot( dfront
, phys
->rb
.forward
) < -0.02f
) &&
802 (v3_dot( dback
, phys
->rb
.forward
) > 0.02f
))
804 float p
= v3_dot( manifold
[i
].n
, phys
->rb
.forward
);
805 v3_muladds( manifold
[i
].n
, phys
->rb
.forward
, -p
, manifold
[i
].n
);
806 v3_normalize( manifold
[i
].n
);
811 return len_f
+ len_b
;
814 VG_STATIC
void player_adhere_ground( rb_ct
*manifold
, int len
)
816 struct player_phys
*phys
= &player
.phys
;
817 int was_in_air
= phys
->in_air
;
820 v3_zero( surface_avg
);
824 phys
->lift_frames
++;
826 if( phys
->lift_frames
>= 8 )
831 for( int i
=0; i
<len
; i
++ )
832 v3_add( surface_avg
, manifold
[i
].n
, surface_avg
);
833 v3_normalize( surface_avg
);
835 if( v3_dot( phys
->rb
.v
, surface_avg
) > 0.7f
)
837 phys
->lift_frames
++;
839 if( phys
->lift_frames
>= 8 )
845 phys
->lift_frames
= 0;
848 float const DOWNFORCE
= -k_downforce
*VG_TIMESTEP_FIXED
;
849 v3_muladds( phys
->rb
.v
, phys
->rb
.up
, DOWNFORCE
, phys
->rb
.v
);
851 float d
= v3_dot( phys
->rb
.forward
, surface_avg
);
852 v3_muladds( surface_avg
, phys
->rb
.forward
, -d
, projected
);
853 v3_normalize( projected
);
855 float angle
= v3_dot( phys
->rb
.up
, projected
);
856 v3_cross( phys
->rb
.up
, projected
, axis
);
859 v3_add( phys
->rb
.co
, projected
, p0
);
860 v3_add( phys
->rb
.co
, phys
->rb
.up
, p1
);
861 vg_line( phys
->rb
.co
, p0
, 0xff00ff00 );
862 vg_line( phys
->rb
.co
, p1
, 0xff000fff );
864 if( fabsf(angle
) < 0.999f
)
867 q_axis_angle( correction
, axis
,
868 acosf(angle
)*4.0f
*VG_TIMESTEP_FIXED
);
869 q_mul( correction
, phys
->rb
.q
, phys
->rb
.q
);
874 if( !was_in_air
&& phys
->in_air
)
878 VG_STATIC
void player_collision_response( rb_ct
*manifold
, int len
)
880 struct player_phys
*phys
= &player
.phys
;
882 for( int j
=0; j
<5; j
++ )
884 for( int i
=0; i
<len
; i
++ )
886 struct contact
*ct
= &manifold
[i
];
889 v3_sub( ct
->co
, phys
->rb
.co
, delta
);
890 v3_cross( phys
->rb
.w
, delta
, dv
);
891 v3_add( phys
->rb
.v
, dv
, dv
);
893 float vn
= -v3_dot( dv
, ct
->n
);
896 float temp
= ct
->norm_impulse
;
897 ct
->norm_impulse
= vg_maxf( temp
+ vn
, 0.0f
);
898 vn
= ct
->norm_impulse
- temp
;
901 v3_muls( ct
->n
, vn
, impulse
);
903 if( fabsf(v3_dot( impulse
, phys
->rb
.forward
)) > 10.0f
||
904 fabsf(v3_dot( impulse
, phys
->rb
.up
)) > 50.0f
)
910 v3_add( impulse
, phys
->rb
.v
, phys
->rb
.v
);
911 v3_cross( delta
, impulse
, impulse
);
914 * W Impulses are limited to the Y and X axises, we don't really want
915 * roll angular velocities being included.
917 * Can also tweak the resistance of each axis here by scaling the wx,wy
921 float wy
= v3_dot( phys
->rb
.up
, impulse
),
922 wx
= v3_dot( phys
->rb
.right
, impulse
)*1.8f
;
924 v3_muladds( phys
->rb
.w
, phys
->rb
.up
, wy
, phys
->rb
.w
);
925 v3_muladds( phys
->rb
.w
, phys
->rb
.right
, wx
, phys
->rb
.w
);
930 VG_STATIC
void player_save_frame(void)
932 player
.phys_gate_frame
= player
.phys
;
935 VG_STATIC
void player_restore_frame(void)
937 player
.phys
= player
.phys_gate_frame
;
938 rb_update_transform( &player
.phys
.rb
);
941 VG_STATIC
void player_integrate(void)
943 struct player_phys
*phys
= &player
.phys
;
944 apply_gravity( phys
->rb
.v
, VG_TIMESTEP_FIXED
);
945 v3_muladds( phys
->rb
.co
, phys
->rb
.v
, VG_TIMESTEP_FIXED
, phys
->rb
.co
);
948 VG_STATIC
void player_do_motion(void)
950 struct player_phys
*phys
= &player
.phys
;
952 if( world
.water
.enabled
)
954 if( (phys
->rb
.co
[1] < 0.0f
) && !player
.is_dead
)
957 audio_player_set_flags( &audio_player_extra
, AUDIO_FLAG_SPACIAL_3D
);
958 audio_player_set_position( &audio_player_extra
, phys
->rb
.co
);
959 audio_player_set_vol( &audio_player_extra
, 20.0f
);
960 audio_player_playclip( &audio_player_extra
, &audio_splash
);
968 v3_copy( phys
->rb
.co
, prevco
);
973 int len
= player_update_collision_manifold( manifold
);
974 int grind_col
= player_update_grind_collision( &manifold
[len
] );
976 static int _grind_col_pre
= 0;
981 v3f up
= { 0.0f
, 1.0f
, 0.0f
};
982 float angle
= v3_dot( phys
->rb
.up
, up
);
984 if( fabsf(angle
) < 0.99f
)
987 v3_cross( phys
->rb
.up
, up
, axis
);
990 q_axis_angle( correction
, axis
,
991 VG_TIMESTEP_FIXED
* 10.0f
* acosf(angle
) );
992 q_mul( correction
, phys
->rb
.q
, phys
->rb
.q
);
995 float const DOWNFORCE
= -k_downforce
*1.2f
*VG_TIMESTEP_FIXED
;
996 v3_muladds( phys
->rb
.v
, manifold
[len
].n
, DOWNFORCE
, phys
->rb
.v
);
997 m3x3_identity( phys
->vr
);
998 m3x3_identity( phys
->vr_pstep
);
1000 if( !_grind_col_pre
)
1003 audio_player_set_flags( &audio_player_extra
,
1004 AUDIO_FLAG_SPACIAL_3D
);
1005 audio_player_set_position( &audio_player_extra
, phys
->rb
.co
);
1006 audio_player_set_vol( &audio_player_extra
, 20.0f
);
1007 audio_player_playclip( &audio_player_extra
, &audio_board
[5] );
1014 player_adhere_ground( manifold
, len
);
1016 if( _grind_col_pre
)
1019 audio_player_set_flags( &audio_player_extra
,
1020 AUDIO_FLAG_SPACIAL_3D
);
1021 audio_player_set_position( &audio_player_extra
, phys
->rb
.co
);
1022 audio_player_set_vol( &audio_player_extra
, 20.0f
);
1023 audio_player_playclip( &audio_player_extra
, &audio_board
[6] );
1028 _grind_col_pre
= grind_col
;
1030 rb_presolve_contacts( manifold
, len
+ VG_MAX(0,grind_col
) );
1031 player_collision_response( manifold
, len
+ VG_MAX(0,grind_col
) );
1033 player_physics_control_passive();
1038 player_physics_control_grind();
1043 player_physics_control_air();
1045 player_physics_control();
1051 player_walk_physics();
1054 /* Real angular velocity integration */
1055 v3_lerp( phys
->rb
.w
, (v3f
){0.0f
,0.0f
,0.0f
}, 0.125f
, phys
->rb
.w
);
1056 if( v3_length2( phys
->rb
.w
) > 0.0f
)
1060 v3_copy( phys
->rb
.w
, axis
);
1062 float mag
= v3_length( axis
);
1063 v3_divs( axis
, mag
, axis
);
1064 q_axis_angle( rotation
, axis
, mag
*k_rb_delta
);
1065 q_mul( rotation
, phys
->rb
.q
, phys
->rb
.q
);
1068 /* Faux angular velocity */
1071 float lerpq
= phys
->in_air
? 0.04f
: 0.3f
;
1072 phys
->siY
= vg_lerpf( phys
->siY
, phys
->iY
, lerpq
);
1074 q_axis_angle( rotate
, phys
->rb
.up
, phys
->siY
);
1075 q_mul( rotate
, phys
->rb
.q
, phys
->rb
.q
);
1079 * Gate intersection, by tracing a line over the gate planes
1081 for( int i
=0; i
<world
.gate_count
; i
++ )
1083 struct route_gate
*rg
= &world
.gates
[i
];
1084 teleport_gate
*gate
= &rg
->gate
;
1086 if( gate_intersect( gate
, phys
->rb
.co
, prevco
) )
1088 m4x3_mulv( gate
->transport
, phys
->rb
.co
, phys
->rb
.co
);
1089 m3x3_mulv( gate
->transport
, phys
->rb
.v
, phys
->rb
.v
);
1090 m3x3_mulv( gate
->transport
, phys
->vl
, phys
->vl
);
1091 m3x3_mulv( gate
->transport
, phys
->v_last
, phys
->v_last
);
1092 m3x3_mulv( gate
->transport
, phys
->m
, phys
->m
);
1093 m3x3_mulv( gate
->transport
, phys
->bob
, phys
->bob
);
1095 v4f transport_rotation
;
1096 m3x3_q( gate
->transport
, transport_rotation
);
1097 q_mul( transport_rotation
, phys
->rb
.q
, phys
->rb
.q
);
1099 world_routes_activate_gate( i
);
1101 if( !phys
->on_board
)
1103 v3f fwd_dir
= {cosf(player
.angles
[0]),
1105 sinf(player
.angles
[0])};
1106 m3x3_mulv( gate
->transport
, fwd_dir
, fwd_dir
);
1108 player
.angles
[0] = atan2f( fwd_dir
[2], fwd_dir
[0] );
1111 player
.rewind_length
= 0;
1112 player
.rewind_total_length
= 0.0f
;
1113 player
.rewind_incrementer
= 10000;
1114 player_save_frame();
1117 audio_play_oneshot( &audio_gate_pass
, 1.0f
);
1123 rb_update_transform( &phys
->rb
);
1126 VG_STATIC
void player_freecam(void)
1130 float movespeed
= fc_speed
;
1131 v3f lookdir
= { 0.0f
, 0.0f
, -1.0f
},
1132 sidedir
= { 1.0f
, 0.0f
, 0.0f
};
1134 m3x3_mulv( main_camera
.transform
, lookdir
, lookdir
);
1135 m3x3_mulv( main_camera
.transform
, sidedir
, sidedir
);
1137 static v3f move_vel
= { 0.0f
, 0.0f
, 0.0f
};
1141 if( vg_get_button( "forward" ) )
1142 v3_muladds( move_vel
, lookdir
, VG_TIMESTEP_FIXED
* movespeed
, move_vel
);
1143 if( vg_get_button( "back" ) )
1144 v3_muladds( move_vel
, lookdir
, VG_TIMESTEP_FIXED
*-movespeed
, move_vel
);
1145 if( vg_get_button( "left" ) )
1146 v3_muladds( move_vel
, sidedir
, VG_TIMESTEP_FIXED
*-movespeed
, move_vel
);
1147 if( vg_get_button( "right" ) )
1148 v3_muladds( move_vel
, sidedir
, VG_TIMESTEP_FIXED
* movespeed
, move_vel
);
1151 v3_muls( move_vel
, 0.7f
, move_vel
);
1152 v3_add( move_vel
, player
.camera_pos
, player
.camera_pos
);
1155 VG_STATIC
int reset_player( int argc
, char const *argv
[] )
1157 struct player_phys
*phys
= &player
.phys
;
1158 struct respawn_point
*rp
= NULL
, *r
;
1162 for( int i
=0; i
<world
.spawn_count
; i
++ )
1164 r
= &world
.spawns
[i
];
1165 if( !strcmp( r
->name
, argv
[0] ) )
1173 vg_warn( "No spawn named '%s'\n", argv
[0] );
1178 float min_dist
= INFINITY
;
1180 for( int i
=0; i
<world
.spawn_count
; i
++ )
1182 r
= &world
.spawns
[i
];
1183 float d
= v3_dist2( r
->co
, phys
->rb
.co
);
1185 vg_info( "Dist %s : %f\n", r
->name
, d
);
1196 vg_error( "No spawn found\n" );
1197 vg_info( "Player position: %f %f %f\n", player
.phys
.rb
.co
[0],
1198 player
.phys
.rb
.co
[1],
1199 player
.phys
.rb
.co
[2] );
1200 vg_info( "Player velocity: %f %f %f\n", player
.phys
.rb
.v
[0],
1201 player
.phys
.rb
.v
[1],
1202 player
.phys
.rb
.v
[2] );
1204 if( !world
.spawn_count
)
1207 rp
= &world
.spawns
[0];
1213 q_m3x3( rp
->q
, the_long_way
);
1215 v3f delta
= {1.0f
,0.0f
,0.0f
};
1216 m3x3_mulv( the_long_way
, delta
, delta
);
1218 player
.angles
[0] = atan2f( delta
[0], -delta
[2] );
1219 player
.angles
[1] = -asinf( delta
[1] );
1222 v4_copy( rp
->q
, phys
->rb
.q
);
1223 v3_copy( rp
->co
, phys
->rb
.co
);
1224 v3_zero( phys
->rb
.v
);
1226 phys
->vswitch
= 1.0f
;
1227 phys
->slip_last
= 0.0f
;
1230 m3x3_identity( phys
->vr
);
1232 player
.mdl
.shoes
[0] = 1;
1233 player
.mdl
.shoes
[1] = 1;
1235 rb_update_transform( &phys
->rb
);
1236 player_save_frame();
1240 #endif /* PLAYER_PHYSICS_H */