/************************************************************************************
	GLOVER PSX	(c) 1998-9 ISL

	quatern.c:		Quaternion rotation routines

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

#include "glover.h"

#define IQEPSILON	4096
#define IQHALFPI	6434

/**************************************************************************
	FUNCTION:	quaternionGetMatrix()
	PURPOSE:	Convert quarternion to rotation matrix
	PARAMETERS:	Source quaternion, dest matrix
	RETURNS:	
**************************************************************************/

void quaternionGetMatrix(IQUATERNION *squat, MATRIX *dmatrix)
{
	LONG	s, xs,ys,zs, wx,wy,wz, xx,xy,xz, yy,yz,zz,tempcalc;
	LONG	squatw;
	VECTOR	sqrin, sqrout;

	sqrin.vx = squat->x;
	sqrin.vy = squat->y;
	sqrin.vz = squat->z;

	gte_ldlvl(&sqrin);
	squatw = squat->w;
	gte_nop();
	gte_sqr0_b();
	squatw = squatw * squatw;
	gte_stlvnl(&sqrout);

	tempcalc = ((sqrout.vx + sqrout.vy + sqrout.vz + squatw) >> 12);
	squatw = squat->w;
	
	s = (tempcalc > 1) ? (33554432 / tempcalc) : (33554432);
	
	xs = (sqrin.vx * s) >> 12;
	ys = (sqrin.vy * s) >> 12;
	zs = (sqrin.vz * s) >> 12;

	wx = (squatw * xs) >> 12;
	wy = (squatw * ys) >> 12;
	wz = (squatw * zs) >> 12;

	xx = (sqrin.vx * xs) >> 12;
	xy = (sqrin.vx * ys) >> 12;
	xz = (sqrin.vx * zs) >> 12;

	yy = (sqrin.vy * ys) >> 12;
	yz = (sqrin.vy * zs) >> 12;
	zz = (sqrin.vz * zs) >> 12;


	dmatrix->m[0][0] = 4096 - (yy + zz);
	dmatrix->m[0][1] = xy + wz;
	dmatrix->m[0][2] = xz - wy;

	dmatrix->m[1][0] = xy - wz;
	dmatrix->m[1][1] = 4096 - (xx + zz);
	dmatrix->m[1][2] = yz + wx;

	dmatrix->m[2][0] = xz + wy;
	dmatrix->m[2][1] = yz - wx;
	dmatrix->m[2][2] = 4096 - (xx + yy);

	//doesn't actually use any of these
  //	dmatrix->matrix[0][3] = 0;
  //	dmatrix->matrix[1][3] = 0;
  //	dmatrix->matrix[2][3] = 0;
  //	dmatrix->matrix[3][3] = 4096;
  //	dmatrix->matrix[3][0] = 0;
  //	dmatrix->matrix[3][1] = 0;
  //	dmatrix->matrix[3][2] = 0;
}
/**********************************************************************************************/
void quaternionSlerpMatrix(IQUATERNION *src1, IQUATERNION *sp2, ULONG t,MATRIX *dmatrix)
{
	LONG		s, xs,ys,zs, wx,wy,wz, xx,xy,xz, yy,yz,zz,tempcalc;
	LONG		w;
	VECTOR		sqrin,sqrout;
	VECTOR		source,dest;
	LONG		cosom;

	source.vx = src1->x;
	source.vy = src1->y;
	source.vz = src1->z;

    // calc cosine
	cosom = (source.vx * sp2->x + source.vy * sp2->y + source.vz * sp2->z
	+ src1->w * sp2->w);

    // adjust signs (if necessary)
	
	if (cosom < 0)
	{
		cosom = -cosom;
		dest.vx = - sp2->x;
		dest.vy = - sp2->y;
		dest.vz = - sp2->z;
		w = - sp2->w;
	}
	else
	{
		dest.vx = sp2->x;
		dest.vy = sp2->y;
		dest.vz = sp2->z;
		w = sp2->w;
    }
    
	// "from" and "to" quaternions are very close 
	//  ... so we can do a linear interpolation
	
	gte_ldlvl(&source);		// load source
	gte_ldfc(&dest);		// load dest
	gte_lddp(t);			// load interpolant
	gte_intpl();			// interpolate (8 cycles)
	
	source.vx = src1->w;	// put source w into source
	dest.vx = w;			// put dest w into dest
		
	gte_stlvnl(&sqrin);		// put interpolated x y & z into sqrin

	gte_ldlvl(&source);		// load source w
	gte_ldfc(&dest);		// load dest w
	gte_intpl();			// interpolate (8 cycles)
	gte_stlvnl(&dest);		// put interpolated w into dest

	gte_ldlvl(&sqrin);		// load interpolated x y & z
	gte_sqr0();				// square (5 cycles)
	gte_stlvnl(&sqrout);	// put into sqrout

	gte_ldlvl(&dest);		// load interpolated w
	gte_sqr0();				// square (5 cycles)

	tempcalc = sqrout.vx + sqrout.vy + sqrout.vz;

	gte_stlvnl(&source);	// put into source

	tempcalc = ((tempcalc + source.vx) >> 12);
		
	s = (tempcalc > 1) ? (33554432 / tempcalc) : (33554432);
	
	xs = (sqrin.vx * s) >> 12;
	ys = (sqrin.vy * s) >> 12;
	zs = (sqrin.vz * s) >> 12;

	wx = (dest.vx * xs) >> 12;
	wy = (dest.vx * ys) >> 12;
	wz = (dest.vx * zs) >> 12;

	xx = (sqrin.vx * xs) >> 12;
	xy = (sqrin.vx * ys) >> 12;
	xz = (sqrin.vx * zs) >> 12;

	yy = (sqrin.vy * ys) >> 12;
	yz = (sqrin.vy * zs) >> 12;
	zz = (sqrin.vz * zs) >> 12;

	dmatrix->m[0][0] = 4096 - (yy + zz);
	dmatrix->m[0][1] = xy + wz;
	dmatrix->m[0][2] = xz - wy;

	dmatrix->m[1][0] = xy - wz;
	dmatrix->m[1][1] = 4096 - (xx + zz);
	dmatrix->m[1][2] = yz + wx;

	dmatrix->m[2][0] = xz + wy;
	dmatrix->m[2][1] = yz - wx;
	dmatrix->m[2][2] = 4096 - (xx + yy);
}

/**************************************************************************
	FUNCTION:	quaternionGetMatrix()
	PURPOSE:	Convert quarternion to rotation matrix
	PARAMETERS:	Source quaternion, dest matrix
	RETURNS:	
**************************************************************************/

void ShortquaternionGetMatrix(SHORTQUAT *squat, MATRIX *dmatrix)
{
	LONG	s, xs,ys,zs, wx,wy,wz, xx,xy,xz, yy,yz,zz,tempcalc;
	LONG	squatw;
	VECTOR	sqrin, sqrout;

	sqrin.vx = squat->x;
	sqrin.vy = squat->y;
	sqrin.vz = squat->z;

	gte_ldlvl(&sqrin);
	squatw = squat->w;
	gte_nop();
	gte_sqr0_b();
	squatw = squatw * squatw;
	gte_stlvnl(&sqrout);

	tempcalc = ((sqrout.vx + sqrout.vy + sqrout.vz + squatw) >> 12);
	squatw = squat->w;
	
	s = (tempcalc > 1) ? (33554432 / tempcalc) : (33554432);
	
	xs = (sqrin.vx * s) >> 12;
	ys = (sqrin.vy * s) >> 12;
	zs = (sqrin.vz * s) >> 12;

	wx = (squatw * xs) >> 12;
	wy = (squatw * ys) >> 12;
	wz = (squatw * zs) >> 12;

	xx = (sqrin.vx * xs) >> 12;
	xy = (sqrin.vx * ys) >> 12;
	xz = (sqrin.vx * zs) >> 12;

	yy = (sqrin.vy * ys) >> 12;
	yz = (sqrin.vy * zs) >> 12;
	zz = (sqrin.vz * zs) >> 12;


	dmatrix->m[0][0] = 4096 - (yy + zz);
	dmatrix->m[0][1] = xy + wz;
	dmatrix->m[0][2] = xz - wy;

	dmatrix->m[1][0] = xy - wz;
	dmatrix->m[1][1] = 4096 - (xx + zz);
	dmatrix->m[1][2] = yz + wx;

	dmatrix->m[2][0] = xz + wy;
	dmatrix->m[2][1] = yz - wx;
	dmatrix->m[2][2] = 4096 - (xx + yy);

	//doesn't actually use any of these
  //	dmatrix->matrix[0][3] = 0;
  //	dmatrix->matrix[1][3] = 0;
  //	dmatrix->matrix[2][3] = 0;
  //	dmatrix->matrix[3][3] = 4096;
  //	dmatrix->matrix[3][0] = 0;
  //	dmatrix->matrix[3][1] = 0;
  //	dmatrix->matrix[3][2] = 0;
}
/*************************************************************************************************/
void ShortquaternionSlerpMatrix(SHORTQUAT *src1, SHORTQUAT *sp2, ULONG t,MATRIX *dmatrix)
{
	LONG		s, xs,ys,zs, wx,wy,wz, xx,xy,xz, yy,yz,zz,tempcalc;
	LONG		w;
	VECTOR		sqrin,sqrout;
	VECTOR		source,dest;
	LONG		cosom;

	source.vx = src1->x;
	source.vy = src1->y;
	source.vz = src1->z;

    // calc cosine
	cosom = (source.vx * sp2->x + source.vy * sp2->y + source.vz * sp2->z
	+ src1->w * sp2->w);

    // adjust signs (if necessary)
	
	if (cosom < 0)
	{
		cosom = -cosom;
		dest.vx = - sp2->x;
		dest.vy = - sp2->y;
		dest.vz = - sp2->z;
		w = - sp2->w;
	}
	else
	{
		dest.vx = sp2->x;
		dest.vy = sp2->y;
		dest.vz = sp2->z;
		w = sp2->w;
    }
    
	// "from" and "to" quaternions are very close 
	//  ... so we can do a linear interpolation
	
	gte_ldlvl(&source);		// load source
	gte_ldfc(&dest);		// load dest
	gte_lddp(t);			// load interpolant
	gte_intpl();			// interpolate (8 cycles)
	
	source.vx = src1->w;	// put source w into source
	dest.vx = w;			// put dest w into dest
		
	gte_stlvnl(&sqrin);		// put interpolated x y & z into sqrin

	gte_ldlvl(&source);		// load source w
	gte_ldfc(&dest);		// load dest w
	gte_intpl();			// interpolate (8 cycles)
	gte_stlvnl(&dest);		// put interpolated w into dest

	gte_ldlvl(&sqrin);		// load interpolated x y & z
	gte_sqr0();				// square (5 cycles)
	gte_stlvnl(&sqrout);	// put into sqrout

	gte_ldlvl(&dest);		// load interpolated w
	gte_sqr0();				// square (5 cycles)

	tempcalc = sqrout.vx + sqrout.vy + sqrout.vz;

	gte_stlvnl(&source);	// put into source

	tempcalc = ((tempcalc + source.vx) >> 12);
		
	s = (tempcalc > 1) ? (33554432 / tempcalc) : (33554432);
	
	xs = (sqrin.vx * s) >> 12;
	ys = (sqrin.vy * s) >> 12;
	zs = (sqrin.vz * s) >> 12;

	wx = (dest.vx * xs) >> 12;
	wy = (dest.vx * ys) >> 12;
	wz = (dest.vx * zs) >> 12;

	xx = (sqrin.vx * xs) >> 12;
	xy = (sqrin.vx * ys) >> 12;
	xz = (sqrin.vx * zs) >> 12;

	yy = (sqrin.vy * ys) >> 12;
	yz = (sqrin.vy * zs) >> 12;
	zz = (sqrin.vz * zs) >> 12;

	dmatrix->m[0][0] = 4096 - (yy + zz);
	dmatrix->m[0][1] = xy + wz;
	dmatrix->m[0][2] = xz - wy;

	dmatrix->m[1][0] = xy - wz;
	dmatrix->m[1][1] = 4096 - (xx + zz);
	dmatrix->m[1][2] = yz + wx;

	dmatrix->m[2][0] = xz + wy;
	dmatrix->m[2][1] = yz - wx;
	dmatrix->m[2][2] = 4096 - (xx + yy);
}
/*	--------------------------------------------------------------------------------
	Function 	: RotateVectorByQuaternion
	Purpose 	: 
	Parameters 	: 
	Returns 	: 
	Info 		: destination CAN be same as source
*/

void RotateVectorByQuaternion(VECTOR *result,VECTOR *vect,IQUATERNION *q)
{
	IQUATERNION rot;

	GetRotationFromQuaternion(&rot,q);
	RotateVectorByRotation(result,vect,&rot);
}
/*	--------------------------------------------------------------------------------
	Function 	: RotateVectorByQuaternion
	Purpose 	: 
	Parameters 	: 
	Returns 	: 
	Info 		: destination CAN be same as source (this routine at the moment does not appear to work
*/
void RotateVectorByRotation(VECTOR *result,VECTOR *vect,IQUATERNION *rot)
{
	LONG	m,n,sinTheta,cosTheta;
	VECTOR	mVec,pVec,vVec;

	m = vect->vx*rot->x;
	m += vect->vy*rot->y;
	m += vect->vz*rot->z;

	// quats are in 4096 format

	m/=4096;	// I think this is needed

	mVec.vx = (m*rot->x)/4096;
	mVec.vy = (m*rot->y)/4096;
	mVec.vz = (m*rot->z)/4096;

	SUBVECTOR(&pVec,vect,&mVec);
	
	// This version does scale down
	CrossProduct(&vVec,(VECTOR *)rot,&pVec);

	m = Magnitude(&pVec);

	if(m == 0){
		COPYVECTOR(result,vect);
	}
	else
	{
		n = Magnitude(&vVec);
		if(n)
		{
			m = m*4096;
			m /= n;
			SCALEVECTOR(&vVec,m);
		}

		cosTheta = rcos(rot->w);	// could the problem be a radians to fixed problem?
		sinTheta = rsin(rot->w);

		result->vz = (mVec.vx + ((cosTheta * pVec.vx)/4096) + ((sinTheta * vVec.vx)/4096) );
		result->vy = (mVec.vy + ((cosTheta * pVec.vy)/4096) + ((sinTheta * vVec.vy)/4096) );
		result->vz = (mVec.vz + ((cosTheta * pVec.vz)/4096) + ((sinTheta * vVec.vz)/4096) );

	}
}

/*	--------------------------------------------------------------------------------
	Function 	: GetRotationFromQuaternion()
	Purpose 	: converts a quaternion into a rotation of theta degrees about an axis
	Parameters 	: destination, source
	Returns 	: nun
	Info 		:
*/
void GetRotationFromQuaternion(IQUATERNION *destQ,IQUATERNION *srcQ)
{
	LONG 	theta,sinThetaOver2,m;
	VECTOR	tempVect;

	theta = 2 * arccos(srcQ->w);
	sinThetaOver2 = rsin(theta/2);
	
	destQ->w = theta;
	if(sinThetaOver2)
	{
		destQ->x = (srcQ->x*4096)/sinThetaOver2;
		destQ->y = (srcQ->y*4096)/sinThetaOver2;
		destQ->z = (srcQ->z*4096)/sinThetaOver2;
		
		tempVect.vx=destQ->x;
		tempVect.vy=destQ->y;
		tempVect.vz=destQ->z;

		m = Magnitude(&tempVect);
		if(m)
		{
			destQ->x = (destQ->x*4096)/m;
			destQ->y = (destQ->y*4096)/m;
			destQ->z = (destQ->z*4096)/m;
		}
	}
	else
	{
		destQ->x = 0;
		destQ->y = 4096;
		destQ->z = 0;
	}
}

/*	--------------------------------------------------------------------------------
	Function 	: GetQuaternionFromRotation()
	Purpose 	: converts from a rotation of theta about an axis to a quaternion
	Parameters 	: dest, source
	Returns 	: nun
	Info 		:
*/
void GetQuaternionFromRotation(IQUATERNION *destQ,IQUATERNION *srcQ)
{
	LONG	thetaOver2;
	LONG	sinThetaOver2;

	thetaOver2 = srcQ->w/2;
	sinThetaOver2 = rsin(thetaOver2);

	destQ->w = rcos(thetaOver2);
	destQ->x = (sinThetaOver2 * srcQ->x)/4096;
	destQ->y = (sinThetaOver2 * srcQ->y)/4096;
	destQ->z = (sinThetaOver2 * srcQ->z)/4096;
}

/*	--------------------------------------------------------------------------------
	Function 	: QuaternionMultiply()
	Purpose 	: 
	Parameters 	: 
	Returns 	: 
	Info 		: destination CAN be same as source
*/

void QuaternionMultiply(IQUATERNION *dest,IQUATERNION *src1,IQUATERNION *src2)
{
	LONG	dp;
	IQUATERNION temp;

	dp = (src1->x*src2->x + src1->y*src2->y + src1->z*src2->z);

// For the benefit of people doing optimisations and breaking things,
// I'll reiterate the point made in the function header
// DESTINATION CAN BE THE SAME AS SOURCE, Hence the temp store (I'll add a dest != src version)

	temp.x = (src1->w*src2->x + src2->w*src1->x + src1->y*src2->z - src1->z*src2->y)/4096;
	temp.y = (src1->w*src2->y + src2->w*src1->y + src1->z*src2->x - src1->x*src2->z)/4096;
	temp.z = (src1->w*src2->z + src2->w*src1->z + src1->x*src2->y - src1->y*src2->x)/4096;

	dest->w= (src1->w*src2->w - dp)/4096;
	dest->x=temp.x;
	dest->y=temp.y;
	dest->z=temp.z;
}


/* Faster Quat multiply, where dest != src */

void QuaternionMultiplyDestNESrc(IQUATERNION *dest,IQUATERNION *src1,IQUATERNION *src2)
{
	LONG	dp;
//	IQUATERNION temp;
	dp = (src1->x*src2->x + src1->y*src2->y + src1->z*src2->z);
	dest->w= (src1->w*src2->w - dp)/4096;
	dest->x= (src1->w*src2->x + src2->w*src1->x + src1->y*src2->z - src1->z*src2->y)/4096;
	dest->y= (src1->w*src2->y + src2->w*src1->y + src1->z*src2->x - src1->x*src2->z)/4096;
	dest->z= (src1->w*src2->z + src2->w*src1->z + src1->x*src2->y - src1->y*src2->x)/4096;
}

/*	--------------------------------------------------------------------------------
	Function 	: UnifyQuat()
	Purpose 	: 
	Parameters 	: 
	Returns 	: 
	Info 		: This routine is to stop quats rounding to zero after a while
*/

void UnifyQuat(IQUATERNION *quat)
{
	int tb,s,d;
	long n;
	long m = MagnitudeQuat(quat);

	if(m != 0)
	{
		// find top bit
		n=ABS(quat->x)|ABS(quat->y)|ABS(quat->z)|ABS(quat->w);

		for(tb=31; !(n&0x80000000); tb--)
			n=n<<1;

		if(tb>18)
		{
			s=30-tb;
			d=tb-18;
			quat->x= (quat->x<<s)/(m>>d);
			quat->y= (quat->y<<s)/(m>>d);
			quat->z= (quat->z<<s)/(m>>d);
			quat->w= (quat->w<<s)/(m>>d);
		}
		else
		{
			quat->x= (quat->x<<FRACTIONAL_BITS)/m;
			quat->y= (quat->y<<FRACTIONAL_BITS)/m;
			quat->z= (quat->z<<FRACTIONAL_BITS)/m;
			quat->w= (quat->w<<FRACTIONAL_BITS)/m;
		}
	}
}

/*	--------------------------------------------------------------------------------
	Function 	: MagnitudeQuat()
	Purpose 	: 
	Parameters 	: 
	Returns 	: 
	Info 		: This routine is to stop quats rounding to zero after a while
*/


long MagnitudeQuat(IQUATERNION *v)
{
	IQUATERNION u;
	long x,y,z,w;
	ULONG l,calc;
	long num;
	int up;
	int s=0;

	num=0;

	l=ABS(v->x);
	num|=l;
	u.x=l;

	l=ABS(v->y);
	num|=l;
	u.y=l;

	l=ABS(v->z);
	num|=l;
	u.z=l;

	l=ABS(v->w);
	num|=l;
	u.w=l;


	while(num > 26754)
	{
		num = num>>2;
		s+=2;
	}

	x=(u.x)>>s;
	y=(u.y)>>s;
	z=(u.z)>>s;
	w=(u.w)>>s;

	calc=x*x+y*y+z*z+w*w;
	FASTSQRT(l,calc);
	up=16-s;
	if(up<0)
	{
//		DB("MAGNITUDE: -ve shift\n");
//		DB("Input = (%d,%d,%d)\n", v->x, v->y, v->z);
	}
	else
		l=l>>up;

	return(l);
}
