diff options
Diffstat (limited to 'src/gui/math3d/qquaternion.cpp')
-rw-r--r-- | src/gui/math3d/qquaternion.cpp | 253 |
1 files changed, 103 insertions, 150 deletions
diff --git a/src/gui/math3d/qquaternion.cpp b/src/gui/math3d/qquaternion.cpp index 440ed3c99b..e7c5945208 100644 --- a/src/gui/math3d/qquaternion.cpp +++ b/src/gui/math3d/qquaternion.cpp @@ -1,41 +1,5 @@ -/**************************************************************************** -** -** 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: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. @@ -234,12 +198,15 @@ QT_BEGIN_NAMESPACE */ float QQuaternion::length() const { - return std::sqrt(xp * xp + yp * yp + zp * zp + wp * wp); + return qHypot(xp, yp, zp, wp); } /*! Returns the squared length of the quaternion. + \note Though cheap to compute, this is susceptible to overflow and underflow + that length() avoids in many cases. + \sa length(), dotProduct() */ float QQuaternion::lengthSquared() const @@ -259,17 +226,10 @@ float QQuaternion::lengthSquared() const */ QQuaternion QQuaternion::normalized() const { - // Need some extra precision if the length is very small. - double len = double(xp) * double(xp) + - double(yp) * double(yp) + - double(zp) * double(zp) + - double(wp) * double(wp); - if (qFuzzyIsNull(len - 1.0f)) - return *this; - else if (!qFuzzyIsNull(len)) - return *this / std::sqrt(len); - else + const float scale = length(); + if (qFuzzyIsNull(scale)) return QQuaternion(0.0f, 0.0f, 0.0f, 0.0f); + return *this / scale; } /*! @@ -280,16 +240,10 @@ QQuaternion QQuaternion::normalized() const */ void QQuaternion::normalize() { - // Need some extra precision if the length is very small. - double len = double(xp) * double(xp) + - double(yp) * double(yp) + - double(zp) * double(zp) + - double(wp) * double(wp); - if (qFuzzyIsNull(len - 1.0f) || qFuzzyIsNull(len)) + const float len = length(); + if (qFuzzyIsNull(len)) return; - len = std::sqrt(len); - xp /= len; yp /= len; zp /= len; @@ -297,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. @@ -307,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 @@ -324,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. @@ -339,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. @@ -348,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. @@ -357,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. @@ -375,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 @@ -391,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 @@ -421,24 +375,22 @@ void QQuaternion::getAxisAndAngle(float *x, float *y, float *z, float *angle) co // The quaternion representing the rotation is // q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k) - float length = xp * xp + yp * yp + zp * zp; + const float length = qHypot(xp, yp, zp); if (!qFuzzyIsNull(length)) { - *x = xp; - *y = yp; - *z = zp; - if (!qFuzzyIsNull(length - 1.0f)) { - length = std::sqrt(length); - *x /= length; - *y /= length; - *z /= length; + if (qFuzzyCompare(length, 1.0f)) { + *x = xp; + *y = yp; + *z = zp; + } else { + *x = xp / length; + *y = yp / length; + *z = zp / length; } - *angle = 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; } - - *angle = qRadiansToDegrees(*angle); } /*! @@ -450,8 +402,8 @@ void QQuaternion::getAxisAndAngle(float *x, float *y, float *z, float *angle) co QQuaternion QQuaternion::fromAxisAndAngle (float x, float y, float z, float angle) { - float length = std::sqrt(x * x + y * y + z * z); - if (!qFuzzyIsNull(length - 1.0f) && !qFuzzyIsNull(length)) { + float length = qHypot(x, y, z); + if (!qFuzzyCompare(length, 1.0f) && !qFuzzyIsNull(length)) { x /= length; y /= length; z /= length; @@ -501,46 +453,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 coordinates, 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); @@ -785,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, @@ -809,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 @@ -819,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 @@ -829,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 @@ -839,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. @@ -850,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 @@ -873,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 @@ -883,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 @@ -902,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) @@ -951,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) |