X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=player_skate.c;h=7088cf1377a24f29453a49839bc43d1540bf7469;hb=b1557496a922904b7abcd47d5a59a6da44d70fe8;hp=87777631ae3e2b42d6477a682f82d2026a7c33cc;hpb=185ff7755bb44b98fe502b1b1c54d3870c45dfa1;p=carveJwlIkooP6JGAAIwe30JlM.git

diff --git a/player_skate.c b/player_skate.c
index 8777763..7088cf1 100644
--- a/player_skate.c
+++ b/player_skate.c
@@ -44,6 +44,8 @@ VG_STATIC int skate_collide_smooth( player_instance *player,
       man[i].rbb = NULL;
    }
 
+   return len;
+
    rb_manifold_filter_coplanar( man, len, 0.05f );
 
    if( len > 1 )
@@ -147,13 +149,11 @@ VG_STATIC int skate_grind_collide( player_instance *player, rb_ct *contact )
    return 0;
 }
 
-VG_STATIC int skate_grind_scansq( player_instance *player, v3f ra )
+VG_STATIC int skate_grind_scansq( player_instance *player, v3f pos,
+                                  v3f result_co, v3f result_dir, v3f result_n )
 {
-   v3f pos;
-   m4x3_mulv( player->rb.to_world, ra, pos );
-
    v4f plane;
-   v3_copy( player->rb.to_world[2], plane );
+   v3_copy( player->rb.v, plane );
    v3_normalize( plane );
    plane[3] = v3_dot( plane, pos );
 
@@ -170,7 +170,9 @@ VG_STATIC int skate_grind_scansq( player_instance *player, v3f ra )
    m3x3_copy( player->rb.to_world, mtx );
    v3_copy( pos, mtx[3] );
 
+#if 0
    debug_sphere( mtx, r, VG__CYAN );
+#endif
    
    bh_iter it;
    bh_iter_init( 0, &it );
@@ -180,7 +182,8 @@ VG_STATIC int skate_grind_scansq( player_instance *player, v3f ra )
    {
       v2f co;
       v2f normal;
-      v3f normal3;
+      v3f normal3,
+          centroid;
    }
    samples[48];
 
@@ -229,6 +232,10 @@ VG_STATIC int skate_grind_scansq( player_instance *player, v3f ra )
             v3_copy( normal, sample->normal3 ); /* normalize later
                                                    if we want to us it */
 
+            v3_muls(                      tri[0], 1.0f/3.0f, sample->centroid );
+            v3_muladds( sample->centroid, tri[1], 1.0f/3.0f, sample->centroid );
+            v3_muladds( sample->centroid, tri[2], 1.0f/3.0f, sample->centroid );
+
             v2_normalize( sample->normal );
             sample_count ++;
 
@@ -244,10 +251,12 @@ VG_STATIC int skate_grind_scansq( player_instance *player, v3f ra )
       return 0;
 
    v3f average_position,
-       average_direction;
+       average_direction,
+       average_normal;
 
    v3_zero( average_position );
    v3_zero( average_direction );
+   v3_zero( average_normal );
 
    int passed_samples = 0;
    
@@ -264,38 +273,57 @@ VG_STATIC int skate_grind_scansq( player_instance *player, v3f ra )
 
          sj = &samples[j];
 
-         if( v2_dist2( si->co, sj->co ) <= (0.01f*0.01f) &&
-             v2_dot( si->normal, sj->normal ) < 0.7f )
-         {
-            /* TODO: Filter concave */
+         /* non overlapping */
+         if( v2_dist2( si->co, sj->co ) >= (0.01f*0.01f) )
+            continue;
 
-            v3f p0;
-            v3_muls( support_axis, sj->co[0], p0 );
-            p0[1] += sj->co[1];
+         /* not sharp angle */
+         if( v2_dot( si->normal, sj->normal ) >= 0.7f )
+            continue;
 
-            v3_add( average_position, p0, average_position );
-            
-            v3f n0, n1, dir;
-            v3_copy( si->normal3, n0 );
-            v3_copy( sj->normal3, n1 );
-            v3_cross( n0, n1, dir );
-            v3_normalize( dir );
+         /* not convex */
+         v3f v0;
+         v3_sub( sj->centroid, si->centroid, v0 );
+         if( v3_dot( v0, si->normal3 ) >= 0.0f ||
+             v3_dot( v0, sj->normal3 ) <= 0.0f )
+            continue;
 
-            /* make sure the directions all face a common hemisphere */
-            v3_muls( dir, vg_signf(v3_dot(dir,plane)), dir );
+         v3f p0;
+         v3_muls( support_axis, sj->co[0], p0 );
+         p0[1] += sj->co[1];
 
-            v3_add( average_direction, dir, average_direction );
-            passed_samples ++;
-         }
+         v3_add( average_position, p0, average_position );
+         
+         v3f n0, n1, dir;
+         v3_copy( si->normal3, n0 );
+         v3_copy( sj->normal3, n1 );
+         v3_cross( n0, n1, dir );
+         v3_normalize( dir );
+
+         /* make sure the directions all face a common hemisphere */
+         v3_muls( dir, vg_signf(v3_dot(dir,plane)), dir );
+         v3_add( average_direction, dir, average_direction );
+
+         if( si->normal3[1] > sj->normal3[1] )
+            v3_add( si->normal3, average_normal, average_normal );
+         else
+            v3_add( sj->normal3, average_normal, average_normal );
+
+         passed_samples ++;
       }
    }
 
    if( !passed_samples )
       return 0;
 
+   if( (v3_length2( average_direction ) <= 0.001f) ||
+       (v3_length2( average_normal ) <= 0.001f ) )
+      return 0;
+
    float div = 1.0f/(float)passed_samples;
    v3_muls( average_position, div, average_position );
-   v3_muls( average_direction, div, average_direction ); /* !! not normed */
+   v3_normalize( average_direction );
+   v3_normalize( average_normal );
    
    v3_add( pos, average_position, average_position );
    vg_line_pt3( average_position, 0.02f, VG__GREEN );
@@ -305,36 +333,11 @@ VG_STATIC int skate_grind_scansq( player_instance *player, v3f ra )
    v3_muladds( average_position, average_direction, -0.35f, p1 );
    vg_line( p0, p1, VG__PINK );
 
-   if( passed_samples )
-   {
-      v3f displacement, dir;
-      v3_sub( pos, average_position, displacement );
-      v3_copy( displacement, dir );
-      v3_normalize( dir );
-
-      v3f rv, raW;
-      q_mulv( player->rb.q, ra, raW );
-
-      v3_cross( player->rb.w, raW, rv );
-      v3_add( player->rb.v, rv, rv );
-
-      v3_muladds( rv, player->rb.to_world[2],
-                      -v3_dot( rv, player->rb.to_world[2] ), rv );
-
-      v3f Fd, Fs, F;
-      v3_muls( displacement, -k_grind_spring, Fs );
-      v3_muls( rv, -k_grind_dampener, Fd );
+   v3_copy( average_position, result_co );
+   v3_copy( average_normal, result_n );
+   v3_copy( average_direction, result_dir );
 
-      v3_add( Fd, Fs, F );
-      v3_muls( F, k_rb_delta, F );
-      
-      v3_add( player->rb.v, F, player->rb.v );
-      v3f wa;
-      v3_cross( raW, F, wa );
-      v3_add( player->rb.w, wa, player->rb.w );
-
-      /* Constraint based */
-   }
+   return passed_samples;
 }
 
 /*
@@ -589,8 +592,10 @@ VG_STATIC void skate_apply_grind_model( player_instance *player,
                  player->input_js1v->axis.value };
    v2_normalize_clamp( steer );
 
+#if 0
    s->state.steery -= steer[0] * k_steer_air * k_rb_delta;
    s->state.steerx += steer[1] * s->state.reverse * k_steer_air * k_rb_delta;
+#endif
    
 #if 0
    v4f rotate;
@@ -720,7 +725,7 @@ VG_STATIC void skate_apply_air_model( player_instance *player )
       limiter *= limiter;
       limiter = 1.0f-limiter;
 
-      if( fabsf(angle) < 0.99f )
+      if( fabsf(angle) < 0.9999f )
       {
          v4f correction;
          q_axis_angle( correction, axis, 
@@ -733,9 +738,11 @@ VG_STATIC void skate_apply_air_model( player_instance *player )
                  player->input_js1v->axis.value };
    v2_normalize_clamp( steer );
 
+#if 0
    s->state.steery -= steer[0] * k_steer_air * VG_TIMESTEP_FIXED;
    s->state.steerx += steer[1] * s->state.reverse * k_steer_air 
                                                          * limiter * k_rb_delta;
+#endif
    s->land_dist = time_to_impact;
    v3_copy( target_normal, s->land_normal );
 }
@@ -1062,7 +1069,23 @@ VG_STATIC void skate_apply_friction_model( player_instance *player )
          steer = input * (1.0f-(s->state.jump_charge+grab)*0.4f),
          steer_scaled = vg_signf(steer) * powf(steer,2.0f) * k_steer_ground;
 
-   s->state.steery -= steer_scaled * k_rb_delta;
+   v3f steer_axis;
+   v3_muls( player->rb.to_world[1], -vg_signf( steer_scaled ), steer_axis );
+
+   float current  = v3_dot( player->rb.to_world[1], player->rb.w ),
+         addspeed = (steer_scaled * -1.0f) - current,
+         maxaccel = 26.0f * k_rb_delta,
+         accel    = vg_clampf( addspeed, -maxaccel, maxaccel );
+
+   v3_muladds( player->rb.w, player->rb.to_world[1], accel, player->rb.w );
+
+
+#if 0
+   player_accelerate( player->rb.w, steer_axis,
+                      fabsf(steer_scaled) * 1.0f, 30.0f );
+
+   //s->state.steery -= steer_scaled * k_rb_delta;
+#endif
 }
 
 VG_STATIC void skate_apply_jump_model( player_instance *player )
@@ -1117,9 +1140,11 @@ VG_STATIC void skate_apply_jump_model( player_instance *player )
                     player->input_js1v->axis.value };
       v2_normalize_clamp( steer );
 
+#if 0
       float maxspin = k_steer_air * k_rb_delta * k_spin_boost;
       s->state.steery_s = -steer[0] * maxspin;
       s->state.steerx = s->state.steerx_s;
+#endif
       s->state.lift_frames ++;
 
       /* FIXME audio events */
@@ -1186,8 +1211,11 @@ VG_STATIC void skate_apply_cog_model( player_instance *player )
 {
    struct player_skate *s = &player->_skate;
 
-   v3f ideal_cog, ideal_diff;
-   v3_muladds( player->rb.co, player->rb.to_world[1],
+   v3f ideal_cog, ideal_diff, ideal_dir;
+   v3_copy( s->state.up_dir, ideal_dir );
+   v3_normalize( ideal_dir );
+
+   v3_muladds( player->rb.co, ideal_dir,
                1.0f-player->input_grab->axis.value, ideal_cog );
    v3_sub( ideal_cog, s->state.cog, ideal_diff );
 
@@ -1219,20 +1247,24 @@ VG_STATIC void skate_collision_response( player_instance *player,
       {
          struct contact *ct = &manifold[i];
          
-         v3f dv, delta;
+         v3f rv, delta;
          v3_sub( ct->co, player->rb.co, delta ); 
-         v3_cross( player->rb.w, delta, dv );
-         v3_add( player->rb.v, dv, dv );
+         v3_cross( player->rb.w, delta, rv );
+         v3_add( player->rb.v, rv, rv );
+
+         v3f raCn;
+         v3_cross( delta, ct->n, raCn );
 
-         float vn = -v3_dot( dv, ct->n );
-         vn += ct->bias;
+         float normal_mass = 1.0f / (1.0f + v3_dot(raCn,raCn));
+         float vn = v3_dot( rv, ct->n );
+         float lambda = normal_mass * ( -vn + ct->bias );
 
          float temp = ct->norm_impulse;
-         ct->norm_impulse = vg_maxf( temp + vn, 0.0f );
-         vn = ct->norm_impulse - temp;
+         ct->norm_impulse = vg_maxf( temp + lambda, 0.0f );
+         lambda = ct->norm_impulse - temp;
 
          v3f impulse;
-         v3_muls( ct->n, vn, impulse );
+         v3_muls( ct->n, lambda, impulse );
 
          if( fabsf(v3_dot( impulse, player->rb.to_world[2] )) > 10.0f ||
              fabsf(v3_dot( impulse, player->rb.to_world[1] )) > 50.0f )
@@ -1252,13 +1284,18 @@ VG_STATIC void skate_collision_response( player_instance *player,
           * components.
           */
          
-         float wy = v3_dot( player->rb.to_world[1], impulse ) * 0.8f,
+         float wy = v3_dot( player->rb.to_world[1], impulse ) * 1.0f,
                wx = v3_dot( player->rb.to_world[0], impulse ) * 1.0f,
-               wz = v3_dot( player->rb.to_world[2], impulse ) * 2.0f;
+               wz = v3_dot( player->rb.to_world[2], 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 );
          v3_muladds( player->rb.w, player->rb.to_world[2], wz, player->rb.w );
+
+
+         v3_cross( player->rb.w, delta, rv );
+         v3_add( player->rb.v, rv, rv );
+         vn = v3_dot( rv, ct->n );
       }
    }
 }
@@ -1274,15 +1311,23 @@ VG_STATIC void skate_integrate( player_instance *player )
    v3_muladds( player->rb.co, player->rb.v, k_rb_delta, player->rb.co );
 #endif
 
-   float decay_rate = 0.5f*0.125f;
+   float decay_rate = 1.0f - (k_rb_delta * 3.0f);
 
+#if 0
    if( s->state.activity == k_skate_activity_air )
    {
       float dist = 1.0f-(s->land_dist/4.0f);
       decay_rate = 0.5f * vg_maxf( dist*dist, 0.0f );
    }
+#endif
+
+   float wx = v3_dot( player->rb.w, player->rb.to_world[0] ) * decay_rate,
+         wy = v3_dot( player->rb.w, player->rb.to_world[1] ),
+         wz = v3_dot( player->rb.w, player->rb.to_world[2] ) * decay_rate;
 
-   v3_lerp( player->rb.w, (v3f){0.0f,0.0f,0.0f}, decay_rate, player->rb.w );
+   v3_muls(                  player->rb.to_world[0], wx, player->rb.w );
+   v3_muladds( player->rb.w, player->rb.to_world[1], wy, player->rb.w );
+   v3_muladds( player->rb.w, player->rb.to_world[2], wz, player->rb.w );
 
 #ifndef SKATE_CCD
    if( v3_length2( player->rb.w ) > 0.0f )
@@ -1299,6 +1344,7 @@ VG_STATIC void skate_integrate( player_instance *player )
 #endif
 
    /* integrate steering velocities */
+#if 0
    v4f rotate; 
    float l = (s->state.activity == k_skate_activity_air)? 0.04f: 0.24f;
 
@@ -1313,6 +1359,7 @@ VG_STATIC void skate_integrate( player_instance *player )
 
    s->state.steerx = 0.0f;
    s->state.steery = 0.0f;
+#endif
 
    s->state.flip_time += s->state.flip_rate * k_rb_delta;
    rb_update_transform( &player->rb );
@@ -1346,6 +1393,17 @@ VG_STATIC void player__skate_pre_update( player_instance *player )
       return;
    }
 
+   if( vg_input_button_down( player->input_reset ) )
+   {
+      player->rb.co[1] += 2.0f;
+      s->state.cog[1] += 2.0f;
+      q_axis_angle( player->rb.q, (v3f){1.0f,0.0f,0.0f}, VG_PIf * 0.25f );
+      v3_zero( player->rb.w );
+      v3_zero( player->rb.v );
+
+      rb_update_transform( &player->rb );
+   }
+
    int trick_id; 
    if( (s->state.lift_frames > 0) &&
        (trick_id = player_skate_trick_input( player )) )
@@ -1401,52 +1459,300 @@ VG_STATIC void player__skate_update( player_instance *player )
    v3_copy( player->rb.co, s->state.prev_pos );
    s->state.activity_prev = s->state.activity;
 
-   float l = k_board_length,
-         w = 0.13f;
-
-   v3f wheel_positions[] = 
+   struct board_collider
    {
-      { -w, 0.0f, -l },
-      {  w, 0.0f, -l },
-      { -w, 0.0f,  l },
-      {  w, 0.0f,  l },
-   };
+      v3f   pos;
+      float radius;
+
+      int   apply_angular;
+      u32   colour;
 
-   int wheel_states[] =
+      enum  board_collider_state
+      {
+         k_collider_state_default,
+         k_collider_state_disabled,
+         k_collider_state_colliding
+      }
+      state;
+   }
+   wheels[] =
    {
-      1, 1, 1, 1
+      { 
+         { 0.0f, 0.0f,    -k_board_length }, 
+         .radius = 0.07f, 
+         .apply_angular = 1,
+         .colour = VG__RED
+      },
+      { 
+         { 0.0f, 0.0f,     k_board_length }, 
+         .radius = 0.07f,
+         .apply_angular = 1,
+         .colour = VG__GREEN
+      },
+      { 
+         { 0.0f, 0.2f,    -k_board_length - k_board_end_radius }, 
+         .radius = k_board_end_radius,
+         .apply_angular = 0,
+         .colour = VG__YELOW
+      },
+      {  
+         { 0.0f, 0.2f,     k_board_length + k_board_end_radius },
+         .radius = k_board_end_radius,
+         .apply_angular = 0,
+         .colour = VG__YELOW
+      },
    };
 
-   if( skate_grind_scansq( player, (v3f){ 0.0f, 0.0f, -l } ) )
+   const int k_wheel_count = 2;
+#if 0
+   if( skate_grind_scansq( player, (v3f){ 0.0f, 0.0f, -k_board_length } ) )
    {
+#if 0
       wheel_states[0] = 0;
       wheel_states[1] = 0;
+#endif
    }
 
-   if( skate_grind_scansq( player, (v3f){ 0.0f, 0.0f,  l } ) )
+   if( skate_grind_scansq( player, (v3f){ 0.0f, 0.0f,  k_board_length } ) )
    {
+#if 0
       wheel_states[2] = 0;
       wheel_states[3] = 0;
+#endif
    }
-
-   rb_sphere collider;
-   collider.radius = 0.07f;
+#endif
 
    s->substep = k_rb_delta;
+   s->substep_delta = s->substep;
+   int substep_count = 0;
+
 
-   for( int i=0; i<4; i++ )
+
+
+
+   /* 
+    * Phase 2: Truck alignment (spring/dampener model)
+    *          it uses the first two colliders as truck positions
+    * --------------------------------------------------------------------------
+    */
+
+   v3f surface_picture;
+   v3_zero( surface_picture );
+   
+   for( int i=0; i<2; i++ )
    {
-      m4x3f mtx;
-      m3x3_copy( player->rb.to_world, mtx );
-      m4x3_mulv( player->rb.to_world, wheel_positions[i], mtx[3] );
-      debug_sphere( mtx, collider.radius, wheel_states[i]? VG__WHITE:
-                                                           VG__BLACK );
+      if( wheels[i].state == k_collider_state_disabled )
+         continue;
+
+      v3f truck, left, right;
+      m4x3_mulv( player->rb.to_world, wheels[i].pos, truck );
+      v3_muladds( truck, player->rb.to_world[0], -k_board_width, left  );
+      v3_muladds( truck, player->rb.to_world[0],  k_board_width, right );
+      
+      vg_line( left, right, wheels[i].colour );
+
+      v3_muladds( left,  player->rb.to_world[1], 0.1f, left  );
+      v3_muladds( right, player->rb.to_world[1], 0.1f, right );
+
+      float k_max_truck_flex = VG_PIf * 0.25f;
+      
+      ray_hit ray_l, ray_r;
+      ray_l.dist = 0.2f;
+      ray_r.dist = 0.2f;
+
+      v3f dir;
+      v3_muls( player->rb.to_world[1], -1.0f, dir );
+
+      int res_l = ray_world( left, dir, &ray_l ),
+          res_r = ray_world( right, dir, &ray_r );
+
+      /* ignore bad normals */
+      if( res_l )
+      {
+         if( v3_dot( ray_l.normal, player->rb.to_world[1] ) < 0.7071f )
+            res_l = 0;
+         else
+            v3_add( ray_l.normal, surface_picture, surface_picture );
+      }
+
+      if( res_r )
+      {
+         if( v3_dot( ray_r.normal, player->rb.to_world[1] ) < 0.7071f )
+            res_r = 0;
+         else
+            v3_add( ray_r.normal, surface_picture, surface_picture );
+      }
+
+      v3f v0;
+      v3f midpoint;
+      v3_muladds( truck, player->rb.to_world[1], -wheels[i].radius, midpoint );
+
+      if( res_l || res_r )
+      {
+         v3f p0, p1;
+         v3_copy( midpoint, p0 );
+         v3_copy( midpoint, p1 );
+
+         if( res_l ) v3_copy( ray_l.pos, p0 );
+         if( res_r ) v3_copy( ray_r.pos, p1 );
+
+         v3_sub( p1, p0, v0 );
+         v3_normalize( v0 );
+      }
+      else
+      {
+         /* fallback: use the closes point to the trucks */
+         v3f closest;
+         int idx = bh_closest_point( world.geo_bh, midpoint, closest, 0.1f );
+
+         if( idx != -1 )
+         {
+            u32 *tri = &world.scene_geo->arrindices[ idx * 3 ];
+            v3f verts[3];
+
+            for( int j=0; j<3; j++ )
+               v3_copy( world.scene_geo->arrvertices[ tri[j] ].co, verts[j] );
+
+            v3f vert0, vert1, n;
+            v3_sub( verts[1], verts[0], vert0 );
+            v3_sub( verts[2], verts[0], vert1 );
+            v3_cross( vert0, vert1, n );
+            v3_normalize( n );
+
+            if( v3_dot( n, player->rb.to_world[1] ) < 0.3f )
+               continue;
+
+            v3_cross( n, player->rb.to_world[2], v0 );
+            v3_muladds( v0, player->rb.to_world[2],
+                        -v3_dot( player->rb.to_world[2], v0 ), v0 );
+            v3_normalize( v0 );
+         }
+         else
+            continue;
+      }
+
+      v3_muladds( truck, v0,  k_board_width, right );
+      v3_muladds( truck, v0, -k_board_width, left );
+
+      vg_line( left, right, VG__WHITE );
+
+      rb_effect_spring_target_vector( &player->rb, player->rb.to_world[0], v0,
+                                       k_board_spring, k_board_dampener,
+                                       s->substep_delta );
+   }
+
+   /* 
+    * Phase 2a: Manual alignment (spring/dampener model)
+    * --------------------------------------------------------------------------
+    */
+
+   v3f weight, world_cog;
+   v3_zero( weight );
+
+   int reverse_dir = v3_dot( player->rb.to_world[2], player->rb.v ) < 0.0f?1:-1;
+
+   if( s->state.manual_direction == 0 )
+   {
+      if( (player->input_js1v->axis.value > 0.7f) && 
+          (s->state.activity == k_skate_activity_ground) &&
+          (s->state.jump_charge <= 0.01f) )
+         s->state.manual_direction = reverse_dir;
+   }
+   else
+   {
+      if( player->input_js1v->axis.value < 0.1f )
+      {
+         s->state.manual_direction = 0;
+      }
+      else
+      {
+         if( reverse_dir != s->state.manual_direction )
+         {
+            player__dead_transition( player );
+            return;
+         }
+      }
+   }
+
+   if( s->state.manual_direction )
+   {
+      float amt = vg_minf( player->input_js1v->axis.value * 8.0f, 1.0f );
+      weight[2] = k_board_length * amt * (float)s->state.manual_direction;
+   }
+
+   if( v3_length2( surface_picture ) > 0.001f )
+   {
+      v3_normalize( surface_picture );
+
+      v3f target;
+      v3_copy( surface_picture, target );
+
+      target[1] += 2.0f * surface_picture[1];
+      v3_normalize( target );
+
+      v3_lerp( s->state.up_dir, target,
+               8.0f * s->substep_delta, s->state.up_dir );
    }
+   else
+   {
+      v3_lerp( s->state.up_dir, player->rb.to_world[1],
+               8.0f * s->substep_delta, s->state.up_dir );
+   }
+
+
+   /* TODO: Fall back on land normal */
+   /* TODO: Lerp weight distribution */
+   /* TODO: Can start manual only if not charge jump */
+   if( v3_length2( surface_picture ) > 0.001f && 
+       v3_length2( weight ) > 0.001f &&
+       s->state.manual_direction )
+   {
+      v3f plane_z;
+
+      m3x3_mulv( player->rb.to_world, weight, plane_z );
+      v3_negate( plane_z, plane_z );
+
+      v3_muladds( plane_z, surface_picture,
+                  -v3_dot( plane_z, surface_picture ), plane_z );
+      v3_normalize( plane_z );
+
+      v3_muladds( plane_z, surface_picture, 0.3f, plane_z );
+      v3_normalize( plane_z );
+
+      v3f p1;
+      v3_muladds( player->rb.co, plane_z, 1.5f, p1 );
+      vg_line( player->rb.co, p1, VG__GREEN );
+
+      v3f refdir;
+      v3_muls( player->rb.to_world[2], -(float)s->state.manual_direction,
+               refdir );
+
+      rb_effect_spring_target_vector( &player->rb, refdir, plane_z,
+                                       k_manul_spring, k_manul_dampener,
+                                       s->substep_delta );
+   }
+
+
+
+
+
+
+
 
 
 begin_collision:;
 
-#ifdef SKATE_CCD
+   /*
+    * Phase 0: Continous collision detection
+    * --------------------------------------------------------------------------
+    */
+
+   v3f head_wp0, head_wp1, start_co;
+   m4x3_mulv( player->rb.to_world, s->state.head_position, head_wp0 );
+   v3_copy( player->rb.co, start_co );
+
+   for( int i=0; i<k_wheel_count; i++ )
+      wheels[i].state = k_collider_state_default;
 
    /* calculate transform one step into future */
    v3f future_co;
@@ -1467,31 +1773,34 @@ begin_collision:;
    }
 
    /* calculate the minimum time we can move */
-   float max_time = s->substep,
-         cast_radius = collider.radius - 0.05f;
+   float max_time = s->substep;
 
-   for( int i=0; i<4; i++ )
+   for( int i=0; i<k_wheel_count; i++ )
    {
-      if( !wheel_states[i] )
+      if( wheels[i].state == k_collider_state_disabled )
          continue;
 
       v3f current, future;
-      q_mulv( future_q, wheel_positions[i], future );
+      q_mulv( future_q, wheels[i].pos, future );
       v3_add( future, future_co, future );
 
-      q_mulv( player->rb.q, wheel_positions[i], current );
+      q_mulv( player->rb.q, wheels[i].pos, current );
       v3_add( current, player->rb.co, current );
       
-      float t;    /* TODO: ignore lightly grazing normals? */
+      float t;
       v3f n;
+
+      float cast_radius = wheels[i].radius - k_penetration_slop * 1.2f;
       if( spherecast_world( current, future, cast_radius, &t, n ) != -1)
-      {
          max_time = vg_minf( max_time, t * s->substep );
-      }
    }
 
    /* clamp to a fraction of delta, to prevent locking */
-   max_time = vg_minf( vg_maxf( max_time, k_rb_delta*0.025f ), s->substep );
+   float rate_lock = substep_count;
+   rate_lock *= k_rb_delta * 0.1f;
+   rate_lock *= rate_lock;
+
+   max_time = vg_maxf( max_time, rate_lock );
    s->substep_delta = max_time;
 
    /* integrate */
@@ -1513,103 +1822,279 @@ begin_collision:;
    v3f gravity = { 0.0f, -9.6f, 0.0f };
    v3_muladds( player->rb.v, gravity, s->substep_delta, player->rb.v );
 
-#else
-   
-   s->substep_delta = k_rb_delta;
+   s->substep -= s->substep_delta;
+
+
+   rb_ct manifold[128];
+   int manifold_len   = 0;
+
+   /*
+    * Phase -1: head detection
+    * --------------------------------------------------------------------------
+    */
+   m4x3_mulv( player->rb.to_world, s->state.head_position, head_wp1 );
+   vg_line( head_wp0, head_wp1, VG__RED );
 
+   float t;
+   v3f n;
+   if( (v3_dist2( head_wp0, head_wp1 ) > 0.001f) &&
+       (spherecast_world( head_wp0, head_wp1, 0.2f, &t, n ) != -1) )
+   {
+      v3_lerp( start_co, player->rb.co, t, player->rb.co );
+      rb_update_transform( &player->rb );
+
+#if 0
+      player__dead_transition( player );
 #endif
+      return;
+   }
 
-   s->substep -= s->substep_delta;
+   /* 
+    * Phase 2-1+0.5: Grind collision
+    * --------------------------------------------------------------------------
+    */
+
+   for( int i=0; i<1; i++ )
+   {
 
+      /* 
+       * Grind collision detection 
+       * ------------------------------------------------
+       */
+      v3f grind_co, grind_n, grind_dir;
+      if( skate_grind_scansq( player, player->rb.co, 
+                              grind_co, grind_dir, grind_n ) )
+      {
+#if 0
+         rb_ct *ct = &manifold[ manifold_len ++ ];
 
-   /* create manifold(s) */
-   rb_ct manifold[128];
+         v3_copy( truck, ct->co );
+         v3_copy( grind_n, ct->n );
+         ct->p = vg_maxf( 0.0f, ct->co[1] - truck[1] );
+#endif
+
+         v3f target_dir;
+         v3_cross( grind_dir, (v3f){0.0f,1.0f,0.0f}, target_dir );
+         target_dir[1] = 0.0f;
+
+         if( v3_length2( target_dir ) <= 0.001f )
+            continue;
+
+         if( fabsf(v3_dot( player->rb.v, grind_dir )) < 0.7071f )
+            continue;
+
+         v3_copy( grind_co, player->rb.co );
+
+         q_axis_angle( player->rb.q, (v3f){0.0f,1.0f,0.0f}, 
+                       -atan2f( target_dir[2], target_dir[0] ) );
+
+         wheels[0].state = k_collider_state_disabled;
+         wheels[1].state = k_collider_state_disabled;
+         v3_muls( grind_dir, v3_dot(player->rb.v,grind_dir), player->rb.v );
+         v3_zero( player->rb.w );
+
+         rb_update_transform( &player->rb );
+
+
+#if 0
+         v3f displacement, dir;
+         v3_sub( truck, grind_co, displacement );
+         v3_copy( displacement, dir );
+         v3_normalize( dir );
+
+         v3f rv, raW;
+         q_mulv( player->rb.q, wheels[i].pos, raW );
+
+         v3_cross( player->rb.w, raW, rv );
+         v3_add( player->rb.v, rv, rv );
 
-   int manifold_len   = 0,
-       manifold_front = 0,
-       manifold_back  = 0,
-       manifold_interface = 0;
+         v3_muladds( rv, player->rb.to_world[2],
+                         -v3_dot( rv, player->rb.to_world[2] ), rv );
 
-   rb_ct *cmanifold = manifold;
+         v3f Fd, Fs, F;
+         v3_muls( displacement, -k_grind_spring, Fs );
+         v3_muls( rv, -k_grind_dampener, Fd );
 
-   for( int i=0; i<4; i++ )
+         v3_add( Fd, Fs, F );
+         v3_muls( F, s->substep_delta, F );
+         
+         v3_add( player->rb.v, F, player->rb.v );
+         v3f wa;
+         v3_cross( raW, F, wa );
+         v3_add( player->rb.w, wa, player->rb.w );
+
+         rb_effect_spring_target_vector( &player->rb, player->rb.to_world[1], 
+                                          grind_n,
+                                          k_board_spring, k_board_dampener,
+                                          s->substep_delta );
+
+         v3f adj;
+         v3_cross( grind_dir, (v3f){0.0f,1.0f,0.0f}, adj );
+         rb_effect_spring_target_vector( &player->rb, player->rb.to_world[2],
+                                          adj,
+                                          k_grind_spring, k_grind_dampener,
+                                          s->substep_delta );
+#endif
+
+         s->state.activity = k_skate_activity_grind;
+      }
+      else
+         s->state.activity = k_skate_activity_ground;
+   }
+
+
+   /*
+    * Phase 1: Regular collision detection
+    *          TODO: Me might want to automatically add contacts from CCD, 
+    *                since at high angular velocities, theres a small change
+    *                that discreet detection will miss.
+    * --------------------------------------------------------------------------
+    */
+
+   for( int i=0; i<k_wheel_count; i++ )
    {
-      if( !wheel_states[i] )
+      if( wheels[i].state == k_collider_state_disabled )
          continue;
 
       m4x3f mtx;
       m3x3_identity( mtx );
-
-      m4x3_mulv( player->rb.to_world, wheel_positions[i], mtx[3] );
+      m4x3_mulv( player->rb.to_world, wheels[i].pos, mtx[3] );
       
-      int l = skate_collide_smooth( player, mtx, &collider, cmanifold );
+      rb_sphere collider = { .radius = wheels[i].radius };
 
-      cmanifold += l;
-      manifold_len += l;
-      manifold_interface += l;
+      rb_ct *man = &manifold[ manifold_len ];
 
-      if( i<=1 )
-         manifold_front ++;
-      else
-         manifold_back ++;
+      int l = skate_collide_smooth( player, mtx, &collider, man );
+      if( l )
+         wheels[i].state = k_collider_state_colliding;
+
+      /* for non-angular contacts we just want Y. contact positions are
+       * snapped to the local xz plane */
+      if( !wheels[i].apply_angular )
+      {
+         for( int j=0; j<l; j++ )
+         {
+            v3f ra;
+            v3_sub( man[j].co, player->rb.co, ra );
+
+            float dy = v3_dot( player->rb.to_world[1], ra );
+            v3_muladds( man[j].co, player->rb.to_world[1], -dy, man[j].co );
+         }
+      }
+
+      manifold_len += l;
    }
 
-   /* try to slap both wheels onto the ground when landing to prevent mega 
-    * angular velocities being added */
-   if( (s->state.activity == k_skate_activity_air) && 
-       (manifold_front != manifold_back ) )
+   /* 
+    * Phase 3: Dynamics
+    * --------------------------------------------------------------------------
+    */
+
+   for( int i=0; i<manifold_len; i ++ )
    {
-      v3f trace_from, trace_dir;
-      v3_muls( player->rb.to_world[1], -1.0f, trace_dir );
+      rb_prepare_contact( &manifold[i], s->substep_delta );
+      rb_debug_contact( &manifold[i] );
+   }
 
-      if( manifold_front )
-         v3_copy( (v3f){0.0f,0.0f, k_board_length}, trace_from );
-      else
-         v3_copy( (v3f){0.0f,0.0f,-k_board_length}, trace_from );
-      m4x3_mulv( player->rb.to_world, trace_from, trace_from );
+   /* yes, we are currently rebuilding mass matrices every frame. too bad! */
+   v3f extent = { k_board_width, 0.1f, k_board_length };
+   float ex2 = k_board_interia*extent[0]*extent[0],
+         ey2 = k_board_interia*extent[1]*extent[1],
+         ez2 = k_board_interia*extent[2]*extent[2];
 
-      ray_hit ray;
-      ray.dist = 0.6f;
+   float mass = 2.0f * (extent[0]*extent[1]*extent[2]);
+   float inv_mass = 1.0f/mass;
 
-      if( ray_world( trace_from, trace_dir, &ray ) )
+   v3f I;
+   I[0] = ((1.0f/12.0f) * mass * (ey2+ez2));
+   I[1] = ((1.0f/12.0f) * mass * (ex2+ez2));
+   I[2] = ((1.0f/12.0f) * mass * (ex2+ey2));
+
+   m3x3f iI;
+   m3x3_identity( iI );
+   iI[0][0] = I[0];
+   iI[1][1] = I[1];
+   iI[2][2] = I[2];
+   m3x3_inv( iI, iI );
+
+   m3x3f iIw;
+   m3x3_mul( iI, player->rb.to_local, iIw );
+   m3x3_mul( player->rb.to_world, iIw, iIw );
+
+   m4x3_mulv( player->rb.to_world, weight, world_cog );
+   vg_line_pt3( world_cog, 0.1f, VG__BLACK );
+
+   for( int j=0; j<10; j++ )
+   {
+      for( int i=0; i<manifold_len; i++ )
       {
-         rb_ct *ct = cmanifold;
+         /* 
+          * regular dance; calculate velocity & total mass, apply impulse.
+          */
+
+         struct contact *ct = &manifold[i];
+         
+         v3f rv, delta;
+         v3_sub( ct->co, world_cog, delta ); 
+         v3_cross( player->rb.w, delta, rv );
+         v3_add( player->rb.v, rv, rv );
+
+         v3f raCn;
+         v3_cross( delta, ct->n, raCn );
+
+         v3f raCnI, rbCnI;
+         m3x3_mulv( iIw, raCn, raCnI );
+
+         float normal_mass = 1.0f / (inv_mass + v3_dot(raCn,raCnI)),
+               vn = v3_dot( rv, ct->n ),
+               lambda = normal_mass * ( -vn );
+
+         /* FIXME! */
+         v3_muladds( player->rb.co, ct->n, ct->bias*0.02f, player->rb.co );
 
-         v3_copy( ray.pos, ct->co );
-         v3_copy( ray.normal, ct->n );
-         ct->p = 0.0f;
+         float temp = ct->norm_impulse;
+         ct->norm_impulse = vg_maxf( temp + lambda, 0.0f );
+         lambda = ct->norm_impulse - temp;
+
+         v3f impulse;
+         v3_muls( ct->n, lambda, impulse );
+
+         v3_muladds( player->rb.v, impulse, inv_mass, player->rb.v );
+         v3_cross( delta, impulse, impulse );
+         m3x3_mulv( iIw, impulse, impulse );
+         v3_add( impulse, player->rb.w, player->rb.w );
 
-         manifold_len ++;
-         manifold_interface ++;
+         v3_cross( player->rb.w, delta, rv );
+         v3_add( player->rb.v, rv, rv );
+         vn = v3_dot( rv, ct->n );
       }
    }
 
-   int grind_len = skate_grind_collide( player, cmanifold );
-   manifold_len += grind_len;
+   substep_count ++;
 
-   for( int i=0; i<manifold_len; i ++ )
+   if( s->substep >= 0.0001f )
+      goto begin_collision;      /* again! */
+
+   /* 
+    * End of collision and dynamics routine
+    * --------------------------------------------------------------------------
+    */
+
+   for( int i=0; i<k_wheel_count; i++ )
    {
-#ifdef SKATE_CCD
-      rb_ct *ct = &manifold[i];
-      ct->bias = -0.2f * 
-                  (s->substep_delta * 3600.0f)
-                  * vg_minf( 0.0f, -ct->p+k_penetration_slop );
-      rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
-      ct->norm_impulse = 0.0f;
-      ct->tangent_impulse[0] = 0.0f;
-      ct->tangent_impulse[1] = 0.0f;
-#else
-      rb_prepare_contact( &manifold[i] );
-#endif
+      m4x3f mtx;
+      m3x3_copy( player->rb.to_world, mtx );
+      m4x3_mulv( player->rb.to_world, wheels[i].pos, mtx[3] );
+      debug_sphere( mtx, wheels[i].radius,
+                   (u32[]){ VG__WHITE, VG__BLACK, 
+                            wheels[i].colour }[ wheels[i].state ]);
    }
 
-   skate_collision_response( player, manifold, manifold_len );
-
-   if( s->substep >= 0.0001f )
-      goto begin_collision;
+#if 0
+   skate_apply_grind_model( player, &manifold[manifold_len], grind_len );
+#endif
 
-   skate_apply_grind_model( player, &manifold[manifold_interface], grind_len );
-   skate_apply_interface_model( player, manifold, manifold_interface );
+   skate_apply_interface_model( player, manifold, manifold_len );
    
    skate_apply_pump_model( player );
    skate_apply_cog_model( player );
@@ -1638,6 +2123,8 @@ begin_collision:;
       m4x3_mulv( gate->transport, s->state.cog,   s->state.cog );
       m3x3_mulv( gate->transport, s->state.cog_v, s->state.cog_v );
       m3x3_mulv( gate->transport, s->state.throw_v, s->state.throw_v );
+      m3x3_mulv( gate->transport, s->state.head_position,
+                                  s->state.head_position );
 
       v4f transport_rotation;
       m3x3_q( gate->transport, transport_rotation );
@@ -1669,9 +2156,11 @@ VG_STATIC void player__skate_im_gui( player_instance *player )
                                              "k_skate_activity_ground",
                                              "k_skate_activity_grind }" }
                                              [s->state.activity] );
+#if 0
    player__debugtext( 1, "steer_s: %5.2f %5.2f [%.2f %.2f]",
                         s->state.steerx_s, s->state.steery_s,
                         k_steer_ground, k_steer_air );
+#endif
    player__debugtext( 1, "flip: %.4f %.4f", s->state.flip_rate, 
                                              s->state.flip_time );
    player__debugtext( 1, "trickv: %.2f %.2f %.2f", 
@@ -1698,8 +2187,18 @@ VG_STATIC void player__skate_animate( player_instance *player,
    v3f offset;
    v3_zero( offset );
 
-   m4x3_mulv( player->rb.to_local, s->state.cog, offset );
-   v3_muls( offset, -4.0f, offset );
+   v3f cog_local, cog_ideal;
+   m4x3_mulv( player->rb.to_local, s->state.cog, cog_local );
+
+   v3_copy( s->state.up_dir, cog_ideal );
+   v3_normalize( cog_ideal );
+   m3x3_mulv( player->rb.to_local, cog_ideal, cog_ideal );
+
+   v3_sub( cog_ideal, cog_local, offset );
+
+
+   v3_muls( offset, 4.0f, offset );
+   offset[1] *= -1.0f;
 
    float curspeed  = v3_length( player->rb.v ),
          kickspeed = vg_clampf( curspeed*(1.0f/40.0f), 0.0f, 1.0f ),
@@ -1848,6 +2347,52 @@ VG_STATIC void player__skate_animate( player_instance *player,
          dest->pose[apply_to[i]-1].co[2] += offset[2]*add_grab_mod;
       }
 
+
+
+
+      /* angle correction */
+      if( v3_length2( s->state.up_dir ) > 0.001f )
+      {
+         v3f ndir;
+         m3x3_mulv( player->rb.to_local, s->state.up_dir, ndir );
+         v3_normalize( ndir );
+
+         v3f up = { 0.0f, 1.0f, 0.0f };
+
+         float a = v3_dot( ndir, up );
+         a = acosf( vg_clampf( a, -1.0f, 1.0f ) );
+
+         v3f axis;
+         v4f q;
+         
+         v3_cross( up, ndir, axis );
+         q_axis_angle( q, axis, a );
+
+         mdl_keyframe *kf_hip = &dest->pose[av->id_hip-1];
+         
+         for( int i=0; i<vg_list_size(apply_to); i ++ )
+         {
+            mdl_keyframe *kf = &dest->pose[apply_to[i]-1];
+
+            v3f v0;
+            v3_sub( kf->co, kf_hip->co, v0 );
+            q_mulv( q, v0, v0 );
+            v3_add( v0, kf_hip->co, kf->co );
+
+            q_mul( q, kf->q, kf->q );
+            q_normalize( kf->q );
+         }
+
+         v3f p1, p2;
+         m4x3_mulv( player->rb.to_world, up, p1 );
+         m4x3_mulv( player->rb.to_world, ndir, p2 );
+
+         vg_line( player->rb.co, p1, VG__PINK );
+         vg_line( player->rb.co, p2, VG__CYAN );
+      }
+
+
+
       mdl_keyframe *kf_board  = &dest->pose[av->id_board-1],
                    *kf_foot_l = &dest->pose[av->id_ik_foot_l-1],
                    *kf_foot_r = &dest->pose[av->id_ik_foot_r-1];
@@ -1915,11 +2460,12 @@ VG_STATIC void player__skate_animate( player_instance *player,
 
    /* transform */
    rb_extrapolate( &player->rb, dest->root_co, dest->root_q );
-   v3_muladds( dest->root_co, player->rb.to_world[1], -0.28f, dest->root_co );
+   v3_muladds( dest->root_co, player->rb.to_world[1], -0.1f, dest->root_co );
 
+   float substep = vg_clampf( vg.accumulator / VG_TIMESTEP_FIXED, 0.0f, 1.0f );
+#if 0
    v4f qresy, qresx, qresidual;
    m3x3f mtx_residual;
-   float substep = vg_clampf( vg.accumulator / VG_TIMESTEP_FIXED, 0.0f, 1.0f );
    q_axis_angle( qresy, player->rb.to_world[1], s->state.steery_s*substep );
    q_axis_angle( qresx, player->rb.to_world[0], s->state.steerx_s*substep );
 
@@ -1927,6 +2473,7 @@ VG_STATIC void player__skate_animate( player_instance *player,
    q_normalize( qresidual );
    q_mul( dest->root_q, qresidual, dest->root_q );
    q_normalize( dest->root_q );
+#endif
 
    v4f qflip;
    if( (s->state.activity == k_skate_activity_air) &&
@@ -1958,6 +2505,11 @@ VG_STATIC void player__skate_post_animate( player_instance *player )
    struct player_avatar *av = player->playeravatar;
 
    player->cam_velocity_influence = 1.0f;
+
+   v3f head = { 0.0f, 1.8f, 0.0f }; /* FIXME: Viewpoint entity */
+   m4x3_mulv( av->sk.final_mtx[ av->id_head ], head, s->state.head_position );
+   m4x3_mulv( player->rb.to_local, s->state.head_position, 
+                                   s->state.head_position );
 }
 
 VG_STATIC void player__skate_reset_animator( player_instance *player )
@@ -1984,10 +2536,12 @@ VG_STATIC void player__skate_clear_mechanics( player_instance *player )
    s->state.jump_charge    = 0.0f;
    s->state.lift_frames    = 0;
    s->state.flip_rate      = 0.0f;
+#if 0
    s->state.steery         = 0.0f;
    s->state.steerx         = 0.0f;
    s->state.steery_s       = 0.0f;
    s->state.steerx_s       = 0.0f;
+#endif
    s->state.reverse        = 0.0f;
    s->state.slip           = 0.0f;
    v3_copy( player->rb.co, s->state.prev_pos );
@@ -2013,6 +2567,9 @@ VG_STATIC void player__skate_reset( player_instance *player,
 
    player__skate_clear_mechanics( player );
    player__skate_reset_animator( player );
+
+   v3_zero( s->state.head_position );
+   s->state.head_position[1] = 1.8f;
 }
 
 #endif /* PLAYER_SKATE_C */