[QQuaternion] Introduce to/from rotation matrix conversion routines

Change-Id: Ic19824e7e135f53a9ce3f2ea7ecd078a589e7425
Reviewed-by: Friedemann Kleint <Friedemann.Kleint@theqtcompany.com>
Reviewed-by: Sean Harmer <sean.harmer@kdab.com>

src/gui/math3d/qquaternion.cpp

@@ -400,6 +400,91 @@ QQuaternion QQuaternion::fromAxisAndAngle
     return QQuaternion(c, x * s, y * s, z * s).normalized();
 }
 
+/*!
+    \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()
+*/
+QMatrix3x3 QQuaternion::toRotationMatrix() const
+{
+    // Algorithm from:
+    // http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q54
+
+    QMatrix3x3 rot3x3(Qt::Uninitialized);
+
+    const float xx = xp * xp;
+    const float xy = xp * yp;
+    const float xz = xp * zp;
+    const float xw = xp * wp;
+    const float yy = yp * yp;
+    const float yz = yp * zp;
+    const float yw = yp * wp;
+    const float zz = zp * zp;
+    const float zw = zp * wp;
+
+    rot3x3(0, 0) = 1.0f - 2.0f * (yy + zz);
+    rot3x3(0, 1) =        2.0f * (xy - zw);
+    rot3x3(0, 2) =        2.0f * (xz + yw);
+    rot3x3(1, 0) =        2.0f * (xy + zw);
+    rot3x3(1, 1) = 1.0f - 2.0f * (xx + zz);
+    rot3x3(1, 2) =        2.0f * (yz - xw);
+    rot3x3(2, 0) =        2.0f * (xz - yw);
+    rot3x3(2, 1) =        2.0f * (yz + xw);
+    rot3x3(2, 2) = 1.0f - 2.0f * (xx + yy);
+
+    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()
+*/
+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 * sqrtf(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 * sqrtf(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]);
+}
+
 /*!
     \fn bool operator==(const QQuaternion &q1, const QQuaternion &q2)
     \relates QQuaternion

src/gui/math3d/qquaternion.h

@@ -34,6 +34,7 @@
 #ifndef QQUATERNION_H
 #define QQUATERNION_H
 
+#include <QtGui/qgenericmatrix.h>
 #include <QtGui/qvector3d.h>
 #include <QtGui/qvector4d.h>
 
@@ -119,6 +120,9 @@ public:
     static QQuaternion fromAxisAndAngle
             (float x, float y, float z, float angle);
 
+    QMatrix3x3 toRotationMatrix() const;
+    static QQuaternion fromRotationMatrix(const QMatrix3x3 &rot3x3);
+
     static QQuaternion slerp
         (const QQuaternion& q1, const QQuaternion& q2, float t);
     static QQuaternion nlerp

tests/auto/gui/math3d/qquaternion/tst_qquaternion.cpp

@@ -83,6 +83,9 @@ private slots:
     void fromAxisAndAngle_data();
     void fromAxisAndAngle();
 
+    void fromRotationMatrix_data();
+    void fromRotationMatrix();
+
     void slerp_data();
     void slerp();
 
@@ -725,6 +728,25 @@ void tst_QQuaternion::fromAxisAndAngle()
     QVERIFY(qFuzzyCompare(answer.scalar(), result.scalar()));
 }
 
+// Test quaternion convertion to and from rotation matrix.
+void tst_QQuaternion::fromRotationMatrix_data()
+{
+    fromAxisAndAngle_data();
+}
+void tst_QQuaternion::fromRotationMatrix()
+{
+    QFETCH(float, x1);
+    QFETCH(float, y1);
+    QFETCH(float, z1);
+    QFETCH(float, angle);
+
+    QQuaternion result = QQuaternion::fromAxisAndAngle(QVector3D(x1, y1, z1), angle);
+    QMatrix3x3 rot3x3 = result.toRotationMatrix();
+    QQuaternion answer = QQuaternion::fromRotationMatrix(rot3x3);
+
+    QVERIFY(qFuzzyCompare(answer, result) || qFuzzyCompare(-answer, result));
+}
+
 // Test spherical interpolation of quaternions.
 void tst_QQuaternion::slerp_data()
 {