1 files changed, 59 insertions, 58 deletions
diff --git a/src/gui/math3d/qquaternion.cpp b/src/gui/math3d/qquaternion.cpp
index 6eaa949029..4f6d063515 100644
--- a/src/gui/math3d/qquaternion.cpp
+++ b/src/gui/math3d/qquaternion.cpp
@@ -1,12 +1,11 @@
 /****************************************************************************
 **
-** Copyright (C) 2021 The Qt Company Ltd.
+** Copyright (C) 2016 The Qt Company Ltd.
 ** Contact: https://www.qt.io/licensing/
 **
 ** This file is part of the QtGui module of the Qt Toolkit.
 **
-** $QT_BEGIN_LICENSE:COMM$
-**
+** $QT_BEGIN_LICENSE:LGPL$
 ** Commercial License Usage
 ** Licensees holding valid commercial Qt licenses may use this file in
 ** accordance with the commercial license agreement provided with the
@@ -15,25 +14,26 @@
 ** and conditions see https://www.qt.io/terms-conditions. For further
 ** information use the contact form at https://www.qt.io/contact-us.
 **
-** $QT_END_LICENSE$
-**
-**
-**
-**
-**
-**
-**
-**
-**
-**
-**
-**
-**
-**
-**
-**
+** GNU Lesser General Public License Usage
+** Alternatively, this file may be used under the terms of the GNU Lesser
+** General Public License version 3 as published by the Free Software
+** Foundation and appearing in the file LICENSE.LGPL3 included in the
+** packaging of this file. Please review the following information to
+** ensure the GNU Lesser General Public License version 3 requirements
+** will be met: https://www.gnu.org/licenses/lgpl-3.0.html.
 **
+** GNU General Public License Usage
+** Alternatively, this file may be used under the terms of the GNU
+** General Public License version 2.0 or (at your option) the GNU General
+** Public license version 3 or any later version approved by the KDE Free
+** Qt Foundation. The licenses are as published by the Free Software
+** Foundation and appearing in the file LICENSE.GPL2 and LICENSE.GPL3
+** included in the packaging of this file. Please review the following
+** information to ensure the GNU General Public License requirements will
+** be met: https://www.gnu.org/licenses/gpl-2.0.html and
+** https://www.gnu.org/licenses/gpl-3.0.html.
 **
+** $QT_END_LICENSE$
 **
 ****************************************************************************/
 
@@ -508,46 +508,47 @@ 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;
-    }
-
-    *pitch = std::asin(-2.0f * (yz - xw));
-    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));
-        }
+    // Algorithm adapted from:
+    // https://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
+    // "A tutorial on SE(3) transformation parameterizations and on-manifold optimization".
+
+    // We can only detect Gimbal lock when we normalize, which we can't do when
+    // length is nearly zero. Do so before multiplying co-ordinates, to avoid
+    // underflow.
+    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 - epsilon) {
+        *pitch = std::asin(sinp);
+        *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);