#ifndef  __JBXL_CPP_QUATERNION_H_
#define  __JBXL_CPP_QUATERNION_H_


/**
@brief    回転・クォータニオン ライブラリ ヘッダ
@file     Rotation.h
@author   Fumi.Iseki (C)
*/


#include  "Matrix++.h"
#include  "Vector.h"


//
namespace  jbxl {



/////////////////////////////////////////////////////////////////////////////////////////////////////////////
// 
// クォータニオン
//

/**
class  DllExport Quaternion

クォータニオンの定義

*/
class  DllExport Quaternion
{
public:
	double s;
	double x;
	double y;
	double z;

	double n;		///< ノルム
	double c;		///< 信頼度


public:
	Quaternion(void)  { init();}
	Quaternion(double S, double X, double Y, double Z, double N=0.0, double C=1.0) { set(S, X, Y, Z, N, C);}
	Quaternion(double S, Vector<double> v) { setRotation(S, v);}
	~Quaternion(void) {}

	double	norm(void)  { n = sqrt(s*s+x*x+y*y+z*z); return n;}
	void	normalize(void);

	void	set(double S, double X, double Y, double Z, double N=0.0, double C=1.0);
	void	init(double C=1.0)  { s = n = 1.0; x = y = z = 0.0; c = C;}

	Vector<double>	getVector() { Vector<double> v(x, y, z); return v;}
	double			getScalar() { return s;}
	double			getAngle()  { return 2.0*acos(s);}	///< 0〜2π
	double			getMathAngle() { double t=2.0*acos(s); if(t>PI)t-=PI2; return t;} ///< -π〜π
	
	Matrix<double>	getRotMatrix();

	void	setRotation(double e, Vector<double> v);
	void	setRotation(double e, double X, double Y, double Z, double N=0.0) { Vector<double> v(X, Y, Z, N); setRotation(e, v);}

	void	setEulerXYZ(Vector<double> e);	///< X->Y->Z
	void	setEulerZYX(Vector<double> e);	///< Z->Y->X

	void	setEulerXZY(Vector<double> e);	// 
	void	setEulerYZX(Vector<double> e);	// 
	void	setEulerYXZ(Vector<double> e);	// 
	void	setEulerZXY(Vector<double> e);	// 

	Vector<double>	execRotation(Vector<double> v);
};







/////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Quaternion オペレータ

inline bool operator == (const Quaternion q1, const Quaternion q2)
{ return (q1.s==q2.s && q1.x==q2.x && q1.y==q2.y && q1.z==q2.z); }


///< ~ 共役
inline Quaternion operator ~ (const Quaternion a)
{ return Quaternion(a.s, -a.x, -a.y, -a.z, a.n, a.c); }


//
inline Quaternion operator - (const Quaternion a)
{ return Quaternion(-a.s, -a.x, -a.y, -a.z, a.n, a.c); }


inline Quaternion operator + (const Quaternion a, const Quaternion b)
{ return Quaternion(a.s+b.s, a.x+b.x, a.y+b.y, a.z+b.z, 0.0, Min(a.c, b.c)); }


inline Quaternion operator - (const Quaternion a, const Quaternion b)
{ return Quaternion(a.s-b.s, a.x-b.x, a.y-b.y, a.z-b.z, 0.0, Min(a.c, b.c)); }


template <typename R> inline Quaternion operator * (const R d, const Quaternion a)
{ return Quaternion(d*a.s, d*a.x, d*a.y, d*a.z, d*a.n, a.c); }


template <typename R> inline Quaternion operator * (const Quaternion a, const R d)
{ return Quaternion(a.s*d, a.x*d, a.y*d, a.z*d, a.n*d, a.c); }


template <typename R> inline Quaternion operator / (const Quaternion a, const R d)
{ return Quaternion(a.s/d, a.x/d, a.y/d, a.z/d, a.n/d, a.c); }


// *
inline Quaternion operator * (const Quaternion a, const Quaternion b)
{
	Quaternion quat;
	if (a.c<0.0 || b.c<0.0) quat.init(-1.0);
	else  quat.set(a.s*b.s-a.x*b.x-a.y*b.y-a.z*b.z, a.s*b.x+a.x*b.s+a.y*b.z-a.z*b.y, 
				   a.s*b.y+a.y*b.s+a.z*b.x-a.x*b.z, a.s*b.z+a.z*b.s+a.x*b.y-a.y*b.x, 1.0, Min(a.c, b.c)); 
	return quat;
}


// *
inline Quaternion operator * (const Quaternion q, const Vector<double> v)
{ 
	Quaternion quat;
	if (q.c<0.0 || v.c<0.0) quat.init(-1.0);
	else quat.set(-q.x*v.x-q.y*v.y-q.z*v.z, q.s*v.x+q.y*v.z-q.z*v.y, 
				   q.s*v.y+q.z*v.x-q.x*v.z, q.s*v.z+q.x*v.y-q.y*v.x, v.n, Min(q.c, v.c));
	return quat;
}


// *
inline Quaternion operator * (const Vector<double> v, const Quaternion q)
{ 
	Quaternion quat;
	if (q.c<0.0 || v.c<0.0) quat.init(-1.0);
	else quat.set(-v.x*q.x-v.y*q.y-v.z*q.z, v.x*q.s+v.y*q.z-v.z*q.y, 
				   v.y*q.s+v.z*q.x-v.x*q.z, v.z*q.s+v.x*q.y-v.y*q.x, v.n, Min(v.c, q.c));
	return quat;
}





/////////////////////////////////////////////////////////////////////////////////////////////////////////////
// 
// 回転行列，オイラー角
//

Matrix<double>  EulerXYZ2RotMatrix(Vector<double> eul);
Matrix<double>  EulerZYX2RotMatrix(Vector<double> eul);

Vector<double>  RotMatrix2EulerXYZ(Matrix<double> mtx, Vector<double>* vct=NULL);
Vector<double>  RotMatrix2EulerZYX(Matrix<double> mtx, Vector<double>* vct=NULL);

Vector<double>  RotMatrixElements2EulerXYZ(double m11, double m12, double m13, double m21, double m31, double m32, double m33, Vector<double>* vct=NULL);
Vector<double>  RotMatrixElements2EulerZYX(double m11, double m12, double m13, double m21, double m23, double m31, double m33, Vector<double>* vct=NULL);

Vector<double>  Quaternion2EulerXYZ(Quaternion qut, Vector<double>* vct=NULL);
Vector<double>  Quaternion2EulerZYX(Quaternion qut, Vector<double>* vct=NULL);

#define  		Quaternion2RotMatrix(q)  (q).getRotMatrix()


Quaternion		V2VQuaternion(Vector<double> a, Vector<double> b);

Quaternion		PPPQuaternion(Vector<double> a, Vector<double> b, Vector<double> c);
Quaternion		VPPQuaternion(Vector<double> a, Vector<double> b, Vector<double> c);
Quaternion		PPVQuaternion(Vector<double> a, Vector<double> b, Vector<double> c);


Quaternion		SlerpQuaternion(Quaternion a, Quaternion b, double t);




}		// namespace



#endif