![]() |
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_EULERANGLESCLASS_H// TODO: Fix previous "EIGEN_EULERANGLES_H" definition? 00011 #define EIGEN_EULERANGLESCLASS_H 00012 00013 namespace Eigen 00014 { 00015 /*template<typename Other, 00016 int OtherRows=Other::RowsAtCompileTime, 00017 int OtherCols=Other::ColsAtCompileTime> 00018 struct ei_eulerangles_assign_impl;*/ 00019 00110 template <typename _Scalar, class _System> 00111 class EulerAngles : public RotationBase<EulerAngles<_Scalar, _System>, 3> 00112 { 00113 public: 00115 typedef _Scalar Scalar; 00116 00118 typedef _System System; 00119 00120 typedef Matrix<Scalar,3,3> Matrix3; 00121 typedef Matrix<Scalar,3,1> Vector3; 00122 typedef Quaternion<Scalar> QuaternionType; 00123 typedef AngleAxis<Scalar> AngleAxisType; 00126 static Vector3 AlphaAxisVector() { 00127 const Vector3& u = Vector3::Unit(System::AlphaAxisAbs - 1); 00128 return System::IsAlphaOpposite ? -u : u; 00129 } 00130 00132 static Vector3 BetaAxisVector() { 00133 const Vector3& u = Vector3::Unit(System::BetaAxisAbs - 1); 00134 return System::IsBetaOpposite ? -u : u; 00135 } 00136 00138 static Vector3 GammaAxisVector() { 00139 const Vector3& u = Vector3::Unit(System::GammaAxisAbs - 1); 00140 return System::IsGammaOpposite ? -u : u; 00141 } 00142 00143 private: 00144 Vector3 m_angles; 00145 00146 public: 00148 EulerAngles() {} 00150 EulerAngles(const Scalar& alpha, const Scalar& beta, const Scalar& gamma) : 00151 m_angles(alpha, beta, gamma) {} 00152 00157 template<typename Derived> 00158 EulerAngles(const MatrixBase<Derived>& m) { *this = m; } 00159 00171 template<typename Derived> 00172 EulerAngles( 00173 const MatrixBase<Derived>& m, 00174 bool positiveRangeAlpha, 00175 bool positiveRangeBeta, 00176 bool positiveRangeGamma) { 00177 00178 System::CalcEulerAngles(*this, m, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma); 00179 } 00180 00187 template<typename Derived> 00188 EulerAngles(const RotationBase<Derived, 3>& rot) { *this = rot; } 00189 00201 template<typename Derived> 00202 EulerAngles( 00203 const RotationBase<Derived, 3>& rot, 00204 bool positiveRangeAlpha, 00205 bool positiveRangeBeta, 00206 bool positiveRangeGamma) { 00207 00208 System::CalcEulerAngles(*this, rot.toRotationMatrix(), positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma); 00209 } 00210 00212 const Vector3& angles() const { return m_angles; } 00214 Vector3& angles() { return m_angles; } 00215 00217 Scalar alpha() const { return m_angles[0]; } 00219 Scalar& alpha() { return m_angles[0]; } 00220 00222 Scalar beta() const { return m_angles[1]; } 00224 Scalar& beta() { return m_angles[1]; } 00225 00227 Scalar gamma() const { return m_angles[2]; } 00229 Scalar& gamma() { return m_angles[2]; } 00230 00234 EulerAngles inverse() const 00235 { 00236 EulerAngles res; 00237 res.m_angles = -m_angles; 00238 return res; 00239 } 00240 00244 EulerAngles operator -() const 00245 { 00246 return inverse(); 00247 } 00248 00260 template< 00261 bool PositiveRangeAlpha, 00262 bool PositiveRangeBeta, 00263 bool PositiveRangeGamma, 00264 typename Derived> 00265 static EulerAngles FromRotation(const MatrixBase<Derived>& m) 00266 { 00267 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3) 00268 00269 EulerAngles e; 00270 System::template CalcEulerAngles< 00271 PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma, _Scalar>(e, m); 00272 return e; 00273 } 00274 00286 template< 00287 bool PositiveRangeAlpha, 00288 bool PositiveRangeBeta, 00289 bool PositiveRangeGamma, 00290 typename Derived> 00291 static EulerAngles FromRotation(const RotationBase<Derived, 3>& rot) 00292 { 00293 return FromRotation<PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma>(rot.toRotationMatrix()); 00294 } 00295 00296 /*EulerAngles& fromQuaternion(const QuaternionType& q) 00297 { 00298 // TODO: Implement it in a faster way for quaternions 00299 // According to http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/ 00300 // we can compute only the needed matrix cells and then convert to euler angles. (see ZYX example below) 00301 // Currently we compute all matrix cells from quaternion. 00302 00303 // Special case only for ZYX 00304 //Scalar y2 = q.y() * q.y(); 00305 //m_angles[0] = std::atan2(2*(q.w()*q.z() + q.x()*q.y()), (1 - 2*(y2 + q.z()*q.z()))); 00306 //m_angles[1] = std::asin( 2*(q.w()*q.y() - q.z()*q.x())); 00307 //m_angles[2] = std::atan2(2*(q.w()*q.x() + q.y()*q.z()), (1 - 2*(q.x()*q.x() + y2))); 00308 }*/ 00309 00311 template<typename Derived> 00312 EulerAngles& operator=(const MatrixBase<Derived>& m) { 00313 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3) 00314 00315 System::CalcEulerAngles(*this, m); 00316 return *this; 00317 } 00318 00319 // TODO: Assign and construct from another EulerAngles (with different system) 00320 00322 template<typename Derived> 00323 EulerAngles& operator=(const RotationBase<Derived, 3>& rot) { 00324 System::CalcEulerAngles(*this, rot.toRotationMatrix()); 00325 return *this; 00326 } 00327 00328 // TODO: Support isApprox function 00329 00331 Matrix3 toRotationMatrix() const 00332 { 00333 return static_cast<QuaternionType>(*this).toRotationMatrix(); 00334 } 00335 00337 operator QuaternionType() const 00338 { 00339 return 00340 AngleAxisType(alpha(), AlphaAxisVector()) * 00341 AngleAxisType(beta(), BetaAxisVector()) * 00342 AngleAxisType(gamma(), GammaAxisVector()); 00343 } 00344 00345 friend std::ostream& operator<<(std::ostream& s, const EulerAngles<Scalar, System>& eulerAngles) 00346 { 00347 s << eulerAngles.angles().transpose(); 00348 return s; 00349 } 00350 }; 00351 00352 #define EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(AXES, SCALAR_TYPE, SCALAR_POSTFIX) \ 00353 \ 00354 typedef EulerAngles<SCALAR_TYPE, EulerSystem##AXES> EulerAngles##AXES##SCALAR_POSTFIX; 00355 00356 #define EIGEN_EULER_ANGLES_TYPEDEFS(SCALAR_TYPE, SCALAR_POSTFIX) \ 00357 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XYZ, SCALAR_TYPE, SCALAR_POSTFIX) \ 00358 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XYX, SCALAR_TYPE, SCALAR_POSTFIX) \ 00359 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XZY, SCALAR_TYPE, SCALAR_POSTFIX) \ 00360 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(XZX, SCALAR_TYPE, SCALAR_POSTFIX) \ 00361 \ 00362 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YZX, SCALAR_TYPE, SCALAR_POSTFIX) \ 00363 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YZY, SCALAR_TYPE, SCALAR_POSTFIX) \ 00364 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YXZ, SCALAR_TYPE, SCALAR_POSTFIX) \ 00365 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(YXY, SCALAR_TYPE, SCALAR_POSTFIX) \ 00366 \ 00367 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZXY, SCALAR_TYPE, SCALAR_POSTFIX) \ 00368 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZXZ, SCALAR_TYPE, SCALAR_POSTFIX) \ 00369 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZYX, SCALAR_TYPE, SCALAR_POSTFIX) \ 00370 EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(ZYZ, SCALAR_TYPE, SCALAR_POSTFIX) 00371 00372 EIGEN_EULER_ANGLES_TYPEDEFS(float, f) 00373 EIGEN_EULER_ANGLES_TYPEDEFS(double, d) 00374 00375 namespace internal 00376 { 00377 template<typename _Scalar, class _System> 00378 struct traits<EulerAngles<_Scalar, _System> > 00379 { 00380 typedef _Scalar Scalar; 00381 }; 00382 } 00383 00384 } 00385 00386 #endif // EIGEN_EULERANGLESCLASS_H