diff --git a/src/lol/math/matrix.h b/src/lol/math/matrix.h index 01e25171..691b0626 100644 --- a/src/lol/math/matrix.h +++ b/src/lol/math/matrix.h @@ -426,14 +426,14 @@ T cofactor(mat_t const &m, int i, int j) // Lu decomposition with partial pivoting template LOL_ATTR_NODISCARD -std::tuple, vec_t> lu_decomposition(mat_t const &m) +std::tuple, vec_t, int> lu_decomposition(mat_t const &m) { mat_t lu = m; - vec_t perm; + vec_t perm; + int sign = 1; for (int i = 0; i < N; ++i) perm[i] = i; - perm[N] = 1; for (int k = 0; k < N; ++k) { @@ -447,7 +447,7 @@ std::tuple, vec_t> lu_decomposition(mat_t co if (best_j != k) { std::swap(perm[k], perm[best_j]); - perm[N] = -perm[N]; + sign = -sign; for (int i = 0; i < N; ++i) std::swap(lu[i][k], lu[i][best_j]); } @@ -461,32 +461,7 @@ std::tuple, vec_t> lu_decomposition(mat_t co } } - return std::make_tuple(lu, perm); -} - -template -void lu_decomposition(mat_t const &m, mat_t & L, mat_t & U) -{ - for (int i = 0; i < N; ++i) - { - for (int j = 0; j < N; ++j) - { - T sum = 0; - for (int k = 0 ; k < min(i, j) ; ++k) - sum += L[k][j] * U[i][k]; - - if (i < j) - { - U[i][j] = 0; - L[i][j] = (m[i][j] - sum) / U[i][i]; - } - else /* if (i >= j) */ - { - L[i][j] = i == j ? T(1) : T(0); - U[i][j] = (m[i][j] - sum) / L[j][j]; - } - } - } + return std::make_tuple(lu, perm, sign); } /* @@ -498,7 +473,7 @@ T determinant(mat_t const &m) { auto lup = lu_decomposition(m); - T det = std::get<1>(lup)[N]; + T det(std::get<2>(lup)); for (int i = 0; i < N; ++i) det *= std::get<0>(lup)[i][i]; @@ -511,124 +486,40 @@ T const & determinant(mat_t const &m) return m[0][0]; } -/* - * Compute permutation vector of a square matrix - */ - +// Compute inverse of the L matrix of an LU decomposition template -vec_t p_vector(mat_t const & m) +mat_t l_inverse(mat_t const & lu) { - vec_t used; - vec_t permutation; + mat_t ret { 0 }; - for (int i = 0 ; i < N ; ++i) + for (int j = 0; j < N; ++j) { - used[i] = 0; - permutation[i] = -1; - } - - for (int i = 0 ; i < N ; ++i) - { - int index_max = -1; - - for (int j = 0 ; j < N ; ++j) - { - if (!used[j] && (index_max == -1 || abs(m[i][j]) > abs(m[i][index_max]))) - index_max = j; - } - - ASSERT(index_max != -1); - ASSERT(index_max < N); - - permutation[i] = index_max; - used[index_max] = 1; - } - - return permutation; -} - -/* - * Compute the permutation of a square matrix according to the permutation vector - */ - -template -mat_t permute_rows(mat_t const & m, vec_t const & permutation) -{ - mat_t result; - - for (int i = 0 ; i < N ; ++i) - for (int j = 0 ; j < N ; ++j) - result[i][j] = m[i][permutation[j]]; - - return result; -} - -template -mat_t permute_cols(mat_t const & m, vec_t const & permutation) -{ - mat_t result; - - for (int i = 0 ; i < N ; ++i) - result[i] = m[permutation[i]]; - - return result; -} - -template -vec_t p_transpose(vec_t P) -{ - vec_t result; - - for (int i = 0 ; i < N ; ++i) - result[P[i]] = i; - - return result; -} - -/* - * 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) + for (int i = j; i >= 0; --i) { 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]; + for (int k = i + 1; k <= j; ++k) + sum += ret[k][j] * lu[i][k]; + ret[i][j] = T(j == i ? 1 : 0) - sum; } } return ret; } -/* - * Compute U matrix inverse - */ - +// Compute inverse of the U matrix of an LU decomposition template -mat_t u_inverse(mat_t const & U) +mat_t u_inverse(mat_t const & lu) { - mat_t ret; + mat_t ret { 0 }; - for (int i = 0 ; i < N ; ++i) + for (int i = 0; i < N; ++i) { - for (int j = i ; j < N ; ++j) + 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]; + for (int k = i; k < j; ++k) + sum += ret[k][i] * lu[j][k]; + ret[j][i] = ((i == j ? 1 : 0) - sum) / lu[j][j]; } } @@ -642,13 +533,15 @@ mat_t u_inverse(mat_t const & U) template mat_t inverse(mat_t const &m) { - mat_t L, U; - vec_t P = p_vector(m); - - lu_decomposition(permute_rows(m, P), L, U); - - mat_t ret = permute_cols(u_inverse(U) * l_inverse(L), p_transpose(P)); + auto lup = lu_decomposition(m); + auto lu = std::get<0>(lup); + auto p = std::get<1>(lup); + auto invlu = u_inverse(lu) * l_inverse(lu); + // Rearrange columns according to the original permutation vector + mat_t ret; + for (int i = 0; i < N; ++i) + ret[p[i]] = invlu[i]; return ret; } diff --git a/src/t/math/matrix.cpp b/src/t/math/matrix.cpp index fc5ad0e3..1c85ae9c 100644 --- a/src/t/math/matrix.cpp +++ b/src/t/math/matrix.cpp @@ -1,7 +1,7 @@ // // Lol Engine — Unit tests // -// Copyright © 2010—2015 Sam Hocevar +// Copyright © 2010—2019 Sam Hocevar // // Lol Engine is free software. It comes without any warranty, to // the extent permitted by applicable law. You can redistribute it @@ -17,6 +17,28 @@ namespace lol { +// Get the L matrix of an LU decomposition +template +static mat_t l_extract(mat_t const & lu) +{ + auto l = lu; + for (int i = 0; i < N; ++i) + for (int j = 0; j <= i; ++j) + l[i][j] = T(j == i ? 1 : 0); + return l; +} + +// Get the U matrix of an LU decomposition +template +static mat_t u_extract(mat_t const & lu) +{ + auto u = lu; + for (int i = 0; i < N; ++i) + for (int j = i + 1; j < N; ++j) + u[i][j] = T(0); + return u; +} + lolunit_declare_fixture(matrix_test) { void setup() @@ -74,95 +96,19 @@ lolunit_declare_fixture(matrix_test) lolunit_assert_doubles_equal(m2[i][j], mat4(1.f)[i][j], 1e-5); } - lolunit_declare_test(inverse_2x2) - { - mat2 m(vec2(4, 3), - vec2(3, 2)); - - /* Invert matrix and check that the results are finite */ - mat2 m1 = inverse(m); - for (int j = 0; j < 2; ++j) - for (int i = 0; i < 2; ++i) - { - lolunit_assert_less(m1[i][j], FLT_MAX); - lolunit_assert_greater(m1[i][j], -FLT_MAX); - } - - /* Multiply with original matrix and check that we get identity */ - mat2 m2 = m1 * m; - for (int j = 0; j < 2; ++j) - for (int i = 0; i < 2; ++i) - lolunit_assert_doubles_equal(m2[i][j], mat2(1.f)[i][j], 1e-5); - } - - lolunit_declare_test(PMatrixTest) - { - mat4 m1(vec4(0, 1, 0, 0), - vec4(1, 0, 0, 0), - vec4(0, 0, 0, 1), - vec4(0, 0, 1, 0)); - - vec_t perm1 = p_vector(m1); - m1 = permute_rows(m1, perm1); - - for (int i = 0 ; i < 4 ; ++i) - { - for (int j = 0 ; j < 4 ; ++j) - { - if (i == j) - lolunit_assert_equal(m1[i][j], 1); - else - lolunit_assert_equal(m1[i][j], 0); - } - } - - mat4 m2(vec4(0, 1, 0, 0), - vec4(0, 0, 1, 0), - vec4(0, 0, 0, 1), - vec4(1, 0, 0, 0)); - - vec_t perm2 = p_vector(m2); - m2 = permute_rows(m2, perm2); - - for (int i = 0 ; i < 4 ; ++i) - { - for (int j = 0 ; j < 4 ; ++j) - { - if (i == j) - lolunit_assert_equal(m2[i][j], 1); - else - lolunit_assert_equal(m2[i][j], 0); - } - } - } - lolunit_declare_test(lu_decomposition_3x3) { mat3 m(vec3(2, 3, 5), vec3(3, 2, 3), vec3(9, 5, 7)); - mat3 L, U; - lu_decomposition(m, L, U); - mat3 m2 = L * U; + auto lup = lu_decomposition(m); + auto lu = std::get<0>(lup); + auto p = std::get<1>(lup); + auto m2 = l_extract(lu) * u_extract(lu); for (int j = 0; j < 3; ++j) for (int i = 0; i < 3; ++i) - { - /* Check that the LU decomposition has valid values */ - lolunit_assert_less(U[i][j], FLT_MAX); - lolunit_assert_greater(U[i][j], -FLT_MAX); - lolunit_assert_less(L[i][j], FLT_MAX); - lolunit_assert_greater(L[i][j], -FLT_MAX); - - if (i < j) - lolunit_assert_doubles_equal(U[i][j], 0.f, 1e-5); - if (i == j) - lolunit_assert_doubles_equal(L[i][j], 1.f, 1e-5); - if (j < i) - lolunit_assert_doubles_equal(L[i][j], 0.f, 1e-5); - - lolunit_assert_doubles_equal(m2[i][j], m[i][j], 1e-5); - } + lolunit_assert_doubles_equal(m2[i][j], m[i][p[j]], 1e-5); } lolunit_declare_test(lu_decomposition_4x4_full) @@ -171,28 +117,14 @@ lolunit_declare_fixture(matrix_test) vec4(-2, -1, -2, 2), vec4( 4, 2, 5, -4), vec4( 5, -3, -7, -6)); - mat4 L, U; - lu_decomposition(m, L, U); - mat4 m2 = L * U; + auto lup = lu_decomposition(m); + auto lu = std::get<0>(lup); + auto p = std::get<1>(lup); + auto m2 = l_extract(lu) * u_extract(lu); for (int j = 0; j < 4; ++j) for (int i = 0; i < 4; ++i) - { - /* Check that the LU decomposition has valid values */ - lolunit_assert_less(U[i][j], FLT_MAX); - lolunit_assert_greater(U[i][j], -FLT_MAX); - lolunit_assert_less(L[i][j], FLT_MAX); - lolunit_assert_greater(L[i][j], -FLT_MAX); - - if (i < j) - lolunit_assert_doubles_equal(U[i][j], 0.f, 1e-5); - if (i == j) - lolunit_assert_doubles_equal(L[i][j], 1.f, 1e-5); - if (j < i) - lolunit_assert_doubles_equal(L[i][j], 0.f, 1e-5); - - lolunit_assert_doubles_equal(m2[i][j], m[i][j], 1e-5); - } + lolunit_assert_doubles_equal(m2[i][j], m[i][p[j]], 1e-5); } lolunit_declare_test(lu_decomposition_4x4_sparse) @@ -201,31 +133,14 @@ lolunit_declare_fixture(matrix_test) vec4(0, 0, 1, 0), vec4(0, -1, 0, 0), vec4(0, 0, -1, 1)); - mat4 L, U; - vec_t P = p_vector(m); - mat4 permuted = permute_rows(m, P); - - lu_decomposition(permute_rows(m, P), L, U); - mat4 m2 = permute_rows(L * U, p_transpose(P)); + auto lup = lu_decomposition(m); + auto lu = std::get<0>(lup); + auto p = std::get<1>(lup); + auto m2 = l_extract(lu) * u_extract(lu); for (int j = 0; j < 4; ++j) for (int i = 0; i < 4; ++i) - { - /* Check that the LU decomposition has valid values */ - lolunit_assert_less(U[i][j], FLT_MAX); - lolunit_assert_greater(U[i][j], -FLT_MAX); - lolunit_assert_less(L[i][j], FLT_MAX); - lolunit_assert_greater(L[i][j], -FLT_MAX); - - if (i < j) - lolunit_assert_doubles_equal(U[i][j], 0.f, 1e-5); - if (i == j) - lolunit_assert_doubles_equal(L[i][j], 1.f, 1e-5); - if (j < i) - lolunit_assert_doubles_equal(L[i][j], 0.f, 1e-5); - - lolunit_assert_doubles_equal(m2[i][j], m[i][j], 1e-5); - } + lolunit_assert_doubles_equal(m2[i][j], m[i][p[j]], 1e-5); } lolunit_declare_test(l_inverse_3x3) @@ -233,10 +148,8 @@ lolunit_declare_fixture(matrix_test) mat3 m(vec3(2, 3, 5), vec3(3, 2, 3), vec3(9, 5, 7)); - mat3 L, U; - lu_decomposition(m, L, U); - mat3 m1 = l_inverse(L); - mat3 m2 = m1 * L; + auto lu = std::get<0>(lu_decomposition(m)); + auto m2 = l_extract(lu) * l_inverse(lu); for (int j = 0; j < 3; ++j) for (int i = 0; i < 3; ++i) @@ -249,10 +162,8 @@ lolunit_declare_fixture(matrix_test) vec4(-2, -1, -2, 2), vec4( 4, 2, 5, -4), vec4( 5, -3, -7, -6)); - mat4 L, U; - lu_decomposition(m, L, U); - mat4 m1 = l_inverse(L); - mat4 m2 = m1 * L; + auto lu = std::get<0>(lu_decomposition(m)); + auto m2 = l_extract(lu) * l_inverse(lu); for (int j = 0; j < 4; ++j) for (int i = 0; i < 4; ++i) @@ -264,10 +175,8 @@ lolunit_declare_fixture(matrix_test) mat3 m(vec3(2, 3, 5), vec3(3, 2, 3), vec3(9, 5, 7)); - mat3 L, U; - lu_decomposition(m, L, U); - mat3 m1 = u_inverse(U); - mat3 m2 = m1 * U; + auto lu = std::get<0>(lu_decomposition(m)); + auto m2 = u_extract(lu) * u_inverse(lu); for (int j = 0; j < 3; ++j) for (int i = 0; i < 3; ++i) @@ -280,16 +189,35 @@ lolunit_declare_fixture(matrix_test) vec4(-2, -1, -2, 2), vec4( 4, 2, 5, -4), vec4( 5, -3, -7, -6)); - mat4 L, U; - lu_decomposition(m, L, U); - mat4 m1 = u_inverse(U); - mat4 m2 = m1 * U; + auto lu = std::get<0>(lu_decomposition(m)); + auto m2 = u_extract(lu) * u_inverse(lu); for (int j = 0; j < 4; ++j) for (int i = 0; i < 4; ++i) lolunit_assert_doubles_equal(m2[i][j], mat4(1.f)[i][j], 1e-5); } + lolunit_declare_test(inverse_2x2) + { + mat2 m(vec2(4, 3), + vec2(3, 2)); + + /* Invert matrix and check that the results are finite */ + mat2 m1 = inverse(m); + for (int j = 0; j < 2; ++j) + for (int i = 0; i < 2; ++i) + { + lolunit_assert_less(m1[i][j], FLT_MAX); + lolunit_assert_greater(m1[i][j], -FLT_MAX); + } + + /* Multiply with original matrix and check that we get identity */ + mat2 m2 = m1 * m; + for (int j = 0; j < 2; ++j) + for (int i = 0; i < 2; ++i) + lolunit_assert_doubles_equal(m2[i][j], mat2(1.f)[i][j], 1e-5); + } + lolunit_declare_test(inverse_3x3) { mat3 m(vec3(2, 3, 5),