3d Math Classes; Vector3, Quaternion, Euler Angle

[zeid]

A series of 3d maths classes.

Code: Select all

#ifndef _MATHCOMMON_H
#define _MATHCOMMON_H

#include <math.h>

namespace mathCommon
{
	const float kPi = 3.14159265f;
	const float k2Pi = kPi * 2.0f;
	const float kPiOver2 = kPi / 2.0f;
	const float k1OverPi = 1.0f / kPi;
	const float k1Over2Pi = 1.0f / k2Pi;

	//"wrap" an angle between -PI and PI
	template <typename T>
	T wrapPi(T theta)
	{
		theta+=kPi;
		theta-=floor(theta*k1OverPi)*k2Pi;
		theta-=kPi;
		return theta;
	}

	//"safe" inverse trig function
	template <typename T>
	T safeAcos(T x)
	{
		//if input is outside of acceptable valid range return nearest valid value
		if(x<=-1.0f)
			return kPi;
		if(x>=1.0f)
			return 0.0f;

		return acos(x);
	}

	template <typename T>
	T radianToDegree(T r)
	{
		return r*(180.0f/kPi);
	}

	template <typename T>
	T degreeToRadian(T d)
	{
		return d*(kPi/180.0f);
	}
}

#endif

Code: Select all

/*
*  MatrixStack.h
*
*
*  Created by Sam Zeid.
*  Copyright 2010 *. All rights reserved.
*
*/

#ifndef _MATRIXSTACK_H
#define _MATRIXSTACK_H

#include "Matrix.h"
#include <stack>

using namespace std;

static stack<Matrixd> _mstack;

class MatrixStack
{
public:
	static void initialise()
	{_mstack.push(Matrixd());}
};

static void pushMatrix()
{_mstack.push(Matrixd(_mstack.top()));}

static void popMatrix()
{if(_mstack.size()>1)_mstack.pop();}

//T R A N S L A T I O N   F U N C T I O N S
template<typename T>
static void translate(T X, T Y, T Z)
{
	Matrixd m;
	m.translate(X,Y,Z);
	_mstack.top()*=m;
}

template<typename T>
static void translate(Vector3<T> V)
{
	Matrixd m;
	m.translate(V);
	_mstack.top()*=m;
}

//R O T A T I O N   F U N C T I O N S

//rotate around the x then y then z axis
template<typename T>
static void rotate(T X, T Y, T Z)
{
	Matrixd m;
	m.rotate(x,y,z);
	_mstack.top()*=m;
}

//rotate around a specified axis
template<typename T>
static void rotate(Vector3<T> axis, T angle)
{
	Matrixd m;
	m.rotate(axis,angle);
	_mstack.top()*=m;
}

//rotate around the x axis
template<typename T>
static void rotateX(T X)
{
	Matrixd m;
	m.rotateX(X);
	_mstack.top()*=m;
}

//rotate around the y axis
template<typename T>
static void rotateY(T Y)
{
	Matrixd m;
	m.rotateY(Y);
	_mstack.top()*=m;
}

//rotate around the z axis
template<typename T>
static void rotateZ(T Z)
{
	Matrixd m;
	m.rotateZ(Z);
	_mstack.top()*=m;
}

//S C A L E   F U N C T I O N S

//scale the x, y and/or z components
template<typename T>
static void scale(T X, T Y, T Z)
{
	Matrixd m;
	m.scale(X,Y,Z);
	_mstack.top()*=m;
}

//scale by a uniform value
template<typename T>
static void scale(T S)
{
	Matrixd m;
	m.scale(S);
	_mstack.top()*=m;
}

#endif

Code: Select all

/*
*  Matrix.h
*
*
*  Created by Sam Zeid.
*  Copyright 2010 *. All rights reserved.
*
*/

#ifndef _MATRIX_H
#define _MATRIX_H

#include "MathCommon.h"
#include "Vector3.h"
#include <string.h>
#include <assert.h>

using namespace mathCommon;

template<typename T>
class Matrix
{
private:
	//the array representing an identity matrix.
	static const T i[16];
	
	//the 16 values of the 4x4 matrix.
	T m[16];

public:
	//create as an identity matrix.
	Matrix()
	{memcpy(m,i,sizeof(m));}

	//create from an array.
	Matrix(const T (&M)[16])
	{memcpy(m,M,sizeof(m));}

	//S P E C I A L   F U N C T I O N S   A N D   O P E R A T O R S
	
	//re-create from an array.
	void operator()(const T (&M)[16])
	{memcpy(m,M,sizeof(m));}

	//access individual elements in the matrix for input or output.
	T &operator()(const int C, const int R)
	{return m[(R*4)+C];}

	//make the matrix equal another.
	const Matrix<T> &operator=(const Matrix<T> &M)
	{
		memcpy(m,M.m,sizeof(m));
		return *this;
	}

	//resets the matrix to an identity matrix.
	void identity()
	{memcpy(m,i,sizeof(m));}

	//translation of the matrix by individual components.
	void translate(T X, T Y, T Z)
	{
		m[3]=X;
		m[7]=Y;
		m[11]=Z;
	}
	
	//translation of the matrix by a vector.
	void translate(const Vector3<T> &V)
	{
		m[3]=V.x;
		m[7]=V.y;
		m[11]=V.z;
	}

	//scaling of the matrix by individual components.
	void scale(const T X, const T Y, const T Z)
	{
		m[0]=X;
		m[5]=Y;
		m[10]=Z;
	}

	//scaling of the matrix by a uniform value.
	void scale(T uniform)
	{
		m[0]=uniform;
		m[5]=uniform;
		m[10]=uniform;
	}

	//rotation of the matrix using euler angles.
	void rotate(const T X, const T Y, const T Z)
	{
		Matrix _matrix;
		_matrix.rotateX(X);
		(*this)*=_matrix;
		_matrix.identity();
		_matrix.rotateY(Y);
		(*this)*=_matrix;
		_matrix.identity();
		_matrix.rotateZ(Z);
		(*this)*=_matrix;
	}

	//about an arbitrary axis.
	void rotate(const Vector3<T> &V, const T S)
	{
		T sine=sin(degreeToRadian(S));
		T cosine=cos(degreeToRadian(S));
		T tangent=1-cosine;

		Vector3<T> _axis=V/V.magnitude();
		
		T x=_axis.x;
		T y=_axis.y;
		T z=_axis.z;

		(*this)(0,0)=tangent*x*x+cosine;
		(*this)(1,0)=tangent*y*x+sine*z;
		(*this)(2,0)=tangent*z*x-sine*y;

		(*this)(0,1)=tangent*x*y-sine*z;
		(*this)(1,1)=tangent*y*y+cosine;
		(*this)(2,1)=tangent*z*y+sine*x;

		(*this)(0,2)=tangent*x*z+sine*y;
		(*this)(1,2)=tangent*y*z-sine*x;
		(*this)(2,2)=tangent*z*z+cosine;
	}
	
	//about the x axis.
	void rotateX(const T S)
	{
		m[5]=cos(degreeToRadian(S));
		m[6]=sin(degreeToRadian(S));
		m[9]=-sin(degreeToRadian(S));
		m[10]=cos(degreeToRadian(S));
	}

	//about the y axis.
	void rotateY(const T S)
	{
		m[0]=cos(degreeToRadian(S));
		m[2]=-sin(degreeToRadian(S));
		m[8]=sin(degreeToRadian(S));
		m[10]=cos(degreeToRadian(S));
	}

	//about the z axis.
	void rotateZ(const T S)
	{
		m[0]=cos(degreeToRadian(S));
		m[1]=sin(degreeToRadian(S));
		m[4]=-sin(degreeToRadian(S));
		m[5]=cos(degreeToRadian(S));
	}

	//C O M P A R I T I V E   O P E R A T O R S

	//returns if the matrices are equal.
	bool operator==(const Matrix<T> &M) const
	{
		return !memcmp(m,M.m,sizeof(m));
	}

	//returns if the matrices are not equal.
	bool operator!=(const Matrix<T> &M) const
	{
		return bool(memcmp(m,M.m,sizeof(m)));
	}

	//A R I T H M E T I C   O P E R A T O R S

	//multiply two matrices.
	const Matrix<T> operator*(const Matrix<T> &M) const
	{
		Matrix result;
		for(int c=0;c<4;c++)
			for(int r=0;r<4;r++)
				result(c,r)=
				m[c] * M.m[r*4] + 
				m[c+4] * M.m[1+(r*4)] +
				m[c+8] * M.m[2+(r*4)] +
				m[c+12] * M.m[3+(r*4)];
		return result;
	}

	//multiply by another matrix.
	const Matrix<T> &operator*=(const Matrix<T> &M)
	{
		(*this)=(*this)*M;
		return *this;
	}

	//multiply by a vector.
	const Vector3<T> operator*(const Vector3<T> &V)
	{
		return Vector3<T>
			(V.x*m[0] + V.y*m[1] + V.z*m[2] + m[3]
			,V.x*m[4] + V.y*m[5] + V.z*m[6] + m[7]
			,V.x*m[8] + V.y*m[9] + V.z*m[10] + m[11]);
	}
};

//the identity matrix for a 4x4 matrix
template<typename T>
const T Matrix<T>::i[16]=
{
	1.0, 0.0, 0.0, 0.0,
	0.0, 1.0, 0.0, 0.0,
	0.0, 0.0, 1.0, 0.0,
	0.0, 0.0, 0.0, 1.0
};

typedef Matrix<float> Matrixf;
typedef Matrix<double> Matrixd;
typedef Matrix<long double> Matrixl;

#endif

Code: Select all

/*
*  Quaternion.h
*
*
*  Created by Sam Zeid.
*  Copyright 2010 *. All rights reserved.
*
*/

#ifndef _QUATERNION_H
#define _QUATERNION_H

#include "Vector3.h"
#include "Matrix.h"
#include <assert.h>

template<typename T>
class Quaternion
{
public:
	//the imaginary components and real component of the quaternion.
	Vector3<float> v;
	float w;

	//defaults to an identity quaternion.
	Quaternion()
		:v(0.0f,0.0f,0.0f)
		,w(1.0f)
	{}

	//pass numbers individually.
	Quaternion(const T X, const T Y, const T Z, const T W)
		:v(X,Y,Z)
		,w(W)
	{}

	//pass a vector for imaginary numbers.
	Quaternion(const Vector3<T> &XYZ, const T W)
		:v(XYZ)
		,w(W)
	{}

	//create a quaternion from a matrix orientation.
	Quaternion(Matrix &M)
	{
		v.x=sqrt(max<T>(0,1+M(0,0)-M(1,1)-M(2,2)))/2;
		v.y=sqrt(max<T>(0,1-M(0,0)+M(1,1)-M(2,2)))/2;
		v.z=sqrt(max<T>(0,1-M(0,0)-M(1,1)+M(2,2)))/2;
		w=sqrt(max<T>(0,1+M(0,0)+M(1,1)+M(2,2)))/2;
		v.x=_copysign(v.x,M(1,2)-M(2,1));
		v.y=_copysign(v.y,M(2,0)-M(0,2));
		v.z=_copysign(v.z,M(0,1)-M(1,0));
	}

	//construct the quaternion from a euler angle.
	Quaternion(const Vector3<T> &E)
	{
		float cosX = cosf(0.5f*E.x);
		float cosY = cosf(0.5f*E.y);
		float cosZ = cosf(0.5f*E.z);

		float sinX = sinf(0.5f*E.x);
		float sinY = sinf(0.5f*E.y);
		float sinZ = sinf(0.5f*E.z);
		
		v.x = cosZ*cosY*sinX - sinZ*sinY*cosX;
		v.y = cosZ*sinY*cosX + sinZ*cosY*sinX;
		v.z = sinZ*cosY*cosX - cosZ*sinY*sinX;
		w   = cosZ*cosY*cosX + sinZ*sinY*sinX;
	}

	//construct the quaternion from a euler angle.
	Quaternion(const T X, const T Y, const T Z)
	{
		float cosX = cosf(0.5f*X);
		float cosY = cosf(0.5f*Y);
		float cosZ = cosf(0.5f*Z);

		float sinX = sinf(0.5f*X);
		float sinY = sinf(0.5f*Y);
		float sinZ = sinf(0.5f*Z);
		
		v.x = cosZ*cosY*sinX - sinZ*sinY*cosX;
		v.y = cosZ*sinY*cosX + sinZ*cosY*sinX;
		v.z = sinZ*cosY*cosX - cosZ*sinY*sinX;
		w   = cosZ*cosY*cosX + sinZ*sinY*sinX;
	}

	//S P E C I A L   F U N C T I O N S   A N D   O P E R A T O R S
	
	//set values of the quaternion individually.
	void operator()(const T X, const T Y, const T Z, const T W)
	{v(X,Y,Z);w=W;}

	//set values of the quaternion with a vector.
	void operator()(const Vector3<T> &V, const T W)
	{v=V;w=W;}

	//returns the length of the quaternion.
	T magnitude()
	{return sqrt(v.lengthSquared()+w*w);}

	//normalises the quaternion.
	void normalise()
	{
		T length = magnitude();
		if(length>0.0)
		{
			v/=length;
			w/=length;
		}
		else
		{
			//the normalisation has failed.
			assert(false);
			identity();
		}
	}

	//set the quaternion to an identity quaternion.
	void identity()
	{
		v(0.0,0.0,0.0);
		w=1.0;
	}

	//reverses the rotation of this quaternion.
	void conjugate()
	{v=-v;}

	//rotate V by this quaternion.
	Vector3<T> rotate(const Vector3<T> &V) const
	{   
		Quaternion<T> vector(V,0.0f);
		Quaternion<T> q(*this);
		q.conjugate();
		return (*this * vector * q).v;
	}

	//A R I T H M E T I C   O P E R A T O R S

	//multiply two quaternions and return the result.
	Quaternion operator*(const Quaternion<T> &rightHandSide) const
	{
		return Quaternion<T>
		(v.y*rightHandSide.v.z-v.z*rightHandSide.v.y+w*rightHandSide.v.x+v.x*rightHandSide.w
		,v.z*rightHandSide.v.x-v.x*rightHandSide.v.z+w*rightHandSide.v.y+v.y*rightHandSide.w
		,v.x*rightHandSide.v.y-v.y*rightHandSide.v.x+w*rightHandSide.v.z+v.z*rightHandSide.w
		,w*rightHandSide.w-v.x*rightHandSide.v.x-v.y*rightHandSide.v.y-v.z*rightHandSide.v.z);
	}

	//multiply this quaternion with another.
	Quaternion &operator*=(const Quaternion<T> &rightHandSide)	
	{
		*this=*this * rightHandSide;
		return *this;
	}

	//C O N V E R S I O N   O P E R A T O R S

	//creates an equivelant euler angle from the quaternion.
	//the angle is given in radian form.
	operator Vector3<T>() const
	{
		float sqw = w*w;
		float sqx = v.x*v.x;
		float sqy = v.y*v.y;
		float sqz = v.z*v.z;
		
		return Vector3<T>
					(
					atan2f(2.0f * (v.z*v.y + v.x*w), 1.0f-2.0f *(sqx + sqy)),
					asinf(-2.0f * (v.x*v.z - v.y*w)),
					atan2f(2.0f * (v.x*v.y + v.z*w), 1.0f-2.0f * (sqy + sqz))
					);
	}

	//cast quaternion to a matrix.
	template<typename T>
	operator Matrix() const
	{
		T _m[16]={1-2*(v.y*v.y+v.z*v.z), 2*(v.x*v.y-w*v.z), 2*(v.x*v.z+w*v.y), 0.0
			,2*(v.x*v.y+w*v.z), 1-2*(v.x*v.x+v.z*v.z), 2*(v.y*v.z-w*v.x), 0.0
			,2*(v.x*v.z-w*v.y), 2*(v.y*v.z+w*v.x), 1-2*(v.x*v.x+v.y*v.y), 0.0
			,0.0, 0.0, 0.0, 1.0};
		return Matrix<T>(_m);
	}

};

//N O N - M E M B E R   F U N C T I O N S

//the dot product returns the cosine of the angle between either quaternion.
template<typename T>
float dot(const Quaternion<T> &q1, const Quaternion<T> &q2)
{return q1.w*q2.w + q1.v.x*q2.v.x + q1.v.y*q2.v.y + q1.v.z*q2.v.z;}

//spherical linear interpolation,
//two quaternions are passed and an orientation between the two is returned
//T is a scalar between 0 and 1:
//a value of 0 would result in the equivelant of q1 being returned,
//while a value of 1 would result in the equivelant of q2 being returned,
//a range between would result in an orientation between the two
template<typename T>
Quaternion<T> slerp(const Quaternion<T> &q1, const Quaternion<T> &q2, T t)
{
	if(t<=0.0)	return q1;
	if(t>=1.0) return q2;

	T cosine=dot(q1,q2);

	//if the resulting dot product is a negative, we inverse the quaternion
	//and the cosine. Negative of all quaternion values returns the same angle,
	//all quaternian angles have exactly 2 representations. +v,+w == -v,-w
	Quaternion<T> q3=q2;
	if(cosine<0.0)
	{
		q3.v=-q3.v;
		q3.w=-q3.w;
		cosine=-cosine;
	}

	//assuming both are unit quaternions there should be no error
	assert(cosine<=1.0);

	T k1,k2;
	if(cosine>0.999)
	{
		k1=1.0-t;
		k2=t;
	}
	else
	{
		//compute the sine from the cosine
		T sine=sqrt(1.0f - cosine*cosine);
		//compute the angle
		T angle=atan2(sine, cosine);
		
		T oneOverSine=1.0f/sine;
		// Compute interpolation parameters
		k1=sin((1.0f-t)*angle)*oneOverSine;
		k2=sin(t*angle)*oneOverSine;
	}
	// Interpolate
	return Quaternion<T>
		(k1*q1.v.x + k2*q3.v.x
		,k1*q1.v.y + k2*q3.v.y
		,k1*q1.v.z + k2*q3.v.z
		,k1*q1.w + k2*q3.w);
}

typedef Quaternion<float> Quaternionf;
typedef Quaternion<double> Quaterniond;
typedef Quaternion<long double> Quaternionl;

#endif

Code: Select all

/*
*  Vector3.h
*
*
*  Created by Sam Zeid.
*  Copyright 2010 *. All rights reserved.
*
*/

#ifndef _VECTOR3_H
#define _VECTOR3_H

#include <math.h>

template<typename T>

class Vector3
{
public:
	//the cartesian coordinates that are stored for a vector.
	T x,y,z;

	//if a vector is declared by default its values are 0.
	Vector3():x(0),y(0),z(0)
	{}

	//the user can define the vector coordinates upon creation.
	Vector3(T X, T Y, T Z):x(X),y(Y),z(Z)
	{}

	//S P E C I A L   F U N C T I O N S   A N D   O P E R A T O R S

	//sets values of the vector.
	void operator()(const T X, const T Y, const T Z)
	{x=X;y=Y;z=Z;}

	//returns the vectors length.
	T magnitude() const
	{return sqrt(lengthSquared());}

	T lengthSquared() const
	{return x*x+y*y+z*z;}

	//normalises the vector (makes it's magnitude 1).
	const Vector3<T> normalise()
	{
		T n=magnitude();
		if(n!=0){x/=n;y/=n;z/=n;}
		return *this;
	}

	//C O M P A R I T I V E   O P E R A T O R S

	//tests if a vector is equal to another.
	bool operator==(const Vector3<T> &rightHandSide) const
	{return (x==rightHandSide.x && y==rightHandSide.y && z==rightHandSide.z);}

	//tests if a vector is not equal to another.
	bool operator!=(const Vector3<T> &rightHandSide) const
	{return (x!=rightHandSide.x || y!=rightHandSide.y || z!=rightHandSide.z);}

	//A R I T H M E T I C   O P E R A T O R S

	//makes a vector equal to another.
	const Vector3<T> &operator=(const Vector3<T> &rightHandSide)
	{
		x=rightHandSide.x;y=rightHandSide.y;z=rightHandSide.z;
		return *this; //the resulting vector is returned
	}

	//return the negative of a vector.
	const Vector3<T> operator-(void) const
	{return Vector3<T>(-x, -y, -z);}

	//may also be used in subtraction.
	const Vector3<T> operator-(const Vector3<T> &rightHandSide) const
	{return Vector3<T>(x-rightHandSide.x, y-rightHandSide.y, z-rightHandSide.z);}

	//returns the addition of two vectors.
	const Vector3<T> operator+(const Vector3<T> &rightHandSide) const
	{return Vector3<T>(x+rightHandSide.x, y+rightHandSide.y, z+rightHandSide.z);}

	//scales a vector by a uniform value.
	const Vector3<T> operator*(T S) const
	{return Vector3<T>(x*S, y*S, z*S);}

	//scales a vector by a uniform value.
	const Vector3<T> operator/(T S) const
	{return Vector3<T>(x/S, y/S, z/S);}

	//adds a vector to this vector.
	const Vector3<T> &operator+=(const Vector3<T> &rightHandSide)
	{
		x+=rightHandSide.x;y+=rightHandSide.y;z+=rightHandSide.z;
		return *this;
	}

	//subtracts a vector from this vector.
	const Vector3<T> &operator-=(const Vector3<T> &rightHandSide)
	{
		x-=rightHandSide.x;y-=rightHandSide.y;z-=rightHandSide.z;
		return *this;
	}

	//scales this vector by a uniform value.
	const Vector3<T> &operator*=(T S)
	{
		x*=S;y*=S;z*=S;
		return *this;
	}

	//scales this vector by a uniform value.
	const Vector3<T> &operator/=(T S)
	{
		x/=S;y/=S;z/=S;
		return *this;
	}
};

//N O N - M E M B E R   F U N C T I O N S

//returns the dot product of two vectors as a scalar.
template<typename T>
T dot(const Vector3<T> &v1, const Vector3<T> &v2)
{return ((v1.x*v2.x)+(v1.y*v2.y)+(v1.z*v2.z));}

//returns the cross product of two vectors.
template<typename T>
const Vector3<T> cross(const Vector3<T> &v1, const Vector3<T> &v2)
{return Vector3<T>((v1.y*v2.z)-(v1.z*v2.y), (v1.z*v2.x)-(v1.x*v2.z), (v1.x*v2.y)-(v1.y*v2.x));}

typedef Vector3<float> Vector3f;
typedef Vector3<double> Vector3d;
typedef Vector3<long double> Vector3l;

#endif