1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
|
export module linalgebra:lu;
import std;
import :error;
import :vector;
import :matrix;
import :triangular_solve;
export namespace linalgebra {
struct LUResult {
Matrix L;
Matrix U;
std::vector<std::size_t> perm;
int sign;
};
LUResult lu_factor(const Matrix& A, double singular_tolerance = 1e-12);
Vector lu_solve(const LUResult& lu, const Vector& b);
} // namespace linalgebra
namespace linalgebra {
LUResult lu_factor(const Matrix& A, double singular_tolerance) {
if (A.rows() != A.cols()) {
std::ostringstream oss;
oss << "lu_factor requires a square matrix, got " << A.rows() << "x" << A.cols();
throw DimensionMismatchError(oss.str());
}
const std::size_t n = A.rows();
Matrix work = A;
Matrix L = Matrix::zeros(n, n);
for (std::size_t i = 0; i < n; ++i) {
L(i, i) = 1.0;
}
Matrix U = Matrix::zeros(n, n);
std::vector<std::size_t> perm(n);
std::iota(perm.begin(), perm.end(), std::size_t{0});
int sign = 1;
for (std::size_t k = 0; k < n; ++k) {
std::size_t pivot_row = k;
double max_val = std::abs(work(k, k));
for (std::size_t i = k + 1; i < n; ++i) {
const double val = std::abs(work(i, k));
if (val > max_val) {
max_val = val;
pivot_row = i;
}
}
if (pivot_row != k) {
for (std::size_t j = 0; j < n; ++j) {
std::swap(work(k, j), work(pivot_row, j));
}
for (std::size_t j = 0; j < k; ++j) {
std::swap(L(k, j), L(pivot_row, j));
}
std::swap(perm[k], perm[pivot_row]);
sign = -sign;
}
if (std::abs(work(k, k)) <= singular_tolerance) {
std::ostringstream oss;
oss << "lu_factor: near-zero pivot " << work(k, k) << " at step " << k
<< " (tolerance " << singular_tolerance << ")";
throw SingularMatrixError(oss.str());
}
for (std::size_t j = k; j < n; ++j) {
U(k, j) = work(k, j);
}
for (std::size_t i = k + 1; i < n; ++i) {
L(i, k) = work(i, k) / work(k, k);
for (std::size_t j = k + 1; j < n; ++j) {
work(i, j) -= L(i, k) * work(k, j);
}
work(i, k) = 0.0;
}
}
return LUResult{std::move(L), std::move(U), std::move(perm), sign};
}
Vector lu_solve(const LUResult& lu, const Vector& b) {
const std::size_t n = lu.L.rows();
if (b.size() != n) {
std::ostringstream oss;
oss << "lu_solve: rhs size " << b.size() << " does not match factorization size " << n;
throw DimensionMismatchError(oss.str());
}
Vector pb(n);
for (std::size_t i = 0; i < n; ++i) {
pb[i] = b[lu.perm[i]];
}
const Vector y = forward_substitution(lu.L, pb, /*singular_tolerance=*/1e-14,
/*unit_diagonal=*/true);
return backward_substitution(lu.U, y);
}
} // namespace linalgebra
|