MeshLib C++ Docs
Loading...
Searching...
No Matches
MRMesh/MRMatrix3.h
Go to the documentation of this file.
1#pragma once
2
3#include "MRVector3.h"
4#include "MRConstants.h"
5
6namespace MR
7{
8
9#ifdef _MSC_VER
10#pragma warning(push)
11#pragma warning(disable: 4804) // unsafe use of type 'bool' in operation
12#pragma warning(disable: 4146) // unary minus operator applied to unsigned type, result still unsigned
13#endif
14
17template <typename T>
18struct Matrix3
19{
20 using ValueType = T;
22
24 Vector3<T> x{ 1, 0, 0 };
25 Vector3<T> y{ 0, 1, 0 };
26 Vector3<T> z{ 0, 0, 1 };
27
28 constexpr Matrix3() noexcept = default;
30 constexpr Matrix3( const Vector3<T> & x, const Vector3<T> & y, const Vector3<T> & z ) : x( x ), y( y ), z( z ) { }
31 template <typename U>
32 constexpr explicit Matrix3( const Matrix3<U> & m ) : x( m.x ), y( m.y ), z( m.z ) { }
33 static constexpr Matrix3 zero() noexcept { return Matrix3( Vector3<T>(), Vector3<T>(), Vector3<T>() ); }
34 static constexpr Matrix3 identity() noexcept { return Matrix3(); }
36 static constexpr Matrix3 scale( T s ) noexcept { return Matrix3( { s, T(0), T(0) }, { T(0), s, T(0) }, { T(0), T(0), s } ); }
38 static constexpr Matrix3 scale( T sx, T sy, T sz ) noexcept { return Matrix3( { sx, T(0), T(0) }, { T(0), sy, T(0) }, { T(0), T(0), sz } ); }
39 static constexpr Matrix3 scale( const Vector3<T> & s ) noexcept { return Matrix3( { s.x, T(0), T(0) }, { T(0), s.y, T(0) }, { T(0), T(0), s.z } ); }
41 static constexpr Matrix3 rotation( const Vector3<T> & axis, T angle ) noexcept MR_REQUIRES_IF_SUPPORTED( std::floating_point<T> );
43 static constexpr Matrix3 rotation( const Vector3<T> & from, const Vector3<T> & to ) noexcept MR_REQUIRES_IF_SUPPORTED( std::floating_point<T> );
46 static constexpr Matrix3 rotationFromEuler( const Vector3<T> & eulerAngles ) noexcept MR_REQUIRES_IF_SUPPORTED( std::is_floating_point_v<T> );
48 static constexpr Matrix3 approximateLinearRotationMatrixFromEuler( const Vector3<T> & eulerAngles ) noexcept MR_REQUIRES_IF_SUPPORTED( std::is_floating_point_v<T> );
50 static constexpr Matrix3 fromRows( const Vector3<T> & x, const Vector3<T> & y, const Vector3<T> & z ) noexcept { return Matrix3( x, y, z ); }
53 static constexpr Matrix3 fromColumns( const Vector3<T> & x, const Vector3<T> & y, const Vector3<T> & z ) noexcept { return Matrix3( x, y, z ).transposed(); }
54
56 constexpr const Vector3<T> & operator []( int row ) const noexcept { return *( &x + row ); }
57 constexpr Vector3<T> & operator []( int row ) noexcept { return *( &x + row ); }
58
60 constexpr Vector3<T> col( int i ) const noexcept { return { x[i], y[i], z[i] }; }
61
63 constexpr T trace() const noexcept { return x.x + y.y + z.z; }
65 constexpr T normSq() const noexcept { return x.lengthSq() + y.lengthSq() + z.lengthSq(); }
66 constexpr auto norm() const noexcept
67 {
68 // Calling `sqrt` this way to hopefully support boost.multiprecision numbers.
69 // Returning `auto` to not break on integral types.
70 using std::sqrt;
71 return sqrt( normSq() );
72 }
74 constexpr T det() const noexcept;
76 constexpr Matrix3<T> inverse() const noexcept MR_REQUIRES_IF_SUPPORTED( !std::is_integral_v<T> );
78 constexpr Matrix3<T> transposed() const noexcept;
80 constexpr Vector3<T> toEulerAngles() const noexcept MR_REQUIRES_IF_SUPPORTED( std::is_floating_point_v<T> );
81
82 struct QR
83 {
85 };
87 QR qr() const noexcept MR_REQUIRES_IF_SUPPORTED( !std::is_integral_v<T> );
88
89 [[nodiscard]] friend constexpr bool operator ==( const Matrix3<T> & a, const Matrix3<T> & b ) { return a.x == b.x && a.y == b.y && a.z == b.z; }
90 [[nodiscard]] friend constexpr bool operator !=( const Matrix3<T> & a, const Matrix3<T> & b ) { return !( a == b ); }
91
92 // NOTE: We use `std::declval()` in the operators below because libclang 18 in our binding generator is bugged and chokes on decltyping `a.x` and such. TODO fix this when we update libclang.
93
94 [[nodiscard]] friend constexpr auto operator +( const Matrix3<T> & a, const Matrix3<T> & b ) -> Matrix3<decltype( std::declval<T>() + std::declval<T>() )> { return { a.x + b.x, a.y + b.y, a.z + b.z }; }
95 [[nodiscard]] friend constexpr auto operator -( const Matrix3<T> & a, const Matrix3<T> & b ) -> Matrix3<decltype( std::declval<T>() - std::declval<T>() )> { return { a.x - b.x, a.y - b.y, a.z - b.z }; }
96 [[nodiscard]] friend constexpr auto operator *( T a, const Matrix3<T> & b ) -> Matrix3<decltype( std::declval<T>() * std::declval<T>() )> { return { a * b.x, a * b.y, a * b.z }; }
97 [[nodiscard]] friend constexpr auto operator *( const Matrix3<T> & b, T a ) -> Matrix3<decltype( std::declval<T>() * std::declval<T>() )> { return { a * b.x, a * b.y, a * b.z }; }
98 [[nodiscard]] friend constexpr auto operator /( Matrix3<T> b, T a ) -> Matrix3<decltype( std::declval<T>() / std::declval<T>() )>
99 {
100 if constexpr ( std::is_integral_v<T> )
101 return { b.x / a, b.y / a, b.z / a };
102 else
103 return b * ( 1 / a );
104 }
105
106 friend constexpr Matrix3<T> & operator +=( Matrix3<T> & a, const Matrix3<T> & b ) MR_REQUIRES_IF_SUPPORTED( requires{ a + b; } ) { a.x += b.x; a.y += b.y; a.z += b.z; return a; }
107 friend constexpr Matrix3<T> & operator -=( Matrix3<T> & a, const Matrix3<T> & b ) MR_REQUIRES_IF_SUPPORTED( requires{ a - b; } ) { a.x -= b.x; a.y -= b.y; a.z -= b.z; return a; }
108 friend constexpr Matrix3<T> & operator *=( Matrix3<T> & a, T b ) MR_REQUIRES_IF_SUPPORTED( requires{ a * b; } ) { a.x *= b; a.y *= b; a.z *= b; return a; }
109 friend constexpr Matrix3<T> & operator /=( Matrix3<T> & a, T b ) MR_REQUIRES_IF_SUPPORTED( requires{ a / b; } )
110 {
111 if constexpr ( std::is_integral_v<T> )
112 { a.x /= b; a.y /= b; a.z /= b; return a; }
113 else
114 return a *= ( 1 / b );
115 }
116
118 [[nodiscard]] friend constexpr auto operator *( const Matrix3<T> & a, const Vector3<T> & b ) -> Vector3<decltype( dot( std::declval<Vector3<T>>(), std::declval<Vector3<T>>() ) )>
119 {
120 return { dot( a.x, b ), dot( a.y, b ), dot( a.z, b ) };
121 }
122
124 [[nodiscard]] friend constexpr auto operator *( const Matrix3<T> & a, const Matrix3<T> & b ) -> Matrix3<decltype( dot( std::declval<Vector3<T>>(), std::declval<Vector3<T>>() ) )>
125 {
126 Matrix3<decltype( dot( std::declval<Vector3<T>>(), std::declval<Vector3<T>>() ) )> res;
127 for ( int i = 0; i < 3; ++i )
128 for ( int j = 0; j < 3; ++j )
129 res[i][j] = dot( a[i], b.col(j) );
130 return res;
131 }
132};
133
136
138template <typename T>
139inline auto dot( const Matrix3<T> & a, const Matrix3<T> & b ) -> decltype( dot( a.x, b.x ) )
140{
141 return dot( a.x, b.x ) + dot( a.y, b.y ) + dot( a.z, b.z );
142}
143
145template <typename T>
146inline Matrix3<T> outer( const Vector3<T> & a, const Vector3<T> & b )
147{
148 return { a.x * b, a.y * b, a.z * b };
149}
150
151template <typename T>
152constexpr Matrix3<T> Matrix3<T>::rotation( const Vector3<T> & axis, T angle ) noexcept MR_REQUIRES_IF_SUPPORTED( std::floating_point<T> )
153{
154 // https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle
155 auto u = axis.normalized();
156 T c = cos( angle );
157 T oc = 1 - c;
158 T s = sin( angle );
159 return {
160 { c + u.x * u.x * oc, u.x * u.y * oc - u.z * s, u.x * u.z * oc + u.y * s },
161 { u.y * u.x * oc + u.z * s, c + u.y * u.y * oc, u.y * u.z * oc - u.x * s },
162 { u.z * u.x * oc - u.y * s, u.z * u.y * oc + u.x * s, c + u.z * u.z * oc }
163 };
164}
165
166template <typename T>
167constexpr Matrix3<T> Matrix3<T>::rotation( const Vector3<T> & from, const Vector3<T> & to ) noexcept MR_REQUIRES_IF_SUPPORTED( std::floating_point<T> )
168{
169 auto axis = cross( from, to );
170 if ( axis.lengthSq() > 0 )
171 return rotation( axis, angle( from, to ) );
172 if ( dot( from, to ) >= 0 )
173 return {}; // identity matrix
174 return rotation( cross( from, from.furthestBasisVector() ), T( PI ) );
175}
176
177template <typename T>
178constexpr Matrix3<T> Matrix3<T>::rotationFromEuler( const Vector3<T> & eulerAngles ) noexcept MR_REQUIRES_IF_SUPPORTED( std::is_floating_point_v<T> )
179{
180 // https://www.geometrictools.com/Documentation/EulerAngles.pdf (36)
181 const auto cx = std::cos( eulerAngles.x );
182 const auto cy = std::cos( eulerAngles.y );
183 const auto cz = std::cos( eulerAngles.z );
184 const auto sx = std::sin( eulerAngles.x );
185 const auto sy = std::sin( eulerAngles.y );
186 const auto sz = std::sin( eulerAngles.z );
187 return {
188 { cy * cz, cz * sx * sy - cx * sz, cx * cz * sy + sx * sz },
189 { cy * sz, cx * cz + sx * sy * sz, -cz * sx + cx * sy * sz },
190 { -sy, cy * sx, cx * cy }
191 };
192}
193
194template <typename T>
195constexpr Matrix3<T> Matrix3<T>::approximateLinearRotationMatrixFromEuler( const Vector3<T> & eulerAngles ) noexcept MR_REQUIRES_IF_SUPPORTED( std::is_floating_point_v<T> )
196{
197 const auto alpha = eulerAngles.x;
198 const auto beta = eulerAngles.y;
199 const auto gamma = eulerAngles.z;
200 return {
201 { T(1), -gamma, beta },
202 { gamma, T(1), -alpha },
203 { -beta, alpha, T(1) }
204 };
205}
206
207template <typename T>
208constexpr T Matrix3<T>::det() const noexcept
209{
210 return
211 x.x * ( y.y * z.z - y.z * z.y )
212 - x.y * ( y.x * z.z - y.z * z.x )
213 + x.z * ( y.x * z.y - y.y * z.x );
214}
215
216template <typename T>
217constexpr Matrix3<T> Matrix3<T>::inverse() const noexcept MR_REQUIRES_IF_SUPPORTED( !std::is_integral_v<T> )
218{
219 auto det = this->det();
220 if ( det == 0 )
221 return {};
222 return Matrix3<T>
223 {
224 { y.y * z.z - y.z * z.y, x.z * z.y - x.y * z.z, x.y * y.z - x.z * y.y },
225 { y.z * z.x - y.x * z.z, x.x * z.z - x.z * z.x, x.z * y.x - x.x * y.z },
226 { y.x * z.y - y.y * z.x, x.y * z.x - x.x * z.y, x.x * y.y - x.y * y.x }
227 } / det;
228}
229
230template <typename T>
231constexpr Matrix3<T> Matrix3<T>::transposed() const noexcept
232{
233 return Matrix3<T>
234 {
235 { x.x, y.x, z.x },
236 { x.y, y.y, z.y },
237 { x.z, y.z, z.z }
238 };
239}
240
241template <typename T>
242constexpr Vector3<T> Matrix3<T>::toEulerAngles() const noexcept MR_REQUIRES_IF_SUPPORTED( std::is_floating_point_v<T> )
243{
244 // https://stackoverflow.com/questions/15022630/how-to-calculate-the-angle-from-rotation-matrix
245 return {
246 std::atan2( z.y, z.z ),
247 std::atan2( -z.x, std::sqrt( z.y * z.y + z.z * z.z ) ),
248 std::atan2( y.x, x.x )
249 };
250}
251
252template <typename T>
253auto Matrix3<T>::qr() const noexcept -> QR MR_REQUIRES_IF_SUPPORTED( !std::is_integral_v<T> )
254{
255 // https://en.wikipedia.org/wiki/QR_decomposition#Computing_the_QR_decomposition
256 const auto a0 = col( 0 );
257 auto a1 = col( 1 );
258 auto a2 = col( 2 );
259 const auto r00 = a0.length();
260 const auto e0 = r00 > 0 ? a0 / r00 : Vector3<T>{};
261 const auto r01 = dot( e0, a1 );
262 const auto r02 = dot( e0, a2 );
263 a1 -= r01 * e0;
264 const auto r11 = a1.length();
265 const auto e1 = r11 > 0 ? a1 / r11 : Vector3<T>{};
266 const auto r12 = dot( e1, a2 );
267 a2 -= r02 * e0 + r12 * e1;
268 const auto r22 = a2.length();
269 const auto e2 = r22 > 0 ? a2 / r22 : Vector3<T>{};
270 return QR
271 {
272 Matrix3::fromColumns( e0, e1, e2 ),
273 Matrix3::fromRows( { r00, r01, r02 }, { T(0), r11, r12 }, { T(0), T(0), r22 } )
274 };
275}
276
278
279#ifdef _MSC_VER
280#pragma warning(pop)
281#endif
282
283} // namespace MR
#define MR_REQUIRES_IF_SUPPORTED(...)
Definition MRMacros.h:31
MRMESH_CLASS Vector3
Definition MRMesh/MRMeshFwd.h:170
int dot(Vector2i a, Vector2i b)
Vector3f cross(Vector3f a, Vector3f b)
Definition MRMesh/MRMatrix3.h:83
Matrix3 q
Definition MRMesh/MRMatrix3.h:84
Definition MRMesh/MRMatrix3.h:19
static constexpr Matrix3 scale(T sx, T sy, T sz) noexcept
returns a matrix that has its own scale along each axis
Definition MRMesh/MRMatrix3.h:38
Matrix3< T > outer(const Vector3< T > &a, const Vector3< T > &b)
x = a * b^T
Definition MRMesh/MRMatrix3.h:146
T ValueType
Definition MRMesh/MRMatrix3.h:20
static constexpr Matrix3 fromRows(const Vector3< T > &x, const Vector3< T > &y, const Vector3< T > &z) noexcept
constructs a matrix from its 3 rows
Definition MRMesh/MRMatrix3.h:50
static constexpr Matrix3 identity() noexcept
Definition MRMesh/MRMatrix3.h:34
friend constexpr auto operator*(T a, const Matrix3< T > &b) -> Matrix3< decltype(std::declval< T >() *std::declval< T >())>
Definition MRMesh/MRMatrix3.h:96
constexpr Matrix3< T > transposed() const noexcept
computes transposed matrix
constexpr T det() const noexcept
computes determinant of the matrix
static constexpr Matrix3 rotationFromEuler(const Vector3< T > &eulerAngles) noexcept
Vector3< T > x
rows, identity matrix by default
Definition MRMesh/MRMatrix3.h:24
constexpr const Vector3< T > & operator[](int row) const noexcept
row access
Definition MRMesh/MRMatrix3.h:56
friend constexpr Matrix3< T > & operator-=(Matrix3< T > &a, const Matrix3< T > &b)
Definition MRMesh/MRMatrix3.h:107
constexpr Matrix3< T > inverse() const noexcept
computes inverse matrix
constexpr T normSq() const noexcept
compute sum of squared matrix elements
Definition MRMesh/MRMatrix3.h:65
friend constexpr Matrix3< T > & operator*=(Matrix3< T > &a, T b)
Definition MRMesh/MRMatrix3.h:108
static constexpr Matrix3 rotation(const Vector3< T > &axis, T angle) noexcept
creates matrix representing rotation around given axis on given angle
friend constexpr Matrix3< T > & operator+=(Matrix3< T > &a, const Matrix3< T > &b)
Definition MRMesh/MRMatrix3.h:106
constexpr auto norm() const noexcept
Definition MRMesh/MRMatrix3.h:66
constexpr Vector3< T > toEulerAngles() const noexcept
returns 3 Euler angles, assuming this is a rotation matrix composed as follows: R=R(z)*R(y)*R(x)
constexpr Vector3< T > col(int i) const noexcept
column access
Definition MRMesh/MRMatrix3.h:60
static constexpr Matrix3 rotation(const Vector3< T > &from, const Vector3< T > &to) noexcept
creates matrix representing rotation that after application to (from) makes (to) vector
static constexpr Matrix3 scale(T s) noexcept
returns a matrix that scales uniformly
Definition MRMesh/MRMatrix3.h:36
constexpr T trace() const noexcept
computes trace of the matrix
Definition MRMesh/MRMatrix3.h:63
friend constexpr bool operator!=(const Matrix3< T > &a, const Matrix3< T > &b)
Definition MRMesh/MRMatrix3.h:90
static constexpr Matrix3 fromColumns(const Vector3< T > &x, const Vector3< T > &y, const Vector3< T > &z) noexcept
Definition MRMesh/MRMatrix3.h:53
Vector3< T > y
Definition MRMesh/MRMatrix3.h:25
static constexpr Matrix3 zero() noexcept
Definition MRMesh/MRMatrix3.h:33
auto dot(const Matrix3< T > &a, const Matrix3< T > &b) -> decltype(dot(a.x, b.x))
double-dot product: x = a : b
Definition MRMesh/MRMatrix3.h:139
friend constexpr auto operator-(const Matrix3< T > &a, const Matrix3< T > &b) -> Matrix3< decltype(std::declval< T >() - std::declval< T >())>
Definition MRMesh/MRMatrix3.h:95
QR qr() const noexcept
decompose this matrix on the product Q*R, where Q is orthogonal and R is upper triangular
friend constexpr auto operator+(const Matrix3< T > &a, const Matrix3< T > &b) -> Matrix3< decltype(std::declval< T >()+std::declval< T >())>
Definition MRMesh/MRMatrix3.h:94
constexpr Matrix3(const Matrix3< U > &m)
Definition MRMesh/MRMatrix3.h:32
friend constexpr auto operator/(Matrix3< T > b, T a) -> Matrix3< decltype(std::declval< T >()/std::declval< T >())>
Definition MRMesh/MRMatrix3.h:98
Vector3< T > z
Definition MRMesh/MRMatrix3.h:26
friend constexpr Matrix3< T > & operator/=(Matrix3< T > &a, T b)
Definition MRMesh/MRMatrix3.h:109
static constexpr Matrix3 approximateLinearRotationMatrixFromEuler(const Vector3< T > &eulerAngles) noexcept
returns linear by angles approximation of the rotation matrix, which is close to true rotation matrix...
constexpr Matrix3() noexcept=default
static constexpr Matrix3 scale(const Vector3< T > &s) noexcept
Definition MRMesh/MRMatrix3.h:39
Definition MRMesh/MRVector3.h:26
T x
Definition MRMesh/MRVector3.h:32
T y
Definition MRMesh/MRVector3.h:32
T z
Definition MRMesh/MRVector3.h:32