Implement Matrix4x4::FromTRS() Scale() Translate()

include/J3ML/LinearAlgebra/Matrix4x4.h

@@ -314,6 +314,20 @@ namespace J3ML::LinearAlgebra {
         /// @note Use the M*v multiplication order to project points with this matrix.
         static Matrix4x4 OpenGLPerspProjRH(float n, float f, float h, float v);
 
+        /// Creates a new transformation matrix that translates by the given offset.
+        static Matrix4x4 Translate(const Vector3& translation);
+        /// Creates a new transformation matrix that scales by the given factors.
+        static Matrix4x4 Scale(const Vector3& scale);
+
+        /// Creates a new Matrix4x4 as a combination of translation, rotation, and scale.
+        /** This function creates a new Matrix4x4 M of the form M = T * R * S, where T is a translation matrix, R is a
+            rotation matrix, and S a scale matrix. Transforming a vector v using this matrix computes the vector
+            v' = M * v = T*R*S*v = (T * (R * (S * v))), which means that the scale operation is applied to the
+            vector first, followed by rotation, and finally translation. */
+        static Matrix4x4 FromTRS(const Vector3& translate, const Quaternion& rotate, const Vector3& scale);
+        static Matrix4x4 FromTRS(const Vector3& translate, const Matrix3x3& rotate, const Vector3& scale);
+        static Matrix4x4 FromTRS(const Vector3& translate, const Matrix4x4& rotate, const Vector3& scale);
+
     public:
         /// Returns the translation part.
         /** The translation part is stored in the fourth column of this matrix.
@@ -507,9 +521,47 @@ namespace J3ML::LinearAlgebra {
         Matrix4x4 &operator = (const Quaternion& rhs);
         Matrix4x4 &operator = (const Matrix4x4& rhs) = default;
 
-
+        /// Returns the scale component of this matrix.
+        /** This function decomposes this matrix M into a form M = M' * S, where M' has the unitary column vectors and S is a diagonal matrix.
+            @return ExtractScale returns the diagonal entries of S, i.e. the scale of the columns of this matrix. If this matrix
+            represents a local->world space transformation for an object, then this scale represents a 'local scale', i.e.
+            scaling that is performed before translating and rotating the object from its local coordinate system to its world
+            position.
+            @note This function assumes that this matrix does not contain projection (the fourth row of this matrix is [0 0 0 1]). */
         Vector3 ExtractScale() const;
 
+
+        /// Decomposes this matrix to translate, rotate, and scale parts.
+        /** This function decomposes this matrix M to a form M = T * R * S, where T is a translation matrix, R is a rotation matrix, and S is a scale matrix
+            @note Remember that in the convention of this class, transforms are applied in the order M * v, so scale is
+            applied first, then rotation, and finally the translation last.
+            @note This function assumes that this matrix does not contain projection (The fourth row of this matrix is [0 0 0 1]).
+            @param translate [out] This vector receives the translation component this matrix performs. The translation is applied last
+            after rotation and scaling.
+            @param rotate [out] This object receives the rotation part of this transform.
+            @param scale [out] This vector receives the scaling along the local (before transformation by R) X, Y, and Z axes performed by this matrix. */
+        void Decompose(Vector3& translate, Quaternion& rotate, Vector3& scale) const
+        {
+            assert(this->IsColOrthogonal3());
+
+            Matrix3x3 r;
+            Decompose(translate, r, scale);
+            rotate = Quaternion(r);
+
+            /// Test that composing back yields the original Matrix4x4.
+            assert(Matrix4x4::FromTRS(translate, rotate, scale).Equals(*this, 0.1f));
+        }
+        void Decompose(Vector3& translate, Matrix3x3& rotate, Vector3& scale) const;
+        void Decompose(Vector3& translate, Matrix4x4& rotate, Vector3& scale) const
+        {
+            assert(this->IsColOrthogonal3());
+
+            Matrix3x3 r;
+            Decompose(translate, r, scale);
+            rotate.SetRotatePart(r);
+            rotate.SetTranslatePart(0,0,0);
+        }
+
         /// Returns true if this matrix only contains uniform scaling, compared to the given epsilon.
         /// @note If the matrix does not really do any scaling, this function returns true (scaling uniformly by a factor of 1).
         /// @note This function only examines the upper 3-by-3 part of this matrix.

src/J3ML/LinearAlgebra/Matrix4x4.cpp

@@ -845,4 +845,57 @@ namespace J3ML::LinearAlgebra {
         return copy;
     }
 
+    void Matrix4x4::Decompose(Vector3 &translate, Matrix3x3 &rotate, Vector3 &scale) const {
+        assert(this->IsColOrthogonal3());
+        assert(Row(3).Equals(0,0,0,1));
+        assert(this->IsColOrthogonal3()); // Duplicate check?
+
+
+        translate = Col3(3);
+        rotate = GetRotatePart();
+        scale.x = rotate.Col3(0).Length();
+        scale.y = rotate.Col3(1).Length();
+        scale.z = rotate.Col3(2).Length();
+        assert(!Math::EqualAbs(scale.x, 0));
+        assert(!Math::EqualAbs(scale.y, 0));
+        assert(!Math::EqualAbs(scale.z, 0));
+        rotate.ScaleCol(0, 1.f / scale.x);
+        rotate.ScaleCol(1, 1.f / scale.y);
+        rotate.ScaleCol(2, 1.f / scale.z);
+
+        // Test that composing back yields the original Matrix4x4
+        assert(Matrix4x4::FromTRS(translate, rotate, scale).Equals(*this, 0.1f));
+
+    }
+
+    Matrix4x4 Matrix4x4::Translate(const Vector3 &translation) {
+        Matrix4x4 m;
+        m.SetRow(0, 1, 0, 0, translation.x);
+        m.SetRow(1, 0, 1, 0, translation.y);
+        m.SetRow(2, 0, 0, 1, translation.y);
+        m.SetRow(3, 0, 0, 0, 1.f);
+        return m;
+    }
+
+    Matrix4x4 Matrix4x4::FromTRS(const Vector3 &translate, const Quaternion &rotate, const Vector3 &scale) {
+        return Matrix4x4::Translate(translate) * Matrix4x4(rotate) * Matrix4x4::Scale(scale);
+    }
+
+    Matrix4x4 Matrix4x4::FromTRS(const Vector3 &translate, const Matrix3x3 &rotate, const Vector3 &scale) {
+        return Matrix4x4::Translate(translate) * Matrix4x4(rotate) * Matrix4x4::Scale(scale);
+    }
+
+    Matrix4x4 Matrix4x4::FromTRS(const Vector3 &translate, const Matrix4x4 &rotate, const Vector3 &scale) {
+        return Matrix4x4::Translate(translate) * Matrix4x4(rotate) * Matrix4x4::Scale(scale);
+    }
+
+    Matrix4x4 Matrix4x4::Scale(const Vector3 &scale) {
+        Matrix4x4 m;
+        m.SetRow(0, scale.x, 0, 0, 0);
+        m.SetRow(1, 0, scale.y, 0, 0);
+        m.SetRow(2, 0, 0, scale.z, 0);
+        m.SetRow(3, 0, 0, 0, 1.f);
+        return m;
+    }
+
 }