From 922b339fb081ad17a381ef000114a66daec58a63 Mon Sep 17 00:00:00 2001
From: Jack Wrenn <me@jswrenn.com>
Date: Tue, 9 Jan 2018 15:15:57 -0500
Subject: [PATCH] Implement to_euler_angles for Rotation3 and UnitQuaternion

Resolves sebcrozet/nalgebra#243.
---
 src/geometry/quaternion.rs              |  8 ++++++++
 src/geometry/rotation_specialization.rs | 18 ++++++++++++++++++
 tests/geometry/quaternion.rs            |  6 ++++++
 tests/geometry/rotation.rs              | 15 +++++++++++++++
 4 files changed, 47 insertions(+)

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<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> {
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<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`.
     ///
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.