nebkor
/
cyber_rider
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

working lean code

This commit is contained in:
Joe Ardent 2023-02-21 12:47:24 -08:00
parent 5aa7cde9aa
commit 32db4fb383
1 changed files with 15 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
src/action/systems.rs
View File

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