[carveJwlIkooP6JGAAIwe30JlM.git] / player_device_walk.h

#ifndef PLAYER_DEVICE_WALK_H
#define PLAYER_DEVICE_WALK_H

#include "player_interface.h"
#include "skeleton.h"
#include "player_model.h"

VG_STATIC float
   k_walkspeed             = 10.0f,
   k_airspeed              = 2.0f,
   k_stopspeed             = 4.0f,
   k_walk_accel            = 10.0f,
   k_walk_air_accel        = 7.0f,
   k_walk_friction         = 10.0f,
   k_walk_step_height      = 0.2f;

struct player_device_walk
{
   rb_capsule collider;

   struct
   {
      v3f angles;

      enum walk_activity
      {
         k_walk_activity_air,
         k_walk_activity_ground,
         k_walk_activity_sleep
      }
      activity;
   }
   state,
   state_gate_storage;

   enum mdl_surface_prop surface;

   struct skeleton_anim *anim_walk, *anim_run, *anim_idle, *anim_jump;

   float blend_fly,
         blend_run,
         blend_walk,

         move_speed,
         walk_timer;
};

VG_STATIC void player_walk_pre_update( player_interface *player,
                                       player_attachment *at )
{
   struct player_device_walk *w = at->storage;
   player_look( player, w->state.angles );

#if 0
   v3f walk = {  player->input_walkh->axis.value,
                 0.0f, 
                -player->input_walkv->axis.value };

   v3_muls( walk, 10.0f * vg.time_delta, walk );

   m3x3f m;
   euler_m3x3( w->angles, m );
   v3_muladds( player->rb.co, m[0], walk[0], player->rb.co );
   v3_muladds( player->rb.co, m[1], walk[1], player->rb.co );
   v3_muladds( player->rb.co, m[2], walk[2], player->rb.co );
#endif
}

VG_STATIC int player_walk_normal_standable( v3f n )
{
   return n[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_interface *player,
                                   player_attachment *at )
{
   struct player_device_walk *w = at->storage;
   w->collider.height = 2.0f;
   w->collider.radius = 0.3f;

   m4x3f mtx;
   m3x3_identity( mtx );
   v3_add( player->rb.co, (v3f){0.0f, 1.0f, 0.0f}, mtx[3] );

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

   rb_ct manifold[64];
   int len;

   float yaw = w->state.angles[0];

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

   v2f walk = { player->input_walkh->axis.value,
                player->input_walkv->axis.value };

   if( v2_length2(walk) > 0.001f )
      v2_normalize_clamp( walk );

   w->move_speed = v2_length( walk );

   /* 
    * 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 };
   w->state.activity = k_walk_activity_air;

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

      if( player_walk_normal_standable( ct->n ) )
      {
         w->state.activity = k_walk_activity_ground;
         v3_add( surface_avg, ct->n, surface_avg );
      }

      rb_prepare_contact( ct );
   }
   

   /* 
    * Move & Friction
    */
   float accel_speed = 0.0f, nominal_speed = 0.0f;
   v3f movedir;
   v3_muls( right_dir,   walk[0], movedir );
   v3_muladds( movedir, forward_dir, walk[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(walk) > 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( player->input_jump->button.value )
      {
         player->rb.v[1] = 5.0f;
         w->state.activity = k_walk_activity_air;
         accel_speed = k_walk_air_accel;
         nominal_speed = k_airspeed;
      }
      else
      {
         player_friction( player->rb.v );

         struct world_material *surface_mat = world_contact_material(manifold);
         w->surface = surface_mat->info.surface_prop;
      }
   }
   else
   {
      accel_speed = k_walk_air_accel;
      nominal_speed = k_airspeed;
   }

   if( v2_length2(walk) > 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 );
      }
   }

   /* 
    * Depenetrate
    */
   v3f dt;
   v3_zero( dt );
   for( int j=0; j<8; j++ )
   {
      for( int i=0; i<len; i++ )
      {
         struct contact *ct = &manifold[i];

         float resolved_amt = v3_dot( ct->n, dt ),
               remaining    = (ct->p-k_penetration_slop) - resolved_amt,
               apply        = vg_maxf( remaining, 0.0f ) * 0.3f;

         v3_muladds( dt, ct->n, apply, dt );
      }
   }
   v3_add( dt, player->rb.co, player->rb.co );

   /* TODO: Stepping......
    *
    *        ideas; walkgrid style steps
    */
#if 0
   if( w->state.activity == k_walk_activity_ground )
   {
      /* step */
      float max_dist = 0.4f;

      v3f pa, pb;
      v3_copy( player->rb.co, pa );
      pa[1] += w->collider.radius + max_dist;

      v3_muladds( pa, (v3f){0.0f,1.0f,0.0f}, -max_dist * 2.0f, pb );
      vg_line( pa, pb, 0xff000000 );

      v3f n;
      float t;
      if( spherecast_world( pa, pb, w->collider.radius, &t, n ) != -1 )
      {
         if( player_walk_normal_standable( n ) )
         {
            v3_lerp( pa, pb, t, player->rb.co );
            player->rb.co[1] -= w->collider.radius;
         }
      }
   }
#endif


   /* integrate */
   if( w->state.activity == k_walk_activity_air )
      player->rb.v[1] += -k_gravity * k_rb_delta;

   v3_muladds( player->rb.co, player->rb.v, k_rb_delta, player->rb.co );


   v3_add( player->rb.co, (v3f){0.0f, 1.0f, 0.0f}, mtx[3] );
   debug_capsule( mtx, w->collider.radius, w->collider.height, VG__GREEN );
}

VG_STATIC void player_walk_post_update( player_interface *player,
                                        player_attachment *at )
{
   struct player_device_walk *w = at->storage;

   m4x3f mtx;
   m3x3_identity( mtx );
   v3_add( player->rb.co, (v3f){0.0f, 1.0f, 0.0f}, mtx[3] );

   float substep = vg_clampf( vg.accumulator / k_rb_delta, 0.0f, 1.0f );
   v3_muladds( mtx[3], player->rb.v, k_rb_delta*substep, mtx[3] );

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

VG_STATIC void player_walk_ui( player_interface *player,
                               player_attachment *at )
{
   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] );
}

VG_STATIC void player_walk_bind( player_interface *player,
                                 player_attachment *at )
{
   struct player_device_walk *w = at->storage;
   struct player_avatar *av = player->playeravatar;
   struct skeleton *sk = &av->sk;

   w->anim_idle = skeleton_get_anim( sk, "idle_cycle" );
   w->anim_walk = skeleton_get_anim( sk, "walk" );
   w->anim_run  = skeleton_get_anim( sk, "run" );
   w->anim_jump = skeleton_get_anim( sk, "jump" );
}

VG_STATIC void player_walk_pose( player_interface *player,
                                 player_attachment *at, 
                                 player_pose pose, m4x3f transform )
{
   struct player_device_walk *w = at->storage;
   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 *
                               (1.0f + player->input_walk->button.value*0.5f),
                               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, pose );

   /* Create transform matrix */
   m3x3f rotation_mtx;
   v4f rot;
   q_axis_angle( rot, (v3f){0.0f,1.0f,0.0f}, -w->state.angles[0]-VG_PIf*0.5f );
   q_m3x3( rot, rotation_mtx );

   rb_extrapolate_transform( &player->rb, transform );
   m3x3_copy( rotation_mtx, transform );
}

VG_STATIC void player_walk_get_camera( player_interface *player,
                                       player_attachment *at, camera *cam )
{
   struct player_device_walk *w = at->storage;
   struct player_avatar *av = player->playeravatar;

   /* FIXME: viewpoint entity */
   v3f vp = {-0.1f,1.8f,0.0f};
   m4x3_mulv( av->sk.final_mtx[ av->id_head-1 ], vp, cam->pos );
   v3_copy( w->state.angles, cam->angles );
   cam->fov = 90.0f;
}

VG_STATIC void player_walk_transport( player_interface *player,
                                      player_attachment *at,
                                      teleport_gate *gate )
{
   struct player_device_walk *w = at->storage;

   m4x3_mulv( gate->transport, player->rb.co, player->rb.co );
   m3x3_mulv( gate->transport, player->rb.v,  player->rb.v );

   /* analytical rotation of yaw */
   v3f fwd_dir = { cosf(w->state.angles[0]),
                   0.0f,
                   sinf(w->state.angles[0])};
   m3x3_mulv( gate->transport, fwd_dir, fwd_dir );
   w->state.angles[0] = atan2f( fwd_dir[2], fwd_dir[0] );

   w->state_gate_storage = w->state;
}

VG_STATIC player_device player_device_walk =
{
   .pre_update    = player_walk_pre_update,
   .update        = player_walk_update,
   .post_update   = player_walk_post_update,
   .get_camera    = player_walk_get_camera,
   .debug_ui      = player_walk_ui,
   .bind          = player_walk_bind,
   .pose          = player_walk_pose,
   .gate_transport= player_walk_transport
};

#endif /* PLAYER_DEVICE_WALK_H */