diff --git a/src/bike.rs b/src/bike.rs
index 6016dd9..d029adc 100644
--- a/src/bike.rs
+++ b/src/bike.rs
@@ -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);
}
}
diff --git a/src/physics.rs b/src/physics.rs
index 2f22c7f..52d36f7 100644
--- a/src/physics.rs
+++ b/src/physics.rs
@@ -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));
}
}