This section of the archives stores flipcode's complete Developer Toolbox collection, featuring a variety of mini-articles and source code contributions from our readers.

| Vector Math & Quaternions Submitted by

The description is part of the code listing.

Download Associated File: vquats.cpp (7,344 bytes)

    Editor's note:
    COTD Entry:  Vector Math & Quaternions by Tim Sweeney [tim@epicgames.com]

    #include <math.h>
    #pragma warning (disable:4244)

    /*
    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++) b+=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 = 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
    	);
    }

The zip file viewer built into the Developer Toolbox made use of the zlib library, as well as the zlibdll source additions.

Copyright 1999-2008 (C) FLIPCODE.COM and/or the original content author(s). All rights reserved.
Please read our Terms, Conditions, and Privacy information.