src/bike.rs

@@ -5,15 +5,12 @@ use bevy::prelude::*;
 
 use crate::physics::CatControllerState;
 
-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);
+pub const REST_DISTANCE: f32 = 0.8;
 
 #[derive(Component)]
 pub struct CyberBikeBody;
 
-#[derive(Component, Clone, Copy, Debug)]
+#[derive(Component)]
 pub enum CyberWheel {
     Front,
     Rear,
@@ -27,27 +24,19 @@ 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: 800.0,
-            damping: -100.0,
-            front_attach: FRONT_ATTACH,
-            rear_attach: REAR_ATTACH,
+            konstant: 150.0,
+            damping: 50.0,
         }
     }
 }
@@ -57,7 +46,7 @@ fn spawn_bike(
     mut meshes: ResMut<Assets<Mesh>>,
     mut materials: ResMut<Assets<StandardMaterial>>,
 ) {
-    let pos = Vec3::new(0.0, 14.0, 0.0);
+    let pos = Vec3::new(0.0, 24.0, 0.0);
     let xform = Transform::from_translation(pos); //.with_rotation(Quat::from_rotation_z(0.0));
 
     let body_collider =
@@ -89,8 +78,10 @@ fn spawn_bike(
                 MeshMaterial3d(materials.add(color)),
             );
             builder.spawn(pbr_bundle);
-            spawn_wheels(builder, meshes, materials, builder.parent_entity());
+            spawn_wheels(builder, meshes, materials, xform, builder.parent_entity());
         });
+
+    //spawn_wheels(commands, meshes, materials, xform, bike);
 }
 
 fn spawn_caster(
@@ -101,10 +92,10 @@ fn spawn_caster(
     parent: Entity,
     is_front: bool,
 ) {
-    let caster = ShapeCaster::new(collider, Vec3::ZERO, Quat::IDENTITY, direction)
+    let caster = ShapeCaster::new(collider, xform.translation, Quat::IDENTITY, direction)
         .with_query_filter(SpatialQueryFilter::from_excluded_entities([parent]));
 
-    let mut entity = commands.spawn((caster, xform, PreviousDisplacement(REST_DISTANCE)));
+    let mut entity = commands.spawn((caster, Transform::default()));
     if is_front {
         entity.insert((Steering, Name::new("front wheel")));
     } else {
@@ -116,17 +107,19 @@ 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_wheel_pos = FRONT_ATTACH + (front_rake * REST_DISTANCE);
+    let front_attach = xform.translation + xform.forward() * 0.7;
+    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,
@@ -141,13 +134,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,
@@ -167,8 +160,8 @@ fn spawn_wheels(
 //-************************************************************************
 
 fn gen_tire() -> (Mesh, Collider) {
-    let tire_mesh: Mesh = Sphere::new(WHEEL_RADIUS).into();
-    let collider = Collider::sphere(WHEEL_RADIUS);
+    let tire_mesh: Mesh = Sphere::new(0.4).into();
+    let collider = Collider::sphere(0.4);
 
     (tire_mesh, collider)
 }
@@ -195,7 +188,6 @@ fn wheel_mesh(
         Mesh3d(meshes.add(tire_mesh)),
         MeshMaterial3d(materials.add(wheel_material.clone())),
         xform,
-        PreviousDisplacement::default(),
         wheel,
     ));
 }
@@ -205,7 +197,6 @@ 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,6 +52,10 @@ 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 {
@@ -61,12 +65,16 @@ mod systems {
     use bevy::prelude::*;
 
     use super::{CatControllerSettings, CatControllerState, CyberLean};
-    use crate::{
-        bike::{
-            CyberBikeBody, CyberWheel, PreviousDisplacement, Rearing, Steering, SuspensionConfig,
-        },
-        input::InputState,
-    };
+    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
+        }
+    }
 
     fn rotate_point(pt: &Vec3, rot: &Quat) -> Vec3 {
         // thanks to https://danceswithcode.net/engineeringnotes/quaternions/quaternions.html
@@ -158,103 +166,9 @@ 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, suspension};
+use systems::{apply_lean, calculate_lean};
 
 pub struct CyberPhysicsPlugin;
 
@@ -267,6 +181,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, suspension));
+            .add_systems(Update, (apply_lean, calculate_lean));
     }
 }