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

diff --git a/player_skate.c b/player_skate.c
index be35667..8777763 100644
--- a/player_skate.c
+++ b/player_skate.c
@@ -35,8 +35,6 @@ VG_STATIC int skate_collide_smooth( player_instance *player,
                                     m4x3f mtx, rb_sphere *sphere,
                                     rb_ct *man )
 {
-   debug_sphere( mtx, sphere->radius, VG__BLACK );
-
    int len = 0;
    len = rb_sphere__scene( mtx, sphere, NULL, &world.rb_geo.inf.scene, man );
 
@@ -113,8 +111,8 @@ VG_STATIC int skate_grind_collide( player_instance *player, rb_ct *contact )
    v3f p0, p1, c0, c1;
    v3_muladds( player->rb.co, player->rb.to_world[2],  0.5f, p0 );
    v3_muladds( player->rb.co, player->rb.to_world[2], -0.5f, p1 );
-   v3_muladds( p0, player->rb.to_world[1], 0.125f-0.15f, p0 );
-   v3_muladds( p1, player->rb.to_world[1], 0.125f-0.15f, p1 );
+   v3_muladds( p0, player->rb.to_world[1], 0.08f, p0 );
+   v3_muladds( p1, player->rb.to_world[1], 0.08f, p1 );
 
    float const k_r = 0.25f;
    struct grind_edge *closest_edge = skate_collect_grind_edge( p0, p1, 
@@ -149,6 +147,196 @@ 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 )
+{
+   v3f pos;
+   m4x3_mulv( player->rb.to_world, ra, pos );
+
+   v4f plane;
+   v3_copy( player->rb.to_world[2], plane );
+   v3_normalize( plane );
+   plane[3] = v3_dot( plane, pos );
+
+   boxf box;
+   float r = 0.3f;
+   v3_add( pos, (v3f){ r, r, r }, box[1] );
+   v3_sub( pos, (v3f){ r, r, r }, box[0] );
+
+#if 0
+   vg_line_boxf( box, VG__BLUE );
+#endif
+
+   m4x3f mtx;
+   m3x3_copy( player->rb.to_world, mtx );
+   v3_copy( pos, mtx[3] );
+
+   debug_sphere( mtx, r, VG__CYAN );
+   
+   bh_iter it;
+   bh_iter_init( 0, &it );
+   int idx;
+
+   struct grind_sample
+   {
+      v2f co;
+      v2f normal;
+      v3f normal3;
+   }
+   samples[48];
+
+   int sample_count = 0;
+
+   v2f support_min,
+       support_max;
+
+   v3f support_axis;
+   v3_cross( plane, (v3f){0.0f,1.0f,0.0f}, support_axis );
+   v3_normalize( support_axis );
+   
+   while( bh_next( world.geo_bh, &it, box, &idx ) )
+   {
+      u32 *ptri = &world.scene_geo->arrindices[ idx*3 ];
+      v3f tri[3];
+
+      for( int j=0; j<3; j++ )
+         v3_copy( world.scene_geo->arrvertices[ptri[j]].co, tri[j] );
+
+      for( int j=0; j<3; j++ )
+      {
+         int i0 = j,
+             i1 = (j+1) % 3;
+         
+         struct grind_sample *sample = &samples[ sample_count ];
+         v3f co;
+
+         if( plane_segment( plane, tri[i0], tri[i1], co ) )
+         {
+            v3f d;
+            v3_sub( co, pos, d );
+            if( v3_length2( d ) > r*r )
+               continue;
+
+            v3f va, vb, normal;
+            v3_sub( tri[1], tri[0], va );
+            v3_sub( tri[2], tri[0], vb );
+            v3_cross( va, vb, normal );
+
+            sample->normal[0] = v3_dot( support_axis, normal );
+            sample->normal[1] = normal[1];
+            sample->co[0]     = v3_dot( support_axis, d );
+            sample->co[1]     = d[1];
+
+            v3_copy( normal, sample->normal3 ); /* normalize later
+                                                   if we want to us it */
+
+            v2_normalize( sample->normal );
+            sample_count ++;
+
+            if( sample_count == vg_list_size( samples ) )
+            {
+               break;
+            }
+         }
+      }
+   }
+
+   if( sample_count < 2 )
+      return 0;
+
+   v3f average_position,
+       average_direction;
+
+   v3_zero( average_position );
+   v3_zero( average_direction );
+
+   int passed_samples = 0;
+   
+   for( int i=0; i<sample_count-1; i++ )
+   {
+      struct grind_sample *si, *sj;
+
+      si = &samples[i];
+
+      for( int j=i+1; j<sample_count; j++ )
+      {
+         if( i == j )
+            continue;
+
+         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 */
+
+            v3f p0;
+            v3_muls( support_axis, sj->co[0], p0 );
+            p0[1] += sj->co[1];
+
+            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 );
+            passed_samples ++;
+         }
+      }
+   }
+
+   if( !passed_samples )
+      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_add( pos, average_position, average_position );
+   vg_line_pt3( average_position, 0.02f, VG__GREEN );
+
+   v3f p0, p1;
+   v3_muladds( average_position, average_direction,  0.35f, p0 );
+   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_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 */
+   }
+}
+
 /*
  *
  * Prediction system
@@ -615,6 +803,8 @@ VG_STATIC int skate_simulate_spring( player_instance *player,
 
 /* 
  * Handles connection between the player and the ground
+ *
+ * TODO: Must save original velocity to use here
  */
 VG_STATIC void skate_apply_interface_model( player_instance *player,
                                             rb_ct *manifold, int len )
@@ -701,6 +891,7 @@ VG_STATIC void skate_apply_interface_model( player_instance *player,
          float angle = v3_dot( player->rb.to_world[1], projected );
          v3_cross( player->rb.to_world[1], projected, axis );
 
+#if 0
          if( fabsf(angle) < 0.9999f )
          {
             v4f correction;
@@ -708,6 +899,7 @@ VG_STATIC void skate_apply_interface_model( player_instance *player,
                           acosf(angle)*4.0f*VG_TIMESTEP_FIXED );
             q_mul( correction, player->rb.q, player->rb.q );
          }
+#endif
       }
    }
 }
@@ -1061,10 +1253,12 @@ VG_STATIC void skate_collision_response( player_instance *player,
           */
          
          float wy = v3_dot( player->rb.to_world[1], impulse ) * 0.8f,
-               wx = v3_dot( player->rb.to_world[0], 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;
 
          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 );
       }
    }
 }
@@ -1074,9 +1268,11 @@ VG_STATIC void skate_integrate( player_instance *player )
    struct player_skate *s = &player->_skate;
 
    /* integrate rigidbody velocities */
+#ifndef SKATE_CCD
    v3f gravity = { 0.0f, -9.6f, 0.0f };
    v3_muladds( player->rb.v, gravity, k_rb_delta, player->rb.v );
    v3_muladds( player->rb.co, player->rb.v, k_rb_delta, player->rb.co );
+#endif
 
    float decay_rate = 0.5f*0.125f;
 
@@ -1088,6 +1284,7 @@ VG_STATIC void skate_integrate( player_instance *player )
 
    v3_lerp( player->rb.w, (v3f){0.0f,0.0f,0.0f}, decay_rate, player->rb.w );
 
+#ifndef SKATE_CCD
    if( v3_length2( player->rb.w ) > 0.0f )
    {
       v4f rotation;
@@ -1099,6 +1296,7 @@ VG_STATIC void skate_integrate( player_instance *player )
       q_axis_angle( rotation, axis, mag*k_rb_delta );
       q_mul( rotation, player->rb.q, player->rb.q );
    }
+#endif
 
    /* integrate steering velocities */
    v4f rotate; 
@@ -1203,72 +1401,218 @@ 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;
 
-   /* Setup colliders */
-   m4x3f mtx_front, mtx_back;
-   m3x3_identity( mtx_front );
-   m3x3_identity( mtx_back );
+   float l = k_board_length,
+         w = 0.13f;
+
+   v3f wheel_positions[] = 
+   {
+      { -w, 0.0f, -l },
+      {  w, 0.0f, -l },
+      { -w, 0.0f,  l },
+      {  w, 0.0f,  l },
+   };
+
+   int wheel_states[] =
+   {
+      1, 1, 1, 1
+   };
+
+   if( skate_grind_scansq( player, (v3f){ 0.0f, 0.0f, -l } ) )
+   {
+      wheel_states[0] = 0;
+      wheel_states[1] = 0;
+   }
+
+   if( skate_grind_scansq( player, (v3f){ 0.0f, 0.0f,  l } ) )
+   {
+      wheel_states[2] = 0;
+      wheel_states[3] = 0;
+   }
+
+   rb_sphere collider;
+   collider.radius = 0.07f;
+
+   s->substep = k_rb_delta;
+
+   for( int i=0; i<4; 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 );
+   }
+
+
+begin_collision:;
+
+#ifdef SKATE_CCD
 
-   skate_get_board_points( player, mtx_front[3], mtx_back[3] );
+   /* calculate transform one step into future */
+   v3f future_co;
+   v4f future_q;
+   v3_muladds( player->rb.co, player->rb.v, s->substep, future_co );
+
+   if( v3_length2( player->rb.w ) > 0.0f )
+   {
+      v4f rotation;
+      v3f axis;
+      v3_copy( player->rb.w, axis );
+      
+      float mag = v3_length( axis );
+      v3_divs( axis, mag, axis );
+      q_axis_angle( rotation, axis, mag*s->substep );
+      q_mul( rotation, player->rb.q, future_q );
+      q_normalize( future_q );
+   }
+
+   /* calculate the minimum time we can move */
+   float max_time = s->substep,
+         cast_radius = collider.radius - 0.05f;
+
+   for( int i=0; i<4; i++ )
+   {
+      if( !wheel_states[i] )
+         continue;
+
+      v3f current, future;
+      q_mulv( future_q, wheel_positions[i], future );
+      v3_add( future, future_co, future );
+
+      q_mulv( player->rb.q, wheel_positions[i], current );
+      v3_add( current, player->rb.co, current );
+      
+      float t;    /* TODO: ignore lightly grazing normals? */
+      v3f n;
+      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 );
+   s->substep_delta = max_time;
+
+   /* integrate */
+   v3_muladds( player->rb.co, player->rb.v, s->substep_delta, player->rb.co );
+   if( v3_length2( player->rb.w ) > 0.0f )
+   {
+      v4f rotation;
+      v3f axis;
+      v3_copy( player->rb.w, axis );
+      
+      float mag = v3_length( axis );
+      v3_divs( axis, mag, axis );
+      q_axis_angle( rotation, axis, mag*s->substep_delta );
+      q_mul( rotation, player->rb.q, player->rb.q );
+   }
+
+   rb_update_transform( &player->rb );
+
+   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;
 
-   s->sphere_back.radius = 0.3f;
-   s->sphere_front.radius = 0.3f;
 
    /* create manifold(s) */
-   rb_ct manifold[72],
-         *interface_manifold = NULL,
-         *grind_manifold = NULL;
+   rb_ct manifold[128];
+
+   int manifold_len   = 0,
+       manifold_front = 0,
+       manifold_back  = 0,
+       manifold_interface = 0;
+
+   rb_ct *cmanifold = manifold;
+
+   for( int i=0; i<4; i++ )
+   {
+      if( !wheel_states[i] )
+         continue;
+
+      m4x3f mtx;
+      m3x3_identity( mtx );
 
-   int 
-   len_front = skate_collide_smooth( player, mtx_front, 
-                                     &s->sphere_front, manifold ),
-   len_back = skate_collide_smooth( player, mtx_back,  
-                                    &s->sphere_back, &manifold[len_front] ),
-   interface_len = len_front + len_back;
+      m4x3_mulv( player->rb.to_world, wheel_positions[i], mtx[3] );
+      
+      int l = skate_collide_smooth( player, mtx, &collider, cmanifold );
+
+      cmanifold += l;
+      manifold_len += l;
+      manifold_interface += l;
+
+      if( i<=1 )
+         manifold_front ++;
+      else
+         manifold_back ++;
+   }
 
    /* 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) && (len_front != len_back) )
+   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 );
 
-      if( len_front )
-         v3_copy( mtx_back[3],  trace_from );
+      if( manifold_front )
+         v3_copy( (v3f){0.0f,0.0f, k_board_length}, trace_from );
       else
-         v3_copy( mtx_front[3], trace_from );
+         v3_copy( (v3f){0.0f,0.0f,-k_board_length}, trace_from );
+      m4x3_mulv( player->rb.to_world, trace_from, trace_from );
 
       ray_hit ray;
       ray.dist = 0.6f;
 
       if( ray_world( trace_from, trace_dir, &ray ) )
       {
-         rb_ct *ct = &manifold[ interface_len ];
+         rb_ct *ct = cmanifold;
 
          v3_copy( ray.pos, ct->co );
          v3_copy( ray.normal, ct->n );
          ct->p = 0.0f;
 
-         interface_len ++;
+         manifold_len ++;
+         manifold_interface ++;
       }
    }
 
-   interface_manifold = manifold;
-   grind_manifold = manifold + interface_len;
+   int grind_len = skate_grind_collide( player, cmanifold );
+   manifold_len += grind_len;
 
-   int grind_len = skate_grind_collide( player, grind_manifold );
-
-   for( int i=0; i<interface_len+grind_len; i ++ )
+   for( int i=0; i<manifold_len; 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] );
-      rb_debug_contact( &manifold[i] );
+#endif
    }
 
-   skate_apply_grind_model( player, grind_manifold, grind_len );
-   skate_apply_interface_model( player, manifold, interface_len );
+   skate_collision_response( player, manifold, manifold_len );
+
+   if( s->substep >= 0.0001f )
+      goto begin_collision;
+
+   skate_apply_grind_model( player, &manifold[manifold_interface], grind_len );
+   skate_apply_interface_model( player, manifold, manifold_interface );
    
    skate_apply_pump_model( player );
    skate_apply_cog_model( player );
-   skate_collision_response( player, manifold, interface_len + grind_len );
 
    skate_apply_grab_model( player );
    skate_apply_friction_model( player );