enh

src/physics.rs

@@ -4,17 +4,18 @@ use bevy::prelude::{
 };
 
 const DRAG_COEFF: Scalar = 0.00001;
+const MAX_THRUST: Scalar = 900.0;
 
 #[derive(Resource, Default, Debug)]
 struct CyberLean {
-    pub lean: f32,
+    pub lean: Scalar,
 }
 
 #[derive(Debug, Resource)]
 pub struct CatControllerSettings {
-    pub kp: f32,
+    pub kp: Scalar,
-    pub kd: f32,
+    pub kd: Scalar,
-    pub ki: f32,
+    pub ki: Scalar,
 }
 
 impl Default for CatControllerSettings {
@@ -22,16 +23,16 @@ impl Default for CatControllerSettings {
         Self {
             kp: 60.0,
             kd: 30.0,
-            ki: 0.0,
+            ki: 2.0,
         }
     }
 }
 
 #[derive(Component, Debug, Clone, Copy)]
 pub struct CatControllerState {
-    pub roll_integral: f32,
+    pub roll_integral: Scalar,
-    pub roll_prev: f32,
+    pub roll_prev: Scalar,
-    roll_limit: f32,
+    roll_limit: Scalar,
 }
 
 impl Default for CatControllerState {
@@ -45,7 +46,7 @@ impl Default for CatControllerState {
 }
 
 impl CatControllerState {
-    pub fn update_roll(&mut self, error: f32, dt: f32) -> (f32, f32) {
+    pub fn update_roll(&mut self, error: Scalar, dt: Scalar) -> (Scalar, Scalar) {
         let lim = self.roll_limit;
         self.roll_integral = (self.roll_integral + (error * dt)).min(lim).max(-lim);
         let derivative = (error - self.roll_prev) / dt;
@@ -55,11 +56,12 @@ impl CatControllerState {
 }
 
 mod systems {
-    use std::f32::consts::{FRAC_PI_3, FRAC_PI_4};
+    use avian3d::{
-
+        math::{Scalar, FRAC_PI_2, PI},
-    use avian3d::prelude::{
+        prelude::{
-        ComputedCenterOfMass, ExternalForce, ExternalTorque, Gravity, LinearVelocity, RayCaster,
+            ComputedCenterOfMass, ExternalForce, ExternalTorque, Gravity, LinearVelocity,
-        RayHits, RigidBodyQueryReadOnly,
+            RayCaster, RayHits, RigidBodyQueryReadOnly,
+        },
     };
     use bevy::{
         ecs::system::{Populated, Single},
@@ -69,12 +71,15 @@ mod systems {
         },
     };
 
-    use super::{CatControllerSettings, CatControllerState, CyberLean, DRAG_COEFF};
+    use super::{CatControllerSettings, CatControllerState, CyberLean, DRAG_COEFF, MAX_THRUST};
     use crate::{
         bike::{CyberBikeBody, CyberWheel, WheelConfig, WheelState, WHEEL_RADIUS},
         input::InputState,
     };
 
+    const FRAC_PI_3: Scalar = PI / 3.0;
+    const FRAC_PI_4: Scalar = FRAC_PI_2 / 2.0;
+
     fn rotate_point(pt: &Vec3, rot: &Quat) -> Vec3 {
         // thanks to https://danceswithcode.net/engineeringnotes/quaternions/quaternions.html
         let [x, y, z] = pt.normalize().to_array();
@@ -106,9 +111,9 @@ mod systems {
         let v2_r = v_squared / radius;
         let tan_theta = (v2_r / gravity).clamp(-FRAC_PI_3, FRAC_PI_3);
 
-        if tan_theta.is_normal() {
+        if tan_theta.is_normal() || tan_theta == 0.0 {
             lean.lean = tan_theta.atan().clamp(-FRAC_PI_3, FRAC_PI_3);
-        } else {
+        } else if lean.lean.abs() > 0.01 {
             lean.lean *= 0.5;
         }
     }
@@ -185,7 +190,7 @@ mod systems {
         }
     }
 
-    fn yaw_to_angle(yaw: f32) -> f32 {
+    fn yaw_to_angle(yaw: Scalar) -> Scalar {
         let v = yaw.powi(5) * FRAC_PI_4;
         if v.is_normal() {
             v
@@ -207,7 +212,7 @@ mod systems {
         time: Res<Time>,
         mut gizmos: Gizmos,
     ) {
-        let dt = time.delta().as_secs_f32();
+        let dt = time.delta().as_secs_f64() as Scalar;
 
         let mut front_caster = &RayCaster::default();
         let mut rear_caster = &RayCaster::default();
@@ -286,8 +291,7 @@ mod systems {
         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();
+        let dt = time.delta().as_secs_f64() as Scalar;
-        let max_thrust = 500.0;
         let yaw_angle = -yaw_to_angle(input.yaw);
 
         for (mut state, config, wheel) in wheels.iter_mut() {
@@ -298,10 +302,10 @@ mod systems {
 
                 let rot = Quat::from_axis_angle(normal, yaw_angle);
                 let forward = normal.cross(*bike_xform.right());
-                let thrust_mag = input.throttle * max_thrust * dt;
+                let thrust_mag = input.throttle * MAX_THRUST * dt;
                 let (thrust_dir, thrust_force) = match wheel {
-                    CyberWheel::Rear => (forward, thrust_mag),
+                    CyberWheel::Rear => (forward, thrust_mag * 0.5),
-                    CyberWheel::Front => (rot * forward, thrust_mag * 0.1),
+                    CyberWheel::Front => (rot * forward, thrust_mag * 0.5),
                 };
 
                 let thrust = thrust_force * thrust_dir;
@@ -438,7 +442,13 @@ impl Plugin for CyberPhysicsPlugin {
             })
             .add_systems(
                 FixedUpdate,
-                (calculate_lean, apply_lean, suspension, wheel_action, drag).chain(),
+                (
+                    calculate_lean,
+                    apply_lean,
+                    suspension,
+                    wheel_action, /* drag */
+                )
+                    .chain(),
             )
             .add_systems(Update, tweak);
     }