diff --git a/src/action/systems.rs b/src/action/systems.rs index 19b9ad9..02208ce 100644 --- a/src/action/systems.rs +++ b/src/action/systems.rs @@ -67,8 +67,8 @@ pub(super) fn cyber_lean( let centripetal = -right * (mass_props.0.mass * v2_r); if v2_r.is_normal() { - forces.force += centripetal; - lean.lean = -tan_theta.atan().clamp(-FRAC_PI_4, FRAC_PI_4); + //forces.force += centripetal; + lean.lean = tan_theta.atan().clamp(-FRAC_PI_4, FRAC_PI_4); } } @@ -81,23 +81,20 @@ pub(super) fn falling_cat( #[cfg(feature = "inspector")] mut debug_instant: ResMut, ) { let (xform, mut forces, mut control_vars) = bike_query.single_mut(); - let wup = xform.translation.normalize(); //Transform::from_translation(xform.translation.normalize()); - let rot = Quat::from_axis_angle(xform.forward(), lean.lean); - let [x, y, z] = wup.to_array(); - let qup = Quat::from_xyzw(x, y, z, 0.0); // turn our "world up" point into a Quat - + let world_up = xform.translation.normalize(); + let [x, y, z] = world_up.to_array(); + // turn our "world up" point into a Quat + let qup = Quat::from_xyzw(x, y, z, 0.0); // p' = rot^-1 * qup * rot - let wup = rot.inverse() * qup * rot; - let wup = Vec3::from_array([wup.x, wup.y, wup.z]).normalize(); - let bike_up = xform.up(); + let rot = Quat::from_axis_angle(xform.back(), lean.lean); + let rot_qup = rot.inverse() * qup * rot; + let target_up = -Vec3::from_array([rot_qup.x, rot_qup.y, rot_qup.z]).normalize(); + let bike_right = xform.right(); - let wright = xform - .forward() - .cross(xform.translation.normalize()) - .normalize(); + //let world_right = xform.forward().cross(world_up).normalize(); - let roll_error = wright.dot(wup); - let pitch_error = xform.translation.normalize().dot(xform.forward()); + let roll_error = bike_right.dot(target_up); + let pitch_error = world_up.dot(xform.forward()); // only try to correct roll if we're not totally vertical if pitch_error.abs() < 0.95 { @@ -109,8 +106,8 @@ pub(super) fn falling_cat( } #[cfg(feature = "inspector")] { - if debug_instant.elapsed().as_millis() > 5000 { - dbg!(&control_vars, mag, &wup); + if debug_instant.elapsed().as_millis() > 1000 { + dbg!(&control_vars, mag, &target_up); debug_instant.reset(); } }