wrmath/tools/euler2quat.cc

64 lines
1.3 KiB
C++
Raw Normal View History

2019-08-06 07:46:02 +00:00
#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)
{
2019-08-07 05:49:20 +00:00
double x = stod(string(argv[1]));
double y = stod(string(argv[2]));
double z = stod(string(argv[3]));
double w = stod(string(argv[0]));
2019-08-06 07:46:02 +00:00
2019-08-07 05:49:20 +00:00
geom::Quaterniond quaternion {w, x, y, z};
2019-08-06 07:46:02 +00:00
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);
}
}