API Documentation
Loading...
Searching...
No Matches
Matrix.hpp
Go to the documentation of this file.
1/**--------------------------------------------------------------------------------------------
2Copyright (c) 2019, NDEVR LLC
3tyler.parke@ndevr.org
4 __ __ ____ _____ __ __ _______
5 | \ | | | __ \ | ___|\ \ / / | __ \
6 | \ | | | | \ \ | |___ \ \ / / | |__) |
7 | . \| | | |__/ / | |___ \ V / | _ /
8 | |\ |_|_____/__|_____|___\_/____| | \ \
9 |__| \__________________________________| \__\
10
11Subject to the terms of the Enterprise+ Agreement, NDEVR hereby grants
12Licensee a limited, non-exclusive, non-transferable, royalty-free license
13(without the right to sublicense) to use the API solely for the purpose of
14Licensee's internal development efforts to develop applications for which
15the API was provided.
16
17The above copyright notice and this permission notice shall be included in all
18copies or substantial portions of the Software.
19
20THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
21INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
22PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE
23FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
24OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
25DEALINGS IN THE SOFTWARE.
26
27Library: Base
28File: Matrix
29Included in API: True
30Author(s): Tyler Parke
31 *-----------------------------------------------------------------------------------------**/
32#pragma once
33#include <NDEVR/BaseValues.h>
34#include <NDEVR/Vector.h>
35#include <NDEVR/Angle.h>
36namespace NDEVR
37{
38 template<uint01 t_cols, uint01 t_rows>
39 class MatrixInverter;
40
41 template<>
42 class MatrixInverter<1, 1>
43 {
44 public:
45 template<class t_type>
46 static Vector<1, Vector<1, t_type>> invert(Vector<1, Vector<1, t_type>>& value)
47 {
48 return value;
49 }
50 };
51
52 template<>
53 class MatrixInverter<2, 2>
54 {
55 public:
56 template<class t_type>
57 static constexpr t_type determinant(const Vector<2, Vector<2, t_type>>& value)
58 {
59 return value[0][0] * value[1][1] - value[1][0] * value[0][1];
60 }
61 template<class t_type>
62 static Vector<2, Vector<2, t_type>> invert(const Vector<2, Vector<2, t_type>>& value)
63 {
64 t_type OneOverDeterminant = static_cast<t_type>(1) / value.determinant();
65 return Vector<3, Vector<3, t_type>>(
66 +value[1][1] * OneOverDeterminant,
67 -value[0][1] * OneOverDeterminant,
68 -value[1][0] * OneOverDeterminant,
69 +value[0][0] * OneOverDeterminant).template getAs<t_type, 2, 2>();
70 }
71 };
72
73 template<>
74 class MatrixInverter<3, 3>
75 {
76 public:
77 template<class t_type>
78 static constexpr t_type determinant(const Vector<3, Vector<3, t_type>>& value)
79 {
80 return + value[0][0] * (value[1][1] * value[2][2] - value[2][1] * value[1][2])
81 - value[1][0] * (value[0][1] * value[2][2] - value[2][1] * value[0][2])
82 + value[2][0] * (value[0][1] * value[1][2] - value[1][1] * value[0][2]);
83 }
84 template<class t_type>
85 static Vector<3, Vector<3, t_type>> invert(const Vector<3, Vector<3, t_type>>& value)
86 {
87 t_type OneOverDeterminant = static_cast<t_type>(1) / determinant(value);
88 Vector<3, Vector<3, t_type>> Inverse;
89 Inverse[0][0] = +(value[1][1] * value[2][2] - value[2][1] * value[1][2]) * OneOverDeterminant;
90 Inverse[1][0] = -(value[1][0] * value[2][2] - value[2][0] * value[1][2]) * OneOverDeterminant;
91 Inverse[2][0] = +(value[1][0] * value[2][1] - value[2][0] * value[1][1]) * OneOverDeterminant;
92 Inverse[0][1] = -(value[0][1] * value[2][2] - value[2][1] * value[0][2]) * OneOverDeterminant;
93 Inverse[1][1] = +(value[0][0] * value[2][2] - value[2][0] * value[0][2]) * OneOverDeterminant;
94 Inverse[2][1] = -(value[0][0] * value[2][1] - value[2][0] * value[0][1]) * OneOverDeterminant;
95 Inverse[0][2] = +(value[0][1] * value[1][2] - value[1][1] * value[0][2]) * OneOverDeterminant;
96 Inverse[1][2] = -(value[0][0] * value[1][2] - value[1][0] * value[0][2]) * OneOverDeterminant;
97 Inverse[2][2] = +(value[0][0] * value[1][1] - value[1][0] * value[0][1]) * OneOverDeterminant;
98
99 return Inverse;
100 }
101 };
102 template<>
103 class MatrixInverter<4, 4>
104 {
105 public:
106 template<class t_type>
107 static Vector<4, Vector<4, t_type>> invert(const Vector<4, Vector<4, t_type>>& value)
108 {
109 t_type Coef00 = value[2][2] * value[3][3] - value[3][2] * value[2][3];
110 t_type Coef02 = value[1][2] * value[3][3] - value[3][2] * value[1][3];
111 t_type Coef03 = value[1][2] * value[2][3] - value[2][2] * value[1][3];
112
113 t_type Coef04 = value[2][1] * value[3][3] - value[3][1] * value[2][3];
114 t_type Coef06 = value[1][1] * value[3][3] - value[3][1] * value[1][3];
115 t_type Coef07 = value[1][1] * value[2][3] - value[2][1] * value[1][3];
116
117 t_type Coef08 = value[2][1] * value[3][2] - value[3][1] * value[2][2];
118 t_type Coef10 = value[1][1] * value[3][2] - value[3][1] * value[1][2];
119 t_type Coef11 = value[1][1] * value[2][2] - value[2][1] * value[1][2];
120
121 t_type Coef12 = value[2][0] * value[3][3] - value[3][0] * value[2][3];
122 t_type Coef14 = value[1][0] * value[3][3] - value[3][0] * value[1][3];
123 t_type Coef15 = value[1][0] * value[2][3] - value[2][0] * value[1][3];
124
125 t_type Coef16 = value[2][0] * value[3][2] - value[3][0] * value[2][2];
126 t_type Coef18 = value[1][0] * value[3][2] - value[3][0] * value[1][2];
127 t_type Coef19 = value[1][0] * value[2][2] - value[2][0] * value[1][2];
128
129 t_type Coef20 = value[2][0] * value[3][1] - value[3][0] * value[2][1];
130 t_type Coef22 = value[1][0] * value[3][1] - value[3][0] * value[1][1];
131 t_type Coef23 = value[1][0] * value[2][1] - value[2][0] * value[1][1];
132
133 Vector<4, t_type> Fac0(Coef00, Coef00, Coef02, Coef03);
134 Vector<4, t_type> Fac1(Coef04, Coef04, Coef06, Coef07);
135 Vector<4, t_type> Fac2(Coef08, Coef08, Coef10, Coef11);
136 Vector<4, t_type> Fac3(Coef12, Coef12, Coef14, Coef15);
137 Vector<4, t_type> Fac4(Coef16, Coef16, Coef18, Coef19);
138 Vector<4, t_type> Fac5(Coef20, Coef20, Coef22, Coef23);
139
140 Vector<4, t_type> Vec0(value[1][0], value[0][0], value[0][0], value[0][0]);
141 Vector<4, t_type> Vec1(value[1][1], value[0][1], value[0][1], value[0][1]);
142 Vector<4, t_type> Vec2(value[1][2], value[0][2], value[0][2], value[0][2]);
143 Vector<4, t_type> Vec3(value[1][3], value[0][3], value[0][3], value[0][3]);
144
145 Vector<4, t_type> Inv0(Vec1 * Fac0 - Vec2 * Fac1 + Vec3 * Fac2);
146 Vector<4, t_type> Inv1(Vec0 * Fac0 - Vec2 * Fac3 + Vec3 * Fac4);
147 Vector<4, t_type> Inv2(Vec0 * Fac1 - Vec1 * Fac3 + Vec3 * Fac5);
148 Vector<4, t_type> Inv3(Vec0 * Fac2 - Vec1 * Fac4 + Vec2 * Fac5);
149
150 Vector<4, t_type> SignA(+1, -1, +1, -1);
151 Vector<4, t_type> SignB(-1, +1, -1, +1);
152
153 Vector<4, t_type> Row0(Inv0[0] * SignA[0], Inv1[0] * SignB[0], Inv2[0] * SignA[0], Inv3[0] * SignB[0]);
154
155 Vector<4, t_type> Dot0(Vector<4, t_type>(value[0][0], value[0][1], value[0][2], value[0][3]) * Row0);
156 t_type Dot1 = Dot0.sum();
157
158 lib_assert(abs(Dot1) > 0.0, "bad matrix inverse");
159 t_type OneOverDeterminant = static_cast<t_type>(1) / Dot1;
160 Inv0 = Inv0 * SignA * OneOverDeterminant;
161 Inv1 = Inv1 * SignB * OneOverDeterminant;
162 Inv2 = Inv2 * SignA * OneOverDeterminant;
163 Inv3 = Inv3 * SignB * OneOverDeterminant;
164 return Vector<4, Vector<4, t_type>>(
165 { Inv0[0], Inv0[1], Inv0[2], Inv0[3] }
166 , { Inv1[0], Inv1[1], Inv1[2], Inv1[3] }
167 , { Inv2[0], Inv2[1], Inv2[2], Inv2[3]}
168 , { Inv3[0], Inv3[1], Inv3[2], Inv3[3]});
169 }
170 };
171 template<class t_type, uint01 t_row_dims = 4, uint01 t_col_dims = 4>
172 class Matrix : public Vector<t_row_dims, Vector<t_col_dims, t_type>>
173 {
174 public:
175 constexpr Matrix()
176 : Vector<t_row_dims, Vector<t_col_dims, t_type>>()
177 {}
178 constexpr explicit Matrix(const t_type diagonal)
179 : Vector<t_row_dims, Vector<t_col_dims, t_type>>(Vector<t_col_dims, t_type>(0))
180 {
181 for (uint01 i = 0; i < getMin(t_row_dims, t_col_dims); i++)
182 {
183 (*this)[i][i] = diagonal;
184 }
185 }
186
187 constexpr Matrix(
188 const t_type n00, const t_type n10,
189 const t_type n01, const t_type n11)
190 : Vector<2, Vector<2, t_type>>(
191 Vector<2, t_type>(n00, n10)
192 , Vector<2, t_type>(n01, n11))
193 {
194 static_assert(t_row_dims == 2 && t_col_dims == 2, "Bad Transform Constructor, should be 2x2");
195 }
196 constexpr Matrix(
197 const t_type n00, const t_type n10, const t_type n20,
198 const t_type n01, const t_type n11, const t_type n21,
199 const t_type n02, const t_type n12, const t_type n22)
200 : Vector<3, Vector<3, t_type>>(
201 Vector<3, t_type>(n00, n10, n20)
202 , Vector<3, t_type>(n01, n11, n21)
203 , Vector<3, t_type>(n02, n12, n22))
204 {
205 static_assert(t_row_dims == 3 && t_col_dims == 3, "Bad Transform Constructor, should be 3x3");
206 }
207 constexpr Matrix(
208 const t_type n00, const t_type n10, const t_type n20, const t_type n30,
209 const t_type n01, const t_type n11, const t_type n21, const t_type n31,
210 const t_type n02, const t_type n12, const t_type n22, const t_type n32,
211 const t_type n03, const t_type n13, const t_type n23, const t_type n33)
212 : Vector<4, Vector<4, t_type>>(
213 Vector<4, t_type>(n00, n10, n20, n30)
214 , Vector<4, t_type>(n01, n11, n21, n31)
215 , Vector<4, t_type>(n02, n12, n22, n32)
216 , Vector<4, t_type>(n03, n13, n23, n33))
217 {
218 static_assert(t_row_dims == 4 && t_col_dims == 4, "Bad Transform Constructor, should be 4x4");
219 }
220 template<class t_angle_type>
221 constexpr Matrix(const Vector<t_row_dims - 1, t_type>& offset, const Vector<3, Angle<t_angle_type>>& orientation, const Vector<t_row_dims - 1, t_type>& scale)
222 : Vector<t_row_dims, Vector<t_col_dims, t_type>>(Vector<t_col_dims, t_type>(0))
223 {
224 //rotate the matrix
225 *this = RotationMatrix(orientation);
226 //Offset the matrix (Note will not overlap with rotation, so order independent)
227 (*this)[t_row_dims - 1][0] = offset[X];
228 if constexpr (t_col_dims >= 2)
229 (*this)[t_row_dims - 1][1] = offset[Y];
230 if constexpr (t_col_dims >= 3)
231 (*this)[t_row_dims - 1][2] = offset[Z];
232 //scale the matrix
233 *this = this->scale(scale);
234 }
235 constexpr Matrix(const Vector<t_row_dims* t_col_dims, t_type>& vector)
236 {
237 memcpy(this, &vector, sizeof(Vector<t_row_dims* t_col_dims, t_type>));
238 }
239 constexpr Matrix(const Vector<t_row_dims, Vector<t_col_dims, t_type>>& vector)
240 : Vector<t_row_dims, Vector<t_col_dims, t_type>>(vector)
241 {}
242 constexpr Matrix(const Matrix& mat) = default;
243
244 template<class t_new_type>
245 [[nodiscard]] constexpr Matrix<t_new_type> as() const
246 {
247 Matrix<t_new_type> mat;
248 for (uint01 row = 0; row < t_row_dims; ++row)
249 {
250 for (uint01 col = 0; col < t_col_dims; ++col)
251 {
252 mat[row][col] = cast<t_new_type>((*this)[row][col]);
253 }
254 }
255 return mat;
256 }
257 template<uint01 t_i0, uint01 t_i1, uint01 t_j0, uint01 t_j1>
258 [[nodiscard]] constexpr Matrix<t_type, t_i1 - t_i0 + 1, t_j1 - t_j0 + 1> subMatrix() const
259 {
260 Matrix<t_type, t_i1 - t_i0 + 1, t_j1 - t_j0 + 1> mat;
261 for (uint01 i = t_i0; i <= t_i1; i++)
262 {
263 for (uint01 j = t_j0; j <= t_j1; j++)
264 mat[i - t_i0][j - t_j0] = (*this)[i][j];
265 }
266 return mat;
267 }
268 template<uint01 t_j0, uint01 t_j1, uint01 t_i_size>
269 [[nodiscard]] constexpr Matrix<t_type, t_i_size, t_j1 - t_j0 + 1> subMatrix(const Vector<t_i_size, uint01>& r) const
270 {
271 Matrix<t_type, t_i_size, t_j1 - t_j0 + 1> mat;
272 for (uint01 i = 0; i < t_i_size; i++)
273 {
274 for (uint01 j = t_j0; j <= t_j1; j++)
275 {
276 mat[i][j - t_j0] = (*this)[r[i]][j];
277 }
278 }
279 return mat;
280 }
281 template<class t_new_type, uint01 t_new_row_dims, uint01 t_new_col_dims>
282 [[nodiscard]] constexpr Matrix<t_new_type, t_new_row_dims, t_new_col_dims> as() const
283 {
284 Matrix<t_new_type, t_new_row_dims, t_new_col_dims> mat(0);
285 for (uint01 row = 0; row < getMin(t_row_dims, t_new_row_dims); ++row)
286 {
287 for (uint01 col = 0; col < getMin(t_col_dims, t_new_col_dims); ++col)
288 {
289 mat[row][col] = cast<t_new_type>((*this)[row][col]);
290 }
291 }
292 return mat;
293 }
294 [[nodiscard]] static constexpr Matrix<t_type> OffsetMatrix(const Vector<3, t_type>& translation)
295 {
296 Matrix<t_type> mat(1);
297 mat[t_row_dims - 1][0] = translation[X];
298 if constexpr (t_col_dims >= 2)
299 mat[t_row_dims - 1][1] = translation[Y];
300 if constexpr (t_col_dims >= 3)
301 mat[t_row_dims - 1][2] = translation[Z];
302 return mat;
303 }
304 [[nodiscard]] static constexpr Matrix<t_type> ScalerMatrix(t_type scale)
305 {
306 Matrix<t_type> mat(1);
307 mat[0][0] = scale;
308 if constexpr (t_col_dims >= 2 && t_row_dims >= 2)
309 mat[1][1] = scale;
310 if constexpr (t_col_dims >= 3 && t_row_dims >= 3)
311 mat[2][2] = scale;
312 return mat;
313 }
314 [[nodiscard]] static constexpr Matrix<t_type> ScalerMatrix(const Vector<2, t_type>& scale)
315 {
316 Matrix mat(1);
317 mat[0][0] = scale[X];
318 if constexpr (t_col_dims >= 2 && t_row_dims >= 2)
319 mat[1][1] = scale[Y];
320 return mat;
321 }
322 [[nodiscard]] static constexpr Matrix<t_type> ScalerMatrix(const Vector<3, t_type>& scale)
323 {
324 Matrix<t_type> mat(1);
325 mat[0][0] = scale[X];
326 if constexpr (t_col_dims >= 2 && t_row_dims >= 2)
327 mat[1][1] = scale[Y];
328 if constexpr (t_col_dims >= 3 && t_row_dims >= 3)
329 mat[2][2] = scale[Z];
330 return mat;
331 }
332 template<class t_angle_type>
333 static constexpr Matrix<t_type> RotationMatrix(const Vector<3, Angle<t_angle_type>>& orientation)
334 {
335 t_type ch = t_type(1);
336 t_type sh = t_type(0);
337 t_type ca = t_type(1);
338 t_type sa = t_type(0);
339 t_type cb = t_type(1);
340 t_type sb = t_type(0);
341 if (!isNaN(orientation[YAW]))
342 {
343 ch = cos(orientation[YAW]);
344 sh = sin(orientation[YAW]);
345 }
346 if (!isNaN(orientation[PITCH]))
347 {
348 ca = cos(orientation[PITCH]);
349 sa = sin(orientation[PITCH]);
350 }
351 if (!isNaN(orientation[ROLL]))
352 {
353 cb = cos(orientation[ROLL]);
354 sb = sin(orientation[ROLL]);
355 }
356
357 t_type m00 = ch * ca;
358 t_type m01 = ch * sa * sb - sh * cb;
359 t_type m02 = ch * sa * cb + sh * sb;
360 t_type m10 = sh * ca;
361 t_type m11 = sh * sa * sb + ch * cb;
362 t_type m12 = sh * sa * cb - ch * sb;
363 t_type m20 = -sa;
364 t_type m21 = ca * sb;
365 t_type m22 = ca * cb;
366 return Matrix<t_type>(
367 m00, m10, m20, 0
368 , m01, m11, m21, 0
369 , m02, m12, m22, 0
370 , 0, 0, 0, 1);
371 }
372 template<class t_angle_type>
373 static constexpr Matrix<t_type> RotationMatrix(const Angle<t_angle_type>& phi, const Vector<3, t_type>& axis)
374 {
375 const t_type cos_val = cos(phi);
376 const t_type sin_val = sin(phi);
377 const t_type x = axis[X];
378 const t_type y = axis[Y];
379 const t_type z = axis[Z];
380 return Matrix<t_type>(
381 (cos_val + x * x * (1 - cos_val)), (z * sin_val + y * x * (1 - cos_val)), (-y * sin_val + z * x * (1 - cos_val)), 0
382 , (-z * sin_val + x * y * (1 - cos_val)), (cos_val + y * y * (1 - cos_val)), (x * sin_val + z * y * (1 - cos_val)), 0
383 , (y * sin_val + x * z * (1 - cos_val)), (-x * sin_val + y * z * (1 - cos_val)), (cos_val + z * z * (1 - cos_val)), 0
384 , 0, 0, 0, 1);
385 }
386 [[nodiscard]] constexpr t_type determinant() const
387 {
388 static_assert(t_col_dims == t_row_dims, "Rows and columns must be equal for determinate");
389 switch (t_col_dims)
390 {
391 case 1:
392 return (*this)[0][0];
393 case 2:
394 return (*this)[0][0] * (*this)[1][1] - (*this)[1][0] * (*this)[0][1];
395 case 3:
396 return
397 +(*this)[0][0] * ((*this)[1][1] * (*this)[2][2] - (*this)[2][1] * (*this)[1][2])
398 - (*this)[1][0] * ((*this)[0][1] * (*this)[2][2] - (*this)[2][1] * (*this)[0][2])
399 + (*this)[2][0] * ((*this)[0][1] * (*this)[1][2] - (*this)[1][1] * (*this)[0][2]);
400 case 4:
401 {
402 t_type SubFactor00 = (*this)[2][2] * (*this)[3][3] - (*this)[3][2] * (*this)[2][3];
403 t_type SubFactor01 = (*this)[2][1] * (*this)[3][3] - (*this)[3][1] * (*this)[2][3];
404 t_type SubFactor02 = (*this)[2][1] * (*this)[3][2] - (*this)[3][1] * (*this)[2][2];
405 t_type SubFactor03 = (*this)[2][0] * (*this)[3][3] - (*this)[3][0] * (*this)[2][3];
406 t_type SubFactor04 = (*this)[2][0] * (*this)[3][2] - (*this)[3][0] * (*this)[2][2];
407 t_type SubFactor05 = (*this)[2][0] * (*this)[3][1] - (*this)[3][0] * (*this)[2][1];
408 Vector<4, t_type> DetCof(
409 +((*this)[1][1] * SubFactor00 - (*this)[1][2] * SubFactor01 + (*this)[1][3] * SubFactor02),
410 -((*this)[1][0] * SubFactor00 - (*this)[1][2] * SubFactor03 + (*this)[1][3] * SubFactor04),
411 +((*this)[1][0] * SubFactor01 - (*this)[1][1] * SubFactor03 + (*this)[1][3] * SubFactor05),
412 -((*this)[1][0] * SubFactor02 - (*this)[1][1] * SubFactor04 + (*this)[1][2] * SubFactor05));
413 return
414 (*this)[0][0] * DetCof[0] + (*this)[0][1] * DetCof[1] +
415 (*this)[0][2] * DetCof[2] + (*this)[0][3] * DetCof[3];
416 }
417 default:
418 return Constant<t_type>::NaN;
419 }
420 }
421 [[nodiscard]] constexpr Vector<3, t_type> decomposeScale() const
422 {
423 static_assert(t_col_dims >= 4 && t_row_dims >= 4, "Cannot decompose lower dimension matrix");
424 Vector<3, t_type> scaling;
425 Vector<3, t_type> rows[3];
426 for (uint01 i = 0; i < 3; i++)
427 {
428 rows[i] = (*this)[i].template as<3, t_type>();
429 scaling[i] = rows[i].template magnitude<t_type>();
430 }
431 scaling = quantize(scaling, Vector<3, t_type>(.000001));
432 const Vector<3, t_type> temp_z = cross(rows[0], rows[1]);
433 if (dot(temp_z, rows[2]) < 0)
434 {
435 scaling[X] = -scaling[X];
436 rows[0] = -rows[0];
437 }
438 return scaling;
439 }
440 [[nodiscard]] constexpr Vector<3, t_type> decomposeOffset() const
441 {
442 static_assert(t_col_dims >= 3 && t_row_dims >= 3, "Cannot decompose lower dimension matrix");
443 Vector<3, t_type> offset;
444 offset[X] = (*this)[3][0];
445 offset[Y] = (*this)[3][1];
446 offset[Z] = (*this)[3][2];
447 return offset;
448 }
449 /* [[nodiscard]] constexpr Vector<3, Angle> decomposeRotation() const
450 {
451 return decomposeRotation<sint04>();
452 }*/
453 template<class t_angle_type>
454 [[nodiscard]] constexpr Vector<3, Angle<t_angle_type>> decomposeRotation() const
455 {
456 static_assert(t_col_dims >= 3 && t_row_dims >= 3, "Cannot decompose lower dimension matrix");
457 Vector<3, Angle<t_angle_type>> rotation;
458 Vector<3, t_type> rows[3];
459 for (uint01 i = 0; i < 3; i++)
460 {
461 rows[i] = (*this)[i].template as<3, t_type>();
462 }
463 const Vector<3, t_type> temp_z = cross(rows[0], rows[1]);
464 if (dot(temp_z, rows[2]) < 0)
465 {
466 rows[0] = -rows[0];
467 }
468 for (uint04 i = 0; i < 3; i++)
469 {
470 rows[i] = rows[i].template normalized<t_type>();
471 }
472 t_type sy = sqrt(rows[0][0] * rows[0][0] + rows[0][1] * rows[0][1]);
473
474 rotation[PITCH] = Angle<t_angle_type>::atan2(-rows[0][2], sy);
475 if (sy > cast<t_type>(1E-6))
476 {
477 rotation[ROLL] = Angle<t_angle_type>::atan2(rows[1][2], rows[2][2]);
478 rotation[YAW] = Angle<t_angle_type>::atan2(rows[0][1], rows[0][0]);
479 }
480 else
481 {
482 rotation[ROLL] = Angle<t_angle_type>::atan2(-rows[2][1], rows[1][1]);
483 rotation[YAW] = Angle<t_angle_type>(RADIANS, 0.0f);
484 }
485 return rotation;
486 }
487
488 [[nodiscard]] constexpr Vector<4, t_type> decomposeRotationQuaternion() const
489 {
490 t_type trace = (*this)[0][0] + (*this)[1][1] + (*this)[2][2];
491 Vector<4, t_type> temp;
492
493 if (trace > t_type(0.0))
494 {
495 t_type s = sqrt(trace + t_type(1.0));
496 temp[3] = (s * t_type(0.5));
497 s = t_type(0.5) / s;
498
499 temp[0] = (((*this)[2][1] - (*this)[1][2]) * s);
500 temp[1] = (((*this)[0][2] - (*this)[2][0]) * s);
501 temp[2] = (((*this)[1][0] - (*this)[0][1]) * s);
502 }
503 else
504 {
505 uint01 i = (*this)[0][0] < (*this)[1][1] ?
506 ((*this)[1][1] < (*this)[2][2] ? 2U : 1U) :
507 ((*this)[0][0] < (*this)[2][2] ? 2U : 0U);
508 uint01 j = (i + 1) % 3;
509 uint01 k = (i + 2) % 3;
510
511 t_type s = t_type((*this)[i][i] - (*this)[j][j] - (*this)[k][k] + t_type(1.0));
512 temp[i] = s * t_type(0.5);
513 s = t_type(0.5) / s;
514
515 temp[3] = ((*this)[k][j] - (*this)[j][k]) * s;
516 temp[j] = ((*this)[j][i] + (*this)[i][j]) * s;
517 temp[k] = ((*this)[k][i] + (*this)[i][k]) * s;
518 }
519 return temp;
520 }
521 template<class t_angle_type>
522 constexpr void decompose(Vector<3, t_type>& scaling, Vector<3, Angle<t_angle_type>>& rotation, Vector<3, t_type>& position) const
523 {
524 static_assert(t_col_dims >= 4 && t_row_dims >= 4, "Cannot decompose lower dimension matrix");
525
526 position[X] = (*this)[3][0];
527 position[Y] = (*this)[3][1];
528 position[Z] = (*this)[3][2];
529
530 Vector<3, t_type> rows[3];
531 for (uint01 i = 0; i < 3; i++)
532 {
533 rows[i] = (*this)[i].template as<3, t_type>();
534 scaling[i] = rows[i].template magnitude<t_type>();
535 }
536 scaling = quantize(scaling, Vector<3, t_type>(.000001));
537 const Vector<3, t_type> temp_z = cross(rows[0], rows[1]);
538 if (dot(temp_z, rows[2]) < 0)
539 {
540 scaling[X] = -scaling[X];
541 rows[0] = -rows[0];
542 }
543 for (uint04 i = 0; i < 3; i++)
544 {
545 rows[i] = rows[i].template normalized<t_type>();
546 }
547 t_type sy = sqrt(rows[0][0] * rows[0][0] + rows[0][1] * rows[0][1]);
548
549 rotation[PITCH] = Angle<t_angle_type>::atan2(-rows[0][2], sy);
550 if (sy > cast<t_type>(1E-6))
551 {
552 rotation[ROLL] = Angle<t_angle_type>::atan2(rows[1][2], rows[2][2]);
553 rotation[YAW] = Angle<t_angle_type>::atan2(rows[0][1], rows[0][0]);
554 }
555 else
556 {
557 rotation[ROLL] = Angle<t_angle_type>::atan2(-rows[2][1], rows[1][1]);
558 rotation[YAW] = Angle<t_angle_type>(RADIANS, 0.0f);
559 }
560 }
561 [[nodiscard]] constexpr Matrix offset(const Vector<2, t_type>& translation) const
562 {
563 Matrix mat(1);
564 mat[t_row_dims - 1][0] = translation[X];
565 if constexpr (t_col_dims >= 2)
566 mat[t_row_dims - 1][1] = translation[Y];
567 if constexpr (t_col_dims >= 3)
568 mat[t_row_dims - 1][2] = 0;
569 return (*this) * mat;
570
571 }
572
573
574 [[nodiscard]] constexpr Matrix offset(const Vector<3, t_type>& translation) const
575 {
576 return (*this) * Matrix::OffsetMatrix(translation);
577 }
578
579 [[nodiscard]] constexpr inline Matrix scale(t_type scale) const
580 {
581 return (*this) * Matrix::ScalerMatrix(scale);
582 }
583
584
585 [[nodiscard]] constexpr Matrix scale(const Vector<2, t_type>& scale) const
586 {
587 return (*this) * Matrix::ScalerMatrix(scale);
588 }
589
590
591 [[nodiscard]] constexpr Matrix scale(const Vector<3, t_type>& scale) const
592 {
593 return (*this) * Matrix::ScalerMatrix(scale);
594 }
595
596 void correctZeroScale(t_type new_value = 1e-6, t_type epsilon = 1e-9)
597 {
598 for (uint01 i = 0; i < getMin(t_row_dims, t_col_dims); i++)
599 {
600 if (abs((*this)[i][i]) < epsilon)
601 (*this)[i][i] = new_value;
602 }
603 }
604
605 [[nodiscard]] constexpr Matrix scale(const Vector<3, t_type>& direction, t_type scale) const
606 {
607 Matrix mat(1);
608 mat[0][0] += (scale - 1) * direction[X] * direction[X];
609 mat[0][1] += (scale - 1) * direction[Y] * direction[X];
610 mat[0][2] += (scale - 1) * direction[Z] * direction[X];
611
612 mat[1][0] += (scale - 1) * direction[X] * direction[Y];
613 mat[1][1] += (scale - 1) * direction[Y] * direction[Y];
614 mat[1][2] += (scale - 1) * direction[Z] * direction[Y];
615
616 mat[2][0] += (scale - 1) * direction[X] * direction[Z];
617 mat[2][1] += (scale - 1) * direction[Y] * direction[Z];
618 mat[2][2] += (scale - 1) * direction[Z] * direction[Z];
619
620 return (*this) * mat;
621 }
622 template<class t_angle_type>
623 [[nodiscard]] constexpr Matrix rotate(const Vector<3, Angle<t_angle_type>>& orientation) const
624 {
625 return *this * Matrix<t_type>::RotationMatrix(orientation);
626 }
627 template<class t_angle_type>
628 [[nodiscard]] constexpr Matrix<t_type> rotate(const Angle<t_angle_type>& phi, const Vector<3, t_type>& axis) const
629 {
630 return *this * RotationMatrix(phi, axis);
631 }
632 [[nodiscard]] constexpr Matrix shear(t_type dx, t_type dy) const
633 {
634 Matrix mat(1);
635 mat[0][1] = dx;
636 mat[1][0] = dy;
637 return (*this) * mat;
638 }
639 [[nodiscard]] constexpr Matrix<t_type, t_col_dims, t_row_dims> transpose() const
640 {
641 Matrix<t_type, t_col_dims, t_row_dims> mat;
642 for (uint01 row = 0; row < t_row_dims; row++)
643 for (uint01 col = 0; col < t_col_dims; col++)
644 mat[col][row] = (*this)[row][col];
645 return mat;
646 }
647
648 template<uint01 t_cols = t_col_dims, uint01 t_rows = t_row_dims>
653
654 [[nodiscard]] inline t_type* begin()
655 {
656 return &(*this)[0][0];
657 }
658
659 [[nodiscard]] inline const t_type* begin() const
660 {
661 return &(*this)[0][0];
662 }
663
664 public:
665 [[nodiscard]] Matrix operator*(const Matrix& right) const
666 {
667 Matrix<t_type, t_row_dims, t_col_dims> product;
668 for(uint01 col = 0; col < t_col_dims; ++col)
669 {
670 for(uint01 row = 0; row < t_row_dims; ++row)
671 {
672 for(uint01 i = 0; i < t_row_dims; ++i)
673 {
674 product[row][col] += ((*this)[i][col] * right[row][i]);
675 }
676 }
677 }
678 return product;
679 }
680 inline Matrix& operator=(const Matrix& right)
681 {
682 for(uint01 col = 0; col < t_col_dims; ++col)
683 {
684 for(uint01 row = 0; row < t_row_dims; ++row)
685 {
686 (*this)[row][col] = right[row][col];
687 }
688 }
689 return *this;
690 }
691 inline Matrix& operator*=(const Matrix& right)
692 {
693 *this = *this * right;
694 return *this;
695 }
696 };
697
698 /**--------------------------------------------------------------------------------------------------
699 Struct: Constant<Matrix<t_dims,t_type,t_vector_type>>
700
701 A constant.
702
703 Author: Tyler Parke
704
705 Date: 2017-11-19
706 *-----------------------------------------------------------------------------------------------**/
707 template<class t_type, uint01 t_row_dims, uint01 t_col_dims>
714
715 /**--------------------------------------------------------------------------------------------------
716 Fn: template<uint01 t_dims, class t_type, class t_vector_type> static constexpr bool isNaN(const Matrix<t_dims, t_type, t_vector_type>& value)
717
718 Query if 'value' is NaN.
719
720 Author: Tyler Parke
721
722 Date: 2017-11-19
723
724 Typeparams:
725 t_dims - Type of the dims.
726 t_type - Type of the type.
727 t_vector_type - Type of the vector type.
728 Parameters:
729 value - The value.
730
731 Returns: True if nan, false if not.
732 *-----------------------------------------------------------------------------------------------**/
733
734 template<class t_type, uint01 t_row_dims, uint01 t_col_dims>
735 static constexpr bool isNaN(const Matrix<t_type, t_row_dims, t_col_dims>& value)
736 {
737 for (uint01 dim_a = 0; dim_a < t_col_dims; ++dim_a)
738 {
739 for (uint01 dim_b = 0; dim_b < t_row_dims; ++dim_b)
740 {
741 if (isNaN(value[dim_b][dim_a]))
742 return true;
743 }
744 }
745 return false;
746 }
747}
748
lib_assert
#define lib_assert(expression, message)
Asserts some logic in the code. Disabled in non debug mode by default. Can be re-enabled in release u...
Definition LibAssert.h:70
NDEVR::Angle
Stores an angle in an optimized format.
Definition StringStream.h:352
NDEVR::Angle::atan2
static Angle atan2(fltp04 value_a, fltp04 value_b)
Definition Angle.h:481
NDEVR::Matrix
Definition Matrix.hpp:173
NDEVR::Matrix::operator*
Matrix operator*(const Matrix &right) const
Definition Matrix.hpp:665
NDEVR::Matrix::decomposeRotationQuaternion
constexpr Vector< 4, t_type > decomposeRotationQuaternion() const
Definition Matrix.hpp:488
NDEVR::Matrix::Matrix
constexpr Matrix(const t_type n00, const t_type n10, const t_type n20, const t_type n01, const t_type n11, const t_type n21, const t_type n02, const t_type n12, const t_type n22)
Definition Matrix.hpp:196
NDEVR::Matrix::RotationMatrix
static constexpr Matrix< t_type > RotationMatrix(const Angle< t_angle_type > &phi, const Vector< 3, t_type > &axis)
Definition Matrix.hpp:373
NDEVR::Matrix::subMatrix
constexpr Matrix< t_type, t_i1 - t_i0+1, t_j1 - t_j0+1 > subMatrix() const
Definition Matrix.hpp:258
NDEVR::Matrix::begin
const t_type * begin() const
Definition Matrix.hpp:659
NDEVR::Matrix::shear
constexpr Matrix shear(t_type dx, t_type dy) const
Definition Matrix.hpp:632
NDEVR::Matrix::Matrix
constexpr Matrix(const Vector< t_row_dims *t_col_dims, t_type > &vector)
Definition Matrix.hpp:235
NDEVR::Matrix::offset
constexpr Matrix offset(const Vector< 2, t_type > &translation) const
Definition Matrix.hpp:561
NDEVR::Matrix::decompose
constexpr void decompose(Vector< 3, t_type > &scaling, Vector< 3, Angle< t_angle_type > > &rotation, Vector< 3, t_type > &position) const
Definition Matrix.hpp:522
NDEVR::Matrix::scale
constexpr Matrix scale(t_type scale) const
Definition Matrix.hpp:579
NDEVR::Matrix::rotate
constexpr Matrix< t_type > rotate(const Angle< t_angle_type > &phi, const Vector< 3, t_type > &axis) const
Definition Matrix.hpp:628
NDEVR::Matrix::Matrix
constexpr Matrix(const t_type n00, const t_type n10, const t_type n20, const t_type n30, const t_type n01, const t_type n11, const t_type n21, const t_type n31, const t_type n02, const t_type n12, const t_type n22, const t_type n32, const t_type n03, const t_type n13, const t_type n23, const t_type n33)
Definition Matrix.hpp:207
NDEVR::Matrix::Matrix
constexpr Matrix()
Definition Matrix.hpp:175
NDEVR::Matrix::Matrix
constexpr Matrix(const t_type diagonal)
Definition Matrix.hpp:178
NDEVR::Matrix::scale
constexpr Matrix scale(const Vector< 3, t_type > &direction, t_type scale) const
Definition Matrix.hpp:605
NDEVR::Matrix::OffsetMatrix
static constexpr Matrix< t_type > OffsetMatrix(const Vector< 3, t_type > &translation)
Definition Matrix.hpp:294
NDEVR::Matrix::operator=
Matrix & operator=(const Matrix &right)
Definition Matrix.hpp:680
NDEVR::Matrix::transpose
constexpr Matrix< t_type, t_col_dims, t_row_dims > transpose() const
Definition Matrix.hpp:639
NDEVR::Matrix::invert
Matrix< t_type, t_cols, t_rows > invert() const
Definition Matrix.hpp:649
NDEVR::Matrix::determinant
constexpr t_type determinant() const
Definition Matrix.hpp:386
NDEVR::Matrix::as
constexpr Matrix< t_new_type > as() const
Definition Matrix.hpp:245
NDEVR::Matrix::decomposeOffset
constexpr Vector< 3, t_type > decomposeOffset() const
Definition Matrix.hpp:440
NDEVR::Matrix::ScalerMatrix
static constexpr Matrix< t_type > ScalerMatrix(const Vector< 2, t_type > &scale)
Definition Matrix.hpp:314
NDEVR::Matrix::Matrix
constexpr Matrix(const Vector< t_row_dims - 1, t_type > &offset, const Vector< 3, Angle< t_angle_type > > &orientation, const Vector< t_row_dims - 1, t_type > &scale)
Definition Matrix.hpp:221
NDEVR::Matrix::RotationMatrix
static constexpr Matrix< t_type > RotationMatrix(const Vector< 3, Angle< t_angle_type > > &orientation)
Definition Matrix.hpp:333
NDEVR::Matrix::rotate
constexpr Matrix rotate(const Vector< 3, Angle< t_angle_type > > &orientation) const
Definition Matrix.hpp:623
NDEVR::Matrix::ScalerMatrix
static constexpr Matrix< t_type > ScalerMatrix(t_type scale)
Definition Matrix.hpp:304
NDEVR::Matrix::begin
t_type * begin()
Definition Matrix.hpp:654
NDEVR::Matrix::scale
constexpr Matrix scale(const Vector< 3, t_type > &scale) const
Definition Matrix.hpp:591
NDEVR::Matrix::offset
constexpr Matrix offset(const Vector< 3, t_type > &translation) const
Definition Matrix.hpp:574
NDEVR::Matrix::subMatrix
constexpr Matrix< t_type, t_i_size, t_j1 - t_j0+1 > subMatrix(const Vector< t_i_size, uint01 > &r) const
Definition Matrix.hpp:269
NDEVR::Matrix::Matrix
constexpr Matrix(const Matrix &mat)=default
NDEVR::Matrix::ScalerMatrix
static constexpr Matrix< t_type > ScalerMatrix(const Vector< 3, t_type > &scale)
Definition Matrix.hpp:322
NDEVR::Matrix::Matrix
constexpr Matrix(const Vector< t_row_dims, Vector< t_col_dims, t_type > > &vector)
Definition Matrix.hpp:239
NDEVR::Matrix::as
constexpr Matrix< t_new_type, t_new_row_dims, t_new_col_dims > as() const
Definition Matrix.hpp:282
NDEVR::Matrix::operator*=
Matrix & operator*=(const Matrix &right)
Definition Matrix.hpp:691
NDEVR::Matrix::decomposeRotation
constexpr Vector< 3, Angle< t_angle_type > > decomposeRotation() const
Definition Matrix.hpp:454
NDEVR::Matrix::correctZeroScale
void correctZeroScale(t_type new_value=1e-6, t_type epsilon=1e-9)
Definition Matrix.hpp:596
NDEVR::Matrix::scale
constexpr Matrix scale(const Vector< 2, t_type > &scale) const
Definition Matrix.hpp:585
NDEVR::Matrix::decomposeScale
constexpr Vector< 3, t_type > decomposeScale() const
Definition Matrix.hpp:421
NDEVR::Matrix::Matrix
constexpr Matrix(const t_type n00, const t_type n10, const t_type n01, const t_type n11)
Definition Matrix.hpp:187
NDEVR::MatrixInverter< 1, 1 >::invert
static Vector< 1, Vector< 1, t_type > > invert(Vector< 1, Vector< 1, t_type > > &value)
Definition Matrix.hpp:46
NDEVR::MatrixInverter< 2, 2 >::determinant
static constexpr t_type determinant(const Vector< 2, Vector< 2, t_type > > &value)
Definition Matrix.hpp:57
NDEVR::MatrixInverter< 2, 2 >::invert
static Vector< 2, Vector< 2, t_type > > invert(const Vector< 2, Vector< 2, t_type > > &value)
Definition Matrix.hpp:62
NDEVR::MatrixInverter< 3, 3 >::invert
static Vector< 3, Vector< 3, t_type > > invert(const Vector< 3, Vector< 3, t_type > > &value)
Definition Matrix.hpp:85
NDEVR::MatrixInverter< 3, 3 >::determinant
static constexpr t_type determinant(const Vector< 3, Vector< 3, t_type > > &value)
Definition Matrix.hpp:78
NDEVR::MatrixInverter< 4, 4 >::invert
static Vector< 4, Vector< 4, t_type > > invert(const Vector< 4, Vector< 4, t_type > > &value)
Definition Matrix.hpp:107
NDEVR::MatrixInverter
Definition Matrix.hpp:39
NDEVR::Vector
An element of a vector space. An element of the real coordinate space Rn Basis vector,...
Definition Vector.hpp:62
NDEVR::Vector< 4, Vector< 4, t_type > >::magnitude
constexpr t_magnitude_type magnitude() const
Definition Vector.hpp:482
NDEVR::Vector::sum
constexpr t_type sum() const
Definition Vector.hpp:545
NDEVR::Vector< 4, Vector< 4, t_type > >::product
constexpr t_type product() const
Definition Vector.hpp:526
NDEVR::Vector< 4, Vector< 4, t_type > >::normalized
constexpr Vector< t_dims, t_norm_type > normalized(Vector< t_dims, t_norm_type > value_if_nan=Constant< Vector< t_dims, t_norm_type > >::NaN) const
Definition Vector.hpp:500
NDEVR
Definition ACIColor.h:37
NDEVR::dot
t_type dot(const Vector< t_dims, t_type > &v1, const Vector< t_dims, t_type > &v2)
Definition VectorFunctions.hpp:1096
NDEVR::ROLL
@ ROLL
Definition Angle.h:51
NDEVR::YAW
@ YAW
Definition Angle.h:53
NDEVR::PITCH
@ PITCH
Definition Angle.h:52
NDEVR::quantize
constexpr Vector< t_dims, Angle< t_angle_type > > quantize(const Vector< t_dims, Angle< t_angle_type > > &value, Angle< t_angle_type > d=Angle< t_angle_type >(DEGREES, 1.0))
Definition AngleFunctions.h:955
NDEVR::uint01
uint8_t uint01
-Defines an alias representing a 1 byte, unsigned integer -Can represent exact integer values 0 throu...
Definition BaseValues.hpp:98
NDEVR::RADIANS
@ RADIANS
Definition Angle.h:65
NDEVR::cos
std::enable_if<!ObjectInfo< t_type >::Float, fltp08 >::type cos(const Angle< t_type > &angle)
Definition AngleFunctions.h:154
NDEVR::cross
constexpr Vector< 1, t_type > cross(const Vector< 1, t_type > &, const Vector< 1, t_type > &)
Definition VectorFunctions.hpp:954
NDEVR::uint04
uint32_t uint04
-Defines an alias representing a 4 byte, unsigned integer -Can represent exact integer values 0 throu...
Definition BaseValues.hpp:120
NDEVR::sqrt
t_type sqrt(const t_type &value)
Definition VectorFunctions.hpp:1309
NDEVR::cast
constexpr t_to cast(const Angle< t_from > &value)
Definition Angle.h:514
NDEVR::abs
constexpr Angle< t_angle_type > abs(const Angle< t_angle_type > &value)
Definition AngleFunctions.h:750
NDEVR::sin
std::enable_if<!ObjectInfo< t_type >::Float, fltp08 >::type sin(const Angle< t_type > &angle)
Definition AngleFunctions.h:113
NDEVR::isNaN
constexpr bool isNaN(const t_type &value)
Query if 'value' is valid or invalid.
Definition BaseFunctions.hpp:200
NDEVR::Y
@ Y
Definition BaseValues.hpp:202
NDEVR::X
@ X
Definition BaseValues.hpp:200
NDEVR::Z
@ Z
Definition BaseValues.hpp:204
NDEVR::getMin
constexpr t_type getMin(const t_type &left, const t_type &right)
Finds the minimum of the given arguments based on the < operator.
Definition BaseFunctions.hpp:67
NDEVR::Constant
Definition BaseValues.hpp:272
NDEVR::Constant::Min
static const t_type Min
Definition BaseValues.hpp:276
NDEVR::Constant::Max
static const t_type Max
Definition BaseValues.hpp:278
NDEVR::Constant::NaN
static const t_type NaN
Definition BaseValues.hpp:274