//-----------------------------------------------------------------------------------------------
// Matrix.cpp
//
// Defines various Matrix math classes and utility functions.
//
#include "GypsyPrecompiledHeader.hpp"

//-----------------------------------------------------------------------------------------------
#include "Engine/Core/Matrix.hpp"
#include "Engine/Core/ErrorWarningAssert.hpp"


//-----------------------------------------------------------------------------------------------
// Static member initialization
//
STATIC const Matrix44 Matrix44::IDENTITY;



//-----------------------------------------------------------------------------------------------
// Uses a "cofactor expansion" method to calculate the matrix's determinant by computing several
// sub-matrix (3x3, and beneath those, 2x2) sub-determinants.
//
// Amounts to about 28 multiplies, 17 adds/subtracts, and 40 array indexes.
//
float Matrix44::CalcDeterminant() const
{
	// Compute each of the six 2x2 sub-determinants needed (for the 3x3 determinant calculations)
	const float xy = ( m_values[0] * m_values[5] )  - ( m_values[1] * m_values[4] );
	const float xz = ( m_values[0] * m_values[9] )  - ( m_values[1] * m_values[8] );
	const float yz = ( m_values[4] * m_values[9] )  - ( m_values[5] * m_values[8] );
	const float tx = ( m_values[1] * m_values[12] ) - ( m_values[0] * m_values[13] );
	const float ty = ( m_values[5] * m_values[12] ) - ( m_values[4] * m_values[13] );
	const float zt = ( m_values[8] * m_values[13] ) - ( m_values[9] * m_values[12] );

	// Compute each of four 3x3 sub-determinants using the precalculated 2x2 determinants
	const float yzt = (m_values[6]  * zt) + (m_values[10] * ty) + (m_values[14] * yz);
	const float ztx = (m_values[10] * tx) + (m_values[14] * xz) + (m_values[2]  * zt);
	const float txy = (m_values[14] * xy) - (m_values[2]  * ty) + (m_values[6]  * tx);
	const float xyz = (m_values[2]  * yz) - (m_values[6]  * xz) + (m_values[10] * xy);

	// Compute determinant by combining each of the 3x3 sub-determinants
	const float determinant = (m_values[3] * yzt) + (m_values[7] * ztx) + (m_values[11] * txy) + (m_values[15] * xyz);
	return determinant;
}


//-----------------------------------------------------------------------------------------------
bool Matrix44::Invert()
{
	FATAL_ERROR( "Matrix44::Invert() not yet implemented!" );
}


//-----------------------------------------------------------------------------------------------
// Construct a temporary right-handed rotation around +Z matrix.
//
STATIC Matrix44 Matrix44::CreateZRotationDegrees( float zRotationDegrees )
{
	Vector2 cosSinTheta;
	cosSinTheta.SetUnitLengthAndYawDegrees( zRotationDegrees );
	Vector2 xBasis( cosSinTheta.x, cosSinTheta.y );
	Vector2 yBasis( -cosSinTheta.y, cosSinTheta.x );
	return Matrix44( xBasis, yBasis );
}


//-----------------------------------------------------------------------------------------------
// Construct a temporary right-handed rotation around +Z matrix.
//
STATIC Matrix44 Matrix44::CreateZRotationRadians( float zRotationDegrees )
{
	Vector2 cosSinTheta;
	cosSinTheta.SetUnitLengthAndYawRadians( zRotationDegrees );
	Vector2 xBasis( cosSinTheta.x, cosSinTheta.y );
	Vector2 yBasis( -cosSinTheta.y, cosSinTheta.x );
	return Matrix44( xBasis, yBasis );
}


//-----------------------------------------------------------------------------------------------
// Construct a temporary right-handed rotation around +Y matrix.
//
STATIC Matrix44 Matrix44::CreateYRotationDegrees( float yRotationDegrees )
{
	Vector2 cosSinTheta;
	cosSinTheta.SetUnitLengthAndYawDegrees( yRotationDegrees );
	Vector3 xBasis( cosSinTheta.x, 0.f, -cosSinTheta.y );
	Vector3 yBasis( 0.f, 1.f, 0.f );
	Vector3 zBasis( cosSinTheta.y, 0.f, cosSinTheta.x );
	return Matrix44( xBasis, yBasis, zBasis );
}


//-----------------------------------------------------------------------------------------------
// Construct a temporary right-handed rotation around +Y matrix.
//
STATIC Matrix44 Matrix44::CreateYRotationRadians( float yRotationRadians )
{
	Vector2 cosSinTheta;
	cosSinTheta.SetUnitLengthAndYawRadians( yRotationRadians );
	Vector3 xBasis( cosSinTheta.x, 0.f, -cosSinTheta.y );
	Vector3 yBasis( 0.f, 1.f, 0.f );
	Vector3 zBasis( cosSinTheta.y, 0.f, cosSinTheta.x );
	return Matrix44( xBasis, yBasis, zBasis );
}


//-----------------------------------------------------------------------------------------------
// Construct a temporary right-handed rotation around +X matrix.
//
STATIC Matrix44 Matrix44::CreateXRotationDegrees( float xRotationDegrees )
{
	Vector2 cosSinTheta;
	cosSinTheta.SetUnitLengthAndYawDegrees( xRotationDegrees );
	Vector3 xBasis( 1.f, 0.f, 0.f );
	Vector3 yBasis( 0.f, cosSinTheta.x, cosSinTheta.y );
	Vector3 zBasis( 0.f, -cosSinTheta.y, cosSinTheta.x );
	return Matrix44( xBasis, yBasis, zBasis );
}


//-----------------------------------------------------------------------------------------------
// Construct a temporary right-handed rotation around +X matrix.
//
STATIC Matrix44 Matrix44::CreateXRotationRadians( float xRotationRadians )
{
	Vector2 cosSinTheta;
	cosSinTheta.SetUnitLengthAndYawRadians( xRotationRadians );
	Vector3 xBasis( 1.f, 0.f, 0.f );
	Vector3 yBasis( 0.f, cosSinTheta.x, cosSinTheta.y );
	Vector3 zBasis( 0.f, -cosSinTheta.y, cosSinTheta.x );
	return Matrix44( xBasis, yBasis, zBasis );
}


//-----------------------------------------------------------------------------------------------
// Construct a temporary angle-axis rotation (right-handed around +rotationAxis) matrix.
//
STATIC Matrix44 Matrix44::CreateRotationAboutAxisDegrees( const Vector3& rotationAxis, float rotationDegrees )
{
	Vector2 cosSinAngle;
	cosSinAngle.SetUnitLengthAndYawDegrees( rotationDegrees );
	const float& c = cosSinAngle.x;
	const float& s = cosSinAngle.y;
	const float ic = (1.f - c);

	Matrix44 rotationMatrix;
	rotationMatrix.m_values[0] = (rotationAxis.x * rotationAxis.x * ic) + c;
	rotationMatrix.m_values[1] = (rotationAxis.x * rotationAxis.y * ic) + (rotationAxis.z * s);
	rotationMatrix.m_values[2] = (rotationAxis.x * rotationAxis.z * ic) - (rotationAxis.y * s);

	rotationMatrix.m_values[4] = (rotationAxis.y * rotationAxis.x * ic) - (rotationAxis.z * s);
	rotationMatrix.m_values[5] = (rotationAxis.y * rotationAxis.y * ic) + c;
	rotationMatrix.m_values[6] = (rotationAxis.y * rotationAxis.z * ic) + (rotationAxis.x * s);

	rotationMatrix.m_values[8]  = (rotationAxis.z * rotationAxis.x * ic) + (rotationAxis.y * s);
	rotationMatrix.m_values[9]  = (rotationAxis.z * rotationAxis.y * ic) - (rotationAxis.x * s);
	rotationMatrix.m_values[10] = (rotationAxis.z * rotationAxis.z * ic) + c;

	return rotationMatrix;
}


//-----------------------------------------------------------------------------------------------
// Construct a temporary angle-axis rotation (right-handed around +rotationAxis) matrix.
//
STATIC Matrix44 Matrix44::CreateRotationAboutAxisRadians( const Vector3& rotationAxis, float rotationRadians )
{
	Vector2 cosSinAngle;
	cosSinAngle.SetUnitLengthAndYawRadians( rotationRadians );
	const float& c = cosSinAngle.x;
	const float& s = cosSinAngle.y;
	const float ic = (1.f - c);

	Matrix44 rotationMatrix;
	rotationMatrix.m_values[0] = (rotationAxis.x * rotationAxis.x * ic) + c;
	rotationMatrix.m_values[1] = (rotationAxis.x * rotationAxis.y * ic) + (rotationAxis.z * s);
	rotationMatrix.m_values[2] = (rotationAxis.x * rotationAxis.z * ic) - (rotationAxis.y * s);

	rotationMatrix.m_values[4] = (rotationAxis.y * rotationAxis.x * ic) - (rotationAxis.z * s);
	rotationMatrix.m_values[5] = (rotationAxis.y * rotationAxis.y * ic) + c;
	rotationMatrix.m_values[6] = (rotationAxis.y * rotationAxis.z * ic) + (rotationAxis.x * s);

	rotationMatrix.m_values[8]  = (rotationAxis.z * rotationAxis.x * ic) + (rotationAxis.y * s);
	rotationMatrix.m_values[9]  = (rotationAxis.z * rotationAxis.y * ic) - (rotationAxis.x * s);
	rotationMatrix.m_values[10] = (rotationAxis.z * rotationAxis.z * ic) + c;

	return rotationMatrix;
}


//-----------------------------------------------------------------------------------------------
// Apply a right-handed around +Z rotation to this matrix.
//
Matrix44& Matrix44::ApplyZRotationRadians( float zRotationRadians )
{
	Vector2 cosSinTheta;
	cosSinTheta.SetUnitLengthAndYawRadians( zRotationRadians );
	const float& c = cosSinTheta.x; // cosine( theta )
	const float& s = cosSinTheta.y; // sine( theta )

	const float Xx = m_values[0];
	const float Xy = m_values[1];
	const float Xz = m_values[2];
	const float Xw = m_values[3];

	const float Yx = m_values[4];
	const float Yy = m_values[5];
	const float Yz = m_values[6];
	const float Yw = m_values[7];

	m_values[0] = (Xx * c) + (Yx * s);
	m_values[1] = (Xy * c) + (Yy * s);
	m_values[2] = (Xz * c) + (Yz * s);
	m_values[3] = (Xw * c) + (Yw * s);

	m_values[4] = (Yx * c) - (Xx * s);
	m_values[5] = (Yy * c) - (Xy * s);
	m_values[6] = (Yz * c) - (Xz * s);
	m_values[7] = (Yw * c) - (Xw * s);

	return *this;
}


//-----------------------------------------------------------------------------------------------
// Apply a right-handed rotation around +Y to this matrix.
//
Matrix44& Matrix44::ApplyYRotationRadians( float yRotationRadians )
{
	Vector2 cosSinTheta;
	cosSinTheta.SetUnitLengthAndYawRadians( yRotationRadians );
	const float& c = cosSinTheta.x; // cosine( theta )
	const float& s = cosSinTheta.y; // sine( theta )

	const float Xx = m_values[0];
	const float Xy = m_values[1];
	const float Xz = m_values[2];
	const float Xw = m_values[3];

	const float Zx = m_values[8];
	const float Zy = m_values[9];
	const float Zz = m_values[10];
	const float Zw = m_values[11];

	m_values[0] = (Xx * c) - (Zx * s);
	m_values[1] = (Xy * c) - (Zy * s);
	m_values[2] = (Xz * c) - (Zz * s);
	m_values[3] = (Xw * c) - (Zw * s);

	m_values[8] = (Xx * s) + (Zx * c);
	m_values[9] = (Xy * s) + (Zy * c);
	m_values[10] = (Xz * s) + (Zz * c);
	m_values[11] = (Xw * s) + (Zw * c);

	return *this;
}


//-----------------------------------------------------------------------------------------------
// Apply a roll rotation (right-handed around +X) to this matrix.
//
Matrix44& Matrix44::ApplyXRotationRadians( float xRotationRadians )
{
	Vector2 cosSinTheta;
	cosSinTheta.SetUnitLengthAndYawRadians( xRotationRadians );
	const float& c = cosSinTheta.x; // cosine( theta )
	const float& s = cosSinTheta.y; // sine( theta )

	const float Yx = m_values[4];
	const float Yy = m_values[5];
	const float Yz = m_values[6];
	const float Yw = m_values[7];

	const float Zx = m_values[8];
	const float Zy = m_values[9];
	const float Zz = m_values[10];
	const float Zw = m_values[11];

	m_values[4] = (Zx * s) + (Yx * c);
	m_values[5] = (Zy * s) + (Yy * c);
	m_values[6] = (Zz * s) + (Yz * c);
	m_values[7] = (Zw * s) + (Yw * c);

	m_values[8] = (Zx * c) - (Yx * s);
	m_values[9] = (Zy * c) - (Yy * s);
	m_values[10] = (Zz * c) - (Yz * s);
	m_values[11] = (Zw * c) - (Yw * s);

	return *this;
}


//-----------------------------------------------------------------------------------------------
bool Matrix44::operator == ( const Matrix44& compare ) const
{
	return(	m_values[0] == compare.m_values[0]
	&&	m_values[1] == compare.m_values[1]
	&&	m_values[2] == compare.m_values[2]
	&&	m_values[3] == compare.m_values[3]
	&&	m_values[4] == compare.m_values[4]
	&&	m_values[5] == compare.m_values[5]
	&&	m_values[6] == compare.m_values[6]
	&&	m_values[7] == compare.m_values[7]
	&&	m_values[8] == compare.m_values[8]
	&&	m_values[9] == compare.m_values[9]
	&&	m_values[10] == compare.m_values[10]
	&&	m_values[11] == compare.m_values[11]
	&&	m_values[12] == compare.m_values[12]
	&&	m_values[13] == compare.m_values[13]
	&&	m_values[14] == compare.m_values[14]
	&&	m_values[15] == compare.m_values[15] );
}


//-----------------------------------------------------------------------------------------------
bool Matrix44::operator != ( const Matrix44& compare ) const
{
	return(	m_values[0] != compare.m_values[0]
	&&	m_values[1] != compare.m_values[1]
	&&	m_values[2] != compare.m_values[2]
	&&	m_values[3] != compare.m_values[3]
	&&	m_values[4] != compare.m_values[4]
	&&	m_values[5] != compare.m_values[5]
	&&	m_values[6] != compare.m_values[6]
	&&	m_values[7] != compare.m_values[7]
	&&	m_values[8] != compare.m_values[8]
	&&	m_values[9] != compare.m_values[9]
	&&	m_values[10] != compare.m_values[10]
	&&	m_values[11] != compare.m_values[11]
	&&	m_values[12] != compare.m_values[12]
	&&	m_values[13] != compare.m_values[13]
	&&	m_values[14] != compare.m_values[14]
	&&	m_values[15] != compare.m_values[15] );
}


//-----------------------------------------------------------------------------------------------
// Transforms this matrix by another 4x4 matrix, similar to OpenGL's glMultMatrix().
// Equivalent to:
//		- a column-major right-hand-side multiply (This * Transform), and also
//		- a row-major left-hand-side multiply (Transform * This), which are identical
// 
// Effectively, this amounts to a matrix multiply, with "this" matrix being the one transformed
//	by "transformToApply", and therefore "this" is on the left and "transformToApply" is on the right
//	(in right-handed, column-major, OpenGL-style notation), e.g.:
//
//	This * Transform = This'
//
// [ A B C D ]       [ E . . . ]       [ (A,B,C,D) dot (E,F,G,H)  etc.  etc.  etc. ]
// [ . . . . ]       [ F . . . ]       [ etc.                     etc.  etc.  etc. ]
// [ . . . . ]   *   [ G . . . ]   =   [ etc.                     etc.  etc.  etc. ]
// [ . . . . ]       [ H . . . ]       [ etc.                     etc.  etc.  etc. ]
//   "this"          "transform"                      "this, transformed"
//
// Conceptually this can be thought of as applying this new transform to the coordinate system,
//	"on top" of the "stack" of however the coordinate system has been transformed previously.
// Therefore, this being the "newest" (and right-most) transform in the flattened "stack",
//	points transformed by the resulting matrix will first be affected by this new transform
//	and then subsequently transformed by the previously existing matrix.
//
// Written in matrix notation:   P' = This x TransformToApply x P
// Written as function calls:    P' = This( TransformToApply( P ) )
// ...where P is any future point we will transform with the resulting matrix
//
Matrix44& Matrix44::ApplyTransformation( const Matrix44& transformToApply )
{
	const Matrix44 previousThis( *this );
	const float (&t)[16] = previousThis.m_values;		// 't' is for "this" matrix
	const float (&m)[16] = transformToApply.m_values;	// 'm' is for other "transformToApply" matrix

	m_values[0] = (t[0] * m[0]) + (t[4] * m[1]) + (t[8]  * m[2]) + (t[12] * m[3]);
	m_values[1] = (t[1] * m[0]) + (t[5] * m[1]) + (t[9]  * m[2]) + (t[13] * m[3]);
	m_values[2] = (t[2] * m[0]) + (t[6] * m[1]) + (t[10] * m[2]) + (t[14] * m[3]);
	m_values[3] = (t[3] * m[0]) + (t[7] * m[1]) + (t[11] * m[2]) + (t[15] * m[3]);

	m_values[4] = (t[0] * m[4]) + (t[4] * m[5]) + (t[8]  * m[6]) + (t[12] * m[7]);
	m_values[5] = (t[1] * m[4]) + (t[5] * m[5]) + (t[9]  * m[6]) + (t[13] * m[7]);
	m_values[6] = (t[2] * m[4]) + (t[6] * m[5]) + (t[10] * m[6]) + (t[14] * m[7]);
	m_values[7] = (t[3] * m[4]) + (t[7] * m[5]) + (t[11] * m[6]) + (t[15] * m[7]);

	m_values[8]  = (t[0] * m[8]) + (t[4] * m[9]) + (t[8]  * m[10]) + (t[12] * m[11]);
	m_values[9]  = (t[1] * m[8]) + (t[5] * m[9]) + (t[9]  * m[10]) + (t[13] * m[11]);
	m_values[10] = (t[2] * m[8]) + (t[6] * m[9]) + (t[10] * m[10]) + (t[14] * m[11]);
	m_values[11] = (t[3] * m[8]) + (t[7] * m[9]) + (t[11] * m[10]) + (t[15] * m[11]);

	m_values[12] = (t[0] * m[12]) + (t[4] * m[13]) + (t[8]  * m[14]) + (t[12] * m[15]);
	m_values[13] = (t[1] * m[12]) + (t[5] * m[13]) + (t[9]  * m[14]) + (t[13] * m[15]);
	m_values[14] = (t[2] * m[12]) + (t[6] * m[13]) + (t[10] * m[14]) + (t[14] * m[15]);
	m_values[15] = (t[3] * m[12]) + (t[7] * m[13]) + (t[11] * m[14]) + (t[15] * m[15]);

	return *this;
}


//-----------------------------------------------------------------------------------------------
// Transforms another 4x4 matrix by this matrix.
// 
// Note: 4 times out of 5, you probably want to use ApplyTransformation(), above, rather than
//	this function (which does the reverse of the typical multiplication/transformation).
//
// Effectively, this amounts to a matrix multiply, with "matrixToTransform" being the one
//	transformed by "this" matrix, and therefore "matrixToTransform" is on the left and "this"
//	is on the right (in right-handed, column-major, OpenGL-style notation, at least), i.e.:
//
//	M * This = M'
// 
//    [ A B C D ]       [ E . . . ]       [ (A,B,C,D) dot (E,F,G,H)  etc.  etc.  etc. ]
//    [ . . . . ]       [ F . . . ]       [ etc.                     etc.  etc.  etc. ]
//    [ . . . . ]   *   [ G . . . ]   =   [ etc.                     etc.  etc.  etc. ]
//    [ . . . . ]       [ H . . . ]       [ etc.                     etc.  etc.  etc. ]
// "matrixToTransform"     "this"                    "transformedMatrix"
//
Matrix44 Matrix44::TransformMatrix( const Matrix44& matrixToTransform ) const
{
	const float (&t)[16] = m_values;					// 't' is for "this" matrix
	const float (&m)[16] = matrixToTransform.m_values;	// 'm' is for other "matrixToTransform"
	Matrix44 transformedMatrix;

	transformedMatrix.m_values[0] = (m[0] * t[0]) + (m[4] * t[1]) + (m[8]  * t[2]) + (m[12] * t[3]);
	transformedMatrix.m_values[1] = (m[1] * t[0]) + (m[5] * t[1]) + (m[9]  * t[2]) + (m[13] * t[3]);
	transformedMatrix.m_values[2] = (m[2] * t[0]) + (m[6] * t[1]) + (m[10] * t[2]) + (m[14] * t[3]);
	transformedMatrix.m_values[3] = (m[3] * t[0]) + (m[7] * t[1]) + (m[11] * t[2]) + (m[15] * t[3]);

	transformedMatrix.m_values[4] = (m[0] * t[4]) + (m[4] * t[5]) + (m[8]  * t[6]) + (m[12] * t[7]);
	transformedMatrix.m_values[5] = (m[1] * t[4]) + (m[5] * t[5]) + (m[9]  * t[6]) + (m[13] * t[7]);
	transformedMatrix.m_values[6] = (m[2] * t[4]) + (m[6] * t[5]) + (m[10] * t[6]) + (m[14] * t[7]);
	transformedMatrix.m_values[7] = (m[3] * t[4]) + (m[7] * t[5]) + (m[11] * t[6]) + (m[15] * t[7]);

	transformedMatrix.m_values[8]  = (m[0] * t[8]) + (m[4] * t[9]) + (m[8]  * t[10]) + (m[12] * t[11]);
	transformedMatrix.m_values[9]  = (m[1] * t[8]) + (m[5] * t[9]) + (m[9]  * t[10]) + (m[13] * t[11]);
	transformedMatrix.m_values[10] = (m[2] * t[8]) + (m[6] * t[9]) + (m[10] * t[10]) + (m[14] * t[11]);
	transformedMatrix.m_values[11] = (m[3] * t[8]) + (m[7] * t[9]) + (m[11] * t[10]) + (m[15] * t[11]);

	transformedMatrix.m_values[12] = (m[0] * t[12]) + (m[4] * t[13]) + (m[8]  * t[14]) + (m[12] * t[15]);
	transformedMatrix.m_values[13] = (m[1] * t[12]) + (m[5] * t[13]) + (m[9]  * t[14]) + (m[13] * t[15]);
	transformedMatrix.m_values[14] = (m[2] * t[12]) + (m[6] * t[13]) + (m[10] * t[14]) + (m[14] * t[15]);
	transformedMatrix.m_values[15] = (m[3] * t[12]) + (m[7] * t[13]) + (m[11] * t[14]) + (m[15] * t[15]);

	return transformedMatrix;
}


//-----------------------------------------------------------------------------------------------
// Note: commented-out lines are unnecessary since Matrix44 default-constructs to Identity.
//
STATIC Matrix44 Matrix44::CreatePerpective( float fovDegreesY, float aspect, float zNearDist, float zFarDist )
{
	Matrix44 perspectiveProjection;
	float f = 1.f / tan( 0.5f * ConvertDegreesToRadians( fovDegreesY ) );
	float s = 1.f / (zNearDist - zFarDist);

	perspectiveProjection.m_values[0]	= f / aspect;
	perspectiveProjection.m_values[5]	= f;
	perspectiveProjection.m_values[10]	= (zNearDist + zFarDist) * s;
	perspectiveProjection.m_values[11]	= -1.f;
	perspectiveProjection.m_values[14]	= (2.f * zNearDist * zFarDist) * s;
	perspectiveProjection.m_values[15]	= 0.f;

	return perspectiveProjection;
}


//-----------------------------------------------------------------------------------------------
// Note: commented-out lines are unnecessary since Matrix44 default-constructs to Identity.
//
STATIC Matrix44 Matrix44::CreateOrtho( float leftX, float rightX, float bottomY, float topY, float nearZ, float farZ )
{
	Matrix44 orthographicProjection;
	float invWidthX		= 1.f / (rightX - leftX);
	float invHeightY	= 1.f / (topY - bottomY);
	float invDepthZ		= 1.f / (farZ - nearZ);

	orthographicProjection.m_values[ 0 ]	= 2.f * invWidthX;
	orthographicProjection.m_values[ 5 ]	= 2.f * invHeightY;
	orthographicProjection.m_values[ 10 ]	= -2.f * invDepthZ;
	orthographicProjection.m_values[ 12 ]	= -(rightX + leftX) * invWidthX;
	orthographicProjection.m_values[ 13 ]	= -(topY + bottomY) * invHeightY;
	orthographicProjection.m_values[ 14 ]	= -(farZ + nearZ) * invDepthZ;

	return orthographicProjection;
}


