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 2266409 : constexpr std::size_t Matrix<T, m, n>::get_m() const noexcept
61 : {
62 2266409 : 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 78403796 : constexpr std::array<T, n>& Matrix<T, m, n>::operator[](
73 : const std::size_t pos) noexcept
74 : {
75 78403796 : return this->data[pos];
76 : }
77 :
78 : template<typename T, std::size_t m, std::size_t n>
79 49378903 : constexpr const std::array<T, n>& Matrix<T, m, n>::operator[](
80 : const std::size_t pos) const noexcept
81 : {
82 49378903 : 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 230339 : constexpr bool Matrix<T, m, n>::fuzzy_equal(const Matrix<T, m, n>& mat,
110 : T fuzz) const noexcept
111 : {
112 921328 : for (std::size_t i = 0; i < m; ++i)
113 : {
114 2763986 : for (std::size_t j = 0; j < n; ++j)
115 : {
116 2072997 : if (std::abs((*this)[i][j] - mat[i][j]) > fuzz)
117 : {
118 5 : return false;
119 : }
120 : }
121 : }
122 :
123 230334 : return true;
124 : }
125 :
126 : template<typename T, std::size_t m, std::size_t n>
127 36 : constexpr bool Matrix<T, m, n>::operator==(const Matrix<T, m, n>& mat) noexcept
128 : {
129 36 : return this->fuzzy_equal(mat, 0);
130 : }
131 :
132 : template<typename T, std::size_t m, std::size_t n>
133 3 : constexpr bool Matrix<T, m, n>::operator!=(const Matrix<T, m, n>& mat) noexcept
134 : {
135 3 : return !(*this == mat);
136 : }
137 :
138 : template<typename T, std::size_t m, std::size_t n>
139 8 : constexpr void Matrix<T, m, n>::operator+=(T scalar) noexcept
140 : {
141 24 : for (std::size_t i = 0; i < m; ++i)
142 : {
143 64 : for (std::size_t j = 0; j < n; ++j)
144 : {
145 48 : (*this)[i][j] += scalar;
146 : }
147 : }
148 8 : }
149 :
150 : template<typename T, std::size_t m, std::size_t n>
151 4 : constexpr void Matrix<T, m, n>::operator-=(T scalar) noexcept
152 : {
153 4 : (*this) += -1 * scalar;
154 4 : }
155 :
156 : template<typename T, std::size_t m, std::size_t n>
157 1345027 : constexpr void Matrix<T, m, n>::operator*=(T scalar) noexcept
158 : {
159 5822324 : for (std::size_t i = 0; i < m; ++i)
160 : {
161 8954618 : for (std::size_t j = 0; j < n; ++j)
162 : {
163 4477321 : (*this)[i][j] *= scalar;
164 : }
165 : }
166 1345027 : }
167 :
168 : template<typename T, std::size_t m, std::size_t n>
169 4 : constexpr void Matrix<T, m, n>::operator/=(T scalar) noexcept
170 : {
171 12 : for (std::size_t i = 0; i < m; ++i)
172 : {
173 32 : for (std::size_t j = 0; j < n; ++j)
174 : {
175 24 : (*this)[i][j] /= scalar;
176 : }
177 : }
178 4 : }
179 :
180 : template<typename T, std::size_t m, std::size_t n>
181 1114702 : constexpr void Matrix<T, m, n>::operator+=(const Matrix<T, m, n>& mat) noexcept
182 : {
183 4689097 : for (std::size_t i = 0; i < m; ++i)
184 : {
185 7148806 : for (std::size_t j = 0; j < n; ++j)
186 : {
187 3574411 : (*this)[i][j] += mat[i][j];
188 : }
189 : }
190 1114702 : }
191 :
192 : template<typename T, std::size_t m, std::size_t n>
193 672508 : constexpr void Matrix<T, m, n>::operator-=(const Matrix<T, m, n>& mat) noexcept
194 : {
195 672508 : (*this) += mat * static_cast<T>(-1);
196 672508 : }
197 :
198 : template<typename T, std::size_t m, std::size_t n>
199 442193 : constexpr Matrix<T, m, n> Matrix<T, m, n>::operator+(
200 : const Matrix<T, m, n>& mat) const noexcept
201 : {
202 442193 : Matrix<T, m, n> copy(*this);
203 442193 : copy += mat;
204 442193 : return copy;
205 : }
206 :
207 : template<typename T, std::size_t m, std::size_t n>
208 672507 : constexpr Matrix<T, m, n> Matrix<T, m, n>::operator-(
209 : const Matrix<T, m, n>& mat) const noexcept
210 : {
211 672507 : Matrix<T, m, n> copy(*this);
212 672507 : copy -= mat;
213 672507 : return copy;
214 : }
215 :
216 : template<typename T, std::size_t m, std::size_t n>
217 : template<std::size_t p>
218 902825 : constexpr Matrix<T, m, p> Matrix<T, m, n>::operator*(
219 : const Matrix<T, n, p>& mat) const noexcept
220 : {
221 902825 : Matrix<T, m, p> product;
222 :
223 2726833 : for (std::size_t i = 0; i < m; ++i)
224 : {
225 5029814 : for (std::size_t j = 0; j < p; ++j)
226 : {
227 12823256 : for (std::size_t k = 0; k < n; ++k)
228 : {
229 9617450 : product[i][j] += (*this)[i][k] * mat[k][j];
230 : }
231 : }
232 : }
233 :
234 902825 : return product;
235 : }
236 :
237 : template<typename T, std::size_t m, std::size_t n>
238 : template<std::size_t p>
239 230299 : constexpr Matrix<T, m, n + p> Matrix<T, m, n>::horizontal_augment(
240 : const Matrix<T, m, p>& mat) const noexcept
241 : {
242 230299 : Matrix<T, m, n + p> augmented;
243 :
244 921196 : for (std::size_t i = 0; i < m; ++i)
245 : {
246 2763586 : for (std::size_t j = 0; j < n; ++j)
247 : {
248 2072689 : augmented[i][j] = (*this)[i][j];
249 : }
250 :
251 2763590 : for (std::size_t j = n; j < n + p; ++j)
252 : {
253 2072693 : augmented[i][j] = mat[i][j - n];
254 : }
255 : }
256 :
257 230299 : return augmented;
258 : }
259 :
260 : template<typename T, std::size_t m, std::size_t n>
261 : template<std::size_t p>
262 2 : constexpr Matrix<T, m + p, n> Matrix<T, m, n>::vertical_augment(
263 : const Matrix<T, p, n>& mat) const noexcept
264 : {
265 2 : Matrix<T, m + p, n> augmented;
266 :
267 5 : for (std::size_t i = 0; i < n; ++i)
268 : {
269 10 : for (std::size_t j = 0; j < m; ++j)
270 : {
271 7 : augmented[j][i] = (*this)[j][i];
272 : }
273 :
274 11 : for (std::size_t j = m; j < m + p; ++j)
275 : {
276 8 : augmented[j][i] = mat[j - m][i];
277 : }
278 : }
279 :
280 2 : return augmented;
281 : }
282 :
283 : template<typename T, std::size_t m, std::size_t n>
284 : template<std::size_t y, std::size_t x, std::size_t p, std::size_t q>
285 902800 : constexpr Matrix<T, p, q> Matrix<T, m, n>::slice() const noexcept
286 : {
287 902800 : Matrix<T, p, q> mat;
288 :
289 3611201 : for (std::size_t i = y; i < m && i - y < p; ++i)
290 : {
291 8180367 : for (std::size_t j = x; j < n && j - x < q; ++j)
292 : {
293 5471966 : mat[i - y][j - x] = (*this)[i][j];
294 : }
295 : }
296 :
297 902800 : return mat;
298 : }
299 :
300 : template<typename T, std::size_t m, std::size_t n>
301 442237 : constexpr Matrix<T, n, m> Matrix<T, m, n>::transpose() const noexcept
302 : {
303 442237 : Matrix<T, n, m> transposed;
304 :
305 1768944 : for (std::size_t i = 0; i < m; ++i)
306 : {
307 2653426 : for (std::size_t j = 0; j < n; ++j)
308 : {
309 1326719 : transposed[j][i] = (*this)[i][j];
310 : }
311 : }
312 :
313 442237 : return transposed;
314 : }
315 :
316 : template<typename T, std::size_t m, std::size_t n>
317 230297 : constexpr std::optional<Matrix<T, m, n>> Matrix<T, m, n>::inverse() const
318 : noexcept
319 : {
320 : static_assert(m == n,
321 : "Inverse matrix is only defined for square matrices.");
322 :
323 230297 : const auto identity = Matrix<T, m, n>::get_identity();
324 230297 : auto augmented = this->horizontal_augment(identity);
325 :
326 921186 : for (std::size_t i = 0; i < m; ++i)
327 : {
328 : /**
329 : * Find the diagonal element closest to -1 or 1.
330 : * And if it isn't in the current row swap it.
331 : * This is done to make the inverse mroe numerically stable.
332 : * Also check if that the row may not be zero since that would
333 : * result in a divide by zero error.
334 : */
335 690890 : std::size_t min_row_index = i;
336 690890 : std::optional<T> closest_to_one;
337 :
338 2072673 : for (std::size_t j = min_row_index; j < m; ++j)
339 : {
340 1381783 : if (std::abs(augmented[j][i]) <= std::numeric_limits<T>::min())
341 : {
342 280331 : continue;
343 : }
344 :
345 1101452 : const T abs_value = std::abs(1 - std::abs(augmented[j][i]));
346 :
347 2202904 : if (!closest_to_one.has_value()
348 1101452 : || abs_value < closest_to_one.value())
349 : {
350 1016750 : closest_to_one = abs_value;
351 1016750 : min_row_index = j;
352 : }
353 : }
354 :
355 690890 : if (!closest_to_one.has_value())
356 : {
357 1 : return std::nullopt;
358 : }
359 :
360 690889 : if (min_row_index != i)
361 : {
362 2631271 : for (std::size_t j = 0; j < 2 * m; ++j)
363 : {
364 2255376 : T tmp = augmented[i][j];
365 2255376 : augmented[i][j] = augmented[min_row_index][j];
366 2255376 : augmented[min_row_index][j] = tmp;
367 : }
368 : }
369 :
370 : // Divide the current row by a scalar so that the current diagonal
371 : // element will be 1.
372 690889 : const auto divisor = augmented[i][i];
373 :
374 4836231 : for (std::size_t j = 0; j < 2 * m; ++j)
375 : {
376 4145342 : augmented[i][j] /= divisor;
377 : }
378 :
379 : // Set all other rows to 0 by using the row that is 1.
380 2763560 : for (std::size_t j = 0; j < m; ++j)
381 : {
382 2072671 : if (i == j)
383 : {
384 690889 : continue;
385 : }
386 :
387 1381782 : const auto multiplier = augmented[j][i];
388 :
389 9672498 : for (std::size_t k = 0; k < 2 * m; ++k)
390 : {
391 8290716 : augmented[j][k] -= multiplier * augmented[i][k];
392 : }
393 : }
394 : }
395 :
396 : /**
397 : * Check if the inversed matrix times the original matrix is equal
398 : * to the identity matrix. The equal precision is the precision
399 : * when the identity matrix is considered equal.
400 : */
401 230296 : const auto inversed = augmented.template slice<0, m, m, m>();
402 230296 : const auto identity_check = inversed * (*this);
403 230296 : constexpr auto equal_precision = 0.0000000001;
404 :
405 230296 : if (!identity_check.fuzzy_equal(identity, equal_precision))
406 : {
407 1 : return std::nullopt;
408 : }
409 :
410 230295 : return augmented.template slice<0, m, m, m>();
411 : }
412 :
413 : template<typename T, std::size_t m, std::size_t n>
414 442234 : constexpr T Matrix<T, m, n>::get_magnitude() const noexcept
415 : {
416 : static_assert(
417 : m == 1 || n == 1,
418 : "Magnitude of a matrix is only defined for column and row matrices.");
419 :
420 442234 : T magnitude = 0;
421 :
422 : if constexpr (m == 1) // NOLINT
423 : {
424 1 : magnitude =
425 2 : static_cast<T>(std::sqrt(((*this) * this->transpose())[0][0]));
426 : }
427 : else
428 : {
429 442233 : magnitude =
430 884466 : static_cast<T>(std::sqrt((this->transpose() * (*this))[0][0]));
431 : }
432 :
433 442234 : return magnitude;
434 : }
435 :
436 : template<typename T, std::size_t m, std::size_t n>
437 1 : std::string Matrix<T, m, n>::str() const
438 : {
439 2 : std::stringstream ss;
440 1 : ss << std::setprecision(9) << std::setw(6) << std::fixed;
441 :
442 3 : for (std::size_t i = 0; i < m; ++i)
443 : {
444 6 : for (std::size_t j = 0; j < n; ++j)
445 : {
446 4 : ss << (*this)[i][j] << "\t";
447 : }
448 2 : ss << std::endl;
449 : }
450 :
451 2 : return ss.str();
452 : }
453 :
454 : template<typename T, std::size_t m, std::size_t n>
455 2 : constexpr Matrix<T, m, n> operator+(const Matrix<T, m, n>& lhs, T rhs) noexcept
456 : {
457 2 : Matrix<T, m, n> copy(lhs);
458 2 : copy += rhs;
459 2 : return copy;
460 : }
461 :
462 : template<typename T, std::size_t m, std::size_t n>
463 1 : constexpr Matrix<T, m, n> operator+(T lhs, const Matrix<T, m, n>& rhs) noexcept
464 : {
465 1 : Matrix<T, m, n> copy(rhs);
466 1 : copy += lhs;
467 1 : return copy;
468 : }
469 :
470 : template<typename T, std::size_t m, std::size_t n>
471 2 : constexpr Matrix<T, m, n> operator-(const Matrix<T, m, n>& lhs, T rhs) noexcept
472 : {
473 2 : Matrix<T, m, n> copy(lhs);
474 2 : copy -= rhs;
475 2 : return copy;
476 : }
477 :
478 : template<typename T, std::size_t m, std::size_t n>
479 1 : constexpr Matrix<T, m, n> operator-(T lhs, const Matrix<T, m, n>& rhs) noexcept
480 : {
481 1 : Matrix<T, m, n> copy(rhs);
482 1 : copy -= lhs;
483 1 : return copy;
484 : }
485 :
486 : template<typename T, std::size_t m, std::size_t n>
487 902803 : constexpr Matrix<T, m, n> operator*(const Matrix<T, m, n>& lhs, T rhs) noexcept
488 : {
489 902803 : Matrix<T, m, n> copy(lhs);
490 902803 : copy *= rhs;
491 902803 : return copy;
492 : }
493 :
494 : template<typename T, std::size_t m, std::size_t n>
495 230294 : constexpr Matrix<T, m, n> operator*(T lhs, const Matrix<T, m, n>& rhs) noexcept
496 : {
497 230294 : Matrix<T, m, n> copy(rhs);
498 230294 : copy *= lhs;
499 230294 : return copy;
500 : }
501 :
502 : template<typename T, std::size_t m, std::size_t n>
503 2 : constexpr Matrix<T, m, n> operator/(const Matrix<T, m, n>& lhs, T rhs) noexcept
504 : {
505 2 : Matrix<T, m, n> copy(lhs);
506 2 : copy /= rhs;
507 2 : return copy;
508 : }
509 :
510 : template<typename T, std::size_t m, std::size_t n>
511 1 : constexpr Matrix<T, m, n> operator/(T lhs, const Matrix<T, m, n>& rhs) noexcept
512 : {
513 1 : Matrix<T, m, n> copy(rhs);
514 1 : copy /= lhs;
515 1 : return copy;
516 : }
517 :
518 : template<typename T, std::size_t m, std::size_t n>
519 1 : std::ostream& operator<<(std::ostream& os, const Matrix<T, m, n>& mat)
520 : {
521 1 : return os << mat.str();
522 : }
523 :
524 : } // namespace Kinematics
525 : } // namespace HLR
|
