Quaternion.h

Go to the documentation of this file.

//    Copyright 2004, SenseGraphics AB
//
//    This file is part of H3D API.
//
//    H3D API 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.
//
//    H3D API 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 H3D API; if not, write to the Free Software
//    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
//
//    A commercial license is also available. Please contact us at 
//    www.sensegraphics.com for more information.
//
//
//

#ifndef __QUATERNION_H__
#define __QUATERNION_H__

#include "H3DApi.h"
#include "H3DBasicTypes.h"
#include "H3DMath.h"
#include "Vec3f.h"

namespace H3D {
  namespace ArithmeticTypes {
    // forward declarations.
    class Matrix3f;
        class Matrix3d;
    class Rotation;

    class H3DAPI_API Quaternion {
    public:
      Quaternion(): v( 0, 0, 0 ), w( 0 ) {}

      Quaternion( H3DFloat x,
                  H3DFloat y,
                  H3DFloat z,
                  H3DFloat _w ) : v( x, y, z ), w(_w) {}
      
      Quaternion( const Vec3f &_v, 
                  H3DFloat _w ) : v( _v ), w( _w ) {}
      
      explicit Quaternion( const Vec3f &euler_angles );

      explicit Quaternion( const Vec3d &euler_angles );

      Quaternion( const Rotation &r );
      
      explicit Quaternion( const Matrix3f &m );

      explicit Quaternion( const Matrix3d &m );

      inline H3DFloat norm() {
        return w*w + v.dotProduct( v );
      }

      inline void normalize() {
        H3DFloat n = norm();
        if (H3DAbs( n ) > Constants::f_epsilon ) {
          H3DFloat length = H3DSqrt( n );
          v = v / length;
          w = w / length;
        }
      }
      
      inline H3DFloat dotProduct( const Quaternion &q) const {
        return v.x*q.v.x + v.y*q.v.y + v.z*q.v.z + w*q.w;
      }
      
      inline Quaternion conjugate() {
        return Quaternion( -v, w );
      }

      inline Quaternion inverse();
      
      Vec3f toEulerAngles();

      Quaternion slerp( const Quaternion &q,
                        H3DFloat t ) const;

      Vec3f v;
      H3DFloat w;
    };


          inline ostream& operator<<( ostream &os, const Quaternion &q ) {
                  os << q.v << " " << q.w;
                  return os;
          }

    inline bool operator==( const Quaternion &q1, const Quaternion &q2 ) {
      return q1.v == q2.v && q1.w == q2.w;
    }

    inline Quaternion operator*( const Quaternion &q1, 
                                 const Quaternion &q2 ) {    
      return Quaternion( q1.w * q2.v + q2.w * q1.v + q1.v % q2.v,
                         q1.w * q2.w - q1.v * q2.v );
    }

    inline Quaternion operator*( const Quaternion &q, 
                                 double d ) {    
      return Quaternion( d *q.v, (H3DFloat)(d * q.w) );
    }

    inline Quaternion operator*( const Quaternion &q, 
                                 float d ) {    
      return Quaternion( d *q.v, d * q.w );
    }

    inline Quaternion operator*( const Quaternion &q, 
                                 int d ) {    
      return Quaternion( d *q.v, (H3DFloat) (d * q.w) );
    }

    inline Quaternion operator*( const Quaternion &q, 
                                 long d ) {    
      return Quaternion( d *q.v, (H3DFloat) (d * q.w) );
    }

    inline Quaternion operator*( const float &a, 
                                 const Quaternion &b ) { 
      return b * a;
    }

    inline Quaternion operator*( const double &a, 
                                 const Quaternion &b ) { 
      return b * a;
    }

    inline Quaternion operator*( const int &a, 
                                 const Quaternion &b ) { 
      return b * a;
    }

    inline Quaternion operator*( const long &a, 
                                 const Quaternion &b ) { 
      return b * a;
    }

    inline Quaternion operator+( const Quaternion &q1, 
                                 const Quaternion &q2 ) {
      return Quaternion( q1.v + q2.v,
                         q1.w + q2.w );
    }

    inline Quaternion operator-( const Quaternion &q ) { return q * -1; }
    
    inline Quaternion operator-( const Quaternion &a, const Quaternion &b ) { 
      return a + (-b); 
    }


    // Returns the inverse of the Quaternion.
    inline Quaternion Quaternion::inverse() {
      H3DFloat n = norm();
      if ( H3DAbs(n ) > Constants::f_epsilon ) {
        return Quaternion(0,0,0,0);
      } else {
        return conjugate() / n;
      }
    }
  }
}

#endif

---