Ocean
Loading...
Searching...
No Matches
StaticMatrix.h
Go to the documentation of this file.
1/*
2 * Copyright (c) Meta Platforms, Inc. and affiliates.
3 *
4 * This source code is licensed under the MIT license found in the
5 * LICENSE file in the root directory of this source tree.
6 */
7
8#ifndef META_OCEAN_MATH_STATIC_MATRIX_H
9#define META_OCEAN_MATH_STATIC_MATRIX_H
10
11#include "ocean/math/Math.h"
12
13namespace Ocean
14{
15
16/**
17 * This class implements a matrix with static dimensions.
18 * In contrast to a dynamic matrix the size of this matrix cannot be changed because the dimension is defined as template parameter.<br>
19 * The elements inside the matrix are stored in a row aligned order.<br>
20 * A StaticMatrix<double, 2, 4> would hold 2 rows and 4 columns.
21 * <pre>
22 * The indices of the internal elements would be:
23 * | 0 1 2 3 |
24 * | 4 5 6 7 |
25 * </pre>
26 * @tparam T Data type of the matrix elements
27 * @tparam tRows Number of rows the matrix holds
28 * @tparam tColumns Number of columns this matrix holds
29 * @see Matrix, MatrixD, MatrixF, Matrix, SparseMatrix.
30 * @ingroup math
31 */
32template <typename T, size_t tRows, size_t tColumns>
34{
35 public:
36
37 /// Definition of the matrix element type.
38 typedef T Type;
39
40 public:
41
42 /**
43 * Creates a new matrix object without initializing the matrix elements.
44 */
45 inline StaticMatrix();
46
47 /**
48 * Creates a new matrix object and sets all elements to one unique value.
49 * @param value The value used to initialize each elements of the new matrix
50 */
51 explicit inline StaticMatrix(const T& value);
52
53 /**
54 * Creates a new matrix and initializes the elements of the matrix so that we receive an identity matrix or a zero matrix.
55 * @param toIdentity True, to initialize the matrix as identity matrix; False, to initialize the matrix as zero matrix
56 */
57 explicit StaticMatrix(const bool toIdentity);
58
59 /**
60 * Creates a new matrix element and initialized the matrix elements by a given data buffer with row aligned elements.
61 * Beware: The given buffer must be large enough.
62 * @param values Values to be copied to the matrix element buffer
63 */
64 explicit StaticMatrix(const T* values);
65
66 /**
67 * Creates a new matrix element and initialized the matrix elements by a given data buffer.
68 * Beware: The given buffer must be large enough.
69 * @param values Values to be copied to the matrix element buffer
70 * @param valuesRowAligned True, if the given values are stored in a row aligned order; False, if the values are stored in a column aligned order
71 */
72 StaticMatrix(const T* values, const bool valuesRowAligned);
73
74 /**
75 * Returns the number of rows this matrix holds.
76 * @return Matrix rows
77 * @see columns().
78 */
79 static inline size_t rows();
80
81 /**
82 * Returns the number of columsn this matrix holds.
83 * @return Matrix columns
84 * @see rows().
85 */
86 static inline size_t columns();
87
88 /**
89 * Returns the number of elements this matrix stores.
90 * @return Number of matrix elements
91 */
92 static inline size_t elements();
93
94 /**
95 * Returns a pointer to a specified row.
96 * @param index The index of the row, with range [0, rows())
97 * @return The pointer to the first element in the specified row of the matrix
98 */
99 inline const T* row(const size_t index) const;
100
101 /**
102 * Returns a pointer to a specified row.
103 * @param index The index of the row, with range [0, rows())
104 * @return The pointer to the first element in the specified row of the matrix
105 */
106 inline T* row(const size_t index);
107
108 /**
109 * Returns a pointer to a specified row.
110 * @return The pointer to the first element in the specified row of the matrix
111 * @tparam tIndex The index of the row, with range [0, rows())
112 */
113 template <size_t tIndex>
114 inline const T* row() const;
115
116 /**
117 * Returns a pointer to a specified row.
118 * @return The pointer to the first element in the specified row of the matrix
119 * @tparam tIndex The index of the row, with range [0, rows())
120 */
121 template <size_t tIndex>
122 inline T* row();
123
124 /**
125 * Returns a pointer to a specified element.
126 * @return The specified element
127 * @tparam tRow The index of the row, with range [0, rows())
128 * @tparam tColumn The index of the column, with range [0, columns())
129 */
130 template <size_t tRow, size_t tColumn>
131 inline const T& element() const;
132
133 /**
134 * Returns a pointer to a specified element.
135 * @return The specified element
136 * @tparam tRow The index of the row, with range [0, rows())
137 * @tparam tColumn The index of the column, with range [0, columns())
138 */
139 template <size_t tRow, size_t tColumn>
140 inline T& element();
141
142 /**
143 * Returns the pointer to the internal element buffer.
144 * @return Matrix element buffer
145 */
146 inline const T* data() const;
147
148 /**
149 * Returns the pointer to the internal element buffer.
150 * @return Matrix element buffer
151 */
152 inline T* data();
153
154 /**
155 * Returns whether all elements of this matrix are zero.
156 * @return True, if so
157 */
158 bool isNull() const;
159
160 /**
161 * Returns whether this matrix is an identity matrix.
162 * @return True, if so
163 */
164 bool isIdentity() const;
165
166 /**
167 * Returns whether two matrices are almost identical up to a specified epsilon.
168 * @param matrix Second matrix that will be checked
169 * @param eps The epsilon threshold to be used, with range [0, infinity)
170 * @return True, if so
171 */
172 bool isEqual(const StaticMatrix<T, tRows, tColumns>& matrix, const T eps = NumericT<T>::eps()) const;
173
174 /**
175 * Returns whether this matrix is symmetric (and whether this matrix is a square matrix).
176 * Beware: An empty matrix (without any rows or colums) is symmetric.
177 * @param eps The epsilon threshold to be used, with range [0, infinity)
178 * @return True, if so
179 */
180 bool isSymmetric(const T eps = NumericT<T>::eps()) const;
181
182 /**
183 * Sets the matrix to a zero matrix.
184 * @see isNull();
185 */
186 inline void toNull();
187
188 /**
189 * Sets the elements of this matrix by copying the values from a given buffer.
190 * @param values The elements to set, ensure that enough values are provided
191 * @param valuesRowAligned True, if the given values are stored in a row aligned order; False, if the values are stored in a column aligned order
192 */
193 inline void setData(const T* values, const bool valuesRowAligned);
194
195 /**
196 * Solves the given linear system by application of the cholesky distribution.
197 * M * x = b, with M and b known.<br>
198 * This matrix is M and M must be a symmetric positive-definite matrix, the given vector is b and the result will be x.
199 * @param vectorB Vector defining the linear system, with size tRows x 1
200 * @param vectorX Solution vector receiving the solution if existing, ensure that the number of elements is equal to tRows x 1
201 * @return True, if succeeded
202 * @see isSymmetric().
203 */
204 bool solveCholesky(const StaticMatrix<T, tRows, 1>& vectorB, StaticMatrix<T, tRows, 1>& vectorX) const;
205
206 /**
207 * Adds this matrix to a given matrix.
208 * Thus, this function calculates: result += thisMatrix.
209 * @param target The matrix to which this matrix will be added
210 */
211 void add(StaticMatrix<T, tRows, tColumns>& target) const;
212
213 /**
214 * Adds this matrix transposed to a given matrix.
215 * Thus, this function calculates: result += thisMatrix.transposed().
216 * @param target The matrix to which this (transposed) matrix will be added
217 */
219
220 /**
221 * Multiplies this matrix with a second matrix and assigns the results to a matrix.
222 * This function calculates: result = this * matrix.
223 * @param matrix Second matrix to multiply this matrix with
224 * @param result Resulting multiplication matrix
225 */
226 template <size_t tColumns2>
228
229 /**
230 * Multiplies this matrix with a second matrix and assigns the results to a given buffer.
231 * The given buffer is interpreted as if the given pointer points to the first element of a resulting matrix object.<br>
232 * @param matrix Second matrix to multiply this matrix with
233 * @param result Target buffer receiving the matrix result
234 */
235 template <size_t tColumns2>
236 inline void multiply(const StaticMatrix<T, tColumns, tColumns2>& matrix, T* result) const;
237
238 /**
239 * Multiplies this matrix with a second matrix and assigns the results to a given buffer.
240 * The given buffer is interpreted as if the given pointer points to the first element of a resulting matrix object.<br>
241 * However, after one row of the target buffer has been computed the given pointer is shifted by a given offset value.<br>
242 * Thus, the multiplication product may be assigned into a matrix even larger than the product matrix itself.<br>
243 * @param matrix Second matrix to multiply this matrix with
244 * @param rowOffset Number of element the target pointer will be shifted after one row of the multiplication matrix has been created before the next row will be assigned
245 * @param result Target buffer receiving the matrix result
246 */
247 template <size_t tColumns2>
248 inline void multiply(const StaticMatrix<T, tColumns, tColumns2>& matrix, const size_t rowOffset, T* result) const;
249
250 /**
251 * Multiplies this matrix (right) with the transposed matrix (left).
252 * Thus, this function returns matrix.transposed() * matrix.
253 * @return The multiplication result
254 */
256
257 /**
258 * Multiplies this matrix (right) with the transposed matrix (left) and stores the result in a provided matrix.
259 * Thus, this function calculates: result = matrix.transposed() * matrix.
260 * @param result The resulting matrix
261 */
263
264 /**
265 * Multiplies this matrix (left) with the transposed matrix (right).
266 * Thus, this function returns matrix * matrix.transposed().
267 * @return The multiplication result
268 */
270
271 /**
272 * Multiplies this matrix (left) with the transposed matrix (right) and stores the result in a provided matrix.
273 * Thus, this function calculates: result = matrix * matrix.transposed().
274 * @param result The resulting matrix
275 */
277
278 /**
279 * Multiplies this matrix (right) with the transposed matrix (left) and adds the resulting matrix to a given matrix.
280 * Thus, this function calculates target += matrix.transposed() * matrix.
281 * @param target The matrix to which the result will be added
282 */
284
285 /**
286 * Multiplies this matrix (left) with the transposed matrix (right) and adds the resulting matrix to a given matrix.
287 * Thus, this function calculates target += matrix * matrix.transposed().
288 * @param target The matrix to which the result will be added
289 */
291
292 /**
293 * Returns the transposed matrix of this matrix.
294 * @return The transposed matrix
295 */
297
298 /**
299 * Transposes this matrix.
300 * @param result The resulting transposed matrix
301 */
302 inline void transposed(StaticMatrix<T, tColumns, tRows>& result) const;
303
304 /**
305 * Adds a given matrix to this matrix.
306 * @param matrix Matrix to be added to this matrix
307 * @return The resulting sum matrix
308 */
310
311 /**
312 * Adds a given matrix to this matrix.
313 * @param matrix Matrix to be added to this matrix
314 * @return Reference to this (modified) matrix
315 */
317
318 /**
319 * Subtracts a given matrix from this matrix.
320 * @param matrix Matrix to be subtracted from this matrix
321 * @return The resulting matrix
322 */
324
325 /**
326 * Subtracts a given matrix from this matrix.
327 * @param matrix Matrix to be subtracted from this matrix
328 * @return Reference to this (modified) matrix
329 */
331
332 /**
333 * Multiplies this matrix with a second matrix objects and returns the result.
334 * @param matrix Matrix to be multiplied with this matrix
335 * @return New resulting product matrix
336 */
337 template <size_t tColumns2>
339
340 /**
341 * Multiplies this matrix with a scalar value element-wise and returns the new matrix.
342 * @param value Scalar value to multiply each element with
343 * @return Resulting new matrix
344 */
345 inline StaticMatrix<T, tRows, tColumns> operator*(const T& value) const;
346
347 /**
348 * Multiplies this matrix with a scalar value element-wise.
349 * @param value Scalar value to multiply each element with
350 * @return Reference to this (multiplied) matrix
351 */
352 inline StaticMatrix<T, tRows, tColumns>& operator*=(const T& value);
353
354 /**
355 * Returns a specified element of this matrix object.<br>
356 * @param row Row of the element to be returned, with range [0, rows())
357 * @param column Column of the element to be returned, with range [0, columns())
358 * @return Specified matrix element
359 */
360 inline T operator()(const size_t row, const size_t column) const;
361
362 /**
363 * Returns a specified element of this matrix object.<br>
364 * @param row Row of the element to be returned, with range [0, rows())
365 * @param column Column of the element to be returned, with range [0, columns())
366 * @return Specified matrix element
367 */
368 inline T& operator()(const size_t row, const size_t column);
369
370 /**
371 * Returns a specified element of this matrix object.<br>
372 * The element index must be defined regarding to the row aligned element order of this matrix.
373 * @param index Index of the matrix element to return, with range [0, rows() * columns())
374 * @return Specified matrix element
375 */
376 inline T operator[](const size_t index) const;
377
378 /**
379 * Returns a specified element of this matrix object.<br>
380 * The element index must be defined regarding to the row aligned element order of this matrix.
381 * @param index Index of the matrix element to return, with range [0, rows() * columns())
382 * @return Specified matrix element
383 */
384 inline T& operator[](const size_t index);
385
386 private:
387
388 /// Matrix elements.
389 T matrixValues[tRows * tColumns];
390};
391
392template <typename T, size_t tRows, size_t tColumns>
394{
395 // nothing to do here
396}
397
398template <typename T, size_t tRows, size_t tColumns>
400{
401 for (size_t n = 0; n < tRows * tColumns; ++n)
402 {
403 matrixValues[n] = value;
404 }
405}
406
407template <typename T, size_t tRows, size_t tColumns>
409{
410 ocean_assert(matrixValues);
411
412 memset(matrixValues, 0, sizeof(T) * elements());
413
414#ifdef OCEAN_DEBUG
415
416 for (size_t n = 0; n < elements(); ++n)
417 {
418 ocean_assert(matrixValues[n] == T());
419 }
420#endif
421
422 if (toIdentity)
423 {
424 for (size_t n = 0; n < min(tRows, tColumns); ++n)
425 {
426 (*this)(n, n) = T(1);
427 }
428 }
429}
430
431template <typename T, size_t tRows, size_t tColumns>
433{
434 ocean_assert(matrixValues);
435 memcpy(matrixValues, values, sizeof(T) * tRows * tColumns);
436}
437
438template <typename T, size_t tRows, size_t tColumns>
439StaticMatrix<T, tRows, tColumns>::StaticMatrix(const T* values, const bool valuesRowAligned)
440{
441 setData(values, valuesRowAligned);
442}
443
444template <typename T, size_t tRows, size_t tColumns>
446{
447 return tRows;
448}
449
450template <typename T, size_t tRows, size_t tColumns>
452{
453 return tColumns;
454}
455
456template <typename T, size_t tRows, size_t tColumns>
458{
459 return tRows * tColumns;
460}
461
462template <typename T, size_t tRows, size_t tColumns>
463inline const T* StaticMatrix<T, tRows, tColumns>::row(const size_t index) const
464{
465 ocean_assert(index < tRows);
466 return matrixValues + index * tColumns;
467}
468
469template <typename T, size_t tRows, size_t tColumns>
470inline T* StaticMatrix<T, tRows, tColumns>::row(const size_t index)
471{
472 ocean_assert(index < tRows);
473 return matrixValues + index * tColumns;
474}
475
476template <typename T, size_t tRows, size_t tColumns>
477template <size_t tIndex>
479{
480 static_assert(tIndex < tRows, "Invalid row index");
481 return matrixValues + tIndex * tColumns;
482}
483
484template <typename T, size_t tRows, size_t tColumns>
485template <size_t tIndex>
487{
488 static_assert(tIndex < tRows, "Invalid row index");
489 return matrixValues + tIndex * tColumns;
490}
491
492template <typename T, size_t tRows, size_t tColumns>
493template <size_t tRow, size_t tColumn>
495{
496 static_assert(tRow < tRows, "Invalid row index");
497 static_assert(tColumn < tColumns, "Invalid row index");
498
499 return matrixValues[tRow * tColumns + tColumn];
500}
501
502template <typename T, size_t tRows, size_t tColumns>
503template <size_t tRow, size_t tColumn>
505{
506 static_assert(tRow < tRows, "Invalid row index");
507 static_assert(tColumn < tColumns, "Invalid row index");
508
509 return matrixValues[tRow * tColumns + tColumn];
510}
511
512template <typename T, size_t tRows, size_t tColumns>
514{
515 return matrixValues;
516}
517
518template <typename T, size_t tRows, size_t tColumns>
520{
521 return matrixValues;
522}
523
524template <typename T, size_t tRows, size_t tColumns>
526{
527 for (size_t n = 0; n < elements(); ++n)
528 {
529 if (NumericT<T>::isNotEqualEps(matrixValues[n]))
530 {
531 return false;
532 }
533 }
534
535 return true;
536}
537
538template <typename T, size_t tRows, size_t tColumns>
540{
541 const T* pointer = matrixValues;
542
543 for (size_t r = 0; r < tRows; ++r)
544 {
545 for (size_t c = 0; c < tColumns; ++c)
546 {
547 if (r == c)
548 {
549 if (NumericT<T>::isNotEqual(*pointer++, T(1)))
550 {
551 return false;
552 }
553 }
554 else
555 {
556 if (NumericT<T>::isNotEqualEps(*pointer++))
557 {
558 return false;
559 }
560 }
561 }
562 }
563
564 return true;
565}
566
567template <typename T, size_t tRows, size_t tColumns>
569{
570 for (size_t n = 0u; n < elements(); ++n)
571 {
572 if (NumericT<T>::isNotEqual(matrixValues[n], matrix.matrixValues[n], eps))
573 {
574 return false;
575 }
576 }
577
578 return true;
579}
580
581template <typename T, size_t tRows, size_t tColumns>
583{
584 if constexpr (tRows != tColumns)
585 {
586 return false;
587 }
588
589 for (size_t r = 0; r < tRows; ++r)
590 {
591 for (size_t c = 0; c < tRows; ++c)
592 {
593 if (NumericT<T>::isNotEqual((*this)(r, c), (*this)(c, r), eps))
594 {
595 return false;
596 }
597 }
598 }
599
600 return true;
601}
602
603template <typename T, size_t tRows, size_t tColumns>
605{
606 for (size_t n = 0; n < elements(); ++n)
607 {
608 matrixValues[n] = T(0);
609 }
610}
611
612template <typename T, size_t tRows, size_t tColumns>
613inline void StaticMatrix<T, tRows, tColumns>::setData(const T* values, const bool valuesRowAligned)
614{
615 ocean_assert(tRows == 0 || tColumns == 0 || values);
616
617 if (valuesRowAligned)
618 {
619 memcpy(matrixValues, values, sizeof(T) * tRows * tColumns);
620 }
621 else
622 {
623 for (size_t c = 0u; c < tColumns; ++c)
624 {
625 for (size_t r = 0u; r < tRows; ++r)
626 {
627 matrixValues[r * tColumns + c] = *values++;
628 }
629 }
630 }
631}
632
633template <typename T, size_t tRows, size_t tColumns>
635{
636 ocean_assert(isSymmetric());
637
638 // decomposition
640 for (unsigned int i = 0u; i < tRows; ++i)
641 {
642 // determine diagonal
643 matrixR(i, i) = matrixValues[i * tColumns + i];
644
645 for (unsigned int k = 0u; k < i; ++k)
646 {
647 matrixR(i, i) -= matrixR(i, k) * matrixR(i, k);
648 }
649
650 // check, if matrixR(i, i) is greater then 0
651 if (NumericT<T>::isBelow(matrixR(i, i), 0))
652 {
653 return false;
654 }
655
656 matrixR(i, i) = NumericT<T>::sqrt(matrixR(i, i));
657
658 for (unsigned int j = i + 1u; j < tRows; ++j)
659 {
660 matrixR(i, j) = matrixValues[i * tColumns + j];
661
662 for (unsigned int k = 0u; k < i; ++k)
663 {
664 matrixR(i, j) -= matrixR(k, i) * matrixR(k, j);
665 }
666
667 // matrixR(i, i) has been proven to be greater than 0
668 ocean_assert(!NumericT<T>::isEqualEps(matrixR(i, i)));
669 matrixR(i, j) /= matrixR(i, i);
670
671 matrixR(j, i) = matrixR(i, j);
672 }
673 }
674
675 // solve for y
677 for (unsigned int i = 0u; i < tRows; ++i)
678 {
679 matrixY(i, 0) = vectorB[i];
680
681 for (unsigned int k = 0; k < i; ++k)
682 {
683 matrixY(i, 0) -= matrixY(k, 0) * matrixR(k, i);
684 }
685
686 // matrixR(i, i) has been proven to be greater than 0
687 ocean_assert(!NumericT<T>::isEqualEps(matrixR(i, i)));
688 matrixY(i, 0) /= matrixR(i, i);
689 }
690
691 // solve for x
692 for (int y = int(tRows - 1u); y >= 0; --y)
693 {
694 vectorX(y, 0) = matrixY(y, 0);
695
696 for (unsigned int i = y + 1u; i < tRows; ++i)
697 {
698 vectorX(y, 0) -= vectorX(i, 0) * matrixR(i, y);
699 }
700
701 // matrixR(y, y) has been proven to be greater than 0
702 ocean_assert(!NumericT<T>::isEqualEps(matrixR(y, y)));
703 vectorX(y, 0) /= matrixR(y, y);
704 }
705
706 return true;
707}
708
709template <typename T, size_t tRows, size_t tColumns>
711{
712 T* targetData = target.matrixValues;
713 const T* thisData = matrixValues;
714
715 const T* const thisDataEnd = thisData + tRows * tColumns;
716
717 while (thisData != thisDataEnd)
718 {
719 ocean_assert(thisData < thisDataEnd);
720 *targetData++ += *thisData++;
721 }
722}
723
724template <typename T, size_t tRows, size_t tColumns>
726{
727 const T* data = matrixValues;
728
729 for (size_t r = 0; r < tRows; ++r)
730 {
731 for (size_t c = 0; c < tColumns; ++c)
732 {
733 target(c, r) += *data++;
734 }
735 }
736}
737
738template <typename T, size_t tRows, size_t tColumns>
739template <size_t tColumns2>
741{
742 multiply(matrix, target.data());
743}
744
745template <typename T, size_t tRows, size_t tColumns>
746template <size_t tColumns2>
748{
749 ocean_assert(target);
750
751 for (size_t r = 0; r < tRows; ++r)
752 {
753 for (size_t c = 0; c < tColumns2; ++c)
754 {
755 T value = 0;
756
757 for (size_t n = 0; n < tColumns; ++n)
758 {
759 value += (*this)(r, n) * matrix(n, c);
760 }
761
762 *target++ = value;
763 }
764 }
765}
766
767
768template <typename T, size_t tRows, size_t tColumns>
769template <size_t tColumns2>
770inline void StaticMatrix<T, tRows, tColumns>::multiply(const StaticMatrix<T, tColumns, tColumns2>& matrix, const size_t rowOffset, T* target) const
771{
772 ocean_assert(target);
773
774 for (size_t r = 0; r < tRows; ++r)
775 {
776 for (size_t c = 0; c < tColumns2; ++c)
777 {
778 T value = 0;
779
780 for (size_t n = 0; n < tColumns; ++n)
781 {
782 value += (*this)(r, n) * matrix(n, c);
783 }
784
785 *target++ = value;
786 }
787
788 target += rowOffset;
789 }
790}
791
792template <typename T, size_t tRows, size_t tColumns>
794{
796 multiplyWithTransposedLeft(result);
797
798 return result;
799}
800
801template <typename T, size_t tRows, size_t tColumns>
803{
804 static_assert(tRows != 0 && tColumns != 0, "Invalid matrix dimension");
805
806 T intermediate;
807 T* target = result.data();
808
809 for (size_t r = 0; r < tColumns; ++r)
810 {
811 for (size_t c = 0; c < tColumns; ++c)
812 {
813 intermediate = (*this)(0, r) * (*this)(0, c);
814
815 for (size_t i = 1; i < tRows; ++i)
816 {
817 intermediate += (*this)(i, r) * (*this)(i, c);
818 }
819
820 *target++ = intermediate;
821 }
822 }
823}
824
825template <typename T, size_t tRows, size_t tColumns>
827{
829 multiplyWithTransposedRight(result);
830
831 return result;
832}
833
834template <typename T, size_t tRows, size_t tColumns>
836{
837 static_assert(tRows != 0 && tColumns != 0, "Invalid matrix dimension");
838
839 T intermediate;
840 T* target = result.data();
841
842 for (size_t r = 0; r < tRows; ++r)
843 {
844 const T* const rowElements = matrixValues + r * tColumns;
845
846 for (size_t c = 0; c < tRows; ++c)
847 {
848 const T* const columnElements = matrixValues + c * tColumns;
849
850 intermediate = rowElements[0] * columnElements[0];
851
852 for (size_t i = 1; i < tColumns; ++i)
853 {
854 intermediate += rowElements[i] * columnElements[i];
855 }
856
857 *target++ = intermediate;
858 }
859 }
860}
861
862template <typename T, size_t tRows, size_t tColumns>
864{
865 static_assert(tRows != 0 && tColumns != 0, "Invalid matrix dimension");
866
867 T intermediate;
868 T* target = result.data();
869
870 for (size_t r = 0; r < tColumns; ++r)
871 {
872 for (size_t c = 0; c < tColumns; ++c)
873 {
874 intermediate = (*this)(0, r) * (*this)(0, c);
875
876 for (size_t i = 1; i < tRows; ++i)
877 {
878 intermediate += (*this)(i, r) * (*this)(i, c);
879 }
880
881 *target++ += intermediate;
882 }
883 }
884}
885
886template <typename T, size_t tRows, size_t tColumns>
888{
889 static_assert(tRows != 0 && tColumns != 0, "Invalid matrix dimension");
890
891 T intermediate;
892 T* target = result.data();
893
894 for (size_t r = 0; r < tRows; ++r)
895 {
896 const T* const rowElements = matrixValues + r * tColumns;
897
898 for (size_t c = 0; c < tRows; ++c)
899 {
900 const T* const columnElements = matrixValues + c * tColumns;
901
902 intermediate = rowElements[0] * columnElements[0];
903
904 for (size_t i = 1; i < tColumns; ++i)
905 {
906 intermediate += rowElements[i] * columnElements[i];
907 }
908
909 *target++ += intermediate;
910 }
911 }
912}
913
914template <typename T, size_t tRows, size_t tColumns>
916{
918 transposed(result);
919
920 return result;
921}
922
923template <typename T, size_t tRows, size_t tColumns>
925{
926 for (size_t r = 0u; r < tRows; ++r)
927 {
928 for (size_t c = 0u; c < tColumns; ++c)
929 {
930 result(c, r) = (*this)(r, c);
931 }
932 }
933}
934
935template <typename T, size_t tRows, size_t tColumns>
937{
939
940 const T* thisData = matrixValues;
941 const T* matrixData = matrix.matrixValues;
942 T* resultData = result.matrixValues;
943
944 const T* const thisDataEnd = thisData + tRows * tColumns;
945
946 while (thisData != thisDataEnd)
947 {
948 *resultData++ = *thisData++ + *matrixData++;
949 }
950
951 return result;
952}
953
954template <typename T, size_t tRows, size_t tColumns>
960
961template <typename T, size_t tRows, size_t tColumns>
963{
965
966 const T* thisData = matrixValues;
967 const T* matrixData = matrix.matrixValues;
968 T* resultData = result.matrixValues;
969
970 const T* const thisDataEnd = thisData + tRows * tColumns;
971
972 while (thisData != thisDataEnd)
973 {
974 ocean_assert(thisData < thisDataEnd);
975 *resultData++ = *thisData++ - *matrixData++;
976 }
977
978 return result;
979}
980
981template <typename T, size_t tRows, size_t tColumns>
983{
984 T* thisData = matrixValues;
985 const T* matrixData = matrix.matrixValues;
986
987 T* const thisDataEnd = thisData + tRows * tColumns;
988
989 while (thisData != thisDataEnd)
990 {
991 ocean_assert(thisData < thisDataEnd);
992 *thisData++ -= *matrixData++;
993 }
994
995 return *this;
996}
997
998template <typename T, size_t tRows, size_t tColumns>
999template <size_t tColumns2>
1001{
1002 static_assert(tRows != 0 && tColumns != 0, "Invalid matrix dimension");
1003
1005
1006 for (size_t r = 0; r < tRows; ++r)
1007 {
1008 for (size_t c = 0; c < tColumns2; ++c)
1009 {
1010 T value = (*this)(r, 0) * matrix(0, c);
1011
1012 for (size_t n = 1; n < tColumns; ++n)
1013 {
1014 value += (*this)(r, n) * matrix(n, c);
1015 }
1016
1017 result(r, c) = value;
1018 }
1019 }
1020
1021 return result;
1022}
1023
1024#if defined(OCEAN_HARDWARE_SSE_VERSION) && OCEAN_HARDWARE_SSE_VERSION >= 41
1025
1026#if defined(OCEAN_HARDWARE_AVX_VERSION) && OCEAN_HARDWARE_AVX_VERSION >= 10
1027
1028template <>
1029template <>
1031{
1032 // the following code uses the following AVX instructions, and needs AVX1 or higher
1033
1034 // AVX1:
1035 // _mm256_loadu_pd
1036 // _mm256_mul_pd
1037 // _mm256_hadd_pd
1038 // _mm256_permute2f128_pd
1039 // _mm256_storeu_pd
1040
1041 // we load the four values of the vector
1042 __m256d v = _mm256_loadu_pd(vector.data());
1043
1044 // we load the first row of the matrix
1045 __m256d r0 = _mm256_loadu_pd(matrixValues + 0);
1046
1047 // we multiply the row with the vector
1048 r0 = _mm256_mul_pd(r0, v);
1049
1050 // we load the second row of the matrix
1051 __m256d r1 = _mm256_loadu_pd(matrixValues + 4);
1052 r1 = _mm256_mul_pd(r1, v);
1053
1054 // we sum both multiplication results horizontally (at least two neighboring products)
1055 __m256d sum_interleaved_r0_r1 = _mm256_hadd_pd(r0, r1);
1056
1057#ifdef OCEAN_COMPILER_MSC
1058 ocean_assert(NumericD::isEqual(sum_interleaved_r0_r1.m256d_f64[0], matrixValues[0 + 0] * vector[0] + matrixValues[0 + 1] * vector[1], NumericD::eps() * 10));
1059 ocean_assert(NumericD::isEqual(sum_interleaved_r0_r1.m256d_f64[1], matrixValues[4 + 0] * vector[0] + matrixValues[4 + 1] * vector[1], NumericD::eps() * 10));
1060 ocean_assert(NumericD::isEqual(sum_interleaved_r0_r1.m256d_f64[2], matrixValues[0 + 2] * vector[2] + matrixValues[0 + 3] * vector[3], NumericD::eps() * 10));
1061 ocean_assert(NumericD::isEqual(sum_interleaved_r0_r1.m256d_f64[3], matrixValues[4 + 2] * vector[2] + matrixValues[4 + 3] * vector[3], NumericD::eps() * 10));
1062#endif
1063
1064 // we load the third row
1065 __m256d r2 = _mm256_loadu_pd(matrixValues + 8);
1066 r2 = _mm256_mul_pd(r2, v);
1067
1068 // we load the fourth row
1069 __m256d r3 = _mm256_loadu_pd(matrixValues + 12);
1070 r3 = _mm256_mul_pd(r3, v);
1071
1072 // we sum the next multiplication results (at least two neighboring products)
1073 __m256d sum_interleaved_r2_r3 = _mm256_hadd_pd(r2, r3);
1074
1075#ifdef OCEAN_COMPILER_MSC
1076 ocean_assert(NumericD::isEqual(sum_interleaved_r2_r3.m256d_f64[0], matrixValues[ 8 + 0] * vector[0] + matrixValues[ 8 + 1] * vector[1], NumericD::eps() * 10));
1077 ocean_assert(NumericD::isEqual(sum_interleaved_r2_r3.m256d_f64[1], matrixValues[12 + 0] * vector[0] + matrixValues[12 + 1] * vector[1], NumericD::eps() * 10));
1078 ocean_assert(NumericD::isEqual(sum_interleaved_r2_r3.m256d_f64[2], matrixValues[ 8 + 2] * vector[2] + matrixValues[ 8 + 3] * vector[3], NumericD::eps() * 10));
1079 ocean_assert(NumericD::isEqual(sum_interleaved_r2_r3.m256d_f64[3], matrixValues[12 + 2] * vector[2] + matrixValues[12 + 3] * vector[3], NumericD::eps() * 10));
1080#endif
1081
1082 // now we reorder the interleaved sums
1083 __m256d sum_first = _mm256_permute2f128_pd(sum_interleaved_r0_r1, sum_interleaved_r2_r3, 0x20); // 0x20 = 0010 0000
1084 __m256d sum_second = _mm256_permute2f128_pd(sum_interleaved_r0_r1, sum_interleaved_r2_r3, 0x31); // 0x31 = 0011 0001
1085
1086 // we finally add both reordered sums
1087 __m256d sum = _mm256_add_pd(sum_first, sum_second);
1088
1090 _mm256_storeu_pd(result.data(), sum);
1091
1092 ocean_assert(NumericD::isEqual(result[0], matrixValues[ 0] * vector[0] + matrixValues[ 1] * vector[1] + matrixValues[ 2] * vector[2] + matrixValues[ 3] * vector[3], NumericD::eps() * 100));
1093 ocean_assert(NumericD::isEqual(result[1], matrixValues[ 4] * vector[0] + matrixValues[ 5] * vector[1] + matrixValues[ 6] * vector[2] + matrixValues[ 7] * vector[3], NumericD::eps() * 100));
1094 ocean_assert(NumericD::isEqual(result[2], matrixValues[ 8] * vector[0] + matrixValues[ 9] * vector[1] + matrixValues[10] * vector[2] + matrixValues[11] * vector[3], NumericD::eps() * 100));
1095 ocean_assert(NumericD::isEqual(result[3], matrixValues[12] * vector[0] + matrixValues[13] * vector[1] + matrixValues[14] * vector[2] + matrixValues[15] * vector[3], NumericD::eps() * 100));
1096
1097 return result;
1098}
1099
1100#else
1101
1102template <>
1103template <>
1105{
1106 // the following code uses the following SSE instructions, and needs SSE 3 or higher
1107
1108 // SSE2:
1109 // _mm_loadu_pd
1110 // _mm_add_pd
1111 // _mm_storeu_pd
1112 // _mm_mul_pd
1113
1114 // SSE3:
1115 // _mm_hadd_pd
1116
1117 // we load the vector into two individual 128 bit registers
1118 __m128d va = _mm_loadu_pd(vector.data());
1119 __m128d vb = _mm_loadu_pd(vector.data() + 2);
1120
1121 // we load the first row of the matrix
1122 __m128d r0a = _mm_loadu_pd(matrixValues + 0);
1123 __m128d r0b = _mm_loadu_pd(matrixValues + 2);
1124
1125 // we multiply the first row of the matrix with the vector and sum both results
1126 r0a = _mm_mul_pd(r0a, va);
1127 r0b = _mm_mul_pd(r0b, vb);
1128 r0a = _mm_add_pd(r0a, r0b);
1129
1130 // we load the second row of the matrix
1131 __m128d r1a = _mm_loadu_pd(matrixValues + 4);
1132 __m128d r1b = _mm_loadu_pd(matrixValues + 6);
1133
1134 // we multiply the second row of the matrix with the vector and sum both results
1135 r1a = _mm_mul_pd(r1a, va);
1136 r1b = _mm_mul_pd(r1b, vb);
1137 r1a = _mm_add_pd(r1a, r1b);
1138
1139 // we sum both intermediate sums horizontally and receive the first half of the result vector
1140 __m128d result0 = _mm_hadd_pd(r0a, r1a);
1141
1143 _mm_storeu_pd(result.data(), result0);
1144
1145 __m128d r2a = _mm_loadu_pd(matrixValues + 8);
1146 __m128d r2b = _mm_loadu_pd(matrixValues + 10);
1147
1148 r2a = _mm_mul_pd(r2a, va);
1149 r2b = _mm_mul_pd(r2b, vb);
1150 r2a = _mm_add_pd(r2a, r2b);
1151
1152 __m128d r3a = _mm_loadu_pd(matrixValues + 12);
1153 __m128d r3b = _mm_loadu_pd(matrixValues + 14);
1154
1155 r3a = _mm_mul_pd(r3a, va);
1156 r3b = _mm_mul_pd(r3b, vb);
1157 r3a = _mm_add_pd(r3a, r3b);
1158
1159 __m128d result1 = _mm_hadd_pd(r2a, r3a);
1160
1161 _mm_storeu_pd(result.data() + 2, result1);
1162
1163 ocean_assert(NumericD::isEqual(result[0], matrixValues[0] * vector[0] + matrixValues[1] * vector[1] + matrixValues[2] * vector[2] + matrixValues[3] * vector[3], NumericD::eps() * 100));
1164 ocean_assert(NumericD::isEqual(result[1], matrixValues[4] * vector[0] + matrixValues[5] * vector[1] + matrixValues[6] * vector[2] + matrixValues[7] * vector[3], NumericD::eps() * 100));
1165 ocean_assert(NumericD::isEqual(result[2], matrixValues[8] * vector[0] + matrixValues[9] * vector[1] + matrixValues[10] * vector[2] + matrixValues[11] * vector[3], NumericD::eps() * 100));
1166 ocean_assert(NumericD::isEqual(result[3], matrixValues[12] * vector[0] + matrixValues[13] * vector[1] + matrixValues[14] * vector[2] + matrixValues[15] * vector[3], NumericD::eps() * 100));
1167
1168 return result;
1169}
1170
1171#endif
1172
1173template <>
1174template <>
1176{
1177 // the following code uses the following SSE instructions, and thus needs SSE 4.1 or higher
1178
1179 // SSE:
1180 // _mm_loadu_ps
1181 // _mm_or_ps
1182 // _mm_storeu_ps
1183
1184 // SSE4.1:
1185 // _mm_dp_ps
1186
1187 // we load the four values of the vector
1188 __m128 v = _mm_loadu_ps(vector.data());
1189
1190 // we load the first four rows of the matrix
1191 __m128 r0 = _mm_loadu_ps(matrixValues + 0);
1192 __m128 r1 = _mm_loadu_ps(matrixValues + 4);
1193 __m128 r2 = _mm_loadu_ps(matrixValues + 8);
1194 __m128 r3 = _mm_loadu_ps(matrixValues + 12);
1195
1196 // we determine the dot product between the first row and the vector and store the result in the first float bin
1197 r0 = _mm_dp_ps(r0, v, 0xF1); // 0xF1 = 1111 0001
1198
1199 // we determine the dot product between the second row and the vector and store the result in the second float bin
1200 r1 = _mm_dp_ps(r1, v, 0xF2); // 0xF2 = 1111 0010
1201 r2 = _mm_dp_ps(r2, v, 0xF4); // 0xF4 = 1111 0100
1202 r3 = _mm_dp_ps(r3, v, 0xF8); // 0xF8 = 1111 1000
1203
1204 // now we blend the results by applying the bit-wise or operator
1205 __m128 result01 = _mm_or_ps(r0, r1);
1206 __m128 result23 = _mm_or_ps(r2, r3);
1207 __m128 result03 = _mm_or_ps(result01, result23);
1208
1210 _mm_storeu_ps(result.data(), result03);
1211
1212 return result;
1213}
1214
1215#endif // OCEAN_HARDWARE_SSE_VERSION >= 41
1216
1217template <typename T, size_t tRows, size_t tColumns>
1219{
1221
1222 T* target = result.data();
1223 const T* source = data();
1224
1225 const T* const targetEnd = target + elements();
1226
1227 while (target != targetEnd)
1228 {
1229 *target++ = *source++ * value;
1230 }
1231
1232 return result;
1233}
1234
1235template <typename T, size_t tRows, size_t tColumns>
1237{
1238 T* data = matrixValues;
1239 T* const dataEnd = data + elements();
1240
1241 while (data != dataEnd)
1242 {
1243 *data++ *= value;
1244 }
1245
1246 return *this;
1247}
1248
1249template <typename T, size_t tRows, size_t tColumns>
1250inline T StaticMatrix<T, tRows, tColumns>::operator()(const size_t row, const size_t column) const
1251{
1252 ocean_assert(row < tRows && column < tColumns);
1253 return matrixValues[row * tColumns + column];
1254}
1255
1256template <typename T, size_t tRows, size_t tColumns>
1257inline T& StaticMatrix<T, tRows, tColumns>::operator()(const size_t row, const size_t column)
1258{
1259 ocean_assert(row < tRows && column < tColumns);
1260 return matrixValues[row * tColumns + column];
1261}
1262
1263template <typename T, size_t tRows, size_t tColumns>
1264inline T StaticMatrix<T, tRows, tColumns>::operator[](const size_t index) const
1265{
1266 ocean_assert(index < tRows * tColumns);
1267 return matrixValues[index];
1268}
1269
1270template <typename T, size_t tRows, size_t tColumns>
1272{
1273 ocean_assert(index < tRows * tColumns);
1274 return matrixValues[index];
1275}
1276
1277}
1278
1279#endif // META_OCEAN_MATH_STATIC_MATRIX_H
This class provides basic numeric functionalities.
Definition Numeric.h:57
static T sqrt(const T value)
Returns the square root of a given value.
Definition Numeric.h:1533
static constexpr T eps()
Returns a small epsilon.
static bool isEqual(const T first, const T second)
Returns whether two values are equal up to a small epsilon.
Definition Numeric.h:2386
This class implements a matrix with static dimensions.
Definition StaticMatrix.h:34
void multiplyWithTransposedRightAndAdd(StaticMatrix< T, tRows, tRows > &target) const
Multiplies this matrix (left) with the transposed matrix (right) and adds the resulting matrix to a g...
Definition StaticMatrix.h:887
bool isNull() const
Returns whether all elements of this matrix are zero.
Definition StaticMatrix.h:525
void multiplyWithTransposedLeftAndAdd(StaticMatrix< T, tColumns, tColumns > &target) const
Multiplies this matrix (right) with the transposed matrix (left) and adds the resulting matrix to a g...
Definition StaticMatrix.h:863
StaticMatrix< T, tRows, tColumns > & operator-=(const StaticMatrix< T, tRows, tColumns > &matrix)
Subtracts a given matrix from this matrix.
Definition StaticMatrix.h:982
T matrixValues[tRows *tColumns]
Matrix elements.
Definition StaticMatrix.h:389
bool isEqual(const StaticMatrix< T, tRows, tColumns > &matrix, const T eps=NumericT< T >::eps()) const
Returns whether two matrices are almost identical up to a specified epsilon.
Definition StaticMatrix.h:568
T Type
Definition of the matrix element type.
Definition StaticMatrix.h:38
StaticMatrix< T, tRows, tColumns > operator+(const StaticMatrix< T, tRows, tColumns > &matrix) const
Adds a given matrix to this matrix.
Definition StaticMatrix.h:936
StaticMatrix< T, tRows, tColumns2 > operator*(const StaticMatrix< T, tColumns, tColumns2 > &matrix) const
Multiplies this matrix with a second matrix objects and returns the result.
Definition StaticMatrix.h:1000
StaticMatrix< T, tRows, tColumns > & operator*=(const T &value)
Multiplies this matrix with a scalar value element-wise.
Definition StaticMatrix.h:1236
static size_t rows()
Returns the number of rows this matrix holds.
Definition StaticMatrix.h:445
void add(StaticMatrix< T, tRows, tColumns > &target) const
Adds this matrix to a given matrix.
Definition StaticMatrix.h:710
const T * data() const
Returns the pointer to the internal element buffer.
Definition StaticMatrix.h:513
void toNull()
Sets the matrix to a zero matrix.
Definition StaticMatrix.h:604
const T * row() const
Returns a pointer to a specified row.
Definition StaticMatrix.h:478
StaticMatrix< T, tRows, tColumns > operator-(const StaticMatrix< T, tRows, tColumns > &matrix) const
Subtracts a given matrix from this matrix.
Definition StaticMatrix.h:962
void multiply(const StaticMatrix< T, tColumns, tColumns2 > &matrix, StaticMatrix< T, tRows, tColumns2 > &result) const
Multiplies this matrix with a second matrix and assigns the results to a matrix.
Definition StaticMatrix.h:740
void addTransposed(StaticMatrix< T, tColumns, tRows > &target) const
Adds this matrix transposed to a given matrix.
Definition StaticMatrix.h:725
bool isSymmetric(const T eps=NumericT< T >::eps()) const
Returns whether this matrix is symmetric (and whether this matrix is a square matrix).
Definition StaticMatrix.h:582
static size_t columns()
Returns the number of columsn this matrix holds.
Definition StaticMatrix.h:451
StaticMatrix< T, tRows, tRows > multiplyWithTransposedRight() const
Multiplies this matrix (left) with the transposed matrix (right).
Definition StaticMatrix.h:826
void setData(const T *values, const bool valuesRowAligned)
Sets the elements of this matrix by copying the values from a given buffer.
Definition StaticMatrix.h:613
static size_t elements()
Returns the number of elements this matrix stores.
Definition StaticMatrix.h:457
bool isIdentity() const
Returns whether this matrix is an identity matrix.
Definition StaticMatrix.h:539
T operator[](const size_t index) const
Returns a specified element of this matrix object.
Definition StaticMatrix.h:1264
bool solveCholesky(const StaticMatrix< T, tRows, 1 > &vectorB, StaticMatrix< T, tRows, 1 > &vectorX) const
Solves the given linear system by application of the cholesky distribution.
Definition StaticMatrix.h:634
T operator()(const size_t row, const size_t column) const
Returns a specified element of this matrix object.
Definition StaticMatrix.h:1250
StaticMatrix< T, tColumns, tColumns > multiplyWithTransposedLeft() const
Multiplies this matrix (right) with the transposed matrix (left).
Definition StaticMatrix.h:793
StaticMatrix< T, tRows, tColumns > & operator+=(const StaticMatrix< T, tRows, tColumns > &matrix)
Adds a given matrix to this matrix.
Definition StaticMatrix.h:955
StaticMatrix()
Creates a new matrix object without initializing the matrix elements.
Definition StaticMatrix.h:393
const T & element() const
Returns a pointer to a specified element.
Definition StaticMatrix.h:494
StaticMatrix< T, tColumns, tRows > transposed() const
Returns the transposed matrix of this matrix.
Definition StaticMatrix.h:915
The namespace covering the entire Ocean framework.
Definition Accessor.h:15