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

Rotation2D: fix slerp to take the shortest path, and add convenient method to get the angle in [-pi,pi] or [0,pi]

Browse Source
This commit is contained in:
Gael Guennebaud 2015-07-07 17:27:12 +02:00
parent 3f2101b03b
commit fa17358c4b
2 changed files with 53 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
Eigen/src/Geometry/Rotation2D.h
View File

@ -70,6 +70,20 @@ public:
/** \returns a read-write reference to the rotation angle */ /** \returns a read-write reference to the rotation angle */
inline Scalar& angle() { return m_angle; } inline Scalar& angle() { return m_angle; }
/** \returns the rotation angle in [0,2pi] */
inline Scalar smallestPositiveAngle() const {
Scalar tmp = fmod(m_angle,Scalar(2)*EIGEN_PI);
return tmp<Scalar(0) ? tmp + Scalar(2)*EIGEN_PI : tmp;
}
/** \returns the rotation angle in [-pi,pi] */
inline Scalar smallestAngle() const {
Scalar tmp = fmod(m_angle,Scalar(2)*EIGEN_PI);
if(tmp>Scalar(EIGEN_PI)) tmp -= Scalar(2)*Scalar(EIGEN_PI);
else if(tmp<-Scalar(EIGEN_PI)) tmp += Scalar(2)*Scalar(EIGEN_PI);
return tmp;
}
/** \returns the inverse rotation */ /** \returns the inverse rotation */
inline Rotation2D inverse() const { return Rotation2D(-m_angle); } inline Rotation2D inverse() const { return Rotation2D(-m_angle); }
@ -93,7 +107,10 @@ public:
* parameter \a t. It is in fact equivalent to a linear interpolation. * parameter \a t. It is in fact equivalent to a linear interpolation.
*/ */
inline Rotation2D slerp(const Scalar& t, const Rotation2D& other) const inline Rotation2D slerp(const Scalar& t, const Rotation2D& other) const
{ return Rotation2D(m_angle * (1-t) + other.angle() * t); } {
Scalar dist = Rotation2D(other.m_angle-m_angle).smallestAngle();
return Rotation2D(m_angle + dist*t);
}
/** \returns \c *this with scalar type casted to \a NewScalarType /** \returns \c *this with scalar type casted to \a NewScalarType
* *
@ -119,6 +136,7 @@ public:
* \sa MatrixBase::isApprox() */ * \sa MatrixBase::isApprox() */
bool isApprox(const Rotation2D& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const bool isApprox(const Rotation2D& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
{ return internal::isApprox(m_angle,other.m_angle, prec); } { return internal::isApprox(m_angle,other.m_angle, prec); }
}; };
/** \ingroup Geometry_Module /** \ingroup Geometry_Module

37
test/geo_transformations.cpp
View File

@ -409,6 +409,23 @@ template<typename Scalar, int Mode, int Options> void transformations()
Rotation2D<double> r2d1d = r2d1.template cast<double>(); Rotation2D<double> r2d1d = r2d1.template cast<double>();
VERIFY_IS_APPROX(r2d1d.template cast<Scalar>(),r2d1); VERIFY_IS_APPROX(r2d1d.template cast<Scalar>(),r2d1);
for(int k=0; k<100; ++k)
{
Scalar angle = internal::random<Scalar>(-100,100);
Rotation2D<Scalar> rot2(angle);
VERIFY( rot2.smallestPositiveAngle() >= 0 );
VERIFY( rot2.smallestPositiveAngle() < Scalar(2)*Scalar(EIGEN_PI) );
VERIFY_IS_APPROX( std::cos(rot2.smallestPositiveAngle()), std::cos(rot2.angle()) );
VERIFY_IS_APPROX( std::sin(rot2.smallestPositiveAngle()), std::sin(rot2.angle()) );
VERIFY( rot2.smallestAngle() >= -Scalar(EIGEN_PI) );
VERIFY( rot2.smallestAngle() <= Scalar(EIGEN_PI) );
VERIFY_IS_APPROX( std::cos(rot2.smallestAngle()), std::cos(rot2.angle()) );
VERIFY_IS_APPROX( std::sin(rot2.smallestAngle()), std::sin(rot2.angle()) );
}
s0 = internal::random<Scalar>(-100,100);
s1 = internal::random<Scalar>(-100,100);
Rotation2D<Scalar> R0(s0), R1(s1); Rotation2D<Scalar> R0(s0), R1(s1);
t20 = Translation2(v20) * (R0 * Eigen::Scaling(s0)); t20 = Translation2(v20) * (R0 * Eigen::Scaling(s0));
@ -420,9 +437,23 @@ template<typename Scalar, int Mode, int Options> void transformations()
VERIFY_IS_APPROX(t20,t21); VERIFY_IS_APPROX(t20,t21);
VERIFY_IS_APPROX(s0, (R0.slerp(0, R1)).angle()); VERIFY_IS_APPROX(s0, (R0.slerp(0, R1)).angle());
VERIFY_IS_APPROX(s1, (R0.slerp(1, R1)).angle()); VERIFY_IS_APPROX(R1.smallestPositiveAngle(), (R0.slerp(1, R1)).smallestPositiveAngle());
VERIFY_IS_APPROX(s0, (R0.slerp(0.5, R0)).angle()); VERIFY_IS_APPROX(R0.smallestPositiveAngle(), (R0.slerp(0.5, R0)).smallestPositiveAngle());
VERIFY_IS_APPROX(Scalar(0), (R0.slerp(0.5, R0.inverse())).angle());
if(std::cos(s0)>0)
VERIFY_IS_MUCH_SMALLER_THAN((R0.slerp(0.5, R0.inverse())).smallestAngle(), Scalar(1));
else
VERIFY_IS_APPROX(Scalar(EIGEN_PI), (R0.slerp(0.5, R0.inverse())).smallestPositiveAngle());
// Check path length
Scalar l = 0;
for(int k=0; k<100; ++k)
{
Scalar a1 = R0.slerp(Scalar(k)/Scalar(100), R1).angle();
Scalar a2 = R0.slerp(Scalar(k+1)/Scalar(100), R1).angle();
l += std::abs(a2-a1);
}
VERIFY(l<=EIGEN_PI);
// check basic features // check basic features
{ {