1 /**************************************************************************** 2 3 Copyright (C) 2002-2014 Gilles Debunne. All rights reserved. 4 5 This file is part of the QGLViewer library version 2.7.2. 6 7 http://www.libqglviewer.com - contact@libqglviewer.com 8 9 This file may be used under the terms of the GNU General Public License 10 versions 2.0 or 3.0 as published by the Free Software Foundation and 11 appearing in the LICENSE file included in the packaging of this file. 12 In addition, as a special exception, Gilles Debunne gives you certain 13 additional rights, described in the file GPL_EXCEPTION in this package. 14 15 libQGLViewer uses dual licensing. Commercial/proprietary software must 16 purchase a libQGLViewer Commercial License. 17 18 This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE 19 WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. 20 21 *****************************************************************************/ 22 23 #ifndef QGLVIEWER_QUATERNION_H 24 #define QGLVIEWER_QUATERNION_H 25 26 #include "vec.h" 27 #include <iostream> 28 #include <math.h> 29 30 namespace qglviewer { 31 /*! \brief The Quaternion class represents 3D rotations and orientations. 32 \class Quaternion quaternion.h QGLViewer/quaternion.h 33 34 The Quaternion is an appropriate (although not very intuitive) 35 representation for 3D rotations and orientations. Many tools are provided to 36 ease the definition of a Quaternion: see constructors, setAxisAngle(), 37 setFromRotationMatrix(), setFromRotatedBasis(). 38 39 You can apply the rotation represented by the Quaternion to 3D points 40 using rotate() and inverseRotate(). See also the Frame class that represents 41 a coordinate system and provides other conversion functions like 42 Frame::coordinatesOf() and Frame::transformOf(). 43 44 You can apply the Quaternion \c q rotation to the OpenGL matrices using: 45 \code glMultMatrixd(q.matrix()); 46 // equivalent to glRotate(q.angle()*180.0/M_PI, q.axis().x, q.axis().y, q.axis().z); \endcode 47 48 Quaternion is part of the \c qglviewer namespace, specify \c 49 qglviewer::Quaternion or use the qglviewer namespace: 50 \code using namespace qglviewer; \endcode 51 52 <h3>Internal representation</h3> 53 54 The internal representation of a Quaternion corresponding to a rotation 55 around axis \c axis, with an angle \c alpha is made of four qreals (i.e. 56 doubles) q[i]: \code {q[0],q[1],q[2]} = sin(alpha/2) * 57 {axis[0],axis[1],axis[2]} q[3] = cos(alpha/2) \endcode 58 59 Note that certain implementations place the cosine term in first 60 position (instead of last here). 61 62 The Quaternion is always normalized, so that its inverse() is actually 63 its conjugate. 64 65 See also the Vec and Frame classes' documentations. 66 \nosubgrouping */ 67 class QGLVIEWER_EXPORT Quaternion { 68 public: 69 /*! @name Defining a Quaternion */ 70 //@{ 71 /*! Default constructor, builds an identity rotation. */ Quaternion()72 Quaternion() { 73 q[0] = q[1] = q[2] = 0.0; 74 q[3] = 1.0; 75 } 76 77 /*! Constructor from rotation axis (non null) and angle (in radians). See also 78 * setAxisAngle(). */ Quaternion(const Vec & axis,qreal angle)79 Quaternion(const Vec &axis, qreal angle) { setAxisAngle(axis, angle); } 80 81 Quaternion(const Vec &from, const Vec &to); 82 83 /*! Constructor from the four values of a Quaternion. First three values are 84 axis*sin(angle/2) and last one is cos(angle/2). 85 86 \attention The identity Quaternion is Quaternion(0,0,0,1) and \e not 87 Quaternion(0,0,0,0) (which is not unitary). The default Quaternion() 88 creates such identity Quaternion. */ Quaternion(qreal q0,qreal q1,qreal q2,qreal q3)89 Quaternion(qreal q0, qreal q1, qreal q2, qreal q3) { 90 q[0] = q0; 91 q[1] = q1; 92 q[2] = q2; 93 q[3] = q3; 94 } 95 96 /*! Copy constructor. */ Quaternion(const Quaternion & Q)97 Quaternion(const Quaternion &Q) { 98 for (int i = 0; i < 4; ++i) 99 q[i] = Q.q[i]; 100 } 101 102 /*! Equal operator. */ 103 Quaternion &operator=(const Quaternion &Q) { 104 for (int i = 0; i < 4; ++i) 105 q[i] = Q.q[i]; 106 return (*this); 107 } 108 109 /*! Sets the Quaternion as a rotation of axis \p axis and angle \p angle (in 110 radians). 111 112 \p axis does not need to be normalized. A null \p axis will result in 113 an identity Quaternion. */ setAxisAngle(const Vec & axis,qreal angle)114 void setAxisAngle(const Vec &axis, qreal angle) { 115 const qreal norm = axis.norm(); 116 if (norm < 1E-8) { 117 // Null rotation 118 q[0] = 0.0; 119 q[1] = 0.0; 120 q[2] = 0.0; 121 q[3] = 1.0; 122 } else { 123 const qreal sin_half_angle = sin(angle / 2.0); 124 q[0] = sin_half_angle * axis[0] / norm; 125 q[1] = sin_half_angle * axis[1] / norm; 126 q[2] = sin_half_angle * axis[2] / norm; 127 q[3] = cos(angle / 2.0); 128 } 129 } 130 131 /*! Sets the Quaternion value. See the Quaternion(qreal, qreal, qreal, qreal) 132 * constructor documentation. */ setValue(qreal q0,qreal q1,qreal q2,qreal q3)133 void setValue(qreal q0, qreal q1, qreal q2, qreal q3) { 134 q[0] = q0; 135 q[1] = q1; 136 q[2] = q2; 137 q[3] = q3; 138 } 139 140 #ifndef DOXYGEN 141 void setFromRotatedBase(const Vec &X, const Vec &Y, const Vec &Z); 142 #endif 143 void setFromRotationMatrix(const qreal m[3][3]); 144 void setFromRotatedBasis(const Vec &X, const Vec &Y, const Vec &Z); 145 //@} 146 147 /*! @name Accessing values */ 148 //@{ 149 Vec axis() const; 150 qreal angle() const; 151 void getAxisAngle(Vec &axis, qreal &angle) const; 152 153 /*! Bracket operator, with a constant return value. \p i must range in [0..3]. 154 * See the Quaternion(qreal, qreal, qreal, qreal) documentation. */ 155 qreal operator[](int i) const { return q[i]; } 156 157 /*! Bracket operator returning an l-value. \p i must range in [0..3]. See the 158 * Quaternion(qreal, qreal, qreal, qreal) documentation. */ 159 qreal &operator[](int i) { return q[i]; } 160 //@} 161 162 /*! @name Rotation computations */ 163 //@{ 164 /*! Returns the composition of the \p a and \p b rotations. 165 166 The order is important. When applied to a Vec \c v (see 167 operator*(const Quaternion&, const Vec&) and rotate()) the resulting 168 Quaternion acts as if \p b was applied first and then \p a was applied. 169 This is obvious since the image \c v' of \p v by the composited rotation 170 satisfies: \code v'= (a*b) * v = a * (b*v) \endcode 171 172 Note that a*b usually differs from b*a. 173 174 \attention For efficiency reasons, the resulting Quaternion is not 175 normalized. Use normalize() in case of numerical drift with small rotation 176 composition. */ 177 friend Quaternion operator*(const Quaternion &a, const Quaternion &b) { 178 return Quaternion( 179 a.q[3] * b.q[0] + b.q[3] * a.q[0] + a.q[1] * b.q[2] - a.q[2] * b.q[1], 180 a.q[3] * b.q[1] + b.q[3] * a.q[1] + a.q[2] * b.q[0] - a.q[0] * b.q[2], 181 a.q[3] * b.q[2] + b.q[3] * a.q[2] + a.q[0] * b.q[1] - a.q[1] * b.q[0], 182 a.q[3] * b.q[3] - b.q[0] * a.q[0] - a.q[1] * b.q[1] - a.q[2] * b.q[2]); 183 } 184 185 /*! Quaternion rotation is composed with \p q. 186 187 See operator*(), since this is equivalent to \c this = \c this * \p q. 188 189 \note For efficiency reasons, the resulting Quaternion is not 190 normalized. You may normalize() it after each application in case of 191 numerical drift. */ 192 Quaternion &operator*=(const Quaternion &q) { 193 *this = (*this) * q; 194 return *this; 195 } 196 197 /*! Returns the image of \p v by the rotation \p q. 198 199 Same as q.rotate(v). See rotate() and inverseRotate(). */ 200 friend Vec operator*(const Quaternion &q, const Vec &v) { 201 return q.rotate(v); 202 } 203 204 Vec rotate(const Vec &v) const; 205 Vec inverseRotate(const Vec &v) const; 206 //@} 207 208 /*! @name Inversion */ 209 //@{ 210 /*! Returns the inverse Quaternion (inverse rotation). 211 212 Result has a negated axis() direction and the same angle(). A 213 composition (see operator*()) of a Quaternion and its inverse() results in 214 an identity function. 215 216 Use invert() to actually modify the Quaternion. */ inverse()217 Quaternion inverse() const { return Quaternion(-q[0], -q[1], -q[2], q[3]); } 218 219 /*! Inverses the Quaternion (same rotation angle(), but negated axis()). 220 221 See also inverse(). */ invert()222 void invert() { 223 q[0] = -q[0]; 224 q[1] = -q[1]; 225 q[2] = -q[2]; 226 } 227 228 /*! Negates all the coefficients of the Quaternion. 229 230 This results in an other representation of the \e same rotation 231 (opposite rotation angle, but with a negated axis direction: the two cancel 232 out). However, note that the results of axis() and angle() are unchanged 233 after a call to this method since angle() always returns a value in [0,pi]. 234 235 This method is mainly useful for Quaternion interpolation, so that the 236 spherical interpolation takes the shortest path on the unit sphere. See 237 slerp() for details. */ negate()238 void negate() { 239 invert(); 240 q[3] = -q[3]; 241 } 242 243 /*! Normalizes the Quaternion coefficients. 244 245 This method should not need to be called since we only deal with unit 246 Quaternions. This is however useful to prevent numerical drifts, especially 247 with small rotational increments. See also normalized(). */ normalize()248 qreal normalize() { 249 const qreal norm = 250 sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); 251 for (int i = 0; i < 4; ++i) 252 q[i] /= norm; 253 return norm; 254 } 255 256 /*! Returns a normalized version of the Quaternion. 257 258 See also normalize(). */ normalized()259 Quaternion normalized() const { 260 qreal Q[4]; 261 const qreal norm = 262 sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); 263 for (int i = 0; i < 4; ++i) 264 Q[i] = q[i] / norm; 265 return Quaternion(Q[0], Q[1], Q[2], Q[3]); 266 } 267 //@} 268 269 /*! @name Associated matrix */ 270 //@{ 271 const GLdouble *matrix() const; 272 void getMatrix(GLdouble m[4][4]) const; 273 void getMatrix(GLdouble m[16]) const; 274 275 void getRotationMatrix(qreal m[3][3]) const; 276 277 const GLdouble *inverseMatrix() const; 278 void getInverseMatrix(GLdouble m[4][4]) const; 279 void getInverseMatrix(GLdouble m[16]) const; 280 281 void getInverseRotationMatrix(qreal m[3][3]) const; 282 //@} 283 284 /*! @name Slerp interpolation */ 285 //@{ 286 static Quaternion slerp(const Quaternion &a, const Quaternion &b, qreal t, 287 bool allowFlip = true); 288 static Quaternion squad(const Quaternion &a, const Quaternion &tgA, 289 const Quaternion &tgB, const Quaternion &b, qreal t); 290 /*! Returns the "dot" product of \p a and \p b: a[0]*b[0] + a[1]*b[1] + 291 * a[2]*b[2] + a[3]*b[3]. */ dot(const Quaternion & a,const Quaternion & b)292 static qreal dot(const Quaternion &a, const Quaternion &b) { 293 return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3]; 294 } 295 296 Quaternion log(); 297 Quaternion exp(); 298 static Quaternion lnDif(const Quaternion &a, const Quaternion &b); 299 static Quaternion squadTangent(const Quaternion &before, 300 const Quaternion ¢er, 301 const Quaternion &after); 302 //@} 303 304 /*! @name Random Quaternion */ 305 //@{ 306 static Quaternion randomQuaternion(); 307 //@} 308 309 /*! @name XML representation */ 310 //@{ 311 explicit Quaternion(const QDomElement &element); 312 QDomElement domElement(const QString &name, QDomDocument &document) const; 313 void initFromDOMElement(const QDomElement &element); 314 //@} 315 316 #ifdef DOXYGEN 317 /*! @name Output stream */ 318 //@{ 319 /*! Output stream operator. Enables debugging code like: 320 \code 321 Quaternion rot(...); 322 cout << "Rotation=" << rot << endl; 323 \endcode */ 324 std::ostream &operator<<(std::ostream &o, const qglviewer::Vec &); 325 //@} 326 #endif 327 328 private: 329 /*! The internal data representation is private, use operator[] to access 330 * values. */ 331 qreal q[4]; 332 }; 333 334 } // namespace qglviewer 335 336 std::ostream &operator<<(std::ostream &o, const qglviewer::Quaternion &); 337 338 #endif // QGLVIEWER_QUATERNION_H 339