/**
* Quaternion
* @version: 01-30-2005
* @original author: Jim 'Entropy' Hunter
* @revised by: Joseph 'Pcjoe' Florencio
*/

#include "stdafx.h"
#include "quaternion.h"

// PI
#ifndef M_PI
#define M_PI		3.14159265358979323846	// matches value in gcc v2 math.h
#endif

// Pitch/Yaw/Roll
#ifndef PITCH
#define	PITCH	0
#define	YAW		1
#define	ROLL	2 
#endif

void QAngleVectors (const float *angles, vec3_t forward, vec3_t right, vec3_t up)
{
	float		angle;
	float		sr, sp, sy, cr, cp, cy;
	
	angle = (float)(angles[YAW] * (M_PI*2 / 360));
	sy = (float)sin(angle);
	cy = (float)cos(angle);
	angle = (float)(angles[PITCH] * (M_PI*2 / 360));
	sp = (float)sin(angle);
	cp = (float)cos(angle);
	angle = (float)(angles[ROLL] * (M_PI*2 / 360));
	sr = (float)sin(angle);
	cr = (float)cos(angle);

	if (forward)
	{
		forward[0] = cp*cy;
		forward[1] = cp*sy;
		forward[2] = -sp;
	}
	if (right)
	{
		right[0] = (-1*sr*sp*cy+-1*cr*-sy);
		right[1] = (-1*sr*sp*sy+-1*cr*cy);
		right[2] = -1*sr*cp;
	}
	if (up)
	{
		up[0] = (cr*sp*cy+-sr*-sy);
		up[1] = (cr*sp*sy+-sr*cy);
		up[2] = cr*cp;
	}
}

CQuaternion::CQuaternion( )
{	CQuaternion(0,0,0,0);
}

CQuaternion::CQuaternion( const float *angles )
{	EulerToQuaternion(angles);
}

CQuaternion::CQuaternion( double dw,double dx ,double dy,double dz )
{	setQuaternion(dw,dx,dy,dz);
}

void CQuaternion::Normalize( void )
{	double mag;

	mag = w*w + x*x + y*y + z*z;
	mag = sqrt(mag);

	w /= mag;
	x /= mag;
	y /= mag;
	z /= mag;
}

void CQuaternion::Multiply( const CQuaternion q1 )
{	w = w*q1.w - x*q1.x - y*q1.y - z*q1.z;
	x = w*q1.x + x*q1.w + y*q1.z - z*q1.y;
	y = w*q1.y + y*q1.w + z*q1.x - x*q1.z;
	z = w*q1.z + z*q1.w + x*q1.y - y*q1.x;
	Normalize();
}

void CQuaternion::Conjugate( void )
{	x = -x;
	y = -y;
	z = -z;
	Normalize();
}

void CQuaternion::VectorToQuaternion( const float *vector )
{	w = 1.0 - sqrt( vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2] );
	x = vector[0];
	y = vector[1];
	z = vector[2];
	Normalize();
}

void CQuaternion::EulerToQuaternion( const float *angles )
{	double cp, cy, cr, sp, sy, sr;
	
	// need angle in radians divided by 2
	cp = cos(angles[PITCH] * M_PI / 360);
	cy = cos(angles[YAW] * M_PI / 360);
	cr = cos(angles[ROLL] * M_PI / 360);
		
	sp = sin(angles[PITCH] * M_PI / 360);
	sy = sin(angles[YAW] * M_PI / 360);
	sr = sin(angles[ROLL] * M_PI / 360);

	w = cp*cy*cr + sp*sy*sr;
	x = cp*cy*sr - sp*sy*cr;
	y = cr*sp*cy + sr*cp*sy;
	z = cr*cp*sy - sr*sp*cy;

	Normalize();
}

vec3_t CQuaternion::QuaternionToEuler( void )
{	vec3_t angles;
	double yaw, pitch, roll;
	double sqw = w*w;
	double sqx = x*x;
	double sqy = y*y;
	double sqz = z*z;

	//yaw = atan(2.0 * (x * y + z * w) / (sqx - sqy - sqz + sqw));
	yaw = atan2(2.0 * (x * y + z * w), (sqx - sqy - sqz + sqw));
	//roll = atan(2.0 * (y * z + x * w) / (-sqx - sqy + sqz + sqw));
	roll = atan2(2.0 * (y * z + x * w), (-sqx - sqy + sqz + sqw));
	pitch = asin(-2.0 * (x * z - y * w));

	//Convert angles to degrees
	angles[YAW] = (float)(yaw * 180 / M_PI);
	angles[ROLL] = (float)(roll * 180 / M_PI);
	angles[PITCH] = (float)(pitch * 180 / M_PI);
	return angles;
}

void CQuaternion::FromAxisAndAngle( const float *axis, const float angle )
{	double sa, ca;
	
	sa = sin( angle * M_PI / 360.0 );
    ca = cos( angle * M_PI / 360.0 );

    x = axis[0] * sa;
    y = axis[1] * sa;
    z = axis[2] * sa;
    w = ca;

	Normalize();
}

// FIXME: Doesn't work!
vec3_t CQuaternion::PointRotate( const float *point )
{	vec3_t result;

	CQuaternion qTemp = (*this);
	CQuaternion qIQ = (*this);
	CQuaternion qPoint(0, point[0], point[1], point[2]);
	
	qTemp.Multiply(qPoint);
	qIQ.Conjugate();
	qTemp.Multiply(qIQ);

	result[0] = (float)qTemp.x;
	result[1] = (float)qTemp.y;
	result[2] = (float)qTemp.z;
	return result;
}

void CQuaternion::QuaternionToMatrix( float (*matrix)[4] )
{	Normalize();
	matrix[0][0] = (float)(1 - 2*y*y - 2*z*z);
	matrix[1][0] = (float)(2*x*y - 2*z*w);
	matrix[2][0] = (float)(2*x*z + 2*y*w);

	matrix[0][1] = (float)(2*x*y + 2*z*w);
	matrix[1][1] = (float)(1 - 2*x*x - 2*z*z);
	matrix[2][1] = (float)(2*y*z - 2*x*w);

	matrix[0][2] = (float)(2*x*z - 2*y*w);
	matrix[1][2] = (float)(2*y*z + 2*x*w);
	matrix[2][2] = (float)(1 - 2*x*x - 2*y*y);

	matrix[0][3] = matrix[1][3] = matrix[2][3] = 0;
}

void CQuaternion::MatrixToQuaternion( float matrix[3][4] )
{	float t = 1 + matrix[0][0] + matrix[1][1] + matrix[2][2];
	float s = 0;

	// trace of matrix larger then 0, instant computation
	if ( t > 0.00000001 )
	{	s = (float)sqrt(t) * 2;
		x = ( matrix[2][1] - matrix[1][2] ) / s;
		y = ( matrix[0][2] - matrix[2][0] ) / s;
		z = ( matrix[1][0] - matrix[0][1] ) / s;
		w = 0.25 * s;
	}
	// Identify major diagnal element with greatest value
	else
	{
		// Column 0: 
		if ( matrix[0][0] > matrix[1][1] && matrix[0][0] > matrix[2][2] ) 
		{	s  = float(sqrt( 1.0 + matrix[0][0] - matrix[1][1] - matrix[2][2] ) * 2);
			x = 0.25 * s;
			y = (matrix[1][0] + matrix[0][1] ) / s;
			z = (matrix[0][2] + matrix[2][0] ) / s;
			w = (matrix[2][1] - matrix[1][2] ) / s;
		}
		// Column 1: 
		else if ( matrix[0][1] > matrix[2][2] )
		{	s  = float(sqrt( 1.0 + matrix[1][1] - matrix[0][0] - matrix[2][2] ) * 2);
			x = (matrix[1][0] + matrix[0][1] ) / s;
			y = 0.25 * s;
			z = (matrix[2][1] + matrix[1][2] ) / s;
			w = (matrix[0][2] - matrix[2][0] ) / s;
		}
		else
		{	s  = float(sqrt( 1.0 + matrix[2][2] - matrix[0][0] - matrix[1][1] ) * 2);
			x = (matrix[0][2] + matrix[2][0] ) / s;
			y = (matrix[2][1] + matrix[1][2] ) / s;
			z = 0.25 * s;
			w = (matrix[1][0] - matrix[0][1] ) / s;
		}
	}
	Normalize();
}

// Ripped from HL SDK
void CQuaternion::QuaternionSlerp( CQuaternion p, CQuaternion q, double t )
{	int i;
	double	omega, cosom, sinom, sclp, sclq;

	// decide if one of the quaternions is backwards
	double a = 0;
	double b = 0;

	for (i = 0; i < 4; i++)
	{
		a += (p[i]-q[i])*(p[i]-q[i]);
		b += (p[i]+q[i])*(p[i]+q[i]);
	}
	if (a > b)
	{
		for (i = 0; i < 4; i++)
		{
			q[i] = -q[i];
		}
	}

	cosom = p[0]*q[0] + p[1]*q[1] + p[2]*q[2] + p[3]*q[3];

	if ((1.0 + cosom) > 0.000001)
	{
		if ((1.0 - cosom) > 0.000001)
		{
			omega = acos( cosom );
			sinom = sin( omega );
			sclp = sin( (1.0 - t)*omega) / sinom;
			sclq = sin( t*omega ) / sinom;
		}
		else
		{
			sclp = 1.0 - t;
			sclq = t;
		}
		for (i = 0; i < 4; i++) {
			(*this)[i] = sclp * p[i] + sclq * q[i];
		}
	}
	else
	{
		(*this)[0] = -q[1];
		(*this)[1] = q[0];
		(*this)[2] = -q[3];
		(*this)[3] = q[2];
		sclp = sin( (1.0 - t) * (0.5 * M_PI));
		sclq = sin( t * (0.5 * M_PI));
		for (i = 0; i < 3; i++)
		{
			(*this)[i] = sclp * p[i] + sclq * (*this)[i];
		}
	}
	Normalize();
}

void CQuaternion::setQuaternion( double dw,double dx ,double dy,double dz )
{	w = dw;
	x = dx;
	y = dy;
	z = dz;
}

void CQuaternion::setQuaternion( CQuaternion q )
{	setQuaternion(q.w, q.x, q.y, q.z);
}

void QuaternionRotate(const float * anglesIn, const float * deltaAngles, float * anglesOut)
{
	static CQuaternion qIn, qYaw, qPitch, qRoll, qDelta, qResult, qTemp;
	vec3_t forward, right, up;

	// Convert the input angles to quaternion notation and find the axes
	qIn.EulerToQuaternion(anglesIn);
	QAngleVectors(anglesIn, forward, right, up);

	// Calculate the additional rotations from the delta angles and the axes
	qYaw.FromAxisAndAngle(up, deltaAngles[YAW]);
	qPitch.FromAxisAndAngle(right, -deltaAngles[PITCH]);
	qRoll.FromAxisAndAngle(forward, deltaAngles[ROLL]);

	// Get the total additional rotation
	qPitch.Multiply(qYaw);
	qPitch.Multiply(qRoll);

	// Calculate the new angles
	qPitch.Multiply(qIn);
	anglesOut = qPitch.QuaternionToEuler();
}