143 lines
4 KiB
Rust
143 lines
4 KiB
Rust
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<RapierConfiguration>) {
|
|
config.gravity = Vec3::ZERO;
|
|
}
|
|
|
|
fn gravity(
|
|
mut query: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
|
|
settings: Res<MovementSettings>,
|
|
) {
|
|
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<Diagnostics>,
|
|
settings: Res<CatControllerSettings>,
|
|
) {
|
|
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<MovementSettings>,
|
|
input: Res<InputState>,
|
|
mut cquery: Query<&mut Friction, With<CyberBikeCollider>>,
|
|
mut bquery: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
|
|
) {
|
|
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<CyberBikeBody>>) {
|
|
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::<MovementSettings>()
|
|
.init_resource::<CatControllerSettings>()
|
|
.register_type::<CatControllerSettings>()
|
|
.add_plugin(RapierPhysicsPlugin::<NoUserData>::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"));
|
|
}
|
|
}
|