X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=player_physics.h;h=6afc1b2c43b0f3e1cf560e83ca1baceb9836a05b;hb=0124cd309a7db70cdd74b5661f2df8b862ca2f2f;hp=e49c4d83434edf3a512982014a9dc8410df6259e;hpb=ac4a2a0176bd9261b114003ee2c5a66bcf2676f3;p=carveJwlIkooP6JGAAIwe30JlM.git

diff --git a/player_physics.h b/player_physics.h
index e49c4d8..6afc1b2 100644
--- a/player_physics.h
+++ b/player_physics.h
@@ -356,14 +356,11 @@ VG_STATIC void player_physics_control(void)
    phys->slip = slip;
    phys->reverse = -vg_signf(vel[2]);
 
-   float substep = VG_TIMESTEP_FIXED * 0.2f;
+   float substep = VG_TIMESTEP_FIXED;
    float fwd_resistance = k_friction_resistance;
 
-   for( int i=0; i<5; i++ )
-   {
-      vel[2] = stable_force( vel[2],vg_signf(vel[2]) * -fwd_resistance*substep);
-      vel[0] = stable_force( vel[0],vg_signf(vel[0]) * -k_friction_lat*substep);
-   }
+   vel[2] = stable_force( vel[2],vg_signf(vel[2]) * -fwd_resistance*substep);
+   vel[0] = stable_force( vel[0],vg_signf(vel[0]) * -k_friction_lat*substep);
    
    if( player.input_jump->button.value )
    {
@@ -406,6 +403,59 @@ VG_STATIC void player_physics_control(void)
          steer_scaled = vg_signf(steer) * powf(steer,2.0f) * k_steer_ground;
 
    phys->iY -= steer_scaled * VG_TIMESTEP_FIXED;
+   
+
+   /*
+    * EXPERIMENTAL
+    * ===============================================
+    */
+#if 0
+   v3f cog_ideal, diff;
+
+   v3_muladds( phys->rb.co, phys->rb.up, 1.0f-grab, cog_ideal );
+   v3_sub( cog_ideal, phys->cog, diff );
+
+   /* temp */
+   if( v3_length2( diff ) > 20.0f*20.0f )
+      v3_copy( cog_ideal, phys->cog );
+   else
+   {
+      float rate_lat  = k_cog_spring_lat * VG_TIMESTEP_FIXED,
+            rate_vert = k_cog_spring_vert * VG_TIMESTEP_FIXED,
+            vert_amt  = v3_dot( diff, phys->rb.up );
+
+      /* Split into vert/lat springs */
+      v3f diff_vert, diff_lat;
+      v3_muladds( diff, phys->rb.up, -vert_amt, diff_lat );
+      v3_muls( phys->rb.up, vert_amt, diff_vert );
+
+
+      if( diff[1] > 0.0f )
+         rate_vert *= k_cog_boost_multiplier;
+
+      float ap_a =        k_cog_mass_ratio,
+            ap_b = -(1.0f-k_cog_mass_ratio);
+
+      v3_muladds( phys->cog_v, diff_lat,  rate_lat  * ap_a,  phys->cog_v );
+      v3_muladds( phys->cog_v, diff_vert, rate_vert * ap_a,  phys->cog_v );
+
+      //v3_muladds( phys->rb.v,  diff_lat,  rate_lat  * ap_b,  phys->rb.v );
+      v3_muladds( phys->rb.v,  diff_vert, rate_vert * ap_b,  phys->rb.v );
+
+      /* dampen */
+      v3_muls( phys->cog_v, 1.0f-(VG_TIMESTEP_FIXED*k_cog_damp), phys->cog_v );
+
+      /* integrate */
+      v3_muladds( phys->cog, phys->cog_v, VG_TIMESTEP_FIXED, phys->cog );
+   }
+
+
+   /*
+    * EXPERIMENTAL
+    * ===============================================
+    */
+#endif
+
 
    if( !phys->jump_charge && phys->jump > 0.2f )
    {
@@ -860,13 +910,17 @@ VG_STATIC int player_update_grind_collision( rb_ct *contact )
                                                                 c0, c1, k_r );
 
    
+#if 0
    vg_line( p0, p1, 0xff0000ff );
+#endif
 
    if( closest_edge )
    {
+#if 0
       vg_line_cross( c0, 0xff000000, 0.1f );
       vg_line_cross( c1, 0xff000000, 0.1f );
       vg_line( c0, c1, 0xff000000 );
+#endif
 
       v3f delta;
       v3_sub( c1, c0, delta );
@@ -959,7 +1013,108 @@ VG_STATIC void player_adhere_ground( rb_ct *manifold, int len )
    v3f surface_avg;
    v3_zero( surface_avg );
 
-   if( len == 0 )
+   /*
+    *
+    * EXPERIMENTAL
+    * ================================================================
+    */
+   if( phys->in_air )
+      player.normal_pressure = 0.0f;
+   else
+      player.normal_pressure = v3_dot( phys->rb.up, phys->rb.v );
+
+   v3f p0_0, p0_1,
+       p1_0, p1_1,
+       n0, n1;
+   float t0, t1;
+
+   float mod      = 0.7f * player.input_grab->axis.value + 0.3f,
+         spring_k = mod * k_spring_force,
+         damp_K   = mod * k_spring_dampener,
+         disp_k   = 0.4f;
+
+   v3_copy( player.collide_front.co, p0_0 );
+   v3_copy( player.collide_back.co,  p1_0 );
+
+   v3_muladds( p0_0, phys->rb.up, -disp_k, p0_1 );
+   v3_muladds( p1_0, phys->rb.up, -disp_k, p1_1 );
+
+   int cast0 = spherecast_world( p0_0, p0_1, 0.2f, &t0, n0 ),
+       cast1 = spherecast_world( p1_0, p1_1, 0.2f, &t1, n1 );
+
+   v3f animp0, animp1;
+
+   m4x3f temp;
+   m3x3_copy( phys->rb.to_world, temp );
+   if( cast0 != -1 )
+   {
+      v3_lerp( p0_0, p0_1, t0, temp[3] );
+      v3_copy( temp[3], animp0 );
+      debug_sphere( temp, 0.2f, VG__PINK );
+      
+      v3f F, delta;
+      v3_sub( p0_0, phys->rb.co, delta );
+      
+      float displacement = vg_clampf( 1.0f-t0, 0.0f, 1.0f ),
+            damp         = vg_maxf( 0.0f, v3_dot( phys->rb.up, phys->rb.v ) );
+      v3_muls( phys->rb.up, displacement*spring_k*k_rb_delta -
+                            damp*damp_K*k_rb_delta, F );
+
+      v3_muladds( phys->rb.v, F, 1.0f, phys->rb.v );
+      
+      /* Angular velocity */
+      v3f wa;
+      v3_cross( delta, F, wa );
+      v3_muladds( phys->rb.w, wa, k_spring_angular, phys->rb.w );
+   }
+   else
+      v3_copy( p0_1, animp0 );
+
+   if( cast1 != -1 )
+   {
+      v3_lerp( p1_0, p1_1, t1, temp[3] );
+      v3_copy( temp[3], animp1 );
+      debug_sphere( temp, 0.2f, VG__PINK );
+
+      v3f F, delta;
+      v3_sub( p1_0, phys->rb.co, delta );
+      
+      float displacement = vg_clampf( 1.0f-t1, 0.0f, 1.0f ),
+            damp         = vg_maxf( 0.0f, v3_dot( phys->rb.up, phys->rb.v ) );
+      v3_muls( phys->rb.up, displacement*spring_k*k_rb_delta -
+                            damp*damp_K*k_rb_delta, F );
+
+      v3_muladds( phys->rb.v, F, 1.0f, phys->rb.v );
+      
+      /* Angular velocity */
+      v3f wa;
+      v3_cross( delta, F, wa );
+      v3_muladds( phys->rb.w, wa, k_spring_angular, phys->rb.w );
+   }
+   else
+      v3_copy( p1_1, animp1 );
+
+   v3f animavg, animdelta;
+   v3_add( animp0, animp1, animavg );
+   v3_muls( animavg, 0.5f, animavg );
+
+   v3_sub( animp1, animp0, animdelta );
+   v3_normalize( animdelta );
+
+   m4x3_mulv( phys->rb.to_local, animavg, player.board_offset );
+
+   float dx = -v3_dot( animdelta, phys->rb.forward ),
+         dy =  v3_dot( animdelta, phys->rb.up );
+
+   float angle = -atan2f( dy, dx );
+   q_axis_angle( player.board_rotation, (v3f){ 1.0f, 0.0f, 0.0f }, angle );
+
+   /*
+    * ================================================================
+    * EXPERIMENTAL
+    */
+
+   if( len == 0 && !((cast0 !=-1)&&(cast1!=-1)) )
    {
       phys->lift_frames ++;
 
@@ -995,11 +1150,13 @@ VG_STATIC void player_adhere_ground( rb_ct *manifold, int len )
          float angle = v3_dot( phys->rb.up, projected );
          v3_cross( phys->rb.up, projected, axis );
 
+#if 0
          v3f p0, p1;
          v3_add( phys->rb.co, projected, p0 );
          v3_add( phys->rb.co, phys->rb.up, p1 );
          vg_line( phys->rb.co, p0, 0xff00ff00 );
          vg_line( phys->rb.co, p1, 0xff000fff );
+#endif
 
          if( fabsf(angle) < 0.999f )
          {
@@ -1019,7 +1176,106 @@ VG_STATIC void player_collision_response( rb_ct *manifold, int len )
 {
    struct player_phys *phys = &player.phys;
 
-   for( int j=0; j<5; j++ )
+   /*
+    * EXPERIMENTAL
+    * ===============================================
+    */
+
+#if 0
+   player.normal_pressure = v3_dot( phys->rb.up, phys->rb.v );
+
+   {
+      float ideal   = 1.0f-player.input_grab->axis.value,
+            diff    = phys->spring - ideal,
+            Fspring = -k_cog_spring_lat * diff,
+            Fdamp   = -k_cog_damp       * phys->springv,
+            F       = (Fspring + Fdamp) * k_rb_delta;
+
+      phys->springv += F;
+      phys->spring  += phys->springv * k_rb_delta;
+
+      if( phys->springv > 0.0f )
+         v3_muladds( phys->rb.v, phys->rb.up, F*k_cog_spring_vert, phys->rb.v );
+
+      if( phys->in_air )
+         player.normal_pressure = 0.0f;
+      else
+         player.normal_pressure = v3_dot( phys->rb.up, phys->rb.v );
+   }
+#endif
+
+   if( player.input_grab->axis.value > 0.5f )
+   {
+      if( !phys->in_air )
+      {
+         /* Throw */
+         v3_muls( phys->rb.up, k_mmthrow_scale, phys->throw_v );
+      }
+   }
+   else
+   {
+      /* Collect */
+      float doty = v3_dot( phys->rb.up, phys->throw_v );
+      
+      v3f Fl, Fv;
+      v3_muladds( phys->throw_v, phys->rb.up, -doty, Fl );
+
+      if( !phys->in_air )
+      {
+         v3_muladds( phys->rb.v,    Fl,  k_mmcollect_lat, phys->rb.v );
+         v3_muladds( phys->throw_v, Fl, -k_mmcollect_lat, phys->throw_v );
+      }
+
+      v3_muls( phys->rb.up, -doty, Fv );
+      v3_muladds( phys->rb.v,    Fv, k_mmcollect_vert, phys->rb.v );
+      v3_muladds( phys->throw_v, Fv, k_mmcollect_vert, phys->throw_v );
+
+      v3_copy( Fl, player.debug_mmcollect_lat );
+      v3_copy( Fv, player.debug_mmcollect_vert );
+   }
+
+   /* Decay */
+   if( v3_length2( phys->throw_v ) > 0.0001f )
+   {
+      v3f dir;
+      v3_copy( phys->throw_v, dir );
+      v3_normalize( dir );
+
+      float max = v3_dot( dir, phys->throw_v ),
+            amt = vg_minf( k_mmdecay * k_rb_delta, max );
+
+      v3_muladds( phys->throw_v, dir, -amt, phys->throw_v );
+   }
+
+
+   /* TODO: RElocate */
+   {
+
+   v3f ideal_cog, ideal_diff;
+   v3_muladds( phys->rb.co, phys->rb.up, 
+               1.0f-player.input_grab->axis.value, ideal_cog );
+   v3_sub( ideal_cog, phys->cog, ideal_diff );
+
+   /* Apply velocities */
+   v3f rv;
+   v3_sub( phys->rb.v, phys->cog_v, rv );
+
+   v3f F;
+   v3_muls( ideal_diff, -k_cog_spring * k_rb_rate, F );
+   v3_muladds( F, rv,   -k_cog_damp * k_rb_rate, F );
+
+   float ra = k_cog_mass_ratio,
+         rb = 1.0f-k_cog_mass_ratio;
+
+   v3_muladds( phys->cog_v, F, -rb, phys->cog_v );
+   }
+
+   /*
+    * EXPERIMENTAL
+    * ===============================================
+    */
+
+   for( int j=0; j<10; j++ )
    {
       for( int i=0; i<len; i++ )
       {
@@ -1065,6 +1321,10 @@ VG_STATIC void player_collision_response( rb_ct *manifold, int len )
          v3_muladds( phys->rb.w, phys->rb.right, wx, phys->rb.w );
       }
    }
+   
+   /* early integrate this */
+   phys->cog_v[1] += -9.8f * k_rb_delta;
+   v3_muladds( phys->cog, phys->cog_v, k_rb_delta, phys->cog );
 }
 
 VG_STATIC void player_save_frame(void)
@@ -1081,6 +1341,10 @@ VG_STATIC void player_restore_frame(void)
 VG_STATIC void player_integrate(void)
 {
    struct player_phys *phys = &player.phys;
+   v3_sub( phys->rb.v, phys->v_last, phys->a );
+   v3_muls( phys->a, 1.0f/VG_TIMESTEP_FIXED, phys->a );
+   v3_copy( phys->rb.v, phys->v_last );
+
    apply_gravity( phys->rb.v, VG_TIMESTEP_FIXED );
    v3_muladds( phys->rb.co, phys->rb.v, VG_TIMESTEP_FIXED, phys->rb.co );
 }
@@ -1228,6 +1492,8 @@ VG_STATIC void player_do_motion(void)
       if( gate_intersect( gate, phys->rb.co, prevco ) )
       {
          m4x3_mulv( gate->transport, phys->rb.co, phys->rb.co );
+         m4x3_mulv( gate->transport, phys->cog, phys->cog );
+         m3x3_mulv( gate->transport, phys->cog_v, phys->cog_v );
          m3x3_mulv( gate->transport, phys->rb.v, phys->rb.v );
          m3x3_mulv( gate->transport, phys->vl, phys->vl );
          m3x3_mulv( gate->transport, phys->v_last, phys->v_last );
@@ -1386,6 +1652,10 @@ VG_STATIC int reset_player( int argc, char const *argv[] )
    player.mdl.shoes[1] = 1;
    
    rb_update_transform( &phys->rb );
+   
+   v3_add( phys->rb.up, phys->rb.co, phys->cog );
+   v3_zero( phys->cog_v );
+
    player_save_frame();
    return 1;
 }
@@ -1403,4 +1673,43 @@ VG_STATIC void reset_player_poll( int argc, char const *argv[] )
    }
 }
 
+VG_STATIC void player_physics_gui(void)
+{
+   return;
+
+	vg_uictx.cursor[0] = 0;
+	vg_uictx.cursor[1] = vg.window_y - 128;
+	vg_uictx.cursor[3] = 14;
+	ui_fill_x();
+
+   char buf[128];
+
+   snprintf( buf, 127, "v: %6.3f %6.3f %6.3f\n", player.phys.rb.v[0],
+                                                 player.phys.rb.v[1],
+                                                 player.phys.rb.v[2] );
+
+   ui_text( vg_uictx.cursor, buf, 1, 0 );
+	vg_uictx.cursor[1] += 14;
+
+
+   snprintf( buf, 127, "a: %6.3f %6.3f %6.3f (%6.3f)\n", player.phys.a[0],
+                                                     player.phys.a[1],
+                                                     player.phys.a[2],
+                                                     v3_length(player.phys.a));
+   ui_text( vg_uictx.cursor, buf, 1, 0 );
+	vg_uictx.cursor[1] += 14;
+
+   float normal_acceleration = v3_dot( player.phys.a, player.phys.rb.up );
+   snprintf( buf, 127, "Normal acceleration: %6.3f\n", normal_acceleration );
+
+   ui_text( vg_uictx.cursor, buf, 1, 0 );
+	vg_uictx.cursor[1] += 14;
+
+   snprintf( buf, 127, "Normal Pressure: %6.3f\n", player.normal_pressure );
+   ui_text( vg_uictx.cursor, buf, 1, 0 );
+	vg_uictx.cursor[1] += 14;
+
+
+}
+
 #endif /* PLAYER_PHYSICS_H */