text: /* Copyright (c) <2003-2019> <Newton Game Dynamics>
  * 
  * This software is provided 'as-is', without any express or implied
  * warranty. In no event will the authors be held liable for any damages
  * arising from the use of this software.
  * 
  * Permission is granted to anyone to use this software for any purpose,
  * including commercial applications, and to alter it and redistribute it
  * freely
  */
  
  
  #ifndef __dMatrix__
  #define __dMatrix__
  
  #include "dVector.h"
  
  class dMatrix;
  class dQuaternion;
  class dQuaternion;
  
  dMatrix dGetZeroMatrix ();
  dMatrix dGetIdentityMatrix();
  
  class dMatrix
  {
  	public:
  	dMatrix ();
  	dMatrix (const dFloat* const array);
  #ifndef _NEWTON_USE_DOUBLE
  	dMatrix (const dFloat64* const array);
  #endif
  	dMatrix (const dVector &front, const dVector &up, const dVector &right, const dVector &posit);
  	dMatrix (const dQuaternion &rotation, const dVector &position);
  	dMatrix (dFloat pitch, dFloat yaw, dFloat roll, const dVector& location);
  
  	dVector& operator[] (int i);
  	const dVector& operator[] (int i) const;
  
  	dMatrix Inverse () const;
  	dMatrix Transpose () const;
  	dMatrix Transpose4X4 () const;
  	
  	dVector RotateVector (const dVector &v) const;
  	dVector UnrotateVector (const dVector &v) const;
  	dVector TransformVector (const dVector &v) const;
  	dVector UntransformVector (const dVector &v) const;
  	dVector TransformPlane (const dVector &localPlane) const;
  	dVector UntransformPlane (const dVector &globalPlane) const;
      //void GetEulerAngles(dVector& euler1, dVector& euler2, dEulerAngleOrder order = m_pitchYawRoll) const;
  
  	bool TestIdentity() const; 
  	bool TestOrthogonal() const; 
  	dMatrix Inverse4x4() const;
  	dVector RotateVector4x4 (const dVector &v) const;
  	dMatrix JacobiDiagonalization (dVector& eigenValues, const dMatrix& initialMatrix = dGetIdentityMatrix()) const;
  
  	// decompose this matrix into [this = transpose(stretchAxis) * matrix(scale) * stretchAxis * transformMatrix];
  	void PolarDecomposition (dMatrix& transformMatrix, dVector& scale, dMatrix& stretchAxis, const dMatrix& initialStretchAxis = dGetIdentityMatrix()) const;
  	
  	// constructor for polar composition
  	dMatrix (const dMatrix& transformMatrix, const dVector& scale, const dMatrix& stretchAxis);
  
  	void TransformTriplex (dFloat* const dst, int dstStrideInBytes, const dFloat* const src, int srcStrideInBytes, int count) const;
  #ifndef _NEWTON_USE_DOUBLE
  	void TransformTriplex (dFloat64* const dst, int dstStrideInBytes, const dFloat64* const src, int srcStrideInBytes, int count) const;
  #endif
  
  	dMatrix operator* (const dMatrix & B) const;
  
  	//void Trace () const
  	//{
  	//	dTrace (("(%ff, %ff, %ff, %ff)\n", m_front.m_x, m_front.m_y, m_front.m_z, m_front.m_w));
  	//	dTrace (("(%ff, %ff, %ff, %ff)\n", m_up.m_x, m_up.m_y, m_up.m_z, m_up.m_w));
  	//	dTrace (("(%ff, %ff, %ff, %ff)\n", m_right.m_x, m_right.m_y, m_right.m_z, m_right.m_w));
  	//	dTrace (("(%ff, %ff, %ff, %ff)\n", m_posit.m_x, m_posit.m_y, m_posit.m_z, m_posit.m_w));
  	//}
  
  
  	bool SanityCheck() const;
  
  	dVector m_front;
  	dVector m_up;
  	dVector m_right;
  	dVector m_posit;
  };
  
  inline dMatrix::dMatrix ()
  {
  }
  
  inline dMatrix::dMatrix (const dVector &front, const dVector &up, const dVector &right,	const dVector &posit)
  	:m_front (front), m_up(up), m_right(right), m_posit(posit)
  {
  }
  
  inline dMatrix::dMatrix (const dFloat* const array)
  {
  	memcpy (&(*this)[0][0], array, sizeof (dMatrix));
  }
  
  #ifndef _NEWTON_USE_DOUBLE
  inline dMatrix::dMatrix (const dFloat64* const array)
  {
  	dFloat* const ptr = &(*this)[0][0];
  	for (int i = 0; i < 16; i ++) {
  		ptr[i] = dFloat (array[i]);
  	}
  }
  #endif
  
  inline dVector& dMatrix::operator[] (int  i)
  {
  	return (&m_front)[i];
  }
  
  inline const dVector& dMatrix::operator[] (int  i) const
  {
  	return (&m_front)[i];
  }
  
  dMatrix dRollMatrix(dFloat ang);
  dMatrix dYawMatrix(dFloat ang);
  dMatrix dPitchMatrix(dFloat ang);
  dMatrix dGrammSchmidt(const dVector& dir);
  
  
  class dSpatialMatrix
  {
  	public:
  	inline dSpatialMatrix()
  	{
  	}
  
  	inline dSpatialMatrix(dFloat val)
  	{
  		for (int i = 0; i < 6; i++) {
  			m_rows[i] = dSpatialVector(val);
  		}
  	}
  
  	inline dSpatialVector& operator[] (int i)
  	{
  		dAssert(i < 6);
  		dAssert(i >= 0);
  		return m_rows[i];
  	}
  
  	inline const dSpatialVector& operator[] (int i) const
  	{
  		dAssert(i < 6);
  		dAssert(i >= 0);
  		return m_rows[i];
  	}
  
  	dSpatialMatrix Inverse(int rows) const;
  
  	inline dSpatialVector VectorTimeMatrix(const dSpatialVector& jacobian) const
  	{
  		dSpatialVector tmp(m_rows[0].Scale(jacobian[0]));
  		for (int i = 1; i < 6; i++) {
  			tmp = tmp + m_rows[i].Scale(jacobian[i]);
  		}
  		return tmp;
  	}
  
  	inline dSpatialVector VectorTimeMatrix(const dSpatialVector& jacobian, int dof) const
  	{
  		dSpatialVector tmp(0.0f);
  		for (int i = 0; i < dof; i++) {
  			tmp = tmp + m_rows[i].Scale(jacobian[i]);
  		}
  		return tmp;
  	}
  
  	dSpatialVector m_rows[6];
  };
  
  
  #endif
  
  