so close

src/action/components.rs

@@ -68,9 +68,9 @@ pub struct CatControllerSettings {
 impl Default for CatControllerSettings {
     fn default() -> Self {
         Self {
-            kp: 40.0,
-            kd: 5.0,
-            ki: 0.1,
+            kp: 80.0,
+            kd: 10.0,
+            ki: 0.4,
         }
     }
 }

src/action/mod.rs

@@ -30,8 +30,8 @@ impl Plugin for CyberActionPlugin {
             .add_plugin(RapierPhysicsPlugin::<NoUserData>::default())
             .add_plugin(FrameTimeDiagnosticsPlugin::default())
             .add_system(surface_fix.label("surface_fix"))
-            .add_system(gravity.before("cat"))
-            .add_system(cyber_lean.before("cat"))
+            .add_system(gravity.label("gravity").before("cat"))
+            .add_system(cyber_lean.before("cat").after("gravity"))
             .add_system(falling_cat.label("cat"))
             .add_system(input_forces.label("iforces").after("cat"))
             .add_system(

src/action/systems.rs

@@ -1,4 +1,4 @@
-use std::f32::consts::PI;
+use std::f32::consts::{FRAC_PI_2, FRAC_PI_3, FRAC_PI_4};
 
 use bevy::prelude::{
     Commands, Entity, Quat, Query, Res, ResMut, Time, Transform, Vec3, With, Without,
@@ -19,7 +19,7 @@ use crate::{
 const PITCH_SCALE: f32 = 0.2;
 
 fn yaw_to_angle(yaw: f32) -> f32 {
-    yaw.powi(3) * (PI / 4.0)
+    yaw.powi(3) * FRAC_PI_4
 }
 
 /// The gravity vector points from the cyberbike to the center of the planet.
@@ -36,21 +36,40 @@ pub(super) fn gravity(
 
 /// The desired lean angle, given steering input and speed.
 pub(super) fn cyber_lean(
-    velocity: Query<&Velocity, With<CyberBikeBody>>,
+    mut bike_state: Query<
+        (
+            &Velocity,
+            &mut ExternalForce,
+            &Transform,
+            &ReadMassProperties,
+        ),
+        With<CyberBikeBody>,
+    >,
     wheels: Query<&Transform, With<CyberWheel>>,
     input: Res<InputState>,
     gravity_settings: Res<MovementSettings>,
     mut lean: ResMut<CyberLean>,
 ) {
-    let vel_squared = velocity.single().linvel.length_squared();
+    let (velocity, mut forces, xform, mass_props) = bike_state.single_mut();
+    let vel = velocity.linvel.dot(xform.forward());
+    let v_squared = vel.powi(2);
     let steering_angle = yaw_to_angle(input.yaw);
     let wheels: Vec<_> = wheels.iter().map(|w| w.translation).collect();
     let wheel_base = (wheels[0] - wheels[1]).length();
     let radius = wheel_base / steering_angle.tan();
     let gravity = gravity_settings.gravity;
-    let tan_theta = vel_squared / (radius * gravity);
+    let v2_r = v_squared / radius;
+    let tan_theta = (v2_r / gravity).clamp(-FRAC_PI_3, FRAC_PI_3);
 
-    lean.lean = (PI / 2.0) - tan_theta.atan();
+    let up = xform.translation.normalize();
+    let right = up.cross(xform.back()).normalize();
+
+    let centripetal = -right * (mass_props.0.mass * v2_r);
+
+    if v2_r.is_normal() {
+        forces.force += centripetal;
+        lean.lean = -tan_theta.atan().clamp(-FRAC_PI_4, FRAC_PI_4);
+    }
 }
 
 /// PID-based controller for stabilizing attitude; keeps the cyberbike upright.
@@ -62,15 +81,23 @@ pub(super) fn falling_cat(
     #[cfg(feature = "inspector")] mut debug_instant: ResMut<ActionDebugInstant>,
 ) {
     let (xform, mut forces, mut control_vars) = bike_query.single_mut();
-    let mut wup = Transform::from_translation(xform.translation);
-    wup.rotate(Quat::from_axis_angle(xform.back(), lean.lean));
-    let wup = wup.up();
+    let wup = xform.translation.normalize(); //Transform::from_translation(xform.translation.normalize());
+    let rot = Quat::from_axis_angle(xform.forward(), lean.lean);
+    let [x, y, z] = wup.to_array();
+    let qup = Quat::from_xyzw(x, y, z, 0.0); // turn our "world up" point into a Quat
+
+    // p' = rot^-1 * qup * rot
+    let wup = rot.inverse() * qup * rot;
+    let wup = Vec3::from_array([wup.x, wup.y, wup.z]).normalize();
     let bike_up = xform.up();
 
-    let wright = xform.forward().cross(wup).normalize();
+    let wright = xform
+        .forward()
+        .cross(xform.translation.normalize())
+        .normalize();
 
-    let roll_error = wright.dot(bike_up);
-    let pitch_error = wup.dot(xform.back());
+    let roll_error = wright.dot(wup);
+    let pitch_error = xform.translation.normalize().dot(xform.forward());
 
     // only try to correct roll if we're not totally vertical
     if pitch_error.abs() < 0.95 {
@@ -80,6 +107,13 @@ pub(super) fn falling_cat(
         if mag.is_finite() {
             forces.torque += xform.back() * mag;
         }
+        #[cfg(feature = "inspector")]
+        {
+            if debug_instant.elapsed().as_millis() > 5000 {
+                dbg!(&control_vars, mag, &wup);
+                debug_instant.reset();
+            }
+        }
     }
 
     // we can do pitch correction any time, it's not coupled to roll
@@ -89,17 +123,10 @@ pub(super) fn falling_cat(
     let mag =
         (settings.kp * scaled_pitch_error) + (settings.ki * integral) + (settings.kd * derivative);
     if mag.is_finite() {
-        forces.torque += wright * mag;
+        //forces.torque += wright * mag;
     }
 
-    #[cfg(feature = "inspector")]
-    {
-        if debug_instant.elapsed().as_millis() > 1000 {
-            dbg!(&control_vars, mag, &lean);
-            debug_instant.reset();
-        }
-    }
-    control_vars.decay();
+    //control_vars.decay();
 }
 
 /// Apply forces to the cyberbike as a result of input.