Compare commits

...

25 commits

Author SHA1 Message Date
Joe Ardent
eaba0edcd2 add drag, bias power to front 2025-04-06 16:52:21 -07:00
Joe Ardent
0b2086ec5f use force to lean instead of torque 2025-04-06 15:20:00 -07:00
Joe Ardent
60fbbf4637 something's weird 2025-04-06 14:45:10 -07:00
Joe Ardent
cc519efdb6 seems to be right? 2025-04-05 20:30:10 -07:00
Joe Ardent
884dc14c9f split suspension and steering 2025-03-30 16:41:03 -07:00
Joe Ardent
cfe8a0acbd checkpoint 2025-03-30 13:31:54 -07:00
Joe Ardent
3498fcc5aa working ray-casting, no jitter 2025-03-29 16:51:13 -07:00
Joe Ardent
ffbe45cf8e use a raycast instead of shapecast 2025-03-29 16:30:06 -07:00
Joe Ardent
3e11c6439f inline suspension force calc 2025-03-29 16:08:27 -07:00
Joe Ardent
7d28281781 add camera reset key 2025-03-29 16:04:37 -07:00
Joe Ardent
bb3aeae0bf shut the key noise up 2025-03-29 13:32:37 -07:00
Joe Ardent
1daae3e5c6 fix lean calc 2025-03-29 13:31:30 -07:00
Joe Ardent
d2ccb14b95 cargo update 2025-03-29 13:31:18 -07:00
Joe Ardent
355dae1f88 add tweak system for tuning suspension at runtime 2025-03-29 13:19:27 -07:00
Joe Ardent
8978a25965 checkpoint 2025-02-25 22:48:15 -08:00
Joe Ardent
5852df32f7 checkpoint 2025-02-24 21:26:46 -08:00
Joe Ardent
b5ed478dbe crappy thrust and yaw handling 2025-02-19 22:16:19 -08:00
Joe Ardent
8c16a7f9d2 working damping/suspension 2025-02-19 10:56:14 -08:00
Joe Ardent
c924428a9e bouncy suspension, no friction 2025-02-18 22:24:44 -08:00
Joe Ardent
b8a54626a7 correct children casters 2025-02-17 21:15:13 -08:00
Joe Ardent
6dd7277584 checkpoint, colliders are parented correctly 2025-02-17 20:30:23 -08:00
Joe Ardent
22701733d4 cargo update 2025-02-16 13:57:37 -08:00
Joe Ardent
4bfd8740fb add input plugin, doesn't do anything yet 2025-02-16 13:57:26 -08:00
Joe Ardent
15c79c62b8 cargo update for avian 2025-01-26 11:31:07 -08:00
Joe Ardent
5618472e3e cargo update 2025-01-25 19:58:55 -08:00
7 changed files with 1058 additions and 925 deletions

1302
Cargo.lock generated

File diff suppressed because it is too large Load diff

View file

@ -1,12 +1,17 @@
[package] [package]
name = "physics-test" name = "physics-test"
version = "0.1.0" version = "0.1.0"
edition = "2021" edition = "2024"
[dependencies] [dependencies]
avian3d = { git = "https://github.com/Jondolf/avian.git" }
bevy = { version = "0.15", features = ["bevy_dev_tools"] } bevy = { version = "0.15", features = ["bevy_dev_tools"] }
bevy-inspector-egui = "0.28" bevy-inspector-egui = "0.29"
[dependencies.avian3d]
default-features = false
version = "0.2"
features = ["3d", "f32", "parry-f32", "debug-plugin", "default-collider", "collider-from-mesh"]
[features] [features]
no-mesh = [] no-mesh = []

View file

@ -1,40 +1,88 @@
use std::f32::consts::FRAC_PI_2; use avian3d::{
math::{Scalar, FRAC_PI_2},
use avian3d::prelude::*; prelude::*,
};
use bevy::prelude::*; use bevy::prelude::*;
use crate::physics::CatControllerState; use crate::physics::CatControllerState;
pub const SPRING_CONSTANT: Scalar = 60.0;
pub const DAMPING_CONSTANT: Scalar = 10.0;
pub const WHEEL_RADIUS: Scalar = 0.4;
pub const REST_DISTANCE: Scalar = 1.5 + WHEEL_RADIUS;
pub const FRICTION_COEFF: Scalar = 0.9;
pub const FRONT_ATTACH: Vec3 = Vec3::new(0.0, 0.0, -0.5);
pub const REAR_ATTACH: Vec3 = Vec3::new(0.0, 0.0, 0.5);
#[derive(Component)] #[derive(Component)]
pub struct CyberBikeBody; pub struct CyberBikeBody;
#[derive(Component)] #[derive(Component, Clone, Copy, Debug)]
pub struct CyberWheel; pub enum CyberWheel {
Front,
Rear,
}
// marker for front suspension joint #[derive(Component, Clone, Copy, Debug, Reflect, Default)]
#[derive(Component)] #[reflect(Component)]
pub struct Steering; pub struct WheelState {
#[derive(Component)] pub displacement: Scalar,
pub struct FrontHub; pub force_normal: Scalar,
pub sliding: bool,
pub contact_point: Option<Vec3>,
pub contact_normal: Option<Vec3>,
}
// marker for rear suspension joint impl WheelState {
#[derive(Component)] pub fn reset(&mut self) {
pub struct Rearing; *self = Self::default();
#[derive(Component)] }
pub struct RearHub; }
#[derive(Component, Clone, Copy, Debug, Reflect)]
#[reflect(Component)]
#[require(WheelState)]
pub struct WheelConfig {
pub attach: Vec3,
pub rest_dist: Scalar,
pub konstant: Scalar,
pub damping: Scalar,
pub friction: Scalar,
pub radius: Scalar,
}
impl WheelConfig {
fn new(
attach: Vec3,
rest_dist: Scalar,
konstant: Scalar,
damping: Scalar,
friction: Scalar,
radius: Scalar,
) -> Self {
WheelConfig {
attach,
rest_dist,
konstant,
damping,
friction,
radius,
}
}
}
fn spawn_bike( fn spawn_bike(
mut commands: Commands, mut commands: Commands,
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, 5.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 =
Collider::capsule_endpoints(0.5, Vec3::new(0.0, 0.0, -0.65), Vec3::new(0.0, 0.0, 0.8)); Collider::capsule_endpoints(0.5, Vec3::new(0.0, 0.0, -0.65), Vec3::new(0.0, 0.0, 0.8));
let bike = commands commands
.spawn((xform, Visibility::default())) .spawn((xform, Visibility::default()))
.insert(( .insert((
Name::new("body"), Name::new("body"),
@ -44,110 +92,120 @@ fn spawn_bike(
SleepingDisabled, SleepingDisabled,
CyberBikeBody, CyberBikeBody,
CatControllerState::default(), CatControllerState::default(),
ColliderDensity(20.0), ColliderDensity(1.2),
AngularDamping(0.2),
LinearDamping(0.1), LinearDamping(0.1),
AngularDamping(2.0),
LinearVelocity::ZERO,
AngularVelocity::ZERO,
ExternalForce::ZERO.with_persistence(false), ExternalForce::ZERO.with_persistence(false),
ExternalTorque::ZERO.with_persistence(false), ExternalTorque::ZERO.with_persistence(false),
)) ))
.with_children(|builder| { .with_children(|builder| {
let color = Color::srgb(0.7, 0.05, 0.7); let color = Color::srgb(0.7, 0.05, 0.7);
let mut rotation = Transform::default(); // Transform::from_rotation(Quat::from_rotation_y(FRAC_PI_2)); let mut xform = Transform::default();
rotation.rotate_x(FRAC_PI_2); xform.rotate_x(FRAC_PI_2);
let pbr_bundle = ( let pbr_bundle = (
Mesh3d(meshes.add(Capsule3d::new(0.5, 1.45))), Mesh3d(meshes.add(Capsule3d::new(0.5, 1.45))),
rotation, xform,
MeshMaterial3d(materials.add(color)), MeshMaterial3d(materials.add(color)),
); );
builder.spawn(pbr_bundle); builder.spawn(pbr_bundle);
}) spawn_wheels(builder, meshes, materials, builder.parent_entity());
.id(); });
spawn_wheels(commands, meshes, materials, xform, bike);
} }
fn spawn_wheels( fn spawn_wheels(
mut commands: Commands, 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 front_rake = Vec3::new(0.0, -1.0, -0.57).normalize(); // about 30 degrees let mesh: Mesh = Sphere::new(WHEEL_RADIUS).into();
let front_pos = xform.translation + front_rake;
let (mesh, collider) = gen_tire(); let front_rake = Vec3::new(0.0, -1.0, -0.9).normalize(); // about 30 degrees
let front_wheel_pos = FRONT_ATTACH + (front_rake * REST_DISTANCE);
let front_hub = wheels_helper( wheel_caster(
&mut commands, commands,
FRONT_ATTACH,
Dir3::new_unchecked(front_rake),
body,
REST_DISTANCE,
CyberWheel::Front,
);
wheel_mesh(
commands,
&mut meshes, &mut meshes,
&mut materials, &mut materials,
front_pos, front_wheel_pos,
mesh.clone(), mesh.clone(),
collider.clone(), WheelConfig::new(
FRONT_ATTACH,
REST_DISTANCE,
SPRING_CONSTANT,
DAMPING_CONSTANT,
FRICTION_COEFF,
WHEEL_RADIUS,
),
CyberWheel::Front,
); );
commands.entity(front_hub).insert(FrontHub);
commands.spawn((
Steering,
FixedJoint::new(front_hub, body).with_local_anchor_2(*xform.forward() + front_rake),
));
let rear_rake = Vec3::new(0.0, -1.0, 0.57).normalize(); let rear_rake = Vec3::new(0.0, -1.0, 0.9).normalize();
let rear_pos = xform.translation + rear_rake; let rear_wheel_pos = REAR_ATTACH + (rear_rake * REST_DISTANCE);
let rear_hub = wheels_helper( wheel_caster(
&mut commands, commands,
REAR_ATTACH,
Dir3::new_unchecked(rear_rake),
body,
REST_DISTANCE,
CyberWheel::Rear,
);
wheel_mesh(
commands,
&mut meshes, &mut meshes,
&mut materials, &mut materials,
rear_pos, rear_wheel_pos,
mesh, mesh,
collider, WheelConfig::new(
REAR_ATTACH,
REST_DISTANCE,
SPRING_CONSTANT,
DAMPING_CONSTANT,
FRICTION_COEFF,
WHEEL_RADIUS,
),
CyberWheel::Rear,
); );
commands.entity(rear_hub).insert(RearHub);
commands.spawn((
Rearing,
FixedJoint::new(rear_hub, body).with_local_anchor_2(*xform.back() + rear_rake),
));
} }
//-************************************************************************ //-************************************************************************
// helper fns for the wheels // helper fns for the wheels
//-************************************************************************ //-************************************************************************
fn gen_tire() -> (Mesh, Collider) { fn wheel_caster(
let mut tire_mesh: Mesh = Torus::new(0.25, 0.40).into(); commands: &mut ChildBuilder,
let tire_verts = tire_mesh origin: Vec3,
.attribute(Mesh::ATTRIBUTE_POSITION) direction: Dir3,
.unwrap() parent: Entity,
.as_float3() rest_dist: Scalar,
.unwrap() wheel: CyberWheel,
.iter() ) {
.map(|v| { let caster = RayCaster::new(origin, direction)
// .with_max_distance(rest_dist)
let v = Vec3::from_array(*v); .with_max_hits(1)
let m = Mat3::from_rotation_z(90.0f32.to_radians()); .with_query_filter(SpatialQueryFilter::from_excluded_entities([parent]));
let p = m.mul_vec3(v);
p.to_array()
})
.collect::<Vec<[f32; 3]>>();
tire_mesh.remove_attribute(Mesh::ATTRIBUTE_POSITION);
tire_mesh.insert_attribute(Mesh::ATTRIBUTE_POSITION, tire_verts);
let collider = Collider::convex_hull_from_mesh(&tire_mesh).unwrap(); commands.spawn((caster, wheel));
(tire_mesh, collider)
} }
fn wheels_helper( fn wheel_mesh(
commands: &mut Commands, commands: &mut ChildBuilder,
meshes: &mut ResMut<Assets<Mesh>>, meshes: &mut ResMut<Assets<Mesh>>,
materials: &mut ResMut<Assets<StandardMaterial>>, materials: &mut ResMut<Assets<StandardMaterial>>,
position: Vec3, position: Vec3,
tire_mesh: Mesh, tire_mesh: Mesh,
collider: Collider, config: WheelConfig,
) -> Entity { wheel: CyberWheel,
) {
let wheel_material = &StandardMaterial { let wheel_material = &StandardMaterial {
base_color: Color::srgb(0.01, 0.01, 0.01), base_color: Color::srgb(0.01, 0.01, 0.01),
alpha_mode: AlphaMode::Opaque, alpha_mode: AlphaMode::Opaque,
@ -156,47 +214,29 @@ fn wheels_helper(
}; };
let xform = Transform::from_translation(position); let xform = Transform::from_translation(position);
let hub_mesh: Mesh = Sphere::new(0.1).into();
let hub = commands let name = match wheel {
.spawn(( CyberWheel::Front => "front tire",
Name::new("hub"), CyberWheel::Rear => "rear tire",
RigidBody::Dynamic, };
MassPropertiesBundle::from_shape(&Collider::sphere(0.1), 200.0),
Mesh3d(meshes.add(hub_mesh)),
MeshMaterial3d(materials.add(wheel_material.clone())),
xform,
))
.id();
let tire = commands commands.spawn((
.spawn(( Name::new(name),
Name::new("tire"), config,
Mesh3d(meshes.add(tire_mesh)), Mesh3d(meshes.add(tire_mesh)),
MeshMaterial3d(materials.add(wheel_material.clone())), MeshMaterial3d(materials.add(wheel_material.clone())),
xform, xform,
CyberWheel, TransformInterpolation,
RigidBody::Dynamic, wheel,
collider, ));
Friction::new(0.9).with_dynamic_coefficient(0.6),
Restitution::new(0.1),
ColliderDensity(30.0),
CollisionLayers::from_bits(2, 2),
ExternalTorque::ZERO.with_persistence(false),
CollisionMargin(0.05),
SweptCcd::NON_LINEAR,
))
.id();
// connect hubs and tires to make wheels
commands.spawn(RevoluteJoint::new(hub, tire).with_aligned_axis(Vec3::X));
hub
} }
pub struct CyberBikePlugin; pub struct CyberBikePlugin;
impl Plugin for CyberBikePlugin { impl Plugin for CyberBikePlugin {
fn build(&self, app: &mut App) { fn build(&self, app: &mut App) {
app.register_type::<WheelConfig>();
app.register_type::<WheelState>();
app.add_systems(Startup, spawn_bike); app.add_systems(Startup, spawn_bike);
} }
} }

View file

@ -16,7 +16,7 @@ impl Default for DebugCamOffset {
fn default() -> Self { fn default() -> Self {
DebugCamOffset { DebugCamOffset {
rot: 60.0, rot: 60.0,
dist: 10.0, dist: 30.0,
alt: 4.0, alt: 4.0,
} }
} }
@ -36,34 +36,28 @@ fn update_camera_pos(mut offset: ResMut<DebugCamOffset>, mut keys: ResMut<Button
KeyCode::ArrowRight => offset.rot += 5.0, KeyCode::ArrowRight => offset.rot += 5.0,
KeyCode::ArrowUp => { KeyCode::ArrowUp => {
if shifted { if shifted {
bevy::log::info!("up, shifted");
offset.alt += 0.5; offset.alt += 0.5;
} else { } else {
bevy::log::info!("up");
offset.dist -= 0.5; offset.dist -= 0.5;
} }
} }
KeyCode::ArrowDown => { KeyCode::ArrowDown => {
if shifted { if shifted {
bevy::log::info!("down, shifted");
offset.alt -= 0.5; offset.alt -= 0.5;
} else { } else {
bevy::log::info!("down");
offset.dist += 0.5; offset.dist += 0.5;
} }
} }
KeyCode::KeyR => {
*offset = DebugCamOffset::default();
}
_ => continue, _ => continue,
} }
} }
let just_released: HashSet<_> = keys.get_just_released().cloned().collect(); let released: Vec<_> = keys.get_just_released().copied().collect();
if !just_released.is_empty() { for key in released {
let released_shift = just_released.contains(&KeyCode::ShiftLeft) keys.clear_just_released(key);
|| just_released.contains(&KeyCode::ShiftRight);
keys.reset_all();
if !released_shift && shifted {
keys.press(KeyCode::ShiftLeft);
}
} }
} }

51
src/input.rs Normal file
View file

@ -0,0 +1,51 @@
use bevy::{
input::gamepad::{GamepadAxisChangedEvent, GamepadButtonChangedEvent, GamepadEvent},
prelude::*,
};
#[derive(Default, Debug, Resource)]
pub(crate) struct InputState {
pub yaw: f32,
pub throttle: f32,
pub brake: bool,
pub pitch: f32,
}
fn update_input(mut events: EventReader<GamepadEvent>, mut istate: ResMut<InputState>) {
for pad_event in events.read() {
match pad_event {
GamepadEvent::Button(button_event) => {
let GamepadButtonChangedEvent { button, value, .. } = button_event;
match button {
GamepadButton::RightTrigger2 => istate.throttle = *value,
GamepadButton::LeftTrigger2 => istate.throttle = -value,
GamepadButton::East => {
istate.brake = value > &0.5;
}
_ => {}
}
}
GamepadEvent::Axis(axis_event) => {
let GamepadAxisChangedEvent { axis, value, .. } = axis_event;
match axis {
GamepadAxis::LeftStickX => {
istate.yaw = *value;
}
GamepadAxis::RightStickY => {
istate.pitch = *value;
}
_ => {}
}
}
GamepadEvent::Connection(_) => {}
}
}
}
pub struct CyberInputPlugin;
impl Plugin for CyberInputPlugin {
fn build(&self, app: &mut App) {
app.init_resource::<InputState>()
.add_systems(Update, update_input);
}
}

View file

@ -2,7 +2,7 @@ use avian3d::prelude::{
Collider, ColliderDensity, CollisionLayers, CollisionMargin, Friction, PhysicsGizmos, RigidBody, Collider, ColliderDensity, CollisionLayers, CollisionMargin, Friction, PhysicsGizmos, RigidBody,
}; };
use bevy::{ use bevy::{
color::palettes::css::SILVER, color::{palettes::css::SILVER, Alpha},
pbr::MeshMaterial3d, pbr::MeshMaterial3d,
prelude::{ prelude::{
default, App, AppGizmoBuilder, Assets, BuildChildren, ButtonInput, ChildBuild, Color, default, App, AppGizmoBuilder, Assets, BuildChildren, ButtonInput, ChildBuild, Color,
@ -15,10 +15,12 @@ use bevy_inspector_egui::quick::WorldInspectorPlugin;
mod bike; mod bike;
mod camera; mod camera;
mod input;
mod physics; mod physics;
use bike::CyberBikePlugin; use bike::CyberBikePlugin;
use camera::CamPlug; use camera::CamPlug;
use input::CyberInputPlugin;
use physics::CyberPhysicsPlugin; use physics::CyberPhysicsPlugin;
fn main() { fn main() {
@ -27,6 +29,7 @@ fn main() {
DefaultPlugins, DefaultPlugins,
CamPlug, CamPlug,
CyberBikePlugin, CyberBikePlugin,
CyberInputPlugin,
CyberPhysicsPlugin, CyberPhysicsPlugin,
WorldInspectorPlugin::new(), WorldInspectorPlugin::new(),
)) ))
@ -57,7 +60,7 @@ fn ground_and_light(
.spawn(( .spawn((
spatial_bundle, spatial_bundle,
RigidBody::Static, RigidBody::Static,
Collider::cuboid(50.0, 5., 50.0), Collider::cuboid(500.0, 5., 500.0),
CollisionMargin(0.1), CollisionMargin(0.1),
ColliderDensity(1000.0), ColliderDensity(1000.0),
CollisionLayers::ALL, CollisionLayers::ALL,
@ -65,8 +68,8 @@ fn ground_and_light(
)) ))
.with_children(|p| { .with_children(|p| {
let bundle = ( let bundle = (
Mesh3d(meshes.add(Plane3d::default().mesh().size(50.0, 50.0))), Mesh3d(meshes.add(Plane3d::default().mesh().size(500.0, 500.0))),
MeshMaterial3d(materials.add(Color::from(SILVER))), MeshMaterial3d(materials.add(Color::from(SILVER).with_alpha(0.7))),
Transform::from_xyz(0.0, 2.5, 0.0), Transform::from_xyz(0.0, 2.5, 0.0),
Visibility::Visible, Visibility::Visible,
); );

View file

@ -1,6 +1,8 @@
use avian3d::prelude::*; use avian3d::{math::Scalar, prelude::*};
use bevy::prelude::*; use bevy::prelude::*;
const DRAG_COEFF: Scalar = 0.1;
#[derive(Resource, Default, Debug, Reflect)] #[derive(Resource, Default, Debug, Reflect)]
#[reflect(Resource)] #[reflect(Resource)]
struct CyberLean { struct CyberLean {
@ -18,9 +20,9 @@ pub struct CatControllerSettings {
impl Default for CatControllerSettings { impl Default for CatControllerSettings {
fn default() -> Self { fn default() -> Self {
Self { Self {
kp: 1200.0, kp: 16.0,
kd: 10.0, kd: 2.0,
ki: 50.0, ki: 0.9,
} }
} }
} }
@ -29,7 +31,6 @@ impl Default for CatControllerSettings {
pub struct CatControllerState { pub struct CatControllerState {
pub roll_integral: f32, pub roll_integral: f32,
pub roll_prev: f32, pub roll_prev: f32,
decay_factor: f32,
roll_limit: f32, roll_limit: f32,
} }
@ -38,19 +39,12 @@ impl Default for CatControllerState {
Self { Self {
roll_integral: Default::default(), roll_integral: Default::default(),
roll_prev: Default::default(), roll_prev: Default::default(),
decay_factor: 0.99,
roll_limit: 1.5, roll_limit: 1.5,
} }
} }
} }
impl CatControllerState { impl CatControllerState {
pub fn decay(&mut self) {
if self.roll_integral.abs() > 0.001 {
self.roll_integral *= self.decay_factor;
}
}
pub fn update_roll(&mut self, error: f32, dt: f32) -> (f32, f32) { pub fn update_roll(&mut self, error: f32, dt: f32) -> (f32, f32) {
let lim = self.roll_limit; let lim = self.roll_limit;
self.roll_integral = (self.roll_integral + (error * dt)).min(lim).max(-lim); self.roll_integral = (self.roll_integral + (error * dt)).min(lim).max(-lim);
@ -58,29 +52,19 @@ 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; use std::f32::consts::{FRAC_PI_3, FRAC_PI_4};
use avian3d::prelude::*; use avian3d::prelude::*;
use bevy::prelude::*; use bevy::prelude::*;
use super::{CatControllerSettings, CatControllerState, CyberLean}; use super::{CatControllerSettings, CatControllerState, CyberLean, DRAG_COEFF};
use crate::bike::CyberBikeBody; use crate::{
bike::{CyberBikeBody, CyberWheel, WheelConfig, WheelState, WHEEL_RADIUS},
fn _yaw_to_angle(yaw: f32) -> f32 { input::InputState,
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
@ -95,32 +79,48 @@ mod systems {
pub(super) fn calculate_lean( pub(super) fn calculate_lean(
bike_state: Query<(&LinearVelocity, &Transform), With<CyberBikeBody>>, bike_state: Query<(&LinearVelocity, &Transform), With<CyberBikeBody>>,
wheels: Query<&GlobalTransform, With<CyberWheel>>,
input: Res<InputState>,
gravity: Res<Gravity>,
mut lean: ResMut<CyberLean>, 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 (velocity, xform) = bike_state.single();
let vel = velocity.dot(*xform.forward()); let vel = velocity.dot(*xform.forward());
let v_squared = vel.powi(2); let v_squared = vel.powi(2);
let wheel_base = 1.145f32; let steering_angle = yaw_to_angle(input.yaw);
let radius = wheel_base; let radius = base / steering_angle.tan();
let gravity = -9.8f32; let gravity = gravity.0.length();
let v2_r = v_squared / radius; let v2_r = v_squared / radius;
let tan_theta = (v2_r / gravity).clamp(-FRAC_PI_3, FRAC_PI_3); let tan_theta = (v2_r / gravity).clamp(-FRAC_PI_3, FRAC_PI_3);
if tan_theta.is_normal() { if tan_theta.is_normal() {
lean.lean = -tan_theta.atan().clamp(-FRAC_PI_3, FRAC_PI_3); lean.lean = tan_theta.atan().clamp(-FRAC_PI_3, FRAC_PI_3);
} else { } else {
lean.lean = 0.0; //lean.lean = 0.0;
} }
} }
pub(super) fn apply_lean( pub(super) fn apply_lean(
mut bike_query: Query<(&Transform, &mut ExternalTorque, &mut CatControllerState)>, mut bike_query: Query<(&Transform, &mut ExternalForce, &mut CatControllerState)>,
wheels: Query<&WheelState>,
time: Res<Time>, time: Res<Time>,
settings: Res<CatControllerSettings>, settings: Res<CatControllerSettings>,
lean: Res<CyberLean>, lean: Res<CyberLean>,
mut gizmos: Gizmos, mut gizmos: Gizmos,
) { ) {
let (xform, mut torque, mut control_vars) = bike_query.single_mut(); let (xform, mut force, mut control_vars) = bike_query.single_mut();
let mut factor = 1.0;
for wheel in wheels.iter() {
if wheel.contact_point.is_none() {
factor -= 0.25;
}
}
let world_up = Vec3::Y; //xform.translation.normalize(); let world_up = Vec3::Y; //xform.translation.normalize();
let rot = Quat::from_axis_angle(*xform.back(), lean.lean); let rot = Quat::from_axis_angle(*xform.back(), lean.lean);
let target_up = rotate_point(&world_up, &rot).normalize(); let target_up = rotate_point(&world_up, &rot).normalize();
@ -135,7 +135,7 @@ mod systems {
// show which is forward // show which is forward
gizmos.arrow( gizmos.arrow(
*xform.forward() + xform.translation, *xform.forward() + xform.translation,
1.5 * *xform.forward() + xform.translation, 2.5 * *xform.forward() + xform.translation,
bevy::color::palettes::basic::PURPLE, bevy::color::palettes::basic::PURPLE,
); );
@ -150,19 +150,252 @@ mod systems {
let mag = let mag =
(settings.kp * roll_error) + (settings.ki * integral) + (settings.kd * derivative); (settings.kp * roll_error) + (settings.ki * integral) + (settings.kd * derivative);
if mag.is_finite() { if mag.is_finite() {
let tork = mag * *xform.back(); let lean_force = factor * mag * *xform.left();
torque.apply_torque(tork); force.apply_force_at_point(
lean_force,
xform.translation + *xform.up(),
xform.translation,
);
gizmos.arrow( gizmos.arrow(
xform.translation + Vec3::Y, xform.translation + *xform.up(),
xform.translation + tork + Vec3::Y, xform.translation + *xform.up() + lean_force,
Color::WHITE, Color::WHITE,
); );
} }
} }
} }
fn yaw_to_angle(yaw: f32) -> f32 {
let v = yaw.powi(5) * FRAC_PI_4;
if v.is_normal() {
v
} else {
0.0
}
}
pub(super) fn suspension(
mut bike_body_query: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
mut wheel_mesh_query: Query<
(&mut Transform, &mut WheelState, &WheelConfig, &CyberWheel),
Without<CyberBikeBody>,
>,
caster_query: Query<(&RayCaster, &RayHits, &CyberWheel)>,
time: Res<Time>,
mut gizmos: Gizmos,
) {
let dt = time.delta().as_secs_f32();
let mut front_caster = &RayCaster::default();
let mut rear_caster = &RayCaster::default();
let mut front_hits = &RayHits::default();
let mut rear_hits = &RayHits::default();
for (caster, hits, wheel) in caster_query.iter() {
match wheel {
CyberWheel::Front => {
front_caster = caster;
front_hits = hits;
}
CyberWheel::Rear => {
rear_caster = caster;
rear_hits = hits;
}
}
}
let (bike_xform, mut bike_forces) = bike_body_query.single_mut();
for (mut xform, mut state, config, wheel) in wheel_mesh_query.iter_mut() {
let (caster, hits) = match wheel {
CyberWheel::Front => (front_caster, front_hits),
CyberWheel::Rear => (rear_caster, rear_hits),
};
if let Some(hit) = hits.iter().next() {
let dist = hit.distance;
let hit_point = caster.global_origin() + caster.global_direction() * dist;
let displacement = config.rest_dist - dist;
let damper_vel = (state.displacement - displacement) / dt;
let mag = config.konstant * displacement - config.damping * damper_vel;
let mag = mag.max(0.0);
state.force_normal = mag;
state.contact_normal = Some(hit.normal);
state.contact_point = Some(hit_point);
state.displacement = displacement;
let cdir = caster.direction.as_vec3();
xform.translation = config.attach + (cdir * (dist - WHEEL_RADIUS));
let force = hit.normal * mag * dt * 100.0;
gizmos.arrow(
hit_point,
hit_point + force,
Color::linear_rgb(1., 0.5, 0.2),
);
bike_forces.apply_force_at_point(
force,
caster.global_origin(),
bike_xform.translation,
);
} else {
xform.translation = config.attach + (caster.direction.as_vec3() * config.rest_dist);
state.reset();
}
}
}
pub(super) fn steering(
mut bike_query: Query<
(
&Transform,
&LinearVelocity,
&mut ExternalForce,
RigidBodyQueryReadOnly,
),
With<CyberBikeBody>,
>,
mut wheels: Query<(&mut WheelState, &WheelConfig, &CyberWheel)>,
time: Res<Time>,
input: Res<InputState>,
mut gizmos: Gizmos,
) {
let Ok((bike_xform, bike_vel, mut bike_force, bike_body)) = bike_query.get_single_mut()
else {
bevy::log::warn!("No entity for bike_query.");
return;
};
let bike_vel = bike_vel.0;
let dt = time.delta().as_secs_f32();
let max_thrust = 2000.0;
let yaw_angle = -yaw_to_angle(input.yaw);
for (mut state, config, wheel) in wheels.iter_mut() {
if let Some(contact_point) = state.contact_point {
// if contact_point is Some, we also have a normal.
let normal = state.contact_normal.unwrap().normalize();
let max_force_mag = state.force_normal * config.friction;
let rot = Quat::from_axis_angle(normal, yaw_angle);
let forward = normal.cross(*bike_xform.right());
let thrust_mag = input.throttle * max_thrust * dt;
let (thrust_dir, thrust_force) = match wheel {
CyberWheel::Rear => (forward, thrust_mag * 0.1),
CyberWheel::Front => (rot * forward, thrust_mag),
};
let thrust = thrust_force * thrust_dir;
let friction_dir = match wheel {
CyberWheel::Front => normal.cross(thrust_dir),
CyberWheel::Rear => normal.cross(*bike_xform.forward()),
};
let vel = bike_body.velocity_at_point(contact_point - bike_xform.translation);
let friction_factor = -0.025 * vel.dot(friction_dir);
let friction = (friction_factor / dt) * friction_dir;
let diff = bike_vel - vel;
bevy::log::debug!("{wheel:?}: vel diff: {diff:?} ({})", diff.length(),);
let mut force = thrust + friction;
force *= dt * 50.0;
let force_mag = force.length();
if force_mag > max_force_mag {
state.sliding = true;
force = force.normalize_or_zero() * max_force_mag;
} else {
state.sliding = false;
}
bike_force.apply_force_at_point(force, contact_point, bike_xform.translation);
gizmos.arrow(
contact_point,
contact_point + friction,
Color::linear_rgb(1., 1., 0.2),
);
gizmos.arrow(
contact_point,
contact_point + thrust,
Color::linear_rgb(1., 0., 0.2),
);
}
}
}
pub(super) fn drag(
mut bike_query: Query<
(&Transform, &LinearVelocity, &mut ExternalForce),
With<CyberBikeBody>,
>,
mut gizmos: Gizmos,
time: Res<Time>,
) {
let (xform, vel, mut force) = bike_query.single_mut();
let dt = time.delta_secs();
let speed = vel.length();
let dspeed = speed.powf(1.2) * 0.1;
let dir = vel.normalize_or_zero();
let drag = -dspeed * dt * DRAG_COEFF * dir;
if speed > 1.0 {
force.apply_force_at_point(drag, xform.translation, xform.translation);
}
bevy::log::debug!("speed: {}, drag force: {}", speed, drag.length());
gizmos.arrow(
xform.translation,
xform.translation + drag * 10.,
Color::linear_rgb(1.0, 0.0, 0.0),
);
}
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}; use systems::{apply_lean, calculate_lean, drag, steering, suspension, tweak};
pub struct CyberPhysicsPlugin; pub struct CyberPhysicsPlugin;
@ -175,6 +408,13 @@ 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(Update, (apply_lean, calculate_lean)); .add_systems(Startup, |mut gravity: ResMut<Gravity>| {
gravity.0 *= 1.0;
})
.add_systems(
FixedUpdate,
(calculate_lean, apply_lean, suspension, steering, drag).chain(),
)
.add_systems(Update, tweak);
} }
} }