Compare commits

..

3 commits

Author SHA1 Message Date
Joe Ardent
f7ddd5bc61 abandoning failed attempt 2025-05-18 15:52:15 -07:00
Joe Ardent
961cf08601 update deps 2025-05-18 15:06:22 -07:00
Joe Ardent
35655d3f3c doesn't really work, but some good ideas 2025-05-11 15:37:43 -07:00
4 changed files with 690 additions and 709 deletions

1143
Cargo.lock generated

File diff suppressed because it is too large Load diff

View file

@ -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]

View file

@ -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,

View file

@ -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(),
)