/**
回転・クォータニオン ライブラリ  Quaternion.cpp

*/


#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();
	}
}



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



// 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.x, ex);
	qy.setRotation(e.y, ey);
	qz.setRotation(e.z, ez);

	(*this) = qz*qy*qx;
}



// 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;
	qx.setRotation(e.z, ex);
	qy.setRotation(e.y, ey);
	qz.setRotation(e.x, ez);

	(*this) = qx*qy*qz;
}



// 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.x, ex);
	qy.setRotation(e.z, ey);
	qz.setRotation(e.y, ez);

	(*this) = qy*qz*qx;
}



// 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;
	qx.setRotation(e.z, ex);
	qy.setRotation(e.x, ey);
	qz.setRotation(e.y, ez);

	(*this) = qx*qz*qy;
}



// 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;
	qx.setRotation(e.y, ex);
	qy.setRotation(e.x, ey);
	qz.setRotation(e.z, ez);

	(*this) = qz*qx*qy;
}



// 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;
	qx.setRotation(e.y, ex);
	qy.setRotation(e.z, ey);
	qz.setRotation(e.x, ez);

	(*this) = qy*qx*qz;
}




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

	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)
{
	Quaternion inv = ~(*this);
	Quaternion qut = ((*this)*(v*inv));
	Vector<double> vct = qut.getVector();

	return vct;
}





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

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

	double sinx = sin(eul.x);
	double cosx = cos(eul.x);
	double siny = sin(eul.y);
	double cosy = cos(eul.y);
	double sinz = sin(eul.z);
	double cosz = cos(eul.z);

	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 sinx = sin(eul.x);
	double cosx = cos(eul.x);
	double siny = sin(eul.y);
	double cosy = cos(eul.y);
	double sinz = sin(eul.z);
	double cosz = cos(eul.z);

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

	return mtx;
}




Vector<double>  jbxl::RotMatrix2EulerXYZ(Matrix<double> mtx, Vector<double>* vct)
{
	Vector<double> eul(0.0, 0.0, 0.0);
	Vector<double> ev[2];

	double m31 = mtx.element(3, 1);
	if (m31<-1.0 || m31>1.0) return eul;

	// m31==1.0 || m31==-1.0
	if (1.0-m31<Zero_Eps || 1.0+m31<Zero_Eps) {
		double m12 = mtx.element(1, 2);
		double m13 = mtx.element(1, 3);
	
		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;
			ev[0].x = atan2(-m12, -m13) - ev[0].z;
			ev[1].x = atan2(-m12, -m13) - 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;
			ev[0].x = atan2(m12, m13) + ev[0].z;
			ev[1].x = atan2(m12, m13) + ev[1].z;
		}
	}

	else {
		double m11 = mtx.element(1, 1);
		double m21 = mtx.element(2, 1);
		double m32 = mtx.element(3, 2);
		double m33 = mtx.element(3, 3);

		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[1].x = atan2(m32/cy2, m33/cy2);
		ev[0].z = atan2(m21/cy1, m11/cy1);
		ev[1].z = atan2(m21/cy2, m11/cy2);
	}

	//
	if (vct!=NULL) {
		vct[0] = ev[0];
		vct[1] = ev[1];
	}


	//
	double t0 = ev[0].norm2();
	double t1 = ev[1].norm2();
	//
	if (t0<=t1) eul = ev[0];
	else        eul = ev[1];

	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);
	Vector<double> ev[2];

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

	// m31==1.0 || m31==-1.0
	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;
			ev[0].x = atan2(-m12, -m13) - ev[0].z;
			ev[1].x = atan2(-m12, -m13) - 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;
			ev[0].x = atan2(m12, m13) + ev[0].z;
			ev[1].x = atan2(m12, m13) + 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[1].x = atan2(m32/cy2, m33/cy2);
		ev[0].z = atan2(m21/cy1, m11/cy1);
		ev[1].z = atan2(m21/cy2, m11/cy2);
	}

	//
	if (vct!=NULL) {
		vct[0] = ev[0];
		vct[1] = ev[1];
	}


	//
	double t0 = ev[0].norm2();
	double t1 = ev[1].norm2();
	//
	if (t0<=t1) eul = ev[0];
	else        eul = ev[1];

	return eul;
}





Vector<double>  jbxl::RotMatrix2EulerZYX(Matrix<double> mtx, Vector<double>* vct)
{
	Vector<double> eul(0.0, 0.0, 0.0);
	Vector<double> ev[2];

	double m13 = mtx.element(1, 3);
	if (m13<-1.0 || m13>1.0) return eul;

	// m13==1.0 || m13==-1.0
	if (1.0-m13<Zero_Eps || 1.0+m13<Zero_Eps) {
		double m21 = mtx.element(2, 1);
		double m31 = mtx.element(3, 1);
	
		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;
			ev[0].x = atan2(m21, -m31) - ev[0].z;
			ev[1].x = atan2(m21, -m31) - 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;
			ev[0].x = atan2(-m21, m31) + ev[0].z;
			ev[1].x = atan2(-m21, m31) + ev[1].z;
		}
	}

	else {
		double m11 = mtx.element(1, 1);
		double m12 = mtx.element(1, 2);
		double m23 = mtx.element(2, 3);
		double m33 = mtx.element(3, 3);

		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[1].x = atan2(-m23/cy2, m33/cy2);
		ev[0].z = atan2(-m12/cy1, m11/cy1);
		ev[1].z = atan2(-m12/cy2, m11/cy2);
	}

	//
	if (vct!=NULL) {
		vct[0] = ev[0];
		vct[1] = ev[1];
	}


	//
	double t0 = ev[0].norm2();
	double t1 = ev[1].norm2();
	//
	if (t0<=t1) eul = ev[0];
	else        eul = ev[1];

	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);
	Vector<double> ev[2];

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

	// m13==1.0 || m13==-1.0
	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;
			ev[0].x = atan2(m21, -m31) - ev[0].z;
			ev[1].x = atan2(m21, -m31) - 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;
			ev[0].x = atan2(-m21, m31) + ev[0].z;
			ev[1].x = atan2(-m21, m31) + 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[1].x = atan2(-m23/cy2, m33/cy2);
		ev[0].z = atan2(-m12/cy1, m11/cy1);
		ev[1].z = atan2(-m12/cy2, m11/cy2);
	}

	//
	if (vct!=NULL) {
		vct[0] = ev[0];
		vct[1] = ev[1];
	}


	//
	double t0 = ev[0].norm2();
	double t1 = ev[1].norm2();
	//
	if (t0<=t1) eul = ev[0];
	else        eul = ev[1];

	return eul;
}




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




/////////////////////////////////////////////////////////////////////////////////////////////////////////////
// 
// ベクトルの回転
//

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;
	//Vector<double> zv(0.0, 0.0, 0.0, 0.0, 1.0);
	//Vector<double> ab = a + b;
	//Vector<double> nr = NewellMethod<double>(zv, a, b);
	nr.normalize();
	//if (nr.n<Zero_Eps) return qut;

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

	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)
{
	if (qa.n!=1.0) qa.normalize();
	if (qb.n!=1.0) qb.normalize();
	
	//
	Quaternion qc;
	Vector<double> va = qa.getVector();
	Vector<double> vb = qb.getVector();
	va.normalize();
	vb.normalize();

	//
	double 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();
	}
	//
	else 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;
	}
	

	// 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<-1.0) dot = -1.0;

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