22 #ifndef __ND_MATRIX_H__
23 #define __ND_MATRIX_H__
25 #include "ndCoreStdafx.h"
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);
44 D_OPERATOR_NEW_AND_DELETE
47 ndMatrix (
const ndFloat32*
const array);
60 const ndVector& operator[] (ndInt32 i)
const;
63 D_CORE_API
ndMatrix Inverse4x4 ()
const;
77 D_CORE_API
void TransformTriplex (
78 ndFloat32*
const dst, ndInt32 dstStrideInBytes,
79 const ndFloat32*
const src, ndInt32 srcStrideInBytes, ndInt32 count)
const;
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;
86 D_CORE_API
void TransformTriplex (
87 ndFloat64*
const dst, ndInt32 dstStrideInBytes,
88 const ndFloat32*
const src, ndInt32 srcStrideInBytes, ndInt32 count)
const;
91 bool TestIdentity()
const;
92 bool TestSymetric3x3()
const;
93 bool TestOrthogonal(ndFloat32 tol = ndFloat32 (1.0e-4f))
const;
100 D_CORE_API
ndVector EigenVectors ();
110 } D_GCC_NEWTON_ALIGN_32 ;
112 inline ndMatrix::ndMatrix ()
116 inline ndMatrix::ndMatrix (
const ndFloat32*
const array)
118 memcpy (&m_front.m_x, array, sizeof (
ndMatrix)) ;
122 :m_front (front), m_up(up), m_right(right), m_posit(posit)
126 inline ndMatrix::~ndMatrix()
131 :m_front(q * p.BroadcastX())
132 ,m_up (q * p.BroadcastY())
133 ,m_right(q * p.BroadcastZ())
138 inline ndMatrix::ndMatrix (
const ndVector& front)
139 :m_front((front &
ndVector::m_triplexMask).Normalize())
142 if (ndAbs(m_front.m_z) > ndFloat32 (0.577f))
144 m_right = m_front.CrossProduct(
ndVector(-m_front.m_y, m_front.m_z, ndFloat32(0.0f), ndFloat32(0.0f)));
148 m_right = m_front.CrossProduct(
ndVector(-m_front.m_y, m_front.m_x, ndFloat32(0.0f), ndFloat32(0.0f)));
150 m_right = m_right.Normalize();
151 m_up = m_right.CrossProduct(m_front);
152 ndAssert(TestOrthogonal());
155 inline ndVector& ndMatrix::operator[] (ndInt32 i)
159 return (&m_front)[i];
162 inline const ndVector& ndMatrix::operator[] (ndInt32 i)
const
166 return (&m_front)[i];
169 inline ndMatrix ndMatrix::Transpose ()
const
172 ndVector::Transpose4x4(inv[0], inv[1], inv[2], inv[3], m_front, m_up, m_right, ndVector::m_wOne);
176 inline ndMatrix ndMatrix::Transpose4X4 ()
const
179 ndVector::Transpose4x4(inv[0], inv[1], inv[2], inv[3], m_front, m_up, m_right, m_posit);
185 return m_front * v.BroadcastX() + m_up * v.BroadcastY() + m_right * v.BroadcastZ();
190 return ndVector ((m_front * v).AddHorizontal().GetScalar(), (m_up * v).AddHorizontal().GetScalar(), (m_right * v).AddHorizontal().GetScalar(), ndFloat32 (0.0f));
195 return RotateVector(v) + m_posit;
200 return m_front * v.BroadcastX() + m_up * v.BroadcastY() +
201 m_right * v.BroadcastZ() + m_posit * v.BroadcastW();
206 return UnrotateVector(v - m_posit) | ndVector::m_wOne;
209 inline ndPlane ndMatrix::TransformPlane (
const ndPlane &localPlane)
const
211 return ndPlane (RotateVector (localPlane), localPlane.m_w - (localPlane.DotProduct(UnrotateVector (m_posit)).GetScalar()));
214 inline ndPlane ndMatrix::UntransformPlane (
const ndPlane &globalPlane)
const
216 return ndPlane (UnrotateVector (globalPlane), globalPlane.Evalue(m_posit));
227 inline ndMatrix ndMatrix::Inverse ()
const
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();
236 inline bool ndMatrix::TestIdentity()
const
239 for (ndInt32 i = 0; i < 4; ++i)
241 if (me[i][i] != ndFloat32 (1.0f))
245 for (ndInt32 j = i + 1; j < 4; ++j)
247 if (me[i][j] != ndFloat32 (0.0f))
251 if (me[j][i] != ndFloat32(0.0f))
260 inline bool ndMatrix::TestOrthogonal(ndFloat32 tol)
const
263 for (ndInt32 i = 0; i < 4; ++i)
265 for (ndInt32 j = 0; j < 4; ++j)
267 ndAssert(ndCheckFloat((*
this)[i][j]));
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);
292 inline bool ndMatrix::TestSymetric3x3()
const
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));