From e666c972acf6da3697cb0a5e20f65d3ff2154c5d Mon Sep 17 00:00:00 2001 From: Joe Ardent Date: Sun, 5 Feb 2023 14:59:41 -0800 Subject: [PATCH] quasi-merge from rolling_wheels --- src/action.rs | 143 ------------------- src/action/components.rs | 123 +++++++++++++++++ src/action/mod.rs | 37 +++++ src/action/systems.rs | 183 +++++++++++++++++++++++++ src/bike.rs | 287 --------------------------------------- src/bike/body.rs | 81 +++++++++++ src/bike/components.rs | 41 ++++++ src/bike/mod.rs | 26 ++++ src/bike/wheels.rs | 157 +++++++++++++++++++++ src/camera.rs | 34 +---- src/glamor.rs | 33 ++++- src/main.rs | 19 +-- 12 files changed, 682 insertions(+), 482 deletions(-) delete mode 100644 src/action.rs create mode 100644 src/action/components.rs create mode 100644 src/action/mod.rs create mode 100644 src/action/systems.rs delete mode 100644 src/bike.rs create mode 100644 src/bike/body.rs create mode 100644 src/bike/components.rs create mode 100644 src/bike/mod.rs create mode 100644 src/bike/wheels.rs diff --git a/src/action.rs b/src/action.rs deleted file mode 100644 index 3819d67..0000000 --- a/src/action.rs +++ /dev/null @@ -1,143 +0,0 @@ -use bevy::{ - diagnostic::{Diagnostics, FrameTimeDiagnosticsPlugin}, - prelude::*, -}; -use bevy_rapier3d::prelude::{ - ExternalForce, Friction, NoUserData, RapierConfiguration, RapierPhysicsPlugin, Velocity, -}; - -use crate::{ - bike::{CyberBikeBody, CyberBikeCollider, CyberBikeControl}, - input::InputState, -}; - -#[derive(Resource)] -pub struct MovementSettings { - pub accel: f32, - pub gravity: f32, - pub sensitivity: f32, -} - -impl Default for MovementSettings { - fn default() -> Self { - Self { - sensitivity: 10.0, - accel: 20.0, - gravity: 9.8, - } - } -} - -#[derive(Debug, Resource, Reflect)] -#[reflect(Resource)] -struct CatControllerSettings { - pub kp: f32, - pub kd: f32, - pub kws: f32, -} - -impl Default for CatControllerSettings { - fn default() -> Self { - Self { - kp: 10.0, - kd: 4.0, - kws: 0.85, - } - } -} - -fn zero_gravity(mut config: ResMut) { - config.gravity = Vec3::ZERO; -} - -fn gravity( - mut query: Query<(&Transform, &mut ExternalForce), With>, - settings: Res, -) { - let (_xform, mut forces) = query.single_mut(); - let grav = Vec3::Y * -settings.gravity; - forces.force = grav; -} - -fn falling_cat( - mut bike_query: Query<(&Transform, &mut ExternalForce, &mut CyberBikeControl)>, - _diagnostics: Res, - settings: Res, -) { - let (xform, mut forces, mut control_vars) = bike_query.single_mut(); - let up = Vec3::Y; - let bike_up = xform.up(); - - let torque = bike_up.cross(up).normalize_or_zero(); - let cos = up.dot(bike_up); - let cos = if cos.is_finite() { cos } else { 1.0 }; - - let error = 1.0 - cos; - - let derivative = error - control_vars.prev_error; - control_vars.prev_error = error; - // this integral term is not an integral, it's more like a weighted moving sum - let weighted_sum = control_vars.error_sum + error; - control_vars.error_sum = weighted_sum * 0.8; - - let mag = (settings.kp * error) + (settings.kws * weighted_sum) + (settings.kd * derivative); - - #[cfg(feature = "inspector")] - if let Some(count) = _diagnostics - .get(FrameTimeDiagnosticsPlugin::FRAME_COUNT) - .and_then(|d| d.smoothed()) - .map(|x| x as u64) - { - if count % 30 == 0 { - dbg!(&control_vars, mag, cos, derivative); - } - } - - forces.torque = torque * mag; -} - -fn input_forces( - settings: Res, - input: Res, - mut cquery: Query<&mut Friction, With>, - mut bquery: Query<(&Transform, &mut ExternalForce), With>, -) { - let (xform, mut forces) = bquery.single_mut(); - let mut friction = cquery.single_mut(); - - // thrust - let thrust = xform.forward() * input.throttle * settings.accel; - forces.force += thrust; - - // brake - friction.coefficient = if input.brake { 2.0 } else { 0.0 }; - - // steering - let torque = xform.down() * input.yaw * settings.sensitivity; - forces.torque += torque; -} - -fn drag(mut query: Query<(&Velocity, &mut ExternalForce), With>) { - let (vels, mut forces) = query.single_mut(); - - if let Some(vel) = vels.linvel.try_normalize() { - let v2 = vels.linvel.length_squared(); - forces.force -= vel * (v2 * 0.02); - } -} - -pub struct CyberActionPlugin; -impl Plugin for CyberActionPlugin { - fn build(&self, app: &mut App) { - app.init_resource::() - .init_resource::() - .register_type::() - .add_plugin(RapierPhysicsPlugin::::default()) - .add_plugin(FrameTimeDiagnosticsPlugin::default()) - .add_startup_system(zero_gravity) - .add_system(gravity.before("cat")) - .add_system(falling_cat.label("cat")) - .add_system(input_forces.label("iforces").after("cat")) - .add_system(drag.label("drag").after("iforces")); - } -} diff --git a/src/action/components.rs b/src/action/components.rs new file mode 100644 index 0000000..34cf983 --- /dev/null +++ b/src/action/components.rs @@ -0,0 +1,123 @@ +use std::time::Instant; + +use bevy::{ + prelude::{Component, ReflectResource, Resource, Vec3}, + reflect::Reflect, +}; + +#[derive(Debug, Resource)] +pub(crate) struct ActionDebugInstant(pub Instant); + +impl Default for ActionDebugInstant { + fn default() -> Self { + ActionDebugInstant(Instant::now()) + } +} + +#[derive(Debug, Component)] +pub(super) struct Tunneling { + pub frames: usize, + pub dir: Vec3, +} + +impl Default for Tunneling { + fn default() -> Self { + Tunneling { + frames: 15, + dir: Vec3::ZERO, + } + } +} + +#[derive(Debug, Resource, Reflect)] +#[reflect(Resource)] +pub struct MovementSettings { + pub accel: f32, + pub gravity: f32, + pub sensitivity: f32, +} + +impl Default for MovementSettings { + fn default() -> Self { + Self { + sensitivity: 6.0, + accel: 40.0, + gravity: 9.8, + } + } +} + +#[derive(Debug, Resource, Reflect)] +#[reflect(Resource)] +pub struct CatControllerSettings { + pub kp: f32, + pub kd: f32, + pub ki: f32, +} + +impl Default for CatControllerSettings { + fn default() -> Self { + Self { + kp: 40.0, + kd: 5.0, + ki: 0.1, + } + } +} + +#[derive(Component, Debug, Clone, Copy)] +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 { + fn default() -> Self { + 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.8, + } + } +} + +impl CatControllerState { + pub fn decay(&mut self) { + 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) { + let lim = self.roll_limit; + self.roll_integral = (self.roll_integral + (error * dt)).min(lim).max(-lim); + let derivative = (error - self.roll_prev) / dt; + self.roll_prev = error; + (derivative, self.roll_integral) + } + + pub fn update_pitch(&mut self, error: f32, dt: f32) -> (f32, 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; + } +} diff --git a/src/action/mod.rs b/src/action/mod.rs new file mode 100644 index 0000000..3591d35 --- /dev/null +++ b/src/action/mod.rs @@ -0,0 +1,37 @@ +use bevy::{ + diagnostic::FrameTimeDiagnosticsPlugin, + prelude::{App, IntoSystemDescriptor, Plugin}, +}; +use bevy_rapier3d::prelude::{NoUserData, RapierPhysicsPlugin}; + +mod components; +mod systems; + +pub use components::*; +use systems::*; + +pub struct CyberActionPlugin; +impl Plugin for CyberActionPlugin { + fn build(&self, app: &mut App) { + app.init_resource::() + .register_type::() + .init_resource::() + .init_resource::() + .register_type::() + .add_plugin(RapierPhysicsPlugin::::default()) + .add_plugin(FrameTimeDiagnosticsPlugin::default()) + .add_startup_system(zero_gravity) + .add_system(surface_fix.label("surface_fix")) + .add_system(gravity.before("cat")) + .add_system(falling_cat.label("cat")) + .add_system(input_forces.label("iforces").after("cat")) + .add_system( + tunnel_out + .label("tunnel") + .before("surface_fix") + .after("drag"), + ) + .add_system(surface_fix.label("surface_fix").after("cat")) + .add_system(drag.label("drag").after("iforces")); + } +} diff --git a/src/action/systems.rs b/src/action/systems.rs new file mode 100644 index 0000000..02bc9e1 --- /dev/null +++ b/src/action/systems.rs @@ -0,0 +1,183 @@ +use std::f32::consts::PI; +#[cfg(feature = "inspector")] +use std::time::Instant; + +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, + }, + rapier::prelude::JointAxis, +}; + +#[cfg(feature = "inspector")] +use super::ActionDebugInstant; +use super::{CatControllerSettings, CatControllerState, MovementSettings, Tunneling}; +use crate::{ + bike::{CyberBikeBody, CyberSteering, CyberWheel, BIKE_WHEEL_COLLISION_GROUP}, + input::InputState, +}; + +/// Disable gravity in Rapier. +pub(super) fn zero_gravity(mut config: ResMut) { + config.gravity = Vec3::ZERO; +} + +/// The gravity vector points from the cyberbike to the center of the planet. +pub(super) fn gravity( + mut query: Query<(&Transform, &mut ExternalForce, &ReadMassProperties), With>, + settings: Res, +) { + let (xform, mut forces, mprops) = query.single_mut(); + let grav = xform.translation.normalize() * -settings.gravity * mprops.0.mass; + forces.force = grav; + forces.torque = Vec3::ZERO; +} + +/// 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