Please, help us to better know about our user community by answering the following short survey: https://forms.gle/wpyrxWi18ox9Z5ae9
EulerSystem.h
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 declarations
16  template <typename _Scalar, class _System>
17  class EulerAngles;
18 
19  namespace internal
20  {
21  // TODO: Add this trait to the Eigen internal 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  template<typename System,
41  typename Other,
42  int OtherRows=Other::RowsAtCompileTime,
43  int OtherCols=Other::ColsAtCompileTime>
44  struct eulerangles_assign_impl;
45  }
46 
47  #define EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(COND,MSG) typedef char static_assertion_##MSG[(COND)?1:-1]
48 
61  enum EulerAxis
62  {
63  EULER_X = 1,
64  EULER_Y = 2,
65  EULER_Z = 3
66  };
67 
125  template <int _AlphaAxis, int _BetaAxis, int _GammaAxis>
127  {
128  public:
129  // It's defined this way and not as enum, because I think
130  // that enum is not guerantee to support negative numbers
131 
133  static const int AlphaAxis = _AlphaAxis;
134 
136  static const int BetaAxis = _BetaAxis;
137 
139  static const int GammaAxis = _GammaAxis;
140 
141  enum
142  {
143  AlphaAxisAbs = internal::Abs<AlphaAxis>::value,
144  BetaAxisAbs = internal::Abs<BetaAxis>::value,
145  GammaAxisAbs = internal::Abs<GammaAxis>::value,
147  IsAlphaOpposite = (AlphaAxis < 0) ? 1 : 0,
148  IsBetaOpposite = (BetaAxis < 0) ? 1 : 0,
149  IsGammaOpposite = (GammaAxis < 0) ? 1 : 0,
151  // Parity is even if alpha axis X is followed by beta axis Y, or Y is followed
152  // by Z, or Z is followed by X; otherwise it is odd.
153  IsOdd = ((AlphaAxisAbs)%3 == (BetaAxisAbs - 1)%3) ? 0 : 1,
154  IsEven = IsOdd ? 0 : 1,
156  IsTaitBryan = ((unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0
157  };
158 
159  private:
160 
161  EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<AlphaAxis>::value,
162  ALPHA_AXIS_IS_INVALID);
163 
164  EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<BetaAxis>::value,
165  BETA_AXIS_IS_INVALID);
166 
167  EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<GammaAxis>::value,
168  GAMMA_AXIS_IS_INVALID);
169 
170  EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT((unsigned)AlphaAxisAbs != (unsigned)BetaAxisAbs,
171  ALPHA_AXIS_CANT_BE_EQUAL_TO_BETA_AXIS);
172 
173  EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT((unsigned)BetaAxisAbs != (unsigned)GammaAxisAbs,
174  BETA_AXIS_CANT_BE_EQUAL_TO_GAMMA_AXIS);
175 
176  static const int
177  // I, J, K are the pivot indexes permutation for the rotation matrix, that match this Euler system.
178  // They are used in this class converters.
179  // They are always different from each other, and their possible values are: 0, 1, or 2.
180  I_ = AlphaAxisAbs - 1,
181  J_ = (AlphaAxisAbs - 1 + 1 + IsOdd)%3,
182  K_ = (AlphaAxisAbs - 1 + 2 - IsOdd)%3
183  ;
184 
185  // TODO: Get @mat parameter in form that avoids double evaluation.
186  template <typename Derived>
187  static void CalcEulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar, 3, 1>& res, const MatrixBase<Derived>& mat, internal::true_type /*isTaitBryan*/)
188  {
189  using std::atan2;
190  using std::sqrt;
191 
192  typedef typename Derived::Scalar Scalar;
193 
194  const Scalar plusMinus = IsEven? 1 : -1;
195  const Scalar minusPlus = IsOdd? 1 : -1;
196 
197  const Scalar Rsum = sqrt((mat(I_,I_) * mat(I_,I_) + mat(I_,J_) * mat(I_,J_) + mat(J_,K_) * mat(J_,K_) + mat(K_,K_) * mat(K_,K_))/2);
198  res[1] = atan2(plusMinus * mat(I_,K_), Rsum);
199 
200  // There is a singularity when cos(beta) == 0
201  if(Rsum > 4 * NumTraits<Scalar>::epsilon()) {// cos(beta) != 0
202  res[0] = atan2(minusPlus * mat(J_, K_), mat(K_, K_));
203  res[2] = atan2(minusPlus * mat(I_, J_), mat(I_, I_));
204  }
205  else if(plusMinus * mat(I_, K_) > 0) {// cos(beta) == 0 and sin(beta) == 1
206  Scalar spos = mat(J_, I_) + plusMinus * mat(K_, J_); // 2*sin(alpha + plusMinus * gamma
207  Scalar cpos = mat(J_, J_) + minusPlus * mat(K_, I_); // 2*cos(alpha + plusMinus * gamma)
208  Scalar alphaPlusMinusGamma = atan2(spos, cpos);
209  res[0] = alphaPlusMinusGamma;
210  res[2] = 0;
211  }
212  else {// cos(beta) == 0 and sin(beta) == -1
213  Scalar sneg = plusMinus * (mat(K_, J_) + minusPlus * mat(J_, I_)); // 2*sin(alpha + minusPlus*gamma)
214  Scalar cneg = mat(J_, J_) + plusMinus * mat(K_, I_); // 2*cos(alpha + minusPlus*gamma)
215  Scalar alphaMinusPlusBeta = atan2(sneg, cneg);
216  res[0] = alphaMinusPlusBeta;
217  res[2] = 0;
218  }
219  }
220 
221  template <typename Derived>
222  static void CalcEulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar,3,1>& res,
223  const MatrixBase<Derived>& mat, internal::false_type /*isTaitBryan*/)
224  {
225  using std::atan2;
226  using std::sqrt;
227 
228  typedef typename Derived::Scalar Scalar;
229 
230  const Scalar plusMinus = IsEven? 1 : -1;
231  const Scalar minusPlus = IsOdd? 1 : -1;
232 
233  const Scalar Rsum = sqrt((mat(I_, J_) * mat(I_, J_) + mat(I_, K_) * mat(I_, K_) + mat(J_, I_) * mat(J_, I_) + mat(K_, I_) * mat(K_, I_)) / 2);
234 
235  res[1] = atan2(Rsum, mat(I_, I_));
236 
237  // There is a singularity when sin(beta) == 0
238  if(Rsum > 4 * NumTraits<Scalar>::epsilon()) {// sin(beta) != 0
239  res[0] = atan2(mat(J_, I_), minusPlus * mat(K_, I_));
240  res[2] = atan2(mat(I_, J_), plusMinus * mat(I_, K_));
241  }
242  else if(mat(I_, I_) > 0) {// sin(beta) == 0 and cos(beta) == 1
243  Scalar spos = plusMinus * mat(K_, J_) + minusPlus * mat(J_, K_); // 2*sin(alpha + gamma)
244  Scalar cpos = mat(J_, J_) + mat(K_, K_); // 2*cos(alpha + gamma)
245  res[0] = atan2(spos, cpos);
246  res[2] = 0;
247  }
248  else {// sin(beta) == 0 and cos(beta) == -1
249  Scalar sneg = plusMinus * mat(K_, J_) + plusMinus * mat(J_, K_); // 2*sin(alpha - gamma)
250  Scalar cneg = mat(J_, J_) - mat(K_, K_); // 2*cos(alpha - gamma)
251  res[0] = atan2(sneg, cneg);
252  res[2] = 0;
253  }
254  }
255 
256  template<typename Scalar>
257  static void CalcEulerAngles(
259  const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat)
260  {
261  CalcEulerAngles_imp(
262  res.angles(), mat,
263  typename internal::conditional<IsTaitBryan, internal::true_type, internal::false_type>::type());
264 
265  if (IsAlphaOpposite)
266  res.alpha() = -res.alpha();
267 
268  if (IsBetaOpposite)
269  res.beta() = -res.beta();
270 
271  if (IsGammaOpposite)
272  res.gamma() = -res.gamma();
273  }
274 
275  template <typename _Scalar, class _System>
276  friend class Eigen::EulerAngles;
277 
278  template<typename System,
279  typename Other,
280  int OtherRows,
281  int OtherCols>
282  friend struct internal::eulerangles_assign_impl;
283  };
284 
285 #define EIGEN_EULER_SYSTEM_TYPEDEF(A, B, C) \
286  \
287  typedef EulerSystem<EULER_##A, EULER_##B, EULER_##C> EulerSystem##A##B##C;
288 
289  EIGEN_EULER_SYSTEM_TYPEDEF(X,Y,Z)
290  EIGEN_EULER_SYSTEM_TYPEDEF(X,Y,X)
291  EIGEN_EULER_SYSTEM_TYPEDEF(X,Z,Y)
292  EIGEN_EULER_SYSTEM_TYPEDEF(X,Z,X)
293 
294  EIGEN_EULER_SYSTEM_TYPEDEF(Y,Z,X)
295  EIGEN_EULER_SYSTEM_TYPEDEF(Y,Z,Y)
296  EIGEN_EULER_SYSTEM_TYPEDEF(Y,X,Z)
297  EIGEN_EULER_SYSTEM_TYPEDEF(Y,X,Y)
298 
299  EIGEN_EULER_SYSTEM_TYPEDEF(Z,X,Y)
300  EIGEN_EULER_SYSTEM_TYPEDEF(Z,X,Z)
301  EIGEN_EULER_SYSTEM_TYPEDEF(Z,Y,X)
302  EIGEN_EULER_SYSTEM_TYPEDEF(Z,Y,Z)
303 }
304 
305 #endif // EIGEN_EULERSYSTEM_H
internal::traits< Derived >::Scalar Scalar
Represents a fixed Euler rotation system.
Definition: EulerSystem.h:126
Scalar gamma() const
Definition: EulerAngles.h:208
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_sqrt_op< typename Derived::Scalar >, const Derived > sqrt(const Eigen::ArrayBase< Derived > &x)
Scalar beta() const
Definition: EulerAngles.h:203
Namespace containing all symbols from the Eigen library.
Definition: EulerSystem.h:63
EulerAxis
Representation of a fixed signed rotation axis for EulerSystem.
Definition: EulerSystem.h:61
Definition: EulerSystem.h:64
Represents a rotation in a 3 dimensional space as three Euler angles.
Definition: EulerAngles.h:100
Definition: EulerSystem.h:65
_System System
Definition: EulerAngles.h:110
Scalar alpha() const
Definition: EulerAngles.h:198
const Vector3 & angles() const
Definition: EulerAngles.h:193