Compare commits
3 commits
Author | SHA1 | Date | |
---|---|---|---|
|
f7ddd5bc61 | ||
|
961cf08601 | ||
|
35655d3f3c |
4 changed files with 690 additions and 709 deletions
1143
Cargo.lock
generated
1143
Cargo.lock
generated
File diff suppressed because it is too large
Load diff
|
@ -10,9 +10,7 @@ bevy = { version = "0.16", features = ["bevy_dev_tools", "configurable_error_han
|
|||
[dependencies.avian3d]
|
||||
default-features = false
|
||||
version = "0.3"
|
||||
# git = "https://github.com/Jondolf/avian"
|
||||
# branch = "main"
|
||||
features = ["3d", "f32", "parry-f32", "debug-plugin", "default-collider", "collider-from-mesh"]
|
||||
features = ["3d", "parry-f32", "debug-plugin", "collider-from-mesh", "diagnostic_ui"]
|
||||
|
||||
|
||||
[features]
|
||||
|
|
12
src/bike.rs
12
src/bike.rs
|
@ -21,10 +21,10 @@ use crate::physics::CatControllerState;
|
|||
pub const SPRING_CONSTANT: Scalar = 40.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 REST_DISTANCE: Scalar = 1.0 + 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);
|
||||
pub const FRONT_ATTACH: Vec3 = Vec3::new(0.0, 0.0, -1.5);
|
||||
pub const REAR_ATTACH: Vec3 = Vec3::new(0.0, 0.0, 1.5);
|
||||
|
||||
#[derive(Component)]
|
||||
pub struct CyberBikeBody;
|
||||
|
@ -141,9 +141,9 @@ fn spawn_children(
|
|||
let mesh = meshes.add(mesh);
|
||||
let material = materials.add(wheel_material);
|
||||
|
||||
let front_rake = Vec3::new(0.0, -1.0, -0.9).normalize(); // about 30 degrees
|
||||
let front_rake = Vec3::new(0.0, -1.0, 0.0); // about 30 degrees
|
||||
let front_wheel_pos = FRONT_ATTACH + (front_rake * REST_DISTANCE);
|
||||
let rear_rake = Vec3::new(0.0, -1.0, 0.9).normalize();
|
||||
let rear_rake = Vec3::new(0.0, -1.0, 0.0);
|
||||
let rear_wheel_pos = REAR_ATTACH + (rear_rake * REST_DISTANCE);
|
||||
|
||||
children![
|
||||
|
@ -161,7 +161,7 @@ fn spawn_children(
|
|||
WheelConfig::new(
|
||||
FRONT_ATTACH,
|
||||
REST_DISTANCE,
|
||||
SPRING_CONSTANT - 5.0,
|
||||
SPRING_CONSTANT - 25.0,
|
||||
DAMPING_CONSTANT,
|
||||
FRICTION_COEFF,
|
||||
WHEEL_RADIUS,
|
||||
|
|
240
src/physics.rs
240
src/physics.rs
|
@ -4,25 +4,24 @@ use bevy::prelude::{
|
|||
};
|
||||
|
||||
const DRAG_COEFF: Scalar = 0.00001;
|
||||
const MAX_THRUST: Scalar = 900.0;
|
||||
|
||||
#[derive(Resource, Default, Debug)]
|
||||
struct CyberLean {
|
||||
pub lean: Scalar,
|
||||
pub lean: f32,
|
||||
}
|
||||
|
||||
#[derive(Debug, Resource)]
|
||||
pub struct CatControllerSettings {
|
||||
pub kp: Scalar,
|
||||
pub kd: Scalar,
|
||||
pub ki: Scalar,
|
||||
pub kp: f32,
|
||||
pub kd: f32,
|
||||
pub ki: f32,
|
||||
}
|
||||
|
||||
impl Default for CatControllerSettings {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
kp: 60.0,
|
||||
kd: 30.0,
|
||||
kp: 40.0,
|
||||
kd: 14.0,
|
||||
ki: 2.0,
|
||||
}
|
||||
}
|
||||
|
@ -30,9 +29,9 @@ impl Default for CatControllerSettings {
|
|||
|
||||
#[derive(Component, Debug, Clone, Copy)]
|
||||
pub struct CatControllerState {
|
||||
pub roll_integral: Scalar,
|
||||
pub roll_prev: Scalar,
|
||||
roll_limit: Scalar,
|
||||
pub roll_integral: f32,
|
||||
pub roll_prev: f32,
|
||||
roll_limit: f32,
|
||||
}
|
||||
|
||||
impl Default for CatControllerState {
|
||||
|
@ -46,7 +45,8 @@ impl Default for CatControllerState {
|
|||
}
|
||||
|
||||
impl CatControllerState {
|
||||
pub fn update_roll(&mut self, error: Scalar, dt: Scalar) -> (Scalar, Scalar) {
|
||||
pub fn update_roll(&mut self, error: f32, dt: f32) -> (f32, f32) {
|
||||
dbg!(error);
|
||||
let lim = self.roll_limit;
|
||||
self.roll_integral = (self.roll_integral + (error * dt)).min(lim).max(-lim);
|
||||
let derivative = (error - self.roll_prev) / dt;
|
||||
|
@ -56,30 +56,26 @@ impl CatControllerState {
|
|||
}
|
||||
|
||||
mod systems {
|
||||
use avian3d::{
|
||||
math::{Scalar, FRAC_PI_2, PI},
|
||||
prelude::{
|
||||
ComputedCenterOfMass, ExternalForce, ExternalTorque, Gravity, LinearVelocity,
|
||||
RayCaster, RayHits, RigidBodyQueryReadOnly,
|
||||
},
|
||||
use std::f32::consts::FRAC_PI_4;
|
||||
|
||||
use avian3d::prelude::{
|
||||
AngularVelocity, Collider, ComputedCenterOfMass, ExternalForce, ExternalTorque, Gravity,
|
||||
LinearVelocity, PhysicsDiagnosticsUiSettings, RayCaster, RayHits, RigidBodyQueryReadOnly,
|
||||
};
|
||||
use bevy::{
|
||||
ecs::system::{Populated, Single},
|
||||
ecs::system::{Populated, Query, Single},
|
||||
prelude::{
|
||||
ButtonInput, Color, Gizmos, GlobalTransform, KeyCode, Quat, Res, ResMut, Time,
|
||||
Transform, Vec, Vec3, With, Without,
|
||||
},
|
||||
};
|
||||
|
||||
use super::{CatControllerSettings, CatControllerState, CyberLean, DRAG_COEFF, MAX_THRUST};
|
||||
use super::{CatControllerSettings, CatControllerState, CyberLean, DRAG_COEFF};
|
||||
use crate::{
|
||||
bike::{CyberBikeBody, CyberWheel, WheelConfig, WheelState, WHEEL_RADIUS},
|
||||
input::InputState,
|
||||
};
|
||||
|
||||
const FRAC_PI_3: Scalar = PI / 3.0;
|
||||
const FRAC_PI_4: Scalar = FRAC_PI_2 / 2.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();
|
||||
|
@ -92,60 +88,80 @@ mod systems {
|
|||
}
|
||||
|
||||
pub(super) fn calculate_lean(
|
||||
bike_state: Single<(&LinearVelocity, &Transform), With<CyberBikeBody>>,
|
||||
wheels: Populated<&GlobalTransform, With<CyberWheel>>,
|
||||
input: Res<InputState>,
|
||||
velocity: Single<&LinearVelocity, With<CyberBikeBody>>,
|
||||
wheels: Populated<&GlobalTransform, (With<Collider>, With<CyberWheel>)>,
|
||||
gravity: Res<Gravity>,
|
||||
input: Res<InputState>,
|
||||
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.into_inner();
|
||||
let vel = velocity.dot(*xform.forward());
|
||||
let v_squared = vel.powi(2);
|
||||
let d: GlobalTransform = Default::default();
|
||||
let mut w = [&d; 2];
|
||||
for (i, xform) in wheels.iter().enumerate() {
|
||||
w[i] = xform;
|
||||
}
|
||||
let wheelbase = w[0].translation() - w[1].translation();
|
||||
let base = wheelbase.length();
|
||||
|
||||
let steering_angle = yaw_to_angle(input.yaw);
|
||||
let radius = base / steering_angle.tan();
|
||||
let gravity = gravity.0.length();
|
||||
let v2_r = v_squared / radius;
|
||||
let tan_theta = (v2_r / gravity).clamp(-FRAC_PI_3, FRAC_PI_3);
|
||||
|
||||
if tan_theta.is_normal() || tan_theta == 0.0 {
|
||||
lean.lean = tan_theta.atan().clamp(-FRAC_PI_3, FRAC_PI_3);
|
||||
} else if lean.lean.abs() > 0.01 {
|
||||
lean.lean *= 0.5;
|
||||
let lin_vel = velocity.into_inner();
|
||||
|
||||
let right = wheelbase.cross(gravity.0).normalize();
|
||||
let up = right.cross(wheelbase).normalize();
|
||||
|
||||
let gdir = gravity.0.normalize();
|
||||
let up = up * -gdir.dot(up).signum();
|
||||
|
||||
let gravity = gravity.0.dot(up);
|
||||
let tan_theta = lin_vel.length_squared() / (radius * gravity);
|
||||
|
||||
if tan_theta.is_normal() {
|
||||
let atan = tan_theta.atan();
|
||||
if atan.is_normal() {
|
||||
lean.lean = atan;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub(super) fn apply_lean(
|
||||
bike_query: Single<(&Transform, &mut ExternalTorque, &mut CatControllerState)>,
|
||||
wheels: Populated<(&WheelState, &GlobalTransform, &CyberWheel)>,
|
||||
wheels: Populated<(&WheelState, &CyberWheel)>,
|
||||
time: Res<Time>,
|
||||
settings: Res<CatControllerSettings>,
|
||||
lean: Res<CyberLean>,
|
||||
mut gizmos: Gizmos,
|
||||
) {
|
||||
let (xform, mut torque, mut control_vars) = bike_query.into_inner();
|
||||
let (xform, mut force, mut control_vars) = bike_query.into_inner();
|
||||
|
||||
let mut factor = 1.0;
|
||||
let mut rxform = GlobalTransform::default();
|
||||
let mut fxform = GlobalTransform::default();
|
||||
for (wheel_state, xform, cwheel) in wheels.iter() {
|
||||
if wheel_state.contact_point.is_none() {
|
||||
factor -= 0.25;
|
||||
}
|
||||
match cwheel {
|
||||
CyberWheel::Front => {
|
||||
fxform = *xform;
|
||||
}
|
||||
CyberWheel::Rear => {
|
||||
rxform = *xform;
|
||||
let mut front = None;
|
||||
let mut rear = None;
|
||||
let mut normal = None;
|
||||
for (state, wheel) in wheels.iter() {
|
||||
if state.contact_point.is_none() {
|
||||
factor -= 0.5;
|
||||
} else {
|
||||
normal = Some(state.contact_normal.unwrap());
|
||||
match wheel {
|
||||
CyberWheel::Front => {
|
||||
front = Some(state.contact_point.unwrap());
|
||||
}
|
||||
CyberWheel::Rear => {
|
||||
rear = Some(state.contact_point.unwrap());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
let tork_axis = (rxform.translation() - fxform.translation()).normalize();
|
||||
let axis = match (front, rear) {
|
||||
(Some(f), Some(r)) => (r - f).normalize(),
|
||||
(None, Some(_)) | (Some(_), None) => {
|
||||
let normal = normal.unwrap();
|
||||
normal.cross(*xform.left())
|
||||
}
|
||||
_ => return,
|
||||
};
|
||||
|
||||
let world_up = Vec3::Y; //xform.translation.normalize();
|
||||
let rot = Quat::from_axis_angle(*xform.back(), lean.lean);
|
||||
|
@ -176,10 +192,9 @@ mod systems {
|
|||
let mag =
|
||||
(settings.kp * roll_error) + (settings.ki * integral) + (settings.kd * derivative);
|
||||
if mag.is_finite() {
|
||||
//let lean_force = factor * mag * *xform.left();
|
||||
let tork = factor * mag * tork_axis;
|
||||
let lean_torq = factor * mag * axis;
|
||||
|
||||
torque.apply_torque(tork);
|
||||
force.apply_torque(lean_torq);
|
||||
|
||||
gizmos.arrow(
|
||||
xform.translation + *xform.up(),
|
||||
|
@ -190,7 +205,7 @@ mod systems {
|
|||
}
|
||||
}
|
||||
|
||||
fn yaw_to_angle(yaw: Scalar) -> Scalar {
|
||||
fn yaw_to_angle(yaw: f32) -> f32 {
|
||||
let v = yaw.powi(5) * FRAC_PI_4;
|
||||
if v.is_normal() {
|
||||
v
|
||||
|
@ -212,7 +227,7 @@ mod systems {
|
|||
time: Res<Time>,
|
||||
mut gizmos: Gizmos,
|
||||
) {
|
||||
let dt = time.delta().as_secs_f64() as Scalar;
|
||||
let dt = time.delta().as_secs_f32();
|
||||
|
||||
let mut front_caster = &RayCaster::default();
|
||||
let mut rear_caster = &RayCaster::default();
|
||||
|
@ -291,7 +306,8 @@ mod systems {
|
|||
let (bike_xform, bike_vel, mut bike_force, bike_body) = bike_query.into_inner();
|
||||
let bike_vel = bike_vel.0;
|
||||
|
||||
let dt = time.delta().as_secs_f64() as Scalar;
|
||||
let dt = time.delta().as_secs_f32();
|
||||
let max_thrust = 8000.0;
|
||||
let yaw_angle = -yaw_to_angle(input.yaw);
|
||||
|
||||
for (mut state, config, wheel) in wheels.iter_mut() {
|
||||
|
@ -302,10 +318,10 @@ mod systems {
|
|||
|
||||
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_mag = input.throttle * max_thrust * dt;
|
||||
let (thrust_dir, thrust_force) = match wheel {
|
||||
CyberWheel::Rear => (forward, thrust_mag * 0.5),
|
||||
CyberWheel::Front => (rot * forward, thrust_mag * 0.5),
|
||||
CyberWheel::Rear => (forward, thrust_mag),
|
||||
CyberWheel::Front => (rot * forward, 0.0),
|
||||
};
|
||||
|
||||
let thrust = thrust_force * thrust_dir;
|
||||
|
@ -322,15 +338,14 @@ mod systems {
|
|||
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;
|
||||
}
|
||||
let force = thrust + friction * dt;
|
||||
// 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,
|
||||
|
@ -380,8 +395,6 @@ mod systems {
|
|||
);
|
||||
}
|
||||
|
||||
bevy::log::debug!("speed: {}, drag force: {}", vel.length(), drag.length());
|
||||
|
||||
gizmos.arrow(
|
||||
xform.translation,
|
||||
xform.translation + drag * 10.,
|
||||
|
@ -389,37 +402,58 @@ mod systems {
|
|||
);
|
||||
}
|
||||
|
||||
pub(super) fn ef(
|
||||
bike: Single<(&ExternalForce, &GlobalTransform), With<CyberBikeBody>>,
|
||||
mut gizmos: Gizmos,
|
||||
) {
|
||||
let (force, xform) = bike.into_inner();
|
||||
|
||||
let start = xform.translation() + (2.0 * Vec3::Y);
|
||||
let force = force.length() * force.normalize_or_zero();
|
||||
gizmos.arrow(start, start + force, Color::linear_rgb(1.0, 0.0, 0.0));
|
||||
}
|
||||
|
||||
pub(super) fn tweak(
|
||||
mut config: Populated<&mut WheelConfig>,
|
||||
mut config: ResMut<CatControllerSettings>,
|
||||
mut keys: ResMut<ButtonInput<KeyCode>>,
|
||||
mut ui: ResMut<PhysicsDiagnosticsUiSettings>,
|
||||
) {
|
||||
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,
|
||||
for key in &keyset {
|
||||
match key {
|
||||
KeyCode::KeyP => {
|
||||
if shifted {
|
||||
config.kp += 1.0;
|
||||
} else {
|
||||
config.kp -= 1.0;
|
||||
}
|
||||
}
|
||||
KeyCode::KeyD => {
|
||||
if shifted {
|
||||
config.kd += 1.0;
|
||||
} else {
|
||||
config.kd -= 1.0;
|
||||
}
|
||||
}
|
||||
KeyCode::KeyI => {
|
||||
if shifted {
|
||||
config.ki += 1.0;
|
||||
} else {
|
||||
config.ki -= 1.0;
|
||||
}
|
||||
}
|
||||
KeyCode::KeyU => {
|
||||
let enabled = ui.enabled;
|
||||
ui.enabled = !enabled;
|
||||
}
|
||||
|
||||
_ => continue,
|
||||
}
|
||||
}
|
||||
bevy::log::info!(?config);
|
||||
|
||||
let released: Vec<_> = keys.get_just_released().copied().collect();
|
||||
for key in released {
|
||||
keys.clear_just_released(key);
|
||||
|
@ -427,7 +461,7 @@ mod systems {
|
|||
}
|
||||
}
|
||||
|
||||
use systems::{apply_lean, calculate_lean, drag, suspension, tweak, wheel_action};
|
||||
use systems::{apply_lean, calculate_lean, drag, ef, suspension, tweak, wheel_action};
|
||||
|
||||
pub struct CyberPhysicsPlugin;
|
||||
|
||||
|
@ -435,8 +469,12 @@ impl Plugin for CyberPhysicsPlugin {
|
|||
fn build(&self, app: &mut App) {
|
||||
app.init_resource::<CatControllerSettings>()
|
||||
.init_resource::<CyberLean>()
|
||||
.add_plugins((PhysicsPlugins::default(), PhysicsDebugPlugin::default()))
|
||||
.insert_resource(SubstepCount(12))
|
||||
.add_plugins((
|
||||
PhysicsPlugins::default(),
|
||||
PhysicsDebugPlugin::default(),
|
||||
PhysicsDiagnosticsUiPlugin,
|
||||
))
|
||||
.insert_resource(SubstepCount(24))
|
||||
.add_systems(Startup, |mut gravity: ResMut<Gravity>| {
|
||||
gravity.0 *= 1.0;
|
||||
})
|
||||
|
@ -446,7 +484,9 @@ impl Plugin for CyberPhysicsPlugin {
|
|||
calculate_lean,
|
||||
apply_lean,
|
||||
suspension,
|
||||
wheel_action, /* drag */
|
||||
wheel_action,
|
||||
drag,
|
||||
ef,
|
||||
)
|
||||
.chain(),
|
||||
)
|
||||
|
|
Loading…
Reference in a new issue