From aa8bc4d1c4f2b4ac6531cd04ff62611563daf4e9 Mon Sep 17 00:00:00 2001
From: josh <josh@stepbro.wtf>
Date: Mon, 27 May 2024 13:34:48 -0400
Subject: [PATCH] Implement Matrix4x4::Equals

---
 src/J3ML/LinearAlgebra/Matrix3x3.cpp | 82 ++++++++++++++++++++++++++++
 src/J3ML/LinearAlgebra/Matrix4x4.cpp | 40 ++++++++++----
 2 files changed, 111 insertions(+), 11 deletions(-)

diff --git a/src/J3ML/LinearAlgebra/Matrix3x3.cpp b/src/J3ML/LinearAlgebra/Matrix3x3.cpp
index be4e81f..f9215de 100644
--- a/src/J3ML/LinearAlgebra/Matrix3x3.cpp
+++ b/src/J3ML/LinearAlgebra/Matrix3x3.cpp
@@ -690,6 +690,88 @@ namespace J3ML::LinearAlgebra {
         return true;
     }
 
+    bool Matrix3x3::SolveAxb(Vector3 b, Vector3 &x) const {
+        // Solve by pivotization.
+        float v00 = At(0, 0);
+        float v10 = At(1, 0);
+        float v20 = At(2, 0);
+
+        float v01 = At(0, 1);
+        float v11 = At(1, 1);
+        float v21 = At(2, 1);
+
+        float v02 = At(0, 2);
+        float v12 = At(1, 2);
+        float v22 = At(2, 2);
+
+        float av00 = std::abs(v00);
+        float av10 = std::abs(v10);
+        float av20 = std::abs(v20);
+
+        // Find which item in first column has largest absolute value.
+        if (av10 >= av00 && av10 >= av20) {
+            Swap(v00, v10);
+            Swap(v01, v11);
+            Swap(v02, v12);
+            Swap(b[0], b[1]);
+        } else if (av20 >= av00) {
+            Swap(v00, v20);
+            Swap(v01, v21);
+            Swap(v02, v22);
+            Swap(b[0], b[2]);
+        }
+
+        /* a b c | x
+           d e f | y
+           g h i | z, where |a| >= |d| && |a| >= |g| */
+
+        if (Math::EqualAbs(v00, 0.f))
+            return false;
+
+
+        // Scale row so that leading element is one.
+        float denom = 1.f / v00;
+        v01 *= denom;
+        v02 *= denom;
+        b[0] *= denom;
+
+        /* 1 b c | x
+           d e f | y
+           g h i | z */
+
+
+        // Pivotize again.
+        if (std::abs(v21) >= std::abs(v11))
+        {
+            Swap(v11, v21);
+            Swap(v12, v22);
+            Swap(b[1], b[2]);
+        }
+
+        if (Math::EqualAbs(v11, 0.f))
+            return false;
+
+        /* 1 b c | x
+           0 e f | y
+           0 h i | z, where |e| >= |h| */
+
+        denom = 1.f / v11;
+        // v11 = 1.f;
+        v12 *= denom;
+        b[1] *= denom;
+
+        /* 1 b c | x
+           0 1 f | y
+           0 h i | z */
+
+        if (Math::EqualAbs(v22, 0.f))
+            return false;
+
+        x[2] = b[2] / v22;
+        x[1] = b[1] - x[2] * v12;
+        x[0] = b[0] - x[2] * v02 - x[1] * v01;
+    }
+
 
 }
 
diff --git a/src/J3ML/LinearAlgebra/Matrix4x4.cpp b/src/J3ML/LinearAlgebra/Matrix4x4.cpp
index ba16c6d..9a05fa3 100644
--- a/src/J3ML/LinearAlgebra/Matrix4x4.cpp
+++ b/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;
+    }
+
 }
\ No newline at end of file