diff --git a/common/src/comp/ori.rs b/common/src/comp/ori.rs index 8ac46732d6..ab9ba00fba 100644 --- a/common/src/comp/ori.rs +++ b/common/src/comp/ori.rs @@ -149,25 +149,25 @@ impl Ori { } pub fn to_horizontal(self) -> Self { - let fw = self.look_dir(); - Dir::from_unnormalized(fw.xy().into()) - .or_else(|| { - // if look_dir is straight down, pitch up, or if straight up, pitch down - Dir::from_unnormalized( - if fw.dot(Vec3::unit_z()) < 0.0 { - self.up() - } else { - self.down() - } - .xy() - .into(), - ) - }) - .map(|dir| dir.into()) - .expect( - "If the horizontal component of a Dir can not be normalized, the horizontal \ - component of a Dir perpendicular to it must be", - ) + // We don't use Self::look_dir to avoid the extra normalization step within + // Dir's Quaternion Mul impl (since we will normalize later below) + let fw = self.to_quat() * Dir::default().to_vec(); + // Check that dir is not straight up/down + // Uses a multiple of EPSILON to be safe + // We can just check z since beyond floating point errors `fw` should be + // normalized + let xy = if 1.0 - fw.z.abs() > f32::EPSILON * 4.0 { + fw.xy().normalized() + } else { + // if look_dir is straight down, pitch up, or if straight up, pitch down + // xy should essentially be normalized so no need to normalize + if fw.z < 0.0 { self.up() } else { self.down() }.xy() + }; + // We know direction lies in the xy plane so we only need to compute a rotation + // about the z-axis + let yaw = xy.y.acos() * fw.x.signum() * -1.0; + + Self(Quaternion::rotation_z(yaw)) } /// Find the angle between two `Ori`s @@ -255,15 +255,16 @@ impl Ori { fn is_normalized(&self) -> bool { self.0.into_vec4().is_normalized() } } + impl From for Ori { fn from(dir: Dir) -> Self { // Check that dir is not straight up/down // Uses a multiple of EPSILON to be safe - let quat = if dir.z.abs() - 1.0 > f32::EPSILON * 4.0 { + let quat = if 1.0 - dir.z.abs() > f32::EPSILON * 4.0 { // Compute rotation that will give an "upright" orientation (no rolling): // Rotation to get to this projected point from the default direction of y+ - let yaw = dir.xy().normalized().y.acos() * dir.x.signum(); + let yaw = dir.xy().normalized().y.acos() * dir.x.signum() * -1.0; // Rotation to then rotate up/down to the match the input direction let pitch = dir.z.asin(); @@ -271,9 +272,9 @@ impl From for Ori { } else { // Nothing in particular can be considered upright if facing up or down // so we just produce a quaternion that will rotate to that direction - let from = Dir::default(); - // This calls normalized() internally - Quaternion::::rotation_from_to_3d(*from, *dir) + // (once again rotating from y+) + let pitch = PI / 2.0 * dir.z.signum(); + Quaternion::rotation_x(pitch) }; Self(quat) @@ -365,33 +366,76 @@ impl Component for Ori { mod tests { use super::*; + // Helper method to produce Dirs at different angles to test + fn dirs() -> impl Iterator { + let angles = 32; + (0..angles).flat_map(move |i| { + let theta = PI * 2.0 * (i as f32) / (angles as f32); + + let v = Vec3::unit_y(); + let q = Quaternion::rotation_x(theta); + let dir_1 = Dir::new(q * v); + + let v = Vec3::unit_z(); + let q = Quaternion::rotation_y(theta); + let dir_2 = Dir::new(q * v); + + let v = Vec3::unit_x(); + let q = Quaternion::rotation_z(theta); + let dir_3 = Dir::new(q * v); + + [dir_1, dir_2, dir_3] + }) + } + + #[test] + fn to_horizontal() { + let to_horizontal = |dir: Dir| { + let ori = Ori::from(dir); + + let horizontal = ori.to_horizontal(); + + approx::assert_relative_eq!(horizontal.look_dir().xy().magnitude(), 1.0); + approx::assert_relative_eq!(horizontal.look_dir().z, 0.0); + }; + + dirs().for_each(to_horizontal); + } + + #[test] + fn angle_between() { + let angle_between = |(dir_a, dir_b): (Dir, Dir)| { + let ori_a = Ori::from(dir_a); + let ori_b = Ori::from(dir_b); + + approx::assert_relative_eq!(ori_a.angle_between(ori_b), dir_a.angle_between(*dir_b)); + }; + + dirs() + .flat_map(|dir| dirs().map(move |dir_two| (dir, dir_two))) + .for_each(angle_between) + } + #[test] fn from_to_dir() { let from_to = |dir: Dir| { let ori = Ori::from(dir); assert!(ori.is_normalized(), "ori {:?}\ndir {:?}", ori, dir); - approx::assert_relative_eq!(ori.look_dir().dot(*dir), 1.0); + assert!( + approx::relative_eq!(ori.look_dir().dot(*dir), 1.0), + "Ori::from(dir).look_dir() != dir\ndir: {:?}\nOri::from(dir).look_dir(): {:?}", + dir, + ori.look_dir(), + ); approx::assert_relative_eq!((ori.to_quat() * Dir::default()).dot(*dir), 1.0); }; - let angles = 32; - for i in 0..angles { - let theta = PI * 2. * (i as f32) / (angles as f32); - let v = Vec3::unit_y(); - let q = Quaternion::rotation_x(theta); - from_to(Dir::new(q * v)); - let v = Vec3::unit_z(); - let q = Quaternion::rotation_y(theta); - from_to(Dir::new(q * v)); - let v = Vec3::unit_x(); - let q = Quaternion::rotation_z(theta); - from_to(Dir::new(q * v)); - } + dirs().for_each(from_to); } #[test] - fn dirs() { + fn orthogonal_dirs() { let ori = Ori::default(); let def = Dir::default(); for dir in &[ori.up(), ori.down(), ori.left(), ori.right()] {