From f501d2e9a480aafed402dbddbb3bd8cc1bb40307 Mon Sep 17 00:00:00 2001
From: Guillaume Bittoun <guillaume.bittoun@gmail.com>
Date: Tue, 20 Jan 2015 21:35:19 +0000
Subject: [PATCH] matrix: adding U-inverse function

---
 src/lol/math/matrix.h | 27 ++++++++++++++++++++++++++-
 src/t/math/matrix.cpp | 28 ++++++++++++++++++++++++++++
 2 files changed, 54 insertions(+), 1 deletion(-)

diff --git a/src/lol/math/matrix.h b/src/lol/math/matrix.h
index ddfb065d..3dd94271 100644
--- a/src/lol/math/matrix.h
+++ b/src/lol/math/matrix.h
@@ -570,7 +570,7 @@ mat_t<T, N, N> l_inverse(mat_t<T, N, N> const & L)
         {
             T sum = 0;
 
-            for (int k = i ; k >= j ; --k)
+            for (int k = i ; k > j ; --k)
                 sum += ret[k][i] * L[j][k];
 
             ret[j][i] = ((i == j ? 1 : 0) - sum) / L[j][j];
@@ -580,6 +580,31 @@ mat_t<T, N, N> l_inverse(mat_t<T, N, N> const & L)
     return ret;
 }
 
+/*
+ * Compute U matrix inverse
+ */
+
+template<typename T, int N>
+mat_t<T, N, N> u_inverse(mat_t<T, N, N> const & U)
+{
+    mat_t<T, N, N> ret;
+
+    for (int i = 0 ; i < N ; ++i)
+    {
+        for (int j = i ; j < N ; ++j)
+        {
+            T sum = 0;
+
+            for (int k = i ; k < j ; ++k)
+                sum += ret[k][i] * U[j][k];
+
+            ret[j][i] = ((i == j ? 1 : 0) - sum) / U[j][j];
+        }
+    }
+
+    return ret;
+}
+
 /*
  * Compute square matrix inverse
  */
diff --git a/src/t/math/matrix.cpp b/src/t/math/matrix.cpp
index ec1dbf01..3aec5dc0 100644
--- a/src/t/math/matrix.cpp
+++ b/src/t/math/matrix.cpp
@@ -187,6 +187,34 @@ lolunit_declare_fixture(MatrixTest)
                 lolunit_assert_doubles_equal(identity[i][j], i == j ? 1 : 0, 1e-5);
     }
 
+    lolunit_declare_test(UInverse3x3)
+    {
+        mat3 m0 = inv3;
+        mat3 L, U;
+        lu_decomposition(inv3, L, U);
+        mat3 u_inv = u_inverse(U);
+
+        mat3 identity = u_inv * U;
+
+        for (int i = 0 ; i < 3 ; ++i)
+            for (int j = 0 ; j < 3 ; ++j)
+                lolunit_assert_doubles_equal(identity[i][j], i == j ? 1 : 0, 1e-5);
+    }
+
+    lolunit_declare_test(UInverse4x4)
+    {
+        mat4 m0 = inv4;
+        mat4 L, U;
+        lu_decomposition(inv4, L, U);
+        mat4 u_inv = u_inverse(U);
+
+        mat4 identity = u_inv * U;
+
+        for (int i = 0 ; i < 4 ; ++i)
+            for (int j = 0 ; j < 4 ; ++j)
+                lolunit_assert_doubles_equal(identity[i][j], i == j ? 1 : 0, 1e-5);
+    }
+
     lolunit_declare_test(Inverse3x3)
     {
         mat3 m0 = inv3;