3 files changed, 48 insertions, 47 deletions

diff --git a/src/gui/math3d/qmatrix4x4.cpp b/src/gui/math3d/qmatrix4x4.cpp
index cdaed788e9..0b1f8c5b30 100644
--- a/src/gui/math3d/qmatrix4x4.cpp
+++ b/src/gui/math3d/qmatrix4x4.cpp
@@ -1950,15 +1950,15 @@ QMatrix4x4 QMatrix4x4::orthonormalInverse() const
 
     Normally the QMatrix4x4 class keeps track of this special type internally
     as operations are performed.  However, if the matrix is modified
-    directly with {QLoggingCategory::operator()}{operator()()} or data(), then QMatrix4x4 will lose track of
-    the special type and will revert to the safest but least efficient
-    operations thereafter.
+    directly with \l {QMatrix4x4::}{operator()()} or data(), then
+    QMatrix4x4 will lose track of the special type and will revert to the
+    safest but least efficient operations thereafter.
 
     By calling optimize() after directly modifying the matrix,
     the programmer can force QMatrix4x4 to recover the special type if
     the elements appear to conform to one of the known optimized types.
 
-    \sa {QLoggingCategory::operator()}{operator()()}, data(), translate()
+    \sa {QMatrix4x4::}{operator()()}, data(), translate()
 */
 void QMatrix4x4::optimize()
 {
diff --git a/src/gui/math3d/qmatrix4x4.h b/src/gui/math3d/qmatrix4x4.h
index 6a726a197c..c1b61db5e5 100644
--- a/src/gui/math3d/qmatrix4x4.h
+++ b/src/gui/math3d/qmatrix4x4.h
@@ -164,14 +164,14 @@ public:
     QTransform toTransform() const;
     QTransform toTransform(float distanceToPlane) const;
 
-    QPoint map(const QPoint& point) const;
-    QPointF map(const QPointF& point) const;
+    inline QPoint map(const QPoint& point) const;
+    inline QPointF map(const QPointF& point) const;
 #ifndef QT_NO_VECTOR3D
-    QVector3D map(const QVector3D& point) const;
-    QVector3D mapVector(const QVector3D& vector) const;
+    inline QVector3D map(const QVector3D& point) const;
+    inline QVector3D mapVector(const QVector3D& vector) const;
 #endif
 #ifndef QT_NO_VECTOR4D
-    QVector4D map(const QVector4D& point) const;
+    inline QVector4D map(const QVector4D& point) const;
 #endif
     QRect mapRect(const QRect& rect) const;
     QRectF mapRect(const QRectF& rect) const;
diff --git a/src/gui/math3d/qquaternion.cpp b/src/gui/math3d/qquaternion.cpp
index 5f15949c5b..4f6d063515 100644
--- a/src/gui/math3d/qquaternion.cpp
+++ b/src/gui/math3d/qquaternion.cpp
@@ -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);