Add to del.icio.usAdd to DiggAdd to TechnoratiAdd to RedditAdd to FurlAdd to StumbleUponAdd to Slashdot
SUBSCRIBE




Quaternion C++ Class

Quaternions are commonly used to represent orientations and rotations in 3d games. They are more efficient than using 3d matrices in number of operations, storage and quaternions also avoid gimbal lock. You can download the entire math package here or visit my downloadable code page. 
/****************************************
 * Quaternion class
 * By Will Perone
 * Original: 12-09-2003  
 * Revised:  27-09-2003
 *           22-11-2003
 *           10-12-2003
 *           15-01-2004
 *           16-04-2004
 *           29-07-2011   added corrections from website,
 *           22-12-2011   added correction to *= operator, thanks Steve Rogers
 * 
 * Notes:  
 * if |q|=1 then q is a unit quaternion
 * if q=(0,v) then q is a pure quaternion 
 * if |q|=1 then q conjugate = q inverse
 * if |q|=1 then q= [cos(angle), u*sin(angle)] where u is a unit vector 
 * q and -q represent the same rotation 
 * q*q.conjugate = (q.length_squared, 0) 
 * ln(cos(theta),sin(theta)*v)= ln(e^(theta*v))= (0, theta*v)
 ****************************************/

#pragma once 

#include "matrix4.h" 
#include "assert.h" 


struct quaternion
{
	union {
		struct {
			float    s; //!< the real component
			vector3f v; //!< the imaginary components
		};
		struct { float elem[4]; }; //! the raw elements of the quaternion
	};


	//! constructors
	quaternion() {}
	quaternion(float real, float x, float y, float z): s(real), v(x,y,z) {}
	quaternion(float real, const vector3f &i): s(real), v(i) {}

	//! from 3 euler angles
	quaternion(float theta_z, float theta_y, float theta_x)
	{
		float cos_z_2 = cosf(0.5*theta_z);
		float cos_y_2 = cosf(0.5*theta_y);
		float cos_x_2 = cosf(0.5*theta_x);

		float sin_z_2 = sinf(0.5*theta_z);
		float sin_y_2 = sinf(0.5*theta_y);
		float sin_x_2 = sinf(0.5*theta_x);

		// and now compute quaternion
		s   = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2;
		v.x = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2;
		v.y = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2;
		v.z = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2;

	}
	
	//! from 3 euler angles 
	quaternion(const vector3f &angles)
	{	
		float cos_z_2 = cosf(0.5*angles.z);
		float cos_y_2 = cosf(0.5*angles.y);
		float cos_x_2 = cosf(0.5*angles.x);

		float sin_z_2 = sinf(0.5*angles.z);
		float sin_y_2 = sinf(0.5*angles.y);
		float sin_x_2 = sinf(0.5*angles.x);

		// and now compute quaternion
		s   = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2;
		v.x = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2;
		v.y = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2;
		v.z = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2;		
	} 
		
	//! basic operations
	quaternion &operator =(const quaternion &q)		
	{ s= q.s; v= q.v; return *this; }

	const quaternion operator +(const quaternion &q) const	
	{ return quaternion(s+q.s, v+q.v); }

	const quaternion operator -(const quaternion &q) const	
	{ return quaternion(s-q.s, v-q.v); }

	const quaternion operator *(const quaternion &q) const	
	{	return quaternion(s*q.s - v*q.v,
				  v.y*q.v.z - v.z*q.v.y + s*q.v.x + v.x*q.s,
				  v.z*q.v.x - v.x*q.v.z + s*q.v.y + v.y*q.s,
				  v.x*q.v.y - v.y*q.v.x + s*q.v.z + v.z*q.s);
	}

	const quaternion operator /(const quaternion &q) const	
	{
		quaternion p(q); 
		p.invert(); 
		return *this * p;
	}

	const quaternion operator *(float scale) const
	{ return quaternion(s*scale,v*scale); }

	const quaternion operator /(float scale) const
	{ return quaternion(s/scale,v/scale); }

	const quaternion operator -() const
	{ return quaternion(-s, -v); }
	
	const quaternion &operator +=(const quaternion &q)		
	{ v+=q.v; s+=q.s; return *this; }

	const quaternion &operator -=(const quaternion &q)		
	{ v-=q.v; s-=q.s; return *this; }

	const quaternion &operator *=(const quaternion &q)		
	{			
		float x= v.x, y= v.y, z= v.z, sn= s*q.s - v*q.v;
		v.x= y*q.v.z - z*q.v.y + s*q.v.x + x*q.s;
		v.y= z*q.v.x - x*q.v.z + s*q.v.y + y*q.s;
		v.z= x*q.v.y - y*q.v.x + s*q.v.z + z*q.s;
		s= sn;
		return *this;
	}
	
	const quaternion &operator *= (float scale)			
	{ v*=scale; s*=scale; return *this; }

	const quaternion &operator /= (float scale)			
	{ v/=scale; s/=scale; return *this; }
	

	//! gets the length of this quaternion
	float length() const
	{ return (float)sqrt(s*s + v*v); }

	//! gets the squared length of this quaternion
	float length_squared() const
	{ return (float)(s*s + v*v); }

	//! normalizes this quaternion
	void normalize()
	{ *this/=length(); }

	//! returns the normalized version of this quaternion
	quaternion normalized() const
	{ return  *this/length(); }

	//! computes the conjugate of this quaternion
	void conjugate()
	{ v=-v; }

	//! inverts this quaternion
	void invert()
	{ conjugate(); *this/=length_squared(); }
	
	//! returns the logarithm of a quaternion = v*a where q = [cos(a),v*sin(a)]
	quaternion log() const
	{
		float a = (float)acos(s);
		float sina = (float)sin(a);
		quaternion ret;

		ret.s = 0;
		if (sina > 0)
		{
			ret.v.x = a*v.x/sina;
			ret.v.y = a*v.y/sina;
			ret.v.z = a*v.z/sina;
		} else {
			ret.v.x= ret.v.y= ret.v.z= 0;
		}
		return ret;
	}

	//! returns e^quaternion = exp(v*a) = [cos(a),vsin(a)]
	quaternion exp() const
	{
		float a = (float)v.length();
		float sina = (float)sin(a);
		float cosa = (float)cos(a);
		quaternion ret;

		ret.s = cosa;
		if (a > 0)
		{
			ret.v.x = sina * v.x / a;
			ret.v.y = sina * v.y / a;
			ret.v.z = sina * v.z / a;
		} else {
			ret.v.x = ret.v.y = ret.v.z = 0;
		}
		return ret;
	}

	//! casting to a 4x4 isomorphic matrix for right multiplication with vector
	operator matrix4() const
	{			
		return matrix4(s,  -v.x, -v.y,-v.z,
				v.x,  s,  -v.z, v.y,
				v.y, v.z,    s,-v.x,
				v.z,-v.y,  v.x,   s);
	}
	
	//! casting to 3x3 rotation matrix
	operator matrix3() const
	{
		Assert(length() > 0.9999 && length() < 1.0001, "quaternion is not normalized");		
		return matrix3(1-2*(v.y*v.y+v.z*v.z), 2*(v.x*v.y-s*v.z),   2*(v.x*v.z+s*v.y),   
				2*(v.x*v.y+s*v.z),   1-2*(v.x*v.x+v.z*v.z), 2*(v.y*v.z-s*v.x),   
				2*(v.x*v.z-s*v.y),   2*(v.y*v.z+s*v.x),   1-2*(v.x*v.x+v.y*v.y));
	}

	//! computes the dot product of 2 quaternions
	static inline float dot(const quaternion &q1, const quaternion &q2) 
	{ return q1.v*q2.v + q1.s*q2.s; }

	//! linear quaternion interpolation
	static quaternion lerp(const quaternion &q1, const quaternion &q2, float t) 
	{ return (q1*(1-t) + q2*t).normalized(); }

	//! spherical linear interpolation
	static quaternion slerp(const quaternion &q1, const quaternion &q2, float t) 
	{
		quaternion q3;
		float dot = quaternion::dot(q1, q2);

		/*	dot = cos(theta)
			if (dot < 0), q1 and q2 are more than 90 degrees apart,
			so we can invert one to reduce spinning	*/
		if (dot < 0)
		{
			dot = -dot;
			q3 = -q2;
		} else q3 = q2;
		
		if (dot < 0.95f)
		{
			float angle = acosf(dot);
			return (q1*sinf(angle*(1-t)) + q3*sinf(angle*t))/sinf(angle);
		} else // if the angle is small, use linear interpolation								
			return lerp(q1,q3,t);		
	}

	//! This version of slerp, used by squad, does not check for theta > 90.
	static quaternion slerpNoInvert(const quaternion &q1, const quaternion &q2, float t) 
	{
		float dot = quaternion::dot(q1, q2);

		if (dot > -0.95f && dot < 0.95f)
		{
			float angle = acosf(dot);			
			return (q1*sinf(angle*(1-t)) + q2*sinf(angle*t))/sinf(angle);
		} else  // if the angle is small, use linear interpolation								
			return lerp(q1,q2,t);			
	}

	//! spherical cubic interpolation
	static quaternion squad(const quaternion &q1,const quaternion &q2,const quaternion &a,const quaternion &b,float t)
	{
		quaternion c= slerpNoInvert(q1,q2,t),
			       d= slerpNoInvert(a,b,t);		
		return slerpNoInvert(c,d,2*t*(1-t));
	}

	//! Shoemake-Bezier interpolation using De Castlejau algorithm
	static quaternion bezier(const quaternion &q1,const quaternion &q2,const quaternion &a,const quaternion &b,float t)
	{
		// level 1
		quaternion q11= slerpNoInvert(q1,a,t),
				q12= slerpNoInvert(a,b,t),
				q13= slerpNoInvert(b,q2,t);		
		// level 2 and 3
		return slerpNoInvert(slerpNoInvert(q11,q12,t), slerpNoInvert(q12,q13,t), t);
	}

	//! Given 3 quaternions, qn-1,qn and qn+1, calculate a control point to be used in spline interpolation
	static quaternion spline(const quaternion &qnm1,const quaternion &qn,const quaternion &qnp1)
	{
		quaternion qni(qn.s, -qn.v);	
		return qn * (( (qni*qnm1).log()+(qni*qnp1).log() )/-4).exp();
	}

	//! converts from a normalized axis - angle pair rotation to a quaternion
	static inline quaternion from_axis_angle(const vector3f &axis, float angle)
	{ return quaternion(cosf(angle/2), axis*sinf(angle/2)); }

	//! returns the axis and angle of this unit quaternion
	void to_axis_angle(vector3f &axis, float &angle) const
	{
		angle = acosf(s);

		// pre-compute to save time
		float sinf_theta_inv = 1.0/sinf(angle);

		// now the vector
		axis.x = v.x*sinf_theta_inv;
		axis.y = v.y*sinf_theta_inv;
		axis.z = v.z*sinf_theta_inv;

		// multiply by 2
		angle*=2;
	}

	//! rotates v by this quaternion (quaternion must be unit)
	vector3f rotate(const vector3f &v)
	{   
		quaternion V(0, v);
		quaternion conjugate(*this);
		conjugate.conjugate();
		return (*this * V * conjugate).v;
	}

	//! returns the euler angles from a rotation quaternion
	vector3f euler_angles(bool homogenous=true) const
	{
		float sqw = s*s;    
		float sqx = v.x*v.x;    
		float sqy = v.y*v.y;    
		float sqz = v.z*v.z;    

		vector3f euler;
		if (homogenous) {
			euler.x = atan2f(2.f * (v.x*v.y + v.z*s), sqx - sqy - sqz + sqw);    		
			euler.y = asinf(-2.f * (v.x*v.z - v.y*s));
			euler.z = atan2f(2.f * (v.y*v.z + v.x*s), -sqx - sqy + sqz + sqw);    
		} else {
			euler.x = atan2f(2.f * (v.z*v.y + v.x*s), 1 - 2*(sqx + sqy));
			euler.y = asinf(-2.f * (v.x*v.z - v.y*s));
			euler.z = atan2f(2.f * (v.x*v.y + v.z*s), 1 - 2*(sqy + sqz));
		}
		return euler;
	}
};
22 Comments
Your Name 0 0
In the Quaternion constructor, shouldn't the divide by half be on the inside of the trig? i.e., sin(theta *.5f)?
Will 0 0
good catch.
rsyndrome 0 0
I think operator*= doesn't work properly as you need to store previously a copy of v. Indeed new y and z components may be invalid since v.x has already been altered.
Will 0 0
another good catch, thanks!
Abtin 0 0
Hey,

I think there are some problem in computing euler angle from quaternion. I changed it to code below and it worked. All came from wikipedia page:
http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles

euler.x = atan2f(2.f * (v.z*v.y + v.x*s), 1 - 2* (sqx + sqy));
euler.y = asinf(-2.f * (v.x*v.z - v.y*s));
euler.z = atan2f(2.f * (v.x*v.y + v.z*s), 1 - 2* (sqy + sqz));
jon 0 0
what's the point of having non-member inline functions static? An inline function has internal linkage, so static does nothing.
Will 0 0
They are member functions.
Dan 2011/04/060 0
Thanks for this. It helped me do a c++ assignment designing user controlled bezier curves using quaternions.
luneart 2011/04/220 0
@ Abtin:
look at the url you gave !
your way is the inhomogeneous way, Will used the homogeneous one.
Hans 2011/06/260 0
You have not applied the above corrections in your downloadable package yet.
Will 2011/06/290 0
Thanks Hans, didn't think people were still downloading my package; I'll correct that.
Susie 2011/07/08 Contact Me0 0
Great hammer of Thor, that is powerfully hefulpl!
Nelle 2011/07/09 Contact Me0 0
This ifnormaiton is off the hizool!
E 2011/07/210 0
I think some revision control is in order. The version shown on this page is different from the ones in your downloadable zip files. Also the last revision date (16-04-2004) must be wrong given the dates of the comments above.
Will 2011/07/220 0
Yes I know, I should get with the times and put it in some sort of public repo.
Will 2011/07/29 Contact Me0 0
Thank for all your feedback, I've updated the class above and the source code zip file with the latest corrections.
Jake 2011/09/09 Contact Me0 0
I can't seem to compile this in ubuntu. Is it portable?
Will 2011/09/090 0
Could be a difference in how gcc interprets some stuff like the templates, I haven't tried compiling it in gcc, feedback would be great.
Jake 2011/09/09 Contact Me0 0
here are some examples of errors it spit out. The full list is too long to reproduce here. First it complains that a bunch of files are missing. But that's easy to fix, because they're just capitalized differently e.g.

#include "matrix3.h"

but the file is called Matrix3.h . More difficult to fix are errors like

vector3.h: In member function 'void vector3<T>::operator()(T, T, T)':
vector3.h:53: error: 'x' was not declared in this scope

or

utility.h:137: error: ISO C++ forbids declaration of 'interp_lin' with no type
Jake 2011/09/09 Contact Me0 0
If you want, I can give you the whole error list
Will 2011/09/09 Contact Me0 0
send me the whole error list to my email, I'll take a look. It's been some years since I worked on this codebase but might find something.
Will 2011/12/230 0
Just a note, I've updated the Quaternion class with a correction to the *= operator. The downloadable zip file has also been updated.
<- for private contact