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 877436 : constexpr std::size_t Matrix<T, m, n>::get_m() const noexcept
61 : {
62 877436 : 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 53819894 : constexpr std::array<T, n>& Matrix<T, m, n>::operator[](
73 : const std::size_t pos) noexcept
74 : {
75 53819894 : return this->data[pos];
76 : }
77 :
78 : template<typename T, std::size_t m, std::size_t n>
79 24159279 : constexpr const std::array<T, n>& Matrix<T, m, n>::operator[](
80 : const std::size_t pos) const noexcept
81 : {
82 24159279 : 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 43 : constexpr bool Matrix<T, m, n>::fuzzy_equal(const Matrix<T, m, n>& mat,
110 : T fuzz) const noexcept
111 : {
112 146 : for (std::size_t i = 0; i < m; ++i)
113 : {
114 436 : for (std::size_t j = 0; j < n; ++j)
115 : {
116 333 : if (std::abs((*this)[i][j] - mat[i][j]) > fuzz)
117 : {
118 4 : return false;
119 : }
120 : }
121 : }
122 :
123 39 : 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 1057668 : constexpr void Matrix<T, m, n>::operator*=(T scalar) noexcept
158 : {
159 4579208 : for (std::size_t i = 0; i < m; ++i)
160 : {
161 7043104 : for (std::size_t j = 0; j < n; ++j)
162 : {
163 3521564 : (*this)[i][j] *= scalar;
164 : }
165 : }
166 1057668 : }
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 877346 : constexpr void Matrix<T, m, n>::operator+=(const Matrix<T, m, n>& mat) noexcept
182 : {
183 3689673 : for (std::size_t i = 0; i < m; ++i)
184 : {
185 5624670 : for (std::size_t j = 0; j < n; ++j)
186 : {
187 2812343 : (*this)[i][j] += mat[i][j];
188 : }
189 : }
190 877346 : }
191 :
192 : template<typename T, std::size_t m, std::size_t n>
193 528829 : constexpr void Matrix<T, m, n>::operator-=(const Matrix<T, m, n>& mat) noexcept
194 : {
195 528829 : (*this) += mat * static_cast<T>(-1);
196 528829 : }
197 :
198 : template<typename T, std::size_t m, std::size_t n>
199 348516 : constexpr Matrix<T, m, n> Matrix<T, m, n>::operator+(
200 : const Matrix<T, m, n>& mat) const noexcept
201 : {
202 348516 : Matrix<T, m, n> copy(*this);
203 348516 : copy += mat;
204 348516 : return copy;
205 : }
206 :
207 : template<typename T, std::size_t m, std::size_t n>
208 528828 : constexpr Matrix<T, m, n> Matrix<T, m, n>::operator-(
209 : const Matrix<T, m, n>& mat) const noexcept
210 : {
211 528828 : Matrix<T, m, n> copy(*this);
212 528828 : copy -= mat;
213 528828 : return copy;
214 : }
215 :
216 : template<typename T, std::size_t m, std::size_t n>
217 : template<std::size_t p>
218 528848 : constexpr Matrix<T, m, p> Matrix<T, m, n>::operator*(
219 : const Matrix<T, n, p>& mat) const noexcept
220 : {
221 528848 : Matrix<T, m, p> product;
222 :
223 1418286 : for (std::size_t i = 0; i < m; ++i)
224 : {
225 1778892 : for (std::size_t j = 0; j < p; ++j)
226 : {
227 3557832 : for (std::size_t k = 0; k < n; ++k)
228 : {
229 2668378 : product[i][j] += (*this)[i][k] * mat[k][j];
230 : }
231 : }
232 : }
233 :
234 528848 : return product;
235 : }
236 :
237 : template<typename T, std::size_t m, std::size_t n>
238 : template<std::size_t p>
239 180299 : constexpr Matrix<T, m, n + p> Matrix<T, m, n>::horizontal_augment(
240 : const Matrix<T, m, p>& mat) const noexcept
241 : {
242 180299 : Matrix<T, m, n + p> augmented;
243 :
244 721196 : for (std::size_t i = 0; i < m; ++i)
245 : {
246 2163586 : for (std::size_t j = 0; j < n; ++j)
247 : {
248 1622689 : augmented[i][j] = (*this)[i][j];
249 : }
250 :
251 2163590 : for (std::size_t j = n; j < n + p; ++j)
252 : {
253 1622693 : augmented[i][j] = mat[i][j - n];
254 : }
255 : }
256 :
257 180299 : 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 528826 : constexpr Matrix<T, p, q> Matrix<T, m, n>::slice() const noexcept
286 : {
287 528826 : Matrix<T, p, q> mat;
288 :
289 2115304 : for (std::size_t i = y; i < m && i - y < p; ++i)
290 : {
291 4254739 : for (std::size_t j = x; j < n && j - x < q; ++j)
292 : {
293 2668261 : mat[i - y][j - x] = (*this)[i][j];
294 : }
295 : }
296 :
297 528826 : return mat;
298 : }
299 :
300 : template<typename T, std::size_t m, std::size_t n>
301 348556 : constexpr Matrix<T, n, m> Matrix<T, m, n>::transpose() const noexcept
302 : {
303 348556 : Matrix<T, n, m> transposed;
304 :
305 1394220 : for (std::size_t i = 0; i < m; ++i)
306 : {
307 2091340 : for (std::size_t j = 0; j < n; ++j)
308 : {
309 1045676 : transposed[j][i] = (*this)[i][j];
310 : }
311 : }
312 :
313 348556 : return transposed;
314 : }
315 :
316 : template<typename T, std::size_t m, std::size_t n>
317 180297 : 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 180297 : auto identity = Matrix<T, m, n>::get_identity();
324 180297 : auto augmented = this->horizontal_augment(identity);
325 :
326 721185 : 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 540890 : std::size_t min_row_index = i;
336 540890 : std::optional<T> closest_to_one;
337 :
338 1622673 : for (std::size_t j = min_row_index; j < m; ++j)
339 : {
340 1081783 : if (std::abs(augmented[j][i]) < 0.000000001)
341 : {
342 230325 : continue;
343 : }
344 :
345 851458 : T abs_value = std::abs(1 - std::abs(augmented[j][i]));
346 :
347 1702916 : if (!closest_to_one.has_value()
348 851458 : || abs_value < closest_to_one.value())
349 : {
350 766756 : closest_to_one = abs_value;
351 766756 : min_row_index = j;
352 : }
353 : }
354 :
355 540890 : if (!closest_to_one.has_value())
356 : {
357 2 : return std::nullopt;
358 : }
359 :
360 540888 : if (min_row_index != i)
361 : {
362 1931271 : for (std::size_t j = 0; j < 2 * m; ++j)
363 : {
364 1655376 : T tmp = augmented[i][j];
365 1655376 : augmented[i][j] = augmented[min_row_index][j];
366 1655376 : 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 540888 : auto divisor = augmented[i][i];
373 :
374 3786224 : for (std::size_t j = 0; j < 2 * m; ++j)
375 : {
376 3245336 : augmented[i][j] /= divisor;
377 : }
378 :
379 : // Set all other rows to 0 by using the row that is 1.
380 2163556 : for (std::size_t j = 0; j < m; ++j)
381 : {
382 1622668 : if (i == j)
383 : {
384 540888 : continue;
385 : }
386 :
387 1081780 : auto multiplier = augmented[j][i];
388 :
389 7572484 : for (std::size_t k = 0; k < 2 * m; ++k)
390 : {
391 6490704 : augmented[j][k] -= multiplier * augmented[i][k];
392 : }
393 : }
394 : }
395 :
396 180295 : return augmented.template slice<0, m, m, m>();
397 : }
398 :
399 : template<typename T, std::size_t m, std::size_t n>
400 348553 : constexpr T Matrix<T, m, n>::get_magnitude() const noexcept
401 : {
402 : static_assert(
403 : m == 1 || n == 1,
404 : "Magnitude of a matrix is only defined for column and row matrices.");
405 :
406 348553 : T magnitude = 0;
407 :
408 : if constexpr (m == 1) // NOLINT
409 : {
410 1 : magnitude =
411 2 : static_cast<T>(std::sqrt(((*this) * this->transpose())[0][0]));
412 : }
413 : else
414 : {
415 348552 : magnitude =
416 697104 : static_cast<T>(std::sqrt((this->transpose() * (*this))[0][0]));
417 : }
418 :
419 348553 : return magnitude;
420 : }
421 :
422 : template<typename T, std::size_t m, std::size_t n>
423 1 : std::string Matrix<T, m, n>::str() const
424 : {
425 2 : std::stringstream ss;
426 1 : ss << std::setprecision(9) << std::setw(6) << std::fixed;
427 :
428 3 : for (std::size_t i = 0; i < m; ++i)
429 : {
430 6 : for (std::size_t j = 0; j < n; ++j)
431 : {
432 4 : ss << (*this)[i][j] << "\t";
433 : }
434 2 : ss << std::endl;
435 : }
436 :
437 2 : return ss.str();
438 : }
439 :
440 : template<typename T, std::size_t m, std::size_t n>
441 2 : constexpr Matrix<T, m, n> operator+(const Matrix<T, m, n>& lhs, T rhs) noexcept
442 : {
443 2 : Matrix<T, m, n> copy(lhs);
444 2 : copy += rhs;
445 2 : return copy;
446 : }
447 :
448 : template<typename T, std::size_t m, std::size_t n>
449 1 : constexpr Matrix<T, m, n> operator+(T lhs, const Matrix<T, m, n>& rhs) noexcept
450 : {
451 1 : Matrix<T, m, n> copy(rhs);
452 1 : copy += lhs;
453 1 : return copy;
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 709124 : constexpr Matrix<T, m, n> operator*(const Matrix<T, m, n>& lhs, T rhs) noexcept
474 : {
475 709124 : Matrix<T, m, n> copy(lhs);
476 709124 : copy *= rhs;
477 709124 : return copy;
478 : }
479 :
480 : template<typename T, std::size_t m, std::size_t n>
481 180294 : constexpr Matrix<T, m, n> operator*(T lhs, const Matrix<T, m, n>& rhs) noexcept
482 : {
483 180294 : Matrix<T, m, n> copy(rhs);
484 180294 : copy *= lhs;
485 180294 : return copy;
486 : }
487 :
488 : template<typename T, std::size_t m, std::size_t n>
489 2 : constexpr Matrix<T, m, n> operator/(const Matrix<T, m, n>& lhs, T rhs) noexcept
490 : {
491 2 : Matrix<T, m, n> copy(lhs);
492 2 : copy /= rhs;
493 2 : return copy;
494 : }
495 :
496 : template<typename T, std::size_t m, std::size_t n>
497 1 : constexpr Matrix<T, m, n> operator/(T lhs, const Matrix<T, m, n>& rhs) noexcept
498 : {
499 1 : Matrix<T, m, n> copy(rhs);
500 1 : copy /= lhs;
501 1 : return copy;
502 : }
503 :
504 : template<typename T, std::size_t m, std::size_t n>
505 1 : std::ostream& operator<<(std::ostream& os, const Matrix<T, m, n>& mat)
506 : {
507 1 : return os << mat.str();
508 : }
509 :
510 : } // namespace Kinematics
511 : } // namespace HLR
|
