add drag, bias power to front

src/bike.rs

@@ -6,13 +6,13 @@ use bevy::prelude::*;
 
 use crate::physics::CatControllerState;
 
-pub const SPRING_CONSTANT: Scalar = 50.0;
+pub const SPRING_CONSTANT: Scalar = 60.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 FRICTION_COEFF: Scalar = 0.8;
-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: Scalar = 1.5 + WHEEL_RADIUS;
+pub const FRICTION_COEFF: Scalar = 0.9;
+pub const FRONT_ATTACH: Vec3 = Vec3::new(0.0, 0.0, -0.5);
+pub const REAR_ATTACH: Vec3 = Vec3::new(0.0, 0.0, 0.5);
 
 #[derive(Component)]
 pub struct CyberBikeBody;
@@ -92,7 +92,7 @@ fn spawn_bike(
             SleepingDisabled,
             CyberBikeBody,
             CatControllerState::default(),
-            ColliderDensity(0.6),
+            ColliderDensity(1.2),
             AngularDamping(0.2),
             LinearDamping(0.1),
             ExternalForce::ZERO.with_persistence(false),
@@ -120,7 +120,7 @@ fn spawn_wheels(
 ) {
     let mesh: Mesh = Sphere::new(WHEEL_RADIUS).into();
 
-    let front_rake = Vec3::new(0.0, -1.0, -0.57).normalize(); // about 30 degrees
+    let front_rake = Vec3::new(0.0, -1.0, -0.9).normalize(); // about 30 degrees
     let front_wheel_pos = FRONT_ATTACH + (front_rake * REST_DISTANCE);
 
     wheel_caster(
@@ -148,7 +148,7 @@ fn spawn_wheels(
         CyberWheel::Front,
     );
 
-    let rear_rake = Vec3::new(0.0, -1.0, 0.57).normalize();
+    let rear_rake = Vec3::new(0.0, -1.0, 0.9).normalize();
     let rear_wheel_pos = REAR_ATTACH + (rear_rake * REST_DISTANCE);
 
     wheel_caster(

src/physics.rs

@@ -1,6 +1,8 @@
-use avian3d::prelude::*;
+use avian3d::{math::Scalar, prelude::*};
 use bevy::prelude::*;
 
+const DRAG_COEFF: Scalar = 0.1;
+
 #[derive(Resource, Default, Debug, Reflect)]
 #[reflect(Resource)]
 struct CyberLean {
@@ -18,9 +20,9 @@ pub struct CatControllerSettings {
 impl Default for CatControllerSettings {
     fn default() -> Self {
         Self {
-            kp: 30.0,
-            kd: 7.0,
-            ki: 1.0,
+            kp: 16.0,
+            kd: 2.0,
+            ki: 0.9,
         }
     }
 }
@@ -29,7 +31,6 @@ impl Default for CatControllerSettings {
 pub struct CatControllerState {
     pub roll_integral: f32,
     pub roll_prev: f32,
-    decay_factor: f32,
     roll_limit: f32,
 }
 
@@ -38,7 +39,6 @@ impl Default for CatControllerState {
         Self {
             roll_integral: Default::default(),
             roll_prev: Default::default(),
-            decay_factor: 0.99,
             roll_limit: 1.5,
         }
     }
@@ -60,7 +60,7 @@ mod systems {
     use avian3d::prelude::*;
     use bevy::prelude::*;
 
-    use super::{CatControllerSettings, CatControllerState, CyberLean};
+    use super::{CatControllerSettings, CatControllerState, CyberLean, DRAG_COEFF};
     use crate::{
         bike::{CyberBikeBody, CyberWheel, WheelConfig, WheelState, WHEEL_RADIUS},
         input::InputState,
@@ -105,13 +105,21 @@ mod systems {
     }
 
     pub(super) fn apply_lean(
-        mut bike_query: Query<(&Transform, &mut ExternalTorque, &mut CatControllerState)>,
+        mut bike_query: Query<(&Transform, &mut ExternalForce, &mut CatControllerState)>,
+        wheels: Query<&WheelState>,
         time: Res<Time>,
         settings: Res<CatControllerSettings>,
         lean: Res<CyberLean>,
         mut gizmos: Gizmos,
     ) {
-        let (xform, mut torque, mut control_vars) = bike_query.single_mut();
+        let (xform, mut force, mut control_vars) = bike_query.single_mut();
+
+        let mut factor = 1.0;
+        for wheel in wheels.iter() {
+            if wheel.contact_point.is_none() {
+                factor -= 0.25;
+            }
+        }
 
         let world_up = Vec3::Y; //xform.translation.normalize();
         let rot = Quat::from_axis_angle(*xform.back(), lean.lean);
@@ -127,7 +135,7 @@ mod systems {
         // show which is forward
         gizmos.arrow(
             *xform.forward() + xform.translation,
-            1.5 * *xform.forward() + xform.translation,
+            2.5 * *xform.forward() + xform.translation,
             bevy::color::palettes::basic::PURPLE,
         );
 
@@ -142,11 +150,15 @@ mod systems {
             let mag =
                 (settings.kp * roll_error) + (settings.ki * integral) + (settings.kd * derivative);
             if mag.is_finite() {
-                let tork = mag * *xform.back();
-                torque.apply_torque(tork);
+                let lean_force = factor * mag * *xform.left();
+                force.apply_force_at_point(
+                    lean_force,
+                    xform.translation + *xform.up(),
+                    xform.translation,
+                );
                 gizmos.arrow(
-                    xform.translation + Vec3::Y,
-                    xform.translation + mag * *xform.right() + Vec3::Y,
+                    xform.translation + *xform.up(),
+                    xform.translation + *xform.up() + lean_force,
                     Color::WHITE,
                 );
             }
@@ -239,7 +251,12 @@ mod systems {
 
     pub(super) fn steering(
         mut bike_query: Query<
-            (&Transform, &mut ExternalForce, RigidBodyQueryReadOnly),
+            (
+                &Transform,
+                &LinearVelocity,
+                &mut ExternalForce,
+                RigidBodyQueryReadOnly,
+            ),
             With<CyberBikeBody>,
         >,
         mut wheels: Query<(&mut WheelState, &WheelConfig, &CyberWheel)>,
@@ -247,16 +264,16 @@ mod systems {
         input: Res<InputState>,
         mut gizmos: Gizmos,
     ) {
-        let Ok((bike_xform, mut bike_force, bike_body)) = bike_query.get_single_mut() else {
+        let Ok((bike_xform, bike_vel, mut bike_force, bike_body)) = bike_query.get_single_mut()
+        else {
             bevy::log::warn!("No entity for bike_query.");
             return;
         };
+        let bike_vel = bike_vel.0;
 
         let dt = time.delta().as_secs_f32();
-        let max_thrust = 1000.0;
-        let thrust_force = input.throttle * max_thrust * dt;
-        let max_yaw = 500.0;
-        let yaw_force = input.yaw * max_yaw * dt;
+        let max_thrust = 2000.0;
+        let yaw_angle = -yaw_to_angle(input.yaw);
 
         for (mut state, config, wheel) in wheels.iter_mut() {
             if let Some(contact_point) = state.contact_point {
@@ -264,45 +281,34 @@ mod systems {
                 let normal = state.contact_normal.unwrap().normalize();
                 let max_force_mag = state.force_normal * config.friction;
 
-                let yaw_dir = match wheel {
-                    CyberWheel::Front => normal.cross(*bike_xform.back()).normalize_or_zero(),
-                    CyberWheel::Rear => Vec3::ZERO,
+                let rot = Quat::from_axis_angle(normal, yaw_angle);
+                let forward = normal.cross(*bike_xform.right());
+                let thrust_mag = input.throttle * max_thrust * dt;
+                let (thrust_dir, thrust_force) = match wheel {
+                    CyberWheel::Rear => (forward, thrust_mag * 0.1),
+                    CyberWheel::Front => (rot * forward, thrust_mag),
                 };
 
-                let thrust_dir = normal.cross(*bike_xform.right());
-
-                let vel = bike_body.velocity_at_point(contact_point - bike_xform.translation);
-
-                let yaw = yaw_force * yaw_dir;
                 let thrust = thrust_force * thrust_dir;
-                let yust = yaw + thrust;
-
-                bevy::log::debug!("yust: {yust:?}");
-                bevy::log::debug!("{:?}", bike_xform.left());
-
-                let left = bike_xform.forward().cross(normal);
 
                 let friction_dir = match wheel {
-                    CyberWheel::Front => {
-                        if yaw_force + thrust_force > 0.1 {
-                            normal.cross(yust.normalize()).normalize()
-                        } else {
-                            left
-                        }
-                    }
-                    CyberWheel::Rear => left,
+                    CyberWheel::Front => normal.cross(thrust_dir),
+                    CyberWheel::Rear => normal.cross(*bike_xform.forward()),
                 };
 
+                let vel = bike_body.velocity_at_point(contact_point - bike_xform.translation);
                 let friction_factor = -0.025 * vel.dot(friction_dir);
                 let friction = (friction_factor / dt) * friction_dir;
 
-                let mut force = yust + friction;
+                let diff = bike_vel - vel;
+                bevy::log::debug!("{wheel:?}: vel diff: {diff:?} ({})", diff.length(),);
+
+                let mut force = thrust + friction;
                 force *= dt * 50.0;
                 let force_mag = force.length();
                 if force_mag > max_force_mag {
                     state.sliding = true;
                     force = force.normalize_or_zero() * max_force_mag;
-                    //bevy::log::info!("clamping {wheel:?} to {max_force_mag}: {friction_factor}");
                 } else {
                     state.sliding = false;
                 }
@@ -313,10 +319,44 @@ mod systems {
                     contact_point + friction,
                     Color::linear_rgb(1., 1., 0.2),
                 );
+                gizmos.arrow(
+                    contact_point,
+                    contact_point + thrust,
+                    Color::linear_rgb(1., 0., 0.2),
+                );
             }
         }
     }
 
+    pub(super) fn drag(
+        mut bike_query: Query<
+            (&Transform, &LinearVelocity, &mut ExternalForce),
+            With<CyberBikeBody>,
+        >,
+        mut gizmos: Gizmos,
+        time: Res<Time>,
+    ) {
+        let (xform, vel, mut force) = bike_query.single_mut();
+
+        let dt = time.delta_secs();
+
+        let speed = vel.length();
+        let dspeed = speed.powf(1.2) * 0.1;
+        let dir = vel.normalize_or_zero();
+        let drag = -dspeed * dt * DRAG_COEFF * dir;
+        if speed > 1.0 {
+            force.apply_force_at_point(drag, xform.translation, xform.translation);
+        }
+
+        bevy::log::debug!("speed: {}, drag force: {}", speed, drag.length());
+
+        gizmos.arrow(
+            xform.translation,
+            xform.translation + drag * 10.,
+            Color::linear_rgb(1.0, 0.0, 0.0),
+        );
+    }
+
     pub(super) fn tweak(
         mut config: Query<&mut WheelConfig>,
         mut keys: ResMut<ButtonInput<KeyCode>>,
@@ -355,7 +395,7 @@ mod systems {
     }
 }
 
-use systems::{apply_lean, calculate_lean, steering, suspension, tweak};
+use systems::{apply_lean, calculate_lean, drag, steering, suspension, tweak};
 
 pub struct CyberPhysicsPlugin;
 
@@ -373,7 +413,7 @@ impl Plugin for CyberPhysicsPlugin {
             })
             .add_systems(
                 FixedUpdate,
-                (calculate_lean, apply_lean, suspension, steering).chain(),
+                (calculate_lean, apply_lean, suspension, steering, drag).chain(),
             )
             .add_systems(Update, tweak);
     }