/******************************************************************************
	libx42 - skinned vertex animation library
	Copyright (C) 2007 HermitWorks Entertainment Corporation

	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.
		51 Franklin Street, Fifth Floor
		Boston, MA  02110-1301, USA.
******************************************************************************/

#include "local.h"

#ifndef M_PI
#define M_PI 3.14159265358979323846F
#endif

NOGLOBALALIAS ALWAYS_INLINE float sin_0_HalfPi( float a )
{
	float s, t;

	s = a * a;

	t = -2.39e-08F;
	t *= s;

	t += 2.7526e-06F;
	t *= s;
	
	t += -1.98409e-04F;
	t *= s;
	
	t += 8.3333315e-03F;
	t *= s;
	
	t += -1.666666664e-01F;
	t *= s;
	
	t += 1.0f;
	t *= a;
	
	return t;
}

NOGLOBALALIAS ALWAYS_INLINE float atan2_pos( float y, float x )
{
	float a, d, s, t;
	
	if( y > x )
	{
		a = -x / y;
		d = ((float)M_PI / 2.0F);
	}
	else
	{
		a = y / x;
		d = 0.0F;
	}

	s = a * a;
	
	t = 0.0028662257F;
	t *= s;
	
	t += -0.0161657367F;
	t *= s;
	
	t += 0.0429096138F;
	t *= s;
	
	t += -0.0752896400F;
	t *= s;
	
	t += 0.1065626393F;
	t *= s;
	
	t += -0.1420889944F;
	t *= s;
	
	t += 0.1999355085F;
	t *= s;
	
	t += -0.3333314528F;
	t *= s;
	
	t += 1.0F;
	t *= a;
	
	t += d;
	
	return t;
}

NOGLOBALALIAS void quat_interp( float *o, const float *a, const float *b, float t )
{
	float cosTheta;
	float ta, tb;
	bool neg, norm;

	cosTheta = a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3];

	if( cosTheta < 0.0F )
	{
		cosTheta = -cosTheta;
		neg = true;
	}
	else
		neg = false;

	if( cosTheta >= 1.0F - 1e-5F )
	{
		//quats are very close, just lerp to avoid the division by zero
		ta = 1.0F - t;
		tb = t;

		norm = false;
	}
	else if( cosTheta >= 0.2F )
	{
		//quats are somewhat close - normalized lerp will do
		ta = 1.0F - t;
		tb = t;

		norm = true;
	}
	else
	{
		//do full slerp - quickly
		float sinThetaSqr = 1.0F - (cosTheta * cosTheta);
		float sinThetaInv = rsqrt( sinThetaSqr );
		float theta = atan2_pos( sinThetaSqr * sinThetaInv, cosTheta );

		//we're good to SLERP
		ta = sin_0_HalfPi( (1.0F - t) * theta ) * sinThetaInv;
		tb = sin_0_HalfPi( t * theta ) * sinThetaInv;

		norm = false;
	}

	if( neg )
		tb = -tb;

	o[0] = a[0] * ta + b[0] * tb;
	o[1] = a[1] * ta + b[1] * tb;
	o[2] = a[2] * ta + b[2] * tb;
	o[3] = a[3] * ta + b[3] * tb;

	if( norm )
	{
		float l = o[0] * o[0] + o[1] * o[1] + o[2] * o[2] + o[3] * o[3];
		l = rsqrt( l );

		o[0] *= l;
		o[1] *= l;
		o[2] *= l;
		o[3] *= l;
	}
}

NOGLOBALALIAS void quat_mul( float *o, const float *a, const float *b )
{
	/*
		From pretty much any math text you'll find that given:

			p = a + bi + cj + dk
			q = w + xi + yj + zk

		You get the product:
		
			pq =	(aw - bx - cy - dz) +
					
					(bw + ax + cz - dy)i +
					(cw + ay + dx - bz)j +
					(dw + az + by - cx)k
					(aw - bx - cy - dz)
			
		So, keeping in mind that p = a and q = b, and that we store
		elements in (xi, yj, zk, w) order we have:

			ab =	(a[3] * b[3] - a[0] * b[0] - a[1] * b[1] - a[2] * b[2]) +
					(a[0] * b[3] + a[3] * b[0] + a[1] * b[2] - a[2] * b[1])i +
					(a[1] * b[3] + a[3] * b[1] + a[2] * b[0] - a[0] * b[2])j +
					(a[2] * b[3] + a[3] * b[2] + a[0] * b[1] - a[1] * b[0])k
	*/

#ifdef LIBX42_NO_ALLOCA
	quat_t tmp;
#endif

	if( a == o )
	{
#ifndef LIBX42_NO_ALLOCA
		float *tmp = _alloca( sizeof( quat_t ) );
#endif
		tmp[0] = a[0];
		tmp[1] = a[1];
		tmp[2] = a[2];
		tmp[3] = a[3];

		a = tmp;

		if( b == o )
			b = tmp;
	}
	else if( b == o )
	{
#ifndef LIBX42_NO_ALLOCA
		float *tmp = _alloca( sizeof( quat_t ) );
#endif
		tmp[0] = b[0];
		tmp[1] = b[1];
		tmp[2] = b[2];
		tmp[3] = b[3];

		b = tmp;
	}

	o[0] = a[0] * b[3] + a[3] * b[0] + a[1] * b[2] - a[2] * b[1];
	o[1] = a[1] * b[3] + a[3] * b[1] + a[2] * b[0] - a[0] * b[2];
	o[2] = a[2] * b[3] + a[3] * b[2] + a[0] * b[1] - a[1] * b[0];
	o[3] = a[3] * b[3] - a[0] * b[0] - a[1] * b[1] - a[2] * b[2];
}

NOGLOBALALIAS void affine_mul( affine_t *o, const affine_t *a, const affine_t *b )
{
	int c, r;

#ifdef LIBX42_NO_ALLOCA
	affine_t _tmp;
	affine_t *tmp = &_tmp;
#endif

	if( a == o )
	{
#ifndef LIBX42_NO_ALLOCA
		affine_t *tmp = _alloca( sizeof( affine_t ) );
#endif
		memcpy( tmp, a, sizeof( affine_t ) );

		a = tmp;

		if( b == o )
			b = tmp;
	}
	else if( b == o )
	{
#ifndef LIBX42_NO_ALLOCA
		affine_t *tmp = _alloca( sizeof( affine_t ) );
#endif
		memcpy( tmp, b, sizeof( affine_t ) );

		b = tmp;
	}

	for( c = 0; c < 3; c++ )
		for( r = 0; r < 3; r++ )
			o->c[c][r] =
				a->c[0][r] * b->c[c][0] +
				a->c[1][r] * b->c[c][1] +
				a->c[2][r] * b->c[c][2];

	for( r = 0; r < 3; r++ )
		o->c[3][r] =
			a->c[0][r] * b->c[3][0] +
			a->c[1][r] * b->c[3][1] +
			a->c[2][r] * b->c[3][2] +
			a->c[3][r];
}

NOGLOBALALIAS void affine_scale( affine_t *o, const affine_t *a, float s )
{
	o->c[0][0] = a->c[0][0] * s;
	o->c[0][1] = a->c[0][1] * s;
	o->c[0][2] = a->c[0][2] * s;

	o->c[1][0] = a->c[1][0] * s;
	o->c[1][1] = a->c[1][1] * s;
	o->c[1][2] = a->c[1][2] * s;

	o->c[2][0] = a->c[2][0] * s;
	o->c[2][1] = a->c[2][1] * s;
	o->c[2][2] = a->c[2][2] * s;

	o->c[3][0] = a->c[3][0] * s;
	o->c[3][1] = a->c[3][1] * s;
	o->c[3][2] = a->c[3][2] * s;
}

NOGLOBALALIAS void affine_mad( affine_t *o, const affine_t *m, float s, const affine_t *a )
{
	o->c[0][0] = m->c[0][0] * s + a->c[0][0];
	o->c[0][1] = m->c[0][1] * s + a->c[0][1];
	o->c[0][2] = m->c[0][2] * s + a->c[0][2];

	o->c[1][0] = m->c[1][0] * s + a->c[1][0];
	o->c[1][1] = m->c[1][1] * s + a->c[1][1];
	o->c[1][2] = m->c[1][2] * s + a->c[1][2];

	o->c[2][0] = m->c[2][0] * s + a->c[2][0];
	o->c[2][1] = m->c[2][1] * s + a->c[2][1];
	o->c[2][2] = m->c[2][2] * s + a->c[2][2];

	o->c[3][0] = m->c[3][0] * s + a->c[3][0];
	o->c[3][1] = m->c[3][1] * s + a->c[3][1];
	o->c[3][2] = m->c[3][2] * s + a->c[3][2];
}

NOGLOBALALIAS void affine_mul_vec( vec3_t o, const affine_t *m, const vec3_t v )
{
	vec3_t t;
	
	if( v == o )
	{
		t[0] = v[0];
		t[1] = v[1];
		t[2] = v[2];

		v = t;
	}

	o[0] = m->c[0][0] * v[0] + m->c[1][0] * v[1] + m->c[2][0] * v[2];
	o[1] = m->c[0][1] * v[0] + m->c[1][1] * v[1] + m->c[2][1] * v[2];
	o[2] = m->c[0][2] * v[0] + m->c[1][2] * v[1] + m->c[2][2] * v[2];
}

NOGLOBALALIAS void affine_mul_pos( vec3_t o, const affine_t *m, const vec3_t p )
{
	vec3_t t;
	
	if( p == o )
	{
		t[0] = p[0];
		t[1] = p[1];
		t[2] = p[2];

		p = t;
	}

	o[0] = m->c[0][0] * p[0] + m->c[1][0] * p[1] + m->c[2][0] * p[2] + m->c[3][0];
	o[1] = m->c[0][1] * p[0] + m->c[1][1] * p[1] + m->c[2][1] * p[2] + m->c[3][1];
	o[2] = m->c[0][2] * p[0] + m->c[1][2] * p[1] + m->c[2][2] * p[2] + m->c[3][2];
}

NOGLOBALALIAS void quat_to_affine( affine_t *o, const quat_t q )
{
	float xx = q[0] * q[0];
	float yy = q[1] * q[1];
	float zz = q[2] * q[2];
	
	float xy = q[0] * q[1];
	float xz = q[0] * q[2];
	float xw = q[0] * q[3];
	float yz = q[1] * q[2];
	float yw = q[1] * q[3];
	float zw = q[2] * q[3];

	o->c[0][0] = 1.0F - 2.0F * (yy + zz);	o->c[1][0] = 2.0F * (xy - zw);			o->c[2][0] = 2.0F * (xz + yw);
	o->c[0][1] = 2.0F * (xy + zw);			o->c[1][1] = 1.0F - 2.0F * (xx + zz);	o->c[2][1] = 2.0F * (yz - xw);
	o->c[0][2] = 2.0F * (xz - yw);			o->c[1][2] = 2.0F * (yz + xw);			o->c[2][2] = 1.0F - 2.0F * (xx + yy);

	o->c[3][0] = 0.0F;
	o->c[3][1] = 0.0F;
	o->c[3][2] = 0.0F;
}