From 0b2086ec5fbcf717aab338cc74b5a4d0f5d1f4e3 Mon Sep 17 00:00:00 2001
From: Joe Ardent <code@ardent.nebcorp.com>
Date: Sun, 6 Apr 2025 15:20:00 -0700
Subject: [PATCH] use force to lean instead of torque

---
 src/physics.rs | 30 +++++++++++++++++++++---------
 1 file changed, 21 insertions(+), 9 deletions(-)

diff --git a/src/physics.rs b/src/physics.rs
index 2f952d4..03a0feb 100644
--- a/src/physics.rs
+++ b/src/physics.rs
@@ -18,9 +18,9 @@ pub struct CatControllerSettings {
 impl Default for CatControllerSettings {
     fn default() -> Self {
         Self {
-            kp: 25.0,
-            kd: 3.0,
-            ki: 1.1,
+            kp: 15.0,
+            kd: 1.5,
+            ki: 0.5,
         }
     }
 }
@@ -105,13 +105,21 @@ mod systems {
     }
 
     pub(super) fn apply_lean(
-        mut bike_query: Query<(&Transform, &mut ExternalTorque, &mut CatControllerState)>,
+        mut bike_query: Query<(&Transform, &mut ExternalForce, &mut CatControllerState)>,
+        wheels: Query<&WheelState>,
         time: Res<Time>,
         settings: Res<CatControllerSettings>,
         lean: Res<CyberLean>,
         mut gizmos: Gizmos,
     ) {
-        let (xform, mut torque, mut control_vars) = bike_query.single_mut();
+        let (xform, mut force, mut control_vars) = bike_query.single_mut();
+
+        let mut factor = 1.0;
+        for wheel in wheels.iter() {
+            if wheel.contact_point.is_none() {
+                factor -= 0.25;
+            }
+        }
 
         let world_up = Vec3::Y; //xform.translation.normalize();
         let rot = Quat::from_axis_angle(*xform.back(), lean.lean);
@@ -142,11 +150,15 @@ mod systems {
             let mag =
                 (settings.kp * roll_error) + (settings.ki * integral) + (settings.kd * derivative);
             if mag.is_finite() {
-                let tork = mag * *xform.back();
-                torque.apply_torque(tork);
+                let lean_force = factor * mag * *xform.left();
+                force.apply_force_at_point(
+                    lean_force,
+                    xform.translation + *xform.up(),
+                    xform.translation,
+                );
                 gizmos.arrow(
-                    xform.translation + Vec3::Y,
-                    xform.translation + mag * *xform.right() + Vec3::Y,
+                    xform.translation + *xform.up(),
+                    xform.translation + *xform.up() + lean_force,
                     Color::WHITE,
                 );
             }