working damping/suspension

src/bike.rs

@@ -5,12 +5,15 @@ use bevy::prelude::*;
 
 use crate::physics::CatControllerState;
 
-pub const REST_DISTANCE: f32 = 0.8;
+pub const REST_DISTANCE: f32 = 1.0;
+pub const WHEEL_RADIUS: f32 = 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);
 
 #[derive(Component)]
 pub struct CyberBikeBody;
 
-#[derive(Component)]
+#[derive(Component, Clone, Copy, Debug)]
 pub enum CyberWheel {
     Front,
     Rear,
@@ -24,19 +27,27 @@ pub struct Steering;
 #[derive(Component)]
 pub struct Rearing;
 
+#[derive(Debug, Component, Clone, Default)]
+pub struct PreviousDisplacement(pub f32);
+
 #[derive(Debug, Resource, Reflect)]
+#[reflect(Resource)]
 pub struct SuspensionConfig {
     pub rest_dist: f32,
     pub konstant: f32,
     pub damping: f32,
+    pub front_attach: Vec3,
+    pub rear_attach: Vec3,
 }
 
 impl Default for SuspensionConfig {
     fn default() -> Self {
         Self {
             rest_dist: REST_DISTANCE,
-            konstant: 150.0,
-            damping: 50.0,
+            konstant: 800.0,
+            damping: -100.0,
+            front_attach: FRONT_ATTACH,
+            rear_attach: REAR_ATTACH,
         }
     }
 }
@@ -46,7 +57,7 @@ fn spawn_bike(
     mut meshes: ResMut<Assets<Mesh>>,
     mut materials: ResMut<Assets<StandardMaterial>>,
 ) {
-    let pos = Vec3::new(0.0, 24.0, 0.0);
+    let pos = Vec3::new(0.0, 14.0, 0.0);
     let xform = Transform::from_translation(pos); //.with_rotation(Quat::from_rotation_z(0.0));
 
     let body_collider =
@@ -78,10 +89,8 @@ fn spawn_bike(
                 MeshMaterial3d(materials.add(color)),
             );
             builder.spawn(pbr_bundle);
-            spawn_wheels(builder, meshes, materials, xform, builder.parent_entity());
+            spawn_wheels(builder, meshes, materials, builder.parent_entity());
         });
-
-    //spawn_wheels(commands, meshes, materials, xform, bike);
 }
 
 fn spawn_caster(
@@ -92,10 +101,10 @@ fn spawn_caster(
     parent: Entity,
     is_front: bool,
 ) {
-    let caster = ShapeCaster::new(collider, xform.translation, Quat::IDENTITY, direction)
+    let caster = ShapeCaster::new(collider, Vec3::ZERO, Quat::IDENTITY, direction)
         .with_query_filter(SpatialQueryFilter::from_excluded_entities([parent]));
 
-    let mut entity = commands.spawn((caster, Transform::default()));
+    let mut entity = commands.spawn((caster, xform, PreviousDisplacement(REST_DISTANCE)));
     if is_front {
         entity.insert((Steering, Name::new("front wheel")));
     } else {
@@ -107,19 +116,17 @@ fn spawn_wheels(
     commands: &mut ChildBuilder,
     mut meshes: ResMut<Assets<Mesh>>,
     mut materials: ResMut<Assets<StandardMaterial>>,
-    xform: Transform,
     body: Entity,
 ) {
     let (mesh, collider) = gen_tire();
 
     let front_rake = Vec3::new(0.0, -1.0, -0.57).normalize(); // about 30 degrees
-    let front_attach = xform.translation + xform.forward() * 0.7;
-    let front_wheel_pos = front_attach + (front_rake * REST_DISTANCE);
+    let front_wheel_pos = FRONT_ATTACH + (front_rake * REST_DISTANCE);
 
     spawn_caster(
         commands,
         collider.clone(),
-        Transform::from_translation(front_attach),
+        Transform::from_translation(FRONT_ATTACH),
         Dir3::new_unchecked(front_rake),
         body,
         true,
@@ -134,13 +141,13 @@ fn spawn_wheels(
     );
 
     let rear_rake = Vec3::new(0.0, -1.0, 0.57).normalize();
-    let rear_attach = xform.translation + xform.back() * 0.7;
-    let rear_wheel_pos = rear_attach + (rear_rake * REST_DISTANCE);
+    //let rear_attach = xform.translation + xform.back() * 0.7;
+    let rear_wheel_pos = REAR_ATTACH + (rear_rake * REST_DISTANCE);
 
     spawn_caster(
         commands,
         collider,
-        Transform::from_translation(rear_attach),
+        Transform::from_translation(REAR_ATTACH),
         Dir3::new_unchecked(rear_rake),
         body,
         false,
@@ -160,8 +167,8 @@ fn spawn_wheels(
 //-************************************************************************
 
 fn gen_tire() -> (Mesh, Collider) {
-    let tire_mesh: Mesh = Sphere::new(0.4).into();
-    let collider = Collider::sphere(0.4);
+    let tire_mesh: Mesh = Sphere::new(WHEEL_RADIUS).into();
+    let collider = Collider::sphere(WHEEL_RADIUS);
 
     (tire_mesh, collider)
 }
@@ -188,6 +195,7 @@ fn wheel_mesh(
         Mesh3d(meshes.add(tire_mesh)),
         MeshMaterial3d(materials.add(wheel_material.clone())),
         xform,
+        PreviousDisplacement::default(),
         wheel,
     ));
 }
@@ -197,6 +205,7 @@ pub struct CyberBikePlugin;
 impl Plugin for CyberBikePlugin {
     fn build(&self, app: &mut App) {
         app.init_resource::<SuspensionConfig>();
+        app.register_type::<SuspensionConfig>();
         app.add_systems(Startup, spawn_bike);
     }
 }

src/physics.rs

@@ -52,10 +52,6 @@ impl CatControllerState {
         self.roll_prev = error;
         (derivative, self.roll_integral)
     }
-
-    pub fn set_integral_limits(&mut self, roll: f32) {
-        self.roll_limit = roll;
-    }
 }
 
 mod systems {
@@ -65,16 +61,12 @@ mod systems {
     use bevy::prelude::*;
 
     use super::{CatControllerSettings, CatControllerState, CyberLean};
-    use crate::{bike::CyberBikeBody, input::InputState};
-
-    fn _yaw_to_angle(yaw: f32) -> f32 {
-        let v = yaw.powi(5) * FRAC_PI_3;
-        if v.is_normal() {
-            v
-        } else {
-            0.0
-        }
-    }
+    use crate::{
+        bike::{
+            CyberBikeBody, CyberWheel, PreviousDisplacement, Rearing, Steering, SuspensionConfig,
+        },
+        input::InputState,
+    };
 
     fn rotate_point(pt: &Vec3, rot: &Quat) -> Vec3 {
         // thanks to https://danceswithcode.net/engineeringnotes/quaternions/quaternions.html
@@ -166,9 +158,103 @@ mod systems {
             0.0
         }
     }
+
+    pub(super) fn suspension(
+        mut bike_body_query: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
+        mut wheel_mesh_query: Query<(&mut Transform, &CyberWheel), Without<CyberBikeBody>>,
+        mut front_wheel_query: Query<
+            (&ShapeCaster, &ShapeHits, &mut PreviousDisplacement),
+            (With<Steering>, Without<Rearing>),
+        >,
+        mut rear_wheel_query: Query<
+            (&ShapeCaster, &ShapeHits, &mut PreviousDisplacement),
+            (With<Rearing>, Without<Steering>),
+        >,
+        config: Res<SuspensionConfig>,
+        time: Res<Time>,
+    ) {
+        let dt = time.delta().as_secs_f32();
+        let mut front_wheel_xform = None;
+        let mut rear_wheel_xform = None;
+        for (xform, wheel) in wheel_mesh_query.iter_mut() {
+            match wheel {
+                CyberWheel::Front => {
+                    front_wheel_xform = Some(xform);
+                }
+                CyberWheel::Rear => {
+                    rear_wheel_xform = Some(xform);
+                }
+            }
+        }
+
+        let (bike_xform, mut bike_forces) = bike_body_query.single_mut();
+
+        for (caster, hits, mut prev) in front_wheel_query.iter_mut() {
+            for hit in hits.iter() {
+                if let Some(ref mut xform) = front_wheel_xform {
+                    let force =
+                        suspension_force(caster, hit, &config, &mut prev.0, dt, xform, true);
+                    bike_forces.apply_force_at_point(
+                        force,
+                        caster.global_origin(),
+                        bike_xform.translation,
+                    );
+                }
+            }
+        }
+
+        for (caster, hits, mut prev) in rear_wheel_query.iter_mut() {
+            for hit in hits.iter() {
+                if let Some(ref mut xform) = rear_wheel_xform {
+                    let force =
+                        suspension_force(caster, hit, &config, &mut prev.0, dt, xform, false);
+                    bike_forces.apply_force_at_point(
+                        force,
+                        caster.global_origin(),
+                        bike_xform.translation,
+                    );
+                }
+            }
+        }
+    }
+
+    fn suspension_force(
+        caster: &ShapeCaster,
+        hit: &ShapeHitData,
+        config: &SuspensionConfig,
+        previous_dispacement: &mut f32,
+        dt: f32,
+        wheel_xform: &mut Transform,
+        is_front: bool,
+    ) -> Vec3 {
+        let mut up_force = Vec3::ZERO;
+        let attach = if is_front {
+            config.front_attach
+        } else {
+            config.rear_attach
+        };
+        let dist = hit.distance;
+        let cdir = caster.direction.as_vec3();
+        let dir = caster.global_direction().as_vec3();
+        let loc = if dist < config.rest_dist {
+            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;
+            attach + (cdir * dist)
+        } else {
+            attach + (cdir * config.rest_dist)
+        };
+
+        wheel_xform.translation = loc;
+
+        up_force
+    }
 }
 
-use systems::{apply_lean, calculate_lean};
+use systems::{apply_lean, calculate_lean, suspension};
 
 pub struct CyberPhysicsPlugin;
 
@@ -181,6 +267,6 @@ impl Plugin for CyberPhysicsPlugin {
             .register_type::<CyberLean>()
             .add_plugins((PhysicsPlugins::default(), PhysicsDebugPlugin::default()))
             .insert_resource(SubstepCount(12))
-            .add_systems(Update, (apply_lean, calculate_lean));
+            .add_systems(Update, (apply_lean, calculate_lean, suspension));
     }
 }