From 37ce29fd713449734ffa900b5233c93768a52b84 Mon Sep 17 00:00:00 2001 From: Joe Ardent Date: Sat, 4 Feb 2023 14:00:56 -0800 Subject: [PATCH 01/12] checkpoint, kinda fucked up --- src/action/components.rs | 8 +-- src/action/systems.rs | 47 ++++++++++------ src/bike/body.rs | 2 +- src/bike/components.rs | 3 + src/bike/wheels.rs | 116 ++++++++++++++++++++++++--------------- src/planet.rs | 2 +- 6 files changed, 112 insertions(+), 66 deletions(-) diff --git a/src/action/components.rs b/src/action/components.rs index 656d6ef..34cf983 100644 --- a/src/action/components.rs +++ b/src/action/components.rs @@ -58,9 +58,9 @@ pub struct CatControllerSettings { impl Default for CatControllerSettings { fn default() -> Self { Self { - kp: 20.0, - kd: 4.5, - ki: 0.07, + kp: 40.0, + kd: 5.0, + ki: 0.1, } } } @@ -85,7 +85,7 @@ impl Default for CatControllerState { pitch_prev: Default::default(), decay_factor: 0.99, roll_limit: 1.5, - pitch_limit: 1.0, + pitch_limit: 0.8, } } } diff --git a/src/action/systems.rs b/src/action/systems.rs index 5b822bb..02bc9e1 100644 --- a/src/action/systems.rs +++ b/src/action/systems.rs @@ -1,17 +1,23 @@ +use std::f32::consts::PI; #[cfg(feature = "inspector")] use std::time::Instant; -use bevy::prelude::{Commands, Entity, Query, Res, ResMut, Time, Transform, Vec3, With, Without}; -use bevy_rapier3d::prelude::{ - CollisionGroups, ExternalForce, Friction, Group, QueryFilter, RapierConfiguration, - RapierContext, ReadMassProperties, Velocity, +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, CyberWheel, BIKE_WHEEL_COLLISION_GROUP}, + bike::{CyberBikeBody, CyberSteering, CyberWheel, BIKE_WHEEL_COLLISION_GROUP}, input::InputState, }; @@ -61,7 +67,7 @@ pub(super) fn falling_cat( let (derivative, integral) = control_vars.update_pitch(pitch_error, time.delta_seconds()); let mag = (settings.kp * pitch_error) + (settings.ki * integral) + (settings.kd * derivative); if mag.is_finite() { - forces.torque += wright * mag; + forces.torque += wright * mag * 0.25; } #[cfg(feature = "inspector")] @@ -79,25 +85,32 @@ pub(super) fn falling_cat( pub(super) fn input_forces( settings: Res, input: Res, - mut cquery: Query<&mut Friction, With>, - mut bquery: Query<(&Transform, &mut ExternalForce), With>, + mut braking_query: Query<&mut MultibodyJoint, With>, + mut body_query: Query< + (&Transform, &mut ExternalForce), + (With, Without), + >, + mut steering_query: Query<&mut Transform, With>, ) { - let (xform, mut forces) = bquery.single_mut(); + let (xform, mut forces) = body_query.single_mut(); // thrust let thrust = xform.forward() * input.throttle * settings.accel; - forces.force += thrust; + let point = xform.translation + xform.back(); + let force = ExternalForce::at_point(thrust, point, xform.translation); + *forces += force; // brake - for mut friction in cquery.iter_mut() { - friction.coefficient = if input.brake { 2.0 } else { 0.0 }; + for mut motor in braking_query.iter_mut() { + let factor = if input.brake { settings.accel } else { 0.0 }; + motor.data.set_motor_velocity(JointAxis::X, 0.0, factor); } // steering - let force = xform.right() * input.yaw * settings.sensitivity; - let point = xform.translation + xform.forward(); - let force = ExternalForce::at_point(force, point, xform.translation); - *forces += force; + let angle = input.yaw * (PI / 2.0); + let mut steering = steering_query.single_mut(); + steering.rotation = Quat::from_axis_angle(Vec3::Y, angle + 90.0f32.to_radians()); + //dbg!(steering.rotation); } /// Don't let the wheels get stuck underneat the planet @@ -153,7 +166,7 @@ pub(super) fn tunnel_out( } tunneling.frames -= 1; force.force = tunneling.dir * settings.gravity * 1.5 * mprops.0.mass; - #[cfg(feature = "inspector")] + //#[cfg(feature = "inspector")] dbg!(&tunneling); } } diff --git a/src/bike/body.rs b/src/bike/body.rs index 765007b..f227804 100644 --- a/src/bike/body.rs +++ b/src/bike/body.rs @@ -82,5 +82,5 @@ pub(super) fn spawn_cyberbike( .insert(CatControllerState::default()) .id(); - spawn_tires(&mut commands, &xform, bike, &wheel_conf, &mut meshterials); + spawn_tires(&mut commands, bike, xform, &wheel_conf, &mut meshterials); } diff --git a/src/bike/components.rs b/src/bike/components.rs index bc89870..f116016 100644 --- a/src/bike/components.rs +++ b/src/bike/components.rs @@ -6,6 +6,9 @@ use bevy::{ #[derive(Component)] pub struct CyberBikeBody; +#[derive(Component)] +pub struct CyberSteering; + #[derive(Debug, Component)] pub struct CyberWheel; diff --git a/src/bike/wheels.rs b/src/bike/wheels.rs index ebf8120..82e5be0 100644 --- a/src/bike/wheels.rs +++ b/src/bike/wheels.rs @@ -1,16 +1,16 @@ use bevy::prelude::{shape::UVSphere as Tire, *}; use bevy_rapier3d::prelude::{ - Ccd, Collider, ColliderMassProperties, CollisionGroups, Damping, ExternalForce, Friction, - MultibodyJoint, PrismaticJointBuilder, Restitution, RigidBody, Sleeping, - TransformInterpolation, + Ccd, CoefficientCombineRule, Collider, ColliderMassProperties, CollisionGroups, Damping, + ExternalForce, Friction, MultibodyJoint, PrismaticJointBuilder, Restitution, + RevoluteJointBuilder, RigidBody, Sleeping, TransformInterpolation, }; -use super::{CyberWheel, Meshterial, WheelConfig, BIKE_WHEEL_COLLISION_GROUP}; +use super::{CyberSteering, CyberWheel, Meshterial, WheelConfig, BIKE_WHEEL_COLLISION_GROUP}; pub fn spawn_tires( commands: &mut Commands, - xform: &Transform, bike: Entity, + xform: Transform, conf: &WheelConfig, meshterials: &mut Meshterial, ) { @@ -47,8 +47,13 @@ pub fn spawn_tires( }; let friction = Friction { - coefficient: 0.0, - ..Default::default() + coefficient: 1.0, + combine_rule: CoefficientCombineRule::Min, + }; + + let restitution = Restitution { + coefficient: 0.8, + combine_rule: CoefficientCombineRule::Average, }; let mut wheel_poses = Vec::with_capacity(2); @@ -58,7 +63,7 @@ pub fn spawn_tires( let wheel_x = 0.0; let wheel_z = -conf.front_forward; let offset = Vec3::new(wheel_x, wheel_y, wheel_z); - wheel_poses.push(offset); + wheel_poses.push((offset, Some(CyberSteering))); } // rear @@ -66,12 +71,10 @@ pub fn spawn_tires( let wheel_x = 0.0; let wheel_z = conf.rear_back; let offset = Vec3::new(wheel_x, wheel_y, wheel_z); - wheel_poses.push(offset); + wheel_poses.push((offset, None)); } - for offset in wheel_poses { - let trans = xform.translation + offset; - let wheel_pos_in_world = Transform::from_rotation(xform.rotation).with_translation(trans); + for (offset, steering) in wheel_poses { let wheel_damping = Damping { linear_damping: 0.8, ..Default::default() @@ -83,42 +86,69 @@ pub fn spawn_tires( let prismatic = PrismaticJointBuilder::new(Vec3::Y) .local_anchor1(offset) .limits(limits) - .motor_position(-0.1, stiffness, damping); - let joint = MultibodyJoint::new(bike, prismatic); + .motor_position(limits[0], stiffness, damping); - let spatial_bundle = SpatialBundle { - transform: wheel_pos_in_world, - ..Default::default() - }; + let fork = commands + .spawn(SpatialBundle { + transform: Transform::from_translation(offset), + ..Default::default() + }) + .set_parent(bike) + .id(); - let tire_spundle = SpatialBundle { - transform: Transform::IDENTITY, - ..Default::default() - }; - - commands + let suspension = MultibodyJoint::new(fork, prismatic); + let sentity = commands .spawn(RigidBody::Dynamic) - .insert(spatial_bundle) - .insert(( - wheel_collider, - mass_props, - wheel_damping, - ccd, - not_sleeping, - joint, - wheels_collision_group, - friction, - CyberWheel, - ExternalForce::default(), - Restitution::new(0.2), - )) + .insert(suspension) + .insert(SpatialBundle { + transform: Transform::from_translation(offset), + ..Default::default() + }) + .set_parent(bike) + .id(); + + if let Some(steering) = steering { + commands.entity(sentity).insert(steering); + } + + let revolute = RevoluteJointBuilder::new(Vec3::X); + let axel = MultibodyJoint::new(sentity, revolute); + let wheel_bundle = ( + wheel_collider, + mass_props, + wheel_damping, + ccd, + wheels_collision_group, + friction, + restitution, + CyberWheel, + ExternalForce::default(), + not_sleeping, + axel, + ); + + let _wentity = commands + .spawn(RigidBody::Dynamic) + .insert(SpatialBundle { + // transform: Transform::from_rotation(xform.rotation) + // .with_translation(xform.translation) + transform: Transform::from_rotation(Quat::from_axis_angle( + Vec3::Z, + 90.0f32.to_radians(), + )), + ..Default::default() + }) + .insert(wheel_bundle) .with_children(|wheel| { - wheel.spawn(tire_spundle).insert(pbr_bundle.clone()).insert( - TransformInterpolation { + wheel + .spawn(SpatialBundle::default()) + .insert(pbr_bundle.clone()) + .insert(TransformInterpolation { start: None, end: None, - }, - ); - }); + }); + }) + .set_parent(sentity) + .id(); } } diff --git a/src/planet.rs b/src/planet.rs index 00a1268..da153b5 100644 --- a/src/planet.rs +++ b/src/planet.rs @@ -32,7 +32,7 @@ fn spawn_planet( let pcollide = ( shape, Friction { - coefficient: 0.05, + coefficient: 0.7, ..Default::default() }, Restitution::new(0.0), From db34880c1e4f46b3b04df17f933ae8f7ae418fbb Mon Sep 17 00:00:00 2001 From: Joe Ardent Date: Sun, 5 Feb 2023 14:29:05 -0800 Subject: [PATCH 02/12] checkpoint with capsule wheel colliders --- src/bike/wheels.rs | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/bike/wheels.rs b/src/bike/wheels.rs index 82e5be0..2cb71ce 100644 --- a/src/bike/wheels.rs +++ b/src/bike/wheels.rs @@ -1,7 +1,10 @@ -use bevy::prelude::{shape::UVSphere as Tire, *}; +use bevy::prelude::{ + shape::Capsule as Tire, AlphaMode, BuildChildren, Color, Commands, Entity, Mesh, PbrBundle, + Quat, SpatialBundle, StandardMaterial, Transform, Vec3, +}; use bevy_rapier3d::prelude::{ Ccd, CoefficientCombineRule, Collider, ColliderMassProperties, CollisionGroups, Damping, - ExternalForce, Friction, MultibodyJoint, PrismaticJointBuilder, Restitution, + ExternalForce, Friction, LockedAxes, MultibodyJoint, PrismaticJointBuilder, Restitution, RevoluteJointBuilder, RigidBody, Sleeping, TransformInterpolation, }; @@ -79,7 +82,7 @@ pub fn spawn_tires( linear_damping: 0.8, ..Default::default() }; - let wheel_collider = Collider::ball(wheel_rad); + let wheel_collider = Collider::capsule(-Vec3::Y, Vec3::Y, 0.3); let mass_props = ColliderMassProperties::Density(0.1); let damping = conf.damping; @@ -111,7 +114,7 @@ pub fn spawn_tires( commands.entity(sentity).insert(steering); } - let revolute = RevoluteJointBuilder::new(Vec3::X); + let revolute = RevoluteJointBuilder::new(Vec3::Y); let axel = MultibodyJoint::new(sentity, revolute); let wheel_bundle = ( wheel_collider, From e666c972acf6da3697cb0a5e20f65d3ff2154c5d Mon Sep 17 00:00:00 2001 From: Joe Ardent Date: Sun, 5 Feb 2023 14:59:41 -0800 Subject: [PATCH 03/12] 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