////////////////////////////////////////////////////////////////////
// Rotation.h
//
// Copyright 2007 cDc@seacave
// Distributed under the Boost Software License, Version 1.0
// (See http://www.boost.org/LICENSE_1_0.txt)

#ifndef __SEACAVE_ROTATION_H__
#define __SEACAVE_ROTATION_H__


// I N C L U D E S /////////////////////////////////////////////////


// D E F I N E S ///////////////////////////////////////////////////

#ifndef _RELEASE
#define _CPC_DEBUG
#endif

#define CPC_ERROR(msg) { SLOG(msg); ASSERT(false); }
#if TD_VERBOSE != TD_VERBOSE_OFF
#define CPC_SLOG(msg) if (VERBOSITY_LEVEL > 0) SLOG(msg)
#define CPC_LOG(t, msg) CPC_SLOG(msg)
#else
#define CPC_SLOG(msg)
#define CPC_LOG(t, msg)
#endif


namespace SEACAVE {

// S T R U C T S ///////////////////////////////////////////////////

// generic matrix struct
#define QUATERNION_EPSILON 1E-10 // DBL_EPSILON, too small (dherzog)

// forward declaration to avoid mutual inclusion
template <typename TYPE> class TRMatrixBase;

  /** @class TQuaternion
   *  @test tested with TestRotationConversion.cpp
	  @ingroup g_geometry
	  @brief class for rotation with axis and angle

	  Quaternions can be used for rotation around an axis through the
	  origin by spcecifing the normal direction vector of the axis and
	  an angle.

	  The rotation for positive angles is clockwise,
	  when looking in direction of the axis.

	  Contains qx=[0], qy=[1], qz=[2], qw=[3].

	  Not every quaternion describes a rotation.
	  Only quaternions of the form:

	  qx = x * sin(phi/2) , which is the imaginary part i

	  qy = y * sin(phi/2)  , which is the imaginary part j

	  qz = z * sin(phi/2)  , which is the imaginary part k

	  qw = cos(phi/2),     , which is the real part

	  where (x, y, z) is the normalized direction vector of the axis
	  and phi is the angle of rtoation.

	  Some useful quaternions:
	  x 	       y 	        z 	       w          Description
	  0 	       0 	        0 	       1          Identity quaternion,
													  no rotation
	  1 	       0 	        0 	       0 	      180' turn around X axis
	  0 	       1 	        0 	       0 	      180' turn around Y axis
	  0 	       0 	        1 	       0 	      180' turn around Z axis
	  sqrt(0.5)    0 	        0 	       sqrt(0.5)  90' rotation around X axis
	  0 	       sqrt(0.5) 	0 	       sqrt(0.5)  90' rotation around Y axis
	  0 	       0 	        sqrt(0.5)  sqrt(0.5)  90' rotation around Z axis
	 -sqrt(0.5)    0 	        0 	       sqrt(0.5)  -90' rotation around X axis
	  0           -sqrt(0.5)    0 	       sqrt(0.5)  -90' rotation around Y axis
	  0 	       0 	       -sqrt(0.5)  sqrt(0.5)  -90' rotation around Z axis

	  @author grest 06/2003
  */
template <typename TYPE>
class TQuaternion : public TMatrix<TYPE,4,1>
{
public:
	typedef TMatrix<TYPE,4,1> Base;
	typedef TMatrix<TYPE,4,4> Mat;
	typedef TMatrix<TYPE,3,1> Vec;
	typedef TMatrix<TYPE,3,3> Rot;

	using Base::val;

public:
	static const TQuaternion IDENTITY;

public:
	inline ~TQuaternion();

	inline TQuaternion();

	inline TQuaternion(const Base& q);

	inline TQuaternion(TYPE i, TYPE j, TYPE k,	TYPE r);

	inline TQuaternion(const Rot& R);

	inline void SetIdentity();

	/**
	* Sets all parts of quaternion in mathematical order,
	* real part first, then 1.,2. and 3. imaginary part
	*/
	inline void SetQuaternion(TYPE real, TYPE i, TYPE j, TYPE k);

	/** returns the Inverse rotation TQuaternion
	*/
	inline TQuaternion<TYPE> Inverse() const;

	/** inverts this, by changing the rotation axis by *=-1
	*/
	inline void Invert();

	/** makes TQuaternion-representation unique, ensuring vector lies
	in upper (by real part) hemisphere. (inplace)
	*/
	inline void MakeUnique();

	/** quaternion multiplication: this= this * quat
	*/
	inline void Mult(const TQuaternion<TYPE> &quat);

	/** quaternion multiplication: res = this * arg
	*/
	inline void Mult(const TQuaternion<TYPE> &arg,
		TQuaternion<TYPE> &res) const;

	/** quaternion multiplication: this = quat * this
	*/
	inline void MultLeft(const TQuaternion<TYPE> &quat);

	/** rotates the given Vector with the quaternion ( q v q* )
	the resulting vector is given in res
	@returns 0 in case of no error
	@author Daniel Grest, June 2003
	*/
	inline int MultVec(const Vec &vec, Vec &res) const;

	/* rets result from MultVec()
	@author herzog 2005-07-15 */
	inline  Vec MultVec(const Vec &vec) const;

	/** Computes this^(scale), which scales the angle from axis/angle-
	representation with 'scale'. This is the same like inter-/extra-
	polating between (0,0,0,1)-quaternion and this!
	*/
	TQuaternion<TYPE> Power(const TYPE & scale) const;

	/** Linear interpolation between this and given quaternion.
	@note Quaternions are assumed to be unit quaternions! */
	TQuaternion<TYPE> InterpolateLinear(const TQuaternion<TYPE> &to, const TYPE & t) const;

	/** Spherical interpolation between this and given quaternion.
	@note Quaternions are assumed to be unit quaternions! */
	TQuaternion<TYPE> Interpolate(const TQuaternion<TYPE> &to, const TYPE & t) const;

	/** sets the quaternion with given rotation axis and angle (in rad)
	@returns 0 in case of no error
	@author Daniel Grest, June 2003
	*/
	inline void SetValueAsAxisRad(const Vec &axis, TYPE angle);

	/** sets the quaternion with given rotation axis and angle (in rad)
	@returns 0 in case of no error
	@author Daniel Grest, June 2003
	*/

	inline void SetValueAsAxisRad(TYPE axisX, TYPE axisY, TYPE axisZ, TYPE angle);

	/** Returns matrix which expresses (left) quaternion multiplication.
	* A quaternions stored in an object can be understood as a 4-vector
	* q =(ix iy iz r)^T.
	* Left multiplying a quaternion q2 with a quaternion q1 can be
	* expressed in terms of matrix multiplication as
	* qRes = q1*q2 = M(q1)*q2.
	* This method returns the matrix M(*this).
	*/
	Mat GetQuaternionMultMatrixLeft() const;

	/** Returns matrix which expresses right quaternion multiplication.
	* Right multiplying a quaternion q1 with quaternion q2 can be
	* expressed as qRes = q1*q2 = N(q2)*q1.
	* This method returns the matrix N(*this).
	*/
	Mat GetQuaternionMultMatrixRight() const;

	/** Returns rotation axis. See GetRotationAxisAngle(). */
	Vec GetRotationAxis() const;

	/** Returns rotation angle notation in radians.
		Note that the returned angle resides in the interval [0,PI)! */
	TYPE GetRotationAngle() const;

	/** Returns rotation in axis and angle notation (angle in radians).
		Note that the returned angle resides in the interval [0,PI) and
		the axis points into the according direction!
		@author woelk 12 2002 */
	int GetAxisAngle(Vec& axis, TYPE& angle) const;

	/** Sets quaternion as concatenated rotations around
		x,y,z-axis; this = q_x * q_y * q_z (BIAS-RMatrix conform)
		@author herzog 2005-07-19 */
	int SetXYZ(TYPE radX, TYPE radY, TYPE radZ);

	/** Sets quaternion as concatenated rotations around
		x,y,z-axis; this = q_z * q_y * q_x (BIAS-RMatrix conform)
		@author herzog 2005-07-19 */
	int SetZYX(TYPE radX, TYPE radY, TYPE radZ);


	/** Scales quaternion to unit length, i.e. norm equals 1. */
	inline void Normalize() {
		const TYPE n = norm(*this);
		if (ISZERO(n))
			*this = IDENTITY;
		else
			*this *= TYPE(1) / n;
	}
	inline bool IsNormalized() const {
		return ISEQUAL(norm(*this), TYPE(1));
	}

	/** assignment operator
		@author grest 06 2003 */
	TQuaternion<TYPE>& operator=(const TQuaternion<TYPE>& vec);

	/** Enforces rigid coupling constraint for this and the other given unit
		quaternion (i.e. equal rotation angle resp. scalar part of both).
		Computes direct simple interpolation of both unit quaternion where
		the scalar parts of the resulting unit quaternion are identical.
		This solution can be used as initial guess for numerical refinement.
		@author esquivel 02/2012 */
	void EnforceRigidCouplingConstraint(TQuaternion<TYPE> &other);

	/** Enforces rigid coupling constraint for all quaternion in given vector.
		@author esquivel 02/2012 */
	static void EnforceRigidCouplingConstraint(std::vector< TQuaternion<TYPE> > &quats);
}; // class
template <typename TYPE> const TQuaternion<TYPE> TQuaternion<TYPE>::IDENTITY(0,0,0,1);
/*----------------------------------------------------------------*/
typedef TQuaternion<float> QuaternionF;
typedef TQuaternion<double> QuaternionD;
/*----------------------------------------------------------------*/


// generic rotation matrix struct
#define ROTATION_MATRIX_EPSILON DBL_EPSILON

/** @class TRMatrixBase
	@ingroup g_geometry
	@brief 3D rotation matrix
	@author frahm, woelk **/
template <typename TYPE>
class TRMatrixBase : public TMatrix<TYPE,3,3>
{
public:
	typedef TMatrix<TYPE,3,3> Base;
	typedef Base Mat;
	typedef TMatrix<TYPE,3,1> Vec;
	typedef TQuaternion<TYPE> Quat;
	typedef typename Base::Base BaseBase;

	using Base::val;

public:
	/** @brief Constructor setting matrix to identity */
	inline TRMatrixBase();
	inline TRMatrixBase(const BaseBase& rhs) : Base(rhs) {}
	template <typename T> inline TRMatrixBase(const cv::Matx<T,3,3>& rhs) : Base(rhs) {}

	/** @brief Copy constructor from 3x3 matrix
		@attention Orthonormality of matrix is enforced automatically! */
	inline TRMatrixBase(const Mat& mat);

	/** @brief Initialization from parametrized rotation (axis-angle) */
	inline TRMatrixBase(const Vec& rot);

	/** @brief Initialization from rotation axis w and angle phi (in rad) using Rodrigues' formula */
	inline TRMatrixBase(const Vec& w, const TYPE phi);

	/** @brief Initialization from quaternion */
	inline TRMatrixBase(const Quat& q);

	/** @brief Initialization with the rotation from roll/pitch/yaw (in rad) */
	inline TRMatrixBase(TYPE roll, TYPE pitch, TYPE yaw);

	/** @brief Initialization with the rotation from direction dir0 to direction dir1 */
	inline TRMatrixBase(const Vec& dir0, const Vec& dir1);

	template <typename T> inline TRMatrixBase& operator = (const cv::Matx<T,3,3>& rhs) { BaseBase::operator = (rhs); return *this; }
	inline TRMatrixBase& operator = (const cv::Mat& rhs) { BaseBase::operator = (rhs); return *this; }
	#ifdef _USE_EIGEN
	inline TRMatrixBase& operator = (const typename Base::EMat& rhs) { Base::operator = (rhs); return *this; }
	#endif

	/** @brief Set Euler angles (in rad) in order XYZ

		Set angles either in order 1. z, 2. y, 3. x and fixed axes
		or in order 1. x, 2. y, 3. z and moving axes.
		Angles are measured as in mathematics (counter-clockwise), i.e.
		Set(x, 0, 0) results in the following matrix:

		| 1  0        0      |
		| 0  cos(x)  -sin(x) |
		| 0  sin(x)   cos(x) |

		(*this) = Rx*Ry*Rz =

		|  c(y)c(z)               -c(y)s(z)                s(y)     |
		|  c(z)s(x)s(y)+c(x)s(z)   c(x)c(z)-s(x)s(y)s(z)  -c(y)s(x) |
		| -c(x)c(z)s(y)+s(x)s(z)   c(z)s(x)+c(x)s(y)s(z)   c(x)c(y) |

		with s(g) = sin(g), c(g) = cos(g), x = PhiX, y = PhiY and z = PhiZ

		@author frahm, woelk */
	void SetXYZ(TYPE PhiX, TYPE PhiY, TYPE PhiZ);


	/** @brief Set Euler angles (in rad) in order XYZ from vector */
	inline void SetXYZ(const Vec& r)
	{ SetXYZ(r[0], r[1], r[2]); }

	/** @brief Set Euler angles (in rad) in order ZYX

		Set angles either in order 1. z, 2. y, 3. x and moving axes
		or in order 1. x, 2. y, 3. z and fixed axes.
		Angles are measured as in mathematics (counter-clockwise), i.e.
		Set(x, 0, 0) results in the following matrix:

		| 1  0        0      |
		| 0  cos(x)  -sin(x) |
		| 0  sin(x)   cos(x) |

		(*this) = Rz*Ry*Rx =

		|  c(y)c(z)  s(x)s(y)c(z)-c(x)s(z)  c(x)s(y)c(z)+s(x)s(z) |
		|  c(y)s(z)  s(x)s(y)s(z)+c(x)c(z)  c(x)s(y)s(z)-s(x)s(z) |
		| -s(y)      s(x)c(y)               c(x)c(y)              |

		with s(g) = sin(g), c(g) = cos(g), x = PhiX, y = PhiY and z = PhiZ

		@author frahm, woelk */
	void SetZYX(TYPE PhiX, TYPE PhiY, TYPE PhiZ);

	/** @brief Set Euler angles (in rad) in order ZYX from vector */
	inline void SetZYX(const Vec& r)
	{ SetZYX(r[0], r[1], r[2]); }

	/** @brief Set Euler angles (in rad) in order ZXY

		Set angles either in order 1. z, 2. x, 3. y and moving axes
		or in order 1. y, 2. x, 3. z and fixed axes.
		Angles are measured as in mathematics (counter-clockwise), i.e.
		Set(x, 0, 0) results in the following matrix:

		| 1  0        0      |
		| 0  cos(x)  -sin(x) |
		| 0  sin(x)   cos(x) |

		(*this) = Rz*Rx*Ry =

		| c(y)c(z)-s(x)s(y)s(z), c(x)s(z), s(y)c(z)+s(x)c(y)s(z) |
		| s(z)c(y)+s(x)s(y)c(z), c(x)c(z),  s(y)*s(z)+s(x)-c(y)c(z) |
		| -c(x)s(y)             ,  s(x)  ,  c(x)c(y)              |

		with s(g) = sin(g), c(g) = cos(g), x = PhiX, y = PhiY and z = PhiZ

		@author haase */
	void SetZXY(TYPE PhiX, TYPE PhiY, TYPE PhiZ);

	/** @brief Set Euler angles (in rad) in order ZXY from vector */
	inline void SetZXY(const Vec& r)
	{ SetZXY(r[0], r[1], r[2]); }

	 /** @brief Set Euler angles (in rad) in order YXZ

		 Set angles either in order 1. y, 2. x, 3. z and moving axes
		 or in order 1. z, 2. x, 3. y and fixed axes.
		 Angles are measured as in mathematics (counter-clockwise), i.e.
		 Set(x, 0, 0) results in the following matrix:

		 | 1  0        0      |
		 | 0  cos(x)  sin(x) |
		 | 0  -sin(x)   cos(x) |

		 (*this) = Rz*Rx*Ry =

		 | c(y)c(z)+s(x)s(y)s(z), c(x)s(z), -s(y)c(z)+s(x)c(y)s(z) |
		 | -s(z)c(y)+s(x)s(y)c(z), c(x)c(z),  -s(y)*-s(z)+s(x)c(y)c(z) |
		 | c(x)s(y)             ,  -s(x)  ,  c(x)c(y)              |

		 with s(g) = sin(g), c(g) = cos(g), x = PhiX, y = PhiY and z = PhiZ

		 @author haase */
	void SetYXZ(TYPE PhiZ, TYPE PhiX, TYPE PhiY);

	/** @brief Set Euler angles (in rad) in order YXZ from vector */
	inline void SetYXZ(const Vec& r)
	{ SetYXZ(r[0], r[1], r[2]); }

	/** @brief Set from rotation axis w and angle phi (in rad)
		@param w Axis vector w will be normalized to length 1, so we need
				 |w|>1e-6 if phi != 0, otherwise an exception is thrown
		@param phi Rotation angle is given in radians
		@author evers, woelk */
	void Set(const Vec& w, TYPE phi);

	/** set this matrix from 3 vectors each representing a column*/
	void SetFromColumnVectors(const Vec& v0,
							  const Vec& v1,
							  const Vec& v2);

	/** set this matrix from 3 vectors, each representing a row */
	void SetFromRowVectors(const Vec& v0,
						   const Vec& v1,
						   const Vec& v2);

	/** @brief Set from rotation axis * angle (modified Rodrigues vector)
		@author evers */
	void SetFromAxisAngle(const Vec& w);

	/* @brief Set rotation matrix from an orthogonal basis given in world
	   coordinate system (WCS)

	   As R' is expressing the transformation from WCS to ICS (image
	   coordinate system), R is ICS to WCS and hereby represents the ICS
	   base vectors expressed in WCS. These are the *rows* of R because
	   the transformation is the scalar product.

	   Assume xxx,yyy,zzz are right-hand orthogonal (RHS), e.g. the orthogonal
	   image coordinate system (ICS) base vectors expressed in world
	   coordinates (WCS).
	   (Inverse doesn't make sense because base would be identity then).
	   Normal vectors of length 1 are computed.

	   @todo Warning if (xxx,yyy,zzz) are not an orthogonal basis!

	   @author jw 09/2003, added exception */
	void SetFromOrthogonalBasis(const Vec &xxx, const Vec &yyy, const Vec &zzz);

	/** @brief Set rotation matrix from two vectors from an orthonormal basis
		@param xh represents the first base vector and is left unchanged
		@param vy should be orthogonal to xh, it is orthogonalized otherwise

		You can think of this routine as computing R from an image plane
		given by two (usually orthogonal) base vectors.
		If the given base vectors are not orthogonal, xh is kept and yv is
		orthogonalized appropriately.

		@author jw */
	void SetFromHV(const Vec& xh, const Vec& vy);

	/** @brief Create rotation matrix that rotates from dir0 to dir1
		@param dir0 represents the first (reference) direction vector
		@param dir1 represents the second (target) direction vector
		@author cDc */
	TRMatrixBase& SetFromDir2Dir(const Vec& dir0, const Vec& dir1);

	/** @brief Calculates quaternion representation for this rotation matrix
		@attention Scalar part of quaternion will always be non-negative
				   for sake of uniqueness of the resulting quaternion!
		@author woelk 12/2003
		@author esquivel 03/2011 (changed algorithm, bugfix) */
	void GetQuaternion(Quat& q) const;
	inline Quat GetQuaternion() const { Quat q; GetQuaternion(q); return q; }

	/** @brief Set rotation matrix from a quaternion
		@author grest 06/2003 */
	void SetFromQuaternion(const Quat& q);

	/** @brief Set rotation matrix from direction and up vector
		@note This is openGL conform, similar to gluLookAt(), i.e. if
		direction is (0,0,-1) and up is (0,1,0) the resulting matrix is identity.
		@author grest 12/2005 */
	void SetFromDirUpGL(const Vec& viewDir, const Vec& viewUp);

	/** @brief Set rotation matrix from direction and up vector */
	void SetFromDirUp(const Vec& viewDir, const Vec& viewUp);

	/** @brief Set rotation matrix from an eye point and a look-at target point and the up vector */
	void LookAt(const Vec& from, const Vec& to, const Vec& up);

	// get parametrized rotation (axis-angle) from the rotation matrix
	inline void SetRotationAxisAngle(const Vec& rot);

	// modify the rotation matrix by the given parametrized delta rotation (axis-angle)
	inline void Apply(const Vec& delta);

	// set rotation matrix to the given parametrized rotation (axis-angle)
	inline Vec GetRotationAxisAngle() const;

	/** @brief Calculates angle and rotation axis representation for
			   this rotation matrix
		@param angle Rotation angle is returned in radians
		@author woelk 12/2003 */
	void GetRotationAxisAngle(Vec& axis, TYPE& angle) const;

	/** @brief Interface for axis component of GetRotationAxisAngle() */
	Vec GetRotationAxis() const;

	/** @brief Interface for angle component of GetRotationAxisAngle() */
	TYPE GetRotationAngle() const;

	/** @brief Get Euler angles for this rotation matrix in order XYZ
		@attention Representation is not unique and has singularities at
		+-pi/2. Assume for example (*this) = Rx * Ry * Rz, then we have
		either Euler angles in order 1. x, 2. y, 3. z and moving axes
		or in order 1. z, 2. y, 3. x and fixed axes.
		@author woelk 01/2003 */
	int GetRotationAnglesXYZ(TYPE& PhiX, TYPE& PhiY, TYPE& PhiZ) const;

	/** @brief Get Euler angles for this rotation matrix in order XYZ
		@see GetRotationAnglesXYZ(TYPE&, TYPE&, TYPE&) */
	inline int GetRotationAnglesXYZ(Vec& r) const
	{ return GetRotationAnglesXYZ(r[0], r[1], r[2]); }

	/** @brief Get Euler angles for this rotation matrix in order YZX
		@attention Representation is not unique and has singularities at
		+-pi/2. Assume for example (*this) = Rz * Ry * Rx, then we have
		either Euler angles in order 1. x, 2. y, 3. z and fixed axes
		or in order 1. z, 2. y, 3. x and moving axes.
		@author woelk 01 2003 */
	int GetRotationAnglesZYX(TYPE& PhiX, TYPE& PhiY, TYPE& PhiZ) const;

	/** @brief Get Euler angles for this rotation matrix in order ZYX
		@see GetRotationAnglesZYX(TYPE&, TYPE&, TYPE&) */
	inline int GetRotationAnglesZYX(Vec& r) const
	{ return GetRotationAnglesZYX(r[0], r[1], r[2]); }

	/** @brief Get Euler angles for this rotation matrix in order ZXY
		@attention Representation is not unique and has singularities at
		+-pi/2. Rotation order ZXY refers to rotation around point/vector!
		@author haase 2007 */
	int GetRotationAnglesZXY(TYPE& PhiZ, TYPE& PhiX, TYPE& PhiY) const;

	/** @brief Get Euler angles for this rotation matrix in order ZXY
		@see GetRotationAnglesZXY(TYPE&, TYPE&, TYPE&) */
	inline int GetRotationAnglesZXY(Vec& r) const
	{ return GetRotationAnglesZXY(r[2], r[0], r[1]); }

	/** @brief Get Euler angles for this rotation matrix in order YXZ
		@attention Representation is not unique and has singularities at
		+-pi/2. Rotation order YXZ refers to rotation with moving axes!
		@author haase 2007 */
	int GetRotationAnglesYXZ(TYPE& PhiY, TYPE& PhiX, TYPE& PhiZ) const;

	/** @brief Get Euler angles for this rotation matrix in order YXZ
		@see GetRotationAnglesYXZ(TYPE&, TYPE&, TYPE&) */
	inline int GetRotationAnglesYXZ(Vec& r) const
	{ return GetRotationAnglesYXZ(r[1], r[0], r[2]); }

	/** @brief Check that the matrix is a valid rotation matrix (orthogonal) */
	inline bool IsValid() const {
		// the trace should be three and the determinant should be one
		#if 1
		return (ISEQUAL((float)cv::determinant(*this), 1.f) && ISEQUAL((float)cv::trace((*this)*(*this).t()), 3.f));
		#else
		return (ISEQUAL((TYPE)cv::determinant(*this), TYPE(1)) && ISEQUAL((TYPE)cv::trace((*this)*(*this).t()), TYPE(3)));
		#endif
	}

	/** @brief Check if this is a rotation matrix, i.e. if the determinant
			   is +1 and the columns are orthonormal
		@param eps Numerical limit for constraint evaluation
		@param verbose Show reason in case of failure */
	bool Check(const TYPE eps = std::numeric_limits<TYPE>::epsilon(),
			   int verbose = 0) const;

	/** @brief Enforce orthogonality constraint on rotation matrix and
			   sets determinant to +1 */
	void EnforceOrthogonality();
}; // class
/*----------------------------------------------------------------*/
typedef TRMatrixBase<float> RMatrixBaseF;
typedef TRMatrixBase<double> RMatrixBaseD;
/*----------------------------------------------------------------*/

} // namespace SEACAVE

#include "Rotation.inl"

#endif // __SEACAVE_ROTATION_H__

