Skip to content

Commit c1454df

Browse files
realthunderrealthunder
committed
Base: add new API Rotation::get/setEulerAngles()
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.
1 parent ccc4151 commit c1454df

File tree

4 files changed

+365
-1
lines changed

4 files changed

+365
-1
lines changed

‎src/Base/Rotation.cpp

Lines changed: 269 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,7 @@
2727
# include <climits>
2828
#endif
2929

30+
#include <boost/algorithm/string/predicate.hpp>
3031
#include "Rotation.h"
3132
#include "Matrix.h"
3233
#include "Base/Exception.h"
@@ -712,3 +713,271 @@ bool Rotation::isNull() const
712713
this->quat[2] == 0.0 &&
713714
this->quat[3] == 0.0);
714715
}
716+
717+
//=======================================================================
718+
// The following code is borrowed from OCCT gp/gp_Quaternion.cxx
719+
720+
namespace { // anonymous namespace
721+
//=======================================================================
722+
//function : translateEulerSequence
723+
//purpose :
724+
// Code supporting conversion between quaternion and generalized
725+
// Euler angles (sequence of three rotations) is based on
726+
// algorithm by Ken Shoemake, published in Graphics Gems IV, p. 222-22
727+
// http://tog.acm.org/resources/GraphicsGems/gemsiv/euler_angle/EulerAngles.c
728+
//=======================================================================
729+
730+
struct EulerSequence_Parameters
731+
{
732+
int i; // first rotation axis
733+
int j; // next axis of rotation
734+
int k; // third axis
735+
bool isOdd; // true if order of two first rotation axes is odd permutation, e.g. XZ
736+
bool isTwoAxes; // true if third rotation is about the same axis as first
737+
bool isExtrinsic; // true if rotations are made around fixed axes
738+
739+
EulerSequence_Parameters (int theAx1,
740+
bool theisOdd,
741+
bool theisTwoAxes,
742+
bool theisExtrinsic)
743+
: i(theAx1),
744+
j(1 + (theAx1 + (theisOdd ? 1 : 0)) % 3),
745+
k(1 + (theAx1 + (theisOdd ? 0 : 1)) % 3),
746+
isOdd(theisOdd),
747+
isTwoAxes(theisTwoAxes),
748+
isExtrinsic(theisExtrinsic)
749+
{}
750+
};
751+
752+
EulerSequence_Parameters translateEulerSequence (const Rotation::EulerSequence theSeq)
753+
{
754+
typedef EulerSequence_Parameters Params;
755+
const bool F = false;
756+
const bool T = true;
757+
758+
switch (theSeq)
759+
{
760+
case Rotation::Extrinsic_XYZ: return Params (1, F, F, T);
761+
case Rotation::Extrinsic_XZY: return Params (1, T, F, T);
762+
case Rotation::Extrinsic_YZX: return Params (2, F, F, T);
763+
case Rotation::Extrinsic_YXZ: return Params (2, T, F, T);
764+
case Rotation::Extrinsic_ZXY: return Params (3, F, F, T);
765+
case Rotation::Extrinsic_ZYX: return Params (3, T, F, T);
766+
767+
// Conversion of intrinsic angles is made by the same code as for extrinsic,
768+
// using equivalence rule: intrinsic rotation is equivalent to extrinsic
769+
// rotation by the same angles but with inverted order of elemental rotations.
770+
// Swapping of angles (Alpha <-> Gamma) is done inside conversion procedure;
771+
// sequence of axes is inverted by setting appropriate parameters here.
772+
// Note that proper Euler angles (last block below) are symmetric for sequence of axes.
773+
case Rotation::Intrinsic_XYZ: return Params (3, T, F, F);
774+
case Rotation::Intrinsic_XZY: return Params (2, F, F, F);
775+
case Rotation::Intrinsic_YZX: return Params (1, T, F, F);
776+
case Rotation::Intrinsic_YXZ: return Params (3, F, F, F);
777+
case Rotation::Intrinsic_ZXY: return Params (2, T, F, F);
778+
case Rotation::Intrinsic_ZYX: return Params (1, F, F, F);
779+
780+
case Rotation::Extrinsic_XYX: return Params (1, F, T, T);
781+
case Rotation::Extrinsic_XZX: return Params (1, T, T, T);
782+
case Rotation::Extrinsic_YZY: return Params (2, F, T, T);
783+
case Rotation::Extrinsic_YXY: return Params (2, T, T, T);
784+
case Rotation::Extrinsic_ZXZ: return Params (3, F, T, T);
785+
case Rotation::Extrinsic_ZYZ: return Params (3, T, T, T);
786+
787+
case Rotation::Intrinsic_XYX: return Params (1, F, T, F);
788+
case Rotation::Intrinsic_XZX: return Params (1, T, T, F);
789+
case Rotation::Intrinsic_YZY: return Params (2, F, T, F);
790+
case Rotation::Intrinsic_YXY: return Params (2, T, T, F);
791+
case Rotation::Intrinsic_ZXZ: return Params (3, F, T, F);
792+
case Rotation::Intrinsic_ZYZ: return Params (3, T, T, F);
793+
794+
default:
795+
case Rotation::EulerAngles : return Params (3, F, T, F); // = Intrinsic_ZXZ
796+
case Rotation::YawPitchRoll: return Params (1, F, F, F); // = Intrinsic_ZYX
797+
};
798+
}
799+
800+
class Mat : public Base::Matrix4D
801+
{
802+
public:
803+
double operator()(int i, int j) const {
804+
return this->operator[](i-1)[j-1];
805+
}
806+
double & operator()(int i, int j) {
807+
return this->operator[](i-1)[j-1];
808+
}
809+
};
810+
811+
const char *EulerSequenceNames[] = {
812+
//! Classic Euler angles, alias to Intrinsic_ZXZ
813+
"Euler",
814+
815+
//! Yaw Pitch Roll (or nautical) angles, alias to Intrinsic_ZYX
816+
"YawPitchRoll",
817+
818+
// Tait-Bryan angles (using three different axes)
819+
"XYZ",
820+
"XZY",
821+
"YZX",
822+
"YXZ",
823+
"ZXY",
824+
"ZYX",
825+
826+
"IXYZ",
827+
"IXZY",
828+
"IYZX",
829+
"IYXZ",
830+
"IZXY",
831+
"IZYX",
832+
833+
// Proper Euler angles (using two different axes, first and third the same)
834+
"XYX",
835+
"XZX",
836+
"YZY",
837+
"YXY",
838+
"ZYZ",
839+
"ZXZ",
840+
841+
"IXYX",
842+
"IXZX",
843+
"IYZY",
844+
"IYXY",
845+
"IZXZ",
846+
"IZYZ",
847+
};
848+
849+
} // anonymous namespace
850+
851+
const char * Rotation::eulerSequenceName(EulerSequence seq)
852+
{
853+
if (seq == Invalid || seq >= EulerSequenceLast)
854+
return 0;
855+
return EulerSequenceNames[seq-1];
856+
}
857+
858+
Rotation::EulerSequence Rotation::eulerSequenceFromName(const char *name)
859+
{
860+
if (name) {
861+
for (unsigned i=0; i<sizeof(EulerSequenceNames)/sizeof(EulerSequenceNames[0]); ++i) {
862+
if (boost::iequals(name, EulerSequenceNames[i]))
863+
return (EulerSequence)(i+1);
864+
}
865+
}
866+
return Invalid;
867+
}
868+
869+
void Rotation::setEulerAngles(EulerSequence theOrder,
870+
double theAlpha,
871+
double theBeta,
872+
double theGamma)
873+
{
874+
if (theOrder == Invalid || theOrder >= EulerSequenceLast)
875+
throw Base::ValueError("invalid euler sequence");
876+
877+
EulerSequence_Parameters o = translateEulerSequence (theOrder);
878+
879+
theAlpha *= D_PI/180.0;
880+
theBeta *= D_PI/180.0;
881+
theGamma *= D_PI/180.0;
882+
883+
double a = theAlpha, b = theBeta, c = theGamma;
884+
if ( ! o.isExtrinsic )
885+
std::swap(a, c);
886+
887+
if ( o.isOdd )
888+
b = -b;
889+
890+
double ti = 0.5 * a;
891+
double tj = 0.5 * b;
892+
double th = 0.5 * c;
893+
double ci = cos (ti);
894+
double cj = cos (tj);
895+
double ch = cos (th);
896+
double si = sin (ti);
897+
double sj = sin (tj);
898+
double sh = sin (th);
899+
double cc = ci * ch;
900+
double cs = ci * sh;
901+
double sc = si * ch;
902+
double ss = si * sh;
903+
904+
double values[4]; // w, x, y, z
905+
if ( o.isTwoAxes )
906+
{
907+
values[o.i] = cj * (cs + sc);
908+
values[o.j] = sj * (cc + ss);
909+
values[o.k] = sj * (cs - sc);
910+
values[0] = cj * (cc - ss);
911+
}
912+
else
913+
{
914+
values[o.i] = cj * sc - sj * cs;
915+
values[o.j] = cj * ss + sj * cc;
916+
values[o.k] = cj * cs - sj * sc;
917+
values[0] = cj * cc + sj * ss;
918+
}
919+
if ( o.isOdd )
920+
values[o.j] = -values[o.j];
921+
922+
quat[0] = values[1];
923+
quat[1] = values[2];
924+
quat[2] = values[3];
925+
quat[3] = values[0];
926+
}
927+
928+
void Rotation::getEulerAngles(EulerSequence theOrder,
929+
double& theAlpha,
930+
double& theBeta,
931+
double& theGamma) const
932+
{
933+
Mat M;
934+
getValue(M);
935+
936+
EulerSequence_Parameters o = translateEulerSequence (theOrder);
937+
if ( o.isTwoAxes )
938+
{
939+
double sy = sqrt (M(o.i, o.j) * M(o.i, o.j) + M(o.i, o.k) * M(o.i, o.k));
940+
if (sy > 16 * DBL_EPSILON)
941+
{
942+
theAlpha = atan2 (M(o.i, o.j), M(o.i, o.k));
943+
theGamma = atan2 (M(o.j, o.i), -M(o.k, o.i));
944+
}
945+
else
946+
{
947+
theAlpha = atan2 (-M(o.j, o.k), M(o.j, o.j));
948+
theGamma = 0.;
949+
}
950+
theBeta = atan2 (sy, M(o.i, o.i));
951+
}
952+
else
953+
{
954+
double cy = sqrt (M(o.i, o.i) * M(o.i, o.i) + M(o.j, o.i) * M(o.j, o.i));
955+
if (cy > 16 * DBL_EPSILON)
956+
{
957+
theAlpha = atan2 (M(o.k, o.j), M(o.k, o.k));
958+
theGamma = atan2 (M(o.j, o.i), M(o.i, o.i));
959+
}
960+
else
961+
{
962+
theAlpha = atan2 (-M(o.j, o.k), M(o.j, o.j));
963+
theGamma = 0.;
964+
}
965+
theBeta = atan2 (-M(o.k, o.i), cy);
966+
}
967+
if ( o.isOdd )
968+
{
969+
theAlpha = -theAlpha;
970+
theBeta = -theBeta;
971+
theGamma = -theGamma;
972+
}
973+
if ( ! o.isExtrinsic )
974+
{
975+
double aFirst = theAlpha;
976+
theAlpha = theGamma;
977+
theGamma = aFirst;
978+
}
979+
980+
theAlpha *= 180.0/D_PI;
981+
theBeta *= 180.0/D_PI;
982+
theGamma *= 180.0/D_PI;
983+
}

‎src/Base/Rotation.h

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -63,6 +63,53 @@ class BaseExport Rotation
6363
void setYawPitchRoll(double y, double p, double r);
6464
/// Euler angles in yaw,pitch,roll notation
6565
void getYawPitchRoll(double& y, double& p, double& r) const;
66+
67+
enum EulerSequence
68+
{
69+
Invalid,
70+
71+
//! Classic Euler angles, alias to Intrinsic_ZXZ
72+
EulerAngles,
73+
74+
//! Yaw Pitch Roll (or nautical) angles, alias to Intrinsic_ZYX
75+
YawPitchRoll,
76+
77+
// Tait-Bryan angles (using three different axes)
78+
Extrinsic_XYZ,
79+
Extrinsic_XZY,
80+
Extrinsic_YZX,
81+
Extrinsic_YXZ,
82+
Extrinsic_ZXY,
83+
Extrinsic_ZYX,
84+
85+
Intrinsic_XYZ,
86+
Intrinsic_XZY,
87+
Intrinsic_YZX,
88+
Intrinsic_YXZ,
89+
Intrinsic_ZXY,
90+
Intrinsic_ZYX,
91+
92+
// Proper Euler angles (using two different axes, first and third the same)
93+
Extrinsic_XYX,
94+
Extrinsic_XZX,
95+
Extrinsic_YZY,
96+
Extrinsic_YXY,
97+
Extrinsic_ZYZ,
98+
Extrinsic_ZXZ,
99+
100+
Intrinsic_XYX,
101+
Intrinsic_XZX,
102+
Intrinsic_YZY,
103+
Intrinsic_YXY,
104+
Intrinsic_ZXZ,
105+
Intrinsic_ZYZ,
106+
107+
EulerSequenceLast,
108+
};
109+
static const char *eulerSequenceName(EulerSequence seq);
110+
static EulerSequence eulerSequenceFromName(const char *name);
111+
void getEulerAngles(EulerSequence seq, double &alpha, double &beta, double &gamma) const;
112+
void setEulerAngles(EulerSequence seq, double alpha, double beta, double gamma);
66113
bool isIdentity() const;
67114
bool isNull() const;
68115
//@}

‎src/Base/RotationPy.xml

Lines changed: 12 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,8 @@
2424
-- a Vector (axis) and a float (angle)
2525
-- two Vectors (rotation from/to vector)
2626
-- three floats (Euler angles) as yaw-pitch-roll in XY'Z'' convention
27+
-- one string and three floats (Euler angles) as euler rotation
28+
of a given type. Call toEulerSequence() for supported sequence types.
2729
-- four floats (Quaternion) where the quaternion is specified as:
2830
q=xi+yj+zk+w, i.e. the last parameter is the real part
2931
-- three vectors that define rotated axes directions + an optional
@@ -84,12 +86,21 @@
8486
<Methode Name="toEuler" Const="true">
8587
<Documentation>
8688
<UserDocu>
87-
toEuler(Vector) -> list
89+
toEuler() -> list
8890
Get the Euler angles of this rotation
8991
as yaw-pitch-roll in XY'Z'' convention
9092
</UserDocu>
9193
</Documentation>
9294
</Methode>
95+
<Methode Name="toEulerAngles" Const="true">
96+
<Documentation>
97+
<UserDocu>
98+
toEulerAngles(seq='') -> list
99+
Get the Euler angles in a given sequence for this rotation.
100+
Call this function without arguments to output all possible values of 'seq'.
101+
</UserDocu>
102+
</Documentation>
103+
</Methode>
93104
<Methode Name="toMatrix" Const="true">
94105
<Documentation>
95106
<UserDocu>

0 commit comments

Comments
 (0)