Michael Ernst Peter / Eigen

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

src/Geometry/Quaternion.h@0:13a5d365ba16, 2016-10-13 (annotated)

Committer:
ykuroda
Date:
Thu Oct 13 04:07:23 2016 +0000
Revision:
0:13a5d365ba16
First commint, Eigne Matrix Class Library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ykuroda 0:13a5d365ba16 1 // This file is part of Eigen, a lightweight C++ template library
ykuroda 0:13a5d365ba16 2 // for linear algebra.
ykuroda 0:13a5d365ba16 3 //
ykuroda 0:13a5d365ba16 4 // Copyright (C) 2008-2010 Gael Guennebaud <gael.guennebaud@inria.fr>
ykuroda 0:13a5d365ba16 5 // Copyright (C) 2009 Mathieu Gautier <mathieu.gautier@cea.fr>
ykuroda 0:13a5d365ba16 6 //
ykuroda 0:13a5d365ba16 7 // This Source Code Form is subject to the terms of the Mozilla
ykuroda 0:13a5d365ba16 8 // Public License v. 2.0. If a copy of the MPL was not distributed
ykuroda 0:13a5d365ba16 9 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
ykuroda 0:13a5d365ba16 10
ykuroda 0:13a5d365ba16 11 #ifndef EIGEN_QUATERNION_H
ykuroda 0:13a5d365ba16 12 #define EIGEN_QUATERNION_H
ykuroda 0:13a5d365ba16 13 namespace Eigen {
ykuroda 0:13a5d365ba16 14
ykuroda 0:13a5d365ba16 15
ykuroda 0:13a5d365ba16 16 /***************************************************************************
ykuroda 0:13a5d365ba16 17 * Definition of QuaternionBase<Derived>
ykuroda 0:13a5d365ba16 18 * The implementation is at the end of the file
ykuroda 0:13a5d365ba16 19 ***************************************************************************/
ykuroda 0:13a5d365ba16 20
ykuroda 0:13a5d365ba16 21 namespace internal {
ykuroda 0:13a5d365ba16 22 template<typename Other,
ykuroda 0:13a5d365ba16 23 int OtherRows=Other::RowsAtCompileTime,
ykuroda 0:13a5d365ba16 24 int OtherCols=Other::ColsAtCompileTime>
ykuroda 0:13a5d365ba16 25 struct quaternionbase_assign_impl;
ykuroda 0:13a5d365ba16 26 }
ykuroda 0:13a5d365ba16 27
ykuroda 0:13a5d365ba16 28 /** \geometry_module \ingroup Geometry_Module
ykuroda 0:13a5d365ba16 29 * \class QuaternionBase
ykuroda 0:13a5d365ba16 30 * \brief Base class for quaternion expressions
ykuroda 0:13a5d365ba16 31 * \tparam Derived derived type (CRTP)
ykuroda 0:13a5d365ba16 32 * \sa class Quaternion
ykuroda 0:13a5d365ba16 33 */
ykuroda 0:13a5d365ba16 34 template<class Derived>
ykuroda 0:13a5d365ba16 35 class QuaternionBase : public RotationBase<Derived, 3>
ykuroda 0:13a5d365ba16 36 {
ykuroda 0:13a5d365ba16 37 typedef RotationBase<Derived, 3> Base;
ykuroda 0:13a5d365ba16 38 public:
ykuroda 0:13a5d365ba16 39 using Base::operator*;
ykuroda 0:13a5d365ba16 40 using Base::derived;
ykuroda 0:13a5d365ba16 41
ykuroda 0:13a5d365ba16 42 typedef typename internal::traits<Derived>::Scalar Scalar;
ykuroda 0:13a5d365ba16 43 typedef typename NumTraits<Scalar>::Real RealScalar;
ykuroda 0:13a5d365ba16 44 typedef typename internal::traits<Derived>::Coefficients Coefficients;
ykuroda 0:13a5d365ba16 45 enum {
ykuroda 0:13a5d365ba16 46 Flags = Eigen::internal::traits<Derived>::Flags
ykuroda 0:13a5d365ba16 47 };
ykuroda 0:13a5d365ba16 48
ykuroda 0:13a5d365ba16 49 // typedef typename Matrix<Scalar,4,1> Coefficients;
ykuroda 0:13a5d365ba16 50 /** the type of a 3D vector */
ykuroda 0:13a5d365ba16 51 typedef Matrix<Scalar,3,1> Vector3;
ykuroda 0:13a5d365ba16 52 /** the equivalent rotation matrix type */
ykuroda 0:13a5d365ba16 53 typedef Matrix<Scalar,3,3> Matrix3;
ykuroda 0:13a5d365ba16 54 /** the equivalent angle-axis type */
ykuroda 0:13a5d365ba16 55 typedef AngleAxis<Scalar> AngleAxisType;
ykuroda 0:13a5d365ba16 56
ykuroda 0:13a5d365ba16 57
ykuroda 0:13a5d365ba16 58
ykuroda 0:13a5d365ba16 59 /** \returns the \c x coefficient */
ykuroda 0:13a5d365ba16 60 inline Scalar x() const { return this->derived().coeffs().coeff(0); }
ykuroda 0:13a5d365ba16 61 /** \returns the \c y coefficient */
ykuroda 0:13a5d365ba16 62 inline Scalar y() const { return this->derived().coeffs().coeff(1); }
ykuroda 0:13a5d365ba16 63 /** \returns the \c z coefficient */
ykuroda 0:13a5d365ba16 64 inline Scalar z() const { return this->derived().coeffs().coeff(2); }
ykuroda 0:13a5d365ba16 65 /** \returns the \c w coefficient */
ykuroda 0:13a5d365ba16 66 inline Scalar w() const { return this->derived().coeffs().coeff(3); }
ykuroda 0:13a5d365ba16 67
ykuroda 0:13a5d365ba16 68 /** \returns a reference to the \c x coefficient */
ykuroda 0:13a5d365ba16 69 inline Scalar& x() { return this->derived().coeffs().coeffRef(0); }
ykuroda 0:13a5d365ba16 70 /** \returns a reference to the \c y coefficient */
ykuroda 0:13a5d365ba16 71 inline Scalar& y() { return this->derived().coeffs().coeffRef(1); }
ykuroda 0:13a5d365ba16 72 /** \returns a reference to the \c z coefficient */
ykuroda 0:13a5d365ba16 73 inline Scalar& z() { return this->derived().coeffs().coeffRef(2); }
ykuroda 0:13a5d365ba16 74 /** \returns a reference to the \c w coefficient */
ykuroda 0:13a5d365ba16 75 inline Scalar& w() { return this->derived().coeffs().coeffRef(3); }
ykuroda 0:13a5d365ba16 76
ykuroda 0:13a5d365ba16 77 /** \returns a read-only vector expression of the imaginary part (x,y,z) */
ykuroda 0:13a5d365ba16 78 inline const VectorBlock<const Coefficients,3> vec() const { return coeffs().template head<3>(); }
ykuroda 0:13a5d365ba16 79
ykuroda 0:13a5d365ba16 80 /** \returns a vector expression of the imaginary part (x,y,z) */
ykuroda 0:13a5d365ba16 81 inline VectorBlock<Coefficients,3> vec() { return coeffs().template head<3>(); }
ykuroda 0:13a5d365ba16 82
ykuroda 0:13a5d365ba16 83 /** \returns a read-only vector expression of the coefficients (x,y,z,w) */
ykuroda 0:13a5d365ba16 84 inline const typename internal::traits<Derived>::Coefficients& coeffs() const { return derived().coeffs(); }
ykuroda 0:13a5d365ba16 85
ykuroda 0:13a5d365ba16 86 /** \returns a vector expression of the coefficients (x,y,z,w) */
ykuroda 0:13a5d365ba16 87 inline typename internal::traits<Derived>::Coefficients& coeffs() { return derived().coeffs(); }
ykuroda 0:13a5d365ba16 88
ykuroda 0:13a5d365ba16 89 EIGEN_STRONG_INLINE QuaternionBase<Derived>& operator=(const QuaternionBase<Derived>& other);
ykuroda 0:13a5d365ba16 90 template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator=(const QuaternionBase<OtherDerived>& other);
ykuroda 0:13a5d365ba16 91
ykuroda 0:13a5d365ba16 92 // disabled this copy operator as it is giving very strange compilation errors when compiling
ykuroda 0:13a5d365ba16 93 // test_stdvector with GCC 4.4.2. This looks like a GCC bug though, so feel free to re-enable it if it's
ykuroda 0:13a5d365ba16 94 // useful; however notice that we already have the templated operator= above and e.g. in MatrixBase
ykuroda 0:13a5d365ba16 95 // we didn't have to add, in addition to templated operator=, such a non-templated copy operator.
ykuroda 0:13a5d365ba16 96 // Derived& operator=(const QuaternionBase& other)
ykuroda 0:13a5d365ba16 97 // { return operator=<Derived>(other); }
ykuroda 0:13a5d365ba16 98
ykuroda 0:13a5d365ba16 99 Derived& operator=(const AngleAxisType& aa);
ykuroda 0:13a5d365ba16 100 template<class OtherDerived> Derived& operator=(const MatrixBase<OtherDerived>& m);
ykuroda 0:13a5d365ba16 101
ykuroda 0:13a5d365ba16 102 /** \returns a quaternion representing an identity rotation
ykuroda 0:13a5d365ba16 103 * \sa MatrixBase::Identity()
ykuroda 0:13a5d365ba16 104 */
ykuroda 0:13a5d365ba16 105 static inline Quaternion<Scalar> Identity() { return Quaternion<Scalar>(Scalar(1), Scalar(0), Scalar(0), Scalar(0)); }
ykuroda 0:13a5d365ba16 106
ykuroda 0:13a5d365ba16 107 /** \sa QuaternionBase::Identity(), MatrixBase::setIdentity()
ykuroda 0:13a5d365ba16 108 */
ykuroda 0:13a5d365ba16 109 inline QuaternionBase& setIdentity() { coeffs() << Scalar(0), Scalar(0), Scalar(0), Scalar(1); return *this; }
ykuroda 0:13a5d365ba16 110
ykuroda 0:13a5d365ba16 111 /** \returns the squared norm of the quaternion's coefficients
ykuroda 0:13a5d365ba16 112 * \sa QuaternionBase::norm(), MatrixBase::squaredNorm()
ykuroda 0:13a5d365ba16 113 */
ykuroda 0:13a5d365ba16 114 inline Scalar squaredNorm() const { return coeffs().squaredNorm(); }
ykuroda 0:13a5d365ba16 115
ykuroda 0:13a5d365ba16 116 /** \returns the norm of the quaternion's coefficients
ykuroda 0:13a5d365ba16 117 * \sa QuaternionBase::squaredNorm(), MatrixBase::norm()
ykuroda 0:13a5d365ba16 118 */
ykuroda 0:13a5d365ba16 119 inline Scalar norm() const { return coeffs().norm(); }
ykuroda 0:13a5d365ba16 120
ykuroda 0:13a5d365ba16 121 /** Normalizes the quaternion \c *this
ykuroda 0:13a5d365ba16 122 * \sa normalized(), MatrixBase::normalize() */
ykuroda 0:13a5d365ba16 123 inline void normalize() { coeffs().normalize(); }
ykuroda 0:13a5d365ba16 124 /** \returns a normalized copy of \c *this
ykuroda 0:13a5d365ba16 125 * \sa normalize(), MatrixBase::normalized() */
ykuroda 0:13a5d365ba16 126 inline Quaternion<Scalar> normalized() const { return Quaternion<Scalar>(coeffs().normalized()); }
ykuroda 0:13a5d365ba16 127
ykuroda 0:13a5d365ba16 128 /** \returns the dot product of \c *this and \a other
ykuroda 0:13a5d365ba16 129 * Geometrically speaking, the dot product of two unit quaternions
ykuroda 0:13a5d365ba16 130 * corresponds to the cosine of half the angle between the two rotations.
ykuroda 0:13a5d365ba16 131 * \sa angularDistance()
ykuroda 0:13a5d365ba16 132 */
ykuroda 0:13a5d365ba16 133 template<class OtherDerived> inline Scalar dot(const QuaternionBase<OtherDerived>& other) const { return coeffs().dot(other.coeffs()); }
ykuroda 0:13a5d365ba16 134
ykuroda 0:13a5d365ba16 135 template<class OtherDerived> Scalar angularDistance(const QuaternionBase<OtherDerived>& other) const;
ykuroda 0:13a5d365ba16 136
ykuroda 0:13a5d365ba16 137 /** \returns an equivalent 3x3 rotation matrix */
ykuroda 0:13a5d365ba16 138 Matrix3 toRotationMatrix() const;
ykuroda 0:13a5d365ba16 139
ykuroda 0:13a5d365ba16 140 /** \returns the quaternion which transform \a a into \a b through a rotation */
ykuroda 0:13a5d365ba16 141 template<typename Derived1, typename Derived2>
ykuroda 0:13a5d365ba16 142 Derived& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
ykuroda 0:13a5d365ba16 143
ykuroda 0:13a5d365ba16 144 template<class OtherDerived> EIGEN_STRONG_INLINE Quaternion<Scalar> operator* (const QuaternionBase<OtherDerived>& q) const;
ykuroda 0:13a5d365ba16 145 template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator*= (const QuaternionBase<OtherDerived>& q);
ykuroda 0:13a5d365ba16 146
ykuroda 0:13a5d365ba16 147 /** \returns the quaternion describing the inverse rotation */
ykuroda 0:13a5d365ba16 148 Quaternion<Scalar> inverse() const;
ykuroda 0:13a5d365ba16 149
ykuroda 0:13a5d365ba16 150 /** \returns the conjugated quaternion */
ykuroda 0:13a5d365ba16 151 Quaternion<Scalar> conjugate() const;
ykuroda 0:13a5d365ba16 152
ykuroda 0:13a5d365ba16 153 template<class OtherDerived> Quaternion<Scalar> slerp(const Scalar& t, const QuaternionBase<OtherDerived>& other) const;
ykuroda 0:13a5d365ba16 154
ykuroda 0:13a5d365ba16 155 /** \returns \c true if \c *this is approximately equal to \a other, within the precision
ykuroda 0:13a5d365ba16 156 * determined by \a prec.
ykuroda 0:13a5d365ba16 157 *
ykuroda 0:13a5d365ba16 158 * \sa MatrixBase::isApprox() */
ykuroda 0:13a5d365ba16 159 template<class OtherDerived>
ykuroda 0:13a5d365ba16 160 bool isApprox(const QuaternionBase<OtherDerived>& other, const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const
ykuroda 0:13a5d365ba16 161 { return coeffs().isApprox(other.coeffs(), prec); }
ykuroda 0:13a5d365ba16 162
ykuroda 0:13a5d365ba16 163 /** return the result vector of \a v through the rotation*/
ykuroda 0:13a5d365ba16 164 EIGEN_STRONG_INLINE Vector3 _transformVector(const Vector3& v) const;
ykuroda 0:13a5d365ba16 165
ykuroda 0:13a5d365ba16 166 /** \returns \c *this with scalar type casted to \a NewScalarType
ykuroda 0:13a5d365ba16 167 *
ykuroda 0:13a5d365ba16 168 * Note that if \a NewScalarType is equal to the current scalar type of \c *this
ykuroda 0:13a5d365ba16 169 * then this function smartly returns a const reference to \c *this.
ykuroda 0:13a5d365ba16 170 */
ykuroda 0:13a5d365ba16 171 template<typename NewScalarType>
ykuroda 0:13a5d365ba16 172 inline typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type cast() const
ykuroda 0:13a5d365ba16 173 {
ykuroda 0:13a5d365ba16 174 return typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type(derived());
ykuroda 0:13a5d365ba16 175 }
ykuroda 0:13a5d365ba16 176
ykuroda 0:13a5d365ba16 177 #ifdef EIGEN_QUATERNIONBASE_PLUGIN
ykuroda 0:13a5d365ba16 178 # include EIGEN_QUATERNIONBASE_PLUGIN
ykuroda 0:13a5d365ba16 179 #endif
ykuroda 0:13a5d365ba16 180 };
ykuroda 0:13a5d365ba16 181
ykuroda 0:13a5d365ba16 182 /***************************************************************************
ykuroda 0:13a5d365ba16 183 * Definition/implementation of Quaternion<Scalar>
ykuroda 0:13a5d365ba16 184 ***************************************************************************/
ykuroda 0:13a5d365ba16 185
ykuroda 0:13a5d365ba16 186 /** \geometry_module \ingroup Geometry_Module
ykuroda 0:13a5d365ba16 187 *
ykuroda 0:13a5d365ba16 188 * \class Quaternion
ykuroda 0:13a5d365ba16 189 *
ykuroda 0:13a5d365ba16 190 * \brief The quaternion class used to represent 3D orientations and rotations
ykuroda 0:13a5d365ba16 191 *
ykuroda 0:13a5d365ba16 192 * \tparam _Scalar the scalar type, i.e., the type of the coefficients
ykuroda 0:13a5d365ba16 193 * \tparam _Options controls the memory alignment of the coefficients. Can be \# AutoAlign or \# DontAlign. Default is AutoAlign.
ykuroda 0:13a5d365ba16 194 *
ykuroda 0:13a5d365ba16 195 * This class represents a quaternion \f$ w+xi+yj+zk \f$ that is a convenient representation of
ykuroda 0:13a5d365ba16 196 * orientations and rotations of objects in three dimensions. Compared to other representations
ykuroda 0:13a5d365ba16 197 * like Euler angles or 3x3 matrices, quaternions offer the following advantages:
ykuroda 0:13a5d365ba16 198 * \li \b compact storage (4 scalars)
ykuroda 0:13a5d365ba16 199 * \li \b efficient to compose (28 flops),
ykuroda 0:13a5d365ba16 200 * \li \b stable spherical interpolation
ykuroda 0:13a5d365ba16 201 *
ykuroda 0:13a5d365ba16 202 * The following two typedefs are provided for convenience:
ykuroda 0:13a5d365ba16 203 * \li \c Quaternionf for \c float
ykuroda 0:13a5d365ba16 204 * \li \c Quaterniond for \c double
ykuroda 0:13a5d365ba16 205 *
ykuroda 0:13a5d365ba16 206 * \warning Operations interpreting the quaternion as rotation have undefined behavior if the quaternion is not normalized.
ykuroda 0:13a5d365ba16 207 *
ykuroda 0:13a5d365ba16 208 * \sa class AngleAxis, class Transform
ykuroda 0:13a5d365ba16 209 */
ykuroda 0:13a5d365ba16 210
ykuroda 0:13a5d365ba16 211 namespace internal {
ykuroda 0:13a5d365ba16 212 template<typename _Scalar,int _Options>
ykuroda 0:13a5d365ba16 213 struct traits<Quaternion<_Scalar,_Options> >
ykuroda 0:13a5d365ba16 214 {
ykuroda 0:13a5d365ba16 215 typedef Quaternion<_Scalar,_Options> PlainObject;
ykuroda 0:13a5d365ba16 216 typedef _Scalar Scalar;
ykuroda 0:13a5d365ba16 217 typedef Matrix<_Scalar,4,1,_Options> Coefficients;
ykuroda 0:13a5d365ba16 218 enum{
ykuroda 0:13a5d365ba16 219 IsAligned = internal::traits<Coefficients>::Flags & AlignedBit,
ykuroda 0:13a5d365ba16 220 Flags = IsAligned ? (AlignedBit | LvalueBit) : LvalueBit
ykuroda 0:13a5d365ba16 221 };
ykuroda 0:13a5d365ba16 222 };
ykuroda 0:13a5d365ba16 223 }
ykuroda 0:13a5d365ba16 224
ykuroda 0:13a5d365ba16 225 template<typename _Scalar, int _Options>
ykuroda 0:13a5d365ba16 226 class Quaternion : public QuaternionBase<Quaternion<_Scalar,_Options> >
ykuroda 0:13a5d365ba16 227 {
ykuroda 0:13a5d365ba16 228 typedef QuaternionBase<Quaternion<_Scalar,_Options> > Base;
ykuroda 0:13a5d365ba16 229 enum { IsAligned = internal::traits<Quaternion>::IsAligned };
ykuroda 0:13a5d365ba16 230
ykuroda 0:13a5d365ba16 231 public:
ykuroda 0:13a5d365ba16 232 typedef _Scalar Scalar;
ykuroda 0:13a5d365ba16 233
ykuroda 0:13a5d365ba16 234 EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Quaternion)
ykuroda 0:13a5d365ba16 235 using Base::operator*=;
ykuroda 0:13a5d365ba16 236
ykuroda 0:13a5d365ba16 237 typedef typename internal::traits<Quaternion>::Coefficients Coefficients;
ykuroda 0:13a5d365ba16 238 typedef typename Base::AngleAxisType AngleAxisType;
ykuroda 0:13a5d365ba16 239
ykuroda 0:13a5d365ba16 240 /** Default constructor leaving the quaternion uninitialized. */
ykuroda 0:13a5d365ba16 241 inline Quaternion() {}
ykuroda 0:13a5d365ba16 242
ykuroda 0:13a5d365ba16 243 /** Constructs and initializes the quaternion \f$ w+xi+yj+zk \f$ from
ykuroda 0:13a5d365ba16 244 * its four coefficients \a w, \a x, \a y and \a z.
ykuroda 0:13a5d365ba16 245 *
ykuroda 0:13a5d365ba16 246 * \warning Note the order of the arguments: the real \a w coefficient first,
ykuroda 0:13a5d365ba16 247 * while internally the coefficients are stored in the following order:
ykuroda 0:13a5d365ba16 248 * [\c x, \c y, \c z, \c w]
ykuroda 0:13a5d365ba16 249 */
ykuroda 0:13a5d365ba16 250 inline Quaternion(const Scalar& w, const Scalar& x, const Scalar& y, const Scalar& z) : m_coeffs(x, y, z, w){}
ykuroda 0:13a5d365ba16 251
ykuroda 0:13a5d365ba16 252 /** Constructs and initialize a quaternion from the array data */
ykuroda 0:13a5d365ba16 253 inline Quaternion(const Scalar* data) : m_coeffs(data) {}
ykuroda 0:13a5d365ba16 254
ykuroda 0:13a5d365ba16 255 /** Copy constructor */
ykuroda 0:13a5d365ba16 256 template<class Derived> EIGEN_STRONG_INLINE Quaternion(const QuaternionBase<Derived>& other) { this->Base::operator=(other); }
ykuroda 0:13a5d365ba16 257
ykuroda 0:13a5d365ba16 258 /** Constructs and initializes a quaternion from the angle-axis \a aa */
ykuroda 0:13a5d365ba16 259 explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; }
ykuroda 0:13a5d365ba16 260
ykuroda 0:13a5d365ba16 261 /** Constructs and initializes a quaternion from either:
ykuroda 0:13a5d365ba16 262 * - a rotation matrix expression,
ykuroda 0:13a5d365ba16 263 * - a 4D vector expression representing quaternion coefficients.
ykuroda 0:13a5d365ba16 264 */
ykuroda 0:13a5d365ba16 265 template<typename Derived>
ykuroda 0:13a5d365ba16 266 explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; }
ykuroda 0:13a5d365ba16 267
ykuroda 0:13a5d365ba16 268 /** Explicit copy constructor with scalar conversion */
ykuroda 0:13a5d365ba16 269 template<typename OtherScalar, int OtherOptions>
ykuroda 0:13a5d365ba16 270 explicit inline Quaternion(const Quaternion<OtherScalar, OtherOptions>& other)
ykuroda 0:13a5d365ba16 271 { m_coeffs = other.coeffs().template cast<Scalar>(); }
ykuroda 0:13a5d365ba16 272
ykuroda 0:13a5d365ba16 273 template<typename Derived1, typename Derived2>
ykuroda 0:13a5d365ba16 274 static Quaternion FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
ykuroda 0:13a5d365ba16 275
ykuroda 0:13a5d365ba16 276 inline Coefficients& coeffs() { return m_coeffs;}
ykuroda 0:13a5d365ba16 277 inline const Coefficients& coeffs() const { return m_coeffs;}
ykuroda 0:13a5d365ba16 278
ykuroda 0:13a5d365ba16 279 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(IsAligned)
ykuroda 0:13a5d365ba16 280
ykuroda 0:13a5d365ba16 281 protected:
ykuroda 0:13a5d365ba16 282 Coefficients m_coeffs;
ykuroda 0:13a5d365ba16 283
ykuroda 0:13a5d365ba16 284 #ifndef EIGEN_PARSED_BY_DOXYGEN
ykuroda 0:13a5d365ba16 285 static EIGEN_STRONG_INLINE void _check_template_params()
ykuroda 0:13a5d365ba16 286 {
ykuroda 0:13a5d365ba16 287 EIGEN_STATIC_ASSERT( (_Options & DontAlign) == _Options,
ykuroda 0:13a5d365ba16 288 INVALID_MATRIX_TEMPLATE_PARAMETERS)
ykuroda 0:13a5d365ba16 289 }
ykuroda 0:13a5d365ba16 290 #endif
ykuroda 0:13a5d365ba16 291 };
ykuroda 0:13a5d365ba16 292
ykuroda 0:13a5d365ba16 293 /** \ingroup Geometry_Module
ykuroda 0:13a5d365ba16 294 * single precision quaternion type */
ykuroda 0:13a5d365ba16 295 typedef Quaternion<float> Quaternionf;
ykuroda 0:13a5d365ba16 296 /** \ingroup Geometry_Module
ykuroda 0:13a5d365ba16 297 * double precision quaternion type */
ykuroda 0:13a5d365ba16 298 typedef Quaternion<double> Quaterniond;
ykuroda 0:13a5d365ba16 299
ykuroda 0:13a5d365ba16 300 /***************************************************************************
ykuroda 0:13a5d365ba16 301 * Specialization of Map<Quaternion<Scalar>>
ykuroda 0:13a5d365ba16 302 ***************************************************************************/
ykuroda 0:13a5d365ba16 303
ykuroda 0:13a5d365ba16 304 namespace internal {
ykuroda 0:13a5d365ba16 305 template<typename _Scalar, int _Options>
ykuroda 0:13a5d365ba16 306 struct traits<Map<Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> >
ykuroda 0:13a5d365ba16 307 {
ykuroda 0:13a5d365ba16 308 typedef Map<Matrix<_Scalar,4,1>, _Options> Coefficients;
ykuroda 0:13a5d365ba16 309 };
ykuroda 0:13a5d365ba16 310 }
ykuroda 0:13a5d365ba16 311
ykuroda 0:13a5d365ba16 312 namespace internal {
ykuroda 0:13a5d365ba16 313 template<typename _Scalar, int _Options>
ykuroda 0:13a5d365ba16 314 struct traits<Map<const Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> >
ykuroda 0:13a5d365ba16 315 {
ykuroda 0:13a5d365ba16 316 typedef Map<const Matrix<_Scalar,4,1>, _Options> Coefficients;
ykuroda 0:13a5d365ba16 317 typedef traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> > TraitsBase;
ykuroda 0:13a5d365ba16 318 enum {
ykuroda 0:13a5d365ba16 319 Flags = TraitsBase::Flags & ~LvalueBit
ykuroda 0:13a5d365ba16 320 };
ykuroda 0:13a5d365ba16 321 };
ykuroda 0:13a5d365ba16 322 }
ykuroda 0:13a5d365ba16 323
ykuroda 0:13a5d365ba16 324 /** \ingroup Geometry_Module
ykuroda 0:13a5d365ba16 325 * \brief Quaternion expression mapping a constant memory buffer
ykuroda 0:13a5d365ba16 326 *
ykuroda 0:13a5d365ba16 327 * \tparam _Scalar the type of the Quaternion coefficients
ykuroda 0:13a5d365ba16 328 * \tparam _Options see class Map
ykuroda 0:13a5d365ba16 329 *
ykuroda 0:13a5d365ba16 330 * This is a specialization of class Map for Quaternion. This class allows to view
ykuroda 0:13a5d365ba16 331 * a 4 scalar memory buffer as an Eigen's Quaternion object.
ykuroda 0:13a5d365ba16 332 *
ykuroda 0:13a5d365ba16 333 * \sa class Map, class Quaternion, class QuaternionBase
ykuroda 0:13a5d365ba16 334 */
ykuroda 0:13a5d365ba16 335 template<typename _Scalar, int _Options>
ykuroda 0:13a5d365ba16 336 class Map<const Quaternion<_Scalar>, _Options >
ykuroda 0:13a5d365ba16 337 : public QuaternionBase<Map<const Quaternion<_Scalar>, _Options> >
ykuroda 0:13a5d365ba16 338 {
ykuroda 0:13a5d365ba16 339 typedef QuaternionBase<Map<const Quaternion<_Scalar>, _Options> > Base;
ykuroda 0:13a5d365ba16 340
ykuroda 0:13a5d365ba16 341 public:
ykuroda 0:13a5d365ba16 342 typedef _Scalar Scalar;
ykuroda 0:13a5d365ba16 343 typedef typename internal::traits<Map>::Coefficients Coefficients;
ykuroda 0:13a5d365ba16 344 EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map)
ykuroda 0:13a5d365ba16 345 using Base::operator*=;
ykuroda 0:13a5d365ba16 346
ykuroda 0:13a5d365ba16 347 /** Constructs a Mapped Quaternion object from the pointer \a coeffs
ykuroda 0:13a5d365ba16 348 *
ykuroda 0:13a5d365ba16 349 * The pointer \a coeffs must reference the four coefficients of Quaternion in the following order:
ykuroda 0:13a5d365ba16 350 * \code *coeffs == {x, y, z, w} \endcode
ykuroda 0:13a5d365ba16 351 *
ykuroda 0:13a5d365ba16 352 * If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */
ykuroda 0:13a5d365ba16 353 EIGEN_STRONG_INLINE Map(const Scalar* coeffs) : m_coeffs(coeffs) {}
ykuroda 0:13a5d365ba16 354
ykuroda 0:13a5d365ba16 355 inline const Coefficients& coeffs() const { return m_coeffs;}
ykuroda 0:13a5d365ba16 356
ykuroda 0:13a5d365ba16 357 protected:
ykuroda 0:13a5d365ba16 358 const Coefficients m_coeffs;
ykuroda 0:13a5d365ba16 359 };
ykuroda 0:13a5d365ba16 360
ykuroda 0:13a5d365ba16 361 /** \ingroup Geometry_Module
ykuroda 0:13a5d365ba16 362 * \brief Expression of a quaternion from a memory buffer
ykuroda 0:13a5d365ba16 363 *
ykuroda 0:13a5d365ba16 364 * \tparam _Scalar the type of the Quaternion coefficients
ykuroda 0:13a5d365ba16 365 * \tparam _Options see class Map
ykuroda 0:13a5d365ba16 366 *
ykuroda 0:13a5d365ba16 367 * This is a specialization of class Map for Quaternion. This class allows to view
ykuroda 0:13a5d365ba16 368 * a 4 scalar memory buffer as an Eigen's Quaternion object.
ykuroda 0:13a5d365ba16 369 *
ykuroda 0:13a5d365ba16 370 * \sa class Map, class Quaternion, class QuaternionBase
ykuroda 0:13a5d365ba16 371 */
ykuroda 0:13a5d365ba16 372 template<typename _Scalar, int _Options>
ykuroda 0:13a5d365ba16 373 class Map<Quaternion<_Scalar>, _Options >
ykuroda 0:13a5d365ba16 374 : public QuaternionBase<Map<Quaternion<_Scalar>, _Options> >
ykuroda 0:13a5d365ba16 375 {
ykuroda 0:13a5d365ba16 376 typedef QuaternionBase<Map<Quaternion<_Scalar>, _Options> > Base;
ykuroda 0:13a5d365ba16 377
ykuroda 0:13a5d365ba16 378 public:
ykuroda 0:13a5d365ba16 379 typedef _Scalar Scalar;
ykuroda 0:13a5d365ba16 380 typedef typename internal::traits<Map>::Coefficients Coefficients;
ykuroda 0:13a5d365ba16 381 EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map)
ykuroda 0:13a5d365ba16 382 using Base::operator*=;
ykuroda 0:13a5d365ba16 383
ykuroda 0:13a5d365ba16 384 /** Constructs a Mapped Quaternion object from the pointer \a coeffs
ykuroda 0:13a5d365ba16 385 *
ykuroda 0:13a5d365ba16 386 * The pointer \a coeffs must reference the four coefficients of Quaternion in the following order:
ykuroda 0:13a5d365ba16 387 * \code *coeffs == {x, y, z, w} \endcode
ykuroda 0:13a5d365ba16 388 *
ykuroda 0:13a5d365ba16 389 * If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */
ykuroda 0:13a5d365ba16 390 EIGEN_STRONG_INLINE Map(Scalar* coeffs) : m_coeffs(coeffs) {}
ykuroda 0:13a5d365ba16 391
ykuroda 0:13a5d365ba16 392 inline Coefficients& coeffs() { return m_coeffs; }
ykuroda 0:13a5d365ba16 393 inline const Coefficients& coeffs() const { return m_coeffs; }
ykuroda 0:13a5d365ba16 394
ykuroda 0:13a5d365ba16 395 protected:
ykuroda 0:13a5d365ba16 396 Coefficients m_coeffs;
ykuroda 0:13a5d365ba16 397 };
ykuroda 0:13a5d365ba16 398
ykuroda 0:13a5d365ba16 399 /** \ingroup Geometry_Module
ykuroda 0:13a5d365ba16 400 * Map an unaligned array of single precision scalars as a quaternion */
ykuroda 0:13a5d365ba16 401 typedef Map<Quaternion<float>, 0> QuaternionMapf;
ykuroda 0:13a5d365ba16 402 /** \ingroup Geometry_Module
ykuroda 0:13a5d365ba16 403 * Map an unaligned array of double precision scalars as a quaternion */
ykuroda 0:13a5d365ba16 404 typedef Map<Quaternion<double>, 0> QuaternionMapd;
ykuroda 0:13a5d365ba16 405 /** \ingroup Geometry_Module
ykuroda 0:13a5d365ba16 406 * Map a 16-byte aligned array of single precision scalars as a quaternion */
ykuroda 0:13a5d365ba16 407 typedef Map<Quaternion<float>, Aligned> QuaternionMapAlignedf;
ykuroda 0:13a5d365ba16 408 /** \ingroup Geometry_Module
ykuroda 0:13a5d365ba16 409 * Map a 16-byte aligned array of double precision scalars as a quaternion */
ykuroda 0:13a5d365ba16 410 typedef Map<Quaternion<double>, Aligned> QuaternionMapAlignedd;
ykuroda 0:13a5d365ba16 411
ykuroda 0:13a5d365ba16 412 /***************************************************************************
ykuroda 0:13a5d365ba16 413 * Implementation of QuaternionBase methods
ykuroda 0:13a5d365ba16 414 ***************************************************************************/
ykuroda 0:13a5d365ba16 415
ykuroda 0:13a5d365ba16 416 // Generic Quaternion * Quaternion product
ykuroda 0:13a5d365ba16 417 // This product can be specialized for a given architecture via the Arch template argument.
ykuroda 0:13a5d365ba16 418 namespace internal {
ykuroda 0:13a5d365ba16 419 template<int Arch, class Derived1, class Derived2, typename Scalar, int _Options> struct quat_product
ykuroda 0:13a5d365ba16 420 {
ykuroda 0:13a5d365ba16 421 static EIGEN_STRONG_INLINE Quaternion<Scalar> run(const QuaternionBase<Derived1>& a, const QuaternionBase<Derived2>& b){
ykuroda 0:13a5d365ba16 422 return Quaternion<Scalar>
ykuroda 0:13a5d365ba16 423 (
ykuroda 0:13a5d365ba16 424 a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
ykuroda 0:13a5d365ba16 425 a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
ykuroda 0:13a5d365ba16 426 a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
ykuroda 0:13a5d365ba16 427 a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()
ykuroda 0:13a5d365ba16 428 );
ykuroda 0:13a5d365ba16 429 }
ykuroda 0:13a5d365ba16 430 };
ykuroda 0:13a5d365ba16 431 }
ykuroda 0:13a5d365ba16 432
ykuroda 0:13a5d365ba16 433 /** \returns the concatenation of two rotations as a quaternion-quaternion product */
ykuroda 0:13a5d365ba16 434 template <class Derived>
ykuroda 0:13a5d365ba16 435 template <class OtherDerived>
ykuroda 0:13a5d365ba16 436 EIGEN_STRONG_INLINE Quaternion<typename internal::traits<Derived>::Scalar>
ykuroda 0:13a5d365ba16 437 QuaternionBase<Derived>::operator* (const QuaternionBase<OtherDerived>& other) const
ykuroda 0:13a5d365ba16 438 {
ykuroda 0:13a5d365ba16 439 EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename OtherDerived::Scalar>::value),
ykuroda 0:13a5d365ba16 440 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
ykuroda 0:13a5d365ba16 441 return internal::quat_product<Architecture::Target, Derived, OtherDerived,
ykuroda 0:13a5d365ba16 442 typename internal::traits<Derived>::Scalar,
ykuroda 0:13a5d365ba16 443 internal::traits<Derived>::IsAligned && internal::traits<OtherDerived>::IsAligned>::run(*this, other);
ykuroda 0:13a5d365ba16 444 }
ykuroda 0:13a5d365ba16 445
ykuroda 0:13a5d365ba16 446 /** \sa operator*(Quaternion) */
ykuroda 0:13a5d365ba16 447 template <class Derived>
ykuroda 0:13a5d365ba16 448 template <class OtherDerived>
ykuroda 0:13a5d365ba16 449 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator*= (const QuaternionBase<OtherDerived>& other)
ykuroda 0:13a5d365ba16 450 {
ykuroda 0:13a5d365ba16 451 derived() = derived() * other.derived();
ykuroda 0:13a5d365ba16 452 return derived();
ykuroda 0:13a5d365ba16 453 }
ykuroda 0:13a5d365ba16 454
ykuroda 0:13a5d365ba16 455 /** Rotation of a vector by a quaternion.
ykuroda 0:13a5d365ba16 456 * \remarks If the quaternion is used to rotate several points (>1)
ykuroda 0:13a5d365ba16 457 * then it is much more efficient to first convert it to a 3x3 Matrix.
ykuroda 0:13a5d365ba16 458 * Comparison of the operation cost for n transformations:
ykuroda 0:13a5d365ba16 459 * - Quaternion2: 30n
ykuroda 0:13a5d365ba16 460 * - Via a Matrix3: 24 + 15n
ykuroda 0:13a5d365ba16 461 */
ykuroda 0:13a5d365ba16 462 template <class Derived>
ykuroda 0:13a5d365ba16 463 EIGEN_STRONG_INLINE typename QuaternionBase<Derived>::Vector3
ykuroda 0:13a5d365ba16 464 QuaternionBase<Derived>::_transformVector(const Vector3& v) const
ykuroda 0:13a5d365ba16 465 {
ykuroda 0:13a5d365ba16 466 // Note that this algorithm comes from the optimization by hand
ykuroda 0:13a5d365ba16 467 // of the conversion to a Matrix followed by a Matrix/Vector product.
ykuroda 0:13a5d365ba16 468 // It appears to be much faster than the common algorithm found
ykuroda 0:13a5d365ba16 469 // in the literature (30 versus 39 flops). It also requires two
ykuroda 0:13a5d365ba16 470 // Vector3 as temporaries.
ykuroda 0:13a5d365ba16 471 Vector3 uv = this->vec().cross(v);
ykuroda 0:13a5d365ba16 472 uv += uv;
ykuroda 0:13a5d365ba16 473 return v + this->w() * uv + this->vec().cross(uv);
ykuroda 0:13a5d365ba16 474 }
ykuroda 0:13a5d365ba16 475
ykuroda 0:13a5d365ba16 476 template<class Derived>
ykuroda 0:13a5d365ba16 477 EIGEN_STRONG_INLINE QuaternionBase<Derived>& QuaternionBase<Derived>::operator=(const QuaternionBase<Derived>& other)
ykuroda 0:13a5d365ba16 478 {
ykuroda 0:13a5d365ba16 479 coeffs() = other.coeffs();
ykuroda 0:13a5d365ba16 480 return derived();
ykuroda 0:13a5d365ba16 481 }
ykuroda 0:13a5d365ba16 482
ykuroda 0:13a5d365ba16 483 template<class Derived>
ykuroda 0:13a5d365ba16 484 template<class OtherDerived>
ykuroda 0:13a5d365ba16 485 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const QuaternionBase<OtherDerived>& other)
ykuroda 0:13a5d365ba16 486 {
ykuroda 0:13a5d365ba16 487 coeffs() = other.coeffs();
ykuroda 0:13a5d365ba16 488 return derived();
ykuroda 0:13a5d365ba16 489 }
ykuroda 0:13a5d365ba16 490
ykuroda 0:13a5d365ba16 491 /** Set \c *this from an angle-axis \a aa and returns a reference to \c *this
ykuroda 0:13a5d365ba16 492 */
ykuroda 0:13a5d365ba16 493 template<class Derived>
ykuroda 0:13a5d365ba16 494 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const AngleAxisType& aa)
ykuroda 0:13a5d365ba16 495 {
ykuroda 0:13a5d365ba16 496 using std::cos;
ykuroda 0:13a5d365ba16 497 using std::sin;
ykuroda 0:13a5d365ba16 498 Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings
ykuroda 0:13a5d365ba16 499 this->w() = cos(ha);
ykuroda 0:13a5d365ba16 500 this->vec() = sin(ha) * aa.axis();
ykuroda 0:13a5d365ba16 501 return derived();
ykuroda 0:13a5d365ba16 502 }
ykuroda 0:13a5d365ba16 503
ykuroda 0:13a5d365ba16 504 /** Set \c *this from the expression \a xpr:
ykuroda 0:13a5d365ba16 505 * - if \a xpr is a 4x1 vector, then \a xpr is assumed to be a quaternion
ykuroda 0:13a5d365ba16 506 * - if \a xpr is a 3x3 matrix, then \a xpr is assumed to be rotation matrix
ykuroda 0:13a5d365ba16 507 * and \a xpr is converted to a quaternion
ykuroda 0:13a5d365ba16 508 */
ykuroda 0:13a5d365ba16 509
ykuroda 0:13a5d365ba16 510 template<class Derived>
ykuroda 0:13a5d365ba16 511 template<class MatrixDerived>
ykuroda 0:13a5d365ba16 512 inline Derived& QuaternionBase<Derived>::operator=(const MatrixBase<MatrixDerived>& xpr)
ykuroda 0:13a5d365ba16 513 {
ykuroda 0:13a5d365ba16 514 EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename MatrixDerived::Scalar>::value),
ykuroda 0:13a5d365ba16 515 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
ykuroda 0:13a5d365ba16 516 internal::quaternionbase_assign_impl<MatrixDerived>::run(*this, xpr.derived());
ykuroda 0:13a5d365ba16 517 return derived();
ykuroda 0:13a5d365ba16 518 }
ykuroda 0:13a5d365ba16 519
ykuroda 0:13a5d365ba16 520 /** Convert the quaternion to a 3x3 rotation matrix. The quaternion is required to
ykuroda 0:13a5d365ba16 521 * be normalized, otherwise the result is undefined.
ykuroda 0:13a5d365ba16 522 */
ykuroda 0:13a5d365ba16 523 template<class Derived>
ykuroda 0:13a5d365ba16 524 inline typename QuaternionBase<Derived>::Matrix3
ykuroda 0:13a5d365ba16 525 QuaternionBase<Derived>::toRotationMatrix(void) const
ykuroda 0:13a5d365ba16 526 {
ykuroda 0:13a5d365ba16 527 // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!)
ykuroda 0:13a5d365ba16 528 // if not inlined then the cost of the return by value is huge ~ +35%,
ykuroda 0:13a5d365ba16 529 // however, not inlining this function is an order of magnitude slower, so
ykuroda 0:13a5d365ba16 530 // it has to be inlined, and so the return by value is not an issue
ykuroda 0:13a5d365ba16 531 Matrix3 res;
ykuroda 0:13a5d365ba16 532
ykuroda 0:13a5d365ba16 533 const Scalar tx = Scalar(2)*this->x();
ykuroda 0:13a5d365ba16 534 const Scalar ty = Scalar(2)*this->y();
ykuroda 0:13a5d365ba16 535 const Scalar tz = Scalar(2)*this->z();
ykuroda 0:13a5d365ba16 536 const Scalar twx = tx*this->w();
ykuroda 0:13a5d365ba16 537 const Scalar twy = ty*this->w();
ykuroda 0:13a5d365ba16 538 const Scalar twz = tz*this->w();
ykuroda 0:13a5d365ba16 539 const Scalar txx = tx*this->x();
ykuroda 0:13a5d365ba16 540 const Scalar txy = ty*this->x();
ykuroda 0:13a5d365ba16 541 const Scalar txz = tz*this->x();
ykuroda 0:13a5d365ba16 542 const Scalar tyy = ty*this->y();
ykuroda 0:13a5d365ba16 543 const Scalar tyz = tz*this->y();
ykuroda 0:13a5d365ba16 544 const Scalar tzz = tz*this->z();
ykuroda 0:13a5d365ba16 545
ykuroda 0:13a5d365ba16 546 res.coeffRef(0,0) = Scalar(1)-(tyy+tzz);
ykuroda 0:13a5d365ba16 547 res.coeffRef(0,1) = txy-twz;
ykuroda 0:13a5d365ba16 548 res.coeffRef(0,2) = txz+twy;
ykuroda 0:13a5d365ba16 549 res.coeffRef(1,0) = txy+twz;
ykuroda 0:13a5d365ba16 550 res.coeffRef(1,1) = Scalar(1)-(txx+tzz);
ykuroda 0:13a5d365ba16 551 res.coeffRef(1,2) = tyz-twx;
ykuroda 0:13a5d365ba16 552 res.coeffRef(2,0) = txz-twy;
ykuroda 0:13a5d365ba16 553 res.coeffRef(2,1) = tyz+twx;
ykuroda 0:13a5d365ba16 554 res.coeffRef(2,2) = Scalar(1)-(txx+tyy);
ykuroda 0:13a5d365ba16 555
ykuroda 0:13a5d365ba16 556 return res;
ykuroda 0:13a5d365ba16 557 }
ykuroda 0:13a5d365ba16 558
ykuroda 0:13a5d365ba16 559 /** Sets \c *this to be a quaternion representing a rotation between
ykuroda 0:13a5d365ba16 560 * the two arbitrary vectors \a a and \a b. In other words, the built
ykuroda 0:13a5d365ba16 561 * rotation represent a rotation sending the line of direction \a a
ykuroda 0:13a5d365ba16 562 * to the line of direction \a b, both lines passing through the origin.
ykuroda 0:13a5d365ba16 563 *
ykuroda 0:13a5d365ba16 564 * \returns a reference to \c *this.
ykuroda 0:13a5d365ba16 565 *
ykuroda 0:13a5d365ba16 566 * Note that the two input vectors do \b not have to be normalized, and
ykuroda 0:13a5d365ba16 567 * do not need to have the same norm.
ykuroda 0:13a5d365ba16 568 */
ykuroda 0:13a5d365ba16 569 template<class Derived>
ykuroda 0:13a5d365ba16 570 template<typename Derived1, typename Derived2>
ykuroda 0:13a5d365ba16 571 inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
ykuroda 0:13a5d365ba16 572 {
ykuroda 0:13a5d365ba16 573 using std::max;
ykuroda 0:13a5d365ba16 574 using std::sqrt;
ykuroda 0:13a5d365ba16 575 Vector3 v0 = a.normalized();
ykuroda 0:13a5d365ba16 576 Vector3 v1 = b.normalized();
ykuroda 0:13a5d365ba16 577 Scalar c = v1.dot(v0);
ykuroda 0:13a5d365ba16 578
ykuroda 0:13a5d365ba16 579 // if dot == -1, vectors are nearly opposites
ykuroda 0:13a5d365ba16 580 // => accurately compute the rotation axis by computing the
ykuroda 0:13a5d365ba16 581 // intersection of the two planes. This is done by solving:
ykuroda 0:13a5d365ba16 582 // x^T v0 = 0
ykuroda 0:13a5d365ba16 583 // x^T v1 = 0
ykuroda 0:13a5d365ba16 584 // under the constraint:
ykuroda 0:13a5d365ba16 585 // ||x|| = 1
ykuroda 0:13a5d365ba16 586 // which yields a singular value problem
ykuroda 0:13a5d365ba16 587 if (c < Scalar(-1)+NumTraits<Scalar>::dummy_precision())
ykuroda 0:13a5d365ba16 588 {
ykuroda 0:13a5d365ba16 589 c = (max)(c,Scalar(-1));
ykuroda 0:13a5d365ba16 590 Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose();
ykuroda 0:13a5d365ba16 591 JacobiSVD<Matrix<Scalar,2,3> > svd(m, ComputeFullV);
ykuroda 0:13a5d365ba16 592 Vector3 axis = svd.matrixV().col(2);
ykuroda 0:13a5d365ba16 593
ykuroda 0:13a5d365ba16 594 Scalar w2 = (Scalar(1)+c)*Scalar(0.5);
ykuroda 0:13a5d365ba16 595 this->w() = sqrt(w2);
ykuroda 0:13a5d365ba16 596 this->vec() = axis * sqrt(Scalar(1) - w2);
ykuroda 0:13a5d365ba16 597 return derived();
ykuroda 0:13a5d365ba16 598 }
ykuroda 0:13a5d365ba16 599 Vector3 axis = v0.cross(v1);
ykuroda 0:13a5d365ba16 600 Scalar s = sqrt((Scalar(1)+c)*Scalar(2));
ykuroda 0:13a5d365ba16 601 Scalar invs = Scalar(1)/s;
ykuroda 0:13a5d365ba16 602 this->vec() = axis * invs;
ykuroda 0:13a5d365ba16 603 this->w() = s * Scalar(0.5);
ykuroda 0:13a5d365ba16 604
ykuroda 0:13a5d365ba16 605 return derived();
ykuroda 0:13a5d365ba16 606 }
ykuroda 0:13a5d365ba16 607
ykuroda 0:13a5d365ba16 608
ykuroda 0:13a5d365ba16 609 /** Returns a quaternion representing a rotation between
ykuroda 0:13a5d365ba16 610 * the two arbitrary vectors \a a and \a b. In other words, the built
ykuroda 0:13a5d365ba16 611 * rotation represent a rotation sending the line of direction \a a
ykuroda 0:13a5d365ba16 612 * to the line of direction \a b, both lines passing through the origin.
ykuroda 0:13a5d365ba16 613 *
ykuroda 0:13a5d365ba16 614 * \returns resulting quaternion
ykuroda 0:13a5d365ba16 615 *
ykuroda 0:13a5d365ba16 616 * Note that the two input vectors do \b not have to be normalized, and
ykuroda 0:13a5d365ba16 617 * do not need to have the same norm.
ykuroda 0:13a5d365ba16 618 */
ykuroda 0:13a5d365ba16 619 template<typename Scalar, int Options>
ykuroda 0:13a5d365ba16 620 template<typename Derived1, typename Derived2>
ykuroda 0:13a5d365ba16 621 Quaternion<Scalar,Options> Quaternion<Scalar,Options>::FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
ykuroda 0:13a5d365ba16 622 {
ykuroda 0:13a5d365ba16 623 Quaternion quat;
ykuroda 0:13a5d365ba16 624 quat.setFromTwoVectors(a, b);
ykuroda 0:13a5d365ba16 625 return quat;
ykuroda 0:13a5d365ba16 626 }
ykuroda 0:13a5d365ba16 627
ykuroda 0:13a5d365ba16 628
ykuroda 0:13a5d365ba16 629 /** \returns the multiplicative inverse of \c *this
ykuroda 0:13a5d365ba16 630 * Note that in most cases, i.e., if you simply want the opposite rotation,
ykuroda 0:13a5d365ba16 631 * and/or the quaternion is normalized, then it is enough to use the conjugate.
ykuroda 0:13a5d365ba16 632 *
ykuroda 0:13a5d365ba16 633 * \sa QuaternionBase::conjugate()
ykuroda 0:13a5d365ba16 634 */
ykuroda 0:13a5d365ba16 635 template <class Derived>
ykuroda 0:13a5d365ba16 636 inline Quaternion<typename internal::traits<Derived>::Scalar> QuaternionBase<Derived>::inverse() const
ykuroda 0:13a5d365ba16 637 {
ykuroda 0:13a5d365ba16 638 // FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite() ??
ykuroda 0:13a5d365ba16 639 Scalar n2 = this->squaredNorm();
ykuroda 0:13a5d365ba16 640 if (n2 > Scalar(0))
ykuroda 0:13a5d365ba16 641 return Quaternion<Scalar>(conjugate().coeffs() / n2);
ykuroda 0:13a5d365ba16 642 else
ykuroda 0:13a5d365ba16 643 {
ykuroda 0:13a5d365ba16 644 // return an invalid result to flag the error
ykuroda 0:13a5d365ba16 645 return Quaternion<Scalar>(Coefficients::Zero());
ykuroda 0:13a5d365ba16 646 }
ykuroda 0:13a5d365ba16 647 }
ykuroda 0:13a5d365ba16 648
ykuroda 0:13a5d365ba16 649 /** \returns the conjugate of the \c *this which is equal to the multiplicative inverse
ykuroda 0:13a5d365ba16 650 * if the quaternion is normalized.
ykuroda 0:13a5d365ba16 651 * The conjugate of a quaternion represents the opposite rotation.
ykuroda 0:13a5d365ba16 652 *
ykuroda 0:13a5d365ba16 653 * \sa Quaternion2::inverse()
ykuroda 0:13a5d365ba16 654 */
ykuroda 0:13a5d365ba16 655 template <class Derived>
ykuroda 0:13a5d365ba16 656 inline Quaternion<typename internal::traits<Derived>::Scalar>
ykuroda 0:13a5d365ba16 657 QuaternionBase<Derived>::conjugate() const
ykuroda 0:13a5d365ba16 658 {
ykuroda 0:13a5d365ba16 659 return Quaternion<Scalar>(this->w(),-this->x(),-this->y(),-this->z());
ykuroda 0:13a5d365ba16 660 }
ykuroda 0:13a5d365ba16 661
ykuroda 0:13a5d365ba16 662 /** \returns the angle (in radian) between two rotations
ykuroda 0:13a5d365ba16 663 * \sa dot()
ykuroda 0:13a5d365ba16 664 */
ykuroda 0:13a5d365ba16 665 template <class Derived>
ykuroda 0:13a5d365ba16 666 template <class OtherDerived>
ykuroda 0:13a5d365ba16 667 inline typename internal::traits<Derived>::Scalar
ykuroda 0:13a5d365ba16 668 QuaternionBase<Derived>::angularDistance(const QuaternionBase<OtherDerived>& other) const
ykuroda 0:13a5d365ba16 669 {
ykuroda 0:13a5d365ba16 670 using std::atan2;
ykuroda 0:13a5d365ba16 671 using std::abs;
ykuroda 0:13a5d365ba16 672 Quaternion<Scalar> d = (*this) * other.conjugate();
ykuroda 0:13a5d365ba16 673 return Scalar(2) * atan2( d.vec().norm(), abs(d.w()) );
ykuroda 0:13a5d365ba16 674 }
ykuroda 0:13a5d365ba16 675
ykuroda 0:13a5d365ba16 676
ykuroda 0:13a5d365ba16 677
ykuroda 0:13a5d365ba16 678 /** \returns the spherical linear interpolation between the two quaternions
ykuroda 0:13a5d365ba16 679 * \c *this and \a other at the parameter \a t in [0;1].
ykuroda 0:13a5d365ba16 680 *
ykuroda 0:13a5d365ba16 681 * This represents an interpolation for a constant motion between \c *this and \a other,
ykuroda 0:13a5d365ba16 682 * see also http://en.wikipedia.org/wiki/Slerp.
ykuroda 0:13a5d365ba16 683 */
ykuroda 0:13a5d365ba16 684 template <class Derived>
ykuroda 0:13a5d365ba16 685 template <class OtherDerived>
ykuroda 0:13a5d365ba16 686 Quaternion<typename internal::traits<Derived>::Scalar>
ykuroda 0:13a5d365ba16 687 QuaternionBase<Derived>::slerp(const Scalar& t, const QuaternionBase<OtherDerived>& other) const
ykuroda 0:13a5d365ba16 688 {
ykuroda 0:13a5d365ba16 689 using std::acos;
ykuroda 0:13a5d365ba16 690 using std::sin;
ykuroda 0:13a5d365ba16 691 using std::abs;
ykuroda 0:13a5d365ba16 692 static const Scalar one = Scalar(1) - NumTraits<Scalar>::epsilon();
ykuroda 0:13a5d365ba16 693 Scalar d = this->dot(other);
ykuroda 0:13a5d365ba16 694 Scalar absD = abs(d);
ykuroda 0:13a5d365ba16 695
ykuroda 0:13a5d365ba16 696 Scalar scale0;
ykuroda 0:13a5d365ba16 697 Scalar scale1;
ykuroda 0:13a5d365ba16 698
ykuroda 0:13a5d365ba16 699 if(absD>=one)
ykuroda 0:13a5d365ba16 700 {
ykuroda 0:13a5d365ba16 701 scale0 = Scalar(1) - t;
ykuroda 0:13a5d365ba16 702 scale1 = t;
ykuroda 0:13a5d365ba16 703 }
ykuroda 0:13a5d365ba16 704 else
ykuroda 0:13a5d365ba16 705 {
ykuroda 0:13a5d365ba16 706 // theta is the angle between the 2 quaternions
ykuroda 0:13a5d365ba16 707 Scalar theta = acos(absD);
ykuroda 0:13a5d365ba16 708 Scalar sinTheta = sin(theta);
ykuroda 0:13a5d365ba16 709
ykuroda 0:13a5d365ba16 710 scale0 = sin( ( Scalar(1) - t ) * theta) / sinTheta;
ykuroda 0:13a5d365ba16 711 scale1 = sin( ( t * theta) ) / sinTheta;
ykuroda 0:13a5d365ba16 712 }
ykuroda 0:13a5d365ba16 713 if(d<Scalar(0)) scale1 = -scale1;
ykuroda 0:13a5d365ba16 714
ykuroda 0:13a5d365ba16 715 return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs());
ykuroda 0:13a5d365ba16 716 }
ykuroda 0:13a5d365ba16 717
ykuroda 0:13a5d365ba16 718 namespace internal {
ykuroda 0:13a5d365ba16 719
ykuroda 0:13a5d365ba16 720 // set from a rotation matrix
ykuroda 0:13a5d365ba16 721 template<typename Other>
ykuroda 0:13a5d365ba16 722 struct quaternionbase_assign_impl<Other,3,3>
ykuroda 0:13a5d365ba16 723 {
ykuroda 0:13a5d365ba16 724 typedef typename Other::Scalar Scalar;
ykuroda 0:13a5d365ba16 725 typedef DenseIndex Index;
ykuroda 0:13a5d365ba16 726 template<class Derived> static inline void run(QuaternionBase<Derived>& q, const Other& mat)
ykuroda 0:13a5d365ba16 727 {
ykuroda 0:13a5d365ba16 728 using std::sqrt;
ykuroda 0:13a5d365ba16 729 // This algorithm comes from "Quaternion Calculus and Fast Animation",
ykuroda 0:13a5d365ba16 730 // Ken Shoemake, 1987 SIGGRAPH course notes
ykuroda 0:13a5d365ba16 731 Scalar t = mat.trace();
ykuroda 0:13a5d365ba16 732 if (t > Scalar(0))
ykuroda 0:13a5d365ba16 733 {
ykuroda 0:13a5d365ba16 734 t = sqrt(t + Scalar(1.0));
ykuroda 0:13a5d365ba16 735 q.w() = Scalar(0.5)*t;
ykuroda 0:13a5d365ba16 736 t = Scalar(0.5)/t;
ykuroda 0:13a5d365ba16 737 q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
ykuroda 0:13a5d365ba16 738 q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
ykuroda 0:13a5d365ba16 739 q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
ykuroda 0:13a5d365ba16 740 }
ykuroda 0:13a5d365ba16 741 else
ykuroda 0:13a5d365ba16 742 {
ykuroda 0:13a5d365ba16 743 DenseIndex i = 0;
ykuroda 0:13a5d365ba16 744 if (mat.coeff(1,1) > mat.coeff(0,0))
ykuroda 0:13a5d365ba16 745 i = 1;
ykuroda 0:13a5d365ba16 746 if (mat.coeff(2,2) > mat.coeff(i,i))
ykuroda 0:13a5d365ba16 747 i = 2;
ykuroda 0:13a5d365ba16 748 DenseIndex j = (i+1)%3;
ykuroda 0:13a5d365ba16 749 DenseIndex k = (j+1)%3;
ykuroda 0:13a5d365ba16 750
ykuroda 0:13a5d365ba16 751 t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
ykuroda 0:13a5d365ba16 752 q.coeffs().coeffRef(i) = Scalar(0.5) * t;
ykuroda 0:13a5d365ba16 753 t = Scalar(0.5)/t;
ykuroda 0:13a5d365ba16 754 q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
ykuroda 0:13a5d365ba16 755 q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
ykuroda 0:13a5d365ba16 756 q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
ykuroda 0:13a5d365ba16 757 }
ykuroda 0:13a5d365ba16 758 }
ykuroda 0:13a5d365ba16 759 };
ykuroda 0:13a5d365ba16 760
ykuroda 0:13a5d365ba16 761 // set from a vector of coefficients assumed to be a quaternion
ykuroda 0:13a5d365ba16 762 template<typename Other>
ykuroda 0:13a5d365ba16 763 struct quaternionbase_assign_impl<Other,4,1>
ykuroda 0:13a5d365ba16 764 {
ykuroda 0:13a5d365ba16 765 typedef typename Other::Scalar Scalar;
ykuroda 0:13a5d365ba16 766 template<class Derived> static inline void run(QuaternionBase<Derived>& q, const Other& vec)
ykuroda 0:13a5d365ba16 767 {
ykuroda 0:13a5d365ba16 768 q.coeffs() = vec;
ykuroda 0:13a5d365ba16 769 }
ykuroda 0:13a5d365ba16 770 };
ykuroda 0:13a5d365ba16 771
ykuroda 0:13a5d365ba16 772 } // end namespace internal
ykuroda 0:13a5d365ba16 773
ykuroda 0:13a5d365ba16 774 } // end namespace Eigen
ykuroda 0:13a5d365ba16 775
ykuroda 0:13a5d365ba16 776 #endif // EIGEN_QUATERNION_H