--- /dev/null
+/*
+ * Copyright (C) 2021-2023 Mt.ZERO Software, Harry Godden - All Rights Reserved
+ */
+
+#ifndef PLAYER_PHYSICS_SKATE_H
+#define PLAYER_PHYSICS_SKATE_H
+
+#include "player.h"
+#include "camera.h"
+
+VG_STATIC struct player_skate
+{
+ struct skate_phys
+ {
+ enum skate_activity
+ {
+ k_skate_activity_air,
+ k_skate_activity_ground,
+ k_skate_activity_grind
+ }
+ activity,
+ activity_prev;
+
+ v3f v_prev, a, m, bob, vl; /* TODO: please clarify */
+
+ float iY, siY; /* Yaw inertia */
+
+ float vswitch, slip, slip_last, reverse;
+ float grab, jump;
+ v2f grab_mouse_delta;
+
+ v3f throw_v;
+ v3f cog,
+ cog_v;
+
+ int lift_frames;
+
+ double start_push,
+ cur_push;
+
+ int charging_jump, jump_dir;
+
+ m3x3f vr,vr_pstep;
+ }
+ phys,
+ phys_frame;
+
+ float normal_pressure;
+ v3f debug_mmcollect_lat,
+ debug_mmcollect_vert;
+
+ struct land_prediction
+ {
+ v3f log[50];
+ v3f n;
+ u32 log_length;
+ float score;
+
+ enum prediction_type
+ {
+ k_prediction_none,
+ k_prediction_land,
+ k_prediction_grind
+ }
+ type;
+
+ u32 colour;
+ }
+ predictions[22];
+ u32 prediction_count;
+
+ rigidbody rbf, rbb;
+}
+player_skater;
+
+/*
+ * Gets the closest grindable edge to the player within max_dist
+ */
+VG_STATIC struct grind_edge *player_collect_grind_edge( v3f p0, v3f p1,
+ v3f c0, v3f c1,
+ float max_dist )
+{
+ bh_iter it;
+ bh_iter_init( 0, &it );
+
+ boxf region;
+
+ box_init_inf( region );
+ box_addpt( region, p0 );
+ box_addpt( region, p1 );
+
+ float k_r = max_dist;
+ v3_add( (v3f){ k_r, k_r, k_r}, region[1], region[1] );
+ v3_add( (v3f){-k_r,-k_r,-k_r}, region[0], region[0] );
+
+ float closest = k_r*k_r;
+ struct grind_edge *closest_edge = NULL;
+
+ int idx;
+ while( bh_next( world.grind_bh, &it, region, &idx ) )
+ {
+ struct grind_edge *edge = &world.grind_edges[ idx ];
+
+ float s,t;
+ v3f pa, pb;
+
+ float d2 =
+ closest_segment_segment( p0, p1, edge->p0, edge->p1, &s,&t, pa, pb );
+
+ if( d2 < closest )
+ {
+ closest = d2;
+ closest_edge = edge;
+ v3_copy( pa, c0 );
+ v3_copy( pb, c1 );
+ }
+ }
+
+ return closest_edge;
+}
+
+/*
+ * Trace a path given a velocity rotation.
+ *
+ * TODO: this MIGHT be worth doing RK4 on the gravity field.
+ */
+VG_STATIC void player_score_vr_path( v3f co, v3f v, m3x3f vr,
+ struct land_prediction *prediction )
+{
+ float pstep = VG_TIMESTEP_FIXED * 10.0f;
+ float k_bias = 0.96f;
+
+ v3f pco, pco1, pv;
+ v3_copy( co, pco );
+ v3_muls( v, k_bias, pv );
+
+ m3x3_mulv( vr, pv, pv );
+ v3_muladds( pco, pv, pstep, pco );
+
+ struct grind_edge *best_grind = NULL;
+ float closest_grind = INFINITY;
+
+ float grind_score = INFINITY,
+ air_score = INFINITY;
+
+ prediction->log_length = 0;
+
+ for( int i=0; i<vg_list_size(prediction->log); i++ )
+ {
+ v3_copy( pco, pco1 );
+
+ pv[1] += -k_gravity * pstep;
+
+ m3x3_mulv( vr, pv, pv );
+ v3_muladds( pco, pv, pstep, pco );
+
+ v3f vdir;
+
+ v3_sub( pco, pco1, vdir );
+
+ float l = v3_length( vdir );
+ v3_muls( vdir, 1.0f/l, vdir );
+
+ v3f c0, c1;
+ struct grind_edge *ge = player_collect_grind_edge( pco, pco1,
+ c0, c1, 0.4f );
+
+ if( ge && (v3_dot((v3f){0.0f,1.0f,0.0f},vdir) < -0.2f ) )
+ {
+ float d2 = v3_dist2( c0, c1 );
+ if( d2 < closest_grind )
+ {
+ closest_grind = d2;
+ best_grind = ge;
+ grind_score = closest_grind * 0.05f;
+ }
+ }
+
+ v3f n1;
+
+ float t1;
+ int idx = spherecast_world( pco1, pco, 0.4f, &t1, n1 );
+ if( idx != -1 )
+ {
+ v3_copy( n1, prediction->n );
+ air_score = -v3_dot( pv, n1 );
+
+ u32 vert_index = world.scene_geo->arrindices[ idx*3 ];
+ struct world_material *mat = world_tri_index_material( vert_index );
+
+ /* Bias prediction towords ramps */
+ if( mat->info.flags & k_material_flag_skate_surface )
+ air_score *= 0.1f;
+
+ v3_lerp( pco1, pco, t1, prediction->log[ prediction->log_length ++ ] );
+ break;
+ }
+
+ v3_copy( pco, prediction->log[ prediction->log_length ++ ] );
+ }
+
+ if( grind_score < air_score )
+ {
+ prediction->score = grind_score;
+ prediction->type = k_prediction_grind;
+ }
+ else if( air_score < INFINITY )
+ {
+ prediction->score = air_score;
+ prediction->type = k_prediction_land;
+ }
+ else
+ {
+ prediction->score = INFINITY;
+ prediction->type = k_prediction_none;
+ }
+}
+
+/*
+ * Calculate best launch trajectory
+ */
+VG_STATIC void player_approximate_best_trajectory( struct player_skate *s )
+{
+ float pstep = VG_TIMESTEP_FIXED * 10.0f;
+ float best_velocity_delta = -9999.9f;
+
+ v3f axis;
+ v3_cross( player.rb.to_world[1], player.rb.v, axis );
+ v3_normalize( axis );
+
+ s->prediction_count = 0;
+ m3x3_identity( s->phys.vr );
+
+ float
+ best_vmod = 0.0f,
+ min_score = INFINITY,
+ max_score = -INFINITY;
+
+ /*
+ * Search a broad selection of futures
+ */
+ for( int m=-3;m<=12; m++ )
+ {
+ struct land_prediction *p = &s->predictions[ s->prediction_count ++ ];
+
+ float vmod = ((float)m / 15.0f)*0.09f;
+
+ m3x3f vr;
+ v4f vr_q;
+
+ q_axis_angle( vr_q, axis, vmod );
+ q_m3x3( vr_q, vr );
+
+ player_score_vr_path( player.rb.co, player.rb.v, vr, p );
+
+ if( p->type != k_prediction_none )
+ {
+ if( p->score < min_score )
+ {
+ min_score = p->score;
+ best_vmod = vmod;
+ }
+
+ if( p->score > max_score )
+ max_score = p->score;
+ }
+ }
+
+ v4f vr_q;
+ q_axis_angle( vr_q, axis, best_vmod*0.1f );
+ q_m3x3( vr_q, s->phys.vr );
+
+ q_axis_angle( vr_q, axis, best_vmod );
+ q_m3x3( vr_q, s->phys.vr_pstep );
+
+ /*
+ * Logging
+ */
+ for( int i=0; i<s->prediction_count; i ++ )
+ {
+ struct land_prediction *p = &s->predictions[i];
+
+ float l = p->score;
+
+ if( l < 0.0f )
+ {
+ vg_error( "negative score! (%f)\n", l );
+ }
+
+ l -= min_score;
+ l /= (max_score-min_score);
+ l = 1.0f - l;
+ l *= 255.0f;
+
+ p->colour = l;
+ p->colour <<= 8;
+ p->colour |= 0xff000000;
+ }
+}
+
+VG_STATIC void player_skate_apply_grab_model( struct player_skate *s )
+{
+ float grabt = player.input_grab->axis.value;
+
+ if( grabt > 0.5f )
+ {
+ v2_muladds( s->phys.grab_mouse_delta, vg.mouse_delta, 0.02f,
+ s->phys.grab_mouse_delta );
+
+ v2_normalize_clamp( s->phys.grab_mouse_delta );
+ }
+ else
+ v2_zero( s->phys.grab_mouse_delta );
+
+ s->phys.grab = vg_lerpf( s->phys.grab, grabt, 8.4f * VG_TIMESTEP_FIXED );
+}
+
+/*
+ * Computes friction and surface interface model
+ */
+VG_STATIC void player_skate_apply_friction_model( struct player_skate *s )
+{
+ if( s->phys.activity != k_skate_activity_ground )
+ return;
+
+ /*
+ * Computing localized friction forces for controlling the character
+ * Friction across X is significantly more than Z
+ */
+
+ v3f vel;
+ m3x3_mulv( player.rb.to_local, player.rb.v, vel );
+ float slip = 0.0f;
+
+ if( fabsf(vel[2]) > 0.01f )
+ slip = fabsf(-vel[0] / vel[2]) * vg_signf(vel[0]);
+
+ if( fabsf( slip ) > 1.2f )
+ slip = vg_signf( slip ) * 1.2f;
+
+ s->phys.slip = slip;
+ s->phys.reverse = -vg_signf(vel[2]);
+
+ float substep = VG_TIMESTEP_FIXED;
+ float fwd_resistance = k_friction_resistance;
+
+ vel[2] = stable_force( vel[2],vg_signf(vel[2]) * -fwd_resistance*substep);
+ vel[0] = stable_force( vel[0],vg_signf(vel[0]) * -k_friction_lat*substep);
+
+ /* Pushing additive force */
+
+ if( !player.input_jump->button.value )
+ {
+ if( player.input_push->button.value )
+ {
+ if( (vg.time - s->phys.cur_push) > 0.25 )
+ {
+ s->phys.start_push = vg.time;
+ }
+
+ s->phys.cur_push = vg.time;
+
+ double push_time = vg.time - s->phys.start_push;
+
+ float cycle_time = push_time*k_push_cycle_rate,
+ accel = k_push_accel * (sinf(cycle_time)*0.5f+0.5f),
+ amt = accel * VG_TIMESTEP_FIXED,
+ current = v3_length( vel ),
+ new_vel = vg_minf( current + amt, k_max_push_speed ),
+ delta = new_vel - vg_minf( current, k_max_push_speed );
+
+ vel[2] += delta * -s->phys.reverse;
+ }
+ }
+
+ /* Send back to velocity */
+ m3x3_mulv( player.rb.to_world, vel, player.rb.v );
+
+ /* Steering */
+ float input = player.input_js1h->axis.value,
+ grab = player.input_grab->axis.value,
+ steer = input * (1.0f-(s->phys.jump+grab)*0.4f),
+ steer_scaled = vg_signf(steer) * powf(steer,2.0f) * k_steer_ground;
+
+ s->phys.iY -= steer_scaled * VG_TIMESTEP_FIXED;
+}
+
+VG_STATIC void player_skate_apply_jump_model( struct player_skate *s )
+{
+ int charging_jump_prev = s->phys.charging_jump;
+ s->phys.charging_jump = player.input_jump->button.value;
+
+ /* Cannot charge this in air */
+ if( s->phys.activity != k_skate_activity_ground )
+ s->phys.charging_jump = 0;
+
+ if( s->phys.charging_jump )
+ {
+ s->phys.jump += VG_TIMESTEP_FIXED * k_jump_charge_speed;
+
+ if( !charging_jump_prev )
+ s->phys.jump_dir = s->phys.reverse > 0.0f? 1: 0;
+ }
+ else
+ {
+ s->phys.jump -= k_jump_charge_speed * VG_TIMESTEP_FIXED;
+ }
+
+ s->phys.jump = vg_clampf( s->phys.jump, 0.0f, 1.0f );
+
+ if( s->phys.activity == k_skate_activity_air )
+ return;
+
+ /* player let go after charging past 0.2: trigger jump */
+ if( !s->phys.charging_jump && s->phys.jump > 0.2f )
+ {
+ v3f jumpdir;
+
+ /* Launch more up if alignment is up else improve velocity */
+ float aup = v3_dot( (v3f){0.0f,1.0f,0.0f}, player.rb.to_world[1] ),
+ mod = 0.5f,
+ dir = mod + fabsf(aup)*(1.0f-mod);
+
+ v3_copy( player.rb.v, jumpdir );
+ v3_normalize( jumpdir );
+ v3_muls( jumpdir, 1.0f-dir, jumpdir );
+ v3_muladds( jumpdir, player.rb.to_world[1], dir, jumpdir );
+ v3_normalize( jumpdir );
+
+ float force = k_jump_force*s->phys.jump;
+ v3_muladds( player.rb.v, jumpdir, force, player.rb.v );
+ s->phys.jump = 0.0f;
+
+ player.jump_time = vg.time;
+
+ /* TODO: Move to audio file */
+ audio_lock();
+ audio_player_set_flags( &audio_player_extra, AUDIO_FLAG_SPACIAL_3D );
+ audio_player_set_position( &audio_player_extra, player.rb.co );
+ audio_player_set_vol( &audio_player_extra, 20.0f );
+ audio_player_playclip( &audio_player_extra, &audio_jumps[rand()%2] );
+ audio_unlock();
+ }
+}
+
+VG_STATIC void player_skate_apply_grind_model( struct player_skate *s,
+ rb_ct *manifold, int len )
+{
+ if( len == 0 )
+ {
+ if( s->phys.activity == k_skate_activity_grind )
+ {
+ audio_lock();
+ audio_player_set_flags( &audio_player_extra,
+ AUDIO_FLAG_SPACIAL_3D );
+ audio_player_set_position( &audio_player_extra, player.rb.co );
+ audio_player_set_vol( &audio_player_extra, 20.0f );
+ audio_player_playclip( &audio_player_extra, &audio_board[6] );
+ audio_unlock();
+
+ s->phys.activity = k_skate_activity_air;
+ }
+ return;
+ }
+
+ v2f steer = { player.input_js1h->axis.value,
+ player.input_js1v->axis.value };
+
+ float l2 = v2_length2( steer );
+ if( l2 > 1.0f )
+ v2_muls( steer, 1.0f/sqrtf(l2), steer );
+
+ s->phys.iY -= steer[0] * k_steer_air * VG_TIMESTEP_FIXED;
+
+ float iX = steer[1] * s->phys.reverse
+ * k_steer_air * VG_TIMESTEP_FIXED;
+
+ static float siX = 0.0f;
+ siX = vg_lerpf( siX, iX, k_steer_air_lerp );
+
+ v4f rotate;
+ q_axis_angle( rotate, player.rb.to_world[0], siX );
+ q_mul( rotate, player.rb.q, player.rb.q );
+
+ s->phys.slip = 0.0f;
+ s->phys.activity = k_skate_activity_grind;
+
+ v3f up = { 0.0f, 1.0f, 0.0f };
+ float angle = v3_dot( player.rb.to_world[1], up );
+
+ if( fabsf(angle) < 0.99f )
+ {
+ v3f axis;
+ v3_cross( player.rb.to_world[1], up, axis );
+
+ v4f correction;
+ q_axis_angle( correction, axis,
+ VG_TIMESTEP_FIXED * 10.0f * acosf(angle) );
+ q_mul( correction, player.rb.q, player.rb.q );
+ }
+
+ float const DOWNFORCE = -k_downforce*1.2f*VG_TIMESTEP_FIXED;
+ v3_muladds( player.rb.v, manifold->n, DOWNFORCE, player.rb.v );
+ m3x3_identity( s->phys.vr );
+ m3x3_identity( s->phys.vr_pstep );
+
+ if( s->phys.activity_prev != k_skate_activity_grind )
+ {
+ audio_lock();
+ audio_player_set_flags( &audio_player_extra,
+ AUDIO_FLAG_SPACIAL_3D );
+ audio_player_set_position( &audio_player_extra, player.rb.co );
+ audio_player_set_vol( &audio_player_extra, 20.0f );
+ audio_player_playclip( &audio_player_extra, &audio_board[5] );
+ audio_unlock();
+ }
+}
+
+/*
+ * Air control, no real physics
+ */
+VG_STATIC void player_skate_apply_air_model( struct player_skate *s )
+{
+ if( s->phys.activity != k_skate_activity_air )
+ return;
+
+ if( s->phys.activity_prev != k_skate_activity_air )
+ player_approximate_best_trajectory( s );
+
+ m3x3_mulv( s->phys.vr, player.rb.v, player.rb.v );
+ ray_hit hit;
+
+ /*
+ * Prediction
+ */
+ float pstep = VG_TIMESTEP_FIXED * 1.0f;
+ float k_bias = 0.98f;
+
+ v3f pco, pco1, pv;
+ v3_copy( player.rb.co, pco );
+ v3_muls( player.rb.v, 1.0f, pv );
+
+ float time_to_impact = 0.0f;
+ float limiter = 1.0f;
+
+ struct grind_edge *best_grind = NULL;
+ float closest_grind = INFINITY;
+
+ v3f target_normal = { 0.0f, 1.0f, 0.0f };
+ int has_target = 0;
+
+ for( int i=0; i<250; i++ )
+ {
+ v3_copy( pco, pco1 );
+ m3x3_mulv( s->phys.vr, pv, pv );
+
+ pv[1] += -k_gravity * pstep;
+ v3_muladds( pco, pv, pstep, pco );
+
+ ray_hit contact;
+ v3f vdir;
+
+ v3_sub( pco, pco1, vdir );
+ contact.dist = v3_length( vdir );
+ v3_divs( vdir, contact.dist, vdir);
+
+ v3f c0, c1;
+ struct grind_edge *ge = player_collect_grind_edge( pco, pco1,
+ c0, c1, 0.4f );
+
+ if( ge && (v3_dot((v3f){0.0f,1.0f,0.0f},vdir) < -0.2f ) )
+ {
+ vg_line( ge->p0, ge->p1, 0xff0000ff );
+ vg_line_cross( pco, 0xff0000ff, 0.25f );
+ has_target = 1;
+ break;
+ }
+
+ float orig_dist = contact.dist;
+ if( ray_world( pco1, vdir, &contact ) )
+ {
+ v3_copy( contact.normal, target_normal );
+ has_target = 1;
+ time_to_impact += (contact.dist/orig_dist)*pstep;
+ vg_line_cross( contact.pos, 0xffff0000, 0.25f );
+ break;
+ }
+ time_to_impact += pstep;
+ }
+
+ if( has_target )
+ {
+ float angle = v3_dot( player.rb.to_world[1], target_normal );
+ v3f axis;
+ v3_cross( player.rb.to_world[1], target_normal, axis );
+
+ limiter = vg_minf( 5.0f, time_to_impact )/5.0f;
+ limiter = 1.0f-limiter;
+ limiter *= limiter;
+ limiter = 1.0f-limiter;
+
+ if( fabsf(angle) < 0.99f )
+ {
+ v4f correction;
+ q_axis_angle( correction, axis,
+ acosf(angle)*(1.0f-limiter)*3.0f*VG_TIMESTEP_FIXED );
+ q_mul( correction, player.rb.q, player.rb.q );
+ }
+ }
+
+ v2f steer = { player.input_js1h->axis.value,
+ player.input_js1v->axis.value };
+
+ float l2 = v2_length2( steer );
+ if( l2 > 1.0f )
+ v2_muls( steer, 1.0f/sqrtf(l2), steer );
+
+ s->phys.iY -= steer[0] * k_steer_air * VG_TIMESTEP_FIXED;
+
+ float iX = steer[1] *
+ s->phys.reverse * k_steer_air
+ * limiter * VG_TIMESTEP_FIXED;
+
+ static float siX = 0.0f;
+ siX = vg_lerpf( siX, iX, k_steer_air_lerp );
+
+ v4f rotate;
+ q_axis_angle( rotate, player.rb.to_world[0], siX );
+ q_mul( rotate, player.rb.q, player.rb.q );
+
+#if 0
+ v2f target = {0.0f,0.0f};
+ v2_muladds( target, (v2f){ vg_get_axis("grabh"), vg_get_axis("grabv") },
+ player_skate.phys.grab, target );
+#endif
+}
+
+VG_STATIC void player_regular_collider_configuration( struct player_skate *s )
+{
+ /* Standard ground configuration */
+ m3x3_copy( player.rb.to_world, s->rbf.to_world );
+ m3x3_copy( player.rb.to_world, s->rbb.to_world );
+
+ v3f front = {0.0f,0.0f,-k_board_length},
+ back = {0.0f,0.0f, k_board_length};
+
+ m4x3_mulv( player.rb.to_world, front, s->rbf.co );
+ m4x3_mulv( player.rb.to_world, back, s->rbb.co );
+ v3_copy( s->rbf.co, s->rbf.to_world[3] );
+ v3_copy( s->rbb.co, s->rbb.to_world[3] );
+
+ m4x3_invert_affine( s->rbf.to_world, s->rbf.to_local );
+ m4x3_invert_affine( s->rbb.to_world, s->rbb.to_local );
+
+ rb_update_bounds( &s->rbf );
+ rb_update_bounds( &s->rbb );
+}
+
+VG_STATIC void player_grind( struct player_skate *s )
+{
+ v3f closest;
+ int idx = bh_closest_point( world.grind_bh, player.rb.co,
+ closest, INFINITY );
+ if( idx == -1 )
+ return;
+
+ struct grind_edge *edge = &world.grind_edges[ idx ];
+
+ vg_line( player.rb.co, closest, 0xff000000 );
+ vg_line_cross( closest, 0xff000000, 0.3f );
+ vg_line( edge->p0, edge->p1, 0xff000000 );
+
+ v3f grind_delta;
+ v3_sub( closest, player.rb.co, grind_delta );
+
+ float p = v3_dot( player.rb.to_world[2], grind_delta );
+ v3_muladds( grind_delta, player.rb.to_world[2], -p, grind_delta );
+
+ float a = vg_maxf( 0.0f, 4.0f-v3_dist2( closest, player.rb.co ) );
+ v3_muladds( player.rb.v, grind_delta, a*0.2f, player.rb.v );
+}
+
+VG_STATIC int player_update_grind_collision( struct player_skate *s,
+ rb_ct *contact )
+{
+ v3f p0, p1, c0, c1;
+ v3_muladds( player.rb.co, player.rb.to_world[2], 0.5f, p0 );
+ v3_muladds( player.rb.co, player.rb.to_world[2], -0.5f, p1 );
+ v3_muladds( p0, player.rb.to_world[1], 0.125f-0.15f, p0 );
+ v3_muladds( p1, player.rb.to_world[1], 0.125f-0.15f, p1 );
+
+ float const k_r = 0.25f;
+ struct grind_edge *closest_edge = player_collect_grind_edge( p0, p1,
+ c0, c1, k_r );
+
+
+ if( closest_edge )
+ {
+ v3f delta;
+ v3_sub( c1, c0, delta );
+
+ if( v3_dot( delta, player.rb.to_world[1] ) > 0.0001f )
+ {
+ contact->p = v3_length( delta );
+ contact->type = k_contact_type_edge;
+ contact->element_id = 0;
+ v3_copy( c1, contact->co );
+ contact->rba = NULL;
+ contact->rbb = NULL;
+
+ v3f edge_dir, axis_dir;
+ v3_sub( closest_edge->p1, closest_edge->p0, edge_dir );
+ v3_normalize( edge_dir );
+ v3_cross( (v3f){0.0f,1.0f,0.0f}, edge_dir, axis_dir );
+ v3_cross( edge_dir, axis_dir, contact->n );
+
+ return 1;
+ }
+ else
+ return 0;
+ }
+
+ return 0;
+}
+
+/*
+ * Handles connection between the player and the ground
+ */
+VG_STATIC void player_skate_apply_interface_model( struct player_skate *s,
+ rb_ct *manifold, int len )
+{
+ if( !((s->phys.activity == k_skate_activity_ground) ||
+ (s->phys.activity == k_skate_activity_air )) )
+ return;
+
+ v3f surface_avg;
+ v3_zero( surface_avg );
+
+ /*
+ *
+ * EXPERIMENTAL
+ * ================================================================
+ */
+ if( s->phys.activity == k_skate_activity_air )
+ s->normal_pressure = 0.0f;
+ else
+ s->normal_pressure = v3_dot( player.rb.to_world[1], player.rb.v );
+
+ v3f p0_0, p0_1,
+ p1_0, p1_1,
+ n0, n1;
+
+ float t0, t1;
+
+ float mod = 0.7f * player.input_grab->axis.value + 0.3f,
+ spring_k = mod * k_spring_force,
+ damp_K = mod * k_spring_dampener,
+ disp_k = 0.4f;
+
+ v3_copy( s->rbf.co, p0_0 );
+ v3_copy( s->rbb.co, p1_0 );
+
+ v3_muladds( p0_0, player.rb.to_world[1], -disp_k, p0_1 );
+ v3_muladds( p1_0, player.rb.to_world[1], -disp_k, p1_1 );
+
+ int cast0 = spherecast_world( p0_0, p0_1, 0.2f, &t0, n0 ),
+ cast1 = spherecast_world( p1_0, p1_1, 0.2f, &t1, n1 );
+
+ v3f animp0, animp1;
+
+ m4x3f temp;
+ m3x3_copy( player.rb.to_world, temp );
+ if( cast0 != -1 )
+ {
+ v3_lerp( p0_0, p0_1, t0, temp[3] );
+ v3_copy( temp[3], animp0 );
+ debug_sphere( temp, 0.2f, VG__PINK );
+
+ v3f F, delta;
+ v3_sub( p0_0, player.rb.co, delta );
+
+ float displacement = vg_clampf( 1.0f-t0, 0.0f, 1.0f ),
+ damp =
+ vg_maxf( 0.0f, v3_dot( player.rb.to_world[1], player.rb.v ) );
+
+ v3_muls( player.rb.to_world[1], displacement*spring_k*k_rb_delta -
+ damp*damp_K*k_rb_delta, F );
+
+ v3_muladds( player.rb.v, F, 1.0f, player.rb.v );
+
+ /* Angular velocity */
+ v3f wa;
+ v3_cross( delta, F, wa );
+ v3_muladds( player.rb.w, wa, k_spring_angular, player.rb.w );
+ }
+ else
+ v3_copy( p0_1, animp0 );
+
+ if( cast1 != -1 )
+ {
+ v3_lerp( p1_0, p1_1, t1, temp[3] );
+ v3_copy( temp[3], animp1 );
+ debug_sphere( temp, 0.2f, VG__PINK );
+
+ v3f F, delta;
+ v3_sub( p1_0, player.rb.co, delta );
+
+ float displacement = vg_clampf( 1.0f-t1, 0.0f, 1.0f ),
+ damp =
+ vg_maxf( 0.0f, v3_dot( player.rb.to_world[1], player.rb.v ) );
+ v3_muls( player.rb.to_world[1], displacement*spring_k*k_rb_delta -
+ damp*damp_K*k_rb_delta, F );
+
+ v3_muladds( player.rb.v, F, 1.0f, player.rb.v );
+
+ /* Angular velocity */
+ v3f wa;
+ v3_cross( delta, F, wa );
+ v3_muladds( player.rb.w, wa, k_spring_angular, player.rb.w );
+ }
+ else
+ v3_copy( p1_1, animp1 );
+
+ v3f animavg, animdelta;
+ v3_add( animp0, animp1, animavg );
+ v3_muls( animavg, 0.5f, animavg );
+
+ v3_sub( animp1, animp0, animdelta );
+ v3_normalize( animdelta );
+
+ m4x3_mulv( player.rb.to_local, animavg, player.board_offset );
+
+ float dx = -v3_dot( animdelta, player.rb.to_world[2] ),
+ dy = v3_dot( animdelta, player.rb.to_world[1] );
+
+ float angle = -atan2f( dy, dx );
+ q_axis_angle( player.board_rotation, (v3f){ 1.0f, 0.0f, 0.0f }, angle );
+
+ /*
+ * ================================================================
+ * EXPERIMENTAL
+ */
+
+ if( len == 0 && !((cast0 !=-1)&&(cast1!=-1)) )
+ {
+ s->phys.lift_frames ++;
+
+ if( s->phys.lift_frames >= 8 )
+ s->phys.activity = k_skate_activity_air;
+ }
+ else
+ {
+ for( int i=0; i<len; i++ )
+ v3_add( surface_avg, manifold[i].n, surface_avg );
+ v3_normalize( surface_avg );
+
+ if( v3_dot( player.rb.v, surface_avg ) > 0.7f )
+ {
+ s->phys.lift_frames ++;
+
+ if( s->phys.lift_frames >= 8 )
+ s->phys.activity = k_skate_activity_air;
+ }
+ else
+ {
+ s->phys.activity = k_skate_activity_ground;
+ s->phys.lift_frames = 0;
+ v3f projected, axis;
+
+ float const DOWNFORCE = -k_downforce*VG_TIMESTEP_FIXED;
+ v3_muladds( player.rb.v, player.rb.to_world[1],
+ DOWNFORCE, player.rb.v );
+
+ float d = v3_dot( player.rb.to_world[2], surface_avg );
+ v3_muladds( surface_avg, player.rb.to_world[2], -d, projected );
+ v3_normalize( projected );
+
+ float angle = v3_dot( player.rb.to_world[1], projected );
+ v3_cross( player.rb.to_world[1], projected, axis );
+
+#if 0
+ v3f p0, p1;
+ v3_add( phys->rb.co, projected, p0 );
+ v3_add( phys->rb.co, player_skate.phys.up, p1 );
+ vg_line( phys->rb.co, p0, 0xff00ff00 );
+ vg_line( phys->rb.co, p1, 0xff000fff );
+#endif
+
+ if( fabsf(angle) < 0.999f )
+ {
+ v4f correction;
+ q_axis_angle( correction, axis,
+ acosf(angle)*4.0f*VG_TIMESTEP_FIXED );
+ q_mul( correction, player.rb.q, player.rb.q );
+ }
+ }
+ }
+}
+
+
+VG_STATIC void player_collision_response( struct player_skate *s,
+ rb_ct *manifold, int len )
+{
+ /* TODO: RElocate */
+ /* Throw / collect routine
+ *
+ * TODO: Max speed boost
+ */
+ if( player.input_grab->axis.value > 0.5f )
+ {
+ if( s->phys.activity == k_skate_activity_ground )
+ {
+ /* Throw */
+ v3_muls( player.rb.to_world[1], k_mmthrow_scale, s->phys.throw_v );
+ }
+ }
+ else
+ {
+ /* Collect */
+ float doty = v3_dot( player.rb.to_world[1], s->phys.throw_v );
+
+ v3f Fl, Fv;
+ v3_muladds( s->phys.throw_v, player.rb.to_world[1], -doty, Fl);
+
+ if( s->phys.activity == k_skate_activity_ground )
+ {
+ v3_muladds( player.rb.v, Fl, k_mmcollect_lat, player.rb.v );
+ v3_muladds( s->phys.throw_v, Fl, -k_mmcollect_lat, s->phys.throw_v );
+ }
+
+ v3_muls( player.rb.to_world[1], -doty, Fv );
+ v3_muladds( player.rb.v, Fv, k_mmcollect_vert, player.rb.v );
+ v3_muladds( s->phys.throw_v, Fv, k_mmcollect_vert, s->phys.throw_v );
+
+ v3_copy( Fl, s->debug_mmcollect_lat );
+ v3_copy( Fv, s->debug_mmcollect_vert );
+ }
+
+ /* Decay */
+ if( v3_length2( s->phys.throw_v ) > 0.0001f )
+ {
+ v3f dir;
+ v3_copy( s->phys.throw_v, dir );
+ v3_normalize( dir );
+
+ float max = v3_dot( dir, s->phys.throw_v ),
+ amt = vg_minf( k_mmdecay * k_rb_delta, max );
+
+ v3_muladds( s->phys.throw_v, dir, -amt, s->phys.throw_v );
+ }
+
+
+ /* TODO: RElocate */
+ {
+
+ v3f ideal_cog, ideal_diff;
+ v3_muladds( player.rb.co, player.rb.to_world[1],
+ 1.0f-player.input_grab->axis.value, ideal_cog );
+ v3_sub( ideal_cog, s->phys.cog, ideal_diff );
+
+ /* Apply velocities */
+ v3f rv;
+ v3_sub( player.rb.v, s->phys.cog_v, rv );
+
+ v3f F;
+ v3_muls( ideal_diff, -k_cog_spring * k_rb_rate, F );
+ v3_muladds( F, rv, -k_cog_damp * k_rb_rate, F );
+
+ float ra = k_cog_mass_ratio,
+ rb = 1.0f-k_cog_mass_ratio;
+
+ v3_muladds( s->phys.cog_v, F, -rb, s->phys.cog_v );
+ }
+
+ /* stripped down presolve */
+ for( int i=0; i<len; i++ )
+ {
+ rb_ct *ct = &manifold[i];
+ ct->bias = -0.2f * k_rb_rate * vg_minf( 0.0f, -ct->p+k_penetration_slop );
+
+ rb_debug_contact( ct );
+ }
+
+ for( int j=0; j<10; j++ )
+ {
+ for( int i=0; i<len; i++ )
+ {
+ struct contact *ct = &manifold[i];
+
+ v3f dv, delta;
+ v3_sub( ct->co, player.rb.co, delta );
+ v3_cross( player.rb.w, delta, dv );
+ v3_add( player.rb.v, dv, dv );
+
+ float vn = -v3_dot( dv, ct->n );
+ vn += ct->bias;
+
+ float temp = ct->norm_impulse;
+ ct->norm_impulse = vg_maxf( temp + vn, 0.0f );
+ vn = ct->norm_impulse - temp;
+
+ v3f impulse;
+ v3_muls( ct->n, vn, impulse );
+
+ if( fabsf(v3_dot( impulse, player.rb.to_world[2] )) > 10.0f ||
+ fabsf(v3_dot( impulse, player.rb.to_world[1] )) > 50.0f )
+ {
+ player_kill();
+ return;
+ }
+
+ v3_add( impulse, player.rb.v, player.rb.v );
+ v3_cross( delta, impulse, impulse );
+
+ /*
+ * W Impulses are limited to the Y and X axises, we don't really want
+ * roll angular velocities being included.
+ *
+ * Can also tweak the resistance of each axis here by scaling the wx,wy
+ * components.
+ */
+
+ float wy = v3_dot( player.rb.to_world[1], impulse ) * 0.8f,
+ wx = v3_dot( player.rb.to_world[0], impulse ) * 1.0f;
+
+ v3_muladds( player.rb.w, player.rb.to_world[1], wy, player.rb.w );
+ v3_muladds( player.rb.w, player.rb.to_world[0], wx, player.rb.w );
+ }
+ }
+
+ /* early integrate this */
+ s->phys.cog_v[1] += -9.8f * k_rb_delta;
+ v3_muladds( s->phys.cog, s->phys.cog_v, k_rb_delta, s->phys.cog );
+}
+
+VG_STATIC void player_skate_update( struct player_skate *s )
+{
+ s->phys.activity_prev = s->phys.activity;
+
+ rb_ct manifold[72],
+ *interface_manifold = NULL,
+ *grind_manifold = NULL;
+
+ player_regular_collider_configuration( s );
+
+ int nfront = player_collide_sphere( &s->rbf, manifold ),
+ nback = player_collide_sphere( &s->rbb, manifold + nfront ),
+ interface_len = nfront + nback;
+
+ interface_manifold = manifold;
+ grind_manifold = manifold + interface_len;
+
+ int grind_len = player_update_grind_collision( s, grind_manifold );
+
+ player_skate_apply_grind_model( s, grind_manifold, grind_len );
+ player_skate_apply_interface_model( s, manifold, interface_len );
+
+ rb_presolve_contacts( manifold, interface_len + grind_len );
+ player_collision_response( s, manifold, interface_len + grind_len );
+
+ player_skate_apply_grab_model( s );
+ player_skate_apply_friction_model( s );
+ player_skate_apply_jump_model( s );
+ player_skate_apply_air_model( s );
+
+ v3f gravity = { 0.0f, -9.6f, 0.0f };
+ v3_muladds( player.rb.v, gravity, k_rb_delta, player.rb.v );
+
+ v3_sub( player.rb.v, s->phys.v_prev, s->phys.a );
+ v3_muls( s->phys.a, 1.0f/VG_TIMESTEP_FIXED, s->phys.a );
+ v3_copy( player.rb.v, s->phys.v_prev );
+
+ player.rb.v[1] += -9.6f * VG_TIMESTEP_FIXED;
+
+ v3_muladds( player.rb.co, player.rb.v, VG_TIMESTEP_FIXED, player.rb.co );
+}
+
+VG_STATIC void player_physics_gui(void)
+{
+#if 0
+ return;
+
+ vg_uictx.cursor[0] = 0;
+ vg_uictx.cursor[1] = vg.window_y - 128;
+ vg_uictx.cursor[3] = 14;
+ ui_fill_x();
+
+ char buf[128];
+
+ snprintf( buf, 127, "v: %6.3f %6.3f %6.3f\n", player.phys.rb.v[0],
+ player.phys.rb.v[1],
+ player.phys.rb.v[2] );
+
+ ui_text( vg_uictx.cursor, buf, 1, 0 );
+ vg_uictx.cursor[1] += 14;
+
+
+ snprintf( buf, 127, "a: %6.3f %6.3f %6.3f (%6.3f)\n", player.phys.a[0],
+ player.phys.a[1],
+ player.phys.a[2],
+ v3_length(player.phys.a));
+ ui_text( vg_uictx.cursor, buf, 1, 0 );
+ vg_uictx.cursor[1] += 14;
+
+ float normal_acceleration = v3_dot( player.phys.a, player.phys.rb.up );
+ snprintf( buf, 127, "Normal acceleration: %6.3f\n", normal_acceleration );
+
+ ui_text( vg_uictx.cursor, buf, 1, 0 );
+ vg_uictx.cursor[1] += 14;
+
+ snprintf( buf, 127, "Normal Pressure: %6.3f\n", player.normal_pressure );
+ ui_text( vg_uictx.cursor, buf, 1, 0 );
+ vg_uictx.cursor[1] += 14;
+#endif
+}
+
+#endif /* PLAYER_PHYSICS_SKATE_H */
projects / carveJwlIkooP6JGAAIwe30JlM.git / blobdiff