Implement Matrix4x4::Equals

src/J3ML/LinearAlgebra/Matrix4x4.cpp

@@ -242,9 +242,6 @@ namespace J3ML::LinearAlgebra {
                          0, 0, 0,     1.f);
     }
 
-    Matrix4x4 Matrix4x4::Translate(const Vector3 &rhs) const {
-        return *this * FromTranslation(rhs);
-    }
 
     Vector3 Matrix4x4::Transform(const Vector3 &rhs) const {
         return Transform(rhs.x, rhs.y, rhs.z);
@@ -424,15 +421,7 @@ namespace J3ML::LinearAlgebra {
         return GetColumn3(3);
     }
 
-    Matrix4x4 Matrix4x4::Scale(const Vector3& scale)
-    {
-        auto mat = *this;
 
-        mat.At(3, 0) *= scale.x;
-        mat.At(3, 1) *= scale.y;
-        mat.At(3, 2) *= scale.z;
-        return mat;
-    }
 
     Matrix4x4
     Matrix4x4::LookAt(const Vector3 &localFwd, const Vector3 &targetDir, const Vector3 &localUp, const Vector3 &worldUp) {
@@ -476,6 +465,7 @@ namespace J3ML::LinearAlgebra {
     }
 
     void Matrix4x4::Pivot() {
+        /// Algorithm from Eric Lengyel's Mathematics for 3D Game Programming & Computer Graphics, 2nd Ed.
         int rowIndex = 0;
 
         for(int col = 0; col < Cols; ++col)
@@ -941,4 +931,32 @@ namespace J3ML::LinearAlgebra {
         return m;
     }
 
+    void Matrix4x4::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 Matrix4x4::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);
+    }
+
+    bool Matrix4x4::Equals(const Matrix4x4 &other, float epsilon) const {
+        for (int iy = 0; iy < Rows; ++iy)
+            for (int ix = 0; ix < Cols; ++ix)
+                if (!Math::EqualAbs(At(iy, ix), other[iy][ix], epsilon))
+                    return false;
+        return true;
+    }
+
 }