diff options
Diffstat (limited to 'src/gui/math3d/qquaternion.cpp')
| -rw-r--r-- | src/gui/math3d/qquaternion.cpp | 450 |
1 files changed, 413 insertions, 37 deletions
diff --git a/src/gui/math3d/qquaternion.cpp b/src/gui/math3d/qquaternion.cpp index f1af8922ca..6914a0b45e 100644 --- a/src/gui/math3d/qquaternion.cpp +++ b/src/gui/math3d/qquaternion.cpp @@ -1,7 +1,7 @@ /**************************************************************************** ** -** Copyright (C) 2014 Digia Plc and/or its subsidiary(-ies). -** Contact: http://www.qt-project.org/legal +** Copyright (C) 2015 The Qt Company Ltd. +** Contact: http://www.qt.io/licensing/ ** ** This file is part of the QtGui module of the Qt Toolkit. ** @@ -10,9 +10,9 @@ ** 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 Digia. For licensing terms and -** conditions see http://qt.digia.com/licensing. For further information -** use the contact form at http://qt.digia.com/contact-us. +** a written agreement between you and The Qt Company. For licensing terms +** and conditions see http://www.qt.io/terms-conditions. For further +** information use the contact form at http://www.qt.io/contact-us. ** ** GNU Lesser General Public License Usage ** Alternatively, this file may be used under the terms of the GNU Lesser @@ -23,8 +23,8 @@ ** requirements will be met: https://www.gnu.org/licenses/lgpl.html and ** http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html. ** -** In addition, as a special exception, Digia gives you certain additional -** rights. These rights are described in the Digia Qt LGPL Exception +** As a special exception, The Qt Company gives you certain additional +** rights. These rights are described in The Qt Company LGPL Exception ** version 1.1, included in the file LGPL_EXCEPTION.txt in this package. ** ** $QT_END_LICENSE$ @@ -63,6 +63,14 @@ QT_BEGIN_NAMESPACE */ /*! + \fn QQuaternion::QQuaternion(Qt::Initialization) + \since 5.5 + \internal + + Constructs a quaternion without initializing the contents. +*/ + +/*! \fn QQuaternion::QQuaternion(float scalar, float xpos, float ypos, float zpos) Constructs a quaternion with the vector (\a xpos, \a ypos, \a zpos) @@ -205,19 +213,28 @@ QT_BEGIN_NAMESPACE */ /*! + \fn float QQuaternion::dotProduct(const QQuaternion &q1, const QQuaternion &q2) + \since 5.5 + + Returns the dot product of \a q1 and \a q2. + + \sa length() +*/ + +/*! Returns the length of the quaternion. This is also called the "norm". - \sa lengthSquared(), normalized() + \sa lengthSquared(), normalized(), dotProduct() */ float QQuaternion::length() const { - return qSqrt(xp * xp + yp * yp + zp * zp + wp * wp); + return std::sqrt(xp * xp + yp * yp + zp * zp + wp * wp); } /*! Returns the squared length of the quaternion. - \sa length() + \sa length(), dotProduct() */ float QQuaternion::lengthSquared() const { @@ -232,7 +249,7 @@ float QQuaternion::lengthSquared() const will be returned as-is. Otherwise the normalized form of the quaternion of length 1 will be returned. - \sa length(), normalize() + \sa normalize(), length(), dotProduct() */ QQuaternion QQuaternion::normalized() const { @@ -244,7 +261,7 @@ QQuaternion QQuaternion::normalized() const if (qFuzzyIsNull(len - 1.0f)) return *this; else if (!qFuzzyIsNull(len)) - return *this / qSqrt(len); + return *this / std::sqrt(len); else return QQuaternion(0.0f, 0.0f, 0.0f, 0.0f); } @@ -265,7 +282,7 @@ void QQuaternion::normalize() if (qFuzzyIsNull(len - 1.0f) || qFuzzyIsNull(len)) return; - len = qSqrt(len); + len = std::sqrt(len); xp /= len; yp /= len; @@ -274,6 +291,16 @@ void QQuaternion::normalize() } /*! + \fn QQuaternion QQuaternion::inverted() const + \since 5.5 + + Returns the inverse of this quaternion. + If this quaternion is null, then a null quaternion is returned. + + \sa isNull(), length() +*/ + +/*! \fn QQuaternion QQuaternion::conjugate() const Returns the conjugate of this quaternion, which is @@ -345,8 +372,21 @@ QVector3D QQuaternion::rotatedVector(const QVector3D& vector) const #ifndef QT_NO_VECTOR3D /*! + \fn void QQuaternion::getAxisAndAngle(QVector3D *axis, float *angle) const + \since 5.5 + \overload + + Extracts a 3D axis \a axis and a rotating angle \a angle (in degrees) + that corresponds to this quaternion. + + \sa fromAxisAndAngle() +*/ + +/*! Creates a normalized quaternion that corresponds to rotating through \a angle degrees about the specified 3D \a axis. + + \sa getAxisAndAngle() */ QQuaternion QQuaternion::fromAxisAndAngle(const QVector3D& axis, float angle) { @@ -355,8 +395,8 @@ QQuaternion QQuaternion::fromAxisAndAngle(const QVector3D& axis, float angle) // We normalize the result just in case the values are close // to zero, as suggested in the above FAQ. float a = (angle / 2.0f) * M_PI / 180.0f; - float s = sinf(a); - float c = cosf(a); + float s = std::sin(a); + float c = std::cos(a); QVector3D ax = axis.normalized(); return QQuaternion(c, ax.x() * s, ax.y() * s, ax.z() * s).normalized(); } @@ -364,24 +404,353 @@ QQuaternion QQuaternion::fromAxisAndAngle(const QVector3D& axis, float angle) #endif /*! + \since 5.5 + + Extracts a 3D axis (\a x, \a y, \a z) and a rotating angle \a angle (in degrees) + that corresponds to this quaternion. + + \sa fromAxisAndAngle() +*/ +void QQuaternion::getAxisAndAngle(float *x, float *y, float *z, float *angle) const +{ + Q_ASSERT(x && y && z && angle); + + // 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; + if (!qFuzzyIsNull(length)) { + *x = xp; + *y = yp; + *z = zp; + if (!qFuzzyIsNull(length - 1.0f)) { + length = std::sqrt(length); + *x /= length; + *y /= length; + *z /= length; + } + *angle = 2.0f * std::acos(wp); + } else { + // angle is 0 (mod 2*pi), so any axis will fit + *x = *y = *z = *angle = 0.0f; + } + + *angle = qRadiansToDegrees(*angle); +} + +/*! Creates a normalized quaternion that corresponds to rotating through \a angle degrees about the 3D axis (\a x, \a y, \a z). + + \sa getAxisAndAngle() */ QQuaternion QQuaternion::fromAxisAndAngle (float x, float y, float z, float angle) { - float length = qSqrt(x * x + y * y + z * z); + float length = std::sqrt(x * x + y * y + z * z); if (!qFuzzyIsNull(length - 1.0f) && !qFuzzyIsNull(length)) { x /= length; y /= length; z /= length; } float a = (angle / 2.0f) * M_PI / 180.0f; - float s = sinf(a); - float c = cosf(a); + float s = std::sin(a); + float c = std::cos(a); return QQuaternion(c, x * s, y * s, z * s).normalized(); } +#ifndef QT_NO_VECTOR3D + +/*! + \fn QVector3D QQuaternion::toEulerAngles() const + \since 5.5 + \overload + + Calculates roll, pitch, and yaw Euler angles (in degrees) + that corresponds to this quaternion. + + \sa fromEulerAngles() +*/ + +/*! + \fn QQuaternion QQuaternion::fromEulerAngles(const QVector3D &eulerAngles) + \since 5.5 + \overload + + Creates a quaternion that corresponds to a rotation of \a eulerAngles: + eulerAngles.z() degrees around the z axis, eulerAngles.x() degrees around the x axis, + and eulerAngles.y() degrees around the y axis (in that order). + + \sa toEulerAngles() +*/ + +#endif // QT_NO_VECTOR3D + +/*! + \since 5.5 + + Calculates \a roll, \a pitch, and \a yaw Euler angles (in degrees) + that corresponds to this quaternion. + + \sa fromEulerAngles() +*/ +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)); + } + } else { + // not a unique solution + *roll = 0.0f; + *yaw = std::atan2(-2.0f * (xy - zw), 1.0f - 2.0f * (yy + zz)); + } + + *pitch = qRadiansToDegrees(*pitch); + *yaw = qRadiansToDegrees(*yaw); + *roll = qRadiansToDegrees(*roll); +} + +/*! + \since 5.5 + + Creates a quaternion that corresponds to a rotation of + \a roll degrees around the z axis, \a pitch degrees around the x axis, + and \a yaw degrees around the y axis (in that order). + + \sa getEulerAngles() +*/ +QQuaternion QQuaternion::fromEulerAngles(float pitch, float yaw, float roll) +{ + // Algorithm from: + // http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q60 + + pitch = qDegreesToRadians(pitch); + yaw = qDegreesToRadians(yaw); + roll = qDegreesToRadians(roll); + + pitch *= 0.5f; + yaw *= 0.5f; + roll *= 0.5f; + + const float c1 = std::cos(yaw); + const float s1 = std::sin(yaw); + const float c2 = std::cos(roll); + const float s2 = std::sin(roll); + const float c3 = std::cos(pitch); + const float s3 = std::sin(pitch); + const float c1c2 = c1 * c2; + const float s1s2 = s1 * s2; + + const float w = c1c2 * c3 + s1s2 * s3; + const float x = c1c2 * s3 + s1s2 * c3; + const float y = s1 * c2 * c3 - c1 * s2 * s3; + const float z = c1 * s2 * c3 - s1 * c2 * s3; + + return QQuaternion(w, x, y, z); +} + +/*! + \since 5.5 + + Creates a rotation matrix that corresponds to this quaternion. + + \note If this quaternion is not normalized, + the resulting rotation matrix will contain scaling information. + + \sa fromRotationMatrix(), getAxes() +*/ +QMatrix3x3 QQuaternion::toRotationMatrix() const +{ + // Algorithm from: + // http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q54 + + QMatrix3x3 rot3x3(Qt::Uninitialized); + + const float f2x = xp + xp; + const float f2y = yp + yp; + const float f2z = zp + zp; + const float f2xw = f2x * wp; + const float f2yw = f2y * wp; + const float f2zw = f2z * wp; + const float f2xx = f2x * xp; + const float f2xy = f2x * yp; + const float f2xz = f2x * zp; + const float f2yy = f2y * yp; + const float f2yz = f2y * zp; + const float f2zz = f2z * zp; + + rot3x3(0, 0) = 1.0f - (f2yy + f2zz); + rot3x3(0, 1) = f2xy - f2zw; + rot3x3(0, 2) = f2xz + f2yw; + rot3x3(1, 0) = f2xy + f2zw; + rot3x3(1, 1) = 1.0f - (f2xx + f2zz); + rot3x3(1, 2) = f2yz - f2xw; + rot3x3(2, 0) = f2xz - f2yw; + rot3x3(2, 1) = f2yz + f2xw; + rot3x3(2, 2) = 1.0f - (f2xx + f2yy); + + return rot3x3; +} + +/*! + \since 5.5 + + Creates a quaternion that corresponds to a rotation matrix \a rot3x3. + + \note If a given rotation matrix is not normalized, + the resulting quaternion will contain scaling information. + + \sa toRotationMatrix(), fromAxes() +*/ +QQuaternion QQuaternion::fromRotationMatrix(const QMatrix3x3 &rot3x3) +{ + // Algorithm from: + // http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q55 + + float scalar; + float axis[3]; + + const float trace = rot3x3(0, 0) + rot3x3(1, 1) + rot3x3(2, 2); + if (trace > 0.00000001f) { + const float s = 2.0f * std::sqrt(trace + 1.0f); + scalar = 0.25f * s; + axis[0] = (rot3x3(2, 1) - rot3x3(1, 2)) / s; + axis[1] = (rot3x3(0, 2) - rot3x3(2, 0)) / s; + axis[2] = (rot3x3(1, 0) - rot3x3(0, 1)) / s; + } else { + static int s_next[3] = { 1, 2, 0 }; + int i = 0; + if (rot3x3(1, 1) > rot3x3(0, 0)) + i = 1; + if (rot3x3(2, 2) > rot3x3(i, i)) + i = 2; + int j = s_next[i]; + int k = s_next[j]; + + const float s = 2.0f * std::sqrt(rot3x3(i, i) - rot3x3(j, j) - rot3x3(k, k) + 1.0f); + axis[i] = 0.25f * s; + scalar = (rot3x3(k, j) - rot3x3(j, k)) / s; + axis[j] = (rot3x3(j, i) + rot3x3(i, j)) / s; + axis[k] = (rot3x3(k, i) + rot3x3(i, k)) / s; + } + + return QQuaternion(scalar, axis[0], axis[1], axis[2]); +} + +#ifndef QT_NO_VECTOR3D + +/*! + \since 5.5 + + Returns the 3 orthonormal axes (\a xAxis, \a yAxis, \a zAxis) defining the quaternion. + + \sa fromAxes(), toRotationMatrix() +*/ +void QQuaternion::getAxes(QVector3D *xAxis, QVector3D *yAxis, QVector3D *zAxis) const +{ + Q_ASSERT(xAxis && yAxis && zAxis); + + const QMatrix3x3 rot3x3(toRotationMatrix()); + + *xAxis = QVector3D(rot3x3(0, 0), rot3x3(1, 0), rot3x3(2, 0)); + *yAxis = QVector3D(rot3x3(0, 1), rot3x3(1, 1), rot3x3(2, 1)); + *zAxis = QVector3D(rot3x3(0, 2), rot3x3(1, 2), rot3x3(2, 2)); +} + +/*! + \since 5.5 + + Constructs the quaternion using 3 axes (\a xAxis, \a yAxis, \a zAxis). + + \note The axes are assumed to be orthonormal. + + \sa getAxes(), fromRotationMatrix() +*/ +QQuaternion QQuaternion::fromAxes(const QVector3D &xAxis, const QVector3D &yAxis, const QVector3D &zAxis) +{ + QMatrix3x3 rot3x3(Qt::Uninitialized); + rot3x3(0, 0) = xAxis.x(); + rot3x3(1, 0) = xAxis.y(); + rot3x3(2, 0) = xAxis.z(); + rot3x3(0, 1) = yAxis.x(); + rot3x3(1, 1) = yAxis.y(); + rot3x3(2, 1) = yAxis.z(); + rot3x3(0, 2) = zAxis.x(); + rot3x3(1, 2) = zAxis.y(); + rot3x3(2, 2) = zAxis.z(); + + return QQuaternion::fromRotationMatrix(rot3x3); +} + +/*! + \since 5.5 + + Returns the shortest arc quaternion to rotate from the direction described by the vector \a from + to the direction described by the vector \a to. +*/ +QQuaternion QQuaternion::rotationTo(const QVector3D &from, const QVector3D &to) +{ + // Based on Stan Melax's article in Game Programming Gems + + const QVector3D v0(from.normalized()); + const QVector3D v1(to.normalized()); + + float d = QVector3D::dotProduct(v0, v1) + 1.0f; + + // if dest vector is close to the inverse of source vector, ANY axis of rotation is valid + if (qFuzzyIsNull(d)) { + QVector3D axis = QVector3D::crossProduct(QVector3D(1.0f, 0.0f, 0.0f), v0); + if (qFuzzyIsNull(axis.lengthSquared())) + axis = QVector3D::crossProduct(QVector3D(0.0f, 1.0f, 0.0f), v0); + axis.normalize(); + + // same as QQuaternion::fromAxisAndAngle(axis, 180.0f) + return QQuaternion(0.0f, axis.x(), axis.y(), axis.z()); + } + + d = std::sqrt(2.0f * d); + const QVector3D axis(QVector3D::crossProduct(v0, v1) / d); + + return QQuaternion(d * 0.5f, axis).normalized(); +} + +#endif // QT_NO_VECTOR3D + /*! \fn bool operator==(const QQuaternion &q1, const QQuaternion &q2) \relates QQuaternion @@ -470,6 +839,18 @@ QQuaternion QQuaternion::fromAxisAndAngle \sa QQuaternion::operator/=() */ +#ifndef QT_NO_VECTOR3D + +/*! + \fn QVector3D operator*(const QQuaternion &quaternion, const QVector3D &vec) + \since 5.5 + \relates QQuaternion + + Rotates a vector \a vec with a quaternion \a quaternion to produce a new vector in 3D space. +*/ + +#endif + /*! \fn bool qFuzzyCompare(const QQuaternion& q1, const QQuaternion& q2) \relates QQuaternion @@ -499,13 +880,10 @@ QQuaternion QQuaternion::slerp return q2; // Determine the angle between the two quaternions. - QQuaternion q2b; - float dot; - dot = q1.xp * q2.xp + q1.yp * q2.yp + q1.zp * q2.zp + q1.wp * q2.wp; - if (dot >= 0.0f) { - q2b = q2; - } else { - q2b = -q2; + QQuaternion q2b(q2); + float dot = QQuaternion::dotProduct(q1, q2); + if (dot < 0.0f) { + q2b = -q2b; dot = -dot; } @@ -514,11 +892,11 @@ QQuaternion QQuaternion::slerp float factor1 = 1.0f - t; float factor2 = t; if ((1.0f - dot) > 0.0000001) { - float angle = acosf(dot); - float sinOfAngle = sinf(angle); + float angle = std::acos(dot); + float sinOfAngle = std::sin(angle); if (sinOfAngle > 0.0000001) { - factor1 = sinf((1.0f - t) * angle) / sinOfAngle; - factor2 = sinf(t * angle) / sinOfAngle; + factor1 = std::sin((1.0f - t) * angle) / sinOfAngle; + factor2 = std::sin(t * angle) / sinOfAngle; } } @@ -551,13 +929,10 @@ QQuaternion QQuaternion::nlerp return q2; // Determine the angle between the two quaternions. - QQuaternion q2b; - float dot; - dot = q1.xp * q2.xp + q1.yp * q2.yp + q1.zp * q2.zp + q1.wp * q2.wp; - if (dot >= 0.0f) - q2b = q2; - else - q2b = -q2; + QQuaternion q2b(q2); + float dot = QQuaternion::dotProduct(q1, q2); + if (dot < 0.0f) + q2b = -q2b; // Perform the linear interpolation. return (q1 * (1.0f - t) + q2b * t).normalized(); @@ -575,10 +950,11 @@ QQuaternion::operator QVariant() const QDebug operator<<(QDebug dbg, const QQuaternion &q) { + QDebugStateSaver saver(dbg); dbg.nospace() << "QQuaternion(scalar:" << q.scalar() << ", vector:(" << q.x() << ", " << q.y() << ", " << q.z() << "))"; - return dbg.space(); + return dbg; } #endif |