//----------------------------------------------------------------------------
//	file: quat.cpp
//	desc: implementation of gem_Quat methods
//----------------------------------------------------------------------------
#include <math.h>
#include "directgem.h"
#include "gemmath.h"

FLOAT Norm(gem_Quat& q)
{
	return q.w*q.w + q.x*q.x + q.y*q.y +q.z*q.z;
}


gem_Quat Normalize(gem_Quat& q)
{
	FLOAT			len = sqrt( q.w*q.w + q.x*q.x + q.y*q.y +q.z*q.z );

	if ( !len )
		return q;

	len = 1/len;
	return gem_Quat( q.w*len, q.x*len, q.y*len, q.z*len );
}


gem_Quat Inverse(gem_Quat& q)
{
	FLOAT			norm = q.w*q.w + q.x*q.x + q.y*q.y +q.z*q.z;

	if ( norm<0.0f )
		return gem_Quat(0,0,0,0);
	
	norm = 1.0f/norm;
	return gem_Quat( q.w*norm, -q.x*norm, -q.y*norm, -q.z*norm );	
}

	
gem_Quat UnityInverse(gem_Quat& q)
{
	return gem_Quat( q.w, -q.x, -q.y, -q.z );
}

gem_Quat exp(gem_Quat& q)
{
	gem_Quat		tmp;
	FLOAT			ang = sqrt( q.x*q.x + q.y*q.y + q.z*q.z );
	FLOAT			sn;
	
	tmp.w = cos( ang );
	sn = sin( ang );

	if( sn < 0.001f )
		sn = 1.0f;
	else
		sn = sn/ang;

	tmp.x = q.x*sn;
	tmp.y = q.y*sn;
	tmp.z = q.z*sn;

	return tmp;
}

gem_Quat ln(gem_Quat& q)
{
	gem_Quat		tmp;
	FLOAT			ang;
	FLOAT			sn;

	tmp.w = 0.0f;

	if( fabs( q.w )<=1.0f )
	{
		ang = acos( q.w );
		sn = sin( ang );

		if( sn>0.001f )
		{
			sn = ang/sn;
			tmp.x = sn*q.x;
			tmp.y = sn*q.y;
			tmp.z = sn*q.z;

			return tmp;
		}
	}

	tmp.x = q.x;
	tmp.y = q.y;
	tmp.z = q.z;

	return tmp;
}


gem_Quat operator^(gem_Quat& q, FLOAT t)
{
	gem_Quat		tmp;	
	FLOAT			ang;
	FLOAT			sn1, sn2;
	FLOAT			coeff;

	if ( fabs(q.w)<=1.0f )
	{
		ang = acos( q.w );
		tmp.w = cos( t*ang );
		sn1 = sin ( t*ang );
		sn2 = sin ( ang );

		if ( sn2<0.001f )
			coeff = t;
		else
			coeff = sn1/sn2;

		tmp.x = q.x*coeff;
		tmp.y = q.y*coeff;
		tmp.z = q.z*coeff;

		return tmp;
	}

	return gem_Quat(0, 0, 0, 0);
}


FLOAT Dot(gem_Quat& p, gem_Quat& q)
{
	return p.w*q.w + p.x*q.x + p.y*q.y + p.z*q.z;
}

gem_Quat operator*(gem_Quat& p, gem_Quat& q)
{
	gem_Quat		tmp;

	tmp.w = p.w*q.w - p.x*q.x - p.y*q.y - p.z*q.z;
    tmp.x = p.w*q.x + p.x*q.w + p.y*q.z - p.z*q.y;
    tmp.y = p.w*q.y + p.y*q.w + p.z*q.x - p.x*q.z;
    tmp.z = p.w*q.z + p.z*q.w + p.x*q.y - p.y*q.x;

	return tmp;
}



gem_Quat operator*(gem_Quat& p, FLOAT a)
{
	return gem_Quat( a*p.w, a*p.x, a*p.y, a*p.z );
}

gem_Quat operator*(FLOAT a, gem_Quat& p)
{
	return gem_Quat( a*p.w, a*p.x, a*p.y, a*p.z );
}

gem_Quat operator+(gem_Quat& p, gem_Quat& q)
{
	return gem_Quat( p.w+q.w, p.x+q.x, p.y+q.y, p.z+q.z );
}

gem_Quat operator-(gem_Quat& p, gem_Quat& q)
{
	return gem_Quat( p.w-q.w, p.x-q.x, p.y-q.y, p.z-q.z );
}

gem_Quat operator-(gem_Quat& q)
{
	return gem_Quat( -q.w, -q.x, -q.y, -q.z );
}

gem_Quat SLERP(gem_Quat& p, gem_Quat& q, FLOAT t)
{
	FLOAT			ang;
	FLOAT			sn;
	FLOAT			dot;
	FLOAT			t1, t2;
	
	dot = Dot( p, q );

	if( dot>1.0f )
		dot = 1.0f;
	
	ang = acos( dot );
	sn = sin( ang );

	if ( sn < 0.001f )
	{
		t1 = 1-t;
		t2 = t;
	}
	else
	{
		sn = 1.0f/sn;
		t1 = sin(ang*(1-t))*sn;
		t2 = sin(ang*t)*sn;
	}

	return t1*p + t2*q;
}


//M_QUATERNION ExtraSpinSlerp(gem_Quat& p, gem_Quat& q, FLOAT t, FLOAT spin);

gem_Quat SQUAD(gem_Quat& p, gem_Quat& a, gem_Quat& b, gem_Quat& q, FLOAT t)
{
	return SLERP( SLERP( p, q, t ), SLERP( a, b, t ), 2*t*(t-1) );
}

gem_Quat FromAxisAngle(FLOAT x, FLOAT y, FLOAT z, FLOAT angle)
{
	FLOAT			ang = 0.5*angle;
	FLOAT			sn = sin( ang );
	gem_Quat		tmp;

	tmp.w = cos( ang );
	tmp.x = -sn*x;
	tmp.y = -sn*y;
	tmp.z = -sn*z;

	return tmp;
}

gem_Quat FromAxisAngle(gem_Vector& vAxis, FLOAT angle)
{
	FLOAT			ang = 0.5*angle;
	FLOAT			sn = sin( ang );
	gem_Quat		tmp;

	tmp.w = cos( ang );
	tmp.x = sn*vAxis.x;
	tmp.y = sn*vAxis.y;
	tmp.z = sn*vAxis.z;

	return tmp;
}

gem_Matrix MtxFromQuat(gem_Quat& q)
{
	FLOAT			xx = q.x*q.x; 
	FLOAT			yy = q.y*q.y; 
	FLOAT			zz = q.z*q.z;
    FLOAT			xy = q.x*q.y; 
	FLOAT			xz = q.x*q.z; 
	FLOAT			yz = q.y*q.z;
    FLOAT			wx = q.w*q.x; 
	FLOAT			wy = q.w*q.y; 
	FLOAT			wz = q.w*q.z;
	gem_Matrix		mat = IdentMtx();
    
    mat(1,1) = 1 - 2 * ( yy + zz ); 
    mat(1,2) =     2 * ( xy - wz );
    mat(1,3) =     2 * ( xz + wy );

    mat(2,1) =     2 * ( xy + wz );
    mat(2,2) = 1 - 2 * ( xx + zz );
    mat(2,3) =     2 * ( yz - wx );

    mat(3,1) =     2 * ( xz - wy );
    mat(3,2) =     2 * ( yz + wx );
    mat(3,3) = 1 - 2 * ( xx + yy );


	return mat;
}
