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