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>
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 
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 
52 };
53 
54 
57 template <class T, int Dim, eMatrixOrder Order, eMatrixLayout Layout, bool Packed>
60 
61 public:
63 
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 
87 };
88 
89 
90 template <class T, int Dim, eMatrixOrder Order, eMatrixLayout Layout, bool Packed>
92  // From: https://www.gamedev.net/resources/_/technical/math-and-physics/matrix-inversion-using-lu-decomposition-r3637
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 
142 }
143 
150 template <class T, int Dim, eMatrixOrder Order, eMatrixLayout Layout, bool Packed>
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 
209 }
210 
211 
214 template <class T, int Dim, eMatrixOrder Order, eMatrixLayout Layout, bool Packed>
216  int ignore;
217  return DecomposeLUP(m, ignore);
218 }
219 
220 
221 template <class T, int Dim, eMatrixOrder Order, eMatrixLayout Layout, bool Packed>
223  // Matrix to do gaussian elimination with
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>
259  // Permute b
261  for (int i : Range(0, P.Dimension())) {
262  bp(i) = b(P(i));
263  }
264 
265  // Solve
267 
268  return x;
269 }
270 
271 
272 } // namespace mathter
DecompositionLU(MatrixT L, MatrixT U)
Definition: DecomposeLU.hpp:27
A utility class that can do common operations with the LU decomposition, i.e. solving equation system...
Definition: DecomposeLU.hpp:17
DecompositionLUP(MatrixT L, MatrixT U, Vector< int, Dim, false > P)
Definition: DecomposeLU.hpp:62
auto Column(int colIdx)
Return the submatrix corresponding to the specified column.
Definition: MatrixImpl.hpp:302
bool Solvable() const
Definition: DecomposeLU.hpp:37
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
MatrixT U
Upper triangular matrix, LU=P&#39;A.
Definition: DecomposeLU.hpp:51
Represents a vector in N-dimensional space.
Definition: Definitions.hpp:57
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
MatrixT L
Lower triangular matrix, LU=P&#39;A.
Definition: DecomposeLU.hpp:49
std::array< Vector< T, StripeDim, Packed >, StripeCount > stripes
Definition: MatrixImpl.hpp:46
Definition: Approx.hpp:11
bool Solvable()
Definition: DecomposeLU.hpp:70
auto DecomposeLUP(const Matrix< T, Dim, Dim, Order, Layout, Packed > &m, int &parity)
Implements LU decomposition with partial pivoting.
Definition: DecomposeLU.hpp:151
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
A utility class that can do common operations with the LUP decomposition, i.e. solving equation syste...
Definition: DecomposeLU.hpp:58
RangeHelper< T > Range(T first, T last, T step)
Definition: Range.hpp:54
auto DecomposeLU(const Matrix< T, Dim, Dim, Order, Layout, Packed > &m)
Definition: DecomposeLU.hpp:91
MatrixT L
Lower triangular matrix, LU=P&#39;A.
Definition: DecomposeLU.hpp:82
MatrixT U
Upper triangular matrix, LU=P&#39;A.
Definition: DecomposeLU.hpp:84
constexpr int RowCount() const
Returns the number of rows of the matrix.
Definition: MatrixImpl.hpp:31