• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2015 Tal Hadad <tal_hd@hotmail.com>
5 //
6 // This Source Code Form is subject to the terms of the Mozilla
7 // Public License v. 2.0. If a copy of the MPL was not distributed
8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9 
10 #ifndef EIGEN_EULERSYSTEM_H
11 #define EIGEN_EULERSYSTEM_H
12 
13 namespace Eigen
14 {
15   // Forward declerations
16   template <typename _Scalar, class _System>
17   class EulerAngles;
18 
19   namespace internal
20   {
21     // TODO: Check if already exists on the rest API
22     template <int Num, bool IsPositive = (Num > 0)>
23     struct Abs
24     {
25       enum { value = Num };
26     };
27 
28     template <int Num>
29     struct Abs<Num, false>
30     {
31       enum { value = -Num };
32     };
33 
34     template <int Axis>
35     struct IsValidAxis
36     {
37       enum { value = Axis != 0 && Abs<Axis>::value <= 3 };
38     };
39   }
40 
41   #define EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(COND,MSG) typedef char static_assertion_##MSG[(COND)?1:-1]
42 
43   /** \brief Representation of a fixed signed rotation axis for EulerSystem.
44     *
45     * \ingroup EulerAngles_Module
46     *
47     * Values here represent:
48     *  - The axis of the rotation: X, Y or Z.
49     *  - The sign (i.e. direction of the rotation along the axis): positive(+) or negative(-)
50     *
51     * Therefore, this could express all the axes {+X,+Y,+Z,-X,-Y,-Z}
52     *
53     * For positive axis, use +EULER_{axis}, and for negative axis use -EULER_{axis}.
54     */
55   enum EulerAxis
56   {
57     EULER_X = 1, /*!< the X axis */
58     EULER_Y = 2, /*!< the Y axis */
59     EULER_Z = 3  /*!< the Z axis */
60   };
61 
62   /** \class EulerSystem
63     *
64     * \ingroup EulerAngles_Module
65     *
66     * \brief Represents a fixed Euler rotation system.
67     *
68     * This meta-class goal is to represent the Euler system in compilation time, for EulerAngles.
69     *
70     * You can use this class to get two things:
71     *  - Build an Euler system, and then pass it as a template parameter to EulerAngles.
72     *  - Query some compile time data about an Euler system. (e.g. Whether it's tait bryan)
73     *
74     * Euler rotation is a set of three rotation on fixed axes. (see \ref EulerAngles)
75     * This meta-class store constantly those signed axes. (see \ref EulerAxis)
76     *
77     * ### Types of Euler systems ###
78     *
79     * All and only valid 3 dimension Euler rotation over standard
80     *  signed axes{+X,+Y,+Z,-X,-Y,-Z} are supported:
81     *  - all axes X, Y, Z in each valid order (see below what order is valid)
82     *  - rotation over the axis is supported both over the positive and negative directions.
83     *  - both tait bryan and proper/classic Euler angles (i.e. the opposite).
84     *
85     * Since EulerSystem support both positive and negative directions,
86     *  you may call this rotation distinction in other names:
87     *  - _right handed_ or _left handed_
88     *  - _counterclockwise_ or _clockwise_
89     *
90     * Notice all axed combination are valid, and would trigger a static assertion.
91     * Same unsigned axes can't be neighbors, e.g. {X,X,Y} is invalid.
92     * This yield two and only two classes:
93     *  - _tait bryan_ - all unsigned axes are distinct, e.g. {X,Y,Z}
94     *  - _proper/classic Euler angles_ - The first and the third unsigned axes is equal,
95     *     and the second is different, e.g. {X,Y,X}
96     *
97     * ### Intrinsic vs extrinsic Euler systems ###
98     *
99     * Only intrinsic Euler systems are supported for simplicity.
100     *  If you want to use extrinsic Euler systems,
101     *   just use the equal intrinsic opposite order for axes and angles.
102     *  I.e axes (A,B,C) becomes (C,B,A), and angles (a,b,c) becomes (c,b,a).
103     *
104     * ### Convenient user typedefs ###
105     *
106     * Convenient typedefs for EulerSystem exist (only for positive axes Euler systems),
107     *  in a form of EulerSystem{A}{B}{C}, e.g. \ref EulerSystemXYZ.
108     *
109     * ### Additional reading ###
110     *
111     * More information about Euler angles: https://en.wikipedia.org/wiki/Euler_angles
112     *
113     * \tparam _AlphaAxis the first fixed EulerAxis
114     *
115     * \tparam _AlphaAxis the second fixed EulerAxis
116     *
117     * \tparam _AlphaAxis the third fixed EulerAxis
118     */
119   template <int _AlphaAxis, int _BetaAxis, int _GammaAxis>
120   class EulerSystem
121   {
122     public:
123     // It's defined this way and not as enum, because I think
124     //  that enum is not guerantee to support negative numbers
125 
126     /** The first rotation axis */
127     static const int AlphaAxis = _AlphaAxis;
128 
129     /** The second rotation axis */
130     static const int BetaAxis = _BetaAxis;
131 
132     /** The third rotation axis */
133     static const int GammaAxis = _GammaAxis;
134 
135     enum
136     {
137       AlphaAxisAbs = internal::Abs<AlphaAxis>::value, /*!< the first rotation axis unsigned */
138       BetaAxisAbs = internal::Abs<BetaAxis>::value, /*!< the second rotation axis unsigned */
139       GammaAxisAbs = internal::Abs<GammaAxis>::value, /*!< the third rotation axis unsigned */
140 
141       IsAlphaOpposite = (AlphaAxis < 0) ? 1 : 0, /*!< weather alpha axis is negative */
142       IsBetaOpposite = (BetaAxis < 0) ? 1 : 0, /*!< weather beta axis is negative */
143       IsGammaOpposite = (GammaAxis < 0) ? 1 : 0, /*!< weather gamma axis is negative */
144 
145       IsOdd = ((AlphaAxisAbs)%3 == (BetaAxisAbs - 1)%3) ? 0 : 1, /*!< weather the Euler system is odd */
146       IsEven = IsOdd ? 0 : 1, /*!< weather the Euler system is even */
147 
148       IsTaitBryan = ((unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0 /*!< weather the Euler system is tait bryan */
149     };
150 
151     private:
152 
153     EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<AlphaAxis>::value,
154       ALPHA_AXIS_IS_INVALID);
155 
156     EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<BetaAxis>::value,
157       BETA_AXIS_IS_INVALID);
158 
159     EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<GammaAxis>::value,
160       GAMMA_AXIS_IS_INVALID);
161 
162     EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT((unsigned)AlphaAxisAbs != (unsigned)BetaAxisAbs,
163       ALPHA_AXIS_CANT_BE_EQUAL_TO_BETA_AXIS);
164 
165     EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT((unsigned)BetaAxisAbs != (unsigned)GammaAxisAbs,
166       BETA_AXIS_CANT_BE_EQUAL_TO_GAMMA_AXIS);
167 
168     enum
169     {
170       // I, J, K are the pivot indexes permutation for the rotation matrix, that match this Euler system.
171       // They are used in this class converters.
172       // They are always different from each other, and their possible values are: 0, 1, or 2.
173       I = AlphaAxisAbs - 1,
174       J = (AlphaAxisAbs - 1 + 1 + IsOdd)%3,
175       K = (AlphaAxisAbs - 1 + 2 - IsOdd)%3
176     };
177 
178     // TODO: Get @mat parameter in form that avoids double evaluation.
179     template <typename Derived>
180     static void CalcEulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar, 3, 1>& res, const MatrixBase<Derived>& mat, internal::true_type /*isTaitBryan*/)
181     {
182       using std::atan2;
183       using std::sin;
184       using std::cos;
185 
186       typedef typename Derived::Scalar Scalar;
187       typedef Matrix<Scalar,2,1> Vector2;
188 
189       res[0] = atan2(mat(J,K), mat(K,K));
190       Scalar c2 = Vector2(mat(I,I), mat(I,J)).norm();
191       if((IsOdd && res[0]<Scalar(0)) || ((!IsOdd) && res[0]>Scalar(0))) {
192         if(res[0] > Scalar(0)) {
193           res[0] -= Scalar(EIGEN_PI);
194         }
195         else {
196           res[0] += Scalar(EIGEN_PI);
197         }
198         res[1] = atan2(-mat(I,K), -c2);
199       }
200       else
201         res[1] = atan2(-mat(I,K), c2);
202       Scalar s1 = sin(res[0]);
203       Scalar c1 = cos(res[0]);
204       res[2] = atan2(s1*mat(K,I)-c1*mat(J,I), c1*mat(J,J) - s1 * mat(K,J));
205     }
206 
207     template <typename Derived>
208     static void CalcEulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar,3,1>& res, const MatrixBase<Derived>& mat, internal::false_type /*isTaitBryan*/)
209     {
210       using std::atan2;
211       using std::sin;
212       using std::cos;
213 
214       typedef typename Derived::Scalar Scalar;
215       typedef Matrix<Scalar,2,1> Vector2;
216 
217       res[0] = atan2(mat(J,I), mat(K,I));
218       if((IsOdd && res[0]<Scalar(0)) || ((!IsOdd) && res[0]>Scalar(0)))
219       {
220         if(res[0] > Scalar(0)) {
221           res[0] -= Scalar(EIGEN_PI);
222         }
223         else {
224           res[0] += Scalar(EIGEN_PI);
225         }
226         Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm();
227         res[1] = -atan2(s2, mat(I,I));
228       }
229       else
230       {
231         Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm();
232         res[1] = atan2(s2, mat(I,I));
233       }
234 
235       // With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first two angles,
236       // we can compute their respective rotation, and apply its inverse to M. Since the result must
237       // be a rotation around x, we have:
238       //
239       //  c2  s1.s2 c1.s2                   1  0   0
240       //  0   c1    -s1       *    M    =   0  c3  s3
241       //  -s2 s1.c2 c1.c2                   0 -s3  c3
242       //
243       //  Thus:  m11.c1 - m21.s1 = c3  &   m12.c1 - m22.s1 = s3
244 
245       Scalar s1 = sin(res[0]);
246       Scalar c1 = cos(res[0]);
247       res[2] = atan2(c1*mat(J,K)-s1*mat(K,K), c1*mat(J,J) - s1 * mat(K,J));
248     }
249 
250     template<typename Scalar>
251     static void CalcEulerAngles(
252       EulerAngles<Scalar, EulerSystem>& res,
253       const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat)
254     {
255       CalcEulerAngles(res, mat, false, false, false);
256     }
257 
258     template<
259       bool PositiveRangeAlpha,
260       bool PositiveRangeBeta,
261       bool PositiveRangeGamma,
262       typename Scalar>
263     static void CalcEulerAngles(
264       EulerAngles<Scalar, EulerSystem>& res,
265       const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat)
266     {
267       CalcEulerAngles(res, mat, PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma);
268     }
269 
270     template<typename Scalar>
271     static void CalcEulerAngles(
272       EulerAngles<Scalar, EulerSystem>& res,
273       const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat,
274       bool PositiveRangeAlpha,
275       bool PositiveRangeBeta,
276       bool PositiveRangeGamma)
277     {
278       CalcEulerAngles_imp(
279         res.angles(), mat,
280         typename internal::conditional<IsTaitBryan, internal::true_type, internal::false_type>::type());
281 
282       if (IsAlphaOpposite == IsOdd)
283         res.alpha() = -res.alpha();
284 
285       if (IsBetaOpposite == IsOdd)
286         res.beta() = -res.beta();
287 
288       if (IsGammaOpposite == IsOdd)
289         res.gamma() = -res.gamma();
290 
291       // Saturate results to the requested range
292       if (PositiveRangeAlpha && (res.alpha() < 0))
293         res.alpha() += Scalar(2 * EIGEN_PI);
294 
295       if (PositiveRangeBeta && (res.beta() < 0))
296         res.beta() += Scalar(2 * EIGEN_PI);
297 
298       if (PositiveRangeGamma && (res.gamma() < 0))
299         res.gamma() += Scalar(2 * EIGEN_PI);
300     }
301 
302     template <typename _Scalar, class _System>
303     friend class Eigen::EulerAngles;
304   };
305 
306 #define EIGEN_EULER_SYSTEM_TYPEDEF(A, B, C) \
307   /** \ingroup EulerAngles_Module */ \
308   typedef EulerSystem<EULER_##A, EULER_##B, EULER_##C> EulerSystem##A##B##C;
309 
310   EIGEN_EULER_SYSTEM_TYPEDEF(X,Y,Z)
311   EIGEN_EULER_SYSTEM_TYPEDEF(X,Y,X)
312   EIGEN_EULER_SYSTEM_TYPEDEF(X,Z,Y)
313   EIGEN_EULER_SYSTEM_TYPEDEF(X,Z,X)
314 
315   EIGEN_EULER_SYSTEM_TYPEDEF(Y,Z,X)
316   EIGEN_EULER_SYSTEM_TYPEDEF(Y,Z,Y)
317   EIGEN_EULER_SYSTEM_TYPEDEF(Y,X,Z)
318   EIGEN_EULER_SYSTEM_TYPEDEF(Y,X,Y)
319 
320   EIGEN_EULER_SYSTEM_TYPEDEF(Z,X,Y)
321   EIGEN_EULER_SYSTEM_TYPEDEF(Z,X,Z)
322   EIGEN_EULER_SYSTEM_TYPEDEF(Z,Y,X)
323   EIGEN_EULER_SYSTEM_TYPEDEF(Z,Y,Z)
324 }
325 
326 #endif // EIGEN_EULERSYSTEM_H
327