diff options
author | Yuya Nishihara <yuya@tcha.org> | 2021-08-01 14:02:27 +0900 |
---|---|---|
committer | Inho Lee <inho.lee@qt.io> | 2022-01-16 01:12:15 +0100 |
commit | 6ffc8d8eb6c44fbd51e37770e7013c4610ead96d (patch) | |
tree | cb915e45be2b701ed8bda4a92b8f6f86e3139101 /src/gui/math3d/qquaternion.cpp | |
parent | 6852c20502ffc78ad76924630d9021552287e35d (diff) |
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
Pick-to: 6.3 6.2 5.15
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>
Diffstat (limited to 'src/gui/math3d/qquaternion.cpp')
-rw-r--r-- | src/gui/math3d/qquaternion.cpp | 34 |
1 files changed, 18 insertions, 16 deletions
diff --git a/src/gui/math3d/qquaternion.cpp b/src/gui/math3d/qquaternion.cpp index ba82aaa737..93e396c5a1 100644 --- a/src/gui/math3d/qquaternion.cpp +++ b/src/gui/math3d/qquaternion.cpp @@ -492,10 +492,13 @@ void QQuaternion::getEulerAngles(float *pitch, float *yaw, float *roll) const Q_ASSERT(pitch && yaw && roll); // Algorithm adapted from: - // http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q37 + // 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 = !qFuzzyCompare(len, 1.0f) && !qFuzzyIsNull(len); + 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; @@ -511,24 +514,23 @@ void QQuaternion::getEulerAngles(float *pitch, float *yaw, float *roll) const 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); |