[QQuaternion] Introduce to/from euler angles conversion routines

Change-Id: I26c0a9d1ce9258048cf44eed5b5238920c2317b1
Reviewed-by: Laszlo Agocs <laszlo.agocs@theqtcompany.com>

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
 

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)

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<float>("pitch");
+    QTest::addColumn<float>("yaw");
+    QTest::addColumn<float>("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()
 {