src/bike.rs

@@ -1,7 +1,6 @@
-use avian3d::{
-    math::{Scalar, FRAC_PI_2},
-    prelude::*,
-};
+use std::f32::consts::FRAC_PI_2;
+
+use avian3d::prelude::*;
 use bevy::prelude::*;
 
 use crate::physics::CatControllerState;
@@ -20,40 +19,35 @@ pub enum CyberWheel {
     Rear,
 }
 
-#[derive(Component, Clone, Copy, Debug, Reflect, Default)]
-#[reflect(Component)]
-pub struct WheelState {
-    pub displacement: Scalar,
-    pub sliding: bool,
-    pub grounded: bool,
-    pub ppos: Vec3,
+// marker for front suspension joint
+#[derive(Component)]
+pub struct Steering;
+
+// marker for rear suspension joint
+#[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,
 }
 
-#[derive(Component, Clone, Copy, Debug, Reflect)]
-#[reflect(Component)]
-#[require(WheelState)]
-pub struct WheelConfig {
-    pub attach: Vec3,
-    pub rest_dist: Scalar,
-    pub konstant: Scalar,
-    pub damping: Scalar,
-    pub friction: Scalar,
-}
-
-impl WheelConfig {
-    fn new(
-        attach: Vec3,
-        rest_dist: Scalar,
-        konstant: Scalar,
-        damping: Scalar,
-        friction: Scalar,
-    ) -> Self {
-        WheelConfig {
-            attach,
-            rest_dist,
-            konstant,
-            damping,
-            friction,
+impl Default for SuspensionConfig {
+    fn default() -> Self {
+        Self {
+            rest_dist: REST_DISTANCE,
+            konstant: 1600.0,
+            damping: -160.0,
+            front_attach: FRONT_ATTACH,
+            rear_attach: REAR_ATTACH,
         }
     }
 }
@@ -99,26 +93,43 @@ fn spawn_bike(
         });
 }
 
+fn spawn_caster(
+    commands: &mut ChildBuilder,
+    collider: Collider,
+    xform: Transform,
+    direction: Dir3,
+    parent: Entity,
+    is_front: bool,
+) {
+    let caster = ShapeCaster::new(collider, Vec3::ZERO, Quat::IDENTITY, direction)
+        .with_query_filter(SpatialQueryFilter::from_excluded_entities([parent]));
+
+    let mut entity = commands.spawn((caster, xform, PreviousDisplacement(REST_DISTANCE)));
+    if is_front {
+        entity.insert((Steering, Name::new("front wheel")));
+    } else {
+        entity.insert((Rearing, Name::new("rear wheel")));
+    }
+}
+
 fn spawn_wheels(
     commands: &mut ChildBuilder,
     mut meshes: ResMut<Assets<Mesh>>,
     mut materials: ResMut<Assets<StandardMaterial>>,
     body: Entity,
 ) {
-    let mesh: Mesh = Sphere::new(WHEEL_RADIUS).into();
-    let collider = Collider::sphere(WHEEL_RADIUS);
+    let (mesh, collider) = gen_tire();
 
     let front_rake = Vec3::new(0.0, -1.0, -0.57).normalize(); // about 30 degrees
     let front_wheel_pos = FRONT_ATTACH + (front_rake * REST_DISTANCE);
 
-    wheel_caster(
+    spawn_caster(
         commands,
         collider.clone(),
         Transform::from_translation(FRONT_ATTACH),
         Dir3::new_unchecked(front_rake),
         body,
-        REST_DISTANCE,
-        CyberWheel::Front,
+        true,
     );
     wheel_mesh(
         commands,
@@ -126,21 +137,20 @@ fn spawn_wheels(
         &mut materials,
         front_wheel_pos,
         mesh.clone(),
-        WheelConfig::new(FRONT_ATTACH, REST_DISTANCE, 800., -160., 0.5),
         CyberWheel::Front,
     );
 
     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);
 
-    wheel_caster(
+    spawn_caster(
         commands,
         collider,
         Transform::from_translation(REAR_ATTACH),
         Dir3::new_unchecked(rear_rake),
         body,
-        REST_DISTANCE,
-        CyberWheel::Rear,
+        false,
     );
     wheel_mesh(
         commands,
@@ -148,7 +158,6 @@ fn spawn_wheels(
         &mut materials,
         rear_wheel_pos,
         mesh,
-        WheelConfig::new(REAR_ATTACH, REST_DISTANCE, 800., -160., 0.5),
         CyberWheel::Rear,
     );
 }
@@ -157,20 +166,11 @@ fn spawn_wheels(
 // helper fns for the wheels
 //-************************************************************************
 
-fn wheel_caster(
-    commands: &mut ChildBuilder,
-    collider: Collider,
-    xform: Transform,
-    direction: Dir3,
-    parent: Entity,
-    rest_dist: Scalar,
-    wheel: CyberWheel,
-) {
-    let caster = ShapeCaster::new(collider, xform.translation, Quat::IDENTITY, direction)
-        .with_max_distance(rest_dist)
-        .with_query_filter(SpatialQueryFilter::from_excluded_entities([parent]));
+fn gen_tire() -> (Mesh, Collider) {
+    let tire_mesh: Mesh = Sphere::new(WHEEL_RADIUS).into();
+    let collider = Collider::sphere(WHEEL_RADIUS);
 
-    commands.spawn((caster, wheel));
+    (tire_mesh, collider)
 }
 
 fn wheel_mesh(
@@ -179,7 +179,6 @@ fn wheel_mesh(
     materials: &mut ResMut<Assets<StandardMaterial>>,
     position: Vec3,
     tire_mesh: Mesh,
-    config: WheelConfig,
     wheel: CyberWheel,
 ) {
     let wheel_material = &StandardMaterial {
@@ -193,10 +192,10 @@ fn wheel_mesh(
 
     commands.spawn((
         Name::new("tire"),
-        config,
         Mesh3d(meshes.add(tire_mesh)),
         MeshMaterial3d(materials.add(wheel_material.clone())),
         xform,
+        PreviousDisplacement::default(),
         wheel,
     ));
 }
@@ -205,6 +204,8 @@ 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/main.rs

@@ -2,7 +2,7 @@ use avian3d::prelude::{
     Collider, ColliderDensity, CollisionLayers, CollisionMargin, Friction, PhysicsGizmos, RigidBody,
 };
 use bevy::{
-    color::{palettes::css::SILVER, Alpha},
+    color::palettes::css::SILVER,
     pbr::MeshMaterial3d,
     prelude::{
         default, App, AppGizmoBuilder, Assets, BuildChildren, ButtonInput, ChildBuild, Color,
@@ -60,7 +60,7 @@ fn ground_and_light(
         .spawn((
             spatial_bundle,
             RigidBody::Static,
-            Collider::cuboid(500.0, 5., 500.0),
+            Collider::cuboid(50.0, 5., 50.0),
             CollisionMargin(0.1),
             ColliderDensity(1000.0),
             CollisionLayers::ALL,
@@ -68,8 +68,8 @@ fn ground_and_light(
         ))
         .with_children(|p| {
             let bundle = (
-                Mesh3d(meshes.add(Plane3d::default().mesh().size(500.0, 500.0))),
-                MeshMaterial3d(materials.add(Color::from(SILVER).with_alpha(0.7))),
+                Mesh3d(meshes.add(Plane3d::default().mesh().size(50.0, 50.0))),
+                MeshMaterial3d(materials.add(Color::from(SILVER))),
                 Transform::from_xyz(0.0, 2.5, 0.0),
                 Visibility::Visible,
             );

src/physics.rs

@@ -57,12 +57,14 @@ impl CatControllerState {
 mod systems {
     use std::f32::consts::{FRAC_PI_3, FRAC_PI_4};
 
-    use avian3d::{math::Scalar, prelude::*};
-    use bevy::{color, math::VectorSpace, prelude::*};
+    use avian3d::prelude::*;
+    use bevy::prelude::*;
 
     use super::{CatControllerSettings, CatControllerState, CyberLean};
     use crate::{
-        bike::{CyberBikeBody, CyberWheel, WheelConfig, WheelState},
+        bike::{
+            CyberBikeBody, CyberWheel, PreviousDisplacement, Rearing, Steering, SuspensionConfig,
+        },
         input::InputState,
     };
 
@@ -141,7 +143,7 @@ mod systems {
                 torque.apply_torque(tork);
                 gizmos.arrow(
                     xform.translation + Vec3::Y,
-                    xform.translation + mag * *xform.right() + Vec3::Y,
+                    xform.translation + tork + Vec3::Y,
                     Color::WHITE,
                 );
             }
@@ -159,81 +161,76 @@ mod systems {
 
     pub(super) fn suspension(
         mut bike_body_query: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
-        mut wheel_mesh_query: Query<
-            (
-                &mut Transform,
-                &mut WheelState,
-                &GlobalTransform,
-                &WheelConfig,
-                &CyberWheel,
-            ),
-            Without<CyberBikeBody>,
+        mut wheel_mesh_query: Query<(&mut Transform, &CyberWheel), Without<CyberBikeBody>>,
+        mut front_wheel_query: Query<
+            (&ShapeCaster, &ShapeHits, &mut PreviousDisplacement),
+            (With<Steering>, Without<Rearing>),
         >,
-        caster_query: Query<(&ShapeCaster, &ShapeHits, &CyberWheel)>,
+        mut rear_wheel_query: Query<
+            (&ShapeCaster, &ShapeHits, &mut PreviousDisplacement),
+            (With<Rearing>, Without<Steering>),
+        >,
+        config: Res<SuspensionConfig>,
         time: Res<Time>,
         input: Res<InputState>,
-        mut gizmos: Gizmos,
     ) {
-        let max_thrust = 100.0;
-        let max_yaw = 50.0;
-        let mut thrust = input.throttle * max_thrust;
+        let max_thrust = 30.0;
+        let max_yaw = 10.0;
+        let mut thrust = (input.throttle * max_thrust);
         if input.brake {
             thrust *= -1.0;
         }
         let yaw = input.yaw * max_yaw;
+
         let dt = time.delta().as_secs_f32();
-
-        let mut front_caster = &ShapeCaster::default();
-        let mut rear_caster = &ShapeCaster::default();
-
-        let mut front_hits = &ShapeHits::default();
-        let mut rear_hits = &ShapeHits::default();
-
-        for (caster, hits, wheel) in caster_query.iter() {
+        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_caster = caster;
-                    front_hits = hits;
+                    front_wheel_xform = Some(xform);
                 }
                 CyberWheel::Rear => {
-                    rear_caster = caster;
-                    rear_hits = hits;
+                    rear_wheel_xform = Some(xform);
                 }
             }
         }
-        let (bike_xform, mut bike_forces) = bike_body_query.single_mut();
-        for (mut xform, mut state, global_xform, config, wheel) in wheel_mesh_query.iter_mut() {
-            let (caster, hits) = match wheel {
-                CyberWheel::Front => (front_caster, front_hits),
-                CyberWheel::Rear => (rear_caster, rear_hits),
-            };
 
-            let prev = &mut state.displacement;
-            if let Some(hit) = hits.iter().next() {
-                let force = suspension_force(caster, hit, config, prev, dt, &mut xform);
-                bike_forces.apply_force_at_point(
-                    force,
-                    caster.global_origin(),
-                    bike_xform.translation,
-                );
-                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,
-                };
-                let thrust = normal.cross(*bike_xform.right()) * thrust;
-                let total = (thrust + steering) * dt;
-                bike_forces.apply_force_at_point(total, hit.point1, bike_xform.translation);
-                gizmos.arrow(
-                    hit.point1,
-                    hit.point1 + total,
-                    Color::linear_rgb(1., 1., 0.2),
-                );
-            } else {
-                xform.translation = config.attach + (caster.direction.as_vec3() * config.rest_dist);
+        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, prev.as_mut(), dt, xform, true);
+                    bike_forces.apply_force_at_point(
+                        force,
+                        caster.global_origin(),
+                        bike_xform.translation,
+                    );
+                    let normal = hit.normal1;
+                    let steering = normal.cross(*bike_xform.back()) * yaw;
+                    let thrust = normal.cross(*bike_xform.right()) * thrust;
+                    let total = thrust + steering;
+                    bike_forces.apply_force_at_point(total, hit.point1, 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, prev.as_mut(), dt, xform, false);
+                    bike_forces.apply_force_at_point(
+                        force,
+                        caster.global_origin(),
+                        bike_xform.translation,
+                    );
+                    let normal = hit.normal1;
+                    let thrust = normal.cross(*bike_xform.right()) * thrust;
+                    bike_forces.apply_force_at_point(thrust, hit.point1, bike_xform.translation);
+                }
             }
         }
     }
@@ -241,24 +238,33 @@ mod systems {
     fn suspension_force(
         caster: &ShapeCaster,
         hit: &ShapeHitData,
-        config: &WheelConfig,
-        previous_dispacement: &mut Scalar,
-        dt: Scalar,
+        config: &SuspensionConfig,
+        previous_dispacement: &mut PreviousDisplacement,
+        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 = {
+        let loc = if dist < config.rest_dist {
             let displacement = config.rest_dist - dist;
-            let damper_vel = (displacement - *previous_dispacement) / dt;
+            let damper_vel = (displacement - previous_dispacement.0) / dt;
             dbg!(damper_vel);
-            *previous_dispacement = displacement;
+            *previous_dispacement = PreviousDisplacement(displacement);
             let mag = config.konstant * displacement - config.damping * damper_vel;
             up_force = -dir * mag;
-            config.attach + (cdir * dist)
+            attach + (cdir * dist)
+        } else {
+            attach + (cdir * config.rest_dist)
         };
+
         wheel_xform.translation = loc;
 
         up_force
@@ -278,12 +284,6 @@ impl Plugin for CyberPhysicsPlugin {
             .register_type::<CyberLean>()
             .add_plugins((PhysicsPlugins::default(), PhysicsDebugPlugin::default()))
             .insert_resource(SubstepCount(12))
-            .add_systems(Startup, |mut gravity: ResMut<Gravity>| {
-                gravity.0 *= 0.02;
-            })
-            .add_systems(
-                FixedUpdate,
-                (calculate_lean, apply_lean, suspension).chain(),
-            );
+            .add_systems(Update, (apply_lean, calculate_lean, suspension));
     }
 }