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>
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);
163 ALPHA_AXIS_CANT_BE_EQUAL_TO_BETA_AXIS);
166 BETA_AXIS_CANT_BE_EQUAL_TO_GAMMA_AXIS);
179 template <
typename Derived>
186 typedef typename Derived::Scalar Scalar;
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(
252 EulerAngles<Scalar, EulerSystem>& res,
253 const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat)
255 CalcEulerAngles(res, mat,
false,
false,
false);
259 bool PositiveRangeAlpha,
260 bool PositiveRangeBeta,
261 bool PositiveRangeGamma,
263 static void CalcEulerAngles(
264 EulerAngles<Scalar, EulerSystem>& res,
265 const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat)
267 CalcEulerAngles(res, mat, PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma);
270 template<
typename Scalar>
271 static void CalcEulerAngles(
272 EulerAngles<Scalar, EulerSystem>& res,
273 const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat,
274 bool PositiveRangeAlpha,
275 bool PositiveRangeBeta,
276 bool PositiveRangeGamma)
280 typename internal::conditional<IsTaitBryan, internal::true_type, internal::false_type>::type());
283 res.alpha() = -res.alpha();
286 res.beta() = -res.beta();
289 res.gamma() = -res.gamma();
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
Definition: EulerSystem.h:145
static const int GammaAxis
Definition: EulerSystem.h:133
Definition: EulerSystem.h:143
Definition: EulerSystem.h:141
Definition: EulerSystem.h:142
Definition: EulerSystem.h:57
EulerAxis
Representation of a fixed signed rotation axis for EulerSystem.
Definition: EulerSystem.h:55
Definition: EulerSystem.h:146
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_cos_op< typename Derived::Scalar >, const Derived > cos(const Eigen::ArrayBase< Derived > &x)
Definition: EulerSystem.h:139
Definition: EulerSystem.h:58
Represents a rotation in a 3 dimensional space as three Euler angles.
Definition: EulerAngles.h:111
Definition: EulerSystem.h:138
Definition: EulerSystem.h:148
Definition: EulerSystem.h:59
Definition: EulerSystem.h:137
static const int AlphaAxis
Definition: EulerSystem.h:127
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_sin_op< typename Derived::Scalar >, const Derived > sin(const Eigen::ArrayBase< Derived > &x)
static const int BetaAxis
Definition: EulerSystem.h:130