diff options
Diffstat (limited to 'extern/Eigen3/Eigen/src/Geometry')
-rw-r--r-- | extern/Eigen3/Eigen/src/Geometry/AlignedBox.h | 14 | ||||
-rw-r--r-- | extern/Eigen3/Eigen/src/Geometry/AngleAxis.h | 13 | ||||
-rw-r--r-- | extern/Eigen3/Eigen/src/Geometry/EulerAngles.h | 60 | ||||
-rw-r--r-- | extern/Eigen3/Eigen/src/Geometry/Homogeneous.h | 2 | ||||
-rw-r--r-- | extern/Eigen3/Eigen/src/Geometry/Hyperplane.h | 11 | ||||
-rw-r--r-- | extern/Eigen3/Eigen/src/Geometry/OrthoMethods.h | 30 | ||||
-rw-r--r-- | extern/Eigen3/Eigen/src/Geometry/ParametrizedLine.h | 8 | ||||
-rw-r--r-- | extern/Eigen3/Eigen/src/Geometry/Quaternion.h | 96 | ||||
-rw-r--r-- | extern/Eigen3/Eigen/src/Geometry/Rotation2D.h | 15 | ||||
-rw-r--r-- | extern/Eigen3/Eigen/src/Geometry/Scaling.h | 6 | ||||
-rw-r--r-- | extern/Eigen3/Eigen/src/Geometry/Transform.h | 22 | ||||
-rw-r--r-- | extern/Eigen3/Eigen/src/Geometry/Umeyama.h | 25 |
12 files changed, 166 insertions, 136 deletions
diff --git a/extern/Eigen3/Eigen/src/Geometry/AlignedBox.h b/extern/Eigen3/Eigen/src/Geometry/AlignedBox.h index 5830fcd35fc..8e186d57a34 100644 --- a/extern/Eigen3/Eigen/src/Geometry/AlignedBox.h +++ b/extern/Eigen3/Eigen/src/Geometry/AlignedBox.h @@ -56,7 +56,7 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) /** Default constructor initializing a null box. */ - inline explicit AlignedBox() + inline AlignedBox() { if (AmbientDimAtCompileTime!=Dynamic) setEmpty(); } /** Constructs a null box with \a _dim the dimension of the ambient space. */ @@ -71,7 +71,7 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) template<typename Derived> inline explicit AlignedBox(const MatrixBase<Derived>& a_p) { - const typename internal::nested<Derived,2>::type p(a_p.derived()); + typename internal::nested<Derived,2>::type p(a_p.derived()); m_min = p; m_max = p; } @@ -79,7 +79,7 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) ~AlignedBox() {} /** \returns the dimension in which the box holds */ - inline Index dim() const { return AmbientDimAtCompileTime==Dynamic ? m_min.size()-1 : Index(AmbientDimAtCompileTime); } + inline Index dim() const { return AmbientDimAtCompileTime==Dynamic ? m_min.size() : Index(AmbientDimAtCompileTime); } /** \deprecated use isEmpty */ inline bool isNull() const { return isEmpty(); } @@ -248,14 +248,14 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) */ template<typename Derived> inline NonInteger exteriorDistance(const MatrixBase<Derived>& p) const - { return internal::sqrt(NonInteger(squaredExteriorDistance(p))); } + { using std::sqrt; return sqrt(NonInteger(squaredExteriorDistance(p))); } /** \returns the distance between the boxes \a b and \c *this, * and zero if the boxes intersect. * \sa squaredExteriorDistance() */ inline NonInteger exteriorDistance(const AlignedBox& b) const - { return internal::sqrt(NonInteger(squaredExteriorDistance(b))); } + { using std::sqrt; return sqrt(NonInteger(squaredExteriorDistance(b))); } /** \returns \c *this with scalar type casted to \a NewScalarType * @@ -282,7 +282,7 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) * determined by \a prec. * * \sa MatrixBase::isApprox() */ - bool isApprox(const AlignedBox& other, RealScalar prec = ScalarTraits::dummy_precision()) const + bool isApprox(const AlignedBox& other, const RealScalar& prec = ScalarTraits::dummy_precision()) const { return m_min.isApprox(other.m_min, prec) && m_max.isApprox(other.m_max, prec); } protected: @@ -296,7 +296,7 @@ template<typename Scalar,int AmbientDim> template<typename Derived> inline Scalar AlignedBox<Scalar,AmbientDim>::squaredExteriorDistance(const MatrixBase<Derived>& a_p) const { - const typename internal::nested<Derived,2*AmbientDim>::type p(a_p.derived()); + typename internal::nested<Derived,2*AmbientDim>::type p(a_p.derived()); Scalar dist2(0); Scalar aux; for (Index k=0; k<dim(); ++k) diff --git a/extern/Eigen3/Eigen/src/Geometry/AngleAxis.h b/extern/Eigen3/Eigen/src/Geometry/AngleAxis.h index 67197ac78c3..553d38c7449 100644 --- a/extern/Eigen3/Eigen/src/Geometry/AngleAxis.h +++ b/extern/Eigen3/Eigen/src/Geometry/AngleAxis.h @@ -76,7 +76,7 @@ public: * \warning If the \a axis vector is not normalized, then the angle-axis object * represents an invalid rotation. */ template<typename Derived> - inline AngleAxis(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. */ template<typename QuatDerived> inline explicit AngleAxis(const QuaternionBase<QuatDerived>& q) { *this = q; } /** Constructs and initialize the angle-axis rotation from a 3x3 rotation matrix. */ @@ -137,7 +137,7 @@ public: * determined by \a prec. * * \sa MatrixBase::isApprox() */ - bool isApprox(const AngleAxis& other, typename NumTraits<Scalar>::Real prec = NumTraits<Scalar>::dummy_precision()) const + bool isApprox(const AngleAxis& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const { return m_axis.isApprox(other.m_axis, prec) && internal::isApprox(m_angle,other.m_angle, prec); } }; @@ -161,6 +161,7 @@ AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived using std::acos; using std::min; using std::max; + using std::sqrt; Scalar n2 = q.vec().squaredNorm(); if (n2 < NumTraits<Scalar>::dummy_precision()*NumTraits<Scalar>::dummy_precision()) { @@ -170,7 +171,7 @@ AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived else { m_angle = Scalar(2)*acos((min)((max)(Scalar(-1),q.w()),Scalar(1))); - m_axis = q.vec() / internal::sqrt(n2); + m_axis = q.vec() / sqrt(n2); } return *this; } @@ -202,9 +203,11 @@ template<typename Scalar> typename AngleAxis<Scalar>::Matrix3 AngleAxis<Scalar>::toRotationMatrix(void) const { + using std::sin; + using std::cos; Matrix3 res; - Vector3 sin_axis = internal::sin(m_angle) * m_axis; - Scalar c = internal::cos(m_angle); + Vector3 sin_axis = sin(m_angle) * m_axis; + Scalar c = cos(m_angle); Vector3 cos1_axis = (Scalar(1)-c) * m_axis; Scalar tmp; diff --git a/extern/Eigen3/Eigen/src/Geometry/EulerAngles.h b/extern/Eigen3/Eigen/src/Geometry/EulerAngles.h index e424d240604..82802fb43cf 100644 --- a/extern/Eigen3/Eigen/src/Geometry/EulerAngles.h +++ b/extern/Eigen3/Eigen/src/Geometry/EulerAngles.h @@ -27,55 +27,75 @@ namespace Eigen { * * AngleAxisf(ea[1], Vector3f::UnitX()) * * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode * This corresponds to the right-multiply conventions (with right hand side frames). + * + * The returned angles are in the ranges [0:pi]x[-pi:pi]x[-pi:pi]. + * + * \sa class AngleAxis */ template<typename Derived> inline Matrix<typename MatrixBase<Derived>::Scalar,3,1> MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const { + using std::atan2; + using std::sin; + using std::cos; /* Implemented from Graphics Gems IV */ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3) Matrix<Scalar,3,1> res; typedef Matrix<typename Derived::Scalar,2,1> Vector2; - const Scalar epsilon = NumTraits<Scalar>::dummy_precision(); const Index odd = ((a0+1)%3 == a1) ? 0 : 1; const Index i = a0; const Index j = (a0 + 1 + odd)%3; const Index k = (a0 + 2 - odd)%3; - + if (a0==a2) { - Scalar s = Vector2(coeff(j,i) , coeff(k,i)).norm(); - res[1] = internal::atan2(s, coeff(i,i)); - if (s > epsilon) + res[0] = atan2(coeff(j,i), coeff(k,i)); + if((odd && res[0]<Scalar(0)) || ((!odd) && res[0]>Scalar(0))) { - res[0] = internal::atan2(coeff(j,i), coeff(k,i)); - res[2] = internal::atan2(coeff(i,j),-coeff(i,k)); + res[0] = (res[0] > Scalar(0)) ? res[0] - Scalar(M_PI) : res[0] + Scalar(M_PI); + Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm(); + res[1] = -atan2(s2, coeff(i,i)); } else { - res[0] = Scalar(0); - res[2] = (coeff(i,i)>0?1:-1)*internal::atan2(-coeff(k,j), coeff(j,j)); + Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm(); + res[1] = atan2(s2, coeff(i,i)); } - } + + // With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first two angles, + // we can compute their respective rotation, and apply its inverse to M. Since the result must + // be a rotation around x, we have: + // + // c2 s1.s2 c1.s2 1 0 0 + // 0 c1 -s1 * M = 0 c3 s3 + // -s2 s1.c2 c1.c2 0 -s3 c3 + // + // Thus: m11.c1 - m21.s1 = c3 & m12.c1 - m22.s1 = s3 + + Scalar s1 = sin(res[0]); + Scalar c1 = cos(res[0]); + res[2] = atan2(c1*coeff(j,k)-s1*coeff(k,k), c1*coeff(j,j) - s1 * coeff(k,j)); + } else { - Scalar c = Vector2(coeff(i,i) , coeff(i,j)).norm(); - res[1] = internal::atan2(-coeff(i,k), c); - if (c > epsilon) - { - res[0] = internal::atan2(coeff(j,k), coeff(k,k)); - res[2] = internal::atan2(coeff(i,j), coeff(i,i)); + res[0] = atan2(coeff(j,k), coeff(k,k)); + Scalar c2 = Vector2(coeff(i,i), coeff(i,j)).norm(); + if((odd && res[0]<Scalar(0)) || ((!odd) && res[0]>Scalar(0))) { + res[0] = (res[0] > Scalar(0)) ? res[0] - Scalar(M_PI) : res[0] + Scalar(M_PI); + res[1] = atan2(-coeff(i,k), -c2); } else - { - res[0] = Scalar(0); - res[2] = (coeff(i,k)>0?1:-1)*internal::atan2(-coeff(k,j), coeff(j,j)); - } + res[1] = atan2(-coeff(i,k), c2); + Scalar s1 = sin(res[0]); + Scalar c1 = cos(res[0]); + res[2] = atan2(s1*coeff(k,i)-c1*coeff(j,i), c1*coeff(j,j) - s1 * coeff(k,j)); } if (!odd) res = -res; + return res; } diff --git a/extern/Eigen3/Eigen/src/Geometry/Homogeneous.h b/extern/Eigen3/Eigen/src/Geometry/Homogeneous.h index df03feb55c6..00e71d190c3 100644 --- a/extern/Eigen3/Eigen/src/Geometry/Homogeneous.h +++ b/extern/Eigen3/Eigen/src/Geometry/Homogeneous.h @@ -59,7 +59,7 @@ template<typename MatrixType,typename Rhs> struct homogeneous_right_product_impl } // end namespace internal template<typename MatrixType,int _Direction> class Homogeneous - : public MatrixBase<Homogeneous<MatrixType,_Direction> > + : internal::no_assignment_operator, public MatrixBase<Homogeneous<MatrixType,_Direction> > { public: diff --git a/extern/Eigen3/Eigen/src/Geometry/Hyperplane.h b/extern/Eigen3/Eigen/src/Geometry/Hyperplane.h index 1b7c7c78c80..aeff43fefa6 100644 --- a/extern/Eigen3/Eigen/src/Geometry/Hyperplane.h +++ b/extern/Eigen3/Eigen/src/Geometry/Hyperplane.h @@ -50,7 +50,7 @@ public: typedef const Block<const Coefficients,AmbientDimAtCompileTime,1> ConstNormalReturnType; /** Default constructor without initialization */ - inline explicit Hyperplane() {} + inline Hyperplane() {} template<int OtherOptions> Hyperplane(const Hyperplane<Scalar,AmbientDimAtCompileTime,OtherOptions>& other) @@ -75,7 +75,7 @@ public: * such that the algebraic equation of the plane is \f$ n \cdot x + d = 0 \f$. * \warning the vector normal is assumed to be normalized. */ - inline Hyperplane(const VectorType& n, Scalar d) + inline Hyperplane(const VectorType& n, const Scalar& d) : m_coeffs(n.size()+1) { normal() = n; @@ -135,7 +135,7 @@ public: /** \returns the absolute distance between the plane \c *this and a point \a p. * \sa signedDistance() */ - inline Scalar absDistance(const VectorType& p) const { return internal::abs(signedDistance(p)); } + inline Scalar absDistance(const VectorType& p) const { using std::abs; return abs(signedDistance(p)); } /** \returns the projection of a point \a p onto the plane \c *this. */ @@ -178,13 +178,14 @@ public: */ VectorType intersection(const Hyperplane& other) const { + using std::abs; EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 2) Scalar det = coeffs().coeff(0) * other.coeffs().coeff(1) - coeffs().coeff(1) * other.coeffs().coeff(0); // since the line equations ax+by=c are normalized with a^2+b^2=1, the following tests // whether the two lines are approximately parallel. if(internal::isMuchSmallerThan(det, Scalar(1))) { // special case where the two lines are approximately parallel. Pick any point on the first line. - if(internal::abs(coeffs().coeff(1))>internal::abs(coeffs().coeff(0))) + if(abs(coeffs().coeff(1))>abs(coeffs().coeff(0))) return VectorType(coeffs().coeff(1), -coeffs().coeff(2)/coeffs().coeff(1)-coeffs().coeff(0)); else return VectorType(-coeffs().coeff(2)/coeffs().coeff(0)-coeffs().coeff(1), coeffs().coeff(0)); @@ -256,7 +257,7 @@ public: * * \sa MatrixBase::isApprox() */ template<int OtherOptions> - bool isApprox(const Hyperplane<Scalar,AmbientDimAtCompileTime,OtherOptions>& other, typename NumTraits<Scalar>::Real prec = NumTraits<Scalar>::dummy_precision()) const + bool isApprox(const Hyperplane<Scalar,AmbientDimAtCompileTime,OtherOptions>& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const { return m_coeffs.isApprox(other.m_coeffs, prec); } protected: diff --git a/extern/Eigen3/Eigen/src/Geometry/OrthoMethods.h b/extern/Eigen3/Eigen/src/Geometry/OrthoMethods.h index 11ad5829cda..556bc81604e 100644 --- a/extern/Eigen3/Eigen/src/Geometry/OrthoMethods.h +++ b/extern/Eigen3/Eigen/src/Geometry/OrthoMethods.h @@ -33,9 +33,9 @@ MatrixBase<Derived>::cross(const MatrixBase<OtherDerived>& other) const typename internal::nested<Derived,2>::type lhs(derived()); typename internal::nested<OtherDerived,2>::type rhs(other.derived()); return typename cross_product_return_type<OtherDerived>::type( - internal::conj(lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1)), - internal::conj(lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2)), - internal::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0)) + numext::conj(lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1)), + numext::conj(lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2)), + numext::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0)) ); } @@ -49,9 +49,9 @@ struct cross3_impl { run(const VectorLhs& lhs, const VectorRhs& rhs) { return typename internal::plain_matrix_type<VectorLhs>::type( - internal::conj(lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1)), - internal::conj(lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2)), - internal::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0)), + numext::conj(lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1)), + numext::conj(lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2)), + numext::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0)), 0 ); } @@ -78,8 +78,8 @@ MatrixBase<Derived>::cross3(const MatrixBase<OtherDerived>& other) const typedef typename internal::nested<Derived,2>::type DerivedNested; typedef typename internal::nested<OtherDerived,2>::type OtherDerivedNested; - const DerivedNested lhs(derived()); - const OtherDerivedNested rhs(other.derived()); + DerivedNested lhs(derived()); + OtherDerivedNested rhs(other.derived()); return internal::cross3_impl<Architecture::Target, typename internal::remove_all<DerivedNested>::type, @@ -141,8 +141,8 @@ struct unitOrthogonal_selector if (maxi==0) sndi = 1; RealScalar invnm = RealScalar(1)/(Vector2() << src.coeff(sndi),src.coeff(maxi)).finished().norm(); - perp.coeffRef(maxi) = -conj(src.coeff(sndi)) * invnm; - perp.coeffRef(sndi) = conj(src.coeff(maxi)) * invnm; + perp.coeffRef(maxi) = -numext::conj(src.coeff(sndi)) * invnm; + perp.coeffRef(sndi) = numext::conj(src.coeff(maxi)) * invnm; return perp; } @@ -168,8 +168,8 @@ struct unitOrthogonal_selector<Derived,3> || (!isMuchSmallerThan(src.y(), src.z()))) { RealScalar invnm = RealScalar(1)/src.template head<2>().norm(); - perp.coeffRef(0) = -conj(src.y())*invnm; - perp.coeffRef(1) = conj(src.x())*invnm; + perp.coeffRef(0) = -numext::conj(src.y())*invnm; + perp.coeffRef(1) = numext::conj(src.x())*invnm; perp.coeffRef(2) = 0; } /* if both x and y are close to zero, then the vector is close @@ -180,8 +180,8 @@ struct unitOrthogonal_selector<Derived,3> { RealScalar invnm = RealScalar(1)/src.template tail<2>().norm(); perp.coeffRef(0) = 0; - perp.coeffRef(1) = -conj(src.z())*invnm; - perp.coeffRef(2) = conj(src.y())*invnm; + perp.coeffRef(1) = -numext::conj(src.z())*invnm; + perp.coeffRef(2) = numext::conj(src.y())*invnm; } return perp; @@ -193,7 +193,7 @@ struct unitOrthogonal_selector<Derived,2> { typedef typename plain_matrix_type<Derived>::type VectorType; static inline VectorType run(const Derived& src) - { return VectorType(-conj(src.y()), conj(src.x())).normalized(); } + { return VectorType(-numext::conj(src.y()), numext::conj(src.x())).normalized(); } }; } // end namespace internal diff --git a/extern/Eigen3/Eigen/src/Geometry/ParametrizedLine.h b/extern/Eigen3/Eigen/src/Geometry/ParametrizedLine.h index 719a904413d..77fa228e6a5 100644 --- a/extern/Eigen3/Eigen/src/Geometry/ParametrizedLine.h +++ b/extern/Eigen3/Eigen/src/Geometry/ParametrizedLine.h @@ -41,7 +41,7 @@ public: typedef Matrix<Scalar,AmbientDimAtCompileTime,1,Options> VectorType; /** Default constructor without initialization */ - inline explicit ParametrizedLine() {} + inline ParametrizedLine() {} template<int OtherOptions> ParametrizedLine(const ParametrizedLine<Scalar,AmbientDimAtCompileTime,OtherOptions>& other) @@ -87,13 +87,13 @@ public: /** \returns the distance of a point \a p to its projection onto the line \c *this. * \sa squaredDistance() */ - RealScalar distance(const VectorType& p) const { return internal::sqrt(squaredDistance(p)); } + RealScalar distance(const VectorType& p) const { using std::sqrt; return sqrt(squaredDistance(p)); } /** \returns the projection of a point \a p onto the line \c *this. */ VectorType projection(const VectorType& p) const { return origin() + direction().dot(p-origin()) * direction(); } - VectorType pointAt( Scalar t ) const; + VectorType pointAt(const Scalar& t) const; template <int OtherOptions> Scalar intersectionParameter(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane) const; @@ -154,7 +154,7 @@ inline ParametrizedLine<_Scalar, _AmbientDim,_Options>::ParametrizedLine(const H */ template <typename _Scalar, int _AmbientDim, int _Options> inline typename ParametrizedLine<_Scalar, _AmbientDim,_Options>::VectorType -ParametrizedLine<_Scalar, _AmbientDim,_Options>::pointAt( _Scalar t ) const +ParametrizedLine<_Scalar, _AmbientDim,_Options>::pointAt(const _Scalar& t) const { return origin() + (direction()*t); } diff --git a/extern/Eigen3/Eigen/src/Geometry/Quaternion.h b/extern/Eigen3/Eigen/src/Geometry/Quaternion.h index 8792e2da2ae..9fee0c91980 100644 --- a/extern/Eigen3/Eigen/src/Geometry/Quaternion.h +++ b/extern/Eigen3/Eigen/src/Geometry/Quaternion.h @@ -150,18 +150,14 @@ public: /** \returns the conjugated quaternion */ Quaternion<Scalar> conjugate() const; - /** \returns an interpolation for a constant motion between \a other and \c *this - * \a t in [0;1] - * see http://en.wikipedia.org/wiki/Slerp - */ - template<class OtherDerived> Quaternion<Scalar> slerp(Scalar t, const QuaternionBase<OtherDerived>& other) const; + template<class OtherDerived> Quaternion<Scalar> slerp(const Scalar& t, const QuaternionBase<OtherDerived>& other) const; /** \returns \c true if \c *this is approximately equal to \a other, within the precision * determined by \a prec. * * \sa MatrixBase::isApprox() */ template<class OtherDerived> - bool isApprox(const QuaternionBase<OtherDerived>& other, RealScalar prec = NumTraits<Scalar>::dummy_precision()) const + bool isApprox(const QuaternionBase<OtherDerived>& other, const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const { return coeffs().isApprox(other.coeffs(), prec); } /** return the result vector of \a v through the rotation*/ @@ -193,11 +189,12 @@ public: * * \brief The quaternion class used to represent 3D orientations and rotations * - * \param _Scalar the scalar type, i.e., the type of the coefficients + * \tparam _Scalar the scalar type, i.e., the type of the coefficients + * \tparam _Options controls the memory alignment of the coefficients. Can be \# AutoAlign or \# DontAlign. Default is AutoAlign. * * This class represents a quaternion \f$ w+xi+yj+zk \f$ that is a convenient representation of * orientations and rotations of objects in three dimensions. Compared to other representations - * like Euler angles or 3x3 matrices, quatertions offer the following advantages: + * like Euler angles or 3x3 matrices, quaternions offer the following advantages: * \li \b compact storage (4 scalars) * \li \b efficient to compose (28 flops), * \li \b stable spherical interpolation @@ -248,7 +245,7 @@ public: * while internally the coefficients are stored in the following order: * [\c x, \c y, \c z, \c w] */ - inline Quaternion(Scalar w, Scalar x, Scalar y, Scalar z) : m_coeffs(x, y, z, w){} + inline Quaternion(const Scalar& w, const Scalar& x, const Scalar& y, const Scalar& z) : m_coeffs(x, y, z, w){} /** Constructs and initialize a quaternion from the array data */ inline Quaternion(const Scalar* data) : m_coeffs(data) {} @@ -304,41 +301,29 @@ typedef Quaternion<double> Quaterniond; namespace internal { template<typename _Scalar, int _Options> - struct traits<Map<Quaternion<_Scalar>, _Options> >: - traits<Quaternion<_Scalar, _Options> > + struct traits<Map<Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> > { - typedef _Scalar Scalar; typedef Map<Matrix<_Scalar,4,1>, _Options> Coefficients; - - typedef traits<Quaternion<_Scalar, _Options> > TraitsBase; - enum { - IsAligned = TraitsBase::IsAligned, - - Flags = TraitsBase::Flags - }; }; } namespace internal { template<typename _Scalar, int _Options> - struct traits<Map<const Quaternion<_Scalar>, _Options> >: - traits<Quaternion<_Scalar> > + struct traits<Map<const Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> > { - typedef _Scalar Scalar; typedef Map<const Matrix<_Scalar,4,1>, _Options> Coefficients; - - typedef traits<Quaternion<_Scalar, _Options> > TraitsBase; + typedef traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> > TraitsBase; enum { - IsAligned = TraitsBase::IsAligned, Flags = TraitsBase::Flags & ~LvalueBit }; }; } -/** \brief Quaternion expression mapping a constant memory buffer +/** \ingroup Geometry_Module + * \brief Quaternion expression mapping a constant memory buffer * - * \param _Scalar the type of the Quaternion coefficients - * \param _Options see class Map + * \tparam _Scalar the type of the Quaternion coefficients + * \tparam _Options see class Map * * This is a specialization of class Map for Quaternion. This class allows to view * a 4 scalar memory buffer as an Eigen's Quaternion object. @@ -371,10 +356,11 @@ class Map<const Quaternion<_Scalar>, _Options > const Coefficients m_coeffs; }; -/** \brief Expression of a quaternion from a memory buffer +/** \ingroup Geometry_Module + * \brief Expression of a quaternion from a memory buffer * - * \param _Scalar the type of the Quaternion coefficients - * \param _Options see class Map + * \tparam _Scalar the type of the Quaternion coefficients + * \tparam _Options see class Map * * This is a specialization of class Map for Quaternion. This class allows to view * a 4 scalar memory buffer as an Eigen's Quaternion object. @@ -395,7 +381,7 @@ class Map<Quaternion<_Scalar>, _Options > /** Constructs a Mapped Quaternion object from the pointer \a coeffs * - * The pointer \a coeffs must reference the four coeffecients of Quaternion in the following order: + * The pointer \a coeffs must reference the four coefficients of Quaternion in the following order: * \code *coeffs == {x, y, z, w} \endcode * * If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */ @@ -409,16 +395,16 @@ class Map<Quaternion<_Scalar>, _Options > }; /** \ingroup Geometry_Module - * Map an unaligned array of single precision scalar as a quaternion */ + * Map an unaligned array of single precision scalars as a quaternion */ typedef Map<Quaternion<float>, 0> QuaternionMapf; /** \ingroup Geometry_Module - * Map an unaligned array of double precision scalar as a quaternion */ + * Map an unaligned array of double precision scalars as a quaternion */ typedef Map<Quaternion<double>, 0> QuaternionMapd; /** \ingroup Geometry_Module - * Map a 16-bits aligned array of double precision scalars as a quaternion */ + * Map a 16-byte aligned array of single precision scalars as a quaternion */ typedef Map<Quaternion<float>, Aligned> QuaternionMapAlignedf; /** \ingroup Geometry_Module - * Map a 16-bits aligned array of double precision scalars as a quaternion */ + * Map a 16-byte aligned array of double precision scalars as a quaternion */ typedef Map<Quaternion<double>, Aligned> QuaternionMapAlignedd; /*************************************************************************** @@ -505,9 +491,11 @@ EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const Quaternion template<class Derived> EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const AngleAxisType& aa) { + using std::cos; + using std::sin; Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings - this->w() = internal::cos(ha); - this->vec() = internal::sin(ha) * aa.axis(); + this->w() = cos(ha); + this->vec() = sin(ha) * aa.axis(); return derived(); } @@ -581,12 +569,13 @@ template<typename Derived1, typename Derived2> inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b) { using std::max; + using std::sqrt; Vector3 v0 = a.normalized(); Vector3 v1 = b.normalized(); Scalar c = v1.dot(v0); // if dot == -1, vectors are nearly opposites - // => accuraletly compute the rotation axis by computing the + // => accurately compute the rotation axis by computing the // intersection of the two planes. This is done by solving: // x^T v0 = 0 // x^T v1 = 0 @@ -601,12 +590,12 @@ inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Deri Vector3 axis = svd.matrixV().col(2); Scalar w2 = (Scalar(1)+c)*Scalar(0.5); - this->w() = internal::sqrt(w2); - this->vec() = axis * internal::sqrt(Scalar(1) - w2); + this->w() = sqrt(w2); + this->vec() = axis * sqrt(Scalar(1) - w2); return derived(); } Vector3 axis = v0.cross(v1); - Scalar s = internal::sqrt((Scalar(1)+c)*Scalar(2)); + Scalar s = sqrt((Scalar(1)+c)*Scalar(2)); Scalar invs = Scalar(1)/s; this->vec() = axis * invs; this->w() = s * Scalar(0.5); @@ -677,24 +666,32 @@ inline typename internal::traits<Derived>::Scalar QuaternionBase<Derived>::angularDistance(const QuaternionBase<OtherDerived>& other) const { using std::acos; - double d = internal::abs(this->dot(other)); + using std::abs; + double d = abs(this->dot(other)); if (d>=1.0) return Scalar(0); return static_cast<Scalar>(2 * acos(d)); } + + /** \returns the spherical linear interpolation between the two quaternions - * \c *this and \a other at the parameter \a t + * \c *this and \a other at the parameter \a t in [0;1]. + * + * This represents an interpolation for a constant motion between \c *this and \a other, + * see also http://en.wikipedia.org/wiki/Slerp. */ template <class Derived> template <class OtherDerived> Quaternion<typename internal::traits<Derived>::Scalar> -QuaternionBase<Derived>::slerp(Scalar t, const QuaternionBase<OtherDerived>& other) const +QuaternionBase<Derived>::slerp(const Scalar& t, const QuaternionBase<OtherDerived>& other) const { using std::acos; + using std::sin; + using std::abs; static const Scalar one = Scalar(1) - NumTraits<Scalar>::epsilon(); Scalar d = this->dot(other); - Scalar absD = internal::abs(d); + Scalar absD = abs(d); Scalar scale0; Scalar scale1; @@ -708,10 +705,10 @@ QuaternionBase<Derived>::slerp(Scalar t, const QuaternionBase<OtherDerived>& oth { // theta is the angle between the 2 quaternions Scalar theta = acos(absD); - Scalar sinTheta = internal::sin(theta); + Scalar sinTheta = sin(theta); - scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta; - scale1 = internal::sin( ( t * theta) ) / sinTheta; + scale0 = sin( ( Scalar(1) - t ) * theta) / sinTheta; + scale1 = sin( ( t * theta) ) / sinTheta; } if(d<0) scale1 = -scale1; @@ -728,6 +725,7 @@ struct quaternionbase_assign_impl<Other,3,3> typedef DenseIndex Index; template<class Derived> static inline void run(QuaternionBase<Derived>& q, const Other& mat) { + using std::sqrt; // This algorithm comes from "Quaternion Calculus and Fast Animation", // Ken Shoemake, 1987 SIGGRAPH course notes Scalar t = mat.trace(); diff --git a/extern/Eigen3/Eigen/src/Geometry/Rotation2D.h b/extern/Eigen3/Eigen/src/Geometry/Rotation2D.h index 868e2ef312f..1cac343a5ee 100644 --- a/extern/Eigen3/Eigen/src/Geometry/Rotation2D.h +++ b/extern/Eigen3/Eigen/src/Geometry/Rotation2D.h @@ -59,7 +59,7 @@ protected: public: /** Construct a 2D counter clock wise rotation from the angle \a a in radian. */ - inline Rotation2D(Scalar a) : m_angle(a) {} + inline Rotation2D(const Scalar& a) : m_angle(a) {} /** \returns the rotation angle */ inline Scalar angle() const { return m_angle; } @@ -89,7 +89,7 @@ public: /** \returns the spherical interpolation between \c *this and \a other using * parameter \a t. It is in fact equivalent to a linear interpolation. */ - inline Rotation2D slerp(Scalar t, const Rotation2D& other) const + inline Rotation2D slerp(const Scalar& t, const Rotation2D& other) const { return m_angle * (1-t) + other.angle() * t; } /** \returns \c *this with scalar type casted to \a NewScalarType @@ -114,7 +114,7 @@ public: * determined by \a prec. * * \sa MatrixBase::isApprox() */ - bool isApprox(const Rotation2D& other, 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); } }; @@ -133,8 +133,9 @@ template<typename Scalar> template<typename Derived> Rotation2D<Scalar>& Rotation2D<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat) { + using std::atan2; EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime==2 && Derived::ColsAtCompileTime==2,YOU_MADE_A_PROGRAMMING_MISTAKE) - m_angle = internal::atan2(mat.coeff(1,0), mat.coeff(0,0)); + m_angle = atan2(mat.coeff(1,0), mat.coeff(0,0)); return *this; } @@ -144,8 +145,10 @@ template<typename Scalar> typename Rotation2D<Scalar>::Matrix2 Rotation2D<Scalar>::toRotationMatrix(void) const { - Scalar sinA = internal::sin(m_angle); - Scalar cosA = internal::cos(m_angle); + using std::sin; + using std::cos; + Scalar sinA = sin(m_angle); + Scalar cosA = cos(m_angle); return (Matrix2() << cosA, -sinA, sinA, cosA).finished(); } diff --git a/extern/Eigen3/Eigen/src/Geometry/Scaling.h b/extern/Eigen3/Eigen/src/Geometry/Scaling.h index 8edcac31c74..1c25f36fe62 100644 --- a/extern/Eigen3/Eigen/src/Geometry/Scaling.h +++ b/extern/Eigen3/Eigen/src/Geometry/Scaling.h @@ -99,7 +99,7 @@ public: * determined by \a prec. * * \sa MatrixBase::isApprox() */ - bool isApprox(const UniformScaling& other, typename NumTraits<Scalar>::Real prec = NumTraits<Scalar>::dummy_precision()) const + bool isApprox(const UniformScaling& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const { return internal::isApprox(m_factor, other.factor(), prec); } }; @@ -122,11 +122,11 @@ static inline UniformScaling<std::complex<RealScalar> > Scaling(const std::compl /** Constructs a 2D axis aligned scaling */ template<typename Scalar> -static inline DiagonalMatrix<Scalar,2> Scaling(Scalar sx, Scalar sy) +static inline DiagonalMatrix<Scalar,2> Scaling(const Scalar& sx, const Scalar& sy) { return DiagonalMatrix<Scalar,2>(sx, sy); } /** Constructs a 3D axis aligned scaling */ template<typename Scalar> -static inline DiagonalMatrix<Scalar,3> Scaling(Scalar sx, Scalar sy, Scalar sz) +static inline DiagonalMatrix<Scalar,3> Scaling(const Scalar& sx, const Scalar& sy, const Scalar& sz) { return DiagonalMatrix<Scalar,3>(sx, sy, sz); } /** Constructs an axis aligned scaling expression from vector expression \a coeffs diff --git a/extern/Eigen3/Eigen/src/Geometry/Transform.h b/extern/Eigen3/Eigen/src/Geometry/Transform.h index 4c1ef8eaade..498fea41a90 100644 --- a/extern/Eigen3/Eigen/src/Geometry/Transform.h +++ b/extern/Eigen3/Eigen/src/Geometry/Transform.h @@ -506,8 +506,8 @@ public: template<typename OtherDerived> inline Transform& prescale(const MatrixBase<OtherDerived> &other); - inline Transform& scale(Scalar s); - inline Transform& prescale(Scalar s); + inline Transform& scale(const Scalar& s); + inline Transform& prescale(const Scalar& s); template<typename OtherDerived> inline Transform& translate(const MatrixBase<OtherDerived> &other); @@ -521,8 +521,8 @@ public: template<typename RotationType> inline Transform& prerotate(const RotationType& rotation); - Transform& shear(Scalar sx, Scalar sy); - Transform& preshear(Scalar sx, Scalar sy); + Transform& shear(const Scalar& sx, const Scalar& sy); + Transform& preshear(const Scalar& sx, const Scalar& sy); inline Transform& operator=(const TranslationType& t); inline Transform& operator*=(const TranslationType& t) { return translate(t.vector()); } @@ -530,9 +530,9 @@ public: inline Transform& operator=(const UniformScaling<Scalar>& t); inline Transform& operator*=(const UniformScaling<Scalar>& s) { return scale(s.factor()); } - inline Transform<Scalar,Dim,(int(Mode)==int(Isometry)?Affine:Isometry)> operator*(const UniformScaling<Scalar>& s) const + inline Transform<Scalar,Dim,(int(Mode)==int(Isometry)?int(Affine):int(Mode))> operator*(const UniformScaling<Scalar>& s) const { - Transform<Scalar,Dim,(int(Mode)==int(Isometry)?Affine:Isometry),Options> res = *this; + Transform<Scalar,Dim,(int(Mode)==int(Isometry)?int(Affine):int(Mode)),Options> res = *this; res.scale(s.factor()); return res; } @@ -584,7 +584,7 @@ public: * determined by \a prec. * * \sa MatrixBase::isApprox() */ - bool isApprox(const Transform& other, typename NumTraits<Scalar>::Real prec = NumTraits<Scalar>::dummy_precision()) const + bool isApprox(const Transform& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const { return m_matrix.isApprox(other.m_matrix, prec); } /** Sets the last row to [0 ... 0 1] @@ -794,7 +794,7 @@ Transform<Scalar,Dim,Mode,Options>::scale(const MatrixBase<OtherDerived> &other) * \sa prescale(Scalar) */ template<typename Scalar, int Dim, int Mode, int Options> -inline Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::scale(Scalar s) +inline Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::scale(const Scalar& s) { EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS) linearExt() *= s; @@ -821,7 +821,7 @@ Transform<Scalar,Dim,Mode,Options>::prescale(const MatrixBase<OtherDerived> &oth * \sa scale(Scalar) */ template<typename Scalar, int Dim, int Mode, int Options> -inline Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::prescale(Scalar s) +inline Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::prescale(const Scalar& s) { EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS) m_matrix.template topRows<Dim>() *= s; @@ -909,7 +909,7 @@ Transform<Scalar,Dim,Mode,Options>::prerotate(const RotationType& rotation) */ template<typename Scalar, int Dim, int Mode, int Options> Transform<Scalar,Dim,Mode,Options>& -Transform<Scalar,Dim,Mode,Options>::shear(Scalar sx, Scalar sy) +Transform<Scalar,Dim,Mode,Options>::shear(const Scalar& sx, const Scalar& sy) { EIGEN_STATIC_ASSERT(int(Dim)==2, YOU_MADE_A_PROGRAMMING_MISTAKE) EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS) @@ -925,7 +925,7 @@ Transform<Scalar,Dim,Mode,Options>::shear(Scalar sx, Scalar sy) */ template<typename Scalar, int Dim, int Mode, int Options> Transform<Scalar,Dim,Mode,Options>& -Transform<Scalar,Dim,Mode,Options>::preshear(Scalar sx, Scalar sy) +Transform<Scalar,Dim,Mode,Options>::preshear(const Scalar& sx, const Scalar& sy) { EIGEN_STATIC_ASSERT(int(Dim)==2, YOU_MADE_A_PROGRAMMING_MISTAKE) EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS) diff --git a/extern/Eigen3/Eigen/src/Geometry/Umeyama.h b/extern/Eigen3/Eigen/src/Geometry/Umeyama.h index ac0939cde52..345b47e0c37 100644 --- a/extern/Eigen3/Eigen/src/Geometry/Umeyama.h +++ b/extern/Eigen3/Eigen/src/Geometry/Umeyama.h @@ -153,16 +153,21 @@ umeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, boo Rt.block(0,0,m,m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose(); } - // Eq. (42) - const Scalar c = 1/src_var * svd.singularValues().dot(S); - - // Eq. (41) - // Note that we first assign dst_mean to the destination so that there no need - // for a temporary. - Rt.col(m).head(m) = dst_mean; - Rt.col(m).head(m).noalias() -= c*Rt.topLeftCorner(m,m)*src_mean; - - if (with_scaling) Rt.block(0,0,m,m) *= c; + if (with_scaling) + { + // Eq. (42) + const Scalar c = 1/src_var * svd.singularValues().dot(S); + + // Eq. (41) + Rt.col(m).head(m) = dst_mean; + Rt.col(m).head(m).noalias() -= c*Rt.topLeftCorner(m,m)*src_mean; + Rt.block(0,0,m,m) *= c; + } + else + { + Rt.col(m).head(m) = dst_mean; + Rt.col(m).head(m).noalias() -= Rt.topLeftCorner(m,m)*src_mean; + } return Rt; } |