///////////////////////////////////////////////////////////////////////////////
/// @file deMatrix4.cpp
///
/// @brief matrix transformation methods
///
/// @author Assassin
///
/// This file is the intellectual property of Novus Delta, LLC.. Usage of the
/// contents of this file is subject to the Destiny3D Member License which
/// can be found at http://www.destiny3d.com.  Any other usage is prohibited.
///
/// This file is distributed "AS IS" without warranty of any kind.  Novus
/// Delta, LLC. does not guarantee the fitness of the contents of this file
/// for any particular purpose.
///
/// Copyright (C) 2001-2003 Novus Delta, LLC. All Rights Reserved.
///
/// <hr>
///                                 Change History
/// <hr>
///
/// @date Feb 2002
/// @author Assassin
/// @remarks Creation
///
/// @date Jul 2002
/// @author Hootie
/// @remarks Convert to static lib
///
///////////////////////////////////////////////////////////////////////////////

#include "deMath.hpp"

/*  | AX, AY, AZ, Translation.X |  
	| BX, BY, BZ, Translation.Y |  
	| CX, CY, CZ, Translation.Z |  
	|  0,  0,  0,      1        |  

	| LX, UX, IX, TX |  
	| LY, UY, IY, TY |  
	| LZ, UZ, IZ, TZ |  
	|  0,  0,  0,  1 | */

deTransform::deTransform(const deVec3d & Right, const deVec3d & Up, const deVec3d & In)
{ 
	m_Identity = deFALSE; 
	Translation = deZeroVec; 
	R = Right; 
	U = Up; 
	I = In; 
}

deTransform::deTransform(const deVec3d & Right, const deVec3d & Up, const deVec3d & In, const deVec3d & Trans)
{ 
	m_Identity = deFALSE; 
	Translation = Trans; 
	R = Right; 
	U = Up; 
	I = In; 
}

void FromFloat16(const float Mat[16]);
void ToFloat16(float Mat[16]);

void deTransform::FromFloat44(const float Mat[4][4], deBoolean d3d)
{
#pragma todo (test FromFloat44 with both layouts)
	// BLEH - Assassin
	if (Mat == NULL)
		return;
	if (d3d)
	{
		// direct3d layout
		R.x = Mat[0][0]; R.y = Mat[1][0]; R.z = Mat[2][0];
		U.x = Mat[0][1]; U.y = Mat[1][1]; U.z = Mat[2][1];
		I.x = Mat[0][2]; I.y = Mat[1][2]; I.z = Mat[2][2];
		Translation.x = Mat[3][0]; Translation.y = Mat[3][1]; Translation.z = Mat[3][2];
	}
	else
	{
		// opengl layout
		R.x = Mat[0][0]; R.y = Mat[0][1]; R.z = Mat[0][2];
		U.x = Mat[1][0]; U.y = Mat[1][1]; U.z = Mat[1][2];
		I.x = Mat[2][0]; I.y = Mat[2][1]; I.z = Mat[2][2];
		Translation.x = Mat[0][3]; Translation.y = Mat[1][3]; Translation.z = Mat[2][3];
	}
	deDouble len;
	len = Vec3d_Length(R);
	Translation.x /= len;
	//	r = r * (1/len);
	len = Vec3d_Length(U);
	Translation.y /= len;
	//	u = u * (1/len);
	len = Vec3d_Length(I);
	Translation.z /= len;
	//	i = i * (1/len);
}

void deTransform::ToFloat44(float Mat[4][4], deBoolean d3d) const
{
#pragma todo (test ToFloat44 with both layouts)
	if (Mat == NULL)
		return;
	if (d3d)
	{
		// direct3d layout
		Mat[0][0] = float(R.x);
		Mat[0][1] = float(R.y);
		Mat[0][2] = float(R.z);
		Mat[1][0] = float(U.x);
		Mat[1][1] = float(U.y);
		Mat[1][2] = float(U.z);
		Mat[2][0] = float(I.x);
		Mat[2][1] = float(I.y);
		Mat[2][2] = float(I.z);
		Mat[3][0] = float(Translation.x);
		Mat[3][1] = float(Translation.y);
		Mat[3][2] = float(Translation.z);
		Mat[0][3] = 0;
		Mat[1][3] = 0;
		Mat[2][3] = 0;
		Mat[3][3] = 1;
	}
	else
	{
		// opengl layout
		Mat[0][0] = float(R.x);
		Mat[1][0] = float(R.y);
		Mat[2][0] = float(R.z);
		Mat[0][1] = float(U.x);
		Mat[1][1] = float(U.y);
		Mat[2][1] = float(U.z);
		Mat[0][2] = float(I.x);
		Mat[1][2] = float(I.y);
		Mat[2][2] = float(I.z);
		Mat[0][3] = float(Translation.x);
		Mat[1][3] = float(Translation.y);
		Mat[2][3] = float(Translation.z);
		Mat[3][0] = 0;
		Mat[3][1] = 0;
		Mat[3][2] = 0;
		Mat[3][3] = 1;
	}
}

void deTransform::FromDouble44(const double Mat[4][4], deBoolean d3d)
{
#pragma todo (test FromDouble44 with both layouts)
	// BLEH - Assassin
	if (Mat == NULL)
		return;
	if (d3d)
	{
		// direct3d layout
		R.x = Mat[0][0]; R.y = Mat[1][0]; R.z = Mat[2][0];
		U.x = Mat[0][1]; U.y = Mat[1][1]; U.z = Mat[2][1];
		I.x = Mat[0][2]; I.y = Mat[1][2]; I.z = Mat[2][2];
		Translation.x = Mat[3][0]; Translation.y = Mat[3][1]; Translation.z = Mat[3][2];
	}
	else
	{
		// opengl layout
		R.x = Mat[0][0]; R.y = Mat[0][1]; R.z = Mat[0][2];
		U.x = Mat[1][0]; U.y = Mat[1][1]; U.z = Mat[1][2];
		I.x = Mat[2][0]; I.y = Mat[2][1]; I.z = Mat[2][2];
		Translation.x = Mat[0][3]; Translation.y = Mat[1][3]; Translation.z = Mat[2][3];
	}
	deDouble len;
	len = Vec3d_Length(R);
	Translation.x /= len;
	//	r = r * (1/len);
	len = Vec3d_Length(U);
	Translation.y /= len;
	//	u = u * (1/len);
	len = Vec3d_Length(I);
	Translation.z /= len;
	//	i = i * (1/len);
}

void deTransform::SqueezeVectors()
{
	Vec3d_Squeeze(Translation);
	Vec3d_Squeeze(R);
	Vec3d_Squeeze(U);
	Vec3d_Squeeze(I);
}

void deTransform::SetRight(const deVec3d & Right)
{
	R = Right;
	m_Identity = deFALSE;
}

void deTransform::SetUp(const deVec3d & Up)
{
	U = Up;
	m_Identity = deFALSE;
}

void deTransform::SetIn(const deVec3d & In)
{
	I = In;
	m_Identity = deFALSE;
}

void deTransform::SetRightUpIn(const deVec3d & Right, const deVec3d & Up, const deVec3d & In)
{
	R = Right;
	U = Up;
	I = In;
	m_Identity = deFALSE;
}

void deTransform::SetXRot(deDouble r)
{
	deDouble Cos, Sin;

	Cos = COS(r);
	Sin = SIN(r);
	U.Y =  Cos;
	I.Y = -Sin;
	U.Z =  Sin;
	I.Z =  Cos;
	R.X = 1.0f;
	U.X = I.X = R.Y = R.Z = 0.0f;
	Translation = deZeroVec;
	m_Identity = deFALSE;
}

void deTransform::SetYRot(deDouble r)
{
	deDouble Cos, Sin;

	Cos = COS(r);
	Sin = SIN(r);
	R.X =  Cos;
	I.X =  Sin;
	R.Z = -Sin;
	I.Z =  Cos;
	U.Y = 1.0f;
	U.X = R.Y = I.Y = U.Z = 0.0f;
	Translation = deZeroVec;
	m_Identity = deFALSE;
}

void deTransform::SetZRot(deDouble r)
{
	deDouble Cos, Sin;

	Cos = COS(r);
	Sin = SIN(r);
	R.X =  Cos;
	U.X = -Sin;
	R.Y =  Sin;
	U.Y =  Cos;
	I.Z = 1.0f;
	I.X = I.Y = R.Z = U.Z = 0.0f;
	Translation = deZeroVec;
	m_Identity = deFALSE;
}

void deTransform::RotateX(deDouble Rads)
{
	deTransform R;

	R.SetXRot(Rads);
	(*this) = R.Multiply(*this);
	m_Identity = deFALSE;
}

void deTransform::RotateY(deDouble Rads)
{
	deTransform R;

	R.SetYRot(Rads);
	(*this) = R.Multiply(*this);
	m_Identity = deFALSE;
}

void deTransform::RotateZ(deDouble Rads)
{
	deTransform R;

	R.SetZRot(Rads);
	(*this) = R.Multiply(*this);
	m_Identity = deFALSE;
}

void deTransform::FreeRot(const deVec3d &Rads)
{
	FreeRot(Rads.x, Rads.y, Rads.z);
}

void deTransform::FreeRot(deDouble x, deDouble y, deDouble z)
{
	deTransform tempmatrix;

	tempmatrix.SetYRot(y);
	Multiply(tempmatrix, *this);
	tempmatrix.SetXRot(x);
	Multiply(tempmatrix, *this);
	tempmatrix.SetZRot(z);
	Multiply(tempmatrix, *this);
	m_Identity = deFALSE;
}

void deTransform::FreeRotInPlace(const deVec3d &Rads)
{
	FreeRotInPlace(Rads.x, Rads.y, Rads.z);
}

void deTransform::FreeRotInPlace(deDouble x, deDouble y, deDouble z)
{
	deVec3d temppos = Translation;
	FreeRot(x,y,z);
	Translation = temppos;
}

void deTransform::GetRight(deVec3d & r) const
{
	r = R;
}

void deTransform::GetUp(deVec3d & u) const
{
	u = U;
}

void deTransform::GetIn(deVec3d & i) const
{
	i = I;
}

deVec3d deTransform::GetRight() const
{
	return R;
}

deVec3d deTransform::GetUp() const
{
	return U;
}

deVec3d deTransform::GetIn() const
{
	return I;
}

void deTransform::GetRightUpIn(deVec3d & r, deVec3d & u, deVec3d & i) const
{
	r = R;
	u = U;
	i = I;
}

void deTransform::ApplyScaling(deDouble UniformScaling)
{
	R *= UniformScaling;
	U *= UniformScaling;
	I *= UniformScaling;
	m_Identity = deFALSE;
}

void deTransform::ApplyScaling(const deVec3d & Multiples)
{
	R *= Multiples.X;
	U *= Multiples.Y;
	I *= Multiples.Z;
	m_Identity = deFALSE;
}

void deTransform::ApplyScaling(deDouble x, deDouble y, deDouble z)
{
	R *= x;
	U *= y;
	I *= z;
	m_Identity = deFALSE;
}

void deTransform::GetBasisScales(deDouble& x, deDouble& y, deDouble &z)
{
	x = Vec3d_Length(R);
	y = Vec3d_Length(U);
	z = Vec3d_Length(I);
}

deBoolean deTransform::TestIdentity(deDouble Epsilon)
{
	deDouble Epsilon1_a = 1-Epsilon;
	deDouble Epsilon1_b = 1+Epsilon;
	m_Identity = deFALSE;
	if (R.x < Epsilon1_a || R.x > Epsilon1_b || 
		R.y < -Epsilon || R.y > Epsilon || 
		R.z < -Epsilon || R.z > Epsilon)
		return deFALSE;
	if (U.x < -Epsilon || U.x > Epsilon ||
		U.y < Epsilon1_a || U.y > Epsilon1_b ||
		U.z < -Epsilon || U.z > Epsilon)
		return deFALSE;
	if (I.x < -Epsilon || I.x > Epsilon ||
		I.y < -Epsilon || I.y > Epsilon ||
		I.z < Epsilon1_a || I.z > Epsilon1_b)
		return deFALSE;
	// reset them a little
	R = deVecX;
	U = deVecY;
	I = deVecZ;
	m_Identity = deTRUE;
	return deTRUE;
}

deBoolean deTransform::IsOrthogonal() const
{
	// orthogonal == perpendicular == isotropic == linearly independent

	if (fabs(Vec3d_Dot(R,U)) > 0.0001)
		return deFALSE;
	if (fabs(Vec3d_Dot(U,I)) > 0.0001)
		return deFALSE;
	if (fabs(Vec3d_Dot(I,R)) > 0.0001)
		return deFALSE;

	return deTRUE;
}

deBoolean deTransform::IsOrthoNormal() const
{
	// orthonormal == orthogonal with unit length basis vectors

	if (!IsOrthogonal())
		return deFALSE;

	if (fabs(Vec3d_Length(R) - 1.0) > 0.0001)
		return deFALSE;
	if (fabs(Vec3d_Length(U) - 1.0) > 0.0001)
		return deFALSE;
	if (fabs(Vec3d_Length(I) - 1.0) > 0.0001)
		return deFALSE;

	return deTRUE;
}

deBoolean deTransform::OrthoNormalize()
{
	// Gram-Schmidt orthonormalization process

	if (Vec3d_Normalize(R) < 0.0001)
		return deFALSE;

	U -= R*Vec3d_Dot(R,U);
	if (Vec3d_Normalize(U) < 0.0001)
		return deFALSE;

	I -= R*Vec3d_Dot(R,I);
	I -= U*Vec3d_Dot(U,I);
	if (Vec3d_Normalize(I) < 0.0001)
		return deFALSE;

	return deTRUE;
}

void deTransform::GetTranspose(deTransformInfo & MInv)
{
	// Transpose of an orthonormal matrix is also the inverse
	if (&MInv == this)
	{
		deVec3d RL,RU,RI;
		deTransform M(Translation);
		M.Translation *= -1;
		RL = R;
		RU = U;
		RI = I;

		MInv.R.X = RL.X;
		MInv.R.Y = RU.X;
		MInv.R.Z = RI.X;
		MInv.U.X = RL.Y;
		MInv.U.Y = RU.Y;
		MInv.U.Z = RI.Y;
		MInv.I.X = RL.Z;
		MInv.I.Y = RU.Z;
		MInv.I.Z = RI.Z;
		
		MInv.Translation = deZeroVec;
		MInv.m_Identity = m_Identity; // transpose of identity is still identity

		((deTransform&)MInv).Multiply(M, MInv);
	}
	else
	{
		MInv.R.X = R.X;
		MInv.R.Y = U.X;
		MInv.R.Z = I.X;
		MInv.U.X = R.Y;
		MInv.U.Y = U.Y;
		MInv.U.Z = I.Y;
		MInv.I.X = R.Z;
		MInv.I.Y = U.Z;
		MInv.I.Z = I.Z;
		
		MInv.Translation = deZeroVec;
		MInv.m_Identity = deFALSE;
		
		{
			deTransform M(Translation);
			M.Translation *= -1;
			((deTransform&)MInv).Multiply(M, MInv);
		}
	}
}

deTransformInfo deTransform::GetTranspose()
{
	deTransform ret;
	GetTranspose(ret);
	return ret;
}

void deTransform::TransformArray(const deVec3d InVec[], deVec3d OutVec[], int num) const
{
	int i;
	for (i = 0; i < num; i++)
		Transform(InVec[i], OutVec[i]);
}

void deTransform::TransformArray(const deVertex InVec[], deVertex OutVec[], int num) const
{
	int i;
	for (i = 0; i < num; i++)
		Transform(InVec[i], OutVec[i]);
}

void deTransform::Transform(deVec3d VL, deVec3d & OutVec) const
{
	if (m_Identity)
	{
		OutVec = VL;
		OutVec += Translation;
	}
	else
	{
		OutVec.X = (VL.X * R.X) + (VL.Y * U.X) + (VL.Z * I.X) + Translation.X;
		OutVec.Y = (VL.X * R.Y) + (VL.Y * U.Y) + (VL.Z * I.Y) + Translation.Y;
		OutVec.Z = (VL.X * R.Z) + (VL.Y * U.Z) + (VL.Z * I.Z) + Translation.Z;
	}
}

void deTransform::Transform(deVertex VL, deVertex & OutVec) const
{
	if (m_Identity)
	{
		OutVec = VL;
		OutVec += deMake_Vertex(Translation);
	}
	else
	{
		OutVec.X = (deFloat)((VL.X * R.X) + (VL.Y * U.X) + (VL.Z * I.X) + Translation.X);
		OutVec.Y = (deFloat)((VL.X * R.Y) + (VL.Y * U.Y) + (VL.Z * I.Y) + Translation.Y);
		OutVec.Z = (deFloat)((VL.X * R.Z) + (VL.Y * U.Z) + (VL.Z * I.Z) + Translation.Z);
	}
}

void deTransform::TransposeTransform(deVec3d VL, deVec3d & OutVec) const
{
#pragma todo (test TransposeTransform)
	if (m_Identity)
	{
		OutVec = VL;
		OutVec -= Translation;
	}
	else
	{
		VL -= Translation;
		OutVec.X = (VL.X * R.X) + (VL.Y * R.Y) + (VL.Z * R.Z);
		OutVec.Y = (VL.X * U.X) + (VL.Y * U.Y) + (VL.Z * U.Z);
		OutVec.Z = (VL.X * I.X) + (VL.Y * I.Y) + (VL.Z * I.Z);
	}
}

void deTransform::Multiply(const deTransformInfo & rhs, deTransformInfo & ret)
{
	deTransform M(rhs); // just in case rhs is also ret
	deVec3d trans = Translation;
	
	if (m_Identity)
	{
		ret = rhs;
		ret.Translation.x = M.Translation.x + trans.x;
		ret.Translation.y = M.Translation.y + trans.y;
		ret.Translation.z = M.Translation.z + trans.z;
		return;
	}
	if (M.m_Identity)
	{
		ret.R = R;
		ret.U = U;
		ret.I = I;
		ret.m_Identity = m_Identity;
		ret.Translation.X =  R.X * M.Translation.X;
		ret.Translation.X += U.X * M.Translation.Y;
		ret.Translation.X += I.X * M.Translation.Z;
		ret.Translation.X += trans.X;

		ret.Translation.Y =  R.Y * M.Translation.X;
		ret.Translation.Y += U.Y * M.Translation.Y;
		ret.Translation.Y += I.Y * M.Translation.Z;
		ret.Translation.Y += trans.Y;

		ret.Translation.Z =  R.Z * M.Translation.X;
		ret.Translation.Z += U.Z * M.Translation.Y;
		ret.Translation.Z += I.Z * M.Translation.Z;
		ret.Translation.Z += trans.Z;
		return;
	}

	if (this == &ret)
	{
		deTransformInfo ref = *this;

		R.X =  ref.R.X * M.R.X;
		R.X += ref.U.X * M.R.Y;
		R.X += ref.I.X * M.R.Z;

		R.Y =  ref.R.Y * M.R.X;
		R.Y += ref.U.Y * M.R.Y;
		R.Y += ref.I.Y * M.R.Z;

		R.Z =  ref.R.Z * M.R.X;
		R.Z += ref.U.Z * M.R.Y;
		R.Z += ref.I.Z * M.R.Z;

		U.X =  ref.R.X * M.U.X;
		U.X += ref.U.X * M.U.Y;
		U.X += ref.I.X * M.U.Z;

		U.Y =  ref.R.Y * M.U.X;
		U.Y += ref.U.Y * M.U.Y;
		U.Y += ref.I.Y * M.U.Z;

		U.Z =  ref.R.Z * M.U.X;
		U.Z += ref.U.Z * M.U.Y;
		U.Z += ref.I.Z * M.U.Z;

		I.X =  ref.R.X * M.I.X;
		I.X += ref.U.X * M.I.Y;
		I.X += ref.I.X * M.I.Z;

		I.Y =  ref.R.Y * M.I.X;
		I.Y += ref.U.Y * M.I.Y;
		I.Y += ref.I.Y * M.I.Z;

		I.Z =  ref.R.Z * M.I.X;
		I.Z += ref.U.Z * M.I.Y;
		I.Z += ref.I.Z * M.I.Z;
	}
	else
	{
		ret.R.X =  R.X * M.R.X;
		ret.R.X += U.X * M.R.Y;
		ret.R.X += I.X * M.R.Z;

		ret.R.Y =  R.Y * M.R.X;
		ret.R.Y += U.Y * M.R.Y;
		ret.R.Y += I.Y * M.R.Z;

		ret.R.Z =  R.Z * M.R.X;
		ret.R.Z += U.Z * M.R.Y;
		ret.R.Z += I.Z * M.R.Z;

		ret.U.X =  R.X * M.U.X;
		ret.U.X += U.X * M.U.Y;
		ret.U.X += I.X * M.U.Z;

		ret.U.Y =  R.Y * M.U.X;
		ret.U.Y += U.Y * M.U.Y;
		ret.U.Y += I.Y * M.U.Z;

		ret.U.Z =  R.Z * M.U.X;
		ret.U.Z += U.Z * M.U.Y;
		ret.U.Z += I.Z * M.U.Z;

		ret.I.X =  R.X * M.I.X;
		ret.I.X += U.X * M.I.Y;
		ret.I.X += I.X * M.I.Z;

		ret.I.Y =  R.Y * M.I.X;
		ret.I.Y += U.Y * M.I.Y;
		ret.I.Y += I.Y * M.I.Z;

		ret.I.Z =  R.Z * M.I.X;
		ret.I.Z += U.Z * M.I.Y;
		ret.I.Z += I.Z * M.I.Z;
	}

	ret.Translation.X =  R.X * M.Translation.X;
	ret.Translation.X += U.X * M.Translation.Y;
	ret.Translation.X += I.X * M.Translation.Z;
	ret.Translation.X += trans.X;

	ret.Translation.Y =  R.Y * M.Translation.X;
	ret.Translation.Y += U.Y * M.Translation.Y;
	ret.Translation.Y += I.Y * M.Translation.Z;
	ret.Translation.Y += trans.Y;

	ret.Translation.Z =  R.Z * M.Translation.X;
	ret.Translation.Z += U.Z * M.Translation.Y;
	ret.Translation.Z += I.Z * M.Translation.Z;
	ret.Translation.Z += trans.Z;

	ret.m_Identity = deFALSE;
}

deTransform deTransform::Multiply(const deTransformInfo & rhs)
{
	deTransform ret;

	Multiply(rhs, ret);

	return ret;
}







deBoolean deTransformStack::Push(const deTransformInfo & transform, deTransformInfo * concat_output)
{
	deTransformInfo concat;
	// grab the current fully-concatenated stack
	deTransform * pTop = (deTransform*)m_Stack.Peek();

	if (!concat_output)
		concat_output = &concat;

	// concatenate the previous top and the to-be-added transforms
	if (pTop)
		pTop->Multiply(transform, *concat_output);
	else
		*concat_output = transform;

	// push the concatenated transform onto the stack
	if (!m_Stack.Push(*concat_output))
		return deFALSE;

	return deTRUE;
}

deBoolean deTransformStack::Pop(deTransformInfo & transform)
{
	deTransform & ref = (deTransform &)transform;
	return m_Stack.Pop(ref);
}

deBoolean deTransformStack::Pop()
{
	return m_Stack.Pop();
}

deBoolean deTransformStack::Peek(deTransformInfo & transform)
{
	deTransform * ptr = m_Stack.Peek();
	if (!ptr)
		return deFALSE;
	transform = *ptr;
	return deTRUE;
}




deMatrix44::deMatrix44(const deFloat f[4][4], deBoolean d3d)
{
	int i,k;
	if (d3d)
	{
		for (k = 0; k < 4; ++k)
			for (i = 0; i < 4; ++i)
				m[k][i] = f[k][i]; // direct copy
	}
	else
	{
		for (k = 0; k < 4; ++k)
			for (i = 0; i < 4; ++i)
				m[k][i] = f[i][k]; // transpose
	}
}

deMatrix44::deMatrix44(const deDouble d[4][4], deBoolean d3d)
{
	int i,k;
	if (d3d)
	{
		for (k = 0; k < 4; ++k)
			for (i = 0; i < 4; ++i)
				m[k][i] = d[k][i]; // direct copy
	}
	else
	{
		for (k = 0; k < 4; ++k)
			for (i = 0; i < 4; ++i)
				m[k][i] = d[i][k]; // transpose
	}
}

void deMatrix44::Multiply(const deMatrix44 & right,  deMatrix44 & out) const
{
#pragma note(this could use a little SIMD...)
	deMatrix44 temp;
	temp._00 = right._00 * _00 + right._10 * _01 + right._20 * _02 + right._30 * _03;
	temp._01 = right._01 * _00 + right._11 * _01 + right._21 * _02 + right._31 * _03;
	temp._02 = right._02 * _00 + right._12 * _01 + right._22 * _02 + right._32 * _03;
	temp._03 = right._03 * _00 + right._13 * _01 + right._23 * _02 + right._33 * _03;

	temp._10 = right._00 * _10 + right._10 * _11 + right._20 * _12 + right._30 * _13;
	temp._11 = right._01 * _10 + right._11 * _11 + right._21 * _12 + right._31 * _13;
	temp._12 = right._02 * _10 + right._12 * _11 + right._22 * _12 + right._32 * _13;
	temp._13 = right._03 * _10 + right._13 * _11 + right._23 * _12 + right._33 * _13;
	
	temp._20 = right._00 * _20 + right._10 * _21 + right._20 * _22 + right._30 * _23;
	temp._21 = right._01 * _20 + right._11 * _21 + right._21 * _22 + right._31 * _23;
	temp._22 = right._02 * _20 + right._12 * _21 + right._22 * _22 + right._32 * _23;
	temp._23 = right._03 * _20 + right._13 * _21 + right._23 * _22 + right._33 * _23;
	
	temp._30 = right._00 * _30 + right._10 * _31 + right._20 * _32 + right._30 * _33;
	temp._31 = right._01 * _30 + right._11 * _31 + right._21 * _32 + right._31 * _33;
	temp._32 = right._02 * _30 + right._12 * _31 + right._22 * _32 + right._32 * _33;
	temp._33 = right._03 * _30 + right._13 * _31 + right._23 * _32 + right._33 * _33;
	out = temp;
}

void deMatrix44::Transform(const deVec3d & in, deVec3d & out) const
{
	if (&out == &in)
	{
		deVec3d temp;
		temp.x = in.x * _00 + in.y * _10 + in.z * _20 + _30;
		temp.y = in.x * _01 + in.y * _11 + in.z * _21 + _31;
		temp.z = in.x * _02 + in.y * _12 + in.z * _22 + _32;
		out = temp;
	}
	else
	{
		out.x = in.x * _00 + in.y * _10 + in.z * _20 + _30;
		out.y = in.x * _01 + in.y * _11 + in.z * _21 + _31;
		out.z = in.x * _02 + in.y * _12 + in.z * _22 + _32;
	}
}

void deMatrix44::Transform(const deVec3d & in, deVec4d & out) const
{
	if ((void*)&out == (void*)&in)
	{
		deVec4d temp;
		temp.x = in.x * _00 + in.y * _10 + in.z * _20 + _30;
		temp.y = in.x * _01 + in.y * _11 + in.z * _21 + _31;
		temp.z = in.x * _02 + in.y * _12 + in.z * _22 + _32;
		temp.w = in.x * _03 + in.y * _13 + in.z * _23 + _33;
		out = temp;
	}
	else
	{
		out.x = in.x * _00 + in.y * _10 + in.z * _20 + _30;
		out.y = in.x * _01 + in.y * _11 + in.z * _21 + _31;
		out.z = in.x * _02 + in.y * _12 + in.z * _22 + _32;
		out.w = in.x * _03 + in.y * _13 + in.z * _23 + _33;
	}
}

void deMatrix44::Transform(const deVertex & in, deVec4d & out) const
{
	if ((void*)&out == (void*)&in)
	{
		deVec4d temp;
		temp.x = in.x * _00 + in.y * _10 + in.z * _20 + _30;
		temp.y = in.x * _01 + in.y * _11 + in.z * _21 + _31;
		temp.z = in.x * _02 + in.y * _12 + in.z * _22 + _32;
		temp.w = in.x * _03 + in.y * _13 + in.z * _23 + _33;
		out = temp;
	}
	else
	{
		out.x = in.x * _00 + in.y * _10 + in.z * _20 + _30;
		out.y = in.x * _01 + in.y * _11 + in.z * _21 + _31;
		out.z = in.x * _02 + in.y * _12 + in.z * _22 + _32;
		out.w = in.x * _03 + in.y * _13 + in.z * _23 + _33;
	}
}

