add tweak system for tuning suspension at runtime
This commit is contained in:
parent
8978a25965
commit
355dae1f88
3 changed files with 119 additions and 49 deletions
54
src/bike.rs
54
src/bike.rs
|
@ -6,8 +6,10 @@ use bevy::prelude::*;
|
|||
|
||||
use crate::physics::CatControllerState;
|
||||
|
||||
pub const REST_DISTANCE: f32 = 1.0;
|
||||
pub const WHEEL_RADIUS: f32 = 0.4;
|
||||
pub const REST_DISTANCE: Scalar = 1.0;
|
||||
pub const SPRING_CONSTANT: Scalar = 50.0;
|
||||
pub const DAMPING_CONSTANT: Scalar = 3.0;
|
||||
pub const WHEEL_RADIUS: Scalar = 0.4;
|
||||
pub const FRONT_ATTACH: Vec3 = Vec3::new(0.0, 0.0, -0.7);
|
||||
pub const REAR_ATTACH: Vec3 = Vec3::new(0.0, 0.0, 0.7);
|
||||
|
||||
|
@ -38,6 +40,7 @@ pub struct WheelConfig {
|
|||
pub konstant: Scalar,
|
||||
pub damping: Scalar,
|
||||
pub friction: Scalar,
|
||||
pub radius: Scalar,
|
||||
}
|
||||
|
||||
impl WheelConfig {
|
||||
|
@ -47,6 +50,7 @@ impl WheelConfig {
|
|||
konstant: Scalar,
|
||||
damping: Scalar,
|
||||
friction: Scalar,
|
||||
radius: Scalar,
|
||||
) -> Self {
|
||||
WheelConfig {
|
||||
attach,
|
||||
|
@ -54,6 +58,7 @@ impl WheelConfig {
|
|||
konstant,
|
||||
damping,
|
||||
friction,
|
||||
radius,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -79,9 +84,11 @@ fn spawn_bike(
|
|||
SleepingDisabled,
|
||||
CyberBikeBody,
|
||||
CatControllerState::default(),
|
||||
ColliderDensity(20.0),
|
||||
LinearVelocity::ZERO,
|
||||
AngularVelocity::ZERO,
|
||||
ColliderDensity(0.5),
|
||||
AngularDamping(0.9),
|
||||
LinearDamping(0.2),
|
||||
// LinearVelocity::ZERO,
|
||||
// AngularVelocity::ZERO,
|
||||
ExternalForce::ZERO.with_persistence(false),
|
||||
ExternalTorque::ZERO.with_persistence(false),
|
||||
))
|
||||
|
@ -114,7 +121,7 @@ fn spawn_wheels(
|
|||
wheel_caster(
|
||||
commands,
|
||||
collider.clone(),
|
||||
Transform::from_translation(FRONT_ATTACH),
|
||||
FRONT_ATTACH,
|
||||
Dir3::new_unchecked(front_rake),
|
||||
body,
|
||||
REST_DISTANCE,
|
||||
|
@ -126,7 +133,14 @@ fn spawn_wheels(
|
|||
&mut materials,
|
||||
front_wheel_pos,
|
||||
mesh.clone(),
|
||||
WheelConfig::new(FRONT_ATTACH, REST_DISTANCE, 800., -160., 0.5),
|
||||
WheelConfig::new(
|
||||
FRONT_ATTACH,
|
||||
REST_DISTANCE,
|
||||
SPRING_CONSTANT,
|
||||
DAMPING_CONSTANT,
|
||||
0.5,
|
||||
WHEEL_RADIUS,
|
||||
),
|
||||
CyberWheel::Front,
|
||||
);
|
||||
|
||||
|
@ -136,7 +150,7 @@ fn spawn_wheels(
|
|||
wheel_caster(
|
||||
commands,
|
||||
collider,
|
||||
Transform::from_translation(REAR_ATTACH),
|
||||
REAR_ATTACH,
|
||||
Dir3::new_unchecked(rear_rake),
|
||||
body,
|
||||
REST_DISTANCE,
|
||||
|
@ -148,7 +162,14 @@ fn spawn_wheels(
|
|||
&mut materials,
|
||||
rear_wheel_pos,
|
||||
mesh,
|
||||
WheelConfig::new(REAR_ATTACH, REST_DISTANCE, 800., -160., 0.5),
|
||||
WheelConfig::new(
|
||||
REAR_ATTACH,
|
||||
REST_DISTANCE,
|
||||
SPRING_CONSTANT,
|
||||
DAMPING_CONSTANT,
|
||||
0.5,
|
||||
WHEEL_RADIUS,
|
||||
),
|
||||
CyberWheel::Rear,
|
||||
);
|
||||
}
|
||||
|
@ -160,14 +181,15 @@ fn spawn_wheels(
|
|||
fn wheel_caster(
|
||||
commands: &mut ChildBuilder,
|
||||
collider: Collider,
|
||||
xform: Transform,
|
||||
origin: Vec3,
|
||||
direction: Dir3,
|
||||
parent: Entity,
|
||||
rest_dist: Scalar,
|
||||
wheel: CyberWheel,
|
||||
) {
|
||||
let caster = ShapeCaster::new(collider, xform.translation, Quat::IDENTITY, direction)
|
||||
let caster = ShapeCaster::new(collider, origin, Quat::IDENTITY, direction)
|
||||
.with_max_distance(rest_dist)
|
||||
.with_max_hits(1)
|
||||
.with_query_filter(SpatialQueryFilter::from_excluded_entities([parent]));
|
||||
|
||||
commands.spawn((caster, wheel));
|
||||
|
@ -191,12 +213,18 @@ fn wheel_mesh(
|
|||
|
||||
let xform = Transform::from_translation(position);
|
||||
|
||||
let name = match wheel {
|
||||
CyberWheel::Front => "front tire",
|
||||
CyberWheel::Rear => "rear tire",
|
||||
};
|
||||
|
||||
commands.spawn((
|
||||
Name::new("tire"),
|
||||
Name::new(name),
|
||||
config,
|
||||
Mesh3d(meshes.add(tire_mesh)),
|
||||
MeshMaterial3d(materials.add(wheel_material.clone())),
|
||||
xform,
|
||||
TransformInterpolation,
|
||||
wheel,
|
||||
));
|
||||
}
|
||||
|
@ -205,6 +233,8 @@ pub struct CyberBikePlugin;
|
|||
|
||||
impl Plugin for CyberBikePlugin {
|
||||
fn build(&self, app: &mut App) {
|
||||
app.register_type::<WheelConfig>();
|
||||
app.register_type::<WheelState>();
|
||||
app.add_systems(Startup, spawn_bike);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -56,14 +56,9 @@ fn update_camera_pos(mut offset: ResMut<DebugCamOffset>, mut keys: ResMut<Button
|
|||
}
|
||||
}
|
||||
|
||||
let just_released: HashSet<_> = keys.get_just_released().cloned().collect();
|
||||
if !just_released.is_empty() {
|
||||
let released_shift = just_released.contains(&KeyCode::ShiftLeft)
|
||||
|| just_released.contains(&KeyCode::ShiftRight);
|
||||
keys.reset_all();
|
||||
if !released_shift && shifted {
|
||||
keys.press(KeyCode::ShiftLeft);
|
||||
}
|
||||
let released: Vec<_> = keys.get_just_released().copied().collect();
|
||||
for key in released {
|
||||
keys.clear_just_released(key);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
103
src/physics.rs
103
src/physics.rs
|
@ -18,9 +18,9 @@ pub struct CatControllerSettings {
|
|||
impl Default for CatControllerSettings {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
kp: 1200.0,
|
||||
kd: 10.0,
|
||||
ki: 50.0,
|
||||
kp: 30.0,
|
||||
kd: 7.0,
|
||||
ki: 1.0,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -58,7 +58,7 @@ mod systems {
|
|||
use std::f32::consts::{FRAC_PI_3, FRAC_PI_4};
|
||||
|
||||
use avian3d::{math::Scalar, prelude::*};
|
||||
use bevy::{color, math::VectorSpace, prelude::*};
|
||||
use bevy::prelude::*;
|
||||
|
||||
use super::{CatControllerSettings, CatControllerState, CyberLean};
|
||||
use crate::{
|
||||
|
@ -79,23 +79,28 @@ mod systems {
|
|||
|
||||
pub(super) fn calculate_lean(
|
||||
bike_state: Query<(&LinearVelocity, &Transform), With<CyberBikeBody>>,
|
||||
wheels: Query<&GlobalTransform, With<CyberWheel>>,
|
||||
input: Res<InputState>,
|
||||
gravity: Res<Gravity>,
|
||||
mut lean: ResMut<CyberLean>,
|
||||
) {
|
||||
let mut wheels = wheels.iter();
|
||||
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 vel = velocity.dot(*xform.forward());
|
||||
let v_squared = vel.powi(2);
|
||||
let steering_angle = yaw_to_angle(input.yaw);
|
||||
let wheel_base = 1.145f32;
|
||||
let radius = wheel_base / steering_angle.tan();
|
||||
let gravity = -9.8f32;
|
||||
let radius = base / steering_angle.tan();
|
||||
let gravity = gravity.0.length();
|
||||
let v2_r = v_squared / radius;
|
||||
let tan_theta = (v2_r / gravity).clamp(-FRAC_PI_3, FRAC_PI_3);
|
||||
|
||||
if tan_theta.is_normal() {
|
||||
lean.lean = -tan_theta.atan().clamp(-FRAC_PI_3, FRAC_PI_3);
|
||||
} else {
|
||||
lean.lean = 0.0;
|
||||
//lean.lean = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -210,19 +215,28 @@ mod systems {
|
|||
|
||||
let prev = &mut state.displacement;
|
||||
if let Some(hit) = hits.iter().next() {
|
||||
let force = suspension_force(caster, hit, config, prev, dt, &mut xform);
|
||||
let mag = suspension_force(caster, hit, config, prev, dt, &mut xform);
|
||||
let mag = mag.max(0.0);
|
||||
//let fdir = -caster.global_direction().as_vec3();
|
||||
let fdir = hit.normal1;
|
||||
let force = fdir * mag;
|
||||
//dbg!(fdir, force);
|
||||
let p = hit.point1 + (hit.normal1 * config.radius);
|
||||
gizmos.arrow(p, p + force, Color::linear_rgb(1., 0.5, 0.2));
|
||||
|
||||
bike_forces.apply_force_at_point(
|
||||
force,
|
||||
caster.global_origin(),
|
||||
bike_xform.translation,
|
||||
);
|
||||
let vel = (global_xform.translation() - state.ppos) / dt;
|
||||
dbg!(vel);
|
||||
//let vel = (global_xform.translation() - state.ppos) / dt;
|
||||
|
||||
state.ppos = global_xform.translation();
|
||||
let normal = hit.normal1;
|
||||
let steering = match wheel {
|
||||
CyberWheel::Front => normal.cross(*bike_xform.back()) * yaw,
|
||||
_ => normal.cross(*bike_xform.forward()) * yaw, // Vec3::ZERO,
|
||||
CyberWheel::Rear => Vec3::ZERO,
|
||||
//_ => normal.cross(*bike_xform.forward()) * yaw, // Vec3::ZERO,
|
||||
};
|
||||
let thrust = normal.cross(*bike_xform.right()) * thrust;
|
||||
let total = (thrust + steering) * dt;
|
||||
|
@ -245,27 +259,57 @@ mod systems {
|
|||
previous_dispacement: &mut Scalar,
|
||||
dt: Scalar,
|
||||
wheel_xform: &mut Transform,
|
||||
) -> Vec3 {
|
||||
let mut up_force = Vec3::ZERO;
|
||||
) -> Scalar {
|
||||
let dist = hit.distance;
|
||||
let cdir = caster.direction.as_vec3();
|
||||
let dir = caster.global_direction().as_vec3();
|
||||
let loc = {
|
||||
let displacement = config.rest_dist - dist;
|
||||
let damper_vel = (displacement - *previous_dispacement) / dt;
|
||||
dbg!(damper_vel);
|
||||
*previous_dispacement = displacement;
|
||||
let mag = config.konstant * displacement - config.damping * damper_vel;
|
||||
up_force = -dir * mag;
|
||||
config.attach + (cdir * dist)
|
||||
};
|
||||
wheel_xform.translation = loc;
|
||||
wheel_xform.translation = config.attach + (cdir * dist);
|
||||
|
||||
up_force
|
||||
let displacement = config.rest_dist - dist;
|
||||
let damper_vel = (*previous_dispacement - displacement) / dt;
|
||||
|
||||
*previous_dispacement = displacement;
|
||||
config.konstant * displacement - config.damping * damper_vel
|
||||
}
|
||||
|
||||
pub(super) fn tweak(
|
||||
mut config: Query<&mut WheelConfig>,
|
||||
mut keys: ResMut<ButtonInput<KeyCode>>,
|
||||
) {
|
||||
let keyset: std::collections::HashSet<_> = keys.get_pressed().collect();
|
||||
let shifted = keyset.contains(&KeyCode::ShiftLeft) || keyset.contains(&KeyCode::ShiftRight);
|
||||
let config = config.iter_mut();
|
||||
for ref mut c in config {
|
||||
for key in &keyset {
|
||||
match key {
|
||||
KeyCode::KeyS => {
|
||||
if shifted {
|
||||
c.konstant += 0.2;
|
||||
} else {
|
||||
c.konstant -= 0.2;
|
||||
}
|
||||
bevy::log::info!(c.konstant);
|
||||
}
|
||||
KeyCode::KeyD => {
|
||||
if shifted {
|
||||
c.damping += 0.1;
|
||||
} else {
|
||||
c.damping -= 0.1;
|
||||
}
|
||||
bevy::log::info!(c.damping);
|
||||
}
|
||||
|
||||
_ => continue,
|
||||
}
|
||||
}
|
||||
}
|
||||
let released: Vec<_> = keys.get_just_released().copied().collect();
|
||||
for key in released {
|
||||
keys.clear_just_released(key);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
use systems::{apply_lean, calculate_lean, suspension};
|
||||
use systems::{apply_lean, calculate_lean, suspension, tweak};
|
||||
|
||||
pub struct CyberPhysicsPlugin;
|
||||
|
||||
|
@ -279,11 +323,12 @@ impl Plugin for CyberPhysicsPlugin {
|
|||
.add_plugins((PhysicsPlugins::default(), PhysicsDebugPlugin::default()))
|
||||
.insert_resource(SubstepCount(12))
|
||||
.add_systems(Startup, |mut gravity: ResMut<Gravity>| {
|
||||
gravity.0 *= 0.02;
|
||||
gravity.0 *= 1.0;
|
||||
})
|
||||
.add_systems(
|
||||
FixedUpdate,
|
||||
(calculate_lean, apply_lean, suspension).chain(),
|
||||
);
|
||||
)
|
||||
.add_systems(Update, tweak);
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in a new issue