Implement generic matrix Inverse, LUDecompose, CholeskyDecompose

src/J3ML/LinearAlgebra/Matrix4x4.cpp

@@ -706,4 +706,61 @@ namespace J3ML::LinearAlgebra {
         SetCol(column, columnVector.x, columnVector.y, columnVector.z, columnVector.w);
     }
 
+    void Matrix4x4::Transpose() {
+        Swap(elems[0][1], elems[1][0]);
+        Swap(elems[0][2], elems[2][0]);
+        Swap(elems[0][3], elems[3][0]);
+
+        Swap(elems[1][2], elems[2][1]);
+        Swap(elems[1][3], elems[3][1]);
+        Swap(elems[2][3], elems[3][2]);
+    }
+
+    Matrix4x4 Matrix4x4::Transposed() const {
+        Matrix4x4 copy;
+        copy.elems[0][0] = elems[0][0]; copy.elems[0][1] = elems[1][0]; copy.elems[0][2] = elems[2][0]; copy.elems[0][3] = elems[3][0];
+        copy.elems[1][0] = elems[0][1]; copy.elems[1][1] = elems[1][1]; copy.elems[1][2] = elems[2][1]; copy.elems[1][3] = elems[3][1];
+        copy.elems[2][0] = elems[0][2]; copy.elems[2][1] = elems[1][2]; copy.elems[2][2] = elems[2][2]; copy.elems[2][3] = elems[3][2];
+        copy.elems[3][0] = elems[0][3]; copy.elems[3][1] = elems[1][3]; copy.elems[3][2] = elems[2][3]; copy.elems[3][3] = elems[3][3];
+        return copy;
+    }
+
+    bool Matrix4x4::InverseTranspose() {
+        bool success = Inverse();
+        Transpose();
+        return success;
+    }
+
+    float Matrix4x4::Trace() const {
+        assert(IsFinite());
+        return elems[0][0] + elems[1][1] + elems[2][2] + elems[3][3];
+    }
+
+    bool Matrix4x4::InverseOrthogonalUniformScale() {
+        assert(!ContainsProjection());
+        assert(IsColOrthogonal(1e-3f));
+        assert(HasUniformScale());
+
+        Swap(At(0, 1), At(1, 0));
+        Swap(At(0, 2), At(2, 0));
+        Swap(At(1, 2), At(2, 1));
+        float scale = Vector3(At(0,0), At(1, 0), At(2, 0)).LengthSquared();
+        if (scale == 0.f)
+            return false;
+        scale = 1.f / scale;
+
+        At(0, 0) *= scale; At(0, 1) *= scale; At(0, 2) *= scale;
+        At(1, 0) *= scale; At(1, 1) *= scale; At(1, 2) *= scale;
+        At(2, 0) *= scale; At(2, 1) *= scale; At(2, 2) *= scale;
+
+        SetTranslatePart(TransformDir(-At(0, 3), -At(1, 3), -At(2, 3)));
+        return true;
+    }
+
+    Matrix4x4 Matrix4x4::Inverted() const {
+        Matrix4x4 copy = *this;
+        copy.Inverse();
+        return copy;
+    }
+
 }