From 94717dd60cbef02bbf2d615d634354eaa0cb6828 Mon Sep 17 00:00:00 2001
From: Joe Ardent <code@ardent.nebcorp.com>
Date: Mon, 17 Jun 2024 13:09:41 -0700
Subject: [PATCH] unstable normal forces

---
 src/action/systems.rs | 41 +++++++++++++++++++++++++++++++++--------
 src/bike/wheels.rs    |  3 ++-
 2 files changed, 35 insertions(+), 9 deletions(-)

diff --git a/src/action/systems.rs b/src/action/systems.rs
index 5461388..8a4d766 100644
--- a/src/action/systems.rs
+++ b/src/action/systems.rs
@@ -1,7 +1,11 @@
 use std::f32::consts::{FRAC_PI_3, FRAC_PI_4};
 
-use bevy::prelude::{
-    info, Commands, Entity, Local, Quat, Query, Res, ResMut, Time, Transform, Vec3, With, Without,
+use bevy::{
+    prelude::{
+        info, Commands, Entity, Gizmos, Local, Quat, Query, Res, ResMut, Time, Transform, Vec3,
+        With, Without,
+    },
+    render::color::Color,
 };
 use bevy_rapier3d::{
     dynamics::MassProperties,
@@ -154,6 +158,7 @@ pub(super) fn wheel_forces(
     context: Res<RapierContext>,
     settings: Res<MovementSettings>,
     input: Res<InputState>,
+    mut gizmos: Gizmos,
 ) {
     let (body_xform, body_vel, mut body_pvel, body_mass) = body_query.single_mut();
 
@@ -171,24 +176,34 @@ pub(super) fn wheel_forces(
     let body_half_mass = body_mass.mass * 0.5;
     let half_weight = settings.gravity * body_half_mass;
 
+    // wheel stuff
+    let radius = wheel_config.radius;
+    let squish_radius = 1.5 * radius;
+
     // inputs
     let steering = input.yaw;
     let throttle = input.throttle * settings.accel;
 
-    for (xform, vel, mut pvel, mut force, steering) in wheels_query.iter_mut() {
+    for (xform, vel, mut pvel, mut external_force, steering) in wheels_query.iter_mut() {
         if let Some((_, projected)) =
-            context.project_point(xform.translation, true, QueryFilter::only_fixed())
+            context.project_point(xform.translation, false, QueryFilter::only_fixed())
         {
-            info!("projected point: {:?}", projected.point);
+            //info!("projected point: {:?}", projected);
             //dbg!("{:?}", projected);
             let normal = (projected.point - xform.translation);
             let len = normal.length();
             let normal = normal.normalize();
-            if len < wheel_config.radius {
+            if len < squish_radius {
+                let squish = if len > radius {
+                    1.0 - ((len - radius) / (squish_radius - radius)).powi(2)
+                } else {
+                    1.0
+                };
+                info!("squish: {squish}");
                 let grav_norm = gvec.normalize().dot(normal);
                 // we're in contact, first gravity
-                let inside = if projected.is_inside { -1.0 } else { 1.0 };
-                let mut force = inside * 10.0 * grav_norm * half_weight * normal;
+                let normal = if projected.is_inside { Vec3::Y } else { normal };
+                let mut force = -1.1 * grav_norm * half_weight * normal;
 
                 // do linear acceleration's share
                 // f = ma
@@ -205,6 +220,16 @@ pub(super) fn wheel_forces(
                 // center_of_mass);
 
                 // do steering's share (also depends on friction)
+
+                // apply the squish factor
+                force *= squish;
+
+                // show the force
+                gizmos.arrow(xform.translation, force, Color::RED);
+
+                // ok apply the force
+                let ef = ExternalForce::at_point(force, projected.point, xform.translation);
+                *external_force = ef;
             }
         }
     }
diff --git a/src/bike/wheels.rs b/src/bike/wheels.rs
index e3c5be3..d5fb23f 100644
--- a/src/bike/wheels.rs
+++ b/src/bike/wheels.rs
@@ -61,7 +61,8 @@ pub(crate) fn spawn_wheels(
         let suspension_damping = conf.damping;
 
         let suspension_axis = if steering.is_some() {
-            rake_vec
+            // rake_vec
+            Vec3::Y
         } else {
             Vec3::Y
         };