diff --git a/src/physics.rs b/src/physics.rs index a952522..2f952d4 100644 --- a/src/physics.rs +++ b/src/physics.rs @@ -18,9 +18,9 @@ pub struct CatControllerSettings { impl Default for CatControllerSettings { fn default() -> Self { Self { - kp: 30.0, - kd: 7.0, - ki: 1.0, + kp: 25.0, + kd: 3.0, + ki: 1.1, } } } @@ -127,7 +127,7 @@ mod systems { // show which is forward gizmos.arrow( *xform.forward() + xform.translation, - 1.5 * *xform.forward() + xform.translation, + 2.5 * *xform.forward() + xform.translation, bevy::color::palettes::basic::PURPLE, ); @@ -239,7 +239,12 @@ mod systems { pub(super) fn steering( mut bike_query: Query< - (&Transform, &mut ExternalForce, RigidBodyQueryReadOnly), + ( + &Transform, + &LinearVelocity, + &mut ExternalForce, + RigidBodyQueryReadOnly, + ), With, >, mut wheels: Query<(&mut WheelState, &WheelConfig, &CyberWheel)>, @@ -247,16 +252,16 @@ mod systems { input: Res, mut gizmos: Gizmos, ) { - let Ok((bike_xform, mut bike_force, bike_body)) = bike_query.get_single_mut() else { + 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 = 1000.0; - let thrust_force = input.throttle * max_thrust * dt; - let max_yaw = 500.0; - let yaw_force = input.yaw * max_yaw * dt; + 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 { @@ -264,39 +269,32 @@ mod systems { let normal = state.contact_normal.unwrap().normalize(); let max_force_mag = state.force_normal * config.friction; - let yaw_dir = match wheel { - CyberWheel::Front => normal.cross(*bike_xform.back()).normalize_or_zero(), - CyberWheel::Rear => Vec3::ZERO, + let rot = Quat::from_axis_angle(normal, yaw_angle); + let forward = normal.cross(*bike_xform.right()); + let (thrust_dir, thrust_force) = match wheel { + CyberWheel::Rear => (forward, input.throttle * max_thrust * dt), + CyberWheel::Front => (rot * forward, 0.0), }; - let thrust_dir = normal.cross(*bike_xform.right()); - - let vel = bike_body.velocity_at_point(contact_point - bike_xform.translation); - - let yaw = yaw_force * yaw_dir; let thrust = thrust_force * thrust_dir; - let yust = yaw + thrust; - - bevy::log::debug!("yust: {yust:?}"); - bevy::log::debug!("{:?}", bike_xform.left()); - - let left = bike_xform.forward().cross(normal); let friction_dir = match wheel { - CyberWheel::Front => { - if yaw_force + thrust_force > 0.1 { - normal.cross(yust.normalize()).normalize() - } else { - left - } - } - CyberWheel::Rear => left, + CyberWheel::Front => normal.cross(thrust_dir), + CyberWheel::Rear => normal.cross(*bike_xform.forward()), }; + // bevy::log::debug!( + // "{wheel:?}, thrust_dir: {thrust_dir:?}, friction_dir: {friction_dir:?}, dot: {}", + // thrust_dir.dot(friction_dir) + // ); + + 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 mut force = yust + friction; + bevy::log::debug!("{wheel:?}: vel diff: {:?}", bike_vel - vel); + + let mut force = thrust + friction; force *= dt * 50.0; let force_mag = force.length(); if force_mag > max_force_mag { @@ -313,6 +311,11 @@ mod systems { contact_point + friction, Color::linear_rgb(1., 1., 0.2), ); + gizmos.arrow( + contact_point, + contact_point + thrust, + Color::linear_rgb(1., 0., 0.2), + ); } } }