path: root/Runtime/Math/Matrix4x4.cpp

#include "UnityPrefix.h"
#include "Matrix4x4.h"
#include "Matrix3x3.h"
#include "Quaternion.h"
#include "Runtime/Utilities/Utility.h"

using namespace std;

namespace
{
	Matrix4x4f CreateIdentityMatrix4x4f ()
	{
		Matrix4x4f temp; 
		temp.SetIdentity (); 
		return temp;
	}
}

const Matrix4x4f Matrix4x4f::identity = CreateIdentityMatrix4x4f ();

Matrix4x4f::Matrix4x4f (const float data[16])
{
	for (int i=0; i<16; i++)
		m_Data[i] = data[i];
}

Matrix4x4f::Matrix4x4f (const Matrix3x3f &other)
{
	m_Data[0] = other.m_Data[0];
	m_Data[1] = other.m_Data[1];
	m_Data[2] = other.m_Data[2];
	m_Data[3] = 0.0F;

	m_Data[4] = other.m_Data[3];
	m_Data[5] = other.m_Data[4];
	m_Data[6] = other.m_Data[5];
	m_Data[7] = 0.0F;

	m_Data[8] = other.m_Data[6];
	m_Data[9] = other.m_Data[7];
	m_Data[10] = other.m_Data[8];
	m_Data[11] = 0.0F;

	m_Data[12] = 0.0F;
	m_Data[13] = 0.0F;
	m_Data[14] = 0.0F;
	m_Data[15] = 1.0F;
}

Matrix4x4f& Matrix4x4f::operator = (const Matrix3x3f& other)
{
	m_Data[0] = other.m_Data[0];
	m_Data[1] = other.m_Data[1];
	m_Data[2] = other.m_Data[2];
	m_Data[3] = 0.0F;

	m_Data[4] = other.m_Data[3];
	m_Data[5] = other.m_Data[4];
	m_Data[6] = other.m_Data[5];
	m_Data[7] = 0.0F;

	m_Data[8] = other.m_Data[6];
	m_Data[9] = other.m_Data[7];
	m_Data[10] = other.m_Data[8];
	m_Data[11] = 0.0F;

	m_Data[12] = 0.0F;
	m_Data[13] = 0.0F;
	m_Data[14] = 0.0F;
	m_Data[15] = 1.0F;
	return *this;
}

bool Matrix4x4f::IsIdentity (float threshold) const
{
	if (CompareApproximately (Get (0,0),1.0f, threshold) && CompareApproximately (Get (0,1),0.0f, threshold) && CompareApproximately (Get (0,2),0.0f, threshold) && CompareApproximately (Get (0,3),0.0f, threshold) &&
	    CompareApproximately (Get (1,0),0.0f, threshold) && CompareApproximately (Get (1,1),1.0f, threshold) && CompareApproximately (Get (1,2),0.0f, threshold) && CompareApproximately (Get (1,3),0.0f, threshold) &&
	    CompareApproximately (Get (2,0),0.0f, threshold) && CompareApproximately (Get (2,1),0.0f, threshold) && CompareApproximately (Get (2,2),1.0f, threshold) && CompareApproximately (Get (2,3),0.0f, threshold) &&
   	    CompareApproximately (Get (3,0),0.0f, threshold) && CompareApproximately (Get (3,1),0.0f, threshold) && CompareApproximately (Get (3,2),0.0f, threshold) && CompareApproximately (Get (3,3),1.0f, threshold))
		return true;
	return false;
}

double Matrix4x4f::GetDeterminant () const
{
	double m00 = Get(0, 0);  double m01 = Get(0, 1);  double m02 = Get(0, 2);  double m03 = Get(0, 3);
	double m10 = Get(1, 0);  double m11 = Get(1, 1);  double m12 = Get(1, 2);  double m13 = Get(1, 3);
	double m20 = Get(2, 0);  double m21 = Get(2, 1);  double m22 = Get(2, 2);  double m23 = Get(2, 3);
	double m30 = Get(3, 0);  double m31 = Get(3, 1);  double m32 = Get(3, 2);  double m33 = Get(3, 3);

	double result =
		m03 * m12 * m21 * m30 - m02 * m13 * m21 * m30 - m03 * m11 * m22 * m30 + m01 * m13 * m22 * m30 +
		m02 * m11 * m23 * m30 - m01 * m12 * m23 * m30 - m03 * m12 * m20 * m31 + m02 * m13 * m20 * m31 +
		m03 * m10 * m22 * m31 - m00 * m13 * m22 * m31 - m02 * m10 * m23 * m31 + m00 * m12 * m23 * m31 +
		m03 * m11 * m20 * m32 - m01 * m13 * m20 * m32 - m03 * m10 * m21 * m32 + m00 * m13 * m21 * m32 +
		m01 * m10 * m23 * m32 - m00 * m11 * m23 * m32 - m02 * m11 * m20 * m33 + m01 * m12 * m20 * m33 +
		m02 * m10 * m21 * m33 - m00 * m12 * m21 * m33 - m01 * m10 * m22 * m33 + m00 * m11 * m22 * m33;
	return result;
}

Matrix4x4f& Matrix4x4f::operator *= (const Matrix4x4f& inM1)
{
	Assert(&inM1 != this);
	Matrix4x4f tmp;
	MultiplyMatrices4x4(this, &inM1, &tmp);
	*this = tmp;
	return *this;
}

void MultiplyMatrices3x4( const Matrix4x4f& lhs, const Matrix4x4f& rhs, Matrix4x4f& res)
{
	for (int i=0;i<3;i++)
	{
		res.m_Data[i]    = lhs.m_Data[i] * rhs.m_Data[0]  + lhs.m_Data[i+4] * rhs.m_Data[1]  + lhs.m_Data[i+8] * rhs.m_Data[2];//  + lhs.m_Data[i+12] * rhs.m_Data[3];
		res.m_Data[i+4]  = lhs.m_Data[i] * rhs.m_Data[4]  + lhs.m_Data[i+4] * rhs.m_Data[5]  + lhs.m_Data[i+8] * rhs.m_Data[6];//  + lhs.m_Data[i+12] * rhs.m_Data[7];
		res.m_Data[i+8]  = lhs.m_Data[i] * rhs.m_Data[8]  + lhs.m_Data[i+4] * rhs.m_Data[9]  + lhs.m_Data[i+8] * rhs.m_Data[10];// + lhs.m_Data[i+12] * rhs.m_Data[11];
		res.m_Data[i+12] = lhs.m_Data[i] * rhs.m_Data[12] + lhs.m_Data[i+4] * rhs.m_Data[13] + lhs.m_Data[i+8] * rhs.m_Data[14] + lhs.m_Data[i+12];// * rhs.m_Data[15];
	}
	
	res.m_Data[3]  = 0.0f;
	res.m_Data[7]  = 0.0f;
	res.m_Data[11] = 0.0f;
	res.m_Data[15] = 1.0f;
}


Matrix4x4f& Matrix4x4f::SetIdentity ()
{
	Get (0, 0) = 1.0;	Get (0, 1) = 0.0;	Get (0, 2) = 0.0;	Get (0, 3) = 0.0;
	Get (1, 0) = 0.0;	Get (1, 1) = 1.0;	Get (1, 2) = 0.0;	Get (1, 3) = 0.0;
	Get (2, 0) = 0.0;	Get (2, 1) = 0.0;	Get (2, 2) = 1.0;	Get (2, 3) = 0.0;
	Get (3, 0) = 0.0;	Get (3, 1) = 0.0;	Get (3, 2) = 0.0;	Get (3, 3) = 1.0;
	return *this;
}

Matrix4x4f& Matrix4x4f::SetOrthoNormalBasis (const Vector3f& inX, const Vector3f& inY, const Vector3f& inZ)
{
	Get (0, 0) = inX[0];	Get (0, 1) = inY[0];	Get (0, 2) = inZ[0];	Get (0, 3) = 0.0;
	Get (1, 0) = inX[1];	Get (1, 1) = inY[1];	Get (1, 2) = inZ[1];	Get (1, 3) = 0.0;
	Get (2, 0) = inX[2];	Get (2, 1) = inY[2];	Get (2, 2) = inZ[2];	Get (2, 3) = 0.0;
	Get (3, 0) = 0.0;		Get (3, 1) = 0.0;		Get (3, 2) = 0.0;		Get (3, 3) = 1.0;
	return *this;
}

Matrix4x4f& Matrix4x4f::SetOrthoNormalBasisInverse (const Vector3f& inX, const Vector3f& inY, const Vector3f& inZ)
{
	Get (0, 0) = inX[0];	Get (1, 0) = inY[0];	Get (2, 0) = inZ[0];	Get (3, 0) = 0.0;
	Get (0, 1) = inX[1];	Get (1, 1) = inY[1];	Get (2, 1) = inZ[1];	Get (3, 1) = 0.0;
	Get (0, 2) = inX[2];	Get (1, 2) = inY[2];	Get (2, 2) = inZ[2];	Get (3, 2) = 0.0;
	Get (0, 3) = 0.0;		Get (1, 3) = 0.0;		Get (2, 3) = 0.0;		Get (3, 3) = 1.0;
	return *this;
}

Matrix4x4f& Matrix4x4f::SetPositionAndOrthoNormalBasis (const Vector3f& inPosition, const Vector3f& inX, const Vector3f& inY, const Vector3f& inZ)
{
	Get (0, 0) = inX[0];	Get (0, 1) = inY[0];	Get (0, 2) = inZ[0];	Get (0, 3) = inPosition[0];
	Get (1, 0) = inX[1];	Get (1, 1) = inY[1];	Get (1, 2) = inZ[1];	Get (1, 3) = inPosition[1];
	Get (2, 0) = inX[2];	Get (2, 1) = inY[2];	Get (2, 2) = inZ[2];	Get (2, 3) = inPosition[2];
	Get (3, 0) = 0.0;		Get (3, 1) = 0.0;		Get (3, 2) = 0.0;		Get (3, 3) = 1.0;
	return *this;
}

Matrix4x4f& Matrix4x4f::SetScale (const Vector3f& inScale)
{
	Get (0, 0) = inScale[0];	Get (0, 1) = 0.0;			Get (0, 2) = 0.0;			Get (0, 3) = 0.0;
	Get (1, 0) = 0.0;			Get (1, 1) = inScale[1];	Get (1, 2) = 0.0;			Get (1, 3) = 0.0;
	Get (2, 0) = 0.0;			Get (2, 1) = 0.0;			Get (2, 2) = inScale[2];	Get (2, 3) = 0.0;
	Get (3, 0) = 0.0;			Get (3, 1) = 0.0;			Get (3, 2) = 0.0;			Get (3, 3) = 1.0;
	return *this;
}

Matrix4x4f& Matrix4x4f::Scale (const Vector3f& inScale)
{
	Get (0, 0) *= inScale[0];
	Get (1, 0) *= inScale[0];
	Get (2, 0) *= inScale[0];
	Get (3, 0) *= inScale[0];

	Get (0, 1) *= inScale[1];
	Get (1, 1) *= inScale[1];
	Get (2, 1) *= inScale[1];
	Get (3, 1) *= inScale[1];

	Get (0, 2) *= inScale[2];
	Get (1, 2) *= inScale[2];
	Get (2, 2) *= inScale[2];
	Get (3, 2) *= inScale[2];
	return *this;
}

Matrix4x4f& Matrix4x4f::Translate (const Vector3f& inTrans)
{
	Get (0, 3) = Get (0, 0) * inTrans[0] + Get (0, 1) * inTrans[1] + Get (0, 2) * inTrans[2] + Get (0, 3);
	Get (1, 3) = Get (1, 0) * inTrans[0] + Get (1, 1) * inTrans[1] + Get (1, 2) * inTrans[2] + Get (1, 3);
	Get (2, 3) = Get (2, 0) * inTrans[0] + Get (2, 1) * inTrans[1] + Get (2, 2) * inTrans[2] + Get (2, 3);
	Get (3, 3) = Get (3, 0) * inTrans[0] + Get (3, 1) * inTrans[1] + Get (3, 2) * inTrans[2] + Get (3, 3);
	return *this;
}

Matrix4x4f& Matrix4x4f::SetTranslate (const Vector3f& inTrans)
{
	Get (0, 0) = 1.0;	Get (0, 1) = 0.0;	Get (0, 2) = 0.0;	Get (0, 3) = inTrans[0];
	Get (1, 0) = 0.0;	Get (1, 1) = 1.0;	Get (1, 2) = 0.0;	Get (1, 3) = inTrans[1];
	Get (2, 0) = 0.0;	Get (2, 1) = 0.0;	Get (2, 2) = 1.0;	Get (2, 3) = inTrans[2];
	Get (3, 0) = 0.0;	Get (3, 1) = 0.0;	Get (3, 2) = 0.0;	Get (3, 3) = 1.0;
	return *this;
}

Matrix4x4f& Matrix4x4f::SetPerspective(
	float fovy,
	float aspect,
	float zNear,
	float zFar )
{
	float cotangent, deltaZ;
	float radians = Deg2Rad (fovy / 2.0f);
	cotangent = cos (radians) / sin (radians);
	deltaZ = zNear - zFar;
	
	Get (0,0) = cotangent / aspect;	Get (0,1) = 0.0F;      Get (0,2) = 0.0F;                    Get (0,3) = 0.0F;
	Get (1,0) = 0.0F;               Get (1,1) = cotangent; Get (1,2) = 0.0F;                    Get (1,3) = 0.0F;
	Get (2,0) = 0.0F;               Get (2,1) = 0.0F;      Get (2,2) = (zFar + zNear) / deltaZ; Get (2,3) = 2.0F * zNear * zFar / deltaZ;
	Get (3,0) = 0.0F;               Get (3,1) = 0.0F;      Get (3,2) = -1.0F;                   Get (3,3) = 0.0F;

	return *this;
}

Matrix4x4f& Matrix4x4f::SetPerspectiveCotan(
	float cotangent,
	float zNear,
	float zFar )
{
	float deltaZ = zNear - zFar;
	
	Get (0,0) = cotangent;			Get (0,1) = 0.0F;      Get (0,2) = 0.0F;                    Get (0,3) = 0.0F;
	Get (1,0) = 0.0F;               Get (1,1) = cotangent; Get (1,2) = 0.0F;                    Get (1,3) = 0.0F;
	Get (2,0) = 0.0F;               Get (2,1) = 0.0F;      Get (2,2) = (zFar + zNear) / deltaZ; Get (2,3) = 2.0F * zNear * zFar / deltaZ;
	Get (3,0) = 0.0F;               Get (3,1) = 0.0F;      Get (3,2) = -1.0F;                   Get (3,3) = 0.0F;

	return *this;
}

Matrix4x4f& Matrix4x4f::SetOrtho (
	float left,
	float right,
	float bottom,
	float top,
	float zNear,
	float zFar )
{
	SetIdentity ();

	float deltax = right - left;
	float deltay = top - bottom;
	float deltaz = zFar - zNear;

	Get(0,0) = 2.0F / deltax;
	Get(0,3) = -(right + left) / deltax;
	Get(1,1) = 2.0F / deltay;
	Get(1,3) = -(top + bottom) / deltay;
	Get(2,2) = -2.0F / deltaz;
	Get(2,3) = -(zFar + zNear) / deltaz;
	return *this;
}

Matrix4x4f& Matrix4x4f::SetFrustum (
	float left,
	float right,
	float bottom,
	float top,
	float nearval,
	float farval )
{
	float x, y, a, b, c, d, e;
	    
	x =  (2.0F * nearval) 		/ (right - left);
	y =  (2.0F * nearval) 		/ (top - bottom);
	a =  (right + left)			/ (right - left);
	b =  (top + bottom)			/ (top - bottom);
	c = -(farval + nearval)		   / (farval - nearval);
	d = -(2.0f * farval * nearval) / (farval - nearval);
	e = -1.0f;

	Get (0,0) = x;    Get (0,1) = 0.0;  Get (0,2) = a;   Get (0,3) = 0.0;
	Get (1,0) = 0.0;  Get (1,1) = y;    Get (1,2) = b;   Get (1,3) = 0.0;
	Get (2,0) = 0.0;  Get (2,1) = 0.0;  Get (2,2) = c;   Get (2,3) = d;
	Get (3,0) = 0.0;  Get (3,1) = 0.0;  Get (3,2) = e;	Get (3,3) = 0.0;
	return *this;
}



TransformType ComputeTransformType (const Matrix4x4f& matrix, float& outUniformScale, float epsilon)
{
	float lengthX = Magnitude(matrix.GetAxisX());
	float lengthY = Magnitude(matrix.GetAxisY());
	float lengthZ = Magnitude(matrix.GetAxisZ());
	float minAxis = std::min(std::min(lengthX, lengthY), lengthZ);
	float maxAxis = std::max(std::max(lengthX, lengthY), lengthZ);
	TransformType transType = kNoScaleTransform;
	outUniformScale = 1.0f;
	if (minAxis < 1.0 - epsilon || maxAxis > 1.0 + epsilon)
	{
		if (minAxis != 0.0f && maxAxis / minAxis < 1.0 + epsilon)
		{
			transType = kUniformScaleTransform;
			outUniformScale = minAxis;
		}
		else
			transType = kNonUniformScaleTransform;
	}
	return transType;
}


#define MAT(m,r,c) (m)[(c)*4+(r)]

#define RETURN_ZERO \
{ \
	for (int i=0;i<16;i++) \
		out[i] = 0.0F; \
	return false; \
}

// 4x4 matrix inversion by Gaussian reduction with partial pivoting followed by back/substitution;
// with loops manually unrolled.

#define SWAP_ROWS(a, b) { float *_tmp = a; (a)=(b); (b)=_tmp; }
bool InvertMatrix4x4_Full(const float* m, float* out)
{
   float wtmp[4][8];
   float m0, m1, m2, m3, s;
   float *r0, *r1, *r2, *r3;

   r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];

   r0[0] = MAT(m,0,0), r0[1] = MAT(m,0,1),
   r0[2] = MAT(m,0,2), r0[3] = MAT(m,0,3),
   r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0,

   r1[0] = MAT(m,1,0), r1[1] = MAT(m,1,1),
   r1[2] = MAT(m,1,2), r1[3] = MAT(m,1,3),
   r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0,

   r2[0] = MAT(m,2,0), r2[1] = MAT(m,2,1),
   r2[2] = MAT(m,2,2), r2[3] = MAT(m,2,3),
   r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0,

   r3[0] = MAT(m,3,0), r3[1] = MAT(m,3,1),
   r3[2] = MAT(m,3,2), r3[3] = MAT(m,3,3),
   r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0;

   /* choose pivot - or die */
   if (Abs(r3[0])>Abs(r2[0])) SWAP_ROWS(r3, r2);
   if (Abs(r2[0])>Abs(r1[0])) SWAP_ROWS(r2, r1);
   if (Abs(r1[0])>Abs(r0[0])) SWAP_ROWS(r1, r0);
   if (0.0F == r0[0]) RETURN_ZERO

   /* eliminate first variable     */
   m1 = r1[0]/r0[0]; m2 = r2[0]/r0[0]; m3 = r3[0]/r0[0];
   s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s;
   s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s;
   s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s;
   s = r0[4];
   if (s != 0.0F) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; }
   s = r0[5];
   if (s != 0.0F) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; }
   s = r0[6];
   if (s != 0.0F) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; }
   s = r0[7];
   if (s != 0.0F) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; }

   /* choose pivot - or die */
   if (Abs(r3[1])>Abs(r2[1])) SWAP_ROWS(r3, r2);
   if (Abs(r2[1])>Abs(r1[1])) SWAP_ROWS(r2, r1);
   if (0.0F == r1[1]) RETURN_ZERO;

   /* eliminate second variable */
   m2 = r2[1]/r1[1]; m3 = r3[1]/r1[1];
   r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2];
   r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3];
   s = r1[4]; if (0.0F != s) { r2[4] -= m2 * s; r3[4] -= m3 * s; }
   s = r1[5]; if (0.0F != s) { r2[5] -= m2 * s; r3[5] -= m3 * s; }
   s = r1[6]; if (0.0F != s) { r2[6] -= m2 * s; r3[6] -= m3 * s; }
   s = r1[7]; if (0.0F != s) { r2[7] -= m2 * s; r3[7] -= m3 * s; }

   /* choose pivot - or die */
   if (Abs(r3[2])>Abs(r2[2])) SWAP_ROWS(r3, r2);
   if (0.0F == r2[2]) RETURN_ZERO;

   /* eliminate third variable */
   m3 = r3[2]/r2[2];
   r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4],
   r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6],
   r3[7] -= m3 * r2[7];

   /* last check */
   if (0.0F == r3[3]) RETURN_ZERO;

   s = 1.0F/r3[3];             /* now back substitute row 3 */
   r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s;

   m2 = r2[3];                 /* now back substitute row 2 */
   s  = 1.0F/r2[2];
   r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2),
   r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2);
   m1 = r1[3];
   r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1,
   r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1;
   m0 = r0[3];
   r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0,
   r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0;

   m1 = r1[2];                 /* now back substitute row 1 */
   s  = 1.0F/r1[1];
   r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1),
   r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1);
   m0 = r0[2];
   r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0,
   r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0;

   m0 = r0[1];                 /* now back substitute row 0 */
   s  = 1.0F/r0[0];
   r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0),
   r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0);

   MAT(out,0,0) = r0[4]; MAT(out,0,1) = r0[5], MAT(out,0,2) = r0[6]; MAT(out,0,3) = r0[7],
   MAT(out,1,0) = r1[4]; MAT(out,1,1) = r1[5], MAT(out,1,2) = r1[6]; MAT(out,1,3) = r1[7],
   MAT(out,2,0) = r2[4]; MAT(out,2,1) = r2[5], MAT(out,2,2) = r2[6]; MAT(out,2,3) = r2[7],
   MAT(out,3,0) = r3[4]; MAT(out,3,1) = r3[5], MAT(out,3,2) = r3[6]; MAT(out,3,3) = r3[7];

   return true;
}

#undef SWAP_ROWS

// Invert 3D transformation matrix (not perspective). Adapted from graphics gems 2.
// Inverts upper left by calculating its determinant and multiplying it to the symmetric
// adjust matrix of each element. Finally deals with the translation by transforming the
// original translation using by the calculated inverse.
bool InvertMatrix4x4_General3D( const float* in, float* out )
{
	float pos, neg, t;
	float det;

	// Calculate the determinant of upper left 3x3 sub-matrix and
	// determine if the matrix is singular.
	pos = neg = 0.0;
	t =  MAT(in,0,0) * MAT(in,1,1) * MAT(in,2,2);
	if (t >= 0.0) pos += t; else neg += t;

	t =  MAT(in,1,0) * MAT(in,2,1) * MAT(in,0,2);
	if (t >= 0.0) pos += t; else neg += t;

	t =  MAT(in,2,0) * MAT(in,0,1) * MAT(in,1,2);
	if (t >= 0.0) pos += t; else neg += t;

	t = -MAT(in,2,0) * MAT(in,1,1) * MAT(in,0,2);
	if (t >= 0.0) pos += t; else neg += t;

	t = -MAT(in,1,0) * MAT(in,0,1) * MAT(in,2,2);
	if (t >= 0.0) pos += t; else neg += t;

	t = -MAT(in,0,0) * MAT(in,2,1) * MAT(in,1,2);
	if (t >= 0.0) pos += t; else neg += t;

	det = pos + neg;

	if (det*det < 1e-25)
		RETURN_ZERO;

	det = 1.0F / det;
	MAT(out,0,0) = (  (MAT(in,1,1)*MAT(in,2,2) - MAT(in,2,1)*MAT(in,1,2) )*det);
	MAT(out,0,1) = (- (MAT(in,0,1)*MAT(in,2,2) - MAT(in,2,1)*MAT(in,0,2) )*det);
	MAT(out,0,2) = (  (MAT(in,0,1)*MAT(in,1,2) - MAT(in,1,1)*MAT(in,0,2) )*det);
	MAT(out,1,0) = (- (MAT(in,1,0)*MAT(in,2,2) - MAT(in,2,0)*MAT(in,1,2) )*det);
	MAT(out,1,1) = (  (MAT(in,0,0)*MAT(in,2,2) - MAT(in,2,0)*MAT(in,0,2) )*det);
	MAT(out,1,2) = (- (MAT(in,0,0)*MAT(in,1,2) - MAT(in,1,0)*MAT(in,0,2) )*det);
	MAT(out,2,0) = (  (MAT(in,1,0)*MAT(in,2,1) - MAT(in,2,0)*MAT(in,1,1) )*det);
	MAT(out,2,1) = (- (MAT(in,0,0)*MAT(in,2,1) - MAT(in,2,0)*MAT(in,0,1) )*det);
	MAT(out,2,2) = (  (MAT(in,0,0)*MAT(in,1,1) - MAT(in,1,0)*MAT(in,0,1) )*det);

	// Do the translation part
	MAT(out,0,3) = - (MAT(in,0,3) * MAT(out,0,0) +
		MAT(in,1,3) * MAT(out,0,1) +
		MAT(in,2,3) * MAT(out,0,2) );
	MAT(out,1,3) = - (MAT(in,0,3) * MAT(out,1,0) +
		MAT(in,1,3) * MAT(out,1,1) +
		MAT(in,2,3) * MAT(out,1,2) );
	MAT(out,2,3) = - (MAT(in,0,3) * MAT(out,2,0) +
		MAT(in,1,3) * MAT(out,2,1) +
		MAT(in,2,3) * MAT(out,2,2) );
	
	MAT(out,3,0) = 0.0f;
	MAT(out,3,1) = 0.0f;
	MAT(out,3,2) = 0.0f;
	MAT(out,3,3) = 1.0f;

	return true;
}

#undef MAT
#undef RETURN_ZERO


/*
4x4 matrix inverse based on Cramer's rule. From Intel's "Streaming SIMD Extensions - Inverse of 4x4 Matrix" paper.
Seems to be about the same speed as our current one, maybe slightly faster. Less numerically robust though,
at very small numbers.

bool InvertMatrix4x4_Cramer (const float* mat, float* dst)
{
	float    tmp[12]; // temp array for pairs
	float    src[16]; // array of transpose source matrix
	float    det;     // determinant
	// transpose matrix
	for (int i = 0; i < 4; i++) {
		src[i]        = mat[i*4];
		src[i + 4]    = mat[i*4 + 1];
		src[i + 8]    = mat[i*4 + 2];
		src[i + 12]   = mat[i*4 + 3];
	}
	// calculate pairs for first 8 elements (cofactors)
	tmp[0]  = src[10] * src[15];
	tmp[1]  = src[11] * src[14];
	tmp[2]  = src[9]  * src[15];
	tmp[3]  = src[11] * src[13];
	tmp[4]  = src[9]  * src[14];
	tmp[5]  = src[10] * src[13];
	tmp[6]  = src[8]  * src[15];
	tmp[7]  = src[11] * src[12];
	tmp[8]  = src[8]  * src[14];
	tmp[9]  = src[10] * src[12];
	tmp[10] = src[8]  * src[13];
	tmp[11] = src[9]  * src[12];
	// calculate first 8 elements (cofactors)
	dst[0]  = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7];
	dst[0] -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7];
	dst[1]  = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7];
	dst[1] -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7];
	dst[2]  = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7];
	dst[2] -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7];
	dst[3]  = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6];
	dst[3] -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6];
	dst[4]  = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3];
	dst[4] -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3];
	dst[5]  = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3];
	dst[5] -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3];
	dst[6]  = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3];
	dst[6] -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3];
	dst[7]  = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2];
	dst[7] -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2];
	// calculate pairs for second 8 elements (cofactors)
	tmp[0]  = src[2]*src[7];
	tmp[1]  = src[3]*src[6];
	tmp[2]  = src[1]*src[7];
	tmp[3]  = src[3]*src[5];
	tmp[4]  = src[1]*src[6];
	tmp[5]  = src[2]*src[5];
	tmp[6]  = src[0]*src[7];
	tmp[7]  = src[3]*src[4];
	tmp[8]  = src[0]*src[6];
	tmp[9]  = src[2]*src[4];
	tmp[10] = src[0]*src[5];
	tmp[11] = src[1]*src[4];
	// calculate second 8 elements (cofactors)
	dst[8]  = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15];
	dst[8] -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15];
	dst[9]  = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15];
	dst[9] -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15];
	dst[10] = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15];
	dst[10]-= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15];
	dst[11] = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14];
	dst[11]-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14];
	dst[12] = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9];
	dst[12]-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10];
	dst[13] = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10];
	dst[13]-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8];
	dst[14] = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8];
	dst[14]-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9];
	dst[15] = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9];
	dst[15]-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8];
	// calculate determinant
	det=src[0]*dst[0]+src[1]*dst[1]+src[2]*dst[2]+src[3]*dst[3];
	// calculate matrix inverse
	if( CompareApproximately(det,0.0f) )
	{
		for (int i=0;i<16;i++)
			dst[i] = 0.0F;
		return false;
	}

	det = 1.0f/det;
	for (int j = 0; j < 16; j++)
		dst[j] *= det;

	return true;
}
*/


/*
// SSE based matrix inverse from Intel's "Streaming SIMD Extensions - Inverse of 4x4 Matrix" paper.
// Does not seem to be much faster on Core 2 Duo. Keeping it here just in case.

#include <emmintrin.h>

bool InvertMatrix4x4( const float* src, float* dst )
{
	__m128 minor0, minor1, minor2, minor3;
	__m128 row0,   row1,   row2,   row3;
	__m128 det,    tmp1;
	tmp1 = _mm_loadh_pi(_mm_loadl_pi(tmp1, (__m64*)(src)), (__m64*)(src+ 4));
	row1 = _mm_loadh_pi(_mm_loadl_pi(row1, (__m64*)(src+8)), (__m64*)(src+12));
	row0 = _mm_shuffle_ps(tmp1, row1, 0x88);
	row1 = _mm_shuffle_ps(row1, tmp1, 0xDD);
	tmp1 = _mm_loadh_pi(_mm_loadl_pi(tmp1, (__m64*)(src+ 2)), (__m64*)(src+ 6));
	row3 = _mm_loadh_pi(_mm_loadl_pi(row3, (__m64*)(src+10)), (__m64*)(src+14));
	row2 = _mm_shuffle_ps(tmp1, row3, 0x88);
	row3 = _mm_shuffle_ps(row3, tmp1, 0xDD);
	// -----------------------------------------------
	tmp1 = _mm_mul_ps(row2, row3);
	tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
	minor0 = _mm_mul_ps(row1, tmp1);
	minor1 = _mm_mul_ps(row0, tmp1);
	tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
	minor0 = _mm_sub_ps(_mm_mul_ps(row1, tmp1), minor0);
	minor1 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor1);
	minor1 = _mm_shuffle_ps(minor1, minor1, 0x4E);
	// -----------------------------------------------
	tmp1 = _mm_mul_ps(row1, row2);
	tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
	minor0 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor0);
	minor3 = _mm_mul_ps(row0, tmp1);
	tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
	minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row3, tmp1));
	minor3 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor3);
	minor3 = _mm_shuffle_ps(minor3, minor3, 0x4E);
	// -----------------------------------------------
	tmp1 = _mm_mul_ps(_mm_shuffle_ps(row1, row1, 0x4E), row3);
	tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
	row2 = _mm_shuffle_ps(row2, row2, 0x4E);
	minor0 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor0);
	minor2 = _mm_mul_ps(row0, tmp1);
	tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
	minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row2, tmp1));
	minor2 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor2);
	minor2 = _mm_shuffle_ps(minor2, minor2, 0x4E);
	// -----------------------------------------------
	tmp1 = _mm_mul_ps(row0, row1);
	tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
	minor2 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor2);
	minor3 = _mm_sub_ps(_mm_mul_ps(row2, tmp1), minor3);
	tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
	minor2 = _mm_sub_ps(_mm_mul_ps(row3, tmp1), minor2);
	minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row2, tmp1));
	// -----------------------------------------------
	tmp1 = _mm_mul_ps(row0, row3);
	tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
	minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row2, tmp1));
	minor2 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor2);
	tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
	minor1 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor1);
	minor2 = _mm_sub_ps(minor2, _mm_mul_ps(row1, tmp1));
	// -----------------------------------------------
	tmp1 = _mm_mul_ps(row0, row2);
	tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
	minor1 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor1);
	minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row1, tmp1));
	tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
	minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row3, tmp1));
	minor3 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor3);
	// -----------------------------------------------
	det = _mm_mul_ps(row0, minor0);
	det = _mm_add_ps(_mm_shuffle_ps(det, det, 0x4E), det);
	det = _mm_add_ss(_mm_shuffle_ps(det, det, 0xB1), det);
	// TODO: detect zero determinant
	tmp1 = _mm_rcp_ss(det);
	det = _mm_sub_ss(_mm_add_ss(tmp1, tmp1), _mm_mul_ss(det, _mm_mul_ss(tmp1, tmp1)));
	det = _mm_shuffle_ps(det, det, 0x00);
	minor0 = _mm_mul_ps(det, minor0);
	_mm_storel_pi((__m64*)(dst), minor0);
	_mm_storeh_pi((__m64*)(dst+2), minor0);
	minor1 = _mm_mul_ps(det, minor1);
	_mm_storel_pi((__m64*)(dst+4), minor1);
	_mm_storeh_pi((__m64*)(dst+6), minor1);
	minor2 = _mm_mul_ps(det, minor2);
	_mm_storel_pi((__m64*)(dst+ 8), minor2);
	_mm_storeh_pi((__m64*)(dst+10), minor2);
	minor3 = _mm_mul_ps(det, minor3);
	_mm_storel_pi((__m64*)(dst+12), minor3);
	_mm_storeh_pi((__m64*)(dst+14), minor3);

	return true;
}
*/


Matrix4x4f& Matrix4x4f::Transpose ()
{
	swap(Get (0,1),Get (1,0));
	swap(Get (0,2),Get (2,0));
	swap(Get (0,3),Get (3,0));
	swap(Get (1,2),Get (2,1));
	swap(Get (1,3),Get (3,1));
	swap(Get (2,3),Get (3,2));
	return *this;
}

Matrix4x4f& Matrix4x4f::Copy (const Matrix4x4f& inM) 
{
	CopyMatrix(inM.m_Data, m_Data);
	return *this;
}

void fromToRotation(const float from[3], const float to[3],float mtx[3][3]);

Matrix4x4f& Matrix4x4f::SetFromToRotation (const Vector3f& from, const Vector3f& to)
{
	float mtx[3][3];
	fromToRotation (from.GetPtr (), to.GetPtr (), mtx);
	Get (0, 0) = mtx[0][0];	Get (0, 1) = mtx[0][1];	Get (0, 2) = mtx[0][2];	Get (0, 3) = 0.0;
	Get (1, 0) = mtx[1][0];	Get (1, 1) = mtx[1][1];	Get (1, 2) = mtx[1][2];	Get (1, 3) = 0.0;
	Get (2, 0) = mtx[2][0];	Get (2, 1) = mtx[2][1];	Get (2, 2) = mtx[2][2];	Get (2, 3) = 0.0;
	Get (3, 0) = 0.0;		Get (3, 1) = 0.0;		Get (3, 2) = 0.0;		Get (3, 3) = 1.0;
	return *this;
}

bool CompareApproximately (const Matrix4x4f& lhs, const Matrix4x4f& rhs, float dist)
{
	for (int i=0;i<16;i++)
	{
		if (!CompareApproximately (lhs[i], rhs[i], dist))
			return false;
	}
	return true;
}

void Matrix4x4f::SetTR (const Vector3f& pos, const Quaternionf& q)
{
	QuaternionToMatrix (q, *this);
	m_Data[12] = pos[0];
	m_Data[13] = pos[1];
	m_Data[14] = pos[2];
}

void Matrix4x4f::SetTRS (const Vector3f& pos, const Quaternionf& q, const Vector3f& s)
{
	QuaternionToMatrix (q, *this);

	m_Data[0] *= s[0];
	m_Data[1] *= s[0];
	m_Data[2] *= s[0];

	m_Data[4] *= s[1];
	m_Data[5] *= s[1];
	m_Data[6] *= s[1];

	m_Data[8] *= s[2];
	m_Data[9] *= s[2];
	m_Data[10] *= s[2];

	m_Data[12] = pos[0];
	m_Data[13] = pos[1];
	m_Data[14] = pos[2];
}

void Matrix4x4f::SetTRInverse (const Vector3f& pos, const Quaternionf& q)
{
	QuaternionToMatrix (::Inverse (q), *this);
	Translate (Vector3f (-pos[0], -pos[1], -pos[2]));
}

void TransformPoints3x3 (const Matrix4x4f& matrix, const Vector3f* in, Vector3f* out, int count)
{
	Matrix3x3f m = Matrix3x3f(matrix);
	for (int i=0;i<count;i++)
		out[i] = m.MultiplyPoint3 (in[i]);
}

void TransformPoints3x4 (const Matrix4x4f& matrix, const Vector3f* in, Vector3f* out, int count)
{
	for (int i=0;i<count;i++)
		out[i] = matrix.MultiplyPoint3 (in[i]);
}

void TransformPoints3x3 (const Matrix4x4f& matrix, const Vector3f* in, size_t inStride, Vector3f* out, size_t outStride, int count)
{
	Matrix3x3f m = Matrix3x3f(matrix);
	for (int i=0;i<count; ++i, in = Stride (in, inStride), out = Stride (out, outStride))
	{
		*out = m.MultiplyPoint3 (*in);
	}
}

void TransformPoints3x4 (const Matrix4x4f& matrix, const Vector3f* in, size_t inStride, Vector3f* out, size_t outStride, int count)
{
	for (int i=0;i<count; ++i, in = Stride (in, inStride), out = Stride (out, outStride))
	{
		*out = matrix.MultiplyPoint3 (*in);
	}
}


#include "Matrix4x4_REF.cpp"