diff --git a/src/Base/Rotation.cpp b/src/Base/Rotation.cpp index ed7d5aaa23..ec15219388 100644 --- a/src/Base/Rotation.cpp +++ b/src/Base/Rotation.cpp @@ -27,6 +27,7 @@ # include #endif +#include #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= 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; +} diff --git a/src/Base/Rotation.h b/src/Base/Rotation.h index f334ce2aca..cb45dd0c19 100644 --- a/src/Base/Rotation.h +++ b/src/Base/Rotation.h @@ -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; //@} diff --git a/src/Base/RotationPy.xml b/src/Base/RotationPy.xml index 528f6bbc10..bfb60e2a70 100644 --- a/src/Base/RotationPy.xml +++ b/src/Base/RotationPy.xml @@ -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 @@ - toEuler(Vector) -> list + toEuler() -> list Get the Euler angles of this rotation as yaw-pitch-roll in XY'Z'' convention + + + + 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'. + + + diff --git a/src/Base/RotationPyImp.cpp b/src/Base/RotationPyImp.cpp index 0437b26149..a66997f3b9 100644 --- a/src/Base/RotationPyImp.cpp +++ b/src/Base/RotationPyImp.cpp @@ -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; igetRotationPtr()->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, ""))