diff --git a/src/lol/math/matrix.h b/src/lol/math/matrix.h index 78ae50ff..ddfb065d 100644 --- a/src/lol/math/matrix.h +++ b/src/lol/math/matrix.h @@ -555,6 +555,31 @@ T determinant(mat_t const &m) return det; } +/* + * Compute L matrix inverse + */ + +template +mat_t l_inverse(mat_t const & L) +{ + mat_t ret; + + for (int i = 0 ; i < N ; ++i) + { + for (int j = i ; j >= 0 ; --j) + { + T sum = 0; + + 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]; + } + } + + return ret; +} + /* * Compute square matrix inverse */ diff --git a/src/t/math/matrix.cpp b/src/t/math/matrix.cpp index 3306e7b0..ec1dbf01 100644 --- a/src/t/math/matrix.cpp +++ b/src/t/math/matrix.cpp @@ -159,6 +159,34 @@ lolunit_declare_fixture(MatrixTest) } } + lolunit_declare_test(LInverse3x3) + { + mat3 m0 = inv3; + mat3 L, U; + lu_decomposition(inv3, L, U); + mat3 l_inv = l_inverse(L); + + mat3 identity = l_inv * L; + + 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(LInverse4x4) + { + mat4 m0 = inv4; + mat4 L, U; + lu_decomposition(inv4, L, U); + mat4 l_inv = l_inverse(L); + + mat4 identity = l_inv * L; + + 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;