quaternion.cpp
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 #include "domUtils.h"
24 #include "quaternion.h"
25 #include <stdlib.h> // RAND_MAX
26 
27 // All the methods are declared inline in Quaternion.h
28 using namespace qglviewer;
29 using namespace std;
30 
35 Quaternion::Quaternion(const Vec& from, const Vec& to)
36 {
37  const double epsilon = 1E-10f;
38 
39  const double fromSqNorm = from.squaredNorm();
40  const double toSqNorm = to.squaredNorm();
41  // Identity Quaternion when one vector is null
42  if ((fromSqNorm < epsilon) || (toSqNorm < epsilon))
43  {
44  q[0]=q[1]=q[2]=0.0;
45  q[3]=1.0;
46  }
47  else
48  {
49  Vec axis = cross(from, to);
50  const double axisSqNorm = axis.squaredNorm();
51 
52  // Aligned vectors, pick any axis, not aligned with from or to
53  if (axisSqNorm < epsilon)
54  axis = from.orthogonalVec();
55 
56  double angle = asin(sqrt(axisSqNorm / (fromSqNorm * toSqNorm)));
57 
58  if (from*to < 0.0)
59  angle = M_PI-angle;
60 
61  setAxisAngle(axis, angle);
62  }
63 }
64 
69 {
70  return inverse().rotate(v);
71 }
72 
76 Vec Quaternion::rotate(const Vec& v) const
77 {
78  const double q00 = 2.0l * q[0] * q[0];
79  const double q11 = 2.0l * q[1] * q[1];
80  const double q22 = 2.0l * q[2] * q[2];
81 
82  const double q01 = 2.0l * q[0] * q[1];
83  const double q02 = 2.0l * q[0] * q[2];
84  const double q03 = 2.0l * q[0] * q[3];
85 
86  const double q12 = 2.0l * q[1] * q[2];
87  const double q13 = 2.0l * q[1] * q[3];
88 
89  const double q23 = 2.0l * q[2] * q[3];
90 
91  return Vec((1.0 - q11 - q22)*v[0] + ( q01 - q23)*v[1] + ( q02 + q13)*v[2],
92  ( q01 + q23)*v[0] + (1.0 - q22 - q00)*v[1] + ( q12 - q03)*v[2],
93  ( q02 - q13)*v[0] + ( q12 + q03)*v[1] + (1.0 - q11 - q00)*v[2] );
94 }
95 
104 void Quaternion::setFromRotationMatrix(const double m[3][3])
105 {
106  // Compute one plus the trace of the matrix
107  const double onePlusTrace = 1.0 + m[0][0] + m[1][1] + m[2][2];
108 
109  if (onePlusTrace > 1E-5)
110  {
111  // Direct computation
112  const double s = sqrt(onePlusTrace) * 2.0;
113  q[0] = (m[2][1] - m[1][2]) / s;
114  q[1] = (m[0][2] - m[2][0]) / s;
115  q[2] = (m[1][0] - m[0][1]) / s;
116  q[3] = 0.25 * s;
117  }
118  else
119  {
120  // Computation depends on major diagonal term
121  if ((m[0][0] > m[1][1])&(m[0][0] > m[2][2]))
122  {
123  const double s = sqrt(1.0 + m[0][0] - m[1][1] - m[2][2]) * 2.0;
124  q[0] = 0.25 * s;
125  q[1] = (m[0][1] + m[1][0]) / s;
126  q[2] = (m[0][2] + m[2][0]) / s;
127  q[3] = (m[1][2] - m[2][1]) / s;
128  }
129  else
130  if (m[1][1] > m[2][2])
131  {
132  const double s = sqrt(1.0 + m[1][1] - m[0][0] - m[2][2]) * 2.0;
133  q[0] = (m[0][1] + m[1][0]) / s;
134  q[1] = 0.25 * s;
135  q[2] = (m[1][2] + m[2][1]) / s;
136  q[3] = (m[0][2] - m[2][0]) / s;
137  }
138  else
139  {
140  const double s = sqrt(1.0 + m[2][2] - m[0][0] - m[1][1]) * 2.0;
141  q[0] = (m[0][2] + m[2][0]) / s;
142  q[1] = (m[1][2] + m[2][1]) / s;
143  q[2] = 0.25 * s;
144  q[3] = (m[0][1] - m[1][0]) / s;
145  }
146  }
147  normalize();
148 }
149 
150 #ifndef DOXYGEN
151 void Quaternion::setFromRotationMatrix(const float m[3][3])
152 {
153  qWarning("setFromRotationMatrix now waits for a double[3][3] parameter");
154 
155  double mat[3][3];
156  for (int i=0; i<3; ++i)
157  for (int j=0; j<3; ++j)
158  mat[i][j] = double(m[i][j]);
159 
160  setFromRotationMatrix(mat);
161 }
162 
163 void Quaternion::setFromRotatedBase(const Vec& X, const Vec& Y, const Vec& Z)
164 {
165  qWarning("setFromRotatedBase is deprecated, use setFromRotatedBasis instead");
166  setFromRotatedBasis(X,Y,Z);
167 }
168 #endif
169 
182 void Quaternion::setFromRotatedBasis(const Vec& X, const Vec& Y, const Vec& Z)
183 {
184  double m[3][3];
185  double normX = X.norm();
186  double normY = Y.norm();
187  double normZ = Z.norm();
188 
189  for (int i=0; i<3; ++i)
190  {
191  m[i][0] = X[i] / normX;
192  m[i][1] = Y[i] / normY;
193  m[i][2] = Z[i] / normZ;
194  }
195 
196  setFromRotationMatrix(m);
197 }
198 
201 void Quaternion::getAxisAngle(Vec& axis, float& angle) const
202 {
203  angle = 2.0*acos(q[3]);
204  axis = Vec(q[0], q[1], q[2]);
205  const double sinus = axis.norm();
206  if (sinus > 1E-8)
207  axis /= sinus;
208 
209  if (angle > M_PI)
210  {
211  angle = 2.0*M_PI - angle;
212  axis = -axis;
213  }
214 }
215 
220 {
221  Vec res = Vec(q[0], q[1], q[2]);
222  const double sinus = res.norm();
223  if (sinus > 1E-8)
224  res /= sinus;
225  return (acos(q[3]) <= M_PI/2.0) ? res : -res;
226 }
227 
234 double Quaternion::angle() const
235 {
236  const double angle = 2.0 * acos(q[3]);
237  return (angle <= M_PI) ? angle : 2.0*M_PI - angle;
238 }
239 
256 QDomElement Quaternion::domElement(const QString& name, QDomDocument& document) const
257 {
258  QDomElement de = document.createElement(name);
259  de.setAttribute("q0", QString::number(q[0]));
260  de.setAttribute("q1", QString::number(q[1]));
261  de.setAttribute("q2", QString::number(q[2]));
262  de.setAttribute("q3", QString::number(q[3]));
263  return de;
264 }
265 
273 void Quaternion::initFromDOMElement(const QDomElement& element)
274 {
275  Quaternion q(element);
276  *this = q;
277 }
278 
286 Quaternion::Quaternion(const QDomElement& element)
287 {
288  QStringList attribute;
289  attribute << "q0" << "q1" << "q2" << "q3";
290 #if QT_VERSION >= 0x040000
291  for (int i=0; i<attribute.size(); ++i)
292 #else
293  for (unsigned int i=0; i<attribute.count(); ++i)
294 #endif
295  q[i] = DomUtils::doubleFromDom(element, attribute[i], ((i<3)?0.0f:1.0f));
296 }
297 
310 const GLdouble* Quaternion::matrix() const
311 {
312  static GLdouble m[4][4];
313  getMatrix(m);
314  return (const GLdouble*)(m);
315 }
316 
321 void Quaternion::getMatrix(GLdouble m[4][4]) const
322 {
323  const double q00 = 2.0l * q[0] * q[0];
324  const double q11 = 2.0l * q[1] * q[1];
325  const double q22 = 2.0l * q[2] * q[2];
326 
327  const double q01 = 2.0l * q[0] * q[1];
328  const double q02 = 2.0l * q[0] * q[2];
329  const double q03 = 2.0l * q[0] * q[3];
330 
331  const double q12 = 2.0l * q[1] * q[2];
332  const double q13 = 2.0l * q[1] * q[3];
333 
334  const double q23 = 2.0l * q[2] * q[3];
335 
336  m[0][0] = 1.0l - q11 - q22;
337  m[1][0] = q01 - q23;
338  m[2][0] = q02 + q13;
339 
340  m[0][1] = q01 + q23;
341  m[1][1] = 1.0l - q22 - q00;
342  m[2][1] = q12 - q03;
343 
344  m[0][2] = q02 - q13;
345  m[1][2] = q12 + q03;
346  m[2][2] = 1.0l - q11 - q00;
347 
348  m[0][3] = 0.0l;
349  m[1][3] = 0.0l;
350  m[2][3] = 0.0l;
351 
352  m[3][0] = 0.0l;
353  m[3][1] = 0.0l;
354  m[3][2] = 0.0l;
355  m[3][3] = 1.0l;
356 }
357 
359 void Quaternion::getMatrix(GLdouble m[16]) const
360 {
361  static GLdouble mat[4][4];
362  getMatrix(mat);
363  int count = 0;
364  for (int i=0; i<4; ++i)
365  for (int j=0; j<4; ++j)
366  m[count++] = mat[i][j];
367 }
368 
375 void Quaternion::getRotationMatrix(float m[3][3]) const
376 {
377  static GLdouble mat[4][4];
378  getMatrix(mat);
379  for (int i=0; i<3; ++i)
380  for (int j=0; j<3; ++j)
381  // Beware of transposition
382  m[i][j] = mat[j][i];
383 }
384 
393 const GLdouble* Quaternion::inverseMatrix() const
394 {
395  static GLdouble m[4][4];
396  getInverseMatrix(m);
397  return (const GLdouble*)(m);
398 }
399 
404 void Quaternion::getInverseMatrix(GLdouble m[4][4]) const
405 {
406  inverse().getMatrix(m);
407 }
408 
410 void Quaternion::getInverseMatrix(GLdouble m[16]) const
411 {
412  inverse().getMatrix(m);
413 }
414 
419 void Quaternion::getInverseRotationMatrix(float m[3][3]) const
420 {
421  static GLdouble mat[4][4];
422  getInverseMatrix(mat);
423  for (int i=0; i<3; ++i)
424  for (int j=0; j<3; ++j)
425  // Beware of transposition
426  m[i][j] = mat[j][i];
427 }
428 
429 
437 Quaternion Quaternion::slerp(const Quaternion& a, const Quaternion& b, float t, bool allowFlip)
438 {
439  double cosAngle = Quaternion::dot(a, b);
440 
441  double c1, c2;
442  // Linear interpolation for close orientations
443  if ((1.0 - fabs(cosAngle)) < 0.01)
444  {
445  c1 = 1.0 - t;
446  c2 = t;
447  }
448  else
449  {
450  // Spherical interpolation
451  double angle = acos(fabs(cosAngle));
452  double sinAngle = sin(angle);
453  c1 = sin(angle * (1.0 - t)) / sinAngle;
454  c2 = sin(angle * t) / sinAngle;
455  }
456 
457  // Use the shortest path
458  if (allowFlip && (cosAngle < 0.0))
459  c1 = -c1;
460 
461  return Quaternion(c1*a[0] + c2*b[0], c1*a[1] + c2*b[1], c1*a[2] + c2*b[2], c1*a[3] + c2*b[3]);
462 }
463 
471 Quaternion Quaternion::squad(const Quaternion& a, const Quaternion& tgA, const Quaternion& tgB, const Quaternion& b, float t)
472 {
473  Quaternion ab = Quaternion::slerp(a, b, t);
474  Quaternion tg = Quaternion::slerp(tgA, tgB, t, false);
475  return Quaternion::slerp(ab, tg, 2.0*t*(1.0-t), false);
476 }
477 
480 {
481  double len = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2]);
482 
483  if (len < 1E-6)
484  return Quaternion(q[0], q[1], q[2], 0.0);
485  else
486  {
487  double coef = acos(q[3]) / len;
488  return Quaternion(q[0]*coef, q[1]*coef, q[2]*coef, 0.0);
489  }
490 }
491 
494 {
495  double theta = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2]);
496 
497  if (theta < 1E-6)
498  return Quaternion(q[0], q[1], q[2], cos(theta));
499  else
500  {
501  double coef = sin(theta) / theta;
502  return Quaternion(q[0]*coef, q[1]*coef, q[2]*coef, cos(theta));
503  }
504 }
505 
508 {
509  Quaternion dif = a.inverse()*b;
510  dif.normalize();
511  return dif.log();
512 }
513 
517 Quaternion Quaternion::squadTangent(const Quaternion& before, const Quaternion& center, const Quaternion& after)
518 {
519  Quaternion l1 = Quaternion::lnDif(center,before);
520  Quaternion l2 = Quaternion::lnDif(center,after);
521  Quaternion e;
522  for (int i=0; i<4; ++i)
523  e.q[i] = -0.25 * (l1.q[i] + l2.q[i]);
524  e = center*(e.exp());
525 
526  // if (Quaternion::dot(e,b) < 0.0)
527  // e.negate();
528 
529  return e;
530 }
531 
532 ostream& operator<<(ostream& o, const Quaternion& Q)
533 {
534  return o << Q[0] << '\t' << Q[1] << '\t' << Q[2] << '\t' << Q[3];
535 }
536 
547 {
548  // The rand() function is not very portable and may not be available on your system.
549  // Add the appropriate include or replace by an other random function in case of problem.
550  double seed = rand()/(double)RAND_MAX;
551  double r1 = sqrt(1.0 - seed);
552  double r2 = sqrt(seed);
553  double t1 = 2.0 * M_PI * (rand()/(double)RAND_MAX);
554  double t2 = 2.0 * M_PI * (rand()/(double)RAND_MAX);
555  return Quaternion(sin(t1)*r1, cos(t1)*r1, sin(t2)*r2, cos(t2)*r2);
556 }