#include "munga.h"
#pragma hdrstop

#include "rotation.h"
#include "linmtrx.h"

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Logical
	Close_Enough(
		const Hinge &a1,
		const Hinge &a2,
		Scalar e
	)
{
	return a1.axisNumber == a2.axisNumber
		&& Close_Enough(a1.rotationAmount, a2.rotationAmount, e);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Hinge&
	Hinge::Lerp(
		const Hinge& v1,
		const Hinge& v2,
		Scalar t
	)
{
	Check_Pointer(this);
	Check(&v1);
	Check(&v2);
	Verify(v1.axisNumber == v2.axisNumber);

	axisNumber = v1.axisNumber;
	rotationAmount.Lerp(v1.rotationAmount, v2.rotationAmount, t);
   return *this;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
std::ostream& operator<<(std::ostream& stream, const Hinge& angles)
{
	Check(&angles);
	stream << angles.rotationAmount << " about ";
	switch (angles.axisNumber)
	{
	case X_Axis:
		stream << "X Axis";
		break;
	case Y_Axis:
		stream << "Y Axis";
		break;
	case Z_Axis:
		stream << "Z Axis";
		break;
	}
	return stream;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Logical
	Hinge::TestInstance() const
{
	return (unsigned)axisNumber <= Z_Axis;
}

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

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ EulerAngles ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

const EulerAngles
	EulerAngles::Identity(0.0f,0.0f,0.0f);

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

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

	pitch = angles.pitch;
	yaw = angles.yaw;
	roll = angles.roll;
	return *this;
}

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

	LinearMatrix m;
	m = angles;
	*this = m;

	return *this;
}

//
//#############################################################################
//#############################################################################
//
EulerAngles&
	EulerAngles::operator=(const Hinge &hinge)
{
	Check_Pointer(this);
	Check(&hinge);

	pitch = 0.0f;
	yaw = 0.0f;
	roll = 0.0f;

	switch (hinge.axisNumber)
	{
	case X_Axis:
		pitch = hinge.rotationAmount;
		break;
	case Y_Axis:
		yaw = hinge.rotationAmount;
		break;
	case Z_Axis:
		roll = hinge.rotationAmount;
		break;
	}
	return *this;
}

//
//#############################################################################
//#############################################################################
//
EulerAngles&
	EulerAngles::operator=(const Quaternion &quaternion)
{
	Check_Pointer(this);
	Check(&quaternion);

	LinearMatrix m;
	m = quaternion;
	return *this = m;
}

//
//#############################################################################
//#############################################################################
//
EulerAngles&
	EulerAngles::operator=(const LinearMatrix &matrix)
{
	Check_Pointer(this);
	Check(&matrix);

	SinCosPair
		p,y,r;

	y.sine = -matrix(0,2);
	if (Close_Enough(y.sine,1.0f,0.0001f)) {
		y.cosine = 0.0f;
		r.sine = 0.0f;
		r.cosine = 1.0f;
		p.sine = matrix(1,0);
		p.cosine = matrix(2,0);
	}
	else if (Close_Enough(y.sine,-1.0f,0.0001f)) {
		y.cosine = 0.0f;
		r.sine = 0.0f;
		r.cosine = 1.0f;
		p.sine = -matrix(1,0);
		p.cosine = -matrix(2,0);
	}
	else {
		y.cosine = Sqrt(1.0f - y.sine*y.sine);
		p.sine = matrix(1,2) / y.cosine;
		p.cosine = matrix(2,2) / y.cosine;
		r.sine = matrix(0,1) / y.cosine;
		r.cosine = matrix(0,0) / y.cosine;

		if (
			!Close_Enough(p.sine*y.sine*r.cosine - p.cosine*r.sine,matrix(1,0))
		) {
			y.sine = -y.sine;
			p.sine = -p.sine;
			p.cosine = -p.cosine;
			r.sine = -r.sine;
			r.cosine = -r.cosine;
		}
	}

	pitch = p;
	yaw = y;
	roll = r;
	return *this;
}

//
//#############################################################################
//#############################################################################
//
Logical
	Small_Enough(
		const EulerAngles& angles,
		Scalar e
	)
{
	Check(&angles);

	return
		Small_Enough(angles.pitch,e)
		 && Small_Enough(angles.yaw,e)
		 && Small_Enough(angles.roll,e);
}

//
//#############################################################################
//#############################################################################
//
Logical
	Close_Enough(
		const EulerAngles& a1,
		const EulerAngles& a2,
		Scalar e
	)
{
	Check(&a1);
	Check(&a2);

	return
		Close_Enough(a1.pitch,a2.pitch,e)
		 && Close_Enough(a1.yaw,a2.yaw,e)
		 && Close_Enough(a1.roll,a2.roll,e);
}

//
//#############################################################################
//#############################################################################
//
EulerAngles&
	EulerAngles::Multiply(
		const EulerAngles &q1,
		const EulerAngles &q2
	)
{
	Check_Pointer(this);
	Check(&q1);
	Check(&q2);

	pitch = q1.pitch + q2.pitch;
	yaw = q1.yaw + q2.yaw;
	roll = q1.roll + q2.roll;

	#if DEBUG_LEVEL>0
		if (!TestInstance())
		{
			Dump(q1);
			Dump(q2);
			Dump(*this);
			Fail("EulerAngle multiplication is unstable!\n");
		}
	#endif
	return *this;
}

//
//#############################################################################
//#############################################################################
//
EulerAngles&
	EulerAngles::Multiply(
		const EulerAngles &q,
		Scalar t
	)
{
	Check_Pointer(this);
	Check(&q);

	pitch = q.pitch * t;
	yaw = q.yaw * t;
	roll = q.roll * t;
	return *this;
}

//
//#############################################################################
//#############################################################################
//
EulerAngles&
	EulerAngles::MultiplyScaled(
		const EulerAngles &q1,
		const EulerAngles &q2,
		Scalar t
	)
{
	Check_Pointer(this);
	Check(&q1);
	Check(&q2);
	Verify(t>=0.0f);

	pitch = q1.pitch + q2.pitch * t;
	yaw = q1.yaw + q2.yaw * t;
	roll = q1.roll + q2.roll * t;
	return *this;
}

//
//#############################################################################
//#############################################################################
//
EulerAngles&
	EulerAngles::Lerp(
		const EulerAngles &a1,
		const EulerAngles &a2,
		Scalar t
	)
{
	Check_Pointer(this);
	Check(&a1);
	Check(&a2);

	pitch = ::Lerp(a1.pitch,a2.pitch,t);
	yaw = ::Lerp(a1.yaw,a2.yaw,t);
	roll = ::Lerp(a1.roll,a2.roll,t);
	return *this;
}

//
//#############################################################################
//#############################################################################
//
EulerAngles&
	EulerAngles::Normalize()
{
	Check_Pointer(this);

	pitch.Normalize();
	yaw.Normalize();
	roll.Normalize();
	return *this;
}

//
//#############################################################################
//#############################################################################
//
std::ostream& operator<<(std::ostream& stream, const EulerAngles& angles)
{
	return stream << '<' << angles.pitch << ',' << angles.yaw << ',' << angles.roll << '>';
}

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

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ YawPitchRoll ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

const YawPitchRoll
	YawPitchRoll::Identity(0.0f,0.0f,0.0f);

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

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

	pitch = angles.pitch;
	yaw = angles.yaw;
	roll = angles.roll;
	return *this;
}

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

	LinearMatrix m;
	m = angles;
	*this = m;

	return *this;
}

//
//#############################################################################
//#############################################################################
//
YawPitchRoll&
	YawPitchRoll::operator=(const Hinge &hinge)
{
	Check_Pointer(this);
	Check(&hinge);

	pitch = 0.0f;
	yaw = 0.0f;
	roll = 0.0f;

	switch (hinge.axisNumber)
	{
	case X_Axis:
		pitch = hinge.rotationAmount;
		break;
	case Y_Axis:
		yaw = hinge.rotationAmount;
		break;
	case Z_Axis:
		roll = hinge.rotationAmount;
		break;
	}
	return *this;
}

//
//#############################################################################
//#############################################################################
//
YawPitchRoll&
	YawPitchRoll::operator=(const Quaternion &quaternion)
{
	Check_Pointer(this);
	Check(&quaternion);

	LinearMatrix m;
	m = quaternion;
	return *this = m;
}

//
//#############################################################################
//#############################################################################
//
YawPitchRoll&
	YawPitchRoll::operator=(const LinearMatrix &matrix)
{
	Check_Pointer(this);
	Check(&matrix);

	SinCosPair
		x,y,z;

	x.sine = -matrix(2,1);
	if (Close_Enough(x.sine,1.0f,0.0001f)) {
		x.cosine = 0.0f;
		z.sine = 0.0f;
		z.cosine = 1.0f;
		y.sine = matrix(1,0);
		y.cosine = matrix(0,0);
	}
	else if (Close_Enough(x.sine,-1.0f,0.0001f)) {
		x.cosine = 0.0f;
		z.sine = 0.0f;
		z.cosine = 1.0f;
		y.sine = matrix(0,2);
		y.cosine = matrix(0,0);
	}
	else {
		x.cosine = Sqrt(1.0f - x.sine*x.sine);
		y.sine = matrix(2,0) / x.cosine;
		y.cosine = matrix(2,2) / x.cosine;
		z.sine = matrix(0,1) / x.cosine;
		z.cosine = matrix(1,1) / x.cosine;

		if (
			!Close_Enough(y.cosine*z.cosine + x.sine*y.sine*z.sine,matrix(0,0))
		)
		{
			x.sine = -x.sine;
			y.sine = -y.sine;
			y.cosine = -y.cosine;
			z.sine = -z.sine;
			z.cosine = -z.cosine;
		}
	}

	pitch = x;
	yaw = y;
	roll = z;
	return *this;
}

//
//#############################################################################
//#############################################################################
//
Logical
	Small_Enough(
		const YawPitchRoll& angles,
		Scalar e
	)
{
	Check(&angles);

	return
		Small_Enough(angles.pitch,e)
		 && Small_Enough(angles.yaw,e)
		 && Small_Enough(angles.roll,e);
}

//
//#############################################################################
//#############################################################################
//
Logical
	Close_Enough(
		const YawPitchRoll& a1,
		const YawPitchRoll& a2,
		Scalar e
	)
{
	Check(&a1);
	Check(&a2);

	return
		Close_Enough(a1.pitch,a2.pitch,e)
		 && Close_Enough(a1.yaw,a2.yaw,e)
		 && Close_Enough(a1.roll,a2.roll,e);
}

//
//#############################################################################
//#############################################################################
//
YawPitchRoll&
	YawPitchRoll::Normalize()
{
	Check_Pointer(this);

	pitch.Normalize();
	yaw.Normalize();
	roll.Normalize();
	return *this;
}

//
//#############################################################################
//#############################################################################
//
std::ostream& operator<<(std::ostream& stream, const YawPitchRoll& angles)
{
	return stream << '<' << angles.yaw << ',' << angles.pitch << ',' << angles.roll << '>';
}

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

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Quaternion ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

const Quaternion
	Quaternion::Identity(0.0f, 0.0f, 0.0f, 1.0f);

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

//
//#############################################################################
//#############################################################################
//
Quaternion::Quaternion(
	Scalar x,
	Scalar y,
	Scalar z,
	Scalar w
)
{
	this->x = x;
	this->y = y;
	this->z = z;
	this->w = w;
}

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

	x = q.x;
	y = q.y;
	z = q.z;
	w = q.w;
	return *this;
}

//
//#############################################################################
//#############################################################################
//
Quaternion&
	Quaternion::operator=(const Hinge &hinge)
{
	Check_Pointer(this);
	Check(&hinge);

	Radian temp;
	temp = hinge.rotationAmount * 0.5f;
	SinCosPair half_angle;
	half_angle = temp;

	w = half_angle.cosine;
	x = 0.0f;
	y = 0.0f;
	z = 0.0f;

	switch (hinge.axisNumber)
	{
	case X_Axis:
		x = half_angle.sine;
		break;
	case Y_Axis:
		y = half_angle.sine;
		break;
	case Z_Axis:
		z = half_angle.sine;
		break;
	}
	return *this;
}

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

	LinearMatrix m;
	m = angles;
	Check(&m);
	*this = m;
	#if DEBUG_LEVEL>0
		if (!TestInstance())
		{
			Dump(angles);
			Dump(*this);
			Fail("Quaternion construction from angles is unstable!\n");
		}
	#endif
	return *this;
}
//
//#############################################################################
//#############################################################################
//
Quaternion&
	Quaternion::operator=(const YawPitchRoll &angles)
{
	LinearMatrix	lin_matrix;
	lin_matrix = angles;
	*this = lin_matrix;
   return  *this;
}
//
//#############################################################################
//#############################################################################
//
Quaternion&
	Quaternion::operator=(const LinearMatrix &matrix)
{
	Check_Pointer(this);
	Check(&matrix);

	//
	//------------------------------------------------------------------------
	// Compute the w component.  If it is close enough to zero, then we have a
	// 180 degree pivot, so figure out the correct axis to rotate around
	//------------------------------------------------------------------------
	//
	w = (1.0f + matrix(0,0) + matrix(1,1) + matrix(2,2)) * 0.25f;
	if (Small_Enough(w,1e-2))
	{
		Verify(w >= -SMALL);
		if (w<0.0f)
		{
			w = 0.0f;
		}

		//
		//----------------------------------------------------------------
		// Figure out the length of each component of the axis of rotation
		//----------------------------------------------------------------
		//
		Scalar temp = (1.0f + matrix(0,0)) * 0.5f - w;
		Min_Clamp(temp, 0.0f);
		x = Sqrt(temp);
		temp = (1.0f + matrix(1,1)) * 0.5f - w;
		Min_Clamp(temp, 0.0f);
		y = Sqrt(temp);
		temp = (1.0f + matrix(2,2)) * 0.5f - w;
		Min_Clamp(temp, 0.0f);
		z = Sqrt(temp);
		w = Sqrt(w);

		//
		//-------------------------------------------
		// Now figure out the signs of the components
		//-------------------------------------------
		//
		if (matrix(0,1) < matrix(1,0))
		{
			z = -z;
		}
		if (matrix(2,0) < matrix(0,2))
		{
			y = -y;
		}
		if (matrix(1,2) < matrix(2,1))
		{
			x = -x;
		}
	}

	//
	//----------------------------------------------------------
	// Otherwise, determine x, y, and z directly from the matrix
	//----------------------------------------------------------
	//
	else
	{
		Verify(w>0.0f);
		w = Sqrt(w);
		x = (matrix(1,2) - matrix(2,1)) * 0.25f / w;
		y = (matrix(2,0) - matrix(0,2)) * 0.25f / w;
		z = (matrix(0,1) - matrix(1,0)) * 0.25f / w;
	}

	#if DEBUG_LEVEL>0
		if (!TestInstance())
		{
			Dump(matrix);
			Dump(*this);
			Fail("Quaternion construction from matrix is unstable!\n");
		}
	#endif
	return *this;
}


//
//#############################################################################
//#############################################################################
//
Scalar
	Quaternion::GetAngle()
{
	Check(this);

	Scalar sine_of_half = Sqrt(x*x + y*y + z*z);
	if (Small_Enough(sine_of_half))
	{
		return 0.0f;
	}

	SinCosPair half_angle(sine_of_half, w);
	Radian angle;
	angle = half_angle;

	return angle * 2.0f;
}

//
//#############################################################################
//#############################################################################
//
void
	Quaternion::GetAxis(UnitVector *axis)
{
	Check(this);
	Check_Pointer(axis);

	Scalar len = Sqrt(x*x + y*y + z*z);
	if (Small_Enough(len))
	{
		axis->x = 1.0f;
		axis->y = 0.0f;
		axis->z = 0.0f;
	}
	else
	{
		axis->x = x / len;
		axis->y = y / len;
		axis->z = z / len;
	}

	Check(axis);
	return;
}

//
//#############################################################################
//#############################################################################
//
Quaternion&
	Quaternion::Multiply(const Quaternion &q1, const Quaternion &q2)
{
	Check_Pointer(this);
	Check(&q1);
	Check(&q2);
	Verify(this != &q1 && this != &q2);

	x = q1.w*q2.x + q2.w*q1.x + q1.y*q2.z - q1.z*q2.y;
	y = q1.w*q2.y + q2.w*q1.y + q1.z*q2.x - q1.x*q2.z;
	z = q1.w*q2.z + q2.w*q1.z + q1.x*q2.y - q1.y*q2.x;
	w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z;
	#if DEBUG_LEVEL>0
		if (!TestInstance())
		{
			Dump(q1);
			Dump(q2);
			Dump(*this);
			Fail("Quaternion multiplication is unstable!\n");
		}
	#endif
	return *this;
}

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

	//
	//---------------------------------------------------------
	// Figure out the half the angle of rotation and scale that
	//---------------------------------------------------------
	//
	Scalar sine_of_half = Sqrt(q.x*q.x + q.y*q.y + q.z*q.z);
	if (Small_Enough(sine_of_half))
	{
		*this = Identity;
		return *this;
	}

	SinCosPair half_angle(sine_of_half, q.w);
	Radian angle;
	angle = half_angle;
	angle *= t;
	half_angle = angle;

	//
	//-----------------------------------------------------------------
	// Build the scaled quaternion out of the components of the old one
	//-----------------------------------------------------------------
	//
	w = half_angle.cosine;
	sine_of_half = half_angle.sine / sine_of_half;
	x = q.x * sine_of_half;
	y = q.y * sine_of_half;
	z = q.z * sine_of_half;

	#if DEBUG_LEVEL>0
		if (!TestInstance())
		{
			Dump(q);
			Dump(t);
			Dump(*this);
			Fail("Quaternion multiplication is unstable!\n");
		}
	#endif
	return *this;
}

//
//#############################################################################
//#############################################################################
//
Quaternion&
	Quaternion::MultiplyScaled(
		const Quaternion &q1,
		const Quaternion &q2,
		Scalar t
	)
{
	Check_Pointer(this);
	Verify(this != &q1);
	Check(&q1);
	Check(&q2);
	Verify(t>=0.0f);

	Quaternion scaled_quat;
	scaled_quat.Multiply(q2, t);
	Multiply(q1, scaled_quat);

	#if DEBUG_LEVEL>0
		if (!TestInstance())
		{
			Dump(q1);
			Dump(q2);
			Dump(t);
			Dump(*this);
			Fail("Quaternion multiplication is unstable!\n");
		}
	#endif
   return *this;
}

//
//#############################################################################
//#############################################################################
//
Quaternion&
	Quaternion::Add(
		const Quaternion &source,
		const Vector3D &delta
	)
{
	Check_Pointer(this);
	Check(&source);
	Check(&delta);
	Verify(&source != this);

	//
	//---------------------------------------------------------------
	// See if there is any rotation to apply to the source quaternion
	//---------------------------------------------------------------
	//
	Scalar rotation = delta.Length();
	if (Small_Enough(rotation))
	{
		return *this = source;
	}

	//
	//---------------------------------------------------------------------
	// Build a quaternion from the delta vector, treating the length as the
	// amount of rotation and the direction of the vector as the axis of
	// rotation
	//---------------------------------------------------------------------
	//
	SinCosPair half_angle;
	half_angle = 0.5f * Radian::Normalize(rotation);
	rotation = half_angle.sine / rotation;
	Quaternion q(
		delta.x * rotation,
		delta.y * rotation,
		delta.z * rotation,
		half_angle.cosine
	);
	Check(&q);
	return Multiply(source, q);
}

//
//#############################################################################
//#############################################################################
//
Quaternion&
	Quaternion::AddScaled(
		const Quaternion &source,
		const Vector3D &delta,
		Scalar t
	)
{
	Check_Pointer(this);
	Check(&source);
	Check(&delta);
	Verify(&source != this);

	//
	//---------------------------------------------------------------
	// See if there is any rotation to apply to the source quaternion
	//---------------------------------------------------------------
	//
	Scalar rotation = delta.Length();
	if (Small_Enough(rotation))
	{
		return *this = source;
	}

	//
	//---------------------------------------------------------------------
	// Build a quaternion from the delta vector, treating the length as the
	// amount of rotation and the direction of the vector as the axis of
	// rotation
	//---------------------------------------------------------------------
	//
	SinCosPair half_angle;
	rotation *= t;
	half_angle = 0.5f*rotation;
	rotation = half_angle.sine / rotation;
	Quaternion q(
		delta.x * rotation,
		delta.y * rotation,
		delta.z * rotation,
		half_angle.cosine
	);
	Check(&q);
	return Multiply(source, q);
}

//
//#############################################################################
//#############################################################################
//
Quaternion&
	Quaternion::Normalize()
{
	Scalar t = x*x + y*y + z*z;
	if (t <= 1.0f)
	{
		t = Sqrt(1.0f - t);
		if (w<0.0f)
		{
			x = -x;
			y = -y;
			z = -z;
		}
		w = t;
	}
	else
	{
		t = Sqrt(t);
		x /= t;
		y /= t;
		z /= t;
		w = 0.0f;
	}
	return *this;
}

//
//#############################################################################
//#############################################################################
//
Quaternion&
	Quaternion::Subtract(
		const Quaternion &end,
		const Quaternion &start
	)
{
	Check_Pointer(this);
	Check(&start);
	Check(&end);

	Quaternion inverse(start);
	inverse.w = -inverse.w;
	return Multiply(inverse, end);
}

//
//#############################################################################
//#############################################################################
//
Quaternion&
	Quaternion::Subtract(
		const UnitVector &end,
		const UnitVector &start
	)
{
	Check_Pointer(this);
	Check(&start);
	Check(&end);

	Vector3D
		axis;
	SinCosPair
		delta;
	delta.cosine = start*end;

	//
	//----------------------------------------------------------------------
	// See if the vectors point in the same direction.  If so, return a null
	// rotation
	//----------------------------------------------------------------------
	//
	if (Close_Enough(delta.cosine, 1.0f))
	{
		x = 0.0f;
		y = 0.0f;
		z = 0.0f;
		w = 1.0f;
	}

	//
	//-------------------------------------------------------------------------
	// See if the vectors directly oppose each other.  If so, pick the smallest
	// axis coordinate and generate a vector along it.  Project this onto the
	// base vector and subtract it out, leaving a perpendicular projection.
	// Extend that out to unit length, then set the angle to PI
	//-------------------------------------------------------------------------
	//
	else if (Close_Enough(delta.cosine, -1.0f))
	{
		//
		//---------------------------
		// Pick out the smallest axis
		//---------------------------
		//
		int
			smallest=0;
		Scalar
			value=2.0f;
		for (int i=X_Axis; i<=Z_Axis; ++i)
		{
			if (Abs(start[i]) < value)
			{
				smallest = i;
				value = Abs(start[i]);
			}
		}

		//
		//----------------------------------------
		// Set up a vector along the selected axis
		//----------------------------------------
		//
		axis.x = 0.0f;
		axis.y = 0.0f;
		axis.z = 0.0f;
		axis[smallest] = 1.0f;

		//
		//-------------------------------------------------------------------
		// If the value on that axis wasn't zero, subtract out the projection
		//-------------------------------------------------------------------
		//
		if (!Small_Enough(value))
		{
			Vector3D t;
			t.Multiply(start, start*axis);
			axis.Subtract(axis, t);
			axis.Normalize(axis);
		}

		//
		//----------------------
		// Convert to quaternion
		//----------------------
		//
		x = axis.x;
		y = axis.y;
		z = axis.z;
		w = 0.0f;
	}

	//
	//--------------------------------------------------
	// Otherwise, generate the cross product and unitize
	//--------------------------------------------------
	//
	else
	{
		axis.Cross(start, end);
		delta.sine = axis.Length();
		axis /= delta.sine;

		//
		//---------------------------------------------------------------
		// Now compute sine and cosine of half the angle and generate the
		// quaternion
		//---------------------------------------------------------------
		//
		delta.sine = Sqrt((1.0f - delta.cosine)*0.5f);
		x = axis.x * delta.sine;
		y = axis.y * delta.sine;
		z = axis.z * delta.sine;
		w = Sqrt((1.0f + delta.cosine)*0.5f);
	}
	return *this;
}

//
//#############################################################################
//#############################################################################
//
Quaternion&
	Quaternion::Subtract(
		const Vector3D &end,
		const Vector3D &start
	)
{
	Check_Pointer(this);
	Check(&start);
	Check(&end);

	UnitVector
		s,e;

	s = start;
	e = end;
	return Subtract(e, s);
}

//
//#############################################################################
//#############################################################################
//
Quaternion&
	Quaternion::Lerp(
		const Quaternion &q1,
		const Quaternion &q2,
		Scalar t
	)
{
	Check_Pointer(this);
	Check(&q1);
	Check(&q2);

	if (q1.x*q2.x + q1.y*q2.y + q1.z*q2.z + q1.w*q2.w >= 0.0f)
	{
		x = ::Lerp(q1.x, q2.x, t);
		y = ::Lerp(q1.y, q2.y, t);
		z = ::Lerp(q1.z, q2.z, t);
		w = ::Lerp(q1.w, q2.w, t);
	}
	else
	{
		x = ::Lerp(q1.x, -q2.x, t);
		y = ::Lerp(q1.y, -q2.y, t);
		z = ::Lerp(q1.z, -q2.z, t);
		w = ::Lerp(q1.w, -q2.w, t);
	}

	Scalar len = x*x + y*y + z*z + w*w;
	if (Small_Enough(len))
	{
		x = 0.0f;
		y = 0.0f;
		z = 0.0f;
		w = 1.0f;
	}
	else
	{
		len = 1.0f / Sqrt(len);
		x *= len;
		y *= len;
		z *= len;
		w *= len;
	}
	Check(this);
	return *this;
}

//
//#############################################################################
//#############################################################################
//
std::ostream& operator<<(std::ostream& stream, const Quaternion& q)
{
	return stream << '<' << q.x << ',' << q.y << ',' << q.z << ',' << q.w
	 << '>';
}

//
//#############################################################################
//#############################################################################
//
Logical Quaternion::TestInstance() const
{
#if 0
	if (!Close_Enough(x*x + y*y + z*z + w*w,1.0f,2e-6))
	{
		Scalar t = 1.0f - x*x - y*y - z*z - w*w;

		if (Small_Enough(t, 2e-6))
		{
			Dump(*this);
			Dump(1.0f - x*x - y*y - z*z - w*w);
			return False;
		}
	}
#endif
	return True;
}

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