use torque for lean, use Single and Populated instead of Query
systems won't run if no entities match a Single or Populated query, so no need to return a Result or set the error handler
This commit is contained in:
parent
5596d883c5
commit
a65e60a316
2 changed files with 51 additions and 57 deletions
|
@ -3,7 +3,6 @@ use avian3d::prelude::{
|
|||
};
|
||||
use bevy::{
|
||||
color::{palettes::css::SILVER, Alpha},
|
||||
ecs::error::{warn, GLOBAL_ERROR_HANDLER},
|
||||
pbr::MeshMaterial3d,
|
||||
prelude::{
|
||||
children, default, App, AppGizmoBuilder, Assets, ButtonInput, Color, Commands,
|
||||
|
@ -24,10 +23,6 @@ use input::CyberInputPlugin;
|
|||
use physics::CyberPhysicsPlugin;
|
||||
|
||||
fn main() {
|
||||
GLOBAL_ERROR_HANDLER
|
||||
.set(warn)
|
||||
.expect("The error handler can only be set once, globally.");
|
||||
// initialize Bevy App here
|
||||
let mut app = App::new();
|
||||
app.add_plugins((
|
||||
DefaultPlugins,
|
||||
|
|
103
src/physics.rs
103
src/physics.rs
|
@ -20,9 +20,9 @@ pub struct CatControllerSettings {
|
|||
impl Default for CatControllerSettings {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
kp: 45.0,
|
||||
kd: 15.0,
|
||||
ki: 2.5,
|
||||
kp: 55.0,
|
||||
kd: 20.0,
|
||||
ki: 12.5,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -58,14 +58,14 @@ mod systems {
|
|||
use std::f32::consts::{FRAC_PI_3, FRAC_PI_4};
|
||||
|
||||
use avian3d::prelude::{
|
||||
ComputedCenterOfMass, ExternalForce, Gravity, LinearVelocity, RayCaster, RayHits,
|
||||
RigidBodyQueryReadOnly,
|
||||
ComputedCenterOfMass, ExternalForce, ExternalTorque, Gravity, LinearVelocity, RayCaster,
|
||||
RayHits, RigidBodyQueryReadOnly,
|
||||
};
|
||||
use bevy::{
|
||||
ecs::error::BevyError,
|
||||
ecs::system::{Populated, Single},
|
||||
prelude::{
|
||||
ButtonInput, Color, Gizmos, GlobalTransform, KeyCode, Quat, Query, Res, ResMut, Result,
|
||||
Time, Transform, Vec, Vec3, With, Without,
|
||||
ButtonInput, Color, Gizmos, GlobalTransform, KeyCode, Quat, Res, ResMut, Time,
|
||||
Transform, Vec, Vec3, With, Without,
|
||||
},
|
||||
};
|
||||
|
||||
|
@ -87,17 +87,17 @@ mod systems {
|
|||
}
|
||||
|
||||
pub(super) fn calculate_lean(
|
||||
bike_state: Query<(&LinearVelocity, &Transform), With<CyberBikeBody>>,
|
||||
wheels: Query<&GlobalTransform, With<CyberWheel>>,
|
||||
bike_state: Single<(&LinearVelocity, &Transform), With<CyberBikeBody>>,
|
||||
wheels: Populated<&GlobalTransform, With<CyberWheel>>,
|
||||
input: Res<InputState>,
|
||||
gravity: Res<Gravity>,
|
||||
mut lean: ResMut<CyberLean>,
|
||||
) -> Result {
|
||||
) {
|
||||
let mut wheels = wheels.iter();
|
||||
let w1 = wheels.next().ok_or(BevyError::from("oops"))?;
|
||||
let w2 = wheels.next().ok_or(BevyError::from("oops"))?;
|
||||
let w1 = wheels.next().unwrap();
|
||||
let w2 = wheels.next().unwrap();
|
||||
let base = (w1.translation() - w2.translation()).length().abs();
|
||||
let (velocity, xform) = bike_state.single()?;
|
||||
let (velocity, xform) = bike_state.into_inner();
|
||||
let vel = velocity.dot(*xform.forward());
|
||||
let v_squared = vel.powi(2);
|
||||
let steering_angle = yaw_to_angle(input.yaw);
|
||||
|
@ -111,31 +111,37 @@ mod systems {
|
|||
} else {
|
||||
//lean.lean = 0.0;
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
pub(super) fn apply_lean(
|
||||
mut bike_query: Query<(
|
||||
&Transform,
|
||||
&ComputedCenterOfMass,
|
||||
&mut ExternalForce,
|
||||
&mut CatControllerState,
|
||||
)>,
|
||||
wheels: Query<&WheelState>,
|
||||
bike_query: Single<(&Transform, &mut ExternalTorque, &mut CatControllerState)>,
|
||||
wheels: Populated<(&WheelState, &GlobalTransform, &CyberWheel)>,
|
||||
time: Res<Time>,
|
||||
settings: Res<CatControllerSettings>,
|
||||
lean: Res<CyberLean>,
|
||||
mut gizmos: Gizmos,
|
||||
) -> Result {
|
||||
let (xform, com, mut force, mut control_vars) = bike_query.single_mut()?;
|
||||
) {
|
||||
let (xform, mut torque, mut control_vars) = bike_query.into_inner();
|
||||
|
||||
let mut factor = 1.0;
|
||||
for wheel in wheels.iter() {
|
||||
if wheel.contact_point.is_none() {
|
||||
let mut rxform = GlobalTransform::default();
|
||||
let mut fxform = GlobalTransform::default();
|
||||
for (wheel_state, xform, cwheel) in wheels.iter() {
|
||||
if wheel_state.contact_point.is_none() {
|
||||
factor -= 0.25;
|
||||
}
|
||||
match cwheel {
|
||||
CyberWheel::Front => {
|
||||
fxform = *xform;
|
||||
}
|
||||
CyberWheel::Rear => {
|
||||
rxform = *xform;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
let tork_axis = (rxform.translation() - fxform.translation()).normalize();
|
||||
|
||||
let world_up = Vec3::Y; //xform.translation.normalize();
|
||||
let rot = Quat::from_axis_angle(*xform.back(), lean.lean);
|
||||
let target_up = rotate_point(&world_up, &rot).normalize();
|
||||
|
@ -165,20 +171,18 @@ mod systems {
|
|||
let mag =
|
||||
(settings.kp * roll_error) + (settings.ki * integral) + (settings.kd * derivative);
|
||||
if mag.is_finite() {
|
||||
let lean_force = factor * mag * *xform.left();
|
||||
force.apply_force_at_point(
|
||||
lean_force,
|
||||
xform.translation + *xform.up(),
|
||||
xform.translation + com.0,
|
||||
);
|
||||
//let lean_force = factor * mag * *xform.left();
|
||||
let tork = factor * mag * tork_axis;
|
||||
|
||||
torque.apply_torque(tork);
|
||||
|
||||
gizmos.arrow(
|
||||
xform.translation + *xform.up(),
|
||||
xform.translation + *xform.up() + lean_force,
|
||||
xform.translation + *xform.up() + mag * xform.left(),
|
||||
Color::WHITE,
|
||||
);
|
||||
}
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn yaw_to_angle(yaw: f32) -> f32 {
|
||||
|
@ -191,18 +195,18 @@ mod systems {
|
|||
}
|
||||
|
||||
pub(super) fn suspension(
|
||||
mut bike_body_query: Query<
|
||||
bike_body_query: Single<
|
||||
(&Transform, &ComputedCenterOfMass, &mut ExternalForce),
|
||||
With<CyberBikeBody>,
|
||||
>,
|
||||
mut wheel_mesh_query: Query<
|
||||
mut wheel_mesh_query: Populated<
|
||||
(&mut Transform, &mut WheelState, &WheelConfig, &CyberWheel),
|
||||
Without<CyberBikeBody>,
|
||||
>,
|
||||
caster_query: Query<(&RayCaster, &RayHits, &CyberWheel)>,
|
||||
caster_query: Populated<(&RayCaster, &RayHits, &CyberWheel)>,
|
||||
time: Res<Time>,
|
||||
mut gizmos: Gizmos,
|
||||
) -> Result {
|
||||
) {
|
||||
let dt = time.delta().as_secs_f32();
|
||||
|
||||
let mut front_caster = &RayCaster::default();
|
||||
|
@ -224,7 +228,7 @@ mod systems {
|
|||
}
|
||||
}
|
||||
|
||||
let (bike_xform, com, mut bike_forces) = bike_body_query.single_mut()?;
|
||||
let (bike_xform, com, mut bike_forces) = bike_body_query.into_inner();
|
||||
|
||||
for (mut xform, mut state, config, wheel) in wheel_mesh_query.iter_mut() {
|
||||
let (caster, hits) = match wheel {
|
||||
|
@ -266,11 +270,10 @@ mod systems {
|
|||
state.reset();
|
||||
}
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
pub(super) fn steering(
|
||||
mut bike_query: Query<
|
||||
bike_query: Single<
|
||||
(
|
||||
&Transform,
|
||||
&LinearVelocity,
|
||||
|
@ -279,12 +282,12 @@ mod systems {
|
|||
),
|
||||
With<CyberBikeBody>,
|
||||
>,
|
||||
mut wheels: Query<(&mut WheelState, &WheelConfig, &CyberWheel)>,
|
||||
mut wheels: Populated<(&mut WheelState, &WheelConfig, &CyberWheel)>,
|
||||
time: Res<Time>,
|
||||
input: Res<InputState>,
|
||||
mut gizmos: Gizmos,
|
||||
) -> Result {
|
||||
let (bike_xform, bike_vel, mut bike_force, bike_body) = bike_query.single_mut()?;
|
||||
) {
|
||||
let (bike_xform, bike_vel, mut bike_force, bike_body) = bike_query.into_inner();
|
||||
let bike_vel = bike_vel.0;
|
||||
|
||||
let dt = time.delta().as_secs_f32();
|
||||
|
@ -346,12 +349,10 @@ mod systems {
|
|||
);
|
||||
}
|
||||
}
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
pub(super) fn drag(
|
||||
mut bike_query: Query<
|
||||
bike_query: Single<
|
||||
(
|
||||
&Transform,
|
||||
&LinearVelocity,
|
||||
|
@ -362,8 +363,8 @@ mod systems {
|
|||
>,
|
||||
mut gizmos: Gizmos,
|
||||
time: Res<Time>,
|
||||
) -> Result {
|
||||
let (xform, vel, com, mut force) = bike_query.single_mut()?;
|
||||
) {
|
||||
let (xform, vel, com, mut force) = bike_query.into_inner();
|
||||
|
||||
let dt = time.delta_secs();
|
||||
|
||||
|
@ -386,12 +387,10 @@ mod systems {
|
|||
xform.translation + drag * 10.,
|
||||
Color::linear_rgb(1.0, 0.0, 0.0),
|
||||
);
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
pub(super) fn tweak(
|
||||
mut config: Query<&mut WheelConfig>,
|
||||
mut config: Populated<&mut WheelConfig>,
|
||||
mut keys: ResMut<ButtonInput<KeyCode>>,
|
||||
) {
|
||||
let keyset: std::collections::HashSet<_> = keys.get_pressed().collect();
|
||||
|
|
Loading…
Reference in a new issue