quaternion.h
1 /****************************************************************************
2 
3  Copyright (C) 2002-2008 Gilles Debunne. All rights reserved.
4 
5  This file is part of the QGLViewer library version 2.3.10.
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 <math.h>
28 #include <iostream>
29 
30 namespace qglviewer {
66  class QGLVIEWER_EXPORT Quaternion
67  {
68  public:
73  { q[0]=q[1]=q[2]=0.0; q[3]=1.0; }
74 
76  Quaternion(const Vec& axis, double angle)
77  {
78  setAxisAngle(axis, angle);
79  }
80 
81  Quaternion(const Vec& from, const Vec& to);
82 
88  Quaternion(double q0, double q1, double q2, double q3)
89  { q[0]=q0; q[1]=q1; q[2]=q2; q[3]=q3; }
90 
93  { for (int i=0; i<4; ++i) q[i] = Q.q[i]; }
94 
96  Quaternion& operator=(const Quaternion& Q)
97  {
98  for (int i=0; i<4; ++i)
99  q[i] = Q.q[i];
100  return (*this);
101  }
102 
106  void setAxisAngle(const Vec& axis, double angle)
107  {
108  const double norm = axis.norm();
109  if (norm < 1E-8)
110  {
111  // Null rotation
112  q[0] = 0.0; q[1] = 0.0; q[2] = 0.0; q[3] = 1.0;
113  }
114  else
115  {
116  const double sin_half_angle = sin(angle / 2.0);
117  q[0] = sin_half_angle*axis[0]/norm;
118  q[1] = sin_half_angle*axis[1]/norm;
119  q[2] = sin_half_angle*axis[2]/norm;
120  q[3] = cos(angle / 2.0);
121  }
122  }
123 
125  void setValue(double q0, double q1, double q2, double q3)
126  { q[0]=q0; q[1]=q1; q[2]=q2; q[3]=q3; }
127 
128 #ifndef DOXYGEN
129  void setFromRotationMatrix(const float m[3][3]);
130  void setFromRotatedBase(const Vec& X, const Vec& Y, const Vec& Z);
131 #endif
132  void setFromRotationMatrix(const double m[3][3]);
133  void setFromRotatedBasis(const Vec& X, const Vec& Y, const Vec& Z);
135 
136 
139  Vec axis() const;
140  double angle() const;
141  void getAxisAngle(Vec& axis, float& angle) const;
142 
144  double operator[](int i) const { return q[i]; }
145 
147  double& operator[](int i) { return q[i]; }
149 
150 
164  friend Quaternion operator*(const Quaternion& a, const Quaternion& b)
165  {
166  return Quaternion(a.q[3]*b.q[0] + b.q[3]*a.q[0] + a.q[1]*b.q[2] - a.q[2]*b.q[1],
167  a.q[3]*b.q[1] + b.q[3]*a.q[1] + a.q[2]*b.q[0] - a.q[0]*b.q[2],
168  a.q[3]*b.q[2] + b.q[3]*a.q[2] + a.q[0]*b.q[1] - a.q[1]*b.q[0],
169  a.q[3]*b.q[3] - b.q[0]*a.q[0] - a.q[1]*b.q[1] - a.q[2]*b.q[2]);
170  }
171 
178  Quaternion& operator*=(const Quaternion &q)
179  {
180  *this = (*this)*q;
181  return *this;
182  }
183 
187  friend Vec operator*(const Quaternion& q, const Vec& v)
188  {
189  return q.rotate(v);
190  }
191 
192  Vec rotate(const Vec& v) const;
193  Vec inverseRotate(const Vec& v) const;
195 
196 
205  Quaternion inverse() const { return Quaternion(-q[0], -q[1], -q[2], q[3]); }
206 
210  void invert() { q[0] = -q[0]; q[1] = -q[1]; q[2] = -q[2]; }
211 
220  void negate() { invert(); q[3] = -q[3]; }
221 
227  double normalize()
228  {
229  const double norm = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
230  for (int i=0; i<4; ++i)
231  q[i] /= norm;
232  return norm;
233  }
234 
238  Quaternion normalized() const
239  {
240  double Q[4];
241  const double norm = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
242  for (int i=0; i<4; ++i)
243  Q[i] = q[i] / norm;
244  return Quaternion(Q[0], Q[1], Q[2], Q[3]);
245  }
247 
248 
251  const GLdouble* matrix() const;
252  void getMatrix(GLdouble m[4][4]) const;
253  void getMatrix(GLdouble m[16]) const;
254 
255  void getRotationMatrix(float m[3][3]) const;
256 
257  const GLdouble* inverseMatrix() const;
258  void getInverseMatrix(GLdouble m[4][4]) const;
259  void getInverseMatrix(GLdouble m[16]) const;
260 
261  void getInverseRotationMatrix(float m[3][3]) const;
263 
264 
267  static Quaternion slerp(const Quaternion& a, const Quaternion& b, float t, bool allowFlip=true);
268  static Quaternion squad(const Quaternion& a, const Quaternion& tgA, const Quaternion& tgB, const Quaternion& b, float t);
270  static double dot(const Quaternion& a, const Quaternion& b) { return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3]; }
271 
272  Quaternion log();
273  Quaternion exp();
274  static Quaternion lnDif(const Quaternion& a, const Quaternion& b);
275  static Quaternion squadTangent(const Quaternion& before, const Quaternion& center, const Quaternion& after);
277 
280  static Quaternion randomQuaternion();
282 
285  explicit Quaternion(const QDomElement& element);
286  QDomElement domElement(const QString& name, QDomDocument& document) const;
287  void initFromDOMElement(const QDomElement& element);
289 
290 #ifdef DOXYGEN
291 
298  std::ostream& operator<<(std::ostream& o, const qglviewer::Vec&);
300 #endif
301 
302  private:
304  double q[4];
305  };
306 
307 } // namespace
308 
309 std::ostream& operator<<(std::ostream& o, const qglviewer::Quaternion&);
310 
311 #endif // QGLVIEWER_QUATERNION_H