/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////*****************************************************************************************************////////////////
///////////////** 																									**///////////////
//////////////**								~ Zoo Engine (Moteur 3D pour SCOL) ~								 **//////////////
/////////////**											  ~ version 1.0 ~											  **/////////////
/////////////**																										  **/////////////
//////////////**										par Pacôme DANHIEZ		  									 **//////////////
///////////////** 																									**///////////////
////////////////*****************************************************************************************************////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///																																  ///
///		FICHIER :	ZooMatrix.cpp																								  ///
///																																  ///
///		NATURE	:	Librairie matricielle en deux versions -> optimisée P3, et 100% C++											  ///
///																																  ///
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////




#include	"ZooMatrix.h"


#include	<fvec.h>



#define		Q_EPSILON (0.001)


extern const F32vec4 _MASKSIGN_;


#define _mm_ror_ps(vec,i)	\
	(((i)%4) ? (_mm_shuffle_ps(vec,vec, _MM_SHUFFLE((unsigned char)(i+3)%4,(unsigned char)(i+2)%4,(unsigned char)(i+1)%4,(unsigned char)(i+0)%4))) : (vec))
#define _mm_rol_ps(vec,i)	\
	(((i)%4) ? (_mm_shuffle_ps(vec,vec, _MM_SHUFFLE((unsigned char)(7-i)%4,(unsigned char)(6-i)%4,(unsigned char)(5-i)%4,(unsigned char)(4-i)%4))) : (vec))

#define _mm_abs_ps(vec)		_mm_andnot_ps(_MASKSIGN_,vec)
#define _mm_neg_ps(vec)		_mm_xor_ps(_MASKSIGN_,vec)


typedef unsigned long DWORD;

#define M_PI 3.14159265358979323846f

const F32vec4 _MASKSIGN_;	// - - - -

const F32vec4 _ZERONE_;		// 1 0 0 1
const F32vec4 _0FFF_;		// 0 * * *
const F32vec4 Sign_PNPN;	// + - + -
const F32vec4 Sign_NPNP;	// - + - +


// This workaround prevents the library from causing Illegal Instruction 
// Exception during initialization, if the program is executed on machine 
// that does not support SSE.
class UTILSConstatntsInit 
{
public:
	UTILSConstatntsInit() 
	{
		const struct 
		{
			float _a[4];
			DWORD _b[16];
			float _c[8];
		} INITData = 
			{
				1.0f, 0.0f, 0.0f, 1.0f,								// 1 0 0 1
				0x00000000, 0x80000000, 0x00000000, 0x80000000, 	// + - + -
				0x80000000, 0x00000000, 0x80000000, 0x00000000, 	// - + - +
				0x80000000, 0x80000000, 0x80000000, 0x80000000, 	// - - - -
				0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0x00000000, 	// 0 * * *
				0.0f, M_PI*0.5f, M_PI*0.5f, -M_PI,				// -sin cos cos sin
				-M_PI, M_PI*0.5f, M_PI*0.5f, 0.0f				// sin cos cos -sin
			};

		#define BLINDCOPY(var, source)		memcpy((void *)&(var), &(source), sizeof(__m128))
		BLINDCOPY(_ZERONE_, INITData._a[0]);
		BLINDCOPY(Sign_PNPN, INITData._b[0]);
		BLINDCOPY(Sign_NPNP, INITData._b[4]);
		BLINDCOPY(_MASKSIGN_, INITData._b[8]);
		BLINDCOPY(_0FFF_, INITData._b[12]);
		#undef BLINDCOPY
	};
} UTILSConstatnts;








///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// ZMatrix Class
///
///		- Classe des matrices 4x4
///		- Méthodes de calculs classiques
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////



void ZMatrix::PerpectiveMatrix(float fovy, float aspect, float znear, float zfar)
{
	float	cotanFOV = 1.0f / (float)tan(fovy/2.0f);

	IdentityMatrix();

	_11 = cotanFOV / aspect;
	_22 = cotanFOV;
	_33 = - (zfar+znear) / (zfar-znear);
	_34 = - 2.0f * (zfar*znear) / (zfar-znear);
	_43 = -1.0f;
	_44 = 0.0f;
}

ZMatrix PerpectiveMatrix(float fovy, float aspect, float znear, float zfar)
{
	ZMatrix ret;
	ret.PerpectiveMatrix(fovy, aspect, znear, zfar);
	return ret;
}


void ZMatrix::OrthoMatrix(float left, float right, float bottom, float top, float znear, float zfar)
{
	IdentityMatrix();

	_11 = 2.0f  / (right-left);
	_22 = 2.0f  / (top-bottom);
	_33 = -2.0f / (zfar-znear);
	_14 = (right+left) / (right-left);
	_24 = (top+bottom) / (top-bottom);
	_34 = (zfar+znear) / (zfar-znear);
}

ZMatrix OrthoMatrix(float left, float right, float bottom, float top, float znear, float zfar)
{
	ZMatrix ret;
	ret.OrthoMatrix(left, right, bottom, top, znear, zfar);
	return ret;
}



///////////////////////////////////////////////////////////////////////////
/// RotateYXZMatrix														///
///////////////////////////////////////////////////////////////////////////
//
// IMPORTANT : en SCOL les rotations ont lieu Y,X,Z, ce qui correspond à 
// effectuer R(y)*R(x)*R(z) et non pas l'inverse !!
//
void ZMatrix::RotateYXZMatrix(const float rotX, const float rotY, const float rotZ)
{
/*	float	cX = (float) cos(rotX), sX = (float) sin(rotX);
	float	cY = (float) cos(rotY), sY = (float) sin(rotY);
	float	cZ = (float) cos(rotZ), sZ = (float) sin(rotZ);

	IdentityMatrix();

	_11 = sX*sY*sZ + cY*cZ;
	_12 = sX*sY*cZ - cY*sZ;
	_13 = cX*sY;

	_21 = cX*sZ;
	_22 = cX*cZ;
	_23 = -sX;

	_31 = -sY*cZ + sX*cY*sZ;
	_32 =  sY*sZ + sX*cY*cZ;
	_33 = cX*cY;
*/
	IdentityMatrix();
	*this = ::RotateYMatrix(rotY) * ::RotateXMatrix(rotX) * ::RotateZMatrix(rotZ);
}


void ZMatrix::RotateYXZMatrix(const ZVector3 angle)
{
/*	float	cX = (float) cos(angle.x), sX = (float) sin(angle.x);
	float	cY = (float) cos(angle.y), sY = (float) sin(angle.y);
	float	cZ = (float) cos(angle.z), sZ = (float) sin(angle.z);

	IdentityMatrix();

	_11 = sX*sY*sZ + cY*cZ;
	_12 = sX*sY*cZ - cY*sZ;
	_13 = cX*sY;

	_21 = cX*sZ;
	_22 = cX*cZ;
	_23 = -sX;

	_31 = -sY*cZ + sX*cY*sZ;
	_32 =  sY*sZ + sX*cY*cZ;
	_33 = cX*cY;
*/
	IdentityMatrix();
	*this = ::RotateYMatrix(angle.y) * ::RotateXMatrix(angle.x) * ::RotateZMatrix(angle.z);
}


ZMatrix RotateYXZMatrix(const float rotX, const float rotY, const float rotZ)
{
	ZMatrix ret;
	ret.RotateYXZMatrix(rotX, rotY, rotZ);
	return ret;
}

ZMatrix RotateYXZMatrix(const ZVector3 angle)
{
	ZMatrix ret;
	ret.RotateYXZMatrix(angle);
	return ret;
}





///////////////////////////////////////////////////////////////////////////
/// RotateXMatrix														///
///////////////////////////////////////////////////////////////////////////
void ZMatrix::RotateXMatrix(const float rads) 
{
#ifndef		__OPTI_P3__

	float	fCosine = (float) cos(rads), fSine = (float) sin(rads);

	IdentityMatrix();

	_22 = +fCosine;
	_23 = -fSine;
	_32 = +fSine;
	_33 = +fCosine;

#else

	if(infoP3)
	{
		__asm
		{
			xorps	xmm0,xmm0
			mov 	eax, this;
			fld		float ptr rads
			movaps	[eax+0x10], xmm0		// clear line _L2
			fsincos
			fst		float ptr [eax+0x14]	// set element _22
			movaps	[eax+0x20], xmm0		// clear line _L3
			fstp	float ptr [eax+0x28]	// set element _33
			fst		float ptr [eax+0x18]	// set element _23
			fchs
			movaps	[eax+0x00], xmm0		// clear line _L1
			fstp	float ptr [eax+0x24]	// set element _32
			fld1
			fst		float ptr [eax+0x00]	// set element _11
			movaps	[eax+0x30], xmm0		// clear line _L4
			fstp	float ptr [eax+0x3C]	// set element _44
		}
	}
	else
	{
		float	fCosine = (float) cos(rads), fSine = (float) sin(rads);

		IdentityMatrix();

		_22 = +fCosine;
		_23 = -fSine;
		_32 = +fSine;
		_33 = +fCosine;
	}

#endif
}


ZMatrix RotateXMatrix(const float rads)
{
	ZMatrix ret;
	ret.RotateXMatrix(rads);
	return ret;
} 



///////////////////////////////////////////////////////////////////////////
/// RotateYMatrix														///
///////////////////////////////////////////////////////////////////////////
void ZMatrix::RotateYMatrix(const float rads) 
{
#ifndef		__OPTI_P3__

	float	fCosine = (float) cos(rads), fSine = (float) sin(rads);

	IdentityMatrix();

	_11 = +fCosine;
	_13 = +fSine;
	_31 = -fSine;
	_33 = +fCosine;

#else
	
	if(infoP3)
	{
		__asm
		{
			xorps	xmm0,xmm0
			mov 	eax, this
			fld		float ptr rads
			movaps	[eax+0x00], xmm0		// clear line _L1
			fsincos
			fst		float ptr [eax+0x00]	// set element _11
			movaps	[eax+0x20], xmm0		// clear line _L3
			fstp	float ptr [eax+0x28]	// set element _33
			fst		float ptr [eax+0x20]	// set element _31
			fchs
			movaps	[eax+0x10], xmm0		// clear line _L2
			fstp	float ptr [eax+0x08]	// set element _13
			fld1
			fst		float ptr [eax+0x14]	// set element _22
			movaps	[eax+0x30], xmm0		// clear line _L4
			fstp	float ptr [eax+0x3C]	// set element _44
		}
	}
	else
	{
		float	fCosine = (float) cos(rads), fSine = (float) sin(rads);

		IdentityMatrix();

		_11 = +fCosine;
		_13 = +fSine;
		_31 = -fSine;
		_33 = +fCosine;
	}

#endif
} 

ZMatrix RotateYMatrix(const float rads) 
{
	ZMatrix ret;
	ret.RotateYMatrix(rads);
	return ret;
} 



///////////////////////////////////////////////////////////////////////////
/// RotateZMatrix														///
///////////////////////////////////////////////////////////////////////////
void ZMatrix::RotateZMatrix(const float rads) 
{
#ifndef		__OPTI_P3__

	float	fCosine = (float) cos(rads), fSine = (float) sin(rads);

	IdentityMatrix();

	_11 = +fCosine;
	_12 = -fSine;
	_21 = +fSine;
	_22 = +fCosine;

#else

	if(infoP3)
	{
		__asm 
		{ 
			xorps	xmm0,xmm0
			mov 	eax, this
			fld		float ptr rads
			movaps	[eax+0x00], xmm0		// clear line _L1
			fsincos
			fst		float ptr [eax+0x00]	// set element _11
			movaps	[eax+0x10], xmm0		// clear line _L2
			fstp	float ptr [eax+0x14]	// set element _22
			fst		float ptr [eax+0x04]	// set element _12
			fchs
			movaps	[eax+0x20], xmm0		// clear line _L3
			fstp	float ptr [eax+0x10]	// set element _21
			fld1
			fst		float ptr [eax+0x28]	// set element _33
			movaps	[eax+0x30], xmm0		// clear line _L4
			fstp	float ptr [eax+0x3C]	// set element _44
		}
	}
	else
	{
		float	fCosine = (float) cos(rads), fSine = (float) sin(rads);

		IdentityMatrix();

		_11 = +fCosine;
		_12 = -fSine;
		_21 = +fSine;
		_22 = +fCosine;
	}

#endif
} 

ZMatrix RotateZMatrix(const float rads) 
{
	ZMatrix ret;
	ret.RotateZMatrix(rads);
	return ret;
} 



///////////////////////////////////////////////////////////////////////////
/// TranslateMatrix														///
///////////////////////////////////////////////////////////////////////////
ZMatrix TranslateMatrix(const float dx, const float dy, const float dz)
{
	ZMatrix ret;
	ret.IdentityMatrix();
	ret._14 = dx;
	ret._24 = dy;
	ret._34 = dz;
	return ret;
}

void ZMatrix::TranslateMatrix(const float dx, const float dy, const float dz)
{
	IdentityMatrix();
	_14 = dx;
	_24 = dy;
	_34 = dz;
}

void ZMatrix::SetTrans(const float dx, const float dy, const float dz)
{
	_14 = dx;
	_24 = dy;
	_34 = dz;
} 




///////////////////////////////////////////////////////////////////////////
/// ScaleMatrix	(3 components)											///
///////////////////////////////////////////////////////////////////////////
ZMatrix ScaleMatrix(const float a, const float b, const float c)
{
	ZMatrix ret;
	ret.IdentityMatrix();
	ret._11 = a;
	ret._22 = b;
	ret._33 = c;
	return ret;
} 

void ZMatrix::ScaleMatrix(const float a, const float b, const float c)
{
	IdentityMatrix();
	_11 = a;
	_22 = b;
	_33 = c;
} 



///////////////////////////////////////////////////////////////////////////
/// ScaleMatrix	(1 component)											///
///////////////////////////////////////////////////////////////////////////
ZMatrix ScaleMatrix(const float a)
{
	ZMatrix ret;
	ret.IdentityMatrix();
	ret._11 = ret._22 = ret._33 = a;
	return ret;
} 

void ZMatrix::ScaleMatrix(const float a)
{
	IdentityMatrix();
	_11 = _22 = _33 = a;
} 



///////////////////////////////////////////////////////////////////////////
/// Determinant															///
///////////////////////////////////////////////////////////////////////////
float ZMatrix::Determinant() 
{
#ifndef		__OPTI_P3__
	
	float	src[16];
	float	tmp[12];
	float	dst[4];

	// Copy the matrix in an array
	for (int i=0; i<16; i++) 		{	src[i] = *(((float *)&_11) + i);		}

	// 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];

	// calculate determinant
	return	( src[0]*dst[0] + src[1]*dst[1] + src[2]*dst[2] + src[3]*dst[3] );

#else

	if(infoP3)
	{
		__m128	_L1, _L2, _L3, _L4;

		_L1 = F32vec4(_41,_31,_21,_11);
		_L2 = F32vec4(_42,_32,_22,_12);
		_L3 = F32vec4(_43,_33,_23,_13);
		_L4 = F32vec4(_44,_34,_24,_14);

		__m128	Va,Vb,Vc;
		__m128	r1,r2,r3,t1,t2,sum;
		F32vec4 Det;

		// First, Let's calculate the first four minterms of the first line
		t1 = _L4;
		t2 = _mm_ror_ps(_L3,1); 
		Vc = _mm_mul_ps(t2,_mm_ror_ps(t1,0));					// V3'·V4
		Va = _mm_mul_ps(t2,_mm_ror_ps(t1,2));					// V3'·V4"
		Vb = _mm_mul_ps(t2,_mm_ror_ps(t1,3));					// V3'·V4^

		r1 = _mm_sub_ps(_mm_ror_ps(Va,1),_mm_ror_ps(Vc,2));		// V3"·V4^ - V3^·V4"
		r2 = _mm_sub_ps(_mm_ror_ps(Vb,2),_mm_ror_ps(Vb,0));		// V3^·V4' - V3'·V4^
		r3 = _mm_sub_ps(_mm_ror_ps(Va,0),_mm_ror_ps(Vc,1));		// V3'·V4" - V3"·V4'

		Va = _mm_ror_ps(_L2,1);		sum = _mm_mul_ps(Va,r1);
		Vb = _mm_ror_ps(Va,1);		sum = _mm_add_ps(sum,_mm_mul_ps(Vb,r2));
		Vc = _mm_ror_ps(Vb,1);		sum = _mm_add_ps(sum,_mm_mul_ps(Vc,r3));

		// Now we can calculate the determinant:
		Det = _mm_mul_ps(sum,_L1);
		Det = _mm_add_ps(Det,_mm_movehl_ps(Det,Det));
		Det = _mm_sub_ss(Det,_mm_shuffle_ps(Det,Det,1));
		
		return Det[0];
	}
	else
	{
		float	src[16];
		float	tmp[12];
		float	dst[4];

		// Copy the matrix in an array
		for (int i=0; i<16; i++) 		{	src[i] = *(((float *)&_11) + i);		}

		// 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];

		// calculate determinant
		return	( src[0]*dst[0] + src[1]*dst[1] + src[2]*dst[2] + src[3]*dst[3] );
	}

#endif
}



///////////////////////////////////////////////////////////////////////////
/// Inverse																///
///////////////////////////////////////////////////////////////////////////
float ZMatrix::Inverse()
{
#ifndef		__OPTI_P3__

	float dst[16];
	float tmp[12]; /* temp array for pairs */
	float src[16]; /* array of transpose source matrix */
	float det, detsave; /* determinant */
	
	// store the transposed matrix in SRC
	for (int i=0; i<4; i++) 
	{
		src[i]		= *(((float *)&_11) + (i<<2));
		src[i + 4]	= *(((float *)&_11) + (i<<2) + 1);
		src[i + 8]	= *(((float *)&_11) + (i<<2) + 2);
		src[i + 12] = *(((float *)&_11) + (i<<2) + 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
	detsave = src[0]*dst[0]+src[1]*dst[1]+src[2]*dst[2]+src[3]*dst[3];
	
	// calculate matrix inverse
	det = 1/detsave;
	for ( int j = 0; j < 16; j++)		
	{
		dst[j] *= det;
		*(((float *)&_11) + j) = dst[j];
	}
	
	return detsave;

#else

	if(infoP3)
	{
		__m128	_L1, _L2, _L3, _L4;
		_L1 = F32vec4(_41,_31,_21,_11);
		_L2 = F32vec4(_42,_32,_22,_12);
		_L3 = F32vec4(_43,_33,_23,_13);
		_L4 = F32vec4(_44,_34,_24,_14);

		__m128	Va,Vb,Vc;
		__m128	r1,r2,r3,tt,tt2;
		__m128	sum,Det,RDet;
		
		__m128	ML1, ML2, ML3, ML4;
		
		__m128	trns0,trns1,trns2,trns3;

		// Calculating the minterms for the first line.

		// _mm_ror_ps is just a macro using _mm_shuffle_ps().
		tt = _L4; 
		tt2 = _mm_ror_ps(_L3,1); 
		Vc = _mm_mul_ps(tt2,_mm_ror_ps(tt,0));					// V3'·V4
		Va = _mm_mul_ps(tt2,_mm_ror_ps(tt,2));					// V3'·V4"
		Vb = _mm_mul_ps(tt2,_mm_ror_ps(tt,3));					// V3'·V4^

		r1 = _mm_sub_ps(_mm_ror_ps(Va,1),_mm_ror_ps(Vc,2));		// V3"·V4^ - V3^·V4"
		r2 = _mm_sub_ps(_mm_ror_ps(Vb,2),_mm_ror_ps(Vb,0));		// V3^·V4' - V3'·V4^
		r3 = _mm_sub_ps(_mm_ror_ps(Va,0),_mm_ror_ps(Vc,1));		// V3'·V4" - V3"·V4'

		tt = _L2;
		Va = _mm_ror_ps(tt,1);		sum = _mm_mul_ps(Va,r1);
		Vb = _mm_ror_ps(tt,2);		sum = _mm_add_ps(sum,_mm_mul_ps(Vb,r2));
		Vc = _mm_ror_ps(tt,3);		sum = _mm_add_ps(sum,_mm_mul_ps(Vc,r3));

		// Calculating the determinant.
		Det = _mm_mul_ps(sum,_L1);
		Det = _mm_add_ps(Det,_mm_movehl_ps(Det,Det));

		ML1 = _mm_xor_ps(sum,Sign_PNPN);

		// Calculating the minterms of the second line (using previous results).
		tt = _mm_ror_ps(_L1,1);		sum = _mm_mul_ps(tt,r1);
		tt = _mm_ror_ps(tt,1);		sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2));
		tt = _mm_ror_ps(tt,1);		sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3));
		ML2 = _mm_xor_ps(sum,Sign_NPNP);

		// Testing the determinant.
		Det = _mm_sub_ss(Det,_mm_shuffle_ps(Det,Det,1));
		#ifdef ZERO_SINGULAR
			int flag = _mm_comieq_ss(Det,_mm_sub_ss(tt,tt));
			// Using _mm_sub_ss, as only the first element has to be zeroed.
		#endif

		// Calculating the minterms of the third line.
		tt = _mm_ror_ps(_L1,1);
		Va = _mm_mul_ps(tt,Vb);									// V1'·V2"
		Vb = _mm_mul_ps(tt,Vc);									// V1'·V2^
		Vc = _mm_mul_ps(tt,_L2);								// V1'·V2

		r1 = _mm_sub_ps(_mm_ror_ps(Va,1),_mm_ror_ps(Vc,2));		// V1"·V2^ - V1^·V2"
		r2 = _mm_sub_ps(_mm_ror_ps(Vb,2),_mm_ror_ps(Vb,0));		// V1^·V2' - V1'·V2^
		r3 = _mm_sub_ps(_mm_ror_ps(Va,0),_mm_ror_ps(Vc,1));		// V1'·V2" - V1"·V2'

		tt = _mm_ror_ps(_L4,1);		sum = _mm_mul_ps(tt,r1);
		tt = _mm_ror_ps(tt,1);		sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2));
		tt = _mm_ror_ps(tt,1);		sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3));
		ML3 = _mm_xor_ps(sum,Sign_PNPN);

		// Dividing is FASTER than rcp_nr! (Because rcp_nr causes many register-memory RWs).
		RDet = _mm_div_ss(_mm_load_ss((float *)&_ZERONE_), Det);
		RDet = _mm_shuffle_ps(RDet,RDet,0x00);

		// Devide the first 12 minterms with the determinant.
		ML1 = _mm_mul_ps(ML1, RDet);
		ML2 = _mm_mul_ps(ML2, RDet);
		ML3 = _mm_mul_ps(ML3, RDet);

		// Calculate the minterms of the forth line and devide by the determinant.
		tt = _mm_ror_ps(_L3,1);		sum = _mm_mul_ps(tt,r1);
		tt = _mm_ror_ps(tt,1);		sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2));
		tt = _mm_ror_ps(tt,1);		sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3));
		ML4 = _mm_xor_ps(sum,Sign_NPNP);
		ML4 = _mm_mul_ps(ML4, RDet);

		#ifdef ZERO_SINGULAR
		// Check if the matrix is inversable.
		// Uses a delayed branch here, so the test would not interfere the calculations.
		// Assuming most of the matrices are inversable, the previous calculations are 
		// not wasted. It is faster this way.
			if (flag) {
				ZeroMatrix();
				return 0.0f;
			}
		#endif

		// Now we just have to transpose the minterms matrix.
		trns0 = _mm_unpacklo_ps(ML1, ML2);
		trns1 = _mm_unpacklo_ps(ML3, ML4);
		trns2 = _mm_unpackhi_ps(ML1, ML2);
		trns3 = _mm_unpackhi_ps(ML3, ML4);
		
		_L1 = _mm_movelh_ps(trns0,trns1);
		_L2 = _mm_movehl_ps(trns1,trns0);
		_L3 = _mm_movelh_ps(trns2,trns3);
		_L4 = _mm_movehl_ps(trns3,trns2);

		
		_mm_store_ps(&_11, _L1);
		_mm_store_ps(&_12, _L2);
		_mm_store_ps(&_13, _L3);
		_mm_store_ps(&_14, _L4);

		// That's all folks!
		return *(float *)&Det;	// Det[0]
	}
	else
	{
		float dst[16];
		float tmp[12]; /* temp array for pairs */
		float src[16]; /* array of transpose source matrix */
		float det, detsave; /* determinant */
		
		// store the transposed matrix in SRC
		for (int i=0; i<4; i++) 
		{
			src[i]		= *(((float *)&_11) + (i<<2));
			src[i + 4]	= *(((float *)&_11) + (i<<2) + 1);
			src[i + 8]	= *(((float *)&_11) + (i<<2) + 2);
			src[i + 12] = *(((float *)&_11) + (i<<2) + 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
		detsave = src[0]*dst[0]+src[1]*dst[1]+src[2]*dst[2]+src[3]*dst[3];
		
		// calculate matrix inverse
		det = 1/detsave;
		for ( int j = 0; j < 16; j++)		
		{
			dst[j] *= det;
			*(((float *)&_11) + j) = dst[j];
		}
		
		return detsave;
	}

#endif
}



///////////////////////////////////////////////////////////////////////////
/// GetQuat																///
///////////////////////////////////////////////////////////////////////////
ZQuat ZMatrix::GetQuat()
{
	ZQuat	quat;
    float	trace, s;

	trace = _11 + _22 + _33 + 1.0f;

	if(trace > 0.0f)
    {
		s = (float) sqrt(trace);
		quat.w = s*0.5f;
		s = 0.5f/s;
		quat.x = (_32-_23) * s;
		quat.y = (_31-_13) * s;
		quat.z = (_21-_12) * s;
    }
	else
    {
		if( (_11>_22) && (_11>_33) )					// _11 est l'élément majorant de la diagonale
		{
			s = 0.5f / ((float)sqrt(1.0f+_11-_22-_33));
			
			quat.x = 0.5f * s;
			quat.y = (_12+_21) * s;
			quat.z = (_13+_31) * s;
			quat.w = (_23+_32) * s;
		}		
		else if( (_22>_11) && (_22>_33) )				// _22 est l'élément majorant de la diagonale
		{
			s = 0.5f / ((float)sqrt(1.0f+_22-_11-_33));
			
			quat.x = (_12+_21) * s;
			quat.y = 0.5f * s;
			quat.z = (_23+_32) * s;
			quat.w = (_13+_31) * s;
		}
		else											// _33 est l'élément majorant de la diagonale
		{
			s = 0.5f / ((float)sqrt(1.0f+_11-_22-_33));
			
			quat.x = (_13+_31) * s;
			quat.y = (_23+_32) * s;
			quat.z = 0.5f * s;
			quat.w = (_12+_21) * s;
		}
    }
	
	return quat;
}



///////////////////////////////////////////////////////////////////////////
/// GetAngles															///
///////////////////////////////////////////////////////////////////////////
ZVector3 ZMatrix::GetAnglesZXY()
{
    float	yy, xx, zz;
    float	k, rd;

	rd = (float) sqrt( _13*_13 + _23*_23 + _33*_33 );
    k  = (1.0f-0.00001f) * rd;

	if(_32 > k)
    {
		zz = 0.0f;
        xx = M_PI/2.0f;
        yy = (float) atan2(_21, _11);
    }
    else if(_32 < -k)
    {
        zz = 0.0f;
        xx = - M_PI/2.0f;
        yy = (float) atan2(-_21, _11);
    }
    else
    {
		if(rd==0)	yy = xx = zz = 0.0f;
		else
		{
			xx = (float) asin(_32/rd);
			yy = (float) atan2(-_31, _33);
			zz = (float) atan2(-_12, _22);
		}
    }

	ZVector3	result;
	
	result.x = xx * 57.2957795130f;
	result.y = yy * 57.2957795130f;
	result.z = zz * 57.2957795130f;

	return result;
}



// Retourne : A = Ry*Rx*Rz, et non pas l'inverse !!!
// C'est cohérent, car Ry rotate bien les axes qui ont déjà été rotaté par les autres
ZVector3 ZMatrix::GetAnglesYXZ()
{
    float	yy, xx, zz;
    float	k, rd;

	rd = (float) sqrt( _13*_13 + _23*_23 + _33*_33 );
	k  = (1.0f-0.00001f) * rd;

	if(_23 > k)
    {
		zz = 0.0f;
        xx = - M_PI/2.0f;
        yy = (float) atan2(-_12, _11);
    }
    else if(_23 < -k)
    {
        zz = 0.0f;
        xx = M_PI/2.0f;
        yy = (float) atan2(_12, _11);
    }
    else
    {
		if(rd==0)	yy = xx = zz = 0.0f;
		else
		{
			xx = (float) -asin(_23/rd);
			yy = (float) atan2(_13, _33);
			zz = (float) atan2(_21, _22);
		}
    }

	ZVector3	result;

	result.x = xx * 57.2957795130f;
	result.y = yy * 57.2957795130f;
	result.z = zz * 57.2957795130f;

	return result;
}








///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// ZQuat Class
///
///		- Classe des quaternions
///		- Méthodes de passage de quaternion a matrice, et vice-versa
///		- Interpolation circulaire de quaternions
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


///////////////////////////////////////////////////////////////////////////
/// Transforme un quaternion en matrice									///
///////////////////////////////////////////////////////////////////////////
ZMatrix ZQuat::GetMatrix()
{
    float s, xs, ys, zs, wxs, wys, wzs, xxs, xys, xzs, yys, yzs, zzs;

	s = 2.0f / (x*x + y*y + z*z + w*w);

	xs  = x * s;	ys = y * s;		zs = z * s;
	wxs = w * xs;	wys = w * ys;	wzs = w * zs;
	xxs = x * xs;	xys = x * ys;	xzs = x * zs;
	yys = y * ys;	yzs = y * zs;	zzs = z * zs;

	ZMatrix mat;
	
	mat._11 = 1.0f - (yys + zzs);
	mat._12 = xys - wzs;
	mat._13 = xzs + wys;

	mat._21 = xys + wzs;
	mat._22 = 1.0f - (xxs + zzs);
	mat._23 = yzs - wxs;

	mat._31 = xzs - wys;
	mat._32 = yzs + wxs;
	mat._33 = 1.0f - (xxs + yys);

	mat._41 = mat._42 = mat._43 = mat._14 = mat._24 = mat._34 = 0.0f;
	mat._44 = 1.0f;

	return mat;
}



///////////////////////////////////////////////////////////////////////////
/// retourne les angles XYZ de rotation equivalents au quaternion		///
///////////////////////////////////////////////////////////////////////////
ZVector3 ZQuat::GetAnglesYXZ()
{
	ZMatrix		mat;
	ZVector3	res;

	mat = GetMatrix();
	res = mat.GetAnglesYXZ();
	
	return res;
}



///////////////////////////////////////////////////////////////////////////
/// retourne les angles XYZ de rotation equivalents au quaternion		///
///////////////////////////////////////////////////////////////////////////
ZVector3 ZQuat::GetAnglesZXY()
{
	ZMatrix		mat;
	ZVector3	res;

	mat = GetMatrix();
	res = mat.GetAnglesZXY();
	
	return res;
}













///////////////////////////////////////////////////////////////////////////
/// Interpolation de deux quads											///
///////////////////////////////////////////////////////////////////////////
ZQuat quatSlerp(ZQuat start, ZQuat end, float t)
{
    ZQuat	r;
	
	ZQuat	startQ;  	    	    	// temp copies
    float	omega, cosOmega, sinOmega;
    float	startScale, endScale;

	startQ	 = start;
	
	cosOmega = startQ.x*end.x + startQ.y*end.y + startQ.z*end.z + startQ.w*end.w;

	// If the above dot product is negative, it would be better to
	// go between the negative of the initial and the final, so that
	// we take the shorter path.
	if ( cosOmega < 0.0f )
    {
		cosOmega  = -cosOmega;
	    startQ	  = -startQ;
    }

	if ( (1.0f + cosOmega) > Q_EPSILON )
    {
		if ( (1.0f - cosOmega) > Q_EPSILON )
    	{
			// usual case
			omega		= (float) acos(cosOmega);
			sinOmega    = (float) sin(omega);
			startScale  = (float) sin((1.0f - t)*omega) / sinOmega;
			endScale	= (float) sin(t*omega) / sinOmega;
    	}
		else
		{
			// ends very close
			startScale	= 1.0f - t;
			endScale	= t;
    	}
		
		r = startScale * startQ + endScale * end;
    }
	else
    {
		// ends nearly opposite
		r.x = -startQ.y;
		r.y =  startQ.x;
		r.z = -startQ.w;
		r.w =  startQ.z;

	    startScale	= (float) sin((0.5f - t) * M_PI);
		endScale	= (float) sin(t * M_PI);

		r = startScale * startQ + endScale * r;
    }

	return r;
}



///////////////////////////////////////////////////////////////////////////
/// Interpolation de deux vecteurs d'angles								///
///////////////////////////////////////////////////////////////////////////
ZVector3 interpAng(ZVector3 a, ZVector3 b, float tx)
{
	ZVector3	res;
	ZMatrix		ma, mb;
	ZQuat		qa, qb, qr;

	ma.IdentityMatrix();
	ma *= RotateYMatrix(0.0174532925f * a.y);
	ma *= RotateXMatrix(0.0174532925f * a.x);
	ma *= RotateZMatrix(0.0174532925f * a.z);
	qa = ma.GetQuat();

	mb.IdentityMatrix();
	mb *= RotateYMatrix(0.0174532925f * b.y);
	mb *= RotateXMatrix(0.0174532925f * b.x);
	mb *= RotateZMatrix(0.0174532925f * b.z);
	qb = mb.GetQuat();

	qr  = quatSlerp(qa, qb, tx);
	ma  = qr.GetMatrix();

	return (ma.GetAnglesYXZ());
}