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

		POLHEMUS PROPRIETARY

		Polhemus 
		P.O. Box 560
		Colchester, Vermont 05446
		(802) 655-3159



        		
	    Copyright © 2005 by Polhemus
		All Rights Reserved.


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

// Quaternion.cpp: implementation of the CQuaternion class.
//
//////////////////////////////////////////////////////////////////////

#include "Quaternion.h"
#include <math.h>


//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

const float PI=3.14159265f;
const float DEG2RADS=PI/180.0f;



// Function name	: CQuaternion::CQuaternion
// Description	    : Default Constructor -- Orientaton 0,0,0
// Return type		: 
CQuaternion::CQuaternion()
{
	q0=1.0f;
	q1=q2=q3=0.0f;
}

CQuaternion::~CQuaternion()
{

}


// Function name	: CQuaternion::CQuaternion
// Description	    : Constructs class from array of four floats
// Argument         : float q[]
CQuaternion::CQuaternion(const float q[])
{
	q0=q[0];
	q1=q[1];
	q2=q[2];
	q3=q[3];
	Normalize();

}


// Function name	: CQuaternion::CQuaternion
// Description	    : Constructs class from 4 individual floats
// Argument         : float quat0
// Argument         : float quat1
// Argument         : float quat2
// Argument         : float quat3
CQuaternion::CQuaternion(float quat0, float quat1, float quat2, float quat3)
{
	q0=quat0;
	q1=quat1;
	q2=quat2;
	q3=quat3;
	Normalize();
}


// Function name	: CQuaternion::CQuaternion
// Description	    : Constructs class from az,el,roll in either deg or radians
// Argument         : float az
// Argument         : float el
// Argument         : float rl
// Argument         : bool deg - bool to indicate that angles are in degrees.  Default is true
CQuaternion::CQuaternion(float az, float el, float rl,bool deg)
{
	float eul[3]={az,el,rl};
	SetFromEulers(eul,deg);

}


// Function name	: CQuaternion::CQuaternion
// Description	    : Constructs class from an axis and an angle of rotation about that axis
// Argument         : float vect[] - 3 values representing the vector which is the axis of rotation
// Argument         : float angle - The angle to rotate about vect.  Degrees or Radians
// Argument         : bool deg - bool to indicate that angles are in degrees.  Default is true
CQuaternion::CQuaternion(const float vect[], float angle, bool deg)
{
	if (deg)
		angle*=DEG2RADS;

	float halfAng=angle/2.0f;

	q0=(float)cos(halfAng);
	q1=vect[0]*(float)sin(halfAng);
	q2=vect[1]*(float)sin(halfAng);
	q3=vect[2]*(float)sin(halfAng);

	Normalize();
}



// Function name	: CQuaternion::GetEuler
// Description	    : Returns the Euler angles associated with this quaternion in deg or rads
// Return type		: void 
// Argument         : float& az - azimuth returned here.
// Argument         : float& el - elevation returned here.
// Argument         : float& rl - roll returned here.
// Argument         : bool deg - If true returned angles are in degrees, otherwise radians.  Default is true
void CQuaternion::GetEuler(float& az, float& el, float& rl,bool deg) const
{
	float mat[3][3];
	float sinAz,sinEl,sinRl,cosAz,cosEl,cosRl;

	// create orthogonal Attitude matrix
	GetAttMat(mat);

	sinEl=-mat[2][0];
	cosEl=(float)sqrt(1.0f-(sinEl*sinEl));

	if (fabs(cosEl)<0.001f){
		sinAz=0.0f;
		cosAz=1.0f;
	}

	else {
		sinAz=mat[1][0]/cosEl;
		cosAz=mat[0][0]/cosEl;
	}

	sinRl=sinAz*mat[0][2]-cosAz*mat[1][2];
	cosRl=-sinAz*mat[0][1]+cosAz*mat[1][1];
	
	az=(float)atan2((double)sinAz,(double)cosAz);
	el=(float)atan2((double)sinEl,(double)cosEl);
	rl=(float)atan2((double)sinRl,(double)cosRl);

	if (deg){			// convert to degrees
		az/=DEG2RADS;
		el/=DEG2RADS;
		rl/=DEG2RADS;
	}

}


// Function name	: CQuaternion::GetEuler
// Description	    : Returns the Euler angles associated with this quaternion in deg or rads
// Return type		: void 
// Argument         : float aer[] - array of 3 floats where az,el,roll are returned.
// Argument         : bool deg - If true returned angles are in degrees, otherwise radians.  Default is true
void CQuaternion::GetEuler(float aer[],bool deg) const
{
	GetEuler(aer[0],aer[1],aer[2],deg);
}


// Function name	: CQuaternion::Normalize
// Description	    : normalizes the quat to a unit quaternion
// Return type		: void 
void CQuaternion::Normalize()
{
	float mag=(float)sqrt(q0*q0+q1*q1+q2*q2+q3*q3);

	if (q0<0.0f)
		mag*=-1.0f;		// make first entry pos
	
		q0/=mag;
		q1/=mag;
		q2/=mag;
		q3/=mag;

}


// Function name	: CQuaternion::GetAttMat
// Description	    : Returns the attitude matrix associated with this quaternion
// Return type		: void 
// Argument         : float mat[][3] - 3x3 float array where att matrix is returned.
void CQuaternion::GetAttMat(float mat[][3]) const
{
	// create orthogonal Attitude matrix
	mat[0][0]=q0*q0+q1*q1-q2*q2-q3*q3;
	mat[0][1]=2*(q1*q2-q0*q3);
	mat[0][2]=2*(q1*q3+q0*q2);
	mat[1][0]=2*(q0*q3+q1*q2);
	mat[1][1]=q0*q0-q1*q1+q2*q2-q3*q3;
	mat[1][2]=2*(q2*q3-q0*q1);
	mat[2][0]=2*(q1*q3-q0*q2);
	mat[2][1]=2*(q0*q1+q2*q3);
	mat[2][2]=q0*q0-q1*q1-q2*q2+q3*q3;

}






// Function name	: *
// Description	    : multiplies two quaternion.
// Return type		: CQuaternion - The product of the multiplication. 
// Argument         : const CQuaternion& quat2 - The quat to mult this quat by.
CQuaternion CQuaternion::operator *(const CQuaternion& quat2) const
{

	CQuaternion prod;

	prod.q0 = q0*quat2.q0 - q1*quat2.q1 - q2*quat2.q2 - q3*quat2.q3;
	prod.q1 = q0*quat2.q1 + q1*quat2.q0 + q2*quat2.q3 - q3*quat2.q2;
	prod.q2 = q0*quat2.q2 - q1*quat2.q3 + q2*quat2.q0 + q3*quat2.q1;
	prod.q3 = q0*quat2.q3 + q1*quat2.q2 - q2*quat2.q1 + q3*quat2.q0;
	
	prod.Normalize();
	return prod;
}

// Function name	: *
// Description	    : Multiplies quaternion by a scaler
// Return type		: CQuaternion -- the product 
// Argument         : const float scaler
CQuaternion CQuaternion::operator *(const float scaler) const
{
	CQuaternion prod;
	prod.q0=q0*scaler;
	prod.q1=q1*scaler;
	prod.q2=q2*scaler;
	prod.q3=q3*scaler;

	return prod;
}



// Function name	: *=
// Description	    : Multiply and assign eg.  thisQuat*=thatQuat--->thisQuat=thisQuat*thatQuat
// Return type		: void  
// Argument         : const CQuaternion &quat - The quat to mult this quat by.
void CQuaternion::operator *=(const CQuaternion &quat)
{
	CQuaternion prod=*this * quat;
	*this=prod;
}


// Function name	: CQuaternion::GetAxisAngle
// Description	    : Returns the axis and angle of rotation that is associated with this quaternion 
// Return type		: void 
// Argument         : float vect[] - array of 3 floats to receive vector representing the axis
// Argument         : float &angle - reference to received the angle.
// Argument         : bool deg - if true angle will be in degrees, otherwise radians.  Default is true.
void CQuaternion::GetAxisAngle(float vect[], float &angle, bool deg) const
{
	angle=2.0f*(float)acos(q0);
	if (deg)
		angle/=DEG2RADS;

	float scale=(float)sqrt(q1*q1+q2*q2+q3*q3);

	if (fabs(scale)<0.001f){		// infinite axis, set to no rotation
		angle=0.0f;
		vect[0]=1.0f;
		vect[1]=vect[2]=0.0f;
		return;
	}

	vect[0]=q1/scale;
	vect[1]=q2/scale;
	vect[2]=q3/scale;

	scale=(float)sqrt(vect[0]*vect[0]+vect[1]*vect[1]+vect[2]*vect[2]);
	for (int i=0;i<3;i++)
		vect[i]/=scale;
}


// Function name	: - 
// Description	    : Returns inverse of quaternion.  eg -thisQuat --> InvthisQuat
// Return type		: CQuaternion - The inverse of this quaternion 
CQuaternion CQuaternion::operator -() const
{
	CQuaternion inv=*this;
	inv.q1*=-1;
	inv.q2*=-1;
	inv.q3*=-1;
	return inv;
}



// Function name	: CQuaternion::GetDeltaQuat
// Description	    : Returns the quaternion that represents the difference between two quaternions
// Return type		: CQuaternion - the delta quaternion
// Argument         : const CQuaternion &quat - reference to a quaternion to measure the difference between
CQuaternion CQuaternion::GetDeltaQuat(const CQuaternion &quat) const
{
/*	CQuaternion inv=-(*this);
	return inv*quat;
*/
	CQuaternion inv=-quat;
	return *this * inv;

}


// Function name	: - 
// Description	    : Also retrieves the delta quat. eg.  thisQuat-thatQuat --> GetDeltaQuat(thatQuat)
//					  Yes this is really a multiplicaton process, but it's kind of intuitive.
// Return type		: CQuaternion CQuaternion::operator 
// Argument         : const CQuaternion &quat
CQuaternion CQuaternion::operator -(const CQuaternion &quat) const
{
	return GetDeltaQuat(quat);
}


// Function name	: CQuaternion::GetQuatVal
// Description	    : returns the four values of the quaternion
// Return type		: void 
// Argument         : float vals[]	- array of 4 floats to recieve the quat values
void CQuaternion::GetQuatVal(float vals[]) const
{
	vals[0]=q0;
	vals[1]=q1;
	vals[2]=q2;
	vals[3]=q3;
	
}


// Function name	: CQuaternion::GetQuatVal
// Description	    : Returns an individual value of the quaternion
// Return type		: float - the requested value of the quaternion
// Argument         : int ind - the index (0-3) of the value to return. Invalid values return q0.
float CQuaternion::GetQuatVal(int ind) const
{
	float retVal;
	switch (ind) {
	case 0:
	default:
		retVal=q0;
		break;
	case 1:
		retVal=q1;
		break;
	case 2:
		retVal=q2;
		break;
	case 3:
		retVal=q3;
	}

	return retVal;
}


// Function name	: CQuaternion::SetQuatVals
// Description	    : Sets the values of the quaternion as indicated.
// Return type		: void 
// Argument         : float val[] - Array of 4 floats that indicate the values to set the quaternion members to.
void CQuaternion::SetQuatVals(const float val[])
{
	SetQuatVals(val[0],val[1],val[2],val[3]);
}


// Function name	: CQuaternion::SetQuatVals
// Description	    : Sets the values of the quaternion as indicated by the individual parameters
// Return type		: void 
// Argument         : float w	- set first quaternion value to this
// Argument         : float x	- set second quaternion value to this
// Argument         : float y	- set third quaternion value to this
// Argument         : float z	- set fourth quaternion value to this
void CQuaternion::SetQuatVals(const float w, const float x, const float y, const float z)
{
	q0=w;
	q1=x;
	q2=y;
	q3=z;
	Normalize();

}

bool CQuaternion::IsIdentity()
{
	return ((q0==1.0f) && (q1==0.0f) && (q2==0.0f) && (q3==0.0f));
}

void CQuaternion::Eul2Quat(float *quat, float *eul, bool isDeg)
{
	CQuaternion q(eul[0],eul[1],eul[2],isDeg);
	q.MakeLargestElementPos();
	q.GetQuatVal(quat);
}

void CQuaternion::Quats2Eul(float *eul, float *quats, bool isDeg)
{
	CQuaternion q(quats);
	q.GetEuler(eul,isDeg);
}

void CQuaternion::MakeLargestElementPos()
{
	float max=(float)fabs(q0);
	bool bChgSign=q0<0;


	if ((float)fabs(q1)>max){
		max=(float)fabs(q1);
		bChgSign=q1<0;
	}

	if ((float)fabs(q2)>max){
		max=(float)fabs(q2);
		bChgSign=q2<0;
	}

	if ((float)fabs(q3)>max){
		max=(float)fabs(q3);
		bChgSign=q3<0;
	}

	if (bChgSign){
		q0*=-1;
		q1*=-1;
		q2*=-1;
		q3*=-1;
	}
}

void CQuaternion::SetFromEulers(float *eul, bool deg/*=true*/)
{
	float azHalf=eul[0]/2.0f;
	float elHalf=eul[1]/2.0f;
	float rollHalf=eul[2]/2.0f;

	if (deg){
		azHalf*=DEG2RADS;
		elHalf*=DEG2RADS;
		rollHalf*=DEG2RADS;
	}

	q0=(float)(cos(azHalf)*cos(elHalf)*cos(rollHalf)+sin(azHalf)*sin(elHalf)*sin(rollHalf));
	q1=(float)(cos(azHalf)*cos(elHalf)*sin(rollHalf)-sin(azHalf)*sin(elHalf)*cos(rollHalf));
	q2=(float)(cos(azHalf)*sin(elHalf)*cos(rollHalf)+sin(azHalf)*cos(elHalf)*sin(rollHalf));
	q3=(float)(sin(azHalf)*cos(elHalf)*cos(rollHalf)-cos(azHalf)*sin(elHalf)*sin(rollHalf));


	Normalize();
	
}

// Function name	: CQuaternion::Slerp
// Description	    : Spherical interpolation of quaternions.  Interpolates between
//						this quaternion and the quaternion passed in as a parameter
// Return type		: CQuaternion -- The interpolated quaternion
// Argument         : const CQuaternion &q
// Argument         : float t -- value between 0 and 1 which indicates the distance between 
//						quaternions to interpolate
CQuaternion CQuaternion::Slerp(const CQuaternion &q, float t) const
{

	//    qa*sin((1-t)theta)+qb*sin(t*theta)
	// q= ----------------------------------
	//                sin(theta)

	// 2*theta = qa.q0*qb.q0+qa.q1*qb.q1+qa.q2*qb.q2+qa.q3*qb.q3




	float angle;

	if (t>=1.0f)
		return CQuaternion(q);

	if (t<=0.0f)
		return CQuaternion(*this);


	float dot=q0*q.q0+q1*q.q1+q2*q.q2+q3*q.q3;
	angle=(float)(acos(dot))/2.0f;
	if (angle<0.0f)
		angle*=-1;

	float coeff1,coeff2;

	if (angle==0.0f){
		coeff1=1-t;
		coeff2=t;
	}
	else {

		coeff1=(float)sin((1.0f-t)*angle)/(float)sin(angle);
		coeff2=(float)sin(t*angle)/(float)sin(angle);
	}

	CQuaternion res=*this*coeff1+q*coeff2;
	res.Normalize();

	return res;
}

// Function name	: +
// Description	    : Adds to quaternions
// Return type		: CQuaternion -- the sum
// Argument         : const CQuaternion &q1
// Argument         : const CQuaternion &q2
CQuaternion CQuaternion::operator +(const CQuaternion &q) const
{
	CQuaternion sum;
	sum.q0=q0+q.q0;
	sum.q1=q1+q.q1;
	sum.q2=q2+q.q2;
	sum.q3=q3+q.q3;

	return sum;

}




void CQuaternion::GetAngle(float &a,bool deg)
{
	a=2.0f*(float)acos(q0);
	if (deg)
		a/=DEG2RADS;

}