10#ifndef EIGEN_EULERSYSTEM_H
11#define EIGEN_EULERSYSTEM_H
16 template <
typename _Scalar,
class _System>
22 template <
int Num,
bool IsPositive = (Num > 0)>
29 struct Abs<Num, false>
31 enum { value = -Num };
37 enum { value = Axis != 0 && Abs<Axis>::value <= 3 };
40 template<
typename System,
42 int OtherRows=Other::RowsAtCompileTime,
43 int OtherCols=Other::ColsAtCompileTime>
44 struct eulerangles_assign_impl;
47 #define EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(COND,MSG) typedef char static_assertion_##MSG[(COND)?1:-1]
125 template <
int _AlphaAxis,
int _BetaAxis,
int _GammaAxis>
161 EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<AlphaAxis>::value,
162 ALPHA_AXIS_IS_INVALID);
164 EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<BetaAxis>::value,
165 BETA_AXIS_IS_INVALID);
167 EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<GammaAxis>::value,
168 GAMMA_AXIS_IS_INVALID);
171 ALPHA_AXIS_CANT_BE_EQUAL_TO_BETA_AXIS);
174 BETA_AXIS_CANT_BE_EQUAL_TO_GAMMA_AXIS);
186 template <
typename Derived>
192 typedef typename Derived::Scalar Scalar;
194 const Scalar plusMinus =
IsEven? 1 : -1;
195 const Scalar minusPlus =
IsOdd? 1 : -1;
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);
202 res[0] = atan2(minusPlus * mat(J_, K_), mat(K_, K_));
203 res[2] = atan2(minusPlus * mat(I_, J_), mat(I_, I_));
205 else if(plusMinus * mat(I_, K_) > 0) {
206 Scalar spos = mat(J_, I_) + plusMinus * mat(K_, J_);
207 Scalar cpos = mat(J_, J_) + minusPlus * mat(K_, I_);
208 Scalar alphaPlusMinusGamma = atan2(spos, cpos);
209 res[0] = alphaPlusMinusGamma;
213 Scalar sneg = plusMinus * (mat(K_, J_) + minusPlus * mat(J_, I_));
214 Scalar cneg = mat(J_, J_) + plusMinus * mat(K_, I_);
215 Scalar alphaMinusPlusBeta = atan2(sneg, cneg);
216 res[0] = alphaMinusPlusBeta;
221 template <
typename Derived>
223 const MatrixBase<Derived>& mat, internal::false_type )
228 typedef typename Derived::Scalar Scalar;
230 const Scalar plusMinus =
IsEven? 1 : -1;
231 const Scalar minusPlus =
IsOdd? 1 : -1;
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);
235 res[1] = atan2(Rsum, mat(I_, I_));
238 if(Rsum > 4 * NumTraits<Scalar>::epsilon()) {
239 res[0] = atan2(mat(J_, I_), minusPlus * mat(K_, I_));
240 res[2] = atan2(mat(I_, J_), plusMinus * mat(I_, K_));
242 else if(mat(I_, I_) > 0) {
243 Scalar spos = plusMinus * mat(K_, J_) + minusPlus * mat(J_, K_);
244 Scalar cpos = mat(J_, J_) + mat(K_, K_);
245 res[0] = atan2(spos, cpos);
249 Scalar sneg = plusMinus * mat(K_, J_) + plusMinus * mat(J_, K_);
250 Scalar cneg = mat(J_, J_) - mat(K_, K_);
251 res[0] = atan2(sneg, cneg);
256 template<
typename Scalar>
257 static void CalcEulerAngles(
258 EulerAngles<Scalar, EulerSystem>& res,
263 typename internal::conditional<IsTaitBryan, internal::true_type, internal::false_type>::type());
266 res.alpha() = -res.alpha();
269 res.beta() = -res.beta();
272 res.gamma() = -res.gamma();
275 template <
typename _Scalar,
class _System>
278 template<
typename System,
282 friend struct internal::eulerangles_assign_impl;
285#define EIGEN_EULER_SYSTEM_TYPEDEF(A, B, C) \
287 typedef EulerSystem<EULER_##A, EULER_##B, EULER_##C> EulerSystem##A##B##C;
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)
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)
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)
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)