QtGui/math3d: Fix QQuaternion::getEulerAngles for GimbalLock cases

This is heavily inspired by the patch written by Inho Lee
<inho.lee@qt.io>, which says "There is a precision problem in the
previous algorithm when checking pitch value. (In the case that the
rotation on the X-axis makes Gimbal lock.)"

In order to work around the precision problem, this patch does:

 1. switch to the algorithm described in the inline comment to make
    the story simple.
 2. forcibly normalize the {x, y, z, w} components to eliminate
    fractional errors.
 3. set threshold to avoid hidden division by cos(pitch) =~ 0.

From my testing which compares dot product of the original quaternion
and the one recreated from Euler angles, calculation within float range
seems okay. (abs(normalize(q_orig) * normalize(q_roundtrip)) >= 0.99999)

Many thanks to Inho Lee for the original patch and discussion about
rounding errors.

Fixes: QTBUG-72103
Change-Id: I8995e4affe603111ff2303a0dfcbdb0b1ae03f10
Reviewed-by: Yuya Nishihara <yuya@tcha.org>
Reviewed-by: Inho Lee <inho.lee@qt.io>
Reviewed-by: Qt CI Bot <qt_ci_bot@qt-project.org>
(cherry picked from commit 6ffc8d8eb6c44fbd51e37770e7013c4610ead96d)
Reviewed-by: Edward Welbourne <edward.welbourne@qt.io>

2 files changed, 64 insertions, 39 deletions

diff --git a/src/gui/math3d/qquaternion.cpp b/src/gui/math3d/qquaternion.cpp
index 71554d15ee..7145330cfd 100644
--- a/src/gui/math3d/qquaternion.cpp
+++ b/src/gui/math3d/qquaternion.cpp
@@ -508,50 +508,46 @@ void QQuaternion::getEulerAngles(float *pitch, float *yaw, float *roll) const
 {
     Q_ASSERT(pitch && yaw && roll);
 
-    // Algorithm from:
-    // http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q37
-
-    float xx = xp * xp;
-    float xy = xp * yp;
-    float xz = xp * zp;
-    float xw = xp * wp;
-    float yy = yp * yp;
-    float yz = yp * zp;
-    float yw = yp * wp;
-    float zz = zp * zp;
-    float zw = zp * wp;
-
-    const float lengthSquared = xx + yy + zz + wp * wp;
-    if (!qFuzzyIsNull(lengthSquared - 1.0f) && !qFuzzyIsNull(lengthSquared)) {
-        xx /= lengthSquared;
-        xy /= lengthSquared; // same as (xp / length) * (yp / length)
-        xz /= lengthSquared;
-        xw /= lengthSquared;
-        yy /= lengthSquared;
-        yz /= lengthSquared;
-        yw /= lengthSquared;
-        zz /= lengthSquared;
-        zw /= lengthSquared;
-    }
+    // Algorithm adapted from:
+    // https://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
+    // "A tutorial on SE(3) transformation parameterizations and on-manifold optimization".
+
+    // Normalize values even if the length is below the margin. Otherwise we might fail
+    // to detect Gimbal lock due to cumulative errors.
+    const float len = length();
+    const bool rescale = !qFuzzyIsNull(len);
+    const float xps = rescale ? xp / len : xp;
+    const float yps = rescale ? yp / len : yp;
+    const float zps = rescale ? zp / len : zp;
+    const float wps = rescale ? wp / len : wp;
+
+    const float xx = xps * xps;
+    const float xy = xps * yps;
+    const float xz = xps * zps;
+    const float xw = xps * wps;
+    const float yy = yps * yps;
+    const float yz = yps * zps;
+    const float yw = yps * wps;
+    const float zz = zps * zps;
+    const float zw = zps * wps;
+
+    // For the common case, we have a hidden division by cos(pitch) to calculate
+    // yaw and roll: atan2(a / cos(pitch), b / cos(pitch)) = atan2(a, b). This equation
+    // wouldn't work if cos(pitch) is close to zero (i.e. abs(sin(pitch)) =~ 1.0).
+    // This threshold is copied from qFuzzyIsNull() to avoid the hidden division by zero.
+    constexpr float epsilon = 0.00001f;
 
     const float sinp = -2.0f * (yz - xw);
-    if (std::abs(sinp) >= 1.0f)
-        *pitch = std::copysign(M_PI_2, sinp);
-    else
+    if (std::abs(sinp) < 1.0f - epsilon) {
         *pitch = std::asin(sinp);
-    if (*pitch < M_PI_2) {
-        if (*pitch > -M_PI_2) {
-            *yaw = std::atan2(2.0f * (xz + yw), 1.0f - 2.0f * (xx + yy));
-            *roll = std::atan2(2.0f * (xy + zw), 1.0f - 2.0f * (xx + zz));
-        } else {
-            // not a unique solution
-            *roll = 0.0f;
-            *yaw = -std::atan2(-2.0f * (xy - zw), 1.0f - 2.0f * (yy + zz));
-        }
+        *yaw = std::atan2(2.0f * (xz + yw), 1.0f - 2.0f * (xx + yy));
+        *roll = std::atan2(2.0f * (xy + zw), 1.0f - 2.0f * (xx + zz));
     } else {
-        // not a unique solution
+        // Gimbal lock case, which doesn't have a unique solution. We just use
+        // XY rotation.
+        *pitch = std::copysign(static_cast<float>(M_PI_2), sinp);
+        *yaw = 2.0f * std::atan2(yps, wps);
         *roll = 0.0f;
-        *yaw = std::atan2(-2.0f * (xy - zw), 1.0f - 2.0f * (yy + zz));
     }
 
     *pitch = qRadiansToDegrees(*pitch);
diff --git a/tests/auto/gui/math3d/qquaternion/tst_qquaternion.cpp b/tests/auto/gui/math3d/qquaternion/tst_qquaternion.cpp
index 7dbd587451..ed7a6a4d5d 100644
--- a/tests/auto/gui/math3d/qquaternion/tst_qquaternion.cpp
+++ b/tests/auto/gui/math3d/qquaternion/tst_qquaternion.cpp
@@ -1113,6 +1113,35 @@ void tst_QQuaternion::fromEulerAngles_data()
 
     QTest::newRow("complex")
         << 30.0f << 240.0f << -45.0f << QQuaternion(-0.531976f, -0.43968f, 0.723317f, -0.02226f);
+
+    // Three gimbal_lock cases are not unique for the conversions from quaternion
+    // to euler, Qt will use only XY rotations for these cases.
+    // For example, QQuaternion(0.5f, 0.5f, -0.5f, 0.5f) can be EulerXYZ(90.0f, 0.0f, 90.0f), too.
+    // But Qt will always convert it to EulerXYZ(90.0f, -90.0f, 0.0f) without Z-rotation.
+    QTest::newRow("gimbal_lock_1")
+            << 90.0f << -90.0f << 0.0f << QQuaternion(0.5f, 0.5f, -0.5f, 0.5f);
+
+    QTest::newRow("gimbal_lock_2")
+            << 90.0f << 40.0f << 0.0f << QQuaternion(0.664463f, 0.664463f, 0.241845f, -0.241845f);
+
+    QTest::newRow("gimbal_lock_3") << 90.0f << 170.0f << 0.0f
+                                   << QQuaternion(0.0616285f, 0.0616285f, 0.704416f, -0.704416f);
+
+    // These four examples have a fraction of errors that would bypass normalize() threshold
+    // and could make Gimbal lock detection fail.
+    QTest::newRow("gimbal_lock_fraction_1")
+            << -90.0f << 90.001152f << 0.0f << QQuaternion(0.499989986f, -0.5f, 0.5f, 0.5f);
+
+    QTest::newRow("gimbal_lock_fraction_2")
+            << -90.0f << -179.999985f << 0.0f
+            << QQuaternion(1.00000001e-07f, 1.00000001e-10f, -0.707106769f, -0.707105756f);
+
+    QTest::newRow("gimbal_lock_fraction_3")
+            << -90.0f << 90.0011597f << 0.0f << QQuaternion(0.499989986f, -0.49999994f, 0.5f, 0.5f);
+
+    QTest::newRow("gimbal_lock_fraction_4")
+            << -90.0f << -180.0f << 0.0f
+            << QQuaternion(9.99999996e-12f, 9.99999996e-12f, -0.707106769f, -0.707096756f);
 }
 void tst_QQuaternion::fromEulerAngles()
 {