#include "toolFunction.h"
void main()
{
cout<<"=====welcome to study the expressions of rotation in 3D space====="<<endl;
double phi, omiga, kappa;
phi = -0.32148229302237 ; omiga = 1.726224465189316 ; kappa = -1.4092143985971; //corresponding to Y, X and Z axis
EulerAngle eulerAngle;
eulerAngle.phi = phi;
eulerAngle.omiga = omiga;
eulerAngle.kappa = kappa;
printEulerAngle(eulerAngle);
Rmatrix rotateMatrix;
Quaternion quaternion;
cout<<"### X-Y-Z axis rotated to Rmatrix ###"<<endl;
EulerAngle2Rmatrix_X(eulerAngle, rotateMatrix);
printRmatrix(rotateMatrix);
cout<<"### Rmatrix to EulerAngle(X-Y-Z) ###"<<endl;
Rmatrix2EulerAngle_X(rotateMatrix, eulerAngle);
printEulerAngle(eulerAngle);
cout<<"### Rmatrix to Quaternion ###"<<endl;
Rmatrix2Quaternion(rotateMatrix, quaternion);
printQuaternion(quaternion);
cout<<"### Quaternion to Rmatrix ###"<<endl;
Quaternion2Rmatrix(quaternion, rotateMatrix);
printRmatrix(rotateMatrix);
cout<<"### Quaternion to EulerAngle(X-Y-Z) ###"<<endl;
Quaternion2eulerAngle_Y(quaternion, eulerAngle);
printEulerAngle(eulerAngle);
}