#include "munga.h"
#pragma hdrstop

#include "matrix.h"
#include "affnmtrx.h"
#include "origin.h"

#if defined(USE_SIGNATURE)
	int
		Is_Signature_Bad(const volatile AbstractMatrix4x4 *)
	{
		return False;
	}
#endif

//
//###########################################################################
//###########################################################################
//
AbstractMatrix4x4&
	AbstractMatrix4x4::operator=(const AbstractMatrix4x4 &m)
{
	//#if sizeof(entries) > sizeof(m.entries)
	//#	error memcpy size mismatch!
	//#endif

	memcpy(entries, m.entries, sizeof(m.entries));
	return *this;
}

//
//###########################################################################
//###########################################################################
//
AbstractMatrix4x4&
	AbstractMatrix4x4::Transpose(const AbstractMatrix4x4 &m)
{
	entries[0]	= m.entries[0];
	entries[1]	= m.entries[4];
	entries[2]	= m.entries[8];
	entries[3]	= m.entries[12];

	entries[4]	= m.entries[1];
	entries[5]	= m.entries[5];
	entries[6]	= m.entries[9];
	entries[7]	= m.entries[13];

	entries[8]	= m.entries[2];
	entries[9]	= m.entries[6];
	entries[10]	= m.entries[10];
	entries[11]	= m.entries[14];

	entries[12]	= m.entries[3];
	entries[13]	= m.entries[7];
	entries[14] = m.entries[11];
	entries[15] = m.entries[15];
	return *this;
}

//
//###########################################################################
//###########################################################################
//
Logical
	AbstractMatrix4x4::TestInstance() const
{
	return True;
}

const Matrix4x4
	Matrix4x4::Identity(True);

//
//###########################################################################
//###########################################################################
//
Matrix4x4&
	Matrix4x4::BuildIdentity()
{
	(*this)(0,0) = 1.0f;
	(*this)(1,0) = 0.0f;
	(*this)(2,0) = 0.0f;
	(*this)(3,0) = 0.0f;

	(*this)(0,1) = 0.0f;
	(*this)(1,1) = 1.0f;
	(*this)(2,1) = 0.0f;
	(*this)(3,1) = 0.0f;

	(*this)(0,2) = 0.0f;
	(*this)(1,2) = 0.0f;
	(*this)(2,2) = 1.0f;
	(*this)(3,2) = 0.0f;

	(*this)(0,3) = 0.0f;
	(*this)(1,3) = 0.0f;
	(*this)(2,3) = 0.0f;
	(*this)(3,3) = 1.0f;

	return *this;
}

//
//###########################################################################
//###########################################################################
//
Matrix4x4&
	Matrix4x4::operator=(const AffineMatrix &m)
{
	(*this)(0,0) = m(0,0);
	(*this)(0,1) = m(0,1);
	(*this)(0,2) = m(0,2);
	(*this)(0,3) = 0.0f;

	(*this)(1,0) = m(1,0);
	(*this)(1,1) = m(1,1);
	(*this)(1,2) = m(1,2);
	(*this)(1,3) = 0.0f;

	(*this)(2,0) = m(2,0);
	(*this)(2,1) = m(2,1);
	(*this)(2,2) = m(2,2);
	(*this)(2,3) = 0.0f;

	(*this)(3,0) = m(3,0);
	(*this)(3,1) = m(3,1);
	(*this)(3,2) = m(3,2);
	(*this)(3,3) = 1.0f;
	return *this;
}

Matrix4x4& Matrix4x4::operator=(const D3DXMATRIX &m)
{
	(*this)(0,0) = m._11;
	(*this)(0,1) = m._12;
	(*this)(0,2) = m._13;
	(*this)(0,3) = m._14;

	(*this)(1,0) = m._21;
	(*this)(1,1) = m._22;
	(*this)(1,2) = m._23;
	(*this)(1,3) = m._24;

	(*this)(2,0) = m._31;
	(*this)(2,1) = m._32;
	(*this)(2,2) = m._33;
	(*this)(2,3) = m._34;

	(*this)(3,0) = m._41;
	(*this)(3,1) = m._42;
	(*this)(3,2) = m._43;
	(*this)(3,3) = m._44;
	return *this;
}

//
//###########################################################################
//###########################################################################
//
Matrix4x4&
	Matrix4x4::operator=(const Origin& p)
{
	Check_Pointer(this);
	Check(&p);

	BuildRotation(p.angularPosition);
	BuildTranslation(p.linearPosition);

	(*this)(0,3) = 0.0f;
	(*this)(1,3) = 0.0f;
	(*this)(2,3) = 0.0f;
	(*this)(3,3) = 1.0f;

	return *this;
}

//
//#############################################################################
//#############################################################################
//
Matrix4x4&
	Matrix4x4::operator=(const EulerAngles &angles)
{
	Check_Pointer(this);
	Check(&angles);

	SinCosPair
		x,
		y,
		z;

	x = angles.pitch;
	y = angles.yaw;
	z = angles.roll;

	(*this)(0,0) = y.cosine*z.cosine;
	(*this)(0,1) = y.cosine*z.sine;
	(*this)(0,2) = -y.sine;
	(*this)(0,3) = 0.0f;

	(*this)(1,0) = x.sine*y.sine*z.cosine - x.cosine*z.sine;
	(*this)(1,1) = x.sine*y.sine*z.sine + x.cosine*z.cosine;
	(*this)(1,2) = x.sine*y.cosine;
	(*this)(1,3) = 0.0f;

	(*this)(2,0) = x.cosine*y.sine*z.cosine + x.sine*z.sine;
	(*this)(2,1) = x.cosine*y.sine*z.sine - x.sine*z.cosine;
	(*this)(2,2) = x.cosine*y.cosine;
	(*this)(2,3) = 0.0f;

	(*this)(3,0) = 0.0f;
	(*this)(3,1) = 0.0f;
	(*this)(3,2) = 0.0f;
	(*this)(3,3) = 1.0f;

	Check(this);
	return *this;
}

//
//###########################################################################
//###########################################################################
//
Matrix4x4&
	Matrix4x4::BuildRotation(const Quaternion &q)
{
	Check_Pointer(this);
	Check(&q);

	Scalar
		a = q.x*q.y,
		b = q.y*q.z,
		c = q.z*q.x,
		d = q.w*q.x,
		e = q.w*q.y,
		f = q.w*q.z,
		g = q.w*q.w,
		h = q.x*q.x,
		i = q.y*q.y,
		j = q.z*q.z;

	(*this)(0,0) = g + h - i - j;
	(*this)(1,0) = 2.0f*(a - f);
	(*this)(2,0) = 2.0f*(c + e);

	(*this)(0,1) = 2.0f*(f + a);
	(*this)(1,1) = g - h + i - j;
	(*this)(2,1) = 2.0f*(b - d);

	(*this)(0,2) = 2.0f*(c - e);
	(*this)(1,2) = 2.0f*(b + d);
	(*this)(2,2) = g - h - i + j;

	return *this;
}

//
//###########################################################################
//###########################################################################
//
Matrix4x4&
	Matrix4x4::operator=(const Quaternion &q)
{
	Check_Pointer(this);
	Check(&q);

	BuildRotation(q);
	(*this)(0,3) = 0.0f;
	(*this)(1,3) = 0.0f;
	(*this)(2,3) = 0.0f;
	(*this)(3,0) = 0.0f;
	(*this)(3,1) = 0.0f;
	(*this)(3,2) = 0.0f;
	(*this)(3,3) = 1.0f;

	return *this;
}

//
//###########################################################################
//###########################################################################
//
Matrix4x4&
	Matrix4x4::BuildTranslation(const Point3D& p)
{
	Check_Pointer(this);
	Check(&p);

	(*this)(3,0) = p.x;
	(*this)(3,1) = p.y;
	(*this)(3,2) = p.z;
	return *this;
}

//
//###########################################################################
//###########################################################################
//
Matrix4x4&
	Matrix4x4::operator=(const Point3D& p)
{
	Check_Pointer(this);
	Check(&p);

	BuildTranslation(p);

	(*this)(0,0) = 1.0f;
	(*this)(0,1) = 0.0f;
	(*this)(0,2) = 0.0f;
	(*this)(0,3) = 0.0f;
	(*this)(1,0) = 0.0f;
	(*this)(1,1) = 1.0f;
	(*this)(1,2) = 0.0f;
	(*this)(1,3) = 0.0f;
	(*this)(2,0) = 0.0f;
	(*this)(2,1) = 0.0f;
	(*this)(2,2) = 1.0f;
	(*this)(2,3) = 0.0f;
	(*this)(3,3) = 1.0f;
	return *this;
}

//
//###########################################################################
//###########################################################################
//
Matrix4x4&
	Matrix4x4::Multiply(
		const Matrix4x4 &Source1,
		const Matrix4x4 &Source2
	)
{
	(*this)(0,0) =
		Source1(0,0)*Source2(0,0)
		 + Source1(0,1)*Source2(1,0)
		 + Source1(0,2)*Source2(2,0)
		 + Source1(0,3)*Source2(3,0);
	(*this)(1,0) =
		Source1(1,0)*Source2(0,0)
		 + Source1(1,1)*Source2(1,0)
		 + Source1(1,2)*Source2(2,0)
		 + Source1(1,3)*Source2(3,0);
	(*this)(2,0) =
		Source1(2,0)*Source2(0,0)
		 + Source1(2,1)*Source2(1,0)
		 + Source1(2,2)*Source2(2,0)
		 + Source1(2,3)*Source2(3,0);
	(*this)(3,0) =
		Source1(3,0)*Source2(0,0)
		 + Source1(3,1)*Source2(1,0)
		 + Source1(3,2)*Source2(2,0)
		 + Source1(3,3)*Source2(3,0);

	(*this)(0,1) =
		Source1(0,0)*Source2(0,1)
		 + Source1(0,1)*Source2(1,1)
		 + Source1(0,2)*Source2(2,1)
		 + Source1(0,3)*Source2(3,1);
	(*this)(1,1) =
		Source1(1,0)*Source2(0,1)
		 + Source1(1,1)*Source2(1,1)
		 + Source1(1,2)*Source2(2,1)
		 + Source1(1,3)*Source2(3,1);
	(*this)(2,1) =
		Source1(2,0)*Source2(0,1)
		 + Source1(2,1)*Source2(1,1)
		 + Source1(2,2)*Source2(2,1)
		 + Source1(2,3)*Source2(3,1);
	(*this)(3,1) =
		Source1(3,0)*Source2(0,1)
		 + Source1(3,1)*Source2(1,1)
		 + Source1(3,2)*Source2(2,1)
		 + Source1(3,3)*Source2(3,1);

	(*this)(0,2) =
		Source1(0,0)*Source2(0,2)
		 + Source1(0,1)*Source2(1,2)
		 + Source1(0,2)*Source2(2,2)
		 + Source1(0,3)*Source2(3,2);
	(*this)(1,2) =
		Source1(1,0)*Source2(0,2)
		 + Source1(1,1)*Source2(1,2)
		 + Source1(1,2)*Source2(2,2)
		 + Source1(1,3)*Source2(3,2);
	(*this)(2,2) =
		Source1(2,0)*Source2(0,2)
		 + Source1(2,1)*Source2(1,2)
		 + Source1(2,2)*Source2(2,2)
		 + Source1(2,3)*Source2(3,2);
	(*this)(3,2) =
		Source1(3,0)*Source2(0,2)
		 + Source1(3,1)*Source2(1,2)
		 + Source1(3,2)*Source2(2,2)
		 + Source1(3,3)*Source2(3,2);

	(*this)(0,3) =
		Source1(0,0)*Source2(0,3)
		 + Source1(0,1)*Source2(1,3)
		 + Source1(0,2)*Source2(2,3)
		 + Source1(0,3)*Source2(3,3);
	(*this)(1,3) =
		Source1(1,0)*Source2(0,3)
		 + Source1(1,1)*Source2(1,3)
		 + Source1(1,2)*Source2(2,3)
		 + Source1(1,3)*Source2(3,3);
	(*this)(2,3) =
		Source1(2,0)*Source2(0,3)
		 + Source1(2,1)*Source2(1,3)
		 + Source1(2,2)*Source2(2,3)
		 + Source1(2,3)*Source2(3,3);
	(*this)(3,3) =
		Source1(3,0)*Source2(0,3)
		 + Source1(3,1)*Source2(1,3)
		 + Source1(3,2)*Source2(2,3)
		 + Source1(3,3)*Source2(3,3);

	return *this;
}

//
//###########################################################################
//###########################################################################
//
Matrix4x4&
	Matrix4x4::Multiply(
		const Matrix4x4 &Source1,
		const AffineMatrix &Source2
	)
{
	(*this)(0,0) =
		Source1(0,0)*Source2(0,0)
		 + Source1(0,1)*Source2(1,0)
		 + Source1(0,2)*Source2(2,0)
		 + Source1(0,3)*Source2(3,0);
	(*this)(1,0) =
		Source1(1,0)*Source2(0,0)
		 + Source1(1,1)*Source2(1,0)
		 + Source1(1,2)*Source2(2,0)
		 + Source1(1,3)*Source2(3,0);
	(*this)(2,0) =
		Source1(2,0)*Source2(0,0)
		 + Source1(2,1)*Source2(1,0)
		 + Source1(2,2)*Source2(2,0)
		 + Source1(2,3)*Source2(3,0);
	(*this)(3,0) =
		Source1(3,0)*Source2(0,0)
		 + Source1(3,1)*Source2(1,0)
		 + Source1(3,2)*Source2(2,0)
		 + Source1(3,3)*Source2(3,0);

	(*this)(0,1) =
		Source1(0,0)*Source2(0,1)
		 + Source1(0,1)*Source2(1,1)
		 + Source1(0,2)*Source2(2,1)
		 + Source1(0,3)*Source2(3,1);
	(*this)(1,1) =
		Source1(1,0)*Source2(0,1)
		 + Source1(1,1)*Source2(1,1)
		 + Source1(1,2)*Source2(2,1)
		 + Source1(1,3)*Source2(3,1);
	(*this)(2,1) =
		Source1(2,0)*Source2(0,1)
		 + Source1(2,1)*Source2(1,1)
		 + Source1(2,2)*Source2(2,1)
		 + Source1(2,3)*Source2(3,1);
	(*this)(3,1) =
		Source1(3,0)*Source2(0,1)
		 + Source1(3,1)*Source2(1,1)
		 + Source1(3,2)*Source2(2,1)
		 + Source1(3,3)*Source2(3,1);

	(*this)(0,2) =
		Source1(0,0)*Source2(0,2)
		 + Source1(0,1)*Source2(1,2)
		 + Source1(0,2)*Source2(2,2)
		 + Source1(0,3)*Source2(3,2);
	(*this)(1,2) =
		Source1(1,0)*Source2(0,2)
		 + Source1(1,1)*Source2(1,2)
		 + Source1(1,2)*Source2(2,2)
		 + Source1(1,3)*Source2(3,2);
	(*this)(2,2) =
		Source1(2,0)*Source2(0,2)
		 + Source1(2,1)*Source2(1,2)
		 + Source1(2,2)*Source2(2,2)
		 + Source1(2,3)*Source2(3,2);
	(*this)(3,2) =
		Source1(3,0)*Source2(0,2)
		 + Source1(3,1)*Source2(1,2)
		 + Source1(3,2)*Source2(2,2)
		 + Source1(3,3)*Source2(3,2);

	(*this)(0,3) = Source1(0,3);
	(*this)(1,3) = Source1(1,3);
	(*this)(2,3) = Source1(2,3);
	(*this)(3,3) = Source1(3,3);

	return *this;
}

//
//###########################################################################
//###########################################################################
//
Matrix4x4&
	Matrix4x4::Multiply(
		const AffineMatrix &Source1,
		const Matrix4x4 &Source2
	)
{
	(*this)(0,0) =
		Source1(0,0)*Source2(0,0)
		 + Source1(0,1)*Source2(1,0)
		 + Source1(0,2)*Source2(2,0);
	(*this)(1,0) =
		Source1(1,0)*Source2(0,0)
		 + Source1(1,1)*Source2(1,0)
		 + Source1(1,2)*Source2(2,0);
	(*this)(2,0) =
		Source1(2,0)*Source2(0,0)
		 + Source1(2,1)*Source2(1,0)
		 + Source1(2,2)*Source2(2,0);
	(*this)(3,0) =
		Source1(3,0)*Source2(0,0)
		 + Source1(3,1)*Source2(1,0)
		 + Source1(3,2)*Source2(2,0)
		 + Source2(3,0);

	(*this)(0,1) =
		Source1(0,0)*Source2(0,1)
		 + Source1(0,1)*Source2(1,1)
		 + Source1(0,2)*Source2(2,1);
	(*this)(1,1) =
		Source1(1,0)*Source2(0,1)
		 + Source1(1,1)*Source2(1,1)
		 + Source1(1,2)*Source2(2,1);
	(*this)(2,1) =
		Source1(2,0)*Source2(0,1)
		 + Source1(2,1)*Source2(1,1)
		 + Source1(2,2)*Source2(2,1);
	(*this)(3,1) =
		Source1(3,0)*Source2(0,1)
		 + Source1(3,1)*Source2(1,1)
		 + Source1(3,2)*Source2(2,1)
		 + Source2(3,1);

	(*this)(0,2) =
		Source1(0,0)*Source2(0,2)
		 + Source1(0,1)*Source2(1,2)
		 + Source1(0,2)*Source2(2,2);
	(*this)(1,2) =
		Source1(1,0)*Source2(0,2)
		 + Source1(1,1)*Source2(1,2)
		 + Source1(1,2)*Source2(2,2);
	(*this)(2,2) =
		Source1(2,0)*Source2(0,2)
		 + Source1(2,1)*Source2(1,2)
		 + Source1(2,2)*Source2(2,2);
	(*this)(3,2) =
		Source1(3,0)*Source2(0,2)
		 + Source1(3,1)*Source2(1,2)
		 + Source1(3,2)*Source2(2,2)
		 + Source2(3,2);

	(*this)(0,3) =
		Source1(0,0)*Source2(0,3)
		 + Source1(0,1)*Source2(1,3)
		 + Source1(0,2)*Source2(2,3);
	(*this)(1,3) =
		Source1(1,0)*Source2(0,3)
		 + Source1(1,1)*Source2(1,3)
		 + Source1(1,2)*Source2(2,3);
	(*this)(2,3) =
		Source1(2,0)*Source2(0,3)
		 + Source1(2,1)*Source2(1,3)
		 + Source1(2,2)*Source2(2,3);
	(*this)(3,3) =
		Source1(3,0)*Source2(0,3)
		 + Source1(3,1)*Source2(1,3)
		 + Source1(3,2)*Source2(2,3)
		 + Source2(3,3);

	return *this;
}

//
//###########################################################################
//###########################################################################
//
Matrix4x4&
	Matrix4x4::Multiply(
		const AffineMatrix &m1,
		const AffineMatrix &m2
	)
{
	Cast_Object(AffineMatrix*,this)->Multiply(m1,m2);
	(*this)(0,3) = 0.0f;
	(*this)(1,3) = 0.0f;
	(*this)(2,3) = 0.0f;
	(*this)(3,3) = 1.0f;
	return *this;
}

//
//###########################################################################
//###########################################################################
//
Matrix4x4&
	Matrix4x4::SetToProjection(
		Scalar Z_Plane,
		Scalar Inv_Q,
		Scalar Dx,
		Scalar Dy,
		Scalar Inv_Dz
	)
{
	Scalar
		temp;

	//
	// This may need to be transposed!!!!
	//
	(*this)(0,0) = 1.0;
	(*this)(0,1) = 0.0;
	(*this)(0,2) = 0.0;
	(*this)(0,3) = 0.0;

	(*this)(1,0) = 0.0;
	(*this)(1,1) = 1.0;
	(*this)(1,2) = 0.0;
	(*this)(1,3) = 0.0;

	temp = Dx * Inv_Dz;
	(*this)(3,0) = temp * Z_Plane;
	(*this)(2,0) = -temp;
	temp = Dy * Inv_Dz;
	(*this)(3,1) = temp * Z_Plane;
	(*this)(2,1) = -temp;

	Inv_Dz *= -Inv_Q;
	(*this)(2,2) = Z_Plane * Inv_Dz;
	(*this)(2,3) = Z_Plane * (1.0 - (*this)(2,2));
	(*this)(3,2) = Inv_Dz;
	(*this)(3,3) = 1.0 - (*this)(2,2);

	return *this;
}

//
//###########################################################################
//###########################################################################
//
D3DXMATRIX Matrix4x4::ToD3DMatrix()
{
	D3DXMATRIX d3dMatrix;
	
	memcpy(d3dMatrix.m, entries, sizeof(Scalar) * 16);
	//// reverse x translation
	//d3dMatrix(3, 0) *= -1;

	//// reverse yaw rotation
	//d3dMatrix(0, 0) *= -1;
	//d3dMatrix(0, 2) *= -1;
	//d3dMatrix(2, 0) *= -1;
	//d3dMatrix(2, 2) *= -1;

	return d3dMatrix;
}

//
//###########################################################################
//###########################################################################
//
std::ostream& operator <<(std::ostream& Stream, const Matrix4x4 &A_Matrix)
{
	Stream << std::setprecision(4);
	Stream << "\n\t| " << std::setw(9) << A_Matrix(0,0) << ", " << std::setw(9);
	Stream << A_Matrix(0,1) << ", " << std::setw(9) << A_Matrix(0,2) << ", ";
	Stream << std::setw(9) << A_Matrix(0,3) << " |\n\t| " << std::setw(9);
	Stream << A_Matrix(1,0) << ", " << std::setw(9) << A_Matrix(1,1) << ", ";
	Stream << std::setw(9) << A_Matrix(1,2) << ", " << std::setw(9) << A_Matrix(1,3);
	Stream << " |\n\t| " << std::setw(9) << A_Matrix(2,0) << ", " << std::setw(9);
	Stream << A_Matrix(2,1) << ", " << std::setw(9) << A_Matrix(2,2) << ", ";
	Stream << std::setw(9) << A_Matrix(2,3) << " |\n\t| " << std::setw(9);
	Stream << A_Matrix(3,0) << ", " << std::setw(9) << A_Matrix(3,1) << ", ";
	Stream << std::setw(9) << A_Matrix(3,2) << ", " << std::setw(9) << A_Matrix(3,3);
	return Stream << " |";
}

//
//###########################################################################
//###########################################################################
//
TransposedMatrix&
	TransposedMatrix::operator=(const AffineMatrix &m)
{
	//#if sizeof(m.entries) > sizeof(entries)
	//#	error memcpy size mismatch!
	//#endif

	memcpy(entries, m.entries, sizeof(m.entries));
	(*this)(0,3) = 0.0f;
	(*this)(1,3) = 0.0f;
	(*this)(2,3) = 0.0f;
	(*this)(3,3) = 1.0f;
	return *this;
}

#if defined(TEST_CLASS)
#	include "matrix.tcp"
#endif