diff options
Diffstat (limited to 'Eigen/src/Geometry/Quaternion.h')
| -rw-r--r-- | Eigen/src/Geometry/Quaternion.h | 113 |
1 files changed, 87 insertions, 26 deletions
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(); |