
#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;


// Q -> E
Vector<double>  Quaternion2EulerXYZ(Quaternion qut, Vector<double>* vct=NULL);
Vector<double>  Quaternion2EulerZYX(Quaternion qut, Vector<double>* vct=NULL);
Vector<double>  Quaternion2EulerXZY(Quaternion qut, Vector<double>* vct=NULL);
Vector<double>  Quaternion2EulerYZX(Quaternion qut, Vector<double>* vct=NULL);
Vector<double>  Quaternion2EulerYXZ(Quaternion qut, Vector<double>* vct=NULL);
Vector<double>  Quaternion2EulerZXY(Quaternion qut, Vector<double>* vct=NULL);





////////////////////////////////////////////////////////////////////////////////////////////////

/**
class  DllExport Quaternion

クォータニオンの定義

回転の合成：A*B => Bの回転 -> Aの回転
*/
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);}
	virtual ~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, 0.0, c); 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	setRotate(double e, Vector<double> v) { setRotation(e, v);}
	void	setRotate(double e, double X, double Y, double Z, double N=0.0)   { Vector<double> v(X, Y, Z, N); setRotation(e, v);}

	// 注：eの成分は演算順
	void	setEulerXYZ(Vector<double> e);	//
	void	setEulerZYX(Vector<double> e);	//
	void	setEulerXZY(Vector<double> e);	// 
	void	setEulerYZX(Vector<double> e);	// 
	void	setEulerYXZ(Vector<double> e);	// 
	void	setEulerZXY(Vector<double> e);	// 

	Vector<double>	getEulerXYZ(Vector<double>* vt=NULL) { return Quaternion2EulerXYZ(*this, vt);}
	Vector<double>	getEulerZYX(Vector<double>* vt=NULL) { return Quaternion2EulerZYX(*this, vt);}
	Vector<double>	getEulerXZY(Vector<double>* vt=NULL) { return Quaternion2EulerXZY(*this, vt);}
	Vector<double>	getEulerYZX(Vector<double>* vt=NULL) { return Quaternion2EulerYZX(*this, vt);}
	Vector<double>	getEulerYXZ(Vector<double>* vt=NULL) { return Quaternion2EulerYXZ(*this, vt);}
	Vector<double>	getEulerZXY(Vector<double>* vt=NULL) { return Quaternion2EulerZXY(*this, vt);}

	Vector<double>	execRotation(Vector<double> v);
	Vector<double>	execInvRotation(Vector<double> v);
	Vector<double>  execRotate(Vector<double> v)    { return execRotation(v);}
	Vector<double>  execInvRotate(Vector<double> v) { return execInvRotation(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 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); }


/**
演算順序 A*B = Bの回転 -> Aの回転
*/
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;
}





/////////////////////////////////////////////////////////////////////////////////////////////////////////////
// 
// 回転行列，オイラー角，クォータニオン
//
// 注：オイラー角ベクトルの成分は演算順に指定する．
//        例 YZX回転なら e.x:Y回転，e.y:Z回転, e.z:X回転
//

// E -> R
Matrix<double>  EulerXYZ2RotMatrix(Vector<double> eul);
Matrix<double>  EulerZYX2RotMatrix(Vector<double> eul);
Matrix<double>  EulerXZY2RotMatrix(Vector<double> eul);
Matrix<double>  EulerYZX2RotMatrix(Vector<double> eul);
Matrix<double>  EulerYXZ2RotMatrix(Vector<double> eul);
Matrix<double>  EulerZXY2RotMatrix(Vector<double> eul);


// R -> E
Vector<double>  RotMatrix2EulerXYZ(Matrix<double> mtx, Vector<double>* vct=NULL);
Vector<double>  RotMatrix2EulerZYX(Matrix<double> mtx, Vector<double>* vct=NULL);
Vector<double>  RotMatrix2EulerXZY(Matrix<double> mtx, Vector<double>* vct=NULL);
Vector<double>  RotMatrix2EulerYZX(Matrix<double> mtx, Vector<double>* vct=NULL);
Vector<double>  RotMatrix2EulerYXZ(Matrix<double> mtx, Vector<double>* vct=NULL);
Vector<double>  RotMatrix2EulerZXY(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>  RotMatrixElements2EulerXZY(double m11, double m12, double m13, double m21, double m22, double m23, double m31, Vector<double>* vct=NULL);
Vector<double>  RotMatrixElements2EulerYZX(double m11, double m12, double m13, double m21, double m22, double m31, double m32, Vector<double>* vct=NULL);
Vector<double>  RotMatrixElements2EulerYXZ(double m12, double m21, double m22, double m23, double m31, double m32, double m33, Vector<double>* vct=NULL);
Vector<double>  RotMatrixElements2EulerZXY(double m12, double m13, double m21, double m22, double m23, double m32, double m33, Vector<double>* vct=NULL);


// Q -> E
/*
Vector<double>  Quaternion2EulerXYZ(Quaternion qut, Vector<double>* vct=NULL);
Vector<double>  Quaternion2EulerZYX(Quaternion qut, Vector<double>* vct=NULL);
Vector<double>  Quaternion2EulerXZY(Quaternion qut, Vector<double>* vct=NULL);
Vector<double>  Quaternion2EulerYZX(Quaternion qut, Vector<double>* vct=NULL);
Vector<double>  Quaternion2EulerYXZ(Quaternion qut, Vector<double>* vct=NULL);
Vector<double>  Quaternion2EulerZXY(Quaternion qut, Vector<double>* vct=NULL);
*/


// R -> Q
Quaternion		RotMatrix2Quaternion(Matrix<double> mtx);


// Q -> R
#define  		Quaternion2RotMatrix(q)  (q).getRotMatrix()


// E -> Q   （注：eの成分は演算順）
inline Quaternion  EulerXYZ2Quaternion(Vector<double> e) { Quaternion q; q.setEulerXYZ(e); return q;}
inline Quaternion  EulerZYX2Quaternion(Vector<double> e) { Quaternion q; q.setEulerZYX(e); return q;}
inline Quaternion  EulerXZY2Quaternion(Vector<double> e) { Quaternion q; q.setEulerXZY(e); return q;}
inline Quaternion  EulerYZX2Quaternion(Vector<double> e) { Quaternion q; q.setEulerYZX(e); return q;}
inline Quaternion  EulerYXZ2Quaternion(Vector<double> e) { Quaternion q; q.setEulerYXZ(e); return q;}
inline Quaternion  EulerZXY2Quaternion(Vector<double> e) { Quaternion q; q.setEulerZXY(e); return q;}





/////////////////////////////////////////////////////////////////////////////////////////////////////////////
// 
// ベクトルの回転，クォータニオン
//

//#define			VectorRotation(v, q)	(q).execRotation((v))
//#define			VectorInvRotation(v, q)	(q).execInvRotation((v))
Vector<double>	VectorRotation(Vector<double> v, Quaternion q);		// たぶん execRotation() よりは高速．
Vector<double>	VectorInvRotation(Vector<double> v, Quaternion q);
#define			VectorRotate(v, q)	 	VectorRotation((v),(q))
#define			VectorInvRotate(v, q)	VectorInvRotation((v), (q))


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);







/////////////////////////////////////////////////////////////////////////////////////////////////////////////
// 
// Affine Transfer
//

/**
アフィン変換
	拡大縮小，回転，平行移動（のみ）

変換の合成：A*B => Bの変換 -> Aの変換
*/
class  DllExport AffineTrans
{
public:
 	Matrix<double>  matrix;		// Matrix(2, 4, 4); 2次元 4x4行列 

	Vector<double> 	shift;
	Vector<double> 	scale;
	Quaternion	   	rotate;

	bool   			isInverse;

public:
    AffineTrans(void) { setup();}
    virtual ~AffineTrans(void) {}

	void   initComponents(void) { initScale(); initRotate(); initShift(); isInverse = false;}
//	void   init(void) { initComponents(); matrix.init();}

	void   setup(void){ initComponents(); matrix = Matrix<double>(2, 4, 4);}
	void   set(Vector<double> s, Quaternion q, Vector<double> t, bool inv=false) { scale=s; shift=t, rotate=q; isInverse=inv; computeMatrix();}
	void   free(void) { initComponents(); matrix.free();}
	void   clear(void){ initComponents(); matrix.clear();}
	void   dup(AffineTrans a);

	void   computeMatrix(void);
	void   computeComponents(void);

	void   initShift (void) { shift.init();}
	void   initScale (void) { scale.set(1.0, 1.0, 1.0);}
	void   initRotate(void) { rotate.init();}

	void   setShift (double x, double y, double z) { shift.set(x, y, z);}
	void   setScale (double x, double y, double z) { scale.set(x, y, z);}
	void   setRotate(double s, double x, double y, double z) { rotate.setRotation(s, x, y, z);}

	void   addShift (double x, double y, double z) { shift.x += x; shift.y += y; shift.z += z;}
	void   addScale (double x, double y, double z) { scale.x *= x; scale.y *= y; scale.z *= z;}
	void   addRotate(double s, double x, double y, double z) { Quaternion q(s, x, y, z); rotate = q*rotate;}

	Vector<double> getShift (void) { return shift;}
	Vector<double> getScale (void) { return scale;}
	Quaternion     getRotate(void) { return rotate;}
	Matrix<double> getMatrix(void) { return matrix;}
	Matrix<double> getRotMatrix(void);
	AffineTrans    getInvAffine(void);

	bool   isSetComponents(void) { if(isSetShift() || isSetScale() || isSetRotate()) return true; else return false;}
	bool   isSetShift(void)  { return (shift !=Vector<double>());}
	bool   isSetScale(void)  { return (scale !=Vector<double>(1.0, 1.0, 1.0));}
	bool   isSetRotate(void) { return (rotate!=Quaternion());}
	bool   isNormal(void)    { if(scale.x>=JBXL_EPS && scale.y>=JBXL_EPS && scale.z>=JBXL_EPS) return true; else return false;}


	Vector<double> execMatrixTrans(Vector<double> v);

	Vector<double> execTrans(Vector<double> v)    { if(!isInverse) return execShift(execRotate(execScale(v))); 
                                                    else           return execScale(execRotate(execShift(v)));}
	Vector<double> execInvTrans(Vector<double> v) { if(!isInverse) return execInvScale(execInvRotate(execInvShift(v)));
                                                    else           return execInvShift(execInvRotate(execInvScale(v)));}

	Vector<double> execShift(Vector<double> v)    { return Vector<double>(v.x+shift.x, v.y+shift.y, v.z+shift.z, 0., Min(v.c, shift.c));}
	Vector<double> execInvShift(Vector<double> v) { return Vector<double>(v.x-shift.x, v.y-shift.y, v.z-shift.z, 0., Min(v.c, shift.c));}
	Vector<double> execScale(Vector<double> v)    { return Vector<double>(v.x*scale.x, v.y*scale.y, v.z*scale.z, 0., Min(v.c, scale.c));}
	Vector<double> execInvScale(Vector<double> v) { return Vector<double>(v.x/scale.x, v.y/scale.y, v.z/scale.z, 0., Min(v.c, scale.c));}
	Vector<double> execRotate(Vector<double> v)   { return VectorRotation(v, rotate);}
	Vector<double> execInvRotate(Vector<double> v){ return VectorInvRotation(v, rotate);}
};



inline void  freeAffineTrans(AffineTrans*& affine) { if(affine!=NULL) { affine->free(); delete(affine); affine = NULL;}}

inline AffineTrans*  newAffineTrans(AffineTrans p) { AffineTrans* a = new AffineTrans(); a->dup(p); return a;}





/**
変換の合成：A*B => Bの変換 -> Aの変換
*/
inline AffineTrans operator * (const AffineTrans a, const AffineTrans b)
{ 
	Quaternion rotate = a.rotate * b.rotate;
	Vector<double> shift = a.shift + b.shift;
	//
	Vector<double> scale;
	scale.set(a.scale.x*b.scale.x, a.scale.y*b.scale.y, a.scale.z*b.scale.z, 0.0, Min(a.scale.c, b.scale.c));
	
	AffineTrans affine;
	affine.set(scale, rotate, shift, a.isInverse);

	return affine;
}




}		// namespace





#endif 


