diff options
Diffstat (limited to 'Eigen/src/Geometry')
-rw-r--r-- | Eigen/src/Geometry/AlignedBox.h | 104 | ||||
-rw-r--r-- | Eigen/src/Geometry/AngleAxis.h | 10 | ||||
-rw-r--r-- | Eigen/src/Geometry/EulerAngles.h | 6 | ||||
-rw-r--r-- | Eigen/src/Geometry/Homogeneous.h | 26 | ||||
-rw-r--r-- | Eigen/src/Geometry/Hyperplane.h | 2 | ||||
-rw-r--r-- | Eigen/src/Geometry/OrthoMethods.h | 5 | ||||
-rw-r--r-- | Eigen/src/Geometry/ParametrizedLine.h | 41 | ||||
-rw-r--r-- | Eigen/src/Geometry/Quaternion.h | 113 | ||||
-rw-r--r-- | Eigen/src/Geometry/Rotation2D.h | 6 | ||||
-rw-r--r--[-rwxr-xr-x] | Eigen/src/Geometry/Scaling.h | 28 | ||||
-rw-r--r-- | Eigen/src/Geometry/Transform.h | 155 | ||||
-rw-r--r-- | Eigen/src/Geometry/Translation.h | 18 | ||||
-rw-r--r-- | Eigen/src/Geometry/Umeyama.h | 2 | ||||
-rw-r--r-- | Eigen/src/Geometry/arch/Geometry_SIMD.h | 168 | ||||
-rw-r--r-- | Eigen/src/Geometry/arch/Geometry_SSE.h | 161 |
15 files changed, 541 insertions, 304 deletions
diff --git a/Eigen/src/Geometry/AlignedBox.h b/Eigen/src/Geometry/AlignedBox.h index 066eae4f9..55a9d0ae1 100644 --- a/Eigen/src/Geometry/AlignedBox.h +++ b/Eigen/src/Geometry/AlignedBox.h @@ -7,10 +7,46 @@ // 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/. +// Function void Eigen::AlignedBox::transform(const Transform& transform) +// is provided under the following license agreement: +// +// Software License Agreement (BSD License) +// +// Copyright (c) 2011-2014, Willow Garage, Inc. +// Copyright (c) 2014-2015, Open Source Robotics Foundation +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of Open Source Robotics Foundation nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + #ifndef EIGEN_ALIGNEDBOX_H #define EIGEN_ALIGNEDBOX_H -namespace Eigen { +namespace Eigen { /** \geometry_module \ingroup Geometry_Module * @@ -63,7 +99,7 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) /** Default constructor initializing a null box. */ EIGEN_DEVICE_FUNC inline AlignedBox() - { if (AmbientDimAtCompileTime!=Dynamic) setEmpty(); } + { if (EIGEN_CONST_CONDITIONAL(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) @@ -231,7 +267,7 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) {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. + * \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)); } @@ -246,6 +282,15 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) return *this; } + /** \returns a copy of \c *this translated by the vector \a t. */ + template<typename Derived> + EIGEN_DEVICE_FUNC inline AlignedBox translated(const MatrixBase<Derived>& a_t) const + { + AlignedBox result(m_min, m_max); + result.translate(a_t); + return result; + } + /** \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&) @@ -265,14 +310,63 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) */ template<typename Derived> EIGEN_DEVICE_FUNC inline NonInteger exteriorDistance(const MatrixBase<Derived>& p) const - { EIGEN_USING_STD_MATH(sqrt) return sqrt(NonInteger(squaredExteriorDistance(p))); } + { EIGEN_USING_STD(sqrt) return sqrt(NonInteger(squaredExteriorDistance(p))); } /** \returns the distance between the boxes \a b and \c *this, * and zero if the boxes intersect. * \sa squaredExteriorDistance(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))); } + { EIGEN_USING_STD(sqrt) return sqrt(NonInteger(squaredExteriorDistance(b))); } + + /** + * Specialization of transform for pure translation. + */ + template<int Mode, int Options> + EIGEN_DEVICE_FUNC inline void transform( + const typename Transform<Scalar, AmbientDimAtCompileTime, Mode, Options>::TranslationType& translation) + { + this->translate(translation); + } + + /** + * Transforms this box by \a transform and recomputes it to + * still be an axis-aligned box. + * + * \note This method is provided under BSD license (see the top of this file). + */ + template<int Mode, int Options> + EIGEN_DEVICE_FUNC inline void transform(const Transform<Scalar, AmbientDimAtCompileTime, Mode, Options>& transform) + { + // Only Affine and Isometry transforms are currently supported. + EIGEN_STATIC_ASSERT(Mode == Affine || Mode == AffineCompact || Mode == Isometry, THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS); + + // Method adapted from FCL src/shape/geometric_shapes_utility.cpp#computeBV<AABB, Box>(...) + // https://github.com/flexible-collision-library/fcl/blob/fcl-0.4/src/shape/geometric_shapes_utility.cpp#L292 + // + // Here's a nice explanation why it works: https://zeuxcg.org/2010/10/17/aabb-from-obb-with-component-wise-abs/ + + // two times rotated extent + const VectorType rotated_extent_2 = transform.linear().cwiseAbs() * sizes(); + // two times new center + const VectorType rotated_center_2 = transform.linear() * (this->m_max + this->m_min) + + Scalar(2) * transform.translation(); + + this->m_max = (rotated_center_2 + rotated_extent_2) / Scalar(2); + this->m_min = (rotated_center_2 - rotated_extent_2) / Scalar(2); + } + + /** + * \returns a copy of \c *this transformed by \a transform and recomputed to + * still be an axis-aligned box. + */ + template<int Mode, int Options> + EIGEN_DEVICE_FUNC AlignedBox transformed(const Transform<Scalar, AmbientDimAtCompileTime, Mode, Options>& transform) const + { + AlignedBox result(m_min, m_max); + result.transform(transform); + return result; + } /** \returns \c *this with scalar type casted to \a NewScalarType * diff --git a/Eigen/src/Geometry/AngleAxis.h b/Eigen/src/Geometry/AngleAxis.h index 0af3c1b08..78328b6b5 100644 --- a/Eigen/src/Geometry/AngleAxis.h +++ b/Eigen/src/Geometry/AngleAxis.h @@ -169,8 +169,8 @@ 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) + EIGEN_USING_STD(atan2) + EIGEN_USING_STD(abs) Scalar n = q.vec().norm(); if(n<NumTraits<Scalar>::epsilon()) n = q.vec().stableNorm(); @@ -178,7 +178,7 @@ EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const Quaterni if (n != Scalar(0)) { m_angle = Scalar(2)*atan2(n, abs(q.w())); - if(q.w() < 0) + if(q.w() < Scalar(0)) n = -n; m_axis = q.vec() / n; } @@ -217,8 +217,8 @@ 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) + EIGEN_USING_STD(sin) + EIGEN_USING_STD(cos) Matrix3 res; Vector3 sin_axis = sin(m_angle) * m_axis; Scalar c = cos(m_angle); diff --git a/Eigen/src/Geometry/EulerAngles.h b/Eigen/src/Geometry/EulerAngles.h index c633268af..19b734ca7 100644 --- a/Eigen/src/Geometry/EulerAngles.h +++ b/Eigen/src/Geometry/EulerAngles.h @@ -36,9 +36,9 @@ 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) + EIGEN_USING_STD(atan2) + EIGEN_USING_STD(sin) + EIGEN_USING_STD(cos) /* Implemented from Graphics Gems IV */ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3) diff --git a/Eigen/src/Geometry/Homogeneous.h b/Eigen/src/Geometry/Homogeneous.h index 5f0da1a9e..94083ac54 100644 --- a/Eigen/src/Geometry/Homogeneous.h +++ b/Eigen/src/Geometry/Homogeneous.h @@ -10,7 +10,7 @@ #ifndef EIGEN_HOMOGENEOUS_H #define EIGEN_HOMOGENEOUS_H -namespace Eigen { +namespace Eigen { /** \geometry_module \ingroup Geometry_Module * @@ -72,9 +72,11 @@ template<typename MatrixType,int _Direction> class Homogeneous : 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 EIGEN_CONSTEXPR + inline Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows() + (int(Direction)==Vertical ? 1 : 0); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR + inline Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols() + (int(Direction)==Horizontal ? 1 : 0); } + EIGEN_DEVICE_FUNC const NestedExpression& nestedExpression() const { return m_matrix; } template<typename Rhs> @@ -262,8 +264,10 @@ struct homogeneous_left_product_impl<Homogeneous<MatrixType,Vertical>,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(); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR + inline Index rows() const EIGEN_NOEXCEPT { return m_lhs.rows(); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR + inline Index cols() const EIGEN_NOEXCEPT { return m_rhs.cols(); } template<typename Dest> EIGEN_DEVICE_FUNC void evalTo(Dest& dst) const { @@ -300,8 +304,8 @@ struct homogeneous_right_product_impl<Homogeneous<MatrixType,Horizontal>,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(); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_lhs.rows(); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_rhs.cols(); } template<typename Dest> EIGEN_DEVICE_FUNC void evalTo(Dest& dst) const { @@ -322,7 +326,7 @@ 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; + typedef HomogeneousShape Shape; }; template<> struct AssignmentKind<DenseShape,HomogeneousShape> { typedef Dense2Dense Kind; }; @@ -414,7 +418,7 @@ struct product_evaluator<Product<Lhs, Rhs, LazyProduct>, ProductTag, Homogeneous 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) ) @@ -467,7 +471,7 @@ struct product_evaluator<Product<Lhs, Rhs, LazyProduct>, ProductTag, DenseShape, 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()) ) diff --git a/Eigen/src/Geometry/Hyperplane.h b/Eigen/src/Geometry/Hyperplane.h index 05929b299..cebe03557 100644 --- a/Eigen/src/Geometry/Hyperplane.h +++ b/Eigen/src/Geometry/Hyperplane.h @@ -119,7 +119,7 @@ public: * 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 ?? + // FIXME to be consistent 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(); diff --git a/Eigen/src/Geometry/OrthoMethods.h b/Eigen/src/Geometry/OrthoMethods.h index a035e6310..524aebe1b 100644 --- a/Eigen/src/Geometry/OrthoMethods.h +++ b/Eigen/src/Geometry/OrthoMethods.h @@ -27,9 +27,10 @@ namespace Eigen { 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 +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE +typename MatrixBase<Derived>::template cross_product_return_type<OtherDerived>::type #else -inline typename MatrixBase<Derived>::PlainObject +typename MatrixBase<Derived>::PlainObject #endif MatrixBase<Derived>::cross(const MatrixBase<OtherDerived>& other) const { diff --git a/Eigen/src/Geometry/ParametrizedLine.h b/Eigen/src/Geometry/ParametrizedLine.h index 1e985d8cd..584f50087 100644 --- a/Eigen/src/Geometry/ParametrizedLine.h +++ b/Eigen/src/Geometry/ParametrizedLine.h @@ -87,7 +87,7 @@ public: /** \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)); } + EIGEN_DEVICE_FUNC RealScalar distance(const VectorType& p) const { EIGEN_USING_STD(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 @@ -104,7 +104,44 @@ public: 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 + /** Applies the transformation matrix \a mat to \c *this and returns a reference to \c *this. + * + * \param mat the Dim x Dim transformation matrix + * \param traits specifies whether the matrix \a mat represents an #Isometry + * or a more generic #Affine transformation. The default is #Affine. + */ + template<typename XprType> + EIGEN_DEVICE_FUNC inline ParametrizedLine& transform(const MatrixBase<XprType>& mat, TransformTraits traits = Affine) + { + if (traits==Affine) + direction() = (mat * direction()).normalized(); + else if (traits==Isometry) + direction() = mat * direction(); + else + { + eigen_assert(0 && "invalid traits value in ParametrizedLine::transform()"); + } + origin() = mat * origin(); + return *this; + } + + /** Applies the transformation \a t to \c *this and returns a reference to \c *this. + * + * \param t the transformation of dimension Dim + * \param traits specifies whether the transformation \a t represents an #Isometry + * or a more generic #Affine transformation. The default is #Affine. + * Other kind of transformations are not supported. + */ + template<int TrOptions> + EIGEN_DEVICE_FUNC inline ParametrizedLine& transform(const Transform<Scalar,AmbientDimAtCompileTime,Affine,TrOptions>& t, + TransformTraits traits = Affine) + { + transform(t.linear(), traits); + origin() += t.translation(); + return *this; + } + +/** \returns \c *this with scalar type casted to \a NewScalarType * * Note that if \a NewScalarType is equal to the current scalar type of \c *this * then this function smartly returns a const reference to \c *this. diff --git a/Eigen/src/Geometry/Quaternion.h b/Eigen/src/Geometry/Quaternion.h index 3e5a9badb..3259e592d 100644 --- a/Eigen/src/Geometry/Quaternion.h +++ b/Eigen/src/Geometry/Quaternion.h @@ -43,6 +43,11 @@ class QuaternionBase : public RotationBase<Derived, 3> 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 }; @@ -58,22 +63,22 @@ class QuaternionBase : public RotationBase<Derived, 3> /** \returns the \c x coefficient */ - EIGEN_DEVICE_FUNC inline Scalar x() const { return this->derived().coeffs().coeff(0); } + EIGEN_DEVICE_FUNC inline CoeffReturnType x() const { return this->derived().coeffs().coeff(0); } /** \returns the \c y coefficient */ - EIGEN_DEVICE_FUNC inline Scalar y() const { return this->derived().coeffs().coeff(1); } + EIGEN_DEVICE_FUNC inline CoeffReturnType y() const { return this->derived().coeffs().coeff(1); } /** \returns the \c z coefficient */ - EIGEN_DEVICE_FUNC inline Scalar z() const { return this->derived().coeffs().coeff(2); } + EIGEN_DEVICE_FUNC inline CoeffReturnType z() const { return this->derived().coeffs().coeff(2); } /** \returns the \c w coefficient */ - EIGEN_DEVICE_FUNC inline Scalar w() const { return this->derived().coeffs().coeff(3); } + EIGEN_DEVICE_FUNC inline CoeffReturnType w() const { return this->derived().coeffs().coeff(3); } - /** \returns a reference to the \c x coefficient */ - EIGEN_DEVICE_FUNC inline Scalar& x() { return this->derived().coeffs().coeffRef(0); } - /** \returns a reference to the \c y coefficient */ - EIGEN_DEVICE_FUNC inline Scalar& y() { return this->derived().coeffs().coeffRef(1); } - /** \returns a reference to the \c z coefficient */ - EIGEN_DEVICE_FUNC inline Scalar& z() { return this->derived().coeffs().coeffRef(2); } - /** \returns a reference to the \c w coefficient */ - EIGEN_DEVICE_FUNC inline Scalar& w() { return this->derived().coeffs().coeffRef(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>(); } @@ -136,7 +141,7 @@ class QuaternionBase : public RotationBase<Derived, 3> 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; + EIGEN_DEVICE_FUNC inline Matrix3 toRotationMatrix() const; /** \returns the quaternion which transform \a a into \a b through a rotation */ template<typename Derived1, typename Derived2> @@ -153,6 +158,22 @@ class QuaternionBase : public RotationBase<Derived, 3> template<class OtherDerived> EIGEN_DEVICE_FUNC Quaternion<Scalar> slerp(const Scalar& t, const QuaternionBase<OtherDerived>& other) const; + /** \returns true if each coefficients of \c *this and \a other are all exactly equal. + * \warning When using floating point scalar values you probably should rather use a + * fuzzy comparison such as isApprox() + * \sa isApprox(), operator!= */ + template<class OtherDerived> + EIGEN_DEVICE_FUNC inline bool operator==(const QuaternionBase<OtherDerived>& other) const + { return coeffs() == other.coeffs(); } + + /** \returns true if at least one pair of coefficients of \c *this and \a other are not exactly equal to each other. + * \warning When using floating point scalar values you probably should rather use a + * fuzzy comparison such as isApprox() + * \sa isApprox(), operator== */ + template<class OtherDerived> + EIGEN_DEVICE_FUNC inline bool operator!=(const QuaternionBase<OtherDerived>& other) const + { return coeffs() != other.coeffs(); } + /** \returns \c true if \c *this is approximately equal to \a other, within the precision * determined by \a prec. * @@ -164,20 +185,45 @@ class QuaternionBase : public RotationBase<Derived, 3> /** return the result vector of \a v through the rotation*/ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Vector3 _transformVector(const Vector3& v) const; + #ifdef EIGEN_PARSED_BY_DOXYGEN /** \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 + EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type cast() const; + + #else + + template<typename NewScalarType> + EIGEN_DEVICE_FUNC inline + typename internal::enable_if<internal::is_same<Scalar,NewScalarType>::value,const Derived&>::type cast() const { - return typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type(derived()); + return derived(); } + template<typename NewScalarType> + EIGEN_DEVICE_FUNC inline + typename internal::enable_if<!internal::is_same<Scalar,NewScalarType>::value,Quaternion<NewScalarType> >::type cast() const + { + return Quaternion<NewScalarType>(coeffs().template cast<NewScalarType>()); + } + #endif + +#ifndef EIGEN_NO_IO + friend std::ostream& operator<<(std::ostream& s, const QuaternionBase<Derived>& q) { + s << q.x() << "i + " << q.y() << "j + " << q.z() << "k" << " + " << q.w(); + return s; + } +#endif + #ifdef EIGEN_QUATERNIONBASE_PLUGIN # include EIGEN_QUATERNIONBASE_PLUGIN #endif +protected: + EIGEN_DEFAULT_COPY_CONSTRUCTOR(QuaternionBase) + EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(QuaternionBase) }; /*************************************************************************** @@ -271,6 +317,21 @@ public: EIGEN_DEVICE_FUNC explicit inline Quaternion(const Quaternion<OtherScalar, OtherOptions>& other) { m_coeffs = other.coeffs().template cast<Scalar>(); } +#if EIGEN_HAS_RVALUE_REFERENCES + // We define a copy constructor, which means we don't get an implicit move constructor or assignment operator. + /** Default move constructor */ + EIGEN_DEVICE_FUNC inline Quaternion(Quaternion&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_constructible<Scalar>::value) + : m_coeffs(std::move(other.coeffs())) + {} + + /** Default move assignment operator */ + EIGEN_DEVICE_FUNC Quaternion& operator=(Quaternion&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_assignable<Scalar>::value) + { + m_coeffs = std::move(other.coeffs()); + return *this; + } +#endif + EIGEN_DEVICE_FUNC static Quaternion UnitRandom(); template<typename Derived1, typename Derived2> @@ -499,8 +560,8 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator 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) + EIGEN_USING_STD(cos) + EIGEN_USING_STD(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(); @@ -576,7 +637,7 @@ 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) + EIGEN_USING_STD(sqrt) Vector3 v0 = a.normalized(); Vector3 v1 = b.normalized(); Scalar c = v1.dot(v0); @@ -617,13 +678,13 @@ EIGEN_DEVICE_FUNC inline Derived& QuaternionBase<Derived>::setFromTwoVectors(con 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) + EIGEN_USING_STD(sqrt) + EIGEN_USING_STD(sin) + EIGEN_USING_STD(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), + const Scalar a = sqrt(Scalar(1) - u1), b = sqrt(u1); return Quaternion (a * sin(u2), a * cos(u2), b * sin(u3), b * cos(u3)); } @@ -702,7 +763,7 @@ 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) + EIGEN_USING_STD(atan2) Quaternion<Scalar> d = (*this) * other.conjugate(); return Scalar(2) * atan2( d.vec().norm(), numext::abs(d.w()) ); } @@ -720,8 +781,8 @@ 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) + EIGEN_USING_STD(acos) + EIGEN_USING_STD(sin) const Scalar one = Scalar(1) - NumTraits<Scalar>::epsilon(); Scalar d = this->dot(other); Scalar absD = numext::abs(d); @@ -758,7 +819,7 @@ struct quaternionbase_assign_impl<Other,3,3> 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) + EIGEN_USING_STD(sqrt) // This algorithm comes from "Quaternion Calculus and Fast Animation", // Ken Shoemake, 1987 SIGGRAPH course notes Scalar t = mat.trace(); diff --git a/Eigen/src/Geometry/Rotation2D.h b/Eigen/src/Geometry/Rotation2D.h index 884b7d0ee..d0bd57569 100644 --- a/Eigen/src/Geometry/Rotation2D.h +++ b/Eigen/src/Geometry/Rotation2D.h @@ -175,7 +175,7 @@ template<typename Scalar> template<typename Derived> EIGEN_DEVICE_FUNC Rotation2D<Scalar>& Rotation2D<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat) { - EIGEN_USING_STD_MATH(atan2) + EIGEN_USING_STD(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; @@ -187,8 +187,8 @@ 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) + EIGEN_USING_STD(sin) + EIGEN_USING_STD(cos) Scalar sinA = sin(m_angle); Scalar cosA = cos(m_angle); return (Matrix2() << cosA, -sinA, sinA, cosA).finished(); diff --git a/Eigen/src/Geometry/Scaling.h b/Eigen/src/Geometry/Scaling.h index f58ca03d9..d352f1f2b 100755..100644 --- a/Eigen/src/Geometry/Scaling.h +++ b/Eigen/src/Geometry/Scaling.h @@ -14,7 +14,7 @@ namespace Eigen { /** \geometry_module \ingroup Geometry_Module * - * \class Scaling + * \class UniformScaling * * \brief Represents a generic uniform scaling transformation * @@ -29,6 +29,22 @@ namespace Eigen { * * \sa Scaling(), class DiagonalMatrix, MatrixBase::asDiagonal(), class Translation, class Transform */ + +namespace internal +{ + // This helper helps nvcc+MSVC to properly parse this file. + // See bug 1412. + template <typename Scalar, int Dim, int Mode> + struct uniformscaling_times_affine_returntype + { + enum + { + NewMode = int(Mode) == int(Isometry) ? Affine : Mode + }; + typedef Transform <Scalar, Dim, NewMode> type; + }; +} + template<typename _Scalar> class UniformScaling { @@ -60,9 +76,11 @@ public: /** 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 + inline typename + internal::uniformscaling_times_affine_returntype<Scalar,Dim,Mode>::type + operator* (const Transform<Scalar, Dim, Mode, Options>& t) const { - Transform<Scalar,Dim,(int(Mode)==int(Isometry)?Affine:Mode)> res = t; + typename internal::uniformscaling_times_affine_returntype<Scalar,Dim,Mode>::type res = t; res.prescale(factor()); return res; } @@ -70,7 +88,7 @@ public: /** 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 + inline typename Eigen::internal::plain_matrix_type<Derived>::type operator* (const MatrixBase<Derived>& other) const { return other * m_factor; } template<typename Derived,int Dim> @@ -110,7 +128,7 @@ public: /** Concatenates a linear transformation matrix and a uniform scaling * \relates UniformScaling */ -// NOTE this operator is defiend in MatrixBase and not as a friend function +// NOTE this operator is defined 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) diff --git a/Eigen/src/Geometry/Transform.h b/Eigen/src/Geometry/Transform.h index 3f31ee45d..52b8c2a4e 100644 --- a/Eigen/src/Geometry/Transform.h +++ b/Eigen/src/Geometry/Transform.h @@ -12,7 +12,7 @@ #ifndef EIGEN_TRANSFORM_H #define EIGEN_TRANSFORM_H -namespace Eigen { +namespace Eigen { namespace internal { @@ -47,7 +47,7 @@ struct transform_left_product_impl; template< typename Lhs, typename Rhs, - bool AnyProjective = + bool AnyProjective = transform_traits<Lhs>::IsProjective || transform_traits<Rhs>::IsProjective> struct transform_transform_product_impl; @@ -97,6 +97,9 @@ template<int Mode> struct transform_make_affine; * - #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. + * - #Isometry: same as #Affine with the additional assumption that + * the linear part represents a rotation. This assumption is exploited + * to speed up some functions such as inverse() and rotation(). * \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. * @@ -115,7 +118,7 @@ template<int Mode> struct transform_make_affine; * \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. + * and then the interpretation of different parts might be slightly different. * * However, unlike a plain matrix, the Transform class provides many features * simplifying both its assembly and usage. In particular, it can be composed @@ -220,9 +223,9 @@ public: /** 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; + typedef Block<MatrixType,Dim,Dim,int(Mode)==(AffineCompact) && (int(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; + typedef const Block<ConstMatrixType,Dim,Dim,int(Mode)==(AffineCompact) && (int(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&, @@ -239,7 +242,7 @@ public: 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 */ @@ -252,17 +255,11 @@ protected: public: /** Default constructor without initialization of the meaningful coefficients. - * If Mode==Affine, then the last row is set to [0 ... 0 1] */ + * If Mode==Affine or Mode==Isometry, 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; + internal::transform_make_affine<(int(Mode)==Affine || int(Mode)==Isometry) ? Affine : AffineCompact>::run(m_matrix); } EIGEN_DEVICE_FUNC inline explicit Transform(const TranslationType& t) @@ -282,9 +279,6 @@ public: *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. */ @@ -308,7 +302,7 @@ public: 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) { @@ -335,7 +329,7 @@ public: OtherModeIsAffineCompact = OtherMode == int(AffineCompact) }; - if(ModeIsAffineCompact == OtherModeIsAffineCompact) + if(EIGEN_CONST_CONDITIONAL(ModeIsAffineCompact == OtherModeIsAffineCompact)) { // We need the block expression because the code is compiled for all // combinations of transformations and will trigger a compile time error @@ -343,7 +337,7 @@ public: m_matrix.template block<Dim,Dim+1>(0,0) = other.matrix().template block<Dim,Dim+1>(0,0); makeAffine(); } - else if(OtherModeIsAffineCompact) + else if(EIGEN_CONST_CONDITIONAL(OtherModeIsAffineCompact)) { typedef typename Transform<Scalar,Dim,OtherMode,OtherOptions>::MatrixType OtherMatrixType; internal::transform_construct_from_matrix<OtherMatrixType,Mode,Options,Dim,HDim>::run(this, other.matrix()); @@ -380,9 +374,9 @@ public: inline Transform& operator=(const QTransform& other); inline QTransform toQTransform(void) const; #endif - - EIGEN_DEVICE_FUNC Index rows() const { return int(Mode)==int(Projective) ? m_matrix.cols() : (m_matrix.cols()-1); } - EIGEN_DEVICE_FUNC Index cols() const { return m_matrix.cols(); } + + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return int(Mode)==int(Projective) ? m_matrix.cols() : (m_matrix.cols()-1); } + EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); } /** shortcut for m_matrix(row,col); * \sa MatrixBase::operator(Index,Index) const */ @@ -456,7 +450,7 @@ public: /** \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 + * 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> @@ -471,7 +465,7 @@ public: /** \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 + * 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> @@ -481,7 +475,7 @@ public: TransformTimeDiagonalReturnType res; res.linear().noalias() = a*b.linear(); res.translation().noalias() = a*b.translation(); - if (Mode!=int(AffineCompact)) + if (EIGEN_CONST_CONDITIONAL(Mode!=int(AffineCompact))) res.matrix().row(Dim) = b.matrix().row(Dim); return res; } @@ -494,7 +488,7 @@ public: { 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: @@ -503,13 +497,13 @@ private: // (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> @@ -542,7 +536,7 @@ public: } template<typename OtherDerived> - EIGEN_DEVICE_FUNC + EIGEN_DEVICE_FUNC inline Transform& scale(const MatrixBase<OtherDerived> &other); template<typename OtherDerived> @@ -572,18 +566,18 @@ public: 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 + 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 { @@ -602,7 +596,9 @@ public: template<typename Derived> EIGEN_DEVICE_FUNC inline Transform operator*(const RotationBase<Derived,Dim>& r) const; - EIGEN_DEVICE_FUNC const LinearMatrixType rotation() const; + typedef typename internal::conditional<int(Mode)==Isometry,ConstLinearPart,const LinearMatrixType>::type RotationReturnType; + EIGEN_DEVICE_FUNC RotationReturnType rotation() const; + template<typename RotationMatrixType, typename ScalingMatrixType> EIGEN_DEVICE_FUNC void computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const; @@ -684,7 +680,7 @@ public: #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() @@ -755,7 +751,7 @@ 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)) + if (EIGEN_CONST_CONDITIONAL(Mode == int(AffineCompact))) m_matrix << other.m11(), other.m21(), other.dx(), other.m12(), other.m22(), other.dy(); else @@ -801,7 +797,7 @@ Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::operator { check_template_params(); EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE) - if (Mode == int(AffineCompact)) + if (EIGEN_CONST_CONDITIONAL(Mode == int(AffineCompact))) m_matrix << other.m11(), other.m21(), other.dx(), other.m12(), other.m22(), other.dy(); else @@ -819,7 +815,7 @@ template<typename Scalar, int Dim, int Mode, int Options> QTransform Transform<Scalar,Dim,Mode,Options>::toQTransform(void) const { EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE) - if (Mode == int(AffineCompact)) + if (EIGEN_CONST_CONDITIONAL(Mode == int(AffineCompact))) return QTransform(m_matrix.coeff(0,0), m_matrix.coeff(1,0), m_matrix.coeff(0,1), m_matrix.coeff(1,1), m_matrix.coeff(0,2), m_matrix.coeff(1,2)); @@ -912,7 +908,7 @@ EIGEN_DEVICE_FUNC Transform<Scalar,Dim,Mode,Options>& Transform<Scalar,Dim,Mode,Options>::pretranslate(const MatrixBase<OtherDerived> &other) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim)) - if(int(Mode)==int(Projective)) + if(EIGEN_CONST_CONDITIONAL(int(Mode)==int(Projective))) affine() += other * m_matrix.row(Dim); else translation() += other; @@ -1046,20 +1042,43 @@ EIGEN_DEVICE_FUNC inline Transform<Scalar,Dim,Mode,Options> Transform<Scalar,Dim *** Special functions *** ************************/ +namespace internal { +template<int Mode> struct transform_rotation_impl { + template<typename TransformType> + EIGEN_DEVICE_FUNC static inline + const typename TransformType::LinearMatrixType run(const TransformType& t) + { + typedef typename TransformType::LinearMatrixType LinearMatrixType; + LinearMatrixType result; + t.computeRotationScaling(&result, (LinearMatrixType*)0); + return result; + } +}; +template<> struct transform_rotation_impl<Isometry> { + template<typename TransformType> + EIGEN_DEVICE_FUNC static inline + typename TransformType::ConstLinearPart run(const TransformType& t) + { + return t.linear(); + } +}; +} /** \returns the rotation part of the transformation * + * If Mode==Isometry, then this method is an alias for linear(), + * otherwise it calls computeRotationScaling() to extract the rotation + * through a SVD decomposition. * * \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 +EIGEN_DEVICE_FUNC +typename Transform<Scalar,Dim,Mode,Options>::RotationReturnType Transform<Scalar,Dim,Mode,Options>::rotation() const { - LinearMatrixType result; - computeRotationScaling(&result, (LinearMatrixType*)0); - return result; + return internal::transform_rotation_impl<Mode>::run(*this); } @@ -1078,17 +1097,18 @@ 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 { + // Note that JacobiSVD is faster than BDCSVD for small matrices. JacobiSVD<LinearMatrixType> svd(linear(), ComputeFullU | ComputeFullV); - Scalar x = (svd.matrixU() * svd.matrixV().adjoint()).determinant(); // so x has absolute value 1 + Scalar x = (svd.matrixU() * svd.matrixV().adjoint()).determinant() < Scalar(0) ? Scalar(-1) : Scalar(1); // 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()); + sv.coeffRef(Dim-1) *= x; + if(scaling) *scaling = svd.matrixV() * sv.asDiagonal() * svd.matrixV().adjoint(); if(rotation) { LinearMatrixType m(svd.matrixU()); - m.col(0) /= x; - rotation->lazyAssign(m * svd.matrixV().adjoint()); + m.col(Dim-1) *= x; + *rotation = m * svd.matrixV().adjoint(); } } @@ -1107,17 +1127,18 @@ 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 { + // Note that JacobiSVD is faster than BDCSVD for small matrices. JacobiSVD<LinearMatrixType> svd(linear(), ComputeFullU | ComputeFullV); - Scalar x = (svd.matrixU() * svd.matrixV().adjoint()).determinant(); // so x has absolute value 1 + Scalar x = (svd.matrixU() * svd.matrixV().adjoint()).determinant() < Scalar(0) ? Scalar(-1) : Scalar(1); // 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()); + sv.coeffRef(Dim-1) *= x; + if(scaling) *scaling = svd.matrixU() * sv.asDiagonal() * svd.matrixU().adjoint(); if(rotation) { LinearMatrixType m(svd.matrixU()); - m.col(0) /= x; - rotation->lazyAssign(m * svd.matrixV().adjoint()); + m.col(Dim-1) *= x; + *rotation = m * svd.matrixV().adjoint(); } } @@ -1156,7 +1177,7 @@ 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 @@ -1297,8 +1318,8 @@ struct transform_construct_from_matrix<Other, AffineCompact,Options,Dim,HDim, HD template<int LhsMode,int RhsMode> struct transform_product_result { - enum - { + enum + { Mode = (LhsMode == (int)Projective || RhsMode == (int)Projective ) ? Projective : (LhsMode == (int)Affine || RhsMode == (int)Affine ) ? Affine : @@ -1312,7 +1333,7 @@ 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) + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType run(const TransformType& T, const MatrixType& other) { return T.matrix() * other; } @@ -1321,8 +1342,8 @@ struct transform_right_product_impl< TransformType, MatrixType, 0, RhsCols> template< typename TransformType, typename MatrixType, int RhsCols> struct transform_right_product_impl< TransformType, MatrixType, 1, RhsCols> { - enum { - Dim = TransformType::Dim, + enum { + Dim = TransformType::Dim, HDim = TransformType::HDim, OtherRows = MatrixType::RowsAtCompileTime, OtherCols = MatrixType::ColsAtCompileTime @@ -1330,7 +1351,7 @@ struct transform_right_product_impl< TransformType, MatrixType, 1, RhsCols> typedef typename MatrixType::PlainObject ResultType; - static EIGEN_STRONG_INLINE ResultType run(const TransformType& T, const MatrixType& other) + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType run(const TransformType& T, const MatrixType& other) { EIGEN_STATIC_ASSERT(OtherRows==HDim, YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES); @@ -1339,7 +1360,7 @@ struct transform_right_product_impl< TransformType, MatrixType, 1, RhsCols> 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; } }; @@ -1347,8 +1368,8 @@ struct transform_right_product_impl< TransformType, MatrixType, 1, RhsCols> template< typename TransformType, typename MatrixType, int RhsCols> struct transform_right_product_impl< TransformType, MatrixType, 2, RhsCols> { - enum { - Dim = TransformType::Dim, + enum { + Dim = TransformType::Dim, HDim = TransformType::HDim, OtherRows = MatrixType::RowsAtCompileTime, OtherCols = MatrixType::ColsAtCompileTime @@ -1356,7 +1377,7 @@ struct transform_right_product_impl< TransformType, MatrixType, 2, RhsCols> typedef typename MatrixType::PlainObject ResultType; - static EIGEN_STRONG_INLINE ResultType run(const TransformType& T, const MatrixType& other) + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType run(const TransformType& T, const MatrixType& other) { EIGEN_STATIC_ASSERT(OtherRows==Dim, YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES); @@ -1381,7 +1402,7 @@ struct transform_right_product_impl< TransformType, MatrixType, 2, 1> // rhs is typedef typename MatrixType::PlainObject ResultType; - static EIGEN_STRONG_INLINE ResultType run(const TransformType& T, const MatrixType& other) + static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType run(const TransformType& T, const MatrixType& other) { EIGEN_STATIC_ASSERT(OtherRows==Dim, YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES); diff --git a/Eigen/src/Geometry/Translation.h b/Eigen/src/Geometry/Translation.h index 51d9a82eb..8c2290121 100644 --- a/Eigen/src/Geometry/Translation.h +++ b/Eigen/src/Geometry/Translation.h @@ -70,18 +70,18 @@ public: /** 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. **/ + /** \brief Returns the x-translation by value. **/ EIGEN_DEVICE_FUNC inline Scalar x() const { return m_coeffs.x(); } - /** \brief Retruns the y-translation by value. **/ + /** \brief Returns the y-translation by value. **/ EIGEN_DEVICE_FUNC inline Scalar y() const { return m_coeffs.y(); } - /** \brief Retruns the z-translation by value. **/ + /** \brief Returns the z-translation by value. **/ EIGEN_DEVICE_FUNC inline Scalar z() const { return m_coeffs.z(); } - /** \brief Retruns the x-translation as a reference. **/ + /** \brief Returns the x-translation as a reference. **/ EIGEN_DEVICE_FUNC inline Scalar& x() { return m_coeffs.x(); } - /** \brief Retruns the y-translation as a reference. **/ + /** \brief Returns the y-translation as a reference. **/ EIGEN_DEVICE_FUNC inline Scalar& y() { return m_coeffs.y(); } - /** \brief Retruns the z-translation as a reference. **/ + /** \brief Returns 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; } @@ -138,12 +138,6 @@ public: /** \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 diff --git a/Eigen/src/Geometry/Umeyama.h b/Eigen/src/Geometry/Umeyama.h index 7e933fca1..6b755008f 100644 --- a/Eigen/src/Geometry/Umeyama.h +++ b/Eigen/src/Geometry/Umeyama.h @@ -87,7 +87,7 @@ struct umeyama_transform_matrix_type * \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 +* minimizing the residual above. This transformation is always returned as an * Eigen::Matrix. */ template <typename Derived, typename OtherDerived> diff --git a/Eigen/src/Geometry/arch/Geometry_SIMD.h b/Eigen/src/Geometry/arch/Geometry_SIMD.h new file mode 100644 index 000000000..9af6a9af7 --- /dev/null +++ b/Eigen/src/Geometry/arch/Geometry_SIMD.h @@ -0,0 +1,168 @@ +// 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_SIMD_H +#define EIGEN_GEOMETRY_SIMD_H + +namespace Eigen { + +namespace internal { + +template<class Derived, class OtherDerived> +struct quat_product<Architecture::Target, 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) + { + evaluator<typename Derived::Coefficients> ae(_a.coeffs()); + evaluator<typename OtherDerived::Coefficients> be(_b.coeffs()); + Quaternion<float> res; + const float neg_zero = numext::bit_cast<float>(0x80000000u); + const float arr[4] = {0.f, 0.f, 0.f, neg_zero}; + const Packet4f mask = ploadu<Packet4f>(arr); + Packet4f a = ae.template packet<AAlignment,Packet4f>(0); + Packet4f b = be.template packet<BAlignment,Packet4f>(0); + Packet4f s1 = pmul(vec4f_swizzle1(a,1,2,0,2),vec4f_swizzle1(b,2,0,1,2)); + Packet4f s2 = pmul(vec4f_swizzle1(a,3,3,3,1),vec4f_swizzle1(b,0,1,2,1)); + pstoret<float,Packet4f,ResAlignment>( + &res.x(), + padd(psub(pmul(a,vec4f_swizzle1(b,3,3,3,3)), + pmul(vec4f_swizzle1(a,2,0,1,0), + vec4f_swizzle1(b,1,2,0,0))), + pxor(mask,padd(s1,s2)))); + + return res; + } +}; + +template<class Derived> +struct quat_conj<Architecture::Target, Derived, float> +{ + enum { + ResAlignment = traits<Quaternion<float> >::Alignment + }; + static inline Quaternion<float> run(const QuaternionBase<Derived>& q) + { + evaluator<typename Derived::Coefficients> qe(q.coeffs()); + Quaternion<float> res; + const float neg_zero = numext::bit_cast<float>(0x80000000u); + const float arr[4] = {neg_zero, neg_zero, neg_zero,0.f}; + const Packet4f mask = ploadu<Packet4f>(arr); + pstoret<float,Packet4f,ResAlignment>(&res.x(), pxor(mask, qe.template packet<traits<Derived>::Alignment,Packet4f>(0))); + return res; + } +}; + + +template<typename VectorLhs,typename VectorRhs> +struct cross3_impl<Architecture::Target,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) + { + evaluator<VectorLhs> lhs_eval(lhs); + evaluator<VectorRhs> rhs_eval(rhs); + Packet4f a = lhs_eval.template packet<traits<VectorLhs>::Alignment,Packet4f>(0); + Packet4f b = rhs_eval.template packet<traits<VectorRhs>::Alignment,Packet4f>(0); + Packet4f mul1 = pmul(vec4f_swizzle1(a,1,2,0,3),vec4f_swizzle1(b,2,0,1,3)); + Packet4f mul2 = pmul(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(),psub(mul1,mul2)); + return res; + } +}; + + + +#if (defined EIGEN_VECTORIZE_SSE) || (EIGEN_ARCH_ARM64) + +template<class Derived, class OtherDerived> +struct quat_product<Architecture::Target, 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) + { + Quaternion<double> res; + + evaluator<typename Derived::Coefficients> ae(_a.coeffs()); + evaluator<typename OtherDerived::Coefficients> be(_b.coeffs()); + + const double* a = _a.coeffs().data(); + Packet2d b_xy = be.template packet<BAlignment,Packet2d>(0); + Packet2d b_zw = be.template packet<BAlignment,Packet2d>(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)); + pstoret<double,Packet2d,ResAlignment>(&res.x(), paddsub(t1, preverse(t2))); + + /* + * 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)); + pstoret<double,Packet2d,ResAlignment>(&res.z(), preverse(paddsub(preverse(t1), t2))); + + return res; +} +}; + +template<class Derived> +struct quat_conj<Architecture::Target, Derived, double> +{ + enum { + ResAlignment = traits<Quaternion<double> >::Alignment + }; + static inline Quaternion<double> run(const QuaternionBase<Derived>& q) + { + evaluator<typename Derived::Coefficients> qe(q.coeffs()); + Quaternion<double> res; + const double neg_zero = numext::bit_cast<double>(0x8000000000000000ull); + const double arr1[2] = {neg_zero, neg_zero}; + const double arr2[2] = {neg_zero, 0.0}; + const Packet2d mask0 = ploadu<Packet2d>(arr1); + const Packet2d mask2 = ploadu<Packet2d>(arr2); + pstoret<double,Packet2d,ResAlignment>(&res.x(), pxor(mask0, qe.template packet<traits<Derived>::Alignment,Packet2d>(0))); + pstoret<double,Packet2d,ResAlignment>(&res.z(), pxor(mask2, qe.template packet<traits<Derived>::Alignment,Packet2d>(2))); + return res; + } +}; + +#endif // end EIGEN_VECTORIZE_SSE_OR_EIGEN_ARCH_ARM64 + +} // end namespace internal + +} // end namespace Eigen + +#endif // EIGEN_GEOMETRY_SIMD_H diff --git a/Eigen/src/Geometry/arch/Geometry_SSE.h b/Eigen/src/Geometry/arch/Geometry_SSE.h deleted file mode 100644 index f68cab583..000000000 --- a/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 |