From c69ab193bed30789cc3cb591595bffdbc3defb92 Mon Sep 17 00:00:00 2001
From: sebcrozet <developer@crozet.re>
Date: Tue, 4 Dec 2018 21:23:21 +0100
Subject: [PATCH] Fix cornercase for Rotation3 conversion to euler angles.

Fix #494
---
 src/geometry/rotation_specialization.rs |  4 ++--
 tests/geometry/rotation.rs              | 16 +++++++++++++++-
 2 files changed, 17 insertions(+), 3 deletions(-)

diff --git a/src/geometry/rotation_specialization.rs b/src/geometry/rotation_specialization.rs
index cdb7aed7..a8947b96 100644
--- a/src/geometry/rotation_specialization.rs
+++ b/src/geometry/rotation_specialization.rs
@@ -370,12 +370,12 @@ impl<N: Real> Rotation3<N> {
     pub fn 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() {
+        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() {
+        } else if self[(2, 0)] <= -N::one() {
             (self[(0, 1)].atan2(self[(0, 2)]), N::frac_pi_2(), N::zero())
         } else {
             (
diff --git a/tests/geometry/rotation.rs b/tests/geometry/rotation.rs
index 9bd3e590..57aac964 100644
--- a/tests/geometry/rotation.rs
+++ b/tests/geometry/rotation.rs
@@ -1,4 +1,4 @@
-use na::{Vector2, Vector3};
+use na::{Quaternion, Real, UnitQuaternion, Vector2, Vector3};
 
 #[test]
 fn angle_2() {
@@ -16,6 +16,20 @@ fn angle_3() {
     assert_eq!(a.angle(&b), 0.0);
 }
 
+#[test]
+fn quaternion_to_euler_angles_issue_494() {
+    let quat = UnitQuaternion::from_quaternion(Quaternion::new(
+        -0.10405792,
+        -0.6993922f32,
+        -0.10406871,
+        0.69942284,
+    ));
+    let angs = quat.euler_angles();
+    assert_eq!(angs.0, 2.8461843);
+    assert_eq!(angs.1, f32::frac_pi_2());
+    assert_eq!(angs.2, 0.0);
+}
+
 #[cfg(feature = "arbitrary")]
 mod quickcheck_tests {
     use alga::general::Real;