src/bike.rs

@@ -6,10 +6,8 @@ use bevy::prelude::*;
 
 use crate::physics::CatControllerState;
 
-pub const SPRING_CONSTANT: Scalar = 50.0;
+pub const REST_DISTANCE: f32 = 1.0;
-pub const DAMPING_CONSTANT: Scalar = 10.0;
+pub const WHEEL_RADIUS: f32 = 0.4;
-pub const WHEEL_RADIUS: Scalar = 0.4;
-pub const REST_DISTANCE: Scalar = 1.0 + WHEEL_RADIUS;
 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);
 
@@ -40,7 +38,6 @@ pub struct WheelConfig {
     pub konstant: Scalar,
     pub damping: Scalar,
     pub friction: Scalar,
-    pub radius: Scalar,
 }
 
 impl WheelConfig {
@@ -50,7 +47,6 @@ impl WheelConfig {
         konstant: Scalar,
         damping: Scalar,
         friction: Scalar,
-        radius: Scalar,
     ) -> Self {
         WheelConfig {
             attach,
@@ -58,7 +54,6 @@ impl WheelConfig {
             konstant,
             damping,
             friction,
-            radius,
         }
     }
 }
@@ -68,7 +63,7 @@ fn spawn_bike(
     mut meshes: ResMut<Assets<Mesh>>,
     mut materials: ResMut<Assets<StandardMaterial>>,
 ) {
-    let pos = Vec3::new(0.0, 5.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 =
@@ -84,11 +79,9 @@ fn spawn_bike(
             SleepingDisabled,
             CyberBikeBody,
             CatControllerState::default(),
-            ColliderDensity(0.6),
+            ColliderDensity(20.0),
-            AngularDamping(0.2),
+            LinearVelocity::ZERO,
-            //LinearDamping(0.2),
+            AngularVelocity::ZERO,
-            // LinearVelocity::ZERO,
-            // AngularVelocity::ZERO,
             ExternalForce::ZERO.with_persistence(false),
             ExternalTorque::ZERO.with_persistence(false),
         ))
@@ -113,16 +106,18 @@ fn spawn_wheels(
     body: Entity,
 ) {
     let mesh: Mesh = Sphere::new(WHEEL_RADIUS).into();
+    let collider = Collider::sphere(WHEEL_RADIUS);
 
     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(
         commands,
-        FRONT_ATTACH,
+        collider.clone(),
+        Transform::from_translation(FRONT_ATTACH),
         Dir3::new_unchecked(front_rake),
         body,
-        REST_DISTANCE + WHEEL_RADIUS,
+        REST_DISTANCE,
         CyberWheel::Front,
     );
     wheel_mesh(
@@ -131,14 +126,7 @@ fn spawn_wheels(
         &mut materials,
         front_wheel_pos,
         mesh.clone(),
-        WheelConfig::new(
+        WheelConfig::new(FRONT_ATTACH, REST_DISTANCE, 800., -160., 0.5),
-            FRONT_ATTACH,
-            REST_DISTANCE,
-            SPRING_CONSTANT,
-            DAMPING_CONSTANT,
-            0.5,
-            WHEEL_RADIUS,
-        ),
         CyberWheel::Front,
     );
 
@@ -147,10 +135,11 @@ fn spawn_wheels(
 
     wheel_caster(
         commands,
-        REAR_ATTACH,
+        collider,
+        Transform::from_translation(REAR_ATTACH),
         Dir3::new_unchecked(rear_rake),
         body,
-        REST_DISTANCE + WHEEL_RADIUS,
+        REST_DISTANCE,
         CyberWheel::Rear,
     );
     wheel_mesh(
@@ -159,14 +148,7 @@ fn spawn_wheels(
         &mut materials,
         rear_wheel_pos,
         mesh,
-        WheelConfig::new(
+        WheelConfig::new(REAR_ATTACH, REST_DISTANCE, 800., -160., 0.5),
-            REAR_ATTACH,
-            REST_DISTANCE,
-            SPRING_CONSTANT,
-            DAMPING_CONSTANT,
-            0.5,
-            WHEEL_RADIUS,
-        ),
         CyberWheel::Rear,
     );
 }
@@ -177,15 +159,15 @@ fn spawn_wheels(
 
 fn wheel_caster(
     commands: &mut ChildBuilder,
-    origin: Vec3,
+    collider: Collider,
+    xform: Transform,
     direction: Dir3,
     parent: Entity,
     rest_dist: Scalar,
     wheel: CyberWheel,
 ) {
-    let caster = RayCaster::new(origin, direction)
+    let caster = ShapeCaster::new(collider, xform.translation, Quat::IDENTITY, direction)
         .with_max_distance(rest_dist)
-        .with_max_hits(1)
         .with_query_filter(SpatialQueryFilter::from_excluded_entities([parent]));
 
     commands.spawn((caster, wheel));
@@ -209,18 +191,12 @@ 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(name),
+        Name::new("tire"),
         config,
         Mesh3d(meshes.add(tire_mesh)),
         MeshMaterial3d(materials.add(wheel_material.clone())),
         xform,
-        TransformInterpolation,
         wheel,
     ));
 }
@@ -229,8 +205,6 @@ 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

@@ -36,28 +36,34 @@ fn update_camera_pos(mut offset: ResMut<DebugCamOffset>, mut keys: ResMut<Button
             KeyCode::ArrowRight => offset.rot += 5.0,
             KeyCode::ArrowUp => {
                 if shifted {
+                    bevy::log::info!("up, shifted");
                     offset.alt += 0.5;
                 } else {
+                    bevy::log::info!("up");
                     offset.dist -= 0.5;
                 }
             }
             KeyCode::ArrowDown => {
                 if shifted {
+                    bevy::log::info!("down, shifted");
                     offset.alt -= 0.5;
                 } else {
+                    bevy::log::info!("down");
                     offset.dist += 0.5;
                 }
             }
-            KeyCode::KeyR => {
-                *offset = DebugCamOffset::default();
-            }
             _ => continue,
         }
     }
 
-    let released: Vec<_> = keys.get_just_released().copied().collect();
+    let just_released: HashSet<_> = keys.get_just_released().cloned().collect();
-    for key in released {
+    if !just_released.is_empty() {
-        keys.clear_just_released(key);
+        let released_shift = just_released.contains(&KeyCode::ShiftLeft)
+            || just_released.contains(&KeyCode::ShiftRight);
+        keys.reset_all();
+        if !released_shift && shifted {
+            keys.press(KeyCode::ShiftLeft);
+        }
     }
 }
 

src/input.rs

@@ -46,6 +46,6 @@ pub struct CyberInputPlugin;
 impl Plugin for CyberInputPlugin {
     fn build(&self, app: &mut App) {
         app.init_resource::<InputState>()
-            .add_systems(Update, update_input);
+            .add_systems(Update, (update_input));
     }
 }

src/physics.rs

@@ -18,9 +18,9 @@ pub struct CatControllerSettings {
 impl Default for CatControllerSettings {
     fn default() -> Self {
         Self {
-            kp: 30.0,
+            kp: 1200.0,
-            kd: 7.0,
+            kd: 10.0,
-            ki: 1.0,
+            ki: 50.0,
         }
     }
 }
@@ -57,12 +57,12 @@ impl CatControllerState {
 mod systems {
     use std::f32::consts::{FRAC_PI_3, FRAC_PI_4};
 
-    use avian3d::prelude::*;
+    use avian3d::{math::Scalar, prelude::*};
-    use bevy::prelude::*;
+    use bevy::{color, math::VectorSpace, prelude::*};
 
     use super::{CatControllerSettings, CatControllerState, CyberLean};
     use crate::{
-        bike::{CyberBikeBody, CyberWheel, WheelConfig, WheelState, WHEEL_RADIUS},
+        bike::{CyberBikeBody, CyberWheel, WheelConfig, WheelState},
         input::InputState,
     };
 
@@ -79,28 +79,23 @@ 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 radius = base / steering_angle.tan();
+        let wheel_base = 1.145f32;
-        let gravity = gravity.0.length();
+        let radius = wheel_base / steering_angle.tan();
+        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);
+            lean.lean = -tan_theta.atan().clamp(-FRAC_PI_3, FRAC_PI_3);
         } else {
-            //lean.lean = 0.0;
+            lean.lean = 0.0;
         }
     }
 
@@ -163,15 +158,7 @@ mod systems {
     }
 
     pub(super) fn suspension(
-        mut bike_body_query: Query<
+        mut bike_body_query: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
-            (
-                &Transform,
-                &LinearVelocity,
-                &AngularVelocity,
-                &mut ExternalForce,
-            ),
-            With<CyberBikeBody>,
-        >,
         mut wheel_mesh_query: Query<
             (
                 &mut Transform,
@@ -182,7 +169,7 @@ mod systems {
             ),
             Without<CyberBikeBody>,
         >,
-        caster_query: Query<(&RayCaster, &RayHits, &CyberWheel)>,
+        caster_query: Query<(&ShapeCaster, &ShapeHits, &CyberWheel)>,
         time: Res<Time>,
         input: Res<InputState>,
         mut gizmos: Gizmos,
@@ -196,11 +183,11 @@ mod systems {
         let yaw = input.yaw * max_yaw;
         let dt = time.delta().as_secs_f32();
 
-        let mut front_caster = &RayCaster::default();
+        let mut front_caster = &ShapeCaster::default();
-        let mut rear_caster = &RayCaster::default();
+        let mut rear_caster = &ShapeCaster::default();
 
-        let mut front_hits = &RayHits::default();
+        let mut front_hits = &ShapeHits::default();
-        let mut rear_hits = &RayHits::default();
+        let mut rear_hits = &ShapeHits::default();
 
         for (caster, hits, wheel) in caster_query.iter() {
             match wheel {
@@ -214,99 +201,71 @@ mod systems {
                 }
             }
         }
-        let (bike_xform, lin_vel, ang_vel, mut bike_forces) = bike_body_query.single_mut();
+        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 dist = hit.distance;
+                let force = suspension_force(caster, hit, config, prev, dt, &mut xform);
-                let cdir = caster.direction.as_vec3();
-                xform.translation = config.attach + (cdir * (dist - WHEEL_RADIUS));
-
-                let displacement = config.rest_dist - dist;
-                let damper_vel = (state.displacement - displacement) / dt;
-                state.displacement = displacement;
-
-                let mag = config.konstant * displacement - config.damping * damper_vel;
-
-                let mag = mag.max(0.0);
-
-                let fdir = hit.normal;
-                let force = fdir * mag;
-
-                let hit_point = caster.global_origin() + caster.global_direction() * dist;
-                gizmos.arrow(
-                    hit_point,
-                    hit_point + 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.normal;
+                let normal = hit.normal1;
                 let steering = match wheel {
                     CyberWheel::Front => normal.cross(*bike_xform.back()) * yaw,
-                    CyberWheel::Rear => Vec3::ZERO,
+                    _ => normal.cross(*bike_xform.forward()) * yaw, // Vec3::ZERO,
-                    //_ => 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_point, bike_xform.translation);
+                bike_forces.apply_force_at_point(total, hit.point1, bike_xform.translation);
-                gizmos.arrow(hit_point, hit_point + total, Color::linear_rgb(1., 1., 0.2));
+                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);
             }
         }
     }
 
-    pub(super) fn tweak(
+    fn suspension_force(
-        mut config: Query<&mut WheelConfig>,
+        caster: &ShapeCaster,
-        mut keys: ResMut<ButtonInput<KeyCode>>,
+        hit: &ShapeHitData,
-    ) {
+        config: &WheelConfig,
-        let keyset: std::collections::HashSet<_> = keys.get_pressed().collect();
+        previous_dispacement: &mut Scalar,
-        let shifted = keyset.contains(&KeyCode::ShiftLeft) || keyset.contains(&KeyCode::ShiftRight);
+        dt: Scalar,
-        let config = config.iter_mut();
+        wheel_xform: &mut Transform,
-        for ref mut c in config {
+    ) -> Vec3 {
-            for key in &keyset {
+        let mut up_force = Vec3::ZERO;
-                match key {
+        let dist = hit.distance;
-                    KeyCode::KeyS => {
+        let cdir = caster.direction.as_vec3();
-                        if shifted {
+        let dir = caster.global_direction().as_vec3();
-                            c.konstant += 0.2;
+        let loc = {
-                        } else {
+            let displacement = config.rest_dist - dist;
-                            c.konstant -= 0.2;
+            let damper_vel = (displacement - *previous_dispacement) / dt;
-                        }
+            dbg!(damper_vel);
-                        bevy::log::info!(c.konstant);
+            *previous_dispacement = displacement;
-                    }
+            let mag = config.konstant * displacement - config.damping * damper_vel;
-                    KeyCode::KeyD => {
+            up_force = -dir * mag;
-                        if shifted {
+            config.attach + (cdir * dist)
-                            c.damping += 0.1;
+        };
-                        } else {
+        wheel_xform.translation = loc;
-                            c.damping -= 0.1;
-                        }
-                        bevy::log::info!(c.damping);
-                    }
 
-                    _ => continue,
+        up_force
-                }
-            }
-        }
-        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, tweak};
+use systems::{apply_lean, calculate_lean, suspension};
 
 pub struct CyberPhysicsPlugin;
 
@@ -320,12 +279,11 @@ impl Plugin for CyberPhysicsPlugin {
             .add_plugins((PhysicsPlugins::default(), PhysicsDebugPlugin::default()))
             .insert_resource(SubstepCount(12))
             .add_systems(Startup, |mut gravity: ResMut<Gravity>| {
-                gravity.0 *= 1.0;
+                gravity.0 *= 0.02;
             })
             .add_systems(
                 FixedUpdate,
                 (calculate_lean, apply_lean, suspension).chain(),
-            )
+            );
-            .add_systems(Update, tweak);
     }
 }