working damping/suspension
This commit is contained in:
parent
c924428a9e
commit
8c16a7f9d2
2 changed files with 48 additions and 78 deletions
38
src/bike.rs
38
src/bike.rs
|
@ -27,6 +27,9 @@ pub struct Steering;
|
||||||
#[derive(Component)]
|
#[derive(Component)]
|
||||||
pub struct Rearing;
|
pub struct Rearing;
|
||||||
|
|
||||||
|
#[derive(Debug, Component, Clone, Default)]
|
||||||
|
pub struct PreviousDisplacement(pub f32);
|
||||||
|
|
||||||
#[derive(Debug, Resource, Reflect)]
|
#[derive(Debug, Resource, Reflect)]
|
||||||
#[reflect(Resource)]
|
#[reflect(Resource)]
|
||||||
pub struct SuspensionConfig {
|
pub struct SuspensionConfig {
|
||||||
|
@ -41,8 +44,8 @@ impl Default for SuspensionConfig {
|
||||||
fn default() -> Self {
|
fn default() -> Self {
|
||||||
Self {
|
Self {
|
||||||
rest_dist: REST_DISTANCE,
|
rest_dist: REST_DISTANCE,
|
||||||
konstant: 50.0,
|
konstant: 800.0,
|
||||||
damping: 50.0,
|
damping: -100.0,
|
||||||
front_attach: FRONT_ATTACH,
|
front_attach: FRONT_ATTACH,
|
||||||
rear_attach: REAR_ATTACH,
|
rear_attach: REAR_ATTACH,
|
||||||
}
|
}
|
||||||
|
@ -54,7 +57,7 @@ fn spawn_bike(
|
||||||
mut meshes: ResMut<Assets<Mesh>>,
|
mut meshes: ResMut<Assets<Mesh>>,
|
||||||
mut materials: ResMut<Assets<StandardMaterial>>,
|
mut materials: ResMut<Assets<StandardMaterial>>,
|
||||||
) {
|
) {
|
||||||
let pos = Vec3::new(0.0, 4.0, 0.0);
|
let pos = Vec3::new(0.0, 14.0, 0.0);
|
||||||
let xform = Transform::from_translation(pos); //.with_rotation(Quat::from_rotation_z(0.0));
|
let xform = Transform::from_translation(pos); //.with_rotation(Quat::from_rotation_z(0.0));
|
||||||
|
|
||||||
let body_collider =
|
let body_collider =
|
||||||
|
@ -86,13 +89,7 @@ fn spawn_bike(
|
||||||
MeshMaterial3d(materials.add(color)),
|
MeshMaterial3d(materials.add(color)),
|
||||||
);
|
);
|
||||||
builder.spawn(pbr_bundle);
|
builder.spawn(pbr_bundle);
|
||||||
spawn_wheels(
|
spawn_wheels(builder, meshes, materials, builder.parent_entity());
|
||||||
builder,
|
|
||||||
meshes,
|
|
||||||
materials,
|
|
||||||
Transform::default(),
|
|
||||||
builder.parent_entity(),
|
|
||||||
);
|
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -107,7 +104,7 @@ fn spawn_caster(
|
||||||
let caster = ShapeCaster::new(collider, Vec3::ZERO, Quat::IDENTITY, direction)
|
let caster = ShapeCaster::new(collider, Vec3::ZERO, Quat::IDENTITY, direction)
|
||||||
.with_query_filter(SpatialQueryFilter::from_excluded_entities([parent]));
|
.with_query_filter(SpatialQueryFilter::from_excluded_entities([parent]));
|
||||||
|
|
||||||
let mut entity = commands.spawn((caster, xform));
|
let mut entity = commands.spawn((caster, xform, PreviousDisplacement(REST_DISTANCE)));
|
||||||
if is_front {
|
if is_front {
|
||||||
entity.insert((Steering, Name::new("front wheel")));
|
entity.insert((Steering, Name::new("front wheel")));
|
||||||
} else {
|
} else {
|
||||||
|
@ -119,19 +116,17 @@ fn spawn_wheels(
|
||||||
commands: &mut ChildBuilder,
|
commands: &mut ChildBuilder,
|
||||||
mut meshes: ResMut<Assets<Mesh>>,
|
mut meshes: ResMut<Assets<Mesh>>,
|
||||||
mut materials: ResMut<Assets<StandardMaterial>>,
|
mut materials: ResMut<Assets<StandardMaterial>>,
|
||||||
xform: Transform,
|
|
||||||
body: Entity,
|
body: Entity,
|
||||||
) {
|
) {
|
||||||
let (mesh, collider) = gen_tire();
|
let (mesh, collider) = gen_tire();
|
||||||
|
|
||||||
let front_rake = Vec3::new(0.0, -1.0, -0.57).normalize(); // about 30 degrees
|
let front_rake = Vec3::new(0.0, -1.0, -0.57).normalize(); // about 30 degrees
|
||||||
let front_attach = xform.translation + xform.forward() * 0.7;
|
let front_wheel_pos = FRONT_ATTACH + (front_rake * REST_DISTANCE);
|
||||||
let front_wheel_pos = front_attach + (front_rake * REST_DISTANCE);
|
|
||||||
|
|
||||||
spawn_caster(
|
spawn_caster(
|
||||||
commands,
|
commands,
|
||||||
collider.clone(),
|
collider.clone(),
|
||||||
Transform::from_translation(front_attach),
|
Transform::from_translation(FRONT_ATTACH),
|
||||||
Dir3::new_unchecked(front_rake),
|
Dir3::new_unchecked(front_rake),
|
||||||
body,
|
body,
|
||||||
true,
|
true,
|
||||||
|
@ -146,13 +141,13 @@ fn spawn_wheels(
|
||||||
);
|
);
|
||||||
|
|
||||||
let rear_rake = Vec3::new(0.0, -1.0, 0.57).normalize();
|
let rear_rake = Vec3::new(0.0, -1.0, 0.57).normalize();
|
||||||
let rear_attach = xform.translation + xform.back() * 0.7;
|
//let rear_attach = xform.translation + xform.back() * 0.7;
|
||||||
let rear_wheel_pos = rear_attach + (rear_rake * REST_DISTANCE);
|
let rear_wheel_pos = REAR_ATTACH + (rear_rake * REST_DISTANCE);
|
||||||
|
|
||||||
spawn_caster(
|
spawn_caster(
|
||||||
commands,
|
commands,
|
||||||
collider,
|
collider,
|
||||||
Transform::from_translation(rear_attach),
|
Transform::from_translation(REAR_ATTACH),
|
||||||
Dir3::new_unchecked(rear_rake),
|
Dir3::new_unchecked(rear_rake),
|
||||||
body,
|
body,
|
||||||
false,
|
false,
|
||||||
|
@ -172,8 +167,8 @@ fn spawn_wheels(
|
||||||
//-************************************************************************
|
//-************************************************************************
|
||||||
|
|
||||||
fn gen_tire() -> (Mesh, Collider) {
|
fn gen_tire() -> (Mesh, Collider) {
|
||||||
let tire_mesh: Mesh = Sphere::new(0.4).into();
|
let tire_mesh: Mesh = Sphere::new(WHEEL_RADIUS).into();
|
||||||
let collider = Collider::sphere(0.4);
|
let collider = Collider::sphere(WHEEL_RADIUS);
|
||||||
|
|
||||||
(tire_mesh, collider)
|
(tire_mesh, collider)
|
||||||
}
|
}
|
||||||
|
@ -200,7 +195,7 @@ fn wheel_mesh(
|
||||||
Mesh3d(meshes.add(tire_mesh)),
|
Mesh3d(meshes.add(tire_mesh)),
|
||||||
MeshMaterial3d(materials.add(wheel_material.clone())),
|
MeshMaterial3d(materials.add(wheel_material.clone())),
|
||||||
xform,
|
xform,
|
||||||
LinearVelocity::ZERO,
|
PreviousDisplacement::default(),
|
||||||
wheel,
|
wheel,
|
||||||
));
|
));
|
||||||
}
|
}
|
||||||
|
@ -210,6 +205,7 @@ pub struct CyberBikePlugin;
|
||||||
impl Plugin for CyberBikePlugin {
|
impl Plugin for CyberBikePlugin {
|
||||||
fn build(&self, app: &mut App) {
|
fn build(&self, app: &mut App) {
|
||||||
app.init_resource::<SuspensionConfig>();
|
app.init_resource::<SuspensionConfig>();
|
||||||
|
app.register_type::<SuspensionConfig>();
|
||||||
app.add_systems(Startup, spawn_bike);
|
app.add_systems(Startup, spawn_bike);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -52,33 +52,22 @@ impl CatControllerState {
|
||||||
self.roll_prev = error;
|
self.roll_prev = error;
|
||||||
(derivative, self.roll_integral)
|
(derivative, self.roll_integral)
|
||||||
}
|
}
|
||||||
|
|
||||||
pub fn set_integral_limits(&mut self, roll: f32) {
|
|
||||||
self.roll_limit = roll;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
mod systems {
|
mod systems {
|
||||||
use std::f32::consts::{FRAC_PI_3, FRAC_PI_4};
|
use std::f32::consts::{FRAC_PI_3, FRAC_PI_4};
|
||||||
|
|
||||||
use avian3d::prelude::*;
|
use avian3d::prelude::*;
|
||||||
use bevy::{math::VectorSpace, prelude::*};
|
use bevy::prelude::*;
|
||||||
|
|
||||||
use super::{CatControllerSettings, CatControllerState, CyberLean};
|
use super::{CatControllerSettings, CatControllerState, CyberLean};
|
||||||
use crate::{
|
use crate::{
|
||||||
bike::{CyberBikeBody, CyberWheel, Rearing, Steering, SuspensionConfig},
|
bike::{
|
||||||
|
CyberBikeBody, CyberWheel, PreviousDisplacement, Rearing, Steering, SuspensionConfig,
|
||||||
|
},
|
||||||
input::InputState,
|
input::InputState,
|
||||||
};
|
};
|
||||||
|
|
||||||
fn _yaw_to_angle(yaw: f32) -> f32 {
|
|
||||||
let v = yaw.powi(5) * FRAC_PI_3;
|
|
||||||
if v.is_normal() {
|
|
||||||
v
|
|
||||||
} else {
|
|
||||||
0.0
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
fn rotate_point(pt: &Vec3, rot: &Quat) -> Vec3 {
|
fn rotate_point(pt: &Vec3, rot: &Quat) -> Vec3 {
|
||||||
// thanks to https://danceswithcode.net/engineeringnotes/quaternions/quaternions.html
|
// thanks to https://danceswithcode.net/engineeringnotes/quaternions/quaternions.html
|
||||||
let [x, y, z] = pt.normalize().to_array();
|
let [x, y, z] = pt.normalize().to_array();
|
||||||
|
@ -172,45 +161,39 @@ mod systems {
|
||||||
|
|
||||||
pub(super) fn suspension(
|
pub(super) fn suspension(
|
||||||
mut bike_body_query: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
|
mut bike_body_query: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
|
||||||
mut wheel_mesh_query: Query<
|
mut wheel_mesh_query: Query<(&mut Transform, &CyberWheel), Without<CyberBikeBody>>,
|
||||||
(&mut Transform, &LinearVelocity, &CyberWheel),
|
mut front_wheel_query: Query<
|
||||||
Without<CyberBikeBody>,
|
(&ShapeCaster, &ShapeHits, &mut PreviousDisplacement),
|
||||||
|
(With<Steering>, Without<Rearing>),
|
||||||
|
>,
|
||||||
|
mut rear_wheel_query: Query<
|
||||||
|
(&ShapeCaster, &ShapeHits, &mut PreviousDisplacement),
|
||||||
|
(With<Rearing>, Without<Steering>),
|
||||||
>,
|
>,
|
||||||
front_wheel_query: Query<(&ShapeCaster, &ShapeHits), (With<Steering>, Without<Rearing>)>,
|
|
||||||
rear_wheel_query: Query<(&ShapeCaster, &ShapeHits), (With<Rearing>, Without<Steering>)>,
|
|
||||||
config: Res<SuspensionConfig>,
|
config: Res<SuspensionConfig>,
|
||||||
|
time: Res<Time>,
|
||||||
) {
|
) {
|
||||||
|
let dt = time.delta().as_secs_f32();
|
||||||
let mut front_wheel_xform = None;
|
let mut front_wheel_xform = None;
|
||||||
let mut rear_wheel_xform = None;
|
let mut rear_wheel_xform = None;
|
||||||
let mut front_wheel_vel = Vec3::ZERO;
|
for (xform, wheel) in wheel_mesh_query.iter_mut() {
|
||||||
let mut rear_wheel_vel = Vec3::ZERO;
|
|
||||||
for (xform, vel, wheel) in wheel_mesh_query.iter_mut() {
|
|
||||||
match wheel {
|
match wheel {
|
||||||
CyberWheel::Front => {
|
CyberWheel::Front => {
|
||||||
front_wheel_xform = Some(xform);
|
front_wheel_xform = Some(xform);
|
||||||
front_wheel_vel = **vel;
|
|
||||||
}
|
}
|
||||||
CyberWheel::Rear => {
|
CyberWheel::Rear => {
|
||||||
rear_wheel_xform = Some(xform);
|
rear_wheel_xform = Some(xform);
|
||||||
rear_wheel_vel = **vel;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
let (bike_xform, mut bike_forces) = bike_body_query.single_mut();
|
let (bike_xform, mut bike_forces) = bike_body_query.single_mut();
|
||||||
|
|
||||||
for (caster, hits) in front_wheel_query.iter() {
|
for (caster, hits, mut prev) in front_wheel_query.iter_mut() {
|
||||||
for hit in hits.iter() {
|
for hit in hits.iter() {
|
||||||
if let Some(ref mut xform) = front_wheel_xform {
|
if let Some(ref mut xform) = front_wheel_xform {
|
||||||
let force = suspension_force(
|
let force =
|
||||||
caster,
|
suspension_force(caster, hit, &config, &mut prev.0, dt, xform, true);
|
||||||
hit,
|
|
||||||
&config,
|
|
||||||
front_wheel_vel,
|
|
||||||
xform,
|
|
||||||
bike_xform.rotation,
|
|
||||||
true,
|
|
||||||
);
|
|
||||||
bike_forces.apply_force_at_point(
|
bike_forces.apply_force_at_point(
|
||||||
force,
|
force,
|
||||||
caster.global_origin(),
|
caster.global_origin(),
|
||||||
|
@ -220,18 +203,11 @@ mod systems {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
for (caster, hits) in rear_wheel_query.iter() {
|
for (caster, hits, mut prev) in rear_wheel_query.iter_mut() {
|
||||||
for hit in hits.iter() {
|
for hit in hits.iter() {
|
||||||
if let Some(ref mut xform) = rear_wheel_xform {
|
if let Some(ref mut xform) = rear_wheel_xform {
|
||||||
let force = suspension_force(
|
let force =
|
||||||
caster,
|
suspension_force(caster, hit, &config, &mut prev.0, dt, xform, false);
|
||||||
hit,
|
|
||||||
&config,
|
|
||||||
rear_wheel_vel,
|
|
||||||
xform,
|
|
||||||
bike_xform.rotation,
|
|
||||||
false,
|
|
||||||
);
|
|
||||||
bike_forces.apply_force_at_point(
|
bike_forces.apply_force_at_point(
|
||||||
force,
|
force,
|
||||||
caster.global_origin(),
|
caster.global_origin(),
|
||||||
|
@ -246,12 +222,11 @@ mod systems {
|
||||||
caster: &ShapeCaster,
|
caster: &ShapeCaster,
|
||||||
hit: &ShapeHitData,
|
hit: &ShapeHitData,
|
||||||
config: &SuspensionConfig,
|
config: &SuspensionConfig,
|
||||||
wheel_vel: Vec3,
|
previous_dispacement: &mut f32,
|
||||||
|
dt: f32,
|
||||||
wheel_xform: &mut Transform,
|
wheel_xform: &mut Transform,
|
||||||
rotation: Quat,
|
|
||||||
is_front: bool,
|
is_front: bool,
|
||||||
) -> Vec3 {
|
) -> Vec3 {
|
||||||
let mut loc = Vec3::ZERO;
|
|
||||||
let mut up_force = Vec3::ZERO;
|
let mut up_force = Vec3::ZERO;
|
||||||
let attach = if is_front {
|
let attach = if is_front {
|
||||||
config.front_attach
|
config.front_attach
|
||||||
|
@ -260,16 +235,18 @@ mod systems {
|
||||||
};
|
};
|
||||||
let dist = hit.distance;
|
let dist = hit.distance;
|
||||||
let cdir = caster.direction.as_vec3();
|
let cdir = caster.direction.as_vec3();
|
||||||
let dir = rotation.mul_vec3(cdir);
|
let dir = caster.global_direction().as_vec3();
|
||||||
if dist < config.rest_dist {
|
let loc = if dist < config.rest_dist {
|
||||||
loc = attach + (cdir * dist);
|
|
||||||
let displacement = config.rest_dist - dist;
|
let displacement = config.rest_dist - dist;
|
||||||
let damper_vel = dir.dot(wheel_vel);
|
let damper_vel = (displacement - *previous_dispacement) / dt;
|
||||||
|
dbg!(damper_vel);
|
||||||
|
*previous_dispacement = displacement;
|
||||||
let mag = config.konstant * displacement - config.damping * damper_vel;
|
let mag = config.konstant * displacement - config.damping * damper_vel;
|
||||||
up_force = -dir * mag;
|
up_force = -dir * mag;
|
||||||
|
attach + (cdir * dist)
|
||||||
} else {
|
} else {
|
||||||
loc = attach + (cdir * config.rest_dist);
|
attach + (cdir * config.rest_dist)
|
||||||
}
|
};
|
||||||
|
|
||||||
wheel_xform.translation = loc;
|
wheel_xform.translation = loc;
|
||||||
|
|
||||||
|
@ -290,9 +267,6 @@ impl Plugin for CyberPhysicsPlugin {
|
||||||
.register_type::<CyberLean>()
|
.register_type::<CyberLean>()
|
||||||
.add_plugins((PhysicsPlugins::default(), PhysicsDebugPlugin::default()))
|
.add_plugins((PhysicsPlugins::default(), PhysicsDebugPlugin::default()))
|
||||||
.insert_resource(SubstepCount(12))
|
.insert_resource(SubstepCount(12))
|
||||||
.add_systems(Startup, |mut gravity: ResMut<Gravity>| {
|
|
||||||
gravity.0 *= 0.01;
|
|
||||||
})
|
|
||||||
.add_systems(Update, (apply_lean, calculate_lean, suspension));
|
.add_systems(Update, (apply_lean, calculate_lean, suspension));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in a new issue