Line data Source code
1 : #include "Matrix.hpp"
2 :
3 : #include <cmath>
4 : #include <iomanip>
5 : #include <sstream>
6 : #include <stdexcept>
7 :
8 : #include <iostream>
9 :
10 : namespace HLR
11 : {
12 : namespace Kinematics
13 : {
14 : template<typename T, std::size_t m, std::size_t n>
15 : constexpr Matrix<T, m, n> Matrix<T, m, n>::get_identity() noexcept
16 : {
17 : auto identity = Matrix<T, m, m>();
18 :
19 : for (std::size_t i = 0; i < m; ++i)
20 : {
21 : identity[i][i] = 1;
22 : }
23 :
24 : return identity;
25 : }
26 :
27 : template<typename T, std::size_t m, std::size_t n>
28 3 : constexpr Matrix<T, m, n>::Matrix(
29 3 : const std::initializer_list<std::initializer_list<T>> matrix_data)
30 : {
31 3 : if (matrix_data.size() != m)
32 : {
33 1 : throw std::invalid_argument("Invalid amount of rows.");
34 : }
35 :
36 3 : for (const auto& col : matrix_data)
37 : {
38 2 : if (col.size() != n)
39 : {
40 1 : throw std::invalid_argument("Invalid amount of columns.");
41 : }
42 : }
43 :
44 1 : std::size_t pos_i = 0;
45 1 : std::size_t pos_j = 0;
46 :
47 2 : for (auto i = matrix_data.begin(); i != matrix_data.end(); ++i)
48 : {
49 2 : for (auto j = i->begin(); j != i->end(); ++j)
50 : {
51 1 : this->data[pos_i][pos_j] = *j;
52 1 : ++pos_j;
53 : }
54 1 : ++pos_i;
55 1 : pos_j = 0;
56 : }
57 1 : }
58 :
59 : template<typename T, std::size_t m, std::size_t n>
60 2720693 : constexpr std::size_t Matrix<T, m, n>::get_m() const noexcept
61 : {
62 2720693 : return m;
63 : }
64 :
65 : template<typename T, std::size_t m, std::size_t n>
66 1 : constexpr std::size_t Matrix<T, m, n>::get_n() const noexcept
67 : {
68 1 : return n;
69 : }
70 :
71 : template<typename T, std::size_t m, std::size_t n>
72 79326873 : constexpr std::array<T, n>& Matrix<T, m, n>::operator[](
73 : const std::size_t pos) noexcept
74 : {
75 79326873 : return this->data[pos];
76 : }
77 :
78 : template<typename T, std::size_t m, std::size_t n>
79 50488048 : constexpr const std::array<T, n>& Matrix<T, m, n>::operator[](
80 : const std::size_t pos) const noexcept
81 : {
82 50488048 : return this->data[pos];
83 : }
84 :
85 : template<typename T, std::size_t m, std::size_t n>
86 2 : constexpr std::array<T, n>& Matrix<T, m, n>::at(const std::size_t pos)
87 : {
88 2 : if (pos >= m)
89 : {
90 1 : throw std::out_of_range("Index out of range.");
91 : }
92 :
93 1 : return this->data.at(pos);
94 : }
95 :
96 : template<typename T, std::size_t m, std::size_t n>
97 2 : constexpr const std::array<T, n>& Matrix<T, m, n>::at(
98 : const std::size_t pos) const
99 : {
100 2 : if (pos >= m)
101 : {
102 1 : throw std::out_of_range("Index out of range.");
103 : }
104 :
105 1 : return this->data.at(pos);
106 : }
107 :
108 : template<typename T, std::size_t m, std::size_t n>
109 231339 : constexpr bool Matrix<T, m, n>::fuzzy_equal(const Matrix<T, m, n>& mat,
110 : T fuzz) const noexcept
111 : {
112 922830 : for (std::size_t i = 0; i < m; ++i)
113 : {
114 2765988 : for (std::size_t j = 0; j < n; ++j)
115 : {
116 2074497 : if (std::abs((*this)[i][j] - mat[i][j]) > fuzz)
117 : {
118 1003 : return false;
119 : }
120 : }
121 : }
122 :
123 230336 : return true;
124 : }
125 :
126 : template<typename T, std::size_t m, std::size_t n>
127 1035 : constexpr bool Matrix<T, m, n>::operator==(const Matrix<T, m, n>& mat) const
128 : noexcept
129 : {
130 1035 : return this->fuzzy_equal(mat, 0);
131 : }
132 :
133 : template<typename T, std::size_t m, std::size_t n>
134 1002 : constexpr bool Matrix<T, m, n>::operator!=(const Matrix<T, m, n>& mat) const
135 : noexcept
136 : {
137 1002 : return !(*this == mat);
138 : }
139 :
140 : template<typename T, std::size_t m, std::size_t n>
141 8 : constexpr void Matrix<T, m, n>::operator+=(T scalar) noexcept
142 : {
143 24 : for (std::size_t i = 0; i < m; ++i)
144 : {
145 64 : for (std::size_t j = 0; j < n; ++j)
146 : {
147 48 : (*this)[i][j] += scalar;
148 : }
149 : }
150 8 : }
151 :
152 : template<typename T, std::size_t m, std::size_t n>
153 4 : constexpr void Matrix<T, m, n>::operator-=(T scalar) noexcept
154 : {
155 4 : (*this) += -1 * scalar;
156 4 : }
157 :
158 : template<typename T, std::size_t m, std::size_t n>
159 1345037 : constexpr void Matrix<T, m, n>::operator*=(T scalar) noexcept
160 : {
161 6264602 : for (std::size_t i = 0; i < m; ++i)
162 : {
163 9839154 : for (std::size_t j = 0; j < n; ++j)
164 : {
165 4919589 : (*this)[i][j] *= scalar;
166 : }
167 : }
168 1345037 : }
169 :
170 : template<typename T, std::size_t m, std::size_t n>
171 4 : constexpr void Matrix<T, m, n>::operator/=(T scalar) noexcept
172 : {
173 12 : for (std::size_t i = 0; i < m; ++i)
174 : {
175 32 : for (std::size_t j = 0; j < n; ++j)
176 : {
177 24 : (*this)[i][j] /= scalar;
178 : }
179 : }
180 4 : }
181 :
182 : template<typename T, std::size_t m, std::size_t n>
183 1114704 : constexpr void Matrix<T, m, n>::operator+=(const Matrix<T, m, n>& mat) noexcept
184 : {
185 4919398 : for (std::size_t i = 0; i < m; ++i)
186 : {
187 7609404 : for (std::size_t j = 0; j < n; ++j)
188 : {
189 3804710 : (*this)[i][j] += mat[i][j];
190 : }
191 : }
192 1114704 : }
193 :
194 : template<typename T, std::size_t m, std::size_t n>
195 672510 : constexpr void Matrix<T, m, n>::operator-=(const Matrix<T, m, n>& mat) noexcept
196 : {
197 672510 : (*this) += mat * static_cast<T>(-1);
198 672510 : }
199 :
200 : template<typename T, std::size_t m, std::size_t n>
201 442193 : constexpr Matrix<T, m, n> Matrix<T, m, n>::operator+(
202 : const Matrix<T, m, n>& mat) const noexcept
203 : {
204 442193 : Matrix<T, m, n> copy(*this);
205 442193 : copy += mat;
206 442193 : return copy;
207 : }
208 :
209 : template<typename T, std::size_t m, std::size_t n>
210 672509 : constexpr Matrix<T, m, n> Matrix<T, m, n>::operator-(
211 : const Matrix<T, m, n>& mat) const noexcept
212 : {
213 672509 : Matrix<T, m, n> copy(*this);
214 672509 : copy -= mat;
215 672509 : return copy;
216 : }
217 :
218 : template<typename T, std::size_t m, std::size_t n>
219 : template<std::size_t p>
220 902830 : constexpr Matrix<T, m, p> Matrix<T, m, n>::operator*(
221 : const Matrix<T, n, p>& mat) const noexcept
222 : {
223 902830 : Matrix<T, m, p> product;
224 :
225 2726843 : for (std::size_t i = 0; i < m; ++i)
226 : {
227 5029824 : for (std::size_t j = 0; j < p; ++j)
228 : {
229 12823276 : for (std::size_t k = 0; k < n; ++k)
230 : {
231 9617465 : product[i][j] += (*this)[i][k] * mat[k][j];
232 : }
233 : }
234 : }
235 :
236 902830 : return product;
237 : }
238 :
239 : template<typename T, std::size_t m, std::size_t n>
240 : template<std::size_t p>
241 230299 : constexpr Matrix<T, m, n + p> Matrix<T, m, n>::horizontal_augment(
242 : const Matrix<T, m, p>& mat) const noexcept
243 : {
244 230299 : Matrix<T, m, n + p> augmented;
245 :
246 921196 : for (std::size_t i = 0; i < m; ++i)
247 : {
248 2763586 : for (std::size_t j = 0; j < n; ++j)
249 : {
250 2072689 : augmented[i][j] = (*this)[i][j];
251 : }
252 :
253 2763590 : for (std::size_t j = n; j < n + p; ++j)
254 : {
255 2072693 : augmented[i][j] = mat[i][j - n];
256 : }
257 : }
258 :
259 230299 : return augmented;
260 : }
261 :
262 : template<typename T, std::size_t m, std::size_t n>
263 : template<std::size_t p>
264 2 : constexpr Matrix<T, m + p, n> Matrix<T, m, n>::vertical_augment(
265 : const Matrix<T, p, n>& mat) const noexcept
266 : {
267 2 : Matrix<T, m + p, n> augmented;
268 :
269 5 : for (std::size_t i = 0; i < n; ++i)
270 : {
271 10 : for (std::size_t j = 0; j < m; ++j)
272 : {
273 7 : augmented[j][i] = (*this)[j][i];
274 : }
275 :
276 11 : for (std::size_t j = m; j < m + p; ++j)
277 : {
278 8 : augmented[j][i] = mat[j - m][i];
279 : }
280 : }
281 :
282 2 : return augmented;
283 : }
284 :
285 : template<typename T, std::size_t m, std::size_t n>
286 : template<std::size_t y, std::size_t x, std::size_t p, std::size_t q>
287 902802 : constexpr Matrix<T, p, q> Matrix<T, m, n>::slice() const noexcept
288 : {
289 902802 : Matrix<T, p, q> mat;
290 :
291 3611209 : for (std::size_t i = y; i < m && i - y < p; ++i)
292 : {
293 8180379 : for (std::size_t j = x; j < n && j - x < q; ++j)
294 : {
295 5471972 : mat[i - y][j - x] = (*this)[i][j];
296 : }
297 : }
298 :
299 902802 : return mat;
300 : }
301 :
302 : template<typename T, std::size_t m, std::size_t n>
303 442242 : constexpr Matrix<T, n, m> Matrix<T, m, n>::transpose() const noexcept
304 : {
305 442242 : Matrix<T, n, m> transposed;
306 :
307 1768964 : for (std::size_t i = 0; i < m; ++i)
308 : {
309 2653456 : for (std::size_t j = 0; j < n; ++j)
310 : {
311 1326734 : transposed[j][i] = (*this)[i][j];
312 : }
313 : }
314 :
315 442242 : return transposed;
316 : }
317 :
318 : template<typename T, std::size_t m, std::size_t n>
319 230297 : constexpr std::optional<Matrix<T, m, n>> Matrix<T, m, n>::inverse() const
320 : noexcept
321 : {
322 : static_assert(m == n,
323 : "Inverse matrix is only defined for square matrices.");
324 :
325 230297 : const auto identity = Matrix<T, m, n>::get_identity();
326 230297 : auto augmented = this->horizontal_augment(identity);
327 :
328 921186 : for (std::size_t i = 0; i < m; ++i)
329 : {
330 : /**
331 : * Find the diagonal element closest to -1 or 1.
332 : * And if it isn't in the current row swap it.
333 : * This is done to make the inverse mroe numerically stable.
334 : * Also check if that the row may not be zero since that would
335 : * result in a divide by zero error.
336 : */
337 690890 : std::size_t min_row_index = i;
338 690890 : std::optional<T> closest_to_one;
339 :
340 2072673 : for (std::size_t j = min_row_index; j < m; ++j)
341 : {
342 1381783 : if (std::abs(augmented[j][i]) <= std::numeric_limits<T>::min())
343 : {
344 280331 : continue;
345 : }
346 :
347 1101452 : const T abs_value = std::abs(1 - std::abs(augmented[j][i]));
348 :
349 2202904 : if (!closest_to_one.has_value()
350 1101452 : || abs_value < closest_to_one.value())
351 : {
352 1016750 : closest_to_one = abs_value;
353 1016750 : min_row_index = j;
354 : }
355 : }
356 :
357 690890 : if (!closest_to_one.has_value())
358 : {
359 1 : return std::nullopt;
360 : }
361 :
362 690889 : if (min_row_index != i)
363 : {
364 2631271 : for (std::size_t j = 0; j < 2 * m; ++j)
365 : {
366 2255376 : T tmp = augmented[i][j];
367 2255376 : augmented[i][j] = augmented[min_row_index][j];
368 2255376 : augmented[min_row_index][j] = tmp;
369 : }
370 : }
371 :
372 : // Divide the current row by a scalar so that the current diagonal
373 : // element will be 1.
374 690889 : const auto divisor = augmented[i][i];
375 :
376 4836231 : for (std::size_t j = 0; j < 2 * m; ++j)
377 : {
378 4145342 : augmented[i][j] /= divisor;
379 : }
380 :
381 : // Set all other rows to 0 by using the row that is 1.
382 2763560 : for (std::size_t j = 0; j < m; ++j)
383 : {
384 2072671 : if (i == j)
385 : {
386 690889 : continue;
387 : }
388 :
389 1381782 : const auto multiplier = augmented[j][i];
390 :
391 9672498 : for (std::size_t k = 0; k < 2 * m; ++k)
392 : {
393 8290716 : augmented[j][k] -= multiplier * augmented[i][k];
394 : }
395 : }
396 : }
397 :
398 : /**
399 : * Check if the inversed matrix times the original matrix is equal
400 : * to the identity matrix. The equal precision is the precision
401 : * when the identity matrix is considered equal.
402 : */
403 230296 : const auto inversed = augmented.template slice<0, m, m, m>();
404 230296 : const auto identity_check = inversed * (*this);
405 230296 : const auto equal_precision = 0.0000000001;
406 :
407 230296 : if (!identity_check.fuzzy_equal(identity, equal_precision))
408 : {
409 1 : return std::nullopt;
410 : }
411 :
412 230295 : return augmented.template slice<0, m, m, m>();
413 : }
414 :
415 : template<typename T, std::size_t m, std::size_t n>
416 442239 : constexpr T Matrix<T, m, n>::get_magnitude() const noexcept
417 : {
418 : static_assert(
419 : m == 1 || n == 1,
420 : "Magnitude of a matrix is only defined for column and row matrices.");
421 :
422 442239 : T magnitude = 0;
423 :
424 : if constexpr (m == 1) // NOLINT
425 : {
426 1 : magnitude =
427 2 : static_cast<T>(std::sqrt(((*this) * this->transpose())[0][0]));
428 : }
429 : else
430 : {
431 442238 : magnitude =
432 884476 : static_cast<T>(std::sqrt((this->transpose() * (*this))[0][0]));
433 : }
434 :
435 442239 : return magnitude;
436 : }
437 :
438 : template<typename T, std::size_t m, std::size_t n>
439 1 : std::string Matrix<T, m, n>::str() const
440 : {
441 2 : std::stringstream ss;
442 1 : ss << std::setprecision(9) << std::setw(6) << std::fixed;
443 :
444 3 : for (std::size_t i = 0; i < m; ++i)
445 : {
446 6 : for (std::size_t j = 0; j < n; ++j)
447 : {
448 4 : ss << (*this)[i][j] << "\t";
449 : }
450 2 : ss << std::endl;
451 : }
452 :
453 2 : return ss.str();
454 : }
455 :
456 : template<typename T, std::size_t m, std::size_t n>
457 2 : constexpr Matrix<T, m, n> operator+(const Matrix<T, m, n>& lhs, T rhs) noexcept
458 : {
459 2 : Matrix<T, m, n> copy(lhs);
460 2 : copy += rhs;
461 2 : return copy;
462 : }
463 :
464 : template<typename T, std::size_t m, std::size_t n>
465 1 : constexpr Matrix<T, m, n> operator+(T lhs, const Matrix<T, m, n>& rhs) noexcept
466 : {
467 1 : Matrix<T, m, n> copy(rhs);
468 1 : copy += lhs;
469 1 : return copy;
470 : }
471 :
472 : template<typename T, std::size_t m, std::size_t n>
473 2 : constexpr Matrix<T, m, n> operator-(const Matrix<T, m, n>& lhs, T rhs) noexcept
474 : {
475 2 : Matrix<T, m, n> copy(lhs);
476 2 : copy -= rhs;
477 2 : return copy;
478 : }
479 :
480 : template<typename T, std::size_t m, std::size_t n>
481 1 : constexpr Matrix<T, m, n> operator-(T lhs, const Matrix<T, m, n>& rhs) noexcept
482 : {
483 1 : Matrix<T, m, n> copy(rhs);
484 1 : copy -= lhs;
485 1 : return copy;
486 : }
487 :
488 : template<typename T, std::size_t m, std::size_t n>
489 902805 : constexpr Matrix<T, m, n> operator*(const Matrix<T, m, n>& lhs, T rhs) noexcept
490 : {
491 902805 : Matrix<T, m, n> copy(lhs);
492 902805 : copy *= rhs;
493 902805 : return copy;
494 : }
495 :
496 : template<typename T, std::size_t m, std::size_t n>
497 230294 : constexpr Matrix<T, m, n> operator*(T lhs, const Matrix<T, m, n>& rhs) noexcept
498 : {
499 230294 : Matrix<T, m, n> copy(rhs);
500 230294 : copy *= lhs;
501 230294 : return copy;
502 : }
503 :
504 : template<typename T, std::size_t m, std::size_t n>
505 2 : constexpr Matrix<T, m, n> operator/(const Matrix<T, m, n>& lhs, T rhs) noexcept
506 : {
507 2 : Matrix<T, m, n> copy(lhs);
508 2 : copy /= rhs;
509 2 : return copy;
510 : }
511 :
512 : template<typename T, std::size_t m, std::size_t n>
513 1 : constexpr Matrix<T, m, n> operator/(T lhs, const Matrix<T, m, n>& rhs) noexcept
514 : {
515 1 : Matrix<T, m, n> copy(rhs);
516 1 : copy /= lhs;
517 1 : return copy;
518 : }
519 :
520 : template<typename T, std::size_t m, std::size_t n>
521 1 : std::ostream& operator<<(std::ostream& os, const Matrix<T, m, n>& mat)
522 : {
523 1 : return os << mat.str();
524 : }
525 :
526 : } // namespace Kinematics
527 : } // namespace HLR
|
