module gedo.math.quaternion;



public import gedo.math.vector;
public import gedo.math.matrix;
private import std.math;



typedef float[4] Quaternion;



public void identity(Quaternion q)
{
	q[] = [cast(float)0.0, 0.0, 0.0, 1.0];
}



public void normalize(Quaternion q)
{
	auto l = .std.math.sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
	if(l > 0.0)
	{
		l = 1 / l;

		q[0] *= l;
		q[1] *= l;
		q[2] *= l;
		q[3] *= l;
	}
	else
	{
		identity(q);
	}
}



public bool fromVector(Quaternion q, Vector a, Vector b)
{
	float d = dot(a, b);

	if(d > -31.0/32.0)
	{
		Vector c;

		cross(c, a, b);

		float s = sqrt((1.0 + d)*2.0);
		float t = 1.0 / s;

		q[0] = c[0] * t;
		q[1] = c[1] * t;
		q[2] = c[2] * t;

		q[3] = s * 0.5;

		return true;
	}
	else
	{
		return false;
	}
}



public void toMatrix(Quaternion q, Vector p, Matrix m)
{
	// a lean function for filling m[16] with
	// a 4x4 transformation matrix based on 
	// translation v and rotation q
	// This routine would likely be used by an opengl
	// programmer calling glmultmatrix()
	m[0]  = 1-2*(q[1]*q[1]+q[2]*q[2]);
	m[1]  = 2*(q[0]*q[1]+q[3]*q[2]);
	m[2]  = 2*(q[0]*q[2]-q[3]*q[1]);
	m[3]  = 0;
	m[4]  = 2*(q[0]*q[1]-q[3]*q[2]);	
	m[5]  = 1-2*(q[0]*q[0]+q[2]*q[2]);
	m[6]  = 2*(q[1]*q[2]+q[3]*q[0]);
	m[7]  = 0;   
	m[8]  = 2*(q[0]*q[2]+q[3]*q[1]); 
	m[9]  = 2*(q[1]*q[2]-q[3]*q[0]); 
	m[10] = 1-2*(q[0]*q[0]+q[1]*q[1]);
	m[11] = 0; 
	m[12] = p[0];
	m[13] = p[1];
	m[14] = p[2];
	m[15] = 1.0f;
}



public void transform(Quaternion q, Vector p)
{
	Matrix m;

	toMatrix(q, p, m);

	.gedo.math.matrix.multiply(m);
}



public void multiply(Quaternion c, Quaternion a, Quaternion b)
{
	c[3] = a[3]*b[3] - a[0]*b[0] - a[1]*b[1] - a[2]*b[2]; 
	c[0] = a[3]*b[0] + a[0]*b[3] + a[1]*b[2] - a[2]*b[1]; 
	c[1] = a[3]*b[1] - a[0]*b[2] + a[1]*b[3] + a[2]*b[0]; 
	c[2] = a[3]*b[2] + a[0]*b[1] - a[1]*b[0] + a[2]*b[3]; 
}



public void invert(Quaternion q)
{
	q[0] = -q[0];
	q[1] = -q[1];
	q[2] = -q[2];
}



public void halfRotation(Quaternion q, Vector r)
{
	q[0..3] = r[];
	q[3]    = 0.0;
}



public void rotate(Quaternion q, Vector v)
{
	Quaternion qv = [v[0], v[1], v[2], 0.0];

	Quaternion qq = q[];

	invert(qq);

	Quaternion qo0;

	multiply(qo0, q, qv);

	Quaternion qo1;

	multiply(qo1, qo0, qq);

	v[] = qo1[0..3];
}



public void slerpR(Quaternion qa, float t)
{
	float angle = acos(qa[3]);
	if(angle != angle || t >= angle)
	{
		return;
	}

	float sina    = sin(angle);
	float sinat   = sin(t)/sina;
	float sinaomt = sin(angle - t)/sina;

	qa[0] *= sinat;
	qa[1] *= sinat;
	qa[2] *= sinat;

	qa[3] = sinaomt + qa[3]*sinat;
}



public void slerp(Quaternion q, Quaternion q1, Quaternion q2, float t)
{
	float dot = q1[0]*q2[0] + q1[1]*q2[1] + q1[2]*q2[2] + q1[3]*q2[3];

	/*
	dot = cos(theta)
	if (dot < 0), q1 and q2 are more than 90 degrees apart,
	so we can invert one to reduce spinning
	*/
	if (dot < 0)
	{
		dot   = -dot;
		q1[0] = -q1[0];
		q1[1] = -q1[1];
		q1[2] = -q1[2];
		q1[3] = -q1[3];
	}
	
	if (dot < 0.95f)
	{
		float angle = acos(dot);
		float sina,sinat,sinaomt;

		sina    = sin(angle);
		sinat   = sin(angle*t) / sina;
		sinaomt = sin(angle*(1-t)) / sina;

		q[0] = (q1[0]*sinaomt) + (q2[0]*sinat);
		q[1] = (q1[1]*sinaomt) + (q2[1]*sinat);
		q[2] = (q1[2]*sinaomt) + (q2[2]*sinat);
		q[3] = (q1[3]*sinaomt) + (q2[3]*sinat);
	}
	/*
	if the angle is small, use linear interpolation
	*/
	else
	{
		lerp(q, q1, q2, t);
	}
}



private void lerp(Quaternion q, Quaternion q1, Quaternion q2, float t)
{
	//q[] = (t*(q2[] - q1[])) + q1[];

	float u = 1.0 - t;

	q[0] = u*q1[0] + t*q2[0];
	q[1] = u*q1[1] + t*q2[1];
	q[2] = u*q1[2] + t*q2[2];
	q[3] = u*q1[3] + t*q2[3];
}

