Repository: bitshifter/glam-rs
Pull request #427 (2023-09-02 11:58:48)

Fix singularities in `Quat::to_euler()`
`Quat::to_euler()` has issues when the second rotation is (near) `PI/2` or `-PI/2` radians, causing the first and third angles to be incorrectly calculated.

For example:
```
    let q = Quat::from_euler(EulerRot::XYZ, 1.0, PI / 2.0, 0.0);
    let e = q.to_euler(EulerRot::XYZ);
    let q2 = Quat::from_euler(EulerRot::XYZ, e.0, e.1, e.2);
    println!("{:?}", q);
    println!("{:?}", e);
    println!("{:?}", q2);
```
Outputs:
```
Quat(0.33900508, 0.6205446, 0.33900508, 0.6205446)
(0.0, 1.5707964, 0.0)
Quat(0.0, 0.7071068, 0.0, 0.7071068)
```

This PR addresses this issue for all orders of rotation. It favors angles on the first axis over the third axis (e.g., a quaternion constructed from euler angles `(a, PI/2, b)` will convert back to either `(a+b, PI/2, 0)` or `(a-b, PI/2, 0)`, depending on the requested order). In any way, converting a quaternion to euler angles and back to a quaternion will always yield the original quaternion, barring any rounding errors.

Unfortunately, the fix requires a common prerequisite calculation for all three individual axes, so to avoid doing the same work multiple times I've removed the default implementation of `convert_quat()` and moved it to an implementation that basically does all the work that `first()`, `second()` and `third()` are doing, causing this code to be duplicated.

As far as the actual math is concerned, this is where the magic happens:
```
let sine_theta = self.sine_theta(q);
let second = math::asin(sine_theta);

if math::abs(sine_theta) > 0.99999 {
	let scale = 2.0 * math::signum(sine_theta);

	return match self {
		ZYX => (scale * math::atan2(-q.x, q.w), second, 0.0),
		ZXY => (scale * math::atan2(q.y, q.w), second, 0.0),
		YXZ => (scale * math::atan2(-q.z, q.w), second, 0.0),
		YZX => (scale * math::atan2(q.x, q.w), second, 0.0),
		XYZ => (scale * math::atan2(q.z, q.w), second, 0.0),
		XZY => (scale * math::atan2(-q.y, q.w), second, 0.0),
	};
}
```
So when the second rotation, `theta`, is `PI/2` or `-PI/2`, `sin(theta)` will be 1 or -1 respectively, and we can calculate the first rotation based on the table in this code.

oisyn: Done! It now tests for the full -180..=180 range on all axes 🙂.

Had to rewrite the tests a bit. It used to test that `to_euler()` returned the same values as given to `from_euler()`, but that's incorrect as those can be ambiguous. The same goes for quaternions: `-q` represents the same rotation as `q`, and `(x, y, z, 0)` for any value of `x`, `y` and `z` represents no rotation at all, so I had to make sure these all compare equal in the test.
oisyn: Alright, they all seem to pass now 🚀

Patch:
diff --git a/src/euler.rs b/src/euler.rs
index bb9236b..85f8099 100644
--- a/src/euler.rs
+++ b/src/euler.rs
@@ -46,9 +46,10 @@ pub(crate) trait EulerFromQuaternion<Q: Copy>: Sized + Copy {
     fn third(self, q: Q) -> Self::Output;
 
     /// Compute all angles of a rotation in the notation order
-    fn convert_quat(self, q: Q) -> (Self::Output, Self::Output, Self::Output) {
-        (self.first(q), self.second(q), self.third(q))
-    }
+    fn convert_quat(self, q: Q) -> (Self::Output, Self::Output, Self::Output);
+
+    #[doc(hidden)]
+    fn sine_theta(self, q: Q) -> Self::Output;
 }
 
 /// Conversion from euler angles to quaternion.
@@ -62,10 +63,127 @@ macro_rules! impl_from_quat {
     ($t:ident, $quat:ident) => {
         impl EulerFromQuaternion<$quat> for EulerRot {
             type Output = $t;
+
+            fn sine_theta(self, q: $quat) -> $t {
+                use EulerRot::*;
+                match self {
+                    ZYX => -2.0 * (q.x * q.z - q.w * q.y),
+                    ZXY => 2.0 * (q.y * q.z + q.w * q.x),
+                    YXZ => -2.0 * (q.y * q.z - q.w * q.x),
+                    YZX => 2.0 * (q.x * q.y + q.w * q.z),
+                    XYZ => 2.0 * (q.x * q.z + q.w * q.y),
+                    XZY => -2.0 * (q.x * q.y - q.w * q.z),
+                }
+                .clamp(-1.0, 1.0)
+            }
+
             fn first(self, q: $quat) -> $t {
                 use crate::$t::math;
                 use EulerRot::*;
-                match self {
+
+                let sine_theta = self.sine_theta(q);
+                if math::abs(sine_theta) > 0.99999 {
+                    let scale = 2.0 * math::signum(sine_theta);
+
+                    match self {
+                        ZYX => scale * math::atan2(-q.x, q.w),
+                        ZXY => scale * math::atan2(q.y, q.w),
+                        YXZ => scale * math::atan2(-q.z, q.w),
+                        YZX => scale * math::atan2(q.x, q.w),
+                        XYZ => scale * math::atan2(q.z, q.w),
+                        XZY => scale * math::atan2(-q.y, q.w),
+                    }
+                } else {
+                    match self {
+                        ZYX => math::atan2(
+                            2.0 * (q.x * q.y + q.w * q.z),
+                            q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
+                        ),
+                        ZXY => math::atan2(
+                            -2.0 * (q.x * q.y - q.w * q.z),
+                            q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
+                        ),
+                        YXZ => math::atan2(
+                            2.0 * (q.x * q.z + q.w * q.y),
+                            q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
+                        ),
+                        YZX => math::atan2(
+                            -2.0 * (q.x * q.z - q.w * q.y),
+                            q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
+                        ),
+                        XYZ => math::atan2(
+                            -2.0 * (q.y * q.z - q.w * q.x),
+                            q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
+                        ),
+                        XZY => math::atan2(
+                            2.0 * (q.y * q.z + q.w * q.x),
+                            q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
+                        ),
+                    }
+                }
+            }
+
+            fn second(self, q: $quat) -> $t {
+                use crate::$t::math;
+                math::asin(self.sine_theta(q))
+            }
+
+            fn third(self, q: $quat) -> $t {
+                use crate::$t::math;
+                use EulerRot::*;
+                if math::abs(self.sine_theta(q)) > 0.99999 {
+                    0.0
+                } else {
+                    match self {
+                        ZYX => math::atan2(
+                            2.0 * (q.y * q.z + q.w * q.x),
+                            q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
+                        ),
+                        ZXY => math::atan2(
+                            -2.0 * (q.x * q.z - q.w * q.y),
+                            q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
+                        ),
+                        YXZ => math::atan2(
+                            2.0 * (q.x * q.y + q.w * q.z),
+                            q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
+                        ),
+                        YZX => math::atan2(
+                            -2.0 * (q.y * q.z - q.w * q.x),
+                            q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
+                        ),
+                        XYZ => math::atan2(
+                            -2.0 * (q.x * q.y - q.w * q.z),
+                            q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
+                        ),
+                        XZY => math::atan2(
+                            2.0 * (q.x * q.z + q.w * q.y),
+                            q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
+                        ),
+                    }
+                }
+            }
+
+            fn convert_quat(self, q: $quat) -> ($t, $t, $t) {
+                use crate::$t::math;
+                use EulerRot::*;
+
+                let sine_theta = self.sine_theta(q);
+                let second = math::asin(sine_theta);
+
+                if math::abs(sine_theta) > 0.99999 {
+                    let scale = 2.0 * math::signum(sine_theta);
+
+                    return match self {
+                        ZYX => (scale * math::atan2(-q.x, q.w), second, 0.0),
+                        ZXY => (scale * math::atan2(q.y, q.w), second, 0.0),
+                        YXZ => (scale * math::atan2(-q.z, q.w), second, 0.0),
+                        YZX => (scale * math::atan2(q.x, q.w), second, 0.0),
+                        XYZ => (scale * math::atan2(q.z, q.w), second, 0.0),
+                        XZY => (scale * math::atan2(-q.y, q.w), second, 0.0),
+                    };
+                }
+
+                let first = match self {
                     ZYX => math::atan2(
                         2.0 * (q.x * q.y + q.w * q.z),
                         q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
@@ -90,26 +208,9 @@ macro_rules! impl_from_quat {
                         2.0 * (q.y * q.z + q.w * q.x),
                         q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
                     ),
-                }
-            }
+                };
 
-            fn second(self, q: $quat) -> $t {
-                use crate::$t::math;
-                use EulerRot::*;
-                match self {
-                    ZYX => math::asin_clamped(-2.0 * (q.x * q.z - q.w * q.y)),
-                    ZXY => math::asin_clamped(2.0 * (q.y * q.z + q.w * q.x)),
-                    YXZ => math::asin_clamped(-2.0 * (q.y * q.z - q.w * q.x)),
-                    YZX => math::asin_clamped(2.0 * (q.x * q.y + q.w * q.z)),
-                    XYZ => math::asin_clamped(2.0 * (q.x * q.z + q.w * q.y)),
-                    XZY => math::asin_clamped(-2.0 * (q.x * q.y - q.w * q.z)),
-                }
-            }
-
-            fn third(self, q: $quat) -> $t {
-                use crate::$t::math;
-                use EulerRot::*;
-                match self {
+                let third = match self {
                     ZYX => math::atan2(
                         2.0 * (q.y * q.z + q.w * q.x),
                         q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
@@ -134,7 +235,9 @@ macro_rules! impl_from_quat {
                         2.0 * (q.x * q.z + q.w * q.y),
                         q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
                     ),
-                }
+                };
+
+                (first, second, third)
             }
         }
         // End - impl EulerFromQuaternion
diff --git a/src/f32/math.rs b/src/f32/math.rs
index b6d5f1f..3e25875 100644
--- a/src/f32/math.rs
+++ b/src/f32/math.rs
@@ -45,8 +45,8 @@ mod libm_math {
     }
 
     #[inline(always)]
-    pub(crate) fn asin_clamped(f: f32) -> f32 {
-        libm::asinf(f.clamp(-1.0, 1.0))
+    pub(crate) fn asin(f: f32) -> f32 {
+        libm::asinf(f)
     }
 
     #[inline(always)]
@@ -158,8 +158,8 @@ mod std_math {
     }
 
     #[inline(always)]
-    pub(crate) fn asin_clamped(f: f32) -> f32 {
-        f32::asin(f32::clamp(f, -1.0, 1.0))
+    pub(crate) fn asin(f: f32) -> f32 {
+        f32::asin(f)
     }
 
     #[inline(always)]
diff --git a/src/f64/math.rs b/src/f64/math.rs
index d851ef4..45ace5c 100644
--- a/src/f64/math.rs
+++ b/src/f64/math.rs
@@ -11,8 +11,8 @@ mod libm_math {
     }
 
     #[inline(always)]
-    pub(crate) fn asin_clamped(f: f64) -> f64 {
-        libm::asin(f.clamp(-1.0, 1.0))
+    pub(crate) fn asin(f: f64) -> f64 {
+        libm::asin(f)
     }
 
     #[inline(always)]
@@ -123,8 +123,8 @@ mod std_math {
     }
 
     #[inline(always)]
-    pub(crate) fn asin_clamped(f: f64) -> f64 {
-        f64::asin(f64::clamp(f, -1.0, 1.0))
+    pub(crate) fn asin(f: f64) -> f64 {
+        f64::asin(f)
     }
 
     #[inline(always)]


Test patch:
diff --git a/tests/euler.rs b/tests/euler.rs
index dc4de53..b2d0726 100644
--- a/tests/euler.rs
+++ b/tests/euler.rs
@@ -1,47 +1,53 @@
 #[macro_use]
 mod support;
 
-/// Helper to calculate the inner angle in the range [0, 2*PI)
-trait AngleDiff {
-    type Output;
-    fn angle_diff(self, other: Self) -> Self::Output;
+/// Helper to get the 'canonical' version of a `Quat`. We define the canonical of quat `q` as:
+/// * `q`, if q.w > epsilon
+/// * `-q`, if q.w < -epsilon
+/// * `(0, 0, 0, 1)` otherwise
+/// The rationale is that q and -q represent the same rotation, and any (_, _, _, 0) respresent no rotation at all.
+trait CanonicalQuat: Copy {
+    fn canonical(self) -> Self;
 }
 
-macro_rules! impl_angle_diff {
-    ($t:ty, $pi:expr) => {
-        impl AngleDiff for $t {
-            type Output = $t;
-            fn angle_diff(self, other: $t) -> $t {
-                const PI2: $t = $pi + $pi;
-                let s = self.rem_euclid(PI2);
-                let o = other.rem_euclid(PI2);
-                if s > o {
-                    (s - o).min(PI2 + o - s)
-                } else {
-                    (o - s).min(PI2 + s - o)
+macro_rules! impl_canonical_quat {
+    ($t:ty) => {
+        impl CanonicalQuat for $t {
+            fn canonical(self) -> Self {
+                match self {
+                    _ if self.w >= 1e-5 => self,
+                    _ if self.w <= -1e-5 => -self,
+                    _ => <$t>::from_xyzw(0.0, 0.0, 0.0, 1.0),
                 }
             }
         }
     };
 }
-impl_angle_diff!(f32, std::f32::consts::PI);
-impl_angle_diff!(f64, std::f64::consts::PI);
-
-macro_rules! assert_approx_angle {
-    ($a:expr, $b:expr, $eps:expr) => {{
-        let (a, b) = ($a, $b);
-        let eps = $eps;
-        let diff = a.angle_diff(b);
-        assert!(
-            diff < $eps,
-            "assertion failed: `(left !== right)` \
-             (left: `{:?}`, right: `{:?}`, expect diff: `{:?}`, real diff: `{:?}`)",
-            a,
-            b,
-            eps,
-            diff
-        );
-    }};
+
+impl_canonical_quat!(glam::Quat);
+impl_canonical_quat!(glam::DQuat);
+
+/// Helper to set some alternative epsilons based on the floating point type used
+trait EulerEpsilon {
+    /// epsilon for comparing quaterions built from eulers and axis-angles
+    const Q_EPS: f32;
+
+    /// epsilon for comparing quaternion round-tripped through eulers (quat -> euler -> quat)
+    const E_EPS: f32;
+}
+impl EulerEpsilon for f32 {
+    const Q_EPS: f32 = 1e-5;
+
+    // The scalar-math and wasm paths seems to use a particularly bad implementation of the trig functions
+    #[cfg(any(feature = "scalar-math", target_arch = "wasm32"))]
+    const E_EPS: f32 = 2e-4;
+
+    #[cfg(not(any(feature = "scalar-math", target_arch = "wasm32")))]
+    const E_EPS: f32 = 1e-5;
+}
+impl EulerEpsilon for f64 {
+    const Q_EPS: f32 = 1e-8;
+    const E_EPS: f32 = 1e-8;
 }
 
 macro_rules! impl_3axis_test {
@@ -49,9 +55,9 @@ macro_rules! impl_3axis_test {
         glam_test!($name, {
             let euler = $euler;
             assert!($U != $W); // First and last axis must be different for three axis
-            for u in (-176..=176).step_by(44) {
-                for v in (-88..=88).step_by(44) {
-                    for w in (-176..=176).step_by(44) {
+            for u in (-180..=180).step_by(15) {
+                for v in (-180..=180).step_by(15) {
+                    for w in (-180..=180).step_by(15) {
                         let u1 = (u as $t).to_radians();
                         let v1 = (v as $t).to_radians();
                         let w1 = (w as $t).to_radians();
@@ -63,19 +69,21 @@ macro_rules! impl_3axis_test {
 
                         // Test if the rotation is the expected
                         let q2: $quat = $quat::from_euler(euler, u1, v1, w1).normalize();
-                        assert_approx_eq!(q1, q2, 1e-5);
+                        assert_approx_eq!(q1.canonical(), q2.canonical(), <$t>::Q_EPS);
 
-                        // Test angle reconstruction
-                        let (u2, v2, w2) = q1.to_euler(euler);
+                        // Test quat reconstruction from angles
+                        let (u2, v2, w2) = q2.to_euler(euler);
                         let q3 = $quat::from_euler(euler, u2, v2, w2).normalize();
-
-                        assert_approx_angle!(u1, u2, 1e-4 as $t);
-                        assert_approx_angle!(v1, v2, 1e-4 as $t);
-                        assert_approx_angle!(w1, w2, 1e-4 as $t);
-
-                        assert_approx_eq!(q1 * $vec::X, q3 * $vec::X, 1e-4);
-                        assert_approx_eq!(q1 * $vec::Y, q3 * $vec::Y, 1e-4);
-                        assert_approx_eq!(q1 * $vec::Z, q3 * $vec::Z, 1e-4);
+                        assert_approx_eq!(
+                            q2.canonical(),
+                            q3.canonical(),
+                            <$t>::E_EPS,
+                            format!(
+                                "angles {:?} -> {:?}",
+                                (u, v, w),
+                                (u2.to_degrees(), v2.to_degrees(), w2.to_degrees())
+                            )
+                        );
                     }
                 }
             }
@@ -95,7 +103,7 @@ macro_rules! impl_all_quat_tests_three_axis {
 }
 
 mod euler {
-    use super::AngleDiff;
+    use super::{CanonicalQuat, EulerEpsilon};
     use glam::*;
     type ER = EulerRot;
 
diff --git a/tests/support/macros.rs b/tests/support/macros.rs
index 9801902..e7fadd1 100644
--- a/tests/support/macros.rs
+++ b/tests/support/macros.rs
@@ -56,6 +56,22 @@ macro_rules! assert_approx_eq {
             a.abs_diff(b)
         );
     }};
+    ($a:expr, $b:expr, $eps:expr, $ctx:expr) => {{
+        use $crate::support::FloatCompare;
+        let (a, b) = (&$a, &$b);
+        let eps = $eps;
+        assert!(
+            a.approx_eq(b, $eps),
+            "assertion failed: `(left !== right)` \
+             (left: `{:?}`, right: `{:?}`, expect diff: `{:?}`, real diff: `{:?}`), \
+             additional context: {}",
+            *a,
+            *b,
+            eps,
+            a.abs_diff(b),
+            $ctx
+        );
+    }};
 }
 
 /// Test vector normalization for float vector