Subversion Repositories gelsvn

#ifndef __CGLA_QUATD_H__
#define __CGLA_QUATD_H__

#include "Vec3d.h"
#include "Vec4d.h"
#include "Mat3x3d.h"
#include "Mat4x4d.h"

namespace CGLA {

  /** A Quaterinion class. Quaternions are algebraic entities 
      useful for rotation. */

  class Quatd
  {
  public:

    /// Vector part of quaternion
    Vec3d qv;

    /// Scalar part of quaternion
    double qw;

    /// Construct undefined quaternion
#ifndef NDEBUG
    Quatd() : qw(CGLA_INIT_VALUE) {}
#else
    Quatd() {}
#endif

    /// Construct quaternion from vector and scalar
    Quatd(const Vec3d& imaginary, double real = 1.0f) : qv(imaginary) , qw(real) {}

    /// Construct quaternion from four scalars
    Quatd(double x, double y, double z, double _qw) : qv(x,y,z), qw(_qw) {}

    /// Construct quaternion from a 4D vector
    explicit Quatd(const Vec4d& v) : qv(v[0], v[1], v[2]), qw(v[3]) {}

    /// Assign values to a quaternion
    void set(const Vec3d& imaginary, double real=1.0f)
    {
      qv = imaginary;
      qw = real;
    }

    void set(double x, double y, double z, double _qw) 
    {
      qv.set(x,y,z);
      qw = _qw;
    }

    void set(const Vec4f& v)
    {
      qv.set(v[0], v[1], v[2]);
      qw = v[3];                  
    }

    /// Get values from a quaternion
    void get(double& x, double& y, double& z, double& _qw) const
    {
      x  = qv[0];
      y  = qv[1];
      z  = qv[2];
      _qw = qw;
    }

    /// Get imaginary part of a quaternion
    Vec3d get_imaginary_part() const { return qv; }

    /// Get real part of a quaternion
    double get_real_part() const { return qw; }

    /// Get a 3x3 rotation matrix from a quaternion
    Mat3x3d get_mat3x3d() const;

    /// Get a 4x4 rotation matrix from a quaternion
    Mat4x4d get_mat4x4d() const;

    /// Obtain angle of rotation and axis
    void get_rot(double& angle, Vec3d& vec);

    /// Construct a Quaternion from an angle and axis of rotation.
    void make_rot(double angle, const Vec3d&);

    /** Construct a Quaternion rotating from the direction given
        by the first argument to the direction given by the second.*/
    void make_rot(const Vec3d&,const Vec3d&);

    //----------------------------------------------------------------------
    // Binary operators
    //----------------------------------------------------------------------
    
    bool operator==(const Quatd& q) const
    {
      return qw == q.qw && qv == q.qv;
    }

    bool operator!=(const Quatd& q) const
    {
      return qw != q.qw || qv != q.qv;
    }

    /// Multiply two quaternions. (Combine their rotation)
    Quatd operator*(const Quatd& q) const
    {
      return Quatd(cross(qv, q.qv) + qv*q.qw + q.qv*qw, 
                         qw*q.qw - dot(qv, q.qv));      
    }

    /// Multiply scalar onto quaternion.
    Quatd operator*(double scalar) const
    {
      return Quatd(qv*scalar, qw*scalar);
    }

    /// Add two quaternions.
    Quatd operator+(const Quatd& q) const
    {
      return Quatd(qv + q.qv, qw + q.qw);
    }

    //----------------------------------------------------------------------
    // Unary operators
    //----------------------------------------------------------------------

    /// Compute the additive inverse of the quaternion
    Quatd operator-() const { return Quatd(-qv, -qw); }

    /// Compute norm of quaternion
    double norm() const { return dot(qv, qv) + qw*qw; }

    /// Return conjugate quaternion
    Quatd conjugate() const { return Quatd(-qv, qw); }

    /// Compute the multiplicative inverse of the quaternion
    Quatd inverse() const { return Quatd(conjugate()*(1/norm())); }
    
    /// Normalize quaternion.
    Quatd normalize() { return Quatd((*this)*(1/norm())); }

    //----------------------------------------------------------------------
    // Application
    //----------------------------------------------------------------------

    /// Rotate vector according to quaternion
    Vec3d apply(const Vec3d& vec) const 
    {
      return ((*this)*Quatd(vec)*inverse()).qv;
    }

    /// Rotate vector according to unit quaternion
    Vec3d apply_unit(const Vec3d& vec) const
    {
      return ((*this)*Quatd(vec)*conjugate()).qv;
    }
  };

  inline Quatd operator*(double scalar, const Quatd& q)
  {
    return q*scalar;
  }

  /** Perform linear interpolation of two quaternions. 
      The last argument is the parameter used to interpolate
      between the two first. SLERP - invented by Shoemake -
      is a good way to interpolate because the interpolation
      is performed on the unit sphere.  
  */
  inline Quatd slerp(Quatd q0, Quatd q1, double t);

  /// Create an identity quaternion
  inline Quatd identity_Quatd()
  {
    return Quatd(Vec3d(0.0));
  }

  /// Print quaternion to stream.
  inline std::ostream& operator<<(std::ostream&os, const Quatd v)
  {
    os << "[ ";
    for(unsigned int i=0;i<3;i++) os << v.qv[i] << " ";
    os << "~ " << v.qw << " ";
    os << "]";

    return os;
  }
}
#endif