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

diff --git a/player_skate.c b/player_skate.c
index eb4e27e..a04a0dd 100644
--- a/player_skate.c
+++ b/player_skate.c
@@ -1475,38 +1475,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, k_board_end_radius,    -k_board_length - k_board_end_radius }, 
+         .radius = k_board_end_radius,
+         .apply_angular = 0,
+         .colour = VG__YELOW
+      },
+      {  
+         { 0.0f, k_board_end_radius,     k_board_length + k_board_end_radius },
+         .radius = k_board_end_radius,
+         .apply_angular = 0,
+         .colour = VG__YELOW
+      },
    };
 
-   const int wheel_count = 2;
+   const int k_wheel_count = 4;
 
-   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[0] = 0;
@@ -1514,7 +1529,7 @@ 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;
@@ -1522,29 +1537,22 @@ VG_STATIC void player__skate_update( player_instance *player )
 #endif
    }
 
-   rb_sphere collider;
-   collider.radius = 0.07f;
-
    s->substep = k_rb_delta;
-
-
    int substep_count = 0;
 
-begin_collision:;
+   v3f original_velocity;
+   v3_muladds( player->rb.v, (v3f){0.0f,-k_gravity,0.0f}, 
+               k_rb_delta, original_velocity );
 
+begin_collision:;
 
-   for( int i=0; i<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] ]);
-   }
-
+   /*
+    * Phase 0: Continous collision detection
+    * --------------------------------------------------------------------------
+    */
 
-#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 +1573,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 +1622,259 @@ 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;
 
+   /*
+    * 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.
+    * --------------------------------------------------------------------------
+    */
 
-   /* create manifold(s) */
    rb_ct manifold[128];
+         
+   int manifold_len   = 0;
 
-   int manifold_len   = 0,
-       manifold_front = 0,
-       manifold_back  = 0,
-       manifold_interface = 0;
-
-   rb_ct *cmanifold = manifold;
-
-   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;
 
       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 };
 
+      rb_ct *man = &manifold[ manifold_len ];
+
+      int l = skate_collide_smooth( player, mtx, &collider, man );
       if( l )
-         wheel_states[i] = 2;
+         wheels[i].state = k_collider_state_colliding;
 
-      cmanifold += l;
-      manifold_len += l;
-      manifold_interface += l;
+      /* 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 );
 
-      if( i<=1 )
-         manifold_front ++;
-      else
-         manifold_back ++;
+            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;
    }
 
-#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 ) )
+   /* 
+    * 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++ )
    {
-      v3f trace_from, trace_dir;
-      v3_muls( player->rb.to_world[1], -1.0f, trace_dir );
+      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 );
 
-      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 );
+      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 );
+      }
 
-      ray_hit ray;
-      ray.dist = 0.6f;
+      if( res_r )
+      {
+         if( v3_dot( ray_r.normal, player->rb.to_world[1] ) < 0.7071f )
+            res_r = 0;
+         else
+            v3_add( ray_l.normal, surface_picture, surface_picture );
+      }
+
+      v3f v0;
+      v3f midpoint;
+      v3_muladds( truck, player->rb.to_world[1], -wheels[i].radius, midpoint );
 
-      if( ray_world( trace_from, trace_dir, &ray ) )
+      if( res_l || res_r )
       {
-         rb_ct *ct = cmanifold;
+         v3f p0, p1;
+         v3_copy( midpoint, p0 );
+         v3_copy( midpoint, p1 );
 
-         v3_copy( ray.pos, ct->co );
-         v3_copy( ray.normal, ct->n );
-         ct->p = 0.0f;
+         if( res_l ) v3_copy( ray_l.pos, p0 );
+         if( res_r ) v3_copy( ray_r.pos, p1 );
 
-         manifold_len ++;
-         manifold_interface ++;
+         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 v0, v1, n;
+            v3_sub( verts[1], verts[0], v0 );
+            v3_sub( verts[2], verts[0], v1 );
+            v3_cross( v0, v1, n );
+            v3_normalize( n );
+
+            if( v3_dot( n, player->rb.to_world[1] ) < 0.7071f )
+               continue;
+
+            continue;
+         }
+         else
+            continue;
       }
+
+      float a = vg_clampf( v3_dot( v0, player->rb.to_world[0] ), -1.0f, 1.0f );
+      a = acosf( a );
+
+      v3_muladds( truck, v0,  k_board_width, right );
+      v3_muladds( truck, v0, -k_board_width, left );
+
+      vg_line( left, right, VG__WHITE );
+
+      v3f axis;
+      v3_cross( v0, player->rb.to_world[0], axis );
+
+      float Fs = -a * k_board_spring,
+            Fd = -v3_dot( player->rb.w, axis ) * k_board_dampener;
+
+      v3_muladds( player->rb.w, axis, (Fs+Fd) * s->substep_delta,
+                  player->rb.w );
    }
 
-   int grind_len = skate_grind_collide( player, cmanifold );
-   manifold_len += grind_len;
-#endif
+   /* 
+    * Phase 2a: Manual alignment (spring/dampener model)
+    * --------------------------------------------------------------------------
+    */
 
-   int grind_len = 0;
+   v3f weight, world_cog;
+   v3_zero( weight );
 
-   v3f surface_normal = {0.0f,0.0f,0.0f};
+   int reverse_dir = v3_dot( player->rb.to_world[2], player->rb.v ) < 0.0f?1:-1;
 
-   for( int i=0; i<manifold_len; i ++ )
+   if( s->state.manual_direction == 0 )
+   {
+      if( (player->input_js1v->axis.value > 0.7f) && 
+           s->state.activity == k_skate_activity_ground )
+         s->state.manual_direction = reverse_dir;
+   }
+   else
    {
-      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( 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;
+         }
+      }
+   }
 
-      v3_add( ct->n, surface_normal, surface_normal );
+   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( manifold_len )
+   m4x3_mulv( player->rb.to_world, weight, world_cog );
+   vg_line_pt3( world_cog, 0.1f, VG__BLACK );
+
+   /* TODO: Fall back on land normal */
+   /* TODO: Lerp weight distribution */
+
+   if( v3_length2( surface_picture ) > 0.001f && 
+       v3_length2( weight ) > 0.001f &&
+       s->state.manual_direction )
    {
-      v3_muls( surface_normal, 1.0f/(float)manifold_len, surface_normal );
+      v3_normalize( surface_picture );
+      v3f plane_z;
 
-      float a = v3_dot( player->rb.to_world[1], surface_normal );
+      m3x3_mulv( player->rb.to_world, weight, plane_z );
+      v3_negate( plane_z, plane_z );
 
-      if( a <= 0.9999f )
-      {
-         v3f axis;
-         v3_cross( surface_normal, player->rb.to_world[1], axis );
+      v3_muladds( plane_z, surface_picture,
+                  -v3_dot( plane_z, surface_picture ), plane_z );
+      v3_normalize( plane_z );
 
-         float Fs = -a * k_board_spring,
-               Fd = -v3_dot( player->rb.w, axis ) * k_board_dampener;
+      v3_muladds( plane_z, surface_picture, 0.3f, plane_z );
+      v3_normalize( plane_z );
 
-         v3_muladds( player->rb.w, axis, (Fs+Fd) * s->substep_delta,
-                     player->rb.w );
-      }
+      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 );
+
+      float a = v3_dot( plane_z, refdir );
+      a = acosf( vg_clampf( a, -1.0f, 1.0f ) );
+
+      v3f axis;
+      v3_cross( plane_z, refdir, axis );
+
+      float Fs = -a * k_manul_spring,
+            Fd = -v3_dot( player->rb.w, axis ) * k_manul_dampener;
+
+      v3_muladds( player->rb.w, axis, (Fs+Fd) * s->substep_delta,
+                  player->rb.w );
    }
 
-   v3f extent = { w, 0.1f, k_board_length };
+   /* 
+    * Phase 3: Dynamics
+    * --------------------------------------------------------------------------
+    */
+
+   for( int i=0; i<manifold_len; i ++ )
+      rb_prepare_contact( &manifold[i], s->substep_delta );
+
+   /* 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];
@@ -1759,10 +1902,14 @@ begin_collision:;
    {
       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 +1919,9 @@ 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 lambda = normal_mass * ( -vn + ct->bias );
+         float normal_mass = 1.0f / (inv_mass + v3_dot(raCn,raCnI)),
+               vn = v3_dot( rv, ct->n ),
+               lambda = normal_mass * ( -vn + ct->bias );
 
          float temp = ct->norm_impulse;
          ct->norm_impulse = vg_maxf( temp + lambda, 0.0f );
@@ -1787,15 +1930,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 +1938,41 @@ 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 ]);
    }
 
+   v3f velocity_change, p1;
+   v3_sub( player->rb.v, original_velocity, velocity_change );
 
+   v3_normalize( velocity_change );
+   v3_muladds( player->rb.co, velocity_change, 2.0f, p1 );
+   vg_line( player->rb.co, p1, VG__PINK );
 
+#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 );