

#include "Matrix3C.h"
#include "Vector3C.h"
#include "QuatC.h"
#include <math.h>
#include <assert.h>

using namespace PajaTypes;


Matrix3C::Matrix3C()
{
	// empty
}

Matrix3C::Matrix3C( const Matrix3C& m )
{
	m_rMat[0] = m[0];
	m_rMat[1] = m[1];
	m_rMat[2] = m[2];
	m_rMat[3] = m[3];
}

Matrix3C::Matrix3C( const float32* m )
{
	for( int32 i = 0; i < 4; i++ )
		for( int32 j = 0; j < 3; j++ )
			m_rMat[i][j] = m[i * 3 + j];
}


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



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

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

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

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

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

Matrix3C
Matrix3C::operator*( const Matrix3C& m ) const
{
	Matrix3C	result;

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


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

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

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

	return *this;
}


Matrix3C&
Matrix3C::set_trans( const Vector3C& v )
{
	set_identity();
	m_rMat[3] = v;
	return *this;
}

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


Matrix3C&
Matrix3C::set_rot_x( float32 a )
{
	float32  sina, cosa;

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

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

Matrix3C&
Matrix3C::set_rot_y( float32 a )
{
	float32  sina, cosa;

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

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

Matrix3C&
Matrix3C::set_rot_z( float32 a )
{
	float32  sina, cosa;

	sina = (float)sin( a );
	cosa = (float)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;
}


Matrix3C&
Matrix3C::set_rot_xyz( float32 xa, float32 ya, float32 za )
{
	set_identity();

	float32	sinx = (float32)sin( xa );
	float32	cosx = (float32)cos( xa );
	float32	siny = (float32)sin( ya );
	float32	cosy = (float32)cos( ya );
	float32	sinz = (float32)sin( za );
	float32	cosz = (float32)cos( za );

	m_rMat[0][0] = cosy * cosz;
	m_rMat[0][1] = cosy * sinz;
	m_rMat[0][2] = -siny;

	m_rMat[1][0] = ((sinx * siny) * cosz) + (cosx * -sinz);
	m_rMat[1][1] = ((sinx * siny) * sinz) + (cosx * cosz);
	m_rMat[1][2] = sinx * cosy;

	m_rMat[2][0] = ((cosx * siny) * cosz) + (-sinx * -sinz);
	m_rMat[2][1] = ((cosx * siny) * sinz) + (-sinx * cosz);
	m_rMat[2][2] = cosx * cosy;

	return *this;
}

Matrix3C
Matrix3C::pre_trans( const Vector3C& v ) const
{
	Matrix3C  rRes;
	int32     i, j;

	for( i = 0; i < 3; i++ )
		for( j = 0; j < 3; j++ )
			rRes[i][j] = m_rMat[i][j];

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

	return rRes;
}


Matrix3C&
Matrix3C::set_rot( const QuatC& rQ )
{
/*	float32 f32XS;
	float32 f32YS;
	float32 f32ZS;
	float32 f32WX;
	float32 f32WY;
	float32 f32WZ;
	float32 f32XX;
	float32 f32XY;
	float32 f32XZ;
	float32 f32YY;
	float32 f32YZ;
	float32 f32ZZ;

    f32XS = rQ[0] * 2.0f;
    f32YS = rQ[1] * 2.0f;
    f32ZS = rQ[2] * 2.0f;
    f32WX = rQ[3] * f32XS;
    f32WY = rQ[3] * f32YS;
    f32WZ = rQ[3] * f32ZS;
    f32XX = rQ[0] * f32XS;
    f32XY = rQ[0] * f32YS;
    f32XZ = rQ[0] * f32ZS;
    f32YY = rQ[1] * f32YS;
    f32YZ = rQ[1] * f32ZS;
    f32ZZ = rQ[2] * f32ZS;

    m_rMat[0][0] = 1.0 - (f32YY + f32ZZ);
    m_rMat[1][0] = f32XY - f32WZ;
    m_rMat[2][0] = f32XZ + f32WY;
    m_rMat[3][0] = 0.0;
    m_rMat[0][1] = f32XY + f32WZ;
    m_rMat[1][1] = 1.0 - (f32XX + f32ZZ);
    m_rMat[2][1] = f32YZ - f32WX;
    m_rMat[3][1] = 0.0;
    m_rMat[0][2] = f32XZ - f32WY;
    m_rMat[1][2] = f32YZ + f32WX;
    m_rMat[2][2] = 1.0 - (f32XX + f32YY);
    m_rMat[3][2] = 0.0;
*/

	float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

	// calculate coefficients
	x2 = rQ[0] + rQ[0];
	y2 = rQ[1] + rQ[1]; 
	z2 = rQ[2] + rQ[2];

	xx = rQ[0] * x2;
	xy = rQ[0] * y2;
	xz = rQ[0] * z2;

	yy = rQ[1] * y2;
	yz = rQ[1] * z2;
	zz = rQ[2] * z2;

	wx = rQ[3] * x2;
	wy = rQ[3] * y2;
	wz = rQ[3] * z2;

	m_rMat[0][0] = 1.0f - (yy + zz);
	m_rMat[0][1] = xy - wz;
	m_rMat[0][2] = xz + wy;

	m_rMat[1][0] = xy + wz;
	m_rMat[1][1] = 1.0f - (xx + zz);
	m_rMat[1][2] = yz - wx;

	m_rMat[2][0] = xz - wy;
	m_rMat[2][1] = yz + wx;
	m_rMat[2][2] = 1.0f - (xx + yy);

	m_rMat[3][0] = 0;
	m_rMat[3][1] = 0;
	m_rMat[3][2] = 0;


	return *this;
}


Matrix3C
Matrix3C::inverse() const
{
	Matrix3C	rRes;

	// calc determinant
	float32	f32S = m_rMat[0][0] * (m_rMat[1][1] * m_rMat[2][2] - m_rMat[1][2] * m_rMat[2][1]) +
					m_rMat[0][1] * (m_rMat[1][2] * m_rMat[2][0] - m_rMat[1][0] * m_rMat[2][2]) +
					m_rMat[0][2] * (m_rMat[1][0] * m_rMat[2][1] - m_rMat[1][1] * m_rMat[2][0]);

	// matrix is singular... return identity
	if( !f32S ) {
		rRes.set_identity();
		return rRes;
	}

	f32S = 1.0f / f32S;

	// Invert the matrix
	rRes[0][0] = f32S * (m_rMat[1][1] * m_rMat[2][2] - m_rMat[1][2] * m_rMat[2][1]);
	rRes[0][1] = f32S * (m_rMat[0][2] * m_rMat[2][1] - m_rMat[0][1] * m_rMat[2][2]);
	rRes[0][2] = f32S * (m_rMat[0][1] * m_rMat[1][2] - m_rMat[0][2] * m_rMat[1][1]);
	rRes[1][0] = f32S * (m_rMat[1][2] * m_rMat[2][0] - m_rMat[1][0] * m_rMat[2][2]);
	rRes[1][1] = f32S * (m_rMat[0][0] * m_rMat[2][2] - m_rMat[0][2] * m_rMat[2][0]);
	rRes[1][2] = f32S * (m_rMat[0][2] * m_rMat[1][0] - m_rMat[0][0] * m_rMat[1][2]);
	rRes[2][0] = f32S * (m_rMat[1][0] * m_rMat[2][1] - m_rMat[1][1] * m_rMat[2][0]);
	rRes[2][1] = f32S * (m_rMat[0][1] * m_rMat[2][0] - m_rMat[0][0] * m_rMat[2][1]);
	rRes[2][2] = f32S * (m_rMat[0][0] * m_rMat[1][1] - m_rMat[0][1] * m_rMat[1][0]);
	rRes[3][0] = -m_rMat[3][0] * rRes[0][0] - m_rMat[3][1] * rRes[1][0] - m_rMat[3][2] * rRes[2][0]; 
	rRes[3][1] = -m_rMat[3][0] * rRes[0][1] - m_rMat[3][1] * rRes[1][1] - m_rMat[3][2] * rRes[2][1]; 
	rRes[3][2] = -m_rMat[3][0] * rRes[0][2] - m_rMat[3][1] * rRes[1][2] - m_rMat[3][2] * rRes[2][2]; 

	return rRes;	
}


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

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

	return rRes;
}


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

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

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

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

	return rRes;
}
