df shadows

[carveJwlIkooP6JGAAIwe30JlM.git] / player_physics_skate.h

/*
 * 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 */