Subversion Repositories gelsvn

#ifndef __CGLA_QUATF_H__

#define __CGLA_QUATF_H__

#include "Vec3f.h"

#include "Vec4f.h"

#include "Mat3x3f.h"

#include "Mat4x4f.h"

#include <cmath>

namespace CGLA {

  /** A Quaterinion class. Quaternions are algebraic entities 

      useful for rotation. */

  class Quatf

  {

  public:

    /// Vector part of quaternion

    Vec3f qv;

    /// Scalar part of quaternion

    float qw;

    /// Construct undefined quaternion

#ifndef NDEBUG

    Quatf() : qw(CGLA_INIT_VALUE) {}

#else

    Quatf() {}

#endif

    /// Construct quaternion from vector and scalar

    Quatf(const Vec3f& imaginary, float real = 1.0f) : qv(imaginary) , qw(real) {}

    /// Construct quaternion from four scalars

    Quatf(float x, float y, float z, float _qw) : qv(x,y,z), qw(_qw) {}

    /// Construct quaternion from a 4D vector

    explicit Quatf(const Vec4f& v) : qv(v[0], v[1], v[2]), qw(v[3]) {}

    /// Assign values to a quaternion

    void set(const Vec3f& imaginary, float real=1.0f)

    {

      qv = imaginary;

      qw = real;

    }

    void set(float x, float y, float z, float _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(float& x, float& y, float& z, float& _qw) const

    {

      x  = qv[0];

      y  = qv[1];

      z  = qv[2];

      _qw = qw;

    }

    /// Get imaginary part of a quaternion

    Vec3f get_imaginary_part() const { return qv; }

    /// Get real part of a quaternion

    float get_real_part() const { return qw; }

    /// Get a 3x3 rotation matrix from a quaternion

    Mat3x3f get_mat3x3f() const

    {

      float s = 2/norm();

      // note that the all q_*q_ are used twice (optimize)

      return Mat3x3f(Vec3f(1.0 - s*(qv[1]*qv[1] + qv[2]*qv[2]),

			         s*(qv[0]*qv[1] - qw*qv[2]),

			         s*(qv[0]*qv[2] + qw*qv[1])),

		     Vec3f(      s*(qv[0]*qv[1] + qw*qv[2]),

  			   1.0 - s*(qv[0]*qv[0] + qv[2]*qv[2]),

			         s*(qv[1]*qv[2] - qw*qv[0])),

		     Vec3f(      s*(qv[0]*qv[2] - qw*qv[1]),

			         s*(qv[1]*qv[2] + qw*qv[0]),

			   1.0 - s*(qv[0]*qv[0] + qv[1]*qv[1])));

    }

    /// Get a 4x4 rotation matrix from a quaternion

    Mat4x4f get_mat4x4f() const

    {

      float s = 2/norm();

      // note that the all q_*q_ are used twice (optimize?)

      return Mat4x4f(Vec4f(1.0 - s*(qv[1]*qv[1] + qv[2]*qv[2]),

			         s*(qv[0]*qv[1] - qw*qv[2]),

			         s*(qv[0]*qv[2] + qw*qv[1]),

		           0.0),

		     Vec4f(      s*(qv[0]*qv[1] + qw*qv[2]),

			   1.0 - s*(qv[0]*qv[0] + qv[2]*qv[2]),

			         s*(qv[1]*qv[2] - qw*qv[0]),

			   0.0),

		     Vec4f(      s*(qv[0]*qv[2] - qw*qv[1]),

			         s*(qv[1]*qv[2] + qw*qv[0]),

			   1.0 - s*(qv[0]*qv[0] + qv[1]*qv[1]),

			   0.0),

		     Vec4f(0.0, 0.0, 0.0, 1.0));

    }

    /// Obtain angle of rotation and axis

    void get_rot(float& angle, Vec3f& v)

    {

      angle = 2*std::acos(qw);

      if(angle < TINY) 

	v = Vec3f(1.0, 0.0, 0.0);

      else 

	v = qv*(1/std::sin(angle));

      if(angle > M_PI)

	v = -v;

      v.normalize();      

    }

    /// Construct a Quaternion from an angle and axis of rotation.

    void make_rot(float angle, const Vec3f& v)

    {

      angle /= 2.0;

      qv = CGLA::normalize(v)*std::sin(angle);

      qw = std::cos(angle);

    }

    /** Construct a Quaternion rotating from the direction given

	by the first argument to the direction given by the second.*/

    void make_rot(const Vec3f& s,const Vec3f& t)

    {

      float tmp = std::sqrt(2*(1 + dot(s, t)));

      qv = cross(s, t)*(1.0/tmp);

      qw = tmp/2.0;    

    }

    //----------------------------------------------------------------------

    // Binary operators

    //----------------------------------------------------------------------

    bool operator==(const Quatf& q) const

    {

      return qw == q.qw && qv == q.qv;

    }

    bool operator!=(const Quatf& q) const

    {

      return qw != q.qw || qv != q.qv;

    }

    /// Multiply two quaternions. (Combine their rotation)

    Quatf operator*(const Quatf& q) const

    {

      return Quatf(cross(qv, q.qv) + qv*q.qw + q.qv*qw, 

		         qw*q.qw - dot(qv, q.qv));      

    }

    /// Multiply scalar onto quaternion.

    Quatf operator*(float scalar) const

    {

      return Quatf(qv*scalar, qw*scalar);

    }

    /// Add two quaternions.

    Quatf operator+(const Quatf& q) const

    {

      return Quatf(qv + q.qv, qw + q.qw);

    }

    //----------------------------------------------------------------------

    // Unary operators

    //----------------------------------------------------------------------

    /// Compute the additive inverse of the quaternion

    Quatf operator-() const { return Quatf(-qv, -qw); }

    /// Compute norm of quaternion

    float norm() const { return dot(qv, qv) + qw*qw; }

    /// Return conjugate quaternion

    Quatf conjugate() const { return Quatf(-qv, qw); }

    /// Compute the multiplicative inverse of the quaternion

    Quatf inverse() const { return Quatf(conjugate()*(1/norm())); }

    /// Normalize quaternion.

    Quatf normalize() { return Quatf((*this)*(1/norm())); }

    //----------------------------------------------------------------------

    // Application

    //----------------------------------------------------------------------

    /// Rotate vector according to quaternion

    Vec3f apply(const Vec3f& vec) const 

    {

      return ((*this)*Quatf(vec)*inverse()).qv;

    }

    /// Rotate vector according to unit quaternion

    Vec3f apply_unit(const Vec3f& vec) const

    {

      return ((*this)*Quatf(vec)*conjugate()).qv;

    }

  };

  inline Quatf operator*(float scalar, const Quatf& 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 Quatf slerp(Quatf q0, Quatf q1, float t)

  {

    float angle = std::acos(q0.qv[0]*q1.qv[0] + q0.qv[1]*q1.qv[1] 

			    + q0.qv[2]*q1.qv[2] + q0.qw*q1.qw);

    return (q0*std::sin((1 - t)*angle) + q1*std::sin(t*angle))*(1/std::sin(angle));

  }

  /// Create an identity quaternion

  inline Quatf identity_Quatf()

  {

    return Quatf(Vec3f(0.0));

  }

  /// Print quaternion to stream.

  inline std::ostream& operator<<(std::ostream&os, const Quatf v)

  {

    os << "[ ";

    for(unsigned int i=0;i<3;i++) os << v.qv[i] << " ";

    os << "~ " << v.qw << " ";

    os << "]";

    return os;

  }

}

#endif