Example for rotation matrix usage and conversion.
#include <Base/Geometry/RMatrixBase.hh>
#include <Base/Geometry/Quaternion.hh>
#ifdef WIN32
# ifndef strcasecmp
# define strcasecmp _strcmpi
# endif
#endif
#include <string.h>
#include <stdio.h>
#include <iostream>
#define ROT_PARAM_Invalid -1
#define ROT_PARAM_Rodrigues 0
#define ROT_PARAM_Euler 1
#define ROT_PARAM_AxisAngle 2
#define ROT_PARAM_Quaternion 3
#define ROT_ORDER_XYZ 0
#define ROT_ORDER_XZY 1
#define ROT_ORDER_YXZ 2
#define ROT_ORDER_YZX 3
#define ROT_ORDER_ZXY 4
#define ROT_ORDER_ZYX 5
using namespace BIAS;
using namespace std;
int main(int argc, char *argv[])
{
int res = 0;
bool showUsage = (argc < 5);
int paramType = ROT_PARAM_Invalid, angleOrder = ROT_ORDER_XYZ;
if (argc > 1) {
if (strcasecmp(argv[1], "Rodrigues") == 0)
paramType = ROT_PARAM_Rodrigues;
else if (strcasecmp(argv[1], "Euler") == 0)
paramType = ROT_PARAM_Euler;
else if (strcasecmp(argv[1], "EulerXYZ") == 0) {
paramType = ROT_PARAM_Euler; angleOrder = ROT_ORDER_XYZ;
} else if (strcasecmp(argv[1], "EulerXZY") == 0) {
paramType = ROT_PARAM_Euler; angleOrder = ROT_ORDER_XZY;
} else if (strcasecmp(argv[1], "EulerYXZ") == 0) {
paramType = ROT_PARAM_Euler; angleOrder = ROT_ORDER_YXZ;
} else if (strcasecmp(argv[1], "EulerYZX") == 0) {
paramType = ROT_PARAM_Euler; angleOrder = ROT_ORDER_YZX;
} else if (strcasecmp(argv[1], "EulerZXY") == 0) {
paramType = ROT_PARAM_Euler; angleOrder = ROT_ORDER_ZXY;
} else if (strcasecmp(argv[1], "EulerZYX") == 0) {
paramType = ROT_PARAM_Euler; angleOrder = ROT_ORDER_ZYX;
}
else if (strcasecmp(argv[1], "AxisAngle") == 0)
paramType = ROT_PARAM_AxisAngle;
else if (strcasecmp(argv[1], "Quaternion") == 0)
paramType = ROT_PARAM_Quaternion;
}
if (paramType == ROT_PARAM_Invalid)
showUsage = true;
else if ((paramType == ROT_PARAM_AxisAngle ||
paramType == ROT_PARAM_Quaternion) && argc < 6)
showUsage = true;
if (showUsage) {
cout << "USAGE : ExampleRMatrix Rodrigues <wx> <wy> <wz> [<degree?>]" << endl
<< " or ExampleRMatrix Euler[<order>] <ax> <ay> <az> [<degree?>]" << endl
<< " or ExampleRMatrix AxisAngle <rx> <ry> <rz> <a> [<degree?>]" << endl
<< " or ExampleRMatrix Quaternion <qx> <qy> <qz> <qw>" << endl << endl
<< "Note that rotation angles and the length of the Rodrigues vector" << endl
<< "are given in radians by default. Euler must be followed by order" << endl
<< "of rotation angles (e.g. EulerYZX), otherwise order XYZ is used." << endl
<< "Use parameter degree = 1 to provide angles in degree instead." << endl << endl;
return 0;
}
bool useDegree = false;
if (argc > ((paramType > 1) ? 6 : 5))
useDegree = (atoi(argv[(paramType > 1) ? 6 : 5]) != 0);
double factor = useDegree ? (M_PI / 180.0) : 1.0;
switch (paramType)
{
case ROT_PARAM_Rodrigues:
{
break;
}
case ROT_PARAM_Euler:
{
double ax = atof(argv[2]) * factor;
double ay = atof(argv[3]) * factor;
double az = atof(argv[4]) * factor;
switch (angleOrder)
{
case ROT_ORDER_XYZ: R.
SetXYZ(ax, ay, az);
break;
case ROT_ORDER_XZY: R.
SetXZY(ax, ay, az);
break;
case ROT_ORDER_YXZ: R.
SetYXZ(ax, ay, az);
break;
case ROT_ORDER_YZX: R.
SetYZX(ax, ay, az);
break;
case ROT_ORDER_ZXY: R.
SetZXY(ax, ay, az);
break;
case ROT_ORDER_ZYX: R.
SetZYX(ax, ay, az);
break;
default:
cerr << "ERROR : Invalid Euler angle order!" << endl;
res = -1;
}
break;
}
case ROT_PARAM_AxisAngle:
{
double a = atof(argv[5]) * factor;
break;
}
case ROT_PARAM_Quaternion:
{
break;
}
default:
cerr << "ERROR : Invalid rotation parameter type!" << endl;
res = -2;
}
if (res < 0) return res;
cout << "Rotation matrix is " << R << endl;
double ax, ay, az;
cout << "Rotation angles in order XYZ are ax = " << ax * 180 / M_PI << ", ay = "
<< ay * 180 / M_PI << ", az = " << az * 180 / M_PI << " degree" << endl;
} else {
cerr << "ERROR : Failed to get rotation angles in order XYZ!" << endl;
res = -3;
}
cout << "Rotation angles in order ZYX are ax = " << ax * 180 / M_PI << ", ay = "
<< ay * 180 / M_PI << ", az = " << az * 180 / M_PI << " degree" << endl;
} else {
cerr << "ERROR : Failed to get rotation angles in order ZYX!" << endl;
res = -4;
}
cout << "Rotation angles in order YXZ are ax = " << ax * 180 / M_PI << ", ay = "
<< ay * 180 / M_PI << ", az = " << az * 180 / M_PI << " degree" << endl;
} else {
cerr << "ERROR : Failed to get rotation angles in order YXZ!" << endl;
res = -5;
}
cout << "Rotation angles in order ZXY are ax = " << ax * 180 / M_PI << ", ay = "
<< ay * 180 / M_PI << ", az = " << az * 180 / M_PI << " degree" << endl;
} else {
cerr << "ERROR : Failed to get rotation angles in order ZXY!" << endl;
res = -6;
}
cout << "Rotation angles in order YZX are ax = " << ax * 180 / M_PI << ", ay = "
<< ay * 180 / M_PI << ", az = " << az * 180 / M_PI << " degree" << endl;
} else {
cerr << "ERROR : Failed to get rotation angles in order YZX!" << endl;
res = -7;
}
cout << "Rotation angles in order XZY are ax = " << ax * 180 / M_PI << ", ay = "
<< ay * 180 / M_PI << ", az = " << az * 180 / M_PI << " degree" << endl;
} else {
cerr << "ERROR : Failed to get rotation angles in order XZY!" << endl;
res = -8;
}
cout << "Unit quaternion is " << q << endl;
} else {
cerr << "ERROR : Failed to get unit quaternion!" << endl;
res = -9;
}
double a;
cout << "Rotation axis is " << r << endl
<< "Rotation angle is " << a * 180.0 / M_PI << " degree" << endl;
} else {
cerr << "ERROR : Failed to get rotation axis/angle!" << endl;
res = -10;
}
cout << "Rodrigues vector is " << w << endl;
} else {
cerr << "ERROR : Failed to get Rodrigues vector!" << endl;
res = -11;
}
return res;
}