/*
	Twilight Prophecy 3D/Multimedia SDK
	A multi-platform development system for virtual reality and multimedia.

	Copyright (C) 1997-2001 by Twilight 3D Finland Oy Ltd.

	This program is free software; you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation; either version 2 of the License, or
	(at your option) any later version.

	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with this program; if not, write to the Free Software
	Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

	Please read the file LICENSE.TXT for additional details.


	source: 
		Quaternion implementation

	revision history:
		Jan/31/2001 - Jukka Liimatta - initial revision
*/
#include <prmath/prmath.hpp>
using namespace prmath;



void Quaternion::operator = (const Matrix4x4& m)
{
	float tr = m.m44[0][0] + m.m44[1][1] + m.m44[2][2];
	if ( tr > 0 )
	{
		float s = (float)sqrt(tr + 1);
		w = s * 0.5f;
		s = 0.5f / s;
		x = (m.m44[1][2] - m.m44[2][1]) * s;
		y = (m.m44[2][0] - m.m44[0][2]) * s;
		z = (m.m44[0][1] - m.m44[1][0]) * s;
	}
	else
	{
		int i = 0;
		if ( m.m44[1][1] > m.m44[0][0] ) i = 1;
		if ( m.m44[2][2] > m.m44[i][i] ) i = 2;

		static int nxt[] = {1,2,0};
		int j = nxt[i];
		int k = nxt[j];

		float s = (float)sqrt(m.m44[i][i] - m.m44[j][j] - m.m44[k][k] + 1);
		if ( s < FLT_EPSILON )
		{
			Identity();
		}
		else
		{
			float* pq = (float*)this;

			pq[i] = s * 0.5f;
			s = 0.5f / s;
			pq[3] = (m.m44[j][k] - m.m44[k][j]) * s;
			pq[j] = (m.m44[i][j] + m.m44[j][i]) * s;
			pq[k] = (m.m44[i][k] + m.m44[k][i]) * s;
		}
	}
}


void Quaternion::SetAngleAxis(float angle, const Vector3& axis)
{
	float theta = angle * 0.5f;
	float s = (float)sin( theta ) / Length(axis);
	float c = (float)cos( theta );
	
	x = axis.x * s;
	y = axis.y * s;
	z = axis.z * s;
	w = c;
}


void Quaternion::GetAngleAxis(float& angle, Vector3& axis) const
{
	float s = 1 / (float)sqrt(1 - w*w);

	angle = (float)acos(w) * 2;
	axis.x = x * s;
	axis.y = y * s;
	axis.z = z * s;
}


void Quaternion::SetEuler(const Vector3& anglevec, EulerOrder euler)
{
	float ex;
	float ey;
	float ez;

	switch ( euler )
	{
		case EULER_XYZ: ex = anglevec.x; ey = anglevec.y; ez = anglevec.z; break;
		case EULER_XZY: ex = anglevec.x; ey = anglevec.z; ez = anglevec.y; break;
		case EULER_YXZ: ex = anglevec.y; ey = anglevec.x; ez = anglevec.z; break;
		case EULER_YZX: ex = anglevec.y; ey = anglevec.z; ez = anglevec.x; break;
		case EULER_ZXY: ex = anglevec.z; ey = anglevec.x; ez = anglevec.y; break;
		case EULER_ZYX: ex = anglevec.z; ey = anglevec.y; ez = anglevec.x; break;
	}

	ex *= 0.5f;
	ey *= 0.5f;
	ez *= 0.5f;

	float cx = (float)cos( ex );
	float cy = (float)cos( ey );
	float cz = (float)cos( ez );
	float sx = (float)sin( ex );
	float sy = (float)sin( ey );
	float sz = (float)sin( ez );

	float cycz = cy * cz;
	float sysz = sy * sz;
	float sycz = sy * cz;
	float cysz = cy * sz;

	x = sx * cycz - cx * sysz;
	y = sx * cysz + cx * sycz;
	z = cx * cysz - sx * sycz;
	w = cx * cycz + sx * sysz;
}


void Quaternion::Normalize()
{
	float s = Norm();
	if ( !s )
	{
		x = y = z = 0;
		w = 1;
	}
	else
	{
		float is = 1 / (float)sqrt( s );
		x *= is;
		y *= is;
		z *= is;
		w *= is;
	}
}


void Quaternion::Exp()
{
	float s = (float)sqrt(x*x + y*y + z*z);
	w = (float)cos( s );
	if ( s > FLT_EPSILON*100 )
	{
		s = (float)sin( s ) / s;
		x *= s;
		y *= s;
		z *= s;
	}
}


void Quaternion::Log()
{
	float s = w ? (float)atan2(sqrt(x*x + y*y + z*z),w) : (float)pi * 2;
	x *= s;
	y *= s;
	z *= s;
	w = 0;
}


void Quaternion::LnDif(const Quaternion& q)
{
	Quaternion invp = *this;
	invp.Inverse();
	Quaternion mulp = invp * q;
	
	float length = (float)sqrt(mulp.x*mulp.x + mulp.y*mulp.y + mulp.z*mulp.z);
	float scale = x*x + y*y + z*z + w*w;
	float mval = scale ? (float)atan2(length,scale) : (float)pi * 2;
	
	if ( length ) mval /= length;
	
	x = mulp.x * mval;
	y = mulp.y * mval;
	z = mulp.z * mval;
	w = 0;
}


Quaternion prmath::Slerp(const Quaternion& a, const Quaternion& b, float t)
{
	float cs = DotProduct(a,b);
	float sn = (float)sqrt( fabs(1-cs*cs) );
	
	if ( fabs(sn) < FLT_EPSILON*100 )
		return a;
	
	if ( cs < 0 )
		cs = -cs;
		
	float angle = (float)atan2(sn,cs);
	sn = 1 / sn;
	
	return a * sn*(float)sin(angle*(1-t)) + b * sn*(float)sin(angle*t);
}


Quaternion prmath::Slerp(const Quaternion& a, const Quaternion& b, float t, int spin)
{
	float bflip = 1;
	float tcos = DotProduct(a,b);
	
	if ( tcos < 0 )
	{
		tcos = -tcos;
		bflip = -1;
	}
	
	if ( (1-tcos) < FLT_EPSILON*100 )
	{
		// linear interpolate
		return a * (1-t) + b * (t*bflip);
	}
	
	float theta = (float)acos(tcos);
	float phi = theta + (float)(spin * pi);
	float tsin = (float)sin(theta);
	float beta = (float)sin(theta - t*phi) / tsin;
	float alpha = (float)sin(t*phi) / tsin * bflip;
	
	return a * beta + b * alpha;
}
