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

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;
+    }
+
 }