From e6407c13aaba6711ac23ede4cf7171d091feda9a Mon Sep 17 00:00:00 2001
From: Joe Ardent <code@ardent.nebcorp.com>
Date: Mon, 20 Feb 2023 13:29:46 -0800
Subject: [PATCH] something is wonky, but it's kinda ok

---
 src/action/mod.rs     |  8 ++++++--
 src/action/systems.rs | 31 +++++++++++++++++++++++++++----
 2 files changed, 33 insertions(+), 6 deletions(-)

diff --git a/src/action/mod.rs b/src/action/mod.rs
index 3f6a308..abb282a 100644
--- a/src/action/mod.rs
+++ b/src/action/mod.rs
@@ -1,6 +1,7 @@
 use bevy::{
     diagnostic::FrameTimeDiagnosticsPlugin,
-    prelude::{App, IntoSystemDescriptor, Plugin, Resource},
+    prelude::{App, IntoSystemDescriptor, Plugin, ReflectResource, Resource},
+    reflect::Reflect,
 };
 use bevy_rapier3d::prelude::{NoUserData, RapierPhysicsPlugin};
 
@@ -10,7 +11,8 @@ mod systems;
 pub use components::*;
 use systems::*;
 
-#[derive(Resource, Default)]
+#[derive(Resource, Default, Debug, Reflect)]
+#[reflect(Resource)]
 struct CyberLean {
     pub lean: f32,
 }
@@ -23,11 +25,13 @@ impl Plugin for CyberActionPlugin {
             .init_resource::<CatControllerSettings>()
             .init_resource::<ActionDebugInstant>()
             .init_resource::<CyberLean>()
+            .register_type::<CyberLean>()
             .register_type::<CatControllerSettings>()
             .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(falling_cat.label("cat"))
             .add_system(input_forces.label("iforces").after("cat"))
             .add_system(
diff --git a/src/action/systems.rs b/src/action/systems.rs
index b32e11e..4a9501a 100644
--- a/src/action/systems.rs
+++ b/src/action/systems.rs
@@ -18,6 +18,10 @@ use crate::{
 
 const PITCH_SCALE: f32 = 0.2;
 
+fn yaw_to_angle(yaw: f32) -> f32 {
+    yaw.powi(3) * (PI / 4.0)
+}
+
 /// The gravity vector points from the cyberbike to the center of the planet.
 pub(super) fn gravity(
     mut query: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
@@ -30,6 +34,25 @@ pub(super) fn gravity(
     forces.torque = Vec3::ZERO;
 }
 
+/// The desired lean angle, given steering input and speed.
+pub(super) fn cyber_lean(
+    velocity: Query<&Velocity, 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 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);
+
+    lean.lean = (PI / 2.0) - tan_theta.atan();
+}
+
 /// PID-based controller for stabilizing attitude; keeps the cyberbike upright.
 pub(super) fn falling_cat(
     mut bike_query: Query<(&Transform, &mut ExternalForce, &mut CatControllerState)>,
@@ -72,7 +95,7 @@ pub(super) fn falling_cat(
     #[cfg(feature = "inspector")]
     {
         if debug_instant.elapsed().as_millis() > 1000 {
-            dbg!(&control_vars, mag);
+            dbg!(&control_vars, mag, &lean);
             debug_instant.reset();
         }
     }
@@ -94,13 +117,13 @@ pub(super) fn input_forces(
 
     // thrust
     let thrust = xform.forward() * input.throttle * settings.accel;
-    let point = xform.translation + xform.back() * 0.5;
+    let point = xform.translation + xform.back();
     let force = ExternalForce::at_point(thrust, point, xform.translation);
     *forces += force;
 
     // brake
     for mut motor in braking_query.iter_mut() {
-        let factor = if input.brake { 200.00 } else { 0.0 };
+        let factor = if input.brake { 500.00 } else { 0.0 };
         motor.data = (*motor
             .data
             .as_revolute_mut()
@@ -111,7 +134,7 @@ pub(super) fn input_forces(
     }
 
     // steering
-    let angle = input.yaw.powf(3.0) * (PI / 4.0);
+    let angle = yaw_to_angle(input.yaw);
     let mut steering = steering_query.single_mut();
     steering.data = (*steering
         .data