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

diff --git a/player_skate.c b/player_skate.c
index eb4e27e..7088cf1 100644
--- a/player_skate.c
+++ b/player_skate.c
@@ -149,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 );
 
@@ -253,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;
    
@@ -302,8 +302,13 @@ VG_STATIC int skate_grind_scansq( player_instance *player, v3f ra )
 
          /* 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 ++;
       }
    }
@@ -311,9 +316,14 @@ VG_STATIC int skate_grind_scansq( player_instance *player, v3f ra )
    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 );
@@ -323,38 +333,9 @@ 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 0
-   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_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 */
-   }
-#endif
+   v3_copy( average_position, result_co );
+   v3_copy( average_normal, result_n );
+   v3_copy( average_direction, result_dir );
 
    return passed_samples;
 }
@@ -1230,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 );
 
@@ -1475,38 +1459,53 @@ 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;
-
-#if 0
-   v3f wheel_positions[] = 
+   struct board_collider
    {
-      { -w, 0.0f, -l },
-      {  w, 0.0f, -l },
-      { -w, 0.0f,  l },
-      {  w, 0.0f,  l },
-   };
-#else
-   v3f wheel_positions[] = 
-   {
-      { 0.0f, 0.0f, -l },
-      { 0.0f, 0.0f,  l },
-   };
-#endif
+      v3f   pos;
+      float radius;
 
-   u32 wheel_colours[] =
-   {
-      VG__RED, VG__GREEN, VG__BLUE, VG__YELOW
-   };
+      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
+      },
    };
 
-   const int wheel_count = 2;
-
-   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;
@@ -1514,37 +1513,246 @@ VG_STATIC void player__skate_update( player_instance *player )
 #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;
+
 
 
-   int substep_count = 0;
 
-begin_collision:;
 
+   /* 
+    * Phase 2: Truck alignment (spring/dampener model)
+    *          it uses the first two colliders as truck positions
+    * --------------------------------------------------------------------------
+    */
 
-   for( int i=0; i<wheel_count; i++ )
+   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, 
-                   (u32[]){ VG__BLACK, VG__WHITE, 
-                            wheel_colours[i] }[ wheel_states[i] ]);
+      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:;
+
+   /*
+    * 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 );
 
-#ifdef SKATE_CCD
+   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;
@@ -1565,27 +1773,26 @@ begin_collision:;
    }
 
    /* calculate the minimum time we can move */
-   float max_time = s->substep,
-         cast_radius = collider.radius - k_penetration_slop*1.2f;
+   float max_time = s->substep;
 
-   for( int i=0; i<wheel_count; 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 */
@@ -1615,123 +1822,182 @@ 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;
-
-#endif
-
    s->substep -= s->substep_delta;
 
 
-   /* create manifold(s) */
    rb_ct manifold[128];
+   int manifold_len   = 0;
 
-   int manifold_len   = 0,
-       manifold_front = 0,
-       manifold_back  = 0,
-       manifold_interface = 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 );
 
-   rb_ct *cmanifold = manifold;
+   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 );
 
-   for( int i=0; i<wheel_count; i++ )
+#if 0
+      player__dead_transition( player );
+#endif
+      return;
+   }
+
+   /* 
+    * Phase 2-1+0.5: Grind collision
+    * --------------------------------------------------------------------------
+    */
+
+   for( int i=0; i<1; i++ )
    {
-      if( !wheel_states[i] )
-         continue;
 
-      m4x3f mtx;
-      m3x3_identity( mtx );
+      /* 
+       * 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 ++ ];
 
-      m4x3_mulv( player->rb.to_world, wheel_positions[i], mtx[3] );
-      
-      int l = skate_collide_smooth( player, mtx, &collider, cmanifold );
+         v3_copy( truck, ct->co );
+         v3_copy( grind_n, ct->n );
+         ct->p = vg_maxf( 0.0f, ct->co[1] - truck[1] );
+#endif
 
-      if( l )
-         wheel_states[i] = 2;
+         v3f target_dir;
+         v3_cross( grind_dir, (v3f){0.0f,1.0f,0.0f}, target_dir );
+         target_dir[1] = 0.0f;
 
-      cmanifold += l;
-      manifold_len += l;
-      manifold_interface += l;
+         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( i<=1 )
-         manifold_front ++;
-      else
-         manifold_back ++;
-   }
 
 #if 0
-   /* 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 ) )
-   {
-      v3f trace_from, trace_dir;
-      v3_muls( player->rb.to_world[1], -1.0f, trace_dir );
+         v3f displacement, dir;
+         v3_sub( truck, grind_co, displacement );
+         v3_copy( displacement, dir );
+         v3_normalize( dir );
 
-      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 );
+         v3f rv, raW;
+         q_mulv( player->rb.q, wheels[i].pos, raW );
 
-      ray_hit ray;
-      ray.dist = 0.6f;
+         v3_cross( player->rb.w, raW, rv );
+         v3_add( player->rb.v, rv, rv );
 
-      if( ray_world( trace_from, trace_dir, &ray ) )
-      {
-         rb_ct *ct = cmanifold;
+         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( ray.pos, ct->co );
-         v3_copy( ray.normal, ct->n );
-         ct->p = 0.0f;
+         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
 
-         manifold_len ++;
-         manifold_interface ++;
+         s->state.activity = k_skate_activity_grind;
       }
+      else
+         s->state.activity = k_skate_activity_ground;
    }
 
-   int grind_len = skate_grind_collide( player, cmanifold );
-   manifold_len += grind_len;
-#endif
-
-   int grind_len = 0;
 
-   v3f surface_normal = {0.0f,0.0f,0.0f};
+   /*
+    * 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<manifold_len; i ++ )
+   for( int i=0; i<k_wheel_count; i++ )
    {
-      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;
+      if( wheels[i].state == k_collider_state_disabled )
+         continue;
 
-      v3_add( ct->n, surface_normal, surface_normal );
-   }
+      m4x3f mtx;
+      m3x3_identity( mtx );
+      m4x3_mulv( player->rb.to_world, wheels[i].pos, mtx[3] );
+      
+      rb_sphere collider = { .radius = wheels[i].radius };
 
-   if( manifold_len )
-   {
-      v3_muls( surface_normal, 1.0f/(float)manifold_len, surface_normal );
+      rb_ct *man = &manifold[ manifold_len ];
 
-      float a = v3_dot( player->rb.to_world[1], surface_normal );
+      int l = skate_collide_smooth( player, mtx, &collider, man );
+      if( l )
+         wheels[i].state = k_collider_state_colliding;
 
-      if( a <= 0.9999f )
+      /* for non-angular contacts we just want Y. contact positions are
+       * snapped to the local xz plane */
+      if( !wheels[i].apply_angular )
       {
-         v3f axis;
-         v3_cross( surface_normal, player->rb.to_world[1], axis );
-
-         float Fs = -a * k_board_spring,
-               Fd = -v3_dot( player->rb.w, axis ) * k_board_dampener;
+         for( int j=0; j<l; j++ )
+         {
+            v3f ra;
+            v3_sub( man[j].co, player->rb.co, ra );
 
-         v3_muladds( player->rb.w, axis, (Fs+Fd) * s->substep_delta,
-                     player->rb.w );
+            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;
+   }
+
+   /* 
+    * Phase 3: Dynamics
+    * --------------------------------------------------------------------------
+    */
+
+   for( int i=0; i<manifold_len; i ++ )
+   {
+      rb_prepare_contact( &manifold[i], s->substep_delta );
+      rb_debug_contact( &manifold[i] );
    }
 
-   v3f extent = { w, 0.1f, k_board_length };
+   /* 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];
@@ -1755,14 +2021,21 @@ begin_collision:;
    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++ )
       {
+         /* 
+          * regular dance; calculate velocity & total mass, apply impulse.
+          */
+
          struct contact *ct = &manifold[i];
          
          v3f rv, delta;
-         v3_sub( ct->co, player->rb.co, delta ); 
+         v3_sub( ct->co, world_cog, delta ); 
          v3_cross( player->rb.w, delta, rv );
          v3_add( player->rb.v, rv, rv );
 
@@ -1772,13 +2045,12 @@ begin_collision:;
          v3f raCnI, rbCnI;
          m3x3_mulv( iIw, raCn, raCnI );
 
-         float normal_mass = 1.0f / (inv_mass + v3_dot(raCn,raCnI));
-         float vn = v3_dot( rv, ct->n );
-
+         float normal_mass = 1.0f / (inv_mass + v3_dot(raCn,raCnI)),
+               vn = v3_dot( rv, ct->n ),
+               lambda = normal_mass * ( -vn );
 
-
-
-         float lambda = normal_mass * ( -vn + ct->bias );
+         /* FIXME! */
+         v3_muladds( player->rb.co, ct->n, ct->bias*0.02f, player->rb.co );
 
          float temp = ct->norm_impulse;
          ct->norm_impulse = vg_maxf( temp + lambda, 0.0f );
@@ -1787,15 +2059,6 @@ begin_collision:;
          v3f impulse;
          v3_muls( ct->n, lambda, impulse );
 
-#if 0
-         if( fabsf(v3_dot( impulse, player->rb.to_world[2] )) > 10.0f ||
-             fabsf(v3_dot( impulse, player->rb.to_world[1] )) > 50.0f )
-         {
-            player__dead_transition( player );
-            return;
-         }
-#endif
-
          v3_muladds( player->rb.v, impulse, inv_mass, player->rb.v );
          v3_cross( delta, impulse, impulse );
          m3x3_mulv( iIw, impulse, impulse );
@@ -1804,42 +2067,34 @@ begin_collision:;
          v3_cross( player->rb.w, delta, rv );
          v3_add( player->rb.v, rv, rv );
          vn = v3_dot( rv, ct->n );
-
       }
    }
 
-
-
-
-
-
-
    substep_count ++;
 
-
    if( s->substep >= 0.0001f )
-      goto begin_collision;
-
+      goto begin_collision;      /* again! */
 
+   /* 
+    * End of collision and dynamics routine
+    * --------------------------------------------------------------------------
+    */
 
-   for( int i=0; i<wheel_count; i++ )
+   for( int i=0; i<k_wheel_count; 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, 
-                   (u32[]){ VG__BLACK, VG__WHITE, 
-                            wheel_colours[i] }[ wheel_states[i] ]);
+      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 ]);
    }
 
+#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 );
@@ -1868,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 );
@@ -1930,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 ),
@@ -2080,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];
@@ -2192,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 )
@@ -2249,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 */