AnimaL

Tutorial

Documentation

---

Documentation

• Overview
• Modules
• Namespace List
• Class Hierarchy
• Alphabetical List
• Compound List
• File List
• Namespace Members
• Compound Members
• File Members
• Examples

---

quaternion.h

Go to the documentation of this file.

#ifndef _ANIMAL_GEOMETRY_QUATERNION_H_
#define _ANIMAL_GEOMETRY_QUATERNION_H_

#include <animal/vec3.h>
//#include <animal/numerics.h>
#include <cmath>
#include <limits.h>



namespace animal
{

  template <class RealT> class Quaternion_Traits;

  // -----------------------------------------------------------
  //
  //  Quaternion class.
  //
  // -----------------------------------------------------------

  template <
  class RealT = double,
  class TraitsT = Quaternion_Traits<RealT>
  >

  class Quaternion
  {

  public:


    typedef TraitsT Traits;

    typedef RealT Real;

    //typedef typename Traits::Vec3 Vec3;

    //typedef Numerics<Real> NumTraits;



    Quaternion();

    Quaternion(const Real w, const Real x, const Real y, const Real z);

    Quaternion(const Vec3& axis, const Real angle);

    Quaternion(const Real angle, const Vec3& axis);

    Quaternion(const Quaternion& );

    ~Quaternion();

    template< class Vect >
    static Quaternion eulerIncrement ( const Vect&, const Real dt );

    static Quaternion eulerIncrement( const Real, const Real, const Real, const Real );

    template< class Vect >
    static Quaternion  elementaryRotation( const Vect& );

    static Quaternion elementaryRotation( const Real, const Real, const Real );




    void getAxisAngle(Real& x, Real& y, Real& z, Real& angle) const;

    void getAxisAngle(Vec3& v, Real& a) const ;

    const Real& w() const;

    const Real& x() const;

    const Real& y() const;

    const Real& z() const;


    void setAxisAngle(const Real x, const Real y, const Real z, const Real angle);

    void setAxisAngle(const Vec3& axis, const Real angle);

    Quaternion& setwxyz(const Real w, const Real x, const Real y, const Real z);



    Quaternion& setEulerXYZ(const Real x, const Real y, const Real z);

    Quaternion& setEulerXYX(const Real x, const Real y, const Real xx);

    Quaternion& setEulerXZY(const Real x, const Real z, const Real y);

    Quaternion& setEulerXZX(const Real x, const Real z, const Real xx);

    Quaternion& setEulerYZX(const Real y, const Real z, const Real x);

    Quaternion& setEulerYZY(const Real y, const Real z, const Real yy);

    Quaternion& setEulerYXY(const Real y, const Real x, const Real yy);

    Quaternion& setEulerYXZ(const Real y, const Real x, const Real z);

    Quaternion& setEulerZXY(const Real z, const Real x, const Real y);

    Quaternion& setEulerZXZ(const Real z, const Real x, const Real zz);

    Quaternion& setEulerZYX(const Real z, const Real y, const Real x);

    Quaternion& setEulerZYZ(const Real z, const Real y, const Real zz);



    Quaternion& operator = (const Quaternion& );

    Quaternion& operator *= (const Quaternion& );

    Quaternion& operator /= (const Quaternion& );

    Quaternion operator * (const Quaternion& ) const;

    Quaternion operator / (const Quaternion& ) const ;

    Quaternion inverse() const ;

    Quaternion operator * ( const Real u ) const;
    //<<<<<<< quaternion.h
    //  //@}


    //  /*! @name Operations on vectors */
    //  //@{
    //  //! Apply rotation to a vector
    //  Vec3 operator * (const Quaternion::Vec3& ) const;

    //  //! Apply inverse rotation to a vector
    //  Vec3 operator / (const Vec3& ) const;
    //  //@}


    //  /*! @name Input/Output */
    //  //@{
    //  //! Text output: axis angle
    //  template < class R, class T>
    //  friend std::ostream& operator << (std::ostream& , const Quaternion<R,T>& );
    //=======



    Vec3 operator * (const Vec3& ) const;

    Vec3 operator / (const Vec3& ) const;



    template < class R, class T>
    friend std::ostream& operator << (std::ostream& , const Quaternion<R,T>& );

    template < class R, class T>
    friend std::istream& operator >> (std::istream& , Quaternion<R,T>& );



    Real norm() const;

    Quaternion& normalize();



    static const Quaternion identity();
    //>>>>>>> 1.3



  private:

    Real v[4];   


  }
  ; // class Quaternion
  // -----------------------------------------------------------
  //
  //
  // -----------------------------------------------------------

  template <class Real, class Traits>
  inline std::ostream&
  operator << (std::ostream& stream, const Quaternion<Real,Traits>& q)
  {
    Real x, y, z, angle;
    q.getAxisAngle(x,y,z,angle);
    //   stream << "(" << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << ")";
    stream << x << " " << y << " " << z << " " << angle;

    return stream;
  }

  template <class Real, class Traits>
  inline std::istream&
  operator >> (std::istream& stream, Quaternion<Real,Traits>& q)
  {
    Real x, y, z, angle;
    stream >> x >> y >> z >> angle;
    q.setAxisAngle(x,y,z,angle);

    return stream;
  }

  template < class Real, class Traits>
  Vec3
  operator*
  (
    const typename Quaternion<Real,Traits>::Vec3&,
    const Quaternion<Real,Traits>&
  );

  template < class Real, class Traits>
  Vec3
  operator/
  (
    const typename Quaternion<Real,Traits>::Vec3&,
    const Quaternion<Real,Traits>&
  );


  template <class Real, class Traits, class Mat33>
  void
  writeRotMatrix
  (
    const Quaternion<Real,Traits>& q,
    Mat33& m
  );

  template <class Real, class Traits, class Mat44>
  void
  writeOpenGLRotMatrix
  (
    const Quaternion<Real,Traits>& q,
    Mat44& m
  );




  // -----------------------------------------------------------
  //
  //  Quaternion_Traits class.
  //
  // -----------------------------------------------------------

  template <class RealT> class Quaternion_Traits
  {

  public:


    typedef RealT Real;
    typedef animal::Vec3 Vec3;




    static const Real x(const Vec3& v) { return v[0]; }

    static const Real y(const Vec3& v) { return v[1]; }

    static const Real z(const Vec3& v) { return v[2]; }


    static Vec3& normalize(Vec3& v) { v_normalize(v); return v; }

    static Real nullAxis() { return 1.0e-5; }

  }
  ; // class Quaternion_Traits










  // -------------------------------------------------------------
  //
  //   D E F I N I T I O N   O F   I N L I N E D   M E T H O D S
  //
  // -------------------------------------------------------------





  template<class Real, class Traits>
  inline
  Quaternion<Real,Traits>::
  Quaternion()
  {}

  template<class Real, class Traits>
  inline
  Quaternion<Real,Traits>::
  Quaternion(const Real w, const Real x, const Real y, const Real z)
  {
    v[3]=w; v[0]=x; v[1]=y; v[2]=z;
  }

  template<class Real, class Traits>
  inline
  Quaternion<Real,Traits>::
  Quaternion(const Vec3& axis, const Real angle)
  {
    setAxisAngle(axis, angle);
  }

  template<class Real, class Traits>
  inline
  Quaternion<Real,Traits>::
  Quaternion(const Real angle, const Vec3& axis)
  {
    setAxisAngle(axis, angle);
  }

  template<class Real, class Traits>
  inline
  Quaternion<Real,Traits>::
  Quaternion(const Quaternion& q)
  {
    v[3]=q.v[3]; v[0]=q.v[0]; v[1]=q.v[1]; v[2]=q.v[2];
  }

  template<class Real, class Traits>
  inline
  Quaternion<Real,Traits>::
  ~Quaternion()
  {}





  template<class Real, class Traits>
  inline void
  Quaternion<Real,Traits>::
  getAxisAngle(Real& x, Real& y, Real& z, Real& an) const
  {
#ifdef DEBUG
    Real v_0 = ( v[3]>1.0 ? 1.0 : v[3]<-1.0 ? -1.0 : v[3] );

    an = 2.0*std::acos(v_0);
    //  std::cerr << "v_0 = " << v_0 <<std::endl;
    //  std::cerr << "angle = " << an <<std::endl;
    //  std::cerr << "threshold = " << NumTraits::numthreshold() <<std::endl;
    Real cos_an_2 = std::cos(an/2);
    Real epsilon =  std::numeric_limits<Real>::epsilon();
    if ( std::abs( v_0 - cos_an_2 ) > epsilon )
    {
      std::cerr << "Operation getAxisAngle(x, y, z, angle) beyond threshold!" << std::endl;
      assert(false);
    }
#else
    an = 2.0*acos( v[3]>1.0 ? 1.0 : v[3]<-1.0 ? -1.0 : v[3] );
#endif

    //  if ( an > NumTraits::numthreshold() )
    if ( an > Traits::nullAxis() )
    {
      Real tmp = 1.0/sin(an/2.0);
      //            std::cerr << " tmp = " << tmp <<std::endl;
      x=v[0]*tmp;
      y=v[1]*tmp;
      z=v[2]*tmp;
    }
    else
    {
      x = 1.0; // Arbitrary!
      y = 0.0;
      z = 0.0;
      an = 0;
    }
  }

  template<class Real, class Traits>
  inline void
  Quaternion<Real,Traits>::
  getAxisAngle(Vec3& ax, Real& an) const
  {
    // #if DEBUG
    //   Real v_0 = ( v[3]>1.0 ? 1.0 : v[3]<-1.0 ? -1.0 : v[3] );
    //
    //   an = 2.0*NumTraits::acosine(v_0);
    //   Real cos_an_2 = NumTraits::cosine(an/2);
    //
    //   if ( NumTraits::fpabs( v_0 - cos_an_2 ) > NumTraits::numthreshold() )
    //     {
    //       std::cerr << "Operation getAxisAngle(axe, angle) beyond threshold!" << std::endl;
    //       assert(false);
    //     }
    // #else
    //   an = 2.0*NumTraits::acosine( v[3]>1.0 ? 1.0 : v[3]<-1.0 ? -1.0 : v[3] );
    // #endif

    an = 2.0*acos( v[3]>1.0 ? 1.0 : v[3]<-1.0 ? -1.0 : v[3] );

    if ( an > Traits::nullAxis() )
      //   if ( an > NumTraits::numthreshold() )
    {
      Real tmp = 1.0/sin(an/2.0);
      ax = Vec3(v[0]*tmp, v[1]*tmp, v[2]*tmp);
    }
    else // Arbitrary!
    {
      ax = Vec3(1.0, 0.0, 0.0);
      an = 0;
    }
  }

  template<class Real, class Traits>
  inline const Real&
  Quaternion<Real,Traits>::w() const
  {
    return v[3];
  }

  template<class Real, class Traits>
  inline const Real&
  Quaternion<Real,Traits>::x() const
  {
    return v[0];
  }

  template<class Real, class Traits>
  inline const Real&
  Quaternion<Real,Traits>::y() const
  {
    return v[1];
  }

  template<class Real, class Traits>
  inline const Real&
  Quaternion<Real,Traits>::z() const
  {
    return v[2];
  }





  template<class Real, class Traits>
  inline void
  Quaternion<Real,Traits>::
  setAxisAngle(const Vec3& ax, const Real an)
  {
    v[3] = cos(an/2.0);

    Vec3 axn = ax;
    Traits::normalize(axn);
    Real tmp = sin(an/2.0);

    v[0] = Traits::x(axn)*tmp;
    v[1] = Traits::y(axn)*tmp;
    v[2] = Traits::z(axn)*tmp;
  }

  template<class Real, class Traits>
  inline void
  Quaternion<Real,Traits>::
  setAxisAngle(const Real x, const Real y, const Real z, const Real an)
  {
    v[3] = cos(an/2.0);

    Vec3 axn(x, y, z);
    Traits::normalize(axn);
    Real tmp = sin(an/2.0);

    v[0] = Traits::x(axn)*tmp;
    v[1] = Traits::y(axn)*tmp;
    v[2] = Traits::z(axn)*tmp;
  }

  template <class Real, class Traits>
  inline Quaternion<Real,Traits>&
  Quaternion<Real,Traits>::
  setwxyz(const Real w, const Real x, const Real y, const Real z)
  {
    v[3]=w; v[0]=x; v[1]=y; v[2]=z;
    return *this;
  }

  template <class Real, class Traits>
  inline Quaternion<Real,Traits>&
  Quaternion<Real,Traits>::
  setEulerXYZ(const Real x, const Real y, const Real z)
  {
    Quaternion qx; qx.setAxisAngle(1,0,0, x);
    Quaternion qy; qy.setAxisAngle(0,1,0, y);
    Quaternion qz; qz.setAxisAngle(0,0,1, z);

    return (*this = qx*qy*qz);
  }

  template <class Real, class Traits>
  inline Quaternion<Real,Traits>&
  Quaternion<Real,Traits>::
  setEulerXYX(const Real x, const Real y, const Real xx)
  {
    Quaternion qx;  qx.setAxisAngle (1,0,0, x );
    Quaternion qy;  qy.setAxisAngle (0,1,0, y );
    Quaternion qxx; qxx.setAxisAngle(1,0,0, xx);

    return (*this = qx*qy*qxx);
  }

  template <class Real, class Traits>
  inline Quaternion<Real,Traits>&
  Quaternion<Real,Traits>::
  setEulerXZY(const Real x, const Real z, const Real y)
  {
    Quaternion qx; qx.setAxisAngle(1,0,0, x);
    Quaternion qz; qz.setAxisAngle(0,0,1, z);
    Quaternion qy; qy.setAxisAngle(0,1,0, y);

    return (*this = qx*qz*qy);
  }

  template <class Real, class Traits>
  inline Quaternion<Real,Traits>&
  Quaternion<Real,Traits>::
  setEulerXZX(const Real x, const Real z, const Real xx)
  {
    Quaternion qx;  qx.setAxisAngle (1,0,0, x );
    Quaternion qz;  qz.setAxisAngle (0,0,1, z );
    Quaternion qxx; qxx.setAxisAngle(1,0,0, xx);

    return (*this = qx*qz*qxx);
  }

  template <class Real, class Traits>
  inline Quaternion<Real,Traits>&
  Quaternion<Real,Traits>::
  setEulerYZX(const Real y, const Real z, const Real x)
  {
    Quaternion qy; qy.setAxisAngle(0,1,0, y);
    Quaternion qz; qz.setAxisAngle(0,0,1, z);
    Quaternion qx; qx.setAxisAngle(1,0,0, x);

    return (*this = qy*qz*qx);
  }

  template <class Real, class Traits>
  inline Quaternion<Real,Traits>&
  Quaternion<Real,Traits>::
  setEulerYZY(const Real y, const Real z, const Real yy)
  {
    Quaternion qy;  qy.setAxisAngle (0,1,0, y );
    Quaternion qz;  qz.setAxisAngle (0,0,1, z );
    Quaternion qyy; qyy.setAxisAngle(0,1,0, yy);

    return (*this = qy*qz*qyy);
  }

  template <class Real, class Traits>
  inline Quaternion<Real,Traits>&
  Quaternion<Real,Traits>::
  setEulerYXY(const Real y, const Real x, const Real yy)
  {
    Quaternion qy;  qy.setAxisAngle (0,1,0, y );
    Quaternion qx;  qx.setAxisAngle (1,0,0, x );
    Quaternion qyy; qyy.setAxisAngle(0,1,0, yy);

    return (*this = qy*qx*qyy);
  }

  template <class Real, class Traits>
  inline Quaternion<Real,Traits>&
  Quaternion<Real,Traits>::
  setEulerYXZ(const Real y, const Real x, const Real z)
  {
    Quaternion qy;  qy.setAxisAngle(0,1,0, y);
    Quaternion qx;  qx.setAxisAngle(1,0,0, x);
    Quaternion qz;  qz.setAxisAngle(0,0,1, z);

    return (*this = qy*qx*qz);
  }

  template <class Real, class Traits>
  inline Quaternion<Real,Traits>&
  Quaternion<Real,Traits>::
  setEulerZXY(const Real z, const Real x, const Real y)
  {
    Quaternion qz; qz.setAxisAngle(0,0,1, z);
    Quaternion qx; qx.setAxisAngle(1,0,0, x);
    Quaternion qy; qy.setAxisAngle(0,1,0, y);

    return (*this = qz*qx*qy);
  }

  template <class Real, class Traits>
  inline Quaternion<Real,Traits>&
  Quaternion<Real,Traits>::
  setEulerZXZ(const Real z, const Real x, const Real zz)
  {
    Quaternion qz;  qz.setAxisAngle (0,0,1, z );
    Quaternion qx;  qx.setAxisAngle (1,0,0, x );
    Quaternion qzz; qzz.setAxisAngle(0,0,1, zz);

    return (*this = qz*qx*qzz);
  }

  template <class Real, class Traits>
  inline Quaternion<Real,Traits>&
  Quaternion<Real,Traits>::
  setEulerZYX(const Real z, const Real y, const Real x)
  {
    Quaternion qz; qz.setAxisAngle(0,0,1, z);
    Quaternion qy; qy.setAxisAngle(0,1,0, y);
    Quaternion qx; qx.setAxisAngle(1,0,0, x);

    return (*this = qz*qy*qx);
  }

  template <class Real, class Traits>
  inline Quaternion<Real,Traits>&
  Quaternion<Real,Traits>::
  setEulerZYZ(const Real z, const Real y, const Real zz)
  {
    Quaternion qz;  qz.setAxisAngle (0,0,1, z );
    Quaternion qy;  qy.setAxisAngle (0,1,0, y );
    Quaternion qzz; qzz.setAxisAngle(0,0,1, zz);

    return (*this = qz*qy*qzz);
  }





  template<class Real, class Traits>
  inline Quaternion<Real,Traits>&
  Quaternion<Real,Traits>::
  operator=(const Quaternion& q)
  {
    v[3] = q.v[3];
    v[0] = q.v[0];
    v[1] = q.v[1];
    v[2] = q.v[2];

    return *this;
  }

  template<class Real, class Traits>
  inline Quaternion<Real,Traits>&
  Quaternion<Real,Traits>::
  operator*=(const Quaternion& q)
  {
    Real w = q.v[3]*v[3] - q.v[0]*v[0] - q.v[1]*v[1] - q.v[2]*v[2];
    Real x = q.v[3]*v[0] + q.v[0]*v[3] + q.v[1]*v[2] - q.v[2]*v[1];
    Real y = q.v[3]*v[1] - q.v[0]*v[2] + q.v[1]*v[3] + q.v[2]*v[0];
    Real z = q.v[3]*v[2] + q.v[0]*v[1] - q.v[1]*v[0] + q.v[2]*v[3];
    v[3] = w; v[0] = x; v[1] = y; v[2] = z;

    return *this;
  }

  template<class Real, class Traits>
  inline Quaternion<Real,Traits>&
  Quaternion<Real,Traits>::
  operator/=(const Quaternion& q)
  {
    Real w = q.v[3]*v[3] + q.v[0]*v[0] + q.v[1]*v[1] + q.v[2]*v[2];
    Real x = q.v[3]*v[0] - q.v[0]*v[3] - q.v[1]*v[2] + q.v[2]*v[1];
    Real y = q.v[3]*v[1] + q.v[0]*v[2] - q.v[1]*v[3] - q.v[2]*v[0];
    Real z = q.v[3]*v[2] - q.v[0]*v[1] + q.v[1]*v[0] - q.v[2]*v[3];
    v[3] = w; v[0] = x; v[1] = y; v[2] = z;

    return *this;
  }

  template<class Real, class Traits>
  inline Quaternion<Real,Traits>
  Quaternion<Real,Traits>::
  operator*(const Quaternion& q) const
  {
    return Quaternion
           (
             q.v[3]*v[3] - q.v[0]*v[0] - q.v[1]*v[1] - q.v[2]*v[2],
             q.v[3]*v[0] + q.v[0]*v[3] + q.v[1]*v[2] - q.v[2]*v[1],
             q.v[3]*v[1] - q.v[0]*v[2] + q.v[1]*v[3] + q.v[2]*v[0],
             q.v[3]*v[2] + q.v[0]*v[1] - q.v[1]*v[0] + q.v[2]*v[3]
           );
  }

  template<class Real, class Traits>
  inline Quaternion<Real,Traits>
  Quaternion<Real,Traits>::
  operator/(const Quaternion& q) const
  {
    return Quaternion
           (
             q.v[3]*v[3] + q.v[0]*v[0] + q.v[1]*v[1] + q.v[2]*v[2],
             q.v[3]*v[0] - q.v[0]*v[3] - q.v[1]*v[2] + q.v[2]*v[1],
             q.v[3]*v[1] + q.v[0]*v[2] - q.v[1]*v[3] - q.v[2]*v[0],
             q.v[3]*v[2] - q.v[0]*v[1] + q.v[1]*v[0] - q.v[2]*v[3]
           );
  }

  template<class Real, class Traits>
  inline Quaternion<Real,Traits>
  Quaternion<Real,Traits>::inverse() const
  {
    return Quaternion(v[3], -v[0], -v[1], -v[2]);
  }

  template < class Real, class Traits>
  inline Quaternion<Real,Traits>
  Quaternion<Real,Traits>::operator *
  (
    const Real u
  )
  const
  {
    Vec3 axis;
    Real angle;
    getAxisAngle( axis, angle );
    return Quaternion( axis, u * angle );
  }




  template <class Real, class Traits>
  Vec3
  Quaternion<Real,Traits>::
  operator * (const Vec3& p) const
  {
    Real r[4];
    r[0] =  Traits::x(p)*v[0] + Traits::y(p)*v[1] + Traits::z(p)*v[2];
    r[1] =  Traits::x(p)*v[3] - Traits::y(p)*v[2] + Traits::z(p)*v[1];
    r[2] =  Traits::x(p)*v[2] + Traits::y(p)*v[3] - Traits::z(p)*v[0];
    r[3] = -Traits::x(p)*v[1] + Traits::y(p)*v[0] + Traits::z(p)*v[3];

    return Vec3//Quaternion<Real,Traits>::Vec3
           (
             v[3]*r[1] + v[0]*r[0] + v[1]*r[3] - v[2]*r[2],
             v[3]*r[2] - v[0]*r[3] + v[1]*r[0] + v[2]*r[1],
             v[3]*r[3] + v[0]*r[2] - v[1]*r[1] + v[2]*r[0]
           );

  }

  template < class Real, class Traits>
  Vec3
  operator * (const typename Quaternion<Real,Traits>::Vec3& p, const Quaternion<Real,Traits>& q )
  {
    return q*p;
  }


  template <class Real, class Traits>
  Vec3
  Quaternion<Real,Traits>::
  operator/(const Vec3& p) const
  {
    Real r[4];
    r[0] = -Traits::x(p)*v[0] - Traits::y(p)*v[1] - Traits::z(p)*v[2];
    r[1] =  Traits::x(p)*v[3] + Traits::y(p)*v[2] - Traits::z(p)*v[1];
    r[2] = -Traits::x(p)*v[2] + Traits::y(p)*v[3] + Traits::z(p)*v[0];
    r[3] =  Traits::x(p)*v[1] - Traits::y(p)*v[0] + Traits::z(p)*v[3];

    return Vec3
           (
             v[3]*r[1] - v[0]*r[0] - v[1]*r[3] + v[2]*r[2],
             v[3]*r[2] + v[0]*r[3] - v[1]*r[0] - v[2]*r[1],
             v[3]*r[3] - v[0]*r[2] + v[1]*r[1] - v[2]*r[0]
           );
  }

  //template < class Real, class Traits>
  //Vec3
  //operator/( const typename Quaternion<Real,Traits>::Vec3& p, const Quaternion<Real,Traits>& q)
  //{
  //  return q/p;
  //}







  template<class Real, class Traits>
  inline Real
  Quaternion<Real,Traits>::norm() const
  {
    return sqrt( v[3]*v[3] + v[0]*v[0] + v[1]*v[1] + v[2]*v[2] );
  }

  template<class Real, class Traits>
  inline Quaternion<Real,Traits>&
  Quaternion<Real,Traits>::normalize()
  {
    Real nrm = (*this).norm();
    Real tmp = 1.0/nrm;

#ifndef DEBUG
    if ( !finite(tmp) )
    {
      std::cerr << "Quaternion normalization with 1/norm not finite!" << std::endl;
      assert(false);
    }

    v[3]*=tmp; v[0]*=tmp; v[1]*=tmp; v[2]*=tmp;
    nrm = (*this).norm();

    int nb = 0;
    Real epsilon =  std::numeric_limits<Real>::epsilon();
    while ( std::abs(nrm - 1.0) > epsilon )
    {
      tmp = 1.0/nrm;
      v[3]*=tmp; v[0]*=tmp; v[1]*=tmp; v[2]*=tmp;
      nrm = (*this).norm();
      nb++;

      if ( nb == 5 )
      {
        std::cerr << "Quaternion normalization beyond threshold!" << std::endl;
        assert(false);
        break;
      }
    }
#else
    v[3]*=tmp; v[0]*=tmp; v[1]*=tmp; v[2]*=tmp;
#endif

    return *this;
  }





  template<class Real, class Traits>
  inline const Quaternion<Real,Traits>
  Quaternion<Real,Traits>::identity()
  {
    return Quaternion(1.0, 0.0, 0.0, 0.0);
  }





  template <class Real, class Traits, class Mat33>
  inline void
  writeRotMatrix(const Quaternion<Real,Traits>& q, Mat33& m)
  {
    Real qw = q.w();
    Real qx = q.x();
    Real qy = q.y();
    Real qz = q.z();

    Real qww = qw*qw;
    Real qwx = qw*qx;
    Real qwy = qw*qy;
    Real qwz = qw*qz;

    Real qxx = qx*qx;
    Real qxy = qx*qy;
    Real qxz = qx*qz;

    Real qyy = qy*qy;
    Real qyz = qy*qz;

    Real qzz = qz*qz;

    m[0][0] = qww+qxx - (qyy + qzz);
    m[0][1] =       2.0*(qxy - qwz);
    m[0][2] =       2.0*(qxz + qwy);

    m[1][0] =       2.0*(qxy + qwz);
    m[1][1] = qww+qyy - (qzz + qxx);
    m[1][2] =       2.0*(qyz - qwx);

    m[2][0] =       2.0*(qxz - qwy);
    m[2][1] =       2.0*(qyz + qwx);
    m[2][2] = qww+qzz - (qyy + qxx);
  }



  //template <class Real, class Traits, class Mat33>
  //inline void
  //writeRotMatrix(const Quaternion<Real,Traits>& q, Mat33& m)
  //{
  //  Real qw = q.w();
  //  Real qx = q.x();
  //  Real qy = q.y();
  //  Real qz = q.z();
  //
  //  Real qww = qw*qw;
  //  Real qwx = qw*qx;
  //  Real qwy = qw*qy;
  //  Real qwz = qw*qz;
  //
  //  Real qxx = qx*qx;
  //  Real qxy = qx*qy;
  //  Real qxz = qx*qz;
  //
  //  Real qyy = qy*qy;
  //  Real qyz = qy*qz;
  //
  //  Real qzz = qz*qz;
  //
  //  m[0][0] = qww+qxx - (qyy + qzz);
  //  m[1][0] =       2.0*(qxy - qwz);
  //  m[2][0] =       2.0*(qxz + qwy);
  //
  //  m[0][1] =       2.0*(qxy + qwz);
  //  m[1][1] = qww+qyy - (qzz + qxx);
  //  m[2][1] =       2.0*(qyz - qwx);
  //
  //  m[0][2] =       2.0*(qxz - qwy);
  //  m[1][2] =       2.0*(qyz + qwx);
  //  m[2][2] = qww+qzz - (qyy + qxx);
  //}

  //template <class Real, class Traits, class Mat44>
  //inline void
  //writeOpenGLRotMatrix(const Quaternion<Real,Traits>& q, Mat44& m)
  //{
  //  Real qw = q.w();
  //  Real qx = q.x();
  //  Real qy = q.y();
  //  Real qz = q.z();
  //
  //  Real qwx = qw*qx;
  //  Real qwy = qw*qy;
  //  Real qwz = qw*qz;
  //
  //  Real qxx = qx*qx;
  //  Real qxy = qx*qy;
  //  Real qxz = qx*qz;
  //
  //  Real qyy = qy*qy;
  //  Real qyz = qy*qz;
  //
  //  Real qzz = qz*qz;
  //
  //  m[0][0] = 1.0 - 2.0*(qyy + qzz);
  //  m[0][1] =       2.0*(qxy - qwz);
  //  m[0][2] =       2.0*(qxz + qwy);
  //  m[0][3] = 0.0;
  //
  //  m[1][0] =       2.0*(qxy + qwz);
  //  m[1][1] = 1.0 - 2.0*(qzz + qxx);
  //  m[1][2] =       2.0*(qyz - qwx);
  //  m[1][3] = 0.0;
  //
  //  m[2][0] =       2.0*(qxz - qwy);
  //  m[2][1] =       2.0*(qyz + qwx);
  //  m[2][2] = 1.0 - 2.0*(qyy + qxx);
  //  m[2][3] = 0.0;
  //
  //  m[3][0] = 0.0;
  //  m[3][1] = 0.0;
  //  m[3][2] = 0.0;
  //  m[3][3] = 1.0;
  //}

  template <class Real, class Traits, class Mat44>
  inline void
  writeOpenGLRotMatrix(const Quaternion<Real,Traits>& q, Mat44& m)
  {
    Real qw = q.w();
    Real qx = q.x();
    Real qy = q.y();
    Real qz = q.z();

    Real qwx = qw*qx;
    Real qwy = qw*qy;
    Real qwz = qw*qz;

    Real qxx = qx*qx;
    Real qxy = qx*qy;
    Real qxz = qx*qz;

    Real qyy = qy*qy;
    Real qyz = qy*qz;

    Real qzz = qz*qz;

    m[0][0] = 1.0 - 2.0*(qyy + qzz);
    m[1][0] =     2.0*(qxy - qwz);
    m[2][0] =     2.0*(qxz + qwy);
    m[3][0] = 0.0;

    m[0][1] =     2.0*(qxy + qwz);
    m[1][1] = 1.0 - 2.0*(qzz + qxx);
    m[2][1] =     2.0*(qyz - qwx);
    m[3][1] = 0.0;

    m[0][2] =     2.0*(qxz - qwy);
    m[1][2] =     2.0*(qyz + qwx);
    m[2][2] = 1.0 - 2.0*(qyy + qxx);
    m[3][2] = 0.0;

    m[0][3] = 0.0;
    m[1][3] = 0.0;
    m[2][3] = 0.0;
    m[3][3] = 1.0;
  }

  // Time integration of a rotation velocity
  template <class Real, class Traits>
  template <class Vect >
  inline Quaternion<Real,Traits>
  Quaternion<Real,Traits>::eulerIncrement
  (
    const Vect& omega, // vector omega
    const Real dt   // time step
  )
  {
    return eulerIncrement( omega[0], omega[1], omega[2], dt );
  }


  // Time integration of a rotation velocity
  template <class Real, class Traits>
  inline Quaternion<Real,Traits>
  Quaternion<Real,Traits>::eulerIncrement
  (
    const Real xa, const Real ya, const Real za, // vector omega
    const Real dt   // time step
  )
  {
    // Create quaternion q with null rotation
    Quaternion<Real,Traits> q = Quaternion<Real,Traits>::identity();

    // If the rotation velocity is big enough,
    // then set q accordingly.
    Real norm = sqrt( xa*xa + ya*ya + za*za );
    if( norm > Traits::nullAxis() )
    {
      q.setAxisAngle( xa, ya, za, norm * dt );
    }

    // return quaternion q
    return q;
  }

  // Time integration of a rotation velocity during a unit time step
  template <class Real, class Traits>
  template <class Vect >
  inline Quaternion<Real,Traits>
  Quaternion<Real,Traits>::elementaryRotation
  (
    const Vect& omega // vector omega
  )
  {
    return elementaryRotation( omega[0], omega[1], omega[2] );
  }

  // Time integration of a rotation velocity during a unit time step
  template <class Real, class Traits>
  inline Quaternion<Real,Traits>
  Quaternion<Real,Traits>::elementaryRotation
  (
    const Real xa, const Real ya, const Real za // vector omega
  )
  {
    // Create quaternion q with null rotation
    Quaternion<Real,Traits> q = Quaternion<Real,Traits>::identity();

    // If the rotation velocity is big enough,
    // then set q accordingly.
    Real norm = sqrt( xa*xa + ya*ya + za*za );
    if( norm > Traits::nullAxis() )
    {
      q.setAxisAngle( xa, ya, za, 1 );
    }

    // return quaternion q
    return q;
  }

} // end namespace animal



#endif // _ANIMAL_GEOMETRY_QUATERNION_H_

---