diff --git a/src/physics.rs b/src/physics.rs index 323ca96..3fe40bc 100644 --- a/src/physics.rs +++ b/src/physics.rs @@ -64,7 +64,6 @@ mod systems { }; use bevy::{ ecs::system::{Populated, Query, Single}, - gizmos::retained::Gizmo, prelude::{ ButtonInput, Color, Gizmos, GlobalTransform, KeyCode, Quat, Res, ResMut, Time, Transform, Vec, Vec3, With, Without, @@ -89,76 +88,51 @@ mod systems { } pub(super) fn calculate_lean( - velocities: Single<(&LinearVelocity, &AngularVelocity), With>, - wheels: Populated<(&GlobalTransform, &CyberWheel), With>, + velocity: Single<&LinearVelocity, With>, + wheels: Populated<&GlobalTransform, (With, With)>, gravity: Res, + input: Res, mut lean: ResMut, - mut gizmos: Gizmos, ) { - let mut w1 = GlobalTransform::default(); - let mut w2 = GlobalTransform::default(); - for (xform, wheel) in wheels.iter() { - match wheel { - CyberWheel::Front => w1 = *xform, - CyberWheel::Rear => w2 = *xform, - } + let d: GlobalTransform = Default::default(); + let mut w = [&d; 2]; + for (i, xform) in wheels.iter().enumerate() { + w[i] = xform; } - let wheelbase = (w2.translation() - w1.translation()).normalize(); + let wheelbase = w[0].translation() - w[1].translation(); + let base = wheelbase.length(); - let (lin_vel, ang_vel) = velocities.into_inner(); + let steering_angle = yaw_to_angle(input.yaw); + let radius = base / steering_angle.tan(); + + let lin_vel = velocity.into_inner(); let right = wheelbase.cross(gravity.0).normalize(); - let up = -right.cross(wheelbase).normalize(); - let spin = ang_vel.dot(up); + let up = right.cross(wheelbase).normalize(); - let mid = (w2.translation() + w1.translation()) / 2.0; + let gdir = gravity.0.normalize(); + let up = up * -gdir.dot(up).signum(); - gizmos.arrow(mid, mid + (up * 5.0), Color::linear_rgb(0.0, 1.0, 1.0)); - - let period = if spin.is_normal() { - 1.0 / spin - } else { - f32::MAX - }; - - let radius = lin_vel.length() * period; let gravity = gravity.0.dot(up); let tan_theta = lin_vel.length_squared() / (radius * gravity); if tan_theta.is_normal() { - lean.lean = tan_theta.atan(); - } else { - lean.lean = 0.0; - } - } - - pub(super) fn clear( - mut forces: Query<&mut ExternalForce>, - mut torques: Query<&mut ExternalTorque>, - ) { - for mut force in forces.iter_mut() { - force.clear(); - } - for mut torq in torques.iter_mut() { - torq.clear(); + 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, - &ComputedCenterOfMass, - )>, + bike_query: Single<(&Transform, &mut ExternalTorque, &mut CatControllerState)>, wheels: Populated<(&WheelState, &CyberWheel)>, time: Res