Please, help us to better know about our user community by answering the following short survey: https://forms.gle/wpyrxWi18ox9Z5ae9
 
Loading...
Searching...
No Matches
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
13namespace 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
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(
258 EulerAngles<Scalar, EulerSystem>& res,
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 rotation in a 3 dimensional space as three Euler angles.
Definition: EulerAngles.h:101
Matrix< Scalar, 3, 3 > Matrix3
Definition: EulerAngles.h:112
Represents a fixed Euler rotation system.
Definition: EulerSystem.h:127
@ GammaAxisAbs
Definition: EulerSystem.h:145
@ IsOdd
Definition: EulerSystem.h:153
@ IsGammaOpposite
Definition: EulerSystem.h:149
@ IsAlphaOpposite
Definition: EulerSystem.h:147
@ IsEven
Definition: EulerSystem.h:154
@ AlphaAxisAbs
Definition: EulerSystem.h:143
@ BetaAxisAbs
Definition: EulerSystem.h:144
@ IsTaitBryan
Definition: EulerSystem.h:156
@ IsBetaOpposite
Definition: EulerSystem.h:148
static const int BetaAxis
Definition: EulerSystem.h:136
static const int AlphaAxis
Definition: EulerSystem.h:133
static const int GammaAxis
Definition: EulerSystem.h:139
EulerAxis
Representation of a fixed signed rotation axis for EulerSystem.
Definition: EulerSystem.h:62
@ EULER_X
Definition: EulerSystem.h:63
@ EULER_Z
Definition: EulerSystem.h:65
@ EULER_Y
Definition: EulerSystem.h:64
Namespace containing all symbols from the Eigen library.
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_sqrt_op< typename Derived::Scalar >, const Derived > sqrt(const Eigen::ArrayBase< Derived > &x)