diff --git a/Eigen/src/Geometry/EulerAngles.h b/Eigen/src/Geometry/EulerAngles.h index 6087a1433..7e7c2c60a 100644 --- a/Eigen/src/Geometry/EulerAngles.h +++ b/Eigen/src/Geometry/EulerAngles.h @@ -48,6 +48,7 @@ template inline Matrix::Scalar,3,1> MatrixBase::eulerAngles(int a0, int a1, int a2) const { + /* Implemented from Graphics Gems IV */ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3) Matrix res; @@ -62,7 +63,6 @@ MatrixBase::eulerAngles(int a0, int a1, int a2) const if (a0==a2) { Scalar s = Vector2(coeff(j,i) , coeff(k,i)).norm(); - res[1] = std::atan2(s, coeff(i,i)); if (s > epsilon) { @@ -72,13 +72,12 @@ MatrixBase::eulerAngles(int a0, int a1, int a2) const else { res[0] = Scalar(0); - res[2] = std::atan2(-coeff(k,j), coeff(j,j)); + res[2] = (coeff(i,i)>0?1:-1)*std::atan2(-coeff(k,j), coeff(j,j)); } } else { Scalar c = Vector2(coeff(i,i) , coeff(i,j)).norm(); - res[1] = std::atan2(-coeff(i,k), c); if (c > epsilon) { @@ -88,7 +87,7 @@ MatrixBase::eulerAngles(int a0, int a1, int a2) const else { res[0] = Scalar(0); - res[2] = -std::atan2(-coeff(k,j), coeff(j,j)); + res[2] = (coeff(i,k)>0?1:-1)*std::atan2(-coeff(k,j), coeff(j,j)); } } if (!odd) diff --git a/test/geometry.cpp b/test/geometry.cpp index af23bce82..97782213b 100644 --- a/test/geometry.cpp +++ b/test/geometry.cpp @@ -320,7 +320,9 @@ template void geometry(void) VERIFY_IS_APPROX(r2d1d.template cast(),r2d1); m = q1; - +// m.col(1) = Vector3(0,ei_random(),ei_random()).normalized(); +// m.col(0) = Vector3(-1,0,0).normalized(); +// m.col(2) = m.col(0).cross(m.col(1)); #define VERIFY_EULER(I,J,K, X,Y,Z) { \ Vector3 ea = m.eulerAngles(I,J,K); \ Matrix3 m1 = Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \