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>
133 static const int AlphaAxis = _AlphaAxis;
136 static const int BetaAxis = _BetaAxis;
139 static const int GammaAxis = _GammaAxis;
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,
153 IsOdd = ((AlphaAxisAbs)%3 == (BetaAxisAbs - 1)%3) ? 0 : 1,
154 IsEven = IsOdd ? 0 : 1,
156 IsTaitBryan = ((unsigned)AlphaAxisAbs != (
unsigned)GammaAxisAbs) ? 1 : 0
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);
170 EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT((
unsigned)AlphaAxisAbs != (
unsigned)BetaAxisAbs,
171 ALPHA_AXIS_CANT_BE_EQUAL_TO_BETA_AXIS);
173 EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT((
unsigned)BetaAxisAbs != (
unsigned)GammaAxisAbs,
174 BETA_AXIS_CANT_BE_EQUAL_TO_GAMMA_AXIS);
180 I_ = AlphaAxisAbs - 1,
181 J_ = (AlphaAxisAbs - 1 + 1 + IsOdd)%3,
182 K_ = (AlphaAxisAbs - 1 + 2 - IsOdd)%3
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>
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_));
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(
263 typename internal::conditional<IsTaitBryan, internal::true_type, internal::false_type>::type());
275 template <
typename _Scalar,
class _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)
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