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

working lean code

Browse source
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);
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<ActionDebugInstant>,
) {
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();
}
}