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