Quaternions For Better And More Flexible 3D Rotation

Quaternions.  I’m sure you’ve heard of them if you’re a graphics programmer pants like me.  It’s interesting to see them used in this way, but it makes sense.  Basically, when you add additional dimensions to a spatial math problem, it becomes simpler, because the viewpoint is broader.  This is true for anything from M-Theory (11 dimensions, whoa!) which I firmly believe is true (because of how it boils everything down to the underlying component of all physics – harmonic motion,)  to Z-transforms in electrical engineering.  All of these simplify, purify, and ease problems by letting you see the whole picture.  The broader your viewpoint, the simpler connections become, because you aren’t bogged down by being stuck in the smaller pictures for any time at all.

A quaternion is a non-commutative extension of the complex numbers, and is effectively a 4-dimensional vector.  This vector is a normed division algebra over the real numbers, which means it is a division algebra existing in normed vector space, as in all space is normalized to the unit.  Division algebra is an algebra in a field where we can do division.  So, once again, we have the power of complex numbers colliding with the simplicity of real numbers to create something awesome.  Remember that a fourth dimension gives us a broader and more flexible vantage point for manipulation.  It’s worth mentioning that quaternions were a stroke of genius by Sir William Hamilton, who came up with this defining equality for this particular algebra space:  i^2=j^2=k^2=ijk=-1 .

So, because quaternions exist purely in normed space, they aren’t terribly useful for scaling or translation.  But here’s what we CAN do: ROTATION.  And we can do it very flexibly.  The primary problems with Euler angles instead are pretty simple.

Euler angles are susceptible to gimbal lock for starters.  For example, assume a level sensing platform on an aircraft flying due north has its three axes mutually at right angles, i.e., Roll, Pitch and Yaw angles each zero. If the aircraft pitches up 90 degrees, the plane’s and platform’s Roll axes become parallel to the Yaw axis, and changes about Yaw can no longer be compensated for.  Yeah, I lifted that example straight out of Wikipedia, what’re you gonna do, shoot me?  But the other problem is that when you concatenate rotations, unless you do a few hairy space transforms, your new Euler set (added to the old) is simply affecting the axis angles at their current values, so any new rotations are locked to the local axes of rotation.  Sucks cock, doesn’t it?  If you watch it, it’s so constrained looking.  Because it IS.  Isn’t it so much more intuitive to rotate something from it’s current orientation as though the axes are globally oriented all the time?  Well, duh yes.

For this we will use only the subset of unit quaternions.  Now, for each rotation, there are two different representations.  This is because all quaternions have a conjugate.  This table here should give you some idea of the relationships, plus a thing at the bottom of the table that shows you what the relationship is.

Simple Rotation Table

Given Rotation	Quaternions (simplified)
Identity.	1, -1
PI rad (halfway) around x-axis.	i, -i
PI rad (halfway) around y-axis.	j, -j
PI rad (halfway) around z-axis.	k, -k
theta around axis (unit vector) n.	+/- [cos(theta/2) + n sin(theta/2)] plus or minus sign creates a set of two as a duality, much like the conjugates above.

Now let’s have a look at how to put this in to action.

Our quaternion will have x, y, z, and w values, representing all four dimensions of space.

Three-dimensional rotations can be thought of as a 3-dimensional surface sitting in 4-dimensional hyperspace.  The surface is non-orientable.  This problem can be solved with double-cover.  We simply duplicate each rotation; one duplicate is positive, and the other is negative.  This gives us a 3-dimensional sphere in 4-dimensional hyperspace.  It completely avoids all singularities, (because of double-cover,) and is therefore fully orientable with no problems at any time.

Now, instead of wasting your time with derivations, let’s dive right in to the math.  First let’s get the data structures out of the way.

Data Structures

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>

#ifndef MATH_PI
#	ifndef M_PI
#		define MATH_PI 3.14159f
#	else
#		define MATH_PI M_PI
#	endif
#endif

/* Data structure for a 4x4/3x3 matrix. */
typedef struct RatMathMatrix4
{
	float mat00,mat10,mat20,mat30;
	float mat01,mat11,mat21,mat31;
	float mat02,mat12,mat22,mat32;
	float mat03,mat13,mat23,mat33;
} RatMathMatrix4;

/* Data structure for a 3D vector */
typedef struct RatMathVector3
{
	float x;
	float y;
	float z;
} RatMathVector3;

/* Data structure for a Quaternion, which is really a 4D vector */
typedef struct RatMathQuaternion
{
	float x;
	float y;
	float z;
	float w;
} RatMathQuaternion;

/* An euler is expressed in the same terms as a vector so need for another type. */
typedef RatMathVector3 RatMathEuler;

As you can see, we will be using vectors and matrices as well, though the math in those is not covered here, as there is a limit to what can be discussed in a single article.  I’ll briefly describe each operation and post a function that performs it.

Operations

This is the normalize function.  It scales the quaternion in to a unit 4-vector.

void RatMathQuaternionNormalize(RatMathQuaternion *quat)
{
	register float mag2=quat->w*quat->w+quat->x*quat->x+quat->y*quat->y+quat->z*quat->z;
	
	if (fabs(mag2-1.0f))
	{
		register float mag=sqrtf(mag2);
		quat->w/=mag;
		quat->x/=mag;
		quat->y/=mag;
		quat->z/=mag;
	}
}

This is the identity function.  It just writes the identity quaternion: x=0,y=0,z=0,w=1.

void RatMathQuaternionWriteIdentity(RatMathQuaternion *quat)
{
	quat->x=0.0f;
	quat->y=0.0f;
	quat->z=0.0f;
	quat->w=1.0f;
}

This is the multiplication function.  It is NOT commutative, since it effectively concatinates rotations.  Try one experiment in applying consecutive rotations and you will see that the result is different for the order you apply them in.

void RatMathQuaternionMult(RatMathQuaternion *quat0,RatMathQuaternion *quat1)
{
	RatMathQuaternion result;

	result.x=quat0->w*quat1->x+quat0->x*quat1->w+quat0->y*quat1->z-quat0->z*quat1->y;
	result.y=quat0->w*quat1->y+quat0->y*quat1->w+quat0->z*quat1->x-quat0->x*quat1->z;
	result.z=quat0->w*quat1->z+quat0->z*quat1->w+quat0->x*quat1->y-quat0->y*quat1->x;
	result.w=quat0->w*quat1->w-quat0->x*quat1->x-quat0->y*quat1->y-quat0->z*quat1->z;
	
	memcpy(quat0,&result,sizeof(RatMathQuaternion));
}

This is the conjugate function.  What it does is it inverts the complex axes, giving you the negative or opposite of the quaternion like I discussed before.

RatMathQuaternion *RatMathQuaternionGetConjugate(RatMathQuaternion *quat)
{
	RatMathQuaternion *newquat=(RatMathQuaternion *)malloc(sizeof(RatMathQuaternion));

	newquat->x=-quat->x;
	newquat->y=-quat->y;
	newquat->z=-quat->z;
	newquat->w=quat->w;

	return newquat;
}

This is the SLerp function.  SLerp stands for spherical linear interpolation.  It basically lets you interpolate between two quaternion represented orientations around the great circle, that is to say, the shortest possible rotation.  This gives you a smooth and natural transition between two orientations.

// this is a zero threshold; less than this is interpreted in SLerp as 0
#define EPSILON 0.00001f

RatMathQuaternion *RatMathQuaternionSlerp(RatMathQuaternion *quat0,RatMathQuaternion *quat1,float t)
{
	float cosineom,sineom,scalar_x,scalar_y,scalar_z,scalar_w;
	float omega,scale0,scale1;

	RatMathQuaternion *newquat=(RatMathQuaternion *)malloc(sizeof(RatMathQuaternion));

	if ((fabs(quat0->x-quat1->x)<EPSILON)&&(fabs(quat0->y-quat1->y)<EPSILON)&&
	   (fabs(quat0->z-quat1->z)<EPSILON)&&(fabs(quat0->w-quat1->w)<EPSILON))
	{
		newquat->x=quat1->x;
		newquat->y=quat1->y;
		newquat->z=quat1->z;
		newquat->w=quat1->w;
		return newquat;
	}

	cosineom=quat0->x*quat1->x+quat0->y*quat1->y+quat0->z*quat1->z+quat0->w*quat1->w;

	if (cosineom<=0)
	{
		cosineom=-cosineom;
		scalar_x=-quat1->x;
		scalar_y=-quat1->y;
		scalar_z=-quat1->z;
		scalar_w=-quat1->w;
	}
	else
	{
		scalar_x=quat1->x;
		scalar_y=quat1->y;
		scalar_z=quat1->z;
		scalar_w=quat1->w;
	}

	if ((1.0f-cosineom)>EPSILON) 
	{	// do this whole SLerp thing
		omega=acosf(cosineom);
		sineom=sinf(omega);
		scale0=sinf((1.0f-t)*omega)/sineom;
		scale1=sinf(t*omega)/sineom;
	}
	else
	{	// close enough to do a straight up linear interpolation
		scale0=1.0f-t;
		scale1=t;
	}

	newquat->x=scale0*quat0->x+scale1*scalar_x;
	newquat->y=scale0*quat0->y+scale1*scalar_y;
	newquat->z=scale0*quat0->z+scale1*scalar_z;
	newquat->w=scale0*quat0->w+scale1*scalar_w;

	return newquat;
}

Conversion From Quaternions

Quaternion to Transform Matrix.

RatMathMatrix4 *RatMathMatrix4CreateFromQuaternion(RatMathQuaternion *quat)
{
	float xx=quat->x*quat->x,yy=quat->y*quat->y,zz=quat->z*quat->z;
	float xy=quat->x*quat->y,xz=quat->x*quat->z,yz=quat->y*quat->z;
	float wx=quat->w*quat->x,wy=quat->w*quat->y,wz=quat->w*quat->z;

	RatMathMatrix4 *newmat=(RatMathMatrix4 *)malloc(sizeof(RatMathMatrix4));

	RatMathMatrix4WriteIdentity(newmat);
	newmat->mat00=1.0f-2.0f*(yy+zz);
	newmat->mat01=2.0f*(xy-wz);
	newmat->mat02=2.0f*(xz+wy);
	newmat->mat10=2.0f*(xy+wz);
	newmat->mat11=1.0f-2.0f*(xx+zz);
	newmat->mat12=2.0f*(yz-wx);
	newmat->mat20=2.0f*(xz-wy);
	newmat->mat21=2.0f*(yz+wx);
	newmat->mat22=1.0f-2.0f*(xx+yy);
	newmat->mat33=1.0f;

	return newmat;
}

Quaternion to Vector (simple truncation to axis w/o rotation).

RatMathVector3 *RatMathVector3CreateFromQuaternion(RatMathQuaternion *quat)
{
	RatMathVector3 *vec=(RatMathVector3 *)malloc(sizeof(RatMathVector3));

	vec->x=quat->x;
	vec->y=quat->y;
	vec->z=quat->z;

	return vec;
}

Conversion To Quaternions

Quaternion from Vector (simple extension with w).

RatMathQuaternion *RatMathQuaternionCreateFromVector3(RatMathVector3 *vec,float w)
{
	RatMathQuaternion *quat=(RatMathQuaternion *)malloc(sizeof(RatMathQuaternion));

	quat->x=vec->x;
	quat->y=vec->y;
	quat->z=vec->z;
	quat->w=w;

	return quat;
}

Quaternion from Euler Angles.

RatMathQuaternion *RatMathQuaternionCreateFromEuler(RatMathEuler *euler)
{
	float p=euler->y;
	float y=euler->x;
	float r=euler->z;
 
	float sinp=sinf(p);
	float siny=sinf(y);
	float sinr=sinf(r);
	float cosp=cosf(p);
	float cosy=cosf(y);
	float cosr=cosf(r);
	
	RatMathQuaternion *quat=(RatMathQuaternion *)malloc(sizeof(RatMathQuaternion));
 
	quat->x=sinr*cosp*cosy-cosr*sinp*siny;
	quat->y=cosr*sinp*cosy+sinr*cosp*siny;
	quat->z=cosr*cosp*siny-sinr*sinp*cosy;
	quat->w=cosr*cosp*cosy+sinr*sinp*siny;
 
	RatMathQuaternionNormalize(quat);
	
	return quat;
}

Quaternion from Axis/Angle.

RatMathQuaternion *RatMathQuaternionCreateFromAxisAngle(RatMathVector3 *axis,float angle)
{
	RatMathQuaternion *quat=(RatMathQuaternion *)malloc(sizeof(RatMathQuaternion));

	float sin_angle;
	RatMathVector3 naxis;
	angle*=0.5f;
	
	memcpy(&naxis,axis,sizeof(RatMathVector3));
	RatMathVector3Normalize(&naxis);
 	sin_angle=sinf(angle);
 
	quat->x=naxis.x*sin_angle;
	quat->y=naxis.y*sin_angle;
	quat->z=naxis.z*sin_angle;
	quat->w=cosf(angle);

	return quat;
}

Euler Angles / Matrix Conversion

Euler Angles from Transform Matrix (rotation only).

RatMathEuler *RatMathEulerCreateFromMatrix4(RatMathMatrix4 *mat)
{
	RatMathEuler *neweul=(RatMathEuler *)malloc(sizeof(RatMathEuler));

	neweul->x=atan2f(mat->mat21,sqrt(mat->mat20*mat->mat20+mat->mat22*mat->mat22));
	neweul->y=atan2f(mat->mat20,mat->mat22);
	neweul->z=atan2f(mat->mat01,mat->mat11);

	return neweul;
}

Rotate Transform Matrix by Euler Angles.

void RatMathMatrix4Rotate(RatMathMatrix4 *mat,RatMathEuler *eul)
{
	float cos_ang,sin_ang;
	float m00,m10;
	float m01,m11;
	float m02,m12;

	/* do yaw */
	cos_ang=cosf(eul->y);
	sin_ang=sinf(eul->y);

	m00=mat->mat00*cos_ang+mat->mat20*-sin_ang;
	m01=mat->mat01*cos_ang+mat->mat21*-sin_ang;
	m02=mat->mat02*cos_ang+mat->mat22*-sin_ang;

	mat->mat20=mat->mat00*sin_ang+mat->mat20*cos_ang;
	mat->mat21=mat->mat01*sin_ang+mat->mat21*cos_ang;
	mat->mat22=mat->mat02*sin_ang+mat->mat22*cos_ang;

	mat->mat00=m00;
	mat->mat01=m01;
	mat->mat02=m02;

	/* do pitch */
	cos_ang=cosf(eul->x);
	sin_ang=sinf(eul->x);

	m10=mat->mat10*cos_ang+mat->mat20*sin_ang;
	m11=mat->mat11*cos_ang+mat->mat21*sin_ang;
	m12=mat->mat12*cos_ang+mat->mat22*sin_ang;

	mat->mat20=mat->mat10*-sin_ang+mat->mat20*cos_ang;
	mat->mat21=mat->mat11*-sin_ang+mat->mat21*cos_ang;
	mat->mat22=mat->mat12*-sin_ang+mat->mat22*cos_ang;

	mat->mat10=m10;
	mat->mat11=m11;
	mat->mat12=m12;

	/* do roll */
	cos_ang=cosf(eul->z);
	sin_ang=sinf(eul->z);

	m00=mat->mat00*cos_ang+mat->mat10*sin_ang;
	m01=mat->mat01*cos_ang+mat->mat11*sin_ang;
	m02=mat->mat02*cos_ang+mat->mat12*sin_ang;

	mat->mat10=mat->mat00*-sin_ang+mat->mat10*cos_ang;
	mat->mat11=mat->mat01*-sin_ang+mat->mat11*cos_ang;
	mat->mat12=mat->mat02*-sin_ang+mat->mat12*cos_ang;

	mat->mat00=m00;
	mat->mat01=m01;
	mat->mat02=m02;
}

Peace.