/******************************************************************************

 @File         PVRTMatrixX.cpp

 @Title        

 @Copyright    Copyright (C)  Imagination Technologies Limited.

 @Platform     ANSI compatible

 @Description  Set of mathematical functions involving matrices, vectors and
               quaternions.

******************************************************************************/
#include "PVRTContext.h"
#include <math.h>
#include <string.h>

#include "PVRTFixedPoint.h"
#include "PVRTMatrix.h"



/****************************************************************************
** Constants
****************************************************************************/
static const PVRTMATRIXx	c_mIdentity = {
	{
	PVRTF2X(1.0f), PVRTF2X(0.0f), PVRTF2X(0.0f), PVRTF2X(0.0f),
	PVRTF2X(0.0f), PVRTF2X(1.0f), PVRTF2X(0.0f), PVRTF2X(0.0f),
	PVRTF2X(0.0f), PVRTF2X(0.0f), PVRTF2X(1.0f), PVRTF2X(0.0f),
	PVRTF2X(0.0f), PVRTF2X(0.0f), PVRTF2X(0.0f), PVRTF2X(1.0f)
	}
};


/****************************************************************************
** Functions
****************************************************************************/

/*!***************************************************************************
 @Function			PVRTMatrixIdentityX
 @Output			mOut	Set to identity
 @Description		Reset matrix to identity matrix.
*****************************************************************************/
void PVRTMatrixIdentityX(PVRTMATRIXx &mOut)
{
	mOut.f[ 0]=PVRTF2X(1.0f);	mOut.f[ 4]=PVRTF2X(0.0f);	mOut.f[ 8]=PVRTF2X(0.0f);	mOut.f[12]=PVRTF2X(0.0f);
	mOut.f[ 1]=PVRTF2X(0.0f);	mOut.f[ 5]=PVRTF2X(1.0f);	mOut.f[ 9]=PVRTF2X(0.0f);	mOut.f[13]=PVRTF2X(0.0f);
	mOut.f[ 2]=PVRTF2X(0.0f);	mOut.f[ 6]=PVRTF2X(0.0f);	mOut.f[10]=PVRTF2X(1.0f);	mOut.f[14]=PVRTF2X(0.0f);
	mOut.f[ 3]=PVRTF2X(0.0f);	mOut.f[ 7]=PVRTF2X(0.0f);	mOut.f[11]=PVRTF2X(0.0f);	mOut.f[15]=PVRTF2X(1.0f);
}

/*!***************************************************************************
 @Function			PVRTMatrixMultiplyX
 @Output			mOut	Result of mA x mB
 @Input				mA		First operand
 @Input				mB		Second operand
 @Description		Multiply mA by mB and assign the result to mOut
					(mOut = p1 * p2). A copy of the result matrix is done in
					the function because mOut can be a parameter mA or mB.
					The fixed-point shift could be performed after adding
					all four intermediate results together however this might
					cause some overflow issues.
****************************************************************************/
void PVRTMatrixMultiplyX(
	PVRTMATRIXx			&mOut,
	const PVRTMATRIXx	&mA,
	const PVRTMATRIXx	&mB)
{
	PVRTMATRIXx mRet;

	/* Perform calculation on a dummy matrix (mRet) */
	mRet.f[ 0] = PVRTXMUL(mA.f[ 0], mB.f[ 0]) + PVRTXMUL(mA.f[ 1], mB.f[ 4]) + PVRTXMUL(mA.f[ 2], mB.f[ 8]) + PVRTXMUL(mA.f[ 3], mB.f[12]);
	mRet.f[ 1] = PVRTXMUL(mA.f[ 0], mB.f[ 1]) + PVRTXMUL(mA.f[ 1], mB.f[ 5]) + PVRTXMUL(mA.f[ 2], mB.f[ 9]) + PVRTXMUL(mA.f[ 3], mB.f[13]);
	mRet.f[ 2] = PVRTXMUL(mA.f[ 0], mB.f[ 2]) + PVRTXMUL(mA.f[ 1], mB.f[ 6]) + PVRTXMUL(mA.f[ 2], mB.f[10]) + PVRTXMUL(mA.f[ 3], mB.f[14]);
	mRet.f[ 3] = PVRTXMUL(mA.f[ 0], mB.f[ 3]) + PVRTXMUL(mA.f[ 1], mB.f[ 7]) + PVRTXMUL(mA.f[ 2], mB.f[11]) + PVRTXMUL(mA.f[ 3], mB.f[15]);

	mRet.f[ 4] = PVRTXMUL(mA.f[ 4], mB.f[ 0]) + PVRTXMUL(mA.f[ 5], mB.f[ 4]) + PVRTXMUL(mA.f[ 6], mB.f[ 8]) + PVRTXMUL(mA.f[ 7], mB.f[12]);
	mRet.f[ 5] = PVRTXMUL(mA.f[ 4], mB.f[ 1]) + PVRTXMUL(mA.f[ 5], mB.f[ 5]) + PVRTXMUL(mA.f[ 6], mB.f[ 9]) + PVRTXMUL(mA.f[ 7], mB.f[13]);
	mRet.f[ 6] = PVRTXMUL(mA.f[ 4], mB.f[ 2]) + PVRTXMUL(mA.f[ 5], mB.f[ 6]) + PVRTXMUL(mA.f[ 6], mB.f[10]) + PVRTXMUL(mA.f[ 7], mB.f[14]);
	mRet.f[ 7] = PVRTXMUL(mA.f[ 4], mB.f[ 3]) + PVRTXMUL(mA.f[ 5], mB.f[ 7]) + PVRTXMUL(mA.f[ 6], mB.f[11]) + PVRTXMUL(mA.f[ 7], mB.f[15]);

	mRet.f[ 8] = PVRTXMUL(mA.f[ 8], mB.f[ 0]) + PVRTXMUL(mA.f[ 9], mB.f[ 4]) + PVRTXMUL(mA.f[10], mB.f[ 8]) + PVRTXMUL(mA.f[11], mB.f[12]);
	mRet.f[ 9] = PVRTXMUL(mA.f[ 8], mB.f[ 1]) + PVRTXMUL(mA.f[ 9], mB.f[ 5]) + PVRTXMUL(mA.f[10], mB.f[ 9]) + PVRTXMUL(mA.f[11], mB.f[13]);
	mRet.f[10] = PVRTXMUL(mA.f[ 8], mB.f[ 2]) + PVRTXMUL(mA.f[ 9], mB.f[ 6]) + PVRTXMUL(mA.f[10], mB.f[10]) + PVRTXMUL(mA.f[11], mB.f[14]);
	mRet.f[11] = PVRTXMUL(mA.f[ 8], mB.f[ 3]) + PVRTXMUL(mA.f[ 9], mB.f[ 7]) + PVRTXMUL(mA.f[10], mB.f[11]) + PVRTXMUL(mA.f[11], mB.f[15]);

	mRet.f[12] = PVRTXMUL(mA.f[12], mB.f[ 0]) + PVRTXMUL(mA.f[13], mB.f[ 4]) + PVRTXMUL(mA.f[14], mB.f[ 8]) + PVRTXMUL(mA.f[15], mB.f[12]);
	mRet.f[13] = PVRTXMUL(mA.f[12], mB.f[ 1]) + PVRTXMUL(mA.f[13], mB.f[ 5]) + PVRTXMUL(mA.f[14], mB.f[ 9]) + PVRTXMUL(mA.f[15], mB.f[13]);
	mRet.f[14] = PVRTXMUL(mA.f[12], mB.f[ 2]) + PVRTXMUL(mA.f[13], mB.f[ 6]) + PVRTXMUL(mA.f[14], mB.f[10]) + PVRTXMUL(mA.f[15], mB.f[14]);
	mRet.f[15] = PVRTXMUL(mA.f[12], mB.f[ 3]) + PVRTXMUL(mA.f[13], mB.f[ 7]) + PVRTXMUL(mA.f[14], mB.f[11]) + PVRTXMUL(mA.f[15], mB.f[15]);

	/* Copy result in pResultMatrix */
	mOut = mRet;
}

/*!***************************************************************************
 @Function Name		PVRTMatrixTranslationX
 @Output			mOut	Translation matrix
 @Input				fX		X component of the translation
 @Input				fY		Y component of the translation
 @Input				fZ		Z component of the translation
 @Description		Build a transaltion matrix mOut using fX, fY and fZ.
*****************************************************************************/
void PVRTMatrixTranslationX(
	PVRTMATRIXx	&mOut,
	const int	fX,
	const int	fY,
	const int	fZ)
{
	mOut.f[ 0]=PVRTF2X(1.0f);	mOut.f[ 4]=PVRTF2X(0.0f);	mOut.f[ 8]=PVRTF2X(0.0f);	mOut.f[12]=fX;
	mOut.f[ 1]=PVRTF2X(0.0f);	mOut.f[ 5]=PVRTF2X(1.0f);	mOut.f[ 9]=PVRTF2X(0.0f);	mOut.f[13]=fY;
	mOut.f[ 2]=PVRTF2X(0.0f);	mOut.f[ 6]=PVRTF2X(0.0f);	mOut.f[10]=PVRTF2X(1.0f);	mOut.f[14]=fZ;
	mOut.f[ 3]=PVRTF2X(0.0f);	mOut.f[ 7]=PVRTF2X(0.0f);	mOut.f[11]=PVRTF2X(0.0f);	mOut.f[15]=PVRTF2X(1.0f);
}

/*!***************************************************************************
 @Function Name		PVRTMatrixScalingX
 @Output			mOut	Scale matrix
 @Input				fX		X component of the scaling
 @Input				fY		Y component of the scaling
 @Input				fZ		Z component of the scaling
 @Description		Build a scale matrix mOut using fX, fY and fZ.
*****************************************************************************/
void PVRTMatrixScalingX(
	PVRTMATRIXx	&mOut,
	const int	fX,
	const int	fY,
	const int	fZ)
{
	mOut.f[ 0]=fX;				mOut.f[ 4]=PVRTF2X(0.0f);	mOut.f[ 8]=PVRTF2X(0.0f);	mOut.f[12]=PVRTF2X(0.0f);
	mOut.f[ 1]=PVRTF2X(0.0f);	mOut.f[ 5]=fY;				mOut.f[ 9]=PVRTF2X(0.0f);	mOut.f[13]=PVRTF2X(0.0f);
	mOut.f[ 2]=PVRTF2X(0.0f);	mOut.f[ 6]=PVRTF2X(0.0f);	mOut.f[10]=fZ;				mOut.f[14]=PVRTF2X(0.0f);
	mOut.f[ 3]=PVRTF2X(0.0f);	mOut.f[ 7]=PVRTF2X(0.0f);	mOut.f[11]=PVRTF2X(0.0f);	mOut.f[15]=PVRTF2X(1.0f);
}

/*!***************************************************************************
 @Function Name		PVRTMatrixRotationXX
 @Output			mOut	Rotation matrix
 @Input				fAngle	Angle of the rotation
 @Description		Create an X rotation matrix mOut.
*****************************************************************************/
void PVRTMatrixRotationXX(
	PVRTMATRIXx	&mOut,
	const int	fAngle)
{
	int		fCosine, fSine;

    /* Precompute cos and sin */
#if defined(BUILD_DX9) || defined(BUILD_D3DM) || defined(BUILD_DX10)
	fCosine	= PVRTXCOS(-fAngle);
    fSine	= PVRTXSIN(-fAngle);
#else
	fCosine	= PVRTXCOS(fAngle);
    fSine	= PVRTXSIN(fAngle);
#endif

	/* Create the trigonometric matrix corresponding to X Rotation */
	mOut.f[ 0]=PVRTF2X(1.0f);	mOut.f[ 4]=PVRTF2X(0.0f);		mOut.f[ 8]=PVRTF2X(0.0f);		mOut.f[12]=PVRTF2X(0.0f);
	mOut.f[ 1]=PVRTF2X(0.0f);	mOut.f[ 5]=fCosine;				mOut.f[ 9]=fSine;				mOut.f[13]=PVRTF2X(0.0f);
	mOut.f[ 2]=PVRTF2X(0.0f);	mOut.f[ 6]=-fSine;				mOut.f[10]=fCosine;				mOut.f[14]=PVRTF2X(0.0f);
	mOut.f[ 3]=PVRTF2X(0.0f);	mOut.f[ 7]=PVRTF2X(0.0f);		mOut.f[11]=PVRTF2X(0.0f);		mOut.f[15]=PVRTF2X(1.0f);
}

/*!***************************************************************************
 @Function Name		PVRTMatrixRotationYX
 @Output			mOut	Rotation matrix
 @Input				fAngle	Angle of the rotation
 @Description		Create an Y rotation matrix mOut.
*****************************************************************************/
void PVRTMatrixRotationYX(
	PVRTMATRIXx	&mOut,
	const int	fAngle)
{
	int		fCosine, fSine;

	/* Precompute cos and sin */
#if defined(BUILD_DX9) || defined(BUILD_D3DM) || defined(BUILD_DX10)
	fCosine	= PVRTXCOS(-fAngle);
    fSine	= PVRTXSIN(-fAngle);
#else
	fCosine	= PVRTXCOS(fAngle);
    fSine	= PVRTXSIN(fAngle);
#endif

	/* Create the trigonometric matrix corresponding to Y Rotation */
	mOut.f[ 0]=fCosine;				mOut.f[ 4]=PVRTF2X(0.0f);	mOut.f[ 8]=-fSine;				mOut.f[12]=PVRTF2X(0.0f);
	mOut.f[ 1]=PVRTF2X(0.0f);		mOut.f[ 5]=PVRTF2X(1.0f);	mOut.f[ 9]=PVRTF2X(0.0f);		mOut.f[13]=PVRTF2X(0.0f);
	mOut.f[ 2]=fSine;				mOut.f[ 6]=PVRTF2X(0.0f);	mOut.f[10]=fCosine;				mOut.f[14]=PVRTF2X(0.0f);
	mOut.f[ 3]=PVRTF2X(0.0f);		mOut.f[ 7]=PVRTF2X(0.0f);	mOut.f[11]=PVRTF2X(0.0f);		mOut.f[15]=PVRTF2X(1.0f);
}

/*!***************************************************************************
 @Function Name		PVRTMatrixRotationZX
 @Output			mOut	Rotation matrix
 @Input				fAngle	Angle of the rotation
 @Description		Create an Z rotation matrix mOut.
*****************************************************************************/
void PVRTMatrixRotationZX(
	PVRTMATRIXx	&mOut,
	const int	fAngle)
{
	int		fCosine, fSine;

	/* Precompute cos and sin */
#if defined(BUILD_DX9) || defined(BUILD_D3DM) || defined(BUILD_DX10)
	fCosine = PVRTXCOS(-fAngle);
    fSine   = PVRTXSIN(-fAngle);
#else
	fCosine = PVRTXCOS(fAngle);
    fSine   = PVRTXSIN(fAngle);
#endif

	/* Create the trigonometric matrix corresponding to Z Rotation */
	mOut.f[ 0]=fCosine;				mOut.f[ 4]=fSine;				mOut.f[ 8]=PVRTF2X(0.0f);	mOut.f[12]=PVRTF2X(0.0f);
	mOut.f[ 1]=-fSine;				mOut.f[ 5]=fCosine;				mOut.f[ 9]=PVRTF2X(0.0f);	mOut.f[13]=PVRTF2X(0.0f);
	mOut.f[ 2]=PVRTF2X(0.0f);		mOut.f[ 6]=PVRTF2X(0.0f);		mOut.f[10]=PVRTF2X(1.0f);	mOut.f[14]=PVRTF2X(0.0f);
	mOut.f[ 3]=PVRTF2X(0.0f);		mOut.f[ 7]=PVRTF2X(0.0f);		mOut.f[11]=PVRTF2X(0.0f);	mOut.f[15]=PVRTF2X(1.0f);
}

/*!***************************************************************************
 @Function Name		PVRTMatrixTransposeX
 @Output			mOut	Transposed matrix
 @Input				mIn		Original matrix
 @Description		Compute the transpose matrix of mIn.
*****************************************************************************/
void PVRTMatrixTransposeX(
	PVRTMATRIXx			&mOut,
	const PVRTMATRIXx	&mIn)
{
	PVRTMATRIXx	mTmp;

	mTmp.f[ 0]=mIn.f[ 0];	mTmp.f[ 4]=mIn.f[ 1];	mTmp.f[ 8]=mIn.f[ 2];	mTmp.f[12]=mIn.f[ 3];
	mTmp.f[ 1]=mIn.f[ 4];	mTmp.f[ 5]=mIn.f[ 5];	mTmp.f[ 9]=mIn.f[ 6];	mTmp.f[13]=mIn.f[ 7];
	mTmp.f[ 2]=mIn.f[ 8];	mTmp.f[ 6]=mIn.f[ 9];	mTmp.f[10]=mIn.f[10];	mTmp.f[14]=mIn.f[11];
	mTmp.f[ 3]=mIn.f[12];	mTmp.f[ 7]=mIn.f[13];	mTmp.f[11]=mIn.f[14];	mTmp.f[15]=mIn.f[15];

	mOut = mTmp;
}

/*!***************************************************************************
 @Function			PVRTMatrixInverseX
 @Output			mOut	Inversed matrix
 @Input				mIn		Original matrix
 @Description		Compute the inverse matrix of mIn.
					The matrix must be of the form :
					A 0
					C 1
					Where A is a 3x3 matrix and C is a 1x3 matrix.
*****************************************************************************/
void PVRTMatrixInverseX(
	PVRTMATRIXx			&mOut,
	const PVRTMATRIXx	&mIn)
{
	PVRTMATRIXx	mDummyMatrix;
	int			det_1;
	int			pos, neg, temp;

    /* Calculate the determinant of submatrix A and determine if the
       the matrix is singular as limited by the double precision
       floating-point data representation. */
    pos = neg = 0;
    temp =  PVRTXMUL(PVRTXMUL(mIn.f[ 0], mIn.f[ 5]), mIn.f[10]);
    if (temp >= 0) pos += temp; else neg += temp;
    temp =  PVRTXMUL(PVRTXMUL(mIn.f[ 4], mIn.f[ 9]), mIn.f[ 2]);
    if (temp >= 0) pos += temp; else neg += temp;
    temp =  PVRTXMUL(PVRTXMUL(mIn.f[ 8], mIn.f[ 1]), mIn.f[ 6]);
    if (temp >= 0) pos += temp; else neg += temp;
	temp =  PVRTXMUL(PVRTXMUL(-mIn.f[ 8], mIn.f[ 5]), mIn.f[ 2]);
    if (temp >= 0) pos += temp; else neg += temp;
    temp =  PVRTXMUL(PVRTXMUL(-mIn.f[ 4], mIn.f[ 1]), mIn.f[10]);
    if (temp >= 0) pos += temp; else neg += temp;
    temp =  PVRTXMUL(PVRTXMUL(-mIn.f[ 0], mIn.f[ 9]), mIn.f[ 6]);
    if (temp >= 0) pos += temp; else neg += temp;
    det_1 = pos + neg;

    /* Is the submatrix A singular? */
    if (det_1 == 0)
	{
        /* Matrix M has no inverse */
        _RPT0(_CRT_WARN, "Matrix has no inverse : singular matrix\n");
        return;
    }
    else
	{
        /* Calculate inverse(A) = adj(A) / det(A) */
        //det_1 = 1.0 / det_1;
		det_1 = PVRTXDIV(PVRTF2X(1.0f), det_1);
		mDummyMatrix.f[ 0] =   PVRTXMUL(( PVRTXMUL(mIn.f[ 5], mIn.f[10]) - PVRTXMUL(mIn.f[ 9], mIn.f[ 6]) ), det_1);
		mDummyMatrix.f[ 1] = - PVRTXMUL(( PVRTXMUL(mIn.f[ 1], mIn.f[10]) - PVRTXMUL(mIn.f[ 9], mIn.f[ 2]) ), det_1);
		mDummyMatrix.f[ 2] =   PVRTXMUL(( PVRTXMUL(mIn.f[ 1], mIn.f[ 6]) - PVRTXMUL(mIn.f[ 5], mIn.f[ 2]) ), det_1);
		mDummyMatrix.f[ 4] = - PVRTXMUL(( PVRTXMUL(mIn.f[ 4], mIn.f[10]) - PVRTXMUL(mIn.f[ 8], mIn.f[ 6]) ), det_1);
		mDummyMatrix.f[ 5] =   PVRTXMUL(( PVRTXMUL(mIn.f[ 0], mIn.f[10]) - PVRTXMUL(mIn.f[ 8], mIn.f[ 2]) ), det_1);
		mDummyMatrix.f[ 6] = - PVRTXMUL(( PVRTXMUL(mIn.f[ 0], mIn.f[ 6]) - PVRTXMUL(mIn.f[ 4], mIn.f[ 2]) ), det_1);
		mDummyMatrix.f[ 8] =   PVRTXMUL(( PVRTXMUL(mIn.f[ 4], mIn.f[ 9]) - PVRTXMUL(mIn.f[ 8], mIn.f[ 5]) ), det_1);
		mDummyMatrix.f[ 9] = - PVRTXMUL(( PVRTXMUL(mIn.f[ 0], mIn.f[ 9]) - PVRTXMUL(mIn.f[ 8], mIn.f[ 1]) ), det_1);
		mDummyMatrix.f[10] =   PVRTXMUL(( PVRTXMUL(mIn.f[ 0], mIn.f[ 5]) - PVRTXMUL(mIn.f[ 4], mIn.f[ 1]) ), det_1);

        /* Calculate -C * inverse(A) */
        mDummyMatrix.f[12] = - ( PVRTXMUL(mIn.f[12], mDummyMatrix.f[ 0]) + PVRTXMUL(mIn.f[13], mDummyMatrix.f[ 4]) + PVRTXMUL(mIn.f[14], mDummyMatrix.f[ 8]) );
		mDummyMatrix.f[13] = - ( PVRTXMUL(mIn.f[12], mDummyMatrix.f[ 1]) + PVRTXMUL(mIn.f[13], mDummyMatrix.f[ 5]) + PVRTXMUL(mIn.f[14], mDummyMatrix.f[ 9]) );
		mDummyMatrix.f[14] = - ( PVRTXMUL(mIn.f[12], mDummyMatrix.f[ 2]) + PVRTXMUL(mIn.f[13], mDummyMatrix.f[ 6]) + PVRTXMUL(mIn.f[14], mDummyMatrix.f[10]) );

        /* Fill in last row */
        mDummyMatrix.f[ 3] = PVRTF2X(0.0f);
		mDummyMatrix.f[ 7] = PVRTF2X(0.0f);
		mDummyMatrix.f[11] = PVRTF2X(0.0f);
        mDummyMatrix.f[15] = PVRTF2X(1.0f);
	}

   	/* Copy contents of dummy matrix in pfMatrix */
	mOut = mDummyMatrix;
}

/*!***************************************************************************
 @Function			PVRTMatrixInverseExX
 @Output			mOut	Inversed matrix
 @Input				mIn		Original matrix
 @Description		Compute the inverse matrix of mIn.
					Uses a linear equation solver and the knowledge that M.M^-1=I.
					Use this fn to calculate the inverse of matrices that
					PVRTMatrixInverse() cannot.
*****************************************************************************/
void PVRTMatrixInverseExX(
	PVRTMATRIXx			&mOut,
	const PVRTMATRIXx	&mIn)
{
	PVRTMATRIXx		mTmp;
	int				*ppfRows[4], pfRes[4], pfIn[20];
	int				i, j;

	for (i = 0; i < 4; ++i)
	{
		ppfRows[i] = &pfIn[i * 5];
	}

	/* Solve 4 sets of 4 linear equations */
	for (i = 0; i < 4; ++i)
	{
		for (j = 0; j < 4; ++j)
		{
			ppfRows[j][0] = c_mIdentity.f[i + 4 * j];
			memcpy(&ppfRows[j][1], &mIn.f[j * 4], 4 * sizeof(float));
		}

		PVRTMatrixLinearEqSolveX(pfRes, (int**)ppfRows, 4);

		for(j = 0; j < 4; ++j)
		{
			mTmp.f[i + 4 * j] = pfRes[j];
		}
	}

	mOut = mTmp;
}

/*!***************************************************************************
 @Function			PVRTMatrixLookAtLHX
 @Output			mOut	Look-at view matrix
 @Input				vEye	Position of the camera
 @Input				vAt		Point the camera is looking at
 @Input				vUp		Up direction for the camera
 @Description		Create a look-at view matrix.
*****************************************************************************/
void PVRTMatrixLookAtLHX(
	PVRTMATRIXx			&mOut,
	const PVRTVECTOR3x	&vEye,
	const PVRTVECTOR3x	&vAt,
	const PVRTVECTOR3x	&vUp)
{
	PVRTVECTOR3x	f, vUpActual, s, u;
	PVRTMATRIXx		t;

	f.x = vEye.x - vAt.x;
	f.y = vEye.y - vAt.y;
	f.z = vEye.z - vAt.z;

	PVRTMatrixVec3NormalizeX(f, f);
	PVRTMatrixVec3NormalizeX(vUpActual, vUp);
	PVRTMatrixVec3CrossProductX(s, f, vUpActual);
	PVRTMatrixVec3CrossProductX(u, s, f);

	mOut.f[ 0] = s.x;
	mOut.f[ 1] = u.x;
	mOut.f[ 2] = -f.x;
	mOut.f[ 3] = PVRTF2X(0.0f);

	mOut.f[ 4] = s.y;
	mOut.f[ 5] = u.y;
	mOut.f[ 6] = -f.y;
	mOut.f[ 7] = PVRTF2X(0.0f);

	mOut.f[ 8] = s.z;
	mOut.f[ 9] = u.z;
	mOut.f[10] = -f.z;
	mOut.f[11] = PVRTF2X(0.0f);

	mOut.f[12] = PVRTF2X(0.0f);
	mOut.f[13] = PVRTF2X(0.0f);
	mOut.f[14] = PVRTF2X(0.0f);
	mOut.f[15] = PVRTF2X(1.0f);

	PVRTMatrixTranslationX(t, -vEye.x, -vEye.y, -vEye.z);
	PVRTMatrixMultiplyX(mOut, t, mOut);
}

/*!***************************************************************************
 @Function			PVRTMatrixLookAtRHX
 @Output			mOut	Look-at view matrix
 @Input				vEye	Position of the camera
 @Input				vAt		Point the camera is looking at
 @Input				vUp		Up direction for the camera
 @Description		Create a look-at view matrix.
*****************************************************************************/
void PVRTMatrixLookAtRHX(
	PVRTMATRIXx			&mOut,
	const PVRTVECTOR3x	&vEye,
	const PVRTVECTOR3x	&vAt,
	const PVRTVECTOR3x	&vUp)
{
	PVRTVECTOR3x	f, vUpActual, s, u;
	PVRTMATRIXx		t;

	f.x = vAt.x - vEye.x;
	f.y = vAt.y - vEye.y;
	f.z = vAt.z - vEye.z;

	PVRTMatrixVec3NormalizeX(f, f);
	PVRTMatrixVec3NormalizeX(vUpActual, vUp);
	PVRTMatrixVec3CrossProductX(s, f, vUpActual);
	PVRTMatrixVec3CrossProductX(u, s, f);

	mOut.f[ 0] = s.x;
	mOut.f[ 1] = u.x;
	mOut.f[ 2] = -f.x;
	mOut.f[ 3] = PVRTF2X(0.0f);

	mOut.f[ 4] = s.y;
	mOut.f[ 5] = u.y;
	mOut.f[ 6] = -f.y;
	mOut.f[ 7] = PVRTF2X(0.0f);

	mOut.f[ 8] = s.z;
	mOut.f[ 9] = u.z;
	mOut.f[10] = -f.z;
	mOut.f[11] = PVRTF2X(0.0f);

	mOut.f[12] = PVRTF2X(0.0f);
	mOut.f[13] = PVRTF2X(0.0f);
	mOut.f[14] = PVRTF2X(0.0f);
	mOut.f[15] = PVRTF2X(1.0f);

	PVRTMatrixTranslationX(t, -vEye.x, -vEye.y, -vEye.z);
	PVRTMatrixMultiplyX(mOut, t, mOut);
}

/*!***************************************************************************
 @Function		PVRTMatrixPerspectiveFovLHX
 @Output		mOut		Perspective matrix
 @Input			fFOVy		Field of view
 @Input			fAspect		Aspect ratio
 @Input			fNear		Near clipping distance
 @Input			fFar		Far clipping distance
 @Input			bRotate		Should we rotate it ? (for upright screens)
 @Description	Create a perspective matrix.
*****************************************************************************/
void PVRTMatrixPerspectiveFovLHX(
	PVRTMATRIXx	&mOut,
	const int	fFOVy,
	const int	fAspect,
	const int	fNear,
	const int	fFar,
	const bool  bRotate)
{
	int		f, fRealAspect;

	if (bRotate)
		fRealAspect = PVRTXDIV(PVRTF2X(1.0f), fAspect);
	else
		fRealAspect = fAspect;

	f = PVRTXDIV(PVRTF2X(1.0f), PVRTXTAN(PVRTXMUL(fFOVy, PVRTF2X(0.5f))));

	mOut.f[ 0] = PVRTXDIV(f, fRealAspect);
	mOut.f[ 1] = PVRTF2X(0.0f);
	mOut.f[ 2] = PVRTF2X(0.0f);
	mOut.f[ 3] = PVRTF2X(0.0f);

	mOut.f[ 4] = PVRTF2X(0.0f);
	mOut.f[ 5] = f;
	mOut.f[ 6] = PVRTF2X(0.0f);
	mOut.f[ 7] = PVRTF2X(0.0f);

	mOut.f[ 8] = PVRTF2X(0.0f);
	mOut.f[ 9] = PVRTF2X(0.0f);
	mOut.f[10] = PVRTXDIV(fFar, fFar - fNear);
	mOut.f[11] = PVRTF2X(1.0f);

	mOut.f[12] = PVRTF2X(0.0f);
	mOut.f[13] = PVRTF2X(0.0f);
	mOut.f[14] = -PVRTXMUL(PVRTXDIV(fFar, fFar - fNear), fNear);
	mOut.f[15] = PVRTF2X(0.0f);

	if (bRotate)
	{
		PVRTMATRIXx mRotation, mTemp = mOut;
		PVRTMatrixRotationZX(mRotation, PVRTF2X(90.0f*PVRT_PIf/180.0f));
		PVRTMatrixMultiplyX(mOut, mTemp, mRotation);
	}
}

/*!***************************************************************************
 @Function		PVRTMatrixPerspectiveFovRHX
 @Output		mOut		Perspective matrix
 @Input			fFOVy		Field of view
 @Input			fAspect		Aspect ratio
 @Input			fNear		Near clipping distance
 @Input			fFar		Far clipping distance
 @Input			bRotate		Should we rotate it ? (for upright screens)
 @Description	Create a perspective matrix.
*****************************************************************************/
void PVRTMatrixPerspectiveFovRHX(
	PVRTMATRIXx	&mOut,
	const int	fFOVy,
	const int	fAspect,
	const int	fNear,
	const int	fFar,
	const bool  bRotate)
{
	int		f;

	int fCorrectAspect = fAspect;
	if (bRotate)
	{
		fCorrectAspect = PVRTXDIV(PVRTF2X(1.0f), fAspect);
	}
	f = PVRTXDIV(PVRTF2X(1.0f), PVRTXTAN(PVRTXMUL(fFOVy, PVRTF2X(0.5f))));

	mOut.f[ 0] = PVRTXDIV(f, fCorrectAspect);
	mOut.f[ 1] = PVRTF2X(0.0f);
	mOut.f[ 2] = PVRTF2X(0.0f);
	mOut.f[ 3] = PVRTF2X(0.0f);

	mOut.f[ 4] = PVRTF2X(0.0f);
	mOut.f[ 5] = f;
	mOut.f[ 6] = PVRTF2X(0.0f);
	mOut.f[ 7] = PVRTF2X(0.0f);

	mOut.f[ 8] = PVRTF2X(0.0f);
	mOut.f[ 9] = PVRTF2X(0.0f);
	mOut.f[10] = PVRTXDIV(fFar + fNear, fNear - fFar);
	mOut.f[11] = PVRTF2X(-1.0f);

	mOut.f[12] = PVRTF2X(0.0f);
	mOut.f[13] = PVRTF2X(0.0f);
	mOut.f[14] = PVRTXMUL(PVRTXDIV(fFar, fNear - fFar), fNear) << 1;	// Cheap 2x
	mOut.f[15] = PVRTF2X(0.0f);

	if (bRotate)
	{
		PVRTMATRIXx mRotation, mTemp = mOut;
		PVRTMatrixRotationZX(mRotation, PVRTF2X(-90.0f*PVRT_PIf/180.0f));
		PVRTMatrixMultiplyX(mOut, mTemp, mRotation);
	}
}

/*!***************************************************************************
 @Function		PVRTMatrixOrthoLHX
 @Output		mOut		Orthographic matrix
 @Input			w			Width of the screen
 @Input			h			Height of the screen
 @Input			zn			Near clipping distance
 @Input			zf			Far clipping distance
 @Input			bRotate		Should we rotate it ? (for upright screens)
 @Description	Create an orthographic matrix.
*****************************************************************************/
void PVRTMatrixOrthoLHX(
	PVRTMATRIXx	&mOut,
	const int	w,
	const int	h,
	const int	zn,
	const int	zf,
	const bool  bRotate)
{
	int fCorrectW = w;
	int fCorrectH = h;
	if (bRotate)
	{
		fCorrectW = h;
		fCorrectH = w;
	}
	mOut.f[ 0] = PVRTXDIV(PVRTF2X(2.0f), fCorrectW);
	mOut.f[ 1] = PVRTF2X(0.0f);
	mOut.f[ 2] = PVRTF2X(0.0f);
	mOut.f[ 3] = PVRTF2X(0.0f);

	mOut.f[ 4] = PVRTF2X(0.0f);
	mOut.f[ 5] = PVRTXDIV(PVRTF2X(2.0f), fCorrectH);
	mOut.f[ 6] = PVRTF2X(0.0f);
	mOut.f[ 7] = PVRTF2X(0.0f);

	mOut.f[ 8] = PVRTF2X(0.0f);
	mOut.f[ 9] = PVRTF2X(0.0f);
	mOut.f[10] = PVRTXDIV(PVRTF2X(1.0f), zf - zn);
	mOut.f[11] = PVRTXDIV(zn, zn - zf);

	mOut.f[12] = PVRTF2X(0.0f);
	mOut.f[13] = PVRTF2X(0.0f);
	mOut.f[14] = PVRTF2X(0.0f);
	mOut.f[15] = PVRTF2X(1.0f);

	if (bRotate)
	{
		PVRTMATRIXx mRotation, mTemp = mOut;
		PVRTMatrixRotationZX(mRotation, PVRTF2X(-90.0f*PVRT_PIf/180.0f));
		PVRTMatrixMultiplyX(mOut, mRotation, mTemp);
	}
}

/*!***************************************************************************
 @Function		PVRTMatrixOrthoRHX
 @Output		mOut		Orthographic matrix
 @Input			w			Width of the screen
 @Input			h			Height of the screen
 @Input			zn			Near clipping distance
 @Input			zf			Far clipping distance
 @Input			bRotate		Should we rotate it ? (for upright screens)
 @Description	Create an orthographic matrix.
*****************************************************************************/
void PVRTMatrixOrthoRHX(
	PVRTMATRIXx	&mOut,
	const int	w,
	const int	h,
	const int	zn,
	const int	zf,
	const bool  bRotate)
{
	int fCorrectW = w;
	int fCorrectH = h;
	if (bRotate)
	{
		fCorrectW = h;
		fCorrectH = w;
	}
	mOut.f[ 0] = PVRTXDIV(PVRTF2X(2.0f), fCorrectW);
	mOut.f[ 1] = PVRTF2X(0.0f);
	mOut.f[ 2] = PVRTF2X(0.0f);
	mOut.f[ 3] = PVRTF2X(0.0f);

	mOut.f[ 4] = PVRTF2X(0.0f);
	mOut.f[ 5] = PVRTXDIV(PVRTF2X(2.0f), fCorrectH);
	mOut.f[ 6] = PVRTF2X(0.0f);
	mOut.f[ 7] = PVRTF2X(0.0f);

	mOut.f[ 8] = PVRTF2X(0.0f);
	mOut.f[ 9] = PVRTF2X(0.0f);
	mOut.f[10] = PVRTXDIV(PVRTF2X(1.0f), zn - zf);
	mOut.f[11] = PVRTXDIV(zn, zn - zf);

	mOut.f[12] = PVRTF2X(0.0f);
	mOut.f[13] = PVRTF2X(0.0f);
	mOut.f[14] = PVRTF2X(0.0f);
	mOut.f[15] = PVRTF2X(1.0f);

	if (bRotate)
	{
		PVRTMATRIXx mRotation, mTemp = mOut;
		PVRTMatrixRotationZX(mRotation, PVRTF2X(-90.0f*PVRT_PIf/180.0f));
		PVRTMatrixMultiplyX(mOut, mRotation, mTemp);
	}
}

/*!***************************************************************************
 @Function			PVRTMatrixVec3LerpX
 @Output			vOut	Result of the interpolation
 @Input				v1		First vector to interpolate from
 @Input				v2		Second vector to interpolate form
 @Input				s		Coefficient of interpolation
 @Description		This function performs the linear interpolation based on
					the following formula: V1 + s(V2-V1).
*****************************************************************************/
void PVRTMatrixVec3LerpX(
	PVRTVECTOR3x		&vOut,
	const PVRTVECTOR3x	&v1,
	const PVRTVECTOR3x	&v2,
	const int			s)
{
	vOut.x = v1.x + PVRTXMUL(s, v2.x - v1.x);
	vOut.y = v1.y + PVRTXMUL(s, v2.y - v1.y);
	vOut.z = v1.z + PVRTXMUL(s, v2.z - v1.z);
}

/*!***************************************************************************
 @Function			PVRTMatrixVec3DotProductX
 @Input				v1		First vector
 @Input				v2		Second vector
 @Return			Dot product of the two vectors.
 @Description		This function performs the dot product of the two
					supplied vectors.
					A single >> 16 shift could be applied to the final accumulated
					result however this runs the risk of overflow between the
					results of the intermediate additions.
*****************************************************************************/
int PVRTMatrixVec3DotProductX(
	const PVRTVECTOR3x	&v1,
	const PVRTVECTOR3x	&v2)
{
	return (PVRTXMUL(v1.x, v2.x) + PVRTXMUL(v1.y, v2.y) + PVRTXMUL(v1.z, v2.z));
}

/*!***************************************************************************
 @Function			PVRTMatrixVec3CrossProductX
 @Output			vOut	Cross product of the two vectors
 @Input				v1		First vector
 @Input				v2		Second vector
 @Description		This function performs the cross product of the two
					supplied vectors.
*****************************************************************************/
void PVRTMatrixVec3CrossProductX(
	PVRTVECTOR3x		&vOut,
	const PVRTVECTOR3x	&v1,
	const PVRTVECTOR3x	&v2)
{
	PVRTVECTOR3x result;

	/* Perform calculation on a dummy VECTOR (result) */
    result.x = PVRTXMUL(v1.y, v2.z) - PVRTXMUL(v1.z, v2.y);
    result.y = PVRTXMUL(v1.z, v2.x) - PVRTXMUL(v1.x, v2.z);
    result.z = PVRTXMUL(v1.x, v2.y) - PVRTXMUL(v1.y, v2.x);

	/* Copy result in pOut */
	vOut = result;
}

/*!***************************************************************************
 @Function			PVRTMatrixVec3NormalizeX
 @Output			vOut	Normalized vector
 @Input				vIn		Vector to normalize
 @Description		Normalizes the supplied vector.
					The square root function is currently still performed
					in floating-point.
					Original vector is scaled down prior to be normalized in
					order to avoid overflow issues.
****************************************************************************/
void PVRTMatrixVec3NormalizeX(
	PVRTVECTOR3x		&vOut,
	const PVRTVECTOR3x	&vIn)
{
	int				f, n;
	PVRTVECTOR3x	vTemp;

	/* Scale vector by uniform value */
	n = PVRTABS(vIn.x) + PVRTABS(vIn.y) + PVRTABS(vIn.z);
	vTemp.x = PVRTXDIV(vIn.x, n);
	vTemp.y = PVRTXDIV(vIn.y, n);
	vTemp.z = PVRTXDIV(vIn.z, n);

	/* Calculate x2+y2+z2/sqrt(x2+y2+z2) */
	f = PVRTMatrixVec3DotProductX(vTemp, vTemp);
	f = PVRTXDIV(PVRTF2X(1.0f), PVRTF2X(sqrt(PVRTX2F(f))));

	/* Multiply vector components by f */
	vOut.x = PVRTXMUL(vTemp.x, f);
	vOut.y = PVRTXMUL(vTemp.y, f);
	vOut.z = PVRTXMUL(vTemp.z, f);
}

/*!***************************************************************************
 @Function			PVRTMatrixVec3LengthX
 @Input				vIn		Vector to get the length of
 @Return			The length of the vector
 @Description		Gets the length of the supplied vector
*****************************************************************************/
int PVRTMatrixVec3LengthX(
	const PVRTVECTOR3x	&vIn)
{
	int temp;

	temp = PVRTXMUL(vIn.x,vIn.x) + PVRTXMUL(vIn.y,vIn.y) + PVRTXMUL(vIn.z,vIn.z);
	return PVRTF2X(sqrt(PVRTX2F(temp)));
}

/*!***************************************************************************
 @Function			PVRTMatrixLinearEqSolveX
 @Input				pSrc	2D array of floats. 4 Eq linear problem is 5x4
							matrix, constants in first column
 @Input				nCnt	Number of equations to solve
 @Output			pRes	Result
 @Description		Solves 'nCnt' simultaneous equations of 'nCnt' variables.
					pRes should be an array large enough to contain the
					results: the values of the 'nCnt' variables.
					This fn recursively uses Gaussian Elimination.
*****************************************************************************/
void PVRTMatrixLinearEqSolveX(
	int			* const pRes,
	int			** const pSrc,
	const int	nCnt)
{
	int		i, j, k;
	int		f;

	if (nCnt == 1)
	{
		_ASSERT(pSrc[0][1] != 0);
		pRes[0] = PVRTXDIV(pSrc[0][0], pSrc[0][1]);
		return;
	}

	// Loop backwards in an attempt avoid the need to swap rows
	i = nCnt;
	while(i)
	{
		--i;

		if(pSrc[i][nCnt] != PVRTF2X(0.0f))
		{
			// Row i can be used to zero the other rows; let's move it to the bottom
			if(i != (nCnt-1))
			{
				for(j = 0; j <= nCnt; ++j)
				{
					// Swap the two values
					f = pSrc[nCnt-1][j];
					pSrc[nCnt-1][j] = pSrc[i][j];
					pSrc[i][j] = f;
				}
			}

			// Now zero the last columns of the top rows
			for(j = 0; j < (nCnt-1); ++j)
			{
				_ASSERT(pSrc[nCnt-1][nCnt] != PVRTF2X(0.0f));
				f = PVRTXDIV(pSrc[j][nCnt], pSrc[nCnt-1][nCnt]);

				// No need to actually calculate a zero for the final column
				for(k = 0; k < nCnt; ++k)
				{
					pSrc[j][k] -= PVRTXMUL(f, pSrc[nCnt-1][k]);
				}
			}

			break;
		}
	}

	// Solve the top-left sub matrix
	PVRTMatrixLinearEqSolveX(pRes, pSrc, nCnt - 1);

	// Now calc the solution for the bottom row
	f = pSrc[nCnt-1][0];
	for(k = 1; k < nCnt; ++k)
	{
		f -= PVRTXMUL(pSrc[nCnt-1][k], pRes[k-1]);
	}
	_ASSERT(pSrc[nCnt-1][nCnt] != PVRTF2X(0));
	f = PVRTXDIV(f, pSrc[nCnt-1][nCnt]);
	pRes[nCnt-1] = f;
}

/*****************************************************************************
 End of file (PVRTMatrixX.cpp)
*****************************************************************************/
