OpenGTA/math/quaternion.h

#ifndef QUATERNION_MATH
#define QUATERNION_MATH
#include <cmath>

namespace Util { namespace Math {
  /*! 
   * \namespace Util::Math
   * \brief Vector/Matrix/Quaternion interpolation
   *
   * Found at http://www.flipcode.com/files/code/vquats.cpp
   *
   *
   * I think this is very sleek, so I kept it.
   *
   * Editor's note:  
   * COTD Entry:  Vector Math & Quaternions by Tim Sweeney [tim@epicgames.com]

       This is some fairly generic vector math code.  Here I want to
       illustrate two things:

       1. The "vectorSpace" template is a nice way to represent the
       general mathematical notion of vectors without regard to
       particular data types.  The idea is that you can declare
       a vector type (like vec4 below), and the vectorSpace template
       automatically generates the appropriate vector operators.

       I wrote this code for clarity and generality. For performance, you would 
       want to specialize the vectorSpace template for common types, like 4-
       component floating-point vectors. You could implement these using 
       KNI and 3DNow instructions, for example.

       2. Useful quaternion functions, most notably "exp" and "ln".

       The traditional and limited way programmers used quaternion
       rotations in the past was to use "spherical linear interpolation",
       interpolating a rotational arc between two quaternions at a constant
       angular rate.  Given quaternions q1 and q2, you might have used 
       a function lik "slerp(q1,q2,0.25)" to find a rotation
       one-quarter between the two quaternions.  This approach
       was unsatisfactory because it was difficult to perform
       smooth, higher order, i.e. hermite or bezier, interpolation
       between quaternions.

       New approach: take the logarithm ("ln" function below) of 
       your quaternions, then use ordinary linear interpolation 
       (sampleLinear below) or any higher-order interpolation
       scheme -- and then take the exponent ("exp") of the result.
       The resulting rotations will have the desired smooth angular
       movement rates.

       Why does this work?  Read up on "geometric algebra" to find out.
       Unit quaternions are just a special 3-dimensional case of 
       spinors, which are the most general representation of
       n-dimensional rotations possible.
       */

    // Template implementing a "vector space" (in the precise mathematical sense).
    // A vector space is a type "u", implemented as 
    // an array of "n" elements of type "t".
    template<class t,int n,class u> struct vectorSpace {
      t x[n];
      vectorSpace() {for(int i=0; i<n; i++) x[i]=t();}
      inline t& operator[](int i) {/*assert(i>=0 && i<n);*/ return x[i];}
      inline const t& operator[](int i) const {/*assert(i>=0 && i<n);*/ return x[i];}
      friend bool operator==    (const u&      a,const u&      b) {bool c=true;  for(int i=0; i<n; i++) c=c&&a[i]==b[i]; return c;}
      friend bool operator!=    (const u&      a,const u&      b) {bool c=false; for(int i=0; i<n; i++) c=c||a[i]!=b[i]; return c;}
      friend inline t vectorSum (const u&      a                ) {t    c=0;     for(int i=0; i<n; i++) c+=a[i];         return c;}
      friend inline t vectorProd(const u&      a                ) {t    c=1;     for(int i=0; i<n; i++) c*=a[i];         return c;}
      friend inline u operator- (const u&      a                ) {u c; for(int i=0; i<n; i++) c[i] =    -a[i]; return c;}
      friend inline u operator+ (const u&      a,const u&      b) {u c; for(int i=0; i<n; i++) c[i] =a[i]+b[i]; return c;}
      friend inline u operator- (const u&      a,const u&      b) {u c; for(int i=0; i<n; i++) c[i] =a[i]-b[i]; return c;}
      friend inline u operator* (const float   a,const u&      b) {u c; for(int i=0; i<n; i++) c[i] =a   *b[i]; return c;}
      friend inline u operator* (const u&      a,const float   b) {u c; for(int i=0; i<n; i++) c[i] =a[i]*b;    return c;}
      friend inline u operator/ (const u&      a,const float   b) {u c; for(int i=0; i<n; i++) c[i] =a[i]/b;    return c;}
      friend inline u operator+=(      u&      a,const u&      b) {     for(int i=0; i<n; i++) a[i]+=b[i];      return a;}
      friend inline u operator-=(      u&      a,const u&      b) {     for(int i=0; i<n; i++) a[i]-=b[i];      return a;}
      friend inline u operator*=(      u&      a,const float   b) {     for(int i=0; i<n; i++) a[i]*=b;         return a;}
      friend inline u operator/=(      u&      a,const float   b) {     for(int i=0; i<n; i++) a[i]/=b;         return a;}
    };
  template<class t> inline t sampleLinear(const t& a,const t& b,float s) {
    return (1-s)*a+s*b;
  }
  template<class t> inline t sampleHermite(const t& a,const t& b,const t& at,const t& bt,float s) {
    return
      (+2*s*s*s -3*s*s    +1)*a+
      (-2*s*s*s +3*s*s      )*b+
      (+1*s*s*s -2*s*s +s   )*at+
      (+1*s*s*s -1*s*s      )*bt;
  }
  template<class t> inline t sampleBezier(const t& a,const t& b,const t& ac,const t& bc,float s) {
    return
      (-1*s*s*s +3*s*s -3*s +1)*a+
      (+3*s*s*s -6*s*s +3*s   )*b+
      (-3*s*s*s +3*s*s        )*ac+
      (+1*s*s*s               )*bc;
  }

  // 4-component floating point vectors.
  struct vec4: public vectorSpace<float,4,vec4> {
    inline vec4() {x[0]=x[1]=x[2]=x[3]=0;}
    inline vec4(float _x,float _y,float _z,float _w) {x[0]=_x; x[1]=_y; x[2]=_z; x[3]=_w;}
  };

  // 4x4 matrices.
  struct mtx44: public vectorSpace<vec4,4,mtx44> {
    mtx44() {}
    mtx44(const vec4& _x,const vec4& _y, const vec4& _z,const vec4& _w) {x[0]=_x; x[1]=_y; x[2]=_z; x[3]=_w;}
  };

  // A 3-dimensional (4-component) quaternion.
  struct quat: public vectorSpace<float,4,quat> {
    quat() {x[0]=x[1]=x[2]=0; x[3]=0;}
    quat(float _x,float _y,float _z,float _w) {x[0]=_x; x[1]=_y; x[2]=_z; x[3]=_w;}
  };
  inline quat operator*(const quat& a,const quat& b) {
    return quat(
        +a[0]*b[3] +a[1]*b[2] -a[2]*b[1] +a[3]*b[0],
        -a[0]*b[2] +a[1]*b[3] +a[2]*b[0] +a[3]*b[1],
        +a[0]*b[1] -a[1]*b[0] +a[2]*b[3] +a[3]*b[2],
        -a[0]*b[0] -a[1]*b[1] -a[2]*b[2] +a[3]*b[3]
        );
  }
  inline float norm(const quat& a) {
    return a[0]*a[0] + a[1]*a[1] + a[2]*a[2] + a[3]*a[3];
  }
  inline float mag(const quat& a) {
    return sqrt(norm(a));
  }
  inline quat normal(const quat& a) {
    return a/mag(a);
  }
  inline quat conj(const quat& a) {
    return quat(-a[0],-a[1],-a[2],a[3]);
  }
  inline quat inv(const quat& a) {
    return 1.f/norm(a)*conj(a);
  }
  inline quat exp(const quat& a) {
    float r  = sqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);
    float et = std::exp(a[3]);
    float s  = r>=0.00001f? et*sin(r)/r: 0.f;
    return quat(s*a[0],s*a[1],s*a[2],et*cos(r));
  }
  inline quat ln(const quat& a) {
    float r  = sqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);
    float t  = r>0.00001f? atan2(r,a[3])/r: 0.f;
    return quat(t*a[0],t*a[1],t*a[2],0.5*log(norm(a)));
  }
  inline vec4 quatToRotationAxisAngle(const quat& a) {
    float t   = a[3]*a[3];
    float rsa = t<0.99999999? 1.f/sqrt(1.f-t): 1.f;
    return vec4(a[0]*rsa,a[1]*rsa,a[2]*rsa,acos(a[3])*2.f);
  }
  inline quat quatFromRotationAxisAngle(const vec4& a) {
    float s = sin(a[3]/2.f);
    float c = cos(a[3]/2.f);
    return quat(a[0]*s,a[1]*s,a[2]*s,c);
  }
  inline mtx44 quatToRotationMatrix(const quat& a) {
    float xx=a[0]*a[0];
    float xy=a[0]*a[1], yy=a[1]*a[1];
    float xz=a[0]*a[2], yz=a[1]*a[2], zz=a[2]*a[2];
    float xw=a[0]*a[3], yw=a[1]*a[3], zw=a[2]*a[3], ww=a[3]*a[3];
    return mtx44(
        vec4(+xx-yy-zz+ww, +xy+zw+xy+zw, +xz-yw+xz-yw, 0),
        vec4(+xy-zw+xy-zw, -xx+yy-zz+ww, +yz+xw+yz+xw, 0),
        vec4(+xz+yw+xz+yw, +yz-xw+yz-xw, -xx-yy+zz+ww, 0),
        vec4(0           , 0           , 0           , 1)
        );
  }
  inline quat quatFromRotationMatrix(const mtx44& m) {
    float w=0.5*sqrt(m[0][0]+m[1][1]+m[2][2]+m[3][3]);
    float s=0.25/w;
    return quat(
        (m[1][2]-m[2][1])*s,
        (m[2][0]-m[0][2])*s,
        (m[0][1]-m[1][0])*s,
        w
        );
  }
}
}

#endif