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