fixed jitter

[carveJwlIkooP6JGAAIwe30JlM.git] / player_walk.c

#ifndef PLAYER_WALK_C
#define PLAYER_WALK_C

#include "player.h"
#include "input.h"
#include "audio.h"

VG_STATIC void player_walk_drop_in_vector( player_instance *player, v3f vec )
{
   struct player_walk *w = &player->_walk;

   v3f axis, init_dir;
   v3_cross( player->basis[1], w->state.drop_in_normal, axis );
   v3_cross( axis, w->state.drop_in_normal, init_dir );
   v3_normalize( init_dir );
   v3_muls( init_dir, 7.0f, vec );
}

VG_STATIC float player_xyspeed2( player_instance *player )
{
   v3f xy;
   v3_muladds( player->rb.v, player->basis[1], 
               -v3_dot( player->basis[1], player->rb.v ), xy );

   return v3_length2(xy);
}

VG_STATIC void player_walk_generic_to_skate( player_instance *player,
                                             enum skate_activity init,
                                             float yaw )
{
   player->subsystem = k_player_subsystem_skate;

   struct player_walk *w = &player->_walk;
   struct player_skate *s = &player->_skate;

   v3f v;

   if( player_xyspeed2(player) < 0.1f * 0.1f )
      q_mulv( player->rb.q, (v3f){0.0f,0.0f,1.6f}, v );
   else
      v3_copy( player->rb.v, v );

   s->state.activity_prev = k_skate_activity_ground;
   s->state.activity = init;

   v3f dir;
   v3_copy( v, dir );
   v3_normalize( dir );
   m3x3_mulv( player->invbasis, dir, dir );

   q_axis_angle( player->rb.q, (v3f){0.0f,1.0f,0.0f}, atan2f(-dir[0],-dir[2]) );
   q_mul( player->qbasis, player->rb.q, player->rb.q );
   q_normalize( player->rb.q );

   q_mulv( player->rb.q, (v3f){0.0f,1.0f,0.0f}, s->state.cog );
   v3_add( s->state.cog, player->rb.co, s->state.cog );

   v3_copy( v, s->state.cog_v );
   v3_copy( v, player->rb.v );

   player__skate_reset_animator( player );
   player__skate_clear_mechanics( player );
   rb_update_transform( &player->rb );
   v3_copy( (v3f){yaw,0.0f,0.0f}, s->state.trick_euler );

   if( init == k_skate_activity_air )
      player__approximate_best_trajectory( player );
}

VG_STATIC void player_walk_drop_in_to_skate( player_instance *player )
{
   player->immobile = 0;
   player->subsystem = k_player_subsystem_skate;

   struct player_walk *w = &player->_walk;
   struct player_skate *s = &player->_skate;
   s->state.activity_prev = k_skate_activity_ground;
   s->state.activity = k_skate_activity_ground;

   player__skate_clear_mechanics( player );
   player__skate_reset_animator( player );

   v3f init_velocity;
   player_walk_drop_in_vector( player, init_velocity );

   rb_update_transform( &player->rb );
   v3_muladds( player->rb.co, player->rb.to_world[1], 1.0f, s->state.cog );
   v3_copy( init_velocity, s->state.cog_v );
   v3_copy( init_velocity, player->rb.v );
   v3_copy( init_velocity, player->cam_velocity_smooth );
   v3_copy( (v3f){1.0f,0.0f,0.0f}, s->state.trick_euler );
}

VG_STATIC void player_walk_drop_in_overhang_transform( player_instance *player,
                                                       float t,
                                                       v3f co, v4f q )
{
   struct player_walk *w = &player->_walk;

   v3f axis;
   v3_cross( player->basis[1], w->state.drop_in_normal, axis );
   v3_normalize( axis );

   float a = acosf( w->state.drop_in_normal[1] ) * t;

   q_axis_angle( q, axis, a );

   float l = t * 0.5f,
         heading_angle = w->state.drop_in_angle;

   v3f overhang;
   overhang[0] = sinf( heading_angle ) * l;
   overhang[1] = 0.28f * l;
   overhang[2] = cosf( heading_angle ) * l;

   q_mulv( q, overhang, overhang );
   v3_add( w->state.drop_in_target, overhang, co );
}

VG_STATIC int player_walk_scan_for_drop_in( player_instance *player )
{
   struct player_walk *w = &player->_walk;
   world_instance *world = world_current_instance();

   v3f dir, center;
   q_mulv( player->rb.q, (v3f){0.0f,0.0f,1.0f}, dir );
   v3_muladds( player->rb.co, player->rb.to_world[1], -1.0f, center );

   ray_hit samples[20];
   int sample_count = 0;

   for( int i=0; i<20; i ++ )
   {
      float t = (float)i * (1.0f/19.0f),
            s = sinf( t * VG_PIf * 0.25f ),
            c = cosf( t * VG_PIf * 0.25f );

      v3f ray_dir, pos;
      v3_muls   ( player->rb.to_world[1], -c, ray_dir );
      v3_muladds( ray_dir, dir, -s, ray_dir );
      v3_muladds( center, ray_dir, -2.0f, pos );

      ray_hit *ray = &samples[ sample_count ];
      ray->dist = 2.0f;

      if( ray_world( world, pos, ray_dir, ray ) )
      {
         vg_line( pos, ray->pos, VG__RED );
         vg_line_pt3( ray->pos, 0.025f, VG__BLACK );
         
         sample_count ++;
      }
   }

   float min_a = 0.70710678118654752f;
   ray_hit *candidate = NULL;

   if( sample_count >= 2 )
   {
      for( int i=0; i<sample_count-1; i++ )
      {
         ray_hit *s0 = &samples[i],
                 *s1 = &samples[i+1];

         float a = v3_dot( s0->normal, s1->normal );

         if( (a < min_a) && (a >= -0.1f) && (s0->normal[1]>s1->normal[1]) )
         {
            min_a = a;
            candidate = s0;
         }
      }
   }

   if( candidate )
   {
      v4f pa, pb, pc;
      
      ray_hit *s0 = candidate,
              *s1 = candidate+1;

      vg_line( s0->pos, s1->pos, VG__WHITE );

      v3_copy( s0->normal, pa );
      v3_copy( s1->normal, pb );
      v3_cross( player->rb.to_world[1], dir, pc );
      v3_normalize( pc );

      pa[3] = v3_dot( pa, s0->pos );
      pb[3] = v3_dot( pb, s1->pos );
      pc[3] = v3_dot( pc, player->rb.co );

      v3f edge;
      if( plane_intersect3( pa, pb, pc, edge ) )
      {
         v3_copy( edge, w->state.drop_in_target );
         v3_copy( s1->normal, w->state.drop_in_normal );
         v3_copy( player->rb.co, w->state.drop_in_start );

         w->state.drop_in_start_angle = player_get_heading_yaw( player );
         w->state.drop_in_angle = atan2f( w->state.drop_in_normal[0],
                                          w->state.drop_in_normal[2] );

         /* TODO: scan multiple of these? */
         v3f oco;
         v4f oq;
         player_walk_drop_in_overhang_transform( player, 1.0f, oco, oq );

         v3f va = {0.0f,0.0f,-k_board_length - 0.3f},
             vb = {0.0f,0.0f, k_board_length + 0.3f};

         q_mulv( oq, va, va );
         q_mulv( oq, vb, vb );
         v3_add( oco, va, va );
         v3_add( oco, vb, vb );

         v3f v0;
         v3_sub( vb, va, v0 );
         v3_normalize( v0 );

         ray_hit ray;
         ray.dist = k_board_length*2.0f + 0.6f;
         
         if( ray_world( world, va, v0, &ray ) )
         {
            vg_line( va, vb, VG__RED );
            vg_line_pt3( ray.pos, 0.1f, VG__RED );
            vg_error( "invalidated\n" );
            return 0;
         }

         v3_muls( v0, -1.0f, v0 );
         if( ray_world( world, vb, v0, &ray ) )
         {
            vg_line( va, vb, VG__RED );
            vg_line_pt3( ray.pos, 0.1f, VG__RED );
            vg_error( "invalidated\n" );
            return 0;
         }

         player_walk_drop_in_vector( player, player->rb.v );
         return 1;
      }
      else
      {
         vg_error( "failed to find intersection of drop in\n" );
      }
   }

   return 0;
}

VG_STATIC void player__walk_pre_update( player_instance *player )
{
   struct player_walk *w = &player->_walk;

   if( !player->immobile )
      player_look( player, player->angles );

   if( w->state.outro_anim ){
      float outro_length = (float)w->state.outro_anim->length /
                                  w->state.outro_anim->rate,
            outro_time   = vg.time - w->state.outro_start_time;

      if( outro_time >= outro_length ){
         w->state.outro_anim = NULL;
         if( w->state.outro_type == k_walk_outro_drop_in ){
            player_walk_drop_in_to_skate( player );
         }
         else if( w->state.outro_type == k_walk_outro_jump_to_air ){
            player_walk_generic_to_skate( player, k_skate_activity_air, 0.0f );
         }
         else{
            player_walk_generic_to_skate( player, 
                                          k_skate_activity_ground, 1.0f );
         }

         return;
      }
   }
   else if( button_down( k_srbind_use ) && !player->immobile ){
      if( v3_dist2( player->rb.co, gzoomer.obj.rb.co ) <= 4.0f*4.0f ){
         player->subsystem = k_player_subsystem_drive;
      }
      else{
         if( !player_get_player_board(player) )
            return;

         if( w->state.activity == k_walk_activity_ground ){
            if( player_walk_scan_for_drop_in( player ) ){
               w->state.outro_type = k_walk_outro_drop_in;
               w->state.outro_anim = w->anim_drop_in;
               w->state.outro_start_time = vg.time;
               player->immobile = 1;

               struct player_avatar *av = player->playeravatar;
               m4x3_mulv( av->sk.final_mtx[ av->id_ik_foot_r ], 
                          av->sk.bones[ av->id_ik_foot_r ].co, 
                          w->state.drop_in_foot_anchor );
            }
            else{
               w->state.outro_type = k_walk_outro_regular;
               w->state.outro_anim = w->anim_intro;
               w->state.outro_start_time = vg.time;
               w->state.activity = k_walk_activity_lockedmove;

               if( player_xyspeed2(player) < 0.1f * 0.1f )
                  q_mulv( player->rb.q, (v3f){0.0f,0.0f,1.6f}, player->rb.v );
            }
         }
         else{
            w->state.outro_type = k_walk_outro_jump_to_air;
            w->state.outro_anim = w->anim_jump_to_air;
            w->state.outro_start_time = vg.time;
            return;
         }
      }
   }
   else if( button_down( k_srbind_jump ) && !player->immobile ){
      w->state.jump_queued = 1;
      w->state.jump_input_time = vg.time;
   }
}

VG_STATIC int player_walk_normal_standable( player_instance *player, v3f n )
{
   return v3_dot( n, player->basis[1] ) > 0.70710678118f;
}

VG_STATIC void player_accelerate( v3f v, v3f movedir, float speed, float accel )
{
   float currentspeed = v3_dot( v, movedir ),
         addspeed     = speed - currentspeed;

   if( addspeed <= 0 )
      return;

   float accelspeed = accel * k_rb_delta * speed;

   if( accelspeed > addspeed )
      accelspeed = addspeed;

   v3_muladds( v, movedir, accelspeed, v );
}

VG_STATIC void player_friction( v3f v )
{
   float speed = v3_length( v ),
         drop  = 0.0f,
         control = vg_maxf( speed, k_stopspeed );

   if( speed < 0.04f )
      return;

   drop += control * k_walk_friction * k_rb_delta;

   float newspeed = vg_maxf( 0.0f, speed - drop );
   newspeed /= speed;

   v3_muls( v, newspeed, v );
}

VG_STATIC void player__walk_update( player_instance *player )
{
   struct player_walk *w = &player->_walk;
   v3_copy( player->rb.co, w->state.prev_pos );
   v3_zero( player->rb.w );

   world_instance *world = world_current_instance();

   if( world->water.enabled ){
      if( player->rb.co[1]+0.4f < world->water.height ){
         audio_oneshot_3d( &audio_splash, player->rb.co, 40.0f, 1.0f );
         player__dead_transition( player );
         return;
      }
   }

   enum walk_activity prev_state = w->state.activity;

   if( player->immobile )
      return;

   w->collider.height = 2.0f;
   w->collider.radius = 0.3f;

   m4x3f mtx;
   m3x3_copy( player->rb.to_world, mtx );
   v3_add( player->rb.co, player->basis[1], mtx[3] );

   debug_capsule( mtx, w->collider.radius, w->collider.height, VG__WHITE );

   rb_ct manifold[64];
   int len;

   float yaw = player->angles[0];

   v3f forward_dir = { -sinf(yaw),         0.0f,  cosf(yaw) };
   v3f right_dir   = {  forward_dir[2],    0.0f, -forward_dir[0] };

   m3x3_mulv( player->basis, forward_dir, forward_dir );
   m3x3_mulv( player->basis, right_dir,   right_dir );

   v2f steer;
   joystick_state( k_srjoystick_steer, steer );

   w->move_speed = v2_length( steer );

   /* 
    * Collision detection
    */

   len = rb_capsule__scene( mtx, &w->collider, NULL, 
                            &world->rb_geo.inf.scene, manifold );
   rb_manifold_filter_coplanar( manifold, len, 0.01f );
   len = rb_manifold_apply_filtered( manifold, len );

   v3f surface_avg = { 0.0f, 0.0f, 0.0f };

   if( w->state.activity != k_walk_activity_lockedmove )
      w->state.activity = k_walk_activity_air;

   w->surface = k_surface_prop_concrete;

   for( int i=0; i<len; i++ ){
      struct contact *ct = &manifold[i];
      rb_debug_contact( ct );

      if( player_walk_normal_standable( player, ct->n ) ){
         if( w->state.activity != k_walk_activity_lockedmove )
            w->state.activity = k_walk_activity_ground;

         v3_add( surface_avg, ct->n, surface_avg );

         struct world_surface *surf = world_contact_surface( world, ct );
         if( surf->info.surface_prop > w->surface )
            w->surface = surf->info.surface_prop;
      }

      rb_prepare_contact( ct, k_rb_delta );
   }

   /* 
    * Move & Friction
    */
   float accel_speed = 0.0f, nominal_speed = 0.0f;
   v3f movedir;

   v3_muls( right_dir, steer[0], movedir );
   v3_muladds( movedir, forward_dir, steer[1], movedir );

   if( w->state.activity == k_walk_activity_ground ){
      v3_normalize( surface_avg );

      v3f tx, ty;
      rb_tangent_basis( surface_avg, tx, ty );

      if( v2_length2(steer) > 0.001f ){
         /* clip movement to the surface */
         float d = v3_dot(surface_avg,movedir);
         v3_muladds( movedir, surface_avg, -d, movedir );
      }

      accel_speed = k_walk_accel;
      nominal_speed = k_walkspeed;

      /* jump */
      if( w->state.jump_queued ){
         w->state.jump_queued = 0;

         f32 t = vg.time - w->state.jump_input_time;
         if( t < PLAYER_JUMP_EPSILON ){
            float d = v3_dot( player->basis[1], player->rb.v );
            v3_muladds( player->rb.v, player->basis[1], -d, player->rb.v );
            v3_muladds( player->rb.v, player->basis[1], 5.0f, player->rb.v );
            w->state.activity = k_walk_activity_air;
            prev_state = k_walk_activity_air;
            accel_speed = k_walk_air_accel;
            nominal_speed = k_airspeed;
         }
      }
      else{
         player_friction( player->rb.v );
      }
   }
   else{
      accel_speed = k_walk_air_accel;
      nominal_speed = k_airspeed;
   }

   if( v2_length2( steer ) > 0.001f ){
      player_accelerate( player->rb.v, movedir, nominal_speed, accel_speed );
      v3_normalize( movedir );
   }
   
   /*
    * Resolve velocity constraints
    */
   for( int j=0; j<5; j++ ){
      for( int i=0; i<len; i++ ){
         struct contact *ct = &manifold[i];
         
         /*normal */
         float vn = -v3_dot( player->rb.v, ct->n );

         float temp = ct->norm_impulse;
         ct->norm_impulse = vg_maxf( temp + vn, 0.0f );
         vn = ct->norm_impulse - temp;

         v3_muladds( player->rb.v, ct->n, vn, player->rb.v );
      }
   }

   /* stepping */
   if( w->state.activity == k_walk_activity_ground||
       prev_state == k_walk_activity_ground ){
      float max_dist = 0.4f;

      v3f pa, pb;
      v3_copy( player->rb.co, pa );
      v3_muladds( pa, player->basis[1],  w->collider.radius + max_dist, pa );
      v3_muladds( pa, player->basis[1], -max_dist * 2.0f, pb );
      vg_line( pa, pb, 0xff000000 );

      v3f n;
      float t;
      if( spherecast_world( world, pa, pb, w->collider.radius, &t, n ) != -1 ){
         if( player_walk_normal_standable( player, n ) ){
            v3_lerp( pa, pb, t, player->rb.co );
            v3_muladds( player->rb.co, player->basis[1], 
                        -w->collider.radius - k_penetration_slop,
                        player->rb.co );
            w->state.activity = k_walk_activity_ground;

            float d = -v3_dot(n,player->rb.v),
                  g = -k_gravity * k_rb_delta;
            v3_muladds( player->rb.v, n, d, player->rb.v );
            v3_muladds( player->rb.v, player->basis[1], g, player->rb.v );
         }
      }
   }

   /* 
    * Depenetrate
    */
   v3f dt;
   rb_depenetrate( manifold, len, dt );
   v3_add( dt, player->rb.co, player->rb.co );

   /* integrate */
   if( w->state.activity == k_walk_activity_air ){
      v3_muladds( player->rb.v, player->basis[1], -k_gravity*k_rb_delta, 
                  player->rb.v );
   }

   v3_muladds( player->rb.co, player->rb.v, k_rb_delta, player->rb.co );
   v3_add( player->rb.co, player->basis[1], mtx[3] );
   debug_capsule( mtx, w->collider.radius, w->collider.height, VG__GREEN );

   /* 
    * CCD routine 
    * ---------------------------------------------------
    *
    */
   v3f lwr_prev,
       lwr_now,
       lwr_offs = { 0.0f, w->collider.radius, 0.0f };

   v3_add( lwr_offs, w->state.prev_pos, lwr_prev );
   v3_add( lwr_offs, player->rb.co, lwr_now );

   v3f movedelta;
   v3_sub( player->rb.co, w->state.prev_pos, movedelta );

   float movedist = v3_length( movedelta );

   if( movedist > 0.3f ){
      float t, sr = w->collider.radius-0.04f;
      v3f n;

      if( spherecast_world( world, lwr_prev, lwr_now, sr, &t, n ) != -1 ){
         v3_lerp( lwr_prev, lwr_now, vg_maxf(0.01f,t), player->rb.co );
         player->rb.co[1] -= w->collider.radius;
         rb_update_transform( &player->rb );

         v3_add( player->rb.co, player->basis[1], mtx[3] );
         debug_capsule( mtx, w->collider.radius, w->collider.height, VG__RED );
      }
   }

   ent_gate *gate = 
      world_intersect_gates( world, player->rb.co, w->state.prev_pos );
   if( gate ){
      m4x3_mulv( gate->transport, player->rb.co, player->rb.co );
      m3x3_mulv( gate->transport, player->rb.v,  player->rb.v );

      v4f transport_rotation;
      m3x3_q( gate->transport, transport_rotation );
      q_mul( transport_rotation, player->rb.q, player->rb.q );

      rb_update_transform( &player->rb );

      w->state_gate_storage = w->state;
      player__pass_gate( player, gate );
   }
   rb_update_transform( &player->rb );
}

VG_STATIC void player__walk_restore( player_instance *player )
{
   struct player_walk *w = &player->_walk;
   w->state = w->state_gate_storage;
}

VG_STATIC void player__walk_post_update( player_instance *player )
{
   struct player_walk *w = &player->_walk;

   m4x3f mtx;
   m3x3_copy( player->rb.to_world, mtx );
   v3_add( player->rb.co, player->basis[1], mtx[3] );

   float substep = vg.time_fixed_extrapolate;
   v3_muladds( mtx[3], player->rb.v, k_rb_delta*substep, mtx[3] );
   debug_capsule( mtx, w->collider.radius, w->collider.height, VG__YELOW );


   /* Calculate header */
   v3f v;
   if( player_xyspeed2(player) > 0.1f*0.1f )
   {
      v3f v_xy;
      m3x3_mulv( player->invbasis, player->rb.v, v_xy );
      float a = atan2f( v_xy[0], v_xy[2] );

      q_axis_angle( player->rb.q, (v3f){0.0f,1.0f,0.0f}, a );
      q_mul( player->qbasis, player->rb.q, player->rb.q );
      q_normalize( player->rb.q );
   }

   vg_line_pt3( w->state.drop_in_target, 0.1f, VG__GREEN );
   v3f p1;
   v3_muladds( w->state.drop_in_target, w->state.drop_in_normal, 0.3f, p1 );
   vg_line( w->state.drop_in_target, p1, VG__GREEN );
   v3_muladds( w->state.drop_in_target, player->rb.to_world[1], 0.3f, p1 );
   vg_line( w->state.drop_in_target, p1, VG__GREEN );

   vg_line( w->state.drop_in_target, w->state.drop_in_foot_anchor, VG__WHITE );
   vg_line_pt3( w->state.drop_in_foot_anchor, 0.08f, VG__PINK );


   float a = player_get_heading_yaw( player );
   p1[0] = sinf( a );
   p1[1] = 0.0f;
   p1[2] = cosf( a );
   m3x3_mulv( player->basis, p1, p1 );

   v3_add( player->rb.co, p1, p1 );
   vg_line( player->rb.co, p1, VG__PINK );


   int walk_phase = 0;
   if( vg_fractf(w->walk_timer) > 0.5f )
      walk_phase = 1;
   else
      walk_phase = 0;

   if( (w->step_phase != walk_phase) && 
       (w->state.activity == k_walk_activity_ground ) )
   {
      audio_lock();
      if( w->surface == k_surface_prop_concrete ){
         audio_oneshot_3d( 
               &audio_footsteps[vg_randu32()%vg_list_size(audio_footsteps)], 
               player->rb.co, 40.0f, 1.0f 
         );
      }
      else if( w->surface == k_surface_prop_grass ){
         audio_oneshot_3d( 
           &audio_footsteps_grass[vg_randu32()%vg_list_size(audio_footsteps_grass)],
            player->rb.co, 40.0f, 1.0f 
         );
      }
      else if( w->surface == k_surface_prop_wood ){
         audio_oneshot_3d( 
           &audio_footsteps_wood[vg_randu32()%vg_list_size(audio_footsteps_wood)],
            player->rb.co, 40.0f, 1.0f 
         );
      }
      audio_unlock();
   }

   w->step_phase = walk_phase;
}

VG_STATIC void player__walk_animate( player_instance *player, 
                                    player_animation *dest )
{
   struct player_walk *w = &player->_walk;
   struct skeleton *sk = &player->playeravatar->sk;

   {
      float fly = (w->state.activity == k_walk_activity_air)? 1.0f: 0.0f,
            rate;

      if( w->state.activity == k_walk_activity_air )
         rate = 2.4f;
      else
         rate = 9.0f;

      w->blend_fly = vg_lerpf( w->blend_fly, fly, rate*vg.time_delta );
      w->blend_run = vg_lerpf( w->blend_run, w->move_speed, 2.0f*vg.time_delta);
   }

   player_pose apose, bpose;

   if( w->move_speed > 0.025f ){
      /* TODO move */
      float walk_norm = 30.0f/(float)w->anim_walk->length,
            run_norm  = 30.0f/(float)w->anim_run->length,
            walk_adv  = vg_lerpf( walk_norm, run_norm, w->move_speed );

      w->walk_timer += walk_adv * vg.time_delta;
   }
   else{
      w->walk_timer = 0.0f;
   }

   float walk_norm = (float)w->anim_walk->length/30.0f,
         run_norm  = (float)w->anim_run->length/30.0f,
         t = w->walk_timer,
         l = vg_clampf( w->blend_run*15.0f, 0.0f, 1.0f ),
         idle_walk = vg_clampf( (w->blend_run-0.1f)/(1.0f-0.1f), 0.0f, 1.0f );

   /* walk/run */
   skeleton_sample_anim( sk, w->anim_walk, t*walk_norm, apose );
   skeleton_sample_anim( sk, w->anim_run,  t*run_norm, bpose );

   skeleton_lerp_pose( sk, apose, bpose, l, apose );

   /* idle */
   skeleton_sample_anim( sk, w->anim_idle, vg.time*0.1f, bpose );
   skeleton_lerp_pose( sk, apose, bpose, 1.0f-idle_walk, apose );

   /* air */
   skeleton_sample_anim( sk, w->anim_jump, vg.time*0.6f, bpose );
   skeleton_lerp_pose( sk, apose, bpose, w->blend_fly, apose );

   /* Create transform */
   rb_extrapolate( &player->rb, dest->root_co, dest->root_q );

   float walk_yaw = player_get_heading_yaw( player );

   if( w->state.outro_anim ){
      struct player_avatar *av = player->playeravatar;
      float outro_length = (float)w->state.outro_anim->length /
                                  w->state.outro_anim->rate,
            outro_time   = vg.time - w->state.outro_start_time,
            outro_t      = outro_time / outro_length;

      /* TODO: Compression */
      skeleton_sample_anim_clamped( sk, w->state.outro_anim, 
                                    outro_time, bpose );
      skeleton_lerp_pose( sk, apose, bpose, outro_t * 10.0f, dest->pose );

      if( w->state.outro_type == k_walk_outro_drop_in ){
         float inv_rate      = 1.0f / w->state.outro_anim->rate,
               anim_frames   = w->state.outro_anim->length * inv_rate,
               step_frames   = 12.0f * inv_rate,
               commit_frames = 6.0f * inv_rate,
               drop_frames   = anim_frames - step_frames,
               step_t        = vg_minf( 1.0f, outro_time / step_frames ),
               remaind_time  = vg_maxf( 0.0f, outro_time - step_frames ),
               dop_t         = vg_minf( 1.0f, remaind_time / drop_frames ),
               commit_t      = vg_minf( 1.0f, remaind_time / commit_frames );

         walk_yaw = vg_alerpf( w->state.drop_in_start_angle, 
                               w->state.drop_in_angle, step_t );

         v3_lerp( w->state.drop_in_start, w->state.drop_in_target,
                  step_t, player->rb.co );
         q_axis_angle( dest->root_q, (v3f){0.0f,1.0f,0.0f}, walk_yaw + VG_PIf );

         m4x3f transform, inverse;
         q_m3x3( dest->root_q, transform );
         v3_copy( dest->root_co, transform[3] );
         m4x3_invert_affine( transform, inverse );

         v3f anchored_pos;
         m4x3_mulv( inverse, w->state.drop_in_foot_anchor, anchored_pos );

         v3_lerp( dest->pose[ av->id_ik_foot_r-1 ].co, anchored_pos, 
                  1.0f-commit_t,
                  dest->pose[ av->id_ik_foot_r-1 ].co );
         

         /* the drop in bit */
         if( step_t >= 1.0f ){
            v4f final_q;
            player_walk_drop_in_overhang_transform( player, dop_t, 
                                                    player->rb.co, final_q );
            q_mul( final_q, dest->root_q, dest->root_q );

            v4_copy( dest->root_q, player->rb.q );
            v3_muladds( dest->root_co, player->rb.to_world[1], 
                        -0.1f * dop_t, dest->root_co );

            skeleton_copy_pose( sk, dest->pose, player->holdout_pose );
            player->holdout_time = 1.0f;
         }
         return;
      }
      else{
         v3_muladds( dest->root_co, player->rb.to_world[1], 
                     -0.1f * outro_t, dest->root_co );

         skeleton_copy_pose( sk, dest->pose, player->holdout_pose );
         player->holdout_time = 1.0f;
      }
   }
   else{
      skeleton_copy_pose( sk, apose, dest->pose );
   }

   q_axis_angle( dest->root_q, (v3f){0.0f,1.0f,0.0f}, walk_yaw + VG_PIf );
   q_mul( player->qbasis, dest->root_q, dest->root_q );
   q_normalize( dest->root_q );
}

VG_STATIC void player__walk_post_animate( player_instance *player )
{
   /* 
    * Camera 
    */
   struct player_walk *w = &player->_walk;
   struct player_avatar *av = player->playeravatar;

   if( w->state.outro_anim )
   {
      float outro_length = (float)w->state.outro_anim->length /
                                  w->state.outro_anim->rate,
            outro_time   = vg.time - w->state.outro_start_time,
            outro_t      = outro_time / outro_length;

      player->cam_velocity_influence = outro_t;
   }
   else
      player->cam_velocity_influence = 0.0f;
}


VG_STATIC void player__walk_im_gui( player_instance *player )
{
   struct player_walk *w = &player->_walk;
   player__debugtext( 1, "V:  %5.2f %5.2f %5.2f",player->rb.v[0],
                                                 player->rb.v[1],
                                                 player->rb.v[2] );
   player__debugtext( 1, "CO: %5.2f %5.2f %5.2f",player->rb.co[0],
                                                 player->rb.co[1],
                                                 player->rb.co[2] );
   player__debugtext( 1, "activity: %s\n",
                           (const char *[]){ "k_walk_activity_air",
                                             "k_walk_activity_ground",
                                             "k_walk_activity_sleep",
                                             "k_walk_activity_lockedmove" }
                                             [w->state.activity] );
   player__debugtext( 1, "surface: %s\n",
                           (const char *[]){ "concrete",
                                             "wood",
                                             "grass",
                                             "tiles",
                                             "metal" }
                                             [w->surface] );

   if( w->state.outro_anim ){
      float outro_length = (float)w->state.outro_anim->length /
                                  w->state.outro_anim->rate,
            outro_time   = vg.time - w->state.outro_start_time;
      player__debugtext( 1, "outro time: %f / %f", outro_time, outro_length );
   }
}

VG_STATIC void player__walk_bind( player_instance *player )
{
   struct player_walk *w = &player->_walk;
   struct player_avatar *av = player->playeravatar;
   struct skeleton *sk = &av->sk;

   w->anim_idle         = skeleton_get_anim( sk, "idle_cycle+y" );
   w->anim_walk         = skeleton_get_anim( sk, "walk+y" );
   w->anim_run          = skeleton_get_anim( sk, "run+y" );
   w->anim_jump         = skeleton_get_anim( sk, "jump+y" );
   w->anim_jump_to_air  = skeleton_get_anim( sk, "jump_to_air" );
   w->anim_drop_in      = skeleton_get_anim( sk, "drop_in" );
   w->anim_intro        = skeleton_get_anim( sk, "into_skate" );
}

VG_STATIC void player__walk_transition( player_instance *player, v3f angles )
{
   struct player_walk *w = &player->_walk;
   w->state.activity = k_walk_activity_air;

   v3f fwd = { 0.0f, 0.0f, 1.0f };
   q_mulv( player->rb.q, fwd, fwd );
   m3x3_mulv( player->invbasis, fwd, fwd );

   q_axis_angle( player->rb.q, (v3f){0.0f,1.0f,0.0f}, atan2f(fwd[0], fwd[2]) );
   q_mul( player->qbasis, player->rb.q, player->rb.q );
   q_normalize( player->rb.q );

   rb_update_transform( &player->rb );
}

VG_STATIC void player__walk_reset( player_instance *player, ent_spawn *rp )
{
   struct player_walk *w = &player->_walk;
   w->state.activity = k_walk_activity_air;
   w->state.outro_type = k_walk_outro_none;
   w->state.outro_anim = NULL;
   w->state.outro_start_time = 0.0;
}

#endif /* PLAYER_DEVICE_WALK_H */