#include "stdafx.h"
#include "Mix/Tool/CLR/Math/Quaternion.h"

namespace Mix{ namespace Tool{ namespace Math{

Quaternion::Quaternion( void ) :
x( 0.0f ),
y( 0.0f ),
z( 0.0f ),
w( 1.0f )
{
}

Quaternion::Quaternion( Quaternion% q ) :
x( q.x ),
y( q.y ),
z( q.z ),
w( q.w )
{
}

Quaternion::Quaternion( Quaternion^ q ) :
x( q->x ),
y( q->y ),
z( q->z ),
w( q->w )
{
}

Quaternion::Quaternion( float _x, float _y, float _z, float _w ) :
x( _x ),
y( _y ),
z( _z ),
w( _w )
{
}

float Quaternion::X::get( void )
{
	return x;
}

void Quaternion::X::set( float value )
{
	x = value;
}

float Quaternion::Y::get( void )
{
	return y;
}

void Quaternion::Y::set( float value )
{
	y = value;
}

float Quaternion::Z::get( void )
{
	return z;
}

void Quaternion::Z::set( float value )
{
	z = value;
}

float Quaternion::W::get( void )
{
	return w;
}

void Quaternion::W::set( float value )
{
	w = value;
}

Mix::Tool::Math::Vector^ Quaternion::Axis::get( void )
{
	float s = ::sinf( ::acosf( w ) );
	Mix::Tool::Math::Vector^ axis = gcnew Mix::Tool::Math::Vector();

	if( MIX_FLOAT_IS_ZERO( s ) == false )
	{
		float invS = 1.0f / s;

		axis->X = x * invS;
		axis->Y = y * invS;
		axis->Z = z * invS;
		axis->W = 1.0f;

		axis->Normalize();
	}
	else
	{
		axis->X = 0.0f;
		axis->Y = 0.0f;
		axis->Z = 0.0f;
		axis->W = 0.0f;
	}

	return axis;
}

float Quaternion::Angle::get( void )
{
	return ::acosf( w ) * 2.0f;
}

Mix::Tool::Math::Matrix^ Quaternion::Matrix::get( void )
{
	Mix::Tool::Math::Matrix^ mat = gcnew Mix::Tool::Math::Matrix();

	float xx = 2.0f * x * x;
	float xy = 2.0f * x * y;
	float xz = 2.0f * x * z;
	float yy = 2.0f * y * y;
	float yz = 2.0f * y * z;
	float zz = 2.0f * z * z;
	float wx = 2.0f * w * x;
	float wy = 2.0f * w * y;
	float wz = 2.0f * w * z;
	float ww = 2.0f * w * w;

	mat->x.X = 1.0f - yy - zz;
    mat->x.Y = xy + wz;
    mat->x.Z = xz - wy;
	mat->x.W = 0.0f;

    mat->y.X = xy - wz;
    mat->y.Y = 1.0f - xx - zz;
    mat->y.Z = yz + wx;
	mat->y.W = 0.0f;

    mat->z.X = xz + wy;
    mat->z.Y = yz - wx;
    mat->z.Z = 1.0f - xx - yy;
	mat->z.W = 0.0f;

	mat->w.X = 0.0f;
	mat->w.Y = 0.0f;
	mat->w.Z = 0.0f;
	mat->w.W = 1.0f;

	return mat;
}

void Quaternion::SetRotationAxis( Mix::Tool::Math::Vector^ axis, float angle )
{
	float ha;
	float s;

	ha = ( angle * 0.5f );
	s = ::sinf( ha );

	x = axis->X * s;
	y = axis->Y * s;
	z = axis->Z * s;
	w = ::cosf( ha );
}

void Quaternion::RotationAxis( Mix::Tool::Math::Vector^ axis, float angle )
{
	float ha = ( angle * 0.5f );
	float s = ::sinf( ha );

	float rx = axis->X * s;
	float ry = axis->Y * s;
	float rz = axis->Z * s;
	float rw = ::cosf( ha );

	float tx = x;
	float ty = y;
	float tz = z;
	float tw = w;

	x = rw * tx + rx * tw + ry * tz - rz * ty;
	y = rw * ty - rx * tz + ry * tw + rz * tx;
	z = rw * tz + rx * ty - ry * tx + rz * tw;
	w = rw * tw - rx * tx - ry * ty - rz * tz;
}

void Quaternion::Normalize( void )
{
	float s = x * x + y * y + z * z + w * w;

	if( MIX_FLOAT_IS_ZERO( s ) == false )
	{
		float invS = 1.0f / s;

		x = -x * invS;
		y = -y * invS;
		z = -z * invS;
		w =  w * invS;
	}
	else
	{
		x = 0.0f;
		y = 0.0f;
		z = 0.0f;
		w = 1.0f;
	}
}

System::String^ Quaternion::ToString()
{
	return System::String::Format( L"{0},{1},{2},{3}", x, y, z, w );
}

Quaternion^ Quaternion::Parse( System::String^ str )
{
	array<wchar_t>^ tokn= { L',' };
	array<System::String^>^ strList = nullptr;
	Quaternion^ quat = gcnew Quaternion();

	strList = str->Split( tokn );

	if( strList->Length != 4 )
	{
		return quat;
	}

	try
	{
		quat->x = System::Single::Parse( strList[0] );
		quat->y = System::Single::Parse( strList[1] );
		quat->z = System::Single::Parse( strList[2] );
		quat->w = System::Single::Parse( strList[3] );
	}
	catch( System::Exception^ )
	{
		throw;
	}

	return quat;
}

}}}
