See what's going on with flipcode!




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.