// copyright Min Zhong, cs838 proj2, April, 2000


#include "proj2.h"
#include "RotConverter.h"

/* conv a euler(angle, x, y, z) to a quat(qw, qx, qy, qz) 
	input: angle in deg, the rot vector (xAxis, yAxis, zAxis)
	out:	q, double array has 4 elems filled.  
	ASSUME q[4] mem allocated. euler(x,y,z) is a unit vector.
*/
void eulerToQuat(double aDeg, double xAxis, double yAxis, double zAxis, double* q)
{
	double aRad_half = aDeg * PI / 180.0f / 2.0f ;
	double sin_half_a = sin(aRad_half);
	double w, x, y, z;

	w = cos(aRad_half);
	x = xAxis * sin_half_a;
	y = yAxis * sin_half_a;
	z = zAxis * sin_half_a;

/*	if ( (x< 0 || y < 0 || z < 0 ) && (w > 0) ) {
		x = -x;
		y = -y;
		z = -z;
		w = -w;
	}
*/
	q[Scalar] = w;
	q[XCoord] = x;
	q[YCoord] = y;
	q[ZCoord] = z;
}



/* resultQ = q1 * q2.  ASSUME result[4] has mem alloc before call
*/
void qt_mult(double *q1, double *q2, double *resultQ)
{
	resultQ[Scalar] = q1[Scalar] * q2[Scalar] - q1[XCoord] * q2[XCoord] - q1[YCoord] * q2[YCoord] - q1[ZCoord] * q2[ZCoord] ;
	resultQ[XCoord] = q1[Scalar] * q2[XCoord] + q1[XCoord] * q2[Scalar] + q1[YCoord] * q2[ZCoord] - q1[ZCoord] * q2[YCoord] ;
	resultQ[YCoord] = q1[Scalar] * q2[YCoord] + q1[YCoord] * q2[Scalar] + q1[ZCoord] * q2[XCoord] - q1[XCoord] * q2[ZCoord] ;
	resultQ[ZCoord] = q1[Scalar] * q2[ZCoord] + q1[ZCoord] * q2[Scalar] + q1[XCoord] * q2[YCoord] - q1[YCoord] * q2[XCoord] ;

}


/* composes 3 euler rot ez, ex, ey, into a quat resultQ.
	assume euler rot in the order of Z, X and then Y.
		ez=(zRotDeg, 0, 0, 1)
		ex=(xRotDeg, 1, 0, 0)
		ey=(yRotDeg, 0, 1, 0)
	ASSUME resultQ[4] has mem alloc before call
*/
void euler3ToQuatern(double zRotDeg, double xRotDeg, double yRotDeg, double* resultQ)

{

	double qz[4];
	double qx[4];
	double qy[4];
	double qzx[4];

	eulerToQuat(zRotDeg, 0, 0, 1, qz);
	eulerToQuat(xRotDeg, 1, 0, 0, qx);
	eulerToQuat(yRotDeg, 0, 1, 0, qy);
	
	qt_mult(qz, qx, qzx);
	qt_mult(qzx, qy, resultQ);

}

/* converts a quat q[4] to an emap m[3] */
void quatToEmap(double *q, double *m)
{

	double x, y, z;
	double sin_half;
	double aRad_half;
	double aRad;

	aRad_half = acos(q[Scalar]);
	sin_half = sin(aRad_half);
	aRad = aRad_half * 2;	// angle rotated by

	// make it 0 if too small, prevent div sin(~0)		
	if ( (aRad < (-NEAR_ZERO)) || (aRad > NEAR_ZERO) ) {
		x	= q[XCoord] / sin_half;
		y	= q[YCoord] / sin_half;
		z	= q[ZCoord] / sin_half;
	}
	else {	// won't rotate around it for angle = 0
		x	= 0;
		y	= 0;
		z	= 0;
		aRad = 0;
	}

	// emap magnitude = rot angle
	m[X] = x * aRad;
	m[Y] = y * aRad;
	m[Z] = z * aRad;
		
}




void emapToQuat(double *m, double *q)
{
	double mx	= m[X];
	double my	= m[Y];
	double mz	= m[Z];
	double aRad;		// rot ang
	double aRad_half;
	double sin_half;
	double sin_m_aRad;	// temp to speed up, = sin(a/2) * a
	double sin_d_aRad;	// temp to speed up, = sin(a/2) / a

	aRad		= sqrt(mx*mx + my*my + mz*mz);
	aRad_half	= aRad / 2.0f;
	sin_half	= sin(aRad_half);
	sin_m_aRad	= sin_half * aRad;
	sin_d_aRad	= sin_half / aRad;

	q[Scalar]	= cos(aRad_half);

	if ( (aRad < (-NEAR_ZERO) ) || (aRad > NEAR_ZERO) ) {	
		// normal case = unit rot / sin_half
		q[XCoord] = mx / sin_m_aRad;
		q[YCoord] = my / sin_m_aRad;
		q[ZCoord] = mz / sin_m_aRad;
	}
	else {	// do the sinc to keep contin.
		q[XCoord] = mx * sin_d_aRad;
		q[YCoord] = my * sin_d_aRad;
		q[ZCoord] = mz * sin_d_aRad;
	}
}


/* emap m[3] to UNIT rot axis <x,y,z> and rot angle aDeg */
void emapToRot(double *m, double &aDeg, double &x, double &y, double &z)
{
	double mx	= m[X];
	double my	= m[Y];
	double mz	= m[Z];
	double aRad;		// rot ang

	aRad		= sqrt(mx*mx + my*my + mz*mz);

	if ( (aRad<(-NEAR_ZERO)) || (aRad > NEAR_ZERO) ) {	// normal
		x = mx / aRad;
		y = my / aRad;
		z = mz / aRad;
		aDeg = aRad * 180 / PI;
	}
	else {	// make it zero
		x = 0;
		y = 0;
		z = 0;
		aDeg = 0;
	}
}


/* convert a UNIT quater (q[4]) to a rotation about vect(x,y,z) for angle aDeg.
*/
void quaternToRot(double *q, double &aDeg, double &x, double &y, double &z)
{
	
	aDeg = acos(q[Scalar]) * 2 * RadianToDegree	;	// angle rotated by

	x = q[XCoord];
	y = q[YCoord];
	z = q[ZCoord];

}





//////////////// below not used ///////////////////////////////////////



/* conv 3 euler rotations in the order of zxy, to a quaternion 
	assumes params pt to valid ptrs
*/
void EulerToQuat(double az, double ax, double ay, double *quat)
{
	double cosx, cosy, cosz, sinx, siny, sinz;	// sine, cos of half ang

	az	= az * DegreeToRadian;
	ax	= ax * DegreeToRadian;
	ay	= ay * DegreeToRadian;

	cosz = cos(az/2.0);
	cosx = cos(ax/2.0);
	cosy = cos(ay/2.0);

	sinz = sin(az/2.0);
	sinx = sin(ax/2.0);
	siny = sin(ay/2.0);

	quat[Scalar] =  cosx * cosy * cosz - sinx * siny * sinz;
	quat[XCoord] = -cosx * siny * sinz + sinx * cosy * cosz;
	quat[YCoord] =  cosx * siny * cosz + sinx * cosy * sinz;
	quat[ZCoord] =  sinx * siny * cosz + cosx * cosy * sinz;

/*	
	if (quat[Scalar] > 0 && 
		(quat[XCoord]<0 || quat[YCoord]<0 || quat[ZCoord]<0 ) ) {

		quat[Scalar] = -quat[Scalar];
		quat[XCoord] = -quat[XCoord];
		quat[YCoord] = -quat[YCoord];
		quat[ZCoord] = -quat[ZCoord];
	}
*/

}