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
|
#include "lu.hpp"
#include <algorithm>
#include <cmath>
#include <numeric>
#include <sstream>
#include "linalg_error.hpp"
#include "triangular_solve.hpp"
namespace linalg {
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();
// Working copy: elimination is performed in-place here.
Matrix work = A;
// L starts as identity; multipliers fill the strict lower triangle.
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);
// perm[i] = original row index now at position i.
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) {
// ---- Partial pivoting: find row with largest magnitude in column 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) {
// Swap rows in the working matrix.
for (std::size_t j = 0; j < n; ++j) {
std::swap(work(k, j), work(pivot_row, j));
}
// Swap already-computed multipliers in L (columns 0 .. k-1).
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;
}
// ---- Singularity check ----
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());
}
// ---- Record U row k ----
for (std::size_t j = k; j < n; ++j) {
U(k, j) = work(k, j);
}
// ---- Compute multipliers and eliminate below pivot ----
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());
}
// Step 1: apply permutation P. (Pb)[i] = b[perm[i]]
Vector pb(n);
for (std::size_t i = 0; i < n; ++i) {
pb[i] = b[lu.perm[i]];
}
// Step 2: forward substitution Ly = Pb (L has unit diagonal)
const Vector y = forward_substitution(lu.L, pb, /*singular_tolerance=*/1e-14,
/*unit_diagonal=*/true);
// Step 3: backward substitution Ux = y
return backward_substitution(lu.U, y);
}
} // namespace linalg
|