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

 @File         PVRTVector.cpp

 @Title        PVRTVector

 @Version      

 @Copyright    Copyright (c) Imagination Technologies Limited.

 @Platform     ANSI compatible

 @Description  Vector and matrix mathematics library

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

#include "PVRTVector.h"

#include <math.h>

/*!***************************************************************************
** PVRTVec2 2 component vector
****************************************************************************/

/*!***************************************************************************
 @Function			PVRTVec2
 @Input				v3Vec a Vec3
 @Description		Constructor from a Vec3
 *****************************************************************************/
	PVRTVec2::PVRTVec2(const PVRTVec3& vec3)
	{
		x = vec3.x; y = vec3.y;
	}

/*!***************************************************************************
** PVRTVec3 3 component vector
****************************************************************************/

/*!***************************************************************************
 @Function			PVRTVec3
 @Input				v4Vec a PVRTVec4
 @Description		Constructor from a PVRTVec4
*****************************************************************************/
	PVRTVec3::PVRTVec3(const PVRTVec4& vec4)
	{
		x = vec4.x; y = vec4.y; z = vec4.z;
	}

/*!***************************************************************************
 @Function			*
 @Input				rhs a PVRTMat3
 @Returns			result of multiplication
 @Description		matrix multiplication operator PVRTVec3 and PVRTMat3
****************************************************************************/
	PVRTVec3 PVRTVec3::operator*(const PVRTMat3& rhs) const
	{
		PVRTVec3 out;

		out.x = VERTTYPEMUL(x,rhs.f[0])+VERTTYPEMUL(y,rhs.f[1])+VERTTYPEMUL(z,rhs.f[2]);
		out.y = VERTTYPEMUL(x,rhs.f[3])+VERTTYPEMUL(y,rhs.f[4])+VERTTYPEMUL(z,rhs.f[5]);
		out.z = VERTTYPEMUL(x,rhs.f[6])+VERTTYPEMUL(y,rhs.f[7])+VERTTYPEMUL(z,rhs.f[8]);

		return out;
	}

/*!***************************************************************************
 @Function			*=
 @Input				rhs a PVRTMat3
 @Returns			result of multiplication and assignment
 @Description		matrix multiplication and assignment operator for PVRTVec3 and PVRTMat3
****************************************************************************/
	PVRTVec3& PVRTVec3::operator*=(const PVRTMat3& rhs)
	{
		VERTTYPE tx = VERTTYPEMUL(x,rhs.f[0])+VERTTYPEMUL(y,rhs.f[1])+VERTTYPEMUL(z,rhs.f[2]);
		VERTTYPE ty = VERTTYPEMUL(x,rhs.f[3])+VERTTYPEMUL(y,rhs.f[4])+VERTTYPEMUL(z,rhs.f[5]);
		z = VERTTYPEMUL(x,rhs.f[6])+VERTTYPEMUL(y,rhs.f[7])+VERTTYPEMUL(z,rhs.f[8]);
		x = tx;
		y = ty;

		return *this;
	}

/*!***************************************************************************
** PVRTVec4 4 component vector
****************************************************************************/

/*!***************************************************************************
 @Function			*
 @Input				rhs a PVRTMat4
 @Returns			result of multiplication
 @Description		matrix multiplication operator PVRTVec4 and PVRTMat4
****************************************************************************/
	PVRTVec4 PVRTVec4::operator*(const PVRTMat4& rhs) const
	{
		PVRTVec4 out;
		out.x = VERTTYPEMUL(x,rhs.f[0])+VERTTYPEMUL(y,rhs.f[1])+VERTTYPEMUL(z,rhs.f[2])+VERTTYPEMUL(w,rhs.f[3]);
		out.y = VERTTYPEMUL(x,rhs.f[4])+VERTTYPEMUL(y,rhs.f[5])+VERTTYPEMUL(z,rhs.f[6])+VERTTYPEMUL(w,rhs.f[7]);
		out.z = VERTTYPEMUL(x,rhs.f[8])+VERTTYPEMUL(y,rhs.f[9])+VERTTYPEMUL(z,rhs.f[10])+VERTTYPEMUL(w,rhs.f[11]);
		out.w = VERTTYPEMUL(x,rhs.f[12])+VERTTYPEMUL(y,rhs.f[13])+VERTTYPEMUL(z,rhs.f[14])+VERTTYPEMUL(w,rhs.f[15]);
		return out;
	}

/*!***************************************************************************
 @Function			*=
 @Input				rhs a PVRTMat4
 @Returns			result of multiplication and assignment
 @Description		matrix multiplication and assignment operator for PVRTVec4 and PVRTMat4
****************************************************************************/
	PVRTVec4& PVRTVec4::operator*=(const PVRTMat4& rhs)
	{
		VERTTYPE tx = VERTTYPEMUL(x,rhs.f[0])+VERTTYPEMUL(y,rhs.f[1])+VERTTYPEMUL(z,rhs.f[2])+VERTTYPEMUL(w,rhs.f[3]);
		VERTTYPE ty = VERTTYPEMUL(x,rhs.f[4])+VERTTYPEMUL(y,rhs.f[5])+VERTTYPEMUL(z,rhs.f[6])+VERTTYPEMUL(w,rhs.f[7]);
		VERTTYPE tz = VERTTYPEMUL(x,rhs.f[8])+VERTTYPEMUL(y,rhs.f[9])+VERTTYPEMUL(z,rhs.f[10])+VERTTYPEMUL(w,rhs.f[11]);
		w = VERTTYPEMUL(x,rhs.f[12])+VERTTYPEMUL(y,rhs.f[13])+VERTTYPEMUL(z,rhs.f[14])+VERTTYPEMUL(w,rhs.f[15]);
		x = tx;
		y = ty;
		z = tz;
		return *this;
	}

/*!***************************************************************************
** PVRTMat3 3x3 matrix
****************************************************************************/
/*!***************************************************************************
 @Function			PVRTMat3
 @Input				mat a PVRTMat4
 @Description		constructor to form a PVRTMat3 from a PVRTMat4
****************************************************************************/
	PVRTMat3::PVRTMat3(const PVRTMat4& mat)
	{
		VERTTYPE *dest = (VERTTYPE*)f, *src = (VERTTYPE*)mat.f;
		for(int i=0;i<3;i++)
		{
			for(int j=0;j<3;j++)
			{
				(*dest++) = (*src++);
			}
			src++;
		}
	}

/*!***************************************************************************
 @Function			RotationX
 @Input				angle the angle of rotation
 @Returns			rotation matrix
 @Description		generates a 3x3 rotation matrix about the X axis
****************************************************************************/
	PVRTMat3 PVRTMat3::RotationX(VERTTYPE angle)
	{
		PVRTMat4 out;
		PVRTMatrixRotationX(out,angle);
		return PVRTMat3(out);
	}
/*!***************************************************************************
 @Function			RotationY
 @Input				angle the angle of rotation
 @Returns			rotation matrix
 @Description		generates a 3x3 rotation matrix about the Y axis
****************************************************************************/
	PVRTMat3 PVRTMat3::RotationY(VERTTYPE angle)
	{
		PVRTMat4 out;
		PVRTMatrixRotationY(out,angle);
		return PVRTMat3(out);
	}
/*!***************************************************************************
 @Function			RotationZ
 @Input				angle the angle of rotation
 @Returns			rotation matrix
 @Description		generates a 3x3 rotation matrix about the Z axis
****************************************************************************/
	PVRTMat3 PVRTMat3::RotationZ(VERTTYPE angle)
	{
		PVRTMat4 out;
		PVRTMatrixRotationZ(out,angle);
		return PVRTMat3(out);
	}


/*!***************************************************************************
** PVRTMat4 4x4 matrix
****************************************************************************/
/*!***************************************************************************
 @Function			RotationX
 @Input				angle the angle of rotation
 @Returns			rotation matrix
 @Description		generates a 4x4 rotation matrix about the X axis
****************************************************************************/
	PVRTMat4 PVRTMat4::RotationX(VERTTYPE angle)
	{
		PVRTMat4 out;
		PVRTMatrixRotationX(out,angle);
		return out;
	}
/*!***************************************************************************
 @Function			RotationY
 @Input				angle the angle of rotation
 @Returns			rotation matrix
 @Description		generates a 4x4 rotation matrix about the Y axis
****************************************************************************/
	PVRTMat4 PVRTMat4::RotationY(VERTTYPE angle)
	{
		PVRTMat4 out;
		PVRTMatrixRotationY(out,angle);
		return out;
	}
/*!***************************************************************************
 @Function			RotationZ
 @Input				angle the angle of rotation
 @Returns			rotation matrix
 @Description		generates a 4x4 rotation matrix about the Z axis
****************************************************************************/
	PVRTMat4 PVRTMat4::RotationZ(VERTTYPE angle)
	{
		PVRTMat4 out;
		PVRTMatrixRotationZ(out,angle);
		return out;
	}

/*!***************************************************************************
 @Function			*
 @Input				rhs another PVRTMat4
 @Returns			result of multiplication
 @Description		Matrix multiplication of two 4x4 matrices.
*****************************************************************************/
	PVRTMat4 PVRTMat4::operator*(const PVRTMat4& rhs) const
	{
		PVRTMat4 out;
		// col 1
		out.f[0] =	VERTTYPEMUL(f[0],rhs.f[0])+VERTTYPEMUL(f[4],rhs.f[1])+VERTTYPEMUL(f[8],rhs.f[2])+VERTTYPEMUL(f[12],rhs.f[3]);
		out.f[1] =	VERTTYPEMUL(f[1],rhs.f[0])+VERTTYPEMUL(f[5],rhs.f[1])+VERTTYPEMUL(f[9],rhs.f[2])+VERTTYPEMUL(f[13],rhs.f[3]);
		out.f[2] =	VERTTYPEMUL(f[2],rhs.f[0])+VERTTYPEMUL(f[6],rhs.f[1])+VERTTYPEMUL(f[10],rhs.f[2])+VERTTYPEMUL(f[14],rhs.f[3]);
		out.f[3] =	VERTTYPEMUL(f[3],rhs.f[0])+VERTTYPEMUL(f[7],rhs.f[1])+VERTTYPEMUL(f[11],rhs.f[2])+VERTTYPEMUL(f[15],rhs.f[3]);

		// col 2
		out.f[4] =	VERTTYPEMUL(f[0],rhs.f[4])+VERTTYPEMUL(f[4],rhs.f[5])+VERTTYPEMUL(f[8],rhs.f[6])+VERTTYPEMUL(f[12],rhs.f[7]);
		out.f[5] =	VERTTYPEMUL(f[1],rhs.f[4])+VERTTYPEMUL(f[5],rhs.f[5])+VERTTYPEMUL(f[9],rhs.f[6])+VERTTYPEMUL(f[13],rhs.f[7]);
		out.f[6] =	VERTTYPEMUL(f[2],rhs.f[4])+VERTTYPEMUL(f[6],rhs.f[5])+VERTTYPEMUL(f[10],rhs.f[6])+VERTTYPEMUL(f[14],rhs.f[7]);
		out.f[7] =	VERTTYPEMUL(f[3],rhs.f[4])+VERTTYPEMUL(f[7],rhs.f[5])+VERTTYPEMUL(f[11],rhs.f[6])+VERTTYPEMUL(f[15],rhs.f[7]);

		// col3
		out.f[8] =	VERTTYPEMUL(f[0],rhs.f[8])+VERTTYPEMUL(f[4],rhs.f[9])+VERTTYPEMUL(f[8],rhs.f[10])+VERTTYPEMUL(f[12],rhs.f[11]);
		out.f[9] =	VERTTYPEMUL(f[1],rhs.f[8])+VERTTYPEMUL(f[5],rhs.f[9])+VERTTYPEMUL(f[9],rhs.f[10])+VERTTYPEMUL(f[13],rhs.f[11]);
		out.f[10] =	VERTTYPEMUL(f[2],rhs.f[8])+VERTTYPEMUL(f[6],rhs.f[9])+VERTTYPEMUL(f[10],rhs.f[10])+VERTTYPEMUL(f[14],rhs.f[11]);
		out.f[11] =	VERTTYPEMUL(f[3],rhs.f[8])+VERTTYPEMUL(f[7],rhs.f[9])+VERTTYPEMUL(f[11],rhs.f[10])+VERTTYPEMUL(f[15],rhs.f[11]);

		// col3
		out.f[12] =	VERTTYPEMUL(f[0],rhs.f[12])+VERTTYPEMUL(f[4],rhs.f[13])+VERTTYPEMUL(f[8],rhs.f[14])+VERTTYPEMUL(f[12],rhs.f[15]);
		out.f[13] =	VERTTYPEMUL(f[1],rhs.f[12])+VERTTYPEMUL(f[5],rhs.f[13])+VERTTYPEMUL(f[9],rhs.f[14])+VERTTYPEMUL(f[13],rhs.f[15]);
		out.f[14] =	VERTTYPEMUL(f[2],rhs.f[12])+VERTTYPEMUL(f[6],rhs.f[13])+VERTTYPEMUL(f[10],rhs.f[14])+VERTTYPEMUL(f[14],rhs.f[15]);
		out.f[15] =	VERTTYPEMUL(f[3],rhs.f[12])+VERTTYPEMUL(f[7],rhs.f[13])+VERTTYPEMUL(f[11],rhs.f[14])+VERTTYPEMUL(f[15],rhs.f[15]);
		return out;
	}


/*!***************************************************************************
 @Function			inverse
 @Returns			inverse mat4
 @Description		Calculates multiplicative inverse of this matrix
					The matrix must be of the form :
					A 0
					C 1
					Where A is a 3x3 matrix and C is a 1x3 matrix.
*****************************************************************************/
	PVRTMat4 PVRTMat4::inverse() const
	{
		PVRTMat4 out;
		VERTTYPE	det_1;
		VERTTYPE	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 = f2vt(0.0);
		temp =  VERTTYPEMUL(VERTTYPEMUL(f[ 0], f[ 5]), f[10]);
		if (temp >= 0) pos += temp; else neg += temp;
		temp =  VERTTYPEMUL(VERTTYPEMUL(f[ 4], f[ 9]), f[ 2]);
		if (temp >= 0) pos += temp; else neg += temp;
		temp =  VERTTYPEMUL(VERTTYPEMUL(f[ 8], f[ 1]), f[ 6]);
		if (temp >= 0) pos += temp; else neg += temp;
		temp =  VERTTYPEMUL(VERTTYPEMUL(-f[ 8], f[ 5]), f[ 2]);
		if (temp >= 0) pos += temp; else neg += temp;
		temp =  VERTTYPEMUL(VERTTYPEMUL(-f[ 4], f[ 1]), f[10]);
		if (temp >= 0) pos += temp; else neg += temp;
		temp =  VERTTYPEMUL(VERTTYPEMUL(-f[ 0], f[ 9]), f[ 6]);
		if (temp >= 0) pos += temp; else neg += temp;
		det_1 = pos + neg;

		/* Is the submatrix A singular? */
		if (det_1 == f2vt(0.0)) //|| (VERTTYPEABS(det_1 / (pos - neg)) < 1.0e-15)
		{
			/* Matrix M has no inverse */
			_RPT0(_CRT_WARN, "Matrix has no inverse : singular matrix\n");
		}
		else
		{
			/* Calculate inverse(A) = adj(A) / det(A) */
			//det_1 = 1.0 / det_1;
			det_1 = VERTTYPEDIV(f2vt(1.0f), det_1);
			out.f[ 0] =   VERTTYPEMUL(( VERTTYPEMUL(f[ 5], f[10]) - VERTTYPEMUL(f[ 9], f[ 6]) ), det_1);
			out.f[ 1] = - VERTTYPEMUL(( VERTTYPEMUL(f[ 1], f[10]) - VERTTYPEMUL(f[ 9], f[ 2]) ), det_1);
			out.f[ 2] =   VERTTYPEMUL(( VERTTYPEMUL(f[ 1], f[ 6]) - VERTTYPEMUL(f[ 5], f[ 2]) ), det_1);
			out.f[ 4] = - VERTTYPEMUL(( VERTTYPEMUL(f[ 4], f[10]) - VERTTYPEMUL(f[ 8], f[ 6]) ), det_1);
			out.f[ 5] =   VERTTYPEMUL(( VERTTYPEMUL(f[ 0], f[10]) - VERTTYPEMUL(f[ 8], f[ 2]) ), det_1);
			out.f[ 6] = - VERTTYPEMUL(( VERTTYPEMUL(f[ 0], f[ 6]) - VERTTYPEMUL(f[ 4], f[ 2]) ), det_1);
			out.f[ 8] =   VERTTYPEMUL(( VERTTYPEMUL(f[ 4], f[ 9]) - VERTTYPEMUL(f[ 8], f[ 5]) ), det_1);
			out.f[ 9] = - VERTTYPEMUL(( VERTTYPEMUL(f[ 0], f[ 9]) - VERTTYPEMUL(f[ 8], f[ 1]) ), det_1);
			out.f[10] =   VERTTYPEMUL(( VERTTYPEMUL(f[ 0], f[ 5]) - VERTTYPEMUL(f[ 4], f[ 1]) ), det_1);

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

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

		return out;
	}

/*!***************************************************************************
 @Function			PVRTLinearEqSolve
 @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 PVRTLinearEqSolve(VERTTYPE * const pRes, VERTTYPE ** const pSrc, const int nCnt)
{
	int			i, j, k;
	VERTTYPE	f;

	if (nCnt == 1)
	{
		_ASSERT(pSrc[0][1] != 0);
		pRes[0] = VERTTYPEDIV(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] != f2vt(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] != f2vt(0.0f));
				f = VERTTYPEDIV(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] -= VERTTYPEMUL(f, pSrc[nCnt-1][k]);
				}
			}

			break;
		}
	}

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

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

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