quaternion.cpp
Go to the documentation of this file.
00001 /****************************************************************************
00002 
00003  Copyright (C) 2002-2013 Gilles Debunne. All rights reserved.
00004 
00005  This file is part of the QGLViewer library version 2.4.0.
00006 
00007  http://www.libqglviewer.com - contact@libqglviewer.com
00008 
00009  This file may be used under the terms of the GNU General Public License 
00010  versions 2.0 or 3.0 as published by the Free Software Foundation and
00011  appearing in the LICENSE file included in the packaging of this file.
00012  In addition, as a special exception, Gilles Debunne gives you certain 
00013  additional rights, described in the file GPL_EXCEPTION in this package.
00014 
00015  libQGLViewer uses dual licensing. Commercial/proprietary software must
00016  purchase a libQGLViewer Commercial License.
00017 
00018  This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
00019  WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
00020 
00021 *****************************************************************************/
00022 
00023 #include "domUtils.h"
00024 #include "quaternion.h"
00025 #include <stdlib.h> // RAND_MAX
00026 
00027 // All the methods are declared inline in Quaternion.h
00028 using namespace qglviewer;
00029 using namespace std;
00030 
00035 Quaternion::Quaternion(const Vec& from, const Vec& to)
00036 {
00037   const double epsilon = 1E-10f;
00038 
00039   const double fromSqNorm = from.squaredNorm();
00040   const double toSqNorm   = to.squaredNorm();
00041   // Identity Quaternion when one vector is null
00042   if ((fromSqNorm < epsilon) || (toSqNorm < epsilon))
00043     {
00044       q[0]=q[1]=q[2]=0.0;
00045       q[3]=1.0;
00046     }
00047   else
00048     {
00049       Vec axis = cross(from, to);
00050       const double axisSqNorm = axis.squaredNorm();
00051 
00052       // Aligned vectors, pick any axis, not aligned with from or to
00053       if (axisSqNorm < epsilon)
00054         axis = from.orthogonalVec();
00055 
00056       double angle = asin(sqrt(axisSqNorm / (fromSqNorm * toSqNorm)));
00057 
00058       if (from*to < 0.0)
00059         angle = M_PI-angle;
00060 
00061       setAxisAngle(axis, angle);
00062     }
00063 }
00064 
00068 Vec Quaternion::inverseRotate(const Vec& v) const
00069 {
00070   return inverse().rotate(v);
00071 }
00072 
00076 Vec Quaternion::rotate(const Vec& v) const
00077 {
00078   const double q00 = 2.0l * q[0] * q[0];
00079   const double q11 = 2.0l * q[1] * q[1];
00080   const double q22 = 2.0l * q[2] * q[2];
00081 
00082   const double q01 = 2.0l * q[0] * q[1];
00083   const double q02 = 2.0l * q[0] * q[2];
00084   const double q03 = 2.0l * q[0] * q[3];
00085 
00086   const double q12 = 2.0l * q[1] * q[2];
00087   const double q13 = 2.0l * q[1] * q[3];
00088 
00089   const double q23 = 2.0l * q[2] * q[3];
00090 
00091   return Vec((1.0 - q11 - q22)*v[0] + (      q01 - q23)*v[1] + (      q02 + q13)*v[2],
00092              (      q01 + q23)*v[0] + (1.0 - q22 - q00)*v[1] + (      q12 - q03)*v[2],
00093              (      q02 - q13)*v[0] + (      q12 + q03)*v[1] + (1.0 - q11 - q00)*v[2] );
00094 }
00095 
00104 void Quaternion::setFromRotationMatrix(const double m[3][3])
00105 {
00106   // Compute one plus the trace of the matrix
00107   const double onePlusTrace = 1.0 + m[0][0] + m[1][1] + m[2][2];
00108 
00109   if (onePlusTrace > 1E-5)
00110     {
00111       // Direct computation
00112       const double s = sqrt(onePlusTrace) * 2.0;
00113       q[0] = (m[2][1] - m[1][2]) / s;
00114       q[1] = (m[0][2] - m[2][0]) / s;
00115       q[2] = (m[1][0] - m[0][1]) / s;
00116       q[3] = 0.25 * s;
00117     }
00118   else
00119     {
00120       // Computation depends on major diagonal term
00121       if ((m[0][0] > m[1][1])&(m[0][0] > m[2][2]))
00122         { 
00123           const double s = sqrt(1.0 + m[0][0] - m[1][1] - m[2][2]) * 2.0; 
00124           q[0] = 0.25 * s;
00125           q[1] = (m[0][1] + m[1][0]) / s; 
00126           q[2] = (m[0][2] + m[2][0]) / s; 
00127           q[3] = (m[1][2] - m[2][1]) / s;
00128         }
00129       else
00130         if (m[1][1] > m[2][2])
00131           { 
00132             const double s = sqrt(1.0 + m[1][1] - m[0][0] - m[2][2]) * 2.0; 
00133             q[0] = (m[0][1] + m[1][0]) / s; 
00134             q[1] = 0.25 * s;
00135             q[2] = (m[1][2] + m[2][1]) / s; 
00136             q[3] = (m[0][2] - m[2][0]) / s;
00137           }
00138         else
00139           { 
00140             const double s = sqrt(1.0 + m[2][2] - m[0][0] - m[1][1]) * 2.0; 
00141             q[0] = (m[0][2] + m[2][0]) / s; 
00142             q[1] = (m[1][2] + m[2][1]) / s; 
00143             q[2] = 0.25 * s;
00144             q[3] = (m[0][1] - m[1][0]) / s;
00145           }
00146     }
00147   normalize();
00148 }
00149 
00150 #ifndef DOXYGEN
00151 void Quaternion::setFromRotationMatrix(const float m[3][3])
00152 {
00153   qWarning("setFromRotationMatrix now waits for a double[3][3] parameter");
00154 
00155   double mat[3][3];
00156   for (int i=0; i<3; ++i)
00157     for (int j=0; j<3; ++j)
00158       mat[i][j] = double(m[i][j]);
00159 
00160   setFromRotationMatrix(mat);
00161 }
00162 
00163 void Quaternion::setFromRotatedBase(const Vec& X, const Vec& Y, const Vec& Z)
00164 {
00165   qWarning("setFromRotatedBase is deprecated, use setFromRotatedBasis instead");
00166   setFromRotatedBasis(X,Y,Z);
00167 }
00168 #endif
00169 
00182 void Quaternion::setFromRotatedBasis(const Vec& X, const Vec& Y, const Vec& Z)
00183 {
00184   double m[3][3];
00185   double normX = X.norm();
00186   double normY = Y.norm();
00187   double normZ = Z.norm();
00188 
00189   for (int i=0; i<3; ++i)
00190     {
00191       m[i][0] = X[i] / normX;
00192       m[i][1] = Y[i] / normY;
00193       m[i][2] = Z[i] / normZ;
00194     }
00195   
00196   setFromRotationMatrix(m);
00197 }
00198 
00201 void Quaternion::getAxisAngle(Vec& axis, float& angle) const
00202 {
00203   angle = 2.0*acos(q[3]);
00204   axis = Vec(q[0], q[1], q[2]);
00205   const double sinus = axis.norm();
00206   if (sinus > 1E-8)
00207     axis /= sinus;
00208 
00209   if (angle > M_PI)
00210     {
00211       angle = 2.0*M_PI - angle;
00212       axis = -axis;
00213     }
00214 }
00215 
00219 Vec Quaternion::axis() const
00220 {
00221   Vec res = Vec(q[0], q[1], q[2]);
00222   const double sinus = res.norm();
00223   if (sinus > 1E-8)
00224     res /= sinus;
00225   return (acos(q[3]) <= M_PI/2.0) ? res : -res;
00226 }
00227 
00234 double Quaternion::angle() const
00235 {
00236   const double angle = 2.0 * acos(q[3]);
00237   return (angle <= M_PI) ? angle : 2.0*M_PI - angle;
00238 }
00239 
00256 QDomElement Quaternion::domElement(const QString& name, QDomDocument& document) const
00257 {
00258   QDomElement de = document.createElement(name);
00259   de.setAttribute("q0", QString::number(q[0]));
00260   de.setAttribute("q1", QString::number(q[1]));
00261   de.setAttribute("q2", QString::number(q[2]));
00262   de.setAttribute("q3", QString::number(q[3]));
00263   return de;
00264 }
00265 
00273 void Quaternion::initFromDOMElement(const QDomElement& element)
00274 {
00275   Quaternion q(element);
00276   *this = q;
00277 }
00278 
00286 Quaternion::Quaternion(const QDomElement& element)
00287 {
00288   QStringList attribute;
00289   attribute << "q0" << "q1" << "q2" << "q3";
00290 #if QT_VERSION >= 0x040000
00291   for (int i=0; i<attribute.size(); ++i)
00292 #else
00293   for (unsigned int i=0; i<attribute.count(); ++i)
00294 #endif
00295     q[i] = DomUtils::doubleFromDom(element, attribute[i], ((i<3)?0.0f:1.0f));
00296 }
00297 
00310 const GLdouble* Quaternion::matrix() const
00311 {
00312   static GLdouble m[4][4];
00313   getMatrix(m);
00314   return (const GLdouble*)(m);
00315 }
00316 
00321 void Quaternion::getMatrix(GLdouble m[4][4]) const
00322 {
00323   const double q00 = 2.0l * q[0] * q[0];
00324   const double q11 = 2.0l * q[1] * q[1];
00325   const double q22 = 2.0l * q[2] * q[2];
00326 
00327   const double q01 = 2.0l * q[0] * q[1];
00328   const double q02 = 2.0l * q[0] * q[2];
00329   const double q03 = 2.0l * q[0] * q[3];
00330 
00331   const double q12 = 2.0l * q[1] * q[2];
00332   const double q13 = 2.0l * q[1] * q[3];
00333 
00334   const double q23 = 2.0l * q[2] * q[3];
00335 
00336   m[0][0] = 1.0l - q11 - q22;
00337   m[1][0] =        q01 - q23;
00338   m[2][0] =        q02 + q13;
00339 
00340   m[0][1] =        q01 + q23;
00341   m[1][1] = 1.0l - q22 - q00;
00342   m[2][1] =        q12 - q03;
00343 
00344   m[0][2] =        q02 - q13;
00345   m[1][2] =        q12 + q03;
00346   m[2][2] = 1.0l - q11 - q00;
00347 
00348   m[0][3] = 0.0l;
00349   m[1][3] = 0.0l;
00350   m[2][3] = 0.0l;
00351 
00352   m[3][0] = 0.0l;
00353   m[3][1] = 0.0l;
00354   m[3][2] = 0.0l;
00355   m[3][3] = 1.0l;
00356 }
00357 
00359 void Quaternion::getMatrix(GLdouble m[16]) const
00360 {
00361   static GLdouble mat[4][4];
00362   getMatrix(mat);
00363   int count = 0;
00364   for (int i=0; i<4; ++i)
00365     for (int j=0; j<4; ++j)
00366       m[count++] = mat[i][j];
00367 }
00368 
00375 void Quaternion::getRotationMatrix(float m[3][3]) const
00376 {
00377   static GLdouble mat[4][4];
00378   getMatrix(mat);
00379   for (int i=0; i<3; ++i)
00380     for (int j=0; j<3; ++j)
00381       // Beware of transposition
00382       m[i][j] = mat[j][i];
00383 }
00384 
00393 const GLdouble* Quaternion::inverseMatrix() const
00394 {
00395   static GLdouble m[4][4];
00396   getInverseMatrix(m);
00397   return (const GLdouble*)(m);
00398 }
00399 
00404 void Quaternion::getInverseMatrix(GLdouble m[4][4]) const
00405 {
00406   inverse().getMatrix(m);
00407 }
00408 
00410 void Quaternion::getInverseMatrix(GLdouble m[16]) const
00411 {
00412   inverse().getMatrix(m);
00413 }
00414 
00419 void Quaternion::getInverseRotationMatrix(float m[3][3]) const
00420 {
00421   static GLdouble mat[4][4];
00422   getInverseMatrix(mat);
00423   for (int i=0; i<3; ++i)
00424     for (int j=0; j<3; ++j)
00425       // Beware of transposition
00426       m[i][j] = mat[j][i];
00427 }
00428 
00429 
00437 Quaternion Quaternion::slerp(const Quaternion& a, const Quaternion& b, float t, bool allowFlip)
00438 {
00439   double cosAngle = Quaternion::dot(a, b);
00440 
00441   double c1, c2;
00442   // Linear interpolation for close orientations
00443   if ((1.0 - fabs(cosAngle)) < 0.01)
00444     {
00445       c1 = 1.0 - t;
00446       c2 = t;
00447     }
00448   else
00449     {
00450       // Spherical interpolation
00451       double angle    = acos(fabs(cosAngle));
00452       double sinAngle = sin(angle);
00453       c1 = sin(angle * (1.0 - t)) / sinAngle;
00454       c2 = sin(angle * t) / sinAngle;
00455     }
00456 
00457   // Use the shortest path
00458   if (allowFlip && (cosAngle < 0.0))
00459     c1 = -c1;
00460 
00461   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]);
00462 }
00463 
00471 Quaternion Quaternion::squad(const Quaternion& a, const Quaternion& tgA, const Quaternion& tgB, const Quaternion& b, float t)
00472 {
00473   Quaternion ab = Quaternion::slerp(a, b, t);
00474   Quaternion tg = Quaternion::slerp(tgA, tgB, t, false);
00475   return Quaternion::slerp(ab, tg, 2.0*t*(1.0-t), false);
00476 }
00477 
00479 Quaternion Quaternion::log()
00480 {
00481   double len = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2]);
00482 
00483   if (len < 1E-6)
00484     return Quaternion(q[0], q[1], q[2], 0.0);
00485   else
00486     {
00487       double coef = acos(q[3]) / len;
00488       return Quaternion(q[0]*coef, q[1]*coef, q[2]*coef, 0.0);
00489     }
00490 }
00491 
00493 Quaternion Quaternion::exp()
00494 {
00495   double theta = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2]);
00496 
00497   if (theta < 1E-6)
00498     return Quaternion(q[0], q[1], q[2], cos(theta));
00499   else
00500     {
00501       double coef = sin(theta) / theta;
00502       return Quaternion(q[0]*coef, q[1]*coef, q[2]*coef, cos(theta));
00503     }
00504 }
00505 
00507 Quaternion Quaternion::lnDif(const Quaternion& a, const Quaternion& b)
00508 {
00509   Quaternion dif = a.inverse()*b;
00510   dif.normalize();
00511   return dif.log();
00512 }
00513 
00517 Quaternion Quaternion::squadTangent(const Quaternion& before, const Quaternion& center, const Quaternion& after)
00518 {
00519   Quaternion l1 = Quaternion::lnDif(center,before);
00520   Quaternion l2 = Quaternion::lnDif(center,after);
00521   Quaternion e;
00522   for (int i=0; i<4; ++i)
00523     e.q[i] = -0.25 * (l1.q[i] + l2.q[i]);
00524   e = center*(e.exp());
00525 
00526   // if (Quaternion::dot(e,b) < 0.0)
00527     // e.negate();
00528 
00529   return e;
00530 }
00531 
00532 ostream& operator<<(ostream& o, const Quaternion& Q)
00533 {
00534   return o << Q[0] << '\t' << Q[1] << '\t' << Q[2] << '\t' << Q[3];
00535 }
00536 
00546 Quaternion Quaternion::randomQuaternion()
00547 {
00548   // The rand() function is not very portable and may not be available on your system.
00549   // Add the appropriate include or replace by an other random function in case of problem.
00550   double seed = rand()/(double)RAND_MAX;
00551   double r1 = sqrt(1.0 - seed);
00552   double r2 = sqrt(seed);
00553   double t1 = 2.0 * M_PI * (rand()/(double)RAND_MAX);
00554   double t2 = 2.0 * M_PI * (rand()/(double)RAND_MAX);
00555   return Quaternion(sin(t1)*r1, cos(t1)*r1, sin(t2)*r2, cos(t2)*r2);
00556 }


octovis
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Aug 27 2015 14:13:26