//===========================================================================//
// File:	linmtrx.cc                                                          //
// Project: MUNGA           Brick: Math Library                              //
// Contents: Implementation details for the linear matrices                  //
//---------------------------------------------------------------------------//
//   Date    Who  Modification                                               //
// --------  ---  ---------------------------------------------------------- //
// 11/20/94  JMA  Initial coding.                                            //
//---------------------------------------------------------------------------//
// Copyright (C) 1994-1995, Virtual World Entertainment, Inc.                //
// All Rights reserved worldwide                                             //
// This unpublished sourcecode is PROPRIETARY and CONFIDENTIAL               //
//===========================================================================//

#include "StuffHeaders.hpp"

const LinearMatrix4D
	LinearMatrix4D::Identity(true);

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	LinearMatrix4D::AlignLocalAxisToWorldVector(
		const Vector3D &world_target,
		int pointing_axis,
		int rotating_axis,
		int minor_axis
	)
{
	Check_Object(this);
	Check_Object(&world_target);
	Verify(static_cast<unsigned>(pointing_axis) <= Z_Axis);
	Verify(static_cast<unsigned>(rotating_axis) <= Z_Axis);
	Verify(rotating_axis != pointing_axis);

	//
	//------------------------------------------------------------------
	// These are the variables that the alignment algorithm must fill in
	//------------------------------------------------------------------
	//
	UnitVector3D
		rotation_vector,
		pointing_vector,
		minor_vector;

	//
	//------------------------------------------------------------------
	// Extract the current target axis direction, then cross it with the
	// plane target to find the minor axis direction (unsigned)
	//------------------------------------------------------------------
	//
	if (Small_Enough(world_target.GetLengthSquared()))
		return;
	rotation_vector.x = (*this)(rotating_axis, X_Axis);
	rotation_vector.y = (*this)(rotating_axis, Y_Axis);
	rotation_vector.z = (*this)(rotating_axis, Z_Axis);
	Check_Object(&rotation_vector);
	Vector3D temp;
	temp.Cross(rotation_vector, world_target);

	//
	//----------------------------------------------------------------------
	// First check to see if we are rotating around a frozen axis.  If so,
	// if the axes specified are in the right-handed configuration, simply
	// generate the new pointing axis values, otherwise negate the minor
	// axis and generate the pointing vector appropriately
	//----------------------------------------------------------------------
	//
	if (minor_axis == -1)
	{
		minor_axis = 3 - pointing_axis - rotating_axis;
		if (Small_Enough(temp.GetLengthSquared()))
		{
			if (world_target*rotation_vector > 0.0f)
			{
				pointing_vector.x = (*this)(rotating_axis, X_Axis);
				pointing_vector.y = (*this)(rotating_axis, Y_Axis);
				pointing_vector.z = (*this)(rotating_axis, Z_Axis);
				rotation_vector.x = -(*this)(pointing_axis, X_Axis);
				rotation_vector.y = -(*this)(pointing_axis, Y_Axis);
				rotation_vector.z = -(*this)(pointing_axis, Z_Axis);
			}
			else
			{
				pointing_vector.x = -(*this)(rotating_axis, X_Axis);
				pointing_vector.y = -(*this)(rotating_axis, Y_Axis);
				pointing_vector.z = -(*this)(rotating_axis, Z_Axis);
				rotation_vector.x = (*this)(pointing_axis, X_Axis);
				rotation_vector.y = (*this)(pointing_axis, Y_Axis);
				rotation_vector.z = (*this)(pointing_axis, Z_Axis);
			}
			minor_vector.x = (*this)(minor_axis, X_Axis);
			minor_vector.y = (*this)(minor_axis, Y_Axis);
			minor_vector.z = (*this)(minor_axis, Z_Axis);
		}

		else
		{
			minor_vector.Normalize(temp);
			if ((rotating_axis+1)%3 == pointing_axis)
				pointing_vector.Vector3D::Cross(minor_vector, rotation_vector);
			else
			{
				minor_vector.Vector3D::Negate(minor_vector);
				pointing_vector.Vector3D::Cross(rotation_vector, minor_vector);
			}
		}
		Check_Object(&pointing_vector);
	}

	//
	//------------------------------------------------------------------------
	// The next case to check is non-frozen rotation.  In this case, maximum
	// effort is taken to preserve the rotating matrix, but it will be rotated
	// around the minor axis so that the pointing axis is exactly aligned with
	// the target vector
	//------------------------------------------------------------------------
	//
	else
	{

		//
		//--------------------------------------------------------------------
		// If the resultant vector is zero, it means the rotating axis is
		// parallel to the target vector, and thus a correct orthogonal set of
		// axis vectors can already be found in the matrix
		//--------------------------------------------------------------------
		//
		Verify(minor_axis == 3 - pointing_axis - rotating_axis);
		if (Small_Enough(temp.GetLengthSquared()))
		{
			if (world_target*rotation_vector > 0.0f)
			{
				pointing_vector.x = (*this)(rotating_axis, X_Axis);
				pointing_vector.y = (*this)(rotating_axis, Y_Axis);
				pointing_vector.z = (*this)(rotating_axis, Z_Axis);
				rotation_vector.x = -(*this)(pointing_axis, X_Axis);
				rotation_vector.y = -(*this)(pointing_axis, Y_Axis);
				rotation_vector.z = -(*this)(pointing_axis, Z_Axis);
			}
			else
			{
				pointing_vector.x = -(*this)(rotating_axis, X_Axis);
				pointing_vector.y = -(*this)(rotating_axis, Y_Axis);
				pointing_vector.z = -(*this)(rotating_axis, Z_Axis);
				rotation_vector.x = (*this)(pointing_axis, X_Axis);
				rotation_vector.y = (*this)(pointing_axis, Y_Axis);
				rotation_vector.z = (*this)(pointing_axis, Z_Axis);
			}
			minor_vector.x = (*this)(minor_axis, X_Axis);
			minor_vector.y = (*this)(minor_axis, Y_Axis);
			minor_vector.z = (*this)(minor_axis, Z_Axis);
		}

		//
		//---------------------------------------------------------------------
		// We have a non-trivial minor vector, so use it to generate the real
		// minor axis, then calculate the new rotation axis.  If the axes
		// specified are in the right-handed configuration, simply generate the
		// new pointing axis values, otherwise negate the minor axis and
		// generate the pointing vector appropriately
		//---------------------------------------------------------------------
		//
		else
		{
			pointing_vector.Normalize(world_target);
			minor_vector.Normalize(temp);
			if ((rotating_axis+1)%3 == pointing_axis)
				rotation_vector.Vector3D::Cross(pointing_vector, minor_vector);
			else
			{
				minor_vector.Vector3D::Negate(minor_vector);
				rotation_vector.Vector3D::Cross(minor_vector, pointing_vector);
			}
			Check_Object(&rotation_vector);
		}
	}

	//
	//------------------------------------------------
	// Now stuff the unit vectors back into the matrix
	//------------------------------------------------
	//
	Check_Object(&pointing_vector);
	(*this)(pointing_axis, X_Axis) = pointing_vector.x;
	(*this)(pointing_axis, Y_Axis) = pointing_vector.y;
	(*this)(pointing_axis, Z_Axis) = pointing_vector.z;

	Check_Object(&rotation_vector);
	(*this)(rotating_axis, X_Axis) = rotation_vector.x;
	(*this)(rotating_axis, Y_Axis) = rotation_vector.y;
	(*this)(rotating_axis, Z_Axis) = rotation_vector.z;

	Check_Object(&minor_vector);
	(*this)(minor_axis, X_Axis) = minor_vector.x;
	(*this)(minor_axis, Y_Axis) = minor_vector.y;
	(*this)(minor_axis, Z_Axis) = minor_vector.z;

	Check_Object(this);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	LinearMatrix4D::ComputeAxes(ReadOnlyArrayOf<Point3D> &points)
{
	Check_Object(&points);

	//
	//-----------------------------
	// Compute the covariant matrix
	//-----------------------------
	//
	unsigned count = points.GetLength();
	Verify(count > 1);
	double x = 0.0;
	double y = 0.0;
	double z = 0.0;
	double xx = 0.0;
	double xy = 0.0;
	double xz = 0.0;
	double yy = 0.0;
	double yz = 0.0;
	double zz = 0.0;
	unsigned i;
	for (i=0; i<count; i++) 
	{
		x += points[i].x;
		y += points[i].y;
		z += points[i].z;
		xx += points[i].x*points[i].x;
		xy += points[i].x*points[i].y;
		xz += points[i].x*points[i].z;
		yy += points[i].y*points[i].y;
		yz += points[i].y*points[i].z;
		zz += points[i].z*points[i].z;
	}
	double t = 1.0 / count;
	xx -= t*x*x;
	xy -= t*x*y;
	xz -= t*x*z;
	yy -= t*y*y;
	yz -= t*y*z;
	zz -= t*z*z;

	//
	//-----------------------------------------------------------
	// Set the center of the matrix to the centroid of the points
	//-----------------------------------------------------------
	//
	(*this)(3,0) = static_cast<Scalar>(x * t);
	(*this)(3,1) = static_cast<Scalar>(y * t);
	(*this)(3,2) = static_cast<Scalar>(z * t);

	//
	//--------------------------------
	// Figure out the two biggest axes
	//--------------------------------
	//
	double coefs[3][3];
	coefs[0][0] = xx;
	coefs[0][1] = xy;
	coefs[0][2] = xz;
	coefs[1][0] = xy;
	coefs[1][1] = yy;
	coefs[1][2] = yz;
	coefs[2][0] = xz;
	coefs[2][1] = yz;
	coefs[2][2] = zz;
	int major_axis;
	int minor_axis;
	if (xx > yy)
	{
		if (xx > zz)
		{
			major_axis = X_Axis;
			minor_axis = (yy > zz) ? Y_Axis : Z_Axis;
		}
		else
		{
			major_axis = Z_Axis;
			minor_axis = X_Axis;
		}
	}
	else
	{
		if (yy > zz)
		{
			major_axis = Y_Axis;
			minor_axis = (xx > zz) ? X_Axis : Z_Axis;
		}
		else
		{
			major_axis = Z_Axis;
			minor_axis = Y_Axis;
		}
	}

	//
	//-------------------------
	// Compute the first vector
	//-------------------------
	//
	int other_axis = 3 - major_axis - minor_axis;
	Vector3D temp;
	temp[major_axis] = 1.0f;
	temp[minor_axis] = static_cast<Scalar>(coefs[major_axis][minor_axis] / coefs[major_axis][major_axis]);
	temp[other_axis] = static_cast<Scalar>(coefs[major_axis][other_axis] / coefs[major_axis][major_axis]);

	//
	//----------------------------------------------------------------------------
	// Compute the second vector by applying the Gram-Schmidt process to the first
	//----------------------------------------------------------------------------
	//
	Vector3D temp2;
	temp2[major_axis] = static_cast<Scalar>(coefs[minor_axis][major_axis] / coefs[minor_axis][minor_axis]);
	temp2[minor_axis] = 1.0f;
	temp2[other_axis] = static_cast<Scalar>(coefs[minor_axis][other_axis] / coefs[minor_axis][minor_axis]);
	temp2.AddScaled(temp2, temp, -(temp*temp2)/(temp*temp));

	//
	//---------------------------------------------------------------------------
	// Now, normalize the two vector and we should end up with orthogonal vectors
	//---------------------------------------------------------------------------
	//
	UnitVector3D major_direction(temp);
	UnitVector3D minor_direction(temp2);
	Verify(Small_Enough(major_direction*minor_direction));
	temp.Cross(major_direction, minor_direction);
	(*this)(0,0) = major_direction.x;
	(*this)(0,1) = major_direction.y;
	(*this)(0,2) = major_direction.z;
	(*this)(1,0) = minor_direction.x;
	(*this)(1,1) = minor_direction.y;
	(*this)(1,2) = minor_direction.z;
	(*this)(2,0) = temp.x;
	(*this)(2,1) = temp.y;
	(*this)(2,2) = temp.z;
	Check_Object(this);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
LinearMatrix4D&
	LinearMatrix4D::Invert(const LinearMatrix4D& m)
{
	Check_Pointer(this);
	Check_Object(&m);
	Verify(this != &m);

	//
	//-----------------------------------------
	// First, transpose the 3x3 rotation matrix
	//-----------------------------------------
	//
	(*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);

	//
	//----------------------------
	// Now run the offsets through
	//----------------------------
	//
	(*this)(3,0) = -m(3,0)*m(0,0) - m(3,1)*m(0,1) - m(3,2)*m(0,2);
	(*this)(3,1) = -m(3,0)*m(1,0) - m(3,1)*m(1,1) - m(3,2)*m(1,2);
	(*this)(3,2) = -m(3,0)*m(2,0) - m(3,1)*m(2,1) - m(3,2)*m(2,2);
	return *this;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
LinearMatrix4D&
	LinearMatrix4D::Normalize()
{
	Check_Pointer(this);

	#if defined(LEFT_HANDED_COORDINATES)
		#error Right handed coordinate dependancy!
	#endif
	(*this)(0,2) = (*this)(1,0)*(*this)(2,1) - (*this)(1,1)*(*this)(2,0);
	(*this)(1,2) = (*this)(2,0)*(*this)(0,1) - (*this)(2,1)*(*this)(0,0);
	(*this)(2,2) = (*this)(0,0)*(*this)(1,1) - (*this)(0,1)*(*this)(1,0);
	return *this;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	LinearMatrix4D::TestInstance() const
{
	UnitVector3D v1;
	v1.x = (*this)(0,0);
	v1.y = (*this)(0,1);
	v1.z = (*this)(0,2);
	Check_Object(&v1);

	UnitVector3D v2;
	v2.x = (*this)(1,0);
	v2.y = (*this)(1,1);
	v2.z = (*this)(1,2);
	Check_Object(&v2);

	UnitVector3D v3;
	v3.Vector3D::Cross(v1,v2);
	#if defined(LEFT_HANDED_COORDINATES)
		#error Right handed coordinate depenancy!
	#endif


	Verify(Close_Enough(v3.x, (*this)(2,0)));
	Verify(Close_Enough(v3.y, (*this)(2,1)));
	Verify(Close_Enough(v3.z, (*this)(2,2)));


}