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)]
pub struct Rearing;
#[derive(Debug, Component, Clone, Default)]
pub struct PreviousDisplacement(pub f32);
#[derive(Debug, Resource, Reflect)]
#[reflect(Resource)]
pub struct SuspensionConfig {
@ -41,8 +44,8 @@ impl Default for SuspensionConfig {
fn default() -> Self {
Self {
rest_dist: REST_DISTANCE,
konstant: 50.0,
damping: 50.0,
konstant: 800.0,
damping: -100.0,
front_attach: FRONT_ATTACH,
rear_attach: REAR_ATTACH,
}
@ -54,7 +57,7 @@ fn spawn_bike(
mut meshes: ResMut<Assets<Mesh>>,
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 body_collider =
@ -86,13 +89,7 @@ fn spawn_bike(
MeshMaterial3d(materials.add(color)),
);
builder.spawn(pbr_bundle);
spawn_wheels(
builder,
meshes,
materials,
Transform::default(),
builder.parent_entity(),
);
spawn_wheels(builder, meshes, materials, builder.parent_entity());
});
}
@ -107,7 +104,7 @@ fn spawn_caster(
let caster = ShapeCaster::new(collider, Vec3::ZERO, Quat::IDENTITY, direction)
.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 {
entity.insert((Steering, Name::new("front wheel")));
} else {
@ -119,19 +116,17 @@ fn spawn_wheels(
commands: &mut ChildBuilder,
mut meshes: ResMut<Assets<Mesh>>,
mut materials: ResMut<Assets<StandardMaterial>>,
xform: Transform,
body: Entity,
) {
let (mesh, collider) = gen_tire();
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(
commands,
collider.clone(),
Transform::from_translation(front_attach),
Transform::from_translation(FRONT_ATTACH),
Dir3::new_unchecked(front_rake),
body,
true,
@ -146,13 +141,13 @@ fn spawn_wheels(
);
let rear_rake = Vec3::new(0.0, -1.0, 0.57).normalize();
let rear_attach = xform.translation + xform.back() * 0.7;
let rear_wheel_pos = rear_attach + (rear_rake * REST_DISTANCE);
//let rear_attach = xform.translation + xform.back() * 0.7;
let rear_wheel_pos = REAR_ATTACH + (rear_rake * REST_DISTANCE);
spawn_caster(
commands,
collider,
Transform::from_translation(rear_attach),
Transform::from_translation(REAR_ATTACH),
Dir3::new_unchecked(rear_rake),
body,
false,
@ -172,8 +167,8 @@ fn spawn_wheels(
//-************************************************************************
fn gen_tire() -> (Mesh, Collider) {
let tire_mesh: Mesh = Sphere::new(0.4).into();
let collider = Collider::sphere(0.4);
let tire_mesh: Mesh = Sphere::new(WHEEL_RADIUS).into();
let collider = Collider::sphere(WHEEL_RADIUS);
(tire_mesh, collider)
}
@ -200,7 +195,7 @@ fn wheel_mesh(
Mesh3d(meshes.add(tire_mesh)),
MeshMaterial3d(materials.add(wheel_material.clone())),
xform,
LinearVelocity::ZERO,
PreviousDisplacement::default(),
wheel,
));
}
@ -210,6 +205,7 @@ pub struct CyberBikePlugin;
impl Plugin for CyberBikePlugin {
fn build(&self, app: &mut App) {
app.init_resource::<SuspensionConfig>();
app.register_type::<SuspensionConfig>();
app.add_systems(Startup, spawn_bike);
}
}

88
src/physics.rs
View file

@ -52,33 +52,22 @@ impl CatControllerState {
self.roll_prev = error;
(derivative, self.roll_integral)
}
pub fn set_integral_limits(&mut self, roll: f32) {
self.roll_limit = roll;
}
}
mod systems {
use std::f32::consts::{FRAC_PI_3, FRAC_PI_4};
use avian3d::prelude::*;
use bevy::{math::VectorSpace, prelude::*};
use bevy::prelude::*;
use super::{CatControllerSettings, CatControllerState, CyberLean};
use crate::{
bike::{CyberBikeBody, CyberWheel, Rearing, Steering, SuspensionConfig},
bike::{
CyberBikeBody, CyberWheel, PreviousDisplacement, Rearing, Steering, SuspensionConfig,
},
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 {
// thanks to https://danceswithcode.net/engineeringnotes/quaternions/quaternions.html
let [x, y, z] = pt.normalize().to_array();
@ -172,45 +161,39 @@ mod systems {
pub(super) fn suspension(
mut bike_body_query: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
mut wheel_mesh_query: Query<
(&mut Transform, &LinearVelocity, &CyberWheel),
Without<CyberBikeBody>,
mut wheel_mesh_query: Query<(&mut Transform, &CyberWheel), Without<CyberBikeBody>>,
mut front_wheel_query: Query<
(&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>,
time: Res<Time>,
) {
let dt = time.delta().as_secs_f32();
let mut front_wheel_xform = None;
let mut rear_wheel_xform = None;
let mut front_wheel_vel = Vec3::ZERO;
let mut rear_wheel_vel = Vec3::ZERO;
for (xform, vel, wheel) in wheel_mesh_query.iter_mut() {
for (xform, wheel) in wheel_mesh_query.iter_mut() {
match wheel {
CyberWheel::Front => {
front_wheel_xform = Some(xform);
front_wheel_vel = **vel;
}
CyberWheel::Rear => {
rear_wheel_xform = Some(xform);
rear_wheel_vel = **vel;
}
}
}
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() {
if let Some(ref mut xform) = front_wheel_xform {
let force = suspension_force(
caster,
hit,
&config,
front_wheel_vel,
xform,
bike_xform.rotation,
true,
);
let force =
suspension_force(caster, hit, &config, &mut prev.0, dt, xform, true);
bike_forces.apply_force_at_point(
force,
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() {
if let Some(ref mut xform) = rear_wheel_xform {
let force = suspension_force(
caster,
hit,
&config,
rear_wheel_vel,
xform,
bike_xform.rotation,
false,
);
let force =
suspension_force(caster, hit, &config, &mut prev.0, dt, xform, false);
bike_forces.apply_force_at_point(
force,
caster.global_origin(),
@ -246,12 +222,11 @@ mod systems {
caster: &ShapeCaster,
hit: &ShapeHitData,
config: &SuspensionConfig,
wheel_vel: Vec3,
previous_dispacement: &mut f32,
dt: f32,
wheel_xform: &mut Transform,
rotation: Quat,
is_front: bool,
) -> Vec3 {
let mut loc = Vec3::ZERO;
let mut up_force = Vec3::ZERO;
let attach = if is_front {
config.front_attach
@ -260,16 +235,18 @@ mod systems {
};
let dist = hit.distance;
let cdir = caster.direction.as_vec3();
let dir = rotation.mul_vec3(cdir);
if dist < config.rest_dist {
loc = attach + (cdir * dist);
let dir = caster.global_direction().as_vec3();
let loc = if dist < config.rest_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;
up_force = -dir * mag;
attach + (cdir * dist)
} else {
loc = attach + (cdir * config.rest_dist);
}
attach + (cdir * config.rest_dist)
};
wheel_xform.translation = loc;
@ -290,9 +267,6 @@ impl Plugin for CyberPhysicsPlugin {
.register_type::<CyberLean>()
.add_plugins((PhysicsPlugins::default(), PhysicsDebugPlugin::default()))
.insert_resource(SubstepCount(12))
.add_systems(Startup, |mut gravity: ResMut<Gravity>| {
gravity.0 *= 0.01;
})
.add_systems(Update, (apply_lean, calculate_lean, suspension));
}
}