merge cyber-lean

src/action/components.rs

@@ -15,10 +15,12 @@ impl Default for ActionDebugInstant {
 }
 
 impl ActionDebugInstant {
+    #[allow(dead_code)]
     pub fn reset(&mut self) {
         self.0 = Instant::now();
     }
 
+    #[allow(dead_code)]
     pub fn elapsed(&self) -> Duration {
         self.0.elapsed()
     }
@@ -68,9 +70,9 @@ pub struct CatControllerSettings {
 impl Default for CatControllerSettings {
     fn default() -> Self {
         Self {
-            kp: 40.0,
+            kp: 80.0,
-            kd: 5.0,
+            kd: 10.0,
-            ki: 0.1,
+            ki: 0.4,
         }
     }
 }
@@ -79,11 +81,8 @@ impl Default for CatControllerSettings {
 pub struct CatControllerState {
     pub roll_integral: f32,
     pub roll_prev: f32,
-    pub pitch_integral: f32,
-    pub pitch_prev: f32,
     decay_factor: f32,
     roll_limit: f32,
-    pitch_limit: f32,
 }
 
 impl Default for CatControllerState {
@@ -91,11 +90,8 @@ impl Default for CatControllerState {
         Self {
             roll_integral: Default::default(),
             roll_prev: Default::default(),
-            pitch_integral: Default::default(),
-            pitch_prev: Default::default(),
             decay_factor: 0.99,
             roll_limit: 1.5,
-            pitch_limit: 0.1,
         }
     }
 }
@@ -105,9 +101,6 @@ impl CatControllerState {
         if self.roll_integral.abs() > 0.001 {
             self.roll_integral *= self.decay_factor;
         }
-        if self.pitch_integral.abs() > 0.001 {
-            self.pitch_integral *= self.decay_factor;
-        }
     }
 
     pub fn update_roll(&mut self, error: f32, dt: f32) -> (f32, f32) {
@@ -118,16 +111,7 @@ impl CatControllerState {
         (derivative, self.roll_integral)
     }
 
-    pub fn update_pitch(&mut self, error: f32, dt: f32) -> (f32, f32) {
+    pub fn set_integral_limits(&mut self, roll: f32) {
-        let lim = self.pitch_limit;
-        self.pitch_integral = (self.pitch_integral + (error * dt)).min(lim).max(-lim);
-        let derivative = (error - self.pitch_prev) / dt;
-        self.pitch_prev = error;
-        (derivative, self.pitch_integral)
-    }
-
-    pub fn set_integral_limits(&mut self, roll: f32, pitch: f32) {
         self.roll_limit = roll;
-        self.pitch_limit = pitch;
     }
 }

src/action/mod.rs

@@ -1,6 +1,7 @@
 use bevy::{
     diagnostic::FrameTimeDiagnosticsPlugin,
-    prelude::{App, IntoSystemDescriptor, Plugin},
+    prelude::{App, IntoSystemDescriptor, Plugin, ReflectResource, Resource},
+    reflect::Reflect,
 };
 use bevy_rapier3d::prelude::{NoUserData, RapierPhysicsPlugin};
 
@@ -10,6 +11,12 @@ mod systems;
 pub use components::*;
 use systems::*;
 
+#[derive(Resource, Default, Debug, Reflect)]
+#[reflect(Resource)]
+struct CyberLean {
+    pub lean: f32,
+}
+
 pub struct CyberActionPlugin;
 impl Plugin for CyberActionPlugin {
     fn build(&self, app: &mut App) {
@@ -17,11 +24,14 @@ impl Plugin for CyberActionPlugin {
             .register_type::<MovementSettings>()
             .init_resource::<CatControllerSettings>()
             .init_resource::<ActionDebugInstant>()
+            .init_resource::<CyberLean>()
+            .register_type::<CyberLean>()
             .register_type::<CatControllerSettings>()
             .add_plugin(RapierPhysicsPlugin::<NoUserData>::default())
             .add_plugin(FrameTimeDiagnosticsPlugin::default())
             .add_system(surface_fix.label("surface_fix"))
-            .add_system(gravity.before("cat"))
+            .add_system(gravity.label("gravity").before("cat"))
+            .add_system(cyber_lean.before("cat").after("gravity"))
             .add_system(falling_cat.label("cat"))
             .add_system(input_forces.label("iforces").after("cat"))
             .add_system(

src/action/systems.rs

@@ -1,6 +1,8 @@
-use std::f32::consts::PI;
+use std::f32::consts::{FRAC_PI_3, FRAC_PI_4};
 
-use bevy::prelude::{Commands, Entity, Query, Res, ResMut, Time, Transform, Vec3, With, Without};
+use bevy::prelude::{
+    Commands, Entity, Quat, Query, Res, ResMut, Time, Transform, Vec3, With, Without,
+};
 use bevy_rapier3d::prelude::{
     CollisionGroups, ExternalForce, Group, MultibodyJoint, QueryFilter, RapierConfiguration,
     RapierContext, ReadMassProperties, Velocity,
@@ -8,13 +10,26 @@ use bevy_rapier3d::prelude::{
 
 #[cfg(feature = "inspector")]
 use super::ActionDebugInstant;
-use super::{CatControllerSettings, CatControllerState, MovementSettings, Tunneling};
+use super::{CatControllerSettings, CatControllerState, CyberLean, MovementSettings, Tunneling};
 use crate::{
     bike::{CyberBikeBody, CyberSteering, CyberWheel, BIKE_WHEEL_COLLISION_GROUP},
     input::InputState,
 };
 
-const PITCH_SCALE: f32 = 0.2;
+fn yaw_to_angle(yaw: f32) -> f32 {
+    yaw.powi(3) * FRAC_PI_4
+}
+
+fn rotate_point(pt: &Vec3, rot: &Quat) -> Vec3 {
+    // thanks to https://danceswithcode.net/engineeringnotes/quaternions/quaternions.html
+    let [x, y, z] = pt.to_array();
+    let qpt = Quat::from_xyzw(x, y, z, 0.0);
+    // p' = rot^-1 * qpt * rot
+    let rot_qpt = rot.inverse() * qpt * *rot;
+
+    // why does this need to be inverted???
+    -Vec3::from_array([rot_qpt.x, rot_qpt.y, rot_qpt.z])
+}
 
 /// The gravity vector points from the cyberbike to the center of the planet.
 pub(super) fn gravity(
@@ -28,21 +43,49 @@ pub(super) fn gravity(
     forces.torque = Vec3::ZERO;
 }
 
+/// The desired lean angle, given steering input and speed.
+pub(super) fn cyber_lean(
+    bike_state: Query<(&Velocity, &Transform), With<CyberBikeBody>>,
+    wheels: Query<&Transform, With<CyberWheel>>,
+    input: Res<InputState>,
+    gravity_settings: Res<MovementSettings>,
+    mut lean: ResMut<CyberLean>,
+) {
+    let (velocity, xform) = bike_state.single();
+    let vel = velocity.linvel.dot(xform.forward());
+    let v_squared = vel.powi(2);
+    let steering_angle = yaw_to_angle(input.yaw);
+    let wheels: Vec<_> = wheels.iter().map(|w| w.translation).collect();
+    let wheel_base = (wheels[0] - wheels[1]).length();
+    let radius = wheel_base / steering_angle.tan();
+    let gravity = gravity_settings.gravity;
+    let v2_r = v_squared / radius;
+    let tan_theta = (v2_r / gravity).clamp(-FRAC_PI_3, FRAC_PI_3);
+
+    if tan_theta.is_finite() && !tan_theta.is_subnormal() {
+        lean.lean = tan_theta.atan().clamp(-FRAC_PI_4, FRAC_PI_4);
+    } else {
+        lean.lean = 0.0;
+    }
+}
+
 /// PID-based controller for stabilizing attitude; keeps the cyberbike upright.
 pub(super) fn falling_cat(
     mut bike_query: Query<(&Transform, &mut ExternalForce, &mut CatControllerState)>,
     time: Res<Time>,
     settings: Res<CatControllerSettings>,
+    lean: Res<CyberLean>,
     #[cfg(feature = "inspector")] mut debug_instant: ResMut<ActionDebugInstant>,
 ) {
     let (xform, mut forces, mut control_vars) = bike_query.single_mut();
-    let wup = xform.translation.normalize();
+    let world_up = xform.translation.normalize();
-    let bike_up = xform.up();
+    let rot = Quat::from_axis_angle(xform.back(), lean.lean);
+    let target_up = rotate_point(&world_up, &rot).normalize();
 
-    let wright = xform.forward().cross(wup).normalize();
+    let bike_right = xform.right();
 
-    let roll_error = wright.dot(bike_up);
+    let roll_error = bike_right.dot(target_up);
-    let pitch_error = wup.dot(xform.back());
+    let pitch_error = world_up.dot(xform.back());
 
     // only try to correct roll if we're not totally vertical
     if pitch_error.abs() < 0.95 {
@@ -52,26 +95,14 @@ pub(super) fn falling_cat(
         if mag.is_finite() {
             forces.torque += xform.back() * mag;
         }
-    }
-
-    // we can do pitch correction any time, it's not coupled to roll
-    let scaled_pitch_error = pitch_error * PITCH_SCALE;
-    let (derivative, integral) =
-        control_vars.update_pitch(scaled_pitch_error, time.delta_seconds());
-    let mag =
-        (settings.kp * scaled_pitch_error) + (settings.ki * integral) + (settings.kd * derivative);
-    if mag.is_finite() {
-        forces.torque += wright * mag;
-    }
-
         #[cfg(feature = "inspector")]
         {
             if debug_instant.elapsed().as_millis() > 1000 {
-            dbg!(&control_vars, mag);
+                dbg!(&control_vars, mag, &target_up);
                 debug_instant.reset();
             }
         }
-    control_vars.decay();
+    }
 }
 
 /// Apply forces to the cyberbike as a result of input.
@@ -89,13 +120,13 @@ pub(super) fn input_forces(
 
     // thrust
     let thrust = xform.forward() * input.throttle * settings.accel;
-    let point = xform.translation + xform.back() * 0.5;
+    let point = xform.translation + xform.back();
     let force = ExternalForce::at_point(thrust, point, xform.translation);
     *forces += force;
 
     // brake
     for mut motor in braking_query.iter_mut() {
-        let factor = if input.brake { 200.00 } else { 0.0 };
+        let factor = if input.brake { 500.00 } else { 0.0 };
         motor.data = (*motor
             .data
             .as_revolute_mut()
@@ -106,7 +137,7 @@ pub(super) fn input_forces(
     }
 
     // steering
-    let angle = input.yaw.powf(3.0) * (PI / 4.0);
+    let angle = yaw_to_angle(input.yaw);
     let mut steering = steering_query.single_mut();
     steering.data = (*steering
         .data