//
// 2x3 matrix class
//

#include "PajaTypes.h"
#include "Matrix2C.h"
#include "Vector2C.h"
#include <math.h>
#include <assert.h>


using namespace PajaTypes;


Matrix2C::Matrix2C()
{
	// empty
}

Matrix2C::Matrix2C( const Matrix2C& rMat )
{
	m_rMat[0] = rMat.m_rMat[0];
	m_rMat[1] = rMat.m_rMat[1];
	m_rMat[2] = rMat.m_rMat[2];
}

Matrix2C::Matrix2C( const float32* pMat )
{
	for( int32 i = 0; i < 3; i++ )
		for( int32 j = 0; j < 2; j++ )
			m_rMat[i][j] = pMat[i * 2 + j];
}


Matrix2C::~Matrix2C()
{
	// empty
}


Matrix2C
Matrix2C::operator-() const
{
	Matrix2C	rRes;
	for( int32 row = 0; row < 3; row++ )
		for( int32 col = 0; col < 2; col++ )
			rRes[row][col] = -m_rMat[row][col];
	return rRes;
}

Matrix2C
Matrix2C::operator-( const Matrix2C& m ) const
{
	Matrix2C	rRes;
	for( int32 row = 0; row < 3; row++ )
		for( int32 col = 0; col < 2; col++ )
			rRes[row][col] = m_rMat[row][col] - m[row][col];
	return rRes;
}

Matrix2C
Matrix2C::operator+( const Matrix2C& m ) const
{
	Matrix2C	rRes;
	for( int32 row = 0; row < 3; row++ )
		for( int32 col = 0; col < 2; col++ )
			rRes[row][col] = m_rMat[row][col] + m[row][col];
	return rRes;
}

Matrix2C&
Matrix2C::operator-=( const Matrix2C& rM )
{
	Matrix2C	rRes;
	for( int32 row = 0; row < 3; row++ )
		for( int32 col = 0; col < 2; col++ )
			rRes[row][col] = m_rMat[row][col] - rM[row][col];
  
	*this = rRes;
	return *this;
}

Matrix2C&
Matrix2C::operator+=( const Matrix2C& rM )
{
	Matrix2C	rRes;
	for( int32 row = 0; row < 3; row++ )
		for( int32 col = 0; col < 2; col++ )
			rRes[row][col] = m_rMat[row][col] + rM[row][col];
  
	*this = rRes;
	return *this;
}
  
Matrix2C
Matrix2C::operator*( const Matrix2C& m ) const
{
	Matrix2C	result;

	for( int32 row = 0; row < 3; row++ ) {
		for( int32 col = 0; col < 2; col++ ) {
			result.m_rMat[row][col] = m_rMat[row][0] * m.m_rMat[0][col] +
									 m_rMat[row][1] * m.m_rMat[1][col];
			if ( row == 2 )
				result.m_rMat[row][col] += m.m_rMat[2][col];
		}
	}
  
	return result;
}


Matrix2C&
Matrix2C::operator*=( const Matrix2C& m )
{
	Matrix2C	result;

	for( int32 row = 0; row < 3; row++ ) {
		for( int32 col = 0; col < 2; col++ ) {
			result.m_rMat[row][col] = m_rMat[row][0] * m.m_rMat[0][col] +
									 m_rMat[row][1] * m.m_rMat[1][col];
			if ( row == 2 )
				result.m_rMat[row][col] += m.m_rMat[2][col];
		}
	}
  
	*this = result;
  
	return *this;
}

Matrix2C&
Matrix2C::set_identity()
{
	for( int32 i = 0; i < 3; i++ )
		for( int32 j = 0; j < 2; j++ )
			if( i == j )
				m_rMat[i][j] = 1.0;
			else
				m_rMat[i][j] = 0.0;

	return *this;
}


Matrix2C&
Matrix2C::set_trans( const Vector2C& v )
{
	set_identity();
	m_rMat[2] = v;
  
	return *this;
}

Matrix2C&
Matrix2C::set_scale( const Vector2C& v )
{
	set_identity();
	m_rMat[0][0] = v[0];
	m_rMat[1][1] = v[1];
  
	return *this;
}


Matrix2C&
Matrix2C::set_rot( float32 a )
{
	float32  sina, cosa;

	sina = (float32)sin( a );
	cosa = (float32)cos( a );

	set_identity();
  
	m_rMat[0][0] = cosa;
	m_rMat[0][1] = sina;
	m_rMat[1][0] = -sina;
	m_rMat[1][1] = cosa;
  
	return *this;
}


Matrix2C
Matrix2C::pre_trans( const Vector2C& v ) const
{
	Matrix2C	rRes;

	for( int32 i = 0; i < 2; i++ )
		for( int32 j = 0; j < 2; j++ )
			rRes.m_rMat[i][j] = m_rMat[i][j];

	rRes.m_rMat[2][0] = v[0] * m_rMat[0][0] + v[1] * m_rMat[1][0] + m_rMat[2][0];
	rRes.m_rMat[2][1] = v[0] * m_rMat[0][1] + v[1] * m_rMat[1][1] + m_rMat[2][1];

	return rRes;
}


Matrix2C
Matrix2C::transpose() const
{
	Matrix2C	rRes;

	for( int32 col = 0; col < 2; col++ ) {
		for( int32 row = 0; row < 2; row++ )
			rRes[row][col] = m_rMat[col][row];
		rRes[2][col] = -(rRes[0][col] * m_rMat[2][0] +
								rRes[1][col] * m_rMat[2][1] );
	}

	return rRes;
}


//
// invert matrix using Cramer's rule
//
Matrix2C
Matrix2C::invert() const
{
	Matrix2C	rRet;
	float32		f32Det = m_rMat[0][0] * m_rMat[1][1] - m_rMat[0][1] * m_rMat[1][0];

	if( f32Det == 0.0f ) {
		rRet.set_identity();
		return rRet;
	}

	f32Det = 1.0f / f32Det;

	rRet[0][0] = m_rMat[1][1] * f32Det;
	rRet[0][1] = -m_rMat[0][1] * f32Det;
	rRet[1][0] = -m_rMat[1][0] * f32Det;
	rRet[1][1] = m_rMat[0][0] * f32Det;

	rRet[2][0] = -m_rMat[2][0] * rRet[0][0] - m_rMat[2][1] * rRet[1][0];
	rRet[2][1] = -m_rMat[2][0] * rRet[0][1] - m_rMat[2][1] * rRet[1][1];

	return rRet;
}


Matrix2C
Matrix2C::ortho_norm() const
{
	Matrix2C	rRes;
	float32		scale;

	for( int32 col = 0; col < 2; col++ ) {

		scale = m_rMat[0][col] * m_rMat[0][col] + m_rMat[1][col] * m_rMat[1][col];
		if( scale == 0.0 ) {
			rRes.set_identity();
			return rRes;
		}
		scale = 1.0f / (float32)sqrt( scale );

		for( int32 row = 0; row < 2; row++ )
			rRes[row][col] = m_rMat[row][col] * scale;
		rRes[2][col] *= scale;
	}

	return rRes;
}
