#include "munga.h"
#pragma hdrstop

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

const AffineMatrix AffineMatrix::Identity(true);

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

AffineMatrix& AffineMatrix::BuildIdentity()
{
	Check_Pointer(this);

	entries[0] = 1.0f;
	entries[1] = 0.0f;
	entries[2] = 0.0f;
	entries[3] = 0.0f;

	entries[4] = 0.0f;
	entries[5] = 1.0f;
	entries[6] = 0.0f;
	entries[7] = 0.0f;

	entries[8] = 0.0f;
	entries[9] = 0.0f;
	entries[10] = 1.0f;
	entries[11] = 0.0f;
	return *this;
}

AffineMatrix& AffineMatrix::operator=(const AffineMatrix& m)
{
	Check_Pointer(this);
	Check(&m);

	//#if sizeof(entries) > sizeof(m.entries)
	//#	error memcpy mismatch
	//#endif

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

AffineMatrix& AffineMatrix::operator=(const Origin& p)
{
	Check_Pointer(this);
	Check(&p);

	*this = p.angularPosition;
	*this = p.linearPosition;
	return *this;
}

AffineMatrix& AffineMatrix::operator=(const Hinge &hinge)
{
	Check_Pointer(this);
	Check(&hinge);
	SinCosPair	x,y,z;

	switch (hinge.axisNumber)
	{
	case X_Axis:
		x = hinge.rotationAmount;
		(*this)(0,0) = 1.0f;
		(*this)(0,1) = 0.0f;
		(*this)(0,2) = 0.0f;

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

		(*this)(2,0) = 0.0f;
		(*this)(2,1) = x.sine;
		(*this)(2,2) = x.cosine;

		break;
	case Y_Axis:
		y = hinge.rotationAmount;
		(*this)(0,0) = y.cosine;
		(*this)(0,1) = 0.0f;
		(*this)(0,2) = y.sine;

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

		(*this)(2,0) = -y.sine;
		(*this)(2,1) = 0.0f;
		(*this)(2,2) = y.cosine;
		break;
	case Z_Axis:
		z = hinge.rotationAmount;
		(*this)(0,0) = z.cosine;
		(*this)(0,1) = -z.sine;
		(*this)(0,2) = 0.0f;

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

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

//
//#############################################################################
//#############################################################################
//
AffineMatrix&
	AffineMatrix::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)(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)(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;

	Check(this);
	return *this;
}

//
//#############################################################################
//#############################################################################
//
AffineMatrix&
	AffineMatrix::operator=(const YawPitchRoll &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 + x.sine*y.sine*z.sine;
	(*this)(0,1) = x.cosine*z.sine;
	(*this)(0,2) = x.sine*y.cosine*z.sine - y.sine*z.cosine;

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

	(*this)(2,0) = x.cosine*y.sine;
	(*this)(2,1) = -x.sine;
	(*this)(2,2) = x.cosine*y.cosine;

	Check(this);
	return *this;
}

//
//###########################################################################
//###########################################################################
//
AffineMatrix&
	AffineMatrix::operator=(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;
}

//
//###########################################################################
//###########################################################################
//
AffineMatrix&
	AffineMatrix::operator=(const Matrix4x4 &m)
{
	Check_Pointer(this);
	Check(&m);
	Warn(!Small_Enough(m(0,3)));
	Warn(!Small_Enough(m(1,3)));
	Warn(!Small_Enough(m(2,3)));
	Warn(!Close_Enough(m(3,3),1.0f));

	(*this)(0,0) = m(0,0);
	(*this)(0,1) = m(0,1);
	(*this)(0,2) = m(0,2);

	(*this)(1,0) = m(1,0);
	(*this)(1,1) = m(1,1);
	(*this)(1,2) = m(1,2);

	(*this)(2,0) = m(2,0);
	(*this)(2,1) = m(2,1);
	(*this)(2,2) = m(2,2);

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

//
//###########################################################################
//###########################################################################
//
AffineMatrix&
	AffineMatrix::operator=(const TransposedMatrix &m)
{
	Check_Pointer(this);
	Check(&m);
	Warn(!Small_Enough(m(3,0)));
	Warn(!Small_Enough(m(3,1)));
	Warn(!Small_Enough(m(3,2)));
	Warn(!Close_Enough(m(3,3),1.0f));

	(*this)(0,0) = m(0,0);
	(*this)(0,1) = m(1,0);
	(*this)(0,2) = m(2,0);

	(*this)(1,0) = m(0,1);
	(*this)(1,1) = m(1,1);
	(*this)(1,2) = m(2,1);

	(*this)(2,0) = m(0,2);
	(*this)(2,1) = m(1,2);
	(*this)(2,2) = m(2,2);

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

//
//###########################################################################
//###########################################################################
//
Logical
	AffineMatrix::operator==(const AffineMatrix& m) const
{
	Check(this);
	Check(&m);

	for (size_t i=0; i<ELEMENTS(entries); ++i) {
		if (!Close_Enough(entries[i],m.entries[i])) {
			return False;
		}
	}
	return True;
}

//
//###########################################################################
//###########################################################################
//
Logical
	AffineMatrix::operator!=(const AffineMatrix& m) const
{
	Check(this);
	Check(&m);

	for (size_t i=0; i<ELEMENTS(entries); ++i) {
		if (!Close_Enough(entries[i],m.entries[i])) {
			return True;
		}
	}
	return False;
}

//
//###########################################################################
//###########################################################################
//
void
	AffineMatrix::GetFromAxis(
		size_t index,
		Vector3D *v
	) const
{
	Check(this);
	Check_Pointer(v);
	Warn(index>W_Axis);

	v->x = (*this)(index,X_Axis);
	v->y = (*this)(index,Y_Axis);
	v->z = (*this)(index,Z_Axis);
}

//
//###########################################################################
//###########################################################################
//
void
	AffineMatrix::GetToAxis(
		size_t index,
		Vector3D *v
	) const
{
	Check(this);
	Check_Pointer(v);
	Warn(index>Z_Axis);

	v->x = (*this)(X_Axis,index);
	v->y = (*this)(Y_Axis,index);
	v->z = (*this)(Z_Axis,index);
}

//
//###########################################################################
//###########################################################################
//
AffineMatrix&
	AffineMatrix::SetFromAxis(
		size_t index,
		const Vector3D &v
	)
{
	Check_Pointer(this);
	Check(&v);
	Warn(index>W_Axis);

	(*this)(index,X_Axis) = v.x;
	(*this)(index,Y_Axis) = v.y;
	(*this)(index,Z_Axis) = v.z;
	return *this;
}

//
//###########################################################################
//###########################################################################
//
AffineMatrix&
	AffineMatrix::SetToAxis(
		size_t index,
		const Vector3D &v
	)
{
	Check_Pointer(this);
	Check(&v);
	Warn(index>Z_Axis);

	(*this)(X_Axis,index) = v.x;
	(*this)(Y_Axis,index) = v.y;
	(*this)(Z_Axis,index) = v.z;
	return *this;
}

//
//###########################################################################
//###########################################################################
//
AffineMatrix&
	AffineMatrix::Multiply(
		const AffineMatrix& Source1,
		const AffineMatrix& Source2
	)
{
	Check_Pointer(this);
	Check(&Source1);
	Check(&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);

	return *this;
}

//
//###########################################################################
//###########################################################################
//
AffineMatrix&
	AffineMatrix::Invert(const AffineMatrix& Source)
{
	Check_Pointer(this);
	Check(&Source);

	(*this)(0,0) = Source(1,1)*Source(2,2) - Source(1,2)*Source(2,1);
	(*this)(1,0) = Source(1,2)*Source(2,0) - Source(1,0)*Source(2,2);
	(*this)(2,0) = Source(1,0)*Source(2,1) - Source(1,1)*Source(2,0);

	Scalar det =
		(*this)(0,0)*Source(0,0)
		 + (*this)(1,0)*Source(0,1)
		 + (*this)(2,0)*Source(0,2);
	Verify(!Small_Enough(det));

	(*this)(3,0) =
		-Source(3,0)*(*this)(0,0)
		 - Source(3,1)*(*this)(1,0)
		 - Source(3,2)*(*this)(2,0);

	(*this)(0,1) = Source(0,2)*Source(2,1) - Source(0,1)*Source(2,2);
	(*this)(1,1) = Source(0,0)*Source(2,2) - Source(0,2)*Source(2,0);
	(*this)(2,1) = Source(0,1)*Source(2,0) - Source(0,0)*Source(2,1);
	(*this)(3,1) =
		-Source(3,0)*(*this)(0,1)
		 - Source(3,1)*(*this)(1,1)
		 - Source(3,2)*(*this)(2,1);

	(*this)(0,2) = Source(0,1)*Source(1,2) - Source(0,2)*Source(1,1);
	(*this)(1,2) = Source(1,0)*Source(0,2) - Source(0,0)*Source(1,2);
	(*this)(2,2) = Source(0,0)*Source(1,1) - Source(0,1)*Source(1,0);
	(*this)(3,2) =
		-Source(3,0)*(*this)(0,2)
		 - Source(3,1)*(*this)(1,2)
		 - Source(3,2)*(*this)(2,2);

	det = 1.0f/det;
	for (int i=0; i<12; ++i)
	{
		entries[i] *= det;
	}

	return *this;
}

//
//###########################################################################
//###########################################################################
//
AffineMatrix&
	AffineMatrix::Multiply(const AffineMatrix &m,const Vector3D &v)
{
	Check_Pointer(this);
	Check(&m);
	Check(&v);

	(*this)(0,0) = m(0,0)*v.x;
	(*this)(1,0) = m(1,0)*v.x;
	(*this)(2,0) = m(2,0)*v.x;
	(*this)(3,0) = m(3,0)*v.x;

	(*this)(0,1) = m(0,1)*v.y;
	(*this)(1,1) = m(1,1)*v.y;
	(*this)(2,1) = m(2,1)*v.y;
	(*this)(3,1) = m(3,1)*v.y;

	(*this)(0,2) = m(0,2)*v.z;
	(*this)(1,2) = m(1,2)*v.z;
	(*this)(2,2) = m(2,2)*v.z;
	(*this)(3,2) = m(3,2)*v.z;

	return *this;
}

//
//###########################################################################
//###########################################################################
//
AffineMatrix&
	AffineMatrix::Multiply(const AffineMatrix& m,const Quaternion &q)
{
	Check_Pointer(this);
	Check(&m);
	Check(&q);

	LinearMatrix t(LinearMatrix::Identity);
	t = q;
	return Multiply(m,t);
}

//
//###########################################################################
//###########################################################################
//
AffineMatrix&
	AffineMatrix::Multiply(const AffineMatrix &m,const Point3D& p)
{
	Check_Pointer(this);
	Check(&m);
	Check(&p);

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

	return *this;
}

//
//###########################################################################
//###########################################################################
//
Scalar
	AffineMatrix::Determinant() const
{
	Check(this);

	return
		(*this)(0,0)*((*this)(1,1)*(*this)(2,2) - (*this)(1,2)*(*this)(2,1))
		 + (*this)(0,1)*((*this)(1,2)*(*this)(2,0) - (*this)(1,0)*(*this)(2,2))
		 + (*this)(0,2)*((*this)(1,0)*(*this)(2,1) - (*this)(1,1)*(*this)(2,0));
}

//
//###########################################################################
//###########################################################################
//
AffineMatrix&
	AffineMatrix::Solve()
{
	Check(this);

	int column;
	Scalar temp;

	//
	//------------------------------------------------------------------
	// Make sure that we get a decent value into the first diagonal spot
	//------------------------------------------------------------------
	//
	if (!(*this)(0,0))
	{
		for (column=0; column<3; ++column)
			if ((*this)(0,column))
				break;
		Verify(column != 3);

		//
		//--------------
		// Swap the columns
		//--------------
		//
		temp					= (*this)(0,0);
		(*this)(0,0)		= (*this)(0,column);
		(*this)(0,column)	= temp;

		temp					= (*this)(1,0);
		(*this)(1,0)		= (*this)(1,column);
		(*this)(1,column)	= temp;

		temp					= (*this)(2,0);
		(*this)(2,0)		= (*this)(2,column);
		(*this)(2,column)	= temp;

		temp					= (*this)(3,0);
		(*this)(3,0)		= (*this)(3,column);
		(*this)(3,column)	= temp;
	}

	//
	//------------------------------------
	// Make sure the diagonal entry is 1.0
	//------------------------------------
	//
	temp = (*this)(0,0);
	(*this)(0,0) = 1.0f;
	(*this)(1,0) /= temp;
	(*this)(2,0) /= temp;
	(*this)(3,0) /= temp;

	//
	//------------------------
	// Make the first row zero
	//------------------------
	//
	temp = (*this)(0,1);
	(*this)(0,1) = 0.0f;
	(*this)(1,1) -= temp * (*this)(1,0);
	(*this)(2,1) -= temp * (*this)(2,0);
	(*this)(3,1) -= temp * (*this)(3,0);

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

	//
	//-------------------------------------------------------------------
	// Make sure that we get a decent value into the second diagonal spot
	//-------------------------------------------------------------------
	//
	if (!(*this)(1,1))
	{
		Verify(!(*this)(2,2));

		//
		//---------------------
		// Swap the (*this) columns
		//---------------------
		//
		temp				= (*this)(1,1);
		(*this)(1,1)	= (*this)(1,2);
		(*this)(1,2)	= temp;

		temp				= (*this)(2,1);
		(*this)(2,1)	= (*this)(2,2);
		(*this)(2,2)	= temp;

		temp				= (*this)(3,1);
		(*this)(3,1)	= (*this)(3,2);
		(*this)(3,2)	= temp;
	}

	//
	//-----------------------------------
	// Make the second diaginal entry 1.0
	//-----------------------------------
	//
	temp				= (*this)(1,1);
	(*this)(1,1)	= 1.0f;
	(*this)(2,1)	/= temp;
	(*this)(3,1)	/= temp;

	//
	//------------------------------------
	// Make the second row zeros otherwise
	//------------------------------------
	//
	temp = (*this)(1,0);
	(*this)(1,0) = 0.0f;
	(*this)(2,0) -= temp * (*this)(2,1);
	(*this)(3,0) -= temp * (*this)(3,1);

	temp = (*this)(1,2);
	(*this)(1,2) = 0.0f;
	(*this)(2,2) -= temp * (*this)(2,1);
	(*this)(3,2) -= temp * (*this)(3,1);

	//
	//---------------------------
	// Make the last diagonal 1.0
	//---------------------------
	//
	Verify((*this)(2,2));
	temp				= (*this)(2,2);
	(*this)(2,2)	= 1.0f;
	(*this)(3,2)	/= temp;

	//
	//------------------------------------
	// Make the third row zeros otherwise
	//------------------------------------
	//
	temp = (*this)(2,0);
	(*this)(2,0) = 0.0f;
	(*this)(3,0) -= temp * (*this)(3,2);

	temp = (*this)(2,1);
	(*this)(2,1) = 0.0f;
	(*this)(3,1) -= temp * (*this)(3,2);

	//
	//-------------------------
	// Return the reduced array
	//-------------------------
	//
	return *this;
}

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

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

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