22 #ifndef __D_MATRIX_H__
23 #define __D_MATRIX_H__
25 #include "dCoreStdafx.h"
34 D_CORE_API
const dMatrix& dGetZeroMatrix ();
35 D_CORE_API
const dMatrix& dGetIdentityMatrix();
42 dMatrix (
const dFloat32*
const array);
53 const dVector& operator[] (dInt32 i)
const;
56 D_CORE_API
dMatrix Inverse4x4 ()
const;
64 dPlane UntransformPlane (
const dPlane &globalPlane)
const;
66 D_CORE_API
dVector SolveByGaussianElimination(
const dVector &v)
const;
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;
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;
79 D_CORE_API
void TransformTriplex (
80 dFloat64*
const dst, dInt32 dstStrideInBytes,
81 const dFloat32*
const src, dInt32 srcStrideInBytes, dInt32 count)
const;
84 bool TestIdentity()
const;
85 bool TestSymetric3x3()
const;
86 bool TestOrthogonal(dFloat32 tol = dFloat32 (1.0e-4f))
const;
93 D_CORE_API
dVector EigenVectors ();
94 D_CORE_API
void PolarDecomposition (
dMatrix& transformMatrix,
dVector& scale,
dMatrix& stretchAxis)
const;
105 static dMatrix m_identityMatrix;
106 } D_GCC_NEWTON_ALIGN_32 ;
108 D_INLINE dMatrix::dMatrix ()
112 D_INLINE dMatrix::dMatrix (
const dFloat32*
const array)
114 memcpy (&m_front.m_x, array, sizeof (
dMatrix)) ;
118 :m_front (front), m_up(up), m_right(right), m_posit(posit)
123 :m_front(q * p.BroadcastX())
124 ,m_up (q * p.BroadcastY())
125 ,m_right(q * p.BroadcastZ())
130 D_INLINE dMatrix::dMatrix (
const dVector& front)
131 :m_front((front &
dVector::m_triplexMask).Normalize())
134 if (dAbs(m_front.m_z) > dFloat32 (0.577f))
136 m_right = m_front.CrossProduct(
dVector(-m_front.m_y, m_front.m_z, dFloat32(0.0f), dFloat32(0.0f)));
140 m_right = m_front.CrossProduct(
dVector(-m_front.m_y, m_front.m_x, dFloat32(0.0f), dFloat32(0.0f)));
142 m_right = m_right.Normalize();
143 m_up = m_right.CrossProduct(m_front);
144 dAssert(TestOrthogonal());
147 D_INLINE
dVector& dMatrix::operator[] (dInt32 i)
151 return (&m_front)[i];
154 D_INLINE
const dVector& dMatrix::operator[] (dInt32 i)
const
158 return (&m_front)[i];
162 D_INLINE
dMatrix dMatrix::Transpose ()
const
165 dVector::Transpose4x4(inv[0], inv[1], inv[2], inv[3], m_front, m_up, m_right, dVector::m_wOne);
169 D_INLINE
dMatrix dMatrix::Transpose4X4 ()
const
172 dVector::Transpose4x4(inv[0], inv[1], inv[2], inv[3], m_front, m_up, m_right, m_posit);
178 return m_front * v.BroadcastX() + m_up * v.BroadcastY() + m_right * v.BroadcastZ();
183 return dVector ((m_front * v).AddHorizontal().GetScalar(), (m_up * v).AddHorizontal().GetScalar(), (m_right * v).AddHorizontal().GetScalar(), dFloat32 (0.0f));
186 D_INLINE
dVector dMatrix::TransformVector (
const dVector &v)
const
188 return RotateVector(v) + m_posit;
191 D_INLINE
dVector dMatrix::TransformVector1x4(
const dVector &v)
const
193 return m_front * v.BroadcastX() + m_up * v.BroadcastY() +
194 m_right * v.BroadcastZ() + m_posit * v.BroadcastW();
197 D_INLINE
dVector dMatrix::UntransformVector (
const dVector &v)
const
199 return UnrotateVector(v - m_posit) | dVector::m_wOne;
202 D_INLINE
dPlane dMatrix::TransformPlane (
const dPlane &localPlane)
const
204 return dPlane (RotateVector (localPlane), localPlane.m_w - (localPlane.DotProduct(UnrotateVector (m_posit)).GetScalar()));
207 D_INLINE
dPlane dMatrix::UntransformPlane (
const dPlane &globalPlane)
const
209 return dPlane (UnrotateVector (globalPlane), globalPlane.Evalue(m_posit));
220 D_INLINE
dMatrix dMatrix::Inverse ()
const
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();
229 D_INLINE
bool dMatrix::TestIdentity()
const
232 for (dInt32 i = 0; i < 4; i++)
234 if (me[i][i] != dFloat32 (1.0f))
238 for (dInt32 j = i + 1; j < 4; j++)
240 if (me[i][j] != dFloat32 (0.0f))
244 if (me[j][i] != dFloat32(0.0f))
253 D_INLINE
bool dMatrix::TestOrthogonal(dFloat32 tol)
const
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();
263 for (dInt32 i = 0; i < 4; i++)
265 for (dInt32 j = 0; j < 4; j++)
267 dAssert(dCheckFloat(me[i][j]));
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);
282 D_INLINE
bool dMatrix::TestSymetric3x3()
const
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));
297 D_INLINE
dMatrix dPitchMatrix(dFloat32 ang)
299 dFloat32 sinAng = dSin (ang);
300 dFloat32 cosAng = dCos (ang);
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)));
308 D_INLINE
dMatrix dYawMatrix(dFloat32 ang)
310 dFloat32 sinAng = dSin (ang);
311 dFloat32 cosAng = dCos (ang);
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)));
319 D_INLINE
dMatrix dRollMatrix(dFloat32 ang)
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)));