diff options
Diffstat (limited to 'src/gui/math3d/qquaternion.cpp')
| -rw-r--r-- | src/gui/math3d/qquaternion.cpp | 163 |
1 files changed, 64 insertions, 99 deletions
diff --git a/src/gui/math3d/qquaternion.cpp b/src/gui/math3d/qquaternion.cpp index ba82aaa737..e7c5945208 100644 --- a/src/gui/math3d/qquaternion.cpp +++ b/src/gui/math3d/qquaternion.cpp @@ -1,41 +1,5 @@ -/**************************************************************************** -** -** Copyright (C) 2020 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:LGPL$ -** Commercial License Usage -** Licensees holding valid commercial Qt licenses may use this file in -** accordance with the commercial license agreement provided with the -** Software or, alternatively, in accordance with the terms contained in -** a written agreement between you and The Qt Company. For licensing terms -** and conditions see https://www.qt.io/terms-conditions. For further -** information use the contact form at https://www.qt.io/contact-us. -** -** 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$ -** -****************************************************************************/ +// Copyright (C) 2020 The Qt Company Ltd. +// SPDX-License-Identifier: LicenseRef-Qt-Commercial OR LGPL-3.0-only OR GPL-2.0-only OR GPL-3.0-only #include "qquaternion.h" #include <QtCore/qdatastream.h> @@ -62,14 +26,14 @@ QT_BEGIN_NAMESPACE */ /*! - \fn QQuaternion::QQuaternion() + \fn QQuaternion::QQuaternion() noexcept Constructs an identity quaternion (1, 0, 0, 0), i.e. with the vector (0, 0, 0) and scalar 1. */ /*! - \fn QQuaternion::QQuaternion(Qt::Initialization) + \fn QQuaternion::QQuaternion(Qt::Initialization) noexcept \since 5.5 \internal @@ -77,7 +41,7 @@ QT_BEGIN_NAMESPACE */ /*! - \fn QQuaternion::QQuaternion(float scalar, float xpos, float ypos, float zpos) + \fn QQuaternion::QQuaternion(float scalar, float xpos, float ypos, float zpos) noexcept Constructs a quaternion with the vector (\a xpos, \a ypos, \a zpos) and \a scalar. @@ -86,7 +50,7 @@ QT_BEGIN_NAMESPACE #ifndef QT_NO_VECTOR3D /*! - \fn QQuaternion::QQuaternion(float scalar, const QVector3D& vector) + \fn QQuaternion::QQuaternion(float scalar, const QVector3D &vector) noexcept Constructs a quaternion vector from the specified \a vector and \a scalar. @@ -95,7 +59,7 @@ QT_BEGIN_NAMESPACE */ /*! - \fn QVector3D QQuaternion::vector() const + \fn QVector3D QQuaternion::vector() const noexcept Returns the vector component of this quaternion. @@ -103,7 +67,7 @@ QT_BEGIN_NAMESPACE */ /*! - \fn void QQuaternion::setVector(const QVector3D& vector) + \fn void QQuaternion::setVector(const QVector3D &vector) noexcept Sets the vector component of this quaternion to \a vector. @@ -113,7 +77,7 @@ QT_BEGIN_NAMESPACE #endif /*! - \fn void QQuaternion::setVector(float x, float y, float z) + \fn void QQuaternion::setVector(float x, float y, float z) noexcept Sets the vector component of this quaternion to (\a x, \a y, \a z). @@ -123,13 +87,13 @@ QT_BEGIN_NAMESPACE #ifndef QT_NO_VECTOR4D /*! - \fn QQuaternion::QQuaternion(const QVector4D& vector) + \fn QQuaternion::QQuaternion(const QVector4D &vector) noexcept Constructs a quaternion from the components of \a vector. */ /*! - \fn QVector4D QQuaternion::toVector4D() const + \fn QVector4D QQuaternion::toVector4D() const noexcept Returns this quaternion as a 4D vector. */ @@ -137,14 +101,14 @@ QT_BEGIN_NAMESPACE #endif /*! - \fn bool QQuaternion::isNull() const + \fn bool QQuaternion::isNull() const noexcept Returns \c true if the x, y, z, and scalar components of this quaternion are set to 0.0; otherwise returns \c false. */ /*! - \fn bool QQuaternion::isIdentity() const + \fn bool QQuaternion::isIdentity() const noexcept Returns \c true if the x, y, and z components of this quaternion are set to 0.0, and the scalar component is set @@ -152,7 +116,7 @@ QT_BEGIN_NAMESPACE */ /*! - \fn float QQuaternion::x() const + \fn float QQuaternion::x() const noexcept Returns the x coordinate of this quaternion's vector. @@ -160,7 +124,7 @@ QT_BEGIN_NAMESPACE */ /*! - \fn float QQuaternion::y() const + \fn float QQuaternion::y() const noexcept Returns the y coordinate of this quaternion's vector. @@ -168,7 +132,7 @@ QT_BEGIN_NAMESPACE */ /*! - \fn float QQuaternion::z() const + \fn float QQuaternion::z() const noexcept Returns the z coordinate of this quaternion's vector. @@ -176,7 +140,7 @@ QT_BEGIN_NAMESPACE */ /*! - \fn float QQuaternion::scalar() const + \fn float QQuaternion::scalar() const noexcept Returns the scalar component of this quaternion. @@ -184,7 +148,7 @@ QT_BEGIN_NAMESPACE */ /*! - \fn void QQuaternion::setX(float x) + \fn void QQuaternion::setX(float x) noexcept Sets the x coordinate of this quaternion's vector to the given \a x coordinate. @@ -193,7 +157,7 @@ QT_BEGIN_NAMESPACE */ /*! - \fn void QQuaternion::setY(float y) + \fn void QQuaternion::setY(float y) noexcept Sets the y coordinate of this quaternion's vector to the given \a y coordinate. @@ -202,7 +166,7 @@ QT_BEGIN_NAMESPACE */ /*! - \fn void QQuaternion::setZ(float z) + \fn void QQuaternion::setZ(float z) noexcept Sets the z coordinate of this quaternion's vector to the given \a z coordinate. @@ -211,7 +175,7 @@ QT_BEGIN_NAMESPACE */ /*! - \fn void QQuaternion::setScalar(float scalar) + \fn void QQuaternion::setScalar(float scalar) noexcept Sets the scalar component of this quaternion to \a scalar. @@ -219,7 +183,7 @@ QT_BEGIN_NAMESPACE */ /*! - \fn float QQuaternion::dotProduct(const QQuaternion &q1, const QQuaternion &q2) + \fn float QQuaternion::dotProduct(const QQuaternion &q1, const QQuaternion &q2) noexcept \since 5.5 Returns the dot product of \a q1 and \a q2. @@ -263,8 +227,6 @@ float QQuaternion::lengthSquared() const QQuaternion QQuaternion::normalized() const { const float scale = length(); - if (qFuzzyCompare(scale, 1.0f)) - return *this; if (qFuzzyIsNull(scale)) return QQuaternion(0.0f, 0.0f, 0.0f, 0.0f); return *this / scale; @@ -279,7 +241,7 @@ QQuaternion QQuaternion::normalized() const void QQuaternion::normalize() { const float len = length(); - if (qFuzzyCompare(len, 1.0f) || qFuzzyIsNull(len)) + if (qFuzzyIsNull(len)) return; xp /= len; @@ -289,7 +251,7 @@ void QQuaternion::normalize() } /*! - \fn QQuaternion QQuaternion::inverted() const + \fn QQuaternion QQuaternion::inverted() const noexcept \since 5.5 Returns the inverse of this quaternion. @@ -299,7 +261,7 @@ void QQuaternion::normalize() */ /*! - \fn QQuaternion QQuaternion::conjugated() const + \fn QQuaternion QQuaternion::conjugated() const noexcept \since 5.5 Returns the conjugate of this quaternion, which is @@ -316,13 +278,13 @@ void QQuaternion::normalize() \snippet code/src_gui_math3d_qquaternion.cpp 1 */ -QVector3D QQuaternion::rotatedVector(const QVector3D& vector) const +QVector3D QQuaternion::rotatedVector(const QVector3D &vector) const { return (*this * QQuaternion(0, vector) * conjugated()).vector(); } /*! - \fn QQuaternion &QQuaternion::operator+=(const QQuaternion &quaternion) + \fn QQuaternion &QQuaternion::operator+=(const QQuaternion &quaternion) noexcept Adds the given \a quaternion to this quaternion and returns a reference to this quaternion. @@ -331,7 +293,7 @@ QVector3D QQuaternion::rotatedVector(const QVector3D& vector) const */ /*! - \fn QQuaternion &QQuaternion::operator-=(const QQuaternion &quaternion) + \fn QQuaternion &QQuaternion::operator-=(const QQuaternion &quaternion) noexcept Subtracts the given \a quaternion from this quaternion and returns a reference to this quaternion. @@ -340,7 +302,7 @@ QVector3D QQuaternion::rotatedVector(const QVector3D& vector) const */ /*! - \fn QQuaternion &QQuaternion::operator*=(float factor) + \fn QQuaternion &QQuaternion::operator*=(float factor) noexcept Multiplies this quaternion's components by the given \a factor, and returns a reference to this quaternion. @@ -349,7 +311,7 @@ QVector3D QQuaternion::rotatedVector(const QVector3D& vector) const */ /*! - \fn QQuaternion &QQuaternion::operator*=(const QQuaternion &quaternion) + \fn QQuaternion &QQuaternion::operator*=(const QQuaternion &quaternion) noexcept Multiplies this quaternion by \a quaternion and returns a reference to this quaternion. @@ -367,7 +329,7 @@ QVector3D QQuaternion::rotatedVector(const QVector3D& vector) const #ifndef QT_NO_VECTOR3D /*! - \fn void QQuaternion::getAxisAndAngle(QVector3D *axis, float *angle) const + \fn void QQuaternion::getAxisAndAngle(QVector3D *axis, float *angle) const noexcept \since 5.5 \overload @@ -383,7 +345,7 @@ QVector3D QQuaternion::rotatedVector(const QVector3D& vector) const \sa getAxisAndAngle() */ -QQuaternion QQuaternion::fromAxisAndAngle(const QVector3D& axis, float angle) +QQuaternion QQuaternion::fromAxisAndAngle(const QVector3D &axis, float angle) { // Algorithm from: // http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q56 @@ -424,7 +386,7 @@ void QQuaternion::getAxisAndAngle(float *x, float *y, float *z, float *angle) co *y = yp / length; *z = zp / length; } - *angle = qRadiansToDegrees(2.0f * std::acos(wp)); + *angle = qRadiansToDegrees(2.0f * std::atan2(length, wp)); } else { // angle is 0 (mod 2*pi), so any axis will fit *x = *y = *z = *angle = 0.0f; @@ -492,10 +454,14 @@ 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". + // We can only detect Gimbal lock when we normalize, which we can't do when + // length is nearly zero. Do so before multiplying coordinates, to avoid + // underflow. 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 +477,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); @@ -773,21 +738,21 @@ QQuaternion QQuaternion::rotationTo(const QVector3D &from, const QVector3D &to) #endif // QT_NO_VECTOR3D /*! - \fn bool QQuaternion::operator==(const QQuaternion &q1, const QQuaternion &q2) + \fn bool QQuaternion::operator==(const QQuaternion &q1, const QQuaternion &q2) noexcept Returns \c true if \a q1 is equal to \a q2; otherwise returns \c false. This operator uses an exact floating-point comparison. */ /*! - \fn bool QQuaternion::operator!=(const QQuaternion &q1, const QQuaternion &q2) + \fn bool QQuaternion::operator!=(const QQuaternion &q1, const QQuaternion &q2) noexcept Returns \c true if \a q1 is not equal to \a q2; otherwise returns \c false. This operator uses an exact floating-point comparison. */ /*! - \fn const QQuaternion operator+(const QQuaternion &q1, const QQuaternion &q2) + \fn const QQuaternion operator+(const QQuaternion &q1, const QQuaternion &q2) noexcept \relates QQuaternion Returns a QQuaternion object that is the sum of the given quaternions, @@ -797,7 +762,7 @@ QQuaternion QQuaternion::rotationTo(const QVector3D &from, const QVector3D &to) */ /*! - \fn const QQuaternion operator-(const QQuaternion &q1, const QQuaternion &q2) + \fn const QQuaternion operator-(const QQuaternion &q1, const QQuaternion &q2) noexcept \relates QQuaternion Returns a QQuaternion object that is formed by subtracting @@ -807,7 +772,7 @@ QQuaternion QQuaternion::rotationTo(const QVector3D &from, const QVector3D &to) */ /*! - \fn const QQuaternion operator*(float factor, const QQuaternion &quaternion) + \fn const QQuaternion operator*(float factor, const QQuaternion &quaternion) noexcept \relates QQuaternion Returns a copy of the given \a quaternion, multiplied by the @@ -817,7 +782,7 @@ QQuaternion QQuaternion::rotationTo(const QVector3D &from, const QVector3D &to) */ /*! - \fn const QQuaternion operator*(const QQuaternion &quaternion, float factor) + \fn const QQuaternion operator*(const QQuaternion &quaternion, float factor) noexcept \relates QQuaternion Returns a copy of the given \a quaternion, multiplied by the @@ -827,7 +792,7 @@ QQuaternion QQuaternion::rotationTo(const QVector3D &from, const QVector3D &to) */ /*! - \fn const QQuaternion operator*(const QQuaternion &q1, const QQuaternion& q2) + \fn const QQuaternion operator*(const QQuaternion &q1, const QQuaternion &q2) noexcept \relates QQuaternion Multiplies \a q1 and \a q2 using quaternion multiplication. @@ -838,7 +803,7 @@ QQuaternion QQuaternion::rotationTo(const QVector3D &from, const QVector3D &to) */ /*! - \fn const QQuaternion operator-(const QQuaternion &quaternion) + \fn const QQuaternion operator-(const QQuaternion &quaternion) noexcept \relates QQuaternion \overload @@ -861,7 +826,7 @@ QQuaternion QQuaternion::rotationTo(const QVector3D &from, const QVector3D &to) #ifndef QT_NO_VECTOR3D /*! - \fn QVector3D operator*(const QQuaternion &quaternion, const QVector3D &vec) + \fn QVector3D operator*(const QQuaternion &quaternion, const QVector3D &vec) noexcept \since 5.5 \relates QQuaternion @@ -871,7 +836,7 @@ QQuaternion QQuaternion::rotationTo(const QVector3D &from, const QVector3D &to) #endif /*! - \fn bool qFuzzyCompare(const QQuaternion& q1, const QQuaternion& q2) + \fn bool qFuzzyCompare(const QQuaternion &q1, const QQuaternion &q2) noexcept \relates QQuaternion Returns \c true if \a q1 and \a q2 are equal, allowing for a small @@ -890,7 +855,7 @@ QQuaternion QQuaternion::rotationTo(const QVector3D &from, const QVector3D &to) \sa nlerp() */ QQuaternion QQuaternion::slerp - (const QQuaternion& q1, const QQuaternion& q2, float t) + (const QQuaternion &q1, const QQuaternion &q2, float t) { // Handle the easy cases first. if (t <= 0.0f) @@ -939,7 +904,7 @@ QQuaternion QQuaternion::slerp \sa slerp() */ QQuaternion QQuaternion::nlerp - (const QQuaternion& q1, const QQuaternion& q2, float t) + (const QQuaternion &q1, const QQuaternion &q2, float t) { // Handle the easy cases first. if (t <= 0.0f) |