something's weird

src/physics.rs

@@ -18,9 +18,9 @@ pub struct CatControllerSettings {
 impl Default for CatControllerSettings {
     fn default() -> Self {
         Self {
-            kp: 30.0,
+            kp: 25.0,
-            kd: 7.0,
+            kd: 3.0,
-            ki: 1.0,
+            ki: 1.1,
         }
     }
 }
@@ -127,7 +127,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,
         );
 
@@ -239,7 +239,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 +252,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 yaw_angle = -yaw_to_angle(input.yaw);
-        let max_yaw = 500.0;
-        let yaw_force = input.yaw * max_yaw * dt;
 
         for (mut state, config, wheel) in wheels.iter_mut() {
             if let Some(contact_point) = state.contact_point {
@@ -264,39 +269,32 @@ mod systems {
                 let normal = state.contact_normal.unwrap().normalize();
                 let max_force_mag = state.force_normal * config.friction;
 
-                let yaw_dir = match wheel {
+                let rot = Quat::from_axis_angle(normal, yaw_angle);
-                    CyberWheel::Front => normal.cross(*bike_xform.back()).normalize_or_zero(),
+                let forward = normal.cross(*bike_xform.right());
-                    CyberWheel::Rear => Vec3::ZERO,
+                let (thrust_dir, thrust_force) = match wheel {
+                    CyberWheel::Rear => (forward, input.throttle * max_thrust * dt),
+                    CyberWheel::Front => (rot * forward, 0.0),
                 };
 
-                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 => {
+                    CyberWheel::Front => normal.cross(thrust_dir),
-                        if yaw_force + thrust_force > 0.1 {
+                    CyberWheel::Rear => normal.cross(*bike_xform.forward()),
-                            normal.cross(yust.normalize()).normalize()
-                        } else {
-                            left
-                        }
-                    }
-                    CyberWheel::Rear => left,
                 };
 
+                // bevy::log::debug!(
+                //     "{wheel:?}, thrust_dir: {thrust_dir:?}, friction_dir: {friction_dir:?}, dot: {}",
+                //     thrust_dir.dot(friction_dir)
+                // );
+
+                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;
+                bevy::log::debug!("{wheel:?}: vel diff: {:?}", bike_vel - vel);
+
+                let mut force = thrust + friction;
                 force *= dt * 50.0;
                 let force_mag = force.length();
                 if force_mag > max_force_mag {
@@ -313,6 +311,11 @@ 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),
+                );
             }
         }
     }