diff --git a/Eigen/src/Geometry/Rotation2D.h b/Eigen/src/Geometry/Rotation2D.h index 4feb3d4d2..65aa83be5 100644 --- a/Eigen/src/Geometry/Rotation2D.h +++ b/Eigen/src/Geometry/Rotation2D.h @@ -69,6 +69,20 @@ public: /** \returns a read-write reference to the rotation 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 tmpScalar(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 */ 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. */ 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 * @@ -119,6 +136,7 @@ public: * \sa MatrixBase::isApprox() */ bool isApprox(const Rotation2D& other, const typename NumTraits::Real& prec = NumTraits::dummy_precision()) const { return internal::isApprox(m_angle,other.m_angle, prec); } + }; /** \ingroup Geometry_Module diff --git a/test/geo_transformations.cpp b/test/geo_transformations.cpp index c4025f32f..e296267cf 100644 --- a/test/geo_transformations.cpp +++ b/test/geo_transformations.cpp @@ -408,7 +408,24 @@ template void transformations() VERIFY_IS_APPROX(r2d1f.template cast(),r2d1); Rotation2D r2d1d = r2d1.template cast(); VERIFY_IS_APPROX(r2d1d.template cast(),r2d1); + + for(int k=0; k<100; ++k) + { + Scalar angle = internal::random(-100,100); + Rotation2D 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(-100,100); + s1 = internal::random(-100,100); Rotation2D R0(s0), R1(s1); t20 = Translation2(v20) * (R0 * Eigen::Scaling(s0)); @@ -420,9 +437,23 @@ template void transformations() VERIFY_IS_APPROX(t20,t21); VERIFY_IS_APPROX(s0, (R0.slerp(0, R1)).angle()); - VERIFY_IS_APPROX(s1, (R0.slerp(1, R1)).angle()); - VERIFY_IS_APPROX(s0, (R0.slerp(0.5, R0)).angle()); - VERIFY_IS_APPROX(Scalar(0), (R0.slerp(0.5, R0.inverse())).angle()); + VERIFY_IS_APPROX(R1.smallestPositiveAngle(), (R0.slerp(1, R1)).smallestPositiveAngle()); + VERIFY_IS_APPROX(R0.smallestPositiveAngle(), (R0.slerp(0.5, R0)).smallestPositiveAngle()); + + 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 {