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 };
41 #define EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(COND,MSG) typedef char static_assertion_##MSG[(COND)?1:-1] 119 template <
int _AlphaAxis,
int _BetaAxis,
int _GammaAxis>
127 static const int AlphaAxis = _AlphaAxis;
130 static const int BetaAxis = _BetaAxis;
133 static const int GammaAxis = _GammaAxis;
137 AlphaAxisAbs = internal::Abs<AlphaAxis>::value,
138 BetaAxisAbs = internal::Abs<BetaAxis>::value,
139 GammaAxisAbs = internal::Abs<GammaAxis>::value,
141 IsAlphaOpposite = (AlphaAxis < 0) ? 1 : 0,
142 IsBetaOpposite = (BetaAxis < 0) ? 1 : 0,
143 IsGammaOpposite = (GammaAxis < 0) ? 1 : 0,
145 IsOdd = ((AlphaAxisAbs)%3 == (BetaAxisAbs - 1)%3) ? 0 : 1,
146 IsEven = IsOdd ? 0 : 1,
148 IsTaitBryan = ((unsigned)AlphaAxisAbs != (
unsigned)GammaAxisAbs) ? 1 : 0
153 EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<AlphaAxis>::value,
154 ALPHA_AXIS_IS_INVALID);
156 EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<BetaAxis>::value,
157 BETA_AXIS_IS_INVALID);
159 EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<GammaAxis>::value,
160 GAMMA_AXIS_IS_INVALID);
162 EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT((
unsigned)AlphaAxisAbs != (
unsigned)BetaAxisAbs,
163 ALPHA_AXIS_CANT_BE_EQUAL_TO_BETA_AXIS);
165 EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT((
unsigned)BetaAxisAbs != (
unsigned)GammaAxisAbs,
166 BETA_AXIS_CANT_BE_EQUAL_TO_GAMMA_AXIS);
173 I = AlphaAxisAbs - 1,
174 J = (AlphaAxisAbs - 1 + 1 + IsOdd)%3,
175 K = (AlphaAxisAbs - 1 + 2 - IsOdd)%3
179 template <
typename Derived>
180 static void CalcEulerAngles_imp(Matrix<
typename MatrixBase<Derived>::Scalar, 3, 1>& res,
const MatrixBase<Derived>& mat, internal::true_type )
186 typedef typename Derived::Scalar Scalar;
187 typedef Matrix<Scalar,2,1> Vector2;
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);
196 res[0] += Scalar(EIGEN_PI);
198 res[1] = atan2(-mat(I,K), -c2);
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));
207 template <
typename Derived>
208 static void CalcEulerAngles_imp(Matrix<
typename MatrixBase<Derived>::Scalar,3,1>& res,
const MatrixBase<Derived>& mat, internal::false_type )
214 typedef typename Derived::Scalar Scalar;
215 typedef Matrix<Scalar,2,1> Vector2;
217 res[0] = atan2(mat(J,I), mat(K,I));
218 if((IsOdd && res[0]<Scalar(0)) || ((!IsOdd) && res[0]>Scalar(0)))
220 if(res[0] > Scalar(0)) {
221 res[0] -= Scalar(EIGEN_PI);
224 res[0] += Scalar(EIGEN_PI);
226 Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm();
227 res[1] = -atan2(s2, mat(I,I));
231 Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm();
232 res[1] = atan2(s2, mat(I,I));
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));
250 template<
typename Scalar>
251 static void CalcEulerAngles(
255 CalcEulerAngles(res, mat,
false,
false,
false);
259 bool PositiveRangeAlpha,
260 bool PositiveRangeBeta,
261 bool PositiveRangeGamma,
263 static void CalcEulerAngles(
267 CalcEulerAngles(res, mat, PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma);
270 template<
typename Scalar>
271 static void CalcEulerAngles(
274 bool PositiveRangeAlpha,
275 bool PositiveRangeBeta,
276 bool PositiveRangeGamma)
280 typename internal::conditional<IsTaitBryan, internal::true_type, internal::false_type>::type());
282 if (IsAlphaOpposite == IsOdd)
285 if (IsBetaOpposite == IsOdd)
288 if (IsGammaOpposite == IsOdd)
292 if (PositiveRangeAlpha && (res.
alpha() < 0))
293 res.
alpha() += Scalar(2 * EIGEN_PI);
295 if (PositiveRangeBeta && (res.
beta() < 0))
296 res.
beta() += Scalar(2 * EIGEN_PI);
298 if (PositiveRangeGamma && (res.
gamma() < 0))
299 res.
gamma() += Scalar(2 * EIGEN_PI);
302 template <
typename _Scalar,
class _System>
306 #define EIGEN_EULER_SYSTEM_TYPEDEF(A, B, C) \ 308 typedef EulerSystem<EULER_##A, EULER_##B, EULER_##C> EulerSystem##A##B##C; 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)
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)
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)
326 #endif // EIGEN_EULERSYSTEM_H Represents a fixed Euler rotation system.
Definition: EulerSystem.h:120
Scalar gamma() const
Definition: EulerAngles.h:227
Scalar beta() const
Definition: EulerAngles.h:222
Namespace containing all symbols from the Eigen library.
Definition: AdolcForward:45
Definition: EulerSystem.h:57
EulerAxis
Representation of a fixed signed rotation axis for EulerSystem.
Definition: EulerSystem.h:55
Definition: EulerSystem.h:58
Represents a rotation in a 3 dimensional space as three Euler angles.
Definition: EulerAngles.h:111
Matrix< Scalar, 3, 3 > Matrix3
Definition: EulerAngles.h:120
Definition: EulerSystem.h:59
Scalar alpha() const
Definition: EulerAngles.h:217
const Vector3 & angles() const
Definition: EulerAngles.h:212