![]() |
Eigen-unsupported
3.3.3
|
00001 // This file is part of Eigen, a lightweight C++ template library 00002 // for linear algebra. 00003 // 00004 // Copyright (C) 2015 Tal Hadad <tal_hd@hotmail.com> 00005 // 00006 // This Source Code Form is subject to the terms of the Mozilla 00007 // Public License v. 2.0. If a copy of the MPL was not distributed 00008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 00009 00010 #ifndef EIGEN_EULERSYSTEM_H 00011 #define EIGEN_EULERSYSTEM_H 00012 00013 namespace Eigen 00014 { 00015 // Forward declerations 00016 template <typename _Scalar, class _System> 00017 class EulerAngles; 00018 00019 namespace internal 00020 { 00021 // TODO: Check if already exists on the rest API 00022 template <int Num, bool IsPositive = (Num > 0)> 00023 struct Abs 00024 { 00025 enum { value = Num }; 00026 }; 00027 00028 template <int Num> 00029 struct Abs<Num, false> 00030 { 00031 enum { value = -Num }; 00032 }; 00033 00034 template <int Axis> 00035 struct IsValidAxis 00036 { 00037 enum { value = Axis != 0 && Abs<Axis>::value <= 3 }; 00038 }; 00039 } 00040 00041 #define EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(COND,MSG) typedef char static_assertion_##MSG[(COND)?1:-1] 00042 00055 enum EulerAxis 00056 { 00057 EULER_X = 1, 00058 EULER_Y = 2, 00059 EULER_Z = 3 00060 }; 00061 00119 template <int _AlphaAxis, int _BetaAxis, int _GammaAxis> 00120 class EulerSystem 00121 { 00122 public: 00123 // It's defined this way and not as enum, because I think 00124 // that enum is not guerantee to support negative numbers 00125 00127 static const int AlphaAxis = _AlphaAxis; 00128 00130 static const int BetaAxis = _BetaAxis; 00131 00133 static const int GammaAxis = _GammaAxis; 00134 00135 enum 00136 { 00137 AlphaAxisAbs = internal::Abs<AlphaAxis>::value, 00138 BetaAxisAbs = internal::Abs<BetaAxis>::value, 00139 GammaAxisAbs = internal::Abs<GammaAxis>::value, 00141 IsAlphaOpposite = (AlphaAxis < 0) ? 1 : 0, 00142 IsBetaOpposite = (BetaAxis < 0) ? 1 : 0, 00143 IsGammaOpposite = (GammaAxis < 0) ? 1 : 0, 00145 IsOdd = ((AlphaAxisAbs)%3 == (BetaAxisAbs - 1)%3) ? 0 : 1, 00146 IsEven = IsOdd ? 0 : 1, 00148 IsTaitBryan = ((unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0 00149 }; 00150 00151 private: 00152 00153 EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<AlphaAxis>::value, 00154 ALPHA_AXIS_IS_INVALID); 00155 00156 EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<BetaAxis>::value, 00157 BETA_AXIS_IS_INVALID); 00158 00159 EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(internal::IsValidAxis<GammaAxis>::value, 00160 GAMMA_AXIS_IS_INVALID); 00161 00162 EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT((unsigned)AlphaAxisAbs != (unsigned)BetaAxisAbs, 00163 ALPHA_AXIS_CANT_BE_EQUAL_TO_BETA_AXIS); 00164 00165 EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT((unsigned)BetaAxisAbs != (unsigned)GammaAxisAbs, 00166 BETA_AXIS_CANT_BE_EQUAL_TO_GAMMA_AXIS); 00167 00168 enum 00169 { 00170 // I, J, K are the pivot indexes permutation for the rotation matrix, that match this Euler system. 00171 // They are used in this class converters. 00172 // They are always different from each other, and their possible values are: 0, 1, or 2. 00173 I = AlphaAxisAbs - 1, 00174 J = (AlphaAxisAbs - 1 + 1 + IsOdd)%3, 00175 K = (AlphaAxisAbs - 1 + 2 - IsOdd)%3 00176 }; 00177 00178 // TODO: Get @mat parameter in form that avoids double evaluation. 00179 template <typename Derived> 00180 static void CalcEulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar, 3, 1>& res, const MatrixBase<Derived>& mat, internal::true_type /*isTaitBryan*/) 00181 { 00182 using std::atan2; 00183 using std::sin; 00184 using std::cos; 00185 00186 typedef typename Derived::Scalar Scalar; 00187 typedef Matrix<Scalar,2,1> Vector2; 00188 00189 res[0] = atan2(mat(J,K), mat(K,K)); 00190 Scalar c2 = Vector2(mat(I,I), mat(I,J)).norm(); 00191 if((IsOdd && res[0]<Scalar(0)) || ((!IsOdd) && res[0]>Scalar(0))) { 00192 if(res[0] > Scalar(0)) { 00193 res[0] -= Scalar(EIGEN_PI); 00194 } 00195 else { 00196 res[0] += Scalar(EIGEN_PI); 00197 } 00198 res[1] = atan2(-mat(I,K), -c2); 00199 } 00200 else 00201 res[1] = atan2(-mat(I,K), c2); 00202 Scalar s1 = sin(res[0]); 00203 Scalar c1 = cos(res[0]); 00204 res[2] = atan2(s1*mat(K,I)-c1*mat(J,I), c1*mat(J,J) - s1 * mat(K,J)); 00205 } 00206 00207 template <typename Derived> 00208 static void CalcEulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar,3,1>& res, const MatrixBase<Derived>& mat, internal::false_type /*isTaitBryan*/) 00209 { 00210 using std::atan2; 00211 using std::sin; 00212 using std::cos; 00213 00214 typedef typename Derived::Scalar Scalar; 00215 typedef Matrix<Scalar,2,1> Vector2; 00216 00217 res[0] = atan2(mat(J,I), mat(K,I)); 00218 if((IsOdd && res[0]<Scalar(0)) || ((!IsOdd) && res[0]>Scalar(0))) 00219 { 00220 if(res[0] > Scalar(0)) { 00221 res[0] -= Scalar(EIGEN_PI); 00222 } 00223 else { 00224 res[0] += Scalar(EIGEN_PI); 00225 } 00226 Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm(); 00227 res[1] = -atan2(s2, mat(I,I)); 00228 } 00229 else 00230 { 00231 Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm(); 00232 res[1] = atan2(s2, mat(I,I)); 00233 } 00234 00235 // With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first two angles, 00236 // we can compute their respective rotation, and apply its inverse to M. Since the result must 00237 // be a rotation around x, we have: 00238 // 00239 // c2 s1.s2 c1.s2 1 0 0 00240 // 0 c1 -s1 * M = 0 c3 s3 00241 // -s2 s1.c2 c1.c2 0 -s3 c3 00242 // 00243 // Thus: m11.c1 - m21.s1 = c3 & m12.c1 - m22.s1 = s3 00244 00245 Scalar s1 = sin(res[0]); 00246 Scalar c1 = cos(res[0]); 00247 res[2] = atan2(c1*mat(J,K)-s1*mat(K,K), c1*mat(J,J) - s1 * mat(K,J)); 00248 } 00249 00250 template<typename Scalar> 00251 static void CalcEulerAngles( 00252 EulerAngles<Scalar, EulerSystem>& res, 00253 const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat) 00254 { 00255 CalcEulerAngles(res, mat, false, false, false); 00256 } 00257 00258 template< 00259 bool PositiveRangeAlpha, 00260 bool PositiveRangeBeta, 00261 bool PositiveRangeGamma, 00262 typename Scalar> 00263 static void CalcEulerAngles( 00264 EulerAngles<Scalar, EulerSystem>& res, 00265 const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat) 00266 { 00267 CalcEulerAngles(res, mat, PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma); 00268 } 00269 00270 template<typename Scalar> 00271 static void CalcEulerAngles( 00272 EulerAngles<Scalar, EulerSystem>& res, 00273 const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat, 00274 bool PositiveRangeAlpha, 00275 bool PositiveRangeBeta, 00276 bool PositiveRangeGamma) 00277 { 00278 CalcEulerAngles_imp( 00279 res.angles(), mat, 00280 typename internal::conditional<IsTaitBryan, internal::true_type, internal::false_type>::type()); 00281 00282 if (IsAlphaOpposite == IsOdd) 00283 res.alpha() = -res.alpha(); 00284 00285 if (IsBetaOpposite == IsOdd) 00286 res.beta() = -res.beta(); 00287 00288 if (IsGammaOpposite == IsOdd) 00289 res.gamma() = -res.gamma(); 00290 00291 // Saturate results to the requested range 00292 if (PositiveRangeAlpha && (res.alpha() < 0)) 00293 res.alpha() += Scalar(2 * EIGEN_PI); 00294 00295 if (PositiveRangeBeta && (res.beta() < 0)) 00296 res.beta() += Scalar(2 * EIGEN_PI); 00297 00298 if (PositiveRangeGamma && (res.gamma() < 0)) 00299 res.gamma() += Scalar(2 * EIGEN_PI); 00300 } 00301 00302 template <typename _Scalar, class _System> 00303 friend class Eigen::EulerAngles; 00304 }; 00305 00306 #define EIGEN_EULER_SYSTEM_TYPEDEF(A, B, C) \ 00307 \ 00308 typedef EulerSystem<EULER_##A, EULER_##B, EULER_##C> EulerSystem##A##B##C; 00309 00310 EIGEN_EULER_SYSTEM_TYPEDEF(X,Y,Z) 00311 EIGEN_EULER_SYSTEM_TYPEDEF(X,Y,X) 00312 EIGEN_EULER_SYSTEM_TYPEDEF(X,Z,Y) 00313 EIGEN_EULER_SYSTEM_TYPEDEF(X,Z,X) 00314 00315 EIGEN_EULER_SYSTEM_TYPEDEF(Y,Z,X) 00316 EIGEN_EULER_SYSTEM_TYPEDEF(Y,Z,Y) 00317 EIGEN_EULER_SYSTEM_TYPEDEF(Y,X,Z) 00318 EIGEN_EULER_SYSTEM_TYPEDEF(Y,X,Y) 00319 00320 EIGEN_EULER_SYSTEM_TYPEDEF(Z,X,Y) 00321 EIGEN_EULER_SYSTEM_TYPEDEF(Z,X,Z) 00322 EIGEN_EULER_SYSTEM_TYPEDEF(Z,Y,X) 00323 EIGEN_EULER_SYSTEM_TYPEDEF(Z,Y,Z) 00324 } 00325 00326 #endif // EIGEN_EULERSYSTEM_H