add tweak system for tuning suspension at runtime

src/bike.rs

@@ -6,8 +6,10 @@ use bevy::prelude::*;
 
 use crate::physics::CatControllerState;
 
-pub const REST_DISTANCE: f32 = 1.0;
+pub const REST_DISTANCE: Scalar = 1.0;
-pub const WHEEL_RADIUS: f32 = 0.4;
+pub const SPRING_CONSTANT: Scalar = 50.0;
+pub const DAMPING_CONSTANT: Scalar = 3.0;
+pub const WHEEL_RADIUS: Scalar = 0.4;
 pub const FRONT_ATTACH: Vec3 = Vec3::new(0.0, 0.0, -0.7);
 pub const REAR_ATTACH: Vec3 = Vec3::new(0.0, 0.0, 0.7);
 
@@ -38,6 +40,7 @@ pub struct WheelConfig {
     pub konstant: Scalar,
     pub damping: Scalar,
     pub friction: Scalar,
+    pub radius: Scalar,
 }
 
 impl WheelConfig {
@@ -47,6 +50,7 @@ impl WheelConfig {
         konstant: Scalar,
         damping: Scalar,
         friction: Scalar,
+        radius: Scalar,
     ) -> Self {
         WheelConfig {
             attach,
@@ -54,6 +58,7 @@ impl WheelConfig {
             konstant,
             damping,
             friction,
+            radius,
         }
     }
 }
@@ -79,9 +84,11 @@ fn spawn_bike(
             SleepingDisabled,
             CyberBikeBody,
             CatControllerState::default(),
-            ColliderDensity(20.0),
+            ColliderDensity(0.5),
-            LinearVelocity::ZERO,
+            AngularDamping(0.9),
-            AngularVelocity::ZERO,
+            LinearDamping(0.2),
+            // LinearVelocity::ZERO,
+            // AngularVelocity::ZERO,
             ExternalForce::ZERO.with_persistence(false),
             ExternalTorque::ZERO.with_persistence(false),
         ))
@@ -114,7 +121,7 @@ fn spawn_wheels(
     wheel_caster(
         commands,
         collider.clone(),
-        Transform::from_translation(FRONT_ATTACH),
+        FRONT_ATTACH,
         Dir3::new_unchecked(front_rake),
         body,
         REST_DISTANCE,
@@ -126,7 +133,14 @@ fn spawn_wheels(
         &mut materials,
         front_wheel_pos,
         mesh.clone(),
-        WheelConfig::new(FRONT_ATTACH, REST_DISTANCE, 800., -160., 0.5),
+        WheelConfig::new(
+            FRONT_ATTACH,
+            REST_DISTANCE,
+            SPRING_CONSTANT,
+            DAMPING_CONSTANT,
+            0.5,
+            WHEEL_RADIUS,
+        ),
         CyberWheel::Front,
     );
 
@@ -136,7 +150,7 @@ fn spawn_wheels(
     wheel_caster(
         commands,
         collider,
-        Transform::from_translation(REAR_ATTACH),
+        REAR_ATTACH,
         Dir3::new_unchecked(rear_rake),
         body,
         REST_DISTANCE,
@@ -148,7 +162,14 @@ fn spawn_wheels(
         &mut materials,
         rear_wheel_pos,
         mesh,
-        WheelConfig::new(REAR_ATTACH, REST_DISTANCE, 800., -160., 0.5),
+        WheelConfig::new(
+            REAR_ATTACH,
+            REST_DISTANCE,
+            SPRING_CONSTANT,
+            DAMPING_CONSTANT,
+            0.5,
+            WHEEL_RADIUS,
+        ),
         CyberWheel::Rear,
     );
 }
@@ -160,14 +181,15 @@ fn spawn_wheels(
 fn wheel_caster(
     commands: &mut ChildBuilder,
     collider: Collider,
-    xform: Transform,
+    origin: Vec3,
     direction: Dir3,
     parent: Entity,
     rest_dist: Scalar,
     wheel: CyberWheel,
 ) {
-    let caster = ShapeCaster::new(collider, xform.translation, Quat::IDENTITY, direction)
+    let caster = ShapeCaster::new(collider, origin, Quat::IDENTITY, direction)
         .with_max_distance(rest_dist)
+        .with_max_hits(1)
         .with_query_filter(SpatialQueryFilter::from_excluded_entities([parent]));
 
     commands.spawn((caster, wheel));
@@ -191,12 +213,18 @@ fn wheel_mesh(
 
     let xform = Transform::from_translation(position);
 
+    let name = match wheel {
+        CyberWheel::Front => "front tire",
+        CyberWheel::Rear => "rear tire",
+    };
+
     commands.spawn((
-        Name::new("tire"),
+        Name::new(name),
         config,
         Mesh3d(meshes.add(tire_mesh)),
         MeshMaterial3d(materials.add(wheel_material.clone())),
         xform,
+        TransformInterpolation,
         wheel,
     ));
 }
@@ -205,6 +233,8 @@ pub struct CyberBikePlugin;
 
 impl Plugin for CyberBikePlugin {
     fn build(&self, app: &mut App) {
+        app.register_type::<WheelConfig>();
+        app.register_type::<WheelState>();
         app.add_systems(Startup, spawn_bike);
     }
 }

src/camera.rs

@@ -56,14 +56,9 @@ fn update_camera_pos(mut offset: ResMut<DebugCamOffset>, mut keys: ResMut<Button
         }
     }
 
-    let just_released: HashSet<_> = keys.get_just_released().cloned().collect();
+    let released: Vec<_> = keys.get_just_released().copied().collect();
-    if !just_released.is_empty() {
+    for key in released {
-        let released_shift = just_released.contains(&KeyCode::ShiftLeft)
+        keys.clear_just_released(key);
-            || just_released.contains(&KeyCode::ShiftRight);
-        keys.reset_all();
-        if !released_shift && shifted {
-            keys.press(KeyCode::ShiftLeft);
-        }
     }
 }
 

src/physics.rs

@@ -18,9 +18,9 @@ pub struct CatControllerSettings {
 impl Default for CatControllerSettings {
     fn default() -> Self {
         Self {
-            kp: 1200.0,
+            kp: 30.0,
-            kd: 10.0,
+            kd: 7.0,
-            ki: 50.0,
+            ki: 1.0,
         }
     }
 }
@@ -58,7 +58,7 @@ mod systems {
     use std::f32::consts::{FRAC_PI_3, FRAC_PI_4};
 
     use avian3d::{math::Scalar, prelude::*};
-    use bevy::{color, math::VectorSpace, prelude::*};
+    use bevy::prelude::*;
 
     use super::{CatControllerSettings, CatControllerState, CyberLean};
     use crate::{
@@ -79,23 +79,28 @@ mod systems {
 
     pub(super) fn calculate_lean(
         bike_state: Query<(&LinearVelocity, &Transform), With<CyberBikeBody>>,
+        wheels: Query<&GlobalTransform, With<CyberWheel>>,
         input: Res<InputState>,
+        gravity: Res<Gravity>,
         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.single();
         let vel = velocity.dot(*xform.forward());
         let v_squared = vel.powi(2);
         let steering_angle = yaw_to_angle(input.yaw);
-        let wheel_base = 1.145f32;
+        let radius = base / steering_angle.tan();
-        let radius = wheel_base / steering_angle.tan();
+        let gravity = gravity.0.length();
-        let gravity = -9.8f32;
         let v2_r = v_squared / radius;
         let tan_theta = (v2_r / gravity).clamp(-FRAC_PI_3, FRAC_PI_3);
 
         if tan_theta.is_normal() {
             lean.lean = -tan_theta.atan().clamp(-FRAC_PI_3, FRAC_PI_3);
         } else {
-            lean.lean = 0.0;
+            //lean.lean = 0.0;
         }
     }
 
@@ -210,19 +215,28 @@ mod systems {
 
             let prev = &mut state.displacement;
             if let Some(hit) = hits.iter().next() {
-                let force = suspension_force(caster, hit, config, prev, dt, &mut xform);
+                let mag = suspension_force(caster, hit, config, prev, dt, &mut xform);
+                let mag = mag.max(0.0);
+                //let fdir = -caster.global_direction().as_vec3();
+                let fdir = hit.normal1;
+                let force = fdir * mag;
+                //dbg!(fdir, force);
+                let p = hit.point1 + (hit.normal1 * config.radius);
+                gizmos.arrow(p, p + force, Color::linear_rgb(1., 0.5, 0.2));
+
                 bike_forces.apply_force_at_point(
                     force,
                     caster.global_origin(),
                     bike_xform.translation,
                 );
-                let vel = (global_xform.translation() - state.ppos) / dt;
+                //let vel = (global_xform.translation() - state.ppos) / dt;
-                dbg!(vel);
+
                 state.ppos = global_xform.translation();
                 let normal = hit.normal1;
                 let steering = match wheel {
                     CyberWheel::Front => normal.cross(*bike_xform.back()) * yaw,
-                    _ => normal.cross(*bike_xform.forward()) * yaw, // Vec3::ZERO,
+                    CyberWheel::Rear => Vec3::ZERO,
+                    //_ => normal.cross(*bike_xform.forward()) * yaw, // Vec3::ZERO,
                 };
                 let thrust = normal.cross(*bike_xform.right()) * thrust;
                 let total = (thrust + steering) * dt;
@@ -245,27 +259,57 @@ mod systems {
         previous_dispacement: &mut Scalar,
         dt: Scalar,
         wheel_xform: &mut Transform,
-    ) -> Vec3 {
+    ) -> Scalar {
-        let mut up_force = Vec3::ZERO;
         let dist = hit.distance;
         let cdir = caster.direction.as_vec3();
-        let dir = caster.global_direction().as_vec3();
+        wheel_xform.translation = config.attach + (cdir * dist);
-        let loc = {
-            let displacement = config.rest_dist - dist;
-            let damper_vel = (displacement - *previous_dispacement) / dt;
-            dbg!(damper_vel);
-            *previous_dispacement = displacement;
-            let mag = config.konstant * displacement - config.damping * damper_vel;
-            up_force = -dir * mag;
-            config.attach + (cdir * dist)
-        };
-        wheel_xform.translation = loc;
 
-        up_force
+        let displacement = config.rest_dist - dist;
+        let damper_vel = (*previous_dispacement - displacement) / dt;
+
+        *previous_dispacement = displacement;
+        config.konstant * displacement - config.damping * damper_vel
+    }
+
+    pub(super) fn tweak(
+        mut config: Query<&mut WheelConfig>,
+        mut keys: ResMut<ButtonInput<KeyCode>>,
+    ) {
+        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,
+                }
+            }
+        }
+        let released: Vec<_> = keys.get_just_released().copied().collect();
+        for key in released {
+            keys.clear_just_released(key);
+        }
     }
 }
 
-use systems::{apply_lean, calculate_lean, suspension};
+use systems::{apply_lean, calculate_lean, suspension, tweak};
 
 pub struct CyberPhysicsPlugin;
 
@@ -279,11 +323,12 @@ impl Plugin for CyberPhysicsPlugin {
             .add_plugins((PhysicsPlugins::default(), PhysicsDebugPlugin::default()))
             .insert_resource(SubstepCount(12))
             .add_systems(Startup, |mut gravity: ResMut<Gravity>| {
-                gravity.0 *= 0.02;
+                gravity.0 *= 1.0;
             })
             .add_systems(
                 FixedUpdate,
                 (calculate_lean, apply_lean, suspension).chain(),
-            );
+            )
+            .add_systems(Update, tweak);
     }
 }