RP412/MUNGA/ROTATION.h

#pragma once

#include "angle.h"

class Quaternion;
class Origin;
class YawPitchRoll;
class UnitVector;
class LinearMatrix;
class Vector3D;

//##########################################################################
//############################    Hinge    #################################
//##########################################################################

class Hinge 
{
public:
	int axisNumber;
	Radian rotationAmount;

	//
	// Constructors
	//
	Hinge()	{}
	Hinge(int axis, const Radian &angle) : axisNumber(axis), rotationAmount(angle)	{}
	#if defined(USE_SIGNATURE)
		friend int Is_Signature_Bad(const volatile Hinge *);
	#endif
	//
	// Assignment operators
	//
	Hinge& operator=(const Hinge &hinge)
	{
		Check_Pointer(this);
		Check(&hinge);
		axisNumber = hinge.axisNumber;
		rotationAmount = hinge.rotationAmount;
		return *this;
	}

	//
	// "Close-enough" comparators
	//
	friend Logical Small_Enough(const Hinge &a, Scalar e = SMALL)
	{
		Check(&a);
		return Small_Enough(a, e);
	}
	Logical operator!() const	{ return Small_Enough(*this,SMALL); }

	friend Logical Close_Enough(const Hinge &a1, const Hinge &a2, Scalar e = SMALL);
	Logical operator==(const Hinge& a) const	{ return Close_Enough(*this, a, SMALL); }
	Logical operator!=(const Hinge& a) const	{ return !Close_Enough(*this, a, SMALL); }

	//
	// Template support
	//
	Hinge& Lerp(const Hinge& v1, const Hinge& v2, Scalar t);

	//
	// Support functions
	//
	friend std::ostream& operator<<(std::ostream& stream, const Hinge& angles);
	Logical TestInstance() const;
};

//##########################################################################
//#########################    EulerAngles     #############################
//##########################################################################

class EulerAngles
{
public:
	Radian pitch, yaw, roll;

	static const EulerAngles Identity;

	#if defined(USE_SIGNATURE)
		friend int Is_Signature_Bad(const volatile EulerAngles *);
	#endif

	//
	// Constructors
	//
	EulerAngles() {}
	EulerAngles(const Radian &pitch, const Radian &yaw, const Radian &roll)
	{
		this->pitch = pitch;
		this->yaw = yaw;
		this->roll = roll;
	}

	//
	// Assignment operators
	//
	EulerAngles& operator=(const EulerAngles &angles);
	EulerAngles& operator=(const YawPitchRoll &angles);
	EulerAngles& operator=(const Hinge &hinge);
	EulerAngles& operator=(const Quaternion &quaternion);
	EulerAngles& operator=(const LinearMatrix &matrix);
	EulerAngles& operator=(const Origin &p);

	//
	// "Close-enough" comparators
	//
	friend Logical Small_Enough(const EulerAngles &a, Scalar e = SMALL);
	Logical operator!() const	{ return Small_Enough(*this); }

	friend Logical Close_Enough(const EulerAngles &a1, const EulerAngles &a2, Scalar e = SMALL);
	Logical operator==(const EulerAngles& a) const	{ return Close_Enough(*this, a, SMALL); }
	Logical	operator!=(const EulerAngles& a) const	{ return !Close_Enough(*this, a, SMALL); }

	//
	// Axis index operators
	//
	const Radian& operator[](size_t index) const
	{
		Check_Pointer(this);
		Warn(index>Z_Axis);
		return (&pitch)[index];
	}
	Radian& operator[](size_t index)
	{
		Check_Pointer(this);
		Warn(index>Z_Axis);
		return (&pitch)[index];
	}

	//
	// Multiplication operators
	//
	EulerAngles& Multiply(const EulerAngles &q1, const EulerAngles &q2);
	EulerAngles& Multiply(const EulerAngles &q, Scalar scale);
	EulerAngles& MultiplyScaled(const EulerAngles &q1, const EulerAngles &q2, Scalar t);

	//
	// Template support
	//
	EulerAngles& Lerp(const EulerAngles& v1, const EulerAngles& v2, Scalar t);

	//
	// Support functions
	//
	EulerAngles& Normalize();
	friend std::ostream& operator<<(std::ostream& stream, const EulerAngles& angles);

	//
	// Test functions
	//
	Logical TestInstance() const;
	static Logical TestClass();
};

//##########################################################################
//#########################    YawPitchRoll     ############################
//##########################################################################

class YawPitchRoll
{
public:
	Radian yaw, pitch, roll;

	static const YawPitchRoll Identity;

	#if defined(USE_SIGNATURE)
		friend int Is_Signature_Bad(const volatile YawPitchRoll *);
	#endif

	//
	// Constructors
	//
	YawPitchRoll()	{}
	YawPitchRoll(const Radian &yaw, const Radian &pitch, const Radian &roll)
	{
		this->pitch = pitch;
		this->yaw = yaw;
		this->roll = roll;
	}

	//
	// Assignment operators
	//
	YawPitchRoll& operator=(const Hinge &hinge);
	YawPitchRoll& operator=(const YawPitchRoll &angles);
	YawPitchRoll& operator=(const EulerAngles &angles);
	YawPitchRoll& operator=(const Quaternion &quaternion);
	YawPitchRoll& operator=(const LinearMatrix &matrix);
	YawPitchRoll& operator=(const Origin &p);

	//
	// "Close-enough" comparators
	//
	friend Logical Small_Enough(const YawPitchRoll &a, Scalar e = SMALL);
	Logical operator!() const	{ return Small_Enough(*this); }

	friend Logical Close_Enough(const YawPitchRoll &a1, const YawPitchRoll &a2, Scalar e = SMALL);
	Logical operator==(const YawPitchRoll& a) const	{ return Close_Enough(*this, a); }
	Logical operator!=(const YawPitchRoll& a) const	{ return !Close_Enough(*this, a); }

	//
	// Support functions
	//
	YawPitchRoll& Normalize();
	friend std::ostream& operator<<(std::ostream& stream, const YawPitchRoll& angles);

	//
	// Test functions
	//
	Logical TestInstance() const;
};

//##########################################################################
//#########################    Quaternion    ###############################
//##########################################################################

class Quaternion
{
public:
	static const Quaternion Identity;

	#if defined(USE_SIGNATURE)
		friend int Is_Signature_Bad(const volatile Quaternion *);
	#endif

	Scalar x, y, z, w;

	//
	// Constructors
	//
	Quaternion()	{}
	Quaternion(Scalar x, Scalar y, Scalar z, Scalar w);

	//
	// Assignment operators
	//
	Quaternion& operator=(const Quaternion &q);
	Quaternion& operator=(const Hinge &hinge);
	Quaternion& operator=(const EulerAngles &angles);
	Quaternion& operator=(const YawPitchRoll &angles);
	Quaternion& operator=(const LinearMatrix &matrix);
	Quaternion& operator=(const Origin &p);

	//
	// "Close-enough" comparators
	//
	friend Logical Small_Enough(const Quaternion &q, Scalar e = SMALL)
	{
		Check(&q);
		return Close_Enough(q.w, 1.0f, e);
	}
	Logical operator!() const	{ return Small_Enough(*this, SMALL); }

	//
	// Axis index operators
	//
	const Scalar& operator[](size_t index) const
	{
		Check_Pointer(this);
		Warn(index>W_Axis);
		return (&x)[index];
	}
	Scalar& operator[](size_t index)
	{
		Check_Pointer(this);
		Warn(index>W_Axis);
		return (&x)[index];
	}

	Scalar GetAngle();
	void GetAxis(UnitVector *axis);

	//
	// Multiplication operators
	//
	Quaternion& Multiply(const Quaternion &q1, const Quaternion &q2);
	Quaternion& Multiply(const Quaternion &q, Scalar scale);
	Quaternion& MultiplyScaled(const Quaternion &q1, const Quaternion &q2, Scalar t);
	Quaternion& Add(const Quaternion &source, const Vector3D &delta);
	Quaternion& AddScaled(const Quaternion &source, const Vector3D &delta, Scalar t);

	//
	// Transform functions
	//
	Quaternion& Multiply(const Quaternion &q, const LinearMatrix &m);
	Quaternion& operator*=(const LinearMatrix &m);

	//
	// Template support
	//
	Quaternion& Lerp(const Quaternion& v1, const Quaternion& v2, Scalar t);

	//
	// Miscellaneous functions
	//
	Quaternion& Normalize();
	Quaternion& Subtract(const Quaternion &end, const Quaternion &start);
	Quaternion& Subtract(const UnitVector &end, const UnitVector &start);
	Quaternion& Subtract(const Vector3D &end, const Vector3D &start);

	//
	// Support functions
	//
	friend std::ostream& operator<<(std::ostream& stream, const Quaternion& quaternion);
	Logical TestInstance() const;
	static Logical TestClass();
};