nebkor/avian3d-sandbox
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

working damping/suspension

Browse source
This commit is contained in:
Joe Ardent 2025-02-19 10:56:14 -08:00
parent c924428a9e
commit 8c16a7f9d2
2 changed files with 48 additions and 78 deletions

38
src/bike.rs
View file

@ -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);
} }
} }

88
src/physics.rs
View file

@ -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));
} }
} }