abandoning failed attempt

src/physics.rs

@@ -64,7 +64,6 @@ mod systems {
     };
     use bevy::{
         ecs::system::{Populated, Query, Single},
-        gizmos::retained::Gizmo,
         prelude::{
             ButtonInput, Color, Gizmos, GlobalTransform, KeyCode, Quat, Res, ResMut, Time,
             Transform, Vec, Vec3, With, Without,
@@ -89,76 +88,51 @@ mod systems {
     }
 
     pub(super) fn calculate_lean(
-        velocities: Single<(&LinearVelocity, &AngularVelocity), With<CyberBikeBody>>,
-        wheels: Populated<(&GlobalTransform, &CyberWheel), With<Collider>>,
+        velocity: Single<&LinearVelocity, With<CyberBikeBody>>,
+        wheels: Populated<&GlobalTransform, (With<Collider>, With<CyberWheel>)>,
         gravity: Res<Gravity>,
+        input: Res<InputState>,
         mut lean: ResMut<CyberLean>,
-        mut gizmos: Gizmos,
     ) {
-        let mut w1 = GlobalTransform::default();
-        let mut w2 = GlobalTransform::default();
-        for (xform, wheel) in wheels.iter() {
-            match wheel {
-                CyberWheel::Front => w1 = *xform,
-                CyberWheel::Rear => w2 = *xform,
-            }
+        let d: GlobalTransform = Default::default();
+        let mut w = [&d; 2];
+        for (i, xform) in wheels.iter().enumerate() {
+            w[i] = xform;
         }
-        let wheelbase = (w2.translation() - w1.translation()).normalize();
+        let wheelbase = w[0].translation() - w[1].translation();
+        let base = wheelbase.length();
 
-        let (lin_vel, ang_vel) = velocities.into_inner();
+        let steering_angle = yaw_to_angle(input.yaw);
+        let radius = base / steering_angle.tan();
+
+        let lin_vel = velocity.into_inner();
 
         let right = wheelbase.cross(gravity.0).normalize();
-        let up = -right.cross(wheelbase).normalize();
-        let spin = ang_vel.dot(up);
+        let up = right.cross(wheelbase).normalize();
 
-        let mid = (w2.translation() + w1.translation()) / 2.0;
+        let gdir = gravity.0.normalize();
+        let up = up * -gdir.dot(up).signum();
 
-        gizmos.arrow(mid, mid + (up * 5.0), Color::linear_rgb(0.0, 1.0, 1.0));
-
-        let period = if spin.is_normal() {
-            1.0 / spin
-        } else {
-            f32::MAX
-        };
-
-        let radius = lin_vel.length() * period;
         let gravity = gravity.0.dot(up);
         let tan_theta = lin_vel.length_squared() / (radius * gravity);
 
         if tan_theta.is_normal() {
-            lean.lean = tan_theta.atan();
-        } else {
-            lean.lean = 0.0;
-        }
-    }
-
-    pub(super) fn clear(
-        mut forces: Query<&mut ExternalForce>,
-        mut torques: Query<&mut ExternalTorque>,
-    ) {
-        for mut force in forces.iter_mut() {
-            force.clear();
-        }
-        for mut torq in torques.iter_mut() {
-            torq.clear();
+            let atan = tan_theta.atan();
+            if atan.is_normal() {
+                lean.lean = atan;
+            }
         }
     }
 
     pub(super) fn apply_lean(
-        bike_query: Single<(
-            &Transform,
-            &mut ExternalTorque,
-            &mut CatControllerState,
-            &ComputedCenterOfMass,
-        )>,
+        bike_query: Single<(&Transform, &mut ExternalTorque, &mut CatControllerState)>,
         wheels: Populated<(&WheelState, &CyberWheel)>,
         time: Res<Time>,
         settings: Res<CatControllerSettings>,
         lean: Res<CyberLean>,
         mut gizmos: Gizmos,
     ) {
-        let (xform, mut force, mut control_vars, ComputedCenterOfMass(com)) =
-            bike_query.into_inner();
+        let (xform, mut force, mut control_vars) = bike_query.into_inner();
 
         let mut factor = 1.0;
         let mut front = None;
@@ -487,7 +461,7 @@ mod systems {
     }
 }
 
-use systems::{apply_lean, calculate_lean, clear, drag, ef, suspension, tweak, wheel_action};
+use systems::{apply_lean, calculate_lean, drag, ef, suspension, tweak, wheel_action};
 
 pub struct CyberPhysicsPlugin;