From a65e60a3162853cb8430119f36aecf98bcb1ee03 Mon Sep 17 00:00:00 2001
From: Joe Ardent <code@ardent.nebcorp.com>
Date: Wed, 30 Apr 2025 16:54:25 -0700
Subject: [PATCH] use torque for lean, use Single and Populated instead of
 Query

systems won't run if no entities match a Single or Populated query, so no need to return a Result or set the error handler
---
 src/main.rs    |   5 ---
 src/physics.rs | 103 ++++++++++++++++++++++++-------------------------
 2 files changed, 51 insertions(+), 57 deletions(-)

diff --git a/src/main.rs b/src/main.rs
index da52c31..128b956 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -3,7 +3,6 @@ use avian3d::prelude::{
 };
 use bevy::{
     color::{palettes::css::SILVER, Alpha},
-    ecs::error::{warn, GLOBAL_ERROR_HANDLER},
     pbr::MeshMaterial3d,
     prelude::{
         children, default, App, AppGizmoBuilder, Assets, ButtonInput, Color, Commands,
@@ -24,10 +23,6 @@ use input::CyberInputPlugin;
 use physics::CyberPhysicsPlugin;
 
 fn main() {
-    GLOBAL_ERROR_HANDLER
-        .set(warn)
-        .expect("The error handler can only be set once, globally.");
-    // initialize Bevy App here
     let mut app = App::new();
     app.add_plugins((
         DefaultPlugins,
diff --git a/src/physics.rs b/src/physics.rs
index 1a96216..f75c372 100644
--- a/src/physics.rs
+++ b/src/physics.rs
@@ -20,9 +20,9 @@ pub struct CatControllerSettings {
 impl Default for CatControllerSettings {
     fn default() -> Self {
         Self {
-            kp: 45.0,
-            kd: 15.0,
-            ki: 2.5,
+            kp: 55.0,
+            kd: 20.0,
+            ki: 12.5,
         }
     }
 }
@@ -58,14 +58,14 @@ mod systems {
     use std::f32::consts::{FRAC_PI_3, FRAC_PI_4};
 
     use avian3d::prelude::{
-        ComputedCenterOfMass, ExternalForce, Gravity, LinearVelocity, RayCaster, RayHits,
-        RigidBodyQueryReadOnly,
+        ComputedCenterOfMass, ExternalForce, ExternalTorque, Gravity, LinearVelocity, RayCaster,
+        RayHits, RigidBodyQueryReadOnly,
     };
     use bevy::{
-        ecs::error::BevyError,
+        ecs::system::{Populated, Single},
         prelude::{
-            ButtonInput, Color, Gizmos, GlobalTransform, KeyCode, Quat, Query, Res, ResMut, Result,
-            Time, Transform, Vec, Vec3, With, Without,
+            ButtonInput, Color, Gizmos, GlobalTransform, KeyCode, Quat, Res, ResMut, Time,
+            Transform, Vec, Vec3, With, Without,
         },
     };
 
@@ -87,17 +87,17 @@ mod systems {
     }
 
     pub(super) fn calculate_lean(
-        bike_state: Query<(&LinearVelocity, &Transform), With<CyberBikeBody>>,
-        wheels: Query<&GlobalTransform, With<CyberWheel>>,
+        bike_state: Single<(&LinearVelocity, &Transform), With<CyberBikeBody>>,
+        wheels: Populated<&GlobalTransform, With<CyberWheel>>,
         input: Res<InputState>,
         gravity: Res<Gravity>,
         mut lean: ResMut<CyberLean>,
-    ) -> Result {
+    ) {
         let mut wheels = wheels.iter();
-        let w1 = wheels.next().ok_or(BevyError::from("oops"))?;
-        let w2 = wheels.next().ok_or(BevyError::from("oops"))?;
+        let w1 = wheels.next().unwrap();
+        let w2 = wheels.next().unwrap();
         let base = (w1.translation() - w2.translation()).length().abs();
-        let (velocity, xform) = bike_state.single()?;
+        let (velocity, xform) = bike_state.into_inner();
         let vel = velocity.dot(*xform.forward());
         let v_squared = vel.powi(2);
         let steering_angle = yaw_to_angle(input.yaw);
@@ -111,31 +111,37 @@ mod systems {
         } else {
             //lean.lean = 0.0;
         }
-        Ok(())
     }
 
     pub(super) fn apply_lean(
-        mut bike_query: Query<(
-            &Transform,
-            &ComputedCenterOfMass,
-            &mut ExternalForce,
-            &mut CatControllerState,
-        )>,
-        wheels: Query<&WheelState>,
+        bike_query: Single<(&Transform, &mut ExternalTorque, &mut CatControllerState)>,
+        wheels: Populated<(&WheelState, &GlobalTransform, &CyberWheel)>,
         time: Res<Time>,
         settings: Res<CatControllerSettings>,
         lean: Res<CyberLean>,
         mut gizmos: Gizmos,
-    ) -> Result {
-        let (xform, com, mut force, mut control_vars) = bike_query.single_mut()?;
+    ) {
+        let (xform, mut torque, mut control_vars) = bike_query.into_inner();
 
         let mut factor = 1.0;
-        for wheel in wheels.iter() {
-            if wheel.contact_point.is_none() {
+        let mut rxform = GlobalTransform::default();
+        let mut fxform = GlobalTransform::default();
+        for (wheel_state, xform, cwheel) in wheels.iter() {
+            if wheel_state.contact_point.is_none() {
                 factor -= 0.25;
             }
+            match cwheel {
+                CyberWheel::Front => {
+                    fxform = *xform;
+                }
+                CyberWheel::Rear => {
+                    rxform = *xform;
+                }
+            }
         }
 
+        let tork_axis = (rxform.translation() - fxform.translation()).normalize();
+
         let world_up = Vec3::Y; //xform.translation.normalize();
         let rot = Quat::from_axis_angle(*xform.back(), lean.lean);
         let target_up = rotate_point(&world_up, &rot).normalize();
@@ -165,20 +171,18 @@ mod systems {
             let mag =
                 (settings.kp * roll_error) + (settings.ki * integral) + (settings.kd * derivative);
             if mag.is_finite() {
-                let lean_force = factor * mag * *xform.left();
-                force.apply_force_at_point(
-                    lean_force,
-                    xform.translation + *xform.up(),
-                    xform.translation + com.0,
-                );
+                //let lean_force = factor * mag * *xform.left();
+                let tork = factor * mag * tork_axis;
+
+                torque.apply_torque(tork);
+
                 gizmos.arrow(
                     xform.translation + *xform.up(),
-                    xform.translation + *xform.up() + lean_force,
+                    xform.translation + *xform.up() + mag * xform.left(),
                     Color::WHITE,
                 );
             }
         }
-        Ok(())
     }
 
     fn yaw_to_angle(yaw: f32) -> f32 {
@@ -191,18 +195,18 @@ mod systems {
     }
 
     pub(super) fn suspension(
-        mut bike_body_query: Query<
+        bike_body_query: Single<
             (&Transform, &ComputedCenterOfMass, &mut ExternalForce),
             With<CyberBikeBody>,
         >,
-        mut wheel_mesh_query: Query<
+        mut wheel_mesh_query: Populated<
             (&mut Transform, &mut WheelState, &WheelConfig, &CyberWheel),
             Without<CyberBikeBody>,
         >,
-        caster_query: Query<(&RayCaster, &RayHits, &CyberWheel)>,
+        caster_query: Populated<(&RayCaster, &RayHits, &CyberWheel)>,
         time: Res<Time>,
         mut gizmos: Gizmos,
-    ) -> Result {
+    ) {
         let dt = time.delta().as_secs_f32();
 
         let mut front_caster = &RayCaster::default();
@@ -224,7 +228,7 @@ mod systems {
             }
         }
 
-        let (bike_xform, com, mut bike_forces) = bike_body_query.single_mut()?;
+        let (bike_xform, com, mut bike_forces) = bike_body_query.into_inner();
 
         for (mut xform, mut state, config, wheel) in wheel_mesh_query.iter_mut() {
             let (caster, hits) = match wheel {
@@ -266,11 +270,10 @@ mod systems {
                 state.reset();
             }
         }
-        Ok(())
     }
 
     pub(super) fn steering(
-        mut bike_query: Query<
+        bike_query: Single<
             (
                 &Transform,
                 &LinearVelocity,
@@ -279,12 +282,12 @@ mod systems {
             ),
             With<CyberBikeBody>,
         >,
-        mut wheels: Query<(&mut WheelState, &WheelConfig, &CyberWheel)>,
+        mut wheels: Populated<(&mut WheelState, &WheelConfig, &CyberWheel)>,
         time: Res<Time>,
         input: Res<InputState>,
         mut gizmos: Gizmos,
-    ) -> Result {
-        let (bike_xform, bike_vel, mut bike_force, bike_body) = bike_query.single_mut()?;
+    ) {
+        let (bike_xform, bike_vel, mut bike_force, bike_body) = bike_query.into_inner();
         let bike_vel = bike_vel.0;
 
         let dt = time.delta().as_secs_f32();
@@ -346,12 +349,10 @@ mod systems {
                 );
             }
         }
-
-        Ok(())
     }
 
     pub(super) fn drag(
-        mut bike_query: Query<
+        bike_query: Single<
             (
                 &Transform,
                 &LinearVelocity,
@@ -362,8 +363,8 @@ mod systems {
         >,
         mut gizmos: Gizmos,
         time: Res<Time>,
-    ) -> Result {
-        let (xform, vel, com, mut force) = bike_query.single_mut()?;
+    ) {
+        let (xform, vel, com, mut force) = bike_query.into_inner();
 
         let dt = time.delta_secs();
 
@@ -386,12 +387,10 @@ mod systems {
             xform.translation + drag * 10.,
             Color::linear_rgb(1.0, 0.0, 0.0),
         );
-
-        Ok(())
     }
 
     pub(super) fn tweak(
-        mut config: Query<&mut WheelConfig>,
+        mut config: Populated<&mut WheelConfig>,
         mut keys: ResMut<ButtonInput<KeyCode>>,
     ) {
         let keyset: std::collections::HashSet<_> = keys.get_pressed().collect();