Merge pull request #305 from jswrenn/to_euler_angles

Implement to_euler_angles for Rotation3 and UnitQuaternion

src/geometry/quaternion.rs

@@ -541,6 +541,14 @@ impl<N: Real> UnitQuaternion<N> {
         )
     }
 
+    /// 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<N, U4> {

src/geometry/rotation_specialization.rs

@@ -177,6 +177,24 @@ impl<N: Real> Rotation3<N> {
             )
     }
 
+    /// 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`.
     ///

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)
+    }
+
 
     /*
      *

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.