64 lines
1.3 KiB
C++
64 lines
1.3 KiB
C++
#include <cstdlib>
|
|
#include <iostream>
|
|
#include <string>
|
|
#include <wrmath/math.h>
|
|
#include <wrmath/geom/quaternion.h>
|
|
|
|
using namespace std;
|
|
using namespace wr;
|
|
|
|
|
|
static void
|
|
usage(ostream& outs)
|
|
{
|
|
outs << "Print conversions between Euler angles and quaternions." << endl;
|
|
outs << "Usage: euler2quat yaw pitch roll" << endl;
|
|
outs << " euler2quat x y z w" << endl;
|
|
}
|
|
|
|
|
|
static void
|
|
convertEulerToQuat(char **argv)
|
|
{
|
|
double yaw = math::DegreesToRadiansD(stod(string(argv[0])));
|
|
double pitch = math::DegreesToRadiansD(stod(string(argv[1])));
|
|
double roll = math::DegreesToRadiansD(stod(string(argv[2])));
|
|
|
|
geom::Vector3d euler {yaw, pitch, roll};
|
|
auto quaternion = geom::quaterniond_from_euler(euler);
|
|
|
|
cout << "Quaternion: " << quaternion.asVector() << endl;
|
|
}
|
|
|
|
|
|
static void
|
|
convertQuatToEuler(char **argv)
|
|
{
|
|
double x = stod(string(argv[1]));
|
|
double y = stod(string(argv[2]));
|
|
double z = stod(string(argv[3]));
|
|
double w = stod(string(argv[0]));
|
|
|
|
geom::Quaterniond quaternion {w, x, y, z};
|
|
auto euler = quaternion.euler() * (180.0 / M_PI);
|
|
|
|
cout << "Euler ZYX: " << euler << endl;
|
|
}
|
|
|
|
|
|
int
|
|
main(int argc, char **argv)
|
|
{
|
|
if ((argc != 4) && (argc != 5)) {
|
|
usage(cerr);
|
|
return EXIT_FAILURE;
|
|
}
|
|
|
|
argv++;
|
|
if (argc == 4) {
|
|
convertEulerToQuat(argv);
|
|
}
|
|
else {
|
|
convertQuatToEuler(argv);
|
|
}
|
|
} |