quat.h

Go to the documentation of this file.

/*
 * $Id: quat.h,v 1.5 2003/10/09 08:27:51 jmk Exp $
 */
///////////////////////////////////////////////////////////////////////////
//              _____________  ______________________    __   __  _____
//             /  ________  |  |   ___   ________   /   |  \ /  \   |
//            |  |       |  |_ |  |_ |  |       /  /    \__  |      |
//            |  |  ___  |  || |  || |  |      /  /        | |      |
//            |  | |   \ |  || |  || |  |     /  /      \__/ \__/ __|__
//            |  | |_@  ||  || |  || |  |    /  /          Institute
//            |  |___/  ||  ||_|  || |  |   /  /_____________________
//             \_______/  \______/ | |__|  /___________________________
//                        |  |__|  |
//                         \______/
//                    University of Utah       
//                           2002
///////////////////////////////////////////////////////////////////////////

// quat.h

#ifndef quat_h
#define quat_h

#include <../mathGutz/vec4.h>

namespace gutz {

template <class T> class vec3;
template <class T> class mat3;

template <class T>
class quat
  : public vec4<T> 
{
public:
  inline quat ();
  inline quat (const T, const T, const T, const T);
  inline quat (const vec4<T>&);
  inline quat (const quat<T>&);
  inline quat (const vec3<T>&);
  inline quat (const T, const vec3<T>&);
  inline quat (const mat3<T>&);
  inline ~quat () {}

  inline quat mult (const quat&) const;
  inline quat conj () const;
  inline quat inv () const;

  inline vec3<T> rot (const vec3<T>&) const;
  inline quat rot (const quat&) const;

  inline void set (const T phi, const vec3<T>& k);
  inline void set (const mat3<T>& M);
};

//-----------------------------------------------------------------------------
// typedefs
//-----------------------------------------------------------------------------

typedef quat<float> quatf;
typedef quat<double> quatd;

//-----------------------------------------------------------------------------
// Implementation
//-----------------------------------------------------------------------------

template <class T>
quat<T>::quat ()
  : vec4<T>(0, 0, 0, 1)
{}

template <class T>
quat<T>::quat (T x0, T y0, T z0, T w0)
  : vec4<T>(x0, y0, z0, w0)
{}

template <class T>
quat<T>::quat (const vec4<T>& v)
  : vec4<T>(v)
{}

template <class T>
quat<T>::quat (const quat<T>& v)
  : vec4<T>((vec4<T>)v)
{}

template <class T>
quat<T>::quat (const vec3<T>& v)
  : vec4<T>(v.x, v.y, v.z, 0)
{}

template <class T>
quat<T>::quat (const T phi, const vec3<T>& k)
{
  set(phi, k);
}

template <class T>
quat<T>::quat (const mat3<T>& M)
{
  set(M);
}

template <class T>
quat<T>
quat<T>::mult (const quat<T>& q) const
{
  return quat( w*q.x + q.w*x + y*q.z - q.y*z,
                    w*q.y + q.w*y + z*q.x - q.z*x,
                    w*q.z + q.w*z + x*q.y - q.x*y,
                    w*q.w - x*q.x - y*q.y - z*q.z);
}

template <class T>
quat<T>
quat<T>::conj () const
{
  return quat(-x, -y, -z, w);
}

template <class T>
quat<T>
quat<T>::inv () const
{
  T n = 1/(w*w + x*x + y*y + z*z);
  return quat(-x*n, -y*n, -z*n, w*n);
}

template <class T>
vec3<T>
quat<T>::rot (const vec3<T>& v) const
{
  // calculate tmp = q*v
  quat tmp(w*v.x + y*v.z - v.y*z,
           w*v.y + z*v.x - v.z*x,
           w*v.z + x*v.y - v.x*y,
           - x*v.x - y*v.y - z*v.z);
  // final result is (q*v)*conj(q)
  return vec3<T>(- tmp.w*x + w*tmp.x - tmp.y*z + y*tmp.z,
                 - tmp.w*y + w*tmp.y - tmp.z*x + z*tmp.x,
                 - tmp.w*z + w*tmp.z - tmp.x*y + x*tmp.y);
}

template <class T>
quat<T>
quat<T>::rot (const quat& r) const
{
  // calculate tmp = q*r
  quat tmp(w*r.x + r.w*x + y*r.z - r.y*z,
           w*r.y + r.w*y + z*r.x - r.z*x,
           w*r.z + r.w*z + x*r.y - r.x*y,
           w*r.w - x*r.x - y*r.y - z*r.z);
  // final result is (q*r)*conj(q)
  return quat(- tmp.w*x + w*tmp.x - tmp.y*z + y*tmp.z,
              - tmp.w*y + w*tmp.y - tmp.z*x + z*tmp.x,
              - tmp.w*z + w*tmp.z - tmp.x*y + x*tmp.y,
              tmp.w*w + tmp.x*x + tmp.y*y + tmp.z*z);
}

template <class T>
void
quat<T>::set (const T phi, const vec3<T>& k)
{
  vec3<T> axis = k;
  axis.normalize();
  T sphi = T(sin(phi*0.5));
  x = axis.x*sphi;
  y = axis.y*sphi;
  z = axis.z*sphi;
  w = T(cos(phi*0.5));
}

template <class T>
void
quat<T>::set (const mat3<T>& M)
{
  T M11 = M.m[0];
  T M22 = M.m[4];
  T M33 = M.m[8];
  T M00 = M11 + M22 + M33;
  switch (maxi4<T>(M00, M11, M22, M33))
  {
    case 0:
    w = sqrt(1 + M00);
    x = (M.m[5] - M.m[7])/w;
    y = (M.m[6] - M.m[2])/w;
    z = (M.m[1] - M.m[3])/w;
    break;
    case 1:
    x = sqrt(1 + 2*M11 - M00);
    w = (M.m[5] - M.m[7])/x;
    y = (M.m[1] + M.m[3])/x;
    z = (M.m[2] + M.m[6])/x;
    break;
    case 2:
    y = sqrt(1 + 2*M22 - M00);
    w = (M.m[6] - M.m[2])/y;
    x = (M.m[1] + M.m[3])/y;
    z = (M.m[5] + M.m[7])/y;
    break;
    case 3:
    z = sqrt(1 + 2*M33 - M00);
    w = (M.m[1] - M.m[3])/z;
    x = (M.m[6] + M.m[2])/z;
    y = (M.m[5] + M.m[7])/z;
    break;
  }
  x *= 0.5;
  y *= 0.5;
  z *= 0.5;
  w *= 0.5;
}


template <class T1, class T2>
inline quat<T2>
mm_cast(quat<T1> v)
{
  return quat<T2>(v.x, v.y, v.z, v.w);
}

} // namespace ve

#endif // quat_h

---

Send questions, comments, and bug reports to: