diff --git a/src/geometry/quaternion.rs b/src/geometry/quaternion.rs index 5519801c..b6a02fa9 100644 --- a/src/geometry/quaternion.rs +++ b/src/geometry/quaternion.rs @@ -541,6 +541,14 @@ impl UnitQuaternion { ) } + /// Converts this unit quaternion into its equivalent Euler angles. + /// + /// The angles are produced in the form (roll, yaw, pitch). + #[inline] + pub fn to_euler_angles(&self) -> (N, N, N) { + self.to_rotation_matrix().to_euler_angles() + } + /// Converts this unit quaternion into its equivalent homogeneous transformation matrix. #[inline] pub fn to_homogeneous(&self) -> MatrixN { diff --git a/src/geometry/rotation_specialization.rs b/src/geometry/rotation_specialization.rs index abe3066f..a5e988d2 100644 --- a/src/geometry/rotation_specialization.rs +++ b/src/geometry/rotation_specialization.rs @@ -177,6 +177,24 @@ impl Rotation3 { ) } + /// Creates Euler angles from a rotation. + /// + /// The angles are produced in the form (roll, yaw, pitch). + pub fn to_euler_angles(&self) -> (N, N, N) { + // Implementation informed by "Computing Euler angles from a rotation matrix", by Gregory G. Slabaugh + // http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.371.6578 + if self[(2,0)].abs() != N::one() { + let yaw = -self[(2,0)].asin(); + let roll = (self[(2,1)] / yaw.cos()).atan2(self[(2,2)] / yaw.cos()); + let pitch = (self[(1,0)] / yaw.cos()).atan2(self[(0,0)] / yaw.cos()); + (roll, yaw, pitch) + } else if self[(2,0)] == -N::one() { + (self[(0,1)].atan2(self[(0,2)]), N::frac_pi_2(), N::zero()) + } else { + (-self[(0,1)].atan2(-self[(0,2)]), -N::frac_pi_2(), N::zero()) + } + } + /// Creates a rotation that corresponds to the local frame of an observer standing at the /// origin and looking toward `dir`. /// diff --git a/tests/geometry/quaternion.rs b/tests/geometry/quaternion.rs index 3000fa9c..c3399399 100644 --- a/tests/geometry/quaternion.rs +++ b/tests/geometry/quaternion.rs @@ -26,6 +26,12 @@ quickcheck!( relative_eq!(yaw * pitch * roll, rpy, epsilon = 1.0e-7) } + fn to_euler_angles(r: f64, p: f64, y: f64) -> bool { + let rpy = UnitQuaternion::from_euler_angles(r, p, y); + let (roll, pitch, yaw) = rpy.to_euler_angles(); + relative_eq!(UnitQuaternion::from_euler_angles(roll, pitch, yaw), rpy, epsilon = 1.0e-7) + } + /* * diff --git a/tests/geometry/rotation.rs b/tests/geometry/rotation.rs index 263577ce..b90ab259 100644 --- a/tests/geometry/rotation.rs +++ b/tests/geometry/rotation.rs @@ -37,6 +37,21 @@ quickcheck!( yaw * pitch * roll == rpy } + fn to_euler_angles(r: f64, p: f64, y: f64) -> bool { + let rpy = Rotation3::from_euler_angles(r, p, y); + let (roll, pitch, yaw) = rpy.to_euler_angles(); + relative_eq!(Rotation3::from_euler_angles(roll, pitch, yaw), rpy, epsilon = 1.0e-7) + } + + fn to_euler_angles_gimble_lock(r: f64, y: f64) -> bool { + let pos = Rotation3::from_euler_angles(r, f64::frac_pi_2(), y); + let neg = Rotation3::from_euler_angles(r, -f64::frac_pi_2(), y); + let (pos_r, pos_p, pos_y) = pos.to_euler_angles(); + let (neg_r, neg_p, neg_y) = neg.to_euler_angles(); + relative_eq!(Rotation3::from_euler_angles(pos_r, pos_p, pos_y), pos, epsilon = 1.0e-7) && + relative_eq!(Rotation3::from_euler_angles(neg_r, neg_p, neg_y), neg, epsilon = 1.0e-7) + } + /* * * Inversion is transposition.