something is wonky, but it's kinda ok

This commit is contained in:
Joe Ardent 2023-02-20 13:29:46 -08:00
parent 9c196861a2
commit e6407c13aa
2 changed files with 33 additions and 6 deletions

View File

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

View File

@ -18,6 +18,10 @@ use crate::{
const PITCH_SCALE: f32 = 0.2; const PITCH_SCALE: f32 = 0.2;
fn yaw_to_angle(yaw: f32) -> f32 {
yaw.powi(3) * (PI / 4.0)
}
/// The gravity vector points from the cyberbike to the center of the planet. /// The gravity vector points from the cyberbike to the center of the planet.
pub(super) fn gravity( pub(super) fn gravity(
mut query: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>, mut query: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
@ -30,6 +34,25 @@ pub(super) fn gravity(
forces.torque = Vec3::ZERO; forces.torque = Vec3::ZERO;
} }
/// The desired lean angle, given steering input and speed.
pub(super) fn cyber_lean(
velocity: Query<&Velocity, With<CyberBikeBody>>,
wheels: Query<&Transform, With<CyberWheel>>,
input: Res<InputState>,
gravity_settings: Res<MovementSettings>,
mut lean: ResMut<CyberLean>,
) {
let vel_squared = velocity.single().linvel.length_squared();
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 tan_theta = vel_squared / (radius * gravity);
lean.lean = (PI / 2.0) - tan_theta.atan();
}
/// PID-based controller for stabilizing attitude; keeps the cyberbike upright. /// PID-based controller for stabilizing attitude; keeps the cyberbike upright.
pub(super) fn falling_cat( pub(super) fn falling_cat(
mut bike_query: Query<(&Transform, &mut ExternalForce, &mut CatControllerState)>, mut bike_query: Query<(&Transform, &mut ExternalForce, &mut CatControllerState)>,
@ -72,7 +95,7 @@ pub(super) fn falling_cat(
#[cfg(feature = "inspector")] #[cfg(feature = "inspector")]
{ {
if debug_instant.elapsed().as_millis() > 1000 { if debug_instant.elapsed().as_millis() > 1000 {
dbg!(&control_vars, mag); dbg!(&control_vars, mag, &lean);
debug_instant.reset(); debug_instant.reset();
} }
} }
@ -94,13 +117,13 @@ pub(super) fn input_forces(
// thrust // thrust
let thrust = xform.forward() * input.throttle * settings.accel; 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); let force = ExternalForce::at_point(thrust, point, xform.translation);
*forces += force; *forces += force;
// brake // brake
for mut motor in braking_query.iter_mut() { 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 motor.data = (*motor
.data .data
.as_revolute_mut() .as_revolute_mut()
@ -111,7 +134,7 @@ pub(super) fn input_forces(
} }
// steering // 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(); let mut steering = steering_query.single_mut();
steering.data = (*steering steering.data = (*steering
.data .data