diff options
Diffstat (limited to 'eigen/Eigen/src/Geometry')
| -rw-r--r-- | eigen/Eigen/src/Geometry/AlignedBox.h | 98 | ||||
| -rw-r--r-- | eigen/Eigen/src/Geometry/AngleAxis.h | 85 | ||||
| -rw-r--r-- | eigen/Eigen/src/Geometry/CMakeLists.txt | 8 | ||||
| -rw-r--r-- | eigen/Eigen/src/Geometry/EulerAngles.h | 22 | ||||
| -rw-r--r-- | eigen/Eigen/src/Geometry/Homogeneous.h | 290 | ||||
| -rw-r--r-- | eigen/Eigen/src/Geometry/Hyperplane.h | 64 | ||||
| -rw-r--r-- | eigen/Eigen/src/Geometry/OrthoMethods.h | 58 | ||||
| -rw-r--r-- | eigen/Eigen/src/Geometry/ParametrizedLine.h | 99 | ||||
| -rw-r--r-- | eigen/Eigen/src/Geometry/Quaternion.h | 225 | ||||
| -rw-r--r-- | eigen/Eigen/src/Geometry/Rotation2D.h | 85 | ||||
| -rw-r--r-- | eigen/Eigen/src/Geometry/RotationBase.h | 48 | ||||
| -rw-r--r-- | eigen/Eigen/src/Geometry/Scaling.h | 38 | ||||
| -rw-r--r-- | eigen/Eigen/src/Geometry/Transform.h | 274 | ||||
| -rw-r--r-- | eigen/Eigen/src/Geometry/Translation.h | 60 | ||||
| -rw-r--r-- | eigen/Eigen/src/Geometry/Umeyama.h | 19 | ||||
| -rw-r--r-- | eigen/Eigen/src/Geometry/arch/CMakeLists.txt | 6 | ||||
| -rw-r--r-- | eigen/Eigen/src/Geometry/arch/Geometry_SSE.h | 56 |
17 files changed, 973 insertions, 562 deletions
diff --git a/eigen/Eigen/src/Geometry/AlignedBox.h b/eigen/Eigen/src/Geometry/AlignedBox.h index 7e1cd9e..c902d8f 100644 --- a/eigen/Eigen/src/Geometry/AlignedBox.h +++ b/eigen/Eigen/src/Geometry/AlignedBox.h @@ -34,10 +34,11 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) enum { AmbientDimAtCompileTime = _AmbientDim }; typedef _Scalar Scalar; typedef NumTraits<Scalar> ScalarTraits; - typedef DenseIndex Index; + typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 typedef typename ScalarTraits::Real RealScalar; - typedef typename ScalarTraits::NonInteger NonInteger; + typedef typename ScalarTraits::NonInteger NonInteger; typedef Matrix<Scalar,AmbientDimAtCompileTime,1> VectorType; + typedef CwiseBinaryOp<internal::scalar_sum_op<Scalar>, const VectorType, const VectorType> VectorTypeSum; /** Define constants to name the corners of a 1D, 2D or 3D axis aligned bounding box */ enum CornerType @@ -61,77 +62,76 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) /** Default constructor initializing a null box. */ - inline AlignedBox() - { if (AmbientDimAtCompileTime!=Dynamic) setEmpty(); } + EIGEN_DEVICE_FUNC inline AlignedBox() + { if (EIGEN_CONST_CONDITIONAL(AmbientDimAtCompileTime!=Dynamic)) setEmpty(); } /** Constructs a null box with \a _dim the dimension of the ambient space. */ - inline explicit AlignedBox(Index _dim) : m_min(_dim), m_max(_dim) + EIGEN_DEVICE_FUNC inline explicit AlignedBox(Index _dim) : m_min(_dim), m_max(_dim) { setEmpty(); } /** Constructs a box with extremities \a _min and \a _max. * \warning If either component of \a _min is larger than the same component of \a _max, the constructed box is empty. */ template<typename OtherVectorType1, typename OtherVectorType2> - inline AlignedBox(const OtherVectorType1& _min, const OtherVectorType2& _max) : m_min(_min), m_max(_max) {} + EIGEN_DEVICE_FUNC inline AlignedBox(const OtherVectorType1& _min, const OtherVectorType2& _max) : m_min(_min), m_max(_max) {} /** Constructs a box containing a single point \a p. */ template<typename Derived> - inline explicit AlignedBox(const MatrixBase<Derived>& p) : m_min(p), m_max(m_min) + EIGEN_DEVICE_FUNC inline explicit AlignedBox(const MatrixBase<Derived>& p) : m_min(p), m_max(m_min) { } - ~AlignedBox() {} + EIGEN_DEVICE_FUNC ~AlignedBox() {} /** \returns the dimension in which the box holds */ - inline Index dim() const { return AmbientDimAtCompileTime==Dynamic ? m_min.size() : Index(AmbientDimAtCompileTime); } + EIGEN_DEVICE_FUNC inline Index dim() const { return AmbientDimAtCompileTime==Dynamic ? m_min.size() : Index(AmbientDimAtCompileTime); } /** \deprecated use isEmpty() */ - inline bool isNull() const { return isEmpty(); } + EIGEN_DEVICE_FUNC inline bool isNull() const { return isEmpty(); } /** \deprecated use setEmpty() */ - inline void setNull() { setEmpty(); } + EIGEN_DEVICE_FUNC inline void setNull() { setEmpty(); } /** \returns true if the box is empty. * \sa setEmpty */ - inline bool isEmpty() const { return (m_min.array() > m_max.array()).any(); } + EIGEN_DEVICE_FUNC inline bool isEmpty() const { return (m_min.array() > m_max.array()).any(); } /** Makes \c *this an empty box. * \sa isEmpty */ - inline void setEmpty() + EIGEN_DEVICE_FUNC inline void setEmpty() { m_min.setConstant( ScalarTraits::highest() ); m_max.setConstant( ScalarTraits::lowest() ); } /** \returns the minimal corner */ - inline const VectorType& (min)() const { return m_min; } + EIGEN_DEVICE_FUNC inline const VectorType& (min)() const { return m_min; } /** \returns a non const reference to the minimal corner */ - inline VectorType& (min)() { return m_min; } + EIGEN_DEVICE_FUNC inline VectorType& (min)() { return m_min; } /** \returns the maximal corner */ - inline const VectorType& (max)() const { return m_max; } + EIGEN_DEVICE_FUNC inline const VectorType& (max)() const { return m_max; } /** \returns a non const reference to the maximal corner */ - inline VectorType& (max)() { return m_max; } + EIGEN_DEVICE_FUNC inline VectorType& (max)() { return m_max; } /** \returns the center of the box */ - inline const CwiseUnaryOp<internal::scalar_quotient1_op<Scalar>, - const CwiseBinaryOp<internal::scalar_sum_op<Scalar>, const VectorType, const VectorType> > + EIGEN_DEVICE_FUNC inline const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(VectorTypeSum, RealScalar, quotient) center() const - { return (m_min+m_max)/2; } + { return (m_min+m_max)/RealScalar(2); } /** \returns the lengths of the sides of the bounding box. * Note that this function does not get the same * result for integral or floating scalar types: see */ - inline const CwiseBinaryOp< internal::scalar_difference_op<Scalar>, const VectorType, const VectorType> sizes() const + EIGEN_DEVICE_FUNC inline const CwiseBinaryOp< internal::scalar_difference_op<Scalar,Scalar>, const VectorType, const VectorType> sizes() const { return m_max - m_min; } /** \returns the volume of the bounding box */ - inline Scalar volume() const + EIGEN_DEVICE_FUNC inline Scalar volume() const { return sizes().prod(); } /** \returns an expression for the bounding box diagonal vector * if the length of the diagonal is needed: diagonal().norm() * will provide it. */ - inline CwiseBinaryOp< internal::scalar_difference_op<Scalar>, const VectorType, const VectorType> diagonal() const + EIGEN_DEVICE_FUNC inline CwiseBinaryOp< internal::scalar_difference_op<Scalar,Scalar>, const VectorType, const VectorType> diagonal() const { return sizes(); } /** \returns the vertex of the bounding box at the corner defined by @@ -143,7 +143,7 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) * For 3D bounding boxes, the following names are added: * BottomLeftCeil, BottomRightCeil, TopLeftCeil, TopRightCeil. */ - inline VectorType corner(CornerType corner) const + EIGEN_DEVICE_FUNC inline VectorType corner(CornerType corner) const { EIGEN_STATIC_ASSERT(_AmbientDim <= 3, THIS_METHOD_IS_ONLY_FOR_VECTORS_OF_A_SPECIFIC_SIZE); @@ -161,7 +161,7 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) /** \returns a random point inside the bounding box sampled with * a uniform distribution */ - inline VectorType sample() const + EIGEN_DEVICE_FUNC inline VectorType sample() const { VectorType r(dim()); for(Index d=0; d<dim(); ++d) @@ -179,27 +179,27 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) /** \returns true if the point \a p is inside the box \c *this. */ template<typename Derived> - inline bool contains(const MatrixBase<Derived>& p) const + EIGEN_DEVICE_FUNC inline bool contains(const MatrixBase<Derived>& p) const { - typename internal::nested<Derived,2>::type p_n(p.derived()); + typename internal::nested_eval<Derived,2>::type p_n(p.derived()); return (m_min.array()<=p_n.array()).all() && (p_n.array()<=m_max.array()).all(); } /** \returns true if the box \a b is entirely inside the box \c *this. */ - inline bool contains(const AlignedBox& b) const + EIGEN_DEVICE_FUNC inline bool contains(const AlignedBox& b) const { return (m_min.array()<=(b.min)().array()).all() && ((b.max)().array()<=m_max.array()).all(); } /** \returns true if the box \a b is intersecting the box \c *this. * \sa intersection, clamp */ - inline bool intersects(const AlignedBox& b) const + EIGEN_DEVICE_FUNC inline bool intersects(const AlignedBox& b) const { return (m_min.array()<=(b.max)().array()).all() && ((b.min)().array()<=m_max.array()).all(); } /** Extends \c *this such that it contains the point \a p and returns a reference to \c *this. * \sa extend(const AlignedBox&) */ template<typename Derived> - inline AlignedBox& extend(const MatrixBase<Derived>& p) + EIGEN_DEVICE_FUNC inline AlignedBox& extend(const MatrixBase<Derived>& p) { - typename internal::nested<Derived,2>::type p_n(p.derived()); + typename internal::nested_eval<Derived,2>::type p_n(p.derived()); m_min = m_min.cwiseMin(p_n); m_max = m_max.cwiseMax(p_n); return *this; @@ -207,7 +207,7 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) /** Extends \c *this such that it contains the box \a b and returns a reference to \c *this. * \sa merged, extend(const MatrixBase&) */ - inline AlignedBox& extend(const AlignedBox& b) + EIGEN_DEVICE_FUNC inline AlignedBox& extend(const AlignedBox& b) { m_min = m_min.cwiseMin(b.m_min); m_max = m_max.cwiseMax(b.m_max); @@ -217,7 +217,7 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) /** Clamps \c *this by the box \a b and returns a reference to \c *this. * \note If the boxes don't intersect, the resulting box is empty. * \sa intersection(), intersects() */ - inline AlignedBox& clamp(const AlignedBox& b) + EIGEN_DEVICE_FUNC inline AlignedBox& clamp(const AlignedBox& b) { m_min = m_min.cwiseMax(b.m_min); m_max = m_max.cwiseMin(b.m_max); @@ -227,20 +227,20 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) /** Returns an AlignedBox that is the intersection of \a b and \c *this * \note If the boxes don't intersect, the resulting box is empty. * \sa intersects(), clamp, contains() */ - inline AlignedBox intersection(const AlignedBox& b) const + EIGEN_DEVICE_FUNC inline AlignedBox intersection(const AlignedBox& b) const {return AlignedBox(m_min.cwiseMax(b.m_min), m_max.cwiseMin(b.m_max)); } /** Returns an AlignedBox that is the union of \a b and \c *this. * \note Merging with an empty box may result in a box bigger than \c *this. * \sa extend(const AlignedBox&) */ - inline AlignedBox merged(const AlignedBox& b) const + EIGEN_DEVICE_FUNC inline AlignedBox merged(const AlignedBox& b) const { return AlignedBox(m_min.cwiseMin(b.m_min), m_max.cwiseMax(b.m_max)); } /** Translate \c *this by the vector \a t and returns a reference to \c *this. */ template<typename Derived> - inline AlignedBox& translate(const MatrixBase<Derived>& a_t) + EIGEN_DEVICE_FUNC inline AlignedBox& translate(const MatrixBase<Derived>& a_t) { - const typename internal::nested<Derived,2>::type t(a_t.derived()); + const typename internal::nested_eval<Derived,2>::type t(a_t.derived()); m_min += t; m_max += t; return *this; @@ -251,28 +251,28 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) * \sa exteriorDistance(const MatrixBase&), squaredExteriorDistance(const AlignedBox&) */ template<typename Derived> - inline Scalar squaredExteriorDistance(const MatrixBase<Derived>& p) const; + EIGEN_DEVICE_FUNC inline Scalar squaredExteriorDistance(const MatrixBase<Derived>& p) const; /** \returns the squared distance between the boxes \a b and \c *this, * and zero if the boxes intersect. * \sa exteriorDistance(const AlignedBox&), squaredExteriorDistance(const MatrixBase&) */ - inline Scalar squaredExteriorDistance(const AlignedBox& b) const; + EIGEN_DEVICE_FUNC inline Scalar squaredExteriorDistance(const AlignedBox& b) const; /** \returns the distance between the point \a p and the box \c *this, * and zero if \a p is inside the box. * \sa squaredExteriorDistance(const MatrixBase&), exteriorDistance(const AlignedBox&) */ template<typename Derived> - inline NonInteger exteriorDistance(const MatrixBase<Derived>& p) const - { using std::sqrt; return sqrt(NonInteger(squaredExteriorDistance(p))); } + EIGEN_DEVICE_FUNC inline NonInteger exteriorDistance(const MatrixBase<Derived>& p) const + { EIGEN_USING_STD_MATH(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(const AlignedBox&), exteriorDistance(const MatrixBase&) */ - inline NonInteger exteriorDistance(const AlignedBox& b) const - { using std::sqrt; return sqrt(NonInteger(squaredExteriorDistance(b))); } + EIGEN_DEVICE_FUNC inline NonInteger exteriorDistance(const AlignedBox& b) const + { EIGEN_USING_STD_MATH(sqrt) return sqrt(NonInteger(squaredExteriorDistance(b))); } /** \returns \c *this with scalar type casted to \a NewScalarType * @@ -280,7 +280,7 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) * then this function smartly returns a const reference to \c *this. */ template<typename NewScalarType> - inline typename internal::cast_return_type<AlignedBox, + EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<AlignedBox, AlignedBox<NewScalarType,AmbientDimAtCompileTime> >::type cast() const { return typename internal::cast_return_type<AlignedBox, @@ -289,7 +289,7 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) /** Copy constructor with scalar type conversion */ template<typename OtherScalarType> - inline explicit AlignedBox(const AlignedBox<OtherScalarType,AmbientDimAtCompileTime>& other) + EIGEN_DEVICE_FUNC inline explicit AlignedBox(const AlignedBox<OtherScalarType,AmbientDimAtCompileTime>& other) { m_min = (other.min)().template cast<Scalar>(); m_max = (other.max)().template cast<Scalar>(); @@ -299,7 +299,7 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) * determined by \a prec. * * \sa MatrixBase::isApprox() */ - bool isApprox(const AlignedBox& other, const RealScalar& prec = ScalarTraits::dummy_precision()) const + EIGEN_DEVICE_FUNC 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: @@ -311,9 +311,9 @@ protected: template<typename Scalar,int AmbientDim> template<typename Derived> -inline Scalar AlignedBox<Scalar,AmbientDim>::squaredExteriorDistance(const MatrixBase<Derived>& a_p) const +EIGEN_DEVICE_FUNC inline Scalar AlignedBox<Scalar,AmbientDim>::squaredExteriorDistance(const MatrixBase<Derived>& a_p) const { - typename internal::nested<Derived,2*AmbientDim>::type p(a_p.derived()); + typename internal::nested_eval<Derived,2*AmbientDim>::type p(a_p.derived()); Scalar dist2(0); Scalar aux; for (Index k=0; k<dim(); ++k) @@ -333,7 +333,7 @@ inline Scalar AlignedBox<Scalar,AmbientDim>::squaredExteriorDistance(const Matri } template<typename Scalar,int AmbientDim> -inline Scalar AlignedBox<Scalar,AmbientDim>::squaredExteriorDistance(const AlignedBox& b) const +EIGEN_DEVICE_FUNC inline Scalar AlignedBox<Scalar,AmbientDim>::squaredExteriorDistance(const AlignedBox& b) const { Scalar dist2(0); Scalar aux; diff --git a/eigen/Eigen/src/Geometry/AngleAxis.h b/eigen/Eigen/src/Geometry/AngleAxis.h index a8d3cdc..0af3c1b 100644 --- a/eigen/Eigen/src/Geometry/AngleAxis.h +++ b/eigen/Eigen/src/Geometry/AngleAxis.h @@ -69,57 +69,61 @@ protected: public: /** Default constructor without initialization. */ - AngleAxis() {} + EIGEN_DEVICE_FUNC AngleAxis() {} /** Constructs and initialize the angle-axis rotation from an \a angle in radian * and an \a axis which \b must \b be \b normalized. * * \warning If the \a axis vector is not normalized, then the angle-axis object * represents an invalid rotation. */ template<typename Derived> + EIGEN_DEVICE_FUNC 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 quaternion \a q. + * This function implicitly normalizes the quaternion \a q. + */ + template<typename QuatDerived> + EIGEN_DEVICE_FUNC inline explicit AngleAxis(const QuaternionBase<QuatDerived>& q) { *this = q; } /** Constructs and initialize the angle-axis rotation from a 3x3 rotation matrix. */ template<typename Derived> - inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; } + EIGEN_DEVICE_FUNC inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; } /** \returns the value of the rotation angle in radian */ - Scalar angle() const { return m_angle; } + EIGEN_DEVICE_FUNC Scalar angle() const { return m_angle; } /** \returns a read-write reference to the stored angle in radian */ - Scalar& angle() { return m_angle; } + EIGEN_DEVICE_FUNC Scalar& angle() { return m_angle; } /** \returns the rotation axis */ - const Vector3& axis() const { return m_axis; } + EIGEN_DEVICE_FUNC const Vector3& axis() const { return m_axis; } /** \returns a read-write reference to the stored rotation axis. * * \warning The rotation axis must remain a \b unit vector. */ - Vector3& axis() { return m_axis; } + EIGEN_DEVICE_FUNC Vector3& axis() { return m_axis; } /** Concatenates two rotations */ - inline QuaternionType operator* (const AngleAxis& other) const + EIGEN_DEVICE_FUNC inline QuaternionType operator* (const AngleAxis& other) const { return QuaternionType(*this) * QuaternionType(other); } /** Concatenates two rotations */ - inline QuaternionType operator* (const QuaternionType& other) const + EIGEN_DEVICE_FUNC inline QuaternionType operator* (const QuaternionType& other) const { return QuaternionType(*this) * other; } /** Concatenates two rotations */ - friend inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b) + friend EIGEN_DEVICE_FUNC inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b) { return a * QuaternionType(b); } /** \returns the inverse rotation, i.e., an angle-axis with opposite rotation angle */ - AngleAxis inverse() const + EIGEN_DEVICE_FUNC AngleAxis inverse() const { return AngleAxis(-m_angle, m_axis); } template<class QuatDerived> - AngleAxis& operator=(const QuaternionBase<QuatDerived>& q); + EIGEN_DEVICE_FUNC AngleAxis& operator=(const QuaternionBase<QuatDerived>& q); template<typename Derived> - AngleAxis& operator=(const MatrixBase<Derived>& m); + EIGEN_DEVICE_FUNC AngleAxis& operator=(const MatrixBase<Derived>& m); template<typename Derived> - AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m); - Matrix3 toRotationMatrix(void) const; + EIGEN_DEVICE_FUNC AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m); + EIGEN_DEVICE_FUNC Matrix3 toRotationMatrix(void) const; /** \returns \c *this with scalar type casted to \a NewScalarType * @@ -127,24 +131,24 @@ public: * then this function smartly returns a const reference to \c *this. */ template<typename NewScalarType> - inline typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const + EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const { return typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); } /** Copy constructor with scalar type conversion */ template<typename OtherScalarType> - inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other) + EIGEN_DEVICE_FUNC inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other) { m_axis = other.axis().template cast<Scalar>(); m_angle = Scalar(other.angle()); } - static inline const AngleAxis Identity() { return AngleAxis(Scalar(0), Vector3::UnitX()); } + EIGEN_DEVICE_FUNC static inline const AngleAxis Identity() { return AngleAxis(Scalar(0), Vector3::UnitX()); } /** \returns \c true if \c *this is approximately equal to \a other, within the precision * determined by \a prec. * * \sa MatrixBase::isApprox() */ - bool isApprox(const AngleAxis& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const + EIGEN_DEVICE_FUNC 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); } }; @@ -156,29 +160,32 @@ 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, and the computed angle is in the [0,pi] range. * - * \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) +EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived>& q) { - 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()) + EIGEN_USING_STD_MATH(atan2) + EIGEN_USING_STD_MATH(abs) + 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, abs(q.w())); + if(q.w() < 0) + n = -n; + 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 = Scalar(0); + m_axis << Scalar(1), Scalar(0), Scalar(0); } return *this; } @@ -187,7 +194,7 @@ AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived */ template<typename Scalar> template<typename Derived> -AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat) +EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat) { // Since a direct conversion would not be really faster, // let's use the robust Quaternion implementation: @@ -199,7 +206,7 @@ AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat) **/ template<typename Scalar> template<typename Derived> -AngleAxis<Scalar>& AngleAxis<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat) +EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat) { return *this = QuaternionType(mat); } @@ -208,10 +215,10 @@ AngleAxis<Scalar>& AngleAxis<Scalar>::fromRotationMatrix(const MatrixBase<Derive */ template<typename Scalar> typename AngleAxis<Scalar>::Matrix3 -AngleAxis<Scalar>::toRotationMatrix(void) const +EIGEN_DEVICE_FUNC AngleAxis<Scalar>::toRotationMatrix(void) const { - using std::sin; - using std::cos; + EIGEN_USING_STD_MATH(sin) + EIGEN_USING_STD_MATH(cos) Matrix3 res; Vector3 sin_axis = sin(m_angle) * m_axis; Scalar c = cos(m_angle); diff --git a/eigen/Eigen/src/Geometry/CMakeLists.txt b/eigen/Eigen/src/Geometry/CMakeLists.txt deleted file mode 100644 index f8f728b..0000000 --- a/eigen/Eigen/src/Geometry/CMakeLists.txt +++ /dev/null @@ -1,8 +0,0 @@ -FILE(GLOB Eigen_Geometry_SRCS "*.h") - -INSTALL(FILES - ${Eigen_Geometry_SRCS} - DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/Geometry COMPONENT Devel - ) - -ADD_SUBDIRECTORY(arch) diff --git a/eigen/Eigen/src/Geometry/EulerAngles.h b/eigen/Eigen/src/Geometry/EulerAngles.h index 82802fb..c633268 100644 --- a/eigen/Eigen/src/Geometry/EulerAngles.h +++ b/eigen/Eigen/src/Geometry/EulerAngles.h @@ -33,12 +33,12 @@ namespace Eigen { * \sa class AngleAxis */ template<typename Derived> -inline Matrix<typename MatrixBase<Derived>::Scalar,3,1> +EIGEN_DEVICE_FUNC 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; + EIGEN_USING_STD_MATH(atan2) + EIGEN_USING_STD_MATH(sin) + EIGEN_USING_STD_MATH(cos) /* Implemented from Graphics Gems IV */ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3) @@ -55,7 +55,12 @@ MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const res[0] = atan2(coeff(j,i), coeff(k,i)); 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); + if(res[0] > Scalar(0)) { + res[0] -= Scalar(EIGEN_PI); + } + else { + res[0] += Scalar(EIGEN_PI); + } Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm(); res[1] = -atan2(s2, coeff(i,i)); } @@ -84,7 +89,12 @@ MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const 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); + if(res[0] > Scalar(0)) { + res[0] -= Scalar(EIGEN_PI); + } + else { + res[0] += Scalar(EIGEN_PI); + } res[1] = atan2(-coeff(i,k), -c2); } else diff --git a/eigen/Eigen/src/Geometry/Homogeneous.h b/eigen/Eigen/src/Geometry/Homogeneous.h index 820ac96..5f0da1a 100644 --- a/eigen/Eigen/src/Geometry/Homogeneous.h +++ b/eigen/Eigen/src/Geometry/Homogeneous.h @@ -34,7 +34,7 @@ struct traits<Homogeneous<MatrixType,Direction> > : traits<MatrixType> { typedef typename traits<MatrixType>::StorageKind StorageKind; - typedef typename nested<MatrixType>::type MatrixTypeNested; + typedef typename ref_selector<MatrixType>::type MatrixTypeNested; typedef typename remove_reference<MatrixTypeNested>::type _MatrixTypeNested; enum { RowsPlusOne = (MatrixType::RowsAtCompileTime != Dynamic) ? @@ -48,8 +48,7 @@ struct traits<Homogeneous<MatrixType,Direction> > TmpFlags = _MatrixTypeNested::Flags & HereditaryBits, Flags = ColsAtCompileTime==1 ? (TmpFlags & ~RowMajorBit) : RowsAtCompileTime==1 ? (TmpFlags | RowMajorBit) - : TmpFlags, - CoeffReadCost = _MatrixTypeNested::CoeffReadCost + : TmpFlags }; }; @@ -59,102 +58,117 @@ template<typename MatrixType,typename Rhs> struct homogeneous_right_product_impl } // end namespace internal template<typename MatrixType,int _Direction> class Homogeneous - : internal::no_assignment_operator, public MatrixBase<Homogeneous<MatrixType,_Direction> > + : public MatrixBase<Homogeneous<MatrixType,_Direction> >, internal::no_assignment_operator { public: + typedef MatrixType NestedExpression; enum { Direction = _Direction }; typedef MatrixBase<Homogeneous> Base; EIGEN_DENSE_PUBLIC_INTERFACE(Homogeneous) - inline Homogeneous(const MatrixType& matrix) + EIGEN_DEVICE_FUNC explicit inline Homogeneous(const MatrixType& matrix) : m_matrix(matrix) {} - inline Index rows() const { return m_matrix.rows() + (int(Direction)==Vertical ? 1 : 0); } - inline Index cols() const { return m_matrix.cols() + (int(Direction)==Horizontal ? 1 : 0); } - - inline Scalar coeff(Index row, Index col=0) const - { - if( (int(Direction)==Vertical && row==m_matrix.rows()) - || (int(Direction)==Horizontal && col==m_matrix.cols())) - return Scalar(1); - return m_matrix.coeff(row, col); - } + EIGEN_DEVICE_FUNC inline Index rows() const { return m_matrix.rows() + (int(Direction)==Vertical ? 1 : 0); } + EIGEN_DEVICE_FUNC inline Index cols() const { return m_matrix.cols() + (int(Direction)==Horizontal ? 1 : 0); } + + EIGEN_DEVICE_FUNC const NestedExpression& nestedExpression() const { return m_matrix; } template<typename Rhs> - inline const internal::homogeneous_right_product_impl<Homogeneous,Rhs> + EIGEN_DEVICE_FUNC inline const Product<Homogeneous,Rhs> operator* (const MatrixBase<Rhs>& rhs) const { eigen_assert(int(Direction)==Horizontal); - return internal::homogeneous_right_product_impl<Homogeneous,Rhs>(m_matrix,rhs.derived()); + return Product<Homogeneous,Rhs>(*this,rhs.derived()); } template<typename Lhs> friend - inline const internal::homogeneous_left_product_impl<Homogeneous,Lhs> + EIGEN_DEVICE_FUNC inline const Product<Lhs,Homogeneous> operator* (const MatrixBase<Lhs>& lhs, const Homogeneous& rhs) { eigen_assert(int(Direction)==Vertical); - return internal::homogeneous_left_product_impl<Homogeneous,Lhs>(lhs.derived(),rhs.m_matrix); + return Product<Lhs,Homogeneous>(lhs.derived(),rhs); } template<typename Scalar, int Dim, int Mode, int Options> friend - inline const internal::homogeneous_left_product_impl<Homogeneous,Transform<Scalar,Dim,Mode,Options> > + EIGEN_DEVICE_FUNC inline const Product<Transform<Scalar,Dim,Mode,Options>, Homogeneous > operator* (const Transform<Scalar,Dim,Mode,Options>& lhs, const Homogeneous& rhs) { eigen_assert(int(Direction)==Vertical); - return internal::homogeneous_left_product_impl<Homogeneous,Transform<Scalar,Dim,Mode,Options> >(lhs,rhs.m_matrix); + return Product<Transform<Scalar,Dim,Mode,Options>, Homogeneous>(lhs,rhs); + } + + template<typename Func> + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::result_of<Func(Scalar,Scalar)>::type + redux(const Func& func) const + { + return func(m_matrix.redux(func), Scalar(1)); } protected: typename MatrixType::Nested m_matrix; }; -/** \geometry_module +/** \geometry_module \ingroup Geometry_Module + * + * \returns a vector expression that is one longer than the vector argument, with the value 1 symbolically appended as the last coefficient. * - * \return an expression of the equivalent homogeneous vector + * This can be used to convert affine coordinates to homogeneous coordinates. * * \only_for_vectors * * Example: \include MatrixBase_homogeneous.cpp * Output: \verbinclude MatrixBase_homogeneous.out * - * \sa class Homogeneous + * \sa VectorwiseOp::homogeneous(), class Homogeneous */ template<typename Derived> -inline typename MatrixBase<Derived>::HomogeneousReturnType +EIGEN_DEVICE_FUNC inline typename MatrixBase<Derived>::HomogeneousReturnType MatrixBase<Derived>::homogeneous() const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived); - return derived(); + return HomogeneousReturnType(derived()); } -/** \geometry_module +/** \geometry_module \ingroup Geometry_Module * - * \returns a matrix expression of homogeneous column (or row) vectors + * \returns an expression where the value 1 is symbolically appended as the final coefficient to each column (or row) of the matrix. + * + * This can be used to convert affine coordinates to homogeneous coordinates. * * Example: \include VectorwiseOp_homogeneous.cpp * Output: \verbinclude VectorwiseOp_homogeneous.out * - * \sa MatrixBase::homogeneous() */ + * \sa MatrixBase::homogeneous(), class Homogeneous */ template<typename ExpressionType, int Direction> -inline Homogeneous<ExpressionType,Direction> +EIGEN_DEVICE_FUNC inline Homogeneous<ExpressionType,Direction> VectorwiseOp<ExpressionType,Direction>::homogeneous() const { - return _expression(); + return HomogeneousReturnType(_expression()); } -/** \geometry_module +/** \geometry_module \ingroup Geometry_Module * - * \returns an expression of the homogeneous normalized vector of \c *this + * \brief homogeneous normalization + * + * \returns a vector expression of the N-1 first coefficients of \c *this divided by that last coefficient. + * + * This can be used to convert homogeneous coordinates to affine coordinates. + * + * It is essentially a shortcut for: + * \code + this->head(this->size()-1)/this->coeff(this->size()-1); + \endcode * * Example: \include MatrixBase_hnormalized.cpp * Output: \verbinclude MatrixBase_hnormalized.out * * \sa VectorwiseOp::hnormalized() */ template<typename Derived> -inline const typename MatrixBase<Derived>::HNormalizedReturnType +EIGEN_DEVICE_FUNC inline const typename MatrixBase<Derived>::HNormalizedReturnType MatrixBase<Derived>::hnormalized() const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived); @@ -163,16 +177,22 @@ MatrixBase<Derived>::hnormalized() const ColsAtCompileTime==1?1:size()-1) / coeff(size()-1); } -/** \geometry_module +/** \geometry_module \ingroup Geometry_Module + * + * \brief column or row-wise homogeneous normalization * - * \returns an expression of the homogeneous normalized vector of \c *this + * \returns an expression of the first N-1 coefficients of each column (or row) of \c *this divided by the last coefficient of each column (or row). + * + * This can be used to convert homogeneous coordinates to affine coordinates. + * + * It is conceptually equivalent to calling MatrixBase::hnormalized() to each column (or row) of \c *this. * * Example: \include DirectionWise_hnormalized.cpp * Output: \verbinclude DirectionWise_hnormalized.out * * \sa MatrixBase::hnormalized() */ template<typename ExpressionType, int Direction> -inline const typename VectorwiseOp<ExpressionType,Direction>::HNormalizedReturnType +EIGEN_DEVICE_FUNC inline const typename VectorwiseOp<ExpressionType,Direction>::HNormalizedReturnType VectorwiseOp<ExpressionType,Direction>::hnormalized() const { return HNormalized_Block(_expression(),0,0, @@ -196,7 +216,7 @@ template<typename MatrixOrTransformType> struct take_matrix_for_product { typedef MatrixOrTransformType type; - static const type& run(const type &x) { return x; } + EIGEN_DEVICE_FUNC static const type& run(const type &x) { return x; } }; template<typename Scalar, int Dim, int Mode,int Options> @@ -204,7 +224,7 @@ struct take_matrix_for_product<Transform<Scalar, Dim, Mode, Options> > { typedef Transform<Scalar, Dim, Mode, Options> TransformType; typedef typename internal::add_const<typename TransformType::ConstAffinePart>::type type; - static type run (const TransformType& x) { return x.affine(); } + EIGEN_DEVICE_FUNC static type run (const TransformType& x) { return x.affine(); } }; template<typename Scalar, int Dim, int Options> @@ -212,7 +232,7 @@ struct take_matrix_for_product<Transform<Scalar, Dim, Projective, Options> > { typedef Transform<Scalar, Dim, Projective, Options> TransformType; typedef typename TransformType::MatrixType type; - static const type& run (const TransformType& x) { return x.matrix(); } + EIGEN_DEVICE_FUNC static const type& run (const TransformType& x) { return x.matrix(); } }; template<typename MatrixType,typename Lhs> @@ -237,16 +257,15 @@ struct homogeneous_left_product_impl<Homogeneous<MatrixType,Vertical>,Lhs> typedef typename traits<homogeneous_left_product_impl>::LhsMatrixType LhsMatrixType; typedef typename remove_all<LhsMatrixType>::type LhsMatrixTypeCleaned; typedef typename remove_all<typename LhsMatrixTypeCleaned::Nested>::type LhsMatrixTypeNested; - typedef typename MatrixType::Index Index; - homogeneous_left_product_impl(const Lhs& lhs, const MatrixType& rhs) + EIGEN_DEVICE_FUNC homogeneous_left_product_impl(const Lhs& lhs, const MatrixType& rhs) : m_lhs(take_matrix_for_product<Lhs>::run(lhs)), m_rhs(rhs) {} - inline Index rows() const { return m_lhs.rows(); } - inline Index cols() const { return m_rhs.cols(); } + EIGEN_DEVICE_FUNC inline Index rows() const { return m_lhs.rows(); } + EIGEN_DEVICE_FUNC inline Index cols() const { return m_rhs.cols(); } - template<typename Dest> void evalTo(Dest& dst) const + template<typename Dest> EIGEN_DEVICE_FUNC void evalTo(Dest& dst) const { // FIXME investigate how to allow lazy evaluation of this product when possible dst = Block<const LhsMatrixTypeNested, @@ -277,15 +296,14 @@ struct homogeneous_right_product_impl<Homogeneous<MatrixType,Horizontal>,Rhs> : public ReturnByValue<homogeneous_right_product_impl<Homogeneous<MatrixType,Horizontal>,Rhs> > { typedef typename remove_all<typename Rhs::Nested>::type RhsNested; - typedef typename MatrixType::Index Index; - homogeneous_right_product_impl(const MatrixType& lhs, const Rhs& rhs) + EIGEN_DEVICE_FUNC homogeneous_right_product_impl(const MatrixType& lhs, const Rhs& rhs) : m_lhs(lhs), m_rhs(rhs) {} - inline Index rows() const { return m_lhs.rows(); } - inline Index cols() const { return m_rhs.cols(); } + EIGEN_DEVICE_FUNC inline Index rows() const { return m_lhs.rows(); } + EIGEN_DEVICE_FUNC inline Index cols() const { return m_rhs.cols(); } - template<typename Dest> void evalTo(Dest& dst) const + template<typename Dest> EIGEN_DEVICE_FUNC void evalTo(Dest& dst) const { // FIXME investigate how to allow lazy evaluation of this product when possible dst = m_lhs * Block<const RhsNested, @@ -300,6 +318,178 @@ struct homogeneous_right_product_impl<Homogeneous<MatrixType,Horizontal>,Rhs> typename Rhs::Nested m_rhs; }; +template<typename ArgType,int Direction> +struct evaluator_traits<Homogeneous<ArgType,Direction> > +{ + typedef typename storage_kind_to_evaluator_kind<typename ArgType::StorageKind>::Kind Kind; + typedef HomogeneousShape Shape; +}; + +template<> struct AssignmentKind<DenseShape,HomogeneousShape> { typedef Dense2Dense Kind; }; + + +template<typename ArgType,int Direction> +struct unary_evaluator<Homogeneous<ArgType,Direction>, IndexBased> + : evaluator<typename Homogeneous<ArgType,Direction>::PlainObject > +{ + typedef Homogeneous<ArgType,Direction> XprType; + typedef typename XprType::PlainObject PlainObject; + typedef evaluator<PlainObject> Base; + + EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& op) + : Base(), m_temp(op) + { + ::new (static_cast<Base*>(this)) Base(m_temp); + } + +protected: + PlainObject m_temp; +}; + +// dense = homogeneous +template< typename DstXprType, typename ArgType, typename Scalar> +struct Assignment<DstXprType, Homogeneous<ArgType,Vertical>, internal::assign_op<Scalar,typename ArgType::Scalar>, Dense2Dense> +{ + typedef Homogeneous<ArgType,Vertical> SrcXprType; + EIGEN_DEVICE_FUNC static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar,typename ArgType::Scalar> &) + { + Index dstRows = src.rows(); + Index dstCols = src.cols(); + if((dst.rows()!=dstRows) || (dst.cols()!=dstCols)) + dst.resize(dstRows, dstCols); + + dst.template topRows<ArgType::RowsAtCompileTime>(src.nestedExpression().rows()) = src.nestedExpression(); + dst.row(dst.rows()-1).setOnes(); + } +}; + +// dense = homogeneous +template< typename DstXprType, typename ArgType, typename Scalar> +struct Assignment<DstXprType, Homogeneous<ArgType,Horizontal>, internal::assign_op<Scalar,typename ArgType::Scalar>, Dense2Dense> +{ + typedef Homogeneous<ArgType,Horizontal> SrcXprType; + EIGEN_DEVICE_FUNC static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar,typename ArgType::Scalar> &) + { + Index dstRows = src.rows(); + Index dstCols = src.cols(); + if((dst.rows()!=dstRows) || (dst.cols()!=dstCols)) + dst.resize(dstRows, dstCols); + + dst.template leftCols<ArgType::ColsAtCompileTime>(src.nestedExpression().cols()) = src.nestedExpression(); + dst.col(dst.cols()-1).setOnes(); + } +}; + +template<typename LhsArg, typename Rhs, int ProductTag> +struct generic_product_impl<Homogeneous<LhsArg,Horizontal>, Rhs, HomogeneousShape, DenseShape, ProductTag> +{ + template<typename Dest> + EIGEN_DEVICE_FUNC static void evalTo(Dest& dst, const Homogeneous<LhsArg,Horizontal>& lhs, const Rhs& rhs) + { + homogeneous_right_product_impl<Homogeneous<LhsArg,Horizontal>, Rhs>(lhs.nestedExpression(), rhs).evalTo(dst); + } +}; + +template<typename Lhs,typename Rhs> +struct homogeneous_right_product_refactoring_helper +{ + enum { + Dim = Lhs::ColsAtCompileTime, + Rows = Lhs::RowsAtCompileTime + }; + typedef typename Rhs::template ConstNRowsBlockXpr<Dim>::Type LinearBlockConst; + typedef typename remove_const<LinearBlockConst>::type LinearBlock; + typedef typename Rhs::ConstRowXpr ConstantColumn; + typedef Replicate<const ConstantColumn,Rows,1> ConstantBlock; + typedef Product<Lhs,LinearBlock,LazyProduct> LinearProduct; + typedef CwiseBinaryOp<internal::scalar_sum_op<typename Lhs::Scalar,typename Rhs::Scalar>, const LinearProduct, const ConstantBlock> Xpr; +}; + +template<typename Lhs, typename Rhs, int ProductTag> +struct product_evaluator<Product<Lhs, Rhs, LazyProduct>, ProductTag, HomogeneousShape, DenseShape> + : public evaluator<typename homogeneous_right_product_refactoring_helper<typename Lhs::NestedExpression,Rhs>::Xpr> +{ + typedef Product<Lhs, Rhs, LazyProduct> XprType; + typedef homogeneous_right_product_refactoring_helper<typename Lhs::NestedExpression,Rhs> helper; + typedef typename helper::ConstantBlock ConstantBlock; + typedef typename helper::Xpr RefactoredXpr; + typedef evaluator<RefactoredXpr> Base; + + EIGEN_DEVICE_FUNC explicit product_evaluator(const XprType& xpr) + : Base( xpr.lhs().nestedExpression() .lazyProduct( xpr.rhs().template topRows<helper::Dim>(xpr.lhs().nestedExpression().cols()) ) + + ConstantBlock(xpr.rhs().row(xpr.rhs().rows()-1),xpr.lhs().rows(), 1) ) + {} +}; + +template<typename Lhs, typename RhsArg, int ProductTag> +struct generic_product_impl<Lhs, Homogeneous<RhsArg,Vertical>, DenseShape, HomogeneousShape, ProductTag> +{ + template<typename Dest> + EIGEN_DEVICE_FUNC static void evalTo(Dest& dst, const Lhs& lhs, const Homogeneous<RhsArg,Vertical>& rhs) + { + homogeneous_left_product_impl<Homogeneous<RhsArg,Vertical>, Lhs>(lhs, rhs.nestedExpression()).evalTo(dst); + } +}; + +// TODO: the following specialization is to address a regression from 3.2 to 3.3 +// In the future, this path should be optimized. +template<typename Lhs, typename RhsArg, int ProductTag> +struct generic_product_impl<Lhs, Homogeneous<RhsArg,Vertical>, TriangularShape, HomogeneousShape, ProductTag> +{ + template<typename Dest> + static void evalTo(Dest& dst, const Lhs& lhs, const Homogeneous<RhsArg,Vertical>& rhs) + { + dst.noalias() = lhs * rhs.eval(); + } +}; + +template<typename Lhs,typename Rhs> +struct homogeneous_left_product_refactoring_helper +{ + enum { + Dim = Rhs::RowsAtCompileTime, + Cols = Rhs::ColsAtCompileTime + }; + typedef typename Lhs::template ConstNColsBlockXpr<Dim>::Type LinearBlockConst; + typedef typename remove_const<LinearBlockConst>::type LinearBlock; + typedef typename Lhs::ConstColXpr ConstantColumn; + typedef Replicate<const ConstantColumn,1,Cols> ConstantBlock; + typedef Product<LinearBlock,Rhs,LazyProduct> LinearProduct; + typedef CwiseBinaryOp<internal::scalar_sum_op<typename Lhs::Scalar,typename Rhs::Scalar>, const LinearProduct, const ConstantBlock> Xpr; +}; + +template<typename Lhs, typename Rhs, int ProductTag> +struct product_evaluator<Product<Lhs, Rhs, LazyProduct>, ProductTag, DenseShape, HomogeneousShape> + : public evaluator<typename homogeneous_left_product_refactoring_helper<Lhs,typename Rhs::NestedExpression>::Xpr> +{ + typedef Product<Lhs, Rhs, LazyProduct> XprType; + typedef homogeneous_left_product_refactoring_helper<Lhs,typename Rhs::NestedExpression> helper; + typedef typename helper::ConstantBlock ConstantBlock; + typedef typename helper::Xpr RefactoredXpr; + typedef evaluator<RefactoredXpr> Base; + + EIGEN_DEVICE_FUNC explicit product_evaluator(const XprType& xpr) + : Base( xpr.lhs().template leftCols<helper::Dim>(xpr.rhs().nestedExpression().rows()) .lazyProduct( xpr.rhs().nestedExpression() ) + + ConstantBlock(xpr.lhs().col(xpr.lhs().cols()-1),1,xpr.rhs().cols()) ) + {} +}; + +template<typename Scalar, int Dim, int Mode,int Options, typename RhsArg, int ProductTag> +struct generic_product_impl<Transform<Scalar,Dim,Mode,Options>, Homogeneous<RhsArg,Vertical>, DenseShape, HomogeneousShape, ProductTag> +{ + typedef Transform<Scalar,Dim,Mode,Options> TransformType; + template<typename Dest> + EIGEN_DEVICE_FUNC static void evalTo(Dest& dst, const TransformType& lhs, const Homogeneous<RhsArg,Vertical>& rhs) + { + homogeneous_left_product_impl<Homogeneous<RhsArg,Vertical>, TransformType>(lhs, rhs.nestedExpression()).evalTo(dst); + } +}; + +template<typename ExpressionType, int Side, bool Transposed> +struct permutation_matrix_product<ExpressionType, Side, Transposed, HomogeneousShape> + : public permutation_matrix_product<ExpressionType, Side, Transposed, DenseShape> +{}; + } // end namespace internal } // end namespace Eigen diff --git a/eigen/Eigen/src/Geometry/Hyperplane.h b/eigen/Eigen/src/Geometry/Hyperplane.h index 00b7c43..05929b2 100644 --- a/eigen/Eigen/src/Geometry/Hyperplane.h +++ b/eigen/Eigen/src/Geometry/Hyperplane.h @@ -22,8 +22,8 @@ namespace Eigen { * A hyperplane is an affine subspace of dimension n-1 in a space of dimension n. * For example, a hyperplane in a plane is a line; a hyperplane in 3-space is a plane. * - * \param _Scalar the scalar type, i.e., the type of the coefficients - * \param _AmbientDim the dimension of the ambient space, can be a compile time value or Dynamic. + * \tparam _Scalar the scalar type, i.e., the type of the coefficients + * \tparam _AmbientDim the dimension of the ambient space, can be a compile time value or Dynamic. * Notice that the dimension of the hyperplane is _AmbientDim-1. * * This class represents an hyperplane as the zero set of the implicit equation @@ -41,7 +41,7 @@ public: }; typedef _Scalar Scalar; typedef typename NumTraits<Scalar>::Real RealScalar; - typedef DenseIndex Index; + typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 typedef Matrix<Scalar,AmbientDimAtCompileTime,1> VectorType; typedef Matrix<Scalar,Index(AmbientDimAtCompileTime)==Dynamic ? Dynamic @@ -50,21 +50,21 @@ public: typedef const Block<const Coefficients,AmbientDimAtCompileTime,1> ConstNormalReturnType; /** Default constructor without initialization */ - inline Hyperplane() {} + EIGEN_DEVICE_FUNC inline Hyperplane() {} template<int OtherOptions> - Hyperplane(const Hyperplane<Scalar,AmbientDimAtCompileTime,OtherOptions>& other) + EIGEN_DEVICE_FUNC Hyperplane(const Hyperplane<Scalar,AmbientDimAtCompileTime,OtherOptions>& other) : m_coeffs(other.coeffs()) {} /** Constructs a dynamic-size hyperplane with \a _dim the dimension * of the ambient space */ - inline explicit Hyperplane(Index _dim) : m_coeffs(_dim+1) {} + EIGEN_DEVICE_FUNC inline explicit Hyperplane(Index _dim) : m_coeffs(_dim+1) {} /** Construct a plane from its normal \a n and a point \a e onto the plane. * \warning the vector normal is assumed to be normalized. */ - inline Hyperplane(const VectorType& n, const VectorType& e) + EIGEN_DEVICE_FUNC inline Hyperplane(const VectorType& n, const VectorType& e) : m_coeffs(n.size()+1) { normal() = n; @@ -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, const Scalar& d) + EIGEN_DEVICE_FUNC inline Hyperplane(const VectorType& n, const Scalar& d) : m_coeffs(n.size()+1) { normal() = n; @@ -85,7 +85,7 @@ public: /** Constructs a hyperplane passing through the two points. If the dimension of the ambient space * is greater than 2, then there isn't uniqueness, so an arbitrary choice is made. */ - static inline Hyperplane Through(const VectorType& p0, const VectorType& p1) + EIGEN_DEVICE_FUNC static inline Hyperplane Through(const VectorType& p0, const VectorType& p1) { Hyperplane result(p0.size()); result.normal() = (p1 - p0).unitOrthogonal(); @@ -96,7 +96,7 @@ public: /** Constructs a hyperplane passing through the three points. The dimension of the ambient space * is required to be exactly 3. */ - static inline Hyperplane Through(const VectorType& p0, const VectorType& p1, const VectorType& p2) + EIGEN_DEVICE_FUNC static inline Hyperplane Through(const VectorType& p0, const VectorType& p1, const VectorType& p2) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 3) Hyperplane result(p0.size()); @@ -120,19 +120,19 @@ public: * so an arbitrary choice is made. */ // FIXME to be consitent with the rest this could be implemented as a static Through function ?? - explicit Hyperplane(const ParametrizedLine<Scalar, AmbientDimAtCompileTime>& parametrized) + EIGEN_DEVICE_FUNC explicit Hyperplane(const ParametrizedLine<Scalar, AmbientDimAtCompileTime>& parametrized) { normal() = parametrized.direction().unitOrthogonal(); offset() = -parametrized.origin().dot(normal()); } - ~Hyperplane() {} + EIGEN_DEVICE_FUNC ~Hyperplane() {} /** \returns the dimension in which the plane holds */ - inline Index dim() const { return AmbientDimAtCompileTime==Dynamic ? m_coeffs.size()-1 : Index(AmbientDimAtCompileTime); } + EIGEN_DEVICE_FUNC inline Index dim() const { return AmbientDimAtCompileTime==Dynamic ? m_coeffs.size()-1 : Index(AmbientDimAtCompileTime); } /** normalizes \c *this */ - void normalize(void) + EIGEN_DEVICE_FUNC void normalize(void) { m_coeffs /= normal().norm(); } @@ -140,45 +140,45 @@ public: /** \returns the signed distance between the plane \c *this and a point \a p. * \sa absDistance() */ - inline Scalar signedDistance(const VectorType& p) const { return normal().dot(p) + offset(); } + EIGEN_DEVICE_FUNC inline Scalar signedDistance(const VectorType& p) const { return normal().dot(p) + offset(); } /** \returns the absolute distance between the plane \c *this and a point \a p. * \sa signedDistance() */ - inline Scalar absDistance(const VectorType& p) const { using std::abs; return abs(signedDistance(p)); } + EIGEN_DEVICE_FUNC inline Scalar absDistance(const VectorType& p) const { return numext::abs(signedDistance(p)); } /** \returns the projection of a point \a p onto the plane \c *this. */ - inline VectorType projection(const VectorType& p) const { return p - signedDistance(p) * normal(); } + EIGEN_DEVICE_FUNC inline VectorType projection(const VectorType& p) const { return p - signedDistance(p) * normal(); } /** \returns a constant reference to the unit normal vector of the plane, which corresponds * to the linear part of the implicit equation. */ - inline ConstNormalReturnType normal() const { return ConstNormalReturnType(m_coeffs,0,0,dim(),1); } + EIGEN_DEVICE_FUNC inline ConstNormalReturnType normal() const { return ConstNormalReturnType(m_coeffs,0,0,dim(),1); } /** \returns a non-constant reference to the unit normal vector of the plane, which corresponds * to the linear part of the implicit equation. */ - inline NormalReturnType normal() { return NormalReturnType(m_coeffs,0,0,dim(),1); } + EIGEN_DEVICE_FUNC inline NormalReturnType normal() { return NormalReturnType(m_coeffs,0,0,dim(),1); } /** \returns the distance to the origin, which is also the "constant term" of the implicit equation * \warning the vector normal is assumed to be normalized. */ - inline const Scalar& offset() const { return m_coeffs.coeff(dim()); } + EIGEN_DEVICE_FUNC inline const Scalar& offset() const { return m_coeffs.coeff(dim()); } /** \returns a non-constant reference to the distance to the origin, which is also the constant part * of the implicit equation */ - inline Scalar& offset() { return m_coeffs(dim()); } + EIGEN_DEVICE_FUNC inline Scalar& offset() { return m_coeffs(dim()); } /** \returns a constant reference to the coefficients c_i of the plane equation: * \f$ c_0*x_0 + ... + c_{d-1}*x_{d-1} + c_d = 0 \f$ */ - inline const Coefficients& coeffs() const { return m_coeffs; } + EIGEN_DEVICE_FUNC inline const Coefficients& coeffs() const { return m_coeffs; } /** \returns a non-constant reference to the coefficients c_i of the plane equation: * \f$ c_0*x_0 + ... + c_{d-1}*x_{d-1} + c_d = 0 \f$ */ - inline Coefficients& coeffs() { return m_coeffs; } + EIGEN_DEVICE_FUNC inline Coefficients& coeffs() { return m_coeffs; } /** \returns the intersection of *this with \a other. * @@ -186,16 +186,15 @@ public: * * \note If \a other is approximately parallel to *this, this method will return any point on *this. */ - VectorType intersection(const Hyperplane& other) const + EIGEN_DEVICE_FUNC 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(abs(coeffs().coeff(1))>abs(coeffs().coeff(0))) + if(numext::abs(coeffs().coeff(1))>numext::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)); @@ -215,10 +214,13 @@ public: * or a more generic #Affine transformation. The default is #Affine. */ template<typename XprType> - inline Hyperplane& transform(const MatrixBase<XprType>& mat, TransformTraits traits = Affine) + EIGEN_DEVICE_FUNC inline Hyperplane& transform(const MatrixBase<XprType>& mat, TransformTraits traits = Affine) { if (traits==Affine) + { normal() = mat.inverse().transpose() * normal(); + m_coeffs /= normal().norm(); + } else if (traits==Isometry) normal() = mat * normal(); else @@ -236,7 +238,7 @@ public: * Other kind of transformations are not supported. */ template<int TrOptions> - inline Hyperplane& transform(const Transform<Scalar,AmbientDimAtCompileTime,Affine,TrOptions>& t, + EIGEN_DEVICE_FUNC inline Hyperplane& transform(const Transform<Scalar,AmbientDimAtCompileTime,Affine,TrOptions>& t, TransformTraits traits = Affine) { transform(t.linear(), traits); @@ -250,7 +252,7 @@ public: * then this function smartly returns a const reference to \c *this. */ template<typename NewScalarType> - inline typename internal::cast_return_type<Hyperplane, + EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<Hyperplane, Hyperplane<NewScalarType,AmbientDimAtCompileTime,Options> >::type cast() const { return typename internal::cast_return_type<Hyperplane, @@ -259,7 +261,7 @@ public: /** Copy constructor with scalar type conversion */ template<typename OtherScalarType,int OtherOptions> - inline explicit Hyperplane(const Hyperplane<OtherScalarType,AmbientDimAtCompileTime,OtherOptions>& other) + EIGEN_DEVICE_FUNC inline explicit Hyperplane(const Hyperplane<OtherScalarType,AmbientDimAtCompileTime,OtherOptions>& other) { m_coeffs = other.coeffs().template cast<Scalar>(); } /** \returns \c true if \c *this is approximately equal to \a other, within the precision @@ -267,7 +269,7 @@ public: * * \sa MatrixBase::isApprox() */ template<int OtherOptions> - bool isApprox(const Hyperplane<Scalar,AmbientDimAtCompileTime,OtherOptions>& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const + EIGEN_DEVICE_FUNC 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/eigen/Eigen/src/Geometry/OrthoMethods.h b/eigen/Eigen/src/Geometry/OrthoMethods.h index 556bc81..a035e63 100644 --- a/eigen/Eigen/src/Geometry/OrthoMethods.h +++ b/eigen/Eigen/src/Geometry/OrthoMethods.h @@ -13,16 +13,24 @@ namespace Eigen { -/** \geometry_module +/** \geometry_module \ingroup Geometry_Module * * \returns the cross product of \c *this and \a other * * Here is a very good explanation of cross-product: http://xkcd.com/199/ + * + * With complex numbers, the cross product is implemented as + * \f$ (\mathbf{a}+i\mathbf{b}) \times (\mathbf{c}+i\mathbf{d}) = (\mathbf{a} \times \mathbf{c} - \mathbf{b} \times \mathbf{d}) - i(\mathbf{a} \times \mathbf{d} - \mathbf{b} \times \mathbf{c})\f$ + * * \sa MatrixBase::cross3() */ template<typename Derived> template<typename OtherDerived> -inline typename MatrixBase<Derived>::template cross_product_return_type<OtherDerived>::type +#ifndef EIGEN_PARSED_BY_DOXYGEN +EIGEN_DEVICE_FUNC inline typename MatrixBase<Derived>::template cross_product_return_type<OtherDerived>::type +#else +inline typename MatrixBase<Derived>::PlainObject +#endif MatrixBase<Derived>::cross(const MatrixBase<OtherDerived>& other) const { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived,3) @@ -30,8 +38,8 @@ MatrixBase<Derived>::cross(const MatrixBase<OtherDerived>& other) const // Note that there is no need for an expression here since the compiler // optimize such a small temporary very well (even within a complex expression) - typename internal::nested<Derived,2>::type lhs(derived()); - typename internal::nested<OtherDerived,2>::type rhs(other.derived()); + typename internal::nested_eval<Derived,2>::type lhs(derived()); + typename internal::nested_eval<OtherDerived,2>::type rhs(other.derived()); return typename cross_product_return_type<OtherDerived>::type( 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)), @@ -45,7 +53,7 @@ template< int Arch,typename VectorLhs,typename VectorRhs, typename Scalar = typename VectorLhs::Scalar, bool Vectorizable = bool((VectorLhs::Flags&VectorRhs::Flags)&PacketAccessBit)> struct cross3_impl { - static inline typename internal::plain_matrix_type<VectorLhs>::type + EIGEN_DEVICE_FUNC static inline typename internal::plain_matrix_type<VectorLhs>::type run(const VectorLhs& lhs, const VectorRhs& rhs) { return typename internal::plain_matrix_type<VectorLhs>::type( @@ -59,7 +67,7 @@ struct cross3_impl { } -/** \geometry_module +/** \geometry_module \ingroup Geometry_Module * * \returns the cross product of \c *this and \a other using only the x, y, and z coefficients * @@ -70,14 +78,14 @@ struct cross3_impl { */ template<typename Derived> template<typename OtherDerived> -inline typename MatrixBase<Derived>::PlainObject +EIGEN_DEVICE_FUNC inline typename MatrixBase<Derived>::PlainObject MatrixBase<Derived>::cross3(const MatrixBase<OtherDerived>& other) const { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived,4) EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,4) - typedef typename internal::nested<Derived,2>::type DerivedNested; - typedef typename internal::nested<OtherDerived,2>::type OtherDerivedNested; + typedef typename internal::nested_eval<Derived,2>::type DerivedNested; + typedef typename internal::nested_eval<OtherDerived,2>::type OtherDerivedNested; DerivedNested lhs(derived()); OtherDerivedNested rhs(other.derived()); @@ -86,38 +94,42 @@ MatrixBase<Derived>::cross3(const MatrixBase<OtherDerived>& other) const typename internal::remove_all<OtherDerivedNested>::type>::run(lhs,rhs); } -/** \returns a matrix expression of the cross product of each column or row +/** \geometry_module \ingroup Geometry_Module + * + * \returns a matrix expression of the cross product of each column or row * of the referenced expression with the \a other vector. * * The referenced matrix must have one dimension equal to 3. * The result matrix has the same dimensions than the referenced one. * - * \geometry_module - * * \sa MatrixBase::cross() */ template<typename ExpressionType, int Direction> template<typename OtherDerived> +EIGEN_DEVICE_FUNC const typename VectorwiseOp<ExpressionType,Direction>::CrossReturnType VectorwiseOp<ExpressionType,Direction>::cross(const MatrixBase<OtherDerived>& other) const { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,3) EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename OtherDerived::Scalar>::value), YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) + + typename internal::nested_eval<ExpressionType,2>::type mat(_expression()); + typename internal::nested_eval<OtherDerived,2>::type vec(other.derived()); CrossReturnType res(_expression().rows(),_expression().cols()); if(Direction==Vertical) { eigen_assert(CrossReturnType::RowsAtCompileTime==3 && "the matrix must have exactly 3 rows"); - res.row(0) = (_expression().row(1) * other.coeff(2) - _expression().row(2) * other.coeff(1)).conjugate(); - res.row(1) = (_expression().row(2) * other.coeff(0) - _expression().row(0) * other.coeff(2)).conjugate(); - res.row(2) = (_expression().row(0) * other.coeff(1) - _expression().row(1) * other.coeff(0)).conjugate(); + res.row(0) = (mat.row(1) * vec.coeff(2) - mat.row(2) * vec.coeff(1)).conjugate(); + res.row(1) = (mat.row(2) * vec.coeff(0) - mat.row(0) * vec.coeff(2)).conjugate(); + res.row(2) = (mat.row(0) * vec.coeff(1) - mat.row(1) * vec.coeff(0)).conjugate(); } else { eigen_assert(CrossReturnType::ColsAtCompileTime==3 && "the matrix must have exactly 3 columns"); - res.col(0) = (_expression().col(1) * other.coeff(2) - _expression().col(2) * other.coeff(1)).conjugate(); - res.col(1) = (_expression().col(2) * other.coeff(0) - _expression().col(0) * other.coeff(2)).conjugate(); - res.col(2) = (_expression().col(0) * other.coeff(1) - _expression().col(1) * other.coeff(0)).conjugate(); + res.col(0) = (mat.col(1) * vec.coeff(2) - mat.col(2) * vec.coeff(1)).conjugate(); + res.col(1) = (mat.col(2) * vec.coeff(0) - mat.col(0) * vec.coeff(2)).conjugate(); + res.col(2) = (mat.col(0) * vec.coeff(1) - mat.col(1) * vec.coeff(0)).conjugate(); } return res; } @@ -130,8 +142,8 @@ struct unitOrthogonal_selector typedef typename plain_matrix_type<Derived>::type VectorType; typedef typename traits<Derived>::Scalar Scalar; typedef typename NumTraits<Scalar>::Real RealScalar; - typedef typename Derived::Index Index; typedef Matrix<Scalar,2,1> Vector2; + EIGEN_DEVICE_FUNC static inline VectorType run(const Derived& src) { VectorType perp = VectorType::Zero(src.size()); @@ -154,6 +166,7 @@ struct unitOrthogonal_selector<Derived,3> typedef typename plain_matrix_type<Derived>::type VectorType; typedef typename traits<Derived>::Scalar Scalar; typedef typename NumTraits<Scalar>::Real RealScalar; + EIGEN_DEVICE_FUNC static inline VectorType run(const Derived& src) { VectorType perp; @@ -192,13 +205,16 @@ template<typename Derived> struct unitOrthogonal_selector<Derived,2> { typedef typename plain_matrix_type<Derived>::type VectorType; + EIGEN_DEVICE_FUNC static inline VectorType run(const Derived& src) { return VectorType(-numext::conj(src.y()), numext::conj(src.x())).normalized(); } }; } // end namespace internal -/** \returns a unit vector which is orthogonal to \c *this +/** \geometry_module \ingroup Geometry_Module + * + * \returns a unit vector which is orthogonal to \c *this * * The size of \c *this must be at least 2. If the size is exactly 2, * then the returned vector is a counter clock wise rotation of \c *this, i.e., (-y,x).normalized(). @@ -206,7 +222,7 @@ struct unitOrthogonal_selector<Derived,2> * \sa cross() */ template<typename Derived> -typename MatrixBase<Derived>::PlainObject +EIGEN_DEVICE_FUNC typename MatrixBase<Derived>::PlainObject MatrixBase<Derived>::unitOrthogonal() const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) diff --git a/eigen/Eigen/src/Geometry/ParametrizedLine.h b/eigen/Eigen/src/Geometry/ParametrizedLine.h index cf3252d..3929ca8 100644 --- a/eigen/Eigen/src/Geometry/ParametrizedLine.h +++ b/eigen/Eigen/src/Geometry/ParametrizedLine.h @@ -23,8 +23,8 @@ namespace Eigen { * direction vector \f$ \mathbf{d} \f$ such that the line corresponds to * the set \f$ l(t) = \mathbf{o} + t \mathbf{d} \f$, \f$ t \in \mathbf{R} \f$. * - * \param _Scalar the scalar type, i.e., the type of the coefficients - * \param _AmbientDim the dimension of the ambient space, can be a compile time value or Dynamic. + * \tparam _Scalar the scalar type, i.e., the type of the coefficients + * \tparam _AmbientDim the dimension of the ambient space, can be a compile time value or Dynamic. */ template <typename _Scalar, int _AmbientDim, int _Options> class ParametrizedLine @@ -37,49 +37,49 @@ public: }; typedef _Scalar Scalar; typedef typename NumTraits<Scalar>::Real RealScalar; - typedef DenseIndex Index; + typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 typedef Matrix<Scalar,AmbientDimAtCompileTime,1,Options> VectorType; /** Default constructor without initialization */ - inline ParametrizedLine() {} + EIGEN_DEVICE_FUNC inline ParametrizedLine() {} template<int OtherOptions> - ParametrizedLine(const ParametrizedLine<Scalar,AmbientDimAtCompileTime,OtherOptions>& other) + EIGEN_DEVICE_FUNC ParametrizedLine(const ParametrizedLine<Scalar,AmbientDimAtCompileTime,OtherOptions>& other) : m_origin(other.origin()), m_direction(other.direction()) {} /** Constructs a dynamic-size line with \a _dim the dimension * of the ambient space */ - inline explicit ParametrizedLine(Index _dim) : m_origin(_dim), m_direction(_dim) {} + EIGEN_DEVICE_FUNC inline explicit ParametrizedLine(Index _dim) : m_origin(_dim), m_direction(_dim) {} /** Initializes a parametrized line of direction \a direction and origin \a origin. * \warning the vector direction is assumed to be normalized. */ - ParametrizedLine(const VectorType& origin, const VectorType& direction) + EIGEN_DEVICE_FUNC ParametrizedLine(const VectorType& origin, const VectorType& direction) : m_origin(origin), m_direction(direction) {} template <int OtherOptions> - explicit ParametrizedLine(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane); + EIGEN_DEVICE_FUNC explicit ParametrizedLine(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane); /** Constructs a parametrized line going from \a p0 to \a p1. */ - static inline ParametrizedLine Through(const VectorType& p0, const VectorType& p1) + EIGEN_DEVICE_FUNC static inline ParametrizedLine Through(const VectorType& p0, const VectorType& p1) { return ParametrizedLine(p0, (p1-p0).normalized()); } - ~ParametrizedLine() {} + EIGEN_DEVICE_FUNC ~ParametrizedLine() {} /** \returns the dimension in which the line holds */ - inline Index dim() const { return m_direction.size(); } + EIGEN_DEVICE_FUNC inline Index dim() const { return m_direction.size(); } - const VectorType& origin() const { return m_origin; } - VectorType& origin() { return m_origin; } + EIGEN_DEVICE_FUNC const VectorType& origin() const { return m_origin; } + EIGEN_DEVICE_FUNC VectorType& origin() { return m_origin; } - const VectorType& direction() const { return m_direction; } - VectorType& direction() { return m_direction; } + EIGEN_DEVICE_FUNC const VectorType& direction() const { return m_direction; } + EIGEN_DEVICE_FUNC VectorType& direction() { return m_direction; } /** \returns the squared distance of a point \a p to its projection onto the line \c *this. * \sa distance() */ - RealScalar squaredDistance(const VectorType& p) const + EIGEN_DEVICE_FUNC RealScalar squaredDistance(const VectorType& p) const { VectorType diff = p - origin(); return (diff - direction().dot(diff) * direction()).squaredNorm(); @@ -87,30 +87,67 @@ 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 { using std::sqrt; return sqrt(squaredDistance(p)); } + EIGEN_DEVICE_FUNC RealScalar distance(const VectorType& p) const { EIGEN_USING_STD_MATH(sqrt) return sqrt(squaredDistance(p)); } /** \returns the projection of a point \a p onto the line \c *this. */ - VectorType projection(const VectorType& p) const + EIGEN_DEVICE_FUNC VectorType projection(const VectorType& p) const { return origin() + direction().dot(p-origin()) * direction(); } - VectorType pointAt(const Scalar& t) const; + EIGEN_DEVICE_FUNC VectorType pointAt(const Scalar& t) const; template <int OtherOptions> - Scalar intersectionParameter(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane) const; + EIGEN_DEVICE_FUNC Scalar intersectionParameter(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane) const; template <int OtherOptions> - Scalar intersection(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane) const; + EIGEN_DEVICE_FUNC Scalar intersection(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane) const; template <int OtherOptions> - VectorType intersectionPoint(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane) const; + EIGEN_DEVICE_FUNC VectorType intersectionPoint(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane) const; - /** \returns \c *this with scalar type casted to \a NewScalarType + /** Applies the transformation matrix \a mat to \c *this and returns a reference to \c *this. + * + * \param mat the Dim x Dim transformation matrix + * \param traits specifies whether the matrix \a mat represents an #Isometry + * or a more generic #Affine transformation. The default is #Affine. + */ + template<typename XprType> + EIGEN_DEVICE_FUNC inline ParametrizedLine& transform(const MatrixBase<XprType>& mat, TransformTraits traits = Affine) + { + if (traits==Affine) + direction() = (mat * direction()).normalized(); + else if (traits==Isometry) + direction() = mat * direction(); + else + { + eigen_assert(0 && "invalid traits value in ParametrizedLine::transform()"); + } + origin() = mat * origin(); + return *this; + } + + /** Applies the transformation \a t to \c *this and returns a reference to \c *this. + * + * \param t the transformation of dimension Dim + * \param traits specifies whether the transformation \a t represents an #Isometry + * or a more generic #Affine transformation. The default is #Affine. + * Other kind of transformations are not supported. + */ + template<int TrOptions> + EIGEN_DEVICE_FUNC inline ParametrizedLine& transform(const Transform<Scalar,AmbientDimAtCompileTime,Affine,TrOptions>& t, + TransformTraits traits = Affine) + { + transform(t.linear(), traits); + origin() += t.translation(); + return *this; + } + +/** \returns \c *this with scalar type casted to \a NewScalarType * * Note that if \a NewScalarType is equal to the current scalar type of \c *this * then this function smartly returns a const reference to \c *this. */ template<typename NewScalarType> - inline typename internal::cast_return_type<ParametrizedLine, + EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<ParametrizedLine, ParametrizedLine<NewScalarType,AmbientDimAtCompileTime,Options> >::type cast() const { return typename internal::cast_return_type<ParametrizedLine, @@ -119,7 +156,7 @@ public: /** Copy constructor with scalar type conversion */ template<typename OtherScalarType,int OtherOptions> - inline explicit ParametrizedLine(const ParametrizedLine<OtherScalarType,AmbientDimAtCompileTime,OtherOptions>& other) + EIGEN_DEVICE_FUNC inline explicit ParametrizedLine(const ParametrizedLine<OtherScalarType,AmbientDimAtCompileTime,OtherOptions>& other) { m_origin = other.origin().template cast<Scalar>(); m_direction = other.direction().template cast<Scalar>(); @@ -129,7 +166,7 @@ public: * determined by \a prec. * * \sa MatrixBase::isApprox() */ - bool isApprox(const ParametrizedLine& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const + EIGEN_DEVICE_FUNC bool isApprox(const ParametrizedLine& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const { return m_origin.isApprox(other.m_origin, prec) && m_direction.isApprox(other.m_direction, prec); } protected: @@ -143,7 +180,7 @@ protected: */ template <typename _Scalar, int _AmbientDim, int _Options> template <int OtherOptions> -inline ParametrizedLine<_Scalar, _AmbientDim,_Options>::ParametrizedLine(const Hyperplane<_Scalar, _AmbientDim,OtherOptions>& hyperplane) +EIGEN_DEVICE_FUNC inline ParametrizedLine<_Scalar, _AmbientDim,_Options>::ParametrizedLine(const Hyperplane<_Scalar, _AmbientDim,OtherOptions>& hyperplane) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 2) direction() = hyperplane.normal().unitOrthogonal(); @@ -153,7 +190,7 @@ inline ParametrizedLine<_Scalar, _AmbientDim,_Options>::ParametrizedLine(const H /** \returns the point at \a t along this line */ template <typename _Scalar, int _AmbientDim, int _Options> -inline typename ParametrizedLine<_Scalar, _AmbientDim,_Options>::VectorType +EIGEN_DEVICE_FUNC inline typename ParametrizedLine<_Scalar, _AmbientDim,_Options>::VectorType ParametrizedLine<_Scalar, _AmbientDim,_Options>::pointAt(const _Scalar& t) const { return origin() + (direction()*t); @@ -163,7 +200,7 @@ ParametrizedLine<_Scalar, _AmbientDim,_Options>::pointAt(const _Scalar& t) const */ template <typename _Scalar, int _AmbientDim, int _Options> template <int OtherOptions> -inline _Scalar ParametrizedLine<_Scalar, _AmbientDim,_Options>::intersectionParameter(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane) const +EIGEN_DEVICE_FUNC inline _Scalar ParametrizedLine<_Scalar, _AmbientDim,_Options>::intersectionParameter(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane) const { return -(hyperplane.offset()+hyperplane.normal().dot(origin())) / hyperplane.normal().dot(direction()); @@ -175,7 +212,7 @@ inline _Scalar ParametrizedLine<_Scalar, _AmbientDim,_Options>::intersectionPara */ template <typename _Scalar, int _AmbientDim, int _Options> template <int OtherOptions> -inline _Scalar ParametrizedLine<_Scalar, _AmbientDim,_Options>::intersection(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane) const +EIGEN_DEVICE_FUNC inline _Scalar ParametrizedLine<_Scalar, _AmbientDim,_Options>::intersection(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane) const { return intersectionParameter(hyperplane); } @@ -184,7 +221,7 @@ inline _Scalar ParametrizedLine<_Scalar, _AmbientDim,_Options>::intersection(con */ template <typename _Scalar, int _AmbientDim, int _Options> template <int OtherOptions> -inline typename ParametrizedLine<_Scalar, _AmbientDim,_Options>::VectorType +EIGEN_DEVICE_FUNC inline typename ParametrizedLine<_Scalar, _AmbientDim,_Options>::VectorType ParametrizedLine<_Scalar, _AmbientDim,_Options>::intersectionPoint(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane) const { return pointAt(intersectionParameter(hyperplane)); diff --git a/eigen/Eigen/src/Geometry/Quaternion.h b/eigen/Eigen/src/Geometry/Quaternion.h index e89ba80..f6ef1bc 100644 --- a/eigen/Eigen/src/Geometry/Quaternion.h +++ b/eigen/Eigen/src/Geometry/Quaternion.h @@ -34,8 +34,9 @@ struct quaternionbase_assign_impl; template<class Derived> class QuaternionBase : public RotationBase<Derived, 3> { + public: typedef RotationBase<Derived, 3> Base; -public: + using Base::operator*; using Base::derived; @@ -57,37 +58,37 @@ public: /** \returns the \c x coefficient */ - inline Scalar x() const { return this->derived().coeffs().coeff(0); } + EIGEN_DEVICE_FUNC inline Scalar x() const { return this->derived().coeffs().coeff(0); } /** \returns the \c y coefficient */ - inline Scalar y() const { return this->derived().coeffs().coeff(1); } + EIGEN_DEVICE_FUNC inline Scalar y() const { return this->derived().coeffs().coeff(1); } /** \returns the \c z coefficient */ - inline Scalar z() const { return this->derived().coeffs().coeff(2); } + EIGEN_DEVICE_FUNC inline Scalar z() const { return this->derived().coeffs().coeff(2); } /** \returns the \c w coefficient */ - inline Scalar w() const { return this->derived().coeffs().coeff(3); } + EIGEN_DEVICE_FUNC inline Scalar w() const { return this->derived().coeffs().coeff(3); } /** \returns a reference to the \c x coefficient */ - inline Scalar& x() { return this->derived().coeffs().coeffRef(0); } + EIGEN_DEVICE_FUNC inline Scalar& x() { return this->derived().coeffs().coeffRef(0); } /** \returns a reference to the \c y coefficient */ - inline Scalar& y() { return this->derived().coeffs().coeffRef(1); } + EIGEN_DEVICE_FUNC inline Scalar& y() { return this->derived().coeffs().coeffRef(1); } /** \returns a reference to the \c z coefficient */ - inline Scalar& z() { return this->derived().coeffs().coeffRef(2); } + EIGEN_DEVICE_FUNC inline Scalar& z() { return this->derived().coeffs().coeffRef(2); } /** \returns a reference to the \c w coefficient */ - inline Scalar& w() { return this->derived().coeffs().coeffRef(3); } + EIGEN_DEVICE_FUNC inline Scalar& w() { return this->derived().coeffs().coeffRef(3); } /** \returns a read-only vector expression of the imaginary part (x,y,z) */ - inline const VectorBlock<const Coefficients,3> vec() const { return coeffs().template head<3>(); } + EIGEN_DEVICE_FUNC inline const VectorBlock<const Coefficients,3> vec() const { return coeffs().template head<3>(); } /** \returns a vector expression of the imaginary part (x,y,z) */ - inline VectorBlock<Coefficients,3> vec() { return coeffs().template head<3>(); } + EIGEN_DEVICE_FUNC inline VectorBlock<Coefficients,3> vec() { return coeffs().template head<3>(); } /** \returns a read-only vector expression of the coefficients (x,y,z,w) */ - inline const typename internal::traits<Derived>::Coefficients& coeffs() const { return derived().coeffs(); } + EIGEN_DEVICE_FUNC inline const typename internal::traits<Derived>::Coefficients& coeffs() const { return derived().coeffs(); } /** \returns a vector expression of the coefficients (x,y,z,w) */ - inline typename internal::traits<Derived>::Coefficients& coeffs() { return derived().coeffs(); } + EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Coefficients& coeffs() { return derived().coeffs(); } - EIGEN_STRONG_INLINE QuaternionBase<Derived>& operator=(const QuaternionBase<Derived>& other); - template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator=(const QuaternionBase<OtherDerived>& other); + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE QuaternionBase<Derived>& operator=(const QuaternionBase<Derived>& other); + template<class OtherDerived> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const QuaternionBase<OtherDerived>& other); // disabled this copy operator as it is giving very strange compilation errors when compiling // test_stdvector with GCC 4.4.2. This looks like a GCC bug though, so feel free to re-enable it if it's @@ -96,72 +97,72 @@ public: // Derived& operator=(const QuaternionBase& other) // { return operator=<Derived>(other); } - Derived& operator=(const AngleAxisType& aa); - template<class OtherDerived> Derived& operator=(const MatrixBase<OtherDerived>& m); + EIGEN_DEVICE_FUNC Derived& operator=(const AngleAxisType& aa); + template<class OtherDerived> EIGEN_DEVICE_FUNC Derived& operator=(const MatrixBase<OtherDerived>& m); /** \returns a quaternion representing an identity rotation * \sa MatrixBase::Identity() */ - static inline Quaternion<Scalar> Identity() { return Quaternion<Scalar>(Scalar(1), Scalar(0), Scalar(0), Scalar(0)); } + EIGEN_DEVICE_FUNC static inline Quaternion<Scalar> Identity() { return Quaternion<Scalar>(Scalar(1), Scalar(0), Scalar(0), Scalar(0)); } /** \sa QuaternionBase::Identity(), MatrixBase::setIdentity() */ - inline QuaternionBase& setIdentity() { coeffs() << Scalar(0), Scalar(0), Scalar(0), Scalar(1); return *this; } + EIGEN_DEVICE_FUNC inline QuaternionBase& setIdentity() { coeffs() << Scalar(0), Scalar(0), Scalar(0), Scalar(1); return *this; } /** \returns the squared norm of the quaternion's coefficients * \sa QuaternionBase::norm(), MatrixBase::squaredNorm() */ - inline Scalar squaredNorm() const { return coeffs().squaredNorm(); } + EIGEN_DEVICE_FUNC inline Scalar squaredNorm() const { return coeffs().squaredNorm(); } /** \returns the norm of the quaternion's coefficients * \sa QuaternionBase::squaredNorm(), MatrixBase::norm() */ - inline Scalar norm() const { return coeffs().norm(); } + EIGEN_DEVICE_FUNC inline Scalar norm() const { return coeffs().norm(); } /** Normalizes the quaternion \c *this * \sa normalized(), MatrixBase::normalize() */ - inline void normalize() { coeffs().normalize(); } + EIGEN_DEVICE_FUNC inline void normalize() { coeffs().normalize(); } /** \returns a normalized copy of \c *this * \sa normalize(), MatrixBase::normalized() */ - inline Quaternion<Scalar> normalized() const { return Quaternion<Scalar>(coeffs().normalized()); } + EIGEN_DEVICE_FUNC inline Quaternion<Scalar> normalized() const { return Quaternion<Scalar>(coeffs().normalized()); } /** \returns the dot product of \c *this and \a other * Geometrically speaking, the dot product of two unit quaternions * corresponds to the cosine of half the angle between the two rotations. * \sa angularDistance() */ - template<class OtherDerived> inline Scalar dot(const QuaternionBase<OtherDerived>& other) const { return coeffs().dot(other.coeffs()); } + template<class OtherDerived> EIGEN_DEVICE_FUNC inline Scalar dot(const QuaternionBase<OtherDerived>& other) const { return coeffs().dot(other.coeffs()); } - template<class OtherDerived> Scalar angularDistance(const QuaternionBase<OtherDerived>& other) const; + template<class OtherDerived> EIGEN_DEVICE_FUNC Scalar angularDistance(const QuaternionBase<OtherDerived>& other) const; /** \returns an equivalent 3x3 rotation matrix */ - Matrix3 toRotationMatrix() const; + EIGEN_DEVICE_FUNC Matrix3 toRotationMatrix() const; /** \returns the quaternion which transform \a a into \a b through a rotation */ template<typename Derived1, typename Derived2> - Derived& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b); + EIGEN_DEVICE_FUNC Derived& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b); - template<class OtherDerived> EIGEN_STRONG_INLINE Quaternion<Scalar> operator* (const QuaternionBase<OtherDerived>& q) const; - template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator*= (const QuaternionBase<OtherDerived>& q); + template<class OtherDerived> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Quaternion<Scalar> operator* (const QuaternionBase<OtherDerived>& q) const; + template<class OtherDerived> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator*= (const QuaternionBase<OtherDerived>& q); /** \returns the quaternion describing the inverse rotation */ - Quaternion<Scalar> inverse() const; + EIGEN_DEVICE_FUNC Quaternion<Scalar> inverse() const; /** \returns the conjugated quaternion */ - Quaternion<Scalar> conjugate() const; + EIGEN_DEVICE_FUNC Quaternion<Scalar> conjugate() const; - template<class OtherDerived> Quaternion<Scalar> slerp(const Scalar& t, const QuaternionBase<OtherDerived>& other) const; + template<class OtherDerived> EIGEN_DEVICE_FUNC 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, const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const + EIGEN_DEVICE_FUNC 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*/ - EIGEN_STRONG_INLINE Vector3 _transformVector(const Vector3& v) const; + /** return the result vector of \a v through the rotation*/ + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Vector3 _transformVector(const Vector3& v) const; /** \returns \c *this with scalar type casted to \a NewScalarType * @@ -169,7 +170,7 @@ public: * then this function smartly returns a const reference to \c *this. */ template<typename NewScalarType> - inline typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type cast() const + EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type cast() const { return typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type(derived()); } @@ -216,8 +217,8 @@ struct traits<Quaternion<_Scalar,_Options> > typedef _Scalar Scalar; typedef Matrix<_Scalar,4,1,_Options> Coefficients; enum{ - IsAligned = internal::traits<Coefficients>::Flags & AlignedBit, - Flags = IsAligned ? (AlignedBit | LvalueBit) : LvalueBit + Alignment = internal::traits<Coefficients>::Alignment, + Flags = LvalueBit }; }; } @@ -225,10 +226,10 @@ struct traits<Quaternion<_Scalar,_Options> > template<typename _Scalar, int _Options> class Quaternion : public QuaternionBase<Quaternion<_Scalar,_Options> > { +public: typedef QuaternionBase<Quaternion<_Scalar,_Options> > Base; - enum { IsAligned = internal::traits<Quaternion>::IsAligned }; + enum { NeedsAlignment = internal::traits<Quaternion>::Alignment>0 }; -public: typedef _Scalar Scalar; EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Quaternion) @@ -238,7 +239,7 @@ public: typedef typename Base::AngleAxisType AngleAxisType; /** Default constructor leaving the quaternion uninitialized. */ - inline Quaternion() {} + EIGEN_DEVICE_FUNC inline Quaternion() {} /** Constructs and initializes the quaternion \f$ w+xi+yj+zk \f$ from * its four coefficients \a w, \a x, \a y and \a z. @@ -247,36 +248,42 @@ public: * while internally the coefficients are stored in the following order: * [\c x, \c y, \c z, \c w] */ - inline Quaternion(const Scalar& w, const Scalar& x, const Scalar& y, const Scalar& z) : m_coeffs(x, y, z, w){} + EIGEN_DEVICE_FUNC 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) {} + EIGEN_DEVICE_FUNC explicit inline Quaternion(const Scalar* data) : m_coeffs(data) {} /** Copy constructor */ - template<class Derived> EIGEN_STRONG_INLINE Quaternion(const QuaternionBase<Derived>& other) { this->Base::operator=(other); } + template<class Derived> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Quaternion(const QuaternionBase<Derived>& other) { this->Base::operator=(other); } /** Constructs and initializes a quaternion from the angle-axis \a aa */ - explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; } + EIGEN_DEVICE_FUNC explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; } /** Constructs and initializes a quaternion from either: * - a rotation matrix expression, * - a 4D vector expression representing quaternion coefficients. */ template<typename Derived> - explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; } + EIGEN_DEVICE_FUNC explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; } /** Explicit copy constructor with scalar conversion */ template<typename OtherScalar, int OtherOptions> - explicit inline Quaternion(const Quaternion<OtherScalar, OtherOptions>& other) + EIGEN_DEVICE_FUNC explicit inline Quaternion(const Quaternion<OtherScalar, OtherOptions>& other) { m_coeffs = other.coeffs().template cast<Scalar>(); } + EIGEN_DEVICE_FUNC static Quaternion UnitRandom(); + template<typename Derived1, typename Derived2> - static Quaternion FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b); + EIGEN_DEVICE_FUNC static Quaternion FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b); - inline Coefficients& coeffs() { return m_coeffs;} - inline const Coefficients& coeffs() const { return m_coeffs;} + EIGEN_DEVICE_FUNC inline Coefficients& coeffs() { return m_coeffs;} + EIGEN_DEVICE_FUNC inline const Coefficients& coeffs() const { return m_coeffs;} - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(bool(IsAligned)) + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(bool(NeedsAlignment)) + +#ifdef EIGEN_QUATERNION_PLUGIN +# include EIGEN_QUATERNION_PLUGIN +#endif protected: Coefficients m_coeffs; @@ -336,9 +343,9 @@ template<typename _Scalar, int _Options> class Map<const Quaternion<_Scalar>, _Options > : public QuaternionBase<Map<const Quaternion<_Scalar>, _Options> > { + public: typedef QuaternionBase<Map<const Quaternion<_Scalar>, _Options> > Base; - public: typedef _Scalar Scalar; typedef typename internal::traits<Map>::Coefficients Coefficients; EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map) @@ -350,9 +357,9 @@ class Map<const Quaternion<_Scalar>, _Options > * \code *coeffs == {x, y, z, w} \endcode * * If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */ - EIGEN_STRONG_INLINE Map(const Scalar* coeffs) : m_coeffs(coeffs) {} + EIGEN_DEVICE_FUNC explicit EIGEN_STRONG_INLINE Map(const Scalar* coeffs) : m_coeffs(coeffs) {} - inline const Coefficients& coeffs() const { return m_coeffs;} + EIGEN_DEVICE_FUNC inline const Coefficients& coeffs() const { return m_coeffs;} protected: const Coefficients m_coeffs; @@ -373,9 +380,9 @@ template<typename _Scalar, int _Options> class Map<Quaternion<_Scalar>, _Options > : public QuaternionBase<Map<Quaternion<_Scalar>, _Options> > { + public: typedef QuaternionBase<Map<Quaternion<_Scalar>, _Options> > Base; - public: typedef _Scalar Scalar; typedef typename internal::traits<Map>::Coefficients Coefficients; EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map) @@ -387,10 +394,10 @@ class Map<Quaternion<_Scalar>, _Options > * \code *coeffs == {x, y, z, w} \endcode * * If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */ - EIGEN_STRONG_INLINE Map(Scalar* coeffs) : m_coeffs(coeffs) {} + EIGEN_DEVICE_FUNC explicit EIGEN_STRONG_INLINE Map(Scalar* coeffs) : m_coeffs(coeffs) {} - inline Coefficients& coeffs() { return m_coeffs; } - inline const Coefficients& coeffs() const { return m_coeffs; } + EIGEN_DEVICE_FUNC inline Coefficients& coeffs() { return m_coeffs; } + EIGEN_DEVICE_FUNC inline const Coefficients& coeffs() const { return m_coeffs; } protected: Coefficients m_coeffs; @@ -418,7 +425,7 @@ typedef Map<Quaternion<double>, Aligned> QuaternionMapAlignedd; namespace internal { template<int Arch, class Derived1, class Derived2, typename Scalar, int _Options> struct quat_product { - static EIGEN_STRONG_INLINE Quaternion<Scalar> run(const QuaternionBase<Derived1>& a, const QuaternionBase<Derived2>& b){ + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Quaternion<Scalar> run(const QuaternionBase<Derived1>& a, const QuaternionBase<Derived2>& b){ return Quaternion<Scalar> ( a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(), @@ -433,20 +440,20 @@ template<int Arch, class Derived1, class Derived2, typename Scalar, int _Options /** \returns the concatenation of two rotations as a quaternion-quaternion product */ template <class Derived> template <class OtherDerived> -EIGEN_STRONG_INLINE Quaternion<typename internal::traits<Derived>::Scalar> +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Quaternion<typename internal::traits<Derived>::Scalar> QuaternionBase<Derived>::operator* (const QuaternionBase<OtherDerived>& other) const { EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename OtherDerived::Scalar>::value), YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) return internal::quat_product<Architecture::Target, Derived, OtherDerived, typename internal::traits<Derived>::Scalar, - internal::traits<Derived>::IsAligned && internal::traits<OtherDerived>::IsAligned>::run(*this, other); + EIGEN_PLAIN_ENUM_MIN(internal::traits<Derived>::Alignment, internal::traits<OtherDerived>::Alignment)>::run(*this, other); } /** \sa operator*(Quaternion) */ template <class Derived> template <class OtherDerived> -EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator*= (const QuaternionBase<OtherDerived>& other) +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator*= (const QuaternionBase<OtherDerived>& other) { derived() = derived() * other.derived(); return derived(); @@ -460,7 +467,7 @@ EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator*= (const Quaterni * - Via a Matrix3: 24 + 15n */ template <class Derived> -EIGEN_STRONG_INLINE typename QuaternionBase<Derived>::Vector3 +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename QuaternionBase<Derived>::Vector3 QuaternionBase<Derived>::_transformVector(const Vector3& v) const { // Note that this algorithm comes from the optimization by hand @@ -474,7 +481,7 @@ QuaternionBase<Derived>::_transformVector(const Vector3& v) const } template<class Derived> -EIGEN_STRONG_INLINE QuaternionBase<Derived>& QuaternionBase<Derived>::operator=(const QuaternionBase<Derived>& other) +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE QuaternionBase<Derived>& QuaternionBase<Derived>::operator=(const QuaternionBase<Derived>& other) { coeffs() = other.coeffs(); return derived(); @@ -482,7 +489,7 @@ EIGEN_STRONG_INLINE QuaternionBase<Derived>& QuaternionBase<Derived>::operator=( template<class Derived> template<class OtherDerived> -EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const QuaternionBase<OtherDerived>& other) +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const QuaternionBase<OtherDerived>& other) { coeffs() = other.coeffs(); return derived(); @@ -491,10 +498,10 @@ EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const Quaternion /** Set \c *this from an angle-axis \a aa and returns a reference to \c *this */ template<class Derived> -EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const AngleAxisType& aa) +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const AngleAxisType& aa) { - using std::cos; - using std::sin; + EIGEN_USING_STD_MATH(cos) + EIGEN_USING_STD_MATH(sin) Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings this->w() = cos(ha); this->vec() = sin(ha) * aa.axis(); @@ -509,7 +516,7 @@ EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const AngleAxisT template<class Derived> template<class MatrixDerived> -inline Derived& QuaternionBase<Derived>::operator=(const MatrixBase<MatrixDerived>& xpr) +EIGEN_DEVICE_FUNC inline Derived& QuaternionBase<Derived>::operator=(const MatrixBase<MatrixDerived>& xpr) { EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename MatrixDerived::Scalar>::value), YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) @@ -521,7 +528,7 @@ inline Derived& QuaternionBase<Derived>::operator=(const MatrixBase<MatrixDerive * be normalized, otherwise the result is undefined. */ template<class Derived> -inline typename QuaternionBase<Derived>::Matrix3 +EIGEN_DEVICE_FUNC inline typename QuaternionBase<Derived>::Matrix3 QuaternionBase<Derived>::toRotationMatrix(void) const { // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!) @@ -568,10 +575,9 @@ QuaternionBase<Derived>::toRotationMatrix(void) const */ template<class Derived> template<typename Derived1, typename Derived2> -inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b) +EIGEN_DEVICE_FUNC inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b) { - using std::max; - using std::sqrt; + EIGEN_USING_STD_MATH(sqrt) Vector3 v0 = a.normalized(); Vector3 v1 = b.normalized(); Scalar c = v1.dot(v0); @@ -586,7 +592,7 @@ inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Deri // which yields a singular value problem if (c < Scalar(-1)+NumTraits<Scalar>::dummy_precision()) { - c = (max)(c,Scalar(-1)); + c = numext::maxi(c,Scalar(-1)); Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose(); JacobiSVD<Matrix<Scalar,2,3> > svd(m, ComputeFullV); Vector3 axis = svd.matrixV().col(2); @@ -605,6 +611,24 @@ inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Deri return derived(); } +/** \returns a random unit quaternion following a uniform distribution law on SO(3) + * + * \note The implementation is based on http://planning.cs.uiuc.edu/node198.html + */ +template<typename Scalar, int Options> +EIGEN_DEVICE_FUNC Quaternion<Scalar,Options> Quaternion<Scalar,Options>::UnitRandom() +{ + EIGEN_USING_STD_MATH(sqrt) + EIGEN_USING_STD_MATH(sin) + EIGEN_USING_STD_MATH(cos) + const Scalar u1 = internal::random<Scalar>(0, 1), + u2 = internal::random<Scalar>(0, 2*EIGEN_PI), + u3 = internal::random<Scalar>(0, 2*EIGEN_PI); + const Scalar a = sqrt(1 - u1), + b = sqrt(u1); + return Quaternion (a * sin(u2), a * cos(u2), b * sin(u3), b * cos(u3)); +} + /** Returns a quaternion representing a rotation between * the two arbitrary vectors \a a and \a b. In other words, the built @@ -618,7 +642,7 @@ inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Deri */ template<typename Scalar, int Options> template<typename Derived1, typename Derived2> -Quaternion<Scalar,Options> Quaternion<Scalar,Options>::FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b) +EIGEN_DEVICE_FUNC Quaternion<Scalar,Options> Quaternion<Scalar,Options>::FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b) { Quaternion quat; quat.setFromTwoVectors(a, b); @@ -633,7 +657,7 @@ Quaternion<Scalar,Options> Quaternion<Scalar,Options>::FromTwoVectors(const Matr * \sa QuaternionBase::conjugate() */ template <class Derived> -inline Quaternion<typename internal::traits<Derived>::Scalar> QuaternionBase<Derived>::inverse() const +EIGEN_DEVICE_FUNC inline Quaternion<typename internal::traits<Derived>::Scalar> QuaternionBase<Derived>::inverse() const { // FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite() ?? Scalar n2 = this->squaredNorm(); @@ -646,6 +670,16 @@ inline Quaternion<typename internal::traits<Derived>::Scalar> QuaternionBase<Der } } +// Generic conjugate of a Quaternion +namespace internal { +template<int Arch, class Derived, typename Scalar, int _Options> struct quat_conj +{ + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Quaternion<Scalar> run(const QuaternionBase<Derived>& q){ + return Quaternion<Scalar>(q.w(),-q.x(),-q.y(),-q.z()); + } +}; +} + /** \returns the conjugate of the \c *this which is equal to the multiplicative inverse * if the quaternion is normalized. * The conjugate of a quaternion represents the opposite rotation. @@ -653,10 +687,13 @@ inline Quaternion<typename internal::traits<Derived>::Scalar> QuaternionBase<Der * \sa Quaternion2::inverse() */ template <class Derived> -inline Quaternion<typename internal::traits<Derived>::Scalar> +EIGEN_DEVICE_FUNC inline Quaternion<typename internal::traits<Derived>::Scalar> QuaternionBase<Derived>::conjugate() const { - return Quaternion<Scalar>(this->w(),-this->x(),-this->y(),-this->z()); + return internal::quat_conj<Architecture::Target, Derived, + typename internal::traits<Derived>::Scalar, + internal::traits<Derived>::Alignment>::run(*this); + } /** \returns the angle (in radian) between two rotations @@ -664,13 +701,12 @@ QuaternionBase<Derived>::conjugate() const */ template <class Derived> template <class OtherDerived> -inline typename internal::traits<Derived>::Scalar +EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Scalar QuaternionBase<Derived>::angularDistance(const QuaternionBase<OtherDerived>& other) const { - using std::atan2; - using std::abs; + EIGEN_USING_STD_MATH(atan2) Quaternion<Scalar> d = (*this) * other.conjugate(); - return Scalar(2) * atan2( d.vec().norm(), abs(d.w()) ); + return Scalar(2) * atan2( d.vec().norm(), numext::abs(d.w()) ); } @@ -683,15 +719,14 @@ QuaternionBase<Derived>::angularDistance(const QuaternionBase<OtherDerived>& oth */ template <class Derived> template <class OtherDerived> -Quaternion<typename internal::traits<Derived>::Scalar> +EIGEN_DEVICE_FUNC Quaternion<typename internal::traits<Derived>::Scalar> 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(); + EIGEN_USING_STD_MATH(acos) + EIGEN_USING_STD_MATH(sin) + const Scalar one = Scalar(1) - NumTraits<Scalar>::epsilon(); Scalar d = this->dot(other); - Scalar absD = abs(d); + Scalar absD = numext::abs(d); Scalar scale0; Scalar scale1; @@ -722,10 +757,10 @@ template<typename Other> struct quaternionbase_assign_impl<Other,3,3> { typedef typename Other::Scalar Scalar; - typedef DenseIndex Index; - template<class Derived> static inline void run(QuaternionBase<Derived>& q, const Other& mat) + template<class Derived> EIGEN_DEVICE_FUNC static inline void run(QuaternionBase<Derived>& q, const Other& a_mat) { - using std::sqrt; + const typename internal::nested_eval<Other,2>::type mat(a_mat); + EIGEN_USING_STD_MATH(sqrt) // This algorithm comes from "Quaternion Calculus and Fast Animation", // Ken Shoemake, 1987 SIGGRAPH course notes Scalar t = mat.trace(); @@ -740,13 +775,13 @@ struct quaternionbase_assign_impl<Other,3,3> } else { - DenseIndex i = 0; + Index i = 0; if (mat.coeff(1,1) > mat.coeff(0,0)) i = 1; if (mat.coeff(2,2) > mat.coeff(i,i)) i = 2; - DenseIndex j = (i+1)%3; - DenseIndex k = (j+1)%3; + Index j = (i+1)%3; + Index k = (j+1)%3; t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0)); q.coeffs().coeffRef(i) = Scalar(0.5) * t; @@ -763,7 +798,7 @@ template<typename Other> struct quaternionbase_assign_impl<Other,4,1> { typedef typename Other::Scalar Scalar; - template<class Derived> static inline void run(QuaternionBase<Derived>& q, const Other& vec) + template<class Derived> EIGEN_DEVICE_FUNC static inline void run(QuaternionBase<Derived>& q, const Other& vec) { q.coeffs() = vec; } diff --git a/eigen/Eigen/src/Geometry/Rotation2D.h b/eigen/Eigen/src/Geometry/Rotation2D.h index a2d59fc..884b7d0 100644 --- a/eigen/Eigen/src/Geometry/Rotation2D.h +++ b/eigen/Eigen/src/Geometry/Rotation2D.h @@ -18,7 +18,7 @@ namespace Eigen { * * \brief Represents a rotation/orientation in a 2 dimensional space. * - * \param _Scalar the scalar type, i.e., the type of the coefficients + * \tparam _Scalar the scalar type, i.e., the type of the coefficients * * This class is equivalent to a single scalar representing a counter clock wise rotation * as a single angle in radian. It provides some additional features such as the automatic @@ -59,41 +59,79 @@ protected: public: /** Construct a 2D counter clock wise rotation from the angle \a a in radian. */ - inline Rotation2D(const Scalar& a) : m_angle(a) {} + EIGEN_DEVICE_FUNC explicit inline Rotation2D(const Scalar& a) : m_angle(a) {} /** Default constructor wihtout initialization. The represented rotation is undefined. */ - Rotation2D() {} + EIGEN_DEVICE_FUNC Rotation2D() {} + + /** Construct a 2D rotation from a 2x2 rotation matrix \a mat. + * + * \sa fromRotationMatrix() + */ + template<typename Derived> + EIGEN_DEVICE_FUNC explicit Rotation2D(const MatrixBase<Derived>& m) + { + fromRotationMatrix(m.derived()); + } /** \returns the rotation angle */ - inline Scalar angle() const { return m_angle; } + EIGEN_DEVICE_FUNC inline Scalar angle() const { return m_angle; } /** \returns a read-write reference to the rotation angle */ - inline Scalar& angle() { return m_angle; } + EIGEN_DEVICE_FUNC inline Scalar& angle() { return m_angle; } + + /** \returns the rotation angle in [0,2pi] */ + EIGEN_DEVICE_FUNC inline Scalar smallestPositiveAngle() const { + Scalar tmp = numext::fmod(m_angle,Scalar(2*EIGEN_PI)); + return tmp<Scalar(0) ? tmp + Scalar(2*EIGEN_PI) : tmp; + } + + /** \returns the rotation angle in [-pi,pi] */ + EIGEN_DEVICE_FUNC inline Scalar smallestAngle() const { + Scalar tmp = numext::fmod(m_angle,Scalar(2*EIGEN_PI)); + if(tmp>Scalar(EIGEN_PI)) tmp -= Scalar(2*EIGEN_PI); + else if(tmp<-Scalar(EIGEN_PI)) tmp += Scalar(2*EIGEN_PI); + return tmp; + } /** \returns the inverse rotation */ - inline Rotation2D inverse() const { return -m_angle; } + EIGEN_DEVICE_FUNC inline Rotation2D inverse() const { return Rotation2D(-m_angle); } /** Concatenates two rotations */ - inline Rotation2D operator*(const Rotation2D& other) const - { return m_angle + other.m_angle; } + EIGEN_DEVICE_FUNC inline Rotation2D operator*(const Rotation2D& other) const + { return Rotation2D(m_angle + other.m_angle); } /** Concatenates two rotations */ - inline Rotation2D& operator*=(const Rotation2D& other) + EIGEN_DEVICE_FUNC inline Rotation2D& operator*=(const Rotation2D& other) { m_angle += other.m_angle; return *this; } /** Applies the rotation to a 2D vector */ - Vector2 operator* (const Vector2& vec) const + EIGEN_DEVICE_FUNC Vector2 operator* (const Vector2& vec) const { return toRotationMatrix() * vec; } template<typename Derived> - Rotation2D& fromRotationMatrix(const MatrixBase<Derived>& m); - Matrix2 toRotationMatrix() const; + EIGEN_DEVICE_FUNC Rotation2D& fromRotationMatrix(const MatrixBase<Derived>& m); + EIGEN_DEVICE_FUNC Matrix2 toRotationMatrix() const; + + /** Set \c *this from a 2x2 rotation matrix \a mat. + * In other words, this function extract the rotation angle from the rotation matrix. + * + * This method is an alias for fromRotationMatrix() + * + * \sa fromRotationMatrix() + */ + template<typename Derived> + EIGEN_DEVICE_FUNC Rotation2D& operator=(const MatrixBase<Derived>& m) + { return fromRotationMatrix(m.derived()); } /** \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(const Scalar& t, const Rotation2D& other) const - { return m_angle * (1-t) + other.angle() * t; } + EIGEN_DEVICE_FUNC inline Rotation2D slerp(const Scalar& t, const Rotation2D& other) const + { + 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 * @@ -101,24 +139,25 @@ public: * then this function smartly returns a const reference to \c *this. */ template<typename NewScalarType> - inline typename internal::cast_return_type<Rotation2D,Rotation2D<NewScalarType> >::type cast() const + EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<Rotation2D,Rotation2D<NewScalarType> >::type cast() const { return typename internal::cast_return_type<Rotation2D,Rotation2D<NewScalarType> >::type(*this); } /** Copy constructor with scalar type conversion */ template<typename OtherScalarType> - inline explicit Rotation2D(const Rotation2D<OtherScalarType>& other) + EIGEN_DEVICE_FUNC inline explicit Rotation2D(const Rotation2D<OtherScalarType>& other) { m_angle = Scalar(other.angle()); } - static inline Rotation2D Identity() { return Rotation2D(0); } + EIGEN_DEVICE_FUNC static inline Rotation2D Identity() { return Rotation2D(0); } /** \returns \c true if \c *this is approximately equal to \a other, within the precision * determined by \a prec. * * \sa MatrixBase::isApprox() */ - bool isApprox(const Rotation2D& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const + EIGEN_DEVICE_FUNC 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); } + }; /** \ingroup Geometry_Module @@ -134,9 +173,9 @@ typedef Rotation2D<double> Rotation2Dd; */ template<typename Scalar> template<typename Derived> -Rotation2D<Scalar>& Rotation2D<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat) +EIGEN_DEVICE_FUNC Rotation2D<Scalar>& Rotation2D<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat) { - using std::atan2; + EIGEN_USING_STD_MATH(atan2) EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime==2 && Derived::ColsAtCompileTime==2,YOU_MADE_A_PROGRAMMING_MISTAKE) m_angle = atan2(mat.coeff(1,0), mat.coeff(0,0)); return *this; @@ -146,10 +185,10 @@ Rotation2D<Scalar>& Rotation2D<Scalar>::fromRotationMatrix(const MatrixBase<Deri */ template<typename Scalar> typename Rotation2D<Scalar>::Matrix2 -Rotation2D<Scalar>::toRotationMatrix(void) const +EIGEN_DEVICE_FUNC Rotation2D<Scalar>::toRotationMatrix(void) const { - using std::sin; - using std::cos; + EIGEN_USING_STD_MATH(sin) + EIGEN_USING_STD_MATH(cos) Scalar sinA = sin(m_angle); Scalar cosA = cos(m_angle); return (Matrix2() << cosA, -sinA, sinA, cosA).finished(); diff --git a/eigen/Eigen/src/Geometry/RotationBase.h b/eigen/Eigen/src/Geometry/RotationBase.h index b88661d..f0ee0bd 100644 --- a/eigen/Eigen/src/Geometry/RotationBase.h +++ b/eigen/Eigen/src/Geometry/RotationBase.h @@ -22,8 +22,8 @@ struct rotation_base_generic_product_selector; * * \brief Common base class for compact rotation representations * - * \param Derived is the derived type, i.e., a rotation type - * \param _Dim the dimension of the space + * \tparam Derived is the derived type, i.e., a rotation type + * \tparam _Dim the dimension of the space */ template<typename Derived, int _Dim> class RotationBase @@ -38,26 +38,26 @@ class RotationBase typedef Matrix<Scalar,Dim,1> VectorType; public: - inline const Derived& derived() const { return *static_cast<const Derived*>(this); } - inline Derived& derived() { return *static_cast<Derived*>(this); } + EIGEN_DEVICE_FUNC inline const Derived& derived() const { return *static_cast<const Derived*>(this); } + EIGEN_DEVICE_FUNC inline Derived& derived() { return *static_cast<Derived*>(this); } /** \returns an equivalent rotation matrix */ - inline RotationMatrixType toRotationMatrix() const { return derived().toRotationMatrix(); } + EIGEN_DEVICE_FUNC inline RotationMatrixType toRotationMatrix() const { return derived().toRotationMatrix(); } /** \returns an equivalent rotation matrix * This function is added to be conform with the Transform class' naming scheme. */ - inline RotationMatrixType matrix() const { return derived().toRotationMatrix(); } + EIGEN_DEVICE_FUNC inline RotationMatrixType matrix() const { return derived().toRotationMatrix(); } /** \returns the inverse rotation */ - inline Derived inverse() const { return derived().inverse(); } + EIGEN_DEVICE_FUNC inline Derived inverse() const { return derived().inverse(); } /** \returns the concatenation of the rotation \c *this with a translation \a t */ - inline Transform<Scalar,Dim,Isometry> operator*(const Translation<Scalar,Dim>& t) const + EIGEN_DEVICE_FUNC inline Transform<Scalar,Dim,Isometry> operator*(const Translation<Scalar,Dim>& t) const { return Transform<Scalar,Dim,Isometry>(*this) * t; } /** \returns the concatenation of the rotation \c *this with a uniform scaling \a s */ - inline RotationMatrixType operator*(const UniformScaling<Scalar>& s) const + EIGEN_DEVICE_FUNC inline RotationMatrixType operator*(const UniformScaling<Scalar>& s) const { return toRotationMatrix() * s.factor(); } /** \returns the concatenation of the rotation \c *this with a generic expression \a e @@ -67,17 +67,17 @@ class RotationBase * - a vector of size Dim */ template<typename OtherDerived> - EIGEN_STRONG_INLINE typename internal::rotation_base_generic_product_selector<Derived,OtherDerived,OtherDerived::IsVectorAtCompileTime>::ReturnType + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::rotation_base_generic_product_selector<Derived,OtherDerived,OtherDerived::IsVectorAtCompileTime>::ReturnType operator*(const EigenBase<OtherDerived>& e) const { return internal::rotation_base_generic_product_selector<Derived,OtherDerived>::run(derived(), e.derived()); } /** \returns the concatenation of a linear transformation \a l with the rotation \a r */ template<typename OtherDerived> friend - inline RotationMatrixType operator*(const EigenBase<OtherDerived>& l, const Derived& r) + EIGEN_DEVICE_FUNC inline RotationMatrixType operator*(const EigenBase<OtherDerived>& l, const Derived& r) { return l.derived() * r.toRotationMatrix(); } /** \returns the concatenation of a scaling \a l with the rotation \a r */ - friend inline Transform<Scalar,Dim,Affine> operator*(const DiagonalMatrix<Scalar,Dim>& l, const Derived& r) + EIGEN_DEVICE_FUNC friend inline Transform<Scalar,Dim,Affine> operator*(const DiagonalMatrix<Scalar,Dim>& l, const Derived& r) { Transform<Scalar,Dim,Affine> res(r); res.linear().applyOnTheLeft(l); @@ -86,11 +86,11 @@ class RotationBase /** \returns the concatenation of the rotation \c *this with a transformation \a t */ template<int Mode, int Options> - inline Transform<Scalar,Dim,Mode> operator*(const Transform<Scalar,Dim,Mode,Options>& t) const + EIGEN_DEVICE_FUNC inline Transform<Scalar,Dim,Mode> operator*(const Transform<Scalar,Dim,Mode,Options>& t) const { return toRotationMatrix() * t; } template<typename OtherVectorType> - inline VectorType _transformVector(const OtherVectorType& v) const + EIGEN_DEVICE_FUNC inline VectorType _transformVector(const OtherVectorType& v) const { return toRotationMatrix() * v; } }; @@ -102,7 +102,7 @@ struct rotation_base_generic_product_selector<RotationDerived,MatrixType,false> { enum { Dim = RotationDerived::Dim }; typedef Matrix<typename RotationDerived::Scalar,Dim,Dim> ReturnType; - static inline ReturnType run(const RotationDerived& r, const MatrixType& m) + EIGEN_DEVICE_FUNC static inline ReturnType run(const RotationDerived& r, const MatrixType& m) { return r.toRotationMatrix() * m; } }; @@ -110,7 +110,7 @@ template<typename RotationDerived, typename Scalar, int Dim, int MaxDim> struct rotation_base_generic_product_selector< RotationDerived, DiagonalMatrix<Scalar,Dim,MaxDim>, false > { typedef Transform<Scalar,Dim,Affine> ReturnType; - static inline ReturnType run(const RotationDerived& r, const DiagonalMatrix<Scalar,Dim,MaxDim>& m) + EIGEN_DEVICE_FUNC static inline ReturnType run(const RotationDerived& r, const DiagonalMatrix<Scalar,Dim,MaxDim>& m) { ReturnType res(r); res.linear() *= m; @@ -123,7 +123,7 @@ struct rotation_base_generic_product_selector<RotationDerived,OtherVectorType,tr { enum { Dim = RotationDerived::Dim }; typedef Matrix<typename RotationDerived::Scalar,Dim,1> ReturnType; - static EIGEN_STRONG_INLINE ReturnType run(const RotationDerived& r, const OtherVectorType& v) + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE ReturnType run(const RotationDerived& r, const OtherVectorType& v) { return r._transformVector(v); } @@ -137,7 +137,7 @@ struct rotation_base_generic_product_selector<RotationDerived,OtherVectorType,tr */ template<typename _Scalar, int _Rows, int _Cols, int _Storage, int _MaxRows, int _MaxCols> template<typename OtherDerived> -Matrix<_Scalar, _Rows, _Cols, _Storage, _MaxRows, _MaxCols> +EIGEN_DEVICE_FUNC Matrix<_Scalar, _Rows, _Cols, _Storage, _MaxRows, _MaxCols> ::Matrix(const RotationBase<OtherDerived,ColsAtCompileTime>& r) { EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix,int(OtherDerived::Dim),int(OtherDerived::Dim)) @@ -150,7 +150,7 @@ Matrix<_Scalar, _Rows, _Cols, _Storage, _MaxRows, _MaxCols> */ template<typename _Scalar, int _Rows, int _Cols, int _Storage, int _MaxRows, int _MaxCols> template<typename OtherDerived> -Matrix<_Scalar, _Rows, _Cols, _Storage, _MaxRows, _MaxCols>& +EIGEN_DEVICE_FUNC Matrix<_Scalar, _Rows, _Cols, _Storage, _MaxRows, _MaxCols>& Matrix<_Scalar, _Rows, _Cols, _Storage, _MaxRows, _MaxCols> ::operator=(const RotationBase<OtherDerived,ColsAtCompileTime>& r) { @@ -164,8 +164,8 @@ namespace internal { * * Helper function to return an arbitrary rotation object to a rotation matrix. * - * \param Scalar the numeric type of the matrix coefficients - * \param Dim the dimension of the current space + * \tparam Scalar the numeric type of the matrix coefficients + * \tparam Dim the dimension of the current space * * It returns a Dim x Dim fixed size matrix. * @@ -179,20 +179,20 @@ namespace internal { * \sa class Transform, class Rotation2D, class Quaternion, class AngleAxis */ template<typename Scalar, int Dim> -static inline Matrix<Scalar,2,2> toRotationMatrix(const Scalar& s) +EIGEN_DEVICE_FUNC static inline Matrix<Scalar,2,2> toRotationMatrix(const Scalar& s) { EIGEN_STATIC_ASSERT(Dim==2,YOU_MADE_A_PROGRAMMING_MISTAKE) return Rotation2D<Scalar>(s).toRotationMatrix(); } template<typename Scalar, int Dim, typename OtherDerived> -static inline Matrix<Scalar,Dim,Dim> toRotationMatrix(const RotationBase<OtherDerived,Dim>& r) +EIGEN_DEVICE_FUNC static inline Matrix<Scalar,Dim,Dim> toRotationMatrix(const RotationBase<OtherDerived,Dim>& r) { return r.toRotationMatrix(); } template<typename Scalar, int Dim, typename OtherDerived> -static inline const MatrixBase<OtherDerived>& toRotationMatrix(const MatrixBase<OtherDerived>& mat) +EIGEN_DEVICE_FUNC static inline const MatrixBase<OtherDerived>& toRotationMatrix(const MatrixBase<OtherDerived>& mat) { EIGEN_STATIC_ASSERT(OtherDerived::RowsAtCompileTime==Dim && OtherDerived::ColsAtCompileTime==Dim, YOU_MADE_A_PROGRAMMING_MISTAKE) diff --git a/eigen/Eigen/src/Geometry/Scaling.h b/eigen/Eigen/src/Geometry/Scaling.h index 1c25f36..f58ca03 100644 --- a/eigen/Eigen/src/Geometry/Scaling.h +++ b/eigen/Eigen/src/Geometry/Scaling.h @@ -18,7 +18,7 @@ namespace Eigen { * * \brief Represents a generic uniform scaling transformation * - * \param _Scalar the scalar type, i.e., the type of the coefficients. + * \tparam _Scalar the scalar type, i.e., the type of the coefficients. * * This class represent a uniform scaling transformation. It is the return * type of Scaling(Scalar), and most of the time this is the only way it @@ -62,10 +62,10 @@ public: template<int Dim, int Mode, int Options> inline Transform<Scalar,Dim,(int(Mode)==int(Isometry)?Affine:Mode)> operator* (const Transform<Scalar,Dim, Mode, Options>& t) const { - Transform<Scalar,Dim,(int(Mode)==int(Isometry)?Affine:Mode)> res = t; - res.prescale(factor()); - return res; -} + Transform<Scalar,Dim,(int(Mode)==int(Isometry)?Affine:Mode)> res = t; + res.prescale(factor()); + return res; + } /** Concatenates a uniform scaling and a linear transformation matrix */ // TODO returns an expression @@ -104,40 +104,44 @@ public: }; -/** Concatenates a linear transformation matrix and a uniform scaling */ +/** \addtogroup Geometry_Module */ +//@{ + +/** Concatenates a linear transformation matrix and a uniform scaling + * \relates UniformScaling + */ // NOTE this operator is defiend in MatrixBase and not as a friend function // of UniformScaling to fix an internal crash of Intel's ICC -template<typename Derived> typename MatrixBase<Derived>::ScalarMultipleReturnType -MatrixBase<Derived>::operator*(const UniformScaling<Scalar>& s) const -{ return derived() * s.factor(); } +template<typename Derived,typename Scalar> +EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(Derived,Scalar,product) +operator*(const MatrixBase<Derived>& matrix, const UniformScaling<Scalar>& s) +{ return matrix.derived() * s.factor(); } /** Constructs a uniform scaling from scale factor \a s */ -static inline UniformScaling<float> Scaling(float s) { return UniformScaling<float>(s); } +inline UniformScaling<float> Scaling(float s) { return UniformScaling<float>(s); } /** Constructs a uniform scaling from scale factor \a s */ -static inline UniformScaling<double> Scaling(double s) { return UniformScaling<double>(s); } +inline UniformScaling<double> Scaling(double s) { return UniformScaling<double>(s); } /** Constructs a uniform scaling from scale factor \a s */ template<typename RealScalar> -static inline UniformScaling<std::complex<RealScalar> > Scaling(const std::complex<RealScalar>& s) +inline UniformScaling<std::complex<RealScalar> > Scaling(const std::complex<RealScalar>& s) { return UniformScaling<std::complex<RealScalar> >(s); } /** Constructs a 2D axis aligned scaling */ template<typename Scalar> -static inline DiagonalMatrix<Scalar,2> Scaling(const Scalar& sx, const Scalar& sy) +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(const Scalar& sx, const Scalar& sy, const Scalar& sz) +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 * This is an alias for coeffs.asDiagonal() */ template<typename Derived> -static inline const DiagonalWrapper<const Derived> Scaling(const MatrixBase<Derived>& coeffs) +inline const DiagonalWrapper<const Derived> Scaling(const MatrixBase<Derived>& coeffs) { return coeffs.asDiagonal(); } -/** \addtogroup Geometry_Module */ -//@{ /** \deprecated */ typedef DiagonalMatrix<float, 2> AlignedScaling2f; /** \deprecated */ diff --git a/eigen/Eigen/src/Geometry/Transform.h b/eigen/Eigen/src/Geometry/Transform.h index 0186f3b..2d36dfa 100644 --- a/eigen/Eigen/src/Geometry/Transform.h +++ b/eigen/Eigen/src/Geometry/Transform.h @@ -32,7 +32,8 @@ template< typename TransformType, typename MatrixType, int Case = transform_traits<TransformType>::IsProjective ? 0 : int(MatrixType::RowsAtCompileTime) == int(transform_traits<TransformType>::HDim) ? 1 - : 2> + : 2, + int RhsCols = MatrixType::ColsAtCompileTime> struct transform_right_product_impl; template< typename Other, @@ -62,6 +63,22 @@ struct transform_construct_from_matrix; template<typename TransformType> struct transform_take_affine_part; +template<typename _Scalar, int _Dim, int _Mode, int _Options> +struct traits<Transform<_Scalar,_Dim,_Mode,_Options> > +{ + typedef _Scalar Scalar; + typedef Eigen::Index StorageIndex; + typedef Dense StorageKind; + enum { + Dim1 = _Dim==Dynamic ? _Dim : _Dim + 1, + RowsAtCompileTime = _Mode==Projective ? Dim1 : _Dim, + ColsAtCompileTime = Dim1, + MaxRowsAtCompileTime = RowsAtCompileTime, + MaxColsAtCompileTime = ColsAtCompileTime, + Flags = 0 + }; +}; + template<int Mode> struct transform_make_affine; } // end namespace internal @@ -176,7 +193,7 @@ template<int Mode> struct transform_make_affine; * preprocessor token EIGEN_QT_SUPPORT is defined. * * This class can be extended with the help of the plugin mechanism described on the page - * \ref TopicCustomizingEigen by defining the preprocessor symbol \c EIGEN_TRANSFORM_PLUGIN. + * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_TRANSFORM_PLUGIN. * * \sa class Matrix, class Quaternion */ @@ -194,7 +211,8 @@ public: }; /** the scalar type of the coefficients */ typedef _Scalar Scalar; - typedef DenseIndex Index; + typedef Eigen::Index StorageIndex; + typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 /** type of the matrix used to represent the transformation */ typedef typename internal::make_proper_matrix_type<Scalar,Rows,HDim,Options>::type MatrixType; /** constified MatrixType */ @@ -216,9 +234,9 @@ public: /** type of a vector */ typedef Matrix<Scalar,Dim,1> VectorType; /** type of a read/write reference to the translation part of the rotation */ - typedef Block<MatrixType,Dim,1,int(Mode)==(AffineCompact)> TranslationPart; + typedef Block<MatrixType,Dim,1,!(internal::traits<MatrixType>::Flags & RowMajorBit)> TranslationPart; /** type of a read reference to the translation part of the rotation */ - typedef const Block<ConstMatrixType,Dim,1,int(Mode)==(AffineCompact)> ConstTranslationPart; + typedef const Block<ConstMatrixType,Dim,1,!(internal::traits<MatrixType>::Flags & RowMajorBit)> ConstTranslationPart; /** corresponding translation type */ typedef Translation<Scalar,Dim> TranslationType; @@ -235,43 +253,43 @@ public: /** Default constructor without initialization of the meaningful coefficients. * If Mode==Affine, then the last row is set to [0 ... 0 1] */ - inline Transform() + EIGEN_DEVICE_FUNC inline Transform() { check_template_params(); internal::transform_make_affine<(int(Mode)==Affine) ? Affine : AffineCompact>::run(m_matrix); } - inline Transform(const Transform& other) + EIGEN_DEVICE_FUNC inline Transform(const Transform& other) { check_template_params(); m_matrix = other.m_matrix; } - inline explicit Transform(const TranslationType& t) + EIGEN_DEVICE_FUNC inline explicit Transform(const TranslationType& t) { check_template_params(); *this = t; } - inline explicit Transform(const UniformScaling<Scalar>& s) + EIGEN_DEVICE_FUNC inline explicit Transform(const UniformScaling<Scalar>& s) { check_template_params(); *this = s; } template<typename Derived> - inline explicit Transform(const RotationBase<Derived, Dim>& r) + EIGEN_DEVICE_FUNC inline explicit Transform(const RotationBase<Derived, Dim>& r) { check_template_params(); *this = r; } - inline Transform& operator=(const Transform& other) + EIGEN_DEVICE_FUNC inline Transform& operator=(const Transform& other) { m_matrix = other.m_matrix; return *this; } typedef internal::transform_take_affine_part<Transform> take_affine_part; /** Constructs and initializes a transformation from a Dim^2 or a (Dim+1)^2 matrix. */ template<typename OtherDerived> - inline explicit Transform(const EigenBase<OtherDerived>& other) + EIGEN_DEVICE_FUNC inline explicit Transform(const EigenBase<OtherDerived>& other) { EIGEN_STATIC_ASSERT((internal::is_same<Scalar,typename OtherDerived::Scalar>::value), YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY); @@ -282,7 +300,7 @@ public: /** Set \c *this from a Dim^2 or (Dim+1)^2 matrix. */ template<typename OtherDerived> - inline Transform& operator=(const EigenBase<OtherDerived>& other) + EIGEN_DEVICE_FUNC inline Transform& operator=(const EigenBase<OtherDerived>& other) { EIGEN_STATIC_ASSERT((internal::is_same<Scalar,typename OtherDerived::Scalar>::value), YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY); @@ -292,7 +310,7 @@ public: } template<int OtherOptions> - inline Transform(const Transform<Scalar,Dim,Mode,OtherOptions>& other) + EIGEN_DEVICE_FUNC inline Transform(const Transform<Scalar,Dim,Mode,OtherOptions>& other) { check_template_params(); // only the options change, we can directly copy the matrices @@ -300,7 +318,7 @@ public: } template<int OtherMode,int OtherOptions> - inline Transform(const Transform<Scalar,Dim,OtherMode,OtherOptions>& other) + EIGEN_DEVICE_FUNC inline Transform(const Transform<Scalar,Dim,OtherMode,OtherOptions>& other) { check_template_params(); // prevent conversions as: @@ -317,7 +335,7 @@ public: OtherModeIsAffineCompact = OtherMode == int(AffineCompact) }; - if(ModeIsAffineCompact == OtherModeIsAffineCompact) + if(EIGEN_CONST_CONDITIONAL(ModeIsAffineCompact == OtherModeIsAffineCompact)) { // We need the block expression because the code is compiled for all // combinations of transformations and will trigger a compile time error @@ -325,7 +343,7 @@ public: m_matrix.template block<Dim,Dim+1>(0,0) = other.matrix().template block<Dim,Dim+1>(0,0); makeAffine(); } - else if(OtherModeIsAffineCompact) + else if(EIGEN_CONST_CONDITIONAL(OtherModeIsAffineCompact)) { typedef typename Transform<Scalar,Dim,OtherMode,OtherOptions>::MatrixType OtherMatrixType; internal::transform_construct_from_matrix<OtherMatrixType,Mode,Options,Dim,HDim>::run(this, other.matrix()); @@ -341,14 +359,14 @@ public: } template<typename OtherDerived> - Transform(const ReturnByValue<OtherDerived>& other) + EIGEN_DEVICE_FUNC Transform(const ReturnByValue<OtherDerived>& other) { check_template_params(); other.evalTo(*this); } template<typename OtherDerived> - Transform& operator=(const ReturnByValue<OtherDerived>& other) + EIGEN_DEVICE_FUNC Transform& operator=(const ReturnByValue<OtherDerived>& other) { other.evalTo(*this); return *this; @@ -362,33 +380,36 @@ public: inline Transform& operator=(const QTransform& other); inline QTransform toQTransform(void) const; #endif + + EIGEN_DEVICE_FUNC Index rows() const { return int(Mode)==int(Projective) ? m_matrix.cols() : (m_matrix.cols()-1); } + EIGEN_DEVICE_FUNC Index cols() const { return m_matrix.cols(); } /** shortcut for m_matrix(row,col); * \sa MatrixBase::operator(Index,Index) const */ - inline Scalar operator() (Index row, Index col) const { return m_matrix(row,col); } + EIGEN_DEVICE_FUNC inline Scalar operator() (Index row, Index col) const { return m_matrix(row,col); } /** shortcut for m_matrix(row,col); * \sa MatrixBase::operator(Index,Index) */ - inline Scalar& operator() (Index row, Index col) { return m_matrix(row,col); } + EIGEN_DEVICE_FUNC inline Scalar& operator() (Index row, Index col) { return m_matrix(row,col); } /** \returns a read-only expression of the transformation matrix */ - inline const MatrixType& matrix() const { return m_matrix; } + EIGEN_DEVICE_FUNC inline const MatrixType& matrix() const { return m_matrix; } /** \returns a writable expression of the transformation matrix */ - inline MatrixType& matrix() { return m_matrix; } + EIGEN_DEVICE_FUNC inline MatrixType& matrix() { return m_matrix; } /** \returns a read-only expression of the linear part of the transformation */ - inline ConstLinearPart linear() const { return ConstLinearPart(m_matrix,0,0); } + EIGEN_DEVICE_FUNC inline ConstLinearPart linear() const { return ConstLinearPart(m_matrix,0,0); } /** \returns a writable expression of the linear part of the transformation */ - inline LinearPart linear() { return LinearPart(m_matrix,0,0); } + EIGEN_DEVICE_FUNC inline LinearPart linear() { return LinearPart(m_matrix,0,0); } /** \returns a read-only expression of the Dim x HDim affine part of the transformation */ - inline ConstAffinePart affine() const { return take_affine_part::run(m_matrix); } + EIGEN_DEVICE_FUNC inline ConstAffinePart affine() const { return take_affine_part::run(m_matrix); } /** \returns a writable expression of the Dim x HDim affine part of the transformation */ - inline AffinePart affine() { return take_affine_part::run(m_matrix); } + EIGEN_DEVICE_FUNC inline AffinePart affine() { return take_affine_part::run(m_matrix); } /** \returns a read-only expression of the translation vector of the transformation */ - inline ConstTranslationPart translation() const { return ConstTranslationPart(m_matrix,0,Dim); } + EIGEN_DEVICE_FUNC inline ConstTranslationPart translation() const { return ConstTranslationPart(m_matrix,0,Dim); } /** \returns a writable expression of the translation vector of the transformation */ - inline TranslationPart translation() { return TranslationPart(m_matrix,0,Dim); } + EIGEN_DEVICE_FUNC inline TranslationPart translation() { return TranslationPart(m_matrix,0,Dim); } /** \returns an expression of the product between the transform \c *this and a matrix expression \a other. * @@ -416,7 +437,7 @@ public: */ // note: this function is defined here because some compilers cannot find the respective declaration template<typename OtherDerived> - EIGEN_STRONG_INLINE const typename OtherDerived::PlainObject + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename internal::transform_right_product_impl<Transform, OtherDerived>::ResultType operator * (const EigenBase<OtherDerived> &other) const { return internal::transform_right_product_impl<Transform, OtherDerived>::run(*this,other.derived()); } @@ -428,7 +449,7 @@ public: * \li a general transformation matrix of size Dim+1 x Dim+1. */ template<typename OtherDerived> friend - inline const typename internal::transform_left_product_impl<OtherDerived,Mode,Options,_Dim,_Dim+1>::ResultType + EIGEN_DEVICE_FUNC inline const typename internal::transform_left_product_impl<OtherDerived,Mode,Options,_Dim,_Dim+1>::ResultType operator * (const EigenBase<OtherDerived> &a, const Transform &b) { return internal::transform_left_product_impl<OtherDerived,Mode,Options,Dim,HDim>::run(a.derived(),b); } @@ -439,11 +460,11 @@ public: * mode is no isometry. In that case, the returned transform is an affinity. */ template<typename DiagonalDerived> - inline const TransformTimeDiagonalReturnType + EIGEN_DEVICE_FUNC inline const TransformTimeDiagonalReturnType operator * (const DiagonalBase<DiagonalDerived> &b) const { TransformTimeDiagonalReturnType res(*this); - res.linear() *= b; + res.linearExt() *= b; return res; } @@ -454,27 +475,27 @@ public: * mode is no isometry. In that case, the returned transform is an affinity. */ template<typename DiagonalDerived> - friend inline TransformTimeDiagonalReturnType + EIGEN_DEVICE_FUNC friend inline TransformTimeDiagonalReturnType operator * (const DiagonalBase<DiagonalDerived> &a, const Transform &b) { TransformTimeDiagonalReturnType res; res.linear().noalias() = a*b.linear(); res.translation().noalias() = a*b.translation(); - if (Mode!=int(AffineCompact)) + if (EIGEN_CONST_CONDITIONAL(Mode!=int(AffineCompact))) res.matrix().row(Dim) = b.matrix().row(Dim); return res; } template<typename OtherDerived> - inline Transform& operator*=(const EigenBase<OtherDerived>& other) { return *this = *this * other; } + EIGEN_DEVICE_FUNC inline Transform& operator*=(const EigenBase<OtherDerived>& other) { return *this = *this * other; } /** Concatenates two transformations */ - inline const Transform operator * (const Transform& other) const + EIGEN_DEVICE_FUNC inline const Transform operator * (const Transform& other) const { return internal::transform_transform_product_impl<Transform,Transform>::run(*this,other); } - #ifdef __INTEL_COMPILER + #if EIGEN_COMP_ICC private: // this intermediate structure permits to workaround a bug in ICC 11: // error: template instantiation resulted in unexpected function type of "Eigen::Transform<double, 3, 32, 0> @@ -501,7 +522,7 @@ public: #else /** Concatenates two different transformations */ template<int OtherMode,int OtherOptions> - inline typename internal::transform_transform_product_impl<Transform,Transform<Scalar,Dim,OtherMode,OtherOptions> >::ResultType + EIGEN_DEVICE_FUNC inline typename internal::transform_transform_product_impl<Transform,Transform<Scalar,Dim,OtherMode,OtherOptions> >::ResultType operator * (const Transform<Scalar,Dim,OtherMode,OtherOptions>& other) const { return internal::transform_transform_product_impl<Transform,Transform<Scalar,Dim,OtherMode,OtherOptions> >::run(*this,other); @@ -509,79 +530,98 @@ public: #endif /** \sa MatrixBase::setIdentity() */ - void setIdentity() { m_matrix.setIdentity(); } + EIGEN_DEVICE_FUNC void setIdentity() { m_matrix.setIdentity(); } /** * \brief Returns an identity transformation. * \todo In the future this function should be returning a Transform expression. */ - static const Transform Identity() + EIGEN_DEVICE_FUNC static const Transform Identity() { return Transform(MatrixType::Identity()); } template<typename OtherDerived> + EIGEN_DEVICE_FUNC inline Transform& scale(const MatrixBase<OtherDerived> &other); template<typename OtherDerived> + EIGEN_DEVICE_FUNC inline Transform& prescale(const MatrixBase<OtherDerived> &other); - inline Transform& scale(const Scalar& s); - inline Transform& prescale(const Scalar& s); + EIGEN_DEVICE_FUNC inline Transform& scale(const Scalar& s); + EIGEN_DEVICE_FUNC inline Transform& prescale(const Scalar& s); template<typename OtherDerived> + EIGEN_DEVICE_FUNC inline Transform& translate(const MatrixBase<OtherDerived> &other); template<typename OtherDerived> + EIGEN_DEVICE_FUNC inline Transform& pretranslate(const MatrixBase<OtherDerived> &other); template<typename RotationType> + EIGEN_DEVICE_FUNC inline Transform& rotate(const RotationType& rotation); template<typename RotationType> + EIGEN_DEVICE_FUNC inline Transform& prerotate(const RotationType& rotation); - Transform& shear(const Scalar& sx, const Scalar& sy); - Transform& preshear(const Scalar& sx, const Scalar& sy); + EIGEN_DEVICE_FUNC Transform& shear(const Scalar& sx, const Scalar& sy); + EIGEN_DEVICE_FUNC Transform& preshear(const Scalar& sx, const Scalar& sy); - inline Transform& operator=(const TranslationType& t); + EIGEN_DEVICE_FUNC inline Transform& operator=(const TranslationType& t); + + EIGEN_DEVICE_FUNC inline Transform& operator*=(const TranslationType& t) { return translate(t.vector()); } - inline Transform operator*(const TranslationType& t) const; + + EIGEN_DEVICE_FUNC inline Transform operator*(const TranslationType& t) const; + EIGEN_DEVICE_FUNC inline Transform& operator=(const UniformScaling<Scalar>& t); + + EIGEN_DEVICE_FUNC inline Transform& operator*=(const UniformScaling<Scalar>& s) { return scale(s.factor()); } - inline Transform<Scalar,Dim,(int(Mode)==int(Isometry)?int(Affine):int(Mode))> operator*(const UniformScaling<Scalar>& s) const + + EIGEN_DEVICE_FUNC + inline TransformTimeDiagonalReturnType operator*(const UniformScaling<Scalar>& s) const { - Transform<Scalar,Dim,(int(Mode)==int(Isometry)?int(Affine):int(Mode)),Options> res = *this; + TransformTimeDiagonalReturnType res = *this; res.scale(s.factor()); return res; } - inline Transform& operator*=(const DiagonalMatrix<Scalar,Dim>& s) { linear() *= s; return *this; } + EIGEN_DEVICE_FUNC + inline Transform& operator*=(const DiagonalMatrix<Scalar,Dim>& s) { linearExt() *= s; return *this; } template<typename Derived> - inline Transform& operator=(const RotationBase<Derived,Dim>& r); + EIGEN_DEVICE_FUNC inline Transform& operator=(const RotationBase<Derived,Dim>& r); template<typename Derived> - inline Transform& operator*=(const RotationBase<Derived,Dim>& r) { return rotate(r.toRotationMatrix()); } + EIGEN_DEVICE_FUNC inline Transform& operator*=(const RotationBase<Derived,Dim>& r) { return rotate(r.toRotationMatrix()); } template<typename Derived> - inline Transform operator*(const RotationBase<Derived,Dim>& r) const; + EIGEN_DEVICE_FUNC inline Transform operator*(const RotationBase<Derived,Dim>& r) const; - const LinearMatrixType rotation() const; + EIGEN_DEVICE_FUNC const LinearMatrixType rotation() const; template<typename RotationMatrixType, typename ScalingMatrixType> + EIGEN_DEVICE_FUNC void computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const; template<typename ScalingMatrixType, typename RotationMatrixType> + EIGEN_DEVICE_FUNC void computeScalingRotation(ScalingMatrixType *scaling, RotationMatrixType *rotation) const; template<typename PositionDerived, typename OrientationType, typename ScaleDerived> + EIGEN_DEVICE_FUNC Transform& fromPositionOrientationScale(const MatrixBase<PositionDerived> &position, const OrientationType& orientation, const MatrixBase<ScaleDerived> &scale); + EIGEN_DEVICE_FUNC inline Transform inverse(TransformTraits traits = (TransformTraits)Mode) const; /** \returns a const pointer to the column major internal matrix */ - const Scalar* data() const { return m_matrix.data(); } + EIGEN_DEVICE_FUNC const Scalar* data() const { return m_matrix.data(); } /** \returns a non-const pointer to the column major internal matrix */ - Scalar* data() { return m_matrix.data(); } + EIGEN_DEVICE_FUNC Scalar* data() { return m_matrix.data(); } /** \returns \c *this with scalar type casted to \a NewScalarType * @@ -589,12 +629,12 @@ public: * then this function smartly returns a const reference to \c *this. */ template<typename NewScalarType> - inline typename internal::cast_return_type<Transform,Transform<NewScalarType,Dim,Mode,Options> >::type cast() const + EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<Transform,Transform<NewScalarType,Dim,Mode,Options> >::type cast() const { return typename internal::cast_return_type<Transform,Transform<NewScalarType,Dim,Mode,Options> >::type(*this); } /** Copy constructor with scalar type conversion */ template<typename OtherScalarType> - inline explicit Transform(const Transform<OtherScalarType,Dim,Mode,Options>& other) + EIGEN_DEVICE_FUNC inline explicit Transform(const Transform<OtherScalarType,Dim,Mode,Options>& other) { check_template_params(); m_matrix = other.matrix().template cast<Scalar>(); @@ -604,12 +644,12 @@ public: * determined by \a prec. * * \sa MatrixBase::isApprox() */ - bool isApprox(const Transform& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const + EIGEN_DEVICE_FUNC 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] */ - void makeAffine() + EIGEN_DEVICE_FUNC void makeAffine() { internal::transform_make_affine<int(Mode)>::run(m_matrix); } @@ -618,26 +658,26 @@ public: * \returns the Dim x Dim linear part if the transformation is affine, * and the HDim x Dim part for projective transformations. */ - inline Block<MatrixType,int(Mode)==int(Projective)?HDim:Dim,Dim> linearExt() + EIGEN_DEVICE_FUNC inline Block<MatrixType,int(Mode)==int(Projective)?HDim:Dim,Dim> linearExt() { return m_matrix.template block<int(Mode)==int(Projective)?HDim:Dim,Dim>(0,0); } /** \internal * \returns the Dim x Dim linear part if the transformation is affine, * and the HDim x Dim part for projective transformations. */ - inline const Block<MatrixType,int(Mode)==int(Projective)?HDim:Dim,Dim> linearExt() const + EIGEN_DEVICE_FUNC inline const Block<MatrixType,int(Mode)==int(Projective)?HDim:Dim,Dim> linearExt() const { return m_matrix.template block<int(Mode)==int(Projective)?HDim:Dim,Dim>(0,0); } /** \internal * \returns the translation part if the transformation is affine, * and the last column for projective transformations. */ - inline Block<MatrixType,int(Mode)==int(Projective)?HDim:Dim,1> translationExt() + EIGEN_DEVICE_FUNC inline Block<MatrixType,int(Mode)==int(Projective)?HDim:Dim,1> translationExt() { return m_matrix.template block<int(Mode)==int(Projective)?HDim:Dim,1>(0,Dim); } /** \internal * \returns the translation part if the transformation is affine, * and the last column for projective transformations. */ - inline const Block<MatrixType,int(Mode)==int(Projective)?HDim:Dim,1> translationExt() const + EIGEN_DEVICE_FUNC inline const Block<MatrixType,int(Mode)==int(Projective)?HDim:Dim,1> translationExt() const { return m_matrix.template block<int(Mode)==int(Projective)?HDim:Dim,1>(0,Dim); } @@ -647,7 +687,7 @@ public: protected: #ifndef EIGEN_PARSED_BY_DOXYGEN - static EIGEN_STRONG_INLINE void check_template_params() + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void check_template_params() { EIGEN_STATIC_ASSERT((Options & (DontAlign|RowMajor)) == Options, INVALID_MATRIX_TEMPLATE_PARAMETERS) } @@ -715,9 +755,13 @@ template<typename Scalar, int Dim, int Mode,int Options> Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::operator=(const QMatrix& other) { EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE) - m_matrix << other.m11(), other.m21(), other.dx(), - other.m12(), other.m22(), other.dy(), - 0, 0, 1; + if (EIGEN_CONST_CONDITIONAL(Mode == int(AffineCompact))) + m_matrix << other.m11(), other.m21(), other.dx(), + other.m12(), other.m22(), other.dy(); + else + m_matrix << other.m11(), other.m21(), other.dx(), + other.m12(), other.m22(), other.dy(), + 0, 0, 1; return *this; } @@ -757,7 +801,7 @@ Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::operator { check_template_params(); EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE) - if (Mode == int(AffineCompact)) + if (EIGEN_CONST_CONDITIONAL(Mode == int(AffineCompact))) m_matrix << other.m11(), other.m21(), other.dx(), other.m12(), other.m22(), other.dy(); else @@ -775,7 +819,7 @@ template<typename Scalar, int Dim, int Mode, int Options> QTransform Transform<Scalar,Dim,Mode,Options>::toQTransform(void) const { EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE) - if (Mode == int(AffineCompact)) + if (EIGEN_CONST_CONDITIONAL(Mode == int(AffineCompact))) return QTransform(m_matrix.coeff(0,0), m_matrix.coeff(1,0), m_matrix.coeff(0,1), m_matrix.coeff(1,1), m_matrix.coeff(0,2), m_matrix.coeff(1,2)); @@ -796,7 +840,7 @@ QTransform Transform<Scalar,Dim,Mode,Options>::toQTransform(void) const */ template<typename Scalar, int Dim, int Mode, int Options> template<typename OtherDerived> -Transform<Scalar,Dim,Mode,Options>& +EIGEN_DEVICE_FUNC Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::scale(const MatrixBase<OtherDerived> &other) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim)) @@ -810,7 +854,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(const Scalar& s) +EIGEN_DEVICE_FUNC 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; @@ -823,12 +867,12 @@ inline Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::s */ template<typename Scalar, int Dim, int Mode, int Options> template<typename OtherDerived> -Transform<Scalar,Dim,Mode,Options>& +EIGEN_DEVICE_FUNC Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::prescale(const MatrixBase<OtherDerived> &other) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim)) EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS) - m_matrix.template block<Dim,HDim>(0,0).noalias() = (other.asDiagonal() * m_matrix.template block<Dim,HDim>(0,0)); + affine().noalias() = (other.asDiagonal() * affine()); return *this; } @@ -837,7 +881,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(const Scalar& s) +EIGEN_DEVICE_FUNC 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; @@ -850,7 +894,7 @@ inline Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::p */ template<typename Scalar, int Dim, int Mode, int Options> template<typename OtherDerived> -Transform<Scalar,Dim,Mode,Options>& +EIGEN_DEVICE_FUNC Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::translate(const MatrixBase<OtherDerived> &other) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim)) @@ -864,11 +908,11 @@ Transform<Scalar,Dim,Mode,Options>::translate(const MatrixBase<OtherDerived> &ot */ template<typename Scalar, int Dim, int Mode, int Options> template<typename OtherDerived> -Transform<Scalar,Dim,Mode,Options>& +EIGEN_DEVICE_FUNC Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::pretranslate(const MatrixBase<OtherDerived> &other) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim)) - if(int(Mode)==int(Projective)) + if(EIGEN_CONST_CONDITIONAL(int(Mode)==int(Projective))) affine() += other * m_matrix.row(Dim); else translation() += other; @@ -894,7 +938,7 @@ Transform<Scalar,Dim,Mode,Options>::pretranslate(const MatrixBase<OtherDerived> */ template<typename Scalar, int Dim, int Mode, int Options> template<typename RotationType> -Transform<Scalar,Dim,Mode,Options>& +EIGEN_DEVICE_FUNC Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::rotate(const RotationType& rotation) { linearExt() *= internal::toRotationMatrix<Scalar,Dim>(rotation); @@ -910,7 +954,7 @@ Transform<Scalar,Dim,Mode,Options>::rotate(const RotationType& rotation) */ template<typename Scalar, int Dim, int Mode, int Options> template<typename RotationType> -Transform<Scalar,Dim,Mode,Options>& +EIGEN_DEVICE_FUNC Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::prerotate(const RotationType& rotation) { m_matrix.template block<Dim,HDim>(0,0) = internal::toRotationMatrix<Scalar,Dim>(rotation) @@ -924,7 +968,7 @@ Transform<Scalar,Dim,Mode,Options>::prerotate(const RotationType& rotation) * \sa preshear() */ template<typename Scalar, int Dim, int Mode, int Options> -Transform<Scalar,Dim,Mode,Options>& +EIGEN_DEVICE_FUNC Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::shear(const Scalar& sx, const Scalar& sy) { EIGEN_STATIC_ASSERT(int(Dim)==2, YOU_MADE_A_PROGRAMMING_MISTAKE) @@ -940,7 +984,7 @@ Transform<Scalar,Dim,Mode,Options>::shear(const Scalar& sx, const Scalar& sy) * \sa shear() */ template<typename Scalar, int Dim, int Mode, int Options> -Transform<Scalar,Dim,Mode,Options>& +EIGEN_DEVICE_FUNC Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::preshear(const Scalar& sx, const Scalar& sy) { EIGEN_STATIC_ASSERT(int(Dim)==2, YOU_MADE_A_PROGRAMMING_MISTAKE) @@ -954,7 +998,7 @@ Transform<Scalar,Dim,Mode,Options>::preshear(const Scalar& sx, const Scalar& sy) ******************************************************/ template<typename Scalar, int Dim, int Mode, int Options> -inline Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::operator=(const TranslationType& t) +EIGEN_DEVICE_FUNC inline Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::operator=(const TranslationType& t) { linear().setIdentity(); translation() = t.vector(); @@ -963,7 +1007,7 @@ inline Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::o } template<typename Scalar, int Dim, int Mode, int Options> -inline Transform<Scalar,Dim,Mode,Options> Transform<Scalar,Dim,Mode,Options>::operator*(const TranslationType& t) const +EIGEN_DEVICE_FUNC inline Transform<Scalar,Dim,Mode,Options> Transform<Scalar,Dim,Mode,Options>::operator*(const TranslationType& t) const { Transform res = *this; res.translate(t.vector()); @@ -971,7 +1015,7 @@ inline Transform<Scalar,Dim,Mode,Options> Transform<Scalar,Dim,Mode,Options>::op } template<typename Scalar, int Dim, int Mode, int Options> -inline Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::operator=(const UniformScaling<Scalar>& s) +EIGEN_DEVICE_FUNC inline Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::operator=(const UniformScaling<Scalar>& s) { m_matrix.setZero(); linear().diagonal().fill(s.factor()); @@ -981,7 +1025,7 @@ inline Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::o template<typename Scalar, int Dim, int Mode, int Options> template<typename Derived> -inline Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::operator=(const RotationBase<Derived,Dim>& r) +EIGEN_DEVICE_FUNC inline Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::operator=(const RotationBase<Derived,Dim>& r) { linear() = internal::toRotationMatrix<Scalar,Dim>(r); translation().setZero(); @@ -991,7 +1035,7 @@ inline Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::o template<typename Scalar, int Dim, int Mode, int Options> template<typename Derived> -inline Transform<Scalar,Dim,Mode,Options> Transform<Scalar,Dim,Mode,Options>::operator*(const RotationBase<Derived,Dim>& r) const +EIGEN_DEVICE_FUNC inline Transform<Scalar,Dim,Mode,Options> Transform<Scalar,Dim,Mode,Options>::operator*(const RotationBase<Derived,Dim>& r) const { Transform res = *this; res.rotate(r.derived()); @@ -1010,7 +1054,7 @@ inline Transform<Scalar,Dim,Mode,Options> Transform<Scalar,Dim,Mode,Options>::op * \sa computeRotationScaling(), computeScalingRotation(), class SVD */ template<typename Scalar, int Dim, int Mode, int Options> -const typename Transform<Scalar,Dim,Mode,Options>::LinearMatrixType +EIGEN_DEVICE_FUNC const typename Transform<Scalar,Dim,Mode,Options>::LinearMatrixType Transform<Scalar,Dim,Mode,Options>::rotation() const { LinearMatrixType result; @@ -1032,7 +1076,7 @@ Transform<Scalar,Dim,Mode,Options>::rotation() const */ template<typename Scalar, int Dim, int Mode, int Options> template<typename RotationMatrixType, typename ScalingMatrixType> -void Transform<Scalar,Dim,Mode,Options>::computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const +EIGEN_DEVICE_FUNC void Transform<Scalar,Dim,Mode,Options>::computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const { JacobiSVD<LinearMatrixType> svd(linear(), ComputeFullU | ComputeFullV); @@ -1048,7 +1092,7 @@ void Transform<Scalar,Dim,Mode,Options>::computeRotationScaling(RotationMatrixTy } } -/** decomposes the linear part of the transformation as a product rotation x scaling, the scaling being +/** decomposes the linear part of the transformation as a product scaling x rotation, the scaling being * not necessarily positive. * * If either pointer is zero, the corresponding computation is skipped. @@ -1061,7 +1105,7 @@ void Transform<Scalar,Dim,Mode,Options>::computeRotationScaling(RotationMatrixTy */ template<typename Scalar, int Dim, int Mode, int Options> template<typename ScalingMatrixType, typename RotationMatrixType> -void Transform<Scalar,Dim,Mode,Options>::computeScalingRotation(ScalingMatrixType *scaling, RotationMatrixType *rotation) const +EIGEN_DEVICE_FUNC void Transform<Scalar,Dim,Mode,Options>::computeScalingRotation(ScalingMatrixType *scaling, RotationMatrixType *rotation) const { JacobiSVD<LinearMatrixType> svd(linear(), ComputeFullU | ComputeFullV); @@ -1082,7 +1126,7 @@ void Transform<Scalar,Dim,Mode,Options>::computeScalingRotation(ScalingMatrixTyp */ template<typename Scalar, int Dim, int Mode, int Options> template<typename PositionDerived, typename OrientationType, typename ScaleDerived> -Transform<Scalar,Dim,Mode,Options>& +EIGEN_DEVICE_FUNC Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::fromPositionOrientationScale(const MatrixBase<PositionDerived> &position, const OrientationType& orientation, const MatrixBase<ScaleDerived> &scale) { @@ -1099,7 +1143,7 @@ template<int Mode> struct transform_make_affine { template<typename MatrixType> - static void run(MatrixType &mat) + EIGEN_DEVICE_FUNC static void run(MatrixType &mat) { static const int Dim = MatrixType::ColsAtCompileTime-1; mat.template block<1,Dim>(Dim,0).setZero(); @@ -1110,21 +1154,21 @@ struct transform_make_affine template<> struct transform_make_affine<AffineCompact> { - template<typename MatrixType> static void run(MatrixType &) { } + template<typename MatrixType> EIGEN_DEVICE_FUNC static void run(MatrixType &) { } }; // selector needed to avoid taking the inverse of a 3x4 matrix template<typename TransformType, int Mode=TransformType::Mode> struct projective_transform_inverse { - static inline void run(const TransformType&, TransformType&) + EIGEN_DEVICE_FUNC static inline void run(const TransformType&, TransformType&) {} }; template<typename TransformType> struct projective_transform_inverse<TransformType, Projective> { - static inline void run(const TransformType& m, TransformType& res) + EIGEN_DEVICE_FUNC static inline void run(const TransformType& m, TransformType& res) { res.matrix() = m.matrix().inverse(); } @@ -1154,7 +1198,7 @@ struct projective_transform_inverse<TransformType, Projective> * \sa MatrixBase::inverse() */ template<typename Scalar, int Dim, int Mode, int Options> -Transform<Scalar,Dim,Mode,Options> +EIGEN_DEVICE_FUNC Transform<Scalar,Dim,Mode,Options> Transform<Scalar,Dim,Mode,Options>::inverse(TransformTraits hint) const { Transform res; @@ -1263,8 +1307,8 @@ struct transform_product_result }; }; -template< typename TransformType, typename MatrixType > -struct transform_right_product_impl< TransformType, MatrixType, 0 > +template< typename TransformType, typename MatrixType, int RhsCols> +struct transform_right_product_impl< TransformType, MatrixType, 0, RhsCols> { typedef typename MatrixType::PlainObject ResultType; @@ -1274,8 +1318,8 @@ struct transform_right_product_impl< TransformType, MatrixType, 0 > } }; -template< typename TransformType, typename MatrixType > -struct transform_right_product_impl< TransformType, MatrixType, 1 > +template< typename TransformType, typename MatrixType, int RhsCols> +struct transform_right_product_impl< TransformType, MatrixType, 1, RhsCols> { enum { Dim = TransformType::Dim, @@ -1300,8 +1344,8 @@ struct transform_right_product_impl< TransformType, MatrixType, 1 > } }; -template< typename TransformType, typename MatrixType > -struct transform_right_product_impl< TransformType, MatrixType, 2 > +template< typename TransformType, typename MatrixType, int RhsCols> +struct transform_right_product_impl< TransformType, MatrixType, 2, RhsCols> { enum { Dim = TransformType::Dim, @@ -1324,6 +1368,30 @@ struct transform_right_product_impl< TransformType, MatrixType, 2 > } }; +template< typename TransformType, typename MatrixType > +struct transform_right_product_impl< TransformType, MatrixType, 2, 1> // rhs is a vector of size Dim +{ + typedef typename TransformType::MatrixType TransformMatrix; + enum { + Dim = TransformType::Dim, + HDim = TransformType::HDim, + OtherRows = MatrixType::RowsAtCompileTime, + WorkingRows = EIGEN_PLAIN_ENUM_MIN(TransformMatrix::RowsAtCompileTime,HDim) + }; + + typedef typename MatrixType::PlainObject ResultType; + + static EIGEN_STRONG_INLINE ResultType run(const TransformType& T, const MatrixType& other) + { + EIGEN_STATIC_ASSERT(OtherRows==Dim, YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES); + + Matrix<typename ResultType::Scalar, Dim+1, 1> rhs; + rhs.template head<Dim>() = other; rhs[Dim] = typename ResultType::Scalar(1); + Matrix<typename ResultType::Scalar, WorkingRows, 1> res(T.matrix() * rhs); + return res.template head<Dim>(); + } +}; + /********************************************************** *** Specializations of operator* with lhs EigenBase *** **********************************************************/ diff --git a/eigen/Eigen/src/Geometry/Translation.h b/eigen/Eigen/src/Geometry/Translation.h index 2e77986..51d9a82 100644 --- a/eigen/Eigen/src/Geometry/Translation.h +++ b/eigen/Eigen/src/Geometry/Translation.h @@ -18,8 +18,8 @@ namespace Eigen { * * \brief Represents a translation transformation * - * \param _Scalar the scalar type, i.e., the type of the coefficients. - * \param _Dim the dimension of the space, can be a compile time value or Dynamic + * \tparam _Scalar the scalar type, i.e., the type of the coefficients. + * \tparam _Dim the dimension of the space, can be a compile time value or Dynamic * * \note This class is not aimed to be used to store a translation transformation, * but rather to make easier the constructions and updates of Transform objects. @@ -51,16 +51,16 @@ protected: public: /** Default constructor without initialization. */ - Translation() {} + EIGEN_DEVICE_FUNC Translation() {} /** */ - inline Translation(const Scalar& sx, const Scalar& sy) + EIGEN_DEVICE_FUNC inline Translation(const Scalar& sx, const Scalar& sy) { eigen_assert(Dim==2); m_coeffs.x() = sx; m_coeffs.y() = sy; } /** */ - inline Translation(const Scalar& sx, const Scalar& sy, const Scalar& sz) + EIGEN_DEVICE_FUNC inline Translation(const Scalar& sx, const Scalar& sy, const Scalar& sz) { eigen_assert(Dim==3); m_coeffs.x() = sx; @@ -68,48 +68,48 @@ public: m_coeffs.z() = sz; } /** Constructs and initialize the translation transformation from a vector of translation coefficients */ - explicit inline Translation(const VectorType& vector) : m_coeffs(vector) {} + EIGEN_DEVICE_FUNC explicit inline Translation(const VectorType& vector) : m_coeffs(vector) {} /** \brief Retruns the x-translation by value. **/ - inline Scalar x() const { return m_coeffs.x(); } + EIGEN_DEVICE_FUNC inline Scalar x() const { return m_coeffs.x(); } /** \brief Retruns the y-translation by value. **/ - inline Scalar y() const { return m_coeffs.y(); } + EIGEN_DEVICE_FUNC inline Scalar y() const { return m_coeffs.y(); } /** \brief Retruns the z-translation by value. **/ - inline Scalar z() const { return m_coeffs.z(); } + EIGEN_DEVICE_FUNC inline Scalar z() const { return m_coeffs.z(); } /** \brief Retruns the x-translation as a reference. **/ - inline Scalar& x() { return m_coeffs.x(); } + EIGEN_DEVICE_FUNC inline Scalar& x() { return m_coeffs.x(); } /** \brief Retruns the y-translation as a reference. **/ - inline Scalar& y() { return m_coeffs.y(); } + EIGEN_DEVICE_FUNC inline Scalar& y() { return m_coeffs.y(); } /** \brief Retruns the z-translation as a reference. **/ - inline Scalar& z() { return m_coeffs.z(); } + EIGEN_DEVICE_FUNC inline Scalar& z() { return m_coeffs.z(); } - const VectorType& vector() const { return m_coeffs; } - VectorType& vector() { return m_coeffs; } + EIGEN_DEVICE_FUNC const VectorType& vector() const { return m_coeffs; } + EIGEN_DEVICE_FUNC VectorType& vector() { return m_coeffs; } - const VectorType& translation() const { return m_coeffs; } - VectorType& translation() { return m_coeffs; } + EIGEN_DEVICE_FUNC const VectorType& translation() const { return m_coeffs; } + EIGEN_DEVICE_FUNC VectorType& translation() { return m_coeffs; } /** Concatenates two translation */ - inline Translation operator* (const Translation& other) const + EIGEN_DEVICE_FUNC inline Translation operator* (const Translation& other) const { return Translation(m_coeffs + other.m_coeffs); } /** Concatenates a translation and a uniform scaling */ - inline AffineTransformType operator* (const UniformScaling<Scalar>& other) const; + EIGEN_DEVICE_FUNC inline AffineTransformType operator* (const UniformScaling<Scalar>& other) const; /** Concatenates a translation and a linear transformation */ template<typename OtherDerived> - inline AffineTransformType operator* (const EigenBase<OtherDerived>& linear) const; + EIGEN_DEVICE_FUNC inline AffineTransformType operator* (const EigenBase<OtherDerived>& linear) const; /** Concatenates a translation and a rotation */ template<typename Derived> - inline IsometryTransformType operator*(const RotationBase<Derived,Dim>& r) const + EIGEN_DEVICE_FUNC inline IsometryTransformType operator*(const RotationBase<Derived,Dim>& r) const { return *this * IsometryTransformType(r); } /** \returns the concatenation of a linear transformation \a l with the translation \a t */ // its a nightmare to define a templated friend function outside its declaration template<typename OtherDerived> friend - inline AffineTransformType operator*(const EigenBase<OtherDerived>& linear, const Translation& t) + EIGEN_DEVICE_FUNC inline AffineTransformType operator*(const EigenBase<OtherDerived>& linear, const Translation& t) { AffineTransformType res; res.matrix().setZero(); @@ -122,7 +122,7 @@ public: /** Concatenates a translation and a transformation */ template<int Mode, int Options> - inline Transform<Scalar,Dim,Mode> operator* (const Transform<Scalar,Dim,Mode,Options>& t) const + EIGEN_DEVICE_FUNC inline Transform<Scalar,Dim,Mode> operator* (const Transform<Scalar,Dim,Mode,Options>& t) const { Transform<Scalar,Dim,Mode> res = t; res.pretranslate(m_coeffs); @@ -130,8 +130,10 @@ public: } /** Applies translation to vector */ - inline VectorType operator* (const VectorType& other) const - { return m_coeffs + other; } + template<typename Derived> + inline typename internal::enable_if<Derived::IsVectorAtCompileTime,VectorType>::type + operator* (const MatrixBase<Derived>& vec) const + { return m_coeffs + vec.derived(); } /** \returns the inverse translation (opposite) */ Translation inverse() const { return Translation(-m_coeffs); } @@ -150,19 +152,19 @@ public: * then this function smartly returns a const reference to \c *this. */ template<typename NewScalarType> - inline typename internal::cast_return_type<Translation,Translation<NewScalarType,Dim> >::type cast() const + EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<Translation,Translation<NewScalarType,Dim> >::type cast() const { return typename internal::cast_return_type<Translation,Translation<NewScalarType,Dim> >::type(*this); } /** Copy constructor with scalar type conversion */ template<typename OtherScalarType> - inline explicit Translation(const Translation<OtherScalarType,Dim>& other) + EIGEN_DEVICE_FUNC inline explicit Translation(const Translation<OtherScalarType,Dim>& other) { m_coeffs = other.vector().template cast<Scalar>(); } /** \returns \c true if \c *this is approximately equal to \a other, within the precision * determined by \a prec. * * \sa MatrixBase::isApprox() */ - bool isApprox(const Translation& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const + EIGEN_DEVICE_FUNC bool isApprox(const Translation& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const { return m_coeffs.isApprox(other.m_coeffs, prec); } }; @@ -176,7 +178,7 @@ typedef Translation<double,3> Translation3d; //@} template<typename Scalar, int Dim> -inline typename Translation<Scalar,Dim>::AffineTransformType +EIGEN_DEVICE_FUNC inline typename Translation<Scalar,Dim>::AffineTransformType Translation<Scalar,Dim>::operator* (const UniformScaling<Scalar>& other) const { AffineTransformType res; @@ -189,7 +191,7 @@ Translation<Scalar,Dim>::operator* (const UniformScaling<Scalar>& other) const template<typename Scalar, int Dim> template<typename OtherDerived> -inline typename Translation<Scalar,Dim>::AffineTransformType +EIGEN_DEVICE_FUNC inline typename Translation<Scalar,Dim>::AffineTransformType Translation<Scalar,Dim>::operator* (const EigenBase<OtherDerived>& linear) const { AffineTransformType res; diff --git a/eigen/Eigen/src/Geometry/Umeyama.h b/eigen/Eigen/src/Geometry/Umeyama.h index 5e20662..7e933fc 100644 --- a/eigen/Eigen/src/Geometry/Umeyama.h +++ b/eigen/Eigen/src/Geometry/Umeyama.h @@ -97,7 +97,6 @@ umeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, boo typedef typename internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type TransformationMatrixType; typedef typename internal::traits<TransformationMatrixType>::Scalar Scalar; typedef typename NumTraits<Scalar>::Real RealScalar; - typedef typename Derived::Index Index; EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsComplex, NUMERIC_TYPE_MUST_BE_REAL) EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename internal::traits<OtherDerived>::Scalar>::value), @@ -136,22 +135,12 @@ umeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, boo // Eq. (39) VectorType S = VectorType::Ones(m); - if (sigma.determinant()<Scalar(0)) S(m-1) = Scalar(-1); + + if ( svd.matrixU().determinant() * svd.matrixV().determinant() < 0 ) + S(m-1) = -1; // Eq. (40) and (43) - const VectorType& d = svd.singularValues(); - Index rank = 0; for (Index i=0; i<m; ++i) if (!internal::isMuchSmallerThan(d.coeff(i),d.coeff(0))) ++rank; - if (rank == m-1) { - if ( svd.matrixU().determinant() * svd.matrixV().determinant() > Scalar(0) ) { - Rt.block(0,0,m,m).noalias() = svd.matrixU()*svd.matrixV().transpose(); - } else { - const Scalar s = S(m-1); S(m-1) = Scalar(-1); - Rt.block(0,0,m,m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose(); - S(m-1) = s; - } - } else { - Rt.block(0,0,m,m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose(); - } + Rt.block(0,0,m,m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose(); if (with_scaling) { diff --git a/eigen/Eigen/src/Geometry/arch/CMakeLists.txt b/eigen/Eigen/src/Geometry/arch/CMakeLists.txt deleted file mode 100644 index 1267a79..0000000 --- a/eigen/Eigen/src/Geometry/arch/CMakeLists.txt +++ /dev/null @@ -1,6 +0,0 @@ -FILE(GLOB Eigen_Geometry_arch_SRCS "*.h") - -INSTALL(FILES - ${Eigen_Geometry_arch_SRCS} - DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/Geometry/arch COMPONENT Devel - ) diff --git a/eigen/Eigen/src/Geometry/arch/Geometry_SSE.h b/eigen/Eigen/src/Geometry/arch/Geometry_SSE.h index 3d8284f..1a86ff8 100644 --- a/eigen/Eigen/src/Geometry/arch/Geometry_SSE.h +++ b/eigen/Eigen/src/Geometry/arch/Geometry_SSE.h @@ -16,35 +16,47 @@ namespace Eigen { namespace internal { template<class Derived, class OtherDerived> -struct quat_product<Architecture::SSE, Derived, OtherDerived, float, Aligned> +struct quat_product<Architecture::SSE, Derived, OtherDerived, float, Aligned16> { static inline Quaternion<float> run(const QuaternionBase<Derived>& _a, const QuaternionBase<OtherDerived>& _b) { - const __m128 mask = _mm_castsi128_ps(_mm_setr_epi32(0,0,0,0x80000000)); Quaternion<float> res; - __m128 a = _a.coeffs().template packet<Aligned>(0); - __m128 b = _b.coeffs().template packet<Aligned>(0); - __m128 flip1 = _mm_xor_ps(_mm_mul_ps(vec4f_swizzle1(a,1,2,0,2), - vec4f_swizzle1(b,2,0,1,2)),mask); - __m128 flip2 = _mm_xor_ps(_mm_mul_ps(vec4f_swizzle1(a,3,3,3,1), - vec4f_swizzle1(b,0,1,2,1)),mask); + const __m128 mask = _mm_setr_ps(0.f,0.f,0.f,-0.f); + __m128 a = _a.coeffs().template packet<Aligned16>(0); + __m128 b = _b.coeffs().template packet<Aligned16>(0); + __m128 s1 = _mm_mul_ps(vec4f_swizzle1(a,1,2,0,2),vec4f_swizzle1(b,2,0,1,2)); + __m128 s2 = _mm_mul_ps(vec4f_swizzle1(a,3,3,3,1),vec4f_swizzle1(b,0,1,2,1)); pstore(&res.x(), _mm_add_ps(_mm_sub_ps(_mm_mul_ps(a,vec4f_swizzle1(b,3,3,3,3)), _mm_mul_ps(vec4f_swizzle1(a,2,0,1,0), vec4f_swizzle1(b,1,2,0,0))), - _mm_add_ps(flip1,flip2))); + _mm_xor_ps(mask,_mm_add_ps(s1,s2)))); + return res; } }; +template<class Derived, int Alignment> +struct quat_conj<Architecture::SSE, Derived, float, Alignment> +{ + static inline Quaternion<float> run(const QuaternionBase<Derived>& q) + { + Quaternion<float> res; + const __m128 mask = _mm_setr_ps(-0.f,-0.f,-0.f,0.f); + pstore(&res.x(), _mm_xor_ps(mask, q.coeffs().template packet<Alignment>(0))); + return res; + } +}; + + template<typename VectorLhs,typename VectorRhs> struct cross3_impl<Architecture::SSE,VectorLhs,VectorRhs,float,true> { static inline typename plain_matrix_type<VectorLhs>::type run(const VectorLhs& lhs, const VectorRhs& rhs) { - __m128 a = lhs.template packet<VectorLhs::Flags&AlignedBit ? Aligned : Unaligned>(0); - __m128 b = rhs.template packet<VectorRhs::Flags&AlignedBit ? Aligned : Unaligned>(0); + __m128 a = lhs.template packet<traits<VectorLhs>::Alignment>(0); + __m128 b = rhs.template packet<traits<VectorRhs>::Alignment>(0); __m128 mul1=_mm_mul_ps(vec4f_swizzle1(a,1,2,0,3),vec4f_swizzle1(b,2,0,1,3)); __m128 mul2=_mm_mul_ps(vec4f_swizzle1(a,2,0,1,3),vec4f_swizzle1(b,1,2,0,3)); typename plain_matrix_type<VectorLhs>::type res; @@ -56,8 +68,8 @@ struct cross3_impl<Architecture::SSE,VectorLhs,VectorRhs,float,true> -template<class Derived, class OtherDerived> -struct quat_product<Architecture::SSE, Derived, OtherDerived, double, Aligned> +template<class Derived, class OtherDerived, int Alignment> +struct quat_product<Architecture::SSE, Derived, OtherDerived, double, Alignment> { static inline Quaternion<double> run(const QuaternionBase<Derived>& _a, const QuaternionBase<OtherDerived>& _b) { @@ -66,8 +78,8 @@ struct quat_product<Architecture::SSE, Derived, OtherDerived, double, Aligned> Quaternion<double> res; const double* a = _a.coeffs().data(); - Packet2d b_xy = _b.coeffs().template packet<Aligned>(0); - Packet2d b_zw = _b.coeffs().template packet<Aligned>(2); + Packet2d b_xy = _b.coeffs().template packet<Alignment>(0); + Packet2d b_zw = _b.coeffs().template packet<Alignment>(2); Packet2d a_xx = pset1<Packet2d>(a[0]); Packet2d a_yy = pset1<Packet2d>(a[1]); Packet2d a_zz = pset1<Packet2d>(a[2]); @@ -108,6 +120,20 @@ struct quat_product<Architecture::SSE, Derived, OtherDerived, double, Aligned> } }; +template<class Derived, int Alignment> +struct quat_conj<Architecture::SSE, Derived, double, Alignment> +{ + static inline Quaternion<double> run(const QuaternionBase<Derived>& q) + { + Quaternion<double> res; + const __m128d mask0 = _mm_setr_pd(-0.,-0.); + const __m128d mask2 = _mm_setr_pd(-0.,0.); + pstore(&res.x(), _mm_xor_pd(mask0, q.coeffs().template packet<Alignment>(0))); + pstore(&res.z(), _mm_xor_pd(mask2, q.coeffs().template packet<Alignment>(2))); + return res; + } +}; + } // end namespace internal } // end namespace Eigen |