checkpoint

Cargo.toml

@@ -1,12 +1,17 @@
 [package]
 name = "physics-test"
 version = "0.1.0"
-edition = "2021"
+edition = "2024"
 
 [dependencies]
-avian3d = { git = "https://github.com/Jondolf/avian.git" }
 bevy = { version = "0.15", features = ["bevy_dev_tools"] }
-bevy-inspector-egui = "0.28"
+bevy-inspector-egui = "0.29"
+
+[dependencies.avian3d]
+default-features = false
+version = "0.2"
+features = ["3d", "f32", "parry-f32", "debug-plugin", "default-collider", "collider-from-mesh"]
+
 
 [features]
 no-mesh = []

src/bike.rs

@@ -1,40 +1,75 @@
-use std::f32::consts::FRAC_PI_2;
-
-use avian3d::prelude::*;
+use avian3d::{
+    math::{Scalar, FRAC_PI_2},
+    prelude::*,
+};
 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);
+
 #[derive(Component)]
 pub struct CyberBikeBody;
 
-#[derive(Component)]
-pub struct CyberWheel;
+#[derive(Component, Clone, Copy, Debug)]
+pub enum CyberWheel {
+    Front,
+    Rear,
+}
 
-// marker for front suspension joint
-#[derive(Component)]
-pub struct Steering;
-#[derive(Component)]
-pub struct FrontHub;
+#[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 rear suspension joint
-#[derive(Component)]
-pub struct Rearing;
-#[derive(Component)]
-pub struct RearHub;
+#[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,
+        }
+    }
+}
 
 fn spawn_bike(
     mut commands: Commands,
     mut meshes: ResMut<Assets<Mesh>>,
     mut materials: ResMut<Assets<StandardMaterial>>,
 ) {
-    let pos = Vec3::new(0.0, 4.0, 0.0);
-    let xform = Transform::from_translation(pos).with_rotation(Quat::from_rotation_z(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 =
         Collider::capsule_endpoints(0.5, Vec3::new(0.0, 0.0, -0.65), Vec3::new(0.0, 0.0, 0.8));
 
-    let bike = commands
+    let _bike = commands
         .spawn((xform, Visibility::default()))
         .insert((
             Name::new("body"),
@@ -45,8 +80,6 @@ fn spawn_bike(
             CyberBikeBody,
             CatControllerState::default(),
             ColliderDensity(20.0),
-            LinearDamping(0.1),
-            AngularDamping(2.0),
             LinearVelocity::ZERO,
             AngularVelocity::ZERO,
             ExternalForce::ZERO.with_persistence(false),
@@ -54,100 +87,101 @@ fn spawn_bike(
         ))
         .with_children(|builder| {
             let color = Color::srgb(0.7, 0.05, 0.7);
-            let mut rotation = Transform::default(); // Transform::from_rotation(Quat::from_rotation_y(FRAC_PI_2));
-            rotation.rotate_x(FRAC_PI_2);
+            let mut xform = Transform::default();
+            xform.rotate_x(FRAC_PI_2);
             let pbr_bundle = (
                 Mesh3d(meshes.add(Capsule3d::new(0.5, 1.45))),
-                rotation,
+                xform,
                 MeshMaterial3d(materials.add(color)),
             );
             builder.spawn(pbr_bundle);
-        })
-        .id();
-
-    spawn_wheels(commands, meshes, materials, xform, bike);
+            spawn_wheels(builder, meshes, materials, builder.parent_entity());
+        });
 }
 
 fn spawn_wheels(
-    mut commands: Commands,
+    commands: &mut ChildBuilder,
     mut meshes: ResMut<Assets<Mesh>>,
     mut materials: ResMut<Assets<StandardMaterial>>,
-    xform: Transform,
     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_pos = xform.translation + front_rake;
+    let front_wheel_pos = FRONT_ATTACH + (front_rake * REST_DISTANCE);
 
-    let (mesh, collider) = gen_tire();
-
-    let front_hub = wheels_helper(
-        &mut commands,
+    wheel_caster(
+        commands,
+        collider.clone(),
+        Transform::from_translation(FRONT_ATTACH),
+        Dir3::new_unchecked(front_rake),
+        body,
+        REST_DISTANCE,
+        CyberWheel::Front,
+    );
+    wheel_mesh(
+        commands,
         &mut meshes,
         &mut materials,
-        front_pos,
+        front_wheel_pos,
         mesh.clone(),
-        collider.clone(),
+        WheelConfig::new(FRONT_ATTACH, REST_DISTANCE, 800., -160., 0.5),
+        CyberWheel::Front,
     );
-    commands.entity(front_hub).insert(FrontHub);
-    commands.spawn((
-        Steering,
-        FixedJoint::new(front_hub, body).with_local_anchor_2(*xform.forward() + front_rake),
-    ));
 
     let rear_rake = Vec3::new(0.0, -1.0, 0.57).normalize();
-    let rear_pos = xform.translation + rear_rake;
+    let rear_wheel_pos = REAR_ATTACH + (rear_rake * REST_DISTANCE);
 
-    let rear_hub = wheels_helper(
-        &mut commands,
+    wheel_caster(
+        commands,
+        collider,
+        Transform::from_translation(REAR_ATTACH),
+        Dir3::new_unchecked(rear_rake),
+        body,
+        REST_DISTANCE,
+        CyberWheel::Rear,
+    );
+    wheel_mesh(
+        commands,
         &mut meshes,
         &mut materials,
-        rear_pos,
+        rear_wheel_pos,
         mesh,
-        collider,
+        WheelConfig::new(REAR_ATTACH, REST_DISTANCE, 800., -160., 0.5),
+        CyberWheel::Rear,
     );
-    commands.entity(rear_hub).insert(RearHub);
-    commands.spawn((
-        Rearing,
-        FixedJoint::new(rear_hub, body).with_local_anchor_2(*xform.back() + rear_rake),
-    ));
 }
 
 //-************************************************************************
 // helper fns for the wheels
 //-************************************************************************
 
-fn gen_tire() -> (Mesh, Collider) {
-    let mut tire_mesh: Mesh = Torus::new(0.25, 0.40).into();
-    let tire_verts = tire_mesh
-        .attribute(Mesh::ATTRIBUTE_POSITION)
-        .unwrap()
-        .as_float3()
-        .unwrap()
-        .iter()
-        .map(|v| {
-            //
-            let v = Vec3::from_array(*v);
-            let m = Mat3::from_rotation_z(90.0f32.to_radians());
-            let p = m.mul_vec3(v);
-            p.to_array()
-        })
-        .collect::<Vec<[f32; 3]>>();
-    tire_mesh.remove_attribute(Mesh::ATTRIBUTE_POSITION);
-    tire_mesh.insert_attribute(Mesh::ATTRIBUTE_POSITION, tire_verts);
+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]));
 
-    let collider = Collider::convex_hull_from_mesh(&tire_mesh).unwrap();
-
-    (tire_mesh, collider)
+    commands.spawn((caster, wheel));
 }
 
-fn wheels_helper(
-    commands: &mut Commands,
+fn wheel_mesh(
+    commands: &mut ChildBuilder,
     meshes: &mut ResMut<Assets<Mesh>>,
     materials: &mut ResMut<Assets<StandardMaterial>>,
     position: Vec3,
     tire_mesh: Mesh,
-    collider: Collider,
-) -> Entity {
+    config: WheelConfig,
+    wheel: CyberWheel,
+) {
     let wheel_material = &StandardMaterial {
         base_color: Color::srgb(0.01, 0.01, 0.01),
         alpha_mode: AlphaMode::Opaque,
@@ -156,41 +190,15 @@ fn wheels_helper(
     };
 
     let xform = Transform::from_translation(position);
-    let hub_mesh: Mesh = Sphere::new(0.1).into();
 
-    let hub = commands
-        .spawn((
-            Name::new("hub"),
-            RigidBody::Dynamic,
-            MassPropertiesBundle::from_shape(&Collider::sphere(0.1), 200.0),
-            Mesh3d(meshes.add(hub_mesh)),
-            MeshMaterial3d(materials.add(wheel_material.clone())),
-            xform,
-        ))
-        .id();
-
-    let tire = commands
-        .spawn((
-            Name::new("tire"),
-            Mesh3d(meshes.add(tire_mesh)),
-            MeshMaterial3d(materials.add(wheel_material.clone())),
-            xform,
-            CyberWheel,
-            RigidBody::Dynamic,
-            collider,
-            Friction::new(0.9).with_dynamic_coefficient(0.6),
-            Restitution::new(0.1),
-            ColliderDensity(30.0),
-            CollisionLayers::from_bits(2, 2),
-            ExternalTorque::ZERO.with_persistence(false),
-            CollisionMargin(0.05),
-            SweptCcd::NON_LINEAR,
-        ))
-        .id();
-
-    // connect hubs and tires to make wheels
-    commands.spawn(RevoluteJoint::new(hub, tire).with_aligned_axis(Vec3::X));
-    hub
+    commands.spawn((
+        Name::new("tire"),
+        config,
+        Mesh3d(meshes.add(tire_mesh)),
+        MeshMaterial3d(materials.add(wheel_material.clone())),
+        xform,
+        wheel,
+    ));
 }
 
 pub struct CyberBikePlugin;

src/camera.rs

@@ -16,7 +16,7 @@ impl Default for DebugCamOffset {
     fn default() -> Self {
         DebugCamOffset {
             rot: 60.0,
-            dist: 10.0,
+            dist: 30.0,
             alt: 4.0,
         }
     }

src/input.rs

@@ -0,0 +1,51 @@
+use bevy::{
+    input::gamepad::{GamepadAxisChangedEvent, GamepadButtonChangedEvent, GamepadEvent},
+    prelude::*,
+};
+
+#[derive(Default, Debug, Resource)]
+pub(crate) struct InputState {
+    pub yaw: f32,
+    pub throttle: f32,
+    pub brake: bool,
+    pub pitch: f32,
+}
+
+fn update_input(mut events: EventReader<GamepadEvent>, mut istate: ResMut<InputState>) {
+    for pad_event in events.read() {
+        match pad_event {
+            GamepadEvent::Button(button_event) => {
+                let GamepadButtonChangedEvent { button, value, .. } = button_event;
+                match button {
+                    GamepadButton::RightTrigger2 => istate.throttle = *value,
+                    GamepadButton::LeftTrigger2 => istate.throttle = -value,
+                    GamepadButton::East => {
+                        istate.brake = value > &0.5;
+                    }
+                    _ => {}
+                }
+            }
+            GamepadEvent::Axis(axis_event) => {
+                let GamepadAxisChangedEvent { axis, value, .. } = axis_event;
+                match axis {
+                    GamepadAxis::LeftStickX => {
+                        istate.yaw = *value;
+                    }
+                    GamepadAxis::RightStickY => {
+                        istate.pitch = *value;
+                    }
+                    _ => {}
+                }
+            }
+            GamepadEvent::Connection(_) => {}
+        }
+    }
+}
+
+pub struct CyberInputPlugin;
+impl Plugin for CyberInputPlugin {
+    fn build(&self, app: &mut App) {
+        app.init_resource::<InputState>()
+            .add_systems(Update, (update_input));
+    }
+}

src/main.rs

@@ -2,7 +2,7 @@ use avian3d::prelude::{
     Collider, ColliderDensity, CollisionLayers, CollisionMargin, Friction, PhysicsGizmos, RigidBody,
 };
 use bevy::{
-    color::palettes::css::SILVER,
+    color::{palettes::css::SILVER, Alpha},
     pbr::MeshMaterial3d,
     prelude::{
         default, App, AppGizmoBuilder, Assets, BuildChildren, ButtonInput, ChildBuild, Color,
@@ -15,10 +15,12 @@ use bevy_inspector_egui::quick::WorldInspectorPlugin;
 
 mod bike;
 mod camera;
+mod input;
 mod physics;
 
 use bike::CyberBikePlugin;
 use camera::CamPlug;
+use input::CyberInputPlugin;
 use physics::CyberPhysicsPlugin;
 
 fn main() {
@@ -27,6 +29,7 @@ fn main() {
         DefaultPlugins,
         CamPlug,
         CyberBikePlugin,
+        CyberInputPlugin,
         CyberPhysicsPlugin,
         WorldInspectorPlugin::new(),
     ))
@@ -57,7 +60,7 @@ fn ground_and_light(
         .spawn((
             spatial_bundle,
             RigidBody::Static,
-            Collider::cuboid(50.0, 5., 50.0),
+            Collider::cuboid(500.0, 5., 500.0),
             CollisionMargin(0.1),
             ColliderDensity(1000.0),
             CollisionLayers::ALL,
@@ -65,8 +68,8 @@ fn ground_and_light(
         ))
         .with_children(|p| {
             let bundle = (
-                Mesh3d(meshes.add(Plane3d::default().mesh().size(50.0, 50.0))),
-                MeshMaterial3d(materials.add(Color::from(SILVER))),
+                Mesh3d(meshes.add(Plane3d::default().mesh().size(500.0, 500.0))),
+                MeshMaterial3d(materials.add(Color::from(SILVER).with_alpha(0.7))),
                 Transform::from_xyz(0.0, 2.5, 0.0),
                 Visibility::Visible,
             );

src/physics.rs

@@ -45,12 +45,6 @@ impl Default for CatControllerState {
 }
 
 impl CatControllerState {
-    pub fn decay(&mut self) {
-        if self.roll_integral.abs() > 0.001 {
-            self.roll_integral *= self.decay_factor;
-        }
-    }
-
     pub fn update_roll(&mut self, error: f32, dt: f32) -> (f32, f32) {
         let lim = self.roll_limit;
         self.roll_integral = (self.roll_integral + (error * dt)).min(lim).max(-lim);
@@ -58,29 +52,19 @@ 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 {
-    use std::f32::consts::FRAC_PI_3;
+    use std::f32::consts::{FRAC_PI_3, FRAC_PI_4};
 
-    use avian3d::prelude::*;
-    use bevy::prelude::*;
+    use avian3d::{math::Scalar, prelude::*};
+    use bevy::{color, math::VectorSpace, prelude::*};
 
     use super::{CatControllerSettings, CatControllerState, CyberLean};
-    use crate::bike::CyberBikeBody;
-
-    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, WheelConfig, WheelState},
+        input::InputState,
+    };
 
     fn rotate_point(pt: &Vec3, rot: &Quat) -> Vec3 {
         // thanks to https://danceswithcode.net/engineeringnotes/quaternions/quaternions.html
@@ -95,13 +79,15 @@ mod systems {
 
     pub(super) fn calculate_lean(
         bike_state: Query<(&LinearVelocity, &Transform), With<CyberBikeBody>>,
+        input: Res<InputState>,
         mut lean: ResMut<CyberLean>,
     ) {
         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;
+        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);
@@ -121,6 +107,7 @@ mod systems {
         mut gizmos: Gizmos,
     ) {
         let (xform, mut torque, mut control_vars) = bike_query.single_mut();
+
         let world_up = Vec3::Y; //xform.translation.normalize();
         let rot = Quat::from_axis_angle(*xform.back(), lean.lean);
         let target_up = rotate_point(&world_up, &rot).normalize();
@@ -154,15 +141,131 @@ mod systems {
                 torque.apply_torque(tork);
                 gizmos.arrow(
                     xform.translation + Vec3::Y,
-                    xform.translation + tork + Vec3::Y,
+                    xform.translation + mag * *xform.right() + Vec3::Y,
                     Color::WHITE,
                 );
             }
         }
     }
+
+    fn yaw_to_angle(yaw: f32) -> f32 {
+        let v = yaw.powi(5) * FRAC_PI_4;
+        if v.is_normal() {
+            v
+        } else {
+            0.0
+        }
+    }
+
+    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>,
+        >,
+        caster_query: Query<(&ShapeCaster, &ShapeHits, &CyberWheel)>,
+        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;
+        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() {
+            match wheel {
+                CyberWheel::Front => {
+                    front_caster = caster;
+                    front_hits = hits;
+                }
+                CyberWheel::Rear => {
+                    rear_caster = caster;
+                    rear_hits = hits;
+                }
+            }
+        }
+        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);
+            }
+        }
+    }
+
+    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;
+
+        up_force
+    }
 }
 
-use systems::{apply_lean, calculate_lean};
+use systems::{apply_lean, calculate_lean, suspension};
 
 pub struct CyberPhysicsPlugin;
 
@@ -175,6 +278,12 @@ 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(Startup, |mut gravity: ResMut<Gravity>| {
+                gravity.0 *= 0.02;
+            })
+            .add_systems(
+                FixedUpdate,
+                (calculate_lean, apply_lean, suspension).chain(),
+            );
     }
 }