kindred/create
1
1
Fork 0
Code Issues 53 Pull Requests Actions Packages Projects 9 Releases 2 Wiki Activity

Base: add new API Rotation::get/setEulerAngles()

Browse Source 
Exposed to Python as new constructor parameters and
Rotation.toEulerAngles()

This function uses the code from OCCT
gp_Quaternion::Get/SetEulerAngles() to support all 24 possible
generalized euler rotation sequences. Call Rotation.toEulerAngles()
without argument to obtain all possible sequence types.
This commit is contained in:
Zheng, Lei
2021-03-07 18:02:16 +08:00
parent ccc4151b30
commit c1454dfbed
4 changed files with 365 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

269
src/Base/Rotation.cpp
View File

@@ -27,6 +27,7 @@
# include <climits>
#endif
#include <boost/algorithm/string/predicate.hpp>
#include "Rotation.h"
#include "Matrix.h"
#include "Base/Exception.h"
@@ -712,3 +713,271 @@ bool Rotation::isNull() const
this->quat[2] == 0.0 &&
this->quat[3] == 0.0);
}
//=======================================================================
// The following code is borrowed from OCCT gp/gp_Quaternion.cxx
namespace { // anonymous namespace
//=======================================================================
//function : translateEulerSequence
//purpose :
// Code supporting conversion between quaternion and generalized
// Euler angles (sequence of three rotations) is based on
// algorithm by Ken Shoemake, published in Graphics Gems IV, p. 222-22
// http://tog.acm.org/resources/GraphicsGems/gemsiv/euler_angle/EulerAngles.c
//=======================================================================
struct EulerSequence_Parameters
{
int i; // first rotation axis
int j; // next axis of rotation
int k; // third axis
bool isOdd; // true if order of two first rotation axes is odd permutation, e.g. XZ
bool isTwoAxes; // true if third rotation is about the same axis as first
bool isExtrinsic; // true if rotations are made around fixed axes
EulerSequence_Parameters (int theAx1,
bool theisOdd,
bool theisTwoAxes,
bool theisExtrinsic)
: i(theAx1),
j(1 + (theAx1 + (theisOdd ? 1 : 0)) % 3),
k(1 + (theAx1 + (theisOdd ? 0 : 1)) % 3),
isOdd(theisOdd),
isTwoAxes(theisTwoAxes),
isExtrinsic(theisExtrinsic)
{}
};
EulerSequence_Parameters translateEulerSequence (const Rotation::EulerSequence theSeq)
{
typedef EulerSequence_Parameters Params;
const bool F = false;
const bool T = true;
switch (theSeq)
{
case Rotation::Extrinsic_XYZ: return Params (1, F, F, T);
case Rotation::Extrinsic_XZY: return Params (1, T, F, T);
case Rotation::Extrinsic_YZX: return Params (2, F, F, T);
case Rotation::Extrinsic_YXZ: return Params (2, T, F, T);
case Rotation::Extrinsic_ZXY: return Params (3, F, F, T);
case Rotation::Extrinsic_ZYX: return Params (3, T, F, T);
// Conversion of intrinsic angles is made by the same code as for extrinsic,
// using equivalence rule: intrinsic rotation is equivalent to extrinsic
// rotation by the same angles but with inverted order of elemental rotations.
// Swapping of angles (Alpha <-> Gamma) is done inside conversion procedure;
// sequence of axes is inverted by setting appropriate parameters here.
// Note that proper Euler angles (last block below) are symmetric for sequence of axes.
case Rotation::Intrinsic_XYZ: return Params (3, T, F, F);
case Rotation::Intrinsic_XZY: return Params (2, F, F, F);
case Rotation::Intrinsic_YZX: return Params (1, T, F, F);
case Rotation::Intrinsic_YXZ: return Params (3, F, F, F);
case Rotation::Intrinsic_ZXY: return Params (2, T, F, F);
case Rotation::Intrinsic_ZYX: return Params (1, F, F, F);
case Rotation::Extrinsic_XYX: return Params (1, F, T, T);
case Rotation::Extrinsic_XZX: return Params (1, T, T, T);
case Rotation::Extrinsic_YZY: return Params (2, F, T, T);
case Rotation::Extrinsic_YXY: return Params (2, T, T, T);
case Rotation::Extrinsic_ZXZ: return Params (3, F, T, T);
case Rotation::Extrinsic_ZYZ: return Params (3, T, T, T);
case Rotation::Intrinsic_XYX: return Params (1, F, T, F);
case Rotation::Intrinsic_XZX: return Params (1, T, T, F);
case Rotation::Intrinsic_YZY: return Params (2, F, T, F);
case Rotation::Intrinsic_YXY: return Params (2, T, T, F);
case Rotation::Intrinsic_ZXZ: return Params (3, F, T, F);
case Rotation::Intrinsic_ZYZ: return Params (3, T, T, F);
default:
case Rotation::EulerAngles : return Params (3, F, T, F); // = Intrinsic_ZXZ
case Rotation::YawPitchRoll: return Params (1, F, F, F); // = Intrinsic_ZYX
};
}
class Mat : public Base::Matrix4D
{
public:
double operator()(int i, int j) const {
return this->operator[](i-1)[j-1];
}
double & operator()(int i, int j) {
return this->operator[](i-1)[j-1];
}
};
const char *EulerSequenceNames[] = {
//! Classic Euler angles, alias to Intrinsic_ZXZ
"Euler",
//! Yaw Pitch Roll (or nautical) angles, alias to Intrinsic_ZYX
"YawPitchRoll",
// Tait-Bryan angles (using three different axes)
"XYZ",
"XZY",
"YZX",
"YXZ",
"ZXY",
"ZYX",
"IXYZ",
"IXZY",
"IYZX",
"IYXZ",
"IZXY",
"IZYX",
// Proper Euler angles (using two different axes, first and third the same)
"XYX",
"XZX",
"YZY",
"YXY",
"ZYZ",
"ZXZ",
"IXYX",
"IXZX",
"IYZY",
"IYXY",
"IZXZ",
"IZYZ",
};
} // anonymous namespace
const char * Rotation::eulerSequenceName(EulerSequence seq)
{
if (seq == Invalid || seq >= EulerSequenceLast)
return 0;
return EulerSequenceNames[seq-1];
}
Rotation::EulerSequence Rotation::eulerSequenceFromName(const char *name)
{
if (name) {
for (unsigned i=0; i<sizeof(EulerSequenceNames)/sizeof(EulerSequenceNames[0]); ++i) {
if (boost::iequals(name, EulerSequenceNames[i]))
return (EulerSequence)(i+1);
}
}
return Invalid;
}
void Rotation::setEulerAngles(EulerSequence theOrder,
double theAlpha,
double theBeta,
double theGamma)
{
if (theOrder == Invalid || theOrder >= EulerSequenceLast)
throw Base::ValueError("invalid euler sequence");
EulerSequence_Parameters o = translateEulerSequence (theOrder);
theAlpha *= D_PI/180.0;
theBeta *= D_PI/180.0;
theGamma *= D_PI/180.0;
double a = theAlpha, b = theBeta, c = theGamma;
if ( ! o.isExtrinsic )
std::swap(a, c);
if ( o.isOdd )
b = -b;
double ti = 0.5 * a;
double tj = 0.5 * b;
double th = 0.5 * c;
double ci = cos (ti);
double cj = cos (tj);
double ch = cos (th);
double si = sin (ti);
double sj = sin (tj);
double sh = sin (th);
double cc = ci * ch;
double cs = ci * sh;
double sc = si * ch;
double ss = si * sh;
double values[4]; // w, x, y, z
if ( o.isTwoAxes )
{
values[o.i] = cj * (cs + sc);
values[o.j] = sj * (cc + ss);
values[o.k] = sj * (cs - sc);
values[0] = cj * (cc - ss);
}
else
{
values[o.i] = cj * sc - sj * cs;
values[o.j] = cj * ss + sj * cc;
values[o.k] = cj * cs - sj * sc;
values[0] = cj * cc + sj * ss;
}
if ( o.isOdd )
values[o.j] = -values[o.j];
quat[0] = values[1];
quat[1] = values[2];
quat[2] = values[3];
quat[3] = values[0];
}
void Rotation::getEulerAngles(EulerSequence theOrder,
double& theAlpha,
double& theBeta,
double& theGamma) const
{
Mat M;
getValue(M);
EulerSequence_Parameters o = translateEulerSequence (theOrder);
if ( o.isTwoAxes )
{
double sy = sqrt (M(o.i, o.j) * M(o.i, o.j) + M(o.i, o.k) * M(o.i, o.k));
if (sy > 16 * DBL_EPSILON)
{
theAlpha = atan2 (M(o.i, o.j), M(o.i, o.k));
theGamma = atan2 (M(o.j, o.i), -M(o.k, o.i));
}
else
{
theAlpha = atan2 (-M(o.j, o.k), M(o.j, o.j));
theGamma = 0.;
}
theBeta = atan2 (sy, M(o.i, o.i));
}
else
{
double cy = sqrt (M(o.i, o.i) * M(o.i, o.i) + M(o.j, o.i) * M(o.j, o.i));
if (cy > 16 * DBL_EPSILON)
{
theAlpha = atan2 (M(o.k, o.j), M(o.k, o.k));
theGamma = atan2 (M(o.j, o.i), M(o.i, o.i));
}
else
{
theAlpha = atan2 (-M(o.j, o.k), M(o.j, o.j));
theGamma = 0.;
}
theBeta = atan2 (-M(o.k, o.i), cy);
}
if ( o.isOdd )
{
theAlpha = -theAlpha;
theBeta = -theBeta;
theGamma = -theGamma;
}
if ( ! o.isExtrinsic )
{
double aFirst = theAlpha;
theAlpha = theGamma;
theGamma = aFirst;
}
theAlpha *= 180.0/D_PI;
theBeta *= 180.0/D_PI;
theGamma *= 180.0/D_PI;
}

47
src/Base/Rotation.h
View File

@@ -63,6 +63,53 @@ public:
void setYawPitchRoll(double y, double p, double r);
/// Euler angles in yaw,pitch,roll notation
void getYawPitchRoll(double& y, double& p, double& r) const;
enum EulerSequence
{
Invalid,
//! Classic Euler angles, alias to Intrinsic_ZXZ
EulerAngles,
//! Yaw Pitch Roll (or nautical) angles, alias to Intrinsic_ZYX
YawPitchRoll,
// Tait-Bryan angles (using three different axes)
Extrinsic_XYZ,
Extrinsic_XZY,
Extrinsic_YZX,
Extrinsic_YXZ,
Extrinsic_ZXY,
Extrinsic_ZYX,
Intrinsic_XYZ,
Intrinsic_XZY,
Intrinsic_YZX,
Intrinsic_YXZ,
Intrinsic_ZXY,
Intrinsic_ZYX,
// Proper Euler angles (using two different axes, first and third the same)
Extrinsic_XYX,
Extrinsic_XZX,
Extrinsic_YZY,
Extrinsic_YXY,
Extrinsic_ZYZ,
Extrinsic_ZXZ,
Intrinsic_XYX,
Intrinsic_XZX,
Intrinsic_YZY,
Intrinsic_YXY,
Intrinsic_ZXZ,
Intrinsic_ZYZ,
EulerSequenceLast,
};
static const char *eulerSequenceName(EulerSequence seq);
static EulerSequence eulerSequenceFromName(const char *name);
void getEulerAngles(EulerSequence seq, double &alpha, double &beta, double &gamma) const;
void setEulerAngles(EulerSequence seq, double alpha, double beta, double gamma);
bool isIdentity() const;
bool isNull() const;
//@}

13
src/Base/RotationPy.xml
View File

@@ -24,6 +24,8 @@
-- a Vector (axis) and a float (angle)
-- two Vectors (rotation from/to vector)
-- three floats (Euler angles) as yaw-pitch-roll in XY'Z'' convention
-- one string and three floats (Euler angles) as euler rotation
of a given type. Call toEulerSequence() for supported sequence types.
-- four floats (Quaternion) where the quaternion is specified as:
q=xi+yj+zk+w, i.e. the last parameter is the real part
-- three vectors that define rotated axes directions + an optional
@@ -84,12 +86,21 @@
<Methode Name="toEuler" Const="true">
<Documentation>
<UserDocu>
toEuler(Vector) -> list
toEuler() -> list
Get the Euler angles of this rotation
as yaw-pitch-roll in XY'Z'' convention
</UserDocu>
</Documentation>
</Methode>
<Methode Name="toEulerAngles" Const="true">
<Documentation>
<UserDocu>
toEulerAngles(seq='') -> list
Get the Euler angles in a given sequence for this rotation.
Call this function without arguments to output all possible values of 'seq'.
</UserDocu>
</Documentation>
</Methode>
<Methode Name="toMatrix" Const="true">
<Documentation>
<UserDocu>

37
src/Base/RotationPyImp.cpp
View File

@@ -102,6 +102,17 @@ int RotationPy::PyInit(PyObject* args, PyObject* /*kwd*/)
return 0;
}
PyErr_Clear();
const char *seq;
double a, b, c;
if (PyArg_ParseTuple(args, "sddd", &seq, &a, &b, &c)) {
PY_TRY {
getRotationPtr()->setEulerAngles(
Rotation::eulerSequenceFromName(seq), a, b, c);
return 0;
} _PY_CATCH(return -1)
}
double a11 = 1.0, a12 = 0.0, a13 = 0.0, a14 = 0.0;
double a21 = 0.0, a22 = 1.0, a23 = 0.0, a24 = 0.0;
double a31 = 0.0, a32 = 0.0, a33 = 1.0, a34 = 0.0;
@@ -282,6 +293,32 @@ PyObject* RotationPy::toEuler(PyObject * args)
return Py::new_reference_to(tuple);
}
PyObject* RotationPy::toEulerAngles(PyObject * args)
{
const char *seq = nullptr;
if (!PyArg_ParseTuple(args, "|s", &seq))
return NULL;
if (!seq) {
Py::List res;
for (int i=1; i<Rotation::EulerSequenceLast; ++i)
res.append(Py::String(Rotation::eulerSequenceName((Rotation::EulerSequence)i)));
return Py::new_reference_to(res);
}
PY_TRY {
double A,B,C;
this->getRotationPtr()->getEulerAngles(
Rotation::eulerSequenceFromName(seq),A,B,C);
Py::Tuple tuple(3);
tuple.setItem(0, Py::Float(A));
tuple.setItem(1, Py::Float(B));
tuple.setItem(2, Py::Float(C));
return Py::new_reference_to(tuple);
} PY_CATCH
}
PyObject* RotationPy::toMatrix(PyObject * args)
{
if (!PyArg_ParseTuple(args, ""))