GitHub-Proxy/eigen
1
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2025-08-08 17:59:00 +08:00
Code Issues Packages Projects Releases Wiki Activity

bugfix in matrix to Euler-angles function

Browse Source
This commit is contained in:
Gael Guennebaud 2008-12-06 11:04:33 +00:00
parent faf942a947
commit 9c0deb55ca
2 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
Eigen/src/Geometry/EulerAngles.h
View File

@ -48,6 +48,7 @@ template<typename Derived>
inline Matrix<typename MatrixBase<Derived>::Scalar,3,1> inline Matrix<typename MatrixBase<Derived>::Scalar,3,1>
MatrixBase<Derived>::eulerAngles(int a0, int a1, int a2) const MatrixBase<Derived>::eulerAngles(int a0, int a1, int 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; Matrix<Scalar,3,1> res;
@ -62,7 +63,6 @@ MatrixBase<Derived>::eulerAngles(int a0, int a1, int a2) const
if (a0==a2) if (a0==a2)
{ {
Scalar s = Vector2(coeff(j,i) , coeff(k,i)).norm(); Scalar s = Vector2(coeff(j,i) , coeff(k,i)).norm();
res[1] = std::atan2(s, coeff(i,i)); res[1] = std::atan2(s, coeff(i,i));
if (s > epsilon) if (s > epsilon)
{ {
@ -72,13 +72,12 @@ MatrixBase<Derived>::eulerAngles(int a0, int a1, int a2) const
else else
{ {
res[0] = Scalar(0); 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 else
{ {
Scalar c = Vector2(coeff(i,i) , coeff(i,j)).norm(); Scalar c = Vector2(coeff(i,i) , coeff(i,j)).norm();
res[1] = std::atan2(-coeff(i,k), c); res[1] = std::atan2(-coeff(i,k), c);
if (c > epsilon) if (c > epsilon)
{ {
@ -88,7 +87,7 @@ MatrixBase<Derived>::eulerAngles(int a0, int a1, int a2) const
else else
{ {
res[0] = Scalar(0); 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) if (!odd)

4
test/geometry.cpp
View File

@ -320,7 +320,9 @@ template<typename Scalar> void geometry(void)
VERIFY_IS_APPROX(r2d1d.template cast<Scalar>(),r2d1); VERIFY_IS_APPROX(r2d1d.template cast<Scalar>(),r2d1);
m = q1; m = q1;
// m.col(1) = Vector3(0,ei_random<Scalar>(),ei_random<Scalar>()).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) { \ #define VERIFY_EULER(I,J,K, X,Y,Z) { \
Vector3 ea = m.eulerAngles(I,J,K); \ 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())); \ Matrix3 m1 = Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \