mirror/eigen
2
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2024-12-21 07:19:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix bug #827: improve accuracy of quaternion to angle-axis conversion

Browse Source
This commit is contained in:
Gael Guennebaud 2014-06-20 15:09:42 +02:00
parent 98ef44fe55
commit 963d338922
1 changed files with 14 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
Eigen/src/Geometry/AngleAxis.h
View File

@ -77,7 +77,9 @@ public:
* represents an invalid rotation. */ * represents an invalid rotation. */
template<typename Derived> template<typename Derived>
inline AngleAxis(const Scalar& angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {} 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. */ /** Constructs and initialize the angle-axis rotation from a quaternion \a q.
* This function implicitly normalizes the quaternion \a q.
*/
template<typename QuatDerived> inline explicit AngleAxis(const QuaternionBase<QuatDerived>& q) { *this = q; } template<typename QuatDerived> inline explicit AngleAxis(const QuaternionBase<QuatDerived>& q) { *this = q; }
/** Constructs and initialize the angle-axis rotation from a 3x3 rotation matrix. */ /** Constructs and initialize the angle-axis rotation from a 3x3 rotation matrix. */
template<typename Derived> template<typename Derived>
@ -149,29 +151,27 @@ typedef AngleAxis<float> AngleAxisf;
typedef AngleAxis<double> AngleAxisd; typedef AngleAxis<double> AngleAxisd;
/** Set \c *this from a \b unit quaternion. /** Set \c *this from a \b unit quaternion.
* The axis is normalized. * The resulting axis is normalized.
* *
* \warning As any other method dealing with quaternion, if the input quaternion * This function implicitly normalizes the quaternion \a q.
* is not normalized then the result is undefined.
*/ */
template<typename Scalar> template<typename Scalar>
template<typename QuatDerived> template<typename QuatDerived>
AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived>& q) AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived>& q)
{ {
using std::acos; using std::atan2;
EIGEN_USING_STD_MATH(min); Scalar n = q.vec().norm();
EIGEN_USING_STD_MATH(max); if(n<NumTraits<Scalar>::epsilon())
using std::sqrt; n = q.vec().stableNorm();
Scalar n2 = q.vec().squaredNorm(); if (n > Scalar(0))
if (n2 < NumTraits<Scalar>::dummy_precision()*NumTraits<Scalar>::dummy_precision())
{ {
m_angle = Scalar(0); m_angle = Scalar(2)*atan2(n, q.w());
m_axis << Scalar(1), Scalar(0), Scalar(0); m_axis = q.vec() / n;
} }
else else
{ {
m_angle = Scalar(2)*acos((min)((max)(Scalar(-1),q.w()),Scalar(1))); m_angle = 0;
m_axis = q.vec() / sqrt(n2); m_axis << 1, 0, 0;
} }
return *this; return *this;
} }