@smake/eigen 1.1.0 → 1.1.1

package/eigen/Eigen/src/Geometry/AngleAxis.h

@@ -10,82 +10,85 @@
 #ifndef EIGEN_ANGLEAXIS_H
 #define EIGEN_ANGLEAXIS_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
 
 /** \geometry_module \ingroup Geometry_Module
-  *
-  * \class AngleAxis
-  *
-  * \brief Represents a 3D rotation as a rotation angle around an arbitrary 3D axis
-  *
-  * \param _Scalar the scalar type, i.e., the type of the coefficients.
-  *
-  * \warning When setting up an AngleAxis object, the axis vector \b must \b be \b normalized.
-  *
-  * The following two typedefs are provided for convenience:
-  * \li \c AngleAxisf for \c float
-  * \li \c AngleAxisd for \c double
-  *
-  * Combined with MatrixBase::Unit{X,Y,Z}, AngleAxis can be used to easily
-  * mimic Euler-angles. Here is an example:
-  * \include AngleAxis_mimic_euler.cpp
-  * Output: \verbinclude AngleAxis_mimic_euler.out
-  *
-  * \note This class is not aimed to be used to store a rotation transformation,
-  * but rather to make easier the creation of other rotation (Quaternion, rotation Matrix)
-  * and transformation objects.
-  *
-  * \sa class Quaternion, class Transform, MatrixBase::UnitX()
-  */
+ *
+ * \class AngleAxis
+ *
+ * \brief Represents a 3D rotation as a rotation angle around an arbitrary 3D axis
+ *
+ * \param Scalar_ the scalar type, i.e., the type of the coefficients.
+ *
+ * \warning When setting up an AngleAxis object, the axis vector \b must \b be \b normalized.
+ *
+ * The following two typedefs are provided for convenience:
+ * \li \c AngleAxisf for \c float
+ * \li \c AngleAxisd for \c double
+ *
+ * Combined with MatrixBase::Unit{X,Y,Z}, AngleAxis can be used to easily
+ * mimic Euler-angles. Here is an example:
+ * \include AngleAxis_mimic_euler.cpp
+ * Output: \verbinclude AngleAxis_mimic_euler.out
+ *
+ * \note This class is not aimed to be used to store a rotation transformation,
+ * but rather to make easier the creation of other rotation (Quaternion, rotation Matrix)
+ * and transformation objects.
+ *
+ * \sa class Quaternion, class Transform, MatrixBase::UnitX()
+ */
 
 namespace internal {
-template<typename _Scalar> struct traits<AngleAxis<_Scalar> >
-{
-  typedef _Scalar Scalar;
+template <typename Scalar_>
+struct traits<AngleAxis<Scalar_> > {
+  typedef Scalar_ Scalar;
 };
-}
+}  // namespace internal
 
-template<typename _Scalar>
-class AngleAxis : public RotationBase<AngleAxis<_Scalar>,3>
-{
-  typedef RotationBase<AngleAxis<_Scalar>,3> Base;
-
-public:
+template <typename Scalar_>
+class AngleAxis : public RotationBase<AngleAxis<Scalar_>, 3> {
+  typedef RotationBase<AngleAxis<Scalar_>, 3> Base;
 
+ public:
   using Base::operator*;
 
   enum { Dim = 3 };
   /** the scalar type of the coefficients */
-  typedef _Scalar Scalar;
-  typedef Matrix<Scalar,3,3> Matrix3;
-  typedef Matrix<Scalar,3,1> Vector3;
+  typedef Scalar_ Scalar;
+  typedef Matrix<Scalar, 3, 3> Matrix3;
+  typedef Matrix<Scalar, 3, 1> Vector3;
   typedef Quaternion<Scalar> QuaternionType;
 
-protected:
-
+ protected:
   Vector3 m_axis;
   Scalar m_angle;
 
-public:
-
+ public:
   /** Default constructor without initialization. */
   EIGEN_DEVICE_FUNC AngleAxis() {}
   /** Constructs and initialize the angle-axis rotation from an \a angle in radian
-    * and an \a axis which \b must \b be \b normalized.
-    *
-    * \warning If the \a axis vector is not normalized, then the angle-axis object
-    *          represents an invalid rotation. */
-  template<typename Derived>
-  EIGEN_DEVICE_FUNC 
-  inline AngleAxis(const Scalar& angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {}
+   * and an \a axis which \b must \b be \b normalized.
+   *
+   * \warning If the \a axis vector is not normalized, then the angle-axis object
+   *          represents an invalid rotation. */
+  template <typename Derived>
+  EIGEN_DEVICE_FUNC inline AngleAxis(const Scalar& angle, const MatrixBase<Derived>& axis)
+      : m_axis(axis), m_angle(angle) {}
   /** Constructs and initialize the angle-axis rotation from a quaternion \a q.
-    * This function implicitly normalizes the quaternion \a q.
-    */
-  template<typename QuatDerived> 
-  EIGEN_DEVICE_FUNC inline explicit AngleAxis(const QuaternionBase<QuatDerived>& q) { *this = q; }
+   * This function implicitly normalizes the quaternion \a q.
+   */
+  template <typename QuatDerived>
+  EIGEN_DEVICE_FUNC inline explicit AngleAxis(const QuaternionBase<QuatDerived>& q) {
+    *this = q;
+  }
   /** Constructs and initialize the angle-axis rotation from a 3x3 rotation matrix. */
-  template<typename Derived>
-  EIGEN_DEVICE_FUNC inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; }
+  template <typename Derived>
+  EIGEN_DEVICE_FUNC inline explicit AngleAxis(const MatrixBase<Derived>& m) {
+    *this = m;
+  }
 
   /** \returns the value of the rotation angle in radian */
   EIGEN_DEVICE_FUNC Scalar angle() const { return m_angle; }
@@ -95,49 +98,52 @@ public:
   /** \returns the rotation axis */
   EIGEN_DEVICE_FUNC const Vector3& axis() const { return m_axis; }
   /** \returns a read-write reference to the stored rotation axis.
-    *
-    * \warning The rotation axis must remain a \b unit vector.
-    */
+   *
+   * \warning The rotation axis must remain a \b unit vector.
+   */
   EIGEN_DEVICE_FUNC Vector3& axis() { return m_axis; }
 
   /** Concatenates two rotations */
-  EIGEN_DEVICE_FUNC inline QuaternionType operator* (const AngleAxis& other) const
-  { return QuaternionType(*this) * QuaternionType(other); }
+  EIGEN_DEVICE_FUNC inline QuaternionType operator*(const AngleAxis& other) const {
+    return QuaternionType(*this) * QuaternionType(other);
+  }
 
   /** Concatenates two rotations */
-  EIGEN_DEVICE_FUNC inline QuaternionType operator* (const QuaternionType& other) const
-  { return QuaternionType(*this) * other; }
+  EIGEN_DEVICE_FUNC inline QuaternionType operator*(const QuaternionType& other) const {
+    return QuaternionType(*this) * other;
+  }
 
   /** Concatenates two rotations */
-  friend EIGEN_DEVICE_FUNC inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b)
-  { return a * QuaternionType(b); }
+  friend EIGEN_DEVICE_FUNC inline QuaternionType operator*(const QuaternionType& a, const AngleAxis& b) {
+    return a * QuaternionType(b);
+  }
 
   /** \returns the inverse rotation, i.e., an angle-axis with opposite rotation angle */
-  EIGEN_DEVICE_FUNC AngleAxis inverse() const
-  { return AngleAxis(-m_angle, m_axis); }
+  EIGEN_DEVICE_FUNC AngleAxis inverse() const { return AngleAxis(-m_angle, m_axis); }
 
-  template<class QuatDerived>
+  template <class QuatDerived>
   EIGEN_DEVICE_FUNC AngleAxis& operator=(const QuaternionBase<QuatDerived>& q);
-  template<typename Derived>
+  template <typename Derived>
   EIGEN_DEVICE_FUNC AngleAxis& operator=(const MatrixBase<Derived>& m);
 
-  template<typename Derived>
+  template <typename Derived>
   EIGEN_DEVICE_FUNC AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m);
   EIGEN_DEVICE_FUNC Matrix3 toRotationMatrix(void) const;
 
   /** \returns \c *this with scalar type casted to \a NewScalarType
-    *
-    * Note that if \a NewScalarType is equal to the current scalar type of \c *this
-    * then this function smartly returns a const reference to \c *this.
-    */
-  template<typename NewScalarType>
-  EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const
-  { return typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); }
+   *
+   * Note that if \a NewScalarType is equal to the current scalar type of \c *this
+   * then this function smartly returns a const reference to \c *this.
+   */
+  template <typename NewScalarType>
+  EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<AngleAxis, AngleAxis<NewScalarType> >::type cast()
+      const {
+    return typename internal::cast_return_type<AngleAxis, AngleAxis<NewScalarType> >::type(*this);
+  }
 
   /** Copy constructor with scalar type conversion */
-  template<typename OtherScalarType>
-  EIGEN_DEVICE_FUNC inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other)
-  {
+  template <typename OtherScalarType>
+  EIGEN_DEVICE_FUNC inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other) {
     m_axis = other.axis().template cast<Scalar>();
     m_angle = Scalar(other.angle());
   }
@@ -145,45 +151,41 @@ public:
   EIGEN_DEVICE_FUNC static inline const AngleAxis Identity() { return AngleAxis(Scalar(0), Vector3::UnitX()); }
 
   /** \returns \c true if \c *this is approximately equal to \a other, within the precision
-    * determined by \a prec.
-    *
-    * \sa MatrixBase::isApprox() */
-  EIGEN_DEVICE_FUNC bool isApprox(const AngleAxis& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
-  { return m_axis.isApprox(other.m_axis, prec) && internal::isApprox(m_angle,other.m_angle, prec); }
+   * determined by \a prec.
+   *
+   * \sa MatrixBase::isApprox() */
+  EIGEN_DEVICE_FUNC bool isApprox(const AngleAxis& other, const typename NumTraits<Scalar>::Real& prec =
+                                                              NumTraits<Scalar>::dummy_precision()) const {
+    return m_axis.isApprox(other.m_axis, prec) && internal::isApprox(m_angle, other.m_angle, prec);
+  }
 };
 
 /** \ingroup Geometry_Module
-  * single precision angle-axis type */
+ * single precision angle-axis type */
 typedef AngleAxis<float> AngleAxisf;
 /** \ingroup Geometry_Module
-  * double precision angle-axis type */
+ * double precision angle-axis type */
 typedef AngleAxis<double> AngleAxisd;
 
 /** Set \c *this from a \b unit quaternion.
-  *
-  * The resulting axis is normalized, and the computed angle is in the [0,pi] range.
-  * 
-  * This function implicitly normalizes the quaternion \a q.
-  */
-template<typename Scalar>
-template<typename QuatDerived>
-EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived>& q)
-{
+ *
+ * The resulting axis is normalized, and the computed angle is in the [0,pi] range.
+ *
+ * This function implicitly normalizes the quaternion \a q.
+ */
+template <typename Scalar>
+template <typename QuatDerived>
+EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived>& q) {
   EIGEN_USING_STD(atan2)
   EIGEN_USING_STD(abs)
   Scalar n = q.vec().norm();
-  if(n<NumTraits<Scalar>::epsilon())
-    n = q.vec().stableNorm();
-
-  if (n != Scalar(0))
-  {
-    m_angle = Scalar(2)*atan2(n, abs(q.w()));
-    if(q.w() < Scalar(0))
-      n = -n;
-    m_axis  = q.vec() / n;
-  }
-  else
-  {
+  if (n < NumTraits<Scalar>::epsilon()) n = q.vec().stableNorm();
+
+  if (n != Scalar(0)) {
+    m_angle = Scalar(2) * atan2(n, abs(q.w()));
+    if (q.w() < Scalar(0)) n = -n;
+    m_axis = q.vec() / n;
+  } else {
     m_angle = Scalar(0);
     m_axis << Scalar(1), Scalar(0), Scalar(0);
   }
@@ -191,57 +193,53 @@ EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const Quaterni
 }
 
 /** Set \c *this from a 3x3 rotation matrix \a mat.
-  */
-template<typename Scalar>
-template<typename Derived>
-EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat)
-{
+ */
+template <typename Scalar>
+template <typename Derived>
+EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat) {
   // Since a direct conversion would not be really faster,
   // let's use the robust Quaternion implementation:
   return *this = QuaternionType(mat);
 }
 
 /**
-* \brief Sets \c *this from a 3x3 rotation matrix.
-**/
-template<typename Scalar>
-template<typename Derived>
-EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat)
-{
+ * \brief Sets \c *this from a 3x3 rotation matrix.
+ **/
+template <typename Scalar>
+template <typename Derived>
+EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat) {
   return *this = QuaternionType(mat);
 }
 
 /** Constructs and \returns an equivalent 3x3 rotation matrix.
-  */
-template<typename Scalar>
-typename AngleAxis<Scalar>::Matrix3
-EIGEN_DEVICE_FUNC AngleAxis<Scalar>::toRotationMatrix(void) const
-{
+ */
+template <typename Scalar>
+typename AngleAxis<Scalar>::Matrix3 EIGEN_DEVICE_FUNC AngleAxis<Scalar>::toRotationMatrix(void) const {
   EIGEN_USING_STD(sin)
   EIGEN_USING_STD(cos)
   Matrix3 res;
-  Vector3 sin_axis  = sin(m_angle) * m_axis;
+  Vector3 sin_axis = sin(m_angle) * m_axis;
   Scalar c = cos(m_angle);
-  Vector3 cos1_axis = (Scalar(1)-c) * m_axis;
+  Vector3 cos1_axis = (Scalar(1) - c) * m_axis;
 
   Scalar tmp;
   tmp = cos1_axis.x() * m_axis.y();
-  res.coeffRef(0,1) = tmp - sin_axis.z();
-  res.coeffRef(1,0) = tmp + sin_axis.z();
+  res.coeffRef(0, 1) = tmp - sin_axis.z();
+  res.coeffRef(1, 0) = tmp + sin_axis.z();
 
   tmp = cos1_axis.x() * m_axis.z();
-  res.coeffRef(0,2) = tmp + sin_axis.y();
-  res.coeffRef(2,0) = tmp - sin_axis.y();
+  res.coeffRef(0, 2) = tmp + sin_axis.y();
+  res.coeffRef(2, 0) = tmp - sin_axis.y();
 
   tmp = cos1_axis.y() * m_axis.z();
-  res.coeffRef(1,2) = tmp - sin_axis.x();
-  res.coeffRef(2,1) = tmp + sin_axis.x();
+  res.coeffRef(1, 2) = tmp - sin_axis.x();
+  res.coeffRef(2, 1) = tmp + sin_axis.x();
 
   res.diagonal() = (cos1_axis.cwiseProduct(m_axis)).array() + c;
 
   return res;
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_ANGLEAXIS_H
+#endif  // EIGEN_ANGLEAXIS_H

package/eigen/Eigen/src/Geometry/EulerAngles.h

@@ -2,6 +2,7 @@
 // for linear algebra.
 //
 // Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2023 Juraj Oršulić, University of Zagreb <juraj.orsulic@fer.hr>
 //
 // This Source Code Form is subject to the terms of the Mozilla
 // Public License v. 2.0. If a copy of the MPL was not distributed
@@ -10,105 +11,193 @@
 #ifndef EIGEN_EULERANGLES_H
 #define EIGEN_EULERANGLES_H
 
-namespace Eigen { 
+// IWYU pragma: private
+#include "./InternalHeaderCheck.h"
+
+namespace Eigen {
+
+/** \geometry_module \ingroup Geometry_Module
+ *
+ *
+ * \returns the canonical Euler-angles of the rotation matrix \c *this using the convention defined by the triplet (\a
+ * a0,\a a1,\a a2)
+ *
+ * Each of the three parameters \a a0,\a a1,\a a2 represents the respective rotation axis as an integer in {0,1,2}.
+ * For instance, in:
+ * \code Vector3f ea = mat.eulerAngles(2, 0, 2); \endcode
+ * "2" represents the z axis and "0" the x axis, etc. The returned angles are such that
+ * we have the following equality:
+ * \code
+ * mat == AngleAxisf(ea[0], Vector3f::UnitZ())
+ *      * AngleAxisf(ea[1], Vector3f::UnitX())
+ *      * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode
+ * This corresponds to the right-multiply conventions (with right hand side frames).
+ *
+ * For Tait-Bryan angle configurations (a0 != a2), the returned angles are in the ranges [-pi:pi]x[-pi/2:pi/2]x[-pi:pi].
+ * For proper Euler angle configurations (a0 == a2), the returned angles are in the ranges [-pi:pi]x[0:pi]x[-pi:pi].
+ *
+ * The approach used is also described here:
+ * https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2012/07/euler-angles.pdf
+ *
+ * \sa class AngleAxis
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline Matrix<typename MatrixBase<Derived>::Scalar, 3, 1> MatrixBase<Derived>::canonicalEulerAngles(
+    Index a0, Index a1, Index a2) const {
+  /* Implemented from Graphics Gems IV */
+  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3)
+
+  Matrix<Scalar, 3, 1> res;
+
+  const Index odd = ((a0 + 1) % 3 == a1) ? 0 : 1;
+  const Index i = a0;
+  const Index j = (a0 + 1 + odd) % 3;
+  const Index k = (a0 + 2 - odd) % 3;
+
+  if (a0 == a2) {
+    // Proper Euler angles (same first and last axis).
+    // The i, j, k indices enable addressing the input matrix as the XYX archetype matrix (see Graphics Gems IV),
+    // where e.g. coeff(k, i) means third column, first row in the XYX archetype matrix:
+    //  c2      s2s1              s2c1
+    //  s2s3   -c2s1s3 + c1c3    -c2c1s3 - s1c3
+    // -s2c3    c2s1c3 + c1s3     c2c1c3 - s1s3
+
+    // Note: s2 is always positive.
+    Scalar s2 = numext::hypot(coeff(j, i), coeff(k, i));
+    if (odd) {
+      res[0] = numext::atan2(coeff(j, i), coeff(k, i));
+      // s2 is always positive, so res[1] will be within the canonical [0, pi] range
+      res[1] = numext::atan2(s2, coeff(i, i));
+    } else {
+      // In the !odd case, signs of all three angles are flipped at the very end. To keep the solution within the
+      // canonical range, we flip the solution and make res[1] always negative here (since s2 is always positive,
+      // -atan2(s2, c2) will always be negative). The final flip at the end due to !odd will thus make res[1] positive
+      // and canonical. NB: in the general case, there are two correct solutions, but only one is canonical. For proper
+      // Euler angles, flipping from one solution to the other involves flipping the sign of the second angle res[1] and
+      // adding/subtracting pi to the first and third angles. The addition/subtraction of pi to the first angle res[0]
+      // is handled here by flipping the signs of arguments to atan2, while the calculation of the third angle does not
+      // need special adjustment since it uses the adjusted res[0] as the input and produces a correct result.
+      res[0] = numext::atan2(-coeff(j, i), -coeff(k, i));
+      res[1] = -numext::atan2(s2, coeff(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 = numext::sin(res[0]);
+    Scalar c1 = numext::cos(res[0]);
+    res[2] = numext::atan2(c1 * coeff(j, k) - s1 * coeff(k, k), c1 * coeff(j, j) - s1 * coeff(k, j));
+  } else {
+    // Tait-Bryan angles (all three axes are different; typically used for yaw-pitch-roll calculations).
+    // The i, j, k indices enable addressing the input matrix as the XYZ archetype matrix (see Graphics Gems IV),
+    // where e.g. coeff(k, i) means third column, first row in the XYZ archetype matrix:
+    //  c2c3    s2s1c3 - c1s3     s2c1c3 + s1s3
+    //  c2s3    s2s1s3 + c1c3     s2c1s3 - s1c3
+    // -s2      c2s1              c2c1
+
+    res[0] = numext::atan2(coeff(j, k), coeff(k, k));
+
+    Scalar c2 = numext::hypot(coeff(i, i), coeff(i, j));
+    // c2 is always positive, so the following atan2 will always return a result in the correct canonical middle angle
+    // range [-pi/2, pi/2]
+    res[1] = numext::atan2(-coeff(i, k), c2);
+
+    Scalar s1 = numext::sin(res[0]);
+    Scalar c1 = numext::cos(res[0]);
+    res[2] = numext::atan2(s1 * coeff(k, i) - c1 * coeff(j, i), c1 * coeff(j, j) - s1 * coeff(k, j));
+  }
+  if (!odd) {
+    res = -res;
+  }
+
+  return res;
+}
 
 /** \geometry_module \ingroup Geometry_Module
-  *
-  *
-  * \returns the Euler-angles of the rotation matrix \c *this using the convention defined by the triplet (\a a0,\a a1,\a a2)
-  *
-  * Each of the three parameters \a a0,\a a1,\a a2 represents the respective rotation axis as an integer in {0,1,2}.
-  * For instance, in:
-  * \code Vector3f ea = mat.eulerAngles(2, 0, 2); \endcode
-  * "2" represents the z axis and "0" the x axis, etc. The returned angles are such that
-  * we have the following equality:
-  * \code
-  * mat == AngleAxisf(ea[0], Vector3f::UnitZ())
-  *      * AngleAxisf(ea[1], Vector3f::UnitX())
-  *      * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode
-  * This corresponds to the right-multiply conventions (with right hand side frames).
-  * 
-  * The returned angles are in the ranges [0:pi]x[-pi:pi]x[-pi:pi].
-  * 
-  * \sa class AngleAxis
-  */
-template<typename Derived>
-EIGEN_DEVICE_FUNC inline Matrix<typename MatrixBase<Derived>::Scalar,3,1>
-MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const
-{
-  EIGEN_USING_STD(atan2)
-  EIGEN_USING_STD(sin)
-  EIGEN_USING_STD(cos)
+ *
+ *
+ * \returns the Euler-angles of the rotation matrix \c *this using the convention defined by the triplet (\a a0,\a a1,\a
+ * a2)
+ *
+ * NB: The returned angles are in non-canonical ranges [0:pi]x[-pi:pi]x[-pi:pi]. For canonical Tait-Bryan/proper Euler
+ * ranges, use canonicalEulerAngles.
+ *
+ * \sa MatrixBase::canonicalEulerAngles
+ * \sa class AngleAxis
+ */
+template <typename Derived>
+EIGEN_DEVICE_FUNC inline Matrix<typename MatrixBase<Derived>::Scalar, 3, 1> MatrixBase<Derived>::eulerAngles(
+    Index a0, Index a1, Index a2) const {
   /* Implemented from Graphics Gems IV */
-  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3)
+  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3)
 
-  Matrix<Scalar,3,1> res;
-  typedef Matrix<typename Derived::Scalar,2,1> Vector2;
+  Matrix<Scalar, 3, 1> res;
 
-  const Index odd = ((a0+1)%3 == a1) ? 0 : 1;
+  const Index odd = ((a0 + 1) % 3 == a1) ? 0 : 1;
   const Index i = a0;
-  const Index j = (a0 + 1 + odd)%3;
-  const Index k = (a0 + 2 - odd)%3;
-  
-  if (a0==a2)
-  {
-    res[0] = atan2(coeff(j,i), coeff(k,i));
-    if((odd && res[0]<Scalar(0)) || ((!odd) && res[0]>Scalar(0)))
-    {
-      if(res[0] > Scalar(0)) {
+  const Index j = (a0 + 1 + odd) % 3;
+  const Index k = (a0 + 2 - odd) % 3;
+
+  if (a0 == a2) {
+    res[0] = numext::atan2(coeff(j, i), coeff(k, i));
+    if ((odd && res[0] < Scalar(0)) || ((!odd) && res[0] > Scalar(0))) {
+      if (res[0] > Scalar(0)) {
         res[0] -= Scalar(EIGEN_PI);
-      }
-      else {
+      } else {
         res[0] += Scalar(EIGEN_PI);
       }
-      Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm();
-      res[1] = -atan2(s2, coeff(i,i));
-    }
-    else
-    {
-      Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm();
-      res[1] = atan2(s2, coeff(i,i));
+
+      Scalar s2 = numext::hypot(coeff(j, i), coeff(k, i));
+      res[1] = -numext::atan2(s2, coeff(i, i));
+    } else {
+      Scalar s2 = numext::hypot(coeff(j, i), coeff(k, i));
+      res[1] = numext::atan2(s2, coeff(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 
+    //  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*coeff(j,k)-s1*coeff(k,k), c1*coeff(j,j) - s1 * coeff(k,j));
-  } 
-  else
-  {
-    res[0] = atan2(coeff(j,k), coeff(k,k));
-    Scalar c2 = Vector2(coeff(i,i), coeff(i,j)).norm();
-    if((odd && res[0]<Scalar(0)) || ((!odd) && res[0]>Scalar(0))) {
-      if(res[0] > Scalar(0)) {
+
+    Scalar s1 = numext::sin(res[0]);
+    Scalar c1 = numext::cos(res[0]);
+    res[2] = numext::atan2(c1 * coeff(j, k) - s1 * coeff(k, k), c1 * coeff(j, j) - s1 * coeff(k, j));
+  } else {
+    res[0] = numext::atan2(coeff(j, k), coeff(k, k));
+    Scalar c2 = numext::hypot(coeff(i, i), coeff(i, j));
+    if ((odd && res[0] < Scalar(0)) || ((!odd) && res[0] > Scalar(0))) {
+      if (res[0] > Scalar(0)) {
         res[0] -= Scalar(EIGEN_PI);
-      }
-      else {
+      } else {
         res[0] += Scalar(EIGEN_PI);
       }
-      res[1] = atan2(-coeff(i,k), -c2);
+      res[1] = numext::atan2(-coeff(i, k), -c2);
+    } else {
+      res[1] = numext::atan2(-coeff(i, k), c2);
     }
-    else
-      res[1] = atan2(-coeff(i,k), c2);
-    Scalar s1 = sin(res[0]);
-    Scalar c1 = cos(res[0]);
-    res[2] = atan2(s1*coeff(k,i)-c1*coeff(j,i), c1*coeff(j,j) - s1 * coeff(k,j));
+    Scalar s1 = numext::sin(res[0]);
+    Scalar c1 = numext::cos(res[0]);
+    res[2] = numext::atan2(s1 * coeff(k, i) - c1 * coeff(j, i), c1 * coeff(j, j) - s1 * coeff(k, j));
   }
-  if (!odd)
+  if (!odd) {
     res = -res;
-  
+  }
+
   return res;
 }
 
-} // end namespace Eigen
+}  // end namespace Eigen
 
-#endif // EIGEN_EULERANGLES_H
+#endif  // EIGEN_EULERANGLES_H