update eigen

2 files changed, 246 insertions, 195 deletions

diff --git a/eigen/unsupported/Eigen/src/EulerAngles/EulerAngles.h b/eigen/unsupported/Eigen/src/EulerAngles/EulerAngles.h
index a5d034d..13a0da1 100644
--- a/eigen/unsupported/Eigen/src/EulerAngles/EulerAngles.h
+++ b/eigen/unsupported/Eigen/src/EulerAngles/EulerAngles.h
@@ -12,6 +12,11 @@
 
 namespace Eigen
 {
+  /*template<typename Other,
+         int OtherRows=Other::RowsAtCompileTime,
+         int OtherCols=Other::ColsAtCompileTime>
+  struct ei_eulerangles_assign_impl;*/
+
   /** \class EulerAngles
     *
     * \ingroup EulerAngles_Module
@@ -31,7 +36,7 @@ namespace Eigen
     * ### Rotation representation and conversions ###
     *
     * It has been proved(see Wikipedia link below) that every rotation can be represented
-    *  by Euler angles, but there is no single representation (e.g. unlike rotation matrices).
+    *  by Euler angles, but there is no singular representation (e.g. unlike rotation matrices).
     * Therefore, you can convert from Eigen rotation and to them
     *  (including rotation matrices, which is not called "rotations" by Eigen design).
     *
@@ -50,27 +55,33 @@ namespace Eigen
     * Additionally, some axes related computation is done in compile time.
     *
     * #### Euler angles ranges in conversions ####
-    * Rotations representation as EulerAngles are not single (unlike matrices),
-    *  and even have infinite EulerAngles representations.<BR>
-    * For example, add or subtract 2*PI from either angle of EulerAngles
-    *  and you'll get the same rotation.
-    * This is the general reason for infinite representation,
-    *  but it's not the only general reason for not having a single representation.
     *
-    * When converting rotation to EulerAngles, this class convert it to specific ranges
-    * When converting some rotation to EulerAngles, the rules for ranges are as follow:
-    * - If the rotation we converting from is an EulerAngles
-    *  (even when it represented as RotationBase explicitly), angles ranges are __undefined__.
-    * - otherwise, alpha and gamma angles will be in the range [-PI, PI].<BR>
-    *   As for Beta angle:
-    *    - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2].
-    *    - otherwise:
-    *      - If the beta axis is positive, the beta angle will be in the range [0, PI]
-    *      - If the beta axis is negative, the beta angle will be in the range [-PI, 0]
+    * When converting some rotation to Euler angles, there are some ways you can guarantee
+    *  the Euler angles ranges.
     *
+    * #### implicit ranges ####
+    * When using implicit ranges, all angles are guarantee to be in the range [-PI, +PI],
+    *  unless you convert from some other Euler angles.
+    * In this case, the range is __undefined__ (might be even less than -PI or greater than +2*PI).
     * \sa EulerAngles(const MatrixBase<Derived>&)
     * \sa EulerAngles(const RotationBase<Derived, 3>&)
     *
+    * #### explicit ranges ####
+    * When using explicit ranges, all angles are guarantee to be in the range you choose.
+    * In the range Boolean parameter, you're been ask whether you prefer the positive range or not:
+    * - _true_ - force the range between [0, +2*PI]
+    * - _false_ - force the range between [-PI, +PI]
+    *
+    * ##### compile time ranges #####
+    * This is when you have compile time ranges and you prefer to
+    *  use template parameter. (e.g. for performance)
+    * \sa FromRotation()
+    *
+    * ##### run-time time ranges #####
+    * Run-time ranges are also supported.
+    * \sa EulerAngles(const MatrixBase<Derived>&, bool, bool, bool)
+    * \sa EulerAngles(const RotationBase<Derived, 3>&, bool, bool, bool)
+    *
     * ### Convenient user typedefs ###
     *
     * Convenient typedefs for EulerAngles exist for float and double scalar,
@@ -92,7 +103,7 @@ namespace Eigen
     *
     * More information about Euler angles: https://en.wikipedia.org/wiki/Euler_angles
     *
-    * \tparam _Scalar the scalar type, i.e. the type of the angles.
+    * \tparam _Scalar the scalar type, i.e., the type of the angles.
     *
     * \tparam _System the EulerSystem to use, which represents the axes of rotation.
     */
@@ -100,11 +111,8 @@ namespace Eigen
   class EulerAngles : public RotationBase<EulerAngles<_Scalar, _System>, 3>
   {
     public:
-      typedef RotationBase<EulerAngles<_Scalar, _System>, 3> Base;
-      
       /** the scalar type of the angles */
       typedef _Scalar Scalar;
-      typedef typename NumTraits<Scalar>::Real RealScalar;
       
       /** the EulerSystem to use, which represents the axes of rotation. */
       typedef _System System;
@@ -138,56 +146,67 @@ namespace Eigen
     public:
       /** Default constructor without initialization. */
       EulerAngles() {}
-      /** Constructs and initialize an EulerAngles (\p alpha, \p beta, \p gamma). */
+      /** Constructs and initialize Euler angles(\p alpha, \p beta, \p gamma). */
       EulerAngles(const Scalar& alpha, const Scalar& beta, const Scalar& gamma) :
         m_angles(alpha, beta, gamma) {}
       
-      // TODO: Test this constructor
-      /** Constructs and initialize an EulerAngles from the array data {alpha, beta, gamma} */
-      explicit EulerAngles(const Scalar* data) : m_angles(data) {}
+      /** Constructs and initialize Euler angles from a 3x3 rotation matrix \p m.
+        *
+        * \note All angles will be in the range [-PI, PI].
+      */
+      template<typename Derived>
+      EulerAngles(const MatrixBase<Derived>& m) { *this = m; }
       
-      /** Constructs and initializes an EulerAngles from either:
-        *  - a 3x3 rotation matrix expression(i.e. pure orthogonal matrix with determinant of +1),
-        *  - a 3D vector expression representing Euler angles.
+      /** Constructs and initialize Euler angles from a 3x3 rotation matrix \p m,
+        *  with options to choose for each angle the requested range.
+        *
+        * If positive range is true, then the specified angle will be in the range [0, +2*PI].
+        * Otherwise, the specified angle will be in the range [-PI, +PI].
         *
-        * \note If \p other is a 3x3 rotation matrix, the angles range rules will be as follow:<BR>
-        *  Alpha and gamma angles will be in the range [-PI, PI].<BR>
-        *  As for Beta angle:
-        *   - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2].
-        *   - otherwise:
-        *     - If the beta axis is positive, the beta angle will be in the range [0, PI]
-        *     - If the beta axis is negative, the beta angle will be in the range [-PI, 0]
-       */
+        * \param m The 3x3 rotation matrix to convert
+        * \param positiveRangeAlpha If true, alpha will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
+        * \param positiveRangeBeta If true, beta will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
+        * \param positiveRangeGamma If true, gamma will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
+      */
       template<typename Derived>
-      explicit EulerAngles(const MatrixBase<Derived>& other) { *this = other; }
+      EulerAngles(
+        const MatrixBase<Derived>& m,
+        bool positiveRangeAlpha,
+        bool positiveRangeBeta,
+        bool positiveRangeGamma) {
+        
+        System::CalcEulerAngles(*this, m, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma);
+      }
       
       /** Constructs and initialize Euler angles from a rotation \p rot.
         *
-        * \note If \p rot is an EulerAngles (even when it represented as RotationBase explicitly),
-        *  angles ranges are __undefined__.
-        *  Otherwise, alpha and gamma angles will be in the range [-PI, PI].<BR>
-        *  As for Beta angle:
-        *   - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2].
-        *   - otherwise:
-        *     - If the beta axis is positive, the beta angle will be in the range [0, PI]
-        *     - If the beta axis is negative, the beta angle will be in the range [-PI, 0]
+        * \note All angles will be in the range [-PI, PI], unless \p rot is an EulerAngles.
+        *  If rot is an EulerAngles, expected EulerAngles range is __undefined__.
+        *  (Use other functions here for enforcing range if this effect is desired)
       */
       template<typename Derived>
-      EulerAngles(const RotationBase<Derived, 3>& rot) { System::CalcEulerAngles(*this, rot.toRotationMatrix()); }
+      EulerAngles(const RotationBase<Derived, 3>& rot) { *this = rot; }
       
-      /*EulerAngles(const QuaternionType& q)
-      {
-        // TODO: Implement it in a faster way for quaternions
-        // According to http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
-        //  we can compute only the needed matrix cells and then convert to euler angles. (see ZYX example below)
-        // Currently we compute all matrix cells from quaternion.
-
-        // Special case only for ZYX
-        //Scalar y2 = q.y() * q.y();
-        //m_angles[0] = std::atan2(2*(q.w()*q.z() + q.x()*q.y()), (1 - 2*(y2 + q.z()*q.z())));
-        //m_angles[1] = std::asin( 2*(q.w()*q.y() - q.z()*q.x()));
-        //m_angles[2] = std::atan2(2*(q.w()*q.x() + q.y()*q.z()), (1 - 2*(q.x()*q.x() + y2)));
-      }*/
+      /** Constructs and initialize Euler angles from a rotation \p rot,
+        *  with options to choose for each angle the requested range.
+        *
+        * If positive range is true, then the specified angle will be in the range [0, +2*PI].
+        * Otherwise, the specified angle will be in the range [-PI, +PI].
+        *
+        * \param rot The 3x3 rotation matrix to convert
+        * \param positiveRangeAlpha If true, alpha will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
+        * \param positiveRangeBeta If true, beta will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
+        * \param positiveRangeGamma If true, gamma will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
+      */
+      template<typename Derived>
+      EulerAngles(
+        const RotationBase<Derived, 3>& rot,
+        bool positiveRangeAlpha,
+        bool positiveRangeBeta,
+        bool positiveRangeGamma) {
+        
+        System::CalcEulerAngles(*this, rot.toRotationMatrix(), positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma);
+      }
 
       /** \returns The angle values stored in a vector (alpha, beta, gamma). */
       const Vector3& angles() const { return m_angles; }
@@ -227,48 +246,90 @@ namespace Eigen
         return inverse();
       }
       
-      /** Set \c *this from either:
-        *  - a 3x3 rotation matrix expression(i.e. pure orthogonal matrix with determinant of +1),
-        *  - a 3D vector expression representing Euler angles.
+      /** Constructs and initialize Euler angles from a 3x3 rotation matrix \p m,
+        *  with options to choose for each angle the requested range (__only in compile time__).
         *
-        * See EulerAngles(const MatrixBase<Derived, 3>&) for more information about
-        *  angles ranges output.
+        * If positive range is true, then the specified angle will be in the range [0, +2*PI].
+        * Otherwise, the specified angle will be in the range [-PI, +PI].
+        *
+        * \param m The 3x3 rotation matrix to convert
+        * \tparam positiveRangeAlpha If true, alpha will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
+        * \tparam positiveRangeBeta If true, beta will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
+        * \tparam positiveRangeGamma If true, gamma will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
+        */
+      template<
+        bool PositiveRangeAlpha,
+        bool PositiveRangeBeta,
+        bool PositiveRangeGamma,
+        typename Derived>
+      static EulerAngles FromRotation(const MatrixBase<Derived>& m)
+      {
+        EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3)
+        
+        EulerAngles e;
+        System::template CalcEulerAngles<
+          PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma, _Scalar>(e, m);
+        return e;
+      }
+      
+      /** Constructs and initialize Euler angles from a rotation \p rot,
+        *  with options to choose for each angle the requested range (__only in compile time__).
+        *
+        * If positive range is true, then the specified angle will be in the range [0, +2*PI].
+        * Otherwise, the specified angle will be in the range [-PI, +PI].
+        *
+        * \param rot The 3x3 rotation matrix to convert
+        * \tparam positiveRangeAlpha If true, alpha will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
+        * \tparam positiveRangeBeta If true, beta will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
+        * \tparam positiveRangeGamma If true, gamma will be in [0, 2*PI]. Otherwise, in [-PI, +PI].
       */
-      template<class Derived>
-      EulerAngles& operator=(const MatrixBase<Derived>& other)
+      template<
+        bool PositiveRangeAlpha,
+        bool PositiveRangeBeta,
+        bool PositiveRangeGamma,
+        typename Derived>
+      static EulerAngles FromRotation(const RotationBase<Derived, 3>& rot)
+      {
+        return FromRotation<PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma>(rot.toRotationMatrix());
+      }
+      
+      /*EulerAngles& fromQuaternion(const QuaternionType& q)
       {
-        EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename Derived::Scalar>::value),
-         YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+        // TODO: Implement it in a faster way for quaternions
+        // According to http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
+        //  we can compute only the needed matrix cells and then convert to euler angles. (see ZYX example below)
+        // Currently we compute all matrix cells from quaternion.
+
+        // Special case only for ZYX
+        //Scalar y2 = q.y() * q.y();
+        //m_angles[0] = std::atan2(2*(q.w()*q.z() + q.x()*q.y()), (1 - 2*(y2 + q.z()*q.z())));
+        //m_angles[1] = std::asin( 2*(q.w()*q.y() - q.z()*q.x()));
+        //m_angles[2] = std::atan2(2*(q.w()*q.x() + q.y()*q.z()), (1 - 2*(q.x()*q.x() + y2)));
+      }*/
+      
+      /** Set \c *this from a rotation matrix(i.e. pure orthogonal matrix with determinant of +1). */
+      template<typename Derived>
+      EulerAngles& operator=(const MatrixBase<Derived>& m) {
+        EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3)
         
-        internal::eulerangles_assign_impl<System, Derived>::run(*this, other.derived());
+        System::CalcEulerAngles(*this, m);
         return *this;
       }
 
       // TODO: Assign and construct from another EulerAngles (with different system)
       
-      /** Set \c *this from a rotation.
-        *
-        * See EulerAngles(const RotationBase<Derived, 3>&) for more information about
-        *  angles ranges output.
-      */
+      /** Set \c *this from a rotation. */
       template<typename Derived>
       EulerAngles& operator=(const RotationBase<Derived, 3>& rot) {
         System::CalcEulerAngles(*this, rot.toRotationMatrix());
         return *this;
       }
       
-      /** \returns \c true if \c *this is approximately equal to \a other, within the precision
-        * determined by \a prec.
-        *
-        * \sa MatrixBase::isApprox() */
-      bool isApprox(const EulerAngles& other,
-        const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const
-      { return angles().isApprox(other.angles(), prec); }
+      // TODO: Support isApprox function
 
       /** \returns an equivalent 3x3 rotation matrix. */
       Matrix3 toRotationMatrix() const
       {
-        // TODO: Calc it faster
         return static_cast<QuaternionType>(*this).toRotationMatrix();
       }
 
@@ -286,15 +347,6 @@ namespace Eigen
         s << eulerAngles.angles().transpose();
         return s;
       }
-      
-      /** \returns \c *this with scalar type casted to \a NewScalarType */
-      template <typename NewScalarType>
-      EulerAngles<NewScalarType, System> cast() const
-      {
-        EulerAngles<NewScalarType, System> e;
-        e.angles() = angles().template cast<NewScalarType>();
-        return e;
-      }
   };
 
 #define EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(AXES, SCALAR_TYPE, SCALAR_POSTFIX) \
@@ -327,29 +379,8 @@ EIGEN_EULER_ANGLES_TYPEDEFS(double, d)
     {
       typedef _Scalar Scalar;
     };
-    
-    // set from a rotation matrix
-    template<class System, class Other>
-    struct eulerangles_assign_impl<System,Other,3,3>
-    {
-      typedef typename Other::Scalar Scalar;
-      static void run(EulerAngles<Scalar, System>& e, const Other& m)
-      {
-        System::CalcEulerAngles(e, m);
-      }
-    };
-    
-    // set from a vector of Euler angles
-    template<class System, class Other>
-    struct eulerangles_assign_impl<System,Other,4,1>
-    {
-      typedef typename Other::Scalar Scalar;
-      static void run(EulerAngles<Scalar, System>& e, const Other& vec)
-      {
-        e.angles() = vec;
-      }
-    };
   }
+  
 }
 
 #endif // EIGEN_EULERANGLESCLASS_H
diff --git a/eigen/unsupported/Eigen/src/EulerAngles/EulerSystem.h b/eigen/unsupported/Eigen/src/EulerAngles/EulerSystem.h
index 28f52da..98f9f64 100644
--- a/eigen/unsupported/Eigen/src/EulerAngles/EulerSystem.h
+++ b/eigen/unsupported/Eigen/src/EulerAngles/EulerSystem.h
@@ -18,7 +18,7 @@ namespace Eigen
   
   namespace internal
   {
-    // TODO: Add this trait to the Eigen internal API?
+    // TODO: Check if already exists on the rest API
     template <int Num, bool IsPositive = (Num > 0)>
     struct Abs
     {
@@ -36,12 +36,6 @@ namespace Eigen
     {
       enum { value = Axis != 0 && Abs<Axis>::value <= 3 };
     };
-    
-    template<typename System,
-            typename Other,
-            int OtherRows=Other::RowsAtCompileTime,
-            int OtherCols=Other::ColsAtCompileTime>
-    struct eulerangles_assign_impl;
   }
   
   #define EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(COND,MSG) typedef char static_assertion_##MSG[(COND)?1:-1]
@@ -75,7 +69,7 @@ namespace Eigen
     *
     * You can use this class to get two things:
     *  - Build an Euler system, and then pass it as a template parameter to EulerAngles.
-    *  - Query some compile time data about an Euler system. (e.g. Whether it's Tait-Bryan)
+    *  - Query some compile time data about an Euler system. (e.g. Whether it's tait bryan)
     *
     * Euler rotation is a set of three rotation on fixed axes. (see \ref EulerAngles)
     * This meta-class store constantly those signed axes. (see \ref EulerAxis)
@@ -86,7 +80,7 @@ namespace Eigen
     *  signed axes{+X,+Y,+Z,-X,-Y,-Z} are supported:
     *  - all axes X, Y, Z in each valid order (see below what order is valid)
     *  - rotation over the axis is supported both over the positive and negative directions.
-    *  - both Tait-Bryan and proper/classic Euler angles (i.e. the opposite).
+    *  - both tait bryan and proper/classic Euler angles (i.e. the opposite).
     *
     * Since EulerSystem support both positive and negative directions,
     *  you may call this rotation distinction in other names:
@@ -96,7 +90,7 @@ namespace Eigen
     * Notice all axed combination are valid, and would trigger a static assertion.
     * Same unsigned axes can't be neighbors, e.g. {X,X,Y} is invalid.
     * This yield two and only two classes:
-    *  - _Tait-Bryan_ - all unsigned axes are distinct, e.g. {X,Y,Z}
+    *  - _tait bryan_ - all unsigned axes are distinct, e.g. {X,Y,Z}
     *  - _proper/classic Euler angles_ - The first and the third unsigned axes is equal,
     *     and the second is different, e.g. {X,Y,X}
     *
@@ -118,9 +112,9 @@ namespace Eigen
     *
     * \tparam _AlphaAxis the first fixed EulerAxis
     *
-    * \tparam _BetaAxis the second fixed EulerAxis
+    * \tparam _AlphaAxis the second fixed EulerAxis
     *
-    * \tparam _GammaAxis the third fixed EulerAxis
+    * \tparam _AlphaAxis the third fixed EulerAxis
     */
   template <int _AlphaAxis, int _BetaAxis, int _GammaAxis>
   class EulerSystem
@@ -144,16 +138,14 @@ namespace Eigen
       BetaAxisAbs = internal::Abs<BetaAxis>::value, /*!< the second rotation axis unsigned */
       GammaAxisAbs = internal::Abs<GammaAxis>::value, /*!< the third rotation axis unsigned */
       
-      IsAlphaOpposite = (AlphaAxis < 0) ? 1 : 0, /*!< whether alpha axis is negative */
-      IsBetaOpposite = (BetaAxis < 0) ? 1 : 0, /*!< whether beta axis is negative */
-      IsGammaOpposite = (GammaAxis < 0) ? 1 : 0, /*!< whether gamma axis is negative */
-
-      // Parity is even if alpha axis X is followed by beta axis Y, or Y is followed
-      // by Z, or Z is followed by X; otherwise it is odd.
-      IsOdd = ((AlphaAxisAbs)%3 == (BetaAxisAbs - 1)%3) ? 0 : 1, /*!< whether the Euler system is odd */
-      IsEven = IsOdd ? 0 : 1, /*!< whether the Euler system is even */
+      IsAlphaOpposite = (AlphaAxis < 0) ? 1 : 0, /*!< weather alpha axis is negative */
+      IsBetaOpposite = (BetaAxis < 0) ? 1 : 0, /*!< weather beta axis is negative */
+      IsGammaOpposite = (GammaAxis < 0) ? 1 : 0, /*!< weather gamma axis is negative */
+      
+      IsOdd = ((AlphaAxisAbs)%3 == (BetaAxisAbs - 1)%3) ? 0 : 1, /*!< weather the Euler system is odd */
+      IsEven = IsOdd ? 0 : 1, /*!< weather the Euler system is even */
 
-      IsTaitBryan = ((unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0 /*!< whether the Euler system is Tait-Bryan */
+      IsTaitBryan = ((unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0 /*!< weather the Euler system is tait bryan */
     };
     
     private:
@@ -188,70 +180,71 @@ namespace Eigen
     static void CalcEulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar, 3, 1>& res, const MatrixBase<Derived>& mat, internal::true_type /*isTaitBryan*/)
     {
       using std::atan2;
-      using std::sqrt;
+      using std::sin;
+      using std::cos;
       
       typedef typename Derived::Scalar Scalar;
-
-      const Scalar plusMinus = IsEven? 1 : -1;
-      const Scalar minusPlus = IsOdd?  1 : -1;
-
-      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);
-      res[1] = atan2(plusMinus * mat(I,K), Rsum);
-
-      // There is a singularity when cos(beta) == 0
-      if(Rsum > 4 * NumTraits<Scalar>::epsilon()) {// cos(beta) != 0
-        res[0] = atan2(minusPlus * mat(J, K), mat(K, K));
-        res[2] = atan2(minusPlus * mat(I, J), mat(I, I));
-      }
-      else if(plusMinus * mat(I, K) > 0) {// cos(beta) == 0 and sin(beta) == 1
-        Scalar spos = mat(J, I) + plusMinus * mat(K, J); // 2*sin(alpha + plusMinus * gamma
-        Scalar cpos = mat(J, J) + minusPlus * mat(K, I); // 2*cos(alpha + plusMinus * gamma)
-        Scalar alphaPlusMinusGamma = atan2(spos, cpos);
-        res[0] = alphaPlusMinusGamma;
-        res[2] = 0;
-      }
-      else {// cos(beta) == 0 and sin(beta) == -1
-        Scalar sneg = plusMinus * (mat(K, J) + minusPlus * mat(J, I)); // 2*sin(alpha + minusPlus*gamma)
-        Scalar cneg = mat(J, J) + plusMinus * mat(K, I);               // 2*cos(alpha + minusPlus*gamma)
-        Scalar alphaMinusPlusBeta = atan2(sneg, cneg);
-        res[0] = alphaMinusPlusBeta;
-        res[2] = 0;
+      typedef Matrix<Scalar,2,1> Vector2;
+      
+      res[0] = atan2(mat(J,K), mat(K,K));
+      Scalar c2 = Vector2(mat(I,I), mat(I,J)).norm();
+      if((IsOdd && res[0]<Scalar(0)) || ((!IsOdd) && res[0]>Scalar(0))) {
+        if(res[0] > Scalar(0)) {
+          res[0] -= Scalar(EIGEN_PI);
+        }
+        else {
+          res[0] += Scalar(EIGEN_PI);
+        }
+        res[1] = atan2(-mat(I,K), -c2);
       }
+      else
+        res[1] = atan2(-mat(I,K), c2);
+      Scalar s1 = sin(res[0]);
+      Scalar c1 = cos(res[0]);
+      res[2] = atan2(s1*mat(K,I)-c1*mat(J,I), c1*mat(J,J) - s1 * mat(K,J));
     }
 
     template <typename Derived>
-    static void CalcEulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar,3,1>& res,
-                                    const MatrixBase<Derived>& mat, internal::false_type /*isTaitBryan*/)
+    static void CalcEulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar,3,1>& res, const MatrixBase<Derived>& mat, internal::false_type /*isTaitBryan*/)
     {
       using std::atan2;
-      using std::sqrt;
+      using std::sin;
+      using std::cos;
 
       typedef typename Derived::Scalar Scalar;
-
-      const Scalar plusMinus = IsEven? 1 : -1;
-      const Scalar minusPlus = IsOdd?  1 : -1;
-
-      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);
-
-      res[1] = atan2(Rsum, mat(I, I));
-
-      // There is a singularity when sin(beta) == 0
-      if(Rsum > 4 * NumTraits<Scalar>::epsilon()) {// sin(beta) != 0
-        res[0] = atan2(mat(J, I), minusPlus * mat(K, I));
-        res[2] = atan2(mat(I, J), plusMinus * mat(I, K));
-      }
-      else if(mat(I, I) > 0) {// sin(beta) == 0 and cos(beta) == 1
-        Scalar spos = plusMinus * mat(K, J) + minusPlus * mat(J, K); // 2*sin(alpha + gamma)
-        Scalar cpos = mat(J, J) + mat(K, K);                         // 2*cos(alpha + gamma)
-        res[0] = atan2(spos, cpos);
-        res[2] = 0;
+      typedef Matrix<Scalar,2,1> Vector2;
+      
+      res[0] = atan2(mat(J,I), mat(K,I));
+      if((IsOdd && res[0]<Scalar(0)) || ((!IsOdd) && res[0]>Scalar(0)))
+      {
+        if(res[0] > Scalar(0)) {
+          res[0] -= Scalar(EIGEN_PI);
+        }
+        else {
+          res[0] += Scalar(EIGEN_PI);
+        }
+        Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm();
+        res[1] = -atan2(s2, mat(I,I));
       }
-      else {// sin(beta) == 0 and cos(beta) == -1
-        Scalar sneg = plusMinus * mat(K, J) + plusMinus * mat(J, K); // 2*sin(alpha - gamma)
-        Scalar cneg = mat(J, J) - mat(K, K);                         // 2*cos(alpha - gamma)
-        res[0] = atan2(sneg, cneg);
-        res[2] = 0;
+      else
+      {
+        Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm();
+        res[1] = atan2(s2, mat(I,I));
       }
+
+      // With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first two angles,
+      // we can compute their respective rotation, and apply its inverse to M. Since the result must
+      // be a rotation around x, we have:
+      //
+      //  c2  s1.s2 c1.s2                   1  0   0 
+      //  0   c1    -s1       *    M    =   0  c3  s3
+      //  -s2 s1.c2 c1.c2                   0 -s3  c3
+      //
+      //  Thus:  m11.c1 - m21.s1 = c3  &   m12.c1 - m22.s1 = s3
+
+      Scalar s1 = sin(res[0]);
+      Scalar c1 = cos(res[0]);
+      res[2] = atan2(c1*mat(J,K)-s1*mat(K,K), c1*mat(J,J) - s1 * mat(K,J));
     }
     
     template<typename Scalar>
@@ -259,28 +252,55 @@ namespace Eigen
       EulerAngles<Scalar, EulerSystem>& res,
       const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat)
     {
+      CalcEulerAngles(res, mat, false, false, false);
+    }
+    
+    template<
+      bool PositiveRangeAlpha,
+      bool PositiveRangeBeta,
+      bool PositiveRangeGamma,
+      typename Scalar>
+    static void CalcEulerAngles(
+      EulerAngles<Scalar, EulerSystem>& res,
+      const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat)
+    {
+      CalcEulerAngles(res, mat, PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma);
+    }
+    
+    template<typename Scalar>
+    static void CalcEulerAngles(
+      EulerAngles<Scalar, EulerSystem>& res,
+      const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat,
+      bool PositiveRangeAlpha,
+      bool PositiveRangeBeta,
+      bool PositiveRangeGamma)
+    {
       CalcEulerAngles_imp(
         res.angles(), mat,
         typename internal::conditional<IsTaitBryan, internal::true_type, internal::false_type>::type());
 
-      if (IsAlphaOpposite)
+      if (IsAlphaOpposite == IsOdd)
         res.alpha() = -res.alpha();
         
-      if (IsBetaOpposite)
+      if (IsBetaOpposite == IsOdd)
         res.beta() = -res.beta();
         
-      if (IsGammaOpposite)
+      if (IsGammaOpposite == IsOdd)
         res.gamma() = -res.gamma();
+      
+      // Saturate results to the requested range
+      if (PositiveRangeAlpha && (res.alpha() < 0))
+        res.alpha() += Scalar(2 * EIGEN_PI);
+      
+      if (PositiveRangeBeta && (res.beta() < 0))
+        res.beta() += Scalar(2 * EIGEN_PI);
+      
+      if (PositiveRangeGamma && (res.gamma() < 0))
+        res.gamma() += Scalar(2 * EIGEN_PI);
     }
     
     template <typename _Scalar, class _System>
     friend class Eigen::EulerAngles;
-    
-    template<typename System,
-            typename Other,
-            int OtherRows,
-            int OtherCols>
-    friend struct internal::eulerangles_assign_impl;
   };
 
 #define EIGEN_EULER_SYSTEM_TYPEDEF(A, B, C) \