QuatTest.h

#ifndef MANTID_TESTQUAT__
#define MANTID_TESTQUAT__

#include <cxxtest/TestSuite.h>
#include <cmath>
#include <ostream>
#include <float.h>
#include "MantidKernel/V3D.h"
#include "MantidKernel/Quat.h" 
#include "MantidKernel/Matrix.h"

using namespace Mantid::Kernel;

class QuatTest : public CxxTest::TestSuite
{
private:

  Mantid::Kernel::Quat q,p;

public:
  void testoperatorbracket()
  {
    p[0]=0;
    p[1]=1;
    p[2]=2;
    p[3]=3;
    TS_ASSERT_EQUALS(p[0],0.0);
    TS_ASSERT_EQUALS(p[1],1.0);
    TS_ASSERT_EQUALS(p[2],2.0);
    TS_ASSERT_EQUALS(p[3],3.0);
  }

  void testEmptyConstructor()
  {
    TS_ASSERT_EQUALS(q[0],1.0);
    TS_ASSERT_EQUALS(q[1],0.0);
    TS_ASSERT_EQUALS(q[2],0.0);
    TS_ASSERT_EQUALS(q[3],0.0);
  }

  void testValueConstructor()
  {
    Mantid::Kernel::Quat q1(1,2,3,4);
    TS_ASSERT_EQUALS(q1[0],1.0);
    TS_ASSERT_EQUALS(q1[1],2.0);
    TS_ASSERT_EQUALS(q1[2],3.0);
    TS_ASSERT_EQUALS(q1[3],4.0);
  }

  void testAngleAxisConstructor()
  {
    Mantid::Kernel::V3D v(1,1,1);
    // Construct quaternion to represent rotation
    // of 45 degrees around the 111 axis.
    Mantid::Kernel::Quat q1(90.0,v);
    double c=1.0/sqrt(2.0);
    double s=c/sqrt(3.0);
    TS_ASSERT_DELTA(q1[0],c,0.000001);
    TS_ASSERT_DELTA(q1[1],s,0.000001);
    TS_ASSERT_DELTA(q1[2],s,0.000001);
    TS_ASSERT_DELTA(q1[3],s,0.000001);
  }

  void testoperatorassignmentfromdouble()
  {
    q(2,3,4,5);
    TS_ASSERT_EQUALS(q[0],2.0);
    TS_ASSERT_EQUALS(q[1],3.0);
    TS_ASSERT_EQUALS(q[2],4.0);
    TS_ASSERT_EQUALS(q[3],5.0);
  }

  void testoperatorassignmentfromangleaxis()
  {
    Mantid::Kernel::V3D v(1,1,1);
    q(90.0,v);
    double c=1.0/sqrt(2.0);
    double s=c/sqrt(3.0);
    TS_ASSERT_DELTA(q[0],c,0.000001);
    TS_ASSERT_DELTA(q[1],s,0.000001);
    TS_ASSERT_DELTA(q[2],s,0.000001);
    TS_ASSERT_DELTA(q[3],s,0.000001);

    //Now rotate 45 degrees around y
    q(45, V3D(0,1,0));
    V3D X(1,0,0);
    q.rotate(X);
    double a = sqrt(2.0)/2;
    TS_ASSERT(X==V3D(a,0,-a));
    //Now rotate -45 degrees around y
    q(-45, V3D(0,1,0));
    X(1,0,0);
    q.rotate(X);
    TS_ASSERT(X==V3D(a,0,a));
  }

  void testoperatorequal()
  {
    q=p;
    TS_ASSERT_EQUALS(q[0],p[0]);
    TS_ASSERT_EQUALS(q[1],p[1]);
    TS_ASSERT_EQUALS(q[2],p[2]);
    TS_ASSERT_EQUALS(q[3],p[3]);
  }

  void testlenmethod()
  {
    q(1,2,3,4);
    TS_ASSERT_EQUALS(q.len(),sqrt(30.0));
  }

  void testlen2method()
  {
    q(1,2,3,4);
    TS_ASSERT_EQUALS(q.len2(),30.0);
  }

  void testinitmehtod()
  {
    q.init();
    TS_ASSERT_EQUALS(q[0],1);
    TS_ASSERT_EQUALS(q[1],0);
    TS_ASSERT_EQUALS(q[2],0);
    TS_ASSERT_EQUALS(q[3],0);
  }

  void testnormalizemethod()
  {
    q(2,2,2,2);
    q.normalize();
    TS_ASSERT_DELTA(q[0],0.5,0.000001);
    TS_ASSERT_DELTA(q[1],0.5,0.000001);
    TS_ASSERT_DELTA(q[2],0.5,0.000001);
    TS_ASSERT_DELTA(q[3],0.5,0.000001);
  }

  void testconjugatemethod()
  {
    q(1,1,1,1);
    q.conjugate();
    TS_ASSERT_EQUALS(q[0],1);
    TS_ASSERT_EQUALS(q[1],-1);
    TS_ASSERT_EQUALS(q[2],-1);
    TS_ASSERT_EQUALS(q[3],-1);
  }

  void testinversemethod()
  {
    q(2,3,4,5);
    Mantid::Kernel::Quat qinv(q);
    qinv.inverse();
    q*=qinv;
    TS_ASSERT_DELTA(q[0],1,0.000001);
    TS_ASSERT_DELTA(q[1],0,0.000001);
    TS_ASSERT_DELTA(q[2],0,0.000001);
    TS_ASSERT_DELTA(q[3],0,0.000001);
  }

  void testoperatorplus()
  {
    q(1,1,1,1);
    p(-1,2,1,3);
    Mantid::Kernel::Quat res;
    res=p+q;
    TS_ASSERT_EQUALS(res[0],0);
    TS_ASSERT_EQUALS(res[1],3);
    TS_ASSERT_EQUALS(res[2],2);
    TS_ASSERT_EQUALS(res[3],4);
  }

  void testoperatorminus()
  {
    q(1,1,1,1);
    p(-1,2,1,3);
    Mantid::Kernel::Quat res;
    res=p-q;
    TS_ASSERT_EQUALS(res[0],-2);
    TS_ASSERT_EQUALS(res[1],1);
    TS_ASSERT_EQUALS(res[2],0);
    TS_ASSERT_EQUALS(res[3],2);
  }

  void testoperatortimes()
  {
    q(1,1,1,1);
    p(-1,2,1,3);
    Mantid::Kernel::Quat res;
    res=p*q;
    TS_ASSERT_EQUALS(res[0],-7);
    TS_ASSERT_EQUALS(res[1],-1);
    TS_ASSERT_EQUALS(res[2],1);
    TS_ASSERT_EQUALS(res[3],3);
  }

  void testoperatordoublequal()
  {
    p=q;
    TS_ASSERT(p==q);
    q(1,4,5,6);
    TS_ASSERT(p!=q);
  }

  void testoperatornotequal()
  {
    q(1,2,3,4);
    TS_ASSERT(p!=q);
    p=q;
    TS_ASSERT(!(p!=q));
  }

  void testRotateVector()
  {
    double a = sqrt(2.0)/2;

    //Trivial
    p(1,0,0,0);//Identity quaternion
    V3D v(1,0,0);
    V3D orig_v = v;
    p.rotate(v);
    TS_ASSERT(orig_v==v);

    //Now do more angles
    v = V3D(1,0,0);
    p(90., V3D(0,1,0)); //90 degrees, right-handed, around y
    p.rotate(v);
    TS_ASSERT(v==V3D(0,0,-1));

    v = V3D(1,0,0);
    p(45., V3D(0,0,1));
    p.rotate(v);
    TS_ASSERT(v==V3D(a, a, 0));

    v = V3D(1,0,0);
    p(-45., V3D(0,0,1));
    p.rotate(v);
    TS_ASSERT(v==V3D(a, -a, 0));

    v = V3D(1,0,0);
    p(30., V3D(0,0,1));
    p.rotate(v);
    TS_ASSERT(v==V3D(sqrt(3.0)/2, 0.5, 0));

    v = V3D(1,0,0);
    p(125., V3D(1,0,0));
    p.rotate(v);
    TS_ASSERT(v==V3D(1,0,0));

    //90 deg around +Z
    p(90, V3D(0,0,1));
    v = V3D(1,0,0);    p.rotate(v);    TS_ASSERT(v==V3D(0,1,0));
    v = V3D(0,1,0);    p.rotate(v);    TS_ASSERT(v==V3D(-1,0,0));

    //std::cout << "Rotated v is" << v << "\n";
  }

 void testGetRotation()
  {
	V3D some(1,0.5,1);
	V3D target(1,2,-1);
	//V3D some(1,0,0);
	//V3D target(0,1,0);


    V3D rotAxis = some.cross_prod(target);
	rotAxis.normalize();

	double targ_norm=target.norm();
	double some_norm=some.norm();
	double cc = some.scalar_prod(target)/some_norm/targ_norm;
	double rotAngle = acos(cc)*180/M_PI;

	// rotator will be unit quaternion as it is build by the constructor this way;
	Quat rotator(rotAngle,rotAxis);	


	std::vector<double> rotMatrix;
    TSM_ASSERT_THROWS_NOTHING("The rotator quaternion has to be a unit quaternion",rotMatrix= rotator.getRotation(true));

	// Kroniker Deltas valid for valid rotational matrix; a_ij*a_jk=delta_jk 
	double cron00= rotMatrix[0]*rotMatrix[0]+rotMatrix[1]*rotMatrix[1]+rotMatrix[2]*rotMatrix[2];	TSM_ASSERT_DELTA("delta_00 should be 1",1.0,cron00,FLT_EPSILON);
	double cron11= rotMatrix[3]*rotMatrix[3]+rotMatrix[4]*rotMatrix[4]+rotMatrix[5]*rotMatrix[5];	TSM_ASSERT_DELTA("delta_11 should be 1",1.0,cron11,FLT_EPSILON);
	double cron22= rotMatrix[6]*rotMatrix[6]+rotMatrix[7]*rotMatrix[7]+rotMatrix[8]*rotMatrix[8];   TSM_ASSERT_DELTA("delta_22 should be 1",1.0,cron22,FLT_EPSILON);

	double cron01= rotMatrix[0]*rotMatrix[1]+rotMatrix[3]*rotMatrix[4]+rotMatrix[6]*rotMatrix[7];	TSM_ASSERT_DELTA("delta_01 should be 0",0.0,cron01,FLT_EPSILON);
	double cron02= rotMatrix[0]*rotMatrix[2]+rotMatrix[3]*rotMatrix[5]+rotMatrix[6]*rotMatrix[8];	TSM_ASSERT_DELTA("delta_02 should be 0",0.0,cron02,FLT_EPSILON);
	double cron12= rotMatrix[1]*rotMatrix[2]+rotMatrix[4]*rotMatrix[5]+rotMatrix[7]*rotMatrix[8];	TSM_ASSERT_DELTA("delta_12 should be 0",0.0,cron12,FLT_EPSILON);


	double det =  rotMatrix[0]*(rotMatrix[4]*rotMatrix[8]-rotMatrix[5]*rotMatrix[7])
	            + rotMatrix[1]*(rotMatrix[5]*rotMatrix[6]-rotMatrix[3]*rotMatrix[8])
		        + rotMatrix[2]*(rotMatrix[3]*rotMatrix[7]-rotMatrix[4]*rotMatrix[6]);

	TSM_ASSERT_DELTA("Determinant for the proper rotation matrix has to be equal to 1 ",1.0,det,FLT_EPSILON);

	double x1=(rotMatrix[0]*some.X()+rotMatrix[1]*some.Y()+rotMatrix[2]*some.Z())*targ_norm/some_norm;
	TSM_ASSERT_DELTA("X -coordinate obtained using the rotation matxis have to coinside with the one obtained by rotation via quat",x1,target.X(),FLT_EPSILON);

	double y1=(rotMatrix[3]*some.X()+rotMatrix[4]*some.Y()+rotMatrix[5]*some.Z())*targ_norm/some_norm;
	TSM_ASSERT_DELTA("Y -coordinate obtained using the rotation matxis have to coinside with the one obtained by rotation via quat",y1,target.Y(),FLT_EPSILON);

 	double z1=(rotMatrix[6]*some.X()+rotMatrix[7]*some.Y()+rotMatrix[8]*some.Z())*targ_norm/some_norm;
	TSM_ASSERT_DELTA("Z -coordinate obtained using the rotation matxis have to coinside with the one obtained by rotation via quat",z1,target.Z(),FLT_EPSILON);

	// if the vectors are not notmalized (not equal), the angle between the vectors calculated by the constructor below would not be equal to the one, calculated 
	// above. 
	some *=(targ_norm/some_norm);
 	Quat rot2(some,target);	
  
	std::vector<double> rotMatrix2;
	TSM_ASSERT_THROWS_NOTHING("The rotator quaternion has to be a unit quaternion",rotMatrix2	=rot2.getRotation(true));

	for(int i=0;i<9;i++){
		TSM_ASSERT_DELTA("Elements of the rotation matrix obtained quat on 2 vectors have to be equivalent",rotMatrix[i],rotMatrix2[i],FLT_EPSILON);
	}

	x1=(rotMatrix2[0]*some.X()+rotMatrix2[1]*some.Y()+rotMatrix2[2]*some.Z());
	TSM_ASSERT_DELTA("X -coordinate obtained using the rotation matxis have to coinside with the one obtained by rotation via quat",x1,target.X(),FLT_EPSILON);

	y1=(rotMatrix2[3]*some.X()+rotMatrix2[4]*some.Y()+rotMatrix2[5]*some.Z());
	TSM_ASSERT_DELTA("Y -coordinate obtained using the rotation matxis have to coinside with the one obtained by rotation via quat",y1,target.Y(),FLT_EPSILON);

 	z1=(rotMatrix2[6]*some.X()+rotMatrix2[7]*some.Y()+rotMatrix2[8]*some.Z());
	TSM_ASSERT_DELTA("Z -coordinate obtained using the rotation matxis have to coinside with the one obtained by rotation via quat",z1,target.Z(),FLT_EPSILON);


 }
 void testUnitQuatFromUnitRotMatrix(){
	 DblMatrix Rot(3,3);
	 Rot[0][0]=1;
	 Rot[1][1]=1;
	 Rot[2][2]=1;

	 Quat Test;
	 Test.setQuat(Rot);

	 std::vector<double> rez = Test.getRotation();
	 std::vector<double> rot = Rot.getVector();
	 TSM_ASSERT_EQUALS("This operation should return rotation matrix",rot,rez);

 }

 void testQuatFromRotMatrix(){
	 DblMatrix Rot(3,3);
	 int Nx(5),Ny(5),Nz(3);
	 double Phi=M_PI/2/Nx;
	 double Tht=M_PI/2/Ny;
	 double Psi=M_PI/2/Ny;
	 Quat Test;
	 std::vector<double> rez;
	 std::vector<double> rot;

	 for(int i=0;i<=Nx;i++){
		 double cT=cos(Tht*i);
		 double sT=sin(Tht*i);
		 for(int j=0;j<=Ny;j++){
			 double cF = cos(j*Phi);
			 double sF = sin(j*Phi);
			 for (int k=0;k<=Nz;k++){
				Rot.zeroMatrix();
				double cP = cos(k*Psi);
				double sP = sin(k*Psi);
	
				Rot[0][0]=cT*cP;   Rot[1][0]=-cF*sP+sF*sT*cP;    Rot[2][0]=sF*sP+cF*sT*cP;
		        Rot[0][1]=cT*sP;   Rot[1][1]= cF*cP+sF*sT*sP;    Rot[2][1]=-sF*cP+cF*sT*sP;
		        Rot[0][2]=-sT;     Rot[1][2]= sF*cT;             Rot[2][2]= cT*cF;
		

			   Test.setQuat(Rot);
			   rez = Test.getRotation();
		       rot = Rot.getVector();
			   for(int ii=0;ii<9;ii++){
					TSM_ASSERT_DELTA("This operation should return initial rotation matrix",rot[ii],rez[ii],1e-4);
			   }
			 }
		 }
	 }

 }

 void testSetFromDirectionCosineMatrix_trival()
  {
    Mantid::Kernel::V3D rX(1,0,0);
    Mantid::Kernel::V3D rY(0,1,0);
    Mantid::Kernel::V3D rZ(0,0,1);
    q(rX,rY,rZ);
    p(1,0,0,0); //Identity quaternion
    TS_ASSERT(p==q); //Trivial rotation
  }

  void testSetFromDirectionCosineMatrix2()
  {
    //Rotate 90 deg around Y
    V3D rX(0,0,-1);
    V3D rY(0,1,0);
    V3D rZ(1,0,0);
    q(rX,rY,rZ);
    p(90, V3D(0,1,0));
    TS_ASSERT(p==q);
  }

  void testSetFromDirectionCosineMatrix2b()
  {
    //Rotate -45 deg around Y
    double a = sqrt(2.0)/2;
    V3D rX(a,0,a);
    V3D rY(0,1,0);
    V3D rZ(-a,0,a);
    q(rX,rY,rZ);
    p(-45.0, V3D(0,1,0));
    TS_ASSERT(p==q);

    V3D oX(1,0,0);
    V3D oY(0,1,0);
    V3D oZ(0,0,1);
    q.rotate(oX);
    q.rotate(oY);
    q.rotate(oZ);
    TS_ASSERT(oX==rX);
    TS_ASSERT(oY==rY);
    TS_ASSERT(oZ==rZ);
  }

  void testSetFromDirectionCosineMatrix3()
  {
    //Rotate 90 deg around Z
    V3D rX(0,1,0);
    V3D rY(-1,0,0);
    V3D rZ(0,0,1);
    q(rX,rY,rZ);
    p(90, V3D(0,0,1));
    TS_ASSERT(p==q);
  }

  void testSetFromDirectionCosineMatrix4()
  {
    //Rotate 90 deg around X
    V3D rX(1,0,0);
    V3D rY(0,0,1);
    V3D rZ(0,-1,0);
    q(rX,rY,rZ);
    p(90, V3D(1,0,0));
    TS_ASSERT(p==q);
  }

  /** Test that the rotation matrix is not transposed or otherwise funny */
  void testGetRotation2()
  {
    V3D x(1,0,0);
    Quat rot(90.0, x);
    std::vector<double> matVec = rot.getRotation(true, true);
    Matrix<double> mat(matVec);

    V3D init(0,1,0);
    V3D final = mat * init;
    TS_ASSERT_DELTA(final.X(), 0.0, 1e-5);
    TS_ASSERT_DELTA(final.Y(), 0.0, 1e-5);
    TS_ASSERT_DELTA(final.Z(), 1.0, 1e-5);
  }

  void testGetEulerAngles1()
  {
    Quat X(120.0, V3D(1,0,0));
    Quat Y(-60.0, V3D(0,1,0));
    Quat Z(90.0, V3D(0,0,1));
    Quat rot = X * Y * Z;

    std::vector<double> angles = rot.getEulerAngles("XYZ");
    TS_ASSERT_DELTA(angles[0], 120.0, 1e-5);
    TS_ASSERT_DELTA(angles[1], -60.0, 1e-5);
    TS_ASSERT_DELTA(angles[2], 90.0, 1e-5);
  }

  void testGetEulerAngles2()
  {
    //Test using a different convention
    Quat X(0.0, V3D(1,0,0));
    Quat Y(15.0, V3D(0,1,0));
    Quat Z(75.0, V3D(0,0,1));
    Quat rot = Z * X * Y;

    std::vector<double> angles = rot.getEulerAngles("ZXY");
    TS_ASSERT_DELTA(angles[0], 75.0, 1e-5);
    TS_ASSERT_DELTA(angles[1], 0.0, 1e-5);
    TS_ASSERT_DELTA(angles[2], 15.0, 1e-5);
  }

  void testGetEulerAngles3()
  {
    //In some cases we don't get the same angles out as we put in.
    //That's okay though, so long as they represent the same rotation.
    Quat X(68.0, V3D(1,0,0));
    Quat Y(175.0, V3D(0,1,0));
    Quat Z(20.0, V3D(0,0,1));
    Quat rot = X * Y * Z;

    Quat X2(-112.0, V3D(1,0,0));
    Quat Y2(5.0, V3D(0,1,0));
    Quat Z2(-160.0, V3D(0,0,1));
    Quat rot2 = X * Y * Z;

    TS_ASSERT(rot == rot2);

    std::vector<double> angles = rot.getEulerAngles("XYZ");
    TS_ASSERT_DELTA(angles[0], -112.0, 1e-5);
    TS_ASSERT_DELTA(angles[1], 5.0, 1e-5);
    TS_ASSERT_DELTA(angles[2], -160.0, 1e-5);
  }

  void compareArbitrary(const Quat& rotQ)
  {
    V3D oX(1,0,0);
    V3D oY(0,1,0);
    V3D oZ(0,0,1);
    V3D rX = oX;
    V3D rY = oY;
    V3D rZ = oZ;

    //Rotate the reference frame
    rotQ.rotate(rX);
    rotQ.rotate(rY);
    rotQ.rotate(rZ);

    //Now find it.
    q(rX,rY,rZ);

    q.rotate(oX);
    q.rotate(oY);
    q.rotate(oZ);
    TS_ASSERT(oX==rX);
    TS_ASSERT(oY==rY);
    TS_ASSERT(oZ==rZ);
    TS_ASSERT(rotQ==q);

    //    std::cout << "\nRotated coordinates are " << rX << rY << rZ << "\n";
    //    std::cout << "Expected (p) is" << p << "; got " << q << "\n";
    //    std::cout << "Re-Rotated coordinates are " << oX << oY << oZ << "\n";
  }

  void testSetFromDirectionCosineMatrix_arbitrary()
  {
    Quat rotQ;
    //Try a couple of random rotations
    rotQ = Quat(124.0, V3D(0.1, 0.2, sqrt(0.95)));
    this->compareArbitrary(rotQ);
    rotQ = Quat(-546.0, V3D(-0.5, 0.5, sqrt(0.5)));
    this->compareArbitrary(rotQ);
    rotQ = Quat(34.0, V3D(-0.5, 0.5, sqrt(0.5))) * Quat(-25.0, V3D(0.1, 0.2, sqrt(0.95)));
    this->compareArbitrary(rotQ);
  }

  void testConstructorFromDirectionCosine()
  {
    double a = sqrt(2.0)/2;
    V3D rX(a,0,a);
    V3D rY(0,1,0);
    V3D rZ(-a,0,a);
    Quat rotQ = Quat(rX,rY,rZ);
    p(-45.0, V3D(0,1,0));
    TS_ASSERT(rotQ==p);
  }

  void test_toString()
  {
    Quat a(1,2,3,4);
    TS_ASSERT_EQUALS( a.toString(), "[1,2,3,4]");
    Quat b;
    b.fromString("[4,5,6,7]");
    TS_ASSERT_EQUALS( b, Quat(4,5,6,7) );
  }

};

#endif