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
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
|
#include "triangular_solve.hpp"
#include "linalg_error.hpp"
#include <cmath>
#include <sstream>
#include <stdexcept>
namespace linalg {
namespace {
void validate_square_system(const Matrix& matrix, const Vector& rhs, const char* operation) {
if (matrix.rows() != matrix.cols()) {
std::ostringstream oss;
oss << operation << " requires a square matrix, got " << matrix.rows() << "x"
<< matrix.cols();
throw DimensionMismatchError(oss.str());
}
if (matrix.rows() != rhs.size()) {
std::ostringstream oss;
oss << operation << " requires matrix dimension to match rhs size, got "
<< matrix.rows() << " and " << rhs.size();
throw DimensionMismatchError(oss.str());
}
}
void validate_tolerance(double singular_tolerance) {
if (singular_tolerance < 0.0) {
throw std::invalid_argument("Singular tolerance must be nonnegative");
}
}
void validate_lower_triangular(
const Matrix& lower,
double singular_tolerance,
bool unit_diagonal) {
for (std::size_t i = 0; i < lower.rows(); ++i) {
for (std::size_t j = i + 1; j < lower.cols(); ++j) {
if (std::abs(lower(i, j)) > singular_tolerance) {
throw std::invalid_argument(
"Forward substitution requires a lower-triangular matrix");
}
}
if (!unit_diagonal && std::abs(lower(i, i)) <= singular_tolerance) {
throw SingularMatrixError(
"Forward substitution encountered a zero or tiny diagonal entry");
}
}
}
void validate_upper_triangular(
const Matrix& upper,
double singular_tolerance,
bool unit_diagonal) {
for (std::size_t i = 0; i < upper.rows(); ++i) {
for (std::size_t j = 0; j < i; ++j) {
if (std::abs(upper(i, j)) > singular_tolerance) {
throw std::invalid_argument(
"Backward substitution requires an upper-triangular matrix");
}
}
if (!unit_diagonal && std::abs(upper(i, i)) <= singular_tolerance) {
throw SingularMatrixError(
"Backward substitution encountered a negligible diagonal entry");
}
}
}
} // namespace
Vector forward_substitution(
const Matrix& lower,
const Vector& rhs,
double singular_tolerance,
bool unit_diagonal) {
validate_tolerance(singular_tolerance);
validate_square_system(lower, rhs, "Forward substitution");
validate_lower_triangular(lower, singular_tolerance, unit_diagonal);
Vector solution(lower.rows());
for (std::size_t i = 0; i < lower.rows(); ++i) {
double sum = rhs[i];
for (std::size_t j = 0; j < i; ++j) {
sum -= lower(i, j) * solution[j];
}
if (unit_diagonal) {
solution[i] = sum;
} else {
solution[i] = sum / lower(i, i);
}
}
return solution;
}
Vector backward_substitution(
const Matrix& upper,
const Vector& rhs,
double singular_tolerance,
bool unit_diagonal) {
validate_tolerance(singular_tolerance);
validate_square_system(upper, rhs, "Backward substitution");
validate_upper_triangular(upper, singular_tolerance, unit_diagonal);
Vector solution(upper.rows());
for (std::size_t offset = 0; offset < upper.rows(); ++offset) {
const std::size_t i = upper.rows() - 1 - offset;
double sum = rhs[i];
for (std::size_t j = i + 1; j < upper.cols(); ++j) {
sum -= upper(i, j) * solution[j];
}
if (unit_diagonal) {
solution[i] = sum;
} else {
solution[i] = sum / upper(i, i);
}
}
return solution;
}
} // namespace linalg
|