Newton Dynamics  4.00
dMatrix.h
1 /* Copyright (c) <2003-2019> <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 __D_MATRIX_H__
23 #define __D_MATRIX_H__
24 
25 #include "dCoreStdafx.h"
26 #include "dTypes.h"
27 #include "dDebug.h"
28 #include "dPlane.h"
29 #include "dVector.h"
30 
31 class dMatrix;
32 class dQuaternion;
33 
34 D_CORE_API const dMatrix& dGetZeroMatrix ();
35 D_CORE_API const dMatrix& dGetIdentityMatrix();
36 
37 D_MSV_NEWTON_ALIGN_32
38 class dMatrix
39 {
40  public:
41  dMatrix ();
42  dMatrix (const dFloat32* const array);
43  dMatrix (const dVector &front, const dVector &up, const dVector &right, const dVector &posit);
44  D_CORE_API dMatrix (const dQuaternion &rotation, const dVector &position);
45 
46  // 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
47  dMatrix (const dVector &front);
48 
49  // create a covariance Matrix = transpose(p) * q
50  dMatrix (const dVector& p, const dVector& q);
51 
52  dVector& operator[] (dInt32 i);
53  const dVector& operator[] (dInt32 i) const;
54 
55  dMatrix Inverse () const;
56  D_CORE_API dMatrix Inverse4x4 () const;
57  dMatrix Transpose () const;
58  dMatrix Transpose4X4 () const;
59  dVector RotateVector (const dVector &v) const;
60  dVector UnrotateVector (const dVector &v) const;
61  dVector TransformVector (const dVector &v) const;
62  dVector UntransformVector (const dVector &v) const;
63  dPlane TransformPlane (const dPlane &localPlane) const;
64  dPlane UntransformPlane (const dPlane &globalPlane) const;
65  dVector TransformVector1x4(const dVector &v) const;
66  D_CORE_API dVector SolveByGaussianElimination(const dVector &v) const;
67  D_CORE_API void TransformBBox (const dVector& p0local, const dVector& p1local, dVector& p0, dVector& p1) const;
68 
69  D_CORE_API void CalcPitchYawRoll (dVector& euler0, dVector& euler1) const;
70  D_CORE_API void TransformTriplex (
71  dFloat32* const dst, dInt32 dstStrideInBytes,
72  const dFloat32* const src, dInt32 srcStrideInBytes, dInt32 count) const;
73 
74 #ifndef D_NEWTON_USE_DOUBLE
75  D_CORE_API void TransformTriplex (
76  dFloat64* const dst, dInt32 dstStrideInBytes,
77  const dFloat64* const src, dInt32 srcStrideInBytes, dInt32 count) const;
78 
79  D_CORE_API void TransformTriplex (
80  dFloat64* const dst, dInt32 dstStrideInBytes,
81  const dFloat32* const src, dInt32 srcStrideInBytes, dInt32 count) const;
82 #endif
83 
84  bool TestIdentity() const;
85  bool TestSymetric3x3() const;
86  bool TestOrthogonal(dFloat32 tol = dFloat32 (1.0e-4f)) const;
87 
88  D_CORE_API dMatrix Multiply3X3 (const dMatrix &B) const;
89  D_CORE_API dMatrix operator* (const dMatrix &B) const;
90 
91  // these function can only be called when dMatrix is a PDS matrix
92  //void EigenVectors ();
93  D_CORE_API dVector EigenVectors ();
94  D_CORE_API void PolarDecomposition (dMatrix& transformMatrix, dVector& scale, dMatrix& stretchAxis) const;
95 
96  // constructor for polar composition
97  D_CORE_API dMatrix (const dMatrix& transformMatrix, const dVector& scale, const dMatrix& stretchAxis);
98 
99  dVector m_front;
100  dVector m_up;
101  dVector m_right;
102  dVector m_posit;
103 
104  static dMatrix m_zeroMatrix;
105  static dMatrix m_identityMatrix;
106 } D_GCC_NEWTON_ALIGN_32 ;
107 
108 D_INLINE dMatrix::dMatrix ()
109 {
110 }
111 
112 D_INLINE dMatrix::dMatrix (const dFloat32* const array)
113 {
114  memcpy (&m_front.m_x, array, sizeof (dMatrix)) ;
115 }
116 
117 D_INLINE dMatrix::dMatrix (const dVector &front, const dVector &up, const dVector &right, const dVector &posit)
118  :m_front (front), m_up(up), m_right(right), m_posit(posit)
119 {
120 }
121 
122 D_INLINE dMatrix::dMatrix (const dVector& p, const dVector& q)
123  :m_front(q * p.BroadcastX())
124  ,m_up (q * p.BroadcastY())
125  ,m_right(q * p.BroadcastZ())
126  ,m_posit (dVector::m_wOne)
127 {
128 }
129 
130 D_INLINE dMatrix::dMatrix (const dVector& front)
131  :m_front((front & dVector::m_triplexMask).Normalize())
132  ,m_posit(dVector::m_wOne)
133 {
134  if (dAbs(m_front.m_z) > dFloat32 (0.577f))
135  {
136  m_right = m_front.CrossProduct(dVector(-m_front.m_y, m_front.m_z, dFloat32(0.0f), dFloat32(0.0f)));
137  }
138  else
139  {
140  m_right = m_front.CrossProduct(dVector(-m_front.m_y, m_front.m_x, dFloat32(0.0f), dFloat32(0.0f)));
141  }
142  m_right = m_right.Normalize();
143  m_up = m_right.CrossProduct(m_front);
144  dAssert(TestOrthogonal());
145 }
146 
147 D_INLINE dVector& dMatrix::operator[] (dInt32 i)
148 {
149  dAssert (i < 4);
150  dAssert (i >= 0);
151  return (&m_front)[i];
152 }
153 
154 D_INLINE const dVector& dMatrix::operator[] (dInt32 i) const
155 {
156  dAssert (i < 4);
157  dAssert (i >= 0);
158  return (&m_front)[i];
159 }
160 
161 
162 D_INLINE dMatrix dMatrix::Transpose () const
163 {
164  dMatrix inv;
165  dVector::Transpose4x4(inv[0], inv[1], inv[2], inv[3], m_front, m_up, m_right, dVector::m_wOne);
166  return inv;
167 }
168 
169 D_INLINE dMatrix dMatrix::Transpose4X4 () const
170 {
171  dMatrix inv;
172  dVector::Transpose4x4(inv[0], inv[1], inv[2], inv[3], m_front, m_up, m_right, m_posit);
173  return inv;
174 }
175 
176 D_INLINE dVector dMatrix::RotateVector (const dVector &v) const
177 {
178  return m_front * v.BroadcastX() + m_up * v.BroadcastY() + m_right * v.BroadcastZ();
179 }
180 
181 D_INLINE dVector dMatrix::UnrotateVector (const dVector &v) const
182 {
183  return dVector ((m_front * v).AddHorizontal().GetScalar(), (m_up * v).AddHorizontal().GetScalar(), (m_right * v).AddHorizontal().GetScalar(), dFloat32 (0.0f));
184 }
185 
186 D_INLINE dVector dMatrix::TransformVector (const dVector &v) const
187 {
188  return RotateVector(v) + m_posit;
189 }
190 
191 D_INLINE dVector dMatrix::TransformVector1x4(const dVector &v) const
192 {
193  return m_front * v.BroadcastX() + m_up * v.BroadcastY() +
194  m_right * v.BroadcastZ() + m_posit * v.BroadcastW();
195 }
196 
197 D_INLINE dVector dMatrix::UntransformVector (const dVector &v) const
198 {
199  return UnrotateVector(v - m_posit) | dVector::m_wOne;
200 }
201 
202 D_INLINE dPlane dMatrix::TransformPlane (const dPlane &localPlane) const
203 {
204  return dPlane (RotateVector (localPlane), localPlane.m_w - (localPlane.DotProduct(UnrotateVector (m_posit)).GetScalar()));
205 }
206 
207 D_INLINE dPlane dMatrix::UntransformPlane (const dPlane &globalPlane) const
208 {
209  return dPlane (UnrotateVector (globalPlane), globalPlane.Evalue(m_posit));
210 }
211 
212 /*
213 D_INLINE void dMatrix::EigenVectors ()
214 {
215  dVector eigenValues;
216  EigenVectors (eigenValues);
217 }
218 */
219 
220 D_INLINE dMatrix dMatrix::Inverse () const
221 {
222  // much faster inverse
223  dMatrix inv;
224  dVector::Transpose4x4 (inv[0], inv[1], inv[2], inv[3], m_front, m_up, m_right, dVector::m_wOne);
225  inv.m_posit -= inv[0] * m_posit.BroadcastX() + inv[1] * m_posit.BroadcastY() + inv[2] * m_posit.BroadcastZ();
226  return inv;
227 }
228 
229 D_INLINE bool dMatrix::TestIdentity() const
230 {
231  const dMatrix& me = *this;
232  for (dInt32 i = 0; i < 4; i++)
233  {
234  if (me[i][i] != dFloat32 (1.0f))
235  {
236  return false;
237  }
238  for (dInt32 j = i + 1; j < 4; j++)
239  {
240  if (me[i][j] != dFloat32 (0.0f))
241  {
242  return false;
243  }
244  if (me[j][i] != dFloat32(0.0f))
245  {
246  return false;
247  }
248  }
249  }
250  return true;
251 }
252 
253 D_INLINE bool dMatrix::TestOrthogonal(dFloat32 tol) const
254 {
255  dVector n (m_front.CrossProduct(m_up));
256  dFloat32 a = m_right.DotProduct(m_right).GetScalar();
257  dFloat32 b = m_up.DotProduct(m_up).GetScalar();
258  dFloat32 c = m_front.DotProduct(m_front).GetScalar();
259  dFloat32 d = n.DotProduct(m_right).GetScalar();
260 
261 #ifdef _DEBUG
262  const dMatrix& me = *this;
263  for (dInt32 i = 0; i < 4; i++)
264  {
265  for (dInt32 j = 0; j < 4; j++)
266  {
267  dAssert(dCheckFloat(me[i][j]));
268  }
269  }
270 #endif
271 
272  return (m_front[3] == dFloat32 (0.0f)) &
273  (m_up[3] == dFloat32 (0.0f)) &
274  (m_right[3] == dFloat32 (0.0f)) &
275  (m_posit[3] == dFloat32 (1.0f)) &
276  (dAbs(a - dFloat32 (1.0f)) < tol) &
277  (dAbs(b - dFloat32 (1.0f)) < tol) &
278  (dAbs(c - dFloat32 (1.0f)) < tol) &
279  (dAbs(d - dFloat32 (1.0f)) < tol);
280 }
281 
282 D_INLINE bool dMatrix::TestSymetric3x3() const
283 {
284  const dMatrix& me = *this;
285  return (dAbs (me[0][1] - me[1][0]) < dFloat32 (1.0e-5f)) &&
286  (dAbs (me[0][2] - me[2][0]) < dFloat32 (1.0e-5f)) &&
287  (dAbs (me[1][2] - me[2][1]) < dFloat32 (1.0e-5f)) &&
288  (me[0][3] == dFloat32 (0.0f)) &&
289  (me[1][3] == dFloat32 (0.0f)) &&
290  (me[2][3] == dFloat32 (0.0f)) &&
291  (me[3][0] == dFloat32 (0.0f)) &&
292  (me[3][1] == dFloat32 (0.0f)) &&
293  (me[3][2] == dFloat32 (0.0f)) &&
294  (me[3][3] == dFloat32 (1.0f));
295 }
296 
297 D_INLINE dMatrix dPitchMatrix(dFloat32 ang)
298 {
299  dFloat32 sinAng = dSin (ang);
300  dFloat32 cosAng = dCos (ang);
301  return dMatrix (
302  dVector (dFloat32(1.0f), dFloat32(0.0f), dFloat32(0.0f), dFloat32(0.0f)),
303  dVector (dFloat32(0.0f), cosAng, sinAng, dFloat32(0.0f)),
304  dVector (dFloat32(0.0f), -sinAng, cosAng, dFloat32(0.0f)),
305  dVector (dFloat32(0.0f), dFloat32(0.0f), dFloat32(0.0f), dFloat32(1.0f)));
306 }
307 
308 D_INLINE dMatrix dYawMatrix(dFloat32 ang)
309 {
310  dFloat32 sinAng = dSin (ang);
311  dFloat32 cosAng = dCos (ang);
312  return dMatrix (
313  dVector (cosAng, dFloat32(0.0f), -sinAng, dFloat32(0.0f)),
314  dVector (dFloat32(0.0f), dFloat32(1.0f), dFloat32(0.0f), dFloat32(0.0f)),
315  dVector (sinAng, dFloat32(0.0f), cosAng, dFloat32(0.0f)),
316  dVector (dFloat32(0.0f), dFloat32(0.0f), dFloat32(0.0f), dFloat32(1.0f)));
317 }
318 
319 D_INLINE dMatrix dRollMatrix(dFloat32 ang)
320 {
321  dFloat32 sinAng = dSin (ang);
322  dFloat32 cosAng = dCos (ang);
323  return dMatrix (dVector ( cosAng, sinAng, dFloat32(0.0f), dFloat32(0.0f)),
324  dVector (-sinAng, cosAng, dFloat32(0.0f), dFloat32(0.0f)),
325  dVector ( dFloat32(0.0f), dFloat32(0.0f), dFloat32(1.0f), dFloat32(0.0f)),
326  dVector ( dFloat32(0.0f), dFloat32(0.0f), dFloat32(0.0f), dFloat32(1.0f)));
327 }
328 
329 #endif
330 
dQuaternion
Definition: dQuaternion.h:33
dVector
Definition: dVectorArmNeon.h:1104
dMatrix
Definition: dMatrix.h:39
dPlane
Definition: dPlane.h:35