diff options
author | Stanislaw Halik <sthalik@misaki.pl> | 2019-03-03 21:09:10 +0100 |
---|---|---|
committer | Stanislaw Halik <sthalik@misaki.pl> | 2019-03-03 21:10:13 +0100 |
commit | f0238cfb6997c4acfc2bd200de7295f3fa36968f (patch) | |
tree | b215183760e4f615b9c1dabc1f116383b72a1b55 /eigen/Eigen/src/Geometry | |
parent | 543edd372a5193d04b3de9f23c176ab439e51b31 (diff) |
don't index Eigen
Diffstat (limited to 'eigen/Eigen/src/Geometry')
-rw-r--r-- | eigen/Eigen/src/Geometry/AlignedBox.h | 392 | ||||
-rw-r--r-- | eigen/Eigen/src/Geometry/AngleAxis.h | 247 | ||||
-rw-r--r-- | eigen/Eigen/src/Geometry/EulerAngles.h | 114 | ||||
-rw-r--r-- | eigen/Eigen/src/Geometry/Homogeneous.h | 497 | ||||
-rw-r--r-- | eigen/Eigen/src/Geometry/Hyperplane.h | 282 | ||||
-rw-r--r-- | eigen/Eigen/src/Geometry/OrthoMethods.h | 234 | ||||
-rw-r--r-- | eigen/Eigen/src/Geometry/ParametrizedLine.h | 195 | ||||
-rw-r--r-- | eigen/Eigen/src/Geometry/Quaternion.h | 814 | ||||
-rw-r--r-- | eigen/Eigen/src/Geometry/Rotation2D.h | 199 | ||||
-rw-r--r-- | eigen/Eigen/src/Geometry/RotationBase.h | 206 | ||||
-rw-r--r-- | eigen/Eigen/src/Geometry/Scaling.h | 170 | ||||
-rw-r--r-- | eigen/Eigen/src/Geometry/Transform.h | 1542 | ||||
-rw-r--r-- | eigen/Eigen/src/Geometry/Translation.h | 208 | ||||
-rw-r--r-- | eigen/Eigen/src/Geometry/Umeyama.h | 166 | ||||
-rw-r--r-- | eigen/Eigen/src/Geometry/arch/Geometry_SSE.h | 161 |
15 files changed, 0 insertions, 5427 deletions
diff --git a/eigen/Eigen/src/Geometry/AlignedBox.h b/eigen/Eigen/src/Geometry/AlignedBox.h deleted file mode 100644 index 066eae4..0000000 --- a/eigen/Eigen/src/Geometry/AlignedBox.h +++ /dev/null @@ -1,392 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr> -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_ALIGNEDBOX_H -#define EIGEN_ALIGNEDBOX_H - -namespace Eigen { - -/** \geometry_module \ingroup Geometry_Module - * - * - * \class AlignedBox - * - * \brief An axis aligned box - * - * \tparam _Scalar the type of the scalar coefficients - * \tparam _AmbientDim the dimension of the ambient space, can be a compile time value or Dynamic. - * - * This class represents an axis aligned box as a pair of the minimal and maximal corners. - * \warning The result of most methods is undefined when applied to an empty box. You can check for empty boxes using isEmpty(). - * \sa alignedboxtypedefs - */ -template <typename _Scalar, int _AmbientDim> -class AlignedBox -{ -public: -EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) - enum { AmbientDimAtCompileTime = _AmbientDim }; - typedef _Scalar Scalar; - typedef NumTraits<Scalar> ScalarTraits; - typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 - typedef typename ScalarTraits::Real RealScalar; - 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 - { - /** 1D names @{ */ - Min=0, Max=1, - /** @} */ - - /** Identifier for 2D corner @{ */ - BottomLeft=0, BottomRight=1, - TopLeft=2, TopRight=3, - /** @} */ - - /** Identifier for 3D corner @{ */ - BottomLeftFloor=0, BottomRightFloor=1, - TopLeftFloor=2, TopRightFloor=3, - BottomLeftCeil=4, BottomRightCeil=5, - TopLeftCeil=6, TopRightCeil=7 - /** @} */ - }; - - - /** Default constructor initializing a null box. */ - EIGEN_DEVICE_FUNC inline AlignedBox() - { if (AmbientDimAtCompileTime!=Dynamic) setEmpty(); } - - /** Constructs a null box with \a _dim the dimension of the ambient space. */ - 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> - 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> - EIGEN_DEVICE_FUNC inline explicit AlignedBox(const MatrixBase<Derived>& p) : m_min(p), m_max(m_min) - { } - - EIGEN_DEVICE_FUNC ~AlignedBox() {} - - /** \returns the dimension in which the box holds */ - EIGEN_DEVICE_FUNC inline Index dim() const { return AmbientDimAtCompileTime==Dynamic ? m_min.size() : Index(AmbientDimAtCompileTime); } - - /** \deprecated use isEmpty() */ - EIGEN_DEVICE_FUNC inline bool isNull() const { return isEmpty(); } - - /** \deprecated use setEmpty() */ - EIGEN_DEVICE_FUNC inline void setNull() { setEmpty(); } - - /** \returns true if the box is empty. - * \sa setEmpty */ - EIGEN_DEVICE_FUNC inline bool isEmpty() const { return (m_min.array() > m_max.array()).any(); } - - /** Makes \c *this an empty box. - * \sa isEmpty */ - EIGEN_DEVICE_FUNC inline void setEmpty() - { - m_min.setConstant( ScalarTraits::highest() ); - m_max.setConstant( ScalarTraits::lowest() ); - } - - /** \returns the minimal corner */ - EIGEN_DEVICE_FUNC inline const VectorType& (min)() const { return m_min; } - /** \returns a non const reference to the minimal corner */ - EIGEN_DEVICE_FUNC inline VectorType& (min)() { return m_min; } - /** \returns the maximal corner */ - EIGEN_DEVICE_FUNC inline const VectorType& (max)() const { return m_max; } - /** \returns a non const reference to the maximal corner */ - EIGEN_DEVICE_FUNC inline VectorType& (max)() { return m_max; } - - /** \returns the center of the box */ - EIGEN_DEVICE_FUNC inline const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(VectorTypeSum, RealScalar, quotient) - center() const - { 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 - */ - 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 */ - 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. - */ - 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 - * the corner-id corner. It works only for a 1D, 2D or 3D bounding box. - * For 1D bounding boxes corners are named by 2 enum constants: - * BottomLeft and BottomRight. - * For 2D bounding boxes, corners are named by 4 enum constants: - * BottomLeft, BottomRight, TopLeft, TopRight. - * For 3D bounding boxes, the following names are added: - * BottomLeftCeil, BottomRightCeil, TopLeftCeil, TopRightCeil. - */ - EIGEN_DEVICE_FUNC inline VectorType corner(CornerType corner) const - { - EIGEN_STATIC_ASSERT(_AmbientDim <= 3, THIS_METHOD_IS_ONLY_FOR_VECTORS_OF_A_SPECIFIC_SIZE); - - VectorType res; - - Index mult = 1; - for(Index d=0; d<dim(); ++d) - { - if( mult & corner ) res[d] = m_max[d]; - else res[d] = m_min[d]; - mult *= 2; - } - return res; - } - - /** \returns a random point inside the bounding box sampled with - * a uniform distribution */ - EIGEN_DEVICE_FUNC inline VectorType sample() const - { - VectorType r(dim()); - for(Index d=0; d<dim(); ++d) - { - if(!ScalarTraits::IsInteger) - { - r[d] = m_min[d] + (m_max[d]-m_min[d]) - * internal::random<Scalar>(Scalar(0), Scalar(1)); - } - else - r[d] = internal::random(m_min[d], m_max[d]); - } - return r; - } - - /** \returns true if the point \a p is inside the box \c *this. */ - template<typename Derived> - EIGEN_DEVICE_FUNC inline bool contains(const MatrixBase<Derived>& p) const - { - 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. */ - 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 */ - 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> - EIGEN_DEVICE_FUNC inline AlignedBox& extend(const MatrixBase<Derived>& p) - { - 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; - } - - /** Extends \c *this such that it contains the box \a b and returns a reference to \c *this. - * \sa merged, extend(const MatrixBase&) */ - 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); - return *this; - } - - /** 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() */ - 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); - return *this; - } - - /** 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() */ - 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&) */ - 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> - EIGEN_DEVICE_FUNC inline AlignedBox& translate(const MatrixBase<Derived>& a_t) - { - const typename internal::nested_eval<Derived,2>::type t(a_t.derived()); - m_min += t; - m_max += t; - return *this; - } - - /** \returns the squared distance between the point \a p and the box \c *this, - * and zero if \a p is inside the box. - * \sa exteriorDistance(const MatrixBase&), squaredExteriorDistance(const AlignedBox&) - */ - template<typename Derived> - 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&) - */ - 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> - 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&) - */ - 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 - * - * 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> - EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<AlignedBox, - AlignedBox<NewScalarType,AmbientDimAtCompileTime> >::type cast() const - { - return typename internal::cast_return_type<AlignedBox, - AlignedBox<NewScalarType,AmbientDimAtCompileTime> >::type(*this); - } - - /** Copy constructor with scalar type conversion */ - template<typename OtherScalarType> - 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>(); - } - - /** \returns \c true if \c *this is approximately equal to \a other, within the precision - * determined by \a prec. - * - * \sa MatrixBase::isApprox() */ - 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: - - VectorType m_min, m_max; -}; - - - -template<typename Scalar,int AmbientDim> -template<typename Derived> -EIGEN_DEVICE_FUNC inline Scalar AlignedBox<Scalar,AmbientDim>::squaredExteriorDistance(const MatrixBase<Derived>& a_p) const -{ - typename internal::nested_eval<Derived,2*AmbientDim>::type p(a_p.derived()); - Scalar dist2(0); - Scalar aux; - for (Index k=0; k<dim(); ++k) - { - if( m_min[k] > p[k] ) - { - aux = m_min[k] - p[k]; - dist2 += aux*aux; - } - else if( p[k] > m_max[k] ) - { - aux = p[k] - m_max[k]; - dist2 += aux*aux; - } - } - return dist2; -} - -template<typename Scalar,int AmbientDim> -EIGEN_DEVICE_FUNC inline Scalar AlignedBox<Scalar,AmbientDim>::squaredExteriorDistance(const AlignedBox& b) const -{ - Scalar dist2(0); - Scalar aux; - for (Index k=0; k<dim(); ++k) - { - if( m_min[k] > b.m_max[k] ) - { - aux = m_min[k] - b.m_max[k]; - dist2 += aux*aux; - } - else if( b.m_min[k] > m_max[k] ) - { - aux = b.m_min[k] - m_max[k]; - dist2 += aux*aux; - } - } - return dist2; -} - -/** \defgroup alignedboxtypedefs Global aligned box typedefs - * - * \ingroup Geometry_Module - * - * Eigen defines several typedef shortcuts for most common aligned box types. - * - * The general patterns are the following: - * - * \c AlignedBoxSizeType where \c Size can be \c 1, \c 2,\c 3,\c 4 for fixed size boxes or \c X for dynamic size, - * and where \c Type can be \c i for integer, \c f for float, \c d for double. - * - * For example, \c AlignedBox3d is a fixed-size 3x3 aligned box type of doubles, and \c AlignedBoxXf is a dynamic-size aligned box of floats. - * - * \sa class AlignedBox - */ - -#define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix) \ -/** \ingroup alignedboxtypedefs */ \ -typedef AlignedBox<Type, Size> AlignedBox##SizeSuffix##TypeSuffix; - -#define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \ -EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 1, 1) \ -EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 2, 2) \ -EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 3, 3) \ -EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 4, 4) \ -EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Dynamic, X) - -EIGEN_MAKE_TYPEDEFS_ALL_SIZES(int, i) -EIGEN_MAKE_TYPEDEFS_ALL_SIZES(float, f) -EIGEN_MAKE_TYPEDEFS_ALL_SIZES(double, d) - -#undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES -#undef EIGEN_MAKE_TYPEDEFS - -} // end namespace Eigen - -#endif // EIGEN_ALIGNEDBOX_H diff --git a/eigen/Eigen/src/Geometry/AngleAxis.h b/eigen/Eigen/src/Geometry/AngleAxis.h deleted file mode 100644 index 83ee1be..0000000 --- a/eigen/Eigen/src/Geometry/AngleAxis.h +++ /dev/null @@ -1,247 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr> -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_ANGLEAXIS_H -#define EIGEN_ANGLEAXIS_H - -namespace Eigen { - -/** \geometry_module \ingroup Geometry_Module - * - * \class AngleAxis - * - * \brief Represents a 3D rotation as a rotation angle around an arbitrary 3D axis - * - * \param _Scalar the scalar type, i.e., the type of the coefficients. - * - * \warning When setting up an AngleAxis object, the axis vector \b must \b be \b normalized. - * - * The following two typedefs are provided for convenience: - * \li \c AngleAxisf for \c float - * \li \c AngleAxisd for \c double - * - * Combined with MatrixBase::Unit{X,Y,Z}, AngleAxis can be used to easily - * mimic Euler-angles. Here is an example: - * \include AngleAxis_mimic_euler.cpp - * Output: \verbinclude AngleAxis_mimic_euler.out - * - * \note This class is not aimed to be used to store a rotation transformation, - * but rather to make easier the creation of other rotation (Quaternion, rotation Matrix) - * and transformation objects. - * - * \sa class Quaternion, class Transform, MatrixBase::UnitX() - */ - -namespace internal { -template<typename _Scalar> struct traits<AngleAxis<_Scalar> > -{ - typedef _Scalar Scalar; -}; -} - -template<typename _Scalar> -class AngleAxis : public RotationBase<AngleAxis<_Scalar>,3> -{ - typedef RotationBase<AngleAxis<_Scalar>,3> Base; - -public: - - using Base::operator*; - - enum { Dim = 3 }; - /** the scalar type of the coefficients */ - typedef _Scalar Scalar; - typedef Matrix<Scalar,3,3> Matrix3; - typedef Matrix<Scalar,3,1> Vector3; - typedef Quaternion<Scalar> QuaternionType; - -protected: - - Vector3 m_axis; - Scalar m_angle; - -public: - - /** Default constructor without initialization. */ - 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. - * 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> - EIGEN_DEVICE_FUNC inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; } - - /** \returns the value of the rotation angle in radian */ - EIGEN_DEVICE_FUNC Scalar angle() const { return m_angle; } - /** \returns a read-write reference to the stored angle in radian */ - EIGEN_DEVICE_FUNC Scalar& angle() { return m_angle; } - - /** \returns the rotation 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. - */ - EIGEN_DEVICE_FUNC Vector3& axis() { return m_axis; } - - /** Concatenates two rotations */ - EIGEN_DEVICE_FUNC inline QuaternionType operator* (const AngleAxis& other) const - { return QuaternionType(*this) * QuaternionType(other); } - - /** Concatenates two rotations */ - EIGEN_DEVICE_FUNC inline QuaternionType operator* (const QuaternionType& other) const - { return QuaternionType(*this) * other; } - - /** Concatenates two rotations */ - 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 */ - EIGEN_DEVICE_FUNC AngleAxis inverse() const - { return AngleAxis(-m_angle, m_axis); } - - template<class QuatDerived> - EIGEN_DEVICE_FUNC AngleAxis& operator=(const QuaternionBase<QuatDerived>& q); - template<typename Derived> - EIGEN_DEVICE_FUNC AngleAxis& operator=(const MatrixBase<Derived>& m); - - template<typename Derived> - 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 - * - * 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> - 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> - EIGEN_DEVICE_FUNC inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other) - { - m_axis = other.axis().template cast<Scalar>(); - m_angle = Scalar(other.angle()); - } - - 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() */ - 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); } -}; - -/** \ingroup Geometry_Module - * single precision angle-axis type */ -typedef AngleAxis<float> AngleAxisf; -/** \ingroup Geometry_Module - * double precision angle-axis type */ -typedef AngleAxis<double> AngleAxisd; - -/** Set \c *this from a \b unit quaternion. - * - * The resulting axis is normalized, and the computed angle is in the [0,pi] range. - * - * This function implicitly normalizes the quaternion \a q. - */ -template<typename Scalar> -template<typename QuatDerived> -EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived>& q) -{ - 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(2)*atan2(n, abs(q.w())); - if(q.w() < Scalar(0)) - n = -n; - m_axis = q.vec() / n; - } - else - { - m_angle = Scalar(0); - m_axis << Scalar(1), Scalar(0), Scalar(0); - } - return *this; -} - -/** Set \c *this from a 3x3 rotation matrix \a mat. - */ -template<typename Scalar> -template<typename Derived> -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: - return *this = QuaternionType(mat); -} - -/** -* \brief Sets \c *this from a 3x3 rotation matrix. -**/ -template<typename Scalar> -template<typename Derived> -EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat) -{ - return *this = QuaternionType(mat); -} - -/** Constructs and \returns an equivalent 3x3 rotation matrix. - */ -template<typename Scalar> -typename AngleAxis<Scalar>::Matrix3 -EIGEN_DEVICE_FUNC AngleAxis<Scalar>::toRotationMatrix(void) const -{ - 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); - Vector3 cos1_axis = (Scalar(1)-c) * m_axis; - - Scalar tmp; - tmp = cos1_axis.x() * m_axis.y(); - res.coeffRef(0,1) = tmp - sin_axis.z(); - res.coeffRef(1,0) = tmp + sin_axis.z(); - - tmp = cos1_axis.x() * m_axis.z(); - res.coeffRef(0,2) = tmp + sin_axis.y(); - res.coeffRef(2,0) = tmp - sin_axis.y(); - - tmp = cos1_axis.y() * m_axis.z(); - res.coeffRef(1,2) = tmp - sin_axis.x(); - res.coeffRef(2,1) = tmp + sin_axis.x(); - - res.diagonal() = (cos1_axis.cwiseProduct(m_axis)).array() + c; - - return res; -} - -} // end namespace Eigen - -#endif // EIGEN_ANGLEAXIS_H diff --git a/eigen/Eigen/src/Geometry/EulerAngles.h b/eigen/Eigen/src/Geometry/EulerAngles.h deleted file mode 100644 index c633268..0000000 --- a/eigen/Eigen/src/Geometry/EulerAngles.h +++ /dev/null @@ -1,114 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr> -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_EULERANGLES_H -#define EIGEN_EULERANGLES_H - -namespace Eigen { - -/** \geometry_module \ingroup Geometry_Module - * - * - * \returns the Euler-angles of the rotation matrix \c *this using the convention defined by the triplet (\a a0,\a a1,\a a2) - * - * Each of the three parameters \a a0,\a a1,\a a2 represents the respective rotation axis as an integer in {0,1,2}. - * For instance, in: - * \code Vector3f ea = mat.eulerAngles(2, 0, 2); \endcode - * "2" represents the z axis and "0" the x axis, etc. The returned angles are such that - * we have the following equality: - * \code - * mat == AngleAxisf(ea[0], Vector3f::UnitZ()) - * * AngleAxisf(ea[1], Vector3f::UnitX()) - * * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode - * This corresponds to the right-multiply conventions (with right hand side frames). - * - * The returned angles are in the ranges [0:pi]x[-pi:pi]x[-pi:pi]. - * - * \sa class AngleAxis - */ -template<typename Derived> -EIGEN_DEVICE_FUNC inline Matrix<typename MatrixBase<Derived>::Scalar,3,1> -MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const -{ - 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) - - Matrix<Scalar,3,1> res; - typedef Matrix<typename Derived::Scalar,2,1> Vector2; - - const Index odd = ((a0+1)%3 == a1) ? 0 : 1; - const Index i = a0; - const Index j = (a0 + 1 + odd)%3; - const Index k = (a0 + 2 - odd)%3; - - if (a0==a2) - { - res[0] = atan2(coeff(j,i), coeff(k,i)); - if((odd && res[0]<Scalar(0)) || ((!odd) && res[0]>Scalar(0))) - { - 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)); - } - else - { - Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm(); - res[1] = atan2(s2, coeff(i,i)); - } - - // With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first two angles, - // we can compute their respective rotation, and apply its inverse to M. Since the result must - // be a rotation around x, we have: - // - // c2 s1.s2 c1.s2 1 0 0 - // 0 c1 -s1 * M = 0 c3 s3 - // -s2 s1.c2 c1.c2 0 -s3 c3 - // - // Thus: m11.c1 - m21.s1 = c3 & m12.c1 - m22.s1 = s3 - - Scalar s1 = sin(res[0]); - Scalar c1 = cos(res[0]); - res[2] = atan2(c1*coeff(j,k)-s1*coeff(k,k), c1*coeff(j,j) - s1 * coeff(k,j)); - } - else - { - 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))) { - if(res[0] > Scalar(0)) { - res[0] -= Scalar(EIGEN_PI); - } - else { - res[0] += Scalar(EIGEN_PI); - } - res[1] = atan2(-coeff(i,k), -c2); - } - else - res[1] = atan2(-coeff(i,k), c2); - Scalar s1 = sin(res[0]); - Scalar c1 = cos(res[0]); - res[2] = atan2(s1*coeff(k,i)-c1*coeff(j,i), c1*coeff(j,j) - s1 * coeff(k,j)); - } - if (!odd) - res = -res; - - return res; -} - -} // end namespace Eigen - -#endif // EIGEN_EULERANGLES_H diff --git a/eigen/Eigen/src/Geometry/Homogeneous.h b/eigen/Eigen/src/Geometry/Homogeneous.h deleted file mode 100644 index 5f0da1a..0000000 --- a/eigen/Eigen/src/Geometry/Homogeneous.h +++ /dev/null @@ -1,497 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2009-2010 Gael Guennebaud <gael.guennebaud@inria.fr> -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_HOMOGENEOUS_H -#define EIGEN_HOMOGENEOUS_H - -namespace Eigen { - -/** \geometry_module \ingroup Geometry_Module - * - * \class Homogeneous - * - * \brief Expression of one (or a set of) homogeneous vector(s) - * - * \param MatrixType the type of the object in which we are making homogeneous - * - * This class represents an expression of one (or a set of) homogeneous vector(s). - * It is the return type of MatrixBase::homogeneous() and most of the time - * this is the only way it is used. - * - * \sa MatrixBase::homogeneous() - */ - -namespace internal { - -template<typename MatrixType,int Direction> -struct traits<Homogeneous<MatrixType,Direction> > - : traits<MatrixType> -{ - typedef typename traits<MatrixType>::StorageKind StorageKind; - typedef typename ref_selector<MatrixType>::type MatrixTypeNested; - typedef typename remove_reference<MatrixTypeNested>::type _MatrixTypeNested; - enum { - RowsPlusOne = (MatrixType::RowsAtCompileTime != Dynamic) ? - int(MatrixType::RowsAtCompileTime) + 1 : Dynamic, - ColsPlusOne = (MatrixType::ColsAtCompileTime != Dynamic) ? - int(MatrixType::ColsAtCompileTime) + 1 : Dynamic, - RowsAtCompileTime = Direction==Vertical ? RowsPlusOne : MatrixType::RowsAtCompileTime, - ColsAtCompileTime = Direction==Horizontal ? ColsPlusOne : MatrixType::ColsAtCompileTime, - MaxRowsAtCompileTime = RowsAtCompileTime, - MaxColsAtCompileTime = ColsAtCompileTime, - TmpFlags = _MatrixTypeNested::Flags & HereditaryBits, - Flags = ColsAtCompileTime==1 ? (TmpFlags & ~RowMajorBit) - : RowsAtCompileTime==1 ? (TmpFlags | RowMajorBit) - : TmpFlags - }; -}; - -template<typename MatrixType,typename Lhs> struct homogeneous_left_product_impl; -template<typename MatrixType,typename Rhs> struct homogeneous_right_product_impl; - -} // end namespace internal - -template<typename MatrixType,int _Direction> class Homogeneous - : public MatrixBase<Homogeneous<MatrixType,_Direction> >, internal::no_assignment_operator -{ - public: - - typedef MatrixType NestedExpression; - enum { Direction = _Direction }; - - typedef MatrixBase<Homogeneous> Base; - EIGEN_DENSE_PUBLIC_INTERFACE(Homogeneous) - - EIGEN_DEVICE_FUNC explicit inline Homogeneous(const MatrixType& matrix) - : m_matrix(matrix) - {} - - 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> - EIGEN_DEVICE_FUNC inline const Product<Homogeneous,Rhs> - operator* (const MatrixBase<Rhs>& rhs) const - { - eigen_assert(int(Direction)==Horizontal); - return Product<Homogeneous,Rhs>(*this,rhs.derived()); - } - - template<typename Lhs> friend - EIGEN_DEVICE_FUNC inline const Product<Lhs,Homogeneous> - operator* (const MatrixBase<Lhs>& lhs, const Homogeneous& rhs) - { - eigen_assert(int(Direction)==Vertical); - return Product<Lhs,Homogeneous>(lhs.derived(),rhs); - } - - template<typename Scalar, int Dim, int Mode, int Options> friend - 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 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 \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. - * - * 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 VectorwiseOp::homogeneous(), class Homogeneous - */ -template<typename Derived> -EIGEN_DEVICE_FUNC inline typename MatrixBase<Derived>::HomogeneousReturnType -MatrixBase<Derived>::homogeneous() const -{ - EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived); - return HomogeneousReturnType(derived()); -} - -/** \geometry_module \ingroup Geometry_Module - * - * \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(), class Homogeneous */ -template<typename ExpressionType, int Direction> -EIGEN_DEVICE_FUNC inline Homogeneous<ExpressionType,Direction> -VectorwiseOp<ExpressionType,Direction>::homogeneous() const -{ - return HomogeneousReturnType(_expression()); -} - -/** \geometry_module \ingroup Geometry_Module - * - * \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> -EIGEN_DEVICE_FUNC inline const typename MatrixBase<Derived>::HNormalizedReturnType -MatrixBase<Derived>::hnormalized() const -{ - EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived); - return ConstStartMinusOne(derived(),0,0, - ColsAtCompileTime==1?size()-1:1, - ColsAtCompileTime==1?1:size()-1) / coeff(size()-1); -} - -/** \geometry_module \ingroup Geometry_Module - * - * \brief column or row-wise homogeneous normalization - * - * \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> -EIGEN_DEVICE_FUNC inline const typename VectorwiseOp<ExpressionType,Direction>::HNormalizedReturnType -VectorwiseOp<ExpressionType,Direction>::hnormalized() const -{ - return HNormalized_Block(_expression(),0,0, - Direction==Vertical ? _expression().rows()-1 : _expression().rows(), - Direction==Horizontal ? _expression().cols()-1 : _expression().cols()).cwiseQuotient( - Replicate<HNormalized_Factors, - Direction==Vertical ? HNormalized_SizeMinusOne : 1, - Direction==Horizontal ? HNormalized_SizeMinusOne : 1> - (HNormalized_Factors(_expression(), - Direction==Vertical ? _expression().rows()-1:0, - Direction==Horizontal ? _expression().cols()-1:0, - Direction==Vertical ? 1 : _expression().rows(), - Direction==Horizontal ? 1 : _expression().cols()), - Direction==Vertical ? _expression().rows()-1 : 1, - Direction==Horizontal ? _expression().cols()-1 : 1)); -} - -namespace internal { - -template<typename MatrixOrTransformType> -struct take_matrix_for_product -{ - typedef MatrixOrTransformType type; - EIGEN_DEVICE_FUNC static const type& run(const type &x) { return x; } -}; - -template<typename Scalar, int Dim, int Mode,int Options> -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; - EIGEN_DEVICE_FUNC static type run (const TransformType& x) { return x.affine(); } -}; - -template<typename Scalar, int Dim, int Options> -struct take_matrix_for_product<Transform<Scalar, Dim, Projective, Options> > -{ - typedef Transform<Scalar, Dim, Projective, Options> TransformType; - typedef typename TransformType::MatrixType type; - EIGEN_DEVICE_FUNC static const type& run (const TransformType& x) { return x.matrix(); } -}; - -template<typename MatrixType,typename Lhs> -struct traits<homogeneous_left_product_impl<Homogeneous<MatrixType,Vertical>,Lhs> > -{ - typedef typename take_matrix_for_product<Lhs>::type LhsMatrixType; - typedef typename remove_all<MatrixType>::type MatrixTypeCleaned; - typedef typename remove_all<LhsMatrixType>::type LhsMatrixTypeCleaned; - typedef typename make_proper_matrix_type< - typename traits<MatrixTypeCleaned>::Scalar, - LhsMatrixTypeCleaned::RowsAtCompileTime, - MatrixTypeCleaned::ColsAtCompileTime, - MatrixTypeCleaned::PlainObject::Options, - LhsMatrixTypeCleaned::MaxRowsAtCompileTime, - MatrixTypeCleaned::MaxColsAtCompileTime>::type ReturnType; -}; - -template<typename MatrixType,typename Lhs> -struct homogeneous_left_product_impl<Homogeneous<MatrixType,Vertical>,Lhs> - : public ReturnByValue<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; - 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) - {} - - 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> EIGEN_DEVICE_FUNC void evalTo(Dest& dst) const - { - // FIXME investigate how to allow lazy evaluation of this product when possible - dst = Block<const LhsMatrixTypeNested, - LhsMatrixTypeNested::RowsAtCompileTime, - LhsMatrixTypeNested::ColsAtCompileTime==Dynamic?Dynamic:LhsMatrixTypeNested::ColsAtCompileTime-1> - (m_lhs,0,0,m_lhs.rows(),m_lhs.cols()-1) * m_rhs; - dst += m_lhs.col(m_lhs.cols()-1).rowwise() - .template replicate<MatrixType::ColsAtCompileTime>(m_rhs.cols()); - } - - typename LhsMatrixTypeCleaned::Nested m_lhs; - typename MatrixType::Nested m_rhs; -}; - -template<typename MatrixType,typename Rhs> -struct traits<homogeneous_right_product_impl<Homogeneous<MatrixType,Horizontal>,Rhs> > -{ - typedef typename make_proper_matrix_type<typename traits<MatrixType>::Scalar, - MatrixType::RowsAtCompileTime, - Rhs::ColsAtCompileTime, - MatrixType::PlainObject::Options, - MatrixType::MaxRowsAtCompileTime, - Rhs::MaxColsAtCompileTime>::type ReturnType; -}; - -template<typename MatrixType,typename Rhs> -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; - EIGEN_DEVICE_FUNC homogeneous_right_product_impl(const MatrixType& lhs, const Rhs& rhs) - : m_lhs(lhs), m_rhs(rhs) - {} - - 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> 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, - RhsNested::RowsAtCompileTime==Dynamic?Dynamic:RhsNested::RowsAtCompileTime-1, - RhsNested::ColsAtCompileTime> - (m_rhs,0,0,m_rhs.rows()-1,m_rhs.cols()); - dst += m_rhs.row(m_rhs.rows()-1).colwise() - .template replicate<MatrixType::RowsAtCompileTime>(m_lhs.rows()); - } - - typename MatrixType::Nested m_lhs; - 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 - -#endif // EIGEN_HOMOGENEOUS_H diff --git a/eigen/Eigen/src/Geometry/Hyperplane.h b/eigen/Eigen/src/Geometry/Hyperplane.h deleted file mode 100644 index 05929b2..0000000 --- a/eigen/Eigen/src/Geometry/Hyperplane.h +++ /dev/null @@ -1,282 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr> -// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com> -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_HYPERPLANE_H -#define EIGEN_HYPERPLANE_H - -namespace Eigen { - -/** \geometry_module \ingroup Geometry_Module - * - * \class Hyperplane - * - * \brief A hyperplane - * - * 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. - * - * \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 - * \f$ n \cdot x + d = 0 \f$ where \f$ n \f$ is a unit normal vector of the plane (linear part) - * and \f$ d \f$ is the distance (offset) to the origin. - */ -template <typename _Scalar, int _AmbientDim, int _Options> -class Hyperplane -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim==Dynamic ? Dynamic : _AmbientDim+1) - enum { - AmbientDimAtCompileTime = _AmbientDim, - Options = _Options - }; - typedef _Scalar Scalar; - typedef typename NumTraits<Scalar>::Real RealScalar; - typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 - typedef Matrix<Scalar,AmbientDimAtCompileTime,1> VectorType; - typedef Matrix<Scalar,Index(AmbientDimAtCompileTime)==Dynamic - ? Dynamic - : Index(AmbientDimAtCompileTime)+1,1,Options> Coefficients; - typedef Block<Coefficients,AmbientDimAtCompileTime,1> NormalReturnType; - typedef const Block<const Coefficients,AmbientDimAtCompileTime,1> ConstNormalReturnType; - - /** Default constructor without initialization */ - EIGEN_DEVICE_FUNC inline Hyperplane() {} - - template<int OtherOptions> - 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 */ - 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. - */ - EIGEN_DEVICE_FUNC inline Hyperplane(const VectorType& n, const VectorType& e) - : m_coeffs(n.size()+1) - { - normal() = n; - offset() = -n.dot(e); - } - - /** Constructs a plane from its normal \a n and distance to the origin \a d - * 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. - */ - EIGEN_DEVICE_FUNC inline Hyperplane(const VectorType& n, const Scalar& d) - : m_coeffs(n.size()+1) - { - normal() = n; - offset() = d; - } - - /** 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. - */ - EIGEN_DEVICE_FUNC static inline Hyperplane Through(const VectorType& p0, const VectorType& p1) - { - Hyperplane result(p0.size()); - result.normal() = (p1 - p0).unitOrthogonal(); - result.offset() = -p0.dot(result.normal()); - return result; - } - - /** Constructs a hyperplane passing through the three points. The dimension of the ambient space - * is required to be exactly 3. - */ - 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()); - VectorType v0(p2 - p0), v1(p1 - p0); - result.normal() = v0.cross(v1); - RealScalar norm = result.normal().norm(); - if(norm <= v0.norm() * v1.norm() * NumTraits<RealScalar>::epsilon()) - { - Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose(); - JacobiSVD<Matrix<Scalar,2,3> > svd(m, ComputeFullV); - result.normal() = svd.matrixV().col(2); - } - else - result.normal() /= norm; - result.offset() = -p0.dot(result.normal()); - return result; - } - - /** Constructs a hyperplane passing through the parametrized line \a parametrized. - * If the dimension of the ambient space is greater than 2, then there isn't uniqueness, - * so an arbitrary choice is made. - */ - // FIXME to be consitent with the rest this could be implemented as a static Through function ?? - EIGEN_DEVICE_FUNC explicit Hyperplane(const ParametrizedLine<Scalar, AmbientDimAtCompileTime>& parametrized) - { - normal() = parametrized.direction().unitOrthogonal(); - offset() = -parametrized.origin().dot(normal()); - } - - EIGEN_DEVICE_FUNC ~Hyperplane() {} - - /** \returns the dimension in which the plane holds */ - EIGEN_DEVICE_FUNC inline Index dim() const { return AmbientDimAtCompileTime==Dynamic ? m_coeffs.size()-1 : Index(AmbientDimAtCompileTime); } - - /** normalizes \c *this */ - EIGEN_DEVICE_FUNC void normalize(void) - { - m_coeffs /= normal().norm(); - } - - /** \returns the signed distance between the plane \c *this and a point \a p. - * \sa absDistance() - */ - 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() - */ - 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. - */ - 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. - */ - 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. - */ - 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. - */ - 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 */ - 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$ - */ - 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$ - */ - EIGEN_DEVICE_FUNC inline Coefficients& coeffs() { return m_coeffs; } - - /** \returns the intersection of *this with \a other. - * - * \warning The ambient space must be a plane, i.e. have dimension 2, so that \c *this and \a other are lines. - * - * \note If \a other is approximately parallel to *this, this method will return any point on *this. - */ - EIGEN_DEVICE_FUNC VectorType intersection(const Hyperplane& other) const - { - 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(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)); - } - else - { // general case - Scalar invdet = Scalar(1) / det; - return VectorType(invdet*(coeffs().coeff(1)*other.coeffs().coeff(2)-other.coeffs().coeff(1)*coeffs().coeff(2)), - invdet*(other.coeffs().coeff(0)*coeffs().coeff(2)-coeffs().coeff(0)*other.coeffs().coeff(2))); - } - } - - /** 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 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 - { - eigen_assert(0 && "invalid traits value in Hyperplane::transform()"); - } - 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 Hyperplane& transform(const Transform<Scalar,AmbientDimAtCompileTime,Affine,TrOptions>& t, - TransformTraits traits = Affine) - { - transform(t.linear(), traits); - offset() -= normal().dot(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> - EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<Hyperplane, - Hyperplane<NewScalarType,AmbientDimAtCompileTime,Options> >::type cast() const - { - return typename internal::cast_return_type<Hyperplane, - Hyperplane<NewScalarType,AmbientDimAtCompileTime,Options> >::type(*this); - } - - /** Copy constructor with scalar type conversion */ - template<typename OtherScalarType,int OtherOptions> - 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 - * determined by \a prec. - * - * \sa MatrixBase::isApprox() */ - template<int OtherOptions> - 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: - - Coefficients m_coeffs; -}; - -} // end namespace Eigen - -#endif // EIGEN_HYPERPLANE_H diff --git a/eigen/Eigen/src/Geometry/OrthoMethods.h b/eigen/Eigen/src/Geometry/OrthoMethods.h deleted file mode 100644 index a035e63..0000000 --- a/eigen/Eigen/src/Geometry/OrthoMethods.h +++ /dev/null @@ -1,234 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr> -// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com> -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_ORTHOMETHODS_H -#define EIGEN_ORTHOMETHODS_H - -namespace Eigen { - -/** \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> -#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) - EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,3) - - // 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_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)), - numext::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0)) - ); -} - -namespace internal { - -template< int Arch,typename VectorLhs,typename VectorRhs, - typename Scalar = typename VectorLhs::Scalar, - bool Vectorizable = bool((VectorLhs::Flags&VectorRhs::Flags)&PacketAccessBit)> -struct cross3_impl { - 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( - numext::conj(lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1)), - numext::conj(lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2)), - numext::conj(lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0)), - 0 - ); - } -}; - -} - -/** \geometry_module \ingroup Geometry_Module - * - * \returns the cross product of \c *this and \a other using only the x, y, and z coefficients - * - * The size of \c *this and \a other must be four. This function is especially useful - * when using 4D vectors instead of 3D ones to get advantage of SSE/AltiVec vectorization. - * - * \sa MatrixBase::cross() - */ -template<typename Derived> -template<typename OtherDerived> -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_eval<Derived,2>::type DerivedNested; - typedef typename internal::nested_eval<OtherDerived,2>::type OtherDerivedNested; - DerivedNested lhs(derived()); - OtherDerivedNested rhs(other.derived()); - - return internal::cross3_impl<Architecture::Target, - typename internal::remove_all<DerivedNested>::type, - typename internal::remove_all<OtherDerivedNested>::type>::run(lhs,rhs); -} - -/** \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. - * - * \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) = (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) = (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; -} - -namespace internal { - -template<typename Derived, int Size = Derived::SizeAtCompileTime> -struct unitOrthogonal_selector -{ - typedef typename plain_matrix_type<Derived>::type VectorType; - typedef typename traits<Derived>::Scalar Scalar; - typedef typename NumTraits<Scalar>::Real RealScalar; - typedef Matrix<Scalar,2,1> Vector2; - EIGEN_DEVICE_FUNC - static inline VectorType run(const Derived& src) - { - VectorType perp = VectorType::Zero(src.size()); - Index maxi = 0; - Index sndi = 0; - src.cwiseAbs().maxCoeff(&maxi); - if (maxi==0) - sndi = 1; - RealScalar invnm = RealScalar(1)/(Vector2() << src.coeff(sndi),src.coeff(maxi)).finished().norm(); - perp.coeffRef(maxi) = -numext::conj(src.coeff(sndi)) * invnm; - perp.coeffRef(sndi) = numext::conj(src.coeff(maxi)) * invnm; - - return perp; - } -}; - -template<typename Derived> -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; - /* Let us compute the crossed product of *this with a vector - * that is not too close to being colinear to *this. - */ - - /* unless the x and y coords are both close to zero, we can - * simply take ( -y, x, 0 ) and normalize it. - */ - if((!isMuchSmallerThan(src.x(), src.z())) - || (!isMuchSmallerThan(src.y(), src.z()))) - { - RealScalar invnm = RealScalar(1)/src.template head<2>().norm(); - perp.coeffRef(0) = -numext::conj(src.y())*invnm; - perp.coeffRef(1) = numext::conj(src.x())*invnm; - perp.coeffRef(2) = 0; - } - /* if both x and y are close to zero, then the vector is close - * to the z-axis, so it's far from colinear to the x-axis for instance. - * So we take the crossed product with (1,0,0) and normalize it. - */ - else - { - RealScalar invnm = RealScalar(1)/src.template tail<2>().norm(); - perp.coeffRef(0) = 0; - perp.coeffRef(1) = -numext::conj(src.z())*invnm; - perp.coeffRef(2) = numext::conj(src.y())*invnm; - } - - return perp; - } -}; - -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 - -/** \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(). - * - * \sa cross() - */ -template<typename Derived> -EIGEN_DEVICE_FUNC typename MatrixBase<Derived>::PlainObject -MatrixBase<Derived>::unitOrthogonal() const -{ - EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return internal::unitOrthogonal_selector<Derived>::run(derived()); -} - -} // end namespace Eigen - -#endif // EIGEN_ORTHOMETHODS_H diff --git a/eigen/Eigen/src/Geometry/ParametrizedLine.h b/eigen/Eigen/src/Geometry/ParametrizedLine.h deleted file mode 100644 index 1e985d8..0000000 --- a/eigen/Eigen/src/Geometry/ParametrizedLine.h +++ /dev/null @@ -1,195 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr> -// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com> -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_PARAMETRIZEDLINE_H -#define EIGEN_PARAMETRIZEDLINE_H - -namespace Eigen { - -/** \geometry_module \ingroup Geometry_Module - * - * \class ParametrizedLine - * - * \brief A parametrized line - * - * A parametrized line is defined by an origin point \f$ \mathbf{o} \f$ and a unit - * 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$. - * - * \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 -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) - enum { - AmbientDimAtCompileTime = _AmbientDim, - Options = _Options - }; - typedef _Scalar Scalar; - typedef typename NumTraits<Scalar>::Real RealScalar; - typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3 - typedef Matrix<Scalar,AmbientDimAtCompileTime,1,Options> VectorType; - - /** Default constructor without initialization */ - EIGEN_DEVICE_FUNC inline ParametrizedLine() {} - - template<int OtherOptions> - 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 */ - 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. - */ - EIGEN_DEVICE_FUNC ParametrizedLine(const VectorType& origin, const VectorType& direction) - : m_origin(origin), m_direction(direction) {} - - template <int OtherOptions> - EIGEN_DEVICE_FUNC explicit ParametrizedLine(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane); - - /** Constructs a parametrized line going from \a p0 to \a p1. */ - EIGEN_DEVICE_FUNC static inline ParametrizedLine Through(const VectorType& p0, const VectorType& p1) - { return ParametrizedLine(p0, (p1-p0).normalized()); } - - EIGEN_DEVICE_FUNC ~ParametrizedLine() {} - - /** \returns the dimension in which the line holds */ - EIGEN_DEVICE_FUNC inline Index dim() const { return m_direction.size(); } - - EIGEN_DEVICE_FUNC const VectorType& origin() const { return m_origin; } - EIGEN_DEVICE_FUNC VectorType& origin() { return m_origin; } - - 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() - */ - EIGEN_DEVICE_FUNC RealScalar squaredDistance(const VectorType& p) const - { - VectorType diff = p - origin(); - return (diff - direction().dot(diff) * direction()).squaredNorm(); - } - /** \returns the distance of a point \a p to its projection onto the line \c *this. - * \sa squaredDistance() - */ - 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. */ - EIGEN_DEVICE_FUNC VectorType projection(const VectorType& p) const - { return origin() + direction().dot(p-origin()) * direction(); } - - EIGEN_DEVICE_FUNC VectorType pointAt(const Scalar& t) const; - - template <int OtherOptions> - EIGEN_DEVICE_FUNC Scalar intersectionParameter(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane) const; - - template <int OtherOptions> - EIGEN_DEVICE_FUNC Scalar intersection(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane) const; - - template <int OtherOptions> - EIGEN_DEVICE_FUNC VectorType intersectionPoint(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane) const; - - /** \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> - EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<ParametrizedLine, - ParametrizedLine<NewScalarType,AmbientDimAtCompileTime,Options> >::type cast() const - { - return typename internal::cast_return_type<ParametrizedLine, - ParametrizedLine<NewScalarType,AmbientDimAtCompileTime,Options> >::type(*this); - } - - /** Copy constructor with scalar type conversion */ - template<typename OtherScalarType,int OtherOptions> - 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>(); - } - - /** \returns \c true if \c *this is approximately equal to \a other, within the precision - * determined by \a prec. - * - * \sa MatrixBase::isApprox() */ - 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: - - VectorType m_origin, m_direction; -}; - -/** Constructs a parametrized line from a 2D hyperplane - * - * \warning the ambient space must have dimension 2 such that the hyperplane actually describes a line - */ -template <typename _Scalar, int _AmbientDim, int _Options> -template <int OtherOptions> -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(); - origin() = -hyperplane.normal()*hyperplane.offset(); -} - -/** \returns the point at \a t along this line - */ -template <typename _Scalar, int _AmbientDim, int _Options> -EIGEN_DEVICE_FUNC inline typename ParametrizedLine<_Scalar, _AmbientDim,_Options>::VectorType -ParametrizedLine<_Scalar, _AmbientDim,_Options>::pointAt(const _Scalar& t) const -{ - return origin() + (direction()*t); -} - -/** \returns the parameter value of the intersection between \c *this and the given \a hyperplane - */ -template <typename _Scalar, int _AmbientDim, int _Options> -template <int OtherOptions> -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()); -} - - -/** \deprecated use intersectionParameter() - * \returns the parameter value of the intersection between \c *this and the given \a hyperplane - */ -template <typename _Scalar, int _AmbientDim, int _Options> -template <int OtherOptions> -EIGEN_DEVICE_FUNC inline _Scalar ParametrizedLine<_Scalar, _AmbientDim,_Options>::intersection(const Hyperplane<_Scalar, _AmbientDim, OtherOptions>& hyperplane) const -{ - return intersectionParameter(hyperplane); -} - -/** \returns the point of the intersection between \c *this and the given hyperplane - */ -template <typename _Scalar, int _AmbientDim, int _Options> -template <int OtherOptions> -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)); -} - -} // end namespace Eigen - -#endif // EIGEN_PARAMETRIZEDLINE_H diff --git a/eigen/Eigen/src/Geometry/Quaternion.h b/eigen/Eigen/src/Geometry/Quaternion.h deleted file mode 100644 index c3fd8c3..0000000 --- a/eigen/Eigen/src/Geometry/Quaternion.h +++ /dev/null @@ -1,814 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2008-2010 Gael Guennebaud <gael.guennebaud@inria.fr> -// Copyright (C) 2009 Mathieu Gautier <mathieu.gautier@cea.fr> -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_QUATERNION_H -#define EIGEN_QUATERNION_H -namespace Eigen { - - -/*************************************************************************** -* Definition of QuaternionBase<Derived> -* The implementation is at the end of the file -***************************************************************************/ - -namespace internal { -template<typename Other, - int OtherRows=Other::RowsAtCompileTime, - int OtherCols=Other::ColsAtCompileTime> -struct quaternionbase_assign_impl; -} - -/** \geometry_module \ingroup Geometry_Module - * \class QuaternionBase - * \brief Base class for quaternion expressions - * \tparam Derived derived type (CRTP) - * \sa class Quaternion - */ -template<class Derived> -class QuaternionBase : public RotationBase<Derived, 3> -{ - public: - typedef RotationBase<Derived, 3> Base; - - using Base::operator*; - using Base::derived; - - typedef typename internal::traits<Derived>::Scalar Scalar; - typedef typename NumTraits<Scalar>::Real RealScalar; - typedef typename internal::traits<Derived>::Coefficients Coefficients; - typedef typename Coefficients::CoeffReturnType CoeffReturnType; - typedef typename internal::conditional<bool(internal::traits<Derived>::Flags&LvalueBit), - Scalar&, CoeffReturnType>::type NonConstCoeffReturnType; - - - enum { - Flags = Eigen::internal::traits<Derived>::Flags - }; - - // typedef typename Matrix<Scalar,4,1> Coefficients; - /** the type of a 3D vector */ - typedef Matrix<Scalar,3,1> Vector3; - /** the equivalent rotation matrix type */ - typedef Matrix<Scalar,3,3> Matrix3; - /** the equivalent angle-axis type */ - typedef AngleAxis<Scalar> AngleAxisType; - - - - /** \returns the \c x coefficient */ - EIGEN_DEVICE_FUNC inline CoeffReturnType x() const { return this->derived().coeffs().coeff(0); } - /** \returns the \c y coefficient */ - EIGEN_DEVICE_FUNC inline CoeffReturnType y() const { return this->derived().coeffs().coeff(1); } - /** \returns the \c z coefficient */ - EIGEN_DEVICE_FUNC inline CoeffReturnType z() const { return this->derived().coeffs().coeff(2); } - /** \returns the \c w coefficient */ - EIGEN_DEVICE_FUNC inline CoeffReturnType w() const { return this->derived().coeffs().coeff(3); } - - /** \returns a reference to the \c x coefficient (if Derived is a non-const lvalue) */ - EIGEN_DEVICE_FUNC inline NonConstCoeffReturnType x() { return this->derived().coeffs().x(); } - /** \returns a reference to the \c y coefficient (if Derived is a non-const lvalue) */ - EIGEN_DEVICE_FUNC inline NonConstCoeffReturnType y() { return this->derived().coeffs().y(); } - /** \returns a reference to the \c z coefficient (if Derived is a non-const lvalue) */ - EIGEN_DEVICE_FUNC inline NonConstCoeffReturnType z() { return this->derived().coeffs().z(); } - /** \returns a reference to the \c w coefficient (if Derived is a non-const lvalue) */ - EIGEN_DEVICE_FUNC inline NonConstCoeffReturnType w() { return this->derived().coeffs().w(); } - - /** \returns a read-only vector expression of the imaginary part (x,y,z) */ - 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) */ - 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) */ - 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) */ - EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Coefficients& coeffs() { return derived().coeffs(); } - - 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 -// useful; however notice that we already have the templated operator= above and e.g. in MatrixBase -// we didn't have to add, in addition to templated operator=, such a non-templated copy operator. -// Derived& operator=(const QuaternionBase& other) -// { return operator=<Derived>(other); } - - 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() - */ - EIGEN_DEVICE_FUNC static inline Quaternion<Scalar> Identity() { return Quaternion<Scalar>(Scalar(1), Scalar(0), Scalar(0), Scalar(0)); } - - /** \sa QuaternionBase::Identity(), MatrixBase::setIdentity() - */ - 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() - */ - EIGEN_DEVICE_FUNC inline Scalar squaredNorm() const { return coeffs().squaredNorm(); } - - /** \returns the norm of the quaternion's coefficients - * \sa QuaternionBase::squaredNorm(), MatrixBase::norm() - */ - EIGEN_DEVICE_FUNC inline Scalar norm() const { return coeffs().norm(); } - - /** Normalizes the quaternion \c *this - * \sa normalized(), MatrixBase::normalize() */ - EIGEN_DEVICE_FUNC inline void normalize() { coeffs().normalize(); } - /** \returns a normalized copy of \c *this - * \sa normalize(), MatrixBase::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> EIGEN_DEVICE_FUNC inline Scalar dot(const QuaternionBase<OtherDerived>& other) const { return coeffs().dot(other.coeffs()); } - - template<class OtherDerived> EIGEN_DEVICE_FUNC Scalar angularDistance(const QuaternionBase<OtherDerived>& other) const; - - /** \returns an equivalent 3x3 rotation matrix */ - EIGEN_DEVICE_FUNC Matrix3 toRotationMatrix() const; - - /** \returns the quaternion which transform \a a into \a b through a rotation */ - template<typename Derived1, typename Derived2> - EIGEN_DEVICE_FUNC Derived& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b); - - 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 */ - EIGEN_DEVICE_FUNC Quaternion<Scalar> inverse() const; - - /** \returns the conjugated quaternion */ - EIGEN_DEVICE_FUNC Quaternion<Scalar> conjugate() 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> - 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_DEVICE_FUNC EIGEN_STRONG_INLINE Vector3 _transformVector(const Vector3& v) const; - - /** \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> - 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()); - } - -#ifdef EIGEN_QUATERNIONBASE_PLUGIN -# include EIGEN_QUATERNIONBASE_PLUGIN -#endif -}; - -/*************************************************************************** -* Definition/implementation of Quaternion<Scalar> -***************************************************************************/ - -/** \geometry_module \ingroup Geometry_Module - * - * \class Quaternion - * - * \brief The quaternion class used to represent 3D orientations and rotations - * - * \tparam _Scalar the scalar type, i.e., the type of the coefficients - * \tparam _Options controls the memory alignment of the coefficients. Can be \# AutoAlign or \# DontAlign. Default is AutoAlign. - * - * This class represents a quaternion \f$ w+xi+yj+zk \f$ that is a convenient representation of - * orientations and rotations of objects in three dimensions. Compared to other representations - * like Euler angles or 3x3 matrices, quaternions offer the following advantages: - * \li \b compact storage (4 scalars) - * \li \b efficient to compose (28 flops), - * \li \b stable spherical interpolation - * - * The following two typedefs are provided for convenience: - * \li \c Quaternionf for \c float - * \li \c Quaterniond for \c double - * - * \warning Operations interpreting the quaternion as rotation have undefined behavior if the quaternion is not normalized. - * - * \sa class AngleAxis, class Transform - */ - -namespace internal { -template<typename _Scalar,int _Options> -struct traits<Quaternion<_Scalar,_Options> > -{ - typedef Quaternion<_Scalar,_Options> PlainObject; - typedef _Scalar Scalar; - typedef Matrix<_Scalar,4,1,_Options> Coefficients; - enum{ - Alignment = internal::traits<Coefficients>::Alignment, - Flags = LvalueBit - }; -}; -} - -template<typename _Scalar, int _Options> -class Quaternion : public QuaternionBase<Quaternion<_Scalar,_Options> > -{ -public: - typedef QuaternionBase<Quaternion<_Scalar,_Options> > Base; - enum { NeedsAlignment = internal::traits<Quaternion>::Alignment>0 }; - - typedef _Scalar Scalar; - - EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Quaternion) - using Base::operator*=; - - typedef typename internal::traits<Quaternion>::Coefficients Coefficients; - typedef typename Base::AngleAxisType AngleAxisType; - - /** Default constructor leaving the quaternion uninitialized. */ - 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. - * - * \warning Note the order of the arguments: the real \a w coefficient first, - * while internally the coefficients are stored in the following order: - * [\c x, \c y, \c z, \c 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 */ - EIGEN_DEVICE_FUNC explicit inline Quaternion(const Scalar* data) : m_coeffs(data) {} - - /** Copy constructor */ - 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 */ - 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> - EIGEN_DEVICE_FUNC explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; } - - /** Explicit copy constructor with scalar conversion */ - template<typename OtherScalar, int OtherOptions> - 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> - EIGEN_DEVICE_FUNC static Quaternion FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b); - - 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(NeedsAlignment)) - -#ifdef EIGEN_QUATERNION_PLUGIN -# include EIGEN_QUATERNION_PLUGIN -#endif - -protected: - Coefficients m_coeffs; - -#ifndef EIGEN_PARSED_BY_DOXYGEN - static EIGEN_STRONG_INLINE void _check_template_params() - { - EIGEN_STATIC_ASSERT( (_Options & DontAlign) == _Options, - INVALID_MATRIX_TEMPLATE_PARAMETERS) - } -#endif -}; - -/** \ingroup Geometry_Module - * single precision quaternion type */ -typedef Quaternion<float> Quaternionf; -/** \ingroup Geometry_Module - * double precision quaternion type */ -typedef Quaternion<double> Quaterniond; - -/*************************************************************************** -* Specialization of Map<Quaternion<Scalar>> -***************************************************************************/ - -namespace internal { - template<typename _Scalar, int _Options> - struct traits<Map<Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> > - { - typedef Map<Matrix<_Scalar,4,1>, _Options> Coefficients; - }; -} - -namespace internal { - template<typename _Scalar, int _Options> - struct traits<Map<const Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> > - { - typedef Map<const Matrix<_Scalar,4,1>, _Options> Coefficients; - typedef traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> > TraitsBase; - enum { - Flags = TraitsBase::Flags & ~LvalueBit - }; - }; -} - -/** \ingroup Geometry_Module - * \brief Quaternion expression mapping a constant memory buffer - * - * \tparam _Scalar the type of the Quaternion coefficients - * \tparam _Options see class Map - * - * This is a specialization of class Map for Quaternion. This class allows to view - * a 4 scalar memory buffer as an Eigen's Quaternion object. - * - * \sa class Map, class Quaternion, class QuaternionBase - */ -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; - - typedef _Scalar Scalar; - typedef typename internal::traits<Map>::Coefficients Coefficients; - EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map) - using Base::operator*=; - - /** Constructs a Mapped Quaternion object from the pointer \a coeffs - * - * The pointer \a coeffs must reference the four coefficients of Quaternion in the following order: - * \code *coeffs == {x, y, z, w} \endcode - * - * If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */ - EIGEN_DEVICE_FUNC explicit EIGEN_STRONG_INLINE Map(const Scalar* coeffs) : m_coeffs(coeffs) {} - - EIGEN_DEVICE_FUNC inline const Coefficients& coeffs() const { return m_coeffs;} - - protected: - const Coefficients m_coeffs; -}; - -/** \ingroup Geometry_Module - * \brief Expression of a quaternion from a memory buffer - * - * \tparam _Scalar the type of the Quaternion coefficients - * \tparam _Options see class Map - * - * This is a specialization of class Map for Quaternion. This class allows to view - * a 4 scalar memory buffer as an Eigen's Quaternion object. - * - * \sa class Map, class Quaternion, class QuaternionBase - */ -template<typename _Scalar, int _Options> -class Map<Quaternion<_Scalar>, _Options > - : public QuaternionBase<Map<Quaternion<_Scalar>, _Options> > -{ - public: - typedef QuaternionBase<Map<Quaternion<_Scalar>, _Options> > Base; - - typedef _Scalar Scalar; - typedef typename internal::traits<Map>::Coefficients Coefficients; - EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map) - using Base::operator*=; - - /** Constructs a Mapped Quaternion object from the pointer \a coeffs - * - * The pointer \a coeffs must reference the four coefficients of Quaternion in the following order: - * \code *coeffs == {x, y, z, w} \endcode - * - * If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */ - EIGEN_DEVICE_FUNC explicit EIGEN_STRONG_INLINE Map(Scalar* coeffs) : m_coeffs(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; -}; - -/** \ingroup Geometry_Module - * Map an unaligned array of single precision scalars as a quaternion */ -typedef Map<Quaternion<float>, 0> QuaternionMapf; -/** \ingroup Geometry_Module - * Map an unaligned array of double precision scalars as a quaternion */ -typedef Map<Quaternion<double>, 0> QuaternionMapd; -/** \ingroup Geometry_Module - * Map a 16-byte aligned array of single precision scalars as a quaternion */ -typedef Map<Quaternion<float>, Aligned> QuaternionMapAlignedf; -/** \ingroup Geometry_Module - * Map a 16-byte aligned array of double precision scalars as a quaternion */ -typedef Map<Quaternion<double>, Aligned> QuaternionMapAlignedd; - -/*************************************************************************** -* Implementation of QuaternionBase methods -***************************************************************************/ - -// Generic Quaternion * Quaternion product -// This product can be specialized for a given architecture via the Arch template argument. -namespace internal { -template<int Arch, class Derived1, class Derived2, typename Scalar> struct quat_product -{ - 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(), - a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(), - a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(), - a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x() - ); - } -}; -} - -/** \returns the concatenation of two rotations as a quaternion-quaternion product */ -template <class Derived> -template <class OtherDerived> -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>::run(*this, other); -} - -/** \sa operator*(Quaternion) */ -template <class Derived> -template <class OtherDerived> -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator*= (const QuaternionBase<OtherDerived>& other) -{ - derived() = derived() * other.derived(); - return derived(); -} - -/** Rotation of a vector by a quaternion. - * \remarks If the quaternion is used to rotate several points (>1) - * then it is much more efficient to first convert it to a 3x3 Matrix. - * Comparison of the operation cost for n transformations: - * - Quaternion2: 30n - * - Via a Matrix3: 24 + 15n - */ -template <class Derived> -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 - // of the conversion to a Matrix followed by a Matrix/Vector product. - // It appears to be much faster than the common algorithm found - // in the literature (30 versus 39 flops). It also requires two - // Vector3 as temporaries. - Vector3 uv = this->vec().cross(v); - uv += uv; - return v + this->w() * uv + this->vec().cross(uv); -} - -template<class Derived> -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE QuaternionBase<Derived>& QuaternionBase<Derived>::operator=(const QuaternionBase<Derived>& other) -{ - coeffs() = other.coeffs(); - return derived(); -} - -template<class Derived> -template<class OtherDerived> -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const QuaternionBase<OtherDerived>& other) -{ - coeffs() = other.coeffs(); - return derived(); -} - -/** Set \c *this from an angle-axis \a aa and returns a reference to \c *this - */ -template<class Derived> -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const AngleAxisType& aa) -{ - 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(); - return derived(); -} - -/** Set \c *this from the expression \a xpr: - * - if \a xpr is a 4x1 vector, then \a xpr is assumed to be a quaternion - * - if \a xpr is a 3x3 matrix, then \a xpr is assumed to be rotation matrix - * and \a xpr is converted to a quaternion - */ - -template<class Derived> -template<class MatrixDerived> -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) - internal::quaternionbase_assign_impl<MatrixDerived>::run(*this, xpr.derived()); - return derived(); -} - -/** Convert the quaternion to a 3x3 rotation matrix. The quaternion is required to - * be normalized, otherwise the result is undefined. - */ -template<class Derived> -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 !!) - // if not inlined then the cost of the return by value is huge ~ +35%, - // however, not inlining this function is an order of magnitude slower, so - // it has to be inlined, and so the return by value is not an issue - Matrix3 res; - - const Scalar tx = Scalar(2)*this->x(); - const Scalar ty = Scalar(2)*this->y(); - const Scalar tz = Scalar(2)*this->z(); - const Scalar twx = tx*this->w(); - const Scalar twy = ty*this->w(); - const Scalar twz = tz*this->w(); - const Scalar txx = tx*this->x(); - const Scalar txy = ty*this->x(); - const Scalar txz = tz*this->x(); - const Scalar tyy = ty*this->y(); - const Scalar tyz = tz*this->y(); - const Scalar tzz = tz*this->z(); - - res.coeffRef(0,0) = Scalar(1)-(tyy+tzz); - res.coeffRef(0,1) = txy-twz; - res.coeffRef(0,2) = txz+twy; - res.coeffRef(1,0) = txy+twz; - res.coeffRef(1,1) = Scalar(1)-(txx+tzz); - res.coeffRef(1,2) = tyz-twx; - res.coeffRef(2,0) = txz-twy; - res.coeffRef(2,1) = tyz+twx; - res.coeffRef(2,2) = Scalar(1)-(txx+tyy); - - return res; -} - -/** Sets \c *this to be a quaternion representing a rotation between - * the two arbitrary vectors \a a and \a b. In other words, the built - * rotation represent a rotation sending the line of direction \a a - * to the line of direction \a b, both lines passing through the origin. - * - * \returns a reference to \c *this. - * - * Note that the two input vectors do \b not have to be normalized, and - * do not need to have the same norm. - */ -template<class Derived> -template<typename Derived1, typename Derived2> -EIGEN_DEVICE_FUNC inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b) -{ - EIGEN_USING_STD_MATH(sqrt) - Vector3 v0 = a.normalized(); - Vector3 v1 = b.normalized(); - Scalar c = v1.dot(v0); - - // if dot == -1, vectors are nearly opposites - // => accurately compute the rotation axis by computing the - // intersection of the two planes. This is done by solving: - // x^T v0 = 0 - // x^T v1 = 0 - // under the constraint: - // ||x|| = 1 - // which yields a singular value problem - if (c < Scalar(-1)+NumTraits<Scalar>::dummy_precision()) - { - 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); - - Scalar w2 = (Scalar(1)+c)*Scalar(0.5); - this->w() = sqrt(w2); - this->vec() = axis * sqrt(Scalar(1) - w2); - return derived(); - } - Vector3 axis = v0.cross(v1); - Scalar s = sqrt((Scalar(1)+c)*Scalar(2)); - Scalar invs = Scalar(1)/s; - this->vec() = axis * invs; - this->w() = s * Scalar(0.5); - - 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 - * rotation represent a rotation sending the line of direction \a a - * to the line of direction \a b, both lines passing through the origin. - * - * \returns resulting quaternion - * - * Note that the two input vectors do \b not have to be normalized, and - * do not need to have the same norm. - */ -template<typename Scalar, int Options> -template<typename Derived1, typename Derived2> -EIGEN_DEVICE_FUNC Quaternion<Scalar,Options> Quaternion<Scalar,Options>::FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b) -{ - Quaternion quat; - quat.setFromTwoVectors(a, b); - return quat; -} - - -/** \returns the multiplicative inverse of \c *this - * Note that in most cases, i.e., if you simply want the opposite rotation, - * and/or the quaternion is normalized, then it is enough to use the conjugate. - * - * \sa QuaternionBase::conjugate() - */ -template <class Derived> -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(); - if (n2 > Scalar(0)) - return Quaternion<Scalar>(conjugate().coeffs() / n2); - else - { - // return an invalid result to flag the error - return Quaternion<Scalar>(Coefficients::Zero()); - } -} - -// Generic conjugate of a Quaternion -namespace internal { -template<int Arch, class Derived, typename Scalar> 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. - * - * \sa Quaternion2::inverse() - */ -template <class Derived> -EIGEN_DEVICE_FUNC inline Quaternion<typename internal::traits<Derived>::Scalar> -QuaternionBase<Derived>::conjugate() const -{ - return internal::quat_conj<Architecture::Target, Derived, - typename internal::traits<Derived>::Scalar>::run(*this); - -} - -/** \returns the angle (in radian) between two rotations - * \sa dot() - */ -template <class Derived> -template <class OtherDerived> -EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Scalar -QuaternionBase<Derived>::angularDistance(const QuaternionBase<OtherDerived>& other) const -{ - EIGEN_USING_STD_MATH(atan2) - Quaternion<Scalar> d = (*this) * other.conjugate(); - return Scalar(2) * atan2( d.vec().norm(), numext::abs(d.w()) ); -} - - - -/** \returns the spherical linear interpolation between the two quaternions - * \c *this and \a other at the parameter \a t in [0;1]. - * - * This represents an interpolation for a constant motion between \c *this and \a other, - * see also http://en.wikipedia.org/wiki/Slerp. - */ -template <class Derived> -template <class OtherDerived> -EIGEN_DEVICE_FUNC Quaternion<typename internal::traits<Derived>::Scalar> -QuaternionBase<Derived>::slerp(const Scalar& t, const QuaternionBase<OtherDerived>& other) const -{ - 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 = numext::abs(d); - - Scalar scale0; - Scalar scale1; - - if(absD>=one) - { - scale0 = Scalar(1) - t; - scale1 = t; - } - else - { - // theta is the angle between the 2 quaternions - Scalar theta = acos(absD); - Scalar sinTheta = sin(theta); - - scale0 = sin( ( Scalar(1) - t ) * theta) / sinTheta; - scale1 = sin( ( t * theta) ) / sinTheta; - } - if(d<Scalar(0)) scale1 = -scale1; - - return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs()); -} - -namespace internal { - -// set from a rotation matrix -template<typename Other> -struct quaternionbase_assign_impl<Other,3,3> -{ - typedef typename Other::Scalar Scalar; - template<class Derived> EIGEN_DEVICE_FUNC static inline void run(QuaternionBase<Derived>& q, const Other& a_mat) - { - 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(); - if (t > Scalar(0)) - { - t = sqrt(t + Scalar(1.0)); - q.w() = Scalar(0.5)*t; - t = Scalar(0.5)/t; - q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t; - q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t; - q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t; - } - else - { - 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; - 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; - t = Scalar(0.5)/t; - q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t; - q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t; - q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t; - } - } -}; - -// set from a vector of coefficients assumed to be a quaternion -template<typename Other> -struct quaternionbase_assign_impl<Other,4,1> -{ - typedef typename Other::Scalar Scalar; - template<class Derived> EIGEN_DEVICE_FUNC static inline void run(QuaternionBase<Derived>& q, const Other& vec) - { - q.coeffs() = vec; - } -}; - -} // end namespace internal - -} // end namespace Eigen - -#endif // EIGEN_QUATERNION_H diff --git a/eigen/Eigen/src/Geometry/Rotation2D.h b/eigen/Eigen/src/Geometry/Rotation2D.h deleted file mode 100644 index 884b7d0..0000000 --- a/eigen/Eigen/src/Geometry/Rotation2D.h +++ /dev/null @@ -1,199 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr> -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_ROTATION2D_H -#define EIGEN_ROTATION2D_H - -namespace Eigen { - -/** \geometry_module \ingroup Geometry_Module - * - * \class Rotation2D - * - * \brief Represents a rotation/orientation in a 2 dimensional space. - * - * \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 - * conversion from/to a 2x2 rotation matrix. Moreover this class aims to provide a similar - * interface to Quaternion in order to facilitate the writing of generic algorithms - * dealing with rotations. - * - * \sa class Quaternion, class Transform - */ - -namespace internal { - -template<typename _Scalar> struct traits<Rotation2D<_Scalar> > -{ - typedef _Scalar Scalar; -}; -} // end namespace internal - -template<typename _Scalar> -class Rotation2D : public RotationBase<Rotation2D<_Scalar>,2> -{ - typedef RotationBase<Rotation2D<_Scalar>,2> Base; - -public: - - using Base::operator*; - - enum { Dim = 2 }; - /** the scalar type of the coefficients */ - typedef _Scalar Scalar; - typedef Matrix<Scalar,2,1> Vector2; - typedef Matrix<Scalar,2,2> Matrix2; - -protected: - - Scalar m_angle; - -public: - - /** Construct a 2D counter clock wise rotation from the angle \a a in radian. */ - EIGEN_DEVICE_FUNC explicit inline Rotation2D(const Scalar& a) : m_angle(a) {} - - /** Default constructor wihtout initialization. The represented rotation is undefined. */ - 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 */ - EIGEN_DEVICE_FUNC inline Scalar angle() const { return m_angle; } - - /** \returns a read-write reference to the rotation 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 */ - EIGEN_DEVICE_FUNC inline Rotation2D inverse() const { return Rotation2D(-m_angle); } - - /** Concatenates two rotations */ - EIGEN_DEVICE_FUNC inline Rotation2D operator*(const Rotation2D& other) const - { return Rotation2D(m_angle + other.m_angle); } - - /** Concatenates two rotations */ - EIGEN_DEVICE_FUNC inline Rotation2D& operator*=(const Rotation2D& other) - { m_angle += other.m_angle; return *this; } - - /** Applies the rotation to a 2D vector */ - EIGEN_DEVICE_FUNC Vector2 operator* (const Vector2& vec) const - { return toRotationMatrix() * vec; } - - template<typename Derived> - 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. - */ - 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 - * - * 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> - 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> - EIGEN_DEVICE_FUNC inline explicit Rotation2D(const Rotation2D<OtherScalarType>& other) - { - m_angle = Scalar(other.angle()); - } - - 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() */ - 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 - * single precision 2D rotation type */ -typedef Rotation2D<float> Rotation2Df; -/** \ingroup Geometry_Module - * double precision 2D rotation type */ -typedef Rotation2D<double> Rotation2Dd; - -/** Set \c *this from a 2x2 rotation matrix \a mat. - * In other words, this function extract the rotation angle - * from the rotation matrix. - */ -template<typename Scalar> -template<typename Derived> -EIGEN_DEVICE_FUNC Rotation2D<Scalar>& Rotation2D<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat) -{ - 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; -} - -/** Constructs and \returns an equivalent 2x2 rotation matrix. - */ -template<typename Scalar> -typename Rotation2D<Scalar>::Matrix2 -EIGEN_DEVICE_FUNC Rotation2D<Scalar>::toRotationMatrix(void) const -{ - 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(); -} - -} // end namespace Eigen - -#endif // EIGEN_ROTATION2D_H diff --git a/eigen/Eigen/src/Geometry/RotationBase.h b/eigen/Eigen/src/Geometry/RotationBase.h deleted file mode 100644 index f0ee0bd..0000000 --- a/eigen/Eigen/src/Geometry/RotationBase.h +++ /dev/null @@ -1,206 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr> -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_ROTATIONBASE_H -#define EIGEN_ROTATIONBASE_H - -namespace Eigen { - -// forward declaration -namespace internal { -template<typename RotationDerived, typename MatrixType, bool IsVector=MatrixType::IsVectorAtCompileTime> -struct rotation_base_generic_product_selector; -} - -/** \class RotationBase - * - * \brief Common base class for compact rotation representations - * - * \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 -{ - public: - enum { Dim = _Dim }; - /** the scalar type of the coefficients */ - typedef typename internal::traits<Derived>::Scalar Scalar; - - /** corresponding linear transformation matrix type */ - typedef Matrix<Scalar,Dim,Dim> RotationMatrixType; - typedef Matrix<Scalar,Dim,1> VectorType; - - public: - 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 */ - 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. - */ - EIGEN_DEVICE_FUNC inline RotationMatrixType matrix() const { return derived().toRotationMatrix(); } - - /** \returns the inverse rotation */ - EIGEN_DEVICE_FUNC inline Derived inverse() const { return derived().inverse(); } - - /** \returns the concatenation of the rotation \c *this with a translation \a t */ - 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 */ - 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 - * \a e can be: - * - a DimxDim linear transformation matrix - * - a DimxDim diagonal matrix (axis aligned scaling) - * - a vector of size Dim - */ - template<typename OtherDerived> - 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 - 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 */ - 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); - return res; - } - - /** \returns the concatenation of the rotation \c *this with a transformation \a t */ - template<int Mode, int Options> - EIGEN_DEVICE_FUNC inline Transform<Scalar,Dim,Mode> operator*(const Transform<Scalar,Dim,Mode,Options>& t) const - { return toRotationMatrix() * t; } - - template<typename OtherVectorType> - EIGEN_DEVICE_FUNC inline VectorType _transformVector(const OtherVectorType& v) const - { return toRotationMatrix() * v; } -}; - -namespace internal { - -// implementation of the generic product rotation * matrix -template<typename RotationDerived, typename MatrixType> -struct rotation_base_generic_product_selector<RotationDerived,MatrixType,false> -{ - enum { Dim = RotationDerived::Dim }; - typedef Matrix<typename RotationDerived::Scalar,Dim,Dim> ReturnType; - EIGEN_DEVICE_FUNC static inline ReturnType run(const RotationDerived& r, const MatrixType& m) - { return r.toRotationMatrix() * m; } -}; - -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; - EIGEN_DEVICE_FUNC static inline ReturnType run(const RotationDerived& r, const DiagonalMatrix<Scalar,Dim,MaxDim>& m) - { - ReturnType res(r); - res.linear() *= m; - return res; - } -}; - -template<typename RotationDerived,typename OtherVectorType> -struct rotation_base_generic_product_selector<RotationDerived,OtherVectorType,true> -{ - enum { Dim = RotationDerived::Dim }; - typedef Matrix<typename RotationDerived::Scalar,Dim,1> ReturnType; - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE ReturnType run(const RotationDerived& r, const OtherVectorType& v) - { - return r._transformVector(v); - } -}; - -} // end namespace internal - -/** \geometry_module - * - * \brief Constructs a Dim x Dim rotation matrix from the rotation \a r - */ -template<typename _Scalar, int _Rows, int _Cols, int _Storage, int _MaxRows, int _MaxCols> -template<typename OtherDerived> -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)) - *this = r.toRotationMatrix(); -} - -/** \geometry_module - * - * \brief Set a Dim x Dim rotation matrix from the rotation \a r - */ -template<typename _Scalar, int _Rows, int _Cols, int _Storage, int _MaxRows, int _MaxCols> -template<typename OtherDerived> -EIGEN_DEVICE_FUNC Matrix<_Scalar, _Rows, _Cols, _Storage, _MaxRows, _MaxCols>& -Matrix<_Scalar, _Rows, _Cols, _Storage, _MaxRows, _MaxCols> -::operator=(const RotationBase<OtherDerived,ColsAtCompileTime>& r) -{ - EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix,int(OtherDerived::Dim),int(OtherDerived::Dim)) - return *this = r.toRotationMatrix(); -} - -namespace internal { - -/** \internal - * - * Helper function to return an arbitrary rotation object to a rotation matrix. - * - * \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. - * - * Default specializations are provided for: - * - any scalar type (2D), - * - any matrix expression, - * - any type based on RotationBase (e.g., Quaternion, AngleAxis, Rotation2D) - * - * Currently toRotationMatrix is only used by Transform. - * - * \sa class Transform, class Rotation2D, class Quaternion, class AngleAxis - */ -template<typename Scalar, int Dim> -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> -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> -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) - return mat; -} - -} // end namespace internal - -} // end namespace Eigen - -#endif // EIGEN_ROTATIONBASE_H diff --git a/eigen/Eigen/src/Geometry/Scaling.h b/eigen/Eigen/src/Geometry/Scaling.h deleted file mode 100644 index f58ca03..0000000 --- a/eigen/Eigen/src/Geometry/Scaling.h +++ /dev/null @@ -1,170 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr> -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_SCALING_H -#define EIGEN_SCALING_H - -namespace Eigen { - -/** \geometry_module \ingroup Geometry_Module - * - * \class Scaling - * - * \brief Represents a generic uniform scaling transformation - * - * \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 - * is used. In particular, this class is not aimed to be used to store a scaling transformation, - * but rather to make easier the constructions and updates of Transform objects. - * - * To represent an axis aligned scaling, use the DiagonalMatrix class. - * - * \sa Scaling(), class DiagonalMatrix, MatrixBase::asDiagonal(), class Translation, class Transform - */ -template<typename _Scalar> -class UniformScaling -{ -public: - /** the scalar type of the coefficients */ - typedef _Scalar Scalar; - -protected: - - Scalar m_factor; - -public: - - /** Default constructor without initialization. */ - UniformScaling() {} - /** Constructs and initialize a uniform scaling transformation */ - explicit inline UniformScaling(const Scalar& s) : m_factor(s) {} - - inline const Scalar& factor() const { return m_factor; } - inline Scalar& factor() { return m_factor; } - - /** Concatenates two uniform scaling */ - inline UniformScaling operator* (const UniformScaling& other) const - { return UniformScaling(m_factor * other.factor()); } - - /** Concatenates a uniform scaling and a translation */ - template<int Dim> - inline Transform<Scalar,Dim,Affine> operator* (const Translation<Scalar,Dim>& t) const; - - /** Concatenates a uniform scaling and an affine transformation */ - 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; - } - - /** Concatenates a uniform scaling and a linear transformation matrix */ - // TODO returns an expression - template<typename Derived> - inline typename internal::plain_matrix_type<Derived>::type operator* (const MatrixBase<Derived>& other) const - { return other * m_factor; } - - template<typename Derived,int Dim> - inline Matrix<Scalar,Dim,Dim> operator*(const RotationBase<Derived,Dim>& r) const - { return r.toRotationMatrix() * m_factor; } - - /** \returns the inverse scaling */ - inline UniformScaling inverse() const - { return UniformScaling(Scalar(1)/m_factor); } - - /** \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 UniformScaling<NewScalarType> cast() const - { return UniformScaling<NewScalarType>(NewScalarType(m_factor)); } - - /** Copy constructor with scalar type conversion */ - template<typename OtherScalarType> - inline explicit UniformScaling(const UniformScaling<OtherScalarType>& other) - { m_factor = Scalar(other.factor()); } - - /** \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 UniformScaling& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const - { return internal::isApprox(m_factor, other.factor(), prec); } - -}; - -/** \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 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 */ -inline UniformScaling<float> Scaling(float s) { return UniformScaling<float>(s); } -/** Constructs a uniform scaling from scale factor \a s */ -inline UniformScaling<double> Scaling(double s) { return UniformScaling<double>(s); } -/** Constructs a uniform scaling from scale factor \a s */ -template<typename RealScalar> -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> -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> -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> -inline const DiagonalWrapper<const Derived> Scaling(const MatrixBase<Derived>& coeffs) -{ return coeffs.asDiagonal(); } - -/** \deprecated */ -typedef DiagonalMatrix<float, 2> AlignedScaling2f; -/** \deprecated */ -typedef DiagonalMatrix<double,2> AlignedScaling2d; -/** \deprecated */ -typedef DiagonalMatrix<float, 3> AlignedScaling3f; -/** \deprecated */ -typedef DiagonalMatrix<double,3> AlignedScaling3d; -//@} - -template<typename Scalar> -template<int Dim> -inline Transform<Scalar,Dim,Affine> -UniformScaling<Scalar>::operator* (const Translation<Scalar,Dim>& t) const -{ - Transform<Scalar,Dim,Affine> res; - res.matrix().setZero(); - res.linear().diagonal().fill(factor()); - res.translation() = factor() * t.vector(); - res(Dim,Dim) = Scalar(1); - return res; -} - -} // end namespace Eigen - -#endif // EIGEN_SCALING_H diff --git a/eigen/Eigen/src/Geometry/Transform.h b/eigen/Eigen/src/Geometry/Transform.h deleted file mode 100644 index 3f31ee4..0000000 --- a/eigen/Eigen/src/Geometry/Transform.h +++ /dev/null @@ -1,1542 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr> -// Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com> -// Copyright (C) 2010 Hauke Heibel <hauke.heibel@gmail.com> -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_TRANSFORM_H -#define EIGEN_TRANSFORM_H - -namespace Eigen { - -namespace internal { - -template<typename Transform> -struct transform_traits -{ - enum - { - Dim = Transform::Dim, - HDim = Transform::HDim, - Mode = Transform::Mode, - IsProjective = (int(Mode)==int(Projective)) - }; -}; - -template< typename TransformType, - typename MatrixType, - int Case = transform_traits<TransformType>::IsProjective ? 0 - : int(MatrixType::RowsAtCompileTime) == int(transform_traits<TransformType>::HDim) ? 1 - : 2, - int RhsCols = MatrixType::ColsAtCompileTime> -struct transform_right_product_impl; - -template< typename Other, - int Mode, - int Options, - int Dim, - int HDim, - int OtherRows=Other::RowsAtCompileTime, - int OtherCols=Other::ColsAtCompileTime> -struct transform_left_product_impl; - -template< typename Lhs, - typename Rhs, - bool AnyProjective = - transform_traits<Lhs>::IsProjective || - transform_traits<Rhs>::IsProjective> -struct transform_transform_product_impl; - -template< typename Other, - int Mode, - int Options, - int Dim, - int HDim, - int OtherRows=Other::RowsAtCompileTime, - int OtherCols=Other::ColsAtCompileTime> -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 - -/** \geometry_module \ingroup Geometry_Module - * - * \class Transform - * - * \brief Represents an homogeneous transformation in a N dimensional space - * - * \tparam _Scalar the scalar type, i.e., the type of the coefficients - * \tparam _Dim the dimension of the space - * \tparam _Mode the type of the transformation. Can be: - * - #Affine: the transformation is stored as a (Dim+1)^2 matrix, - * where the last row is assumed to be [0 ... 0 1]. - * - #AffineCompact: the transformation is stored as a (Dim)x(Dim+1) matrix. - * - #Projective: the transformation is stored as a (Dim+1)^2 matrix - * without any assumption. - * \tparam _Options has the same meaning as in class Matrix. It allows to specify DontAlign and/or RowMajor. - * These Options are passed directly to the underlying matrix type. - * - * The homography is internally represented and stored by a matrix which - * is available through the matrix() method. To understand the behavior of - * this class you have to think a Transform object as its internal - * matrix representation. The chosen convention is right multiply: - * - * \code v' = T * v \endcode - * - * Therefore, an affine transformation matrix M is shaped like this: - * - * \f$ \left( \begin{array}{cc} - * linear & translation\\ - * 0 ... 0 & 1 - * \end{array} \right) \f$ - * - * Note that for a projective transformation the last row can be anything, - * and then the interpretation of different parts might be sightly different. - * - * However, unlike a plain matrix, the Transform class provides many features - * simplifying both its assembly and usage. In particular, it can be composed - * with any other transformations (Transform,Translation,RotationBase,DiagonalMatrix) - * and can be directly used to transform implicit homogeneous vectors. All these - * operations are handled via the operator*. For the composition of transformations, - * its principle consists to first convert the right/left hand sides of the product - * to a compatible (Dim+1)^2 matrix and then perform a pure matrix product. - * Of course, internally, operator* tries to perform the minimal number of operations - * according to the nature of each terms. Likewise, when applying the transform - * to points, the latters are automatically promoted to homogeneous vectors - * before doing the matrix product. The conventions to homogeneous representations - * are performed as follow: - * - * \b Translation t (Dim)x(1): - * \f$ \left( \begin{array}{cc} - * I & t \\ - * 0\,...\,0 & 1 - * \end{array} \right) \f$ - * - * \b Rotation R (Dim)x(Dim): - * \f$ \left( \begin{array}{cc} - * R & 0\\ - * 0\,...\,0 & 1 - * \end{array} \right) \f$ - *<!-- - * \b Linear \b Matrix L (Dim)x(Dim): - * \f$ \left( \begin{array}{cc} - * L & 0\\ - * 0\,...\,0 & 1 - * \end{array} \right) \f$ - * - * \b Affine \b Matrix A (Dim)x(Dim+1): - * \f$ \left( \begin{array}{c} - * A\\ - * 0\,...\,0\,1 - * \end{array} \right) \f$ - *--> - * \b Scaling \b DiagonalMatrix S (Dim)x(Dim): - * \f$ \left( \begin{array}{cc} - * S & 0\\ - * 0\,...\,0 & 1 - * \end{array} \right) \f$ - * - * \b Column \b point v (Dim)x(1): - * \f$ \left( \begin{array}{c} - * v\\ - * 1 - * \end{array} \right) \f$ - * - * \b Set \b of \b column \b points V1...Vn (Dim)x(n): - * \f$ \left( \begin{array}{ccc} - * v_1 & ... & v_n\\ - * 1 & ... & 1 - * \end{array} \right) \f$ - * - * The concatenation of a Transform object with any kind of other transformation - * always returns a Transform object. - * - * A little exception to the "as pure matrix product" rule is the case of the - * transformation of non homogeneous vectors by an affine transformation. In - * that case the last matrix row can be ignored, and the product returns non - * homogeneous vectors. - * - * Since, for instance, a Dim x Dim matrix is interpreted as a linear transformation, - * it is not possible to directly transform Dim vectors stored in a Dim x Dim matrix. - * The solution is either to use a Dim x Dynamic matrix or explicitly request a - * vector transformation by making the vector homogeneous: - * \code - * m' = T * m.colwise().homogeneous(); - * \endcode - * Note that there is zero overhead. - * - * Conversion methods from/to Qt's QMatrix and QTransform are available if the - * preprocessor token EIGEN_QT_SUPPORT is defined. - * - * This class can be extended with the help of the plugin mechanism described on the page - * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_TRANSFORM_PLUGIN. - * - * \sa class Matrix, class Quaternion - */ -template<typename _Scalar, int _Dim, int _Mode, int _Options> -class Transform -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_Dim==Dynamic ? Dynamic : (_Dim+1)*(_Dim+1)) - enum { - Mode = _Mode, - Options = _Options, - Dim = _Dim, ///< space dimension in which the transformation holds - HDim = _Dim+1, ///< size of a respective homogeneous vector - Rows = int(Mode)==(AffineCompact) ? Dim : HDim - }; - /** the scalar type of the coefficients */ - typedef _Scalar Scalar; - 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 */ - typedef const MatrixType ConstMatrixType; - /** type of the matrix used to represent the linear part of the transformation */ - typedef Matrix<Scalar,Dim,Dim,Options> LinearMatrixType; - /** type of read/write reference to the linear part of the transformation */ - typedef Block<MatrixType,Dim,Dim,int(Mode)==(AffineCompact) && (Options&RowMajor)==0> LinearPart; - /** type of read reference to the linear part of the transformation */ - typedef const Block<ConstMatrixType,Dim,Dim,int(Mode)==(AffineCompact) && (Options&RowMajor)==0> ConstLinearPart; - /** type of read/write reference to the affine part of the transformation */ - typedef typename internal::conditional<int(Mode)==int(AffineCompact), - MatrixType&, - Block<MatrixType,Dim,HDim> >::type AffinePart; - /** type of read reference to the affine part of the transformation */ - typedef typename internal::conditional<int(Mode)==int(AffineCompact), - const MatrixType&, - const Block<const MatrixType,Dim,HDim> >::type ConstAffinePart; - /** 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,!(internal::traits<MatrixType>::Flags & RowMajorBit)> TranslationPart; - /** type of a read reference to the translation part of the rotation */ - typedef const Block<ConstMatrixType,Dim,1,!(internal::traits<MatrixType>::Flags & RowMajorBit)> ConstTranslationPart; - /** corresponding translation type */ - typedef Translation<Scalar,Dim> TranslationType; - - // this intermediate enum is needed to avoid an ICE with gcc 3.4 and 4.0 - enum { TransformTimeDiagonalMode = ((Mode==int(Isometry))?Affine:int(Mode)) }; - /** The return type of the product between a diagonal matrix and a transform */ - typedef Transform<Scalar,Dim,TransformTimeDiagonalMode> TransformTimeDiagonalReturnType; - -protected: - - MatrixType m_matrix; - -public: - - /** Default constructor without initialization of the meaningful coefficients. - * If Mode==Affine, then the last row is set to [0 ... 0 1] */ - EIGEN_DEVICE_FUNC inline Transform() - { - check_template_params(); - internal::transform_make_affine<(int(Mode)==Affine) ? Affine : AffineCompact>::run(m_matrix); - } - - EIGEN_DEVICE_FUNC inline Transform(const Transform& other) - { - check_template_params(); - m_matrix = other.m_matrix; - } - - EIGEN_DEVICE_FUNC inline explicit Transform(const TranslationType& t) - { - check_template_params(); - *this = t; - } - EIGEN_DEVICE_FUNC inline explicit Transform(const UniformScaling<Scalar>& s) - { - check_template_params(); - *this = s; - } - template<typename Derived> - EIGEN_DEVICE_FUNC inline explicit Transform(const RotationBase<Derived, Dim>& r) - { - check_template_params(); - *this = r; - } - - 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> - 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); - - check_template_params(); - internal::transform_construct_from_matrix<OtherDerived,Mode,Options,Dim,HDim>::run(this, other.derived()); - } - - /** Set \c *this from a Dim^2 or (Dim+1)^2 matrix. */ - template<typename OtherDerived> - 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); - - internal::transform_construct_from_matrix<OtherDerived,Mode,Options,Dim,HDim>::run(this, other.derived()); - return *this; - } - - template<int OtherOptions> - 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 - m_matrix = other.matrix(); - } - - template<int OtherMode,int OtherOptions> - EIGEN_DEVICE_FUNC inline Transform(const Transform<Scalar,Dim,OtherMode,OtherOptions>& other) - { - check_template_params(); - // prevent conversions as: - // Affine | AffineCompact | Isometry = Projective - EIGEN_STATIC_ASSERT(EIGEN_IMPLIES(OtherMode==int(Projective), Mode==int(Projective)), - YOU_PERFORMED_AN_INVALID_TRANSFORMATION_CONVERSION) - - // prevent conversions as: - // Isometry = Affine | AffineCompact - EIGEN_STATIC_ASSERT(EIGEN_IMPLIES(OtherMode==int(Affine)||OtherMode==int(AffineCompact), Mode!=int(Isometry)), - YOU_PERFORMED_AN_INVALID_TRANSFORMATION_CONVERSION) - - enum { ModeIsAffineCompact = Mode == int(AffineCompact), - OtherModeIsAffineCompact = OtherMode == int(AffineCompact) - }; - - if(ModeIsAffineCompact == OtherModeIsAffineCompact) - { - // We need the block expression because the code is compiled for all - // combinations of transformations and will trigger a compile time error - // if one tries to assign the matrices directly - m_matrix.template block<Dim,Dim+1>(0,0) = other.matrix().template block<Dim,Dim+1>(0,0); - makeAffine(); - } - else if(OtherModeIsAffineCompact) - { - typedef typename Transform<Scalar,Dim,OtherMode,OtherOptions>::MatrixType OtherMatrixType; - internal::transform_construct_from_matrix<OtherMatrixType,Mode,Options,Dim,HDim>::run(this, other.matrix()); - } - else - { - // here we know that Mode == AffineCompact and OtherMode != AffineCompact. - // if OtherMode were Projective, the static assert above would already have caught it. - // So the only possibility is that OtherMode == Affine - linear() = other.linear(); - translation() = other.translation(); - } - } - - template<typename OtherDerived> - EIGEN_DEVICE_FUNC Transform(const ReturnByValue<OtherDerived>& other) - { - check_template_params(); - other.evalTo(*this); - } - - template<typename OtherDerived> - EIGEN_DEVICE_FUNC Transform& operator=(const ReturnByValue<OtherDerived>& other) - { - other.evalTo(*this); - return *this; - } - - #ifdef EIGEN_QT_SUPPORT - inline Transform(const QMatrix& other); - inline Transform& operator=(const QMatrix& other); - inline QMatrix toQMatrix(void) const; - inline Transform(const QTransform& other); - 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 */ - 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) */ - EIGEN_DEVICE_FUNC inline Scalar& operator() (Index row, Index col) { return m_matrix(row,col); } - - /** \returns a read-only expression of the transformation matrix */ - EIGEN_DEVICE_FUNC inline const MatrixType& matrix() const { return m_matrix; } - /** \returns a writable expression of the transformation matrix */ - EIGEN_DEVICE_FUNC inline MatrixType& matrix() { return m_matrix; } - - /** \returns a read-only expression of the linear part of the transformation */ - EIGEN_DEVICE_FUNC inline ConstLinearPart linear() const { return ConstLinearPart(m_matrix,0,0); } - /** \returns a writable expression of the linear part of the transformation */ - 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 */ - 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 */ - 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 */ - EIGEN_DEVICE_FUNC inline ConstTranslationPart translation() const { return ConstTranslationPart(m_matrix,0,Dim); } - /** \returns a writable expression of the translation vector of the transformation */ - 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. - * - * The right-hand-side \a other can be either: - * \li an homogeneous vector of size Dim+1, - * \li a set of homogeneous vectors of size Dim+1 x N, - * \li a transformation matrix of size Dim+1 x Dim+1. - * - * Moreover, if \c *this represents an affine transformation (i.e., Mode!=Projective), then \a other can also be: - * \li a point of size Dim (computes: \code this->linear() * other + this->translation()\endcode), - * \li a set of N points as a Dim x N matrix (computes: \code (this->linear() * other).colwise() + this->translation()\endcode), - * - * In all cases, the return type is a matrix or vector of same sizes as the right-hand-side \a other. - * - * If you want to interpret \a other as a linear or affine transformation, then first convert it to a Transform<> type, - * or do your own cooking. - * - * Finally, if you want to apply Affine transformations to vectors, then explicitly apply the linear part only: - * \code - * Affine3f A; - * Vector3f v1, v2; - * v2 = A.linear() * v1; - * \endcode - * - */ - // note: this function is defined here because some compilers cannot find the respective declaration - template<typename OtherDerived> - 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()); } - - /** \returns the product expression of a transformation matrix \a a times a transform \a b - * - * The left hand side \a other can be either: - * \li a linear transformation matrix of size Dim x Dim, - * \li an affine transformation matrix of size Dim x Dim+1, - * \li a general transformation matrix of size Dim+1 x Dim+1. - */ - template<typename OtherDerived> friend - 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); } - - /** \returns The product expression of a transform \a a times a diagonal matrix \a b - * - * The rhs diagonal matrix is interpreted as an affine scaling transformation. The - * product results in a Transform of the same type (mode) as the lhs only if the lhs - * mode is no isometry. In that case, the returned transform is an affinity. - */ - template<typename DiagonalDerived> - EIGEN_DEVICE_FUNC inline const TransformTimeDiagonalReturnType - operator * (const DiagonalBase<DiagonalDerived> &b) const - { - TransformTimeDiagonalReturnType res(*this); - res.linearExt() *= b; - return res; - } - - /** \returns The product expression of a diagonal matrix \a a times a transform \a b - * - * The lhs diagonal matrix is interpreted as an affine scaling transformation. The - * product results in a Transform of the same type (mode) as the lhs only if the lhs - * mode is no isometry. In that case, the returned transform is an affinity. - */ - template<typename DiagonalDerived> - 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)) - res.matrix().row(Dim) = b.matrix().row(Dim); - return res; - } - - template<typename OtherDerived> - EIGEN_DEVICE_FUNC inline Transform& operator*=(const EigenBase<OtherDerived>& other) { return *this = *this * other; } - - /** Concatenates two transformations */ - EIGEN_DEVICE_FUNC inline const Transform operator * (const Transform& other) const - { - return internal::transform_transform_product_impl<Transform,Transform>::run(*this,other); - } - - #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> - // (const Eigen::Transform<double, 3, 2, 0> &) const" - // (the meaning of a name may have changed since the template declaration -- the type of the template is: - // "Eigen::internal::transform_transform_product_impl<Eigen::Transform<double, 3, 32, 0>, - // Eigen::Transform<double, 3, Mode, Options>, <expression>>::ResultType (const Eigen::Transform<double, 3, Mode, Options> &) const") - // - template<int OtherMode,int OtherOptions> struct icc_11_workaround - { - typedef internal::transform_transform_product_impl<Transform,Transform<Scalar,Dim,OtherMode,OtherOptions> > ProductType; - typedef typename ProductType::ResultType ResultType; - }; - -public: - /** Concatenates two different transformations */ - template<int OtherMode,int OtherOptions> - inline typename icc_11_workaround<OtherMode,OtherOptions>::ResultType - operator * (const Transform<Scalar,Dim,OtherMode,OtherOptions>& other) const - { - typedef typename icc_11_workaround<OtherMode,OtherOptions>::ProductType ProductType; - return ProductType::run(*this,other); - } - #else - /** Concatenates two different transformations */ - template<int OtherMode,int OtherOptions> - 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); - } - #endif - - /** \sa MatrixBase::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. - */ - 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); - - 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); - - EIGEN_DEVICE_FUNC Transform& shear(const Scalar& sx, const Scalar& sy); - EIGEN_DEVICE_FUNC Transform& preshear(const Scalar& sx, const Scalar& sy); - - EIGEN_DEVICE_FUNC inline Transform& operator=(const TranslationType& t); - - EIGEN_DEVICE_FUNC - inline Transform& operator*=(const TranslationType& t) { return translate(t.vector()); } - - 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()); } - - EIGEN_DEVICE_FUNC - inline TransformTimeDiagonalReturnType operator*(const UniformScaling<Scalar>& s) const - { - TransformTimeDiagonalReturnType res = *this; - res.scale(s.factor()); - return res; - } - - EIGEN_DEVICE_FUNC - inline Transform& operator*=(const DiagonalMatrix<Scalar,Dim>& s) { linearExt() *= s; return *this; } - - template<typename Derived> - EIGEN_DEVICE_FUNC inline Transform& operator=(const RotationBase<Derived,Dim>& r); - template<typename Derived> - EIGEN_DEVICE_FUNC inline Transform& operator*=(const RotationBase<Derived,Dim>& r) { return rotate(r.toRotationMatrix()); } - template<typename Derived> - EIGEN_DEVICE_FUNC inline Transform operator*(const RotationBase<Derived,Dim>& r) 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 */ - EIGEN_DEVICE_FUNC const Scalar* data() const { return m_matrix.data(); } - /** \returns a non-const pointer to the column major internal matrix */ - EIGEN_DEVICE_FUNC Scalar* data() { return m_matrix.data(); } - - /** \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> - 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> - EIGEN_DEVICE_FUNC inline explicit Transform(const Transform<OtherScalarType,Dim,Mode,Options>& other) - { - check_template_params(); - m_matrix = other.matrix().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() */ - 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] - */ - EIGEN_DEVICE_FUNC void makeAffine() - { - internal::transform_make_affine<int(Mode)>::run(m_matrix); - } - - /** \internal - * \returns the Dim x Dim linear part if the transformation is affine, - * and the HDim x Dim part for projective transformations. - */ - 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. - */ - 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. - */ - 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. - */ - 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); } - - - #ifdef EIGEN_TRANSFORM_PLUGIN - #include EIGEN_TRANSFORM_PLUGIN - #endif - -protected: - #ifndef EIGEN_PARSED_BY_DOXYGEN - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void check_template_params() - { - EIGEN_STATIC_ASSERT((Options & (DontAlign|RowMajor)) == Options, INVALID_MATRIX_TEMPLATE_PARAMETERS) - } - #endif - -}; - -/** \ingroup Geometry_Module */ -typedef Transform<float,2,Isometry> Isometry2f; -/** \ingroup Geometry_Module */ -typedef Transform<float,3,Isometry> Isometry3f; -/** \ingroup Geometry_Module */ -typedef Transform<double,2,Isometry> Isometry2d; -/** \ingroup Geometry_Module */ -typedef Transform<double,3,Isometry> Isometry3d; - -/** \ingroup Geometry_Module */ -typedef Transform<float,2,Affine> Affine2f; -/** \ingroup Geometry_Module */ -typedef Transform<float,3,Affine> Affine3f; -/** \ingroup Geometry_Module */ -typedef Transform<double,2,Affine> Affine2d; -/** \ingroup Geometry_Module */ -typedef Transform<double,3,Affine> Affine3d; - -/** \ingroup Geometry_Module */ -typedef Transform<float,2,AffineCompact> AffineCompact2f; -/** \ingroup Geometry_Module */ -typedef Transform<float,3,AffineCompact> AffineCompact3f; -/** \ingroup Geometry_Module */ -typedef Transform<double,2,AffineCompact> AffineCompact2d; -/** \ingroup Geometry_Module */ -typedef Transform<double,3,AffineCompact> AffineCompact3d; - -/** \ingroup Geometry_Module */ -typedef Transform<float,2,Projective> Projective2f; -/** \ingroup Geometry_Module */ -typedef Transform<float,3,Projective> Projective3f; -/** \ingroup Geometry_Module */ -typedef Transform<double,2,Projective> Projective2d; -/** \ingroup Geometry_Module */ -typedef Transform<double,3,Projective> Projective3d; - -/************************** -*** Optional QT support *** -**************************/ - -#ifdef EIGEN_QT_SUPPORT -/** Initializes \c *this from a QMatrix assuming the dimension is 2. - * - * This function is available only if the token EIGEN_QT_SUPPORT is defined. - */ -template<typename Scalar, int Dim, int Mode,int Options> -Transform<Scalar,Dim,Mode,Options>::Transform(const QMatrix& other) -{ - check_template_params(); - *this = other; -} - -/** Set \c *this from a QMatrix assuming the dimension is 2. - * - * This function is available only if the token EIGEN_QT_SUPPORT is defined. - */ -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) - if (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; -} - -/** \returns a QMatrix from \c *this assuming the dimension is 2. - * - * \warning this conversion might loss data if \c *this is not affine - * - * This function is available only if the token EIGEN_QT_SUPPORT is defined. - */ -template<typename Scalar, int Dim, int Mode, int Options> -QMatrix Transform<Scalar,Dim,Mode,Options>::toQMatrix(void) const -{ - check_template_params(); - EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE) - return QMatrix(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)); -} - -/** Initializes \c *this from a QTransform assuming the dimension is 2. - * - * This function is available only if the token EIGEN_QT_SUPPORT is defined. - */ -template<typename Scalar, int Dim, int Mode,int Options> -Transform<Scalar,Dim,Mode,Options>::Transform(const QTransform& other) -{ - check_template_params(); - *this = other; -} - -/** Set \c *this from a QTransform assuming the dimension is 2. - * - * This function is available only if the token EIGEN_QT_SUPPORT is defined. - */ -template<typename Scalar, int Dim, int Mode, int Options> -Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::operator=(const QTransform& other) -{ - check_template_params(); - EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE) - if (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(), - other.m13(), other.m23(), other.m33(); - return *this; -} - -/** \returns a QTransform from \c *this assuming the dimension is 2. - * - * This function is available only if the token EIGEN_QT_SUPPORT is defined. - */ -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)) - 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)); - else - return QTransform(m_matrix.coeff(0,0), m_matrix.coeff(1,0), m_matrix.coeff(2,0), - m_matrix.coeff(0,1), m_matrix.coeff(1,1), m_matrix.coeff(2,1), - m_matrix.coeff(0,2), m_matrix.coeff(1,2), m_matrix.coeff(2,2)); -} -#endif - -/********************* -*** Procedural API *** -*********************/ - -/** Applies on the right the non uniform scale transformation represented - * by the vector \a other to \c *this and returns a reference to \c *this. - * \sa prescale() - */ -template<typename Scalar, int Dim, int Mode, int Options> -template<typename OtherDerived> -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)) - EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS) - linearExt().noalias() = (linearExt() * other.asDiagonal()); - return *this; -} - -/** Applies on the right a uniform scale of a factor \a c to \c *this - * and returns a reference to \c *this. - * \sa prescale(Scalar) - */ -template<typename Scalar, int Dim, int Mode, int Options> -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; - return *this; -} - -/** Applies on the left the non uniform scale transformation represented - * by the vector \a other to \c *this and returns a reference to \c *this. - * \sa scale() - */ -template<typename Scalar, int Dim, int Mode, int Options> -template<typename OtherDerived> -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) - affine().noalias() = (other.asDiagonal() * affine()); - return *this; -} - -/** Applies on the left a uniform scale of a factor \a c to \c *this - * and returns a reference to \c *this. - * \sa scale(Scalar) - */ -template<typename Scalar, int Dim, int Mode, int Options> -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; - return *this; -} - -/** Applies on the right the translation matrix represented by the vector \a other - * to \c *this and returns a reference to \c *this. - * \sa pretranslate() - */ -template<typename Scalar, int Dim, int Mode, int Options> -template<typename OtherDerived> -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)) - translationExt() += linearExt() * other; - return *this; -} - -/** Applies on the left the translation matrix represented by the vector \a other - * to \c *this and returns a reference to \c *this. - * \sa translate() - */ -template<typename Scalar, int Dim, int Mode, int Options> -template<typename OtherDerived> -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)) - affine() += other * m_matrix.row(Dim); - else - translation() += other; - return *this; -} - -/** Applies on the right the rotation represented by the rotation \a rotation - * to \c *this and returns a reference to \c *this. - * - * The template parameter \a RotationType is the type of the rotation which - * must be known by internal::toRotationMatrix<>. - * - * Natively supported types includes: - * - any scalar (2D), - * - a Dim x Dim matrix expression, - * - a Quaternion (3D), - * - a AngleAxis (3D) - * - * This mechanism is easily extendable to support user types such as Euler angles, - * or a pair of Quaternion for 4D rotations. - * - * \sa rotate(Scalar), class Quaternion, class AngleAxis, prerotate(RotationType) - */ -template<typename Scalar, int Dim, int Mode, int Options> -template<typename RotationType> -EIGEN_DEVICE_FUNC Transform<Scalar,Dim,Mode,Options>& -Transform<Scalar,Dim,Mode,Options>::rotate(const RotationType& rotation) -{ - linearExt() *= internal::toRotationMatrix<Scalar,Dim>(rotation); - return *this; -} - -/** Applies on the left the rotation represented by the rotation \a rotation - * to \c *this and returns a reference to \c *this. - * - * See rotate() for further details. - * - * \sa rotate() - */ -template<typename Scalar, int Dim, int Mode, int Options> -template<typename RotationType> -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) - * m_matrix.template block<Dim,HDim>(0,0); - return *this; -} - -/** Applies on the right the shear transformation represented - * by the vector \a other to \c *this and returns a reference to \c *this. - * \warning 2D only. - * \sa preshear() - */ -template<typename Scalar, int Dim, int Mode, int 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) - EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS) - VectorType tmp = linear().col(0)*sy + linear().col(1); - linear() << linear().col(0) + linear().col(1)*sx, tmp; - return *this; -} - -/** Applies on the left the shear transformation represented - * by the vector \a other to \c *this and returns a reference to \c *this. - * \warning 2D only. - * \sa shear() - */ -template<typename Scalar, int Dim, int Mode, int 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) - EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS) - m_matrix.template block<Dim,HDim>(0,0) = LinearMatrixType(1, sx, sy, 1) * m_matrix.template block<Dim,HDim>(0,0); - return *this; -} - -/****************************************************** -*** Scaling, Translation and Rotation compatibility *** -******************************************************/ - -template<typename Scalar, int Dim, int Mode, int Options> -EIGEN_DEVICE_FUNC inline Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::operator=(const TranslationType& t) -{ - linear().setIdentity(); - translation() = t.vector(); - makeAffine(); - return *this; -} - -template<typename Scalar, int Dim, int Mode, int Options> -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()); - return res; -} - -template<typename Scalar, int Dim, int Mode, int Options> -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()); - makeAffine(); - return *this; -} - -template<typename Scalar, int Dim, int Mode, int Options> -template<typename Derived> -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(); - makeAffine(); - return *this; -} - -template<typename Scalar, int Dim, int Mode, int Options> -template<typename Derived> -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()); - return res; -} - -/************************ -*** Special functions *** -************************/ - -/** \returns the rotation part of the transformation - * - * - * \svd_module - * - * \sa computeRotationScaling(), computeScalingRotation(), class SVD - */ -template<typename Scalar, int Dim, int Mode, int Options> -EIGEN_DEVICE_FUNC const typename Transform<Scalar,Dim,Mode,Options>::LinearMatrixType -Transform<Scalar,Dim,Mode,Options>::rotation() const -{ - LinearMatrixType result; - computeRotationScaling(&result, (LinearMatrixType*)0); - return result; -} - - -/** decomposes the linear part of the transformation as a product rotation x scaling, the scaling being - * not necessarily positive. - * - * If either pointer is zero, the corresponding computation is skipped. - * - * - * - * \svd_module - * - * \sa computeScalingRotation(), rotation(), class SVD - */ -template<typename Scalar, int Dim, int Mode, int Options> -template<typename RotationMatrixType, typename ScalingMatrixType> -EIGEN_DEVICE_FUNC void Transform<Scalar,Dim,Mode,Options>::computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const -{ - JacobiSVD<LinearMatrixType> svd(linear(), ComputeFullU | ComputeFullV); - - Scalar x = (svd.matrixU() * svd.matrixV().adjoint()).determinant(); // so x has absolute value 1 - VectorType sv(svd.singularValues()); - sv.coeffRef(0) *= x; - if(scaling) scaling->lazyAssign(svd.matrixV() * sv.asDiagonal() * svd.matrixV().adjoint()); - if(rotation) - { - LinearMatrixType m(svd.matrixU()); - m.col(0) /= x; - rotation->lazyAssign(m * svd.matrixV().adjoint()); - } -} - -/** 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. - * - * - * - * \svd_module - * - * \sa computeRotationScaling(), rotation(), class SVD - */ -template<typename Scalar, int Dim, int Mode, int Options> -template<typename ScalingMatrixType, typename RotationMatrixType> -EIGEN_DEVICE_FUNC void Transform<Scalar,Dim,Mode,Options>::computeScalingRotation(ScalingMatrixType *scaling, RotationMatrixType *rotation) const -{ - JacobiSVD<LinearMatrixType> svd(linear(), ComputeFullU | ComputeFullV); - - Scalar x = (svd.matrixU() * svd.matrixV().adjoint()).determinant(); // so x has absolute value 1 - VectorType sv(svd.singularValues()); - sv.coeffRef(0) *= x; - if(scaling) scaling->lazyAssign(svd.matrixU() * sv.asDiagonal() * svd.matrixU().adjoint()); - if(rotation) - { - LinearMatrixType m(svd.matrixU()); - m.col(0) /= x; - rotation->lazyAssign(m * svd.matrixV().adjoint()); - } -} - -/** Convenient method to set \c *this from a position, orientation and scale - * of a 3D object. - */ -template<typename Scalar, int Dim, int Mode, int Options> -template<typename PositionDerived, typename OrientationType, typename ScaleDerived> -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) -{ - linear() = internal::toRotationMatrix<Scalar,Dim>(orientation); - linear() *= scale.asDiagonal(); - translation() = position; - makeAffine(); - return *this; -} - -namespace internal { - -template<int Mode> -struct transform_make_affine -{ - template<typename MatrixType> - EIGEN_DEVICE_FUNC static void run(MatrixType &mat) - { - static const int Dim = MatrixType::ColsAtCompileTime-1; - mat.template block<1,Dim>(Dim,0).setZero(); - mat.coeffRef(Dim,Dim) = typename MatrixType::Scalar(1); - } -}; - -template<> -struct transform_make_affine<AffineCompact> -{ - 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 -{ - EIGEN_DEVICE_FUNC static inline void run(const TransformType&, TransformType&) - {} -}; - -template<typename TransformType> -struct projective_transform_inverse<TransformType, Projective> -{ - EIGEN_DEVICE_FUNC static inline void run(const TransformType& m, TransformType& res) - { - res.matrix() = m.matrix().inverse(); - } -}; - -} // end namespace internal - - -/** - * - * \returns the inverse transformation according to some given knowledge - * on \c *this. - * - * \param hint allows to optimize the inversion process when the transformation - * is known to be not a general transformation (optional). The possible values are: - * - #Projective if the transformation is not necessarily affine, i.e., if the - * last row is not guaranteed to be [0 ... 0 1] - * - #Affine if the last row can be assumed to be [0 ... 0 1] - * - #Isometry if the transformation is only a concatenations of translations - * and rotations. - * The default is the template class parameter \c Mode. - * - * \warning unless \a traits is always set to NoShear or NoScaling, this function - * requires the generic inverse method of MatrixBase defined in the LU module. If - * you forget to include this module, then you will get hard to debug linking errors. - * - * \sa MatrixBase::inverse() - */ -template<typename Scalar, int Dim, int Mode, int Options> -EIGEN_DEVICE_FUNC Transform<Scalar,Dim,Mode,Options> -Transform<Scalar,Dim,Mode,Options>::inverse(TransformTraits hint) const -{ - Transform res; - if (hint == Projective) - { - internal::projective_transform_inverse<Transform>::run(*this, res); - } - else - { - if (hint == Isometry) - { - res.matrix().template topLeftCorner<Dim,Dim>() = linear().transpose(); - } - else if(hint&Affine) - { - res.matrix().template topLeftCorner<Dim,Dim>() = linear().inverse(); - } - else - { - eigen_assert(false && "Invalid transform traits in Transform::Inverse"); - } - // translation and remaining parts - res.matrix().template topRightCorner<Dim,1>() - = - res.matrix().template topLeftCorner<Dim,Dim>() * translation(); - res.makeAffine(); // we do need this, because in the beginning res is uninitialized - } - return res; -} - -namespace internal { - -/***************************************************** -*** Specializations of take affine part *** -*****************************************************/ - -template<typename TransformType> struct transform_take_affine_part { - typedef typename TransformType::MatrixType MatrixType; - typedef typename TransformType::AffinePart AffinePart; - typedef typename TransformType::ConstAffinePart ConstAffinePart; - static inline AffinePart run(MatrixType& m) - { return m.template block<TransformType::Dim,TransformType::HDim>(0,0); } - static inline ConstAffinePart run(const MatrixType& m) - { return m.template block<TransformType::Dim,TransformType::HDim>(0,0); } -}; - -template<typename Scalar, int Dim, int Options> -struct transform_take_affine_part<Transform<Scalar,Dim,AffineCompact, Options> > { - typedef typename Transform<Scalar,Dim,AffineCompact,Options>::MatrixType MatrixType; - static inline MatrixType& run(MatrixType& m) { return m; } - static inline const MatrixType& run(const MatrixType& m) { return m; } -}; - -/***************************************************** -*** Specializations of construct from matrix *** -*****************************************************/ - -template<typename Other, int Mode, int Options, int Dim, int HDim> -struct transform_construct_from_matrix<Other, Mode,Options,Dim,HDim, Dim,Dim> -{ - static inline void run(Transform<typename Other::Scalar,Dim,Mode,Options> *transform, const Other& other) - { - transform->linear() = other; - transform->translation().setZero(); - transform->makeAffine(); - } -}; - -template<typename Other, int Mode, int Options, int Dim, int HDim> -struct transform_construct_from_matrix<Other, Mode,Options,Dim,HDim, Dim,HDim> -{ - static inline void run(Transform<typename Other::Scalar,Dim,Mode,Options> *transform, const Other& other) - { - transform->affine() = other; - transform->makeAffine(); - } -}; - -template<typename Other, int Mode, int Options, int Dim, int HDim> -struct transform_construct_from_matrix<Other, Mode,Options,Dim,HDim, HDim,HDim> -{ - static inline void run(Transform<typename Other::Scalar,Dim,Mode,Options> *transform, const Other& other) - { transform->matrix() = other; } -}; - -template<typename Other, int Options, int Dim, int HDim> -struct transform_construct_from_matrix<Other, AffineCompact,Options,Dim,HDim, HDim,HDim> -{ - static inline void run(Transform<typename Other::Scalar,Dim,AffineCompact,Options> *transform, const Other& other) - { transform->matrix() = other.template block<Dim,HDim>(0,0); } -}; - -/********************************************************** -*** Specializations of operator* with rhs EigenBase *** -**********************************************************/ - -template<int LhsMode,int RhsMode> -struct transform_product_result -{ - enum - { - Mode = - (LhsMode == (int)Projective || RhsMode == (int)Projective ) ? Projective : - (LhsMode == (int)Affine || RhsMode == (int)Affine ) ? Affine : - (LhsMode == (int)AffineCompact || RhsMode == (int)AffineCompact ) ? AffineCompact : - (LhsMode == (int)Isometry || RhsMode == (int)Isometry ) ? Isometry : Projective - }; -}; - -template< typename TransformType, typename MatrixType, int RhsCols> -struct transform_right_product_impl< TransformType, MatrixType, 0, RhsCols> -{ - typedef typename MatrixType::PlainObject ResultType; - - static EIGEN_STRONG_INLINE ResultType run(const TransformType& T, const MatrixType& other) - { - return T.matrix() * other; - } -}; - -template< typename TransformType, typename MatrixType, int RhsCols> -struct transform_right_product_impl< TransformType, MatrixType, 1, RhsCols> -{ - enum { - Dim = TransformType::Dim, - HDim = TransformType::HDim, - OtherRows = MatrixType::RowsAtCompileTime, - OtherCols = MatrixType::ColsAtCompileTime - }; - - typedef typename MatrixType::PlainObject ResultType; - - static EIGEN_STRONG_INLINE ResultType run(const TransformType& T, const MatrixType& other) - { - EIGEN_STATIC_ASSERT(OtherRows==HDim, YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES); - - typedef Block<ResultType, Dim, OtherCols, int(MatrixType::RowsAtCompileTime)==Dim> TopLeftLhs; - - ResultType res(other.rows(),other.cols()); - TopLeftLhs(res, 0, 0, Dim, other.cols()).noalias() = T.affine() * other; - res.row(OtherRows-1) = other.row(OtherRows-1); - - return res; - } -}; - -template< typename TransformType, typename MatrixType, int RhsCols> -struct transform_right_product_impl< TransformType, MatrixType, 2, RhsCols> -{ - enum { - Dim = TransformType::Dim, - HDim = TransformType::HDim, - OtherRows = MatrixType::RowsAtCompileTime, - OtherCols = MatrixType::ColsAtCompileTime - }; - - 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); - - typedef Block<ResultType, Dim, OtherCols, true> TopLeftLhs; - ResultType res(Replicate<typename TransformType::ConstTranslationPart, 1, OtherCols>(T.translation(),1,other.cols())); - TopLeftLhs(res, 0, 0, Dim, other.cols()).noalias() += T.linear() * other; - - return res; - } -}; - -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 *** -**********************************************************/ - -// generic HDim x HDim matrix * T => Projective -template<typename Other,int Mode, int Options, int Dim, int HDim> -struct transform_left_product_impl<Other,Mode,Options,Dim,HDim, HDim,HDim> -{ - typedef Transform<typename Other::Scalar,Dim,Mode,Options> TransformType; - typedef typename TransformType::MatrixType MatrixType; - typedef Transform<typename Other::Scalar,Dim,Projective,Options> ResultType; - static ResultType run(const Other& other,const TransformType& tr) - { return ResultType(other * tr.matrix()); } -}; - -// generic HDim x HDim matrix * AffineCompact => Projective -template<typename Other, int Options, int Dim, int HDim> -struct transform_left_product_impl<Other,AffineCompact,Options,Dim,HDim, HDim,HDim> -{ - typedef Transform<typename Other::Scalar,Dim,AffineCompact,Options> TransformType; - typedef typename TransformType::MatrixType MatrixType; - typedef Transform<typename Other::Scalar,Dim,Projective,Options> ResultType; - static ResultType run(const Other& other,const TransformType& tr) - { - ResultType res; - res.matrix().noalias() = other.template block<HDim,Dim>(0,0) * tr.matrix(); - res.matrix().col(Dim) += other.col(Dim); - return res; - } -}; - -// affine matrix * T -template<typename Other,int Mode, int Options, int Dim, int HDim> -struct transform_left_product_impl<Other,Mode,Options,Dim,HDim, Dim,HDim> -{ - typedef Transform<typename Other::Scalar,Dim,Mode,Options> TransformType; - typedef typename TransformType::MatrixType MatrixType; - typedef TransformType ResultType; - static ResultType run(const Other& other,const TransformType& tr) - { - ResultType res; - res.affine().noalias() = other * tr.matrix(); - res.matrix().row(Dim) = tr.matrix().row(Dim); - return res; - } -}; - -// affine matrix * AffineCompact -template<typename Other, int Options, int Dim, int HDim> -struct transform_left_product_impl<Other,AffineCompact,Options,Dim,HDim, Dim,HDim> -{ - typedef Transform<typename Other::Scalar,Dim,AffineCompact,Options> TransformType; - typedef typename TransformType::MatrixType MatrixType; - typedef TransformType ResultType; - static ResultType run(const Other& other,const TransformType& tr) - { - ResultType res; - res.matrix().noalias() = other.template block<Dim,Dim>(0,0) * tr.matrix(); - res.translation() += other.col(Dim); - return res; - } -}; - -// linear matrix * T -template<typename Other,int Mode, int Options, int Dim, int HDim> -struct transform_left_product_impl<Other,Mode,Options,Dim,HDim, Dim,Dim> -{ - typedef Transform<typename Other::Scalar,Dim,Mode,Options> TransformType; - typedef typename TransformType::MatrixType MatrixType; - typedef TransformType ResultType; - static ResultType run(const Other& other, const TransformType& tr) - { - TransformType res; - if(Mode!=int(AffineCompact)) - res.matrix().row(Dim) = tr.matrix().row(Dim); - res.matrix().template topRows<Dim>().noalias() - = other * tr.matrix().template topRows<Dim>(); - return res; - } -}; - -/********************************************************** -*** Specializations of operator* with another Transform *** -**********************************************************/ - -template<typename Scalar, int Dim, int LhsMode, int LhsOptions, int RhsMode, int RhsOptions> -struct transform_transform_product_impl<Transform<Scalar,Dim,LhsMode,LhsOptions>,Transform<Scalar,Dim,RhsMode,RhsOptions>,false > -{ - enum { ResultMode = transform_product_result<LhsMode,RhsMode>::Mode }; - typedef Transform<Scalar,Dim,LhsMode,LhsOptions> Lhs; - typedef Transform<Scalar,Dim,RhsMode,RhsOptions> Rhs; - typedef Transform<Scalar,Dim,ResultMode,LhsOptions> ResultType; - static ResultType run(const Lhs& lhs, const Rhs& rhs) - { - ResultType res; - res.linear() = lhs.linear() * rhs.linear(); - res.translation() = lhs.linear() * rhs.translation() + lhs.translation(); - res.makeAffine(); - return res; - } -}; - -template<typename Scalar, int Dim, int LhsMode, int LhsOptions, int RhsMode, int RhsOptions> -struct transform_transform_product_impl<Transform<Scalar,Dim,LhsMode,LhsOptions>,Transform<Scalar,Dim,RhsMode,RhsOptions>,true > -{ - typedef Transform<Scalar,Dim,LhsMode,LhsOptions> Lhs; - typedef Transform<Scalar,Dim,RhsMode,RhsOptions> Rhs; - typedef Transform<Scalar,Dim,Projective> ResultType; - static ResultType run(const Lhs& lhs, const Rhs& rhs) - { - return ResultType( lhs.matrix() * rhs.matrix() ); - } -}; - -template<typename Scalar, int Dim, int LhsOptions, int RhsOptions> -struct transform_transform_product_impl<Transform<Scalar,Dim,AffineCompact,LhsOptions>,Transform<Scalar,Dim,Projective,RhsOptions>,true > -{ - typedef Transform<Scalar,Dim,AffineCompact,LhsOptions> Lhs; - typedef Transform<Scalar,Dim,Projective,RhsOptions> Rhs; - typedef Transform<Scalar,Dim,Projective> ResultType; - static ResultType run(const Lhs& lhs, const Rhs& rhs) - { - ResultType res; - res.matrix().template topRows<Dim>() = lhs.matrix() * rhs.matrix(); - res.matrix().row(Dim) = rhs.matrix().row(Dim); - return res; - } -}; - -template<typename Scalar, int Dim, int LhsOptions, int RhsOptions> -struct transform_transform_product_impl<Transform<Scalar,Dim,Projective,LhsOptions>,Transform<Scalar,Dim,AffineCompact,RhsOptions>,true > -{ - typedef Transform<Scalar,Dim,Projective,LhsOptions> Lhs; - typedef Transform<Scalar,Dim,AffineCompact,RhsOptions> Rhs; - typedef Transform<Scalar,Dim,Projective> ResultType; - static ResultType run(const Lhs& lhs, const Rhs& rhs) - { - ResultType res(lhs.matrix().template leftCols<Dim>() * rhs.matrix()); - res.matrix().col(Dim) += lhs.matrix().col(Dim); - return res; - } -}; - -} // end namespace internal - -} // end namespace Eigen - -#endif // EIGEN_TRANSFORM_H diff --git a/eigen/Eigen/src/Geometry/Translation.h b/eigen/Eigen/src/Geometry/Translation.h deleted file mode 100644 index 51d9a82..0000000 --- a/eigen/Eigen/src/Geometry/Translation.h +++ /dev/null @@ -1,208 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr> -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_TRANSLATION_H -#define EIGEN_TRANSLATION_H - -namespace Eigen { - -/** \geometry_module \ingroup Geometry_Module - * - * \class Translation - * - * \brief Represents a translation transformation - * - * \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. - * - * \sa class Scaling, class Transform - */ -template<typename _Scalar, int _Dim> -class Translation -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_Dim) - /** dimension of the space */ - enum { Dim = _Dim }; - /** the scalar type of the coefficients */ - typedef _Scalar Scalar; - /** corresponding vector type */ - typedef Matrix<Scalar,Dim,1> VectorType; - /** corresponding linear transformation matrix type */ - typedef Matrix<Scalar,Dim,Dim> LinearMatrixType; - /** corresponding affine transformation type */ - typedef Transform<Scalar,Dim,Affine> AffineTransformType; - /** corresponding isometric transformation type */ - typedef Transform<Scalar,Dim,Isometry> IsometryTransformType; - -protected: - - VectorType m_coeffs; - -public: - - /** Default constructor without initialization. */ - EIGEN_DEVICE_FUNC Translation() {} - /** */ - EIGEN_DEVICE_FUNC inline Translation(const Scalar& sx, const Scalar& sy) - { - eigen_assert(Dim==2); - m_coeffs.x() = sx; - m_coeffs.y() = sy; - } - /** */ - EIGEN_DEVICE_FUNC inline Translation(const Scalar& sx, const Scalar& sy, const Scalar& sz) - { - eigen_assert(Dim==3); - m_coeffs.x() = sx; - m_coeffs.y() = sy; - m_coeffs.z() = sz; - } - /** Constructs and initialize the translation transformation from a vector of translation coefficients */ - EIGEN_DEVICE_FUNC explicit inline Translation(const VectorType& vector) : m_coeffs(vector) {} - - /** \brief Retruns the x-translation by value. **/ - EIGEN_DEVICE_FUNC inline Scalar x() const { return m_coeffs.x(); } - /** \brief Retruns the y-translation by value. **/ - EIGEN_DEVICE_FUNC inline Scalar y() const { return m_coeffs.y(); } - /** \brief Retruns the z-translation by value. **/ - EIGEN_DEVICE_FUNC inline Scalar z() const { return m_coeffs.z(); } - - /** \brief Retruns the x-translation as a reference. **/ - EIGEN_DEVICE_FUNC inline Scalar& x() { return m_coeffs.x(); } - /** \brief Retruns the y-translation as a reference. **/ - EIGEN_DEVICE_FUNC inline Scalar& y() { return m_coeffs.y(); } - /** \brief Retruns the z-translation as a reference. **/ - EIGEN_DEVICE_FUNC inline Scalar& z() { return m_coeffs.z(); } - - EIGEN_DEVICE_FUNC const VectorType& vector() const { return m_coeffs; } - EIGEN_DEVICE_FUNC VectorType& vector() { return m_coeffs; } - - EIGEN_DEVICE_FUNC const VectorType& translation() const { return m_coeffs; } - EIGEN_DEVICE_FUNC VectorType& translation() { return m_coeffs; } - - /** Concatenates two translation */ - EIGEN_DEVICE_FUNC inline Translation operator* (const Translation& other) const - { return Translation(m_coeffs + other.m_coeffs); } - - /** Concatenates a translation and a uniform scaling */ - EIGEN_DEVICE_FUNC inline AffineTransformType operator* (const UniformScaling<Scalar>& other) const; - - /** Concatenates a translation and a linear transformation */ - template<typename OtherDerived> - EIGEN_DEVICE_FUNC inline AffineTransformType operator* (const EigenBase<OtherDerived>& linear) const; - - /** Concatenates a translation and a rotation */ - template<typename Derived> - 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 - EIGEN_DEVICE_FUNC inline AffineTransformType operator*(const EigenBase<OtherDerived>& linear, const Translation& t) - { - AffineTransformType res; - res.matrix().setZero(); - res.linear() = linear.derived(); - res.translation() = linear.derived() * t.m_coeffs; - res.matrix().row(Dim).setZero(); - res(Dim,Dim) = Scalar(1); - return res; - } - - /** Concatenates a translation and a transformation */ - template<int Mode, int Options> - 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); - return res; - } - - /** Applies translation to vector */ - 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); } - - Translation& operator=(const Translation& other) - { - m_coeffs = other.m_coeffs; - return *this; - } - - static const Translation Identity() { return Translation(VectorType::Zero()); } - - /** \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> - 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> - 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() */ - 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); } - -}; - -/** \addtogroup Geometry_Module */ -//@{ -typedef Translation<float, 2> Translation2f; -typedef Translation<double,2> Translation2d; -typedef Translation<float, 3> Translation3f; -typedef Translation<double,3> Translation3d; -//@} - -template<typename Scalar, int Dim> -EIGEN_DEVICE_FUNC inline typename Translation<Scalar,Dim>::AffineTransformType -Translation<Scalar,Dim>::operator* (const UniformScaling<Scalar>& other) const -{ - AffineTransformType res; - res.matrix().setZero(); - res.linear().diagonal().fill(other.factor()); - res.translation() = m_coeffs; - res(Dim,Dim) = Scalar(1); - return res; -} - -template<typename Scalar, int Dim> -template<typename OtherDerived> -EIGEN_DEVICE_FUNC inline typename Translation<Scalar,Dim>::AffineTransformType -Translation<Scalar,Dim>::operator* (const EigenBase<OtherDerived>& linear) const -{ - AffineTransformType res; - res.matrix().setZero(); - res.linear() = linear.derived(); - res.translation() = m_coeffs; - res.matrix().row(Dim).setZero(); - res(Dim,Dim) = Scalar(1); - return res; -} - -} // end namespace Eigen - -#endif // EIGEN_TRANSLATION_H diff --git a/eigen/Eigen/src/Geometry/Umeyama.h b/eigen/Eigen/src/Geometry/Umeyama.h deleted file mode 100644 index 7e933fc..0000000 --- a/eigen/Eigen/src/Geometry/Umeyama.h +++ /dev/null @@ -1,166 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2009 Hauke Heibel <hauke.heibel@gmail.com> -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_UMEYAMA_H -#define EIGEN_UMEYAMA_H - -// This file requires the user to include -// * Eigen/Core -// * Eigen/LU -// * Eigen/SVD -// * Eigen/Array - -namespace Eigen { - -#ifndef EIGEN_PARSED_BY_DOXYGEN - -// These helpers are required since it allows to use mixed types as parameters -// for the Umeyama. The problem with mixed parameters is that the return type -// cannot trivially be deduced when float and double types are mixed. -namespace internal { - -// Compile time return type deduction for different MatrixBase types. -// Different means here different alignment and parameters but the same underlying -// real scalar type. -template<typename MatrixType, typename OtherMatrixType> -struct umeyama_transform_matrix_type -{ - enum { - MinRowsAtCompileTime = EIGEN_SIZE_MIN_PREFER_DYNAMIC(MatrixType::RowsAtCompileTime, OtherMatrixType::RowsAtCompileTime), - - // When possible we want to choose some small fixed size value since the result - // is likely to fit on the stack. So here, EIGEN_SIZE_MIN_PREFER_DYNAMIC is not what we want. - HomogeneousDimension = int(MinRowsAtCompileTime) == Dynamic ? Dynamic : int(MinRowsAtCompileTime)+1 - }; - - typedef Matrix<typename traits<MatrixType>::Scalar, - HomogeneousDimension, - HomogeneousDimension, - AutoAlign | (traits<MatrixType>::Flags & RowMajorBit ? RowMajor : ColMajor), - HomogeneousDimension, - HomogeneousDimension - > type; -}; - -} - -#endif - -/** -* \geometry_module \ingroup Geometry_Module -* -* \brief Returns the transformation between two point sets. -* -* The algorithm is based on: -* "Least-squares estimation of transformation parameters between two point patterns", -* Shinji Umeyama, PAMI 1991, DOI: 10.1109/34.88573 -* -* It estimates parameters \f$ c, \mathbf{R}, \f$ and \f$ \mathbf{t} \f$ such that -* \f{align*} -* \frac{1}{n} \sum_{i=1}^n \vert\vert y_i - (c\mathbf{R}x_i + \mathbf{t}) \vert\vert_2^2 -* \f} -* is minimized. -* -* The algorithm is based on the analysis of the covariance matrix -* \f$ \Sigma_{\mathbf{x}\mathbf{y}} \in \mathbb{R}^{d \times d} \f$ -* of the input point sets \f$ \mathbf{x} \f$ and \f$ \mathbf{y} \f$ where -* \f$d\f$ is corresponding to the dimension (which is typically small). -* The analysis is involving the SVD having a complexity of \f$O(d^3)\f$ -* though the actual computational effort lies in the covariance -* matrix computation which has an asymptotic lower bound of \f$O(dm)\f$ when -* the input point sets have dimension \f$d \times m\f$. -* -* Currently the method is working only for floating point matrices. -* -* \todo Should the return type of umeyama() become a Transform? -* -* \param src Source points \f$ \mathbf{x} = \left( x_1, \hdots, x_n \right) \f$. -* \param dst Destination points \f$ \mathbf{y} = \left( y_1, \hdots, y_n \right) \f$. -* \param with_scaling Sets \f$ c=1 \f$ when <code>false</code> is passed. -* \return The homogeneous transformation -* \f{align*} -* T = \begin{bmatrix} c\mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{bmatrix} -* \f} -* minimizing the resudiual above. This transformation is always returned as an -* Eigen::Matrix. -*/ -template <typename Derived, typename OtherDerived> -typename internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type -umeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, bool with_scaling = true) -{ - typedef typename internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type TransformationMatrixType; - typedef typename internal::traits<TransformationMatrixType>::Scalar Scalar; - typedef typename NumTraits<Scalar>::Real RealScalar; - - EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsComplex, NUMERIC_TYPE_MUST_BE_REAL) - EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename internal::traits<OtherDerived>::Scalar>::value), - YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) - - enum { Dimension = EIGEN_SIZE_MIN_PREFER_DYNAMIC(Derived::RowsAtCompileTime, OtherDerived::RowsAtCompileTime) }; - - typedef Matrix<Scalar, Dimension, 1> VectorType; - typedef Matrix<Scalar, Dimension, Dimension> MatrixType; - typedef typename internal::plain_matrix_type_row_major<Derived>::type RowMajorMatrixType; - - const Index m = src.rows(); // dimension - const Index n = src.cols(); // number of measurements - - // required for demeaning ... - const RealScalar one_over_n = RealScalar(1) / static_cast<RealScalar>(n); - - // computation of mean - const VectorType src_mean = src.rowwise().sum() * one_over_n; - const VectorType dst_mean = dst.rowwise().sum() * one_over_n; - - // demeaning of src and dst points - const RowMajorMatrixType src_demean = src.colwise() - src_mean; - const RowMajorMatrixType dst_demean = dst.colwise() - dst_mean; - - // Eq. (36)-(37) - const Scalar src_var = src_demean.rowwise().squaredNorm().sum() * one_over_n; - - // Eq. (38) - const MatrixType sigma = one_over_n * dst_demean * src_demean.transpose(); - - JacobiSVD<MatrixType> svd(sigma, ComputeFullU | ComputeFullV); - - // Initialize the resulting transformation with an identity matrix... - TransformationMatrixType Rt = TransformationMatrixType::Identity(m+1,m+1); - - // Eq. (39) - VectorType S = VectorType::Ones(m); - - if ( svd.matrixU().determinant() * svd.matrixV().determinant() < 0 ) - S(m-1) = -1; - - // Eq. (40) and (43) - Rt.block(0,0,m,m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose(); - - if (with_scaling) - { - // Eq. (42) - const Scalar c = Scalar(1)/src_var * svd.singularValues().dot(S); - - // Eq. (41) - Rt.col(m).head(m) = dst_mean; - Rt.col(m).head(m).noalias() -= c*Rt.topLeftCorner(m,m)*src_mean; - Rt.block(0,0,m,m) *= c; - } - else - { - Rt.col(m).head(m) = dst_mean; - Rt.col(m).head(m).noalias() -= Rt.topLeftCorner(m,m)*src_mean; - } - - return Rt; -} - -} // end namespace Eigen - -#endif // EIGEN_UMEYAMA_H diff --git a/eigen/Eigen/src/Geometry/arch/Geometry_SSE.h b/eigen/Eigen/src/Geometry/arch/Geometry_SSE.h deleted file mode 100644 index f68cab5..0000000 --- a/eigen/Eigen/src/Geometry/arch/Geometry_SSE.h +++ /dev/null @@ -1,161 +0,0 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2009 Rohit Garg <rpg.314@gmail.com> -// Copyright (C) 2009-2010 Gael Guennebaud <gael.guennebaud@inria.fr> -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_GEOMETRY_SSE_H -#define EIGEN_GEOMETRY_SSE_H - -namespace Eigen { - -namespace internal { - -template<class Derived, class OtherDerived> -struct quat_product<Architecture::SSE, Derived, OtherDerived, float> -{ - enum { - AAlignment = traits<Derived>::Alignment, - BAlignment = traits<OtherDerived>::Alignment, - ResAlignment = traits<Quaternion<float> >::Alignment - }; - static inline Quaternion<float> run(const QuaternionBase<Derived>& _a, const QuaternionBase<OtherDerived>& _b) - { - Quaternion<float> res; - const __m128 mask = _mm_setr_ps(0.f,0.f,0.f,-0.f); - __m128 a = _a.coeffs().template packet<AAlignment>(0); - __m128 b = _b.coeffs().template packet<BAlignment>(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)); - pstoret<float,Packet4f,ResAlignment>( - &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_xor_ps(mask,_mm_add_ps(s1,s2)))); - - return res; - } -}; - -template<class Derived> -struct quat_conj<Architecture::SSE, Derived, float> -{ - enum { - ResAlignment = traits<Quaternion<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); - pstoret<float,Packet4f,ResAlignment>(&res.x(), _mm_xor_ps(mask, q.coeffs().template packet<traits<Derived>::Alignment>(0))); - return res; - } -}; - - -template<typename VectorLhs,typename VectorRhs> -struct cross3_impl<Architecture::SSE,VectorLhs,VectorRhs,float,true> -{ - enum { - ResAlignment = traits<typename plain_matrix_type<VectorLhs>::type>::Alignment - }; - static inline typename plain_matrix_type<VectorLhs>::type - run(const VectorLhs& lhs, const VectorRhs& rhs) - { - __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; - pstoret<float,Packet4f,ResAlignment>(&res.x(),_mm_sub_ps(mul1,mul2)); - return res; - } -}; - - - - -template<class Derived, class OtherDerived> -struct quat_product<Architecture::SSE, Derived, OtherDerived, double> -{ - enum { - BAlignment = traits<OtherDerived>::Alignment, - ResAlignment = traits<Quaternion<double> >::Alignment - }; - - static inline Quaternion<double> run(const QuaternionBase<Derived>& _a, const QuaternionBase<OtherDerived>& _b) - { - const Packet2d mask = _mm_castsi128_pd(_mm_set_epi32(0x0,0x0,0x80000000,0x0)); - - Quaternion<double> res; - - const double* a = _a.coeffs().data(); - Packet2d b_xy = _b.coeffs().template packet<BAlignment>(0); - Packet2d b_zw = _b.coeffs().template packet<BAlignment>(2); - Packet2d a_xx = pset1<Packet2d>(a[0]); - Packet2d a_yy = pset1<Packet2d>(a[1]); - Packet2d a_zz = pset1<Packet2d>(a[2]); - Packet2d a_ww = pset1<Packet2d>(a[3]); - - // two temporaries: - Packet2d t1, t2; - - /* - * t1 = ww*xy + yy*zw - * t2 = zz*xy - xx*zw - * res.xy = t1 +/- swap(t2) - */ - t1 = padd(pmul(a_ww, b_xy), pmul(a_yy, b_zw)); - t2 = psub(pmul(a_zz, b_xy), pmul(a_xx, b_zw)); -#ifdef EIGEN_VECTORIZE_SSE3 - EIGEN_UNUSED_VARIABLE(mask) - pstoret<double,Packet2d,ResAlignment>(&res.x(), _mm_addsub_pd(t1, preverse(t2))); -#else - pstoret<double,Packet2d,ResAlignment>(&res.x(), padd(t1, pxor(mask,preverse(t2)))); -#endif - - /* - * t1 = ww*zw - yy*xy - * t2 = zz*zw + xx*xy - * res.zw = t1 -/+ swap(t2) = swap( swap(t1) +/- t2) - */ - t1 = psub(pmul(a_ww, b_zw), pmul(a_yy, b_xy)); - t2 = padd(pmul(a_zz, b_zw), pmul(a_xx, b_xy)); -#ifdef EIGEN_VECTORIZE_SSE3 - EIGEN_UNUSED_VARIABLE(mask) - pstoret<double,Packet2d,ResAlignment>(&res.z(), preverse(_mm_addsub_pd(preverse(t1), t2))); -#else - pstoret<double,Packet2d,ResAlignment>(&res.z(), psub(t1, pxor(mask,preverse(t2)))); -#endif - - return res; -} -}; - -template<class Derived> -struct quat_conj<Architecture::SSE, Derived, double> -{ - enum { - ResAlignment = traits<Quaternion<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.); - pstoret<double,Packet2d,ResAlignment>(&res.x(), _mm_xor_pd(mask0, q.coeffs().template packet<traits<Derived>::Alignment>(0))); - pstoret<double,Packet2d,ResAlignment>(&res.z(), _mm_xor_pd(mask2, q.coeffs().template packet<traits<Derived>::Alignment>(2))); - return res; - } -}; - -} // end namespace internal - -} // end namespace Eigen - -#endif // EIGEN_GEOMETRY_SSE_H |