basic 5050

diff --git a/common.h b/common.h
index 79ea5265adc3b4117221f26d9f1038cf045bd3d0..6bb6951c633852a10909e05e8a3e44ca70a83238 100644 (file)
--- a/common.h
+++ b/common.h
@@ -131,7 +131,10 @@ VG_STATIC float
    k_spring_angular        = 1.0f,
 
    k_spring_force          = 15.0f,
-   k_spring_dampener       = 5.0f;
+   k_spring_dampener       = 5.0f,
+
+   k_grind_spring          = 100.0f,
+   k_grind_dampener        = 1.0f;
 
 
 VG_STATIC float
@@ -169,6 +172,9 @@ VG_STATIC void common_var_temp(void)
    VG_VAR_F32( k_cam_damp );
    VG_VAR_F32( k_cam_spring );
 
+   VG_VAR_F32( k_grind_dampener );
+   VG_VAR_F32( k_grind_spring );
+
    VG_VAR_F32( k_walkspeed );
    VG_VAR_F32( k_stopspeed );
    VG_VAR_F32( k_airspeed );

diff --git a/player_skate.c b/player_skate.c
index b2d951e9026e07bccd3beed2b716deb454914a5f..87777631ae3e2b42d6477a682f82d2026a7c33cc 100644 (file)
--- 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 );
 
@@ -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
@@ -1224,11 +1412,38 @@ VG_STATIC void player__skate_update( player_instance *player )
       {  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
@@ -1257,6 +1472,9 @@ begin_collision:;
 
    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 );
@@ -1316,6 +1534,9 @@ begin_collision:;
 
    for( int i=0; i<4; i++ )
    {
+      if( !wheel_states[i] )
+         continue;
+
       m4x3f mtx;
       m3x3_identity( mtx );
 
@@ -1380,7 +1601,6 @@ begin_collision:;
 #else
       rb_prepare_contact( &manifold[i] );
 #endif
-      rb_debug_contact( &manifold[i] );
    }
 
    skate_collision_response( player, manifold, manifold_len );