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
29 {
30 static_assert( sizeof( Matrix3<ValueType> ) == 3 * sizeof( VectorType ), "Struct size invalid" );
31 }
33 constexpr Matrix3( const Vector3<T> & x, const Vector3<T> & y, const Vector3<T> & z ) : x( x ), y( y ), z( z ) { }
34 template <typename U>
35 constexpr explicit Matrix3( const Matrix3<U> & m ) : x( m.x ), y( m.y ), z( m.z ) { }
36 static constexpr Matrix3 zero() noexcept { return Matrix3( Vector3<T>(), Vector3<T>(), Vector3<T>() ); }
37 static constexpr Matrix3 identity() noexcept { return Matrix3(); }
39 static constexpr Matrix3 scale( T s ) noexcept { return Matrix3( { s, T(0), T(0) }, { T(0), s, T(0) }, { T(0), T(0), s } ); }
41 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 } ); }
42 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 } ); }
44 static constexpr Matrix3 rotation( const Vector3<T> & axis, T angle ) noexcept MR_REQUIRES_IF_SUPPORTED( std::floating_point<T> );
46 static constexpr Matrix3 rotation( const Vector3<T> & from, const Vector3<T> & to ) noexcept MR_REQUIRES_IF_SUPPORTED( std::floating_point<T> );
49 static constexpr Matrix3 rotationFromEuler( const Vector3<T> & eulerAngles ) noexcept MR_REQUIRES_IF_SUPPORTED( std::is_floating_point_v<T> );
51 static constexpr Matrix3 approximateLinearRotationMatrixFromEuler( const Vector3<T> & eulerAngles ) noexcept MR_REQUIRES_IF_SUPPORTED( std::is_floating_point_v<T> );
53 static constexpr Matrix3 fromRows( const Vector3<T> & x, const Vector3<T> & y, const Vector3<T> & z ) noexcept { return Matrix3( x, y, z ); }
56 static constexpr Matrix3 fromColumns( const Vector3<T> & x, const Vector3<T> & y, const Vector3<T> & z ) noexcept { return Matrix3( x, y, z ).transposed(); }
57
59 constexpr const Vector3<T> & operator []( int row ) const noexcept { return *( ( VectorType* )this + row ); }
60 constexpr Vector3<T> & operator []( int row ) noexcept { return *( ( VectorType* )this + row ); }
61
63 constexpr Vector3<T> col( int i ) const noexcept { return { x[i], y[i], z[i] }; }
64
66 constexpr T trace() const noexcept { return x.x + y.y + z.z; }
68 constexpr T normSq() const noexcept { return x.lengthSq() + y.lengthSq() + z.lengthSq(); }
69 constexpr auto norm() const noexcept
70 {
71 // Calling `sqrt` this way to hopefully support boost.multiprecision numbers.
72 // Returning `auto` to not break on integral types.
73 using std::sqrt;
74 return sqrt( normSq() );
75 }
77 constexpr T det() const noexcept;
79 constexpr Matrix3<T> inverse() const noexcept MR_REQUIRES_IF_SUPPORTED( !std::is_integral_v<T> );
81 constexpr Matrix3<T> transposed() const noexcept;
83 constexpr Vector3<T> toEulerAngles() const noexcept MR_REQUIRES_IF_SUPPORTED( std::is_floating_point_v<T> );
84
85 struct QR
86 {
88 };
90 QR qr() const noexcept MR_REQUIRES_IF_SUPPORTED( !std::is_integral_v<T> );
91
92 [[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; }
93 [[nodiscard]] friend constexpr bool operator !=( const Matrix3<T> & a, const Matrix3<T> & b ) { return !( a == b ); }
94
95 // 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.
96
97 [[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 }; }
98 [[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 }; }
99 [[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 }; }
100 [[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 }; }
101 [[nodiscard]] friend constexpr auto operator /( Matrix3<T> b, T a ) -> Matrix3<decltype( std::declval<T>() / std::declval<T>() )>
102 {
103 if constexpr ( std::is_integral_v<T> )
104 return { b.x / a, b.y / a, b.z / a };
105 else
106 return b * ( 1 / a );
107 }
108
109 friend constexpr Matrix3<T> & operator +=( Matrix3<T> & a, const Matrix3<T> & b ) { a.x += b.x; a.y += b.y; a.z += b.z; return a; }
110 friend constexpr Matrix3<T> & operator -=( Matrix3<T> & a, const Matrix3<T> & b ) { a.x -= b.x; a.y -= b.y; a.z -= b.z; return a; }
111 friend constexpr Matrix3<T> & operator *=( Matrix3<T> & a, T b ) { a.x *= b; a.y *= b; a.z *= b; return a; }
112 friend constexpr Matrix3<T> & operator /=( Matrix3<T> & a, T b )
113 {
114 if constexpr ( std::is_integral_v<T> )
115 { a.x /= b; a.y /= b; a.z /= b; return a; }
116 else
117 return a *= ( 1 / b );
118 }
119
121 [[nodiscard]] friend constexpr auto operator *( const Matrix3<T> & a, const Vector3<T> & b ) -> Vector3<decltype( dot( std::declval<Vector3<T>>(), std::declval<Vector3<T>>() ) )>
122 {
123 return { dot( a.x, b ), dot( a.y, b ), dot( a.z, b ) };
124 }
125
127 [[nodiscard]] friend constexpr auto operator *( const Matrix3<T> & a, const Matrix3<T> & b ) -> Matrix3<decltype( dot( std::declval<Vector3<T>>(), std::declval<Vector3<T>>() ) )>
128 {
129 Matrix3<decltype( dot( std::declval<Vector3<T>>(), std::declval<Vector3<T>>() ) )> res;
130 for ( int i = 0; i < 3; ++i )
131 for ( int j = 0; j < 3; ++j )
132 res[i][j] = dot( a[i], b.col(j) );
133 return res;
134 }
135};
136
139
141template <typename T>
142inline auto dot( const Matrix3<T> & a, const Matrix3<T> & b ) -> decltype( dot( a.x, b.x ) )
143{
144 return dot( a.x, b.x ) + dot( a.y, b.y ) + dot( a.z, b.z );
145}
146
148template <typename T>
149inline Matrix3<T> outer( const Vector3<T> & a, const Vector3<T> & b )
150{
151 return { a.x * b, a.y * b, a.z * b };
152}
153
154template <typename T>
155constexpr Matrix3<T> Matrix3<T>::rotation( const Vector3<T> & axis, T angle ) noexcept MR_REQUIRES_IF_SUPPORTED( std::floating_point<T> )
156{
157 // https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle
158 auto u = axis.normalized();
159 T c = cos( angle );
160 T oc = 1 - c;
161 T s = sin( angle );
162 return {
163 { c + u.x * u.x * oc, u.x * u.y * oc - u.z * s, u.x * u.z * oc + u.y * s },
164 { u.y * u.x * oc + u.z * s, c + u.y * u.y * oc, u.y * u.z * oc - u.x * s },
165 { u.z * u.x * oc - u.y * s, u.z * u.y * oc + u.x * s, c + u.z * u.z * oc }
166 };
167}
168
169template <typename T>
170constexpr Matrix3<T> Matrix3<T>::rotation( const Vector3<T> & from, const Vector3<T> & to ) noexcept MR_REQUIRES_IF_SUPPORTED( std::floating_point<T> )
171{
172 auto axis = cross( from, to );
173 if ( axis.lengthSq() > 0 )
174 return rotation( axis, angle( from, to ) );
175 if ( dot( from, to ) >= 0 )
176 return {}; // identity matrix
177 return rotation( cross( from, from.furthestBasisVector() ), T( PI ) );
178}
179
180template <typename T>
181constexpr Matrix3<T> Matrix3<T>::rotationFromEuler( const Vector3<T> & eulerAngles ) noexcept MR_REQUIRES_IF_SUPPORTED( std::is_floating_point_v<T> )
182{
183 // https://www.geometrictools.com/Documentation/EulerAngles.pdf (36)
184 const auto cx = std::cos( eulerAngles.x );
185 const auto cy = std::cos( eulerAngles.y );
186 const auto cz = std::cos( eulerAngles.z );
187 const auto sx = std::sin( eulerAngles.x );
188 const auto sy = std::sin( eulerAngles.y );
189 const auto sz = std::sin( eulerAngles.z );
190 return {
191 { cy * cz, cz * sx * sy - cx * sz, cx * cz * sy + sx * sz },
192 { cy * sz, cx * cz + sx * sy * sz, -cz * sx + cx * sy * sz },
193 { -sy, cy * sx, cx * cy }
194 };
195}
196
197template <typename T>
198constexpr Matrix3<T> Matrix3<T>::approximateLinearRotationMatrixFromEuler( const Vector3<T> & eulerAngles ) noexcept MR_REQUIRES_IF_SUPPORTED( std::is_floating_point_v<T> )
199{
200 const auto alpha = eulerAngles.x;
201 const auto beta = eulerAngles.y;
202 const auto gamma = eulerAngles.z;
203 return {
204 { T(1), -gamma, beta },
205 { gamma, T(1), -alpha },
206 { -beta, alpha, T(1) }
207 };
208}
209
210template <typename T>
211constexpr T Matrix3<T>::det() const noexcept
212{
213 return
214 x.x * ( y.y * z.z - y.z * z.y )
215 - x.y * ( y.x * z.z - y.z * z.x )
216 + x.z * ( y.x * z.y - y.y * z.x );
217}
218
219template <typename T>
220constexpr Matrix3<T> Matrix3<T>::inverse() const noexcept MR_REQUIRES_IF_SUPPORTED( !std::is_integral_v<T> )
221{
222 auto det = this->det();
223 if ( det == 0 )
224 return {};
225 return Matrix3<T>
226 {
227 { y.y * z.z - y.z * z.y, x.z * z.y - x.y * z.z, x.y * y.z - x.z * y.y },
228 { y.z * z.x - y.x * z.z, x.x * z.z - x.z * z.x, x.z * y.x - x.x * y.z },
229 { y.x * z.y - y.y * z.x, x.y * z.x - x.x * z.y, x.x * y.y - x.y * y.x }
230 } / det;
231}
232
233template <typename T>
234constexpr Matrix3<T> Matrix3<T>::transposed() const noexcept
235{
236 return Matrix3<T>
237 {
238 { x.x, y.x, z.x },
239 { x.y, y.y, z.y },
240 { x.z, y.z, z.z }
241 };
242}
243
244template <typename T>
245constexpr Vector3<T> Matrix3<T>::toEulerAngles() const noexcept MR_REQUIRES_IF_SUPPORTED( std::is_floating_point_v<T> )
246{
247 // https://stackoverflow.com/questions/15022630/how-to-calculate-the-angle-from-rotation-matrix
248 return {
249 std::atan2( z.y, z.z ),
250 std::atan2( -z.x, std::sqrt( z.y * z.y + z.z * z.z ) ),
251 std::atan2( y.x, x.x )
252 };
253}
254
255template <typename T>
256auto Matrix3<T>::qr() const noexcept -> QR MR_REQUIRES_IF_SUPPORTED( !std::is_integral_v<T> )
257{
258 // https://en.wikipedia.org/wiki/QR_decomposition#Computing_the_QR_decomposition
259 const auto a0 = col( 0 );
260 auto a1 = col( 1 );
261 auto a2 = col( 2 );
262 const auto r00 = a0.length();
263 const auto e0 = r00 > 0 ? a0 / r00 : Vector3<T>{};
264 const auto r01 = dot( e0, a1 );
265 const auto r02 = dot( e0, a2 );
266 a1 -= r01 * e0;
267 const auto r11 = a1.length();
268 const auto e1 = r11 > 0 ? a1 / r11 : Vector3<T>{};
269 const auto r12 = dot( e1, a2 );
270 a2 -= r02 * e0 + r12 * e1;
271 const auto r22 = a2.length();
272 const auto e2 = r22 > 0 ? a2 / r22 : Vector3<T>{};
273 return QR
274 {
275 Matrix3::fromColumns( e0, e1, e2 ),
276 Matrix3::fromRows( { r00, r01, r02 }, { T(0), r11, r12 }, { T(0), T(0), r22 } )
277 };
278}
279
281
282#ifdef _MSC_VER
283#pragma warning(pop)
284#endif
285
286} // namespace MR
#define MR_REQUIRES_IF_SUPPORTED(...)
Definition MRMacros.h:34
Definition MRCameraOrientationPlugin.h:8
Vector3f cross(Vector3f a, Vector3f b)
int dot(Vector4b a, Vector4b b)
returns 3 Euler angles, assuming this is a rotation matrix composed as follows: R=R(z)*R(y)*R(x)
Definition MRMesh/MRMatrix3.h:86
Matrix3 q
Definition MRMesh/MRMatrix3.h:87
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:41
Matrix3< T > outer(const Vector3< T > &a, const Vector3< T > &b)
x = a * b^T
Definition MRMesh/MRMatrix3.h:149
T ValueType
Definition MRMesh/MRMatrix3.h:20
static constexpr Matrix3 identity() noexcept
Definition MRMesh/MRMatrix3.h:37
friend constexpr auto operator*(T a, const Matrix3< T > &b) -> Matrix3< decltype(std::declval< T >() *std::declval< T >())>
Definition MRMesh/MRMatrix3.h:99
constexpr Matrix3< T > inverse() const noexcept MR_REQUIRES_IF_SUPPORTED(!std constexpr Matrix3< T > transposed() const noexcept
computes inverse matrix
constexpr T det() const noexcept
computes determinant of the matrix
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:59
friend constexpr Matrix3< T > & operator-=(Matrix3< T > &a, const Matrix3< T > &b)
Definition MRMesh/MRMatrix3.h:110
constexpr Matrix3(const Vector3< T > &x, const Vector3< T > &y, const Vector3< T > &z)
initializes matrix from its 3 rows
Definition MRMesh/MRMatrix3.h:33
constexpr T normSq() const noexcept
compute sum of squared matrix elements
Definition MRMesh/MRMatrix3.h:68
friend constexpr Matrix3< T > & operator*=(Matrix3< T > &a, T b)
Definition MRMesh/MRMatrix3.h:111
friend constexpr Matrix3< T > & operator+=(Matrix3< T > &a, const Matrix3< T > &b)
Definition MRMesh/MRMatrix3.h:109
constexpr auto norm() const noexcept
Definition MRMesh/MRMatrix3.h:69
constexpr Vector3< T > col(int i) const noexcept
column access
Definition MRMesh/MRMatrix3.h:63
static constexpr Matrix3 scale(T s) noexcept
returns a matrix that scales uniformly
Definition MRMesh/MRMatrix3.h:39
constexpr T trace() const noexcept
computes trace of the matrix
Definition MRMesh/MRMatrix3.h:66
friend constexpr bool operator!=(const Matrix3< T > &a, const Matrix3< T > &b)
Definition MRMesh/MRMatrix3.h:93
static constexpr Matrix3 fromColumns(const Vector3< T > &x, const Vector3< T > &y, const Vector3< T > &z) noexcept
Definition MRMesh/MRMatrix3.h:56
constexpr Matrix3() noexcept
Definition MRMesh/MRMatrix3.h:28
Vector3< T > y
Definition MRMesh/MRMatrix3.h:25
static constexpr Matrix3 zero() noexcept
Definition MRMesh/MRMatrix3.h:36
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:142
friend constexpr auto operator-(const Matrix3< T > &a, const Matrix3< T > &b) -> Matrix3< decltype(std::declval< T >() - std::declval< T >())>
Definition MRMesh/MRMatrix3.h:98
friend constexpr auto operator+(const Matrix3< T > &a, const Matrix3< T > &b) -> Matrix3< decltype(std::declval< T >()+std::declval< T >())>
Definition MRMesh/MRMatrix3.h:97
constexpr Matrix3(const Matrix3< U > &m)
Definition MRMesh/MRMatrix3.h:35
friend constexpr auto operator/(Matrix3< T > b, T a) -> Matrix3< decltype(std::declval< T >()/std::declval< T >())>
Definition MRMesh/MRMatrix3.h:101
static constexpr Matrix3 static rotation(const Vector3< T > &axis, T angle) noexcept MR_REQUIRES_IF_SUPPORTED(std constexpr Matrix3 static rotation(const Vector3< T > &from, const Vector3< T > &to) noexcept MR_REQUIRES_IF_SUPPORTED(std constexpr Matrix3 static rotationFromEuler(const Vector3< T > &eulerAngles) noexcept MR_REQUIRES_IF_SUPPORTED(std constexpr Matrix3 static approximateLinearRotationMatrixFromEuler(const Vector3< T > &eulerAngles) noexcept MR_REQUIRES_IF_SUPPORTED(std constexpr Matrix fromRows)(const Vector3< T > &x, const Vector3< T > &y, const Vector3< T > &z) noexcept
creates matrix representing rotation around given axis on given angle
Definition MRMesh/MRMatrix3.h:53
Vector3< T > z
Definition MRMesh/MRMatrix3.h:26
friend constexpr Matrix3< T > & operator/=(Matrix3< T > &a, T b)
Definition MRMesh/MRMatrix3.h:112
static constexpr Matrix3 scale(const Vector3< T > &s) noexcept
Definition MRMesh/MRMatrix3.h:42
Definition MRMesh/MRVector3.h:29
T x
Definition MRMesh/MRVector3.h:35
T y
Definition MRMesh/MRVector3.h:35
T z
Definition MRMesh/MRVector3.h:35