/******************************************************************************
	libx42.net - skinned vertex animation library (.NET 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 "Stdafx.h"
#include "Affine.h"
#include "Vector.h"
#include "Quaternion.h"

using ::System::Runtime::InteropServices::OutAttribute;

namespace Libx42
{

namespace Math
{

bool Angle::Equals( Angle a )
{
	return rads == a.rads;
}

bool Angle::Equals( Object^ obj )
{
	if( !obj )
		return false;

	try
	{
		Angle a = safe_cast< Angle >( obj );
		return Equals( a );
	}
	catch( InvalidCastException^ )
	{
		return false;
	}
}

bool Vector2::Equals( Vector2 v )
{
	return x == v.x && y == v.y;
}

bool Vector2::Equals( Object^ obj )
{
	if( !obj )
		return false;

	try
	{
		Vector2 v = safe_cast< Vector2 >( obj );
		return Equals( v );
	}
	catch( InvalidCastException^ )
	{
		return false;
	}
}

bool Vector3::Equals( Vector3 v )
{
	return x == v.x && y == v.y && z == v.z;
}

bool Vector3::Equals( Object^ obj )
{
	if( !obj )
		return false;

	try
	{
		Vector3 v = safe_cast< Vector3 >( obj );
		return Equals( v );
	}
	catch( InvalidCastException^ )
	{
		return false;
	}
}

bool Quaternion::Equals( Quaternion q )
{
	return i == q.i && j == q.j && k == q.k && w == q.w;
}

bool Quaternion::Equals( Object ^obj )
{
	if( !obj )
		return false;

	try
	{
		Quaternion q = safe_cast< Quaternion >( obj );
		return Equals( q );
	}
	catch( InvalidCastException^ )
	{
		return false;
	}
}

Quaternion Quaternion::Slerp( Quaternion a, Quaternion b, float t )
{
	x42::math::quat ret = x42::math::slerp( *(x42::math::quat*)&a, *(x42::math::quat*)&b, t );
	return Quaternion( ret );
}

Quaternion Quaternion::Conjugate( Quaternion q )
{
	return Quaternion( -q.i, -q.j, -q.k, q.w );
}

Quaternion Quaternion::operator * ( Quaternion a, Quaternion b )
{
	Quaternion ret;

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

	return ret;
}

Quaternion Quaternion::FromAxisAngle( Vector3 axis, Angle angle )
{
	float hr = angle.AsRadians * 0.5F;
	float st = sinf( hr );

	return Quaternion( axis.x * st,
		axis.y * st, axis.z * st, cosf( hr ) );
}

void Quaternion::ToAxisAngle( [Out] Vector3 %axis, [Out] Angle %angle )
{
	float ht = acosf( w );
	angle = Angle::FromRadians( ht * 2.0F );

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

Affine Quaternion::ToAffine( void )
{
	using ::x42::math::quat;

	pin_ptr< Quaternion > pinThis = this;
	return Affine( quat_to_affine( *(quat*)pinThis ) );
}

Affine::Affine( const affine_t &a )
{
	pin_ptr< Affine > pinThis = this;
	memcpy( pinThis, &a, sizeof( a ) );
}

Affine::Affine( const ::x42::math::affine &a )
{
	pin_ptr< Affine > pinThis = this;
	memcpy( pinThis, &a, sizeof( a ) );
}

Affine::Affine(
		float c00, float c01, float c02,
		float c10, float c11, float c12,
		float c20, float c21, float c22,
		float c30, float c31, float c32 )

		: c00( c00 ), c01( c01 ), c02( c02 ),
		c10( c10 ), c11( c11 ), c12( c12 ),
		c20( c20 ), c21( c21 ), c22( c22 ),
		c30( c30 ), c31( c31 ), c32( c32 )
{

}

Affine Affine::operator * ( Affine a, Affine b )
{
	Affine ret;

	ret.c00 = a.c00 * b.c00 + a.c10 * b.c01 + a.c20 * b.c02;
	ret.c01 = a.c01 * b.c00 + a.c11 * b.c01 + a.c21 * b.c02;
	ret.c02 = a.c02 * b.c00 + a.c12 * b.c01 + a.c22 * b.c02;

	ret.c10 = a.c00 * b.c10 + a.c10 * b.c11 + a.c20 * b.c12;
	ret.c11 = a.c01 * b.c10 + a.c11 * b.c11 + a.c21 * b.c12;
	ret.c12 = a.c02 * b.c10 + a.c12 * b.c11 + a.c22 * b.c12;

	ret.c20 = a.c00 * b.c20 + a.c10 * b.c21 + a.c20 * b.c22;
	ret.c21 = a.c01 * b.c20 + a.c11 * b.c21 + a.c21 * b.c22;
	ret.c22 = a.c02 * b.c20 + a.c12 * b.c21 + a.c22 * b.c22;

	ret.c30 = a.c00 * b.c30 + a.c10 * b.c31 + a.c20 * b.c32 + a.c30;
	ret.c31 = a.c01 * b.c30 + a.c11 * b.c31 + a.c21 * b.c32 + a.c31;
	ret.c32 = a.c02 * b.c30 + a.c12 * b.c31 + a.c22 * b.c32 + a.c32;

	return ret;
}

bool Affine::Equals( Affine a )
{
	return
		c00 == a.c00 && c01 == a.c01 && c02 == a.c02 &&
		c00 == a.c10 && c11 == a.c11 && c02 == a.c12 &&
		c00 == a.c20 && c21 == a.c21 && c02 == a.c22 &&
		c00 == a.c30 && c31 == a.c31 && c02 == a.c32;
}

bool Affine::Equals( Affine a, float tolerance )
{
	return Equals( *this, a, tolerance );
}

bool Affine::Equals( Affine a, Affine b, float tolerance )
{
	return
		fabsf( a.c00 - b.c00 ) < tolerance &&
		fabsf( a.c01 - b.c01 ) < tolerance &&
		fabsf( a.c02 - b.c02 ) < tolerance &&

		fabsf( a.c10 - b.c10 ) < tolerance &&
		fabsf( a.c11 - b.c11 ) < tolerance &&
		fabsf( a.c12 - b.c12 ) < tolerance &&

		fabsf( a.c20 - b.c20 ) < tolerance &&
		fabsf( a.c21 - b.c21 ) < tolerance &&
		fabsf( a.c22 - b.c22 ) < tolerance &&

		fabsf( a.c30 - b.c30 ) < tolerance &&
		fabsf( a.c31 - b.c31 ) < tolerance &&
		fabsf( a.c32 - b.c32 ) < tolerance;
}

bool Affine::Equals( Object^ obj )
{
	if( !obj )
		return false;

	try
	{
		Affine a = safe_cast< Affine >( obj );
		return Equals( a );
	}
	catch( InvalidCastException^ )
	{
		return false;
	}
}

String^ Affine::ToString( IFormatProvider ^formatProvider )
{
	return String::Format( formatProvider,
		"[ [{0} {1} {2}] [{3} {4} {5}] [{6} {7} {8}] [{9} {10} {11}] ]",
		c00, c01, c02,
		c10, c11, c12,
		c20, c21, c22,
		c30, c31, c32 );
		
}

Vector3 Affine::TransformPoint( Vector3 p )
{
	Vector3 ret;

	ret.x = c00 * p.x + c10 * p.y + c20 * p.z + c30;
	ret.y = c01 * p.x + c11 * p.y + c21 * p.z + c31;
	ret.z = c02 * p.x + c12 * p.y + c22 * p.z + c32;

	return ret;
}

Vector3 Affine::TransformVector( Vector3 v )
{
	Vector3 ret;

	ret.x = c00 * v.x + c10 * v.y + c20 * v.z;
	ret.y = c01 * v.x + c11 * v.y + c21 * v.z;
	ret.z = c02 * v.x + c12 * v.y + c22 * v.z;

	return ret;
}

Affine Affine::ComputeInverse( [Out] float %determinant )
{
	using ::x42::math::affine;

	pin_ptr< Affine > pinThis = this;

	float det;
	
	Affine a = Affine( inverse( *(affine*)pinThis, det ) );
	
	determinant = det;
	return a;
}

Affine Affine::ComputeInverse( void )
{
	using ::x42::math::affine;

	pin_ptr< Affine > pinThis = this;
	return Affine( inverse( *(affine*)pinThis ) );
}

float Affine::ComputeDeterminant( void )
{
	using ::x42::math::affine;

	pin_ptr< Affine > pinThis = this;
	return determinant( *(affine*)pinThis );
}

Affine Affine::Identity::get( void )
{
	return Affine( x42::math::affinei() );
}

};
};