From 88142e73d5e062cf26638511a42140b59ef736cc Mon Sep 17 00:00:00 2001 From: Konstantin Ritt Date: Mon, 9 Feb 2015 17:13:20 +0400 Subject: [QQuaternion] Introduce to/from euler angles conversion routines Change-Id: I26c0a9d1ce9258048cf44eed5b5238920c2317b1 Reviewed-by: Laszlo Agocs --- src/gui/math3d/qquaternion.cpp | 125 +++++++++++++++++++++ src/gui/math3d/qquaternion.h | 19 ++++ .../gui/math3d/qquaternion/tst_qquaternion.cpp | 112 ++++++++++++++++++ 3 files changed, 256 insertions(+) diff --git a/src/gui/math3d/qquaternion.cpp b/src/gui/math3d/qquaternion.cpp index 9c8fa3310d..0653ace5a4 100644 --- a/src/gui/math3d/qquaternion.cpp +++ b/src/gui/math3d/qquaternion.cpp @@ -450,6 +450,131 @@ QQuaternion QQuaternion::fromAxisAndAngle return QQuaternion(c, x * s, y * s, z * s).normalized(); } +#ifndef QT_NO_VECTOR3D + +/*! + \fn QVector3D QQuaternion::toEulerAngles() const + \since 5.5 + \overload + + Calculates \a roll, \a pitch, and \a 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.z() degrees around the z axis, \a eulerAngles.x() degrees around the x axis, + and \a 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::toEulerAngles(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 = asinf(-2.0f * (yz - xw)); + if (*pitch < M_PI_2) { + if (*pitch > -M_PI_2) { + *yaw = atan2f(2.0f * (xz + yw), 1.0f - 2.0f * (xx + yy)); + *roll = atan2f(2.0f * (xy + zw), 1.0f - 2.0f * (xx + zz)); + } else { + // not a unique solution + *roll = 0.0f; + *yaw = -atan2f(-2.0f * (xy - zw), 1.0f - 2.0f * (yy + zz)); + } + } else { + // not a unique solution + *roll = 0.0f; + *yaw = atan2f(-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 toEulerAngles() +*/ +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 = cosf(yaw); + const float s1 = sinf(yaw); + const float c2 = cosf(roll); + const float s2 = sinf(roll); + const float c3 = cosf(pitch); + const float s3 = sinf(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 diff --git a/src/gui/math3d/qquaternion.h b/src/gui/math3d/qquaternion.h index eb835ef806..4a87b63d25 100644 --- a/src/gui/math3d/qquaternion.h +++ b/src/gui/math3d/qquaternion.h @@ -122,6 +122,13 @@ public: static QQuaternion fromAxisAndAngle (float x, float y, float z, float angle); +#ifndef QT_NO_VECTOR3D + inline QVector3D toEulerAngles() const; + static inline QQuaternion fromEulerAngles(const QVector3D &eulerAngles); +#endif + void toEulerAngles(float *pitch, float *yaw, float *roll) const; + static QQuaternion fromEulerAngles(float pitch, float yaw, float roll); + QMatrix3x3 toRotationMatrix() const; static QQuaternion fromRotationMatrix(const QMatrix3x3 &rot3x3); @@ -308,6 +315,18 @@ inline void QQuaternion::toAxisAndAngle(QVector3D *axis, float *angle) const *axis = QVector3D(aX, aY, aZ); } +inline QVector3D QQuaternion::toEulerAngles() const +{ + float pitch, yaw, roll; + toEulerAngles(&pitch, &yaw, &roll); + return QVector3D(pitch, yaw, roll); +} + +inline QQuaternion QQuaternion::fromEulerAngles(const QVector3D &eulerAngles) +{ + return QQuaternion::fromEulerAngles(eulerAngles.x(), eulerAngles.y(), eulerAngles.z()); +} + #endif inline void QQuaternion::setVector(float aX, float aY, float aZ) diff --git a/tests/auto/gui/math3d/qquaternion/tst_qquaternion.cpp b/tests/auto/gui/math3d/qquaternion/tst_qquaternion.cpp index 59dcd7a015..444ccdeb88 100644 --- a/tests/auto/gui/math3d/qquaternion/tst_qquaternion.cpp +++ b/tests/auto/gui/math3d/qquaternion/tst_qquaternion.cpp @@ -86,6 +86,9 @@ private slots: void fromRotationMatrix_data(); void fromRotationMatrix(); + void fromEulerAngles_data(); + void fromEulerAngles(); + void slerp_data(); void slerp(); @@ -767,6 +770,115 @@ void tst_QQuaternion::fromRotationMatrix() QVERIFY(qFuzzyCompare(answer, result) || qFuzzyCompare(-answer, result)); } +// This is a more tolerant version of qFuzzyCompare that also handles the case +// where one or more of the values being compare are close to zero +static inline bool myFuzzyCompare(float p1, float p2) +{ + if (qFuzzyIsNull(p1)) + return qFuzzyIsNull(p2); + if (qFuzzyIsNull(p2)) + return false; + // a very slightly looser version of qFuzzyCompare + // for use with values that are not very close to zero + return qAbs(p1 - p2) <= 0.00003f * qMin(qAbs(p1), qAbs(p2)); +} + +static inline bool myFuzzyCompareRadians(float p1, float p2) +{ + static const float fPI = float(M_PI); + if (p1 < -fPI) + p1 += 2.0f * fPI; + else if (p1 > fPI) + p1 -= 2.0f * fPI; + + if (p2 < -fPI) + p2 += 2.0f * fPI; + else if (p2 > fPI) + p2 -= 2.0f * fPI; + + return qAbs(qAbs(p1) - qAbs(p2)) <= qDegreesToRadians(0.05f); +} + +static inline bool myFuzzyCompareDegrees(float p1, float p2) +{ + p1 = qDegreesToRadians(p1); + p2 = qDegreesToRadians(p2); + return myFuzzyCompareRadians(p1, p2); +} + +// Test quaternion creation from an axis and an angle. +void tst_QQuaternion::fromEulerAngles_data() +{ + QTest::addColumn("pitch"); + QTest::addColumn("yaw"); + QTest::addColumn("roll"); + + QTest::newRow("null") + << 0.0f << 0.0f << 0.0f; + + QTest::newRow("xonly") + << 90.0f << 0.0f << 0.0f; + + QTest::newRow("yonly") + << 0.0f << 180.0f << 0.0f; + + QTest::newRow("zonly") + << 0.0f << 0.0f << 270.0f; + + QTest::newRow("x+z") + << 30.0f << 0.0f << 45.0f; + + QTest::newRow("x+y") + << 30.0f << 90.0f << 0.0f; + + QTest::newRow("y+z") + << 0.0f << 45.0f << 30.0f; + + QTest::newRow("complex") + << 30.0f << 240.0f << -45.0f; +} +void tst_QQuaternion::fromEulerAngles() +{ + QFETCH(float, pitch); + QFETCH(float, yaw); + QFETCH(float, roll); + + // Use a straight-forward implementation of the algorithm at: + // http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q60 + // to calculate the answer we expect to get. + QQuaternion qx = QQuaternion::fromAxisAndAngle(QVector3D(1, 0, 0), pitch); + QQuaternion qy = QQuaternion::fromAxisAndAngle(QVector3D(0, 1, 0), yaw); + QQuaternion qz = QQuaternion::fromAxisAndAngle(QVector3D(0, 0, 1), roll); + QQuaternion result = qy * (qx * qz); + QQuaternion answer = QQuaternion::fromEulerAngles(QVector3D(pitch, yaw, roll)); + + QVERIFY(myFuzzyCompare(answer.x(), result.x())); + QVERIFY(myFuzzyCompare(answer.y(), result.y())); + QVERIFY(myFuzzyCompare(answer.z(), result.z())); + QVERIFY(myFuzzyCompare(answer.scalar(), result.scalar())); + + { + QVector3D answerEulerAngles = answer.toEulerAngles(); + QVERIFY(myFuzzyCompareDegrees(answerEulerAngles.x(), pitch)); + QVERIFY(myFuzzyCompareDegrees(answerEulerAngles.y(), yaw)); + QVERIFY(myFuzzyCompareDegrees(answerEulerAngles.z(), roll)); + } + + answer = QQuaternion::fromEulerAngles(pitch, yaw, roll); + QVERIFY(myFuzzyCompare(answer.x(), result.x())); + QVERIFY(myFuzzyCompare(answer.y(), result.y())); + QVERIFY(myFuzzyCompare(answer.z(), result.z())); + QVERIFY(myFuzzyCompare(answer.scalar(), result.scalar())); + + { + float answerPitch, answerYaw, answerRoll; + answer.toEulerAngles(&answerPitch, &answerYaw, &answerRoll); + QVERIFY(myFuzzyCompareDegrees(answerPitch, pitch)); + QVERIFY(myFuzzyCompareDegrees(answerYaw, yaw)); + QVERIFY(myFuzzyCompareDegrees(answerRoll, roll)); + } +} + // Test spherical interpolation of quaternions. void tst_QQuaternion::slerp_data() { -- cgit v1.2.3