EulerSystem.h (11620B)
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 13 namespace 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 49 /** \brief Representation of a fixed signed rotation axis for EulerSystem. 50 * 51 * \ingroup EulerAngles_Module 52 * 53 * Values here represent: 54 * - The axis of the rotation: X, Y or Z. 55 * - The sign (i.e. direction of the rotation along the axis): positive(+) or negative(-) 56 * 57 * Therefore, this could express all the axes {+X,+Y,+Z,-X,-Y,-Z} 58 * 59 * For positive axis, use +EULER_{axis}, and for negative axis use -EULER_{axis}. 60 */ 61 enum EulerAxis 62 { 63 EULER_X = 1, /*!< the X axis */ 64 EULER_Y = 2, /*!< the Y axis */ 65 EULER_Z = 3 /*!< the Z axis */ 66 }; 67 68 /** \class EulerSystem 69 * 70 * \ingroup EulerAngles_Module 71 * 72 * \brief Represents a fixed Euler rotation system. 73 * 74 * This meta-class goal is to represent the Euler system in compilation time, for EulerAngles. 75 * 76 * You can use this class to get two things: 77 * - Build an Euler system, and then pass it as a template parameter to EulerAngles. 78 * - Query some compile time data about an Euler system. (e.g. Whether it's Tait-Bryan) 79 * 80 * Euler rotation is a set of three rotation on fixed axes. (see \ref EulerAngles) 81 * This meta-class store constantly those signed axes. (see \ref EulerAxis) 82 * 83 * ### Types of Euler systems ### 84 * 85 * All and only valid 3 dimension Euler rotation over standard 86 * signed axes{+X,+Y,+Z,-X,-Y,-Z} are supported: 87 * - all axes X, Y, Z in each valid order (see below what order is valid) 88 * - rotation over the axis is supported both over the positive and negative directions. 89 * - both Tait-Bryan and proper/classic Euler angles (i.e. the opposite). 90 * 91 * Since EulerSystem support both positive and negative directions, 92 * you may call this rotation distinction in other names: 93 * - _right handed_ or _left handed_ 94 * - _counterclockwise_ or _clockwise_ 95 * 96 * Notice all axed combination are valid, and would trigger a static assertion. 97 * Same unsigned axes can't be neighbors, e.g. {X,X,Y} is invalid. 98 * This yield two and only two classes: 99 * - _Tait-Bryan_ - all unsigned axes are distinct, e.g. {X,Y,Z} 100 * - _proper/classic Euler angles_ - The first and the third unsigned axes is equal, 101 * and the second is different, e.g. {X,Y,X} 102 * 103 * ### Intrinsic vs extrinsic Euler systems ### 104 * 105 * Only intrinsic Euler systems are supported for simplicity. 106 * If you want to use extrinsic Euler systems, 107 * just use the equal intrinsic opposite order for axes and angles. 108 * I.e axes (A,B,C) becomes (C,B,A), and angles (a,b,c) becomes (c,b,a). 109 * 110 * ### Convenient user typedefs ### 111 * 112 * Convenient typedefs for EulerSystem exist (only for positive axes Euler systems), 113 * in a form of EulerSystem{A}{B}{C}, e.g. \ref EulerSystemXYZ. 114 * 115 * ### Additional reading ### 116 * 117 * More information about Euler angles: https://en.wikipedia.org/wiki/Euler_angles 118 * 119 * \tparam _AlphaAxis the first fixed EulerAxis 120 * 121 * \tparam _BetaAxis the second fixed EulerAxis 122 * 123 * \tparam _GammaAxis the third fixed EulerAxis 124 */ 125 template <int _AlphaAxis, int _BetaAxis, int _GammaAxis> 126 class EulerSystem 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 132 /** The first rotation axis */ 133 static const int AlphaAxis = _AlphaAxis; 134 135 /** The second rotation axis */ 136 static const int BetaAxis = _BetaAxis; 137 138 /** The third rotation axis */ 139 static const int GammaAxis = _GammaAxis; 140 141 enum 142 { 143 AlphaAxisAbs = internal::Abs<AlphaAxis>::value, /*!< the first rotation axis unsigned */ 144 BetaAxisAbs = internal::Abs<BetaAxis>::value, /*!< the second rotation axis unsigned */ 145 GammaAxisAbs = internal::Abs<GammaAxis>::value, /*!< the third rotation axis unsigned */ 146 147 IsAlphaOpposite = (AlphaAxis < 0) ? 1 : 0, /*!< whether alpha axis is negative */ 148 IsBetaOpposite = (BetaAxis < 0) ? 1 : 0, /*!< whether beta axis is negative */ 149 IsGammaOpposite = (GammaAxis < 0) ? 1 : 0, /*!< whether gamma axis is negative */ 150 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, /*!< whether the Euler system is odd */ 154 IsEven = IsOdd ? 0 : 1, /*!< whether the Euler system is even */ 155 156 IsTaitBryan = ((unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0 /*!< whether the Euler system is Tait-Bryan */ 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, 259 const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat) 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 /** \ingroup EulerAngles_Module */ \ 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