quaternion.h

Go to the documentation of this file.

/****************************************************************************

 This file is part of the QGLViewer library
 Copyright (C) 2002-2004  Gilles Debunne (Gilles.Debunne@imag.fr)
 Version 1.3.5 Release 8. Packaged on Monday December 22, 2003.

 http://www-imagis.imag.fr/Membres/Gilles.Debunne/CODE/QGLViewer

 libQGLViewer is free software; you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation; either version 2 of the License, or
 (at your option) any later version.

 libQGLViewer is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with libQGLViewer; if not, write to the Free Software
 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

*****************************************************************************/

#ifndef QGLVIEWER_QUATERNION_H
#define QGLVIEWER_QUATERNION_H

#include "vec.h"
#include <math.h>
#include <iostream>

namespace qglviewer {
class QGLVIEWER_EXPORT Quaternion
{
public:

  explicit Quaternion()
  { q[0]=q[1]=q[2]=0.0;  q[3]=1.0; }

  explicit Quaternion(const Vec& axis, const double angle)
  {
    setAxisAngle(axis, angle);
  }

  explicit Quaternion(const Vec& v, const double sin_ha, const double cos_ha)
  {
    q[0] = v[0]*sin_ha ;
    q[1] = v[1]*sin_ha ;
    q[2] = v[2]*sin_ha ;
    q[3] = cos_ha ;
  }
  
  explicit Quaternion(const double q0, const double q1, const double q2, const double q3)
  { q[0]=q0;    q[1]=q1;    q[2]=q2;    q[3]=q3; }

  explicit Quaternion(const Vec& from, const Vec& to);

  Quaternion& operator=(const Quaternion& Q)
  {
    for (int i=0; i<4; ++i)
      q[i] = Q.q[i];
    return (*this);
  }

  Quaternion(const Quaternion& Q)
  { for (int i=0; i<4; ++i) q[i] = Q.q[i]; }

  void setAxisAngle(const Vec& v, const double angle)
  {
    const double norm = v.norm();
    if (norm < 1E-9)
      {
    // Null rotation
    q[0] = 0.0;      q[1] = 0.0;      q[2] = 0.0;      q[3] = 1.0;
      }
    else
      {
    const double sin_half_angle = sin(angle / 2.0);
    q[0] = sin_half_angle*v[0]/norm;
    q[1] = sin_half_angle*v[1]/norm;
    q[2] = sin_half_angle*v[2]/norm;
    q[3] = cos(angle / 2.0);
      }
  }

  void setValue(const double q0, const double q1, const double q2, const double q3)
  { q[0]=q0;    q[1]=q1;    q[2]=q2;    q[3]=q3; }

  void setFromRotationMatrix(const float m[3][3]);
  void setFromRotatedBase(const Vec& X,
              const Vec& Y,
              const Vec& Z);

  
  Vec axis() const;  
  float angle() const;
  void getAxisAngle(Vec& axis, float& angle) const;

  double operator[](int i) const { return q[i]; }

  double& operator[](int i) { return q[i]; }





  friend const Quaternion operator*(const Quaternion& a, const Quaternion& b)
  {
    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],
               a.q[3]*b.q[1] + b.q[3]*a.q[1] + a.q[2]*b.q[0] - a.q[0]*b.q[2],
               a.q[3]*b.q[2] + b.q[3]*a.q[2] + a.q[0]*b.q[1] - a.q[1]*b.q[0],
               a.q[3]*b.q[3] - b.q[0]*a.q[0] - a.q[1]*b.q[1] - a.q[2]*b.q[2]);
  }

  Quaternion& operator*=(const Quaternion &q)
  {
    *this = (*this)*q;
    return *this;
  }

  friend const Vec operator*(const Quaternion& a, const Vec& b)
  {
    return a.rotate(b);
  }
  
  Vec inverseRotate(const Vec& v) const;
  Vec rotate(const Vec& v) const;



  Quaternion inverse() const { return Quaternion(-q[0], -q[1], -q[2], q[3]); }

  void invert() { q[0] = -q[0]; q[1] = -q[1]; q[2] = -q[2]; }

  void negate() { invert(); q[3] = -q[3]; }

  double normalize()
  {
    const double norm = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
    for (int i=0; i<4; ++i)
      q[i] /= norm;
    return norm;
  }

  
  const GLdouble* matrix() const;
  void getMatrix(GLdouble m[4][4]) const;
  void getMatrix(GLdouble m[16]) const;

  void getRotationMatrix(float m[3][3]) const;

  
  static Quaternion slerp(const Quaternion& a, const Quaternion& b, float t, bool allowFlip=true);
  static Quaternion squad(const Quaternion& a, const Quaternion& tgA, const Quaternion& tgB, const Quaternion& b, float t);
  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]; }

  Quaternion log();
  Quaternion exp();
  static Quaternion lnDif(const Quaternion& a, const Quaternion& b);
  static Quaternion squadTangent(const Quaternion& a, const Quaternion& b, const Quaternion& c);

  static Quaternion randomOrientation();
  
  QDomElement domElement(const QString& name, QDomDocument& doc) const;  
  void initFromDOMElement(const QDomElement& de);

private: 
  double q[4];
};

} // namespace

std::ostream& operator<<(std::ostream& o, const qglviewer::Quaternion&);

#endif // QGLVIEWER_QUATERNION_H

---