diff --git a/libs/geom/include/psemek/geom/gauss.hpp b/libs/geom/include/psemek/geom/gauss.hpp index 417cdced..48da6204 100644 --- a/libs/geom/include/psemek/geom/gauss.hpp +++ b/libs/geom/include/psemek/geom/gauss.hpp @@ -243,4 +243,82 @@ namespace psemek::geom } } + template + bool solve_lower_triangular(matrix const & m, RHS & ... rhs) + { + for (std::size_t i = 0; i < N; ++i) + { + if (m[i][i] == T{}) return false; + + for (std::size_t j = 0; j < i; ++j) + { + detail::for_each([&](auto & rhs){ + detail::gauss_helper h{rhs}; + for (std::size_t c = 0; c < h.columns(); ++c) + { + h.at(i, c) -= h.at(j, c) * m[i][j]; + } + }, rhs...); + } + + detail::for_each([&](auto & rhs){ + detail::gauss_helper h{rhs}; + for (std::size_t c = 0; c < h.columns(); ++c) + { + h.at(i, c) /= m[i][i]; + } + }, rhs...); + } + + return true; + } + + template + bool solve_upper_triangular(matrix const & m, RHS & ... rhs) + { + for (std::size_t i = N; i --> 0;) + { + if (m[i][i] == T{}) return false; + + for (std::size_t j = i + 1; j < N; ++j) + { + detail::for_each([&](auto & rhs){ + detail::gauss_helper h{rhs}; + for (std::size_t c = 0; c < h.columns(); ++c) + { + h.at(i, c) -= h.at(j, c) * m[i][j]; + } + }, rhs...); + } + + detail::for_each([&](auto & rhs){ + detail::gauss_helper h{rhs}; + for (std::size_t c = 0; c < h.columns(); ++c) + { + h.at(i, c) /= m[i][i]; + } + }, rhs...); + } + + return true; + } + + template + std::optional> inverse_lower_triangular(matrix const & m) + { + auto result = matrix::identity(); + if (!solve_lower_triangular(m, result)) + return std::nullopt; + return result; + } + + template + std::optional> inverse_upper_triangular(matrix const & m) + { + auto result = matrix::identity(); + if (!solve_upper_triangular(m, result)) + return std::nullopt; + return result; + } + } diff --git a/libs/geom/tests/matrix.cpp b/libs/geom/tests/matrix.cpp index c9f4b2fd..92d73b4b 100644 --- a/libs/geom/tests/matrix.cpp +++ b/libs/geom/tests/matrix.cpp @@ -196,6 +196,96 @@ test_case(geom_matrix_inverse) expect_small(frobenius_norm(*i - by_rows(vector{-2.f, 1.f}, vector{1.5f, -0.5f})), 1e-6); } +test_case(geom_matrix_solve__lower__triangular) +{ + random::generator rng; + random::normal_distribution normal; + + for (int iteration = 0; iteration < 1024; ++iteration) + { + matrix m = matrix::zero(); + + for (std::size_t i = 0; i < m.rows(); ++i) + for (std::size_t j = 0; j <= i; ++j) + m[i][j] = (i == j) ? (1.f + std::abs(normal(rng))) : normal(rng); + + vector v; + + for (std::size_t i = 0; i < v.dimension(); ++i) + v[i] = normal(rng); + + auto u = v; + solve_lower_triangular(m, u); + + expect_small(length(m * u - v), 1e-5); + } +} + +test_case(geom_matrix_solve__upper__triangular) +{ + random::generator rng; + random::normal_distribution normal; + + for (int iteration = 0; iteration < 1024; ++iteration) + { + matrix m = matrix::zero(); + + for (std::size_t i = 0; i < m.rows(); ++i) + for (std::size_t j = i; j < m.columns(); ++j) + m[i][j] = (i == j) ? (1.f + std::abs(normal(rng))) : normal(rng); + + vector v; + + for (std::size_t i = 0; i < v.dimension(); ++i) + v[i] = normal(rng); + + auto u = v; + solve_upper_triangular(m, u); + + expect_small(length(m * u - v), 1e-5); + } +} + +test_case(geom_matrix_inverse__lower__triangular) +{ + random::generator rng; + random::normal_distribution normal; + + for (int iteration = 0; iteration < 1024; ++iteration) + { + matrix m = matrix::zero(); + + for (std::size_t i = 0; i < m.rows(); ++i) + for (std::size_t j = 0; j <= i; ++j) + m[i][j] = (i == j) ? (1.f + std::abs(normal(rng))) : normal(rng); + + auto inv = inverse_lower_triangular(m); + + expect(inv); + expect_small(frobenius_norm(m.identity() - m * *inv), 1e-4); + } +} + +test_case(geom_matrix_inverse__upper__triangular) +{ + random::generator rng; + random::normal_distribution normal; + + for (int iteration = 0; iteration < 1024; ++iteration) + { + matrix m = matrix::zero(); + + for (std::size_t i = 0; i < m.rows(); ++i) + for (std::size_t j = i; j < m.columns(); ++j) + m[i][j] = (i == j) ? (1.f + std::abs(normal(rng))) : normal(rng); + + auto inv = inverse_upper_triangular(m); + + expect(inv); + expect_small(frobenius_norm(m.identity() - m * *inv), 1e-4); + } +} + test_case(geom_matrix_qr) { random::generator rng;