/****************************************************************************
*
* MATHS.C
*
* PROGRAMMER : Chris
*    VERSION : 00.00
*    CREATED : 19/11/97
*   MODIFIED : 19/11/97
*	   TABS : 5
*
* DESCRIPTION
*	PlayStation maths functions
*
*****************************************************************************/

/*---------------------------------------------------------------------------
 * Import Headers
\*--------------------------------------------------------------------------*/

#include <sys\types.h>
#include <libgte.h>
#include "types.h"
#include "maths.h"
#include "bgenv.h"

/*---------------------------------------------------------------------------
 * Externals
\*--------------------------------------------------------------------------*/

extern WORD rcossin_tbl[4096][2];

/*---------------------------------------------------------------------------
 * Globals
\*--------------------------------------------------------------------------*/

MATRIX3D *Matrix;
MATRIX3D *iMatrix;
SVECTOR3D CamRot;

/*---------------------------------------------------------------------------
 * Locals
\*--------------------------------------------------------------------------*/

UWORD MatrixSP;
MATRIX3D MatrixStack[MAX_NESTED_MATRICES];
MATRIX3D iMatrixStack[MAX_NESTED_MATRICES];

LONG iFrac;
LONG iRate;
LONG iAmbient;

/*---------------------------------------------------------------------------
 * Macros
\*--------------------------------------------------------------------------*/

/*---------------------------------------------------------------------------
 * Public Functions
\*--------------------------------------------------------------------------*/

#if 0

/****************************************************************************
* mPushMatrix
*
* SYNOPSIS
*	void mPushMatrix()
*
* PURPOSE
*	Stores the current matrix on the matrix stack
*
* INPUTS
*	None
*
* RETURN
*	Nothing
*/
void mPushMatrix()
{
	MatrixSP++;
	MatrixStack[MatrixSP].m00 = Matrix->m00;
	MatrixStack[MatrixSP].m01 = Matrix->m01;
	MatrixStack[MatrixSP].m02 = Matrix->m02;
	MatrixStack[MatrixSP].m10 = Matrix->m10;
	MatrixStack[MatrixSP].m11 = Matrix->m11;
	MatrixStack[MatrixSP].m12 = Matrix->m12;
	MatrixStack[MatrixSP].m20 = Matrix->m20;
	MatrixStack[MatrixSP].m21 = Matrix->m21;
	MatrixStack[MatrixSP].m22 = Matrix->m22;
	MatrixStack[MatrixSP].tx = Matrix->tx;
	MatrixStack[MatrixSP].ty = Matrix->ty;
	MatrixStack[MatrixSP].tz = Matrix->tz;

	Matrix = &MatrixStack[MatrixSP];
}

/****************************************************************************
* mPopMatrix
*
* SYNOPSIS
*	void mPopMatrix()
*
* PURPOSE
*	Restores the previous matrix from the matrix stack
*
* INPUTS
*	None
*
* RETURN
*	Nothing
*/
void mPopMatrix()
{
	MatrixSP--;
	Matrix = &MatrixStack[MatrixSP];
}

/****************************************************************************
* mTranslateXYZ
*
* SYNOPSIS
*	void mTranslateXYZ(LONG x, LONG y, LONG z)
*
* PURPOSE
*	Translates the current matrix to the required position
*
* INPUTS
*
* RETURN
*	Nothing
*/
void	mTranslateXYZ(LONG x, LONG y, LONG z)
{
	register MATRIX3D *m = Matrix;

	m->tx += ((m->m00*x) + (m->m01*y) + (m->m02*z)) >> 12;
	m->ty += ((m->m10*x) + (m->m11*y) + (m->m12*z)) >> 12;
	m->tz += ((m->m20*x) + (m->m21*y) + (m->m22*z)) >> 12;

	mLoadMatrix(m);
}

/****************************************************************************
* mGetAngle
*
* SYNOPSIS
*	LONG mGetAngle(LONG x1, LONG y1, LONG x2, LONG y2)
*
* PURPOSE
*	Calculates the angle between two 2d vectors
*
* INPUTS
*	x1		= x co-ordinate of source 2d vector
*	y1		= y co-ordinate of source 2d vector
*	x1		= x co-ordinate of target 2d vector
*	y1		= y co-ordinate of target 2d vector
*
* RETURN
*	Angle
*/
LONG mGetAngle(LONG x1, LONG y1, LONG x2, LONG y2)
{
	register LONG x, y;
	long	octant,n,angle ;

/* -------- calculate gradient */

	x = x2 - x1;
	y = y2 - y1;

	if ((x == 0) && (y == 0))
		return 0 ;

/* -------- determine octant */

	octant = 0;

	if (x < 0)
	{
		octant += 4;
		x = -x;
	}

	if (y < 0)
	{
		octant += 2;
		y = -y;
	}

	if (y > x)
	{
		octant++;
		n = x;
		x = y;
		y = n;
	}

	while ((short)y != y)		/* Scale X and Y down into Word */
	{						/* So that when we shift Y up */
		y >>= 1;				/* we dont overflow 32 Bits!! */
		x >>= 1;
	}

	n = atanTab[(y<<11)/x];

	if ((n += atanOctantTab[octant]) < 0)	/* add on req Octant*/
		angle = -(n);
	else
		angle = n;

	return (-angle)&0xffff;
}

#endif

/****************************************************************************
* mRotX
*
* SYNOPSIS
*	void mRotX(LONG ang)
*
* PURPOSE
*	Rotates the current matrix around it's x-axis
*
* INPUTS
*	ang		= angle of rotation
*
* RETURN
*	Nothing
*/
#if 0
void mRotX(LONG ang)
{
	register LONG a, b;
	register LONG sin, cos;

	if ((ang>>=4))
	{
		sin = rcossin_tbl[ang&4095][0];
		cos = rcossin_tbl[ang&4095][1];

		a = ((cos * Matrix->m01) + (sin * Matrix->m02)) >> 12;
		b = ((cos * Matrix->m02) - (sin * Matrix->m01)) >> 12;
		Matrix->m01 = a;
		Matrix->m02 = b;

		a = ((cos * Matrix->m11) + (sin * Matrix->m12)) >> 12;
		b = ((cos * Matrix->m12) - (sin * Matrix->m11)) >> 12;
		Matrix->m11 = a;
		Matrix->m12 = b;

		a = ((cos * Matrix->m21) + (sin * Matrix->m22)) >> 12;
		b = ((cos * Matrix->m22) - (sin * Matrix->m21)) >> 12;
		Matrix->m21 = a;
		Matrix->m22 = b;
	}

	mLoadMatrix(Matrix);
}
#endif

/****************************************************************************
* mRotY
*
* SYNOPSIS
*	void mRotY(LONG ang)
*
* PURPOSE
*	Rotates the current matrix around it's y-axis
*
* INPUTS
*	ang		= angle of rotation
*
* RETURN
*	Nothing
*/
#if 0
void mRotY(LONG ang)
{
	register LONG a, b;
	register LONG sin, cos;

	if ((ang>>=4))
	{
		sin = rcossin_tbl[ang&4095][0];
		cos = rcossin_tbl[ang&4095][1];

		a = ((cos * Matrix->m00) - (sin * Matrix->m02)) >> 12;
		b = ((cos * Matrix->m02) + (sin * Matrix->m00)) >> 12;
		Matrix->m00 = a;
		Matrix->m02 = b;

		a = ((cos * Matrix->m10) - (sin * Matrix->m12)) >> 12;
		b = ((cos * Matrix->m12) + (sin * Matrix->m10)) >> 12;
		Matrix->m10 = a;
		Matrix->m12 = b;

		a = ((cos * Matrix->m20) - (sin * Matrix->m22)) >> 12;
		b = ((cos * Matrix->m22) + (sin * Matrix->m20)) >> 12;
		Matrix->m20 = a;
		Matrix->m22 = b;
	}

	mLoadMatrix(Matrix);
}
#endif
/****************************************************************************
* mRotZ
*
* SYNOPSIS
*	void mRotZ(LONG ang)
*
* PURPOSE
*	Rotates the current matrix around it's z-axis
*
* INPUTS
*	ang		= angle of rotation
*
* RETURN
*	Nothing
*/
#if 0
void mRotZ(LONG ang)
{
	register LONG a, b;
	register LONG sin, cos;

	if ((ang>>=4))
	{
		sin = rcossin_tbl[ang&4095][0];
		cos = rcossin_tbl[ang&4095][1];

		a = ((cos * Matrix->m00) + (sin * Matrix->m01)) >> 12;
		b = ((cos * Matrix->m01) - (sin * Matrix->m00)) >> 12;
		Matrix->m00 = a;
		Matrix->m01 = b;

		a = ((cos * Matrix->m10) + (sin * Matrix->m11)) >> 12;
		b = ((cos * Matrix->m11) - (sin * Matrix->m10)) >> 12;
		Matrix->m10 = a;
		Matrix->m11 = b;

		a = ((cos * Matrix->m20) + (sin * Matrix->m21)) >> 12;
		b = ((cos * Matrix->m21) - (sin * Matrix->m20)) >> 12;
		Matrix->m20 = a;
		Matrix->m21 = b;
	}

	mLoadMatrix(Matrix);
}
#endif

/****************************************************************************
* mInitW2VMatrix
*
* SYNOPSIS
*	void mInitW2VMatrix(VECTOR3D *pos, VECTOR3D *target)
*
* PURPOSE
*	Constructs the world-to-view matrix from the camera position and
* 	target.
*
* INPUTS
*	pos		= camera position
*	target	= camera target
*	m		= output matrix
*
* RETURN
*	Nothing
*/
void mInitW2VMatrix(VECTOR3D *pos, VECTOR3D *target)
{
	LONG len;
	LONG dx, dy, dz;
	register MATRIX3D *m;

/* -------- initialise matrix stack */

	MatrixSP = 0;
	m = Matrix = &MatrixStack[0];

/* -------- calculate camera's rotation angles */

	dx = pos->x - target->x;
	dy = pos->y - target->y;
	dz = pos->z - target->z;

	len = mSqrt((dx*dx)+(dz*dz));

	CamRot.x = (mGetAngle(0, 0, len, dy ) >> 4) & 4095 ;
	CamRot.y = (mGetAngle(pos->z, pos->x, target->z, target->x) >> 4) & 4095;
	CamRot.z = 0 ;

/* -------- construct matrix */

	RotMatrix((SVECTOR *)&CamRot, (MATRIX *)m);

/* -------- correct aspect ratio */

	m->m10 = FixAspectRatio(m->m10);
	m->m11 = FixAspectRatio(m->m11);
	m->m12 = FixAspectRatio(m->m12);
//	m->tx  = FixAspectRatio(m->tx);

	m->tx = -pos->x;
	m->ty = -pos->y;
	m->tz = -pos->z;
	ApplyMatrixLV((MATRIX *)m, (VECTOR *)&m->tx, (VECTOR *)&m->tx);
}

#if 0

#define X 0
#define Y 1
#define Z 2

void mCalcPoint(long dx,long dy,long dz,long *result)
{
	VECTOR3D src;

	src.x = dx;
	src.y = dy;
	src.z = dz;

	ApplyMatrixLV((MATRIX *)Matrix, (VECTOR *)&src, (VECTOR *)result);
	*(result+X) += Matrix->tx;
	*(result+Y) += Matrix->ty;
	*(result+Z) += Matrix->tz;
}

void ProjectPCoord(long x,long y,long z,long *result,long cx,long cy,long fov)
{
	if(z!=0)
	{
		*(result+X)=((x*fov)/z)+cx;
		*(result+Y)=((y*fov)/z)+cy;
		*(result+Z)=z;
	}
//	else	if(z<0)
//	{
//		*(result+X)=((x*fov)/-z)+cx;
//		*(result+Y)=((y*fov)/-z)+cy;
//		*(result+Z)=z;
//	}
	else
	{
		*(result+X)=((x*fov))+cx;
		*(result+Y)=((y*fov))+cy;
		*(result+Z)=z;
	}
}

#endif


void mRotYXZ(LONG y, LONG x, LONG z)
{
	mRotY(y);
	mRotX(x);
	mRotZ(z);
}

void mRotPackedYXZ(LONG xyz)
{
	LONG ang;

	if ((ang = ((xyz >> 10) & 1023) << 6))
		mRotY(ang);

	if ((ang = ((xyz >> 20) & 1023) << 6))
		mRotX(ang);

	if ((ang = (xyz & 1023) << 6))
		mRotZ(ang);
}

extern LONG w2v_matrix[];
extern LONG *phd_mxptr;
extern MATRIX3D CamGTE;

void mQuickW2VMatrix()
{
	register MATRIX3D *m;

/* -------- initialise matrix stack */

	MatrixSP = 0;
	m = Matrix = &MatrixStack[0];

/* -------- construct matrix */

	m->m00 = phd_mxptr[0];
	m->m01 = phd_mxptr[1];
	m->m02 = phd_mxptr[2];

	m->m10 = phd_mxptr[4];
	m->m11 = phd_mxptr[5];
	m->m12 = phd_mxptr[6];

	m->m20 = phd_mxptr[8];
	m->m21 = phd_mxptr[9];
	m->m22 = phd_mxptr[10];

	m->tx = phd_mxptr[3];
	m->ty = phd_mxptr[7];
	m->tz = phd_mxptr[11];

	mLoadMatrix(m);

/* -------- construct no-apsect matrix */

	CamGTE.m00 = w2v_matrix[0];
	CamGTE.m01 = w2v_matrix[1];
	CamGTE.m02 = w2v_matrix[2];
	CamGTE.m10 = w2v_matrix[4];
	CamGTE.m11 = w2v_matrix[5];
	CamGTE.m12 = w2v_matrix[6];
	CamGTE.m20 = w2v_matrix[8];
	CamGTE.m21 = w2v_matrix[9];
	CamGTE.m22 = w2v_matrix[10];
}

#if 0

void	mTranslateAbsXYZ(LONG x, LONG y, LONG z)
{
	register MATRIX3D *m = Matrix;

	x -= MatrixStack[0].tx;
	y -= MatrixStack[0].ty;
	z -= MatrixStack[0].tz;

	m->tx = ((m->m00*x) + (m->m01*y) + (m->m02*z)) >> 12;
	m->ty = ((m->m10*x) + (m->m11*y) + (m->m12*z)) >> 12;
	m->tz = ((m->m20*x) + (m->m21*y) + (m->m22*z)) >> 12;

	mLoadMatrix(m);
}

/*******************************************************************************
 *			Calculate On screen Coordinates Of Bounding Box For Object.
 *
 * Returns 0 If Object Box totally OFF Screen
 *         1 If Object Box Totally ON Screen
 *        -1 If Object Box Is Z-Clipped Or X/Y Clipped
 ******************************************************************************/
//#define LEFT_TOLERANCE 100
//#define TOP_TOLERANCE 100
//#define RIGHT_TOLERANCE 200
//#define BOTTOM_TOLERANCE 200

//#define TOLERANCE 100
#define TOLERANCE 10

extern LONG phd_left;
extern LONG phd_right;
extern LONG phd_top;
extern LONG phd_bottom;
extern LONG phd_persp;
#define DPQ_END	(20*1024)
#define phd_viewdist	DPQ_END
#define phd_zfar	(phd_viewdist<<12)
#define phd_znear		(20<<12)
#define phd_centerx		256
#define phd_centery		120

int mClipBoundingBox(WORD *bptr)
{
	MATRIX3D *m = Matrix;
	LONG minx,maxx;
	LONG miny,maxy;
	LONG minz,maxz;
	LONG zv,coord,numz,i;
	LONG vertex[8][3];
	LONG *vertptr;

	if (m->tz >= phd_zfar)
		return 0;

	minx = (LONG)*(bptr+0);
	maxx = (LONG)*(bptr+1);
	miny = (LONG)*(bptr+2);
	maxy = (LONG)*(bptr+3);
	minz = (LONG)*(bptr+4);
	maxz = (LONG)*(bptr+5);

	vertex[0][0] = minx;		// V.0
	vertex[0][1] = miny;
	vertex[0][2] = minz;

	vertex[1][0] = maxx;		// V.1
	vertex[1][1] = miny;
	vertex[1][2] = minz;

	vertex[2][0] = maxx;		// V.2
	vertex[2][1] = maxy;
	vertex[2][2] = minz;

	vertex[3][0] = minx;		// V.3
	vertex[3][1] = maxy;
	vertex[3][2] = minz;

	vertex[4][0] = minx;		// V.4
	vertex[4][1] = miny;
	vertex[4][2] = maxz;

	vertex[5][0] = maxx;		// V.5
	vertex[5][1] = miny;
	vertex[5][2] = maxz;

	vertex[6][0] = maxx;		// V.6
	vertex[6][1] = maxy;
	vertex[6][2] = maxz;

	vertex[7][0] = minx;		// V.7
	vertex[7][1] = maxy;
	vertex[7][2] = maxz;

	numz = 0;
	minx = miny =  0x3fffffff;
	maxx = maxy = -0x3fffffff;
	vertptr = &vertex[0][0];

	for ( i=0; i<8; i++,vertptr+=3 )
	{
	// First Work Out Z view-frame Coordinate...
		zv = (LONG)((m->m20 * vertptr[0]) +
				  (m->m21 * vertptr[1]) +
				  (m->m22 * vertptr[2]) +
				   m->tz);

		if (zv > phd_znear)
		{
			numz++;
			zv /= phd_persp;

		// Calculate X coordinate....
			coord = (LONG)(((m->m00 * vertptr[0]) +
						 (m->m01 * vertptr[1]) +
						 (m->m02 * vertptr[2]) +
						  m->tx) / zv);

			if (coord < minx)
				minx = coord;

			if (coord > maxx)
				maxx = coord;

		// Calculate Y coordinate....
			coord = (LONG)(((m->m10 * vertptr[0]) +
						 (m->m11 * vertptr[1]) +
						 (m->m12 * vertptr[2]) +
						  m->ty) / zv);

			if (coord < miny)
				miny = coord;

			if (coord > maxy)
				maxy = coord;
		}
	}
	minx += phd_centerx;							// Add on Screen Offsets
	maxx += phd_centerx;                            // to MinMax
	miny += phd_centery;
	maxy += phd_centery;

//	if ( !numz ||     								// If No Coords
//		 minx>phd_right || miny>phd_bottom ||       // Are On Screen Exit Now
//		 maxx<phd_left  || maxy<phd_top	)
//			return(0);

//	if ( numz<8 ||
//		 minx<(phd_left-LEFT_TOLERANCE)  || miny<(phd_top-TOP_TOLERANCE) ||			// Bounding Box is partially
//		 maxx>(phd_right+RIGHT_TOLERANCE) || maxy>(phd_bottom+BOTTOM_TOLERANCE))	// On screen or/and Z-Clipped...
//			return(-1);

	if ( !numz ||     								// If No Coords
		 minx>(phd_right+TOLERANCE) || miny>(phd_bottom+TOLERANCE) ||       // Are On Screen Exit Now
		 maxx<(phd_left-TOLERANCE)  || maxy<(phd_top-TOLERANCE)	)
			return(0);

	if ( numz<8 ||
		 minx<(phd_left-TOLERANCE)  || miny<(phd_top-TOLERANCE) ||			// Bounding Box is partially
		 maxx>(phd_right+TOLERANCE) || maxy>(phd_bottom+TOLERANCE))	// On screen or/and Z-Clipped...
			return(-1);

	return(1);										// Return 1 If Box Wholly On Screen...
}

#endif

void	InterpolateMatrix()
{
	LONG frac, rate;
	MATRIX3D dest, *d, *m, *i;

	m = Matrix;
	i = iMatrix;
	d = &dest;

	frac = iFrac;
	rate = iRate;

	if ((rate == 2)
	|| ((frac == 2) && (rate == 4)))
	{
		d->m00 = (i->m00 + m->m00) >> 1;
		d->m01 = (i->m01 + m->m01) >> 1;
		d->m02 = (i->m02 + m->m02) >> 1;

		d->m10 = (i->m10 + m->m10) >> 1;
		d->m11 = (i->m11 + m->m11) >> 1;
		d->m12 = (i->m12 + m->m12) >> 1;

		d->m20 = (i->m20 + m->m20) >> 1;
		d->m21 = (i->m21 + m->m21) >> 1;
		d->m22 = (i->m22 + m->m22) >> 1;

		d->tx = (i->tx + m->tx) >> 1;
		d->ty = (i->ty + m->ty) >> 1;
		d->tz = (i->tz + m->tz) >> 1;
	}

	else if (frac == 1)
	{
		d->m00 = m->m00 + ((i->m00 - m->m00) >> 2);
		d->m01 = m->m01 + ((i->m01 - m->m01) >> 2);
		d->m02 = m->m02 + ((i->m02 - m->m02) >> 2);

		d->m10 = m->m10 + ((i->m10 - m->m10) >> 2);
		d->m11 = m->m11 + ((i->m11 - m->m11) >> 2);
		d->m12 = m->m12 + ((i->m12 - m->m12) >> 2);

		d->m20 = m->m20 + ((i->m20 - m->m20) >> 2);
		d->m21 = m->m21 + ((i->m21 - m->m21) >> 2);
		d->m22 = m->m22 + ((i->m22 - m->m22) >> 2);

		d->tx = m->tx + ((i->tx - m->tx) >> 2);
		d->ty = m->ty + ((i->ty - m->ty) >> 2);
		d->tz = m->tz + ((i->tz - m->tz) >> 2);
	}

	else
	{
		d->m00 = i->m00 - ((i->m00 - m->m00) >> 2);
		d->m01 = i->m01 - ((i->m01 - m->m01) >> 2);
		d->m02 = i->m02 - ((i->m02 - m->m02) >> 2);

		d->m10 = i->m10 - ((i->m10 - m->m10) >> 2);
		d->m11 = i->m11 - ((i->m11 - m->m11) >> 2);
		d->m12 = i->m12 - ((i->m12 - m->m12) >> 2);

		d->m20 = i->m20 - ((i->m20 - m->m20) >> 2);
		d->m21 = i->m21 - ((i->m21 - m->m21) >> 2);
		d->m22 = i->m22 - ((i->m22 - m->m22) >> 2);

		d->tx = i->tx - ((i->tx - m->tx) >> 2);
		d->ty = i->ty - ((i->ty - m->ty) >> 2);
		d->tz = i->tz - ((i->tz - m->tz) >> 2);
	}

	mLoadMatrix(d);
	mResetCols();
}

void	InterpolateArmMatrix(MATRIX3D *arm)
{
	LONG frac, rate;
	MATRIX3D dest, *d, *m, *i;

	m = Matrix;
	i = iMatrix;
	d = &dest;

	frac = iFrac;
	rate = iRate;

	d->m00 = arm->m00;
	d->m01 = arm->m01;
	d->m02 = arm->m02;
	d->m10 = arm->m10;
	d->m11 = arm->m11;
	d->m12 = arm->m12;
	d->m20 = arm->m20;
	d->m21 = arm->m21;
	d->m22 = arm->m22;

	if ((rate == 2)
	|| ((frac == 2) && (rate == 4)))
	{
		d->tx = (i->tx + m->tx) >> 1;
		d->ty = (i->ty + m->ty) >> 1;
		d->tz = (i->tz + m->tz) >> 1;
	}

	else if (frac == 1)
	{
		d->tx = m->tx + ((i->tx - m->tx) >> 2);
		d->ty = m->ty + ((i->ty - m->ty) >> 2);
		d->tz = m->tz + ((i->tz - m->tz) >> 2);
	}

	else
	{
		d->tx = i->tx - ((i->tx - m->tx) >> 2);
		d->ty = i->ty - ((i->ty - m->ty) >> 2);
		d->tz = i->tz - ((i->tz - m->tz) >> 2);
	}

	mLoadMatrix(d);
	mResetCols();
}













