#include <iostream>
#include <stdio.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
#define PI (3.1415926535897932346f)
::Eigen::Quaterniond Euler_to_Quaterniond(double yaw, double pitching, double droll)
{
::Eigen::Vector3d ea0(yaw,pitching,droll);
::Eigen::Matrix3d R;
R = ::Eigen::AngleAxisd(ea0[0], ::Eigen::Vector3d::UnitZ())
* ::Eigen::AngleAxisd(ea0[1], ::Eigen::Vector3d::UnitY())
* ::Eigen::AngleAxisd(ea0[2], ::Eigen::Vector3d::UnitX());
//cout << R << endl << endl;
//RotationMatrix to Quaterniond
::Eigen::Quaterniond q;
q = R;
cout <<"x:"<<q.x()<<"\t"<<"y:"<<q.y()<<"\t"<<"z:"<<q.z()<<"\t"<<"w:"<<q.w()<<endl;
return q;
}
::Eigen::Vector3d Quaterniond_to_Euler(double x, double y, double z, double w)
{
::Eigen::Quaterniond q(w,x,y,z);
::Eigen::Matrix3d Rx = q.toRotationMatrix();
::Eigen::Vector3d ea1 = Rx.eulerAngles(2,1,0);
cout <<ea1[0]<<" "<<ea1[1]<<" "<<ea1[2]<<endl;
return ea1;
}
int main(int argc, char **argv)
{
double yaw = PI/3,pitching = PI/4,droll = PI/6;
//double yaw = PI,pitching = PI,droll = PI;
printf("T yaw[%f] pitching[%f] droll[%f]\n",yaw,pitching,droll);
::Eigen::Quaterniond q = Euler_to_Quaterniond(yaw, pitching, droll);
::Eigen::Vector3d V = Quaterniond_to_Euler(q.x(), q.y(), q.z(), q.w());
//cout << V/PI*180 << endl;
#if 0
printf("T yaw[%f] pitch[%f] droll[%f]\n", yaw, pitching, droll);
//EulerAngles to RotationMatrix
::Eigen::Vector3d ea0(yaw,pitching,droll);
::Eigen::Matrix3d R;
R = ::Eigen::AngleAxisd(ea0[0], ::Eigen::Vector3d::UnitZ())
* ::Eigen::AngleAxisd(ea0[1], ::Eigen::Vector3d::UnitY())
* ::Eigen::AngleAxisd(ea0[2], ::Eigen::Vector3d::UnitX());
cout << R << endl << endl;
//RotationMatrix to Quaterniond
::Eigen::Quaterniond q;
q = R;
cout << q.x() << endl << endl;
cout << q.y() << endl << endl;
cout << q.z() << endl << endl;
cout << q.w() << endl << endl;
::Eigen::Quaterniond q1(q.w(),q.x(),q.y(),q.z());
//Quaterniond to RotationMatrix
::Eigen::Matrix3d Rx = q1.toRotationMatrix();
cout << Rx << endl << endl;
//RotationMatrix to EulerAngles
::Eigen::Vector3d ea1 = Rx.eulerAngles(2,1,0);
//cout << ea1/PI*180 << endl << endl;
cout << ea1 << endl << endl;
std::cin.ignore();
#endif
return 0;
}
