Compare commits

..

No commits in common. "eaba0edcd2aa1533b18ce0caaa529f1a6f21c0db" and "008ef5a3f8f11807932f3d9e1f052f9df4d017ea" have entirely different histories.

7 changed files with 928 additions and 1061 deletions

1308
Cargo.lock generated

File diff suppressed because it is too large Load diff

View file

@ -1,17 +1,12 @@
[package] [package]
name = "physics-test" name = "physics-test"
version = "0.1.0" version = "0.1.0"
edition = "2024" edition = "2021"
[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.29" bevy-inspector-egui = "0.28"
[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,88 +1,40 @@
use avian3d::{ use std::f32::consts::FRAC_PI_2;
math::{Scalar, FRAC_PI_2},
prelude::*, use avian3d::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, Clone, Copy, Debug)] #[derive(Component)]
pub enum CyberWheel { pub struct CyberWheel;
Front,
Rear,
}
#[derive(Component, Clone, Copy, Debug, Reflect, Default)] // marker for front suspension joint
#[reflect(Component)] #[derive(Component)]
pub struct WheelState { pub struct Steering;
pub displacement: Scalar, #[derive(Component)]
pub force_normal: Scalar, pub struct FrontHub;
pub sliding: bool,
pub contact_point: Option<Vec3>,
pub contact_normal: Option<Vec3>,
}
impl WheelState { // marker for rear suspension joint
pub fn reset(&mut self) { #[derive(Component)]
*self = Self::default(); pub struct Rearing;
} #[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, 5.0, 0.0); let pos = Vec3::new(0.0, 4.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));
commands let bike = commands
.spawn((xform, Visibility::default())) .spawn((xform, Visibility::default()))
.insert(( .insert((
Name::new("body"), Name::new("body"),
@ -92,120 +44,110 @@ fn spawn_bike(
SleepingDisabled, SleepingDisabled,
CyberBikeBody, CyberBikeBody,
CatControllerState::default(), CatControllerState::default(),
ColliderDensity(1.2), ColliderDensity(20.0),
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 xform = Transform::default(); let mut rotation = Transform::default(); // Transform::from_rotation(Quat::from_rotation_y(FRAC_PI_2));
xform.rotate_x(FRAC_PI_2); rotation.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))),
xform, rotation,
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(
commands: &mut ChildBuilder, mut commands: Commands,
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: Mesh = Sphere::new(WHEEL_RADIUS).into(); let front_rake = Vec3::new(0.0, -1.0, -0.57).normalize(); // about 30 degrees
let front_pos = xform.translation + front_rake;
let front_rake = Vec3::new(0.0, -1.0, -0.9).normalize(); // about 30 degrees let (mesh, collider) = gen_tire();
let front_wheel_pos = FRONT_ATTACH + (front_rake * REST_DISTANCE);
wheel_caster( let front_hub = wheels_helper(
commands, &mut 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_wheel_pos, front_pos,
mesh.clone(), mesh.clone(),
WheelConfig::new( collider.clone(),
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.9).normalize(); let rear_rake = Vec3::new(0.0, -1.0, 0.57).normalize();
let rear_wheel_pos = REAR_ATTACH + (rear_rake * REST_DISTANCE); let rear_pos = xform.translation + rear_rake;
wheel_caster( let rear_hub = wheels_helper(
commands, &mut 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_wheel_pos, rear_pos,
mesh, mesh,
WheelConfig::new( collider,
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 wheel_caster( fn gen_tire() -> (Mesh, Collider) {
commands: &mut ChildBuilder, let mut tire_mesh: Mesh = Torus::new(0.25, 0.40).into();
origin: Vec3, let tire_verts = tire_mesh
direction: Dir3, .attribute(Mesh::ATTRIBUTE_POSITION)
parent: Entity, .unwrap()
rest_dist: Scalar, .as_float3()
wheel: CyberWheel, .unwrap()
) { .iter()
let caster = RayCaster::new(origin, direction) .map(|v| {
.with_max_distance(rest_dist) //
.with_max_hits(1) let v = Vec3::from_array(*v);
.with_query_filter(SpatialQueryFilter::from_excluded_entities([parent])); let m = Mat3::from_rotation_z(90.0f32.to_radians());
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);
commands.spawn((caster, wheel)); let collider = Collider::convex_hull_from_mesh(&tire_mesh).unwrap();
(tire_mesh, collider)
} }
fn wheel_mesh( fn wheels_helper(
commands: &mut ChildBuilder, commands: &mut Commands,
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,
config: WheelConfig, collider: Collider,
wheel: CyberWheel, ) -> Entity {
) {
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,
@ -214,29 +156,47 @@ fn wheel_mesh(
}; };
let xform = Transform::from_translation(position); let xform = Transform::from_translation(position);
let hub_mesh: Mesh = Sphere::new(0.1).into();
let name = match wheel { let hub = commands
CyberWheel::Front => "front tire", .spawn((
CyberWheel::Rear => "rear tire", Name::new("hub"),
}; RigidBody::Dynamic,
MassPropertiesBundle::from_shape(&Collider::sphere(0.1), 200.0),
Mesh3d(meshes.add(hub_mesh)),
MeshMaterial3d(materials.add(wheel_material.clone())),
xform,
))
.id();
commands.spawn(( let tire = commands
Name::new(name), .spawn((
config, Name::new("tire"),
Mesh3d(meshes.add(tire_mesh)), Mesh3d(meshes.add(tire_mesh)),
MeshMaterial3d(materials.add(wheel_material.clone())), MeshMaterial3d(materials.add(wheel_material.clone())),
xform, xform,
TransformInterpolation, CyberWheel,
wheel, RigidBody::Dynamic,
)); 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: 30.0, dist: 10.0,
alt: 4.0, alt: 4.0,
} }
} }
@ -36,28 +36,34 @@ 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 released: Vec<_> = keys.get_just_released().copied().collect(); let just_released: HashSet<_> = keys.get_just_released().cloned().collect();
for key in released { if !just_released.is_empty() {
keys.clear_just_released(key); let released_shift = just_released.contains(&KeyCode::ShiftLeft)
|| just_released.contains(&KeyCode::ShiftRight);
keys.reset_all();
if !released_shift && shifted {
keys.press(KeyCode::ShiftLeft);
}
} }
} }

View file

@ -1,51 +0,0 @@
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, Alpha}, color::palettes::css::SILVER,
pbr::MeshMaterial3d, pbr::MeshMaterial3d,
prelude::{ prelude::{
default, App, AppGizmoBuilder, Assets, BuildChildren, ButtonInput, ChildBuild, Color, default, App, AppGizmoBuilder, Assets, BuildChildren, ButtonInput, ChildBuild, Color,
@ -15,12 +15,10 @@ 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() {
@ -29,7 +27,6 @@ fn main() {
DefaultPlugins, DefaultPlugins,
CamPlug, CamPlug,
CyberBikePlugin, CyberBikePlugin,
CyberInputPlugin,
CyberPhysicsPlugin, CyberPhysicsPlugin,
WorldInspectorPlugin::new(), WorldInspectorPlugin::new(),
)) ))
@ -60,7 +57,7 @@ fn ground_and_light(
.spawn(( .spawn((
spatial_bundle, spatial_bundle,
RigidBody::Static, RigidBody::Static,
Collider::cuboid(500.0, 5., 500.0), Collider::cuboid(50.0, 5., 50.0),
CollisionMargin(0.1), CollisionMargin(0.1),
ColliderDensity(1000.0), ColliderDensity(1000.0),
CollisionLayers::ALL, CollisionLayers::ALL,
@ -68,8 +65,8 @@ fn ground_and_light(
)) ))
.with_children(|p| { .with_children(|p| {
let bundle = ( let bundle = (
Mesh3d(meshes.add(Plane3d::default().mesh().size(500.0, 500.0))), Mesh3d(meshes.add(Plane3d::default().mesh().size(50.0, 50.0))),
MeshMaterial3d(materials.add(Color::from(SILVER).with_alpha(0.7))), MeshMaterial3d(materials.add(Color::from(SILVER))),
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,8 +1,6 @@
use avian3d::{math::Scalar, prelude::*}; use avian3d::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 {
@ -20,9 +18,9 @@ pub struct CatControllerSettings {
impl Default for CatControllerSettings { impl Default for CatControllerSettings {
fn default() -> Self { fn default() -> Self {
Self { Self {
kp: 16.0, kp: 1200.0,
kd: 2.0, kd: 10.0,
ki: 0.9, ki: 50.0,
} }
} }
} }
@ -31,6 +29,7 @@ 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,
} }
@ -39,12 +38,19 @@ 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);
@ -52,19 +58,29 @@ 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;
use avian3d::prelude::*; use avian3d::prelude::*;
use bevy::prelude::*; use bevy::prelude::*;
use super::{CatControllerSettings, CatControllerState, CyberLean, DRAG_COEFF}; use super::{CatControllerSettings, CatControllerState, CyberLean};
use crate::{ use crate::bike::CyberBikeBody;
bike::{CyberBikeBody, CyberWheel, WheelConfig, WheelState, WHEEL_RADIUS},
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
@ -79,48 +95,32 @@ 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 steering_angle = yaw_to_angle(input.yaw); let wheel_base = 1.145f32;
let radius = base / steering_angle.tan(); let radius = wheel_base;
let gravity = gravity.0.length(); let gravity = -9.8f32;
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 ExternalForce, &mut CatControllerState)>, mut bike_query: Query<(&Transform, &mut ExternalTorque, &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 force, mut control_vars) = bike_query.single_mut(); let (xform, mut torque, 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,
2.5 * *xform.forward() + xform.translation, 1.5 * *xform.forward() + xform.translation,
bevy::color::palettes::basic::PURPLE, bevy::color::palettes::basic::PURPLE,
); );
@ -150,252 +150,19 @@ 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 lean_force = factor * mag * *xform.left(); let tork = mag * *xform.back();
force.apply_force_at_point( torque.apply_torque(tork);
lean_force,
xform.translation + *xform.up(),
xform.translation,
);
gizmos.arrow( gizmos.arrow(
xform.translation + *xform.up(), xform.translation + Vec3::Y,
xform.translation + *xform.up() + lean_force, xform.translation + tork + Vec3::Y,
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( use systems::{apply_lean, calculate_lean};
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, drag, steering, suspension, tweak};
pub struct CyberPhysicsPlugin; pub struct CyberPhysicsPlugin;
@ -408,13 +175,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>| { .add_systems(Update, (apply_lean, calculate_lean));
gravity.0 *= 1.0;
})
.add_systems(
FixedUpdate,
(calculate_lean, apply_lean, suspension, steering, drag).chain(),
)
.add_systems(Update, tweak);
} }
} }