Ocean
Loading...
Searching...
No Matches
SquareMatrix3.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_SQUARE_MATRIX_3_H
9#define META_OCEAN_MATH_SQUARE_MATRIX_3_H
10
11#include "ocean/math/Math.h"
12#include "ocean/math/Equation.h"
13#include "ocean/math/Vector2.h"
14#include "ocean/math/Vector3.h"
15
17
18#include <vector>
19
20namespace Ocean
21{
22
23// Forward declaration.
24template <typename T> class EulerT;
25// Forward declaration.
26template <typename T> class HomogenousMatrixT4;
27// Forward declaration.
28template <typename T> class QuaternionT;
29// Forward declaration.
30template <typename T> class RotationT;
31// Forward declaration.
32template <typename T> class SquareMatrixT4;
33
34// Forward declaration.
35template <typename T> class SquareMatrixT3;
36
37/**
38 * Definition of the SquareMatrix3 object, depending on the OCEAN_MATH_USE_SINGLE_PRECISION either with single or double precision float data type.
39 * @see SquareMatrixT3
40 * @ingroup math
41 */
43
44/**
45 * Instantiation of the SquareMatrixT3 template class using a double precision float data type.
46 * @see SquareMatrixT3
47 * @ingroup math
48 */
50
51/**
52 * Instantiation of the SquareMatrixT3 template class using a single precision float data type.
53 * @see SquareMatrixT3
54 * @ingroup math
55 */
57
58/**
59 * Definition of a typename alias for vectors with SquareMatrixT3 objects.
60 * @see SquareMatrixT3
61 * @ingroup math
62 */
63template <typename T>
64using SquareMatricesT3 = std::vector<SquareMatrixT3<T>>;
65
66/**
67 * Definition of a vector holding SquareMatrix3 objects.
68 * @see SquareMatrix3
69 * @ingroup math
70 */
71typedef std::vector<SquareMatrix3> SquareMatrices3;
72
73/**
74 * This class implements a 3x3 square matrix.
75 * The matrix can be applied as e.g., rotation matrix for 3D vectors or can represent a Homography and so on.<br>
76 * The values are stored in a column aligned order with indices:
77 * <pre>
78 * | 0 3 6 |
79 * | 1 4 7 |
80 * | 2 5 8 |
81 * </pre>
82 * @tparam T Data type of matrix elements
83 * @see SquareMatrix3, SquareMatrixF3, SquareMatrixD3, Rotation, Euler, Quaternion, ExponentialMap.
84 * @ingroup math
85 */
86template <typename T>
88{
89 template <typename U> friend class SquareMatrixT3;
90
91 public:
92
93 /**
94 * Definition of the used data type.
95 */
96 typedef T Type;
97
98 public:
99
100 /**
101 * Creates a new SquareMatrixT3 object with undefined elements.
102 * Beware: This matrix is neither a zero nor an entity matrix!
103 */
105
106 /**
107 * Copy constructor.
108 * @param matrix The matrix to copy
109 */
110 SquareMatrixT3(const SquareMatrixT3<T>& matrix) = default;
111
112 /**
113 * Copy constructor for a matrix with difference element data type than T.
114 * @param matrix The matrix to copy
115 * @tparam U The element data type of the second matrix
116 */
117 template <typename U>
118 inline explicit SquareMatrixT3(const SquareMatrixT3<U>& matrix);
119
120 /**
121 * Creates a new SquareMatrixT3 object.
122 * @param setToIdentity Determines whether a entity matrix will be created, otherwise the matrix is initialized with zeros
123 */
124 explicit SquareMatrixT3(const bool setToIdentity);
125
126 /**
127 * Creates a new SquareMatrixT3 rotation matrix by a given Euler rotation.
128 * @param euler Euler rotation to create a rotation matrix from, must be valid
129 */
130 explicit SquareMatrixT3(const EulerT<T>& euler);
131
132 /**
133 * Creates a new 3x3 matrix object by a given angle-axis rotation.
134 * @param rotation The angle-axis rotation to create a matrix from, must be valid
135 */
136 explicit SquareMatrixT3(const RotationT<T>& rotation);
137
138 /**
139 * Creates a new 3x3 matrix object by a given quaternion rotation.
140 * @param quaternion The quaternion rotation to create a matrix from, must be valid
141 */
142 explicit SquareMatrixT3(const QuaternionT<T>& quaternion);
143
144 /**
145 * Creates a new SquareMatrixT3 object by three axes.
146 * @param xAxis First column of the matrix
147 * @param yAxis Middle column of the matrix
148 * @param zAxis Last column of the matrix
149 */
151
152 /**
153 * Creates a new SquareMatrixT3 object by a given diagonal vector.
154 * @param diagonal The diagonal vector for the new matrix
155 */
157
158 /**
159 * Creates a new SquareMatrixT3 object by nine elements of float type U.
160 * @param arrayValues The nine matrix elements defining the new matrix, must be valid
161 * @tparam U The floating point type of the given elements
162 */
163 template <typename U>
164 explicit SquareMatrixT3(const U* arrayValues);
165
166 /**
167 * Creates a new SquareMatrixT3 object by nine elements.
168 * @param arrayValues The nine matrix elements defining the new matrix, must be valid
169 */
170 explicit SquareMatrixT3(const T* arrayValues);
171
172 /**
173 * Creates a new SquareMatrixT3 object by nine elements.
174 * @param arrayValues The nine matrix elements defining the new matrix, must be valid
175 * @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 (which is the default case for this matrix)
176 * @tparam U The floating point type of the given elements
177 */
178 template <typename U>
179 SquareMatrixT3(const U* arrayValues, const bool valuesRowAligned);
180
181 /**
182 * Creates a new SquareMatrixT3 object by nine elements.
183 * @param arrayValues The nine matrix elements defining the new matrix, must be valid
184 * @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 (which is the default case for this matrix)
185 */
186 SquareMatrixT3(const T* arrayValues, const bool valuesRowAligned);
187
188 /**
189 * Creates a 3x3 rotation matrix by a given 4x4 homogeneous transformation.
190 * @param transformation The transformation to create a 3x3 rotation matrix from
191 */
192 explicit SquareMatrixT3(const HomogenousMatrixT4<T>& transformation);
193
194 /**
195 * Creates a 3x3 square matrix by a given 4x4 square transformation.
196 * @param transformation The transformation to create a 3x3 square matrix from
197 */
198 explicit SquareMatrixT3(const SquareMatrixT4<T>& transformation);
199
200 /**
201 * Creates a 3x3 rotation matrix by 9 given matrix elements.
202 * @param m00 Element of the first row and first column
203 * @param m10 Element of the second row and first column
204 * @param m20 Element of the third row and first column
205 * @param m01 Element of the first row and second column
206 * @param m11 Element of the second row and second column
207 * @param m21 Element of the third row and second column
208 * @param m02 Element of the first row and third column
209 * @param m12 Element of the second row and third column
210 * @param m22 Element of the third row and third column
211 */
212 explicit SquareMatrixT3(const T& m00, const T& m10, const T& m20, const T& m01, const T& m11, const T& m21, const T& m02, const T& m12, const T& m22);
213
214 /**
215 * Returns the transposed of this matrix.
216 * @return Transposed matrix
217 */
219
220 /**
221 * Transposes the matrix.
222 */
223 void transpose();
224
225 /**
226 * Returns the inverted matrix of this matrix.
227 * This matrix must not be singular.<br>
228 * Beware: This function does not throw an exception if the matrix cannot be inverted.<br>
229 * Thus, ensure that the matrix is invertible before calling this function.<br>
230 * Even better: avoid the usage of this function and call invert() instead.<br>
231 * In case, this matrix is not invertible, this matrix will be returned instead.
232 * @return The inverted matrix
233 * @see invert(), isSingular().
234 */
236
237 /**
238 * Inverts this matrix in place.
239 * @return True, if the matrix is not singular and could be inverted
240 * @see inverted(), solve().
241 */
242 bool invert();
243
244 /**
245 * Inverts the matrix and returns the result as parameter.
246 * @param invertedMatrix The resulting inverted matrix
247 * @return True, if the matrix is not singular and could be inverted
248 * @see inverted(), solve().
249 */
250 bool invert(SquareMatrixT3<T>& invertedMatrix) const;
251
252 /**
253 * Returns the determinant of the matrix.
254 * @return Matrix determinant
255 */
256 T determinant() const;
257
258 /**
259 * Returns the trace of the matrix which is the sum of the diagonal elements.
260 * @return Trace of the matrix
261 */
262 T trace() const;
263
264 /**
265 * Sets the matrix to the identity matrix.
266 * @see isIdentity().
267 */
268 inline void toIdentity();
269
270 /**
271 * Sets the matrix to a zero matrix.
272 * @see isNull();
273 */
274 inline void toNull();
275
276 /**
277 * Returns whether this matrix is an identity matrix.
278 * @return True, if so
279 * @see toIdentity().
280 */
281 bool isIdentity() const;
282
283 /**
284 * Returns whether this matrix is a zero matrix.
285 * @return True, if so
286 * @see toNull().
287 */
288 bool isNull() const;
289
290 /**
291 * Returns whether this matrix is singular (and thus cannot be inverted).
292 * A matrix is singular if the determinant of a matrix is 0.<br>
293 * @return True, if so
294 */
295 inline bool isSingular() const;
296
297 /**
298 * Returns true if this matrix is a similarity transformation.
299 * A similarity transformation has four degrees of freedom and contains a rotation, a scale, and a 2D translation and is not singular.<br>
300 * The 3x3 matrix representing the similarity transformation has the following layout:
301 * <pre>
302 * | a -b tx |
303 * | b a ty |
304 * | 0 0 1 |
305 * </pre>
306 * @return True, if this matrix is a similarity transformation, otherwise false
307 */
308 inline bool isSimilarity() const;
309
310 /**
311 * Returns true if this matrix is a affine transformation.
312 * In order to be considered affine, the matrix mustn't be singular and the last row must be equivalent to [0 0 1].
313 * @return True, if this matrix is an affine transformation, otherwise false
314 */
315 inline bool isAffine() const;
316
317 /**
318 * Returns true if this matrix is perspective transform/homography.
319 * In order to be considered a homography, the matrix mustn't be singular and the bottom-right matrix element must be nonzero.
320 * @return True, if this matrix is a homography, otherwise false
321 */
322 inline bool isHomography() const;
323
324 /**
325 * Returns whether this matrix is an orthonormal matrix.
326 * @param epsilon The epsilon threshold to be used, with range [0, infinity)
327 * @return True, if so
328 */
329 bool isOrthonormal(const T epsilon = NumericT<T>::eps()) const;
330
331 /**
332 * Returns whether this matrix is symmetric.
333 * @param epsilon The epsilon threshold to be used, with range [0, infinity)
334 * @return True, if so
335 */
336 inline bool isSymmetric(const T epsilon = NumericT<T>::eps()) const;
337
338 /**
339 * Returns whether two matrices are almost identical up to a specified epsilon.
340 * @param matrix Second matrix that will be checked
341 * @param eps The epsilon threshold to be used, with range [0, infinity)
342 * @return True, if so
343 */
344 inline bool isEqual(const SquareMatrixT3<T>& matrix, const T eps = NumericT<T>::eps()) const;
345
346 /**
347 * Returns the x axis which is the first column of the matrix.
348 * @return Vector with the first column
349 */
351
352 /**
353 * Returns the y axis which is the middle column of the matrix.
354 * @return Vector with the middle column
355 */
357
358 /**
359 * Returns the z axis which is the last column of the matrix.
360 * @return Vector with the last column
361 */
363
364 /**
365 * Returns the orthonormal matrix of this matrix by scaling the x-axis and adjusting y- and z-axis.
366 * This matrix must not be singular.
367 * @return An orthonormal version of this matrix
368 */
370
371 /**
372 * Determines the eigen values of this matrix.
373 * @param eigenValues The three resulting eigen values, sorted: the highest first
374 * @return True, if succeeded
375 */
376 bool eigenValues(T* eigenValues) const;
377
378 /**
379 * Performs an eigen value analysis.
380 * @param eigenValues The three resulting eigen values, sorted: the highest first
381 * @param eigenVectors The three corresponding eigen vectors
382 * @return True, if succeeded
383 */
384 bool eigenSystem(T* eigenValues,VectorT3<T>* eigenVectors) const;
385
386 /**
387 * Returns a 3d vector with values of the matrix diagonal.
388 * @return Vector with diagonal values
389 */
391
392 /**
393 * Solve a simple 3x3 system of linear equations: Ax = b
394 * Beware: The system of linear equations is assumed to be fully determined.
395 * @param b The right-hand side vector
396 * @param x The resulting solution vector
397 * @return True, if the solution is valid, otherwise false
398 * @see invert(), inverted().
399 */
400 inline bool solve(const VectorT3<T>& b, VectorT3<T>& x) const;
401
402 /**
403 * Calculates the sum of absolute value of elements
404 * @return Sum of absolute value of elements
405 */
406 inline T absSum() const;
407
408 /**
409 * Calculates the sum of elements
410 * @return Sum of elements
411 */
412 inline T sum() const;
413
414 /**
415 * Returns a pointer to the internal values.
416 * @return Pointer to the internal values
417 */
418 inline const T* data() const;
419
420 /**
421 * Returns a pointer to the internal values.
422 * @return Pointer to the internal values
423 */
424 inline T* data();
425
426 /**
427 * Copies the elements of this matrix to an array with floating point values of type U.
428 * @param arrayValues Array with 9 floating point values of type U receiving the elements of this matrix
429 * @param valuesRowAligned True, if the target values are stored in a row aligned order; False, if the target values are stored in a column aligned order
430 * @tparam U Floating point type
431 */
432 template <typename U>
433 void copyElements(U* arrayValues, const bool valuesRowAligned = false) const;
434
435 /**
436 * Copies the elements of this matrix to an array with floating point values of type T.
437 * @param arrayValues Array with 9 floating point values of type T receiving the elements of this matrix
438 * @param valuesRowAligned True, if the target values are stored in a row aligned order; False, if the target values are stored in a column aligned order
439 */
440 void copyElements(T* arrayValues, const bool valuesRowAligned = false) const;
441
442 /**
443 * Creates a skew symmetric 3x3 matrix for a specific vector.
444 * The skew symmetric matrix allows to calculate the cross product of the specified vector with a second vector by a matrix multiplication.<br>
445 * That means: skewSymmetricMatrix(vectorA) * vectorB == vectorA.cross(vectorB)<br>
446 * The final matrix has the following form for a vector (v0, v1, v2):
447 * <pre>
448 * | 0 -v2 v1 |
449 * | v2 0 -v0 |
450 * | -v1 v0 0 |
451 * </pre>
452 * @param vector The vector for which the skew symmetric matrix will be created
453 * @return Resulting matrix
454 */
456
457 /**
458 * Multiplies a 2D vector with this matrix (from the right).
459 * The 2D vector is interpreted as a 3D vector with third component equal to 1.<br>
460 * This function is equivalent with the corresponding multiplication operator but returns False if the dot product between the augmented vector and the last row is zero.<br>
461 * The multiplication result will be de-homogenized to provide a 2D vector result, if possible.<br>
462 * Actually this function does:
463 * @param vector The vector to be multiplied/transformed
464 * @param result The de-homogenized resulting 2D vector, if this function succeeds
465 * @return True, if the dot product between the augmented vector and the last row is non-zero; False, otherwise
466 * @see operator*(const VectorT2<T>& vector).
467 */
468 inline bool multiply(const VectorT2<T>& vector, VectorT2<T>& result) const;
469
470 /**
471 * Multiplies this transposed matrix with a second matrix.
472 * Actually, the following matrix will be returned: (*this).transposed() * right.<br>
473 * @param right Matrix to multiply
474 * @return Matrix product
475 */
477
478 /**
479 * Default copy assignment operator.
480 * @return Reference to this object
481 */
483
484 /**
485 * Returns whether two matrices are identical up to a small epsilon.
486 * @param matrix Right operand
487 * @return True, if so
488 */
489 bool operator==(const SquareMatrixT3<T>& matrix) const;
490
491 /**
492 * Returns whether two matrices are not identical up to a small epsilon.
493 * @param matrix Right operand
494 * @return True, if so
495 */
496 inline bool operator!=(const SquareMatrixT3<T>& matrix) const;
497
498 /**
499 * Adds two matrices.
500 * @param matrix Right operand
501 * @return Sum matrix
502 */
504
505 /**
506 * Adds and assigns two matrices.
507 * @param matrix Right operand
508 * @return Reference to this object
509 */
511
512 /**
513 * Subtracts two matrices.
514 * @param matrix Right operand
515 * @return Difference matrix
516 */
518
519 /**
520 * Subtracts and assigns two matrices.
521 * @param matrix Right operand
522 * @return Reference to this object
523 */
525
526 /**
527 * Returns the negative matrix of this matrix (all matrix elements are multiplied by -1).
528 * @return Resulting negative matrix
529 */
531
532 /**
533 * Multiplies two matrices.
534 * @param matrix Right operand
535 * @return Product matrix
536 */
537 OCEAN_FORCE_INLINE SquareMatrixT3<T> operator*(const SquareMatrixT3<T>& matrix) const;
538
539 /**
540 * Multiplies and assigns two matrices.
541 * @param matrix Right operand
542 * @return Reference to this object
543 */
544 OCEAN_FORCE_INLINE SquareMatrixT3<T>& operator*=(const SquareMatrixT3<T>& matrix);
545
546 /**
547 * Multiply operator for a 2D vector.
548 * The 2D vector is interpreted as a 3D vector with third component equal to 1.<br>
549 * The final result will be de-homogenized to provide a 2D vector result.<br>
550 * Beware the dot product between the last row and the (augmented) vector must not be zero!
551 * @param vector Right operand, vector to be multiplied from the right
552 * @return Resulting 2D vector
553 * @see multiply().
554 */
555 OCEAN_FORCE_INLINE VectorT2<T> operator*(const VectorT2<T>& vector) const;
556
557 /**
558 * Multiply operator for a 3D vector.
559 * @param vector Right operand
560 * @return Resulting 3D vector
561 */
562 OCEAN_FORCE_INLINE VectorT3<T> operator*(const VectorT3<T>& vector) const;
563
564 /**
565 * Multiplies this matrix with a scalar value.
566 * @param value Right operand
567 * @return Resulting matrix
568 */
569 OCEAN_FORCE_INLINE SquareMatrixT3<T> operator*(const T value) const;
570
571 /**
572 * Multiplies and assigns this matrix with a scalar value.
573 * @param value right operand
574 * @return Reference to this object
575 */
577
578 /**
579 * Element operator.
580 * Beware: No range check will be done!
581 * @param index The index of the element to return [0, 8]
582 * @return Specified element
583 */
584 inline T operator[](const unsigned int index) const;
585
586 /**
587 * Element operator.
588 * Beware: No range check will be done!
589 * @param index The index of the element to return [0, 8]
590 * @return Specified element
591 */
592 inline T& operator[](const unsigned int index);
593
594 /**
595 * Element operator.
596 * Beware: No range check will be done!
597 * @param row The row of the element to return [0, 2]
598 * @param column The column of the element to return [0, 2]
599 * @return Specified element
600 */
601 inline T operator()(const unsigned int row, const unsigned int column) const;
602
603 /**
604 * Element operator.
605 * Beware: No range check will be done!
606 * @param row The row of the element to return [0, 2]
607 * @param column The column of the element to return [0, 2]
608 * @return Specified element
609 */
610 inline T& operator()(const unsigned int row, const unsigned int column);
611
612 /**
613 * Element operator.
614 * Beware: No range check will be done!
615 * @param index The index of the element to return [0, 8]
616 * @return Specified element
617 */
618 inline T operator()(const unsigned int index) const;
619
620 /**
621 * Element operator.
622 * Beware: No range check will be done!
623 * @param index The index of the element to return [0, 8]
624 * @return Specified element
625 */
626 inline T& operator()(const unsigned int index);
627
628 /**
629 * Access operator.
630 * @return Pointer to the internal values
631 */
632 inline const T* operator()() const;
633
634 /**
635 * Access operator.
636 * @return Pointer to the internal values
637 */
638 inline T* operator()();
639
640 /**
641 * Hash function.
642 * @param matrix The matrix for which the hash value will be determined
643 * @return The resulting hash value
644 */
645 inline size_t operator()(const SquareMatrixT3<T>& matrix) const;
646
647 /**
648 * Returns the number of elements this matrix has.
649 * @return The number of elements, always 9
650 */
651 static inline size_t elements();
652
653 /**
654 * Multiplies several 2D vectors with a given 3x3 matrix.
655 * Each 2D vector is interpreted as a 3D vector with third component equal to 1.<br>
656 * The final result will be de-homogenized to provide a 2D vector result.
657 * @param matrix The matrix to be used for multiplication
658 * @param vectors The input vectors that will be multiplied, may be nullptr if number is 0
659 * @param results The resulting output (multiplied/transformed) vectors, with same number as the provided input vectors
660 * @param number The number of provided vectors (input and output), with range [0, infinity)
661 */
662 static void multiply(const SquareMatrixT3<T>& matrix, const VectorT2<T>* vectors, VectorT2<T>* results, const size_t number);
663
664 /**
665 * Multiplies several 3D vectors with a given 3x3 matrix.
666 * @param matrix The matrix to be used for multiplication
667 * @param vectors The input vectors that will be multiplied, may be nullptr if number is 0
668 * @param results The resulting output (multiplied/transformed) vectors, with same number as the provided input vectors
669 * @param number The number of provided vectors (input and output), with range [0, infinity)
670 */
671 static void multiply(const SquareMatrixT3<T>& matrix, const VectorT3<T>* vectors, VectorT3<T>* results, const size_t number);
672
673 /**
674 * Converts matrices with specific data type to matrices with different data type.
675 * @param matrices The matrices to convert
676 * @return The converted matrices
677 * @tparam U The element data type of the matrices to convert
678 */
679 template <typename U>
681
682 /**
683 * Converts matrices with specific data type to matrices with different data type.
684 * @param matrices The matrices to convert
685 * @param size The number of matrices to convert
686 * @return The converted matrices
687 * @tparam U The element data type of the matrices to convert
688 */
689 template <typename U>
690 static inline SquareMatricesT3<T> matrices2matrices(const SquareMatrixT3<U>* matrices, const size_t size);
691
692 protected:
693
694 /// The nine values of the matrix.
696};
697
698template <typename T>
700{
701 // nothing to do here
702}
703
704template <typename T>
705template <typename U>
707{
708 values_[0] = T(matrix.values_[0]);
709 values_[1] = T(matrix.values_[1]);
710 values_[2] = T(matrix.values_[2]);
711 values_[3] = T(matrix.values_[3]);
712 values_[4] = T(matrix.values_[4]);
713 values_[5] = T(matrix.values_[5]);
714 values_[6] = T(matrix.values_[6]);
715 values_[7] = T(matrix.values_[7]);
716 values_[8] = T(matrix.values_[8]);
717}
718
719template <typename T>
720SquareMatrixT3<T>::SquareMatrixT3(const bool setToIdentity)
721{
722 if (setToIdentity)
723 {
724 values_[0] = T(1.0);
725 values_[1] = T(0.0);
726 values_[2] = T(0.0);
727 values_[3] = T(0.0);
728 values_[4] = T(1.0);
729 values_[5] = T(0.0);
730 values_[6] = T(0.0);
731 values_[7] = T(0.0);
732 values_[8] = T(1.0);
733 }
734 else
735 {
736 values_[0] = T(0.0);
737 values_[1] = T(0.0);
738 values_[2] = T(0.0);
739 values_[3] = T(0.0);
740 values_[4] = T(0.0);
741 values_[5] = T(0.0);
742 values_[6] = T(0.0);
743 values_[7] = T(0.0);
744 values_[8] = T(0.0);
745 }
746}
747
748template <typename T>
750{
751 /**
752 * Rotation matrix around x-axis R(x):
753 * [ 1 0 0 ]
754 * [ 0 cos -sin ]
755 * [ 0 sin cos ]
756 */
757
758 /**
759 * Rotation matrix around y-axis R(y):
760 * [ cos 0 sin ]
761 * [ 0 1 0 ]
762 * [ -sin 0 cos ]
763 */
764
765 /**
766 * Rotation matrix around z-axis R(z):
767 * [ cos -sin 0 ]
768 * [ sin cos 0 ]
769 * [ 0 0 1 ]
770 */
771
772 /**
773 * Combined rotation matrix for R(y)R(x)R(z)
774 * [ cy cz + sx sy sz cz sx sy - cy sz cx sy ]
775 * [ cx sz cx cz -sx ]
776 * [ -cz sy + cy sx sz cy cz sx + sy sz cx cy ]
777 */
778
779 const T cx = NumericT<T>::cos(euler.pitch());
780 const T sx = NumericT<T>::sin(euler.pitch());
781
782 const T cy = NumericT<T>::cos(euler.yaw());
783 const T sy = NumericT<T>::sin(euler.yaw());
784
785 const T cz = NumericT<T>::cos(euler.roll());
786 const T sz = NumericT<T>::sin(euler.roll());
787
788 values_[0] = cy * cz + sx * sy * sz;
789 values_[1] = cx * sz;
790 values_[2] = -cz * sy + cy * sx * sz;
791 values_[3] = cz * sx * sy - cy * sz;
792 values_[4] = cx * cz;
793 values_[5] = cy * cz * sx + sy * sz;
794 values_[6] = cx * sy;
795 values_[7] = -sx;
796 values_[8] = cx * cy;
797
798 ocean_assert(NumericT<T>::isEqual(determinant(), T(1.0)));
799}
800
801template <typename T>
803{
804 // R(n, angle) = cos(angle) * I + (1 - cos(angle) * nn^T - sin(angle) * X(n)
805
806 ocean_assert(rotation.isValid());
807
808 const T cosValue = NumericT<T>::cos(rotation.angle());
809 const T cosValue1 = T(1.0) - cosValue;
810 const T sinValue = NumericT<T>::sin(rotation.angle());
811
812 const VectorT3<T> axis(rotation.axis());
813
814 const T xx = axis.x() * axis.x() * cosValue1;
815 const T yy = axis.y() * axis.y() * cosValue1;
816 const T zz = axis.z() * axis.z() * cosValue1;
817 const T xy = axis.x() * axis.y() * cosValue1;
818 const T xz = axis.x() * axis.z() * cosValue1;
819 const T yz = axis.y() * axis.z() * cosValue1;
820
821 const T nx = axis.x() * sinValue;
822 const T ny = axis.y() * sinValue;
823 const T nz = axis.z() * sinValue;
824
825 values_[0] = xx + cosValue;
826 values_[1] = xy + nz;
827 values_[2] = xz - ny;
828
829 values_[3] = xy - nz;
830 values_[4] = yy + cosValue;
831 values_[5] = yz + nx;
832
833 values_[6] = xz + ny;
834 values_[7] = yz - nx;
835 values_[8] = zz + cosValue;
836
837 ocean_assert(NumericT<T>::isEqual(determinant(), T(1.0)));
838}
839
840template <typename T>
842{
843 ocean_assert(quaternion.isValid());
844
845 const T xx = quaternion.x() * quaternion.x();
846 const T yy = quaternion.y() * quaternion.y();
847 const T zz = quaternion.z() * quaternion.z();
848
849 const T wx = quaternion.w() * quaternion.x();
850 const T wy = quaternion.w() * quaternion.y();
851 const T wz = quaternion.w() * quaternion.z();
852 const T xy = quaternion.x() * quaternion.y();
853 const T xz = quaternion.x() * quaternion.z();
854 const T yz = quaternion.y() * quaternion.z();
855
856 values_[0] = T(1.0) - T(2.0) * (yy + zz);
857 values_[1] = T(2.0) * (wz + xy);
858 values_[2] = T(2.0) * (xz - wy);
859
860 values_[3] = T(2.0) * (xy - wz);
861 values_[4] = T(1.0) - T(2.0) * (xx + zz);
862 values_[5] = T(2.0) * (wx + yz);
863
864 values_[6] = T(2.0) * (wy + xz);
865 values_[7] = T(2.0) * (yz - wx);
866 values_[8] = T(1.0) - T(2.0) * (xx + yy);
867
868 ocean_assert(NumericT<T>::isWeakEqual(determinant(), T(1.0)) && "the quaternion is not normalized");
869}
870
871template <typename T>
872SquareMatrixT3<T>::SquareMatrixT3(const T& m00, const T& m10, const T& m20, const T& m01, const T& m11, const T& m21, const T& m02, const T& m12, const T& m22)
873{
874 values_[0] = m00;
875 values_[1] = m10;
876 values_[2] = m20;
877
878 values_[3] = m01;
879 values_[4] = m11;
880 values_[5] = m21;
881
882 values_[6] = m02;
883 values_[7] = m12;
884 values_[8] = m22;
885}
886
887template <typename T>
889{
890 memcpy(values_, xAxis(), sizeof(T) * 3);
891 memcpy(values_ + 3, yAxis(), sizeof(T) * 3);
892 memcpy(values_ + 6, zAxis(), sizeof(T) * 3);
893}
894
895template <typename T>
897{
898 values_[0] = diagonal[0];
899 values_[1] = T(0.0);
900 values_[2] = T(0.0);
901 values_[3] = T(0.0);
902 values_[4] = diagonal[1];
903 values_[5] = T(0.0);
904 values_[6] = T(0.0);
905 values_[7] = T(0.0);
906 values_[8] = diagonal[2];
907}
908
909template <typename T>
910template <typename U>
912{
913 ocean_assert(arrayValues);
914
915 for (unsigned int n = 0u; n < 9u; ++n)
916 {
917 values_[n] = T(arrayValues[n]);
918 }
919}
920
921template <typename T>
923{
924 ocean_assert(arrayValues);
925 memcpy(values_, arrayValues, sizeof(T) * 9);
926}
927
928template <typename T>
929template <typename U>
930SquareMatrixT3<T>::SquareMatrixT3(const U* arrayValues, const bool valuesRowAligned)
931{
932 ocean_assert(arrayValues);
933
934 if (valuesRowAligned)
935 {
936 values_[0] = T(arrayValues[0]);
937 values_[1] = T(arrayValues[3]);
938 values_[2] = T(arrayValues[6]);
939 values_[3] = T(arrayValues[1]);
940 values_[4] = T(arrayValues[4]);
941 values_[5] = T(arrayValues[7]);
942 values_[6] = T(arrayValues[2]);
943 values_[7] = T(arrayValues[5]);
944 values_[8] = T(arrayValues[8]);
945
946 }
947 else
948 {
949 for (unsigned int n = 0u; n < 9u; ++n)
950 {
951 values_[n] = T(arrayValues[n]);
952 }
953 }
954}
955
956template <typename T>
957SquareMatrixT3<T>::SquareMatrixT3(const T* arrayValues, const bool valuesRowAligned)
958{
959 ocean_assert(arrayValues);
960
961 if (valuesRowAligned)
962 {
963 values_[0] = arrayValues[0];
964 values_[1] = arrayValues[3];
965 values_[2] = arrayValues[6];
966 values_[3] = arrayValues[1];
967 values_[4] = arrayValues[4];
968 values_[5] = arrayValues[7];
969 values_[6] = arrayValues[2];
970 values_[7] = arrayValues[5];
971 values_[8] = arrayValues[8];
972 }
973 else
974 {
975 memcpy(values_, arrayValues, sizeof(T) * 9);
976 }
977}
978
979template <typename T>
981{
982 memcpy(values_, transformation(), sizeof(T) * 3);
983 memcpy(values_ + 3, transformation() + 4, sizeof(T) * 3);
984 memcpy(values_ + 6, transformation() + 8, sizeof(T) * 3);
985}
986
987template <typename T>
989{
990 memcpy(values_, transformation(), sizeof(T) * 3);
991 memcpy(values_ + 3, transformation() + 4, sizeof(T) * 3);
992 memcpy(values_ + 6, transformation() + 8, sizeof(T) * 3);
993}
994
995template<typename T>
996inline bool SquareMatrixT3<T>::solve(const VectorT3<T>& b, VectorT3<T>& x) const
997{
998 // Solve the system of linear equations, Ax=b, using Cramer's rule
999 //
1000 // [a0 a3 a6] [b0]
1001 // A = [a1 a4 a7], b = [b1]
1002 // [a2 a5 a8] [b2]
1003 //
1004 // d = det(A)
1005 //
1006 // [b0 a3 a6] [a0 b0 a6] [a0 a3 b0]
1007 // d0 = det( [b1 a4 a7] ), d1 = det( [a1 b1 a7] ), d2 = det( [a1 a4 b1] )
1008 // [b2 a5 a8] [a2 b2 a8] [a2 a5 b2]
1009 //
1010 // [d0 / d]
1011 // x = [d1 / d]
1012 // [d2 / d]
1013 const T d = determinant();
1014
1016 {
1017 const T d0 = b[0] * (values_[4] * values_[8] - values_[5] * values_[7]) + b[1] * (values_[5] * values_[6] - values_[3] * values_[8]) + b[2] * (values_[3] * values_[7] - values_[4] * values_[6]);
1018 const T d1 = values_[0] * ( b[1] * values_[8] - b[2] * values_[7]) + values_[1] * ( b[2] * values_[6] - b[0] * values_[8]) + values_[2] * ( b[0] * values_[7] - b[1] * values_[6]);
1019 const T d2 = values_[0] * (values_[4] * b[2] - values_[5] * b[1]) + values_[1] * (values_[5] * b[0] - values_[3] * b[2]) + values_[2] * (values_[3] * b[1] - values_[4] * b[0]);
1020
1021 const T invD = T(1) / d;
1022
1023 x[0] = d0 * invD;
1024 x[1] = d1 * invD;
1025 x[2] = d2 * invD;
1026
1027 return true;
1028 }
1029
1030 return false;
1031}
1032
1033template<typename T>
1035{
1036 return (NumericT<T>::abs(values_[0]) + NumericT<T>::abs(values_[1]) + NumericT<T>::abs(values_[2]) + NumericT<T>::abs(values_[3]) + NumericT<T>::abs(values_[4]) + NumericT<T>::abs(values_[5]) + NumericT<T>::abs(values_[6]) + NumericT<T>::abs(values_[7]) + NumericT<T>::abs(values_[8]));
1037}
1038
1039template<typename T>
1041{
1042 return (values_[0] + values_[1] + values_[2] + values_[3] + values_[4] + values_[5] + values_[6] + values_[7] + values_[8]);
1043}
1044
1045template <typename T>
1046inline const T* SquareMatrixT3<T>::data() const
1047{
1048 return values_;
1049}
1050
1051template <typename T>
1053{
1054 return values_;
1055}
1056
1057template <typename T>
1058inline bool SquareMatrixT3<T>::operator!=(const SquareMatrixT3<T>& matrix) const
1059{
1060 return !(*this == matrix);
1061}
1062
1063template <typename T>
1065{
1066 *this = *this * matrix;
1067 return *this;
1068}
1069
1070template <typename T>
1071inline T SquareMatrixT3<T>::operator[](const unsigned int index) const
1072{
1073 ocean_assert(index < 9u);
1074 return values_[index];
1075}
1076
1077template <typename T>
1078inline T& SquareMatrixT3<T>::operator[](const unsigned int index)
1079{
1080 ocean_assert(index < 9u);
1081 return values_[index];
1082}
1083
1084template <typename T>
1085inline T SquareMatrixT3<T>::operator()(const unsigned int row, const unsigned int column) const
1086{
1087 ocean_assert(row < 3u && column < 3u);
1088 return values_[column * 3 + row];
1089}
1090
1091template <typename T>
1092inline T& SquareMatrixT3<T>::operator()(const unsigned int row, const unsigned int column)
1093{
1094 ocean_assert(row < 3u && column < 3u);
1095 return values_[column * 3 + row];
1096}
1097
1098template <typename T>
1099inline T SquareMatrixT3<T>::operator()(const unsigned int index) const
1100{
1101 ocean_assert(index < 9u);
1102 return values_[index];
1103}
1104
1105template <typename T>
1106inline T& SquareMatrixT3<T>::operator()(const unsigned int index)
1107{
1108 ocean_assert(index < 9u);
1109 return values_[index];
1110}
1111
1112template <typename T>
1113inline const T* SquareMatrixT3<T>::operator()() const
1114{
1115 return values_;
1116}
1117
1118template <typename T>
1120{
1121 return values_;
1122}
1123
1124template <typename T>
1125inline size_t SquareMatrixT3<T>::operator()(const SquareMatrixT3<T>& matrix) const
1126{
1127 size_t seed = std::hash<T>{}(matrix.values_[0]);
1128
1129 for (unsigned int n = 1u; n < 9u; ++n)
1130 {
1131 seed ^= std::hash<T>{}(matrix.values_[n]) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
1132 }
1133
1134 return seed;
1135}
1136
1137template <typename T>
1139{
1140 return 9;
1141}
1142
1143template <typename T>
1145{
1146 SquareMatrixT3<T> result(*this);
1147
1148 result.values_[1] = values_[3];
1149 result.values_[3] = values_[1];
1150
1151 result.values_[2] = values_[6];
1152 result.values_[6] = values_[2];
1153
1154 result.values_[5] = values_[7];
1155 result.values_[7] = values_[5];
1156
1157 return result;
1158}
1159
1160template <typename T>
1162{
1163 SquareMatrixT3<T> tmp(*this);
1164
1165 values_[3] = tmp.values_[1];
1166 values_[1] = tmp.values_[3];
1167
1168 values_[6] = tmp.values_[2];
1169 values_[2] = tmp.values_[6];
1170
1171 values_[7] = tmp.values_[5];
1172 values_[5] = tmp.values_[7];
1173}
1174
1175template <typename T>
1177{
1178 SquareMatrixT3<T> invertedMatrix;
1179
1180 if (!invert(invertedMatrix))
1181 {
1182 ocean_assert(false && "Could not invert the matrix.");
1183 return *this;
1184 }
1185
1186 return invertedMatrix;
1187}
1188
1189template <typename T>
1191{
1192 SquareMatrixT3<T> invertedMatrix;
1193
1194 if (!invert(invertedMatrix))
1195 {
1196 return false;
1197 }
1198
1199 *this = invertedMatrix;
1200
1201 return true;
1202}
1203
1204template <typename T>
1206{
1207 // calculate determinant
1208 const T v48 = values_[4] * values_[8];
1209 const T v57 = values_[5] * values_[7];
1210 const T v56 = values_[5] * values_[6];
1211 const T v38 = values_[3] * values_[8];
1212 const T v37 = values_[3] * values_[7];
1213 const T v46 = values_[4] * values_[6];
1214
1215 const T v48_57 = v48 - v57;
1216 const T v56_38 = v56 - v38;
1217 const T v37_46 = v37 - v46;
1218
1219 const T det = values_[0] * v48_57 + values_[1] * v56_38 + values_[2] * v37_46;
1220
1221 if (NumericT<T>::isEqualEps(det))
1222 {
1223 return false;
1224 }
1225
1226 const T factor = T(1.0) / det;
1227
1228 invertedMatrix.values_[0] = (v48_57) * factor;
1229 invertedMatrix.values_[1] = (values_[2] * values_[7] - values_[1] * values_[8]) * factor;
1230 invertedMatrix.values_[2] = (values_[1] * values_[5] - values_[2] * values_[4]) * factor;
1231
1232 invertedMatrix.values_[3] = (v56_38) * factor;
1233 invertedMatrix.values_[4] = (values_[0] * values_[8] - values_[2] * values_[6]) * factor;
1234 invertedMatrix.values_[5] = (values_[2] * values_[3] - values_[0] * values_[5]) * factor;
1235
1236 invertedMatrix.values_[6] = (v37_46) * factor;
1237 invertedMatrix.values_[7] = (values_[1] * values_[6] - values_[0] * values_[7]) * factor;
1238 invertedMatrix.values_[8] = (values_[0] * values_[4] - values_[1] * values_[3]) * factor;
1239
1240#ifdef OCEAN_INTENSIVE_DEBUG
1241 if (!std::is_same<T, float>::value)
1242 {
1243 const SquareMatrixT3<T> test(*this * invertedMatrix);
1244 const SquareMatrixT3<T> entity(true);
1245
1246 T sqrDistance = T(0);
1247 for (unsigned int n = 0; n < 9u; ++n)
1248 {
1249 sqrDistance += NumericT<T>::sqr(test[n] - entity[n]);
1250 }
1251
1252 const T distance = NumericT<T>::sqrt(sqrDistance * T(0.111111111111111111)); // 1 / 9
1253
1254 if (NumericT<T>::isWeakEqualEps(distance) == false)
1255 {
1256 T absolusteAverageEnergy = 0;
1257 for (unsigned int n = 0u; n < 9u; ++n)
1258 {
1259 absolusteAverageEnergy += NumericT<T>::abs(values[n]);
1260 }
1261
1262 absolusteAverageEnergy *= T(0.111111111111111111); // 1 / 9
1263
1264 // we expect/accept for each magnitude (larger than 1) a zero-inaccuracy of one magnitude (and we again comare it with the weak eps)
1265
1266 if (absolusteAverageEnergy <= 1)
1267 {
1268 ocean_assert_accuracy(!"This should never happen!");
1269 }
1270 else
1271 {
1272 const T adjustedDistance = distance / absolusteAverageEnergy;
1273 ocean_assert_accuracy(NumericT<T>::isWeakEqualEps(adjustedDistance));
1274 }
1275 }
1276 }
1277#endif // OCEAN_DEBUG
1278
1279 return true;
1280}
1281
1282template <typename T>
1284{
1285 return values_[0] * (values_[4] * values_[8] - values_[5] * values_[7])
1286 + values_[1] * (values_[5] * values_[6] - values_[3] * values_[8])
1287 + values_[2] * (values_[3] * values_[7] - values_[4] * values_[6]);
1288}
1289
1290template <typename T>
1292{
1293 return values_[0] + values_[4] + values_[8];
1294}
1295
1296template <typename T>
1298{
1299 values_[0] = T(1.0);
1300 values_[1] = T(0.0);
1301 values_[2] = T(0.0);
1302 values_[3] = T(0.0);
1303 values_[4] = T(1.0);
1304 values_[5] = T(0.0);
1305 values_[6] = T(0.0);
1306 values_[7] = T(0.0);
1307 values_[8] = T(1.0);
1308}
1309
1310template <typename T>
1312{
1313 values_[0] = T(0.0);
1314 values_[1] = T(0.0);
1315 values_[2] = T(0.0);
1316 values_[3] = T(0.0);
1317 values_[4] = T(0.0);
1318 values_[5] = T(0.0);
1319 values_[6] = T(0.0);
1320 values_[7] = T(0.0);
1321 values_[8] = T(0.0);
1322}
1323
1324template <typename T>
1326{
1327 return NumericT<T>::isEqual(values_[0], 1) && NumericT<T>::isEqualEps(values_[1]) && NumericT<T>::isEqualEps(values_[2])
1328 && NumericT<T>::isEqualEps(values_[3]) && NumericT<T>::isEqual(values_[4], 1) && NumericT<T>::isEqualEps(values_[5])
1329 && NumericT<T>::isEqualEps(values_[6]) && NumericT<T>::isEqualEps(values_[7]) && NumericT<T>::isEqual(values_[8], 1);
1330}
1331
1332template <typename T>
1334{
1335 return NumericT<T>::isEqualEps(values_[0]) && NumericT<T>::isEqualEps(values_[1]) && NumericT<T>::isEqualEps(values_[2])
1336 && NumericT<T>::isEqualEps(values_[3]) && NumericT<T>::isEqualEps(values_[4]) && NumericT<T>::isEqualEps(values_[5])
1337 && NumericT<T>::isEqualEps(values_[6]) && NumericT<T>::isEqualEps(values_[7]) && NumericT<T>::isEqualEps(values_[8]);
1338}
1339
1340template <typename T>
1342{
1343 return NumericT<T>::isEqualEps(determinant());
1344}
1345
1346template <typename T>
1348{
1349 return NumericT<T>::isEqual(values_[0], values_[4]) && NumericT<T>::isEqual(values_[1], -values_[3]) && NumericT<T>::isEqualEps(values_[2]) && NumericT<T>::isEqualEps(values_[5]) && NumericT<T>::isEqual(values_[8], 1) && !isSingular();
1350}
1351
1352template <typename T>
1354{
1355 return NumericT<T>::isEqualEps(values_[2]) && NumericT<T>::isEqualEps(values_[5]) && NumericT<T>::isEqual(values_[8], 1) && !isSingular();
1356}
1357
1358template <typename T>
1360{
1361 return NumericT<T>::isNotEqualEps(values_[8]) && !isSingular();
1362}
1363
1364template <typename T>
1365bool SquareMatrixT3<T>::isOrthonormal(const T epsilon) const
1366{
1367 ocean_assert(epsilon >= 0);
1368
1369 const VectorT3<T> xAxis(values_);
1370 const VectorT3<T> yAxis(values_ + 3);
1371 const VectorT3<T> zAxis(values_ + 6);
1372
1373 return NumericT<T>::isEqual(xAxis * yAxis, 0, epsilon) && NumericT<T>::isEqual(xAxis * zAxis, 0, epsilon) && NumericT<T>::isEqual(yAxis * zAxis, 0, epsilon)
1374 && NumericT<T>::isEqual(xAxis.length(), 1, epsilon) && NumericT<T>::isEqual(yAxis.length(), 1, epsilon) && NumericT<T>::isEqual(zAxis.length(), 1, epsilon);
1375}
1376
1377template <typename T>
1378bool SquareMatrixT3<T>::isSymmetric(const T epsilon) const
1379{
1380 ocean_assert(epsilon >= T(0));
1381
1382 return NumericT<T>::isEqual(values_[1], values_[3], epsilon) && NumericT<T>::isEqual(values_[2], values_[6], epsilon) && NumericT<T>::isEqual(values_[5], values_[7], epsilon);
1383}
1384
1385template <typename T>
1386inline bool SquareMatrixT3<T>::isEqual(const SquareMatrixT3<T>& matrix, const T eps) const
1387{
1388 return NumericT<T>::isEqual(values_[0], matrix.values_[0], eps) && NumericT<T>::isEqual(values_[1], matrix.values_[1], eps)
1389 && NumericT<T>::isEqual(values_[2], matrix.values_[2], eps) && NumericT<T>::isEqual(values_[3], matrix.values_[3], eps)
1390 && NumericT<T>::isEqual(values_[4], matrix.values_[4], eps) && NumericT<T>::isEqual(values_[5], matrix.values_[5], eps)
1391 && NumericT<T>::isEqual(values_[6], matrix.values_[6], eps) && NumericT<T>::isEqual(values_[7], matrix.values_[7], eps)
1392 && NumericT<T>::isEqual(values_[8], matrix.values_[8], eps);
1393}
1394
1395template <typename T>
1397{
1398 return VectorT3<T>(values_);
1399}
1400
1401template <typename T>
1403{
1404 return VectorT3<T>(values_ + 3);
1405}
1406
1407template <typename T>
1409{
1410 return VectorT3<T>(values_ + 6);
1411}
1412
1413template <typename T>
1415{
1416 ocean_assert(!isSingular());
1417
1418 VectorT3<T> xAxis(values_);
1419 VectorT3<T> yAxis(values_ + 3);
1420 VectorT3<T> zAxis(values_ + 6);
1421
1422 // X scale factor
1423 const T xScale = xAxis.length();
1424 // normalize x axis
1425 xAxis /= xScale;
1426
1427 // xy shear factor
1428 const T xyShear = xAxis * yAxis;
1429
1430 // compute orthogonal y axis
1431 yAxis -= xAxis * xyShear;
1432
1433 // y scale factor
1434 const T yScale = yAxis.length();
1435 // normalize y axis
1436 yAxis /= yScale;
1437
1438 // xz shear
1439 const T xzShear = xAxis * zAxis;
1440 // compute orthogonal z axis
1441 zAxis -= xAxis * xzShear;
1442
1443 // yz shear
1444 const T yzShear = yAxis * zAxis;
1445 // compute orthogonal y axis
1446 zAxis -= yAxis * yzShear;
1447
1448 // z scale factor
1449 const T zScale = zAxis.length();
1450 // normalize z axis
1451 zAxis /= zScale;
1452
1453#ifdef OCEAN_INTENSIVE_DEBUG
1454 if (std::is_same<T, double>::value)
1455 {
1456 ocean_assert(NumericT<T>::isEqualEps(xAxis * yAxis));
1457 ocean_assert(NumericT<T>::isEqualEps(xAxis * zAxis));
1458 ocean_assert(NumericT<T>::isEqualEps(yAxis * zAxis));
1459 ocean_assert(SquareMatrixT3<T>(xAxis, yAxis, zAxis).isOrthonormal());
1460 }
1461#endif
1462
1463 return SquareMatrixT3<T>(xAxis, yAxis, zAxis);
1464}
1465
1466template <typename T>
1467bool SquareMatrixT3<T>::eigenValues(T* eigenValues) const
1468{
1469 ocean_assert(eigenValues);
1470
1471 /**
1472 * Computation of the characteristic polynomial
1473 * <pre>
1474 *
1475 * [ a b c ]
1476 * A = [ d e f ]
1477 * [ g h i ]
1478 *
1479 * [ a-x b c ]
1480 * A - x * E = [ d e-x f ]
1481 * [ g h i-x ]
1482 *
1483 * Polynomial = Det|A - x * E| = 0
1484 * = (a - x) * (e - x) * (i - x) + bfg + cdh - g * (e - x) * c - h * f * (a - x) - (i - x) * d * b
1485 * = (ae - a * x - e * x + x^2) * (i - x) + bfg + cdh - g * (ec - c * x) - h * (fa - f * x) - d * (ib - x * b)
1486 * = aei - ae * x - ai * x + a * x^2 - ei * x + e * x^2 + i * x^2 - x^3 + bfg + cdh - gec + gc * x - hfa + hf * x - dib + db * x
1487 * = -x^3 + (a + e + i) * x^2 + (-ae - ai - ei + gc + hf + db) * x + aei + bfg + cdh - gec - hfa - dib
1488 * = x^3 - (a + e + i) * x^2 - (-ae - ai - ei + gc + hf + db) * x - aei - bfg - cdh + gec + hfa + dib
1489 * = a1x^3 + a2x^2 + a3x + a4 = 0
1490 * </pre>
1491 */
1492
1493 const T a = values_[0];
1494 const T d = values_[1];
1495 const T g = values_[2];
1496
1497 const T b = values_[3];
1498 const T e = values_[4];
1499 const T h = values_[5];
1500
1501 const T c = values_[6];
1502 const T f = values_[7];
1503 const T i = values_[8];
1504
1505 const T a1 = T(1.0);
1506 const T a2 = -(a + e + i);
1507 const T a3 = -(-a * e - a * i - e * i + g * c + h * f + d * b);
1508 const T a4 = -a * e * i - b * f * g - c * d * h + g * e * c + h * f * a + d * i * b;
1509
1510 if (EquationT<T>::solveCubic(a1, a2, a3, a4, eigenValues[0], eigenValues[1], eigenValues[2]) != 3u)
1511 {
1512 return false;
1513 }
1514
1515 Utilities::sortHighestToFront3(eigenValues[0], eigenValues[1], eigenValues[2]);
1516 return true;
1517}
1518
1519template <typename T>
1520bool SquareMatrixT3<T>::eigenSystem(T* eigenValues, VectorT3<T>* eigenVectors) const
1521{
1522 ocean_assert(eigenValues != nullptr && eigenVectors != nullptr);
1523
1524 /**
1525 * Computation of the characteristic polynomial
1526 * <pre>
1527 * [ a b c ]
1528 * A = [ d e f ]
1529 * [ g h i ]
1530 *
1531 * [ a-x b c ]
1532 * A - x * E = [ d e-x f ]
1533 * [ g h i-x ]
1534 *
1535 * Polynomial = Det|A - x * E| = 0
1536 * = (a - x) * (e - x) * (i - x) + bfg + cdh - g * (e - x) * c - h * f * (a - x) - (i - x) * d * b
1537 * = (ae - a * x - e * x + x^2) * (i - x) + bfg + cdh - g * (ec - c * x) - h * (fa - f * x) - d * (ib - x * b)
1538 * = aei - ae * x - ai * x + a * x^2 - ei * x + e * x^2 + i * x^2 - x^3 + bfg + cdh - gec + gc * x - hfa + hf * x - dib + db * x
1539 * = -x^3 + (a + e + i) * x^2 + (-ae - ai - ei + gc + hf + db) * x + aei + bfg + cdh - gec - hfa - dib
1540 * = x^3 - (a + e + i) * x^2 - (-ae - ai - ei + gc + hf + db) * x - aei - bfg - cdh + gec + hfa + dib
1541 * = a1x^3 + a2x^2 + a3x + a4 = 0
1542 * </pre>
1543 */
1544
1545 const T a = values_[0];
1546 const T d = values_[1];
1547 const T g = values_[2];
1548
1549 const T b = values_[3];
1550 const T e = values_[4];
1551 const T h = values_[5];
1552
1553 const T c = values_[6];
1554 const T f = values_[7];
1555 const T i = values_[8];
1556
1557 const T a1 = T(1.0);
1558 const T a2 = -(a + e + i);
1559 const T a3 = -(-a * e - a * i - e * i + g * c + h * f + d * b);
1560 const T a4 = -a * e * i - b * f * g - c * d * h + g * e * c + h * f * a + d * i * b;
1561
1562 if (EquationT<T>::solveCubic(a1, a2, a3, a4, eigenValues[0], eigenValues[1], eigenValues[2]) != 3u)
1563 {
1564 return false;
1565 }
1566
1567 /**
1568 * <pre>
1569 * Determination of the eigen vectors (vx, vy, vz):
1570 * [ a-x b c ] [ vx ]
1571 * A - x * E = [ d e-x f ] * [ vy ] = 0
1572 * [ g h i-x ] [ vz ]
1573 * We can apply the cross product to find a vector that is perpendicular to the two top rows of the matrix A - x * E
1574 * </pre>
1575 */
1576
1577 Utilities::sortHighestToFront3(eigenValues[0], eigenValues[1], eigenValues[2]);
1578
1579 for (unsigned int n = 0u; n < 3u; ++n)
1580 {
1581 const VectorT3<T> row0(a - eigenValues[n], b, c);
1582 const VectorT3<T> row1(d, e - eigenValues[n], f);
1583 const VectorT3<T> row2(g, h, i - eigenValues[n]);
1584
1585 VectorT3<T> candidate0(row0.cross(row1));
1586 VectorT3<T> candidate1(row0.cross(row2));
1587 VectorT3<T> candidate2(row1.cross(row2));
1588
1589 T sqrCandidate0(candidate0.sqr());
1590 T sqrCandidate1(candidate1.sqr());
1591 T sqrCandidate2(candidate2.sqr());
1592
1593 Utilities::sortHighestToFront3(sqrCandidate0, sqrCandidate1, sqrCandidate2, candidate0, candidate1, candidate2);
1594
1595 // if all rows (row0, row1 and row2) are parallel, we can decide any vector that is perpendicular to these rows
1596 if (sqrCandidate0 < NumericT<T>::eps() * NumericT<T>::eps())
1597 {
1598 // find one row that is not a null row
1599
1600 candidate0 = row0;
1601 candidate1 = row1;
1602 candidate2 = row2;
1603
1604 sqrCandidate0 = candidate0.sqr();
1605 sqrCandidate1 = candidate1.sqr();
1606 sqrCandidate2 = candidate2.sqr();
1607
1608 Utilities::sortHighestToFront3(sqrCandidate0, sqrCandidate1, sqrCandidate2, candidate0, candidate1, candidate2);
1609
1610 ocean_assert(NumericT<T>::isNotEqualEps(candidate0.length()));
1611 eigenVectors[n] = candidate0.perpendicular();
1612 }
1613 else
1614 {
1615 eigenVectors[n] = candidate0;
1616 }
1617
1618 eigenVectors[n].normalize();
1619 }
1620
1621 return true;
1622}
1623
1624template <typename T>
1626{
1627 return VectorT3<T>(values_[0], values_[4], values_[8]);
1628}
1629
1630template <typename T>
1631template <typename U>
1632void SquareMatrixT3<T>::copyElements(U* arrayValues, const bool valuesRowAligned) const
1633{
1634 ocean_assert(arrayValues != nullptr);
1635
1636 if (valuesRowAligned)
1637 {
1638 // this matrix and the provided array are both column aligned
1639 // thus, we can simply copy the data
1640
1641 arrayValues[0] = U(values_[0]);
1642 arrayValues[1] = U(values_[3]);
1643 arrayValues[2] = U(values_[6]);
1644
1645 arrayValues[3] = U(values_[1]);
1646 arrayValues[4] = U(values_[4]);
1647 arrayValues[5] = U(values_[7]);
1648
1649 arrayValues[6] = U(values_[2]);
1650 arrayValues[7] = U(values_[5]);
1651 arrayValues[8] = U(values_[8]);
1652 }
1653 else
1654 {
1655 // this matrix and the provided array are both column aligned
1656 // thus, we can simply copy the data
1657
1658 arrayValues[0] = U(values_[0]);
1659 arrayValues[1] = U(values_[1]);
1660 arrayValues[2] = U(values_[2]);
1661 arrayValues[3] = U(values_[3]);
1662 arrayValues[4] = U(values_[4]);
1663 arrayValues[5] = U(values_[5]);
1664 arrayValues[6] = U(values_[6]);
1665 arrayValues[7] = U(values_[7]);
1666 arrayValues[8] = U(values_[8]);
1667 }
1668}
1669
1670template <typename T>
1671void SquareMatrixT3<T>::copyElements(T* arrayValues, const bool valuesRowAligned) const
1672{
1673 ocean_assert(arrayValues != nullptr);
1674
1675 if (valuesRowAligned)
1676 {
1677 // this matrix and the provided array are both column aligned
1678 // thus, we can simply copy the data
1679
1680 arrayValues[0] = values_[0];
1681 arrayValues[1] = values_[3];
1682 arrayValues[2] = values_[6];
1683
1684 arrayValues[3] = values_[1];
1685 arrayValues[4] = values_[4];
1686 arrayValues[5] = values_[7];
1687
1688 arrayValues[6] = values_[2];
1689 arrayValues[7] = values_[5];
1690 arrayValues[8] = values_[8];
1691 }
1692 else
1693 {
1694 // this matrix and the provided array are both column aligned
1695 // thus, we can simply copy the data
1696
1697 memcpy(arrayValues, values_, sizeof(T) * 9);
1698 }
1699}
1700
1701template <typename T>
1703{
1704 return SquareMatrixT3<T>(0, vector(2), -vector(1), -vector(2), 0, vector(0), vector(1), -vector(0), 0);
1705}
1706
1707template <typename T>
1708inline bool SquareMatrixT3<T>::multiply(const VectorT2<T>& vector, VectorT2<T>& result) const
1709{
1710 /**
1711 * | x' | | 0 3 6 | | x |
1712 * | y' | = | 1 4 7 | * | y |
1713 * | 1 | | 2 5 8 | | 1 |
1714 */
1715
1716 const T z = values_[2] * vector[0] + values_[5] * vector[1] + values_[8];
1717
1719 {
1720 const T factor = T(1) / z;
1721
1722 result = VectorT2<T>((values_[0] * vector[0] + values_[3] * vector[1] + values_[6]) * factor,
1723 (values_[1] * vector[0] + values_[4] * vector[1] + values_[7]) * factor);
1724
1725 return true;
1726 }
1727
1728 return false;
1729}
1730
1731template <typename T>
1733{
1734 SquareMatrixT3<T> result;
1735
1736 result.values_[0] = values_[0] * matrix.values_[0] + values_[1] * matrix.values_[1] + values_[2] * matrix.values_[2];
1737 result.values_[1] = values_[3] * matrix.values_[0] + values_[4] * matrix.values_[1] + values_[5] * matrix.values_[2];
1738 result.values_[2] = values_[6] * matrix.values_[0] + values_[7] * matrix.values_[1] + values_[8] * matrix.values_[2];
1739 result.values_[3] = values_[0] * matrix.values_[3] + values_[1] * matrix.values_[4] + values_[2] * matrix.values_[5];
1740 result.values_[4] = values_[3] * matrix.values_[3] + values_[4] * matrix.values_[4] + values_[5] * matrix.values_[5];
1741 result.values_[5] = values_[6] * matrix.values_[3] + values_[7] * matrix.values_[4] + values_[8] * matrix.values_[5];
1742 result.values_[6] = values_[0] * matrix.values_[6] + values_[1] * matrix.values_[7] + values_[2] * matrix.values_[8];
1743 result.values_[7] = values_[3] * matrix.values_[6] + values_[4] * matrix.values_[7] + values_[5] * matrix.values_[8];
1744 result.values_[8] = values_[6] * matrix.values_[6] + values_[7] * matrix.values_[7] + values_[8] * matrix.values_[8];
1745
1746#ifdef OCEAN_INTENSIVE_DEBUG
1747 const SquareMatrixT3<T> debugMatrix(transposed() * matrix);
1748 ocean_assert(debugMatrix == result);
1749#endif
1750
1751 return result;
1752}
1753
1754template <typename T>
1756{
1757 return isEqual(matrix);
1758}
1759
1760template <typename T>
1762{
1763 SquareMatrixT3<T> result(*this);
1764
1765 result.values_[0] += matrix.values_[0];
1766 result.values_[1] += matrix.values_[1];
1767 result.values_[2] += matrix.values_[2];
1768 result.values_[3] += matrix.values_[3];
1769 result.values_[4] += matrix.values_[4];
1770 result.values_[5] += matrix.values_[5];
1771 result.values_[6] += matrix.values_[6];
1772 result.values_[7] += matrix.values_[7];
1773 result.values_[8] += matrix.values_[8];
1774
1775 return result;
1776}
1777
1778template <typename T>
1780{
1781 values_[0] += matrix.values_[0];
1782 values_[1] += matrix.values_[1];
1783 values_[2] += matrix.values_[2];
1784 values_[3] += matrix.values_[3];
1785 values_[4] += matrix.values_[4];
1786 values_[5] += matrix.values_[5];
1787 values_[6] += matrix.values_[6];
1788 values_[7] += matrix.values_[7];
1789 values_[8] += matrix.values_[8];
1790
1791 return *this;
1792}
1793
1794template <typename T>
1796{
1797 SquareMatrixT3<T> result(*this);
1798
1799 result.values_[0] -= matrix.values_[0];
1800 result.values_[1] -= matrix.values_[1];
1801 result.values_[2] -= matrix.values_[2];
1802 result.values_[3] -= matrix.values_[3];
1803 result.values_[4] -= matrix.values_[4];
1804 result.values_[5] -= matrix.values_[5];
1805 result.values_[6] -= matrix.values_[6];
1806 result.values_[7] -= matrix.values_[7];
1807 result.values_[8] -= matrix.values_[8];
1808
1809 return result;
1810}
1811
1812template <typename T>
1814{
1815 values_[0] -= matrix.values_[0];
1816 values_[1] -= matrix.values_[1];
1817 values_[2] -= matrix.values_[2];
1818 values_[3] -= matrix.values_[3];
1819 values_[4] -= matrix.values_[4];
1820 values_[5] -= matrix.values_[5];
1821 values_[6] -= matrix.values_[6];
1822 values_[7] -= matrix.values_[7];
1823 values_[8] -= matrix.values_[8];
1824
1825 return *this;
1826}
1827
1828template <typename T>
1830{
1831 SquareMatrixT3<T> result;
1832
1833 result.values_[0] = -values_[0];
1834 result.values_[1] = -values_[1];
1835 result.values_[2] = -values_[2];
1836 result.values_[3] = -values_[3];
1837 result.values_[4] = -values_[4];
1838 result.values_[5] = -values_[5];
1839 result.values_[6] = -values_[6];
1840 result.values_[7] = -values_[7];
1841 result.values_[8] = -values_[8];
1842
1843 return result;
1844}
1845
1846template <typename T>
1848{
1849 SquareMatrixT3<T> result;
1850
1851 result.values_[0] = values_[0] * matrix.values_[0] + values_[3] * matrix.values_[1] + values_[6] * matrix.values_[2];
1852 result.values_[1] = values_[1] * matrix.values_[0] + values_[4] * matrix.values_[1] + values_[7] * matrix.values_[2];
1853 result.values_[2] = values_[2] * matrix.values_[0] + values_[5] * matrix.values_[1] + values_[8] * matrix.values_[2];
1854 result.values_[3] = values_[0] * matrix.values_[3] + values_[3] * matrix.values_[4] + values_[6] * matrix.values_[5];
1855 result.values_[4] = values_[1] * matrix.values_[3] + values_[4] * matrix.values_[4] + values_[7] * matrix.values_[5];
1856 result.values_[5] = values_[2] * matrix.values_[3] + values_[5] * matrix.values_[4] + values_[8] * matrix.values_[5];
1857 result.values_[6] = values_[0] * matrix.values_[6] + values_[3] * matrix.values_[7] + values_[6] * matrix.values_[8];
1858 result.values_[7] = values_[1] * matrix.values_[6] + values_[4] * matrix.values_[7] + values_[7] * matrix.values_[8];
1859 result.values_[8] = values_[2] * matrix.values_[6] + values_[5] * matrix.values_[7] + values_[8] * matrix.values_[8];
1860
1861 return result;
1862}
1863
1864template <typename T>
1865OCEAN_FORCE_INLINE VectorT2<T> SquareMatrixT3<T>::operator*(const VectorT2<T>& vector) const
1866{
1867 /**
1868 * | x' | | 0 3 6 | | x |
1869 * | y' | = | 1 4 7 | * | y |
1870 * | 1 | | 2 5 8 | | 1 |
1871 */
1872
1873 const T z = values_[2] * vector[0] + values_[5] * vector[1] + values_[8];
1874 ocean_assert(NumericT<T>::isNotEqualEps(z) && "Division by zero!");
1875
1876 const T factor = T(1) / z;
1877
1878 return VectorT2<T>((values_[0] * vector[0] + values_[3] * vector[1] + values_[6]) * factor,
1879 (values_[1] * vector[0] + values_[4] * vector[1] + values_[7]) * factor);
1880}
1881
1882template <typename T>
1883OCEAN_FORCE_INLINE VectorT3<T> SquareMatrixT3<T>::operator*(const VectorT3<T>& vector) const
1884{
1885 return VectorT3<T>(values_[0] * vector[0] + values_[3] * vector[1] + values_[6] * vector[2],
1886 values_[1] * vector[0] + values_[4] * vector[1] + values_[7] * vector[2],
1887 values_[2] * vector[0] + values_[5] * vector[1] + values_[8] * vector[2]);
1888}
1889
1890#if defined(OCEAN_HARDWARE_SSE_VERSION) && OCEAN_HARDWARE_SSE_VERSION >= 20
1891
1892template <>
1894{
1895 // the following code uses the following SSE instructions, and needs SSE2 or higher
1896
1897 // SSE2:
1898 // _mm_load1_pd
1899 // _mm_loadu_pd
1900 // _mm_mul_pd
1901 // _mm_add_pd
1902 // _mm_storeu_pd
1903
1904
1905 // we use the following strategy:
1906 // the values of the matrix are column aligned so that we normally would need to transpose the matrix before we can apply simple SIMD instructions
1907 // however, we do not transpose the matrix (we avoid the shuffle instructions) and instead multiply the matrix column-wise:
1908 // finally we sum the four columns and have the result, compared to the transpose-based approach this approach is approx. two times faster
1909 //
1910 // A D G a Aa + Db + Gc
1911 // B E H b Ba + Eb + Hc
1912 // C F I * c = Ca + Fb + Ic
1913
1914 const double* const vectorValues = vector.data();
1915
1916 // first we load the first vector element in all 64bit elements of the 128 bit register, so that we receive [a, a]
1917 const __m128d v0 = _mm_load1_pd(vectorValues + 0);
1918
1919 const __m128d c0 = _mm_loadu_pd(values_ + 0);
1920
1921 // now we multiply the 128 bit register [A, B] * [a, a] = [Aa, Ba]
1922 const __m128d r0 = _mm_mul_pd(c0, v0);
1923
1924
1925 // now we proceed with the second column
1926 const __m128d v1 = _mm_load1_pd(vectorValues + 1);
1927 const __m128d c1 = _mm_loadu_pd(values_ + 3);
1928 const __m128d r1 = _mm_mul_pd(c1, v1);
1929
1930 // and we sum the result of the first column with the result of the second column
1931 __m128d result_f64x2 = _mm_add_pd(r0, r1);
1932
1933
1934 // now we proceed with the third column
1935 const __m128d v2 = _mm_load1_pd(vectorValues + 2);
1936 const __m128d c2 = _mm_loadu_pd(values_ + 6);
1937 const __m128d r2 = _mm_mul_pd(c2, v2);
1938
1939 // we sum the results
1940 result_f64x2 = _mm_add_pd(result_f64x2, r2);
1941
1942 VectorT3<double> result;
1943 result[2] = values_[2] * vector[0] + values_[5] * vector[1] + values_[8] * vector[2];
1944 _mm_storeu_pd(result.data(), result_f64x2);
1945
1946 return result;
1947}
1948
1949template <>
1951{
1952 // the following code uses the following SSE instructions, and needs SSE2 or higher
1953
1954 // SSE1:
1955 // _mm_load1_ps
1956 // _mm_loadu_ps
1957 // _mm_mul_ps
1958 // _mm_add_ps
1959 // _mm_storeu_ps
1960
1961 // SSE2:
1962 // _mm_castps_si128
1963 // _mm_srli_si128
1964 // _mm_castsi128_ps
1965
1966
1967 // we use the following strategy:
1968 // the values of the matrix are column aligned so that we normally would need to transpose the matrix before we can apply simple SIMD instructions
1969 // however, we do not transpose the matrix (we avoid the shuffle instructions) and instead multiply the matrix column-wise:
1970 // finally we sum the four columns and have the result, compared to the transpose-based approach this approach is approx. two times faster
1971 //
1972 // A D G a Aa + Db + Gc
1973 // B E H b Ba + Eb + Hc
1974 // C F I * c = Ca + Fb + Ic
1975
1976
1977 // first we load the first vector element in all 32bit elements of the 128 bit register, so that we receive [a, a, a, a]
1978 const __m128 v0 = _mm_load1_ps(vector.data() + 0);
1979
1980 // now we load the first column to receive: [A, B, C, D], beware: D will be unused
1981 const __m128 c0 = _mm_loadu_ps(values_ + 0);
1982
1983 // now we multiply the 128 bit register [A, B, C, D] * [a, a, a, a] = [Aa, Ba, Ca, Da]
1984 const __m128 r0 = _mm_mul_ps(c0, v0);
1985
1986
1987 // now we proceed with the second column
1988 const __m128 v1 = _mm_load1_ps(vector.data() + 1);
1989 const __m128 c1 = _mm_loadu_ps(values_ + 3);
1990 const __m128 r1 = _mm_mul_ps(c1, v1);
1991
1992 // and we sum the result of the first column with the result of the second column
1993 __m128 result_f32x4 = _mm_add_ps(r0, r1);
1994
1995
1996 // now we proceed with the third column, this time we have to load [F, G, H, I] and we shift the values
1997 const __m128 v2 = _mm_load1_ps(vector.data() + 2);
1998 __m128 c2 = _mm_loadu_ps(values_ + 5);
1999 c2 = _mm_castsi128_ps(_mm_srli_si128(_mm_castps_si128(c2), 4)); // shift the 128 bit register by 4 bytes to the right to receive [G H I 0]
2000 const __m128 r2 = _mm_mul_ps(c2, v2);
2001
2002 // we sum the results
2003 result_f32x4 = _mm_add_ps(result_f32x4, r2);
2004
2005 float resultValues[4];
2006 _mm_storeu_ps(resultValues, result_f32x4);
2007
2008 // and finally we store the results back to the vector
2009 return VectorT3<float>(resultValues);
2010}
2011
2012#endif // OCEAN_HARDWARE_SSE_VERSION >= 20
2013
2014template <typename T>
2016{
2017 SquareMatrixT3<T> result(*this);
2018 result.values_[0] *= value;
2019 result.values_[1] *= value;
2020 result.values_[2] *= value;
2021 result.values_[3] *= value;
2022 result.values_[4] *= value;
2023 result.values_[5] *= value;
2024 result.values_[6] *= value;
2025 result.values_[7] *= value;
2026 result.values_[8] *= value;
2027
2028 return result;
2029}
2030
2031template <typename T>
2033{
2034 values_[0] *= value;
2035 values_[1] *= value;
2036 values_[2] *= value;
2037 values_[3] *= value;
2038 values_[4] *= value;
2039 values_[5] *= value;
2040 values_[6] *= value;
2041 values_[7] *= value;
2042 values_[8] *= value;
2043
2044 return *this;
2045}
2046
2047template <typename T>
2048void SquareMatrixT3<T>::multiply(const SquareMatrixT3<T>& matrix, const VectorT2<T>* vectors, VectorT2<T>* results, const size_t number)
2049{
2050 ocean_assert((vectors && results) || number == 0);
2051
2052 for (size_t n = 0; n < number; ++n)
2053 {
2054 results[n] = matrix * vectors[n];
2055 }
2056}
2057
2058template <typename T>
2059void SquareMatrixT3<T>::multiply(const SquareMatrixT3<T>& matrix, const VectorT3<T>* vectors, VectorT3<T>* results, const size_t number)
2060{
2061 ocean_assert((vectors && results) || number == 0);
2062
2063 for (size_t n = 0; n < number; ++n)
2064 {
2065 results[n] = matrix * vectors[n];
2066 }
2067}
2068
2069#if defined(OCEAN_HARDWARE_SSE_VERSION) && OCEAN_HARDWARE_SSE_VERSION >= 20
2070
2071template <>
2072inline void SquareMatrixT3<float>::multiply(const SquareMatrixT3<float>& matrix, const VectorT3<float>* vectors, VectorT3<float>* results, const size_t number)
2073{
2074 // the following code uses the following SSE instructions, and needs SSE2 or higher
2075
2076 // SSE1:
2077 // _mm_load1_ps
2078 // _mm_loadu_ps
2079 // _mm_mul_ps
2080 // _mm_add_ps
2081 // _mm_storeu_ps
2082
2083 // SSE2:
2084 // _mm_castps_si128
2085 // _mm_srli_si128
2086 // _mm_castsi128_ps
2087
2088 // now we load the three columns (and ignore the last entry)
2089 const __m128 c0 = _mm_loadu_ps(matrix.values_ + 0);
2090 const __m128 c1 = _mm_loadu_ps(matrix.values_ + 3);
2091 __m128 c2 = _mm_loadu_ps(matrix.values_ + 5);
2092 c2 = _mm_castsi128_ps(_mm_srli_si128(_mm_castps_si128(c2), 4)); // shift the 128 bit register by 4 bytes to the right to receive [G H I 0]
2093
2094 for (size_t n = 0u; n < number - 1; ++n)
2095 {
2096 const __m128 v0 = _mm_load1_ps(vectors[n].data() + 0);
2097 const __m128 v1 = _mm_load1_ps(vectors[n].data() + 1);
2098 const __m128 v2 = _mm_load1_ps(vectors[n].data() + 2);
2099
2100 const __m128 r0 = _mm_mul_ps(c0, v0);
2101 const __m128 r1 = _mm_mul_ps(c1, v1);
2102
2103 __m128 result_f32x4= _mm_add_ps(r0, r1);
2104
2105 const __m128 r2 = _mm_mul_ps(c2, v2);
2106
2107 result_f32x4 = _mm_add_ps(result_f32x4, r2);
2108
2109 // now we write all four values (although the target vector holds 3 elements - we can due that as there is still one 3D vector target vector left)
2110 _mm_storeu_ps(results[n].data(), result_f32x4);
2111 }
2112
2113 // now we handle the last vector explicitly
2114
2115 const __m128 v0 = _mm_load1_ps(vectors[number - 1].data() + 0);
2116 const __m128 v1 = _mm_load1_ps(vectors[number - 1].data() + 1);
2117 const __m128 v2 = _mm_load1_ps(vectors[number - 1].data() + 2);
2118
2119 const __m128 r0 = _mm_mul_ps(c0, v0);
2120 const __m128 r1 = _mm_mul_ps(c1, v1);
2121
2122 __m128 result_f32x4 = _mm_add_ps(r0, r1);
2123
2124 const __m128 r2 = _mm_mul_ps(c2, v2);
2125
2126 result_f32x4 = _mm_add_ps(result_f32x4, r2);
2127
2128 float resultValues[4];
2129 _mm_storeu_ps(resultValues, result_f32x4);
2130
2131 results[number - 1] = VectorT3<float>(resultValues);
2132}
2133
2134#endif // OCEAN_HARDWARE_SSE_VERSION >= 20
2135
2136template <typename T>
2137template <typename U>
2139{
2140 SquareMatricesT3<T> result;
2141 result.reserve(matrices.size());
2142
2143 for (const SquareMatrixT3<U>& matrix : matrices)
2144 {
2145 result.emplace_back(matrix);
2146 }
2147
2148 return result;
2149}
2150
2151template <>
2152template <>
2153inline std::vector< SquareMatrixT3<float> > SquareMatrixT3<float>::matrices2matrices(const std::vector< SquareMatrixT3<float> >& matrices)
2154{
2155 return matrices;
2156}
2157
2158template <>
2159template <>
2160inline std::vector< SquareMatrixT3<double> > SquareMatrixT3<double>::matrices2matrices(const std::vector< SquareMatrixT3<double> >& matrices)
2161{
2162 return matrices;
2163}
2164
2165template <typename T>
2166template <typename U>
2167inline std::vector< SquareMatrixT3<T> > SquareMatrixT3<T>::matrices2matrices(const SquareMatrixT3<U>* matrices, const size_t size)
2168{
2169 std::vector< SquareMatrixT3<T> > result;
2170 result.reserve(size);
2171
2172 for (size_t n = 0; n < size; ++n)
2173 {
2174 result.emplace_back(matrices[n]);
2175 }
2176
2177 return result;
2178}
2179
2180template <typename T>
2181std::ostream& operator<<(std::ostream& stream, const SquareMatrixT3<T>& matrix)
2182{
2183 stream << "|" << matrix(0, 0) << ", " << matrix(0, 1) << ", " << matrix(0, 2) << "|" << std::endl;
2184 stream << "|" << matrix(1, 0) << ", " << matrix(1, 1) << ", " << matrix(1, 2) << "|" << std::endl;
2185 stream << "|" << matrix(2, 0) << ", " << matrix(2, 1) << ", " << matrix(2, 2) << "|";
2186
2187 return stream;
2188}
2189
2190template <bool tActive, typename T>
2191MessageObject<tActive>& operator<<(MessageObject<tActive>& messageObject, const SquareMatrixT3<T>& matrix)
2192{
2193 return messageObject << "|" << matrix(0, 0) << ", " << matrix(0, 1) << ", " << matrix(0, 2) << "|\n|"
2194 << matrix(1, 0) << ", " << matrix(1, 1) << ", " << matrix(1, 2) << "|\n|"
2195 << matrix(2, 0) << ", " << matrix(2, 1) << ", " << matrix(2, 2) << "|";
2196}
2197
2198template <bool tActive, typename T>
2199MessageObject<tActive>& operator<<(MessageObject<tActive>&& messageObject, const SquareMatrixT3<T>& matrix)
2200{
2201 return messageObject << "|" << matrix(0, 0) << ", " << matrix(0, 1) << ", " << matrix(0, 2) << "|\n|"
2202 << matrix(1, 0) << ", " << matrix(1, 1) << ", " << matrix(1, 2) << "|\n|"
2203 << matrix(2, 0) << ", " << matrix(2, 1) << ", " << matrix(2, 2) << "|";
2204}
2205
2206}
2207
2208#endif // META_OCEAN_MATH_SQUARE_MATRIX_3_H
This class provides several functions to solve equations with different degree using floating point v...
Definition Equation.h:53
This class implements an euler rotation with angles: yaw, pitch and roll.
Definition Euler.h:80
const T & yaw() const
Returns the yaw angle.
Definition Euler.h:315
const T & roll() const
Returns the roll angle.
Definition Euler.h:339
const T & pitch() const
Returns the pitch angle.
Definition Euler.h:327
This class implements a 4x4 homogeneous transformation matrix using floating point values with the pr...
Definition HomogenousMatrix4.h:110
This class provides basic numeric functionalities.
Definition Numeric.h:57
static T sin(const T value)
Returns the sine of a given value.
Definition Numeric.h:1568
static T abs(const T value)
Returns the absolute value of a given value.
Definition Numeric.h:1220
static T sqrt(const T value)
Returns the square root of a given value.
Definition Numeric.h:1533
static bool isEqual(const T first, const T second)
Returns whether two values are equal up to a small epsilon.
Definition Numeric.h:2386
static constexpr T sqr(const T value)
Returns the square of a given value.
Definition Numeric.h:1495
static constexpr bool isEqualEps(const T value)
Returns whether a value is smaller than or equal to a small epsilon.
Definition Numeric.h:2087
static T cos(const T value)
Returns the cosine of a given value.
Definition Numeric.h:1584
static constexpr bool isNotEqualEps(const T value)
Returns whether a value is not smaller than or equal to a small epsilon.
Definition Numeric.h:2237
This class implements a unit quaternion rotation.
Definition Quaternion.h:100
const T & x() const
Returns the x value of the quaternion.
Definition Quaternion.h:917
bool isValid() const
Returns whether this quaternion is a valid unit quaternion.
Definition Quaternion.h:899
const T & w() const
Returns the w value of the quaternion.
Definition Quaternion.h:905
const T & y() const
Returns the y value of the quaternion.
Definition Quaternion.h:929
const T & z() const
Returns the z value of the quaternion.
Definition Quaternion.h:941
This class implements a axis-angle rotation using floating point values.
Definition Rotation.h:79
VectorT3< T > axis() const
Returns the axis of the rotation.
Definition Rotation.h:740
T angle() const
Returns the angle of the rotation.
Definition Rotation.h:746
bool isValid() const
Returns whether this rotation has valid parameters.
Definition Rotation.h:665
This class implements a 3x3 square matrix.
Definition SquareMatrix3.h:88
size_t operator()(const SquareMatrixT3< T > &matrix) const
Hash function.
Definition SquareMatrix3.h:1125
VectorT3< T > xAxis() const
Returns the x axis which is the first column of the matrix.
Definition SquareMatrix3.h:1396
bool eigenValues(T *eigenValues) const
Determines the eigen values of this matrix.
Definition SquareMatrix3.h:1467
T * operator()()
Access operator.
Definition SquareMatrix3.h:1119
SquareMatrixT3< T > orthonormalMatrix() const
Returns the orthonormal matrix of this matrix by scaling the x-axis and adjusting y- and z-axis.
Definition SquareMatrix3.h:1414
SquareMatrixT3(const SquareMatrixT3< U > &matrix)
Copy constructor for a matrix with difference element data type than T.
Definition SquareMatrix3.h:706
OCEAN_FORCE_INLINE VectorT2< T > operator*(const VectorT2< T > &vector) const
Multiply operator for a 2D vector.
Definition SquareMatrix3.h:1865
static SquareMatrixT3< T > skewSymmetricMatrix(const VectorT3< T > &vector)
Creates a skew symmetric 3x3 matrix for a specific vector.
Definition SquareMatrix3.h:1702
T operator[](const unsigned int index) const
Element operator.
Definition SquareMatrix3.h:1071
SquareMatrixT3(const U *arrayValues)
Creates a new SquareMatrixT3 object by nine elements of float type U.
Definition SquareMatrix3.h:911
bool eigenSystem(T *eigenValues, VectorT3< T > *eigenVectors) const
Performs an eigen value analysis.
Definition SquareMatrix3.h:1520
bool isEqual(const SquareMatrixT3< T > &matrix, const T eps=NumericT< T >::eps()) const
Returns whether two matrices are almost identical up to a specified epsilon.
Definition SquareMatrix3.h:1386
bool invert()
Inverts this matrix in place.
Definition SquareMatrix3.h:1190
T & operator()(const unsigned int index)
Element operator.
Definition SquareMatrix3.h:1106
bool isNull() const
Returns whether this matrix is a zero matrix.
Definition SquareMatrix3.h:1333
static size_t elements()
Returns the number of elements this matrix has.
Definition SquareMatrix3.h:1138
SquareMatrixT3()
Creates a new SquareMatrixT3 object with undefined elements.
Definition SquareMatrix3.h:699
bool isAffine() const
Returns true if this matrix is a affine transformation.
Definition SquareMatrix3.h:1353
SquareMatrixT3< T > operator+(const SquareMatrixT3< T > &matrix) const
Adds two matrices.
Definition SquareMatrix3.h:1761
SquareMatrixT3(const VectorT3< T > &xAxis, const VectorT3< T > &yAxis, const VectorT3< T > &zAxis)
Creates a new SquareMatrixT3 object by three axes.
Definition SquareMatrix3.h:888
T values_[9]
The nine values of the matrix.
Definition SquareMatrix3.h:695
SquareMatrixT3(const U *arrayValues, const bool valuesRowAligned)
Creates a new SquareMatrixT3 object by nine elements.
Definition SquareMatrix3.h:930
SquareMatrixT3< T > transposedMultiply(const SquareMatrixT3< T > &right) const
Multiplies this transposed matrix with a second matrix.
Definition SquareMatrix3.h:1732
SquareMatrixT3(const T *arrayValues, const bool valuesRowAligned)
Creates a new SquareMatrixT3 object by nine elements.
Definition SquareMatrix3.h:957
void transpose()
Transposes the matrix.
Definition SquareMatrix3.h:1161
SquareMatrixT3< T > & operator=(const SquareMatrixT3< T > &)=default
Default copy assignment operator.
OCEAN_FORCE_INLINE SquareMatrixT3< T > operator*(const T value) const
Multiplies this matrix with a scalar value.
Definition SquareMatrix3.h:2015
OCEAN_FORCE_INLINE VectorT3< T > operator*(const VectorT3< T > &vector) const
Multiply operator for a 3D vector.
Definition SquareMatrix3.h:1883
SquareMatrixT3< T > transposed() const
Returns the transposed of this matrix.
Definition SquareMatrix3.h:1144
static SquareMatricesT3< T > matrices2matrices(const SquareMatricesT3< U > &matrices)
Converts matrices with specific data type to matrices with different data type.
Definition SquareMatrix3.h:2138
void copyElements(U *arrayValues, const bool valuesRowAligned=false) const
Copies the elements of this matrix to an array with floating point values of type U.
Definition SquareMatrix3.h:1632
bool multiply(const VectorT2< T > &vector, VectorT2< T > &result) const
Multiplies a 2D vector with this matrix (from the right).
Definition SquareMatrix3.h:1708
SquareMatrixT3(const SquareMatrixT3< T > &matrix)=default
Copy constructor.
T absSum() const
Calculates the sum of absolute value of elements.
Definition SquareMatrix3.h:1034
T & operator[](const unsigned int index)
Element operator.
Definition SquareMatrix3.h:1078
SquareMatrixT3< T > operator-() const
Returns the negative matrix of this matrix (all matrix elements are multiplied by -1).
Definition SquareMatrix3.h:1829
friend class SquareMatrixT3
Definition SquareMatrix3.h:89
OCEAN_FORCE_INLINE SquareMatrixT3< T > operator*(const SquareMatrixT3< T > &matrix) const
Multiplies two matrices.
Definition SquareMatrix3.h:1847
SquareMatrixT3< T > & operator-=(const SquareMatrixT3< T > &matrix)
Subtracts and assigns two matrices.
Definition SquareMatrix3.h:1813
static void multiply(const SquareMatrixT3< T > &matrix, const VectorT3< T > *vectors, VectorT3< T > *results, const size_t number)
Multiplies several 3D vectors with a given 3x3 matrix.
Definition SquareMatrix3.h:2059
bool operator!=(const SquareMatrixT3< T > &matrix) const
Returns whether two matrices are not identical up to a small epsilon.
Definition SquareMatrix3.h:1058
static void multiply(const SquareMatrixT3< T > &matrix, const VectorT2< T > *vectors, VectorT2< T > *results, const size_t number)
Multiplies several 2D vectors with a given 3x3 matrix.
Definition SquareMatrix3.h:2048
SquareMatrixT3(const bool setToIdentity)
Creates a new SquareMatrixT3 object.
Definition SquareMatrix3.h:720
SquareMatrixT3(const EulerT< T > &euler)
Creates a new SquareMatrixT3 rotation matrix by a given Euler rotation.
Definition SquareMatrix3.h:749
SquareMatrixT3(const HomogenousMatrixT4< T > &transformation)
Creates a 3x3 rotation matrix by a given 4x4 homogeneous transformation.
Definition SquareMatrix3.h:980
VectorT3< T > zAxis() const
Returns the z axis which is the last column of the matrix.
Definition SquareMatrix3.h:1408
SquareMatrixT3(const RotationT< T > &rotation)
Creates a new 3x3 matrix object by a given angle-axis rotation.
Definition SquareMatrix3.h:802
void toNull()
Sets the matrix to a zero matrix.
Definition SquareMatrix3.h:1311
const T * data() const
Returns a pointer to the internal values.
Definition SquareMatrix3.h:1046
T * data()
Returns a pointer to the internal values.
Definition SquareMatrix3.h:1052
const T * operator()() const
Access operator.
Definition SquareMatrix3.h:1113
void copyElements(T *arrayValues, const bool valuesRowAligned=false) const
Copies the elements of this matrix to an array with floating point values of type T.
Definition SquareMatrix3.h:1671
bool isHomography() const
Returns true if this matrix is perspective transform/homography.
Definition SquareMatrix3.h:1359
bool isSimilarity() const
Returns true if this matrix is a similarity transformation.
Definition SquareMatrix3.h:1347
VectorT3< T > yAxis() const
Returns the y axis which is the middle column of the matrix.
Definition SquareMatrix3.h:1402
void toIdentity()
Sets the matrix to the identity matrix.
Definition SquareMatrix3.h:1297
bool solve(const VectorT3< T > &b, VectorT3< T > &x) const
Solve a simple 3x3 system of linear equations: Ax = b Beware: The system of linear equations is assum...
Definition SquareMatrix3.h:996
SquareMatrixT3(const T &m00, const T &m10, const T &m20, const T &m01, const T &m11, const T &m21, const T &m02, const T &m12, const T &m22)
Creates a 3x3 rotation matrix by 9 given matrix elements.
Definition SquareMatrix3.h:872
T operator()(const unsigned int index) const
Element operator.
Definition SquareMatrix3.h:1099
T determinant() const
Returns the determinant of the matrix.
Definition SquareMatrix3.h:1283
SquareMatrixT3< T > operator-(const SquareMatrixT3< T > &matrix) const
Subtracts two matrices.
Definition SquareMatrix3.h:1795
SquareMatrixT3< T > & operator+=(const SquareMatrixT3< T > &matrix)
Adds and assigns two matrices.
Definition SquareMatrix3.h:1779
SquareMatrixT3(const T *arrayValues)
Creates a new SquareMatrixT3 object by nine elements.
Definition SquareMatrix3.h:922
bool invert(SquareMatrixT3< T > &invertedMatrix) const
Inverts the matrix and returns the result as parameter.
Definition SquareMatrix3.h:1205
SquareMatrixT3(const SquareMatrixT4< T > &transformation)
Creates a 3x3 square matrix by a given 4x4 square transformation.
Definition SquareMatrix3.h:988
T sum() const
Calculates the sum of elements.
Definition SquareMatrix3.h:1040
T Type
Definition of the used data type.
Definition SquareMatrix3.h:96
SquareMatrixT3< T > & operator*=(const T value)
Multiplies and assigns this matrix with a scalar value.
Definition SquareMatrix3.h:2032
bool isIdentity() const
Returns whether this matrix is an identity matrix.
Definition SquareMatrix3.h:1325
VectorT3< T > diagonal() const
Returns a 3d vector with values of the matrix diagonal.
Definition SquareMatrix3.h:1625
SquareMatrixT3(const VectorT3< T > &diagonal)
Creates a new SquareMatrixT3 object by a given diagonal vector.
Definition SquareMatrix3.h:896
OCEAN_FORCE_INLINE SquareMatrixT3< T > & operator*=(const SquareMatrixT3< T > &matrix)
Multiplies and assigns two matrices.
Definition SquareMatrix3.h:1064
static SquareMatricesT3< T > matrices2matrices(const SquareMatrixT3< U > *matrices, const size_t size)
Converts matrices with specific data type to matrices with different data type.
SquareMatrixT3< T > inverted() const
Returns the inverted matrix of this matrix.
Definition SquareMatrix3.h:1176
bool isOrthonormal(const T epsilon=NumericT< T >::eps()) const
Returns whether this matrix is an orthonormal matrix.
Definition SquareMatrix3.h:1365
bool isSymmetric(const T epsilon=NumericT< T >::eps()) const
Returns whether this matrix is symmetric.
Definition SquareMatrix3.h:1378
bool operator==(const SquareMatrixT3< T > &matrix) const
Returns whether two matrices are identical up to a small epsilon.
Definition SquareMatrix3.h:1755
T & operator()(const unsigned int row, const unsigned int column)
Element operator.
Definition SquareMatrix3.h:1092
bool isSingular() const
Returns whether this matrix is singular (and thus cannot be inverted).
Definition SquareMatrix3.h:1341
T trace() const
Returns the trace of the matrix which is the sum of the diagonal elements.
Definition SquareMatrix3.h:1291
SquareMatrixT3(const QuaternionT< T > &quaternion)
Creates a new 3x3 matrix object by a given quaternion rotation.
Definition SquareMatrix3.h:841
T operator()(const unsigned int row, const unsigned int column) const
Element operator.
Definition SquareMatrix3.h:1085
This class implements a 4x4 square matrix.
Definition SquareMatrix4.h:85
static void sortHighestToFront3(T &value0, T &value1, T &value2)
Sorts three values so that the highest value will finally be the first value.
Definition base/Utilities.h:700
This class implements a vector with two elements.
Definition Vector2.h:96
This class implements a vector with three elements.
Definition Vector3.h:97
VectorT3< T > perpendicular() const
Returns a vector that is perpendicular to this vector.
Definition Vector3.h:768
const T & y() const noexcept
Returns the y value.
Definition Vector3.h:824
bool normalize()
Normalizes this vector.
Definition Vector3.h:659
VectorT3< T > cross(const VectorT3< T > &vector) const
Returns the cross product of two vectors.
Definition Vector3.h:609
const T & x() const noexcept
Returns the x value.
Definition Vector3.h:812
const T & z() const noexcept
Returns the z value.
Definition Vector3.h:836
const T * data() const noexcept
Returns an pointer to the vector elements.
Definition Vector3.h:854
T length() const
Returns the length of the vector.
Definition Vector3.h:676
T sqr() const
Returns the square of the vector length.
Definition Vector3.h:682
unsigned int sqrDistance(const char first, const char second)
Returns the square distance between two values.
Definition base/Utilities.h:1089
SquareMatrixT3< Scalar > SquareMatrix3
Definition of the SquareMatrix3 object, depending on the OCEAN_MATH_USE_SINGLE_PRECISION either with ...
Definition SquareMatrix3.h:42
std::vector< SquareMatrixT3< T > > SquareMatricesT3
Definition of a typename alias for vectors with SquareMatrixT3 objects.
Definition SquareMatrix3.h:64
std::vector< SquareMatrix3 > SquareMatrices3
Definition of a vector holding SquareMatrix3 objects.
Definition SquareMatrix3.h:71
SquareMatrixT3< double > SquareMatrixD3
Instantiation of the SquareMatrixT3 template class using a double precision float data type.
Definition SquareMatrix3.h:49
SquareMatrixT3< float > SquareMatrixF3
Instantiation of the SquareMatrixT3 template class using a single precision float data type.
Definition SquareMatrix3.h:56
The namespace covering the entire Ocean framework.
Definition Accessor.h:15
std::ostream & operator<<(std::ostream &stream, const HighPerformanceStatistic &highPerformanceStatistic)
Definition HighPerformanceTimer.h:963