From e2bc6e02755845f8cd754e01d15df1f7e6d17e3a Mon Sep 17 00:00:00 2001 From: Yuya Nishihara Date: Sun, 1 Aug 2021 14:02:27 +0900 Subject: QtGui/math3d: Fix QQuaternion::getEulerAngles for GimbalLock cases This is heavily inspired by the patch written by Inho Lee , 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 Reviewed-by: Inho Lee Reviewed-by: Qt CI Bot (cherry picked from commit 6ffc8d8eb6c44fbd51e37770e7013c4610ead96d) Reviewed-by: Edward Welbourne --- src/gui/math3d/qquaternion.cpp | 74 ++++++++++------------ .../gui/math3d/qquaternion/tst_qquaternion.cpp | 29 +++++++++ 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(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() { -- cgit v1.2.3