lolengine
/
lol
ミラー元 https://github.com/lolengine/lol
1
0
フォーク 0
コード 課題 0 リリース 0 Wiki アクティビティ
ソースを参照

math: rewrite the LU decomposition code and the matrix inversion code.

legacy
Sam Hocevar 5年前
親
14f2fd354f
コミット
32583a7c41
2個のファイルの変更、97行の追加、276行の削除
分割表示
差分オプション
統計情報を表示 Patchファイルをダウンロード Diffファイルをダウンロード
  1. +30
    -137
      src/lol/math/matrix.h
  2. +67
    -139
      src/t/math/matrix.cpp

+ 30
- 137
src/lol/math/matrix.h ファイルの表示

@@ -426,14 +426,14 @@ T cofactor(mat_t<T, 2, 2> const &m, int i, int j)

// Lu decomposition with partial pivoting
template<typename T, int N> LOL_ATTR_NODISCARD
std::tuple<mat_t<T, N, N>, vec_t<int, N + 1>> lu_decomposition(mat_t<T, N, N> const &m)
std::tuple<mat_t<T, N, N>, vec_t<int, N>, int> lu_decomposition(mat_t<T, N, N> const &m)
{
mat_t<T, N, N> lu = m;
vec_t<int, N + 1> perm;
vec_t<int, N> 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<mat_t<T, N, N>, vec_t<int, N + 1>> lu_decomposition(mat_t<T, N, N> 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<mat_t<T, N, N>, vec_t<int, N + 1>> lu_decomposition(mat_t<T, N, N> co
}
}

return std::make_tuple(lu, perm);
}

template<typename T, int N>
void lu_decomposition(mat_t<T, N, N> const &m, mat_t<T, N, N> & L, mat_t<T, N, N> & 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<T, N, N> 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<T, 1, 1> const &m)
return m[0][0];
}

/*
* Compute permutation vector of a square matrix
*/

// Compute inverse of the L matrix of an LU decomposition
template<typename T, int N>
vec_t<int, N> p_vector(mat_t<T, N, N> const & m)
mat_t<T, N, N> l_inverse(mat_t<T, N, N> const & lu)
{
vec_t<int, N> used;
vec_t<int, N> permutation;
mat_t<T, N, N> 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<typename T, int N>
mat_t<T, N, N> permute_rows(mat_t<T, N, N> const & m, vec_t<int, N> const & permutation)
{
mat_t<T, N, N> 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<typename T, int N>
mat_t<T, N, N> permute_cols(mat_t<T, N, N> const & m, vec_t<int, N> const & permutation)
{
mat_t<T, N, N> result;

for (int i = 0 ; i < N ; ++i)
result[i] = m[permutation[i]];

return result;
}

template<int N>
vec_t<int, N> p_transpose(vec_t<int, N> P)
{
vec_t<int, N> result;

for (int i = 0 ; i < N ; ++i)
result[P[i]] = i;

return result;
}

/*
* Compute L matrix inverse
*/

template<typename T, int N>
mat_t<T, N, N> l_inverse(mat_t<T, N, N> const & L)
{
mat_t<T, N, N> 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<typename T, int N>
mat_t<T, N, N> u_inverse(mat_t<T, N, N> const & U)
mat_t<T, N, N> u_inverse(mat_t<T, N, N> const & lu)
{
mat_t<T, N, N> ret;
mat_t<T, N, N> 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<T, N, N> u_inverse(mat_t<T, N, N> const & U)
template<typename T, int N>
mat_t<T, N, N> inverse(mat_t<T, N, N> const &m)
{
mat_t<T, N, N> L, U;
vec_t<int, N> P = p_vector(m);

lu_decomposition(permute_rows(m, P), L, U);

mat_t<T, N, N> 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<T, N, N> ret;
for (int i = 0; i < N; ++i)
ret[p[i]] = invlu[i];
return ret;
}



+ 67
- 139
src/t/math/matrix.cpp ファイルの表示

@@ -1,7 +1,7 @@
//
// Lol Engine — Unit tests
//
// Copyright © 2010—2015 Sam Hocevar <sam@hocevar.net>
// Copyright © 2010—2019 Sam Hocevar <sam@hocevar.net>
//
// 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<typename T, int N>
static mat_t<T, N, N> l_extract(mat_t<T, N, N> 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<typename T, int N>
static mat_t<T, N, N> u_extract(mat_t<T, N, N> 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<int, 4> 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<int, 4> 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<int, 4> 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),


書き込み プレビュー
読み込み中…
キャンセル
保存