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 1627113 : constexpr std::size_t Matrix<T, m, n>::get_m() const noexcept
61 : {
62 1627113 : 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 91136559 : constexpr std::array<T, n>& Matrix<T, m, n>::operator[](
73 : const std::size_t pos) noexcept
74 : {
75 91136559 : return this->data[pos];
76 : }
77 :
78 : template<typename T, std::size_t m, std::size_t n>
79 44698937 : constexpr const std::array<T, n>& Matrix<T, m, n>::operator[](
80 : const std::size_t pos) const noexcept
81 : {
82 44698937 : 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 1957189 : constexpr void Matrix<T, m, n>::operator*=(T scalar) noexcept
158 : {
159 8477051 : for (std::size_t i = 0; i < m; ++i)
160 : {
161 13039748 : for (std::size_t j = 0; j < n; ++j)
162 : {
163 6519886 : (*this)[i][j] *= scalar;
164 : }
165 : }
166 1957189 : }
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 1626867 : constexpr void Matrix<T, m, n>::operator+=(const Matrix<T, m, n>& mat) noexcept
182 : {
183 6837757 : for (std::size_t i = 0; i < m; ++i)
184 : {
185 10421796 : for (std::size_t j = 0; j < n; ++j)
186 : {
187 5210906 : (*this)[i][j] += mat[i][j];
188 : }
189 : }
190 1626867 : }
191 :
192 : template<typename T, std::size_t m, std::size_t n>
193 978591 : constexpr void Matrix<T, m, n>::operator-=(const Matrix<T, m, n>& mat) noexcept
194 : {
195 978591 : (*this) += mat * static_cast<T>(-1);
196 978591 : }
197 :
198 : template<typename T, std::size_t m, std::size_t n>
199 648275 : constexpr Matrix<T, m, n> Matrix<T, m, n>::operator+(
200 : const Matrix<T, m, n>& mat) const noexcept
201 : {
202 648275 : Matrix<T, m, n> copy(*this);
203 648275 : copy += mat;
204 648275 : return copy;
205 : }
206 :
207 : template<typename T, std::size_t m, std::size_t n>
208 978590 : constexpr Matrix<T, m, n> Matrix<T, m, n>::operator-(
209 : const Matrix<T, m, n>& mat) const noexcept
210 : {
211 978590 : Matrix<T, m, n> copy(*this);
212 978590 : copy -= mat;
213 978590 : return copy;
214 : }
215 :
216 : template<typename T, std::size_t m, std::size_t n>
217 : template<std::size_t p>
218 978601 : constexpr Matrix<T, m, p> Matrix<T, m, n>::operator*(
219 : const Matrix<T, n, p>& mat) const noexcept
220 : {
221 978601 : Matrix<T, m, p> product;
222 :
223 2617792 : for (std::size_t i = 0; i < m; ++i)
224 : {
225 3278398 : for (std::size_t j = 0; j < p; ++j)
226 : {
227 6556844 : for (std::size_t k = 0; k < n; ++k)
228 : {
229 4917637 : product[i][j] += (*this)[i][k] * mat[k][j];
230 : }
231 : }
232 : }
233 :
234 978601 : return product;
235 : }
236 :
237 : template<typename T, std::size_t m, std::size_t n>
238 : template<std::size_t p>
239 330299 : constexpr Matrix<T, m, n + p> Matrix<T, m, n>::horizontal_augment(
240 : const Matrix<T, m, p>& mat) const noexcept
241 : {
242 330299 : Matrix<T, m, n + p> augmented;
243 :
244 1321196 : for (std::size_t i = 0; i < m; ++i)
245 : {
246 3963586 : for (std::size_t j = 0; j < n; ++j)
247 : {
248 2972689 : augmented[i][j] = (*this)[i][j];
249 : }
250 :
251 3963590 : for (std::size_t j = n; j < n + p; ++j)
252 : {
253 2972693 : augmented[i][j] = mat[i][j - n];
254 : }
255 : }
256 :
257 330299 : 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 978585 : constexpr Matrix<T, p, q> Matrix<T, m, n>::slice() const noexcept
286 : {
287 978585 : Matrix<T, p, q> mat;
288 :
289 3914340 : for (std::size_t i = y; i < m && i - y < p; ++i)
290 : {
291 7853293 : for (std::size_t j = x; j < n && j - x < q; ++j)
292 : {
293 4917538 : mat[i - y][j - x] = (*this)[i][j];
294 : }
295 : }
296 :
297 978585 : return mat;
298 : }
299 :
300 : template<typename T, std::size_t m, std::size_t n>
301 648309 : constexpr Matrix<T, n, m> Matrix<T, m, n>::transpose() const noexcept
302 : {
303 648309 : Matrix<T, n, m> transposed;
304 :
305 2593232 : for (std::size_t i = 0; i < m; ++i)
306 : {
307 3889858 : for (std::size_t j = 0; j < n; ++j)
308 : {
309 1944935 : transposed[j][i] = (*this)[i][j];
310 : }
311 : }
312 :
313 648309 : return transposed;
314 : }
315 :
316 : template<typename T, std::size_t m, std::size_t n>
317 330297 : 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 330297 : auto identity = Matrix<T, m, n>::get_identity();
324 330297 : auto augmented = this->horizontal_augment(identity);
325 :
326 1321185 : 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 990890 : std::size_t min_row_index = i;
336 990890 : std::optional<T> closest_to_one;
337 :
338 2972673 : for (std::size_t j = min_row_index; j < m; ++j)
339 : {
340 1981783 : if (std::abs(augmented[j][i]) < 0.000000001)
341 : {
342 380355 : continue;
343 : }
344 :
345 1601428 : T abs_value = std::abs(1 - std::abs(augmented[j][i]));
346 :
347 3202856 : if (!closest_to_one.has_value()
348 1601428 : || abs_value < closest_to_one.value())
349 : {
350 1115782 : closest_to_one = abs_value;
351 1115782 : min_row_index = j;
352 : }
353 : }
354 :
355 990890 : if (!closest_to_one.has_value())
356 : {
357 2 : return std::nullopt;
358 : }
359 :
360 990888 : if (min_row_index != i)
361 : {
362 1224663 : for (std::size_t j = 0; j < 2 * m; ++j)
363 : {
364 1049712 : T tmp = augmented[i][j];
365 1049712 : augmented[i][j] = augmented[min_row_index][j];
366 1049712 : 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 990888 : auto divisor = augmented[i][i];
373 :
374 6936224 : for (std::size_t j = 0; j < 2 * m; ++j)
375 : {
376 5945336 : augmented[i][j] /= divisor;
377 : }
378 :
379 : // Set all other rows to 0 by using the row that is 1.
380 3963556 : for (std::size_t j = 0; j < m; ++j)
381 : {
382 2972668 : if (i == j)
383 : {
384 990888 : continue;
385 : }
386 :
387 1981780 : auto multiplier = augmented[j][i];
388 :
389 13872484 : for (std::size_t k = 0; k < 2 * m; ++k)
390 : {
391 11890704 : augmented[j][k] -= multiplier * augmented[i][k];
392 : }
393 : }
394 : }
395 :
396 330295 : return augmented.template slice<0, m, m, m>();
397 : }
398 :
399 : template<typename T, std::size_t m, std::size_t n>
400 648306 : 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 648306 : 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 648305 : magnitude =
416 1296610 : static_cast<T>(std::sqrt((this->transpose() * (*this))[0][0]));
417 : }
418 :
419 648306 : 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 1308886 : constexpr Matrix<T, m, n> operator*(const Matrix<T, m, n>& lhs, T rhs) noexcept
474 : {
475 1308886 : Matrix<T, m, n> copy(lhs);
476 1308886 : copy *= rhs;
477 1308886 : return copy;
478 : }
479 :
480 : template<typename T, std::size_t m, std::size_t n>
481 330294 : constexpr Matrix<T, m, n> operator*(T lhs, const Matrix<T, m, n>& rhs) noexcept
482 : {
483 330294 : Matrix<T, m, n> copy(rhs);
484 330294 : copy *= lhs;
485 330294 : 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
|
