working ray-casting, no jitter

src/bike.rs

@@ -6,8 +6,10 @@ use bevy::prelude::*;
 
 use crate::physics::CatControllerState;
 
-pub const REST_DISTANCE: f32 = 1.0;
-pub const WHEEL_RADIUS: f32 = 0.4;
+pub const SPRING_CONSTANT: Scalar = 50.0;
+pub const DAMPING_CONSTANT: Scalar = 10.0;
+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);
 
@@ -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,
         }
     }
 }
@@ -63,7 +68,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, 5.0, 0.0);
     let xform = Transform::from_translation(pos); //.with_rotation(Quat::from_rotation_z(0.0));
 
     let body_collider =
@@ -79,9 +84,11 @@ fn spawn_bike(
             SleepingDisabled,
             CyberBikeBody,
             CatControllerState::default(),
-            ColliderDensity(20.0),
-            LinearVelocity::ZERO,
-            AngularVelocity::ZERO,
+            ColliderDensity(0.6),
+            AngularDamping(0.2),
+            //LinearDamping(0.2),
+            // LinearVelocity::ZERO,
+            // AngularVelocity::ZERO,
             ExternalForce::ZERO.with_persistence(false),
             ExternalTorque::ZERO.with_persistence(false),
         ))
@@ -106,18 +113,16 @@ 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,
-        collider.clone(),
-        Transform::from_translation(FRONT_ATTACH),
+        FRONT_ATTACH,
         Dir3::new_unchecked(front_rake),
         body,
-        REST_DISTANCE,
+        REST_DISTANCE + WHEEL_RADIUS,
         CyberWheel::Front,
     );
     wheel_mesh(
@@ -126,7 +131,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,
     );
 
@@ -135,11 +147,10 @@ fn spawn_wheels(
 
     wheel_caster(
         commands,
-        collider,
-        Transform::from_translation(REAR_ATTACH),
+        REAR_ATTACH,
         Dir3::new_unchecked(rear_rake),
         body,
-        REST_DISTANCE,
+        REST_DISTANCE + WHEEL_RADIUS,
         CyberWheel::Rear,
     );
     wheel_mesh(
@@ -148,7 +159,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,
     );
 }
@@ -159,15 +177,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 = RayCaster::new(origin, direction)
         .with_max_distance(rest_dist)
+        .with_max_hits(1)
         .with_query_filter(SpatialQueryFilter::from_excluded_entities([parent]));
 
     commands.spawn((caster, wheel));
@@ -191,12 +209,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 +229,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

@@ -36,34 +36,28 @@ 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 just_released: HashSet<_> = keys.get_just_released().cloned().collect();
-    if !just_released.is_empty() {
-        let released_shift = just_released.contains(&KeyCode::ShiftLeft)
-            || just_released.contains(&KeyCode::ShiftRight);
-        keys.reset_all();
-        if !released_shift && shifted {
-            keys.press(KeyCode::ShiftLeft);
-        }
+    let released: Vec<_> = keys.get_just_released().copied().collect();
+    for key in released {
+        keys.clear_just_released(key);
     }
 }
 

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: 1200.0,
-            kd: 10.0,
-            ki: 50.0,
+            kp: 30.0,
+            kd: 7.0,
+            ki: 1.0,
         }
     }
 }
@@ -57,12 +57,12 @@ 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, WheelConfig, WheelState, WHEEL_RADIUS},
         input::InputState,
     };
 
@@ -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 = wheel_base / steering_angle.tan();
-        let gravity = -9.8f32;
+        let radius = base / steering_angle.tan();
+        let gravity = gravity.0.length();
         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;
         }
     }
 
@@ -158,7 +163,15 @@ mod systems {
     }
 
     pub(super) fn suspension(
-        mut bike_body_query: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
+        mut bike_body_query: Query<
+            (
+                &Transform,
+                &LinearVelocity,
+                &AngularVelocity,
+                &mut ExternalForce,
+            ),
+            With<CyberBikeBody>,
+        >,
         mut wheel_mesh_query: Query<
             (
                 &mut Transform,
@@ -169,7 +182,7 @@ mod systems {
             ),
             Without<CyberBikeBody>,
         >,
-        caster_query: Query<(&ShapeCaster, &ShapeHits, &CyberWheel)>,
+        caster_query: Query<(&RayCaster, &RayHits, &CyberWheel)>,
         time: Res<Time>,
         input: Res<InputState>,
         mut gizmos: Gizmos,
@@ -183,11 +196,11 @@ mod systems {
         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_caster = &RayCaster::default();
+        let mut rear_caster = &RayCaster::default();
 
-        let mut front_hits = &ShapeHits::default();
-        let mut rear_hits = &ShapeHits::default();
+        let mut front_hits = &RayHits::default();
+        let mut rear_hits = &RayHits::default();
 
         for (caster, hits, wheel) in caster_query.iter() {
             match wheel {
@@ -201,71 +214,99 @@ mod systems {
                 }
             }
         }
-        let (bike_xform, mut bike_forces) = bike_body_query.single_mut();
+        let (bike_xform, lin_vel, ang_vel, 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);
+                let dist = hit.distance;
+                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;
-                dbg!(vel);
+                //let vel = (global_xform.translation() - state.ppos) / dt;
+
                 state.ppos = global_xform.translation();
-                let normal = hit.normal1;
+                let normal = hit.normal;
                 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;
-                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),
-                );
+                bike_forces.apply_force_at_point(total, hit_point, bike_xform.translation);
+                gizmos.arrow(hit_point, hit_point + total, Color::linear_rgb(1., 1., 0.2));
             } else {
                 xform.translation = config.attach + (caster.direction.as_vec3() * config.rest_dist);
             }
         }
     }
 
-    fn suspension_force(
-        caster: &ShapeCaster,
-        hit: &ShapeHitData,
-        config: &WheelConfig,
-        previous_dispacement: &mut Scalar,
-        dt: Scalar,
-        wheel_xform: &mut Transform,
-    ) -> Vec3 {
-        let mut up_force = Vec3::ZERO;
-        let dist = hit.distance;
-        let cdir = caster.direction.as_vec3();
-        let dir = caster.global_direction().as_vec3();
-        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;
+    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);
+                    }
 
-        up_force
+                    _ => 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 +320,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);
     }
 }