Added matrix decomposition and construction of quaternion from a rotation matrix.

Engine/Math/Matrix4.cpp

@@ -36,6 +36,27 @@ const Matrix4 Matrix4::sIdentity(
     0.0f, 0.0f, 1.0f, 0.0f,
     0.0f, 0.0f, 0.0f, 1.0f);
 
+void Matrix4::getDecomposition(Vector3& translation, Quaternion& rotation, Vector3& scale) const
+{
+    translation.mX = m03;
+    translation.mY = m13;
+    translation.mZ = m23;
+    
+    Vector3 row1(m00, m10, m20);
+    Vector3 row2(m01, m11, m21);
+    Vector3 row3(m02, m12, m22);
+    
+    scale.mX = row1.getLength();
+    scale.mY = row2.getLength();
+    scale.mZ = row3.getLength();
+    
+    // Remove scaling from the 3x3 matrix to get rotation
+    row1 /= scale.mX;
+    row2 /= scale.mY;
+    row3 /= scale.mZ;
+    rotation.fromRotationMatrix(Matrix3(row1.mX, row2.mX, row3.mX, row1.mY, row2.mY, row3.mY, row1.mZ, row2.mZ, row3.mZ));
+}
+
 Matrix4 Matrix4::getInverse() const
 {
     float v0 = m20 * m31 - m21 * m30;
@@ -92,4 +113,3 @@ Matrix4 Matrix4::getInverse() const
         i20, i21, i22, i23,
         i30, i31, i32, i33);
 }
-

Engine/Math/Matrix4.h

@@ -348,16 +348,6 @@ public:
         );
     }
     
-    //! Return the scaling elements
-    Vector3 getScale() const
-    {
-        return Vector3(
-            m00,
-            m11,
-            m22
-        );
-    }
-    
     //! Return transpose
     Matrix4 getTranspose() const
     {
@@ -381,6 +371,9 @@ public:
         );
     }
     
+    //! Return decomposition to translation, rotation and scale
+    void getDecomposition(Vector3& translation, Quaternion& rotation, Vector3& scale) const;
+    
     //! Return inverse
     Matrix4 getInverse() const;
     

Engine/Math/Matrix4x3.cpp

@@ -39,21 +39,11 @@ Matrix4x3::Matrix4x3(const Vector3& translation, const Quaternion& rotation, flo
     define(translation, rotation, scale);
 }
 
-Matrix4x3::Matrix4x3(const Vector3& translation, const Matrix3& rotation, float scale)
-{
-    define(translation, rotation, scale);
-}
-
 Matrix4x3::Matrix4x3(const Vector3& translation, const Quaternion& rotation, const Vector3& scale)
 {
     define(translation, rotation, scale);
 }
 
-Matrix4x3::Matrix4x3(const Vector3& translation, const Matrix3& rotation, const Vector3& scale)
-{
-    define(translation, rotation, scale);
-}
-
 void Matrix4x3::define(const Vector3& translation, const Quaternion& rotation, float scale)
 {
     Matrix3 scaleMatrix(Matrix3::sIdentity);
@@ -63,15 +53,6 @@ void Matrix4x3::define(const Vector3& translation, const Quaternion& rotation, f
     setTranslation(translation);
 }
 
-void Matrix4x3::define(const Vector3& translation, const Matrix3& rotation, float scale)
-{
-    Matrix3 scaleMatrix(Matrix3::sIdentity);
-    scaleMatrix.setScale(scale);
-    
-    *this = rotation * scaleMatrix;
-    setTranslation(translation);
-}
-
 void Matrix4x3::define(const Vector3& translation, const Quaternion& rotation, const Vector3& scale)
 {
     Matrix3 scaleMatrix(Matrix3::sIdentity);
@@ -81,13 +62,25 @@ void Matrix4x3::define(const Vector3& translation, const Quaternion& rotation, c
     setTranslation(translation);
 }
 
-void Matrix4x3::define(const Vector3& translation, const Matrix3& rotation, const Vector3& scale)
+void Matrix4x3::getDecomposition(Vector3& translation, Quaternion& rotation, Vector3& scale) const
 {
-    Matrix3 scaleMatrix(Matrix3::sIdentity);
-    scaleMatrix.setScale(scale);
+    translation.mX = m03;
+    translation.mY = m13;
+    translation.mZ = m23;
     
-    *this = rotation * scaleMatrix;
-    setTranslation(translation);
+    Vector3 row1(m00, m10, m20);
+    Vector3 row2(m01, m11, m21);
+    Vector3 row3(m02, m12, m22);
+    
+    scale.mX = row1.getLength();
+    scale.mY = row2.getLength();
+    scale.mZ = row3.getLength();
+    
+    // Remove scaling from the 3x3 matrix to get rotation
+    row1 /= scale.mX;
+    row2 /= scale.mY;
+    row3 /= scale.mZ;
+    rotation.fromRotationMatrix(Matrix3(row1.mX, row2.mX, row3.mX, row1.mY, row2.mY, row3.mY, row1.mZ, row2.mZ, row3.mZ));
 }
 
 Matrix4x3 Matrix4x3::getInverse() const

Engine/Math/Matrix4x3.h

@@ -107,12 +107,8 @@ public:
     
     //! Construct from translation, rotation and uniform scale
     Matrix4x3(const Vector3& translation, const Quaternion& rotation, float scale);
-    //! Construct from translation, rotation matrix and uniform scale
-    Matrix4x3(const Vector3& translation, const Matrix3& rotation, float scale);
     //! Construct from translation, rotation and nonuniform scale
     Matrix4x3(const Vector3& translation, const Quaternion& rotation, const Vector3& scale);
-    //! Construct from translation, rotation matrix and nonuniform scale
-    Matrix4x3(const Vector3& translation, const Matrix3& rotation, const Vector3& scale);
     
     //! Assign from another matrix
     Matrix4x3& operator = (const Matrix4x3& rhs)
@@ -311,12 +307,8 @@ public:
     
     //! Define from translation, rotation and uniform scale
     void define(const Vector3& translation, const Quaternion& rotation, float scale);
-    //! Define from translation, rotation matrix and uniform scale
-    void define(const Vector3& translation, const Matrix3& rotation, float scale);
     //! Define from translation, rotation and nonuniform scale
     void define(const Vector3& translation, const Quaternion& rotation, const Vector3& scale);
-    //! Define from translation, rotation matrix and nonuniform scale
-    void define(const Vector3& translation, const Matrix3& rotation, const Vector3& scale);
     
     //! Return the rotation matrix
     Matrix3 getRotationMatrix() const
@@ -344,15 +336,8 @@ public:
         );
     }
     
-    //! Return the scaling elements
-    Vector3 getScale() const
-    {
-        return Vector3(
-            m00,
-            m11,
-            m22
-        );
-    }
+    //! Return decomposition to translation, rotation and scale
+    void getDecomposition(Vector3& translation, Quaternion& rotation, Vector3& scale) const;
     
     //! Return inverse
     Matrix4x3 getInverse() const;

Engine/Math/Quaternion.cpp

@@ -46,6 +46,11 @@ Quaternion::Quaternion(const Vector3& start, const Vector3& end)
     fromRotationTo(start, end);
 }
 
+Quaternion::Quaternion(const Matrix3& matrix)
+{
+    fromRotationMatrix(matrix);
+}
+
 void Quaternion::fromAngleAxis(float angle, const Vector3& axis)
 {
     Vector3 normAxis = axis.getNormalized();
@@ -90,7 +95,7 @@ void Quaternion::fromRotationTo(const Vector3& start, const Vector3& end)
         mX = c.mX * invS;
         mY = c.mY * invS;
         mZ = c.mZ * invS;
-        mW = s * 0.5f;
+        mW = 0.5f * s;
     }
     else
     {
@@ -102,10 +107,58 @@ void Quaternion::fromRotationTo(const Vector3& start, const Vector3& end)
     }
 }
 
+void Quaternion::fromRotationMatrix(const Matrix3& matrix)
+{
+    float t = matrix.m00 + matrix.m11 + matrix.m22 + 1.0f;
+    if (t > 0.0f)
+    {
+        float s = sqrtf(t) * 2.0f;
+        float invS = 1.0f / s;
+        
+        mX = (matrix.m21 - matrix.m12) * invS;
+        mY = (matrix.m02 - matrix.m20) * invS;
+        mZ = (matrix.m10 - matrix.m01) * invS;
+        mW = 0.25f * s;
+    }
+    else
+    {
+        if ((matrix.m00 > matrix.m11) && (matrix.m00 > matrix.m22))
+        {
+            float s = sqrtf(1.0f + matrix.m00 - matrix.m11 - matrix.m22) * 2.0f;
+            float invS = 1.0f / s;
+            
+            mX = 0.25f * s;
+            mY = (matrix.m01 + matrix.m10) * invS;
+            mZ = (matrix.m20 + matrix.m02) * invS;
+            mW = (matrix.m21 - matrix.m12) * invS;
+        }
+        else if (matrix.m11 > matrix.m22)
+        {
+            float s = sqrtf(1.0f + matrix.m11 - matrix.m00 - matrix.m22) * 2.0f;
+            float invS = 1.0f / s;
+            
+            mX = (matrix.m01 + matrix.m10) * invS;
+            mY = 0.25f * s;
+            mZ = (matrix.m12 + matrix.m21) * invS;
+            mW = (matrix.m02 - matrix.m20) * invS;
+        }
+        else
+        {
+            float s = sqrtf(1.0f + matrix.m22 - matrix.m00 - matrix.m11) * 2.0f;
+            float invS = 1.0f / s;
+            
+            mX = (matrix.m02 + matrix.m20) * invS;
+            mY = (matrix.m12 + matrix.m21) * invS;
+            mZ = 0.25f * s;
+            mW = (matrix.m10 - matrix.m01) * invS;
+        }
+    }
+}
+
 Vector3 Quaternion::getEulerAngles() const
 {
     // Derivation from http://www.geometrictools.com/Documentation/EulerAngles.pdf
-    
+    // Order of rotations: Z first, then X, then Y
     float check = 2.0f * (-mY * mZ + mW * mX);
     
     if (check < -0.995f)

Engine/Math/Quaternion.h

@@ -74,6 +74,8 @@ public:
     Quaternion(float angleX, float angleY, float angleZ);
     //! Construct from the rotation difference between two vectors
     Quaternion(const Vector3& start, const Vector3& end);
+    //! Construct from a rotation matrix
+    Quaternion(const Matrix3& matrix);
     
     //! Assign from another quaternion
     Quaternion& operator = (const Quaternion& rhs)
@@ -185,6 +187,8 @@ public:
     void fromEulerAngles(const Vector3& euler);
     //! Set from the rotation difference between two vectors
     void fromRotationTo(const Vector3& start, const Vector3& end);
+    //! Set from a rotation matrix
+    void fromRotationMatrix(const Matrix3& matrix);
     
     //! Return normalized to unit length
     Quaternion getNormalized() const