My Project: dynamics.mod/NewtonDynamics/source/core/dgMatrix.h Source File

My Project

dgMatrix.h
Go to the documentation of this file.
1 /* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
2 *
3 * This software is provided 'as-is', without any express or implied
4 * warranty. In no event will the authors be held liable for any damages
5 * arising from the use of this software.
6 *
7 * Permission is granted to anyone to use this software for any purpose,
8 * including commercial applications, and to alter it and redistribute it
9 * freely, subject to the following restrictions:
10 *
11 * 1. The origin of this software must not be misrepresented; you must not
12 * claim that you wrote the original software. If you use this software
13 * in a product, an acknowledgment in the product documentation would be
14 * appreciated but is not required.
15 *
16 * 2. Altered source versions must be plainly marked as such, and must not be
17 * misrepresented as being the original software.
18 *
19 * 3. This notice may not be removed or altered from any source distribution.
20 */
21
22 #ifndef __dgMatrix__
23 #define __dgMatrix__
24
25
26 #include "dgStdafx.h"
27 #include "dgDebug.h"
28 #include "dgVector.h"
29 #include "dgPlane.h"
30 #include <math.h>
31
32 class dgMatrix;
33 class dgQuaternion;
34
35 const dgMatrix& dgGetZeroMatrix ();
36 const dgMatrix& dgGetIdentityMatrix();
37
38
39 DG_MSC_VECTOR_ALIGMENT
40 class dgMatrix
41 {
42 public:
43 DG_CLASS_ALLOCATOR(allocator)
44
45 dgMatrix ();
46 dgMatrix (const dgFloat32* const array);
47 dgMatrix (const dgVector &front, const dgVector &up, const dgVector &right, const dgVector &posit);
48 dgMatrix (const dgQuaternion &rotation, const dgVector &position);
49
50 // create a orthonormal normal vector basis, front become m_front vector, and m_up and m_right are mutualiperpendicular to fron and to each other
51 dgMatrix (const dgVector &front);
52
53 // create a covariance Matrix = transpose(p) * q
54 dgMatrix (const dgVector& p, const dgVector& q);
55
56 dgVector& operator[] (dgInt32 i);
57 const dgVector& operator[] (dgInt32 i) const;
58
59 dgMatrix Inverse () const;
60 dgMatrix Inverse4x4 () const;
61 dgMatrix Transpose () const;
62 dgMatrix Transpose4X4 () const;
63 dgMatrix Symetric3by3Inverse () const;
64 dgVector RotateVector (const dgVector &v) const;
65 dgVector UnrotateVector (const dgVector &v) const;
66 dgVector TransformVector (const dgVector &v) const;
67 dgVector UntransformVector (const dgVector &v) const;
68 dgPlane TransformPlane (const dgPlane &localPlane) const;
69 dgPlane UntransformPlane (const dgPlane &globalPlane) const;
70 void TransformBBox (const dgVector& p0local, const dgVector& p1local, dgVector& p0, dgVector& p1) const;
71
72 void CalcPitchYawRoll (dgVector& euler0, dgVector& euler1) const;
73 void TransformTriplex (dgFloat32* const dst, dgInt32 dstStrideInBytes,
74 const dgFloat32* const src, dgInt32 srcStrideInBytes, dgInt32 count) const;
75
76 #ifndef _NEWTON_USE_DOUBLE
77 void TransformTriplex (dgFloat64* const dst, dgInt32 dstStrideInBytes,
78 const dgFloat64* const src, dgInt32 srcStrideInBytes, dgInt32 count) const;
79
80 void TransformTriplex (dgFloat64* const dst, dgInt32 dstStrideInBytes,
81 const dgFloat32* const src, dgInt32 srcStrideInBytes, dgInt32 count) const;
82 #endif
83
84 bool TestOrthogonal() const;
85 bool TestSymetric3x3() const;
86
87 dgMatrix operator* (const dgMatrix &B) const;
88
89
90
91
92
93 // these function can only be called when dgMatrix is a PDS matrix
94 void EigenVectors (dgVector &eigenValues, const dgMatrix& initialGuess = dgGetIdentityMatrix());
95 void EigenVectors (const dgMatrix& initialGuess = dgGetIdentityMatrix());
96 void PolarDecomposition (dgMatrix& transformMatrix, dgVector& scale, dgMatrix& stretchAxis, const dgMatrix& initialStretchAxis = dgGetIdentityMatrix()) const;
97
98 // constructor for polar composition
99 dgMatrix (const dgMatrix& transformMatrix, const dgVector& scale, const dgMatrix& stretchAxis);
100
101 dgVector m_front;
102 dgVector m_up;
103 dgVector m_right;
104 dgVector m_posit;
105
106 static dgMatrix m_zeroMatrix;
107 static dgMatrix m_identityMatrix;
108 } DG_GCC_VECTOR_ALIGMENT;
109
110
111
112 DG_INLINE dgMatrix::dgMatrix ()
113 {
114 }
115
116 DG_INLINE dgMatrix::dgMatrix (const dgFloat32* const array)
117 {
118 memcpy (&m_front.m_x, array, sizeof (dgMatrix)) ;
119 }
120
121 DG_INLINE dgMatrix::dgMatrix (const dgVector &front, const dgVector &up, const dgVector &right, const dgVector &posit)
122 :m_front (front), m_up(up), m_right(right), m_posit(posit)
123 {
124 }
125
126 DG_INLINE dgMatrix::dgMatrix (const dgVector& p, const dgVector& q)
127 :m_front(q.CompProduct4(p.BroadcastX()))
128 ,m_up(q.CompProduct4(p.BroadcastY()))
129 ,m_right(q.CompProduct4(p.BroadcastZ()))
130 ,m_posit (dgVector::m_wOne)
131 {
132 }
133
134 DG_INLINE dgMatrix::dgMatrix (const dgVector& front)
135 {
136 m_front = front;
137 if (dgAbsf (front.m_z) > dgFloat32 (0.577f)) {
138 m_right = front * dgVector (-front.m_y, front.m_z, dgFloat32(0.0f), dgFloat32(0.0f));
139 } else {
140 m_right = front * dgVector (-front.m_y, front.m_x, dgFloat32(0.0f), dgFloat32(0.0f));
141 }
142 m_right = m_right.Scale3 (dgRsqrt (m_right % m_right));
143 m_up = m_right * m_front;
144
145 m_front.m_w = dgFloat32(0.0f);
146 m_up.m_w = dgFloat32(0.0f);
147 m_right.m_w = dgFloat32(0.0f);
148 m_posit = dgVector (dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f));
149
150 dgAssert ((dgAbsf (m_front % m_front) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f));
151 dgAssert ((dgAbsf (m_up % m_up) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f));
152 dgAssert ((dgAbsf (m_right % m_right) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f));
153 dgAssert ((dgAbsf (m_right % (m_front * m_up)) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f));
154 }
155
156
157 DG_INLINE dgVector& dgMatrix::operator[] (dgInt32 i)
158 {
159 dgAssert (i < 4);
160 dgAssert (i >= 0);
161 return (&m_front)[i];
162 }
163
164 DG_INLINE const dgVector& dgMatrix::operator[] (dgInt32 i) const
165 {
166 dgAssert (i < 4);
167 dgAssert (i >= 0);
168 return (&m_front)[i];
169 }
170
171
172 DG_INLINE dgMatrix dgMatrix::Transpose () const
173 {
174 #ifdef DG_SCALAR_VECTOR_CLASS
175 return dgMatrix (dgVector (m_front.m_x, m_up.m_x, m_right.m_x, dgFloat32(0.0f)),
176 dgVector (m_front.m_y, m_up.m_y, m_right.m_y, dgFloat32(0.0f)),
177 dgVector (m_front.m_z, m_up.m_z, m_right.m_z, dgFloat32(0.0f)),
178 dgVector (dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f)));
179 #else
180 dgMatrix tmp;
181 dgVector::Transpose4x4 (tmp.m_front, tmp.m_up, tmp.m_right, tmp.m_posit, m_front, m_up, m_right, dgVector::m_wOne);
182 return tmp;
183 #endif
184 }
185
186 DG_INLINE dgMatrix dgMatrix::Transpose4X4 () const
187 {
188 #ifdef DG_SCALAR_VECTOR_CLASS
189 return dgMatrix (dgVector (m_front.m_x, m_up.m_x, m_right.m_x, m_posit.m_x),
190 dgVector (m_front.m_y, m_up.m_y, m_right.m_y, m_posit.m_y),
191 dgVector (m_front.m_z, m_up.m_z, m_right.m_z, m_posit.m_z),
192 dgVector (m_front.m_w, m_up.m_w, m_right.m_w, m_posit.m_w));
193 #else
194 dgMatrix tmp;
195 dgVector::Transpose4x4 (tmp.m_front, tmp.m_up, tmp.m_right, tmp.m_posit, m_front, m_up, m_right, m_posit);
196 return tmp;
197 #endif
198 }
199
200 DG_INLINE dgVector dgMatrix::RotateVector (const dgVector &v) const
201 {
202 return dgVector (m_front.Scale4(v.m_x) + m_up.Scale4(v.m_y) + m_right.Scale4(v.m_z));
203 }
204
205
206 DG_INLINE dgVector dgMatrix::UnrotateVector (const dgVector &v) const
207 {
208 #ifdef DG_SCALAR_VECTOR_CLASS
209 return dgVector (v % m_front, v % m_up, v % m_right, dgFloat32 (0.0f));
210 #else
211 dgVector x (v.CompProduct4(m_front));
212 dgVector y (v.CompProduct4(m_up));
213 dgVector z (v.CompProduct4(m_right));
214 dgVector w (dgFloat32 (0.0f));
215 dgVector::Transpose4x4 (x, y, z, w, x, y, z, w);
216 return x + y + z;
217
218 #endif
219 }
220
221
222 DG_INLINE dgVector dgMatrix::TransformVector (const dgVector &v) const
223 {
224 /*
225 dgVector tmp0 (v.m_x * m_front.m_x + v.m_y * m_up.m_x + v.m_z * m_right.m_x + m_posit.m_x,
226 v.m_x * m_front.m_y + v.m_y * m_up.m_y + v.m_z * m_right.m_y + m_posit.m_y,
227 v.m_x * m_front.m_z + v.m_y * m_up.m_z + v.m_z * m_right.m_z + m_posit.m_z, v.m_w);
228 reurn dgVector (m_front.Scale4(v.m_x) + m_up.Scale4(v.m_y) + m_right.Scale4(v.m_z) + m_posit.Scale4(v.m_w));
229 */
230 return dgVector (m_front.Scale4(v.m_x) + m_up.Scale4(v.m_y) + m_right.Scale4(v.m_z) + m_posit);
231 }
232
233 DG_INLINE dgVector dgMatrix::UntransformVector (const dgVector &v) const
234 {
235 return UnrotateVector(v - m_posit);
236 }
237
238 DG_INLINE dgPlane dgMatrix::TransformPlane (const dgPlane &localPlane) const
239 {
240 return dgPlane (RotateVector (localPlane), localPlane.m_w - (localPlane % UnrotateVector (m_posit)));
241 }
242
243 DG_INLINE dgPlane dgMatrix::UntransformPlane (const dgPlane &globalPlane) const
244 {
245 return dgPlane (UnrotateVector (globalPlane), globalPlane.Evalue(m_posit));
246 }
247
248 DG_INLINE void dgMatrix::EigenVectors (const dgMatrix& initialGuess)
249 {
250 dgVector eigenValues;
251 EigenVectors (eigenValues, initialGuess);
252 }
253
254
255 DG_INLINE dgMatrix dgMatrix::Inverse () const
256 {
257 return dgMatrix (dgVector (m_front.m_x, m_up.m_x, m_right.m_x, dgFloat32(0.0f)),
258 dgVector (m_front.m_y, m_up.m_y, m_right.m_y, dgFloat32(0.0f)),
259 dgVector (m_front.m_z, m_up.m_z, m_right.m_z, dgFloat32(0.0f)),
260 dgVector (- (m_posit % m_front), - (m_posit % m_up), - (m_posit % m_right), dgFloat32(1.0f)));
261 }
262
263
264 DG_INLINE bool dgMatrix::TestOrthogonal() const
265 {
266 dgVector n (m_front * m_up);
267 dgFloat32 a = m_right % m_right;
268 dgFloat32 b = m_up % m_up;
269 dgFloat32 c = m_front % m_front;
270 dgFloat32 d = n % m_right;
271
272 return (m_front[3] == dgFloat32 (0.0f)) &
273 (m_up[3] == dgFloat32 (0.0f)) &
274 (m_right[3] == dgFloat32 (0.0f)) &
275 (m_posit[3] == dgFloat32 (1.0f)) &
276 (dgAbsf(a - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)) &
277 (dgAbsf(b - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)) &
278 (dgAbsf(c - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)) &
279 (dgAbsf(d - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
280 }
281
282 DG_INLINE bool dgMatrix::TestSymetric3x3() const
283 {
284 const dgMatrix& me = *this;
285 return (dgAbsf (me[0][1] - me[1][0]) < dgFloat32 (1.0e-5f)) &&
286 (dgAbsf (me[0][2] - me[2][0]) < dgFloat32 (1.0e-5f)) &&
287 (dgAbsf (me[1][2] - me[2][1]) < dgFloat32 (1.0e-5f)) &&
288 (me[0][3] == dgFloat32 (0.0f)) &&
289 (me[1][3] == dgFloat32 (0.0f)) &&
290 (me[2][3] == dgFloat32 (0.0f)) &&
291 (me[3][0] == dgFloat32 (0.0f)) &&
292 (me[3][1] == dgFloat32 (0.0f)) &&
293 (me[3][2] == dgFloat32 (0.0f)) &&
294 (me[3][3] == dgFloat32 (1.0f));
295 }
296
297
298 DG_INLINE dgMatrix dgPitchMatrix(dgFloat32 ang)
299 {
300 dgFloat32 sinAng = dgSin (ang);
301 dgFloat32 cosAng = dgCos (ang);
302 return dgMatrix (dgVector (dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)),
303 dgVector (dgFloat32(0.0f), cosAng, sinAng, dgFloat32(0.0f)),
304 dgVector (dgFloat32(0.0f), -sinAng, cosAng, dgFloat32(0.0f)),
305 dgVector (dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f)));
306
307 }
308
309 DG_INLINE dgMatrix dgYawMatrix(dgFloat32 ang)
310 {
311 dgFloat32 sinAng = dgSin (ang);
312 dgFloat32 cosAng = dgCos (ang);
313 return dgMatrix (dgVector (cosAng, dgFloat32(0.0f), -sinAng, dgFloat32(0.0f)),
314 dgVector (dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f)),
315 dgVector (sinAng, dgFloat32(0.0f), cosAng, dgFloat32(0.0f)),
316 dgVector (dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f)));
317 }
318
319 DG_INLINE dgMatrix dgRollMatrix(dgFloat32 ang)
320 {
321 dgFloat32 sinAng = dgSin (ang);
322 dgFloat32 cosAng = dgCos (ang);
323 return dgMatrix (dgVector ( cosAng, sinAng, dgFloat32(0.0f), dgFloat32(0.0f)),
324 dgVector (-sinAng, cosAng, dgFloat32(0.0f), dgFloat32(0.0f)),
325 dgVector ( dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f)),
326 dgVector ( dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f)));
327 }
328
329
330
331 #endif
332
dgGetIdentityMatrix
const dgMatrix & dgGetIdentityMatrix()
Definition: dgMatrix.cpp:47
dgPlane
Definition: dgPlane.h:29
dgGetZeroMatrix
const dgMatrix & dgGetZeroMatrix()
Definition: dgMatrix.cpp:52
dgMatrix::Transpose4X4
dgMatrix Transpose4X4() const
Definition: dgMatrix.h:186
dgMatrix::m_front
dgVector m_front
Definition: dgMatrix.h:101
dgMatrix::operator[]
dgVector & operator[](dgInt32 i)
Definition: dgMatrix.h:157
dgPitchMatrix
DG_INLINE dgMatrix dgPitchMatrix(dgFloat32 ang)
Definition: dgMatrix.h:298
dgAbsf
DG_INLINE dgFloat32 dgAbsf(dgFloat32 x)
Definition: dgTypes.h:627
dgVector::Transpose4x4
static DG_INLINE void Transpose4x4(dgVector &dst0, dgVector &dst1, dgVector &dst2, dgVector &dst3, const dgVector &src0, const dgVector &src1, const dgVector &src2, const dgVector &src3)
Definition: dgVector.h:1003
dgMatrix::dgMatrix
dgMatrix()
Definition: dgMatrix.h:112
dgDebug.h
dgMatrix::UnrotateVector
dgVector UnrotateVector(const dgVector &v) const
Definition: dgMatrix.h:206
DG_INLINE
#define DG_INLINE
Definition: dgTypes.h:178
dgAssert
#define dgAssert(x)
Definition: dgTypes.h:158
dgFloat32
float dgFloat32
Definition: dgTypes.h:233
dgInt32
int dgInt32
Definition: dgTypes.h:218
dgMatrix::TransformVector
dgVector TransformVector(const dgVector &v) const
Definition: dgMatrix.h:222
dgVector
Definition: dgVector.h:639
dgMatrix::TestOrthogonal
bool TestOrthogonal() const
Definition: dgMatrix.h:264
dgMatrix::Symetric3by3Inverse
dgMatrix Symetric3by3Inverse() const
Definition: dgMatrix.cpp:225
DG_GCC_VECTOR_ALIGMENT
DG_MSC_VECTOR_ALIGMENT class dgMatrix DG_GCC_VECTOR_ALIGMENT
dgVector::m_z
dgFloat32 m_z
Definition: dgVector.h:1027
dgMatrix::TransformTriplex
void TransformTriplex(dgFloat32 *const dst, dgInt32 dstStrideInBytes, const dgFloat32 *const src, dgInt32 srcStrideInBytes, dgInt32 count) const
Definition: dgMatrix.cpp:109
dgVector::Scale3
DG_INLINE dgVector Scale3(dgFloat32 s) const
Definition: dgVector.h:724
dgRollMatrix
DG_INLINE dgMatrix dgRollMatrix(dgFloat32 ang)
Definition: dgMatrix.h:319
dgMatrix::EigenVectors
void EigenVectors(dgVector &eigenValues, const dgMatrix &initialGuess=dgGetIdentityMatrix())
Definition: dgMatrix.cpp:491
DG_CLASS_ALLOCATOR
#define DG_CLASS_ALLOCATOR(allocator)
Definition: dgMemory.h:60
dgQuaternion
Definition: dgQuaternion.h:31
dgFloat64
double dgFloat64
Definition: dgTypes.h:224
dgYawMatrix
DG_INLINE dgMatrix dgYawMatrix(dgFloat32 ang)
Definition: dgMatrix.h:309
dgMatrix::RotateVector
dgVector RotateVector(const dgVector &v) const
Definition: dgMatrix.h:200
dgMatrix::TransformBBox
void TransformBBox(const dgVector &p0local, const dgVector &p1local, dgVector &p0, dgVector &p1) const
Definition: dgMatrix.cpp:168
dgVector.h
dgVector::m_wOne
static dgVector m_wOne
Definition: dgVector.h:1039
DG_MSC_VECTOR_ALIGMENT
#define DG_MSC_VECTOR_ALIGMENT
Definition: dgTypes.h:194
dgMatrix::PolarDecomposition
void PolarDecomposition(dgMatrix &transformMatrix, dgVector &scale, dgMatrix &stretchAxis, const dgMatrix &initialStretchAxis=dgGetIdentityMatrix()) const
Definition: dgMatrix.cpp:339
dgRsqrt
DG_INLINE dgFloat32 dgRsqrt(dgFloat32 x)
Definition: dgTypes.h:686
dgVector::m_x
dgFloat32 m_x
Definition: dgVector.h:1025
dgMatrix::UntransformVector
dgVector UntransformVector(const dgVector &v) const
Definition: dgMatrix.h:233
dgStdafx.h
dgMatrix::TestSymetric3x3
bool TestSymetric3x3() const
Definition: dgMatrix.h:282
dgMatrix::Inverse
dgMatrix Inverse() const
Definition: dgMatrix.h:255
dgPlane::Evalue
dgFloat32 Evalue(const dgFloat32 *const point) const
Definition: dgPlane.h:98
dgMatrix::UntransformPlane
dgPlane UntransformPlane(const dgPlane &globalPlane) const
Definition: dgMatrix.h:243
dgMatrix::m_identityMatrix
static dgMatrix m_identityMatrix
Definition: dgMatrix.h:107
dgMatrix::m_right
dgVector m_right
Definition: dgMatrix.h:103
dgMatrix::CalcPitchYawRoll
void CalcPitchYawRoll(dgVector &euler0, dgVector &euler1) const
Definition: dgMatrix.cpp:267
dgMatrix::Transpose
dgMatrix Transpose() const
Definition: dgMatrix.h:172
dgSin
#define dgSin(x)
Definition: dgTypes.h:692
dgVector::CompProduct4
DG_INLINE dgVector CompProduct4(const dgVector &A) const
Definition: dgVector.h:851
dgMatrix::m_up
dgVector m_up
Definition: dgMatrix.h:102
dgMatrix::Inverse4x4
dgMatrix Inverse4x4() const
Definition: dgMatrix.cpp:182
dgMatrix::TransformPlane
dgPlane TransformPlane(const dgPlane &localPlane) const
Definition: dgMatrix.h:238
dgMatrix::m_zeroMatrix
static dgMatrix m_zeroMatrix
Definition: dgMatrix.h:106
dgMatrix
Definition: dgMatrix.h:40
dgMatrix::operator*
dgMatrix operator*(const dgMatrix &B) const
Definition: dgMatrix.cpp:99
dgMatrix::m_posit
dgVector m_posit
Definition: dgMatrix.h:104
dgPlane.h
dgVector::m_w
dgFloat32 m_w
Definition: dgVector.h:1028
dgCos
#define dgCos(x)
Definition: dgTypes.h:693
dgVector::Scale4
DG_INLINE dgVector Scale4(dgFloat32 s) const
Definition: dgVector.h:730
dgVector::m_y
dgFloat32 m_y
Definition: dgVector.h:1026