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


#include  "Rotation.h"


using namespace jbxl;



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

void  Quaternion::set(double S, double X, double Y, double Z, double N, double C)
{
	s  = S;
	x  = X;
	y  = Y;
	z  = Z;
	n  = N;
	c  = C;

	//if (n<=0.0) {
	//	n = sqrt(s*s + x*x + y*y + z*z);
	//}
}



void  Quaternion::normalize() 
{
	double nrm = sqrt(s*s + x*x + y*y + z*z);
	
	if (nrm>=Zero_Eps) {
		s  = s/nrm;
		x  = x/nrm;
		y  = y/nrm;
		z  = z/nrm;
		n  = 1.0;
	}
	else {
		init();
	}
}



/**


@param e 回転角
@param v 回転軸
*/
void  Quaternion::setRotation(double e, Vector<double> v)
{
	if (v.n!=1.0) v.normalize();

	if (e>PI)       e = e - PI2;
	else if (e<-PI) e = PI2 + e;

	double s2 = e/2.0;
	double sn = sin(s2);
	s  = cos(s2);
	x  = v.x*sn;
	y  = v.y*sn;
	z  = v.z*sn;
	n  = v.n;
	c  = 1.0;
}





/**
void  Quaternion::setEulerYZX(Vector<double> e)

Y->Z->X の順にオイラー角でベクトルを回転させる場合のクオータニオンを計算する．

@param e オイラー角の成分．作用させる順番に格納する．
         この場合, e.xはY軸回転，e.yはZ軸回転，e.z はX軸回転のそれぞれの回転角（ラジアン）を格納する．
*/

/// Y->Z->X
void  Quaternion::setEulerYZX(Vector<double> e)
{
	Vector<double> ex(1.0, 0.0, 0.0, 1.0);  
	Vector<double> ey(0.0, 1.0, 0.0, 1.0);  
	Vector<double> ez(0.0, 0.0, 1.0, 1.0);

	Quaternion qx, qy, qz;
	qy.setRotation(e.element1(), ey);
	qz.setRotation(e.element2(), ez);
	qx.setRotation(e.element3(), ex);

	(*this) = qx*qz*qy;
	if (s<0.0) (*this) = - (*this);
}


/// X->Y->Z
void  Quaternion::setEulerXYZ(Vector<double> e)
{
	Vector<double> ex(1.0, 0.0, 0.0, 1.0);  
	Vector<double> ey(0.0, 1.0, 0.0, 1.0);  
	Vector<double> ez(0.0, 0.0, 1.0, 1.0);

	Quaternion qx, qy, qz;
	qx.setRotation(e.element1(), ex);
	qy.setRotation(e.element2(), ey);
	qz.setRotation(e.element3(), ez);

	(*this) = qz*qy*qx;
	if (s<0.0) (*this) = - (*this);
}


/// Z->Y->X
void  Quaternion::setEulerZYX(Vector<double> e)
{
	Vector<double> ex(1.0, 0.0, 0.0, 1.0);  
	Vector<double> ey(0.0, 1.0, 0.0, 1.0);  
	Vector<double> ez(0.0, 0.0, 1.0, 1.0);

	Quaternion qx, qy, qz;
	qz.setRotation(e.element1(), ez);
	qy.setRotation(e.element2(), ey);
	qx.setRotation(e.element3(), ex);

	(*this) = qx*qy*qz;
	if (s<0.0) (*this) = - (*this);
}


/// X->Z->Y
void  Quaternion::setEulerXZY(Vector<double> e)
{
	Vector<double> ex(1.0, 0.0, 0.0, 1.0);  
	Vector<double> ey(0.0, 1.0, 0.0, 1.0);  
	Vector<double> ez(0.0, 0.0, 1.0, 1.0);

	Quaternion qx, qy, qz;
	qx.setRotation(e.element1(), ex);
	qz.setRotation(e.element2(), ez);
	qy.setRotation(e.element3(), ey);

	(*this) = qy*qz*qx;
	if (s<0.0) (*this) = - (*this);
}


/// Y->X->Z
void  Quaternion::setEulerYXZ(Vector<double> e)
{
	Vector<double> ex(1.0, 0.0, 0.0, 1.0);  
	Vector<double> ey(0.0, 1.0, 0.0, 1.0);  
	Vector<double> ez(0.0, 0.0, 1.0, 1.0);

	Quaternion qx, qy, qz;
	qy.setRotation(e.element1(), ey);
	qx.setRotation(e.element2(), ex);
	qz.setRotation(e.element3(), ez);

	(*this) = qz*qx*qy;
	if (s<0.0) (*this) = - (*this);
}


/// Z->X->Y
void  Quaternion::setEulerZXY(Vector<double> e)
{
	Vector<double> ex(1.0, 0.0, 0.0, 1.0);  
	Vector<double> ey(0.0, 1.0, 0.0, 1.0);  
	Vector<double> ez(0.0, 0.0, 1.0, 1.0);

	Quaternion qx, qy, qz;
	qz.setRotation(e.element1(), ez);
	qx.setRotation(e.element2(), ex);
	qy.setRotation(e.element3(), ey);

	(*this) = qy*qx*qz;
	if (s<0.0) (*this) = - (*this);
}




/**
ベクトルを回転させる場合の回転行列を求める．
座標軸の回転の場合は共役クォータニオンで計算する．

*/
Matrix<double>  Quaternion::getRotMatrix()
{
	Matrix<double> mtx(2, 3, 3);	// 2次元マトリックス, 3x3

	if (n!=1.0) normalize();

	mtx.element(1, 1) = 1.0 - 2.0*y*y - 2.0*z*z;
	mtx.element(1, 2) = 2.0*x*y - 2.0*s*z;
	mtx.element(1, 3) = 2.0*x*z + 2.0*s*y;
	mtx.element(2, 1) = 2.0*x*y + 2.0*s*z;
	mtx.element(2, 2) = 1.0 - 2.0*x*x - 2.0*z*z;
	mtx.element(2, 3) = 2.0*y*z - 2.0*s*x;
	mtx.element(3, 1) = 2.0*x*z - 2.0*s*y;
	mtx.element(3, 2) = 2.0*y*z + 2.0*s*x;
	mtx.element(3, 3) = 1.0 - 2.0*x*x - 2.0*y*y;

	return mtx;
}



/// Exec Rotation  qv(~q)
Vector<double>  Quaternion::execRotation(Vector<double> v)
{
	if (c<0.0) return v;

	Quaternion inv = ~(*this);
	Quaternion qut =  (*this)*(v*inv);
	Vector<double> vct = qut.getVector();

	return vct;
}





/////////////////////////////////////////////////////////////////////////////////////////////////////////////
// 
// 回転行列，オイラー角
//
//   オイラー角の角度は全て右手系の正の方向が基準
//   オイラー角ベクトルの成分は演算順
//

//
// Euler -> Rotation Matrix
//
Matrix<double>  jbxl::EulerXYZ2RotMatrix(Vector<double> eul)
{
	Matrix<double> mtx(2, 3, 3);

	double sinx = sin(eul.element1());
	double cosx = cos(eul.element1());
	double siny = sin(eul.element2());
	double cosy = cos(eul.element2());
	double sinz = sin(eul.element3());
	double cosz = cos(eul.element3());

	mtx.element(1, 1) =  cosy*cosz;
	mtx.element(1, 2) =  sinx*siny*cosz - cosx*sinz;
	mtx.element(1, 3) =  cosx*siny*cosz + sinx*sinz;
	mtx.element(2, 1) =  cosy*sinz;
	mtx.element(2, 2) =  sinx*siny*sinz + cosx*cosz;
	mtx.element(2, 3) =  cosx*siny*sinz - sinx*cosz;
	mtx.element(3, 1) = -siny;
	mtx.element(3, 2) =  sinx*cosy;
	mtx.element(3, 3) =  cosx*cosy;

	return mtx;
}



Matrix<double>  jbxl::EulerZYX2RotMatrix(Vector<double> eul)
{
	Matrix<double> mtx(2, 3, 3);

	double sinz = sin(eul.element1());
	double cosz = cos(eul.element1());
	double siny = sin(eul.element2());
	double cosy = cos(eul.element2());
	double sinx = sin(eul.element3());
	double cosx = cos(eul.element3());

	mtx.element(1, 1) =  cosy*cosz;
	mtx.element(1, 2) = -cosy*sinz;
	mtx.element(1, 3) =  siny;
	mtx.element(2, 1) =  sinx*siny*cosz + cosx*sinz;
	mtx.element(2, 2) = -sinx*siny*sinz + cosx*cosz;
	mtx.element(2, 3) = -sinx*cosy;
	mtx.element(3, 1) = -cosx*siny*cosz + sinx*sinz;
	mtx.element(3, 2) =  cosx*siny*sinz + sinx*cosz;
	mtx.element(3, 3) =  cosx*cosy;

	return mtx;
}



Matrix<double>  jbxl::EulerXZY2RotMatrix(Vector<double> eul)
{
	Matrix<double> mtx(2, 3, 3);

	double sinx = sin(eul.element1());
	double cosx = cos(eul.element1());
	double sinz = sin(eul.element2());
	double cosz = cos(eul.element2());
	double siny = sin(eul.element3());
	double cosy = cos(eul.element3());

	mtx.element(1, 1) =  cosy*cosz;
	mtx.element(1, 2) = -cosx*cosy*sinz + sinx*siny;
	mtx.element(1, 3) =  sinx*cosy*sinz + cosx*siny;
	mtx.element(2, 1) =  sinz;
	mtx.element(2, 2) =  cosx*cosz;
	mtx.element(2, 3) = -sinx*cosz;
	mtx.element(3, 1) = -siny*cosz;
	mtx.element(3, 2) =  cosx*siny*sinz + sinx*cosy;
	mtx.element(3, 3) = -sinx*siny*sinz + cosx*cosy;

	return mtx;
}



Matrix<double>  jbxl::EulerYZX2RotMatrix(Vector<double> eul)
{
	Matrix<double> mtx(2, 3, 3);

	double siny = sin(eul.element1());
	double cosy = cos(eul.element1());
	double sinz = sin(eul.element2());
	double cosz = cos(eul.element2());
	double sinx = sin(eul.element3());
	double cosx = cos(eul.element3());

	mtx.element(1, 1) =  cosy*cosz;
	mtx.element(1, 2) = -sinz;
	mtx.element(1, 3) =  siny*cosz;
	mtx.element(2, 1) =  cosx*cosy*sinz + sinx*siny;
	mtx.element(2, 2) =  cosx*cosz;
	mtx.element(2, 3) =  cosx*siny*sinz - sinx*cosy;
	mtx.element(3, 1) =  sinx*cosy*sinz - cosx*siny;
	mtx.element(3, 2) =  sinx*cosz;
	mtx.element(3, 3) =  sinx*siny*sinz + cosx*cosy;

	return mtx;
}



Matrix<double>  jbxl::EulerZXY2RotMatrix(Vector<double> eul)
{
	Matrix<double> mtx(2, 3, 3);

	double sinz = sin(eul.element1());
	double cosz = cos(eul.element1());
	double sinx = sin(eul.element2());
	double cosx = cos(eul.element2());
	double siny = sin(eul.element3());
	double cosy = cos(eul.element3());

	mtx.element(1, 1) =  sinx*siny*sinz + cosy*cosz;
	mtx.element(1, 2) =  sinx*siny*cosz - cosy*sinz;
	mtx.element(1, 3) =  cosx*siny;
	mtx.element(2, 1) =  cosx*sinz;
	mtx.element(2, 2) =  cosx*cosz;
	mtx.element(2, 3) = -sinx;
	mtx.element(3, 1) =  sinx*cosy*sinz - siny*cosz;
	mtx.element(3, 2) =  sinx*cosy*cosz + siny*sinz;
	mtx.element(3, 3) =  cosx*cosy;
	
	return mtx;
}



Matrix<double>  jbxl::EulerYXZ2RotMatrix(Vector<double> eul)
{
	Matrix<double> mtx(2, 3, 3);

	double siny = sin(eul.element1());
	double cosy = cos(eul.element1());
	double sinx = sin(eul.element2());
	double cosx = cos(eul.element2());
	double sinz = sin(eul.element3());
	double cosz = cos(eul.element3());

	mtx.element(1, 1) = -sinx*siny*sinz + cosy*cosz;
	mtx.element(1, 2) = -cosx*sinz;
	mtx.element(1, 3) =  sinx*cosy*sinz + siny*cosz;
	mtx.element(2, 1) =  sinx*siny*cosz + cosy*sinz;
	mtx.element(2, 2) =  cosx*cosz;
	mtx.element(2, 3) = -sinx*cosy*cosz + siny*sinz;
	mtx.element(3, 1) = -cosx*siny;
	mtx.element(3, 2) =  sinx;
	mtx.element(3, 3) =  cosx*cosy;

	return mtx;
}




//
// Rotation Matrix -> Euler
//
Vector<double>  jbxl::RotMatrix2EulerXYZ(Matrix<double> mtx, Vector<double>* vct)
{
	double m11 = mtx.element(1, 1);
	double m12 = mtx.element(1, 2);
	double m13 = mtx.element(1, 3);
	double m21 = mtx.element(2, 1);
	double m31 = mtx.element(3, 1);
	double m32 = mtx.element(3, 2);
	double m33 = mtx.element(3, 3);

	Vector<double> eul = RotMatrixElements2EulerXYZ(m11, m12, m13, m21, m31, m32, m33, vct);

	return eul;
}



Vector<double>  jbxl::RotMatrix2EulerZYX(Matrix<double> mtx, Vector<double>* vct)
{
	double m11 = mtx.element(1, 1);
	double m12 = mtx.element(1, 2);
	double m13 = mtx.element(1, 3);
	double m21 = mtx.element(2, 1);
	double m23 = mtx.element(2, 3);
	double m31 = mtx.element(3, 1);
	double m33 = mtx.element(3, 3);

	Vector<double> eul = RotMatrixElements2EulerZYX(m11, m12, m13, m21, m23, m31, m33, vct);

	return eul;
}



Vector<double>  jbxl::RotMatrix2EulerXZY(Matrix<double> mtx, Vector<double>* vct)
{
	double m11 = mtx.element(1, 1);
	double m12 = mtx.element(1, 2);
	double m13 = mtx.element(1, 3);
	double m21 = mtx.element(2, 1);
	double m22 = mtx.element(2, 2);
	double m23 = mtx.element(2, 3);
	double m31 = mtx.element(3, 1);

	Vector<double> eul = RotMatrixElements2EulerXZY(m11, m12, m13, m21, m22, m23, m31, vct);

	return eul;
}



Vector<double>  jbxl::RotMatrix2EulerYZX(Matrix<double> mtx, Vector<double>* vct)
{
	double m11 = mtx.element(1, 1);
	double m12 = mtx.element(1, 2);
	double m13 = mtx.element(1, 3);
	double m21 = mtx.element(2, 1);
	double m22 = mtx.element(2, 2);
	double m31 = mtx.element(3, 1);
	double m32 = mtx.element(3, 2);

	Vector<double> eul = RotMatrixElements2EulerYZX(m11, m12, m13, m21, m22, m31, m32, vct);

	return eul;
}



Vector<double>  jbxl::RotMatrix2EulerYXZ(Matrix<double> mtx, Vector<double>* vct)
{
	double m12 = mtx.element(1, 2);
	double m21 = mtx.element(2, 1);
	double m22 = mtx.element(2, 2);
	double m23 = mtx.element(2, 3);
	double m31 = mtx.element(3, 1);
	double m32 = mtx.element(3, 2);
	double m33 = mtx.element(3, 3);

	Vector<double> eul = RotMatrixElements2EulerYXZ(m12, m21, m22, m23, m31, m32, m33, vct);

	return eul;
}



Vector<double>  jbxl::RotMatrix2EulerZXY(Matrix<double> mtx, Vector<double>* vct)
{
	double m12 = mtx.element(1, 2);
	double m13 = mtx.element(1, 3);
	double m21 = mtx.element(2, 1);
	double m22 = mtx.element(2, 2);
	double m23 = mtx.element(2, 3);
	double m32 = mtx.element(3, 2);
	double m33 = mtx.element(3, 3);

	Vector<double> eul = RotMatrixElements2EulerZXY(m12, m13, m21, m22, m23, m32, m33, vct);

	return eul;
}




Vector<double>  jbxl::RotMatrixElements2EulerXYZ(double m11, double m12, double m13, double m21, double m31, double m32, double m33, Vector<double>* vct)
{
	Vector<double> eul(0.0, 0.0, 0.0, 0.0, -1.0);
	Vector<double> ev[2];
	double arctang;

	if (m31<-1.0 || m31>1.0) return eul;

	if (1.0-m31<Zero_Eps || 1.0+m31<Zero_Eps) {
		if (1.0-m31<Zero_Eps) {				// m31==1.0
			ev[0].y = ev[1].y = -PI_DIV2;
			ev[0].z = 0.0;
			ev[1].z = PI_DIV2;
			arctang = atan2(-m12, -m13);
			ev[0].x = arctang - ev[0].z;
			ev[1].x = arctang - ev[1].z;
		}
		else {								// m31==-1.0
			ev[0].y = ev[1].y = PI_DIV2;
			ev[0].z = 0.0;
			ev[1].z = PI_DIV2;
			arctang = atan2(m12, m13);
			ev[0].x = arctang + ev[0].z;
			ev[1].x = arctang + ev[1].z;
		}
	}

	else {
		ev[0].y = -asin(m31); 
		if (ev[0].y>=0.0) ev[1].y =  PI - ev[0].y;	// 別解
		else		  	  ev[1].y = -PI - ev[0].y;

		double cy1 = cos(ev[0].y);
		double cy2 = cos(ev[1].y);
		if (Xabs(cy1)<Zero_Eps || Xabs(cy2)<Zero_Eps) return eul;

		ev[0].x = atan2(m32/cy1, m33/cy1);
		ev[0].z = atan2(m21/cy1, m11/cy1);

		if (ev[0].x>=0.0) ev[1].x = ev[0].x - PI;
		else              ev[1].x = ev[0].x + PI;
		if (ev[0].z>=0.0) ev[1].z = ev[0].z - PI;
		else              ev[1].z = ev[0].z + PI;
	}

	//
	// XYZ
	if (vct!=NULL) {
		vct[0].set(ev[0].x, ev[0].y, ev[0].z);
		vct[1].set(ev[1].x, ev[1].y, ev[1].z);
	}
	eul.set(ev[0].x, ev[0].y, ev[0].z);

	return eul;
}



Vector<double>  jbxl::RotMatrixElements2EulerZYX(double m11, double m12, double m13, double m21, double m23, double m31, double m33, Vector<double>* vct)
{
	Vector<double> eul(0.0, 0.0, 0.0, 0.0, -1.0);
	Vector<double> ev[2];
	double arctang;

	if (m13<-1.0 || m13>1.0) return eul;

	if (1.0-m13<Zero_Eps || 1.0+m13<Zero_Eps) {	
		if (1.0-m13<Zero_Eps) {				// m13==1.0
			ev[0].y = ev[1].y = PI_DIV2;
			ev[0].z = 0.0;
			ev[1].z = PI_DIV2;
			arctang = atan2(m21, -m31);
			ev[0].x = arctang - ev[0].z;
			ev[1].x = arctang - ev[1].z;
		}
		else {								// m13==-1.0
			ev[0].y = ev[1].y = -PI_DIV2;
			ev[0].z = 0.0;
			ev[1].z = PI_DIV2;
			arctang = atan2(-m21, m31);
			ev[0].x = arctang + ev[0].z;
			ev[1].x = arctang + ev[1].z;
		}
	}

	else {
		ev[0].y = asin(m13); 
		if (ev[0].y>=0.0) ev[1].y =  PI - ev[0].y;
		else		      ev[1].y = -PI - ev[0].y;

		double cy1 = cos(ev[0].y);
		double cy2 = cos(ev[1].y);
		if (Xabs(cy1)<Zero_Eps || Xabs(cy2)<Zero_Eps) return eul;

		ev[0].x = atan2(-m23/cy1, m33/cy1);
		ev[0].z = atan2(-m12/cy1, m11/cy1);

		if (ev[0].x>=0.0) ev[1].x = ev[0].x - PI;
		else              ev[1].x = ev[0].x + PI;
		if (ev[0].z>=0.0) ev[1].z = ev[0].z - PI;
		else              ev[1].z = ev[0].z + PI;
	}

	//
	// ZYX
	if (vct!=NULL) {
		vct[0].set(ev[0].z, ev[0].y, ev[0].x);
		vct[1].set(ev[1].z, ev[1].y, ev[1].x);
	}
	eul.set(ev[0].z, ev[0].y, ev[0].x);

	return eul;
}



Vector<double>  jbxl::RotMatrixElements2EulerXZY(double m11, double m12, double m13, double m21, double m22, double m23, double m31, Vector<double>* vct)
{
	Vector<double> eul(0.0, 0.0, 0.0, 0.0, -1.0);
	Vector<double> ev[2];
	double arctang;

	if (m21<-1.0 || m21>1.0) return eul;

	if (1.0-m21<Zero_Eps || 1.0+m21<Zero_Eps) {	
		if (1.0-m21<Zero_Eps) {				// m21==1.0
			ev[0].z = ev[1].z = PI_DIV2;
			ev[0].y = 0.0;
			ev[1].y = PI_DIV2;
			arctang = atan2(m13, -m12);
			ev[0].x = arctang - ev[0].y;
			ev[1].x = arctang - ev[1].y;
		}
		else {								// m21==-1.0
			ev[0].z = ev[1].y = -PI_DIV2;
			ev[0].y = 0.0;
			ev[1].y = PI_DIV2;
			arctang = atan2(-m13, m12);
			ev[0].x = arctang + ev[0].y;
			ev[1].x = arctang + ev[1].y;
		}
	}

	else {
		ev[0].z = asin(m21); 
		if (ev[0].z>=0.0) ev[1].z =  PI - ev[0].z;
		else		      ev[1].z = -PI - ev[0].z;

		double cz1 = cos(ev[0].z);
		double cz2 = cos(ev[1].z);
		if (Xabs(cz1)<Zero_Eps || Xabs(cz2)<Zero_Eps) return eul;

		ev[0].x = atan2(-m23/cz1, m22/cz1);
		ev[0].y = atan2(-m31/cz1, m11/cz1);

		if (ev[0].x>=0.0) ev[1].x = ev[0].x - PI;
		else              ev[1].x = ev[0].x + PI;
		if (ev[0].y>=0.0) ev[1].y = ev[0].y - PI;
		else              ev[1].y = ev[0].y + PI;
	}

	//
	// XZY
	if (vct!=NULL) {
		vct[0].set(ev[0].x, ev[0].z, ev[0].y);
		vct[1].set(ev[1].x, ev[1].z, ev[1].y);
	}
	eul.set(ev[0].x, ev[0].z, ev[0].y);

	return eul;
}



Vector<double>  jbxl::RotMatrixElements2EulerYZX(double m11, double m12, double m13, double m21, double m22, double m31, double m32, Vector<double>* vct)
{
	Vector<double> eul(0.0, 0.0, 0.0, 0.0, -1.0);
	Vector<double> ev[2];
	double arctang;

	if (m12<-1.0 || m12>1.0) return eul;

	if (1.0-m12<Zero_Eps || 1.0+m12<Zero_Eps) {	
		if (1.0-m12<Zero_Eps) {				// m12==1.0
			ev[0].z = ev[1].z = -PI_DIV2;
			ev[0].y = 0.0;
			ev[1].y = PI_DIV2;
			arctang = atan2(-m31, -m21);
			ev[0].x = arctang - ev[0].y;
			ev[1].x = arctang - ev[1].y;
		}
		else {								// m12==-1.0
			ev[0].z = ev[1].y = PI_DIV2;
			ev[0].y = 0.0;
			ev[1].y = PI_DIV2;
			arctang = atan2(m31, m21);
			ev[0].x = arctang + ev[0].y;
			ev[1].x = arctang + ev[1].y;
		}
	}

	else {
		ev[0].z = -asin(m12); 
		if (ev[0].z>=0.0) ev[1].z =  PI - ev[0].z;
		else		      ev[1].z = -PI - ev[0].z;

		double cz1 = cos(ev[0].z);
		double cz2 = cos(ev[1].z);
		if (Xabs(cz1)<Zero_Eps || Xabs(cz2)<Zero_Eps) return eul;

		ev[0].x = atan2(m32/cz1, m22/cz1);
		ev[0].y = atan2(m13/cz1, m11/cz1);

		if (ev[0].x>=0.0) ev[1].x = ev[0].x - PI;
		else              ev[1].x = ev[0].x + PI;
		if (ev[0].y>=0.0) ev[1].y = ev[0].y - PI;
		else              ev[1].y = ev[0].y + PI;
	}

	//
	// YZX
	if (vct!=NULL) {
		vct[0].set(ev[0].y, ev[0].z, ev[0].x);
		vct[1].set(ev[1].y, ev[1].z, ev[1].x);
	}
	eul.set(ev[0].y, ev[0].z, ev[0].x);

	return eul;
}



Vector<double>  jbxl::RotMatrixElements2EulerYXZ(double m12, double m21, double m22, double m23, double m31, double m32, double m33, Vector<double>* vct)
{
	Vector<double> eul(0.0, 0.0, 0.0, 0.0, -1.0);
	Vector<double> ev[2];
	double arctang;

	if (m32<-1.0 || m32>1.0) return eul;

	if (1.0-m32<Zero_Eps || 1.0+m32<Zero_Eps) {	
		if (1.0-m32<Zero_Eps) {				// m32==1.0
			ev[0].x = ev[1].x = PI_DIV2;
			ev[0].z = 0.0;
			ev[1].z = PI_DIV2;
			arctang = atan2(m21, -m23);
			ev[0].y = arctang - ev[0].z;
			ev[1].y = arctang - ev[1].z;
		}
		else {								// m32==-1.0
			ev[0].x = ev[1].x = -PI_DIV2;
			ev[0].z = 0.0;
			ev[1].z = PI_DIV2;
			arctang = atan2(-m21, m23);
			ev[0].y = arctang + ev[0].z;
			ev[1].y = arctang + ev[1].z;
		}
	}

	else {
		ev[0].x = asin(m32); 
		if (ev[0].x>=0.0) ev[1].x =  PI - ev[0].x;
		else		      ev[1].x = -PI - ev[0].x;

		double cx1 = cos(ev[0].x);
		double cx2 = cos(ev[1].x);
		if (Xabs(cx1)<Zero_Eps || Xabs(cx2)<Zero_Eps) return eul;

		ev[0].y = atan2(-m31/cx1, m33/cx1);
		ev[0].z = atan2(-m12/cx1, m22/cx1);

		if (ev[0].y>=0.0) ev[1].y = ev[0].y - PI;
		else              ev[1].y = ev[0].y + PI;
		if (ev[0].z>=0.0) ev[1].z = ev[0].z - PI;
		else              ev[1].z = ev[0].z + PI;
	}

	//
	// YXZ
	if (vct!=NULL) {
		vct[0].set(ev[0].y, ev[0].x, ev[0].z);
		vct[1].set(ev[1].y, ev[1].x, ev[1].z);
	}
	eul.set(ev[0].y, ev[0].x, ev[0].z);

	return eul;
}



Vector<double>  jbxl::RotMatrixElements2EulerZXY(double m12, double m13, double m21, double m22, double m23, double m32, double m33, Vector<double>* vct)
{
	Vector<double> eul(0.0, 0.0, 0.0, 0.0, -1.0);
	Vector<double> ev[2];
	double arctang;

	if (m23<-1.0 || m23>1.0) return eul;

	if (1.0-m23<Zero_Eps || 1.0+m23<Zero_Eps) {	
		if (1.0-m23<Zero_Eps) {				// m23==1.0
			ev[0].x = ev[1].x = -PI_DIV2;
			ev[0].z = 0.0;
			ev[1].z = PI_DIV2;
			arctang = atan2(-m12, -m32);
			ev[0].y = arctang - ev[0].z;
			ev[1].y = arctang - ev[1].z;
		}
		else {								// m23==-1.0
			ev[0].x = ev[1].x = PI_DIV2;
			ev[0].z = 0.0;
			ev[1].z = PI_DIV2;
			arctang = atan2(m12, m32);
			ev[0].y = arctang + ev[0].z;
			ev[1].y = arctang + ev[1].z;
		}
	}

	else {
		ev[0].x = -asin(m23); 
		if (ev[0].x>=0.0) ev[1].x =  PI - ev[0].x;
		else		      ev[1].x = -PI - ev[0].x;

		double cx1 = cos(ev[0].x);
		double cx2 = cos(ev[1].x);
		if (Xabs(cx1)<Zero_Eps || Xabs(cx2)<Zero_Eps) return eul;

		ev[0].y = atan2(m13/cx1, m33/cx1);
		ev[0].z = atan2(m21/cx1, m22/cx1);

		if (ev[0].y>=0.0) ev[1].y = ev[0].y - PI;
		else              ev[1].y = ev[0].y + PI;
		if (ev[0].z>=0.0) ev[1].z = ev[0].z - PI;
		else              ev[1].z = ev[0].z + PI;
	}

	//
	// ZXY
	if (vct!=NULL) {
		vct[0].set(ev[0].z, ev[0].x, ev[0].y);
		vct[1].set(ev[1].z, ev[1].x, ev[1].y);
	}
	eul.set(ev[0].z, ev[0].x, ev[0].y);

	return eul;
}




//
// Quaternion -> Euler
//
Vector<double>  jbxl::Quaternion2EulerXYZ(Quaternion qut, Vector<double>* vct)
{
	Matrix<double> mtx = qut.getRotMatrix();
	Vector<double> eul = RotMatrix2EulerXYZ(mtx, vct);
	mtx.free();

	return eul;
}



Vector<double>  jbxl::Quaternion2EulerZYX(Quaternion qut, Vector<double>* vct)
{
	Matrix<double> mtx = qut.getRotMatrix();
	Vector<double> eul = RotMatrix2EulerZYX(mtx, vct);
	mtx.free();

	return eul;
}



Vector<double>  jbxl::Quaternion2EulerXZY(Quaternion qut, Vector<double>* vct)
{
	Matrix<double> mtx = qut.getRotMatrix();
	Vector<double> eul = RotMatrix2EulerXZY(mtx, vct);
	mtx.free();

	return eul;
}



Vector<double>  jbxl::Quaternion2EulerYZX(Quaternion qut, Vector<double>* vct)
{
	Matrix<double> mtx = qut.getRotMatrix();
	Vector<double> eul = RotMatrix2EulerYZX(mtx, vct);
	mtx.free();

	return eul;
}



Vector<double>  jbxl::Quaternion2EulerYXZ(Quaternion qut, Vector<double>* vct)
{
	Matrix<double> mtx = qut.getRotMatrix();
	Vector<double> eul = RotMatrix2EulerYXZ(mtx, vct);
	mtx.free();

	return eul;
}



Vector<double>  jbxl::Quaternion2EulerZXY(Quaternion qut, Vector<double>* vct)
{
	Matrix<double> mtx = qut.getRotMatrix();
	Vector<double> eul = RotMatrix2EulerZXY(mtx, vct);
	mtx.free();

	return eul;
}




//
// Rotation Matrix -> Quaternion
//
Quaternion  jbxl::RotMatrix2Quaternion(Matrix<double> mtx)
{
	Quaternion qut;
	Vector<double> xyz = RotMatrix2EulerXYZ(mtx, NULL);
	qut.setEulerXYZ(xyz);

	return qut;
}







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

Quaternion  jbxl::V2VQuaternion(Vector<double> a, Vector<double> b)
{
	Quaternion qut(1.0, 0.0, 0.0, 0.0, 1.0);

	a.normalize();
	b.normalize();
	if (a.n<Zero_Eps || b.n<Zero_Eps) return qut;
	
	Vector<double> nr = a^b;
	nr.normalize();

	double cs2 = (a*b)/2.0;			// θ/2 = 0〜π/2
	
	double sn;
	if (cs2>0.5) sn = 0.0;
	else         sn = sqrt(0.5 - cs2);

	if (Xabs(sn-1.0)<Zero_Eps) {
		Vector<double> c = a + b;
		if (c.norm()<1.0) {
			qut.setRotation(PI,  nr);
			if (nr.n<Zero_Eps) {
				qut.s = 0.0;
				qut.n = 0.0;
				qut.c = -1.0;
			}
		}
		else {
			qut.setRotation(0.0, nr);
		}
		return qut;
	}

	double cs;
	if (cs2<-0.5) cs = 0.0;
	else	      cs = sqrt(0.5 + cs2);

	//
	if (cs>1.0) qut.set(1.0, 0.0, 0.0, 0.0, 1.0, -1.0);
	else		qut.set(cs, nr.x*sn, nr.y*sn, nr.z*sn, 1.0, Min(a.c, b.c));
	
	return qut;
}



Quaternion  jbxl::PPPQuaternion(Vector<double> a, Vector<double> b, Vector<double> c)
{
	Quaternion qut(1.0, 0.0, 0.0, 0.0, 1.0);

	if (a.c<0.0 || b.c<0.0 || c.c<0.0) return qut;

	qut = V2VQuaternion(b-a, c-b);
	return qut;
}



Quaternion  jbxl::VPPQuaternion(Vector<double> a, Vector<double> b, Vector<double> c)
{
	Quaternion qut(1.0, 0.0, 0.0, 0.0, 1.0);

	if (a.c<0.0 || b.c<0.0 || c.c<0.0) return qut;

	qut = V2VQuaternion(a, c-b);
	return qut;
}



Quaternion  jbxl::PPVQuaternion(Vector<double> a, Vector<double> b, Vector<double> c)
{
	Quaternion qut(1.0, 0.0, 0.0, 0.0, 1.0);

	if (a.c<0.0 || b.c<0.0 || c.c<0.0) return qut;

	qut = V2VQuaternion(b-a, c);
	return qut;
}




/**
Quaternion  jbxl::SlerpQuaternion(Quaternion qa, Quaternion qb, double t)

クオータニオンの球面線形補間 (Sphercal Linear Interpolation)@n

回転軸が変化しない，または180度反転した状態での 180度以上の回転に対応．

@param   qa  始点のクオータニオン
@param   qb  終点のクオータニオン
@param   t   パラメータ 0〜1

@return  補間結果のクオータニオン

@par SLERP
q(t) = qa*sin((1-t)*th)/sin(th) + qb*sin(t*th)/sin(th)
*/
Quaternion  jbxl::SlerpQuaternion(Quaternion qa, Quaternion qb, double t)
{
	if (qa.n!=1.0) qa.normalize();
	if (qb.n!=1.0) qb.normalize();
	
	//
	double    dot;
	Quaternion qc;


	///////////////////////////////////////////
	/**/
	Vector<double> va = qa.getVector();
	Vector<double> vb = qb.getVector();
	va.normalize();
	vb.normalize();

	//
	dot = va*vb;
	if (dot>1.0)       dot =  1.0;
	else if (dot<-1.0) dot = -1.0;

	// 回転軸が変わらない場合
	if (dot>1.0-Sin_Tolerance) {
		double anga = qa.getMathAngle();
		double angb = qb.getMathAngle();
		double angd = angb - anga;
		if (angd<-PI)     angd += PI2; 
		else if (angd>PI) angd -= PI2;

		double angc = anga + t*angd;
		qc.setRotation(angc, va);
		qc.normalize();
		return qc;
	}

	// 回転軸が反転する場合
//	else if (dot<-1.0+Sin_Tolerance) {
	else if (dot<-0.99) {
		double anga = qa.getMathAngle();
		double angb = - qb.getMathAngle();
		double angd = angb - anga;
		if (angd<-PI)     angd += PI2; 
		else if (angd>PI) angd -= PI2;

		double angc = anga + t*angd;
		qc.setRotation(angc, va);
		qc.normalize();
		return qc;
	}
	/**/


	// SLERP
	dot = qa.x*qb.x + qa.y*qb.y + qa.z*qb.z + qa.s*qb.s;
	if (dot>1.0) dot =  1.0;
	else if (dot<0.0) {
		if (t<=0.5) return qa;
		else        return qb;
	}
	//if (dot<0.0) DEBUG_WARN("SlerpQuaternion: dot = %f", dot);

	//
	double th = acos(dot);
	double sn = sin(th);
	if (Xabs(sn)<Sin_Tolerance) return qa;

	//
	qc = qa*(sin((1.0-t)*th)/sn) + qb*(sin(t*th)/sn);
	qc.normalize();

	return qc;
}