/******************************************************************************
	libx42pp - skinned vertex animation library (C++ API)
	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"

namespace x42
{
namespace math
{

#ifndef M_PI
#define M_PI 3.14159265358979323846F
#endif

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;
}

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;
}

inline float rsqrt( float x )
{
	long i;
	float y, r;

	y = x * 0.5F;

	i = *(long*)&x;
	i = 0x5F375A86 - (i >> 1);
	r = *(float*)&i;

	//need at least two newton steps, things get twitchy with just one
	r = r * (1.5F - r * r * y);
	r = r * (1.5F - r * r * y);
	
	return r;
}

quat quat::operator * ( const quat &q ) const
{
	quat ret;

	ret.i = i * q.w + w * q.i + j * q.k - k * q.j;
	ret.j = j * q.w + w * q.j + k * q.i - i * q.k;
	ret.k = k * q.w + w * q.k + i * q.j - j * q.i;
	ret.w = w * q.w - i * q.i - j * q.j - k * q.k;

	return ret;
}

quat& quat::operator *= ( const quat &q )
{
	quat tmp = operator * ( q );
	return (*this = tmp);
}

quat quat_from_aa( const vec3 &axis, angle a )
{
	float hr = a.rad_val * 0.5F;
	float st = sin( hr );

	quat ret =
	{
		axis.x * st,
		axis.y * st,
		axis.z * st,
		
		cos( hr )
	};

	return ret;
}

void quat_to_aa( vec3 &axis, angle &a, const quat &q )
{
	a = rads( 2.0F * acosf( q.w ) );

	if( q.w >= 1.0F - 1e-5F )
	{
		//very close to zero angle, throw back any vector
		
		axis = vec3c( 0, 0, 1 );
	}
	else
	{
		float ist = 1.0F / sqrtf( 1.0F - q.w * q.w );
		
		axis.x = q.i * ist;
		axis.y = q.j * ist;
		axis.z = q.k * ist;
	}
}

quat slerp( const quat &a, const quat &b, float t )
{
	float cosTheta;
	float ta, tb;
	bool neg, norm;

	cosTheta = a.i * a.i + a.j * a.j + a.k * a.k + a.w * a.w;

	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;

	quat ret;

	ret.i = a.i * ta + b.i * tb;
	ret.j = a.j * ta + b.j * tb;
	ret.k = a.k * ta + b.k * tb;
	ret.w = a.w * ta + b.w * tb;

	if( norm )
	{
		float l = ret.i * ret.i + ret.j * ret.j + ret.k * ret.k + ret.w * ret.w;
		l = rsqrt( l );

		ret.i *= l;
		ret.j *= l;
		ret.k *= l;
		ret.w *= l;
	}

	return ret;
}

quat conjugate( const quat &q )
{
	quat ret = { -q.i, -q.j, -q.k, q.w };
	return ret;
}

affine quat_to_affine( const quat &q )
{
	affine ret;

	float xx = q.i * q.i;
	float yy = q.j * q.j;
	float zz = q.k * q.k;
	
	float xy = q.i * q.j;
	float xz = q.i * q.k;
	float xw = q.i * q.w;
	float yz = q.j * q.k;
	float yw = q.j * q.w;
	float zw = q.k * q.w;

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

	ret.c[3][0] = 0.0F;
	ret.c[3][1] = 0.0F;
	ret.c[3][2] = 0.0F;

	return ret;
}

inline float _copysignf( float f, float s )
{
	uint uf = *(uint*)&f;
	uint us = *(uint*)&s;

	uf = (uf & 0x7FFFFFFF) | (us & 0x80000000);

	return *(float*)&uf;
}

quat quat_from_affine( const affine &a )
{
	quat ret;

	ret.w = sqrtf( max( 0.0F, 1 + a.c[0][0] + a.c[1][1] + a.c[2][2] ) ) * 0.5F; 
	ret.i = sqrtf( max( 0.0F, 1 + a.c[0][0] - a.c[1][1] - a.c[2][2] ) ) * 0.5F; 
	ret.j = sqrtf( max( 0.0F, 1 - a.c[0][0] + a.c[1][1] - a.c[2][2] ) ) * 0.5F; 
	ret.k = sqrtf( max( 0.0F, 1 - a.c[0][0] - a.c[1][1] + a.c[2][2] ) ) * 0.5F; 

	ret.i = _copysignf( ret.i, a.c[1][2] - a.c[2][1] );
	ret.j = _copysignf( ret.j, a.c[2][0] - a.c[0][2] );
	ret.k = _copysignf( ret.k, a.c[0][1] - a.c[1][0] );

	return ret;
}

const vec3& affine::col( int i ) const
{
	return *reinterpret_cast< const vec3* >( c[i] );
}

vec3& affine::col( int i )
{
	return *reinterpret_cast< vec3* >( c[i] );
}

affine affine::operator * ( float s ) const
{
	affine ret;

	ret.c[0][0] = c[0][0] * s;
	ret.c[0][1] = c[0][1] * s;
	ret.c[0][2] = c[0][2] * s;

	ret.c[1][0] = c[1][0] * s;
	ret.c[1][1] = c[1][1] * s;
	ret.c[1][2] = c[1][2] * s;

	ret.c[2][0] = c[2][0] * s;
	ret.c[2][1] = c[2][1] * s;
	ret.c[2][2] = c[2][2] * s;

	ret.c[3][0] = c[3][0] * s;
	ret.c[3][1] = c[3][1] * s;
	ret.c[3][2] = c[3][2] * s;

	return ret;
}

affine& affine::operator *= ( float s )
{
	c[0][0] *= s;
	c[0][1] *= s;
	c[0][2] *= s;

	c[1][0] *= s;
	c[1][1] *= s;
	c[1][2] *= s;

	c[2][0] *= s;
	c[2][1] *= s;
	c[2][2] *= s;

	c[3][0] *= s;
	c[3][1] *= s;
	c[3][2] *= s;

	return *this;
}

affine affine::operator * ( const affine &a ) const
{
	affine ret;

	ret.c[0][0] = c[0][0] * a.c[0][0] + c[1][0] * a.c[0][1] + c[2][0] * a.c[0][2];
	ret.c[0][1] = c[0][1] * a.c[0][0] + c[1][1] * a.c[0][1] + c[2][1] * a.c[0][2];
	ret.c[0][2] = c[0][2] * a.c[0][0] + c[1][2] * a.c[0][1] + c[2][2] * a.c[0][2];

	ret.c[1][0] = c[0][0] * a.c[1][0] + c[1][0] * a.c[1][1] + c[2][0] * a.c[1][2];
	ret.c[1][1] = c[0][1] * a.c[1][0] + c[1][1] * a.c[1][1] + c[2][1] * a.c[1][2];
	ret.c[1][2] = c[0][2] * a.c[1][0] + c[1][2] * a.c[1][1] + c[2][2] * a.c[1][2];

	ret.c[2][0] = c[0][0] * a.c[2][0] + c[1][0] * a.c[2][1] + c[2][0] * a.c[2][2];
	ret.c[2][1] = c[0][1] * a.c[2][0] + c[1][1] * a.c[2][1] + c[2][1] * a.c[2][2];
	ret.c[2][2] = c[0][2] * a.c[2][0] + c[1][2] * a.c[2][1] + c[2][2] * a.c[2][2];

	ret.c[3][0] = c[0][0] * a.c[3][0] + c[1][0] * a.c[3][1] + c[2][0] * a.c[3][2] + c[3][0];
	ret.c[3][1] = c[0][1] * a.c[3][0] + c[1][1] * a.c[3][1] + c[2][1] * a.c[3][2] + c[3][1];
	ret.c[3][2] = c[0][2] * a.c[3][0] + c[1][2] * a.c[3][1] + c[2][2] * a.c[3][2] + c[3][2];

	return ret;
}

affine& affine::operator *= ( const affine &a ) 
{
	affine tmp = operator * ( a );
	return (*this = tmp);
}

affine affine::operator + ( const affine &a ) const
{
	affine ret;

	ret.c[0][0] = c[0][0] + a.c[0][0];
	ret.c[0][1] = c[0][1] + a.c[0][1];
	ret.c[0][2] = c[0][2] + a.c[0][2];

	ret.c[1][0] = c[1][0] + a.c[1][0];
	ret.c[1][1] = c[1][1] + a.c[1][1];
	ret.c[1][2] = c[1][2] + a.c[1][2];

	ret.c[2][0] = c[2][0] + a.c[2][0];
	ret.c[2][1] = c[2][1] + a.c[2][1];
	ret.c[2][2] = c[2][2] + a.c[2][2];

	ret.c[3][0] = c[3][0] + a.c[3][0];
	ret.c[3][1] = c[3][1] + a.c[3][1];
	ret.c[3][2] = c[3][2] + a.c[3][2];

	return ret;
}

affine& affine::operator += ( const affine &a )
{
	c[0][0] += a.c[0][0];
	c[0][1] += a.c[0][1];
	c[0][2] += a.c[0][2];

	c[1][0] += a.c[1][0];
	c[1][1] += a.c[1][1];
	c[1][2] += a.c[1][2];

	c[2][0] += a.c[2][0];
	c[2][1] += a.c[2][1];
	c[2][2] += a.c[2][2];

	c[3][0] += a.c[3][0];
	c[3][1] += a.c[3][1];
	c[3][2] += a.c[3][2];

	return *this;
}

bool affine::operator == ( const affine &a ) const
{
	return
		c[0][0] == a.c[0][0] &&
		c[0][1] == a.c[0][1] &&
		c[0][2] == a.c[0][2] &&

		c[1][0] == a.c[1][0] &&
		c[1][1] == a.c[1][1] &&
		c[1][2] == a.c[1][2] &&

		c[2][0] == a.c[2][0] &&
		c[2][1] == a.c[2][1] &&
		c[2][2] == a.c[2][2] &&

		c[3][0] == a.c[3][0] &&
		c[3][1] == a.c[3][1] &&
		c[3][2] == a.c[3][2];
}

bool affine::operator != ( const affine &a ) const
{
	return !operator == ( a );
}

vec3 affine::mul_point( const vec3 &p ) const
{
	vec3 ret;

	ret.x = c[0][0] * p.x + c[1][0] * p.y + c[2][0] * p.z + c[3][0];
	ret.y = c[0][1] * p.x + c[1][1] * p.y + c[2][1] * p.z + c[3][1];
	ret.z = c[0][2] * p.x + c[1][2] * p.y + c[2][2] * p.z + c[3][2];

	return ret;
}

vec3 affine::mul_vec( const vec3 &v ) const
{
	vec3 ret;

	ret.x = c[0][0] * v.x + c[1][0] * v.y + c[2][0] * v.z;
	ret.y = c[0][1] * v.x + c[1][1] * v.y + c[2][1] * v.z;
	ret.z = c[0][2] * v.x + c[1][2] * v.y + c[2][2] * v.z;

	return ret;
}

const affine& affinez( void )
{
	static const affine ret =
	{ {
		{ 0, 0, 0 },
		{ 0, 0, 0 },
		{ 0, 0, 0 },
		{ 0, 0, 0 }
	} };

	return ret;
}

const affine& affinei( void )
{
	static const affine ret =
	{ {
		{ 1, 0, 0 },
		{ 0, 1, 0 },
		{ 0, 0, 1 },
		{ 0, 0, 0 }
	} };

	return ret;
}

affine affinevr( const float *row_major_data )
{
	affine ret;

	ret.c[0][0] = row_major_data[0];
	ret.c[0][1] = row_major_data[4];
	ret.c[0][2] = row_major_data[8];

	ret.c[1][0] = row_major_data[1];
	ret.c[1][1] = row_major_data[5];
	ret.c[1][2] = row_major_data[9];

	ret.c[2][0] = row_major_data[2];
	ret.c[2][1] = row_major_data[6];
	ret.c[2][2] = row_major_data[10];

	ret.c[3][0] = row_major_data[3];
	ret.c[3][1] = row_major_data[7];
	ret.c[3][2] = row_major_data[11];

	return ret;
}

affine affinevc( const float *col_major_data )
{
	affine ret;

	ret.c[0][0] = col_major_data[0];
	ret.c[0][1] = col_major_data[1];
	ret.c[0][2] = col_major_data[2];

	ret.c[1][0] = col_major_data[3];
	ret.c[1][1] = col_major_data[4];
	ret.c[1][2] = col_major_data[5];

	ret.c[2][0] = col_major_data[6];
	ret.c[2][1] = col_major_data[7];
	ret.c[2][2] = col_major_data[8];

	ret.c[3][0] = col_major_data[9];
	ret.c[3][1] = col_major_data[10];
	ret.c[3][2] = col_major_data[11];

	return ret;
}

affine affinec( const vec4 &r0, const vec4 &r1, const vec4 &r2 )
{
	affine ret;

	ret.c[0][0] = r0.x; 
	ret.c[0][1] = r1.x;
	ret.c[0][2] = r2.x;

	ret.c[1][0] = r0.y;
	ret.c[1][1] = r1.y;
	ret.c[1][2] = r2.y;

	ret.c[2][0] = r0.z;
	ret.c[2][1] = r1.z;
	ret.c[2][2] = r2.z;

	ret.c[3][0] = r0.w;
	ret.c[3][1] = r1.w;
	ret.c[3][2] = r2.w;

	return ret;

}

affine affinec( const vec3 &c0, const vec3 &c1, const vec3 &c2, const vec3 &c3 )
{
	affine ret;

	ret.c[0][0] = c0.x;
	ret.c[0][1] = c0.y;
	ret.c[0][2] = c0.z;

	ret.c[1][0] = c1.x;
	ret.c[1][1] = c1.y;
	ret.c[1][2] = c1.z;

	ret.c[2][0] = c2.x;
	ret.c[2][1] = c2.y;
	ret.c[2][2] = c2.z;

	ret.c[3][0] = c3.x;
	ret.c[3][1] = c3.y;
	ret.c[3][2] = c3.z;

	return ret;

}

affine affinec( const affine_t &a )
{
	affine ret;

	ret.c[0][0] = a.c[0][0];
	ret.c[0][1] = a.c[0][1];
	ret.c[0][2] = a.c[0][2];

	ret.c[1][0] = a.c[1][0];
	ret.c[1][1] = a.c[1][1];
	ret.c[1][2] = a.c[1][2];

	ret.c[2][0] = a.c[2][0];
	ret.c[2][1] = a.c[2][1];
	ret.c[2][2] = a.c[2][2];

	ret.c[3][0] = a.c[3][0];
	ret.c[3][1] = a.c[3][1];
	ret.c[3][2] = a.c[3][2];

	return ret;
}

affine transpose( const affine &a )
{
	affine ret;

	ret.c[0][0] = a.c[0][0];
	ret.c[0][1] = a.c[1][0];
	ret.c[0][2] = a.c[2][0];

	ret.c[1][0] = a.c[0][1];
	ret.c[1][1] = a.c[1][1];
	ret.c[1][2] = a.c[2][1];

	ret.c[2][0] = a.c[0][2];
	ret.c[2][1] = a.c[1][2];
	ret.c[2][2] = a.c[2][2];

	ret.c[3][0] = 0;
	ret.c[3][1] = 0;
	ret.c[3][2] = 0;

	return ret;
}

float determinant( const affine &a )
{
	return
		a.c[2][0] * (a.c[0][1] * a.c[1][2] - a.c[0][2] * a.c[1][1]) +
		a.c[2][1] * (a.c[0][2] * a.c[1][0] - a.c[0][0] * a.c[1][2]) +
		a.c[2][2] * (a.c[0][0] * a.c[1][1] - a.c[0][1] * a.c[1][0]);
}

affine inverse( const affine &a )
{
	float dummy_det;
	return inverse( a, dummy_det );
}

affine inverse( const affine &a, float &out_det )
{
	//compute the 2x2 minors for C03
	float m0[3] =
	{
		a.c[1][1] * a.c[2][2] - a.c[1][2] * a.c[2][1],
		a.c[1][2] * a.c[2][0] - a.c[1][0] * a.c[2][2],
		a.c[1][0] * a.c[2][1] - a.c[1][1] * a.c[2][0],
	};

	//compute the 2x2 minors for C13
	float m1[3] =
	{
		a.c[0][2] * a.c[2][1] - a.c[0][1] * a.c[2][2],
		a.c[0][0] * a.c[2][2] - a.c[0][2] * a.c[2][0],
		a.c[0][1] * a.c[2][0] - a.c[0][0] * a.c[2][1],
	};

	//compute the 2x2 minors for C23 and det( a )
	float m2[3] =
	{
		a.c[0][1] * a.c[1][2] - a.c[0][2] * a.c[1][1],
		a.c[0][2] * a.c[1][0] - a.c[0][0] * a.c[1][2],
		a.c[0][0] * a.c[1][1] - a.c[0][1] * a.c[1][0],
	};

	//compute the determinant and its inverse
	float in_det =
		a.c[2][0] * m2[0] +
		a.c[2][1] * m2[1] +
		a.c[2][2] * m2[2];
	
	//more of a sanity check than anything - this shouln't really happen
	if( fabsf( in_det ) < 1e-7 )
	{
		out_det = 0;
		return a;
	}

	float inv_det = 1.0F / in_det;

	//compute the adjoint's fourth column
	float a3[3] =
	{
		a.c[3][0] * m0[0] + a.c[3][1] * m0[1] + a.c[3][2] * m0[2],
		a.c[3][0] * m1[0] + a.c[3][1] * m1[1] + a.c[3][2] * m1[2],
		a.c[3][0] * m2[0] + a.c[3][1] * m2[1] + a.c[3][2] * m2[2],
	};

	//compute the inverse
	affine ret;

	ret.c[0][0] = inv_det * m0[0];
	ret.c[0][1] = inv_det * m1[0];
	ret.c[0][2] = inv_det * m2[0];

	ret.c[1][0] = inv_det * m0[1];
	ret.c[1][1] = inv_det * m1[1];
	ret.c[1][2] = inv_det * m2[1];

	ret.c[2][0] = inv_det * m0[2];
	ret.c[2][1] = inv_det * m1[2];
	ret.c[2][2] = inv_det * m2[2];

	ret.c[3][0] = -inv_det * a3[0];
	ret.c[3][1] = -inv_det * a3[1];
	ret.c[3][2] = -inv_det * a3[2];

	out_det = inv_det; //det( inverse( a ) ) = 1/det( a )
	return ret;
}

affine orthogonal_inverse( const affine &a )
{
	affine ret;

	//inverse of the upper 3x3 is just a transpose
	ret.c[0][0] = a.c[0][0];
	ret.c[0][1] = a.c[1][0];
	ret.c[0][2] = a.c[2][0];

	ret.c[1][0] = a.c[0][1];
	ret.c[1][1] = a.c[1][1];
	ret.c[1][2] = a.c[2][1];

	ret.c[2][0] = a.c[0][2];
	ret.c[2][1] = a.c[1][2];
	ret.c[2][2] = a.c[2][2];

	//invert the last column
	ret.c[3][0] = -(a.c[3][0] * a.c[0][0] + a.c[3][1] * a.c[0][1] + a.c[3][2] * a.c[0][2]);
	ret.c[3][1] = -(a.c[3][0] * a.c[1][0] + a.c[3][1] * a.c[1][1] + a.c[3][2] * a.c[1][2]);
	ret.c[3][2] = -(a.c[3][0] * a.c[2][0] + a.c[3][1] * a.c[2][1] + a.c[3][2] * a.c[2][2]);

	return ret;
}

bool equal( const affine &a0, const affine &a1, float tol )
{
	for( uint c = 0; c < 4; c++ )
	{
		for( uint r = 0; r < 3; r++ )
		{
			if( fabsf( a0.c[c][r] - a1.c[c][r] ) > tol )
				return false;
		}
	}

	return true;
}

affine orthonormalize( const affine &a )
{
	affine ret;

	float d;

	const float EPS = 1e-5F;

	ret.col( 0 ) = normalize( a.col( 0 ) );

	d = dot( a.col( 0 ), a.col( 1 ) );
	if( fabsf( d ) > EPS )
		ret.col( 1 ) = normalize( a.col( 1 ) - a.col( 0 ) * d );
	else
		ret.col( 1 ) = normalize( a.col( 1 ) );

	d = dot( a.col( 0 ), a.col( 2 ) );
	if( fabsf( d ) > EPS )
		ret.col( 2 ) = normalize( a.col( 2 ) - a.col( 0 ) * d );
	else
		ret.col( 2 ) = normalize( a.col( 2 ) );

	d = dot( ret.col( 1 ), ret.col( 2 ) );
	if( fabsf( d ) > EPS )
		ret.col( 2 ) = normalize( ret.col( 2 ) - ret.col( 1 ) * d );
	else
		ret.col( 2 ) = normalize( ret.col( 2 ) );
		
	return ret;
}

};
};