From e9fed1940e11b0770572c41fd0ecc97f862d5150 Mon Sep 17 00:00:00 2001
From: Joe Ardent <code@ardent.nebcorp.com>
Date: Tue, 21 Feb 2023 13:25:55 -0800
Subject: [PATCH] re-enable pitch stabilization

---
 src/action/systems.rs | 28 ++++++----------------------
 1 file changed, 6 insertions(+), 22 deletions(-)

diff --git a/src/action/systems.rs b/src/action/systems.rs
index 02208ce..086bdb0 100644
--- a/src/action/systems.rs
+++ b/src/action/systems.rs
@@ -1,4 +1,4 @@
-use std::f32::consts::{FRAC_PI_2, FRAC_PI_3, FRAC_PI_4};
+use std::f32::consts::{FRAC_PI_3, FRAC_PI_4};
 
 use bevy::prelude::{
     Commands, Entity, Quat, Query, Res, ResMut, Time, Transform, Vec3, With, Without,
@@ -36,21 +36,13 @@ pub(super) fn gravity(
 
 /// The desired lean angle, given steering input and speed.
 pub(super) fn cyber_lean(
-    mut bike_state: Query<
-        (
-            &Velocity,
-            &mut ExternalForce,
-            &Transform,
-            &ReadMassProperties,
-        ),
-        With<CyberBikeBody>,
-    >,
+    bike_state: Query<(&Velocity, &Transform), With<CyberBikeBody>>,
     wheels: Query<&Transform, With<CyberWheel>>,
     input: Res<InputState>,
     gravity_settings: Res<MovementSettings>,
     mut lean: ResMut<CyberLean>,
 ) {
-    let (velocity, mut forces, xform, mass_props) = bike_state.single_mut();
+    let (velocity, xform) = bike_state.single();
     let vel = velocity.linvel.dot(xform.forward());
     let v_squared = vel.powi(2);
     let steering_angle = yaw_to_angle(input.yaw);
@@ -61,13 +53,7 @@ pub(super) fn cyber_lean(
     let v2_r = v_squared / radius;
     let tan_theta = (v2_r / gravity).clamp(-FRAC_PI_3, FRAC_PI_3);
 
-    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);
     }
 }
@@ -91,10 +77,10 @@ pub(super) fn falling_cat(
     let target_up = -Vec3::from_array([rot_qup.x, rot_qup.y, rot_qup.z]).normalize();
     let bike_right = xform.right();
 
-    //let world_right = xform.forward().cross(world_up).normalize();
+    let world_right = xform.forward().cross(world_up).normalize();
 
     let roll_error = bike_right.dot(target_up);
-    let pitch_error = world_up.dot(xform.forward());
+    let pitch_error = world_up.dot(xform.back());
 
     // only try to correct roll if we're not totally vertical
     if pitch_error.abs() < 0.95 {
@@ -120,10 +106,8 @@ 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 += world_right * mag;
     }
-
-    //control_vars.decay();
 }
 
 /// Apply forces to the cyberbike as a result of input.