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 declerations 16 template <typename _Scalar, class _System> 17 class EulerAngles; 18 19 namespace internal 20 { 21 // TODO: Check if already exists on the rest 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 41 #define EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(COND,MSG) typedef char static_assertion_##MSG[(COND)?1:-1] 42 43 /** \brief Representation of a fixed signed rotation axis for EulerSystem. 44 * 45 * \ingroup EulerAngles_Module 46 * 47 * Values here represent: 48 * - The axis of the rotation: X, Y or Z. 49 * - The sign (i.e. direction of the rotation along the axis): positive(+) or negative(-) 50 * 51 * Therefore, this could express all the axes {+X,+Y,+Z,-X,-Y,-Z} 52 * 53 * For positive axis, use +EULER_{axis}, and for negative axis use -EULER_{axis}. 54 */ 55 enum EulerAxis 56 { 57 EULER_X = 1, /*!< the X axis */ 58 EULER_Y = 2, /*!< the Y axis */ 59 EULER_Z = 3 /*!< the Z axis */ 60 }; 61 62 /** \class EulerSystem 63 * 64 * \ingroup EulerAngles_Module 65 * 66 * \brief Represents a fixed Euler rotation system. 67 * 68 * This meta-class goal is to represent the Euler system in compilation time, for EulerAngles. 69 * 70 * You can use this class to get two things: 71 * - Build an Euler system, and then pass it as a template parameter to EulerAngles. 72 * - Query some compile time data about an Euler system. (e.g. Whether it's tait bryan) 73 * 74 * Euler rotation is a set of three rotation on fixed axes. (see \ref EulerAngles) 75 * This meta-class store constantly those signed axes. (see \ref EulerAxis) 76 * 77 * ### Types of Euler systems ### 78 * 79 * All and only valid 3 dimension Euler rotation over standard 80 * signed axes{+X,+Y,+Z,-X,-Y,-Z} are supported: 81 * - all axes X, Y, Z in each valid order (see below what order is valid) 82 * - rotation over the axis is supported both over the positive and negative directions. 83 * - both tait bryan and proper/classic Euler angles (i.e. the opposite). 84 * 85 * Since EulerSystem support both positive and negative directions, 86 * you may call this rotation distinction in other names: 87 * - _right handed_ or _left handed_ 88 * - _counterclockwise_ or _clockwise_ 89 * 90 * Notice all axed combination are valid, and would trigger a static assertion. 91 * Same unsigned axes can't be neighbors, e.g. {X,X,Y} is invalid. 92 * This yield two and only two classes: 93 * - _tait bryan_ - all unsigned axes are distinct, e.g. {X,Y,Z} 94 * - _proper/classic Euler angles_ - The first and the third unsigned axes is equal, 95 * and the second is different, e.g. {X,Y,X} 96 * 97 * ### Intrinsic vs extrinsic Euler systems ### 98 * 99 * Only intrinsic Euler systems are supported for simplicity. 100 * If you want to use extrinsic Euler systems, 101 * just use the equal intrinsic opposite order for axes and angles. 102 * I.e axes (A,B,C) becomes (C,B,A), and angles (a,b,c) becomes (c,b,a). 103 * 104 * ### Convenient user typedefs ### 105 * 106 * Convenient typedefs for EulerSystem exist (only for positive axes Euler systems), 107 * in a form of EulerSystem{A}{B}{C}, e.g. \ref EulerSystemXYZ. 108 * 109 * ### Additional reading ### 110 * 111 * More information about Euler angles: https://en.wikipedia.org/wiki/Euler_angles 112 * 113 * \tparam _AlphaAxis the first fixed EulerAxis 114 * 115 * \tparam _AlphaAxis the second fixed EulerAxis 116 * 117 * \tparam _AlphaAxis the third fixed EulerAxis 118 */ 119 template <int _AlphaAxis, int _BetaAxis, int _GammaAxis> 120 class EulerSystem 121 { 122 public: 123 // It's defined this way and not as enum, because I think 124 // that enum is not guerantee to support negative numbers 125 126 /** The first rotation axis */ 127 static const int AlphaAxis = _AlphaAxis; 128 129 /** The second rotation axis */ 130 static const int BetaAxis = _BetaAxis; 131 132 /** The third rotation axis */ 133 static const int GammaAxis = _GammaAxis; 134 135 enum 136 { 137 AlphaAxisAbs = internal::Abs<AlphaAxis>::value, /*!< the first rotation axis unsigned */ 138 BetaAxisAbs = internal::Abs<BetaAxis>::value, /*!< the second rotation axis unsigned */ 139 GammaAxisAbs = internal::Abs<GammaAxis>::value, /*!< the third rotation axis unsigned */ 140 141 IsAlphaOpposite = (AlphaAxis < 0) ? 1 : 0, /*!< weather alpha axis is negative */ 142 IsBetaOpposite = (BetaAxis < 0) ? 1 : 0, /*!< weather beta axis is negative */ 143 IsGammaOpposite = (GammaAxis < 0) ? 1 : 0, /*!< weather gamma axis is negative */ 144 145 IsOdd = ((AlphaAxisAbs)%3 == (BetaAxisAbs - 1)%3) ? 0 : 1, /*!< weather the Euler system is odd */ 146 IsEven = IsOdd ? 0 : 1, /*!< weather the Euler system is even */ 147 148 IsTaitBryan = ((unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0 /*!< weather the Euler system is tait bryan */ 149 }; 150 151 private: 152 153 EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<AlphaAxis>::value, 154 ALPHA_AXIS_IS_INVALID); 155 156 EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<BetaAxis>::value, 157 BETA_AXIS_IS_INVALID); 158 159 EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<GammaAxis>::value, 160 GAMMA_AXIS_IS_INVALID); 161 162 EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT((unsigned)AlphaAxisAbs != (unsigned)BetaAxisAbs, 163 ALPHA_AXIS_CANT_BE_EQUAL_TO_BETA_AXIS); 164 165 EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT((unsigned)BetaAxisAbs != (unsigned)GammaAxisAbs, 166 BETA_AXIS_CANT_BE_EQUAL_TO_GAMMA_AXIS); 167 168 enum 169 { 170 // I, J, K are the pivot indexes permutation for the rotation matrix, that match this Euler system. 171 // They are used in this class converters. 172 // They are always different from each other, and their possible values are: 0, 1, or 2. 173 I = AlphaAxisAbs - 1, 174 J = (AlphaAxisAbs - 1 + 1 + IsOdd)%3, 175 K = (AlphaAxisAbs - 1 + 2 - IsOdd)%3 176 }; 177 178 // TODO: Get @mat parameter in form that avoids double evaluation. 179 template <typename Derived> 180 static void CalcEulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar, 3, 1>& res, const MatrixBase<Derived>& mat, internal::true_type /*isTaitBryan*/) 181 { 182 using std::atan2; 183 using std::sin; 184 using std::cos; 185 186 typedef typename Derived::Scalar Scalar; 187 typedef Matrix<Scalar,2,1> Vector2; 188 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); 194 } 195 else { 196 res[0] += Scalar(EIGEN_PI); 197 } 198 res[1] = atan2(-mat(I,K), -c2); 199 } 200 else 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)); 205 } 206 207 template <typename Derived> 208 static void CalcEulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar,3,1>& res, const MatrixBase<Derived>& mat, internal::false_type /*isTaitBryan*/) 209 { 210 using std::atan2; 211 using std::sin; 212 using std::cos; 213 214 typedef typename Derived::Scalar Scalar; 215 typedef Matrix<Scalar,2,1> Vector2; 216 217 res[0] = atan2(mat(J,I), mat(K,I)); 218 if((IsOdd && res[0]<Scalar(0)) || ((!IsOdd) && res[0]>Scalar(0))) 219 { 220 if(res[0] > Scalar(0)) { 221 res[0] -= Scalar(EIGEN_PI); 222 } 223 else { 224 res[0] += Scalar(EIGEN_PI); 225 } 226 Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm(); 227 res[1] = -atan2(s2, mat(I,I)); 228 } 229 else 230 { 231 Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm(); 232 res[1] = atan2(s2, mat(I,I)); 233 } 234 235 // With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first two angles, 236 // we can compute their respective rotation, and apply its inverse to M. Since the result must 237 // be a rotation around x, we have: 238 // 239 // c2 s1.s2 c1.s2 1 0 0 240 // 0 c1 -s1 * M = 0 c3 s3 241 // -s2 s1.c2 c1.c2 0 -s3 c3 242 // 243 // Thus: m11.c1 - m21.s1 = c3 & m12.c1 - m22.s1 = s3 244 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)); 248 } 249 250 template<typename Scalar> 251 static void CalcEulerAngles( 252 EulerAngles<Scalar, EulerSystem>& res, 253 const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat) 254 { 255 CalcEulerAngles(res, mat, false, false, false); 256 } 257 258 template< 259 bool PositiveRangeAlpha, 260 bool PositiveRangeBeta, 261 bool PositiveRangeGamma, 262 typename Scalar> 263 static void CalcEulerAngles( 264 EulerAngles<Scalar, EulerSystem>& res, 265 const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat) 266 { 267 CalcEulerAngles(res, mat, PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma); 268 } 269 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) 277 { 278 CalcEulerAngles_imp( 279 res.angles(), mat, 280 typename internal::conditional<IsTaitBryan, internal::true_type, internal::false_type>::type()); 281 282 if (IsAlphaOpposite == IsOdd) 283 res.alpha() = -res.alpha(); 284 285 if (IsBetaOpposite == IsOdd) 286 res.beta() = -res.beta(); 287 288 if (IsGammaOpposite == IsOdd) 289 res.gamma() = -res.gamma(); 290 291 // Saturate results to the requested range 292 if (PositiveRangeAlpha && (res.alpha() < 0)) 293 res.alpha() += Scalar(2 * EIGEN_PI); 294 295 if (PositiveRangeBeta && (res.beta() < 0)) 296 res.beta() += Scalar(2 * EIGEN_PI); 297 298 if (PositiveRangeGamma && (res.gamma() < 0)) 299 res.gamma() += Scalar(2 * EIGEN_PI); 300 } 301 302 template <typename _Scalar, class _System> 303 friend class Eigen::EulerAngles; 304 }; 305 306 #define EIGEN_EULER_SYSTEM_TYPEDEF(A, B, C) \ 307 /** \ingroup EulerAngles_Module */ \ 308 typedef EulerSystem<EULER_##A, EULER_##B, EULER_##C> EulerSystem##A##B##C; 309 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) 314 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) 319 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) 324 } 325 326 #endif // EIGEN_EULERSYSTEM_H 327