#include "c4d_quaternion.h"
#include "c4d_tools.h"
#include "ge_math.h"

#define EPSILON 0.00001

Quaternion::Quaternion()
{
	w = 0.0;
	v = Vector(1,0,0);
}

void Quaternion::SetAxis(const Vector &ax, Real ww)
{	
	Real len,wink=ww;
	Vector axis = ax;

	len = Len(axis);
	if (len>EPSILON)
	{
		axis /= len;

		wink = -0.5*wink;
		
		v    = axis*Sin(wink);
		w    = Cos(wink);
	}
	else
	{
		w = 0.0;
		v = Vector(1,0,0);
	}
}

void Quaternion::SetMatrix(const Matrix &m)
{
	Vector ax;
	Real   wi;
	MatrixToRotAxis(m,&ax,&wi);
	SetAxis(ax,wi);
}

Matrix Quaternion::GetMatrix(void) const
{
	Matrix m;
	
	m.off = Vector(0,0,0);
	m.v1  = Vector(1.0-2.0*(v.y*v.y+v.z*v.z),2.0*(v.x*v.y-w*v.z)      ,2.0*(v.x*v.z+w*v.y));
	m.v2  = Vector(2.0*(v.x*v.y+w*v.z)      ,1.0-2.0*(v.x*v.x+v.z*v.z),2.0*(v.y*v.z-w*v.x));
	m.v3  = Vector(2.0*(v.x*v.z-w*v.y)      ,2.0*(v.y*v.z+w*v.x)      ,1.0-2.0*(v.x*v.x+v.y*v.y));

	return m;
}

Quaternion Slerp(const Quaternion &q1, const Quaternion &q2, const Real alpha)
{
	Real       sum,teta,beta1,beta2;
	Quaternion q,qd=q2;

	sum = q1.w*q2.w + q1.v*q2.v;

	if (sum<0.0)
	{
		sum=-sum;
		qd.v=-qd.v;
		qd.w=-qd.w;
	}

	if (1.0-sum>EPSILON)
	{
		teta = ACos(sum);
		Real sn = 1.0/Sin(teta);
		beta1 = Sin((1.0-alpha)*teta)*sn;
		beta2 = Sin(alpha*teta)*sn;
	}
	else
	{
		beta1 = 1.0-alpha;
		beta2 = alpha;
	}
	q.w   = beta1*q1.w   + beta2*qd.w;
	q.v.x = beta1*q1.v.x + beta2*qd.v.x;
	q.v.y = beta1*q1.v.y + beta2*qd.v.y;
	q.v.z = beta1*q1.v.z + beta2*qd.v.z;

	return q;
}
