Line data Source code
1 : #ifndef E4BF6CCD_1BFD_497E_A5CD_75BCD8DD1ED3
2 : #define E4BF6CCD_1BFD_497E_A5CD_75BCD8DD1ED3
3 :
4 : #include "math/linalg/matrix.h"
5 :
6 : #include <algorithm>
7 : #include <cmath>
8 : #include <functional>
9 : #include <limits>
10 : #include <stdexcept>
11 : #include <string>
12 : #include <type_traits>
13 :
14 : namespace tracking
15 : {
16 : namespace math
17 : {
18 :
19 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
20 263 : inline void Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::setZeros()
21 : {
22 866 : data().fill(static_cast<ValueType_>(0));
23 : }
24 :
25 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
26 88 : inline auto Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::Zeros() -> Matrix
27 : {
28 653 : Matrix tmp;
29 3590 : tmp.setZeros();
30 : return tmp;
31 : }
32 :
33 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
34 673 : inline void Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::setOnes()
35 : {
36 1021 : data().fill(static_cast<ValueType_>(1));
37 : }
38 :
39 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
40 20 : inline auto Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::Ones() -> Matrix
41 : {
42 20 : Matrix tmp;
43 228 : tmp.setOnes();
44 : return tmp;
45 : }
46 :
47 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
48 643 : inline auto Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::FromList(
49 : const std::initializer_list<std::initializer_list<ValueType_>>& list) -> Matrix
50 : {
51 643 : Matrix result{};
52 :
53 : // Validate row count - input is always in logical row-major format
54 643 : if (list.size() != static_cast<std::size_t>(Rows))
55 : {
56 28 : throw std::runtime_error("Matrix::FromList: expected " + std::to_string(Rows) + " rows, got " + std::to_string(list.size()));
57 : }
58 :
59 : // Validate column count for each row - input is always in logical row-major format
60 2441 : for (const auto& row : list)
61 : {
62 1806 : if (row.size() != static_cast<std::size_t>(Cols))
63 : {
64 28 : throw std::runtime_error("Matrix::FromList: expected " + std::to_string(Cols) + " columns, got " +
65 : std::to_string(row.size()));
66 : }
67 : }
68 :
69 : if constexpr (IsRowMajor_)
70 : {
71 : // For row-major storage, copy data directly
72 503 : auto iter = result.data().begin();
73 1988 : for (const auto& row : list)
74 : {
75 1485 : std::copy(row.begin(), row.end(), iter);
76 1485 : iter += row.size();
77 : }
78 : }
79 : else
80 : {
81 : // For column-major storage, transpose the input data
82 569 : for (sint32 col = 0; col < Cols; ++col)
83 : {
84 1492 : for (sint32 row = 0; row < Rows; ++row)
85 : {
86 : // Get element from row-major input
87 1055 : auto row_iter = list.begin();
88 1055 : std::advance(row_iter, row);
89 1055 : auto col_iter = row_iter->begin();
90 1055 : std::advance(col_iter, col);
91 :
92 : // Store in column-major order
93 1055 : result.data()[col * Rows + row] = *col_iter;
94 : }
95 : }
96 : }
97 635 : return result;
98 8 : }
99 :
100 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
101 308140 : inline auto Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::at_unsafe(sint32 row, sint32 col) const -> ValueType_
102 : {
103 308036 : assert((0 <= row) && (row < Rows));
104 308036 : assert((0 <= col) && (col < Cols));
105 308142 : const auto idx = IsRowMajor_ ? (row * ColsInMem) + col : (col * ColsInMem) + row;
106 308142 : return data()[idx];
107 : }
108 :
109 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
110 200749 : inline auto Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::at_unsafe(sint32 row, sint32 col) -> ValueType_&
111 : {
112 200749 : assert((0 <= row) && (row < Rows));
113 200749 : assert((0 <= col) && (col < Cols));
114 200925 : const auto idx = IsRowMajor_ ? (row * ColsInMem) + col : (col * ColsInMem) + row;
115 200926 : return data()[idx];
116 : }
117 :
118 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
119 8 : inline auto Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::operator()(sint32 row,
120 : sint32 col) const -> tl::expected<ValueType_, Errors>
121 : {
122 4 : if (!(row >= 0 && row < Rows))
123 : {
124 3 : return tl::unexpected<Errors>{Errors::invalid_access_row};
125 : }
126 3 : if (!(col >= 0 && col < Cols))
127 : {
128 2 : return tl::unexpected<Errors>{Errors::invalid_access_col};
129 : }
130 3 : return at_unsafe(row, col);
131 : }
132 :
133 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
134 20 : inline auto Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::operator()(sint32 row, sint32 col)
135 : -> tl::expected<std::reference_wrapper<ValueType_>, Errors>
136 : {
137 12 : if (!(row >= 0 && row < Rows))
138 : {
139 4 : return tl::unexpected<Errors>{Errors::invalid_access_row};
140 : }
141 12 : if (!(col >= 0 && col < Cols))
142 : {
143 4 : return tl::unexpected<Errors>{Errors::invalid_access_col};
144 : }
145 12 : return at_unsafe(row, col);
146 : }
147 :
148 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
149 : template <bool IsRowMajor2_>
150 49 : inline auto Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::operator==(
151 : const Matrix<ValueType_, Rows_, Cols_, IsRowMajor2_>& other) const -> bool
152 : {
153 : if constexpr (IsRowMajor_ == IsRowMajor2_)
154 : {
155 : // same memory layout: can compare underlying data arrays directly
156 : // TODO(matthias): check whether this only checks pointers or also the elements
157 39 : return this->data() == other.data();
158 : }
159 : else
160 : {
161 : // different memory layouts: compare element-wise
162 24 : bool isEqual = true;
163 90 : for (auto row = 0; row < Rows; ++row)
164 : {
165 264 : for (auto col = 0; col < Cols; ++col)
166 : {
167 216 : isEqual = isEqual && (at_unsafe(row, col) == other.at_unsafe(row, col));
168 : }
169 : }
170 24 : return isEqual;
171 : }
172 : }
173 :
174 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
175 : template <bool IsRowMajor2_>
176 3 : inline auto Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::operator!=(
177 : const Matrix<ValueType_, Rows_, Cols_, IsRowMajor2_>& other) const -> bool
178 : {
179 3 : return !(*this == other);
180 : }
181 :
182 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
183 : template <bool IsRowMajor2_>
184 41 : inline void Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::operator+=(const Matrix<ValueType_, Rows_, Cols_, IsRowMajor2_>& other)
185 : {
186 : if constexpr (IsRowMajor_ == IsRowMajor2_)
187 : {
188 : // same memory layout: can add underlying data arrays directly
189 74 : const auto& otherData = other.data();
190 74 : const sint32 size = data().size();
191 1736 : for (sint32 idx = 0; idx < size; ++idx)
192 : {
193 1662 : data()[idx] += otherData[idx];
194 : }
195 : }
196 : else
197 : {
198 : // different memory layouts: add element-wise
199 : // When both matrixes share the same underlying data and are square Matrixes (e.g., transposed view),
200 : // we must make a copy to avoid read-after-write corruption.
201 : // Example: a += a.transpose() would overwrite a[0][1] before reading it as a.transpose()[1][0]
202 : // clang-format off
203 40 : const auto&& tmp = ((*this==other) && (Rows_ == Cols_))
204 18 : ? Matrix<ValueType_, Rows_, Cols_, IsRowMajor2_>{other}
205 : : std::move(other);
206 : // clang-format on
207 84 : for (auto row = 0; row < Rows; ++row)
208 : {
209 248 : for (auto col = 0; col < Cols; ++col)
210 : {
211 186 : at_unsafe(row, col) += tmp.at_unsafe(row, col);
212 : }
213 : }
214 22 : }
215 22 : }
216 :
217 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
218 : template <bool IsRowMajor2_>
219 66 : inline void Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::operator-=(const Matrix<ValueType_, Rows_, Cols_, IsRowMajor2_>& other)
220 : {
221 : if constexpr (IsRowMajor_ == IsRowMajor2_)
222 : {
223 61 : if (this->data().data() == other.data().data())
224 : {
225 : // case: A -= A
226 61 : this->setZeros();
227 : }
228 : else
229 : {
230 : // case: A -= B
231 : const auto& otherData = other.data();
232 : const sint32 size = data().size();
233 428 : for (sint32 idx = 0; idx < size; ++idx)
234 : {
235 370 : data()[idx] -= otherData[idx];
236 : }
237 : }
238 : }
239 : else
240 : {
241 : // different memory layouts: subtract element-wise
242 : // When both matrixes share the same underlying data and are square Matrixes (e.g., transposed view),
243 : // we must make a copy to avoid read-after-write corruption.
244 : // Example: a -= a.transpose() would overwrite a[0][1] before reading it as a.transpose()[1][0]
245 5 : auto&& tmp = ((this->data().data() == other.data().data()) && (Rows_ == Cols_))
246 1 : ? Matrix<ValueType_, Rows_, Cols_, IsRowMajor2_>{other}
247 : : std::move(other);
248 16 : for (auto row = 0; row < Rows; ++row)
249 : {
250 44 : for (auto col = 0; col < Cols; ++col)
251 : {
252 33 : at_unsafe(row, col) -= tmp.at_unsafe(row, col);
253 : }
254 : }
255 5 : }
256 66 : }
257 :
258 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
259 766 : inline void Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::operator*=(ValueType_ scalar)
260 : {
261 7460 : for (auto& val : data())
262 : {
263 6681 : val *= scalar;
264 : }
265 : }
266 :
267 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
268 : template <typename T, typename std::enable_if_t<std::is_integral<T>::value, bool>>
269 3 : inline auto Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::operator/=(T scalar) -> tl::expected<void, Errors>
270 : {
271 2 : if (std::abs(scalar) > static_cast<T>(0))
272 : {
273 2 : inplace_div_by_int_unsafe(scalar);
274 2 : return {};
275 : }
276 1 : return tl::unexpected<Errors>{Errors::divide_by_zero};
277 : }
278 :
279 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
280 : template <typename T, typename std::enable_if_t<std::is_floating_point<T>::value, bool>>
281 758 : inline auto Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::operator/=(T scalar) -> tl::expected<void, Errors>
282 : {
283 758 : if (std::abs(scalar) > std::numeric_limits<value_type>::min())
284 : {
285 757 : inplace_mul_by_inverse_factor_unsafe(scalar);
286 757 : return {};
287 : }
288 1 : return tl::unexpected<Errors>{Errors::divide_by_zero};
289 : }
290 :
291 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
292 : template <bool IsRowMajor2_>
293 60 : inline auto Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::operator+(
294 : const Matrix<ValueType_, Rows_, Cols_, IsRowMajor2_>& other) const -> Matrix
295 : {
296 60 : Matrix res{*this};
297 59 : res += other;
298 45 : return res;
299 : }
300 :
301 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
302 : template <bool IsRowMajor2_>
303 47 : inline auto Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::operator-(
304 : const Matrix<ValueType_, Rows_, Cols_, IsRowMajor2_>& other) const -> Matrix
305 : {
306 47 : Matrix res{*this};
307 47 : res -= other;
308 : return res;
309 : }
310 :
311 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
312 6 : inline auto Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::operator+(ValueType_ scalar) const -> Matrix
313 : {
314 6 : Matrix res{*this};
315 31 : for (auto& val : res.data())
316 : {
317 25 : val += scalar;
318 : }
319 : return res;
320 : }
321 :
322 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
323 5 : inline auto Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::operator-(ValueType_ scalar) const -> Matrix
324 : {
325 5 : Matrix res{*this};
326 26 : for (auto& val : res.data())
327 : {
328 21 : val -= scalar;
329 : }
330 : return res;
331 : }
332 :
333 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
334 6 : inline auto Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::operator*(ValueType_ scalar) const -> Matrix
335 : {
336 6 : Matrix res{*this};
337 6 : res *= scalar;
338 : return res;
339 : }
340 :
341 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
342 : template <typename T, typename std::enable_if_t<std::is_integral<T>::value, bool>>
343 4 : inline auto Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::operator/(T scalar) const -> tl::expected<Matrix, Errors>
344 : {
345 2 : if (std::abs(scalar) > static_cast<T>(0))
346 : {
347 2 : Matrix res{*this};
348 2 : res.inplace_div_by_int_unsafe(scalar);
349 2 : return res;
350 2 : }
351 2 : return tl::unexpected<Errors>{Errors::divide_by_zero};
352 : }
353 :
354 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
355 : template <typename T, typename std::enable_if_t<std::is_floating_point<T>::value, bool>>
356 5 : inline auto Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::operator/(T scalar) const -> tl::expected<Matrix, Errors>
357 : {
358 5 : if (std::abs(scalar) > std::numeric_limits<T>::min())
359 : {
360 2 : Matrix res{*this};
361 2 : res.inplace_mul_by_inverse_factor_unsafe(scalar);
362 2 : return res;
363 2 : }
364 3 : return tl::unexpected<Errors>{Errors::divide_by_zero};
365 : }
366 :
367 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
368 : template <sint32 Cols2_, bool IsRowMajor2_>
369 633 : inline auto Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::operator*(
370 : const Matrix<ValueType_, Cols_, Cols2_, IsRowMajor2_>& other) const -> Matrix<ValueType_, Rows_, Cols2_, IsRowMajor_>
371 : {
372 : using ResultMatrix = Matrix<ValueType_, Rows_, Cols2_, IsRowMajor_>;
373 3589 : ResultMatrix result{ResultMatrix::Zeros()};
374 :
375 : // Optimized loop order (i-j-k) for cache-friendly row-major access (4-8x faster)
376 3589 : for (auto i = 0; i < ResultMatrix::Rows; ++i)
377 : {
378 14348 : for (auto j = 0; j < ResultMatrix::Cols; ++j)
379 : {
380 63216 : for (auto k = 0; k < Cols; ++k)
381 : {
382 51824 : result.at_unsafe(i, j) += at_unsafe(i, k) * other.at_unsafe(k, j);
383 : }
384 : }
385 : }
386 633 : return result;
387 : }
388 :
389 :
390 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
391 140 : inline auto Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::isZeros() const -> bool
392 : {
393 140 : const auto [min, max] = minmax();
394 140 : return (min == static_cast<ValueType_>(0)) && (max == static_cast<ValueType_>(0));
395 : }
396 :
397 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
398 274 : inline auto Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::minmax() const -> std::tuple<ValueType_, ValueType_>
399 : {
400 274 : const auto [min, max] = std::minmax_element(data().begin(), data().end());
401 274 : return std::make_tuple(*min, *max);
402 : }
403 :
404 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
405 : template <typename T, typename std::enable_if_t<std::is_floating_point<T>::value, bool>>
406 4 : inline auto Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::frobenius_norm() const -> ValueType_
407 : {
408 4 : ValueType_ sum_of_squares = static_cast<ValueType_>(0);
409 22 : for (const auto& val : data())
410 : {
411 18 : sum_of_squares += val * val;
412 : }
413 4 : return std::sqrt(sum_of_squares);
414 : }
415 :
416 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
417 : inline auto Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::transpose() const -> const transpose_type& // LCOV_EXCL_LINE
418 : {
419 : return reinterpret_cast<const transpose_type&>(*this);
420 : }
421 :
422 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
423 : inline auto Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::transpose() -> transpose_type& // LCOV_EXCL_LINE
424 : {
425 : return reinterpret_cast<transpose_type&>(*this);
426 : }
427 :
428 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
429 300 : inline auto Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::transpose_rvalue() && -> transpose_type
430 : {
431 300 : return reinterpret_cast<transpose_type&>(*this);
432 : }
433 :
434 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
435 : template <sint32 SrcRowSize_,
436 : sint32 SrcColSize_,
437 : sint32 SrcRowCount_,
438 : sint32 SrcColCount_,
439 : sint32 SrcRowBeg_,
440 : sint32 SrcColBeg_,
441 : bool SrcIsRowMajor_,
442 : sint32 DstRowBeg_,
443 : sint32 DstColBeg_>
444 99 : inline void Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::setBlock(
445 : const Matrix<ValueType_, SrcRowSize_, SrcColSize_, SrcIsRowMajor_>& block)
446 : {
447 : static_assert(SrcRowBeg_ + SrcRowCount_ <= SrcRowSize_, "copy to many rows from src");
448 : static_assert(SrcColBeg_ + SrcColCount_ <= SrcColSize_, "copy to many cols from src");
449 :
450 : static_assert(DstRowBeg_ + SrcRowCount_ <= Rows_, "copy to many rows to dst");
451 : static_assert(DstColBeg_ + SrcColCount_ <= Cols_, "copy to many cols to dst");
452 :
453 537 : for (sint32 row = 0; row < SrcRowCount_; ++row)
454 : {
455 1498 : for (sint32 col = 0; col < SrcColCount_; ++col)
456 : {
457 1060 : this->at_unsafe(DstRowBeg_ + row, DstColBeg_ + col) = block.at_unsafe(SrcRowBeg_ + row, SrcColBeg_ + col);
458 : }
459 : }
460 99 : }
461 :
462 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
463 : template <sint32 SrcRowSize_, sint32 SrcColSize_, bool SrcIsRowMajor_>
464 626 : inline void Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::setBlock(
465 : const sint32 srcRowCount,
466 : const sint32 srcColCount,
467 : const sint32 srcRowBeg,
468 : const sint32 srcColBeg,
469 : const sint32 dstRowBeg,
470 : const sint32 dstColBeg,
471 : const Matrix<ValueType_, SrcRowSize_, SrcColSize_, SrcIsRowMajor_>& block)
472 : {
473 626 : assert((srcRowBeg + srcRowCount <= SrcRowSize_) && "copy to many rows from src");
474 626 : assert((srcColBeg + srcColCount <= SrcColSize_) && "copy to many cols from src");
475 :
476 626 : assert((dstRowBeg + srcRowCount <= Rows) && "copy to many rows to dst");
477 626 : assert((dstColBeg + srcColCount <= Cols) && "copy to many cols to dst");
478 :
479 2540 : for (sint32 row = 0; row < srcRowCount; ++row)
480 : {
481 3830 : for (sint32 col = 0; col < srcColCount; ++col)
482 : {
483 1916 : this->at_unsafe(dstRowBeg + row, dstColBeg + col) = block.at_unsafe(srcRowBeg + row, srcColBeg + col);
484 : }
485 : }
486 626 : }
487 :
488 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
489 : template <typename T, typename std::enable_if_t<std::is_integral<T>::value, bool>>
490 4 : inline void Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::inplace_div_by_int_unsafe(T scalar)
491 : {
492 28 : for (auto& val : data())
493 : {
494 24 : val /= scalar;
495 : }
496 : }
497 :
498 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
499 : template <typename T, typename std::enable_if_t<std::is_floating_point<T>::value, bool>>
500 759 : inline void Matrix<ValueType_, Rows_, Cols_, IsRowMajor_>::inplace_mul_by_inverse_factor_unsafe(T scalar)
501 : {
502 759 : const T factor = 1 / scalar;
503 759 : this->operator*=(factor);
504 : }
505 :
506 : } // namespace math
507 : } // namespace tracking
508 :
509 : #endif // E4BF6CCD_1BFD_497E_A5CD_75BCD8DD1ED3
|
