2 * Copyright (C) 2021-2023 Mt.ZERO Software, Harry Godden - All Rights Reserved
5 #ifndef PLAYER_PHYSICS_SKATE_H
6 #define PLAYER_PHYSICS_SKATE_H
11 VG_STATIC
struct player_skate
18 k_skate_activity_ground
,
19 k_skate_activity_grind
24 v3f v_prev
, a
, m
, bob
, vl
; /* TODO: please clarify */
26 float iY
, siY
; /* Yaw inertia */
28 float vswitch
, slip
, slip_last
, reverse
;
41 int charging_jump
, jump_dir
;
48 float normal_pressure
;
49 v3f debug_mmcollect_lat
,
52 struct land_prediction
77 * Gets the closest grindable edge to the player within max_dist
79 VG_STATIC
struct grind_edge
*player_collect_grind_edge( v3f p0
, v3f p1
,
84 bh_iter_init( 0, &it
);
88 box_init_inf( region
);
89 box_addpt( region
, p0
);
90 box_addpt( region
, p1
);
93 v3_add( (v3f
){ k_r
, k_r
, k_r
}, region
[1], region
[1] );
94 v3_add( (v3f
){-k_r
,-k_r
,-k_r
}, region
[0], region
[0] );
96 float closest
= k_r
*k_r
;
97 struct grind_edge
*closest_edge
= NULL
;
100 while( bh_next( world
.grind_bh
, &it
, region
, &idx
) )
102 struct grind_edge
*edge
= &world
.grind_edges
[ idx
];
108 closest_segment_segment( p0
, p1
, edge
->p0
, edge
->p1
, &s
,&t
, pa
, pb
);
123 * Trace a path given a velocity rotation.
125 * TODO: this MIGHT be worth doing RK4 on the gravity field.
127 VG_STATIC
void player_score_vr_path( v3f co
, v3f v
, m3x3f vr
,
128 struct land_prediction
*prediction
)
130 float pstep
= VG_TIMESTEP_FIXED
* 10.0f
;
131 float k_bias
= 0.96f
;
135 v3_muls( v
, k_bias
, pv
);
137 m3x3_mulv( vr
, pv
, pv
);
138 v3_muladds( pco
, pv
, pstep
, pco
);
140 struct grind_edge
*best_grind
= NULL
;
141 float closest_grind
= INFINITY
;
143 float grind_score
= INFINITY
,
144 air_score
= INFINITY
;
146 prediction
->log_length
= 0;
148 for( int i
=0; i
<vg_list_size(prediction
->log
); i
++ )
150 v3_copy( pco
, pco1
);
152 pv
[1] += -k_gravity
* pstep
;
154 m3x3_mulv( vr
, pv
, pv
);
155 v3_muladds( pco
, pv
, pstep
, pco
);
159 v3_sub( pco
, pco1
, vdir
);
161 float l
= v3_length( vdir
);
162 v3_muls( vdir
, 1.0f
/l
, vdir
);
165 struct grind_edge
*ge
= player_collect_grind_edge( pco
, pco1
,
168 if( ge
&& (v3_dot((v3f
){0.0f
,1.0f
,0.0f
},vdir
) < -0.2f
) )
170 float d2
= v3_dist2( c0
, c1
);
171 if( d2
< closest_grind
)
175 grind_score
= closest_grind
* 0.05f
;
182 int idx
= spherecast_world( pco1
, pco
, 0.4f
, &t1
, n1
);
185 v3_copy( n1
, prediction
->n
);
186 air_score
= -v3_dot( pv
, n1
);
188 u32 vert_index
= world
.scene_geo
->arrindices
[ idx
*3 ];
189 struct world_material
*mat
= world_tri_index_material( vert_index
);
191 /* Bias prediction towords ramps */
192 if( mat
->info
.flags
& k_material_flag_skate_surface
)
195 v3_lerp( pco1
, pco
, t1
, prediction
->log
[ prediction
->log_length
++ ] );
199 v3_copy( pco
, prediction
->log
[ prediction
->log_length
++ ] );
202 if( grind_score
< air_score
)
204 prediction
->score
= grind_score
;
205 prediction
->type
= k_prediction_grind
;
207 else if( air_score
< INFINITY
)
209 prediction
->score
= air_score
;
210 prediction
->type
= k_prediction_land
;
214 prediction
->score
= INFINITY
;
215 prediction
->type
= k_prediction_none
;
220 * Calculate best launch trajectory
222 VG_STATIC
void player_approximate_best_trajectory( struct player_skate
*s
)
224 float pstep
= VG_TIMESTEP_FIXED
* 10.0f
;
225 float best_velocity_delta
= -9999.9f
;
228 v3_cross( player
.rb
.to_world
[1], player
.rb
.v
, axis
);
229 v3_normalize( axis
);
231 s
->prediction_count
= 0;
232 m3x3_identity( s
->phys
.vr
);
236 min_score
= INFINITY
,
237 max_score
= -INFINITY
;
240 * Search a broad selection of futures
242 for( int m
=-3;m
<=12; m
++ )
244 struct land_prediction
*p
= &s
->predictions
[ s
->prediction_count
++ ];
246 float vmod
= ((float)m
/ 15.0f
)*0.09f
;
251 q_axis_angle( vr_q
, axis
, vmod
);
254 player_score_vr_path( player
.rb
.co
, player
.rb
.v
, vr
, p
);
256 if( p
->type
!= k_prediction_none
)
258 if( p
->score
< min_score
)
260 min_score
= p
->score
;
264 if( p
->score
> max_score
)
265 max_score
= p
->score
;
270 q_axis_angle( vr_q
, axis
, best_vmod
*0.1f
);
271 q_m3x3( vr_q
, s
->phys
.vr
);
273 q_axis_angle( vr_q
, axis
, best_vmod
);
274 q_m3x3( vr_q
, s
->phys
.vr_pstep
);
279 for( int i
=0; i
<s
->prediction_count
; i
++ )
281 struct land_prediction
*p
= &s
->predictions
[i
];
287 vg_error( "negative score! (%f)\n", l
);
291 l
/= (max_score
-min_score
);
297 p
->colour
|= 0xff000000;
301 VG_STATIC
void player_skate_apply_grab_model( struct player_skate
*s
)
303 float grabt
= player
.input_grab
->axis
.value
;
307 v2_muladds( s
->phys
.grab_mouse_delta
, vg
.mouse_delta
, 0.02f
,
308 s
->phys
.grab_mouse_delta
);
310 v2_normalize_clamp( s
->phys
.grab_mouse_delta
);
313 v2_zero( s
->phys
.grab_mouse_delta
);
315 s
->phys
.grab
= vg_lerpf( s
->phys
.grab
, grabt
, 8.4f
* VG_TIMESTEP_FIXED
);
319 * Computes friction and surface interface model
321 VG_STATIC
void player_skate_apply_friction_model( struct player_skate
*s
)
323 if( s
->phys
.activity
!= k_skate_activity_ground
)
327 * Computing localized friction forces for controlling the character
328 * Friction across X is significantly more than Z
332 m3x3_mulv( player
.rb
.to_local
, player
.rb
.v
, vel
);
335 if( fabsf(vel
[2]) > 0.01f
)
336 slip
= fabsf(-vel
[0] / vel
[2]) * vg_signf(vel
[0]);
338 if( fabsf( slip
) > 1.2f
)
339 slip
= vg_signf( slip
) * 1.2f
;
342 s
->phys
.reverse
= -vg_signf(vel
[2]);
344 float substep
= VG_TIMESTEP_FIXED
;
345 float fwd_resistance
= k_friction_resistance
;
347 vel
[2] = stable_force( vel
[2],vg_signf(vel
[2]) * -fwd_resistance
*substep
);
348 vel
[0] = stable_force( vel
[0],vg_signf(vel
[0]) * -k_friction_lat
*substep
);
350 /* Pushing additive force */
352 if( !player
.input_jump
->button
.value
)
354 if( player
.input_push
->button
.value
)
356 if( (vg
.time
- s
->phys
.cur_push
) > 0.25 )
358 s
->phys
.start_push
= vg
.time
;
361 s
->phys
.cur_push
= vg
.time
;
363 double push_time
= vg
.time
- s
->phys
.start_push
;
365 float cycle_time
= push_time
*k_push_cycle_rate
,
366 accel
= k_push_accel
* (sinf(cycle_time
)*0.5f
+0.5f
),
367 amt
= accel
* VG_TIMESTEP_FIXED
,
368 current
= v3_length( vel
),
369 new_vel
= vg_minf( current
+ amt
, k_max_push_speed
),
370 delta
= new_vel
- vg_minf( current
, k_max_push_speed
);
372 vel
[2] += delta
* -s
->phys
.reverse
;
376 /* Send back to velocity */
377 m3x3_mulv( player
.rb
.to_world
, vel
, player
.rb
.v
);
380 float input
= player
.input_js1h
->axis
.value
,
381 grab
= player
.input_grab
->axis
.value
,
382 steer
= input
* (1.0f
-(s
->phys
.jump
+grab
)*0.4f
),
383 steer_scaled
= vg_signf(steer
) * powf(steer
,2.0f
) * k_steer_ground
;
385 s
->phys
.iY
-= steer_scaled
* VG_TIMESTEP_FIXED
;
388 VG_STATIC
void player_skate_apply_jump_model( struct player_skate
*s
)
390 int charging_jump_prev
= s
->phys
.charging_jump
;
391 s
->phys
.charging_jump
= player
.input_jump
->button
.value
;
393 /* Cannot charge this in air */
394 if( s
->phys
.activity
!= k_skate_activity_ground
)
395 s
->phys
.charging_jump
= 0;
397 if( s
->phys
.charging_jump
)
399 s
->phys
.jump
+= VG_TIMESTEP_FIXED
* k_jump_charge_speed
;
401 if( !charging_jump_prev
)
402 s
->phys
.jump_dir
= s
->phys
.reverse
> 0.0f
? 1: 0;
406 s
->phys
.jump
-= k_jump_charge_speed
* VG_TIMESTEP_FIXED
;
409 s
->phys
.jump
= vg_clampf( s
->phys
.jump
, 0.0f
, 1.0f
);
411 if( s
->phys
.activity
== k_skate_activity_air
)
414 /* player let go after charging past 0.2: trigger jump */
415 if( !s
->phys
.charging_jump
&& s
->phys
.jump
> 0.2f
)
419 /* Launch more up if alignment is up else improve velocity */
420 float aup
= v3_dot( (v3f
){0.0f
,1.0f
,0.0f
}, player
.rb
.to_world
[1] ),
422 dir
= mod
+ fabsf(aup
)*(1.0f
-mod
);
424 v3_copy( player
.rb
.v
, jumpdir
);
425 v3_normalize( jumpdir
);
426 v3_muls( jumpdir
, 1.0f
-dir
, jumpdir
);
427 v3_muladds( jumpdir
, player
.rb
.to_world
[1], dir
, jumpdir
);
428 v3_normalize( jumpdir
);
430 float force
= k_jump_force
*s
->phys
.jump
;
431 v3_muladds( player
.rb
.v
, jumpdir
, force
, player
.rb
.v
);
434 player
.jump_time
= vg
.time
;
436 /* TODO: Move to audio file */
438 audio_player_set_flags( &audio_player_extra
, AUDIO_FLAG_SPACIAL_3D
);
439 audio_player_set_position( &audio_player_extra
, player
.rb
.co
);
440 audio_player_set_vol( &audio_player_extra
, 20.0f
);
441 audio_player_playclip( &audio_player_extra
, &audio_jumps
[rand()%2] );
446 VG_STATIC
void player_skate_apply_grind_model( struct player_skate
*s
,
447 rb_ct
*manifold
, int len
)
451 if( s
->phys
.activity
== k_skate_activity_grind
)
454 audio_player_set_flags( &audio_player_extra
,
455 AUDIO_FLAG_SPACIAL_3D
);
456 audio_player_set_position( &audio_player_extra
, player
.rb
.co
);
457 audio_player_set_vol( &audio_player_extra
, 20.0f
);
458 audio_player_playclip( &audio_player_extra
, &audio_board
[6] );
461 s
->phys
.activity
= k_skate_activity_air
;
466 v2f steer
= { player
.input_js1h
->axis
.value
,
467 player
.input_js1v
->axis
.value
};
469 float l2
= v2_length2( steer
);
471 v2_muls( steer
, 1.0f
/sqrtf(l2
), steer
);
473 s
->phys
.iY
-= steer
[0] * k_steer_air
* VG_TIMESTEP_FIXED
;
475 float iX
= steer
[1] * s
->phys
.reverse
476 * k_steer_air
* VG_TIMESTEP_FIXED
;
478 static float siX
= 0.0f
;
479 siX
= vg_lerpf( siX
, iX
, k_steer_air_lerp
);
482 q_axis_angle( rotate
, player
.rb
.to_world
[0], siX
);
483 q_mul( rotate
, player
.rb
.q
, player
.rb
.q
);
486 s
->phys
.activity
= k_skate_activity_grind
;
488 v3f up
= { 0.0f
, 1.0f
, 0.0f
};
489 float angle
= v3_dot( player
.rb
.to_world
[1], up
);
491 if( fabsf(angle
) < 0.99f
)
494 v3_cross( player
.rb
.to_world
[1], up
, axis
);
497 q_axis_angle( correction
, axis
,
498 VG_TIMESTEP_FIXED
* 10.0f
* acosf(angle
) );
499 q_mul( correction
, player
.rb
.q
, player
.rb
.q
);
502 float const DOWNFORCE
= -k_downforce
*1.2f
*VG_TIMESTEP_FIXED
;
503 v3_muladds( player
.rb
.v
, manifold
->n
, DOWNFORCE
, player
.rb
.v
);
504 m3x3_identity( s
->phys
.vr
);
505 m3x3_identity( s
->phys
.vr_pstep
);
507 if( s
->phys
.activity_prev
!= k_skate_activity_grind
)
510 audio_player_set_flags( &audio_player_extra
,
511 AUDIO_FLAG_SPACIAL_3D
);
512 audio_player_set_position( &audio_player_extra
, player
.rb
.co
);
513 audio_player_set_vol( &audio_player_extra
, 20.0f
);
514 audio_player_playclip( &audio_player_extra
, &audio_board
[5] );
520 * Air control, no real physics
522 VG_STATIC
void player_skate_apply_air_model( struct player_skate
*s
)
524 if( s
->phys
.activity
!= k_skate_activity_air
)
527 if( s
->phys
.activity_prev
!= k_skate_activity_air
)
528 player_approximate_best_trajectory( s
);
530 m3x3_mulv( s
->phys
.vr
, player
.rb
.v
, player
.rb
.v
);
536 float pstep
= VG_TIMESTEP_FIXED
* 1.0f
;
537 float k_bias
= 0.98f
;
540 v3_copy( player
.rb
.co
, pco
);
541 v3_muls( player
.rb
.v
, 1.0f
, pv
);
543 float time_to_impact
= 0.0f
;
544 float limiter
= 1.0f
;
546 struct grind_edge
*best_grind
= NULL
;
547 float closest_grind
= INFINITY
;
549 v3f target_normal
= { 0.0f
, 1.0f
, 0.0f
};
552 for( int i
=0; i
<250; i
++ )
554 v3_copy( pco
, pco1
);
555 m3x3_mulv( s
->phys
.vr
, pv
, pv
);
557 pv
[1] += -k_gravity
* pstep
;
558 v3_muladds( pco
, pv
, pstep
, pco
);
563 v3_sub( pco
, pco1
, vdir
);
564 contact
.dist
= v3_length( vdir
);
565 v3_divs( vdir
, contact
.dist
, vdir
);
568 struct grind_edge
*ge
= player_collect_grind_edge( pco
, pco1
,
571 if( ge
&& (v3_dot((v3f
){0.0f
,1.0f
,0.0f
},vdir
) < -0.2f
) )
573 vg_line( ge
->p0
, ge
->p1
, 0xff0000ff );
574 vg_line_cross( pco
, 0xff0000ff, 0.25f
);
579 float orig_dist
= contact
.dist
;
580 if( ray_world( pco1
, vdir
, &contact
) )
582 v3_copy( contact
.normal
, target_normal
);
584 time_to_impact
+= (contact
.dist
/orig_dist
)*pstep
;
585 vg_line_cross( contact
.pos
, 0xffff0000, 0.25f
);
588 time_to_impact
+= pstep
;
593 float angle
= v3_dot( player
.rb
.to_world
[1], target_normal
);
595 v3_cross( player
.rb
.to_world
[1], target_normal
, axis
);
597 limiter
= vg_minf( 5.0f
, time_to_impact
)/5.0f
;
598 limiter
= 1.0f
-limiter
;
600 limiter
= 1.0f
-limiter
;
602 if( fabsf(angle
) < 0.99f
)
605 q_axis_angle( correction
, axis
,
606 acosf(angle
)*(1.0f
-limiter
)*3.0f
*VG_TIMESTEP_FIXED
);
607 q_mul( correction
, player
.rb
.q
, player
.rb
.q
);
611 v2f steer
= { player
.input_js1h
->axis
.value
,
612 player
.input_js1v
->axis
.value
};
614 float l2
= v2_length2( steer
);
616 v2_muls( steer
, 1.0f
/sqrtf(l2
), steer
);
618 s
->phys
.iY
-= steer
[0] * k_steer_air
* VG_TIMESTEP_FIXED
;
620 float iX
= steer
[1] *
621 s
->phys
.reverse
* k_steer_air
622 * limiter
* VG_TIMESTEP_FIXED
;
624 static float siX
= 0.0f
;
625 siX
= vg_lerpf( siX
, iX
, k_steer_air_lerp
);
628 q_axis_angle( rotate
, player
.rb
.to_world
[0], siX
);
629 q_mul( rotate
, player
.rb
.q
, player
.rb
.q
);
632 v2f target
= {0.0f
,0.0f
};
633 v2_muladds( target
, (v2f
){ vg_get_axis("grabh"), vg_get_axis("grabv") },
634 player_skate
.phys
.grab
, target
);
638 VG_STATIC
void player_regular_collider_configuration( struct player_skate
*s
)
640 /* Standard ground configuration */
641 m3x3_copy( player
.rb
.to_world
, s
->rbf
.to_world
);
642 m3x3_copy( player
.rb
.to_world
, s
->rbb
.to_world
);
644 v3f front
= {0.0f
,0.0f
,-k_board_length
},
645 back
= {0.0f
,0.0f
, k_board_length
};
647 m4x3_mulv( player
.rb
.to_world
, front
, s
->rbf
.co
);
648 m4x3_mulv( player
.rb
.to_world
, back
, s
->rbb
.co
);
649 v3_copy( s
->rbf
.co
, s
->rbf
.to_world
[3] );
650 v3_copy( s
->rbb
.co
, s
->rbb
.to_world
[3] );
652 m4x3_invert_affine( s
->rbf
.to_world
, s
->rbf
.to_local
);
653 m4x3_invert_affine( s
->rbb
.to_world
, s
->rbb
.to_local
);
655 rb_update_bounds( &s
->rbf
);
656 rb_update_bounds( &s
->rbb
);
659 VG_STATIC
void player_grind( struct player_skate
*s
)
662 int idx
= bh_closest_point( world
.grind_bh
, player
.rb
.co
,
667 struct grind_edge
*edge
= &world
.grind_edges
[ idx
];
669 vg_line( player
.rb
.co
, closest
, 0xff000000 );
670 vg_line_cross( closest
, 0xff000000, 0.3f
);
671 vg_line( edge
->p0
, edge
->p1
, 0xff000000 );
674 v3_sub( closest
, player
.rb
.co
, grind_delta
);
676 float p
= v3_dot( player
.rb
.to_world
[2], grind_delta
);
677 v3_muladds( grind_delta
, player
.rb
.to_world
[2], -p
, grind_delta
);
679 float a
= vg_maxf( 0.0f
, 4.0f
-v3_dist2( closest
, player
.rb
.co
) );
680 v3_muladds( player
.rb
.v
, grind_delta
, a
*0.2f
, player
.rb
.v
);
683 VG_STATIC
int player_update_grind_collision( struct player_skate
*s
,
687 v3_muladds( player
.rb
.co
, player
.rb
.to_world
[2], 0.5f
, p0
);
688 v3_muladds( player
.rb
.co
, player
.rb
.to_world
[2], -0.5f
, p1
);
689 v3_muladds( p0
, player
.rb
.to_world
[1], 0.125f
-0.15f
, p0
);
690 v3_muladds( p1
, player
.rb
.to_world
[1], 0.125f
-0.15f
, p1
);
692 float const k_r
= 0.25f
;
693 struct grind_edge
*closest_edge
= player_collect_grind_edge( p0
, p1
,
700 v3_sub( c1
, c0
, delta
);
702 if( v3_dot( delta
, player
.rb
.to_world
[1] ) > 0.0001f
)
704 contact
->p
= v3_length( delta
);
705 contact
->type
= k_contact_type_edge
;
706 contact
->element_id
= 0;
707 v3_copy( c1
, contact
->co
);
711 v3f edge_dir
, axis_dir
;
712 v3_sub( closest_edge
->p1
, closest_edge
->p0
, edge_dir
);
713 v3_normalize( edge_dir
);
714 v3_cross( (v3f
){0.0f
,1.0f
,0.0f
}, edge_dir
, axis_dir
);
715 v3_cross( edge_dir
, axis_dir
, contact
->n
);
727 * Handles connection between the player and the ground
729 VG_STATIC
void player_skate_apply_interface_model( struct player_skate
*s
,
730 rb_ct
*manifold
, int len
)
732 if( !((s
->phys
.activity
== k_skate_activity_ground
) ||
733 (s
->phys
.activity
== k_skate_activity_air
)) )
737 v3_zero( surface_avg
);
742 * ================================================================
744 if( s
->phys
.activity
== k_skate_activity_air
)
745 s
->normal_pressure
= 0.0f
;
747 s
->normal_pressure
= v3_dot( player
.rb
.to_world
[1], player
.rb
.v
);
755 float mod
= 0.7f
* player
.input_grab
->axis
.value
+ 0.3f
,
756 spring_k
= mod
* k_spring_force
,
757 damp_K
= mod
* k_spring_dampener
,
760 v3_copy( s
->rbf
.co
, p0_0
);
761 v3_copy( s
->rbb
.co
, p1_0
);
763 v3_muladds( p0_0
, player
.rb
.to_world
[1], -disp_k
, p0_1
);
764 v3_muladds( p1_0
, player
.rb
.to_world
[1], -disp_k
, p1_1
);
766 int cast0
= spherecast_world( p0_0
, p0_1
, 0.2f
, &t0
, n0
),
767 cast1
= spherecast_world( p1_0
, p1_1
, 0.2f
, &t1
, n1
);
772 m3x3_copy( player
.rb
.to_world
, temp
);
775 v3_lerp( p0_0
, p0_1
, t0
, temp
[3] );
776 v3_copy( temp
[3], animp0
);
777 debug_sphere( temp
, 0.2f
, VG__PINK
);
780 v3_sub( p0_0
, player
.rb
.co
, delta
);
782 float displacement
= vg_clampf( 1.0f
-t0
, 0.0f
, 1.0f
),
784 vg_maxf( 0.0f
, v3_dot( player
.rb
.to_world
[1], player
.rb
.v
) );
786 v3_muls( player
.rb
.to_world
[1], displacement
*spring_k
*k_rb_delta
-
787 damp
*damp_K
*k_rb_delta
, F
);
789 v3_muladds( player
.rb
.v
, F
, 1.0f
, player
.rb
.v
);
791 /* Angular velocity */
793 v3_cross( delta
, F
, wa
);
794 v3_muladds( player
.rb
.w
, wa
, k_spring_angular
, player
.rb
.w
);
797 v3_copy( p0_1
, animp0
);
801 v3_lerp( p1_0
, p1_1
, t1
, temp
[3] );
802 v3_copy( temp
[3], animp1
);
803 debug_sphere( temp
, 0.2f
, VG__PINK
);
806 v3_sub( p1_0
, player
.rb
.co
, delta
);
808 float displacement
= vg_clampf( 1.0f
-t1
, 0.0f
, 1.0f
),
810 vg_maxf( 0.0f
, v3_dot( player
.rb
.to_world
[1], player
.rb
.v
) );
811 v3_muls( player
.rb
.to_world
[1], displacement
*spring_k
*k_rb_delta
-
812 damp
*damp_K
*k_rb_delta
, F
);
814 v3_muladds( player
.rb
.v
, F
, 1.0f
, player
.rb
.v
);
816 /* Angular velocity */
818 v3_cross( delta
, F
, wa
);
819 v3_muladds( player
.rb
.w
, wa
, k_spring_angular
, player
.rb
.w
);
822 v3_copy( p1_1
, animp1
);
824 v3f animavg
, animdelta
;
825 v3_add( animp0
, animp1
, animavg
);
826 v3_muls( animavg
, 0.5f
, animavg
);
828 v3_sub( animp1
, animp0
, animdelta
);
829 v3_normalize( animdelta
);
831 m4x3_mulv( player
.rb
.to_local
, animavg
, player
.board_offset
);
833 float dx
= -v3_dot( animdelta
, player
.rb
.to_world
[2] ),
834 dy
= v3_dot( animdelta
, player
.rb
.to_world
[1] );
836 float angle
= -atan2f( dy
, dx
);
837 q_axis_angle( player
.board_rotation
, (v3f
){ 1.0f
, 0.0f
, 0.0f
}, angle
);
840 * ================================================================
844 if( len
== 0 && !((cast0
!=-1)&&(cast1
!=-1)) )
846 s
->phys
.lift_frames
++;
848 if( s
->phys
.lift_frames
>= 8 )
849 s
->phys
.activity
= k_skate_activity_air
;
853 for( int i
=0; i
<len
; i
++ )
854 v3_add( surface_avg
, manifold
[i
].n
, surface_avg
);
855 v3_normalize( surface_avg
);
857 if( v3_dot( player
.rb
.v
, surface_avg
) > 0.7f
)
859 s
->phys
.lift_frames
++;
861 if( s
->phys
.lift_frames
>= 8 )
862 s
->phys
.activity
= k_skate_activity_air
;
866 s
->phys
.activity
= k_skate_activity_ground
;
867 s
->phys
.lift_frames
= 0;
870 float const DOWNFORCE
= -k_downforce
*VG_TIMESTEP_FIXED
;
871 v3_muladds( player
.rb
.v
, player
.rb
.to_world
[1],
872 DOWNFORCE
, player
.rb
.v
);
874 float d
= v3_dot( player
.rb
.to_world
[2], surface_avg
);
875 v3_muladds( surface_avg
, player
.rb
.to_world
[2], -d
, projected
);
876 v3_normalize( projected
);
878 float angle
= v3_dot( player
.rb
.to_world
[1], projected
);
879 v3_cross( player
.rb
.to_world
[1], projected
, axis
);
883 v3_add( phys
->rb
.co
, projected
, p0
);
884 v3_add( phys
->rb
.co
, player_skate
.phys
.up
, p1
);
885 vg_line( phys
->rb
.co
, p0
, 0xff00ff00 );
886 vg_line( phys
->rb
.co
, p1
, 0xff000fff );
889 if( fabsf(angle
) < 0.999f
)
892 q_axis_angle( correction
, axis
,
893 acosf(angle
)*4.0f
*VG_TIMESTEP_FIXED
);
894 q_mul( correction
, player
.rb
.q
, player
.rb
.q
);
901 VG_STATIC
void player_collision_response( struct player_skate
*s
,
902 rb_ct
*manifold
, int len
)
905 /* Throw / collect routine
907 * TODO: Max speed boost
909 if( player
.input_grab
->axis
.value
> 0.5f
)
911 if( s
->phys
.activity
== k_skate_activity_ground
)
914 v3_muls( player
.rb
.to_world
[1], k_mmthrow_scale
, s
->phys
.throw_v
);
920 float doty
= v3_dot( player
.rb
.to_world
[1], s
->phys
.throw_v
);
923 v3_muladds( s
->phys
.throw_v
, player
.rb
.to_world
[1], -doty
, Fl
);
925 if( s
->phys
.activity
== k_skate_activity_ground
)
927 v3_muladds( player
.rb
.v
, Fl
, k_mmcollect_lat
, player
.rb
.v
);
928 v3_muladds( s
->phys
.throw_v
, Fl
, -k_mmcollect_lat
, s
->phys
.throw_v
);
931 v3_muls( player
.rb
.to_world
[1], -doty
, Fv
);
932 v3_muladds( player
.rb
.v
, Fv
, k_mmcollect_vert
, player
.rb
.v
);
933 v3_muladds( s
->phys
.throw_v
, Fv
, k_mmcollect_vert
, s
->phys
.throw_v
);
935 v3_copy( Fl
, s
->debug_mmcollect_lat
);
936 v3_copy( Fv
, s
->debug_mmcollect_vert
);
940 if( v3_length2( s
->phys
.throw_v
) > 0.0001f
)
943 v3_copy( s
->phys
.throw_v
, dir
);
946 float max
= v3_dot( dir
, s
->phys
.throw_v
),
947 amt
= vg_minf( k_mmdecay
* k_rb_delta
, max
);
949 v3_muladds( s
->phys
.throw_v
, dir
, -amt
, s
->phys
.throw_v
);
956 v3f ideal_cog
, ideal_diff
;
957 v3_muladds( player
.rb
.co
, player
.rb
.to_world
[1],
958 1.0f
-player
.input_grab
->axis
.value
, ideal_cog
);
959 v3_sub( ideal_cog
, s
->phys
.cog
, ideal_diff
);
961 /* Apply velocities */
963 v3_sub( player
.rb
.v
, s
->phys
.cog_v
, rv
);
966 v3_muls( ideal_diff
, -k_cog_spring
* k_rb_rate
, F
);
967 v3_muladds( F
, rv
, -k_cog_damp
* k_rb_rate
, F
);
969 float ra
= k_cog_mass_ratio
,
970 rb
= 1.0f
-k_cog_mass_ratio
;
972 v3_muladds( s
->phys
.cog_v
, F
, -rb
, s
->phys
.cog_v
);
975 /* stripped down presolve */
976 for( int i
=0; i
<len
; i
++ )
978 rb_ct
*ct
= &manifold
[i
];
979 ct
->bias
= -0.2f
* k_rb_rate
* vg_minf( 0.0f
, -ct
->p
+k_penetration_slop
);
981 rb_debug_contact( ct
);
984 for( int j
=0; j
<10; j
++ )
986 for( int i
=0; i
<len
; i
++ )
988 struct contact
*ct
= &manifold
[i
];
991 v3_sub( ct
->co
, player
.rb
.co
, delta
);
992 v3_cross( player
.rb
.w
, delta
, dv
);
993 v3_add( player
.rb
.v
, dv
, dv
);
995 float vn
= -v3_dot( dv
, ct
->n
);
998 float temp
= ct
->norm_impulse
;
999 ct
->norm_impulse
= vg_maxf( temp
+ vn
, 0.0f
);
1000 vn
= ct
->norm_impulse
- temp
;
1003 v3_muls( ct
->n
, vn
, impulse
);
1005 if( fabsf(v3_dot( impulse
, player
.rb
.to_world
[2] )) > 10.0f
||
1006 fabsf(v3_dot( impulse
, player
.rb
.to_world
[1] )) > 50.0f
)
1012 v3_add( impulse
, player
.rb
.v
, player
.rb
.v
);
1013 v3_cross( delta
, impulse
, impulse
);
1016 * W Impulses are limited to the Y and X axises, we don't really want
1017 * roll angular velocities being included.
1019 * Can also tweak the resistance of each axis here by scaling the wx,wy
1023 float wy
= v3_dot( player
.rb
.to_world
[1], impulse
) * 0.8f
,
1024 wx
= v3_dot( player
.rb
.to_world
[0], impulse
) * 1.0f
;
1026 v3_muladds( player
.rb
.w
, player
.rb
.to_world
[1], wy
, player
.rb
.w
);
1027 v3_muladds( player
.rb
.w
, player
.rb
.to_world
[0], wx
, player
.rb
.w
);
1031 /* early integrate this */
1032 s
->phys
.cog_v
[1] += -9.8f
* k_rb_delta
;
1033 v3_muladds( s
->phys
.cog
, s
->phys
.cog_v
, k_rb_delta
, s
->phys
.cog
);
1036 VG_STATIC
void player_skate_update( struct player_skate
*s
)
1038 s
->phys
.activity_prev
= s
->phys
.activity
;
1041 *interface_manifold
= NULL
,
1042 *grind_manifold
= NULL
;
1044 player_regular_collider_configuration( s
);
1046 int nfront
= player_collide_sphere( &s
->rbf
, manifold
),
1047 nback
= player_collide_sphere( &s
->rbb
, manifold
+ nfront
),
1048 interface_len
= nfront
+ nback
;
1050 interface_manifold
= manifold
;
1051 grind_manifold
= manifold
+ interface_len
;
1053 int grind_len
= player_update_grind_collision( s
, grind_manifold
);
1055 player_skate_apply_grind_model( s
, grind_manifold
, grind_len
);
1056 player_skate_apply_interface_model( s
, manifold
, interface_len
);
1058 rb_presolve_contacts( manifold
, interface_len
+ grind_len
);
1059 player_collision_response( s
, manifold
, interface_len
+ grind_len
);
1061 player_skate_apply_grab_model( s
);
1062 player_skate_apply_friction_model( s
);
1063 player_skate_apply_jump_model( s
);
1064 player_skate_apply_air_model( s
);
1066 v3f gravity
= { 0.0f
, -9.6f
, 0.0f
};
1067 v3_muladds( player
.rb
.v
, gravity
, k_rb_delta
, player
.rb
.v
);
1069 v3_sub( player
.rb
.v
, s
->phys
.v_prev
, s
->phys
.a
);
1070 v3_muls( s
->phys
.a
, 1.0f
/VG_TIMESTEP_FIXED
, s
->phys
.a
);
1071 v3_copy( player
.rb
.v
, s
->phys
.v_prev
);
1073 player
.rb
.v
[1] += -9.6f
* VG_TIMESTEP_FIXED
;
1075 v3_muladds( player
.rb
.co
, player
.rb
.v
, VG_TIMESTEP_FIXED
, player
.rb
.co
);
1078 VG_STATIC
void player_physics_gui(void)
1083 vg_uictx
.cursor
[0] = 0;
1084 vg_uictx
.cursor
[1] = vg
.window_y
- 128;
1085 vg_uictx
.cursor
[3] = 14;
1090 snprintf( buf
, 127, "v: %6.3f %6.3f %6.3f\n", player
.phys
.rb
.v
[0],
1091 player
.phys
.rb
.v
[1],
1092 player
.phys
.rb
.v
[2] );
1094 ui_text( vg_uictx
.cursor
, buf
, 1, 0 );
1095 vg_uictx
.cursor
[1] += 14;
1098 snprintf( buf
, 127, "a: %6.3f %6.3f %6.3f (%6.3f)\n", player
.phys
.a
[0],
1101 v3_length(player
.phys
.a
));
1102 ui_text( vg_uictx
.cursor
, buf
, 1, 0 );
1103 vg_uictx
.cursor
[1] += 14;
1105 float normal_acceleration
= v3_dot( player
.phys
.a
, player
.phys
.rb
.up
);
1106 snprintf( buf
, 127, "Normal acceleration: %6.3f\n", normal_acceleration
);
1108 ui_text( vg_uictx
.cursor
, buf
, 1, 0 );
1109 vg_uictx
.cursor
[1] += 14;
1111 snprintf( buf
, 127, "Normal Pressure: %6.3f\n", player
.normal_pressure
);
1112 ui_text( vg_uictx
.cursor
, buf
, 1, 0 );
1113 vg_uictx
.cursor
[1] += 14;
1117 #endif /* PLAYER_PHYSICS_SKATE_H */