Mathter: /home/petiaccja/Programming/Mathter/Mathter/Decompositions/DecomposeLU.hpp Source File

Mathter A configurable 3D math library for game developers.

DecomposeLU.hpp
Go to the documentation of this file.
1 //==============================================================================
2 // This software is distributed under The Unlicense.
3 // For more information, please refer to <http://unlicense.org/>
4 //==============================================================================
5
6 #pragma once
7
8 #include "../Common/Range.hpp"
9
10
11 namespace mathter {
12
13
16 template <class T, int Dim, eMatrixOrder Order, eMatrixLayout Layout, bool Packed>
17 class DecompositionLU {
18 using MatrixT = Matrix<T, Dim, Dim, Order, Layout, Packed>;
19
20 template <class T2, int Dim2, eMatrixOrder Order2, eMatrixLayout Layout2, bool Packed2>
21 friend class DecompositionLUP;
22
23 private:
24 static Vector<float, Dim, Packed> Solve(const MatrixT& L, const MatrixT& U, const Vector<T, Dim, Packed>& b);
25
26 public:
27 DecompositionLU(MatrixT L, MatrixT U) : L(L), U(U) {}
28
33 Vector<float, Dim, Packed> Solve(const Vector<T, Dim, Packed>& b) const {
34 return Solve(L, U, b);
35 }
36
37 bool Solvable() const {
38 T prod = L(0, 0);
39 T sum = std::abs(prod);
40 for (int i = 1; i < Dim; ++i) {
41 prod *= L(i, i);
42 sum += std::abs(L(i, i));
43 }
44 sum /= Dim;
45 return std::abs(prod) / sum > T(1e-6);
46 }
47
49 MatrixT L;
51 MatrixT U;
52 };
53
54
57 template <class T, int Dim, eMatrixOrder Order, eMatrixLayout Layout, bool Packed>
58 class DecompositionLUP {
59 using MatrixT = Matrix<T, Dim, Dim, Order, Layout, Packed>;
60
61 public:
62 DecompositionLUP(MatrixT L, MatrixT U, Vector<int, Dim, false> P) : L(L), U(U), P(P) {}
63
68 Vector<float, Dim, Packed> Solve(const Vector<T, Dim, Packed>& b) const;
69
70 bool Solvable() {
71 T prod = L(0, 0);
72 T sum = std::abs(prod);
73 for (int i = 1; i < Dim; ++i) {
74 prod *= L(i, i);
75 sum += std::abs(L(i, i));
76 }
77 sum /= Dim;
78 return std::abs(prod) / sum > T(1e-6);
79 }
80
82 MatrixT L;
84 MatrixT U;
86 Vector<int, Dim, false> P;
87 };
88
89
90 template <class T, int Dim, eMatrixOrder Order, eMatrixLayout Layout, bool Packed>
91 auto DecomposeLU(const Matrix<T, Dim, Dim, Order, Layout, Packed>& m) {
92 // From: https://www.gamedev.net/resources/_/technical/math-and-physics/matrix-inversion-using-lu-decomposition-r3637
93 Matrix<T, Dim, Dim, Order, Layout, Packed> L;
94 Matrix<T, Dim, Dim, Order, Layout, Packed> U;
95
96 const auto& A = m;
97 constexpr int n = Dim;
98
99 for (int i = 0; i < n; ++i) {
100 for (int j = i + 1; j < n; ++j) {
101 L(i, j) = 0;
102 }
103 for (int j = 0; j <= i; ++j) {
104 U(i, j) = i == j;
105 }
106 }
107
108 // Crout's algorithm
109 for (int i = 0; i < n; ++i) {
110 L(i, 0) = A(i, 0);
111 }
112 for (int j = 1; j < n; ++j) {
113 U(0, j) = A(0, j) / L(0, 0);
114 }
115
116 for (int j = 1; j < n - 1; ++j) {
117 for (int i = j; i < n; ++i) {
118 float Lij;
119 Lij = A(i, j);
120 for (int k = 0; k <= j - 1; ++k) {
121 Lij -= L(i, k) * U(k, j);
122 }
123 L(i, j) = Lij;
124 }
125 for (int k = j; k < n; ++k) {
126 float Ujk;
127 Ujk = A(j, k);
128 for (int i = 0; i <= j - 1; ++i) {
129 Ujk -= L(j, i) * U(i, k);
130 }
131 Ujk /= L(j, j);
132 U(j, k) = Ujk;
133 }
134 }
135
136 L(n - 1, n - 1) = A(n - 1, n - 1);
137 for (int k = 0; k < n - 1; ++k) {
138 L(n - 1, n - 1) -= L(n - 1, k) * U(k, n - 1);
139 }
140
141 return DecompositionLU<T, Dim, Order, Layout, Packed>{ L, U };
142 }
143
150 template <class T, int Dim, eMatrixOrder Order, eMatrixLayout Layout, bool Packed>
151 auto DecomposeLUP(const Matrix<T, Dim, Dim, Order, Layout, Packed>& m, int& parity) {
152 Matrix<T, Dim, Dim, Order, Layout, Packed> L;
153 Matrix<T, Dim, Dim, Order, Layout, Packed> U;
154 Vector<int, Dim, false> P;
155 U = m;
156
157 int n = m.RowCount();
158 parity = 1;
159
160 for (int i : Range(0, n)) {
161 P(i) = i;
162 }
163
164 for (int j : Range(0, n)) {
165 // find largest pivot elements
166 T p = 0;
167 int largest;
168 for (int i : Range(j, n)) {
169 if (std::abs(U(i, j)) > p) {
170 largest = i;
171 p = std::abs(U(i, j));
172 }
173 }
174
175 // if pivot is zero TODO
176 if (p == 0) {
177 continue;
178 }
179
180 // swap rows to move pivot to top row
181 std::swap(P(j), P(largest));
182 parity *= (j != largest ? -1 : 1);
183 for (int i : Range(0, n)) {
184 std::swap(U(j, i), U(largest, i));
185 }
186
187 // do some magic
188 for (int i : Range(j + 1, n)) {
189 U(i, j) = U(i, j) / U(j, j);
190 for (int k : Range(j + 1, n)) {
191 U(i, k) = U(i, k) - U(i, j) * U(j, k);
192 }
193 }
194 }
195
196 // copy elements to L
197 for (int j : Range(0, n)) {
198 for (int i : Range(j + 1, n)) {
199 L(i, j) = U(i, j);
200 U(i, j) = T(0);
201 L(j, i) = T(0);
202 }
203 }
204 for (int i : Range(n)) {
205 L(i, i) = 1;
206 }
207
208 return DecompositionLUP<T, Dim, Order, Layout, Packed>(L, U, P);
209 }
210
211
214 template <class T, int Dim, eMatrixOrder Order, eMatrixLayout Layout, bool Packed>
215 auto DecomposeLUP(const Matrix<T, Dim, Dim, Order, Layout, Packed>& m) {
216 int ignore;
217 return DecomposeLUP(m, ignore);
218 }
219
220
221 template <class T, int Dim, eMatrixOrder Order, eMatrixLayout Layout, bool Packed>
222 Vector<float, Dim, Packed> DecompositionLU<T, Dim, Order, Layout, Packed>::Solve(const MatrixT& L, const MatrixT& U, const Vector<T, Dim, Packed>& b) {
223 // Matrix to do gaussian elimination with
224 Matrix<T, Dim, Dim + 1, eMatrixOrder::FOLLOW_VECTOR, eMatrixLayout::ROW_MAJOR, Packed> E;
225
226 // Solve Ld = b;
227 E.template Submatrix<Dim, Dim>(0, 0) = L;
228 E.Column(Dim) = b;
229
230 for (int i = 0; i < Dim - 1; ++i) {
231 for (int i2 = i + 1; i2 < Dim; ++i2) {
232 E.stripes[i] /= E(i, i);
233 T coeff = E(i2, i);
234 E.stripes[i2] -= E.stripes[i] * coeff;
235 }
236 }
237 E(Dim - 1, Dim) /= E(Dim - 1, Dim - 1);
238 // d is now the last column of E
239
240 // Solve Ux = d
241 E.template Submatrix<Dim, Dim>(0, 0) = U;
242
243 for (int i = Dim - 1; i > 0; --i) {
244 for (int i2 = i - 1; i2 >= 0; --i2) {
245 E.stripes[i] /= E(i, i);
246 T coeff = E(i2, i);
247 E.stripes[i2] -= E.stripes[i] * coeff;
248 }
249 }
250 E(0, Dim) /= E(0, 0);
251 // x is now the last column of E
252
253 return E.Column(Dim);
254 }
255
256
257 template <class T, int Dim, eMatrixOrder Order, eMatrixLayout Layout, bool Packed>
258 Vector<float, Dim, Packed> DecompositionLUP<T, Dim, Order, Layout, Packed>::Solve(const Vector<T, Dim, Packed>& b) const {
259 // Permute b
260 Vector<T, Dim, Packed> bp;
261 for (int i : Range(0, P.Dimension())) {
262 bp(i) = b(P(i));
263 }
264
265 // Solve
266 auto x = DecompositionLU<T, Dim, Order, Layout, Packed>::Solve(L, U, bp);
267
268 return x;
269 }
270
271
272 } // namespace mathter
mathter::DecompositionLU::DecompositionLU
DecompositionLU(MatrixT L, MatrixT U)
Definition: DecomposeLU.hpp:27
mathter::DecompositionLU
A utility class that can do common operations with the LU decomposition, i.e. solving equation system...
Definition: DecomposeLU.hpp:17
mathter::DecompositionLUP::DecompositionLUP
DecompositionLUP(MatrixT L, MatrixT U, Vector< int, Dim, false > P)
Definition: DecomposeLU.hpp:62
mathter::Matrix::Column
auto Column(int colIdx)
Return the submatrix corresponding to the specified column.
Definition: MatrixImpl.hpp:302
mathter::DecompositionLU::Solvable
bool Solvable() const
Definition: DecomposeLU.hpp:37
mathter::DecompositionLUP::Solve
Vector< float, Dim, Packed > Solve(const Vector< T, Dim, Packed > &b) const
Solves the equation system Ax=b, that is LUx=Pb.
Definition: DecomposeLU.hpp:258
mathter::DecompositionLU::U
MatrixT U
Upper triangular matrix, LU=P&#39;A.
Definition: DecomposeLU.hpp:51
mathter::Vector
Represents a vector in N-dimensional space.
Definition: Definitions.hpp:57
mathter::DecompositionLU::Solve
Vector< float, Dim, Packed > Solve(const Vector< T, Dim, Packed > &b) const
Solves the equation system Ax=b, that is LUx=b.
Definition: DecomposeLU.hpp:33
mathter::DecompositionLU::L
MatrixT L
Lower triangular matrix, LU=P&#39;A.
Definition: DecomposeLU.hpp:49
mathter::MatrixData::stripes
std::array< Vector< T, StripeDim, Packed >, StripeCount > stripes
Definition: MatrixImpl.hpp:46
mathter
Definition: Approx.hpp:11
mathter::DecompositionLUP::Solvable
bool Solvable()
Definition: DecomposeLU.hpp:70
mathter::DecomposeLUP
auto DecomposeLUP(const Matrix< T, Dim, Dim, Order, Layout, Packed > &m, int &parity)
Implements LU decomposition with partial pivoting.
Definition: DecomposeLU.hpp:151
mathter::Matrix< T, Dim, Dim, Order, Layout, Packed >
mathter::DecompositionLUP::P
Vector< int, Dim, false > P
Row permutations. LU=P&#39;A, where P&#39; is a matrix whose i-th row&#39;s P[i]-th element is one...
Definition: DecomposeLU.hpp:86
mathter::DecompositionLUP
A utility class that can do common operations with the LUP decomposition, i.e. solving equation syste...
Definition: DecomposeLU.hpp:58
mathter::Range
RangeHelper< T > Range(T first, T last, T step)
Definition: Range.hpp:54
mathter::DecomposeLU
auto DecomposeLU(const Matrix< T, Dim, Dim, Order, Layout, Packed > &m)
Definition: DecomposeLU.hpp:91
mathter::DecompositionLUP::L
MatrixT L
Lower triangular matrix, LU=P&#39;A.
Definition: DecomposeLU.hpp:82
mathter::DecompositionLUP::U
MatrixT U
Upper triangular matrix, LU=P&#39;A.
Definition: DecomposeLU.hpp:84
mathter::MatrixData::RowCount
constexpr int RowCount() const
Returns the number of rows of the matrix.
Definition: MatrixImpl.hpp:31