abandoning failed attempt

Cargo.toml

@@ -10,9 +10,7 @@ bevy = { version = "0.16", features = ["bevy_dev_tools", "configurable_error_han
 [dependencies.avian3d]
 default-features = false
 version = "0.3"
-# git = "https://github.com/Jondolf/avian"
-# branch = "main"
-features = ["3d", "f32", "parry-f32", "debug-plugin", "default-collider", "collider-from-mesh"]
+features = ["3d", "parry-f32", "debug-plugin", "collider-from-mesh", "diagnostic_ui"]
 
 
 [features]

src/bike.rs

@@ -21,10 +21,10 @@ use crate::physics::CatControllerState;
 pub const SPRING_CONSTANT: Scalar = 40.0;
 pub const DAMPING_CONSTANT: Scalar = 10.0;
 pub const WHEEL_RADIUS: Scalar = 0.4;
-pub const REST_DISTANCE: Scalar = 1.5 + WHEEL_RADIUS;
+pub const REST_DISTANCE: Scalar = 1.0 + WHEEL_RADIUS;
 pub const FRICTION_COEFF: Scalar = 0.9;
-pub const FRONT_ATTACH: Vec3 = Vec3::new(0.0, 0.0, -0.5);
-pub const REAR_ATTACH: Vec3 = Vec3::new(0.0, 0.0, 0.5);
+pub const FRONT_ATTACH: Vec3 = Vec3::new(0.0, 0.0, -1.5);
+pub const REAR_ATTACH: Vec3 = Vec3::new(0.0, 0.0, 1.5);
 
 #[derive(Component)]
 pub struct CyberBikeBody;
@@ -141,9 +141,9 @@ fn spawn_children(
     let mesh = meshes.add(mesh);
     let material = materials.add(wheel_material);
 
-    let front_rake = Vec3::new(0.0, -1.0, -0.9).normalize(); // about 30 degrees
+    let front_rake = Vec3::new(0.0, -1.0, 0.0); // about 30 degrees
     let front_wheel_pos = FRONT_ATTACH + (front_rake * REST_DISTANCE);
-    let rear_rake = Vec3::new(0.0, -1.0, 0.9).normalize();
+    let rear_rake = Vec3::new(0.0, -1.0, 0.0);
     let rear_wheel_pos = REAR_ATTACH + (rear_rake * REST_DISTANCE);
 
     children![
@@ -161,7 +161,7 @@ fn spawn_children(
             WheelConfig::new(
                 FRONT_ATTACH,
                 REST_DISTANCE,
-                SPRING_CONSTANT - 5.0,
+                SPRING_CONSTANT - 25.0,
                 DAMPING_CONSTANT,
                 FRICTION_COEFF,
                 WHEEL_RADIUS,

src/physics.rs

@@ -4,25 +4,24 @@ use bevy::prelude::{
 };
 
 const DRAG_COEFF: Scalar = 0.00001;
-const MAX_THRUST: Scalar = 900.0;
 
 #[derive(Resource, Default, Debug)]
 struct CyberLean {
-    pub lean: Scalar,
+    pub lean: f32,
 }
 
 #[derive(Debug, Resource)]
 pub struct CatControllerSettings {
-    pub kp: Scalar,
-    pub kd: Scalar,
-    pub ki: Scalar,
+    pub kp: f32,
+    pub kd: f32,
+    pub ki: f32,
 }
 
 impl Default for CatControllerSettings {
     fn default() -> Self {
         Self {
-            kp: 60.0,
-            kd: 30.0,
+            kp: 40.0,
+            kd: 14.0,
             ki: 2.0,
         }
     }
@@ -30,9 +29,9 @@ impl Default for CatControllerSettings {
 
 #[derive(Component, Debug, Clone, Copy)]
 pub struct CatControllerState {
-    pub roll_integral: Scalar,
-    pub roll_prev: Scalar,
-    roll_limit: Scalar,
+    pub roll_integral: f32,
+    pub roll_prev: f32,
+    roll_limit: f32,
 }
 
 impl Default for CatControllerState {
@@ -46,7 +45,8 @@ impl Default for CatControllerState {
 }
 
 impl CatControllerState {
-    pub fn update_roll(&mut self, error: Scalar, dt: Scalar) -> (Scalar, Scalar) {
+    pub fn update_roll(&mut self, error: f32, dt: f32) -> (f32, f32) {
+        dbg!(error);
         let lim = self.roll_limit;
         self.roll_integral = (self.roll_integral + (error * dt)).min(lim).max(-lim);
         let derivative = (error - self.roll_prev) / dt;
@@ -56,30 +56,26 @@ impl CatControllerState {
 }
 
 mod systems {
-    use avian3d::{
-        math::{Scalar, FRAC_PI_2, PI},
-        prelude::{
-            ComputedCenterOfMass, ExternalForce, ExternalTorque, Gravity, LinearVelocity,
-            RayCaster, RayHits, RigidBodyQueryReadOnly,
-        },
+    use std::f32::consts::FRAC_PI_4;
+
+    use avian3d::prelude::{
+        AngularVelocity, Collider, ComputedCenterOfMass, ExternalForce, ExternalTorque, Gravity,
+        LinearVelocity, PhysicsDiagnosticsUiSettings, RayCaster, RayHits, RigidBodyQueryReadOnly,
     };
     use bevy::{
-        ecs::system::{Populated, Single},
+        ecs::system::{Populated, Query, Single},
         prelude::{
             ButtonInput, Color, Gizmos, GlobalTransform, KeyCode, Quat, Res, ResMut, Time,
             Transform, Vec, Vec3, With, Without,
         },
     };
 
-    use super::{CatControllerSettings, CatControllerState, CyberLean, DRAG_COEFF, MAX_THRUST};
+    use super::{CatControllerSettings, CatControllerState, CyberLean, DRAG_COEFF};
     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();
@@ -92,60 +88,80 @@ mod systems {
     }
 
     pub(super) fn calculate_lean(
-        bike_state: Single<(&LinearVelocity, &Transform), With<CyberBikeBody>>,
-        wheels: Populated<&GlobalTransform, With<CyberWheel>>,
-        input: Res<InputState>,
+        velocity: Single<&LinearVelocity, With<CyberBikeBody>>,
+        wheels: Populated<&GlobalTransform, (With<Collider>, With<CyberWheel>)>,
         gravity: Res<Gravity>,
+        input: Res<InputState>,
         mut lean: ResMut<CyberLean>,
     ) {
-        let mut wheels = wheels.iter();
-        let w1 = wheels.next().unwrap();
-        let w2 = wheels.next().unwrap();
-        let base = (w1.translation() - w2.translation()).length().abs();
-        let (velocity, xform) = bike_state.into_inner();
-        let vel = velocity.dot(*xform.forward());
-        let v_squared = vel.powi(2);
+        let d: GlobalTransform = Default::default();
+        let mut w = [&d; 2];
+        for (i, xform) in wheels.iter().enumerate() {
+            w[i] = xform;
+        }
+        let wheelbase = w[0].translation() - w[1].translation();
+        let base = wheelbase.length();
+
         let steering_angle = yaw_to_angle(input.yaw);
         let radius = base / steering_angle.tan();
-        let gravity = gravity.0.length();
-        let v2_r = v_squared / radius;
-        let tan_theta = (v2_r / gravity).clamp(-FRAC_PI_3, FRAC_PI_3);
 
-        if tan_theta.is_normal() || tan_theta == 0.0 {
-            lean.lean = tan_theta.atan().clamp(-FRAC_PI_3, FRAC_PI_3);
-        } else if lean.lean.abs() > 0.01 {
-            lean.lean *= 0.5;
+        let lin_vel = velocity.into_inner();
+
+        let right = wheelbase.cross(gravity.0).normalize();
+        let up = right.cross(wheelbase).normalize();
+
+        let gdir = gravity.0.normalize();
+        let up = up * -gdir.dot(up).signum();
+
+        let gravity = gravity.0.dot(up);
+        let tan_theta = lin_vel.length_squared() / (radius * gravity);
+
+        if tan_theta.is_normal() {
+            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)>,
-        wheels: Populated<(&WheelState, &GlobalTransform, &CyberWheel)>,
+        wheels: Populated<(&WheelState, &CyberWheel)>,
         time: Res<Time>,
         settings: Res<CatControllerSettings>,
         lean: Res<CyberLean>,
         mut gizmos: Gizmos,
     ) {
-        let (xform, mut torque, mut control_vars) = bike_query.into_inner();
+        let (xform, mut force, mut control_vars) = bike_query.into_inner();
 
         let mut factor = 1.0;
-        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 mut front = None;
+        let mut rear = None;
+        let mut normal = None;
+        for (state, wheel) in wheels.iter() {
+            if state.contact_point.is_none() {
+                factor -= 0.5;
+            } else {
+                normal = Some(state.contact_normal.unwrap());
+                match wheel {
+                    CyberWheel::Front => {
+                        front = Some(state.contact_point.unwrap());
+                    }
+                    CyberWheel::Rear => {
+                        rear = Some(state.contact_point.unwrap());
+                    }
                 }
             }
         }
 
-        let tork_axis = (rxform.translation() - fxform.translation()).normalize();
+        let axis = match (front, rear) {
+            (Some(f), Some(r)) => (r - f).normalize(),
+            (None, Some(_)) | (Some(_), None) => {
+                let normal = normal.unwrap();
+                normal.cross(*xform.left())
+            }
+            _ => return,
+        };
 
         let world_up = Vec3::Y; //xform.translation.normalize();
         let rot = Quat::from_axis_angle(*xform.back(), lean.lean);
@@ -176,10 +192,9 @@ 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();
-                let tork = factor * mag * tork_axis;
+                let lean_torq = factor * mag * axis;
 
-                torque.apply_torque(tork);
+                force.apply_torque(lean_torq);
 
                 gizmos.arrow(
                     xform.translation + *xform.up(),
@@ -190,7 +205,7 @@ mod systems {
         }
     }
 
-    fn yaw_to_angle(yaw: Scalar) -> Scalar {
+    fn yaw_to_angle(yaw: f32) -> f32 {
         let v = yaw.powi(5) * FRAC_PI_4;
         if v.is_normal() {
             v
@@ -212,7 +227,7 @@ mod systems {
         time: Res<Time>,
         mut gizmos: Gizmos,
     ) {
-        let dt = time.delta().as_secs_f64() as Scalar;
+        let dt = time.delta().as_secs_f32();
 
         let mut front_caster = &RayCaster::default();
         let mut rear_caster = &RayCaster::default();
@@ -291,7 +306,8 @@ 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_f64() as Scalar;
+        let dt = time.delta().as_secs_f32();
+        let max_thrust = 8000.0;
         let yaw_angle = -yaw_to_angle(input.yaw);
 
         for (mut state, config, wheel) in wheels.iter_mut() {
@@ -302,10 +318,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 * 0.5),
-                    CyberWheel::Front => (rot * forward, thrust_mag * 0.5),
+                    CyberWheel::Rear => (forward, thrust_mag),
+                    CyberWheel::Front => (rot * forward, 0.0),
                 };
 
                 let thrust = thrust_force * thrust_dir;
@@ -322,15 +338,14 @@ mod systems {
                 let diff = bike_vel - vel;
                 bevy::log::debug!("{wheel:?}: vel diff: {diff:?} ({})", diff.length(),);
 
-                let mut force = thrust + friction;
-                force *= dt * 50.0;
-                let force_mag = force.length();
-                if force_mag > max_force_mag {
-                    state.sliding = true;
-                    force = force.normalize_or_zero() * max_force_mag;
-                } else {
-                    state.sliding = false;
-                }
+                let force = thrust + friction * dt;
+                // let force_mag = force.length();
+                // if force_mag > max_force_mag {
+                //     state.sliding = true;
+                //     force = force.normalize_or_zero() * max_force_mag;
+                // } else {
+                //     state.sliding = false;
+                // }
 
                 bike_force.apply_force_at_point(
                     force,
@@ -380,8 +395,6 @@ mod systems {
             );
         }
 
-        bevy::log::debug!("speed: {}, drag force: {}", vel.length(), drag.length());
-
         gizmos.arrow(
             xform.translation,
             xform.translation + drag * 10.,
@@ -389,37 +402,58 @@ mod systems {
         );
     }
 
+    pub(super) fn ef(
+        bike: Single<(&ExternalForce, &GlobalTransform), With<CyberBikeBody>>,
+        mut gizmos: Gizmos,
+    ) {
+        let (force, xform) = bike.into_inner();
+
+        let start = xform.translation() + (2.0 * Vec3::Y);
+        let force = force.length() * force.normalize_or_zero();
+        gizmos.arrow(start, start + force, Color::linear_rgb(1.0, 0.0, 0.0));
+    }
+
     pub(super) fn tweak(
-        mut config: Populated<&mut WheelConfig>,
+        mut config: ResMut<CatControllerSettings>,
         mut keys: ResMut<ButtonInput<KeyCode>>,
+        mut ui: ResMut<PhysicsDiagnosticsUiSettings>,
     ) {
         let keyset: std::collections::HashSet<_> = keys.get_pressed().collect();
         let shifted = keyset.contains(&KeyCode::ShiftLeft) || keyset.contains(&KeyCode::ShiftRight);
-        let config = config.iter_mut();
-        for ref mut c in config {
-            for key in &keyset {
-                match key {
-                    KeyCode::KeyS => {
-                        if shifted {
-                            c.konstant += 0.2;
-                        } else {
-                            c.konstant -= 0.2;
-                        }
-                        bevy::log::info!(c.konstant);
-                    }
-                    KeyCode::KeyD => {
-                        if shifted {
-                            c.damping += 0.1;
-                        } else {
-                            c.damping -= 0.1;
-                        }
-                        bevy::log::info!(c.damping);
-                    }
 
-                    _ => continue,
+        for key in &keyset {
+            match key {
+                KeyCode::KeyP => {
+                    if shifted {
+                        config.kp += 1.0;
+                    } else {
+                        config.kp -= 1.0;
+                    }
                 }
+                KeyCode::KeyD => {
+                    if shifted {
+                        config.kd += 1.0;
+                    } else {
+                        config.kd -= 1.0;
+                    }
+                }
+                KeyCode::KeyI => {
+                    if shifted {
+                        config.ki += 1.0;
+                    } else {
+                        config.ki -= 1.0;
+                    }
+                }
+                KeyCode::KeyU => {
+                    let enabled = ui.enabled;
+                    ui.enabled = !enabled;
+                }
+
+                _ => continue,
             }
         }
+        bevy::log::info!(?config);
+
         let released: Vec<_> = keys.get_just_released().copied().collect();
         for key in released {
             keys.clear_just_released(key);
@@ -427,7 +461,7 @@ mod systems {
     }
 }
 
-use systems::{apply_lean, calculate_lean, drag, suspension, tweak, wheel_action};
+use systems::{apply_lean, calculate_lean, drag, ef, suspension, tweak, wheel_action};
 
 pub struct CyberPhysicsPlugin;
 
@@ -435,8 +469,12 @@ impl Plugin for CyberPhysicsPlugin {
     fn build(&self, app: &mut App) {
         app.init_resource::<CatControllerSettings>()
             .init_resource::<CyberLean>()
-            .add_plugins((PhysicsPlugins::default(), PhysicsDebugPlugin::default()))
-            .insert_resource(SubstepCount(12))
+            .add_plugins((
+                PhysicsPlugins::default(),
+                PhysicsDebugPlugin::default(),
+                PhysicsDiagnosticsUiPlugin,
+            ))
+            .insert_resource(SubstepCount(24))
             .add_systems(Startup, |mut gravity: ResMut<Gravity>| {
                 gravity.0 *= 1.0;
             })
@@ -446,7 +484,9 @@ impl Plugin for CyberPhysicsPlugin {
                     calculate_lean,
                     apply_lean,
                     suspension,
-                    wheel_action, /* drag */
+                    wheel_action,
+                    drag,
+                    ef,
                 )
                     .chain(),
             )