Math: ability to create Quaternion and DualQuaternion from matrix.

src/Math/DualQuaternion.h

@@ -79,6 +79,21 @@ template<class T> class DualQuaternion: public Dual<Quaternion<T>> {
             return {{}, {vector/T(2), T(0)}};
         }
 
+        /**
+         * @brief Create dual quaternion from transformation matrix
+         *
+         * Expects that the matrix represents rigid transformation.
+         * @see toMatrix(), Quaternion::fromMatrix(),
+         *      Matrix4::isRigidTransformation()
+         */
+        inline static DualQuaternion<T> fromMatrix(const Matrix4<T>& matrix) {
+            CORRADE_ASSERT(matrix.isRigidTransformation(),
+                "Math::DualQuaternion::fromMatrix(): the matrix doesn't represent rigid transformation", {});
+
+            Quaternion<T> q = Implementation::quaternionFromMatrix(matrix.rotationScaling());
+            return {q, Quaternion<T>(matrix.translation()/2)*q};
+        }
+
         /**
          * @brief Default constructor
          *
@@ -158,7 +173,7 @@ template<class T> class DualQuaternion: public Dual<Quaternion<T>> {
         /**
          * @brief Convert dual quaternion to transformation matrix
          *
-         * @see Quaternion::toMatrix()
+         * @see fromMatrix(), Quaternion::toMatrix()
          */
         Matrix4<T> toMatrix() const {
             return Matrix4<T>::from(this->real().toMatrix(), translation());

src/Math/Quaternion.h

@@ -38,6 +38,45 @@
 
 namespace Magnum { namespace Math {
 
+#ifndef DOXYGEN_GENERATING_OUTPUT
+namespace Implementation {
+
+/* No assertions fired, for internal use */
+template<class T> inline Quaternion<T> quaternionFromMatrix(const Matrix<3, T>& m) {
+    const Vector<3, T> diagonal = m.diagonal();
+    const T trace = diagonal.sum();
+
+    /* Diagonal is positive */
+    if(trace > T(0)) {
+        const T s = std::sqrt(trace + T(1));
+        const T t = T(0.5)/s;
+        return {Vector3<T>(m[1][2] - m[2][1],
+                           m[2][0] - m[0][2],
+                           m[0][1] - m[1][0])*t, s*T(0.5)};
+    }
+
+    /* Diagonal is negative */
+    std::size_t i = 0;
+    if(diagonal[1] > diagonal[0]) i = 1;
+    if(diagonal[2] > diagonal[i]) i = 2;
+
+    const std::size_t j = (i + 1) % 3;
+    const std::size_t k = (i + 2) % 3;
+
+    const T s = std::sqrt(diagonal[i] - diagonal[j] - diagonal[k] + T(1));
+    const T t = (s == T(0) ? T(0) : T(0.5)/s);
+
+    Vector3<T> vec;
+    vec[i] = s*T(0.5);
+    vec[j] = (m[i][j] + m[j][i])*t;
+    vec[k] = (m[i][k] + m[k][i])*t;
+
+    return {vec, (m[j][k] - m[k][j])*t};
+}
+
+}
+#endif
+
 /**
 @brief %Quaternion
 @tparam T   Underlying data type
@@ -134,6 +173,18 @@ template<class T> class Quaternion {
             return {normalizedAxis*std::sin(T(angle)/2), std::cos(T(angle)/2)};
         }
 
+        /**
+         * @brief Create quaternion from rotation matrix
+         *
+         * Expects that the matrix is orthogonal (i.e. pure rotation).
+         * @see toMatrix(), DualComplex::fromMatrix(), Matrix::isOrthogonal()
+         */
+        inline static Quaternion<T> fromMatrix(const Matrix<3, T>& matrix) {
+            CORRADE_ASSERT(matrix.isOrthogonal(),
+                "Math::Quaternion::fromMatrix(): the matrix is not orthogonal", {});
+            return Implementation::quaternionFromMatrix(matrix);
+        }
+
         /**
          * @brief Default constructor
          *
@@ -213,7 +264,8 @@ template<class T> class Quaternion {
         /**
          * @brief Convert quaternion to rotation matrix
          *
-         * @see DualQuaternion::toMatrix(), Matrix4::from(const Matrix<3, T>&, const Vector3<T>&)
+         * @see fromMatrix(), DualQuaternion::toMatrix(),
+         *      Matrix4::from(const Matrix<3, T>&, const Vector3<T>&)
          */
         Matrix<3, T> toMatrix() const {
             return {

src/Math/Test/DualQuaternionTest.cpp

@@ -234,6 +234,14 @@ void DualQuaternionTest::matrix() {
     /* Verify that negated dual quaternion gives the same transformation */
     CORRADE_COMPARE(q.toMatrix(), m);
     CORRADE_COMPARE((-q).toMatrix(), m);
+
+    std::ostringstream o;
+    Error::setOutput(&o);
+    DualQuaternion::fromMatrix(m*2);
+    CORRADE_COMPARE(o.str(), "Math::DualQuaternion::fromMatrix(): the matrix doesn't represent rigid transformation\n");
+
+    DualQuaternion p = DualQuaternion::fromMatrix(m);
+    CORRADE_COMPARE(p, q);
 }
 
 void DualQuaternionTest::transformPoint() {

src/Math/Test/QuaternionTest.cpp

@@ -282,12 +282,29 @@ void QuaternionTest::angle() {
 }
 
 void QuaternionTest::matrix() {
-    Quaternion q = Quaternion::rotation(Deg(37.0f), Vector3(1.0f/Constants<Float>::sqrt3()));
-    Matrix3 m = Matrix4::rotation(Deg(37.0f), Vector3(1.0f/Constants<Float>::sqrt3())).rotationScaling();
+    Vector3 axis = Vector3(1.0f, -3.0f, 5.0f).normalized();
+
+    Quaternion q = Quaternion::rotation(Deg(37.0f), axis);
+    Matrix3 m = Matrix4::rotation(Deg(37.0f), axis).rotationScaling();
 
     /* Verify that negated quaternion gives the same rotation */
     CORRADE_COMPARE(q.toMatrix(), m);
     CORRADE_COMPARE((-q).toMatrix(), m);
+
+    std::ostringstream o;
+    Error::setOutput(&o);
+    Quaternion::fromMatrix(m*2);
+    CORRADE_COMPARE(o.str(), "Math::Quaternion::fromMatrix(): the matrix is not orthogonal\n");
+
+    /* Trace > 0 */
+    CORRADE_VERIFY(m.trace() > 0.0f);
+    CORRADE_COMPARE(Quaternion::fromMatrix(m), q);
+
+    /* Trace < 0 */
+    Matrix3 m2 = Matrix4::rotation(Deg(130.0f), axis).rotationScaling();
+    Quaternion q2 = Quaternion::rotation(Deg(130.0f), axis);
+    CORRADE_VERIFY(m2.trace() < 0.0f);
+    CORRADE_COMPARE(Quaternion::fromMatrix(m2), q2);
 }
 
 void QuaternionTest::lerp() {