diff --git a/src/lol/math/matrix.h b/src/lol/math/matrix.h index 2ab6d4bb..19b79e14 100644 --- a/src/lol/math/matrix.h +++ b/src/lol/math/matrix.h @@ -547,21 +547,15 @@ void lu_decomposition(mat_t const &m, mat_t & L, mat_t T determinant(mat_t const &m) { -#if 0 /* XXX: LU decomposition is currently broken */ mat_t L, U; - lu_decomposition(m, L, U); + vec_t P = p_vector(m); + lu_decomposition(permute_rows(m, P), L, U); T det = 1; for (int i = 0 ; i < N ; ++i) det *= U[i][i]; - return det; -#else - T ret = (T)0; - for (int i = 0; i < N; ++i) - ret += m[i][0] * cofactor(m, i, 0); - return ret; -#endif + return permutation_det(P) * det; } template @@ -592,7 +586,7 @@ vec_t p_vector(mat_t const & m) for (int j = 0 ; j < N ; ++j) { - if (!used[j] && (index_max == -1 || m[i][j] > m[i][index_max])) + if (!used[j] && (index_max == -1 || abs(m[i][j]) > abs(m[i][index_max]))) index_max = j; } @@ -626,6 +620,33 @@ mat_t permute_rows(mat_t const & m, vec_t const & perm 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) + { + for (int j = 0 ; j < N ; ++j) + { + result[i][j] = m[permutation[i]][j]; + } + } + + 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 the determinant of a permutation square matrix corresponding to the permutation vector */ @@ -706,26 +727,14 @@ mat_t u_inverse(mat_t const & U) template mat_t inverse(mat_t const &m) { -#if 0 /* XXX: LU decomposition is currently broken */ mat_t L, U; - lu_decomposition(m, L, U); + vec_t P = p_vector(m); - mat_t ret = u_inverse(U) * l_inverse(L); + lu_decomposition(permute_rows(m, P), L, U); - return ret; -#else - mat_t ret; + mat_t ret = permute_cols(u_inverse(U) * l_inverse(L), p_transpose(P)); - T d = determinant(m); - if (d) - { - /* Transpose result matrix on the fly */ - for (int i = 0; i < N; ++i) - for (int j = 0; j < N; ++j) - ret[i][j] = cofactor(m, j, i) / d; - } return ret; -#endif } /* diff --git a/src/t/math/matrix.cpp b/src/t/math/matrix.cpp index d4cc292d..ce184ffe 100644 --- a/src/t/math/matrix.cpp +++ b/src/t/math/matrix.cpp @@ -136,7 +136,6 @@ lolunit_declare_fixture(MatrixTest) } } -#if 0 /* XXX: LU decomposition is currently broken */ lolunit_declare_test(lu_decomposition_3x3) { mat3 m(vec3(2, 3, 5), @@ -203,8 +202,11 @@ lolunit_declare_fixture(MatrixTest) vec4(0, -1, 0, 0), vec4(0, 0, -1, 1)); mat4 L, U; - lu_decomposition(m, L, U); - mat4 m2 = 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)); for (int j = 0; j < 4; ++j) for (int i = 0; i < 4; ++i) @@ -287,7 +289,6 @@ lolunit_declare_fixture(MatrixTest) for (int i = 0; i < 4; ++i) lolunit_assert_doubles_equal(m2[i][j], mat4(1.f)[i][j], 1e-5); } -#endif lolunit_declare_test(inverse_3x3) {