Polygon Crucher SDK - Documentation
Documentation
Loading...
Searching...
No Matches
4x4Matrix.h
Go to the documentation of this file.
1//! @file 4x4Matrix.h
2//! C4x4TMatrix class implementation for handling 3D transformations
3//!
4//////////////////////////////////////////////////////////////////////
5
6#if !defined(AFX_4X4MATRIX_H__BB3D80EA_003C_11D2_A0E3_000000000000__INCLUDED_)
7#define AFX_4X4MATRIX_H__BB3D80EA_003C_11D2_A0E3_000000000000__INCLUDED_
8
9#ifdef _MSC_VER
10#pragma once
11#endif // _MSC_VER
12
13#ifdef _DEBUG
14#include "StringTools.h"
15#endif
16
17#include "3DVector.h"
18#include "Quaternion.h"
19#include "4DVector.h"
20
21BEGIN_MOOTOOLS_NAMESPACE
22
23//! @struct MatrixDecomposition
24//! @brief Get the different components of a C4x4Matrix, or set a matrix from this different components
25typedef struct MatrixDecomposition
26{
27 friend DLL_3DFUNCTION void Decompose(const C4x4MatrixD& matrix, MatrixDecomposition& components);
28 friend DLL_3DFUNCTION C4x4MatrixD Recompose(const MatrixDecomposition& components);
29 friend DLL_3DFUNCTION bool IsTrsDecomposable(const C4x4MatrixD& matrix);
30
31protected:
32 C3DVectorD translation;
33 C3DVectorD scale;
34 CQuaternion rotation;
35 CQuaternion stretch;
36 double determinant;
37
38public:
39 inline C3DVectorD& Translation() { return translation; };
40 inline C3DVectorD& Scale() { return scale; };
41 inline CQuaternion& Rotation() { return rotation; };
42 inline CQuaternion& Stretch() { return stretch; };
43 inline double& Determinant() { return determinant; };
44
45 inline C3DVector GetTranslationF() const { return translation; };
46 inline C3DVector GetScaleF() const { return scale; };
47} MatrixDecomposition;
48
49
50//! @class C4x4TMatrix
51//! @brief The class defines a 4x4 row major order matrix
52//!
53//! @tparam TYPE
54//! can be float (C4x4Matrix or C4x4MatrixF) or double (C4x4MatrixD)
55template<class TYPE> class C4x4TMatrix
56{
57public:
58 TYPE matrix[4][4];
59
60public:
61 C4x4TMatrix();
62 explicit C4x4TMatrix(bool init);
63 template <class TYPE2> C4x4TMatrix(const C4x4TMatrix<TYPE2>& matrix);
64 template <class TYPE2> C4x4TMatrix(const TYPE2 *values, bool rotate);
65 C4x4TMatrix(const CQuaternion& quat); //!< Init the matrix from a CQuaternion
66 virtual ~C4x4TMatrix() {};
67
68 void Init();
69 template <class TYPE2> void LoadMatrix(const TYPE2 *matrix);
70 template <class TYPE2> void SaveMatrix(TYPE2 *matrix) const;
71 bool IsIdentity(double precision = FLOAT_EPSILON2) const; //!< Check that matrix is Identity (FLOAT_EPSILON3 limit)
72 bool IsSimilar(const C4x4TMatrix<TYPE>& compmat, double precision = FLOAT_EPSILON2) const; //!< Check that both matrix are similar (FLOAT_EPSILON2 limit by default)
73 bool IsAnormal() const; //!< Check some validity about the matrix (inf / nan values...)
74 virtual void Serialize(CXArchive& ar);
75 template <class TYPE2> C4DTVector<TYPE2> operator*(const C4DTVector<TYPE2>&) const;
76 void operator *=(TYPE value);
77 C4x4TMatrix& operator *=(const C4x4TMatrix& matrix1);
78 C4x4TMatrix operator*(const C4x4TMatrix&) const;
79 bool operator==(const C4x4TMatrix&) const;
80 bool operator!=(const C4x4TMatrix&) const;
81 C4DTVector<TYPE> Mult(double, double, double) const;
82 C4x4TMatrix& operator=(const C4x4TMatrix&);
83 template <class TYPE2> C4x4TMatrix& operator=(const C4x4TMatrix<TYPE2>&);
84 C4x4TMatrix& operator=(const CQuaternion& quaternion);
85 void operator =(const TYPE *newmatrix);
86 void InitTranslation(double, double, double);
87 template <class TYPE2> void InitTranslation(const C3DTVector<TYPE2>& vect);
88 void InitTranslation(const C4x4TMatrix<TYPE>& matrix);
89 void NoTranslation();
90 void InitRotation(double, double, double, double);
91 void InitRotation(const C4x4TMatrix<TYPE>& matrix);
92 void InitScale(double, double, double);
93 template <class TYPE2> void InitScale(const C3DTVector<TYPE2>& vect);
94 template <class TYPE2> void GetTranslation(C3DTVector<TYPE2>& vect) const;
95 template <class TYPE2> void GetTranslation(C3DTPoint<TYPE2>& position) const;
96 void PreScale(double x, double y, double z); //!< This affect the translation vector
97 template <class TYPE2> void PreScale(const C3DTVector<TYPE2>& vect);
98 void PostScale(double x, double y, double z ); //!< Does not affect translation vector
99 template <class TYPE2> void PostScale(const C3DTVector<TYPE2>& vect);
100 template <class TYPE2> void GetScale(C3DTVector<TYPE2>& vect) const;
101 void NoScale();
102 void Translate(double, double, double);
103 template <class TYPE2> void Translate(const C3DTVector<TYPE2>& vect);
104 void Rotate(double angle, double x, double y, double z);
105 void SetHPBAngles(double heading, double pitch, double bank);
106 bool GetHPBAngles(double& heading, double& pitch, double& bank) const; //!< Return true if angle can be retrieved without singularity, meaning that the matrix could be recompose safely using SetHPBAngle
107 void Transpose(C4x4TMatrix& transMat) const;
108 void Transpose();
109 void Decompose(MatrixDecomposition& components) const;
110 void Recompose(const MatrixDecomposition& components);
111 bool IsTrsDecomposable() const;
112
113 C3DTVector<TYPE> GetRow(int i) const;
114 void SetRow(int i, const C3DTVector<TYPE>& vector);
115 C3DTVector<TYPE> GetColumn(int i) const;
116 void SetColumn(int i, const C3DTVector<TYPE>& vector);
117 bool SelfInverse();
118 bool Inverse(C4x4TMatrix& invertedMatrix) const;
119 double GetDeterminant() const;
120 bool HasReflection() const;
121
122 const TYPE *ValPtr() const;
123 TYPE *ValPtr();
124 operator TYPE*();
125 operator const TYPE*() const;
126 unsigned int SizeOf() const;
127 TYPE& operator()(int i, int j);
128 TYPE operator()(int i, int j) const;
129
130 void operator-= (const C4x4TMatrix& submatrix);
131 void operator+= (const C4x4TMatrix& matrix);
132
133#ifdef _DEBUG
134 void Dump() const;
135#endif
136};
137
138#if 0
139template <class TYPE> C3DTPoint<TYPE> operator*(const C3DTPoint<TYPE>& point, const C4x4TMatrix<TYPE>& matrix);
140template <class TYPE> C3DTVector<TYPE> operator*(const C3DTVector<TYPE>& point, const C4x4TMatrix<TYPE>& matrix);
141template <class TYPE> C3DTPoint<TYPE>& operator*=(C3DTPoint<TYPE>& point, const C4x4TMatrix<TYPE>& matrix);
142template <class TYPE> C3DTVector<TYPE>& operator*=(C3DTVector<TYPE>& point, const C4x4TMatrix<TYPE>& matrix);
143
144#endif // 0
145
146//////////////////////
147// Mix type multiplication
148template <class TYPE, class TYPE2> C3DTPoint<TYPE> operator*(const C3DTPoint<TYPE>& point, const C4x4TMatrix<TYPE2>& matrix);
149template <class TYPE, class TYPE2> C3DTVector<TYPE> operator*(const C3DTVector<TYPE>& point, const C4x4TMatrix<TYPE2>& matrix);
150template <class TYPE, class TYPE2> C3DTPoint<TYPE>& operator*=(C3DTPoint<TYPE>& point, const C4x4TMatrix<TYPE2>& matrix);
151template <class TYPE, class TYPE2> C3DTVector<TYPE>& operator*=(C3DTVector<TYPE>& point, const C4x4TMatrix<TYPE2>& matrix);
152
153//! Swap matrix matrix values. To be used only when no swap transformation has been applied to the points
154DLL_3DFUNCTION C4x4Matrix SwapMatrix(unsigned int swapXYZMode, const C4x4Matrix& refmatrix);
155//! Swap the matrix values in order when points coordinates has also been swapped
156DLL_3DFUNCTION C4x4Matrix AdjustMatrix(unsigned int swapMode, const C4x4Matrix& refmatrix);
157//! Decompose a C4x4Matrix and get MatrixDecomposition components
158DLL_3DFUNCTION void Decompose(const C4x4MatrixD& matrix, MatrixDecomposition& components);
159//! Recompose a C4x4Matrix from MatrixDecomposition provided components
160DLL_3DFUNCTION C4x4MatrixD Recompose(const MatrixDecomposition& components);
161//! Returns true if the matrix can be decomposed using a translation / rotation and scale matrix. Return false if the matrix has some stretch for example
162DLL_3DFUNCTION bool IsTrsDecomposable(const C4x4MatrixD& matrix);
163
164//////////////////////////////////////////////////////////////////////
165// Implementation
166//////////////////////////////////////////////////////////////////////
167#if 0
168// transform a point. This method lost flags and data attached to input point
169template <class TYPE> C3DTPoint<TYPE>& operator*=(C3DTPoint<TYPE>& point, const C4x4TMatrix<TYPE>& matrix)
170{
171 C3DTPoint<TYPE> tmppoint(point);
172
173 point.x = matrix.matrix[0][0]*tmppoint.x + matrix.matrix[0][1]*tmppoint.y + matrix.matrix[0][2]*tmppoint.z + matrix.matrix[0][3];
174 point.y = matrix.matrix[1][0]*tmppoint.x + matrix.matrix[1][1]*tmppoint.y + matrix.matrix[1][2]*tmppoint.z + matrix.matrix[1][3];
175 point.z = matrix.matrix[2][0]*tmppoint.x + matrix.matrix[2][1]*tmppoint.y + matrix.matrix[2][2]*tmppoint.z + matrix.matrix[2][3];
176
177 return point;
178}
179
180template <class TYPE> C3DTPoint<TYPE> operator *(const C3DTPoint<TYPE>& point, const C4x4TMatrix<TYPE>& matrix)
181{
182 C3DTPoint<TYPE> retpoint;
183
184 retpoint.x = matrix.matrix[0][0] * point.x + matrix.matrix[0][1] * point.y + matrix.matrix[0][2] * point.z + matrix.matrix[0][3];
185 retpoint.y = matrix.matrix[1][0] * point.x + matrix.matrix[1][1] * point.y + matrix.matrix[1][2] * point.z + matrix.matrix[1][3];
186 retpoint.z = matrix.matrix[2][0] * point.x + matrix.matrix[2][1] * point.y + matrix.matrix[2][2] * point.z + matrix.matrix[2][3];
187
188 return retpoint;
189}
190
191// transform a vector.
192template <class TYPE> C3DTVector<TYPE>& operator*=(C3DTVector<TYPE>& vector, const C4x4TMatrix<TYPE>& matrix)
193{
194 C3DTVector<TYPE> tmpvector(vector);
195
196 vector.x = matrix.matrix[0][0] * tmpvector.x + matrix.matrix[0][1] * tmpvector.y + matrix.matrix[0][2] * tmpvector.z; // 01/2017: Fix vector.t = 0, no translation
197 vector.y = matrix.matrix[1][0] * tmpvector.x + matrix.matrix[1][1] * tmpvector.y + matrix.matrix[1][2] * tmpvector.z;
198 vector.z = matrix.matrix[2][0] * tmpvector.x + matrix.matrix[2][1] * tmpvector.y + matrix.matrix[2][2] * tmpvector.z;
199
200 return vector;
201}
202
203template <class TYPE> C3DTVector<TYPE> operator *(const C3DTVector<TYPE>& vector, const C4x4TMatrix<TYPE>& matrix)
204{
205 C3DTVector<TYPE> retvect;
206
207 retvect.x = matrix.matrix[0][0]*vector.x + matrix.matrix[0][1]*vector.y + matrix.matrix[0][2]*vector.z; // 01/2017: Fix vector.t = 0, no translation
208 retvect.y = matrix.matrix[1][0]*vector.x + matrix.matrix[1][1]*vector.y + matrix.matrix[1][2]*vector.z;
209 retvect.z = matrix.matrix[2][0]*vector.x + matrix.matrix[2][1]*vector.y + matrix.matrix[2][2]*vector.z;
210
211 return retvect;
212}
213#endif
214
215// Mix type multiplication
216// transform a point. This method lost flags and data attached to input point
217template <class TYPE, class TYPE2> C3DTPoint<TYPE>& operator*=(C3DTPoint<TYPE>& point, const C4x4TMatrix<TYPE2>& matrix)
218{
219 C3DTPoint<TYPE> tmppoint(point);
220
221 point.x = (TYPE)(matrix.matrix[0][0] * tmppoint.x + matrix.matrix[0][1] * tmppoint.y + matrix.matrix[0][2] * tmppoint.z + matrix.matrix[0][3]);
222 point.y = (TYPE)(matrix.matrix[1][0] * tmppoint.x + matrix.matrix[1][1] * tmppoint.y + matrix.matrix[1][2] * tmppoint.z + matrix.matrix[1][3]);
223 point.z = (TYPE)(matrix.matrix[2][0] * tmppoint.x + matrix.matrix[2][1] * tmppoint.y + matrix.matrix[2][2] * tmppoint.z + matrix.matrix[2][3]);
224
225 return point;
226}
227
228template <class TYPE, class TYPE2> C3DTPoint<TYPE> operator *(const C3DTPoint<TYPE>& point, const C4x4TMatrix<TYPE2>& matrix)
229{
230 C3DTPoint<TYPE> retpoint;
231
232 retpoint.x = (TYPE)(matrix.matrix[0][0] * point.x + matrix.matrix[0][1] * point.y + matrix.matrix[0][2] * point.z + matrix.matrix[0][3]);
233 retpoint.y = (TYPE)(matrix.matrix[1][0] * point.x + matrix.matrix[1][1] * point.y + matrix.matrix[1][2] * point.z + matrix.matrix[1][3]);
234 retpoint.z = (TYPE)(matrix.matrix[2][0] * point.x + matrix.matrix[2][1] * point.y + matrix.matrix[2][2] * point.z + matrix.matrix[2][3]);
235
236 return retpoint;
237}
238
239// transform a vector.
240template <class TYPE, class TYPE2> C3DTVector<TYPE>& operator*=(C3DTVector<TYPE>& vector, const C4x4TMatrix<TYPE2>& matrix)
241{
242 C3DTVector<TYPE> tmpvector(vector);
243
244 vector.x = (TYPE)(matrix.matrix[0][0] * tmpvector.x + matrix.matrix[0][1] * tmpvector.y + matrix.matrix[0][2] * tmpvector.z); // 01/2017: Fix vector.t = 0, no translation
245 vector.y = (TYPE)(matrix.matrix[1][0] * tmpvector.x + matrix.matrix[1][1] * tmpvector.y + matrix.matrix[1][2] * tmpvector.z);
246 vector.z = (TYPE)(matrix.matrix[2][0] * tmpvector.x + matrix.matrix[2][1] * tmpvector.y + matrix.matrix[2][2] * tmpvector.z);
247
248 return vector;
249}
250
251template <class TYPE, class TYPE2> C3DTVector<TYPE> operator *(const C3DTVector<TYPE>& vector, const C4x4TMatrix<TYPE2>& matrix)
252{
253 C3DTVector<TYPE> retvect;
254
255 retvect.x = (TYPE)(matrix.matrix[0][0] * vector.x + matrix.matrix[0][1] * vector.y + matrix.matrix[0][2] * vector.z); // 01/2017: Fix vector.t = 0, no translation
256 retvect.y = (TYPE)(matrix.matrix[1][0] * vector.x + matrix.matrix[1][1] * vector.y + matrix.matrix[1][2] * vector.z);
257 retvect.z = (TYPE)(matrix.matrix[2][0] * vector.x + matrix.matrix[2][1] * vector.y + matrix.matrix[2][2] * vector.z);
258
259 return retvect;
260}
261
262//////////////////////////////////////////////////////////////////////
263// Construction/Destruction
264//////////////////////////////////////////////////////////////////////
265template <class TYPE> C4x4TMatrix<TYPE>::C4x4TMatrix()
266{
267}
268
269// Set matrix to identity
270template <class TYPE> C4x4TMatrix<TYPE>::C4x4TMatrix(bool init)
271{
272 if (init)
273 Init();
274}
275
276template <class TYPE>
277template <class TYPE2> C4x4TMatrix<TYPE>::C4x4TMatrix(const C4x4TMatrix<TYPE2>& refmatrix)
278{
279 int i, j;
280 for (i = 0; i < 4; i++)
281 for (j = 0; j < 4; j++)
282 matrix[i][j] = (TYPE)refmatrix.matrix[i][j];
283}
284
285template <class TYPE>
286template <class TYPE2> C4x4TMatrix<TYPE>::C4x4TMatrix(const TYPE2 *values, bool rotate)
287{
288 int i, j;
289
290 if (rotate)
291 {
292 for (i=0; i<4; i++)
293 for (j=0; j<4; j++)
294 matrix[j][i] = (TYPE)values[i*4+j];
295 }
296 else
297 {
298 for (i=0; i<4; i++)
299 for (j=0; j<4; j++)
300 matrix[i][j] = (TYPE)values[i*4+j];
301 }
302}
303
304template <class TYPE>
309
310template <class TYPE>
311inline const TYPE *C4x4TMatrix<TYPE>::ValPtr() const
312{
313 return matrix[0];
314}
315
316template <class TYPE>
317inline TYPE *C4x4TMatrix<TYPE>::ValPtr()
318{
319 return matrix[0];
320}
321
322template <class TYPE>
323inline C4x4TMatrix<TYPE>::operator TYPE*()
324{
325 return matrix[0];
326}
327
328template <class TYPE>
329inline C4x4TMatrix<TYPE>::operator const TYPE*() const
330{
331 return matrix[0];
332}
333
334template <class TYPE>
335inline unsigned int C4x4TMatrix<TYPE>::SizeOf() const
336{
337 return static_cast<unsigned int>(sizeof(matrix));
338}
339
340template <class TYPE>
341inline void C4x4TMatrix<TYPE>::operator =(const TYPE *newmatrix)
342{
343 memcpy(matrix, newmatrix, sizeof(matrix));
344}
345
346template <class TYPE>
347C4x4TMatrix<TYPE>& C4x4TMatrix<TYPE>::operator=(const C4x4TMatrix<TYPE>& refmatrix)
348{
349 memcpy(matrix, refmatrix.matrix, sizeof(matrix));
350 return *this;
351}
352
353template <class TYPE>
354template <class TYPE2> C4x4TMatrix<TYPE>& C4x4TMatrix<TYPE>::operator=(const C4x4TMatrix<TYPE2>& refmatrix)
355{
356 int i, j;
357 for (i = 0; i < 4; i++)
358 for (j = 0; j < 4; j++)
359 matrix[i][j] = (TYPE)(refmatrix.matrix[i][j]);
360
361 return *this;
362}
363
364template <class TYPE>
365inline TYPE& C4x4TMatrix<TYPE>::operator()(int i, int j)
366{
367 return matrix[i][j];
368}
369
370template <class TYPE>
371inline TYPE C4x4TMatrix<TYPE>::operator()(int i, int j) const
372{
373 return matrix[i][j];
374}
375
376template <class TYPE> void C4x4TMatrix<TYPE>::operator+= (const C4x4TMatrix<TYPE>& addmatrix)
377{
378 matrix[0][0] += addmatrix.matrix[0][0];
379 matrix[0][1] += addmatrix.matrix[0][1];
380 matrix[0][2] += addmatrix.matrix[0][2];
381 matrix[0][3] += addmatrix.matrix[0][3];
382
383 matrix[1][0] += addmatrix.matrix[1][0];
384 matrix[1][1] += addmatrix.matrix[1][1];
385 matrix[1][2] += addmatrix.matrix[1][2];
386 matrix[1][3] += addmatrix.matrix[1][3];
387
388 matrix[2][0] += addmatrix.matrix[2][0];
389 matrix[2][1] += addmatrix.matrix[2][1];
390 matrix[2][2] += addmatrix.matrix[2][2];
391 matrix[2][3] += addmatrix.matrix[2][3];
392
393#if 0
394 matrix[3][0] += addmatrix.matrix[3][0];
395 matrix[3][1] += addmatrix.matrix[3][1];
396 matrix[3][2] += addmatrix.matrix[3][2];
397 matrix[3][3] += addmatrix.matrix[3][3];
398#endif
399}
400
401// Code from Teddy\Maths\Matrix.cpp
402template <class TYPE>
403C4x4TMatrix<TYPE>& C4x4TMatrix<TYPE>::operator=(const CQuaternion& quat)
404{
405#if 0
406 // Code from teddy seems to be buggy (quat = mat then mat = quat gives different results)
407 // 9 muls, 15 adds
408 double x2 = quat.val[0] + quat.val[0];
409 double y2 = quat.val[1] + quat.val[1];
410 double z2 = quat.val[2] + quat.val[2];
411 double xx = quat.val[0] * x2; double xy = quat.val[0] * y2; double xz = quat.val[0] * z2;
412 double yy = quat.val[1] * y2; double yz = quat.val[1] * z2; double zz = quat.val[2] * z2;
413 double wx = quat.val[3] * x2; double wy = quat.val[3] * y2; double wz = quat.val[3] * z2;
414
415 matrix[0][3] =
416 matrix[1][3] =
417 matrix[2][3] =
418 matrix[3][0] = matrix[3][1] = matrix[3][2] = 0;
419 matrix[3][3] = 1;
420 matrix[0][0] = (float)(1-(yy+zz)); matrix[1][0] = (float)(xy+wz); matrix[2][0] = (float)(xz-wy);
421 matrix[0][1] = (float)(xy-wz); matrix[1][1] = (float)(1-(xx+zz)); matrix[2][1] = (float)(yz+wx);
422 matrix[0][2] = (float)(xz+wy); matrix[1][2] = (float)(yz-wx); matrix[2][2] = (float)(1-(xx+yy));
423#elif 0
424 // This one from http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm
425 // Seems better (matrix in the original code is transposed).
426 matrix[0][3] =
427 matrix[1][3] =
428 matrix[2][3] =
429 matrix[3][0] = matrix[3][1] = matrix[3][2] = 0;
430 matrix[3][3] = 1;
431
432 double sqw = quat.val[3]*quat.val[3];
433 double sqx = quat.val[2]*quat.val[2];
434 double sqy = quat.val[1]*quat.val[1];
435 double sqz = quat.val[0]*quat.val[0];
436
437 matrix[2][2] = (real)(sqx - sqy - sqz + sqw); // since sqw + sqx + sqy + sqz =1
438 matrix[1][1] = (real)(-sqx + sqy - sqz + sqw);
439 matrix[0][0] = (real)(-sqx - sqy + sqz + sqw);
440
441 double tmp1 = quat.val[2]*quat.val[1];
442 double tmp2 = quat.val[0]*quat.val[3];
443 matrix[1][2] = (real)(2.0 *(tmp1 + tmp2));
444 matrix[2][1] = (real)(2.0 *(tmp1 - tmp2));
445 tmp1 = quat.val[2]*quat.val[0];
446 tmp2 = quat.val[1]*quat.val[3];
447 matrix[0][2] = (real)(2.0 *(tmp1 - tmp2));
448 matrix[2][0] = (real)(2.0 *(tmp1 + tmp2));
449 tmp1 = quat.val[1]*quat.val[0];
450 tmp2 = quat.val[2]*quat.val[3];
451 matrix[0][1] = (real)(2.0 *(tmp1 + tmp2));
452 matrix[1][0] = (real)(2.0 *(tmp1 - tmp2));
453#else
454 // This one from http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm
455 // Seems better (matrix in the original code is transposed).
456 // It does not need the quaternion to be normalized
457 double x2 = quat.val[0] * quat.val[0];
458 double y2 = quat.val[1] * quat.val[1];
459 double z2 = quat.val[2] * quat.val[2];
460 double w2 = quat.val[3] * quat.val[3];
461
462 // invs (inverse square length) is only required if quaternion is not already normalised
463 double invs = 1 / (x2 + y2 + z2 + w2);
464
465 matrix[0][3] =
466 matrix[1][3] =
467 matrix[2][3] =
468 matrix[3][0] = matrix[3][1] = matrix[3][2] = 0;
469 matrix[3][3] = 1;
470
471 matrix[0][0] = static_cast<real>(( x2 - y2 - z2 + w2)*invs); // since w2 + x2 + y2 + z2 = 1/invs*invs
472 matrix[1][1] = static_cast<real>((-x2 + y2 - z2 + w2)*invs);
473 matrix[2][2] = static_cast<real>((-x2 - y2 + z2 + w2)*invs);
474
475
476 double tmp1 = quat.val[0]*quat.val[1]; // xy
477 double tmp2 = quat.val[2]*quat.val[3]; // zw
478 matrix[0][1] = static_cast<real>(2.0 * (tmp1 + tmp2)*invs); // xy + zw
479 matrix[1][0] = static_cast<real>(2.0 * (tmp1 - tmp2)*invs); // xy - zw
480
481 tmp1 = quat.val[0]*quat.val[2]; // xz
482 tmp2 = quat.val[1]*quat.val[3]; // yw
483 matrix[0][2] = static_cast<real>(2.0 * (tmp1 - tmp2)*invs); // xz - yw
484 matrix[2][0] = static_cast<real>(2.0 * (tmp1 + tmp2)*invs); // xz + yw
485
486 tmp1 = quat.val[1]*quat.val[2]; // yz
487 tmp2 = quat.val[0]*quat.val[3]; // xw
488 matrix[1][2] = static_cast<real>(2.0 * (tmp1 + tmp2)*invs); // yz + xw
489 matrix[2][1] = static_cast<real>(2.0 * (tmp1 - tmp2)*invs); // yz - xw
490#endif
491
492 return *this;
493}
494
495template <class TYPE> bool C4x4TMatrix<TYPE>::operator==(const C4x4TMatrix<TYPE>& compmatrix) const
496{
497 int i, j;
498
499 for (i=0; i<4; i++)
500 for (j=0; j<4; j++)
501 {
502 if (matrix[i][j] != compmatrix.matrix[i][j])
503 return false;
504 }
505
506 return true;
507}
508
509template <class TYPE> void C4x4TMatrix<TYPE>::Transpose(C4x4TMatrix<TYPE>& transMat) const
510{
511 int i, j;
512 for (i=0; i<3; i++) // i<3 has last line never change (we swap (3,3) with (3, 3))
513 for (j=i; j<4; j++)
514 transMat.matrix[j][i] = matrix[i][j];
515}
516
517// Specialization must be inline or defined in cpp file
518template <> inline void C4x4TMatrix<float>::Decompose(MatrixDecomposition& components) const
519{
520 C4x4MatrixD matd(*this);
521 mootools::Decompose(matd, components);
522}
523
524template <> inline void C4x4TMatrix<double>::Decompose(MatrixDecomposition& components) const
525{
526 mootools::Decompose(*this, components);
527}
528
529
530template <> inline bool C4x4TMatrix<float>::IsTrsDecomposable() const
531{
532 C4x4MatrixD matd(*this);
533 return mootools::IsTrsDecomposable(matd);
534}
535
536template <> inline bool C4x4TMatrix<double>::IsTrsDecomposable() const
537{
538 return mootools::IsTrsDecomposable(*this);
539}
540
541template <class TYPE> void C4x4TMatrix<TYPE>::Recompose(const MatrixDecomposition& components)
542{
543 *this = mootools::Recompose(components);
544}
545
546// Compiler incorrectly optimizes when compiling in x64 bits release mode
547// optimization set to /O2 /Ot. The bug is actually on VS2005 SP1
548// It can be check in the 3D viewer when we turn around the scene or use the left point of view
549#if defined(__WINDOWS__) && defined(__64BITS__)
550#pragma optimize("", off)
551#endif
552template <class TYPE> void C4x4TMatrix<TYPE>::Transpose()
553{
554 TYPE tmp;
555 int i, j;
556 for (i=0; i<3; i++) // i<3 has last line never change (we swap (3,3) with (3, 3))
557 for (j=i; j<4; j++)
558 {
559 tmp = matrix[j][i];
560 matrix[j][i] = matrix[i][j];
561 matrix[i][j] = tmp;
562 }
563}
564#if defined(__WINDOWS__) && defined(__64BITS__)
565#pragma optimize("", on)
566#endif
567
568template <class TYPE> bool C4x4TMatrix<TYPE>::operator!=(const C4x4TMatrix<TYPE>& compmatrix) const
569{
570 return !(*this == compmatrix);
571}
572
573template <class TYPE> void C4x4TMatrix<TYPE>::operator-= (const C4x4TMatrix<TYPE>& submatrix)
574{
575 matrix[0][0] -= submatrix.matrix[0][0];
576 matrix[0][1] -= submatrix.matrix[0][1];
577 matrix[0][2] -= submatrix.matrix[0][2];
578 matrix[0][3] -= submatrix.matrix[0][3];
579
580 matrix[1][0] -= submatrix.matrix[1][0];
581 matrix[1][1] -= submatrix.matrix[1][1];
582 matrix[1][2] -= submatrix.matrix[1][2];
583 matrix[1][3] -= submatrix.matrix[1][3];
584
585 matrix[2][0] -= submatrix.matrix[2][0];
586 matrix[2][1] -= submatrix.matrix[2][1];
587 matrix[2][2] -= submatrix.matrix[2][2];
588 matrix[2][3] -= submatrix.matrix[2][3];
589
590#if 0
591 matrix[3][0] -= submatrix.matrix[3][0];
592 matrix[3][1] -= submatrix.matrix[3][1];
593 matrix[3][2] -= submatrix.matrix[3][2];
594 matrix[3][3] -= submatrix.matrix[3][3];
595#endif
596}
597
598// May be optimizable
599template <class TYPE> C4x4TMatrix<TYPE>& C4x4TMatrix<TYPE>::operator *=(const C4x4TMatrix<TYPE>& matrix1)
600{
601 C4x4TMatrix<TYPE> copy = *this;
602
603 this->matrix[0][0] = copy.matrix[0][0]*matrix1.matrix[0][0]+copy.matrix[0][1]*matrix1.matrix[1][0]+copy.matrix[0][2]*matrix1.matrix[2][0]+copy.matrix[0][3]*matrix1.matrix[3][0];
604 this->matrix[0][1] = copy.matrix[0][0]*matrix1.matrix[0][1]+copy.matrix[0][1]*matrix1.matrix[1][1]+copy.matrix[0][2]*matrix1.matrix[2][1]+copy.matrix[0][3]*matrix1.matrix[3][1];
605 this->matrix[0][2] = copy.matrix[0][0]*matrix1.matrix[0][2]+copy.matrix[0][1]*matrix1.matrix[1][2]+copy.matrix[0][2]*matrix1.matrix[2][2]+copy.matrix[0][3]*matrix1.matrix[3][2];
606 this->matrix[0][3] = copy.matrix[0][0]*matrix1.matrix[0][3]+copy.matrix[0][1]*matrix1.matrix[1][3]+copy.matrix[0][2]*matrix1.matrix[2][3]+copy.matrix[0][3]*matrix1.matrix[3][3];
607
608 this->matrix[1][0] = copy.matrix[1][0]*matrix1.matrix[0][0]+copy.matrix[1][1]*matrix1.matrix[1][0]+copy.matrix[1][2]*matrix1.matrix[2][0]+copy.matrix[1][3]*matrix1.matrix[3][0];
609 this->matrix[1][1] = copy.matrix[1][0]*matrix1.matrix[0][1]+copy.matrix[1][1]*matrix1.matrix[1][1]+copy.matrix[1][2]*matrix1.matrix[2][1]+copy.matrix[1][3]*matrix1.matrix[3][1];
610 this->matrix[1][2] = copy.matrix[1][0]*matrix1.matrix[0][2]+copy.matrix[1][1]*matrix1.matrix[1][2]+copy.matrix[1][2]*matrix1.matrix[2][2]+copy.matrix[1][3]*matrix1.matrix[3][2];
611 this->matrix[1][3] = copy.matrix[1][0]*matrix1.matrix[0][3]+copy.matrix[1][1]*matrix1.matrix[1][3]+copy.matrix[1][2]*matrix1.matrix[2][3]+copy.matrix[1][3]*matrix1.matrix[3][3];
612
613 this->matrix[2][0] = copy.matrix[2][0]*matrix1.matrix[0][0]+copy.matrix[2][1]*matrix1.matrix[1][0]+copy.matrix[2][2]*matrix1.matrix[2][0]+copy.matrix[2][3]*matrix1.matrix[3][0];
614 this->matrix[2][1] = copy.matrix[2][0]*matrix1.matrix[0][1]+copy.matrix[2][1]*matrix1.matrix[1][1]+copy.matrix[2][2]*matrix1.matrix[2][1]+copy.matrix[2][3]*matrix1.matrix[3][1];
615 this->matrix[2][2] = copy.matrix[2][0]*matrix1.matrix[0][2]+copy.matrix[2][1]*matrix1.matrix[1][2]+copy.matrix[2][2]*matrix1.matrix[2][2]+copy.matrix[2][3]*matrix1.matrix[3][2];
616 this->matrix[2][3] = copy.matrix[2][0]*matrix1.matrix[0][3]+copy.matrix[2][1]*matrix1.matrix[1][3]+copy.matrix[2][2]*matrix1.matrix[2][3]+copy.matrix[2][3]*matrix1.matrix[3][3];
617
618 this->matrix[3][0] = copy.matrix[3][0]*matrix1.matrix[0][0]+copy.matrix[3][1]*matrix1.matrix[1][0]+copy.matrix[3][2]*matrix1.matrix[2][0]+copy.matrix[3][3]*matrix1.matrix[3][0];
619 this->matrix[3][1] = copy.matrix[3][0]*matrix1.matrix[0][1]+copy.matrix[3][1]*matrix1.matrix[1][1]+copy.matrix[3][2]*matrix1.matrix[2][1]+copy.matrix[3][3]*matrix1.matrix[3][1];
620 this->matrix[3][2] = copy.matrix[3][0]*matrix1.matrix[0][2]+copy.matrix[3][1]*matrix1.matrix[1][2]+copy.matrix[3][2]*matrix1.matrix[2][2]+copy.matrix[3][3]*matrix1.matrix[3][2];
621 this->matrix[3][3] = copy.matrix[3][0]*matrix1.matrix[0][3]+copy.matrix[3][1]*matrix1.matrix[1][3]+copy.matrix[3][2]*matrix1.matrix[2][3]+copy.matrix[3][3]*matrix1.matrix[3][3];
622
623 return *this;
624}
625
626// May be optimizable
627template <class TYPE> C4x4TMatrix<TYPE> C4x4TMatrix<TYPE>::operator *(const C4x4TMatrix<TYPE>& matrix1) const
628{
629 C4x4TMatrix<TYPE> result;
630
631 result.matrix[0][0] = this->matrix[0][0]*matrix1.matrix[0][0]+this->matrix[0][1]*matrix1.matrix[1][0]+this->matrix[0][2]*matrix1.matrix[2][0]+this->matrix[0][3]*matrix1.matrix[3][0];
632 result.matrix[0][1] = this->matrix[0][0]*matrix1.matrix[0][1]+this->matrix[0][1]*matrix1.matrix[1][1]+this->matrix[0][2]*matrix1.matrix[2][1]+this->matrix[0][3]*matrix1.matrix[3][1];
633 result.matrix[0][2] = this->matrix[0][0]*matrix1.matrix[0][2]+this->matrix[0][1]*matrix1.matrix[1][2]+this->matrix[0][2]*matrix1.matrix[2][2]+this->matrix[0][3]*matrix1.matrix[3][2];
634 result.matrix[0][3] = this->matrix[0][0]*matrix1.matrix[0][3]+this->matrix[0][1]*matrix1.matrix[1][3]+this->matrix[0][2]*matrix1.matrix[2][3]+this->matrix[0][3]*matrix1.matrix[3][3];
635
636 result.matrix[1][0] = this->matrix[1][0]*matrix1.matrix[0][0]+this->matrix[1][1]*matrix1.matrix[1][0]+this->matrix[1][2]*matrix1.matrix[2][0]+this->matrix[1][3]*matrix1.matrix[3][0];
637 result.matrix[1][1] = this->matrix[1][0]*matrix1.matrix[0][1]+this->matrix[1][1]*matrix1.matrix[1][1]+this->matrix[1][2]*matrix1.matrix[2][1]+this->matrix[1][3]*matrix1.matrix[3][1];
638 result.matrix[1][2] = this->matrix[1][0]*matrix1.matrix[0][2]+this->matrix[1][1]*matrix1.matrix[1][2]+this->matrix[1][2]*matrix1.matrix[2][2]+this->matrix[1][3]*matrix1.matrix[3][2];
639 result.matrix[1][3] = this->matrix[1][0]*matrix1.matrix[0][3]+this->matrix[1][1]*matrix1.matrix[1][3]+this->matrix[1][2]*matrix1.matrix[2][3]+this->matrix[1][3]*matrix1.matrix[3][3];
640
641 result.matrix[2][0] = this->matrix[2][0]*matrix1.matrix[0][0]+this->matrix[2][1]*matrix1.matrix[1][0]+this->matrix[2][2]*matrix1.matrix[2][0]+this->matrix[2][3]*matrix1.matrix[3][0];
642 result.matrix[2][1] = this->matrix[2][0]*matrix1.matrix[0][1]+this->matrix[2][1]*matrix1.matrix[1][1]+this->matrix[2][2]*matrix1.matrix[2][1]+this->matrix[2][3]*matrix1.matrix[3][1];
643 result.matrix[2][2] = this->matrix[2][0]*matrix1.matrix[0][2]+this->matrix[2][1]*matrix1.matrix[1][2]+this->matrix[2][2]*matrix1.matrix[2][2]+this->matrix[2][3]*matrix1.matrix[3][2];
644 result.matrix[2][3] = this->matrix[2][0]*matrix1.matrix[0][3]+this->matrix[2][1]*matrix1.matrix[1][3]+this->matrix[2][2]*matrix1.matrix[2][3]+this->matrix[2][3]*matrix1.matrix[3][3];
645
646 result.matrix[3][0] = this->matrix[3][0]*matrix1.matrix[0][0]+this->matrix[3][1]*matrix1.matrix[1][0]+this->matrix[3][2]*matrix1.matrix[2][0]+this->matrix[3][3]*matrix1.matrix[3][0];
647 result.matrix[3][1] = this->matrix[3][0]*matrix1.matrix[0][1]+this->matrix[3][1]*matrix1.matrix[1][1]+this->matrix[3][2]*matrix1.matrix[2][1]+this->matrix[3][3]*matrix1.matrix[3][1];
648 result.matrix[3][2] = this->matrix[3][0]*matrix1.matrix[0][2]+this->matrix[3][1]*matrix1.matrix[1][2]+this->matrix[3][2]*matrix1.matrix[2][2]+this->matrix[3][3]*matrix1.matrix[3][2];
649 result.matrix[3][3] = this->matrix[3][0]*matrix1.matrix[0][3]+this->matrix[3][1]*matrix1.matrix[1][3]+this->matrix[3][2]*matrix1.matrix[2][3]+this->matrix[3][3]*matrix1.matrix[3][3];
650
651 return result;
652}
653
654template <class TYPE> void C4x4TMatrix<TYPE>::operator *=(TYPE value)
655{
656 matrix[0][0] *= value;
657 matrix[0][1] *= value;
658 matrix[0][2] *= value;
659 matrix[0][3] *= value;
660
661 matrix[1][0] *= value;
662 matrix[1][1] *= value;
663 matrix[1][2] *= value;
664 matrix[1][3] *= value;
665
666 matrix[2][0] *= value;
667 matrix[2][1] *= value;
668 matrix[2][2] *= value;
669 matrix[2][3] *= value;
670}
671
672template <class TYPE>
673template <class TYPE2>
674C4DTVector<TYPE2> C4x4TMatrix<TYPE>::operator *(const C4DTVector<TYPE2>& vector) const
675{
676 C4DTVector<TYPE2> retvector;
677
678 retvector.dir.x = matrix[0][0]*vector.dir.x + matrix[0][1]*vector.dir.y + matrix[0][2]*vector.dir.z + matrix[0][3]*vector.t;
679 retvector.dir.y = matrix[1][0]*vector.dir.x + matrix[1][1]*vector.dir.y + matrix[1][2]*vector.dir.z + matrix[1][3]*vector.t;
680 retvector.dir.z = matrix[2][0]*vector.dir.x + matrix[2][1]*vector.dir.y + matrix[2][2]*vector.dir.z + matrix[2][3]*vector.t;
681 retvector.t = matrix[3][0]*vector.dir.x + matrix[3][1]*vector.dir.y + matrix[3][2]*vector.dir.z + matrix[3][3]*vector.t;
682
683 return retvector;
684}
685
686template <class TYPE> C3DTVector<TYPE> C4x4TMatrix<TYPE>::GetRow(int i) const
687{
688 return C3DTVector<TYPE>(matrix[i][0], matrix[i][1], matrix[i][2]);
689}
690
691template <class TYPE> void C4x4TMatrix<TYPE>::SetRow(int i, const C3DTVector<TYPE>& vector)
692{
693 matrix[i][0] = vector.x;
694 matrix[i][1] = vector.y;
695 matrix[i][2] = vector.z;
696}
697
698template <class TYPE> C3DTVector<TYPE> C4x4TMatrix<TYPE>::GetColumn(int i) const
699{
700 return C3DTVector<TYPE>(matrix[0][i], matrix[1][i], matrix[2][i]);
701}
702
703template <class TYPE> void C4x4TMatrix<TYPE>::SetColumn(int i, const C3DTVector<TYPE>& vector)
704{
705 matrix[0][i] = vector.x;
706 matrix[1][i] = vector.y;
707 matrix[2][i] = vector.z;
708}
709
710// For a point : t = 1.0
711template <class TYPE> C4DTVector<TYPE> C4x4TMatrix<TYPE>::Mult(double x, double y, double z) const
712{
713 C4DTVector<TYPE> retvector;
714
715 retvector.dir.x = matrix[0][0]*x + matrix[0][1]*y + matrix[0][2]*z + matrix[0][3];
716 retvector.dir.y = matrix[1][0]*x + matrix[1][1]*y + matrix[1][2]*z + matrix[1][3];
717 retvector.dir.z = matrix[2][0]*x + matrix[2][1]*y + matrix[2][2]*z + matrix[2][3];
718 retvector.t = matrix[3][0]*x + matrix[3][1]*y + matrix[3][2]*z + matrix[3][3];
719
720 return retvector;
721}
722
723template <class TYPE> void C4x4TMatrix<TYPE>::Init()
724{
725 matrix[0][0] = 1.0f;
726 matrix[0][1] = 0.0f;
727 matrix[0][2] = 0.0f;
728 matrix[0][3] = 0.0f;
729
730 matrix[1][0] = 0.0f;
731 matrix[1][1] = 1.0f;
732 matrix[1][2] = 0.0f;
733 matrix[1][3] = 0.0f;
734
735 matrix[2][0] = 0.0f;
736 matrix[2][1] = 0.0f;
737 matrix[2][2] = 1.0f;
738 matrix[2][3] = 0.0f;
739
740 matrix[3][0] = 0.0f;
741 matrix[3][1] = 0.0f;
742 matrix[3][2] = 0.0f;
743 matrix[3][3] = 1.0f;
744}
745
746template <class TYPE>
747template <class TYPE2> void C4x4TMatrix<TYPE>::LoadMatrix(const TYPE2 *refmatrix)
748{
749 int i, j;
750 for (i=0; i<4; i++)
751 for (j=0; j<4; j++)
752 matrix[i][j] = (TYPE)refmatrix[i*4+j];
753}
754
755template <class TYPE>
756template <class TYPE2> void C4x4TMatrix<TYPE>::SaveMatrix(TYPE2 *refmatrix) const
757{
758 int i, j;
759 for (i=0; i<4; i++)
760 for (j=0; j<4; j++)
761 refmatrix[i*4+j] = (TYPE)matrix[i][j];
762}
763
764template <class TYPE> bool C4x4TMatrix<TYPE>::IsIdentity(double precision) const
765{
766 int i, j;
767
768 for (i=0 ; i<4; i++)
769 for (j=0 ; j<4; j++)
770 {
771 if (i != j)
772 {
773 if (abs(matrix[i][j]) > precision)
774 return false;
775
776 continue;
777 }
778 else if (abs(matrix[i][j] - 1.0f) > precision)
779 return false;
780 }
781
782 return true;
783}
784
785template <class TYPE> bool C4x4TMatrix<TYPE>::IsSimilar(const C4x4TMatrix<TYPE>& mat, double precision) const
786{
787 int i, j;
788
789 for (i = 0; i < 4; i++)
790 for (j = 0; j < 4; j++)
791 {
792 if (abs(matrix[i][j]-mat.matrix[i][j]) > precision)
793 return false;
794 }
795
796 return true;
797}
798
799template<class TYPE>
800inline bool C4x4TMatrix<TYPE>::IsAnormal() const
801{
802 int i, j;
803 for (i = 0; i < 4; i++)
804 for (j = 0; j < 4; j++)
805 {
806 if (_isnan(matrix[i][j]) || !_finite(matrix[i][j]))
807 return true;
808 }
809
810 return false;
811}
812
813template <class TYPE> void C4x4TMatrix<TYPE>::Translate(double x, double y, double z)
814{
815 matrix[0][3] += (TYPE)x;
816 matrix[1][3] += (TYPE)y;
817 matrix[2][3] += (TYPE)z;
818}
819
820template <class TYPE>
821template <class TYPE2>
822void C4x4TMatrix<TYPE>::Translate(const C3DTVector<TYPE2>& vect)
823{
824 matrix[0][3] += (TYPE)vect.x;
825 matrix[1][3] += (TYPE)vect.y;
826 matrix[2][3] += (TYPE)vect.z;
827}
828
829template <class TYPE> void C4x4TMatrix<TYPE>::InitTranslation(double x, double y, double z)
830{
831 matrix[0][3] = (TYPE)x;
832 matrix[1][3] = (TYPE)y;
833 matrix[2][3] = (TYPE)z;
834 matrix[3][3] = (TYPE)1.0;
835}
836
837template <class TYPE>
838template <class TYPE2> void C4x4TMatrix<TYPE>::InitTranslation(const C3DTVector<TYPE2>& vect)
839{
840 matrix[0][3] = (TYPE)vect.x;
841 matrix[1][3] = (TYPE)vect.y;
842 matrix[2][3] = (TYPE)vect.z;
843 matrix[3][3] = (TYPE)1.0;
844}
845
846template <class TYPE> void C4x4TMatrix<TYPE>::InitTranslation(const C4x4TMatrix<TYPE>& refmatrix)
847{
848 matrix[0][3] = refmatrix.matrix[0][3];
849 matrix[1][3] = refmatrix.matrix[1][3];
850 matrix[2][3] = refmatrix.matrix[2][3];
851}
852
853template <class TYPE> void C4x4TMatrix<TYPE>::NoTranslation()
854{
855 matrix[0][3] = 0.0f;
856 matrix[1][3] = 0.0f;
857 matrix[2][3] = 0.0f;
858 matrix[3][3] = 1.0f;
859}
860
861template <class TYPE>
862template <class TYPE2> void C4x4TMatrix<TYPE>::GetTranslation(C3DTVector<TYPE2>& vect) const
863{
864 vect.x = (TYPE2)matrix[0][3];
865 vect.y = (TYPE2)matrix[1][3];
866 vect.z = (TYPE2)matrix[2][3];
867}
868
869template <class TYPE>
870template <class TYPE2> void C4x4TMatrix<TYPE>::GetTranslation(C3DTPoint<TYPE2>& position) const
871{
872 position.x = (TYPE2)matrix[0][3];
873 position.y = (TYPE2)matrix[1][3];
874 position.z = (TYPE2)matrix[2][3];
875}
876
877template <class TYPE> void C4x4TMatrix<TYPE>::InitScale(double x, double y, double z)
878{
879 matrix[0][0] = (TYPE)x;
880 matrix[1][1] = (TYPE)y;
881 matrix[2][2] = (TYPE)z;
882}
883
884template <class TYPE>
885template <class TYPE2> void C4x4TMatrix<TYPE>::InitScale(const C3DTVector<TYPE2>& vect)
886{
887 matrix[0][0] = (TYPE2)vect.x;
888 matrix[1][1] = (TYPE2)vect.y;
889 matrix[2][2] = (TYPE2)vect.z;
890}
891
892// 02/2016: We previously only use PostScale
893template <class TYPE> void C4x4TMatrix<TYPE>::PostScale(double x, double y, double z )
894{
895 C4x4TMatrix<TYPE> scaleMat(true);
896 scaleMat.InitScale(x, y, z);
897
898 *this = (*this)*scaleMat;
899}
900
901template <class TYPE>
902template <class TYPE2> void C4x4TMatrix<TYPE>::PostScale(const C3DTVector<TYPE2>& vect)
903{
904 C4x4TMatrix<TYPE> scaleMat(true);
905 scaleMat.InitScale(vect);
906
907 *this = (*this)*scaleMat;
908}
909
910template <class TYPE> void C4x4TMatrix<TYPE>::PreScale(double x, double y, double z)
911{
912 C4x4TMatrix<TYPE> scaleMat(true);
913 scaleMat.InitScale(x, y, z);
914
915 *this = scaleMat*(*this);
916}
917
918template <class TYPE>
919template <class TYPE2> void C4x4TMatrix<TYPE>::PreScale(const C3DTVector<TYPE2>& vect)
920{
921 C4x4TMatrix<TYPE> scaleMat(true);
922 scaleMat.InitScale(vect);
923
924 *this = scaleMat*(*this);
925}
926
927// Beware: this code does not give information of a possible reflection. Returned values are always positive
928template <class TYPE>
929template <class TYPE2> void C4x4TMatrix<TYPE>::GetScale(C3DTVector<TYPE2>& vect) const
930{
931 const C4x4TMatrix<TYPE>& me = *this; // just for lisibility
932 C3DTVector<TYPE2> UU(1.0,0.0,0.0), VV(0.0,1.0,0.0), WW(0.0,0.0,1.0), OO(0.0,0.0,0.0);
933 C3DTVector<TYPE2> X(UU*me - OO*me), Y(VV*me - OO*me), Z(WW*me - OO*me);
934
935 vect.x = (TYPE2)(X.Length());
936 vect.y = (TYPE2)(Y.Length());
937 vect.z = (TYPE2)(Z.Length());
938}
939
940template <class TYPE> void C4x4TMatrix<TYPE>::NoScale()
941{
942 C3DTVector<TYPE> scale;
943 GetScale(scale);
944
945 if (scale.IsUnit()) // BUG: What if matrix has reflection?
946 return;
947
948 scale = scale.Invert();
949 PostScale(scale);
950
951#ifdef _DEBUG
952 GetScale(scale);
953 if (fabs(scale.x - 1.0f) > FLOAT_EPSILON || fabs(scale.y - 1.0f) > FLOAT_EPSILON || fabs(scale.z - 1.0f) > FLOAT_EPSILON)
954 XTRACE(trace3DModule, 0, "C4x4Matrix::NoScale lack of precision\n");
955#endif
956}
957
958// Code verified
959template <class TYPE> void C4x4TMatrix<TYPE>::SetHPBAngles(double heading, double pitch, double bank)
960{
961 Rotate(heading, 0.0, 1.0, 0.0); // h
962 Rotate(pitch, 1.0, 0.0, 0.0); // p
963 Rotate(bank, 0.0, 0.0, 1.0); // b
964}
965
966template <class TYPE> bool C4x4TMatrix<TYPE>::GetHPBAngles(double& heading, double& pitch, double& bank) const
967{
968#if 1
969 MatrixDecomposition components;
970 Decompose(components);
971
972 CQuaternion rotquat(components.Rotation());
973 rotquat.GetHPBAngles(heading, pitch, bank);
974
975 return (fabs(1.0 - components.Determinant()) < PRECISION_LIMIT && components.Stretch().IsIdentity());
976#else
977 // 03/23: use decomposition and quaternion, which is better in many case
978 // Code from http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToEuler/index.htm
979 // Beware: this code works if the matrix has no reflection
980
981 C3DTVector<TYPE> scale;
982 C4x4TMatrix<TYPE> matrixCopy = *this;
983
984 GetScale(scale);
985 matrixCopy.PostScale(scale.Invert());
986
987 // Assuming the angles are in radians.
988 if (matrixCopy.matrix[1][2] > 0.998)
989 {
990 // singularity at north pole
991 heading = atan2(matrixCopy.matrix[2][0], matrixCopy.matrix[0][0]);
992 pitch = F_PI / 2;
993 bank = 0;
994 return false;
995 }
996
997 if (matrixCopy.matrix[1][2] < -0.998)
998 {
999 // singularity at south pole
1000 heading = atan2(matrixCopy.matrix[2][0], matrixCopy.matrix[0][0]);
1001 pitch = -F_PI / 2;
1002 bank = 0;
1003 return false;
1004 }
1005
1006 heading = atan2(-matrixCopy.matrix[0][2], matrixCopy.matrix[2][2]);
1007 bank = atan2(-matrixCopy.matrix[1][0], matrixCopy.matrix[1][1]);
1008 pitch = asin(matrixCopy.matrix[1][2]);
1009
1010 if (HasReflection())
1011 {
1012 XTRACE("C4x4TMatrix::GetHPBAngles matrix has reflection, angle might be false\n");
1013 return false;
1014 }
1015
1016 return true;
1017#endif
1018}
1019
1020// Rotate the given matrix from an angle around an axis
1021// Rotation is done around the axis origin (object turn if vector translation is not null)
1022// Code from Yves
1023template <class TYPE> void C4x4TMatrix<TYPE>::Rotate(double angle, double x, double y, double z)
1024{
1025 if (fabs(angle) < PRECISION_LIMIT)
1026 return;
1027
1028 C4x4TMatrix<TYPE> mat;
1029 mat.InitRotation(angle, x, y, z);
1030
1031 *this *= mat;
1032}
1033
1034// Init matrix with a given rotation
1035// Code from Yves
1036template <class TYPE> void C4x4TMatrix<TYPE>::InitRotation(double angle, double x, double y, double z)
1037{
1038 if (fabs(angle) < PRECISION_LIMIT || (fabs(x) < PRECISION_LIMIT && fabs(y) < PRECISION_LIMIT && fabs(z) < PRECISION_LIMIT))
1039 {
1040 Init();
1041 return;
1042 }
1043
1044 double m = sqrt(x*x + y*y + z*z);
1045 double c = cos(angle);
1046 double s = sin(angle);
1047 x /= m; y /= m; z /= m;
1048
1049 matrix[0][0] = (TYPE)(x*x + c*(1.0 - x*x));
1050 matrix[1][0] = (TYPE)(x*y - c*x*y - s*z);
1051 matrix[2][0] = (TYPE)(x*z - c*x*z + s*y);
1052 matrix[3][0] = 0.0;
1053 matrix[0][1] = (TYPE)(y*x - c*y*x + s*z);
1054 matrix[1][1] = (TYPE)(y*y + c*(1.0 - y*y));
1055 matrix[2][1] = (TYPE)(y*z - c*y*z - s*x);
1056 matrix[3][1] = 0.0;
1057 matrix[0][2] = (TYPE)(z*x - c*z*x - s*y);
1058 matrix[1][2] = (TYPE)(z*y - c*z*y + s*x);
1059 matrix[2][2] = (TYPE)(z*z + c*(1.0 - z*z));
1060 matrix[3][2] = 0.0;
1061 matrix[0][3] = 0.0; // No translation...
1062 matrix[1][3] = 0.0;
1063 matrix[2][3] = 0.0;
1064 matrix[3][3] = 1.0;
1065
1066#ifdef MOOTOOLS_PRIVATE_BUILD
1067#ifdef _DEBUG
1068 {
1069 // Quaternion verification
1070 C4x4Matrix mat = CQuaternion(C3DVector(x, y, z), angle);
1071 bool similar = this->IsSimilar(mat);
1072 XASSERT(similar);
1073 }
1074#endif
1075#endif
1076}
1077
1078template <class TYPE> void C4x4TMatrix<TYPE>::InitRotation(const C4x4TMatrix<TYPE>& refmatrix)
1079{
1080 for (int i=0; i<3; i++)
1081 memcpy_s(&matrix[i][0], 3*sizeof(float), &refmatrix.matrix[i][0], 3*sizeof(float));
1082}
1083
1084// Return true, if the matrix include some symmetric transformation
1085// Reflection involves that faces order is swapped (from counter clock wise to clock wise)
1086template <class TYPE> bool C4x4TMatrix<TYPE>::HasReflection() const
1087{
1088 return (GetDeterminant() < 0.0f);
1089}
1090
1091#define ACCUMULATE \
1092 if (temp >= 0.0) \
1093 pos += temp; \
1094 else \
1095 neg += temp;
1096
1097template <class TYPE> double C4x4TMatrix<TYPE>::GetDeterminant() const
1098{
1099 double det_1;
1100 double pos, neg, temp;
1101
1102 /*
1103 * Calculate the determinant of submatrix A and determine if the
1104 * the matrix is singular as limited by the double precision
1105 * floating-point data representation.
1106 */
1107 pos = neg = 0.0;
1108 temp = matrix[0][0] * matrix[1][1] * matrix[2][2];
1109 ACCUMULATE
1110 temp = matrix[1][0] * matrix[2][1] * matrix[0][2];
1111 ACCUMULATE
1112 temp = matrix[2][0] * matrix[0][1] * matrix[1][2];
1113 ACCUMULATE
1114 temp = -matrix[2][0] * matrix[1][1] * matrix[0][2];
1115 ACCUMULATE
1116 temp = -matrix[1][0] * matrix[0][1] * matrix[2][2];
1117 ACCUMULATE
1118 temp = -matrix[0][0] * matrix[2][1] * matrix[1][2];
1119 ACCUMULATE
1120 det_1 = pos + neg;
1121
1122 return det_1;
1123}
1124
1125template <class TYPE> bool C4x4TMatrix<TYPE>::SelfInverse()
1126{
1127 C4x4Matrix invert;
1128 if (!Inverse(invert))
1129 return false;
1130
1131 *this = invert;
1132 return true;
1133}
1134
1135// Compute the inverse of the afine matrix (cf. graphics gems inverse.c)
1136// Code tested 4/11/99 (verify that pt*matrix*invmatrix = pt)
1137template <class TYPE> bool C4x4TMatrix<TYPE>::Inverse(C4x4TMatrix<TYPE>& invertedMatrix) const
1138{
1139 XASSERT(&invertedMatrix != this);
1140
1141 double det_1;
1142 double pos, neg, temp;
1143
1144 /*
1145 * Calculate the determinant of submatrix A and determine if the
1146 * the matrix is singular as limited by the double precision
1147 * floating-point data representation.
1148 */
1149 pos = neg = 0.0;
1150 temp = matrix[0][0] * matrix[1][1] * matrix[2][2];
1151 ACCUMULATE
1152 temp = matrix[1][0] * matrix[2][1] * matrix[0][2];
1153 ACCUMULATE
1154 temp = matrix[2][0] * matrix[0][1] * matrix[1][2];
1155 ACCUMULATE
1156 temp = -matrix[2][0] * matrix[1][1] * matrix[0][2];
1157 ACCUMULATE
1158 temp = -matrix[1][0] * matrix[0][1] * matrix[2][2];
1159 ACCUMULATE
1160 temp = -matrix[0][0] * matrix[2][1] * matrix[1][2];
1161 ACCUMULATE
1162 det_1 = pos + neg;
1163
1164 /* Is the submatrix A singular? */
1165 if ((det_1 == 0.0) || (ABS_MACRO(det_1 / (pos - neg)) < PRECISION_LIMIT))
1166 return false;
1167 else
1168 {
1169
1170 /* Calculate inverse(A) = adj(A) / det(A) */
1171 det_1 = 1.0 / det_1;
1172 invertedMatrix.matrix[0][0] = (TYPE)(( matrix[1][1] * matrix[2][2] -
1173 matrix[2][1] * matrix[1][2] )
1174 * det_1);
1175 invertedMatrix.matrix[0][1] = (TYPE)(-( matrix[0][1] * matrix[2][2] -
1176 matrix[2][1] * matrix[0][2] )
1177 * det_1);
1178 invertedMatrix.matrix[0][2] = (TYPE)(( matrix[0][1] * matrix[1][2] -
1179 matrix[1][1] * matrix[0][2] )
1180 * det_1);
1181 invertedMatrix.matrix[1][0] = (TYPE)(-( matrix[1][0] * matrix[2][2] -
1182 matrix[2][0] * matrix[1][2] )
1183 * det_1);
1184 invertedMatrix.matrix[1][1] = (TYPE)(( matrix[0][0] * matrix[2][2] -
1185 matrix[2][0] * matrix[0][2] )
1186 * det_1);
1187 invertedMatrix.matrix[1][2] = (TYPE)(-( matrix[0][0] * matrix[1][2] -
1188 matrix[1][0] * matrix[0][2] )
1189 * det_1);
1190 invertedMatrix.matrix[2][0] = (TYPE)(( matrix[1][0] * matrix[2][1] -
1191 matrix[2][0] * matrix[1][1] )
1192 * det_1);
1193 invertedMatrix.matrix[2][1] = (TYPE)(-( matrix[0][0] * matrix[2][1] -
1194 matrix[2][0] * matrix[0][1] )
1195 * det_1);
1196 invertedMatrix.matrix[2][2] = (TYPE)(( matrix[0][0] * matrix[1][1] -
1197 matrix[1][0] * matrix[0][1] )
1198 * det_1);
1199
1200 /* Calculate -C * inverse(A) */
1201 invertedMatrix.matrix[0][3] = (TYPE)(-( matrix[0][3] * invertedMatrix.matrix[0][0] +
1202 matrix[1][3] * invertedMatrix.matrix[0][1] +
1203 matrix[2][3] * invertedMatrix.matrix[0][2] ));
1204 invertedMatrix.matrix[1][3] = (TYPE)(-( matrix[0][3] * invertedMatrix.matrix[1][0] +
1205 matrix[1][3] * invertedMatrix.matrix[1][1] +
1206 matrix[2][3] * invertedMatrix.matrix[1][2] ));
1207 invertedMatrix.matrix[2][3] = (TYPE)(-( matrix[0][3] * invertedMatrix.matrix[2][0] +
1208 matrix[1][3] * invertedMatrix.matrix[2][1] +
1209 matrix[2][3] * invertedMatrix.matrix[2][2] ));
1210
1211 /* Fill in last column */
1212 invertedMatrix.matrix[3][0] = invertedMatrix.matrix[3][1] = invertedMatrix.matrix[3][2] = 0.0;
1213 invertedMatrix.matrix[3][3] = 1.0;
1214
1215#ifdef _DEBUG
1216 C4x4TMatrix<TYPE> A = *this;
1217 C4x4TMatrix<TYPE> id;
1218 id = A * invertedMatrix;
1219 if (!id.IsIdentity(FLOAT_EPSILON))
1220 {
1221 XTRACE(trace3DModule, 0, "C4x4Matrix::Inverse lack of precision\n");
1222 MOOTOOLS_PRIVATE_ASSERT(0);
1223 }
1224#endif
1225 return true;
1226 }
1227}
1228
1229#undef ACCUMULATE
1230
1231template <class TYPE> void C4x4TMatrix<TYPE>::Serialize(CXArchive& ar)
1232{
1233 int i, j;
1234
1235 if (ar.IsStoring())
1236 {
1237 for (i=0; i<4; i++)
1238 {
1239 for (j=0; j<4; j++)
1240 ar << matrix[i][j];
1241 }
1242 }
1243 else
1244 {
1245 for (i=0; i<4; i++)
1246 {
1247 for (j=0; j<4; j++)
1248 ar >> matrix[i][j];
1249 }
1250 }
1251}
1252
1253#ifdef _DEBUG
1254template <class TYPE> void C4x4TMatrix<TYPE>::Dump() const
1255{
1256 XTRACE(_T("Matrix:\n{"));
1257 for (int i=0; i<4; i++)
1258 XTRACE(_T("{ %.6f, %.6f, %.6f, %.6f }\n"), matrix[i][0], matrix[i][1], matrix[i][2], matrix[i][3]);
1259 XTRACE(_T("}\n"));
1260}
1261#endif
1262
1263END_MOOTOOLS_NAMESPACE
1264
1265#endif // !defined(AFX_4X4MATRIX_H__BB3D80EA_003C_11D2_A0E3_000000000000__INCLUDED_)
SwapMatrix
DLL_3DFUNCTION C4x4Matrix SwapMatrix(unsigned int swapXYZMode, const C4x4Matrix &refmatrix)
Swap matrix matrix values. To be used only when no swap transformation has been applied to the points...
Recompose
DLL_3DFUNCTION C4x4MatrixD Recompose(const MatrixDecomposition &components)
Recompose a C4x4Matrix from MatrixDecomposition provided components.
Decompose
DLL_3DFUNCTION void Decompose(const C4x4MatrixD &matrix, MatrixDecomposition &components)
Decompose a C4x4Matrix and get MatrixDecomposition components.
AdjustMatrix
DLL_3DFUNCTION C4x4Matrix AdjustMatrix(unsigned int swapMode, const C4x4Matrix &refmatrix)
Swap the matrix values in order when points coordinates has also been swapped.
IsTrsDecomposable
DLL_3DFUNCTION bool IsTrsDecomposable(const C4x4MatrixD &matrix)
Returns true if the matrix can be decomposed using a translation / rotation and scale matrix....
C3DTPoint
The class defines an x, y, z 3D point which can use int, float or double.
Definition 3DPoint.h:27
C4x4TMatrix
The class defines a 4x4 row major order matrix.
Definition 4x4Matrix.h:56
C4x4TMatrix::C4x4TMatrix
C4x4TMatrix(const CQuaternion &quat)
Init the matrix from a CQuaternion.
Definition 4x4Matrix.h:305
C4x4TMatrix::GetHPBAngles
bool GetHPBAngles(double &heading, double &pitch, double &bank) const
Return true if angle can be retrieved without singularity, meaning that the matrix could be recompose...
Definition 4x4Matrix.h:966
C4x4TMatrix::IsSimilar
bool IsSimilar(const C4x4TMatrix< TYPE > &compmat, double precision=FLOAT_EPSILON2) const
Check that both matrix are similar (FLOAT_EPSILON2 limit by default)
Definition 4x4Matrix.h:785
C4x4TMatrix::PostScale
void PostScale(double x, double y, double z)
Does not affect translation vector.
Definition 4x4Matrix.h:893
C4x4TMatrix::IsAnormal
bool IsAnormal() const
Check some validity about the matrix (inf / nan values...)
Definition 4x4Matrix.h:800
C4x4TMatrix::IsIdentity
bool IsIdentity(double precision=FLOAT_EPSILON2) const
Check that matrix is Identity (FLOAT_EPSILON3 limit)
Definition 4x4Matrix.h:764
C4x4TMatrix::PreScale
void PreScale(double x, double y, double z)
This affect the translation vector.
Definition 4x4Matrix.h:910
CQuaternion
The class defines a quaternion transformation.
Definition Quaternion.h:20
CXArchive
Definition XArchive.h:17
MatrixDecomposition
Get the different components of a C4x4Matrix, or set a matrix from this different components.
Definition 4x4Matrix.h:26
MatrixDecomposition::Recompose
friend DLL_3DFUNCTION C4x4MatrixD Recompose(const MatrixDecomposition &components)
Recompose a C4x4Matrix from MatrixDecomposition provided components.
MatrixDecomposition::Decompose
friend DLL_3DFUNCTION void Decompose(const C4x4MatrixD &matrix, MatrixDecomposition &components)
Decompose a C4x4Matrix and get MatrixDecomposition components.
MatrixDecomposition::IsTrsDecomposable
friend DLL_3DFUNCTION bool IsTrsDecomposable(const C4x4MatrixD &matrix)
Returns true if the matrix can be decomposed using a translation / rotation and scale matrix....