#include "Mix/Matrix3x3.h"

namespace Mix{

Matrix3x3::Matrix3x3( void )
{
	m00 = 1.0f; m01 = 0.0f; m02 = 0.0f;
	m10 = 0.0f; m11 = 1.0f; m12 = 0.0f;
	m20 = 0.0f; m21 = 0.0f; m22 = 1.0f;
}

Matrix3x3::Matrix3x3(	Float32 _m00, Float32 _m01, Float32 _m02,
						Float32 _m10, Float32 _m11, Float32 _m12,
						Float32 _m20, Float32 _m21, Float32 _m22 )
{
	m00 = _m00; m01 = _m01; m02 = _m02;
	m10 = _m10; m11 = _m11; m12 = _m12;
	m20 = _m20; m21 = _m21; m22 = _m22;
}

Matrix3x3::Matrix3x3( const Matrix3x3& mat )
{
	Mix::Memory::Copy( &( m[0][0] ), &( mat.m[0][0] ), sizeof( Float32[3][3] ) );
}

Matrix3x3::~Matrix3x3( void )
{
}

void Matrix3x3::SetScaling( Float32 s )
{
	m00 = s;    m01 = 0.0f; m02 = 0.0f;
	m10 = 0.0f; m11 = s;    m12 = 0.0f;
	m20 = 0.0f; m21 = 0.0f; m22 = 1.0f;
}

void Matrix3x3::SetScaling( const Mix::Vector2& s )
{
	m00 = s.x;  m01 = 0.0f; m02 = 0.0f;
	m10 = 0.0f; m11 = s.y;  m12 = 0.0f;
	m20 = 0.0f; m21 = 0.0f; m22 = 1.0f;
}

void Matrix3x3::SetSkew( const Mix::Vector2& s )
{
	m00 = 1.0f; m01 = s.x;  m02 = 0.0f;
	m10 = s.y;  m11 = 1.0f; m12 = 0.0f;
	m20 = 0.0f; m21 = 0.0f; m22 = 1.0f;
}

void Matrix3x3::SetRotation( Float32 theta )
{
	Float32 s = ::sinf( theta );
	Float32 c = ::cosf( theta );

	m00 = +c;   m01 = +s;   m02 = 0.0f;
	m10 = -s;   m11 = +c;   m12 = 0.0f;
	m20 = 0.0f; m21 = 0.0f; m22 = 1.0f;
}

void Matrix3x3::SetTranslation( const Mix::Vector2& t )
{
	m00 = 1.0f; m01 = 0.0f; m02 = 0.0f;
	m10 = 0.0f; m11 = 1.0f; m12 = 0.0f;
	m20 = t.x;  m21 = t.y;  m22 = 1.0f;
}

Mix::Vector3 Matrix3x3::GetColumn( UInt32 index ) const
{
	MIX_ASSERT( index <= 2 );

	return Mix::Vector3( m[0][index], m[1][index], m[2][index] );
}

void Matrix3x3::SetColumn( UInt32 index, const Mix::Vector3& v )
{
	MIX_ASSERT( index <= 2 );

	m[0][index] = v.x;
	m[1][index] = v.y;
	m[2][index] = v.z;
}

Mix::Vector3 Matrix3x3::GetRow( UInt32 index ) const
{
	MIX_ASSERT( index <= 2 );

	return Mix::Vector3( m[index][0], m[index][1], m[index][2] );
}

void Matrix3x3::SetRow( UInt32 index, const Mix::Vector3& v )
{
	MIX_ASSERT( index <= 2 );

	m[index][0] = v.x;
	m[index][1] = v.y;
	m[index][2] = v.z;
}

Boolean Matrix3x3::Inverse( void )
{
	Float32 a11 = m00;
	Float32 a12 = m01;
	Float32 a13 = m02;
	Float32 a21 = m10;
	Float32 a22 = m11;
	Float32 a23 = m12;
	Float32 a31 = m20;
	Float32 a32 = m21;
	Float32 a33 = m22;

	Float32 det = a11 * a22 * a33 + a21 * a32 * a13 + a31 * a12 * a23 - a11 * a32 * a23 - a31 * a22 * a13 - a21 * a12 * a33;

	if( MIX_FLOAT_IS_ZERO( det ) == True )
	{
		return False;
	}

	Float32 b11 = a22 * a33 - a23 * a32;
	Float32 b12 = a13 * a32 - a12 * a33;
	Float32 b13 = a12 * a23 - a13 * a22;
	Float32 b21 = a23 * a31 - a21 * a33;
	Float32 b22 = a11 * a33 - a13 * a31;
	Float32 b23 = a13 * a21 - a11 * a23;
	Float32 b31 = a21 * a32 - a22 * a31;
	Float32 b32 = a12 * a31 - a11 * a32;
	Float32 b33 = a11 * a22 - a12 * a21;

	Float32 invDet = 1.0f / det;

	m00 = a11 * invDet;
	m01 = a12 * invDet;
	m02 = a13 * invDet;
	m10 = a21 * invDet;
	m11 = a22 * invDet;
	m12 = a23 * invDet;
	m20 = a31 * invDet;
	m21 = a32 * invDet;
	m22 = a33 * invDet;

	return True;
}

Mix::Matrix3x3 Matrix3x3::ToInverse( void ) const
{
	Mix::Matrix3x3 temp = *this;

	if( temp.Inverse() == False )
	{
		temp = Mix::Matrix3x3::Identity();
	}

	return temp;
}

void Matrix3x3::Transpose( void )
{
	Mix::Matrix3x3 temp;

	temp = *this;

	for( UInt32 i = 0; i < 3; i++ )
	{
		for( UInt32  j = 0; j < 3; j++ )
		{
			m[i][j] = temp.m[j][i];
		}
	}
}

Mix::Matrix3x3 Matrix3x3::ToTranspose( void ) const
{
	Mix::Matrix3x3 temp = *this;

	temp.Transpose();

	return temp;
}

void Matrix3x3::Transform( Mix::Vector2* vectors, UInt32 count ) const
{
	Mix::Vector2* pp = &( vectors[0] );
	Mix::Vector2* pp_end = ( pp + count );
	Float32 tx;
	Float32 ty;

	while( pp != pp_end )
	{
		tx = pp->x;
		ty = pp->y;

		pp->x = ( ( m00 * tx ) + ( m10 * ty ) + m20 );
		pp->y = ( ( m01 * tx ) + ( m11 * ty ) + m21 );

		pp++;
	}
}

void Matrix3x3::Transform( Mix::Vector3* vectors, UInt32 count ) const
{
	Mix::Vector3* pp = &( vectors[0] );
	Mix::Vector3* pp_end = ( pp + count );
	Float32 tx;
	Float32 ty;

	while( pp != pp_end )
	{
		tx = pp->x;
		ty = pp->y;

		pp->x = ( ( m00 * tx ) + ( m10 * ty ) + m20 );
		pp->y = ( ( m01 * tx ) + ( m11 * ty ) + m21 );

		pp++;
	}
}

void Matrix3x3::Transform( const Mix::Vector2* srcVectors, Mix::Vector2* dstVectors, UInt32 count ) const
{
	const Mix::Vector2* psp = &( srcVectors[0] );
	const Mix::Vector2* psp_end = ( psp + count );
	Mix::Vector2* pdp = &( dstVectors[0] );

	while( psp != psp_end )
	{
		pdp->x = ( ( m00 * psp->x ) + ( m10 * psp->y ) + m20 );
		pdp->y = ( ( m01 * psp->x ) + ( m11 * psp->y ) + m21 );

		psp++;
		pdp++;
	}
}

void Matrix3x3::Transform( const Mix::Vector3* srcVectors, Mix::Vector3* dstVectors, UInt32 count ) const
{
	const Mix::Vector3* psp = &( srcVectors[0] );
	const Mix::Vector3* psp_end = ( psp + count );
	Mix::Vector3* pdp = &( dstVectors[0] );

	while( psp != psp_end )
	{
		pdp->x = ( ( m00 * psp->x ) + ( m10 * psp->y ) + m20 );
		pdp->y = ( ( m01 * psp->x ) + ( m11 * psp->y ) + m21 );

		psp++;
		pdp++;
	}
}

void Matrix3x3::TransformSR( Mix::Vector2* vectors, UInt32 count ) const
{
	Mix::Vector2* pp = &( vectors[0] );
	Mix::Vector2* pp_end = ( pp + count );
	Float32 tx;
	Float32 ty;

	while( pp != pp_end )
	{
		tx = pp->x;
		ty = pp->y;

		pp->x = ( m00 * tx ) + ( m10 * ty );
		pp->y = ( m01 * tx ) + ( m11 * ty );

		pp++;
	}
}

void Matrix3x3::TransformSR( Mix::Vector3* vectors, UInt32 count ) const
{
	Mix::Vector3* pp = &( vectors[0] );
	Mix::Vector3* pp_end = ( pp + count );
	Float32 tx;
	Float32 ty;

	while( pp != pp_end )
	{
		tx = pp->x;
		ty = pp->y;

		pp->x = ( m00 * tx ) + ( m10 * ty );
		pp->y = ( m01 * tx ) + ( m11 * ty );

		pp++;
	}
}

void Matrix3x3::TransformSR( const Mix::Vector2* srcVectors, Mix::Vector2* dstVectors, UInt32 count ) const
{
	const Mix::Vector2* psp = &( srcVectors[0] );
	const Mix::Vector2* psp_end = ( psp + count );
	Mix::Vector2* pdp = &( dstVectors[0] );

	while( psp != psp_end )
	{
		pdp->x = ( m00 * psp->x ) + ( m10 * psp->y );
		pdp->y = ( m01 * psp->x ) + ( m11 * psp->y );

		psp++;
		pdp++;
	}
}

void Matrix3x3::TransformSR( const Mix::Vector3* srcVectors, Mix::Vector3* dstVectors, UInt32 count ) const
{
	const Mix::Vector3* psp = &( srcVectors[0] );
	const Mix::Vector3* psp_end = ( psp + count );
	Mix::Vector3* pdp = &( dstVectors[0] );

	while( psp != psp_end )
	{
		pdp->x = ( m00 * psp->x ) + ( m10 * psp->y );
		pdp->y = ( m01 * psp->x ) + ( m11 * psp->y );

		psp++;
		pdp++;
	}
}

Boolean Matrix3x3::operator == ( const Matrix3x3& mat ) const
{
	for( UInt32 i = 0; i < 3; i++ )
	{
		for( UInt32 j = 0; j < 3; j++ )
		{
			if( MIX_FLOAT_EQUAL( m[i][j], mat.m[i][j] ) == False )
			{
				return False;
			}
		}
	}

	return True;
}

Boolean Matrix3x3::operator != ( const Matrix3x3& mat ) const
{
	for( UInt32 i = 0; i < 3; i++ )
	{
		for( UInt32 j = 0; j < 3; j++ )
		{
			if( MIX_FLOAT_EQUAL( m[i][j], mat.m[i][j] ) == False )
			{
				return True;
			}
		}
	}

	return False;
}

Matrix3x3& Matrix3x3::operator = ( const Matrix3x3& mat )
{
	Mix::Memory::Copy( &( m[0][0] ), &( mat.m[0][0] ), sizeof( Float32[3][3] ) );

	return *this;
}

Matrix3x3& Matrix3x3::operator *= ( const Matrix3x3& mat )
{
	Float32 tm00 = ( m00 * mat.m00 ) + ( m01 * mat.m10 ) + ( m02 * mat.m20 );
	Float32 tm01 = ( m00 * mat.m01 ) + ( m01 * mat.m11 ) + ( m02 * mat.m21 );
	Float32 tm02 = ( m00 * mat.m02 ) + ( m01 * mat.m12 ) + ( m02 * mat.m22 );
	Float32 tm10 = ( m10 * mat.m00 ) + ( m11 * mat.m10 ) + ( m12 * mat.m20 );
	Float32 tm11 = ( m10 * mat.m01 ) + ( m11 * mat.m11 ) + ( m12 * mat.m21 );
	Float32 tm12 = ( m10 * mat.m02 ) + ( m11 * mat.m12 ) + ( m12 * mat.m22 );
	Float32 tm20 = ( m20 * mat.m00 ) + ( m21 * mat.m10 ) + ( m22 * mat.m20 );
	Float32 tm21 = ( m20 * mat.m01 ) + ( m21 * mat.m11 ) + ( m22 * mat.m21 );
	Float32 tm22 = ( m20 * mat.m02 ) + ( m21 * mat.m12 ) + ( m22 * mat.m22 );

	m00 = tm00; m01 = tm01; m02 = tm02;
	m10 = tm10; m11 = tm11; m12 = tm12;
	m20 = tm20; m21 = tm21; m22 = tm22;

	return *this;
}

Matrix3x3 Matrix3x3::operator * ( const Matrix3x3& mat ) const
{
	Matrix3x3 ret = *this;

	ret *= mat;

	return ret;
}

Vector2 Matrix3x3::operator * ( const Vector2& v ) const
{
	return Mix::Vector2(	( m00 * v.x ) + ( m10 * v.y ) + m20,
							( m01 * v.x ) + ( m11 * v.y ) + m21 );
}

Vector3 Matrix3x3::operator * ( const Vector3& v ) const
{
	return Mix::Vector3(	( m00 * v.x ) + ( m10 * v.y ) + m20,
							( m01 * v.x ) + ( m11 * v.y ) + m21,
							v.z );
}

}
