Math tools

Matrix.hpp

#pragma once


//Row Major templated matrix class.

#define _USE_MATH_DEFINES
#include <math.h>
#include <iostream>
#include <vector>
#include <stdexcept>
#include <algorithm>

template <class T>
class Matrix
{
public:
	explicit Matrix(int r = 0, int c = 0, T init_with = T())
		: rows_( r < 1 ? 4 : r), columns_( c < 1 ? 4 : c ),
		data_( rows_ * columns_, init_with ) {}

	template <typename U>
	explicit Matrix(Matrix<U> const &other)
		: rows_( other.rows() ), columns_( other.columns() ),
		data_( &*other, &*other+(rows_*columns_) ) {}

	//Allows data to be access as a RxC matrix such as mat(x,y)
	T &operator()(int row, int column) { return data_[column+(columns_*row)]; }
	//Allows data to be access as a RxC matrix such as mat(x,y)
	T const &operator()(int row, int column) const { return data_[column+(columns_*row)]; }

	//Returns number of rows
	unsigned int rows() const { return rows_; }
	//Returns number of columns
	unsigned int columns() const { return columns_; }
	//Access the data as a pointer
	operator T*() { return data_.empty() ? (T*)NULL : &data_[0]; }
	//Access the data as a pointer
	operator T const*() const { return data_.empty() ? 0 : &data_[0]; }

	Matrix &operator+=(const Matrix& other)
	{
		if ( rows() != other.rows() || columns() != other.columns() ) {
			throw std::invalid_argument("Unable to add: Matrices must have same dimensions.");
		}

		std::transform( data_.begin(), data_.end(),
			other.data_.begin(),
			data_.begin(),
			std::plus<T>() );
		return *this;
	}
	Matrix &operator-=(const Matrix& other) 
	{
		if ( rows() != other.rows() || columns() != other.columns() ) {
			throw std::invalid_argument("Unable to subtract: Matrices must have same dimensions.");
		}

		std::transform( data_.begin(), data_.end(),
			other.data_.begin(),
			data_.begin(),
			std::minus<T>() );
		return *this;
	}
	template <class U>
	friend const Matrix<U> operator*(const Matrix<U>& one, const Matrix<U>& two);
	Matrix &operator*=(const Matrix& other)
	{
		return *this = *this * other;
	}
	Matrix &operator*=(T const &scale)
	{
		std::transform( data_.begin(), data_.end(),
			data_.begin(),
			std::bind2nd( std::multiplies<T>(), scale ) );
		return *this;
	}
	Matrix &operator/=(T const &scale)
	{
		std::transform( data_.begin(), data_.end(),
			data_.begin(),
			std::bind2nd( std::divides<T>(), scale ) );
		return *this;
	}
	Matrix &operator+=(T const &add)
	{
		std::transform( data_.begin(), data_.end(),
			data_.begin(),
			std::bind2nd( std::plus<T>(), add ) );
		return *this;
	}
	Matrix &operator-=(T const &sub)
	{
		std::transform( data_.begin(), data_.end(),
			data_.begin(),
			std::bind2nd( std::minus<T>(), sub ) );
		return *this;
	}
	//Number of elements in the matrix
	std::size_t Count() const
	{
		return data_.size();
	}
	//Size of the matrix in bytes
	std::size_t Size() const
	{
		return data_.size()*sizeof(T);
	}
	//Loads identity matrix
	//Matrix size must be that of a square.
	void LoadIdentity() const
	{
		if ( rows() != columns() )
		{
			throw std::invalid_argument("Identity matrice only works on squared matrices.");
		}

		for (unsigned int i=0; i<rows(); i++)
		{
			for (unsigned int j=0; j<columns(); j++) 
			{
				i==j ? (operator()( i, j ) = static_cast<T>(1.0)) : 0;
			}
		}
	}
	//Returned the transposed matrix such that rows become columns
	//Useful for openGL
	Matrix<T> Transpose() const
	{
		if ( rows() != columns() )
		{
			throw std::invalid_argument("Transposition can only occur for squared matrices");
		}
		Matrix<T> trans(rows(),columns());
		for ( unsigned int i = 0 ; i < rows(); i++)
		{
			for ( unsigned int j = 0; j < columns(); j++)
			{

				trans(j,i) = operator()(i,j);
			}
		}
		return trans;
	}
	//Returns the cofactor of the matrix
	Matrix<T> Cofactor() const
	{
		if ( rows() != columns() )
		{
			throw std::invalid_argument("Transposition can only occur for squared matrices");
		}
		Matrix<T> result(rows(),columns());
		switch(rows())
		{
		case 1:
			{
				break;
			}
		case 2:
			{
				result(0,0) = operator()(1,1);
				result(0,1) = -operator()(1,0);
				result(1,0) = -operator()(0,1);
				result(1,1) = operator()(0,0);
				break;
			}
		default:
			{
				std::vector<std::vector<Matrix<T>>>temp;//oh god.
				temp.resize(rows());
				for(unsigned int i = 0; i < rows(); i++)
				{
					temp[i].resize(columns());
				}
				for (unsigned int i = 0; i < rows(); i ++)
				{
					for (unsigned int j = 0; j < columns(); j ++)
					{
						temp[i][j] = Matrix<T>(rows()-1,columns()-1);
					}
				}
				for(unsigned int k1 = 0; k1 < rows(); k1++)
				{
					for(unsigned int k2 = 0; k2 < rows(); k2++)
					{
						unsigned int i1 = 0;
						for(unsigned int i = 0; i < rows(); i++)
						{
							unsigned int j1 = 0;
							for(unsigned int j = 0; j < rows(); j++)
							{
								if ( k1 == i || k2 == j)
								{
									continue;
								}
								(temp[k1][k2])(i1,j1++) = operator()(i,j);
							}
							if (k1 != i)
							{
								i1++;
							}
						}
					}
				}

				bool sign = true;
				for(unsigned int k1 = 0; k1 < rows(); k1++)
				{
					sign = ((k1 % 2) == 0);
					for(unsigned int k2 = 0; k2 < rows(); k2++)
					{
						if(sign == true)
						{
							result(k1,k2) = -temp[k1][k2].Determinant();
							sign = false;
						}
						else
						{
							result(k1,k2) = temp[k1][k2].Determinant();
							sign = true;
						}
					}
				}
				break;
			}
		}
		return result;
	}
	//Returns the adjugate of the matrix.
	Matrix<T> Adjugate() const
	{
		if ( rows() != columns() )
		{
			throw std::invalid_argument("Transposition can only occur for squared matrices");
		}
		return Cofactor().Transpose();
	}
	//Returns the inverse of the matrix
	Matrix<T> Inverse() const
	{
		T d = Determinant();
		if ( std::abs(d) < std::numeric_limits<T>::epsilon())
		{
			throw std::runtime_error("Inverse matrix does not exist.");
		}
		return Adjugate() / Determinant();
	}
	//Returns the determinate of the matrix
	T Determinant() const
	{
		if ( rows() != columns() )
		{
			throw std::invalid_argument("Determinant can only be calculated"
				"on sqared matrices");
		}
		T result = static_cast<T>(0);
		switch(rows())
		{

		case 1:
			{
				result = operator()(0,0);
				break;
			}
		case 2:
			{
				result = operator()(0,0) * operator()(1,1) - operator()(0,1) * operator()(1,0);
				break;
			}
		default:
			{
				Matrix<T> recur(rows()-1,columns()-1);
				for(unsigned int count = 0; count < rows(); count++)
				{
					unsigned int icount = 0;
					for (unsigned int i = 1; i < rows(); i++)
					{
						unsigned int jcount = 0;
						for (unsigned int j = 0; j < rows(); j++)
						{
							if ( j == count) continue;
							recur(icount,jcount) = operator()(i,j);
							jcount++;
						}
						icount++;
					}
					result += pow(-1,count) * operator()(0,count) * recur.Determinant();
				}
				break;
			}
		}
		return result;
	}
	//Returns the trace of the matrix
	T trace() const
	{
		if ( rows() != columns() )
		{
			throw std::invalid_argument("Transposition can only occur for squared matrices");
		}
		T result = (T)0;
		for (unsigned int i = 0; i < rows(); i++)
		{
			result += operator()(i,i);
		}
		return result;
	}
	template <typename U>
	Matrix &operator=(Matrix<U> const &other)
	{
		// note that the assign needs to be done first for exception safety.
		data_.assign( other.data_.begin(), other.data_.end() );
		rows_ = other.rows_;
		columns_ = other.columns_;
		return *this;
	}
private:
	unsigned int rows_;
	unsigned int columns_;
	std::vector<T> data_;
};
template <class T>
const Matrix<T> operator+(Matrix<T> one, Matrix<T> const &two) 
{
	return one += two;
}
template <class T>
const Matrix<T> operator-(Matrix<T> one, Matrix<T> const &two) 
{
	return one -= two;
}
template <class T>
const Matrix<T> operator*(const Matrix<T>& one, const Matrix<T>& two) 
{
	if ( one.columns() != two.rows() ) {
		throw std::invalid_argument("Matrices can only be multiplied if the first has"
			" exactly as many columns as the second has rows.");
	}

	Matrix<T> result( one.rows(), two.columns() );

	for (unsigned int i=0; i<result.rows(); i++)
		for (unsigned int j=0; j<result.columns(); j++)
			for (unsigned int a=0; a<one.columns(); a++)
				result(i,j) += one(i,a) * two(a,j);

	return result;
}
template <class T>
const Matrix<T> operator*(Matrix<T> one, T const &factor)
{
	return one *= factor;
}
template <class T>
const Matrix<T> operator*(T const &factor, Matrix<T> one) 
{
	return one *= factor;
}
template <class T>
const Matrix<T> operator+(Matrix<T> one, T const &add)
{
	return one += add;
}
template <class T>
const Matrix<T> operator+(T const &add, Matrix<T> one) 
{
	return one += add;
}
template <class T>
const Matrix<T> operator-(Matrix<T> one, T const &sub)
{
	return one += sub;
}
template <class T>
const Matrix<T> operator-(T const &sub, Matrix<T> one) 
{
	return one += sub;
}
template <class T>
const Matrix<T> operator/(Matrix<T> one, T const &factor) 
{
	return one /= factor;
}
template <typename T, typename U>
bool operator==(Matrix<T> const &one, Matrix<U> const &two) 
{
	return (one.rows() == two.rows()
		&& one.columns() == two.columns()
		&& std::equal( &*one, &*one + one.rows()*one.columns(),
		&*two ));
}
template <typename T, typename U>
bool operator!=(Matrix<T> const &one, Matrix<U> const &two) 
{
	return !( one == two );
}
template<class T>
std::ostream& operator<<(std::ostream& os, const Matrix<T>& mat) 
{
	for (unsigned int i=0; i< mat.rows(); i++) {
		os << "[";
		for (unsigned int j=0; j<mat.columns(); j++) {
			os << mat(i,j);
			if ( j+1 < mat.columns() )
				os << "\t";
		}
		os << "]" << std::endl;
	}
	return os;
}




//Remember, column major so you must do Vertex * Matrix and not Matrix * Vertex.
//In openGL if you do not transpose you must do the same as well in GLSL.
//Really this should be a 3x3 matrix to allow for translations but vectors
//can simply be added to each other to save memory and to allow for packed vertex arrays
//without the additional W value.
template <class T>
class Matrix2D : public Matrix<T>
{
public:
	Matrix2D() : Matrix<T>(3,3)
	{
	}
	//Returns a matrix used for scaling.
	static const Matrix2D<T> Scale(T factor)
	{
		Matrix2D<T> temp;
		temp.LoadIdentity();
		temp(0,0) = factor;
		temp(1,1) = factor;
		return temp;
	}
	//Returns a matrix used for scaling.
	static const Matrix2D<T> Scale(T xfactor, T yfactor)
	{
		Matrix2D<T> temp;
		temp.LoadIdentity();
		temp(0,0) = xfactor;
		temp(1,1) = yfactor;
		return temp;
	}
	//Returns a matrix used for rotation.
	//Counter clock-wise with the X axis being 0 degrees.
	static const Matrix2D<T>  RotateAng(T angle)
	{
		//Just convert it to rads
		return RotateRad(angle*(M_PI/180));
	}
	//Returns a matrix used for rotation.
	//Counter clock-wise with the X axis being 0 radians.
	static const Matrix2D<T>  RotateRad(T radian)
	{
		Matrix2D<T> temp;
		temp.LoadIdentity();
		temp(0,0) = cos(radian);
		temp(0,1) = sin(radian);
		temp(1,0) = -sin(radian);
		temp(1,1) = cos(radian);
		return temp;
	}
	//Returns a matrix used for translation
	static const Matrix2D<T>  Translate(T x,T y)
	{
		Matrix2D<T> temp;
		temp.LoadIdentity();
		temp(2,0) = x;
		temp(2,1) = y;
		return temp;
	}
	~Matrix2D()
	{
	}
};

typedef Matrix2D<double> Matrix2Dd;
typedef Matrix2D<float> Matrix2Df;
typedef Matrix2D<int> Matrix2Di;
typedef Matrix2D<unsigned int> Matrix2Dui;

//Remember, column major so you must do Vertex * Matrix and not Matrix * Vertex.
//In openGL if you do not transpose you must do the same as well in GLSL.
//Really this should be a 4x4 matrix to allow for translations but vectors
//can simply be added to each other to save memory and to allow for packed vertex arrays
//without the additional W value.
template <class T>
class Matrix3D : public Matrix<T>
{
public:
	Matrix3D() : Matrix<T>(4,4)
	{
	}
	//Returns a matrix used for scaling.
	static const Matrix3D<T> Scale(T factor)
	{
		Matrix3D<T> temp;
		temp.LoadIdentity();
		temp(0,0) = factor;
		temp(1,1) = factor;
		temp(2,2) = factor;
		return temp;
	}
	//Returns a matrix used for scaling.
	static const Matrix3D<T> Scale(T xfactor, T yfactor, T zfactor)
	{
		Matrix3D<T> temp;
		temp.LoadIdentity();
		temp(0,0) = xfactor;
		temp(1,1) = yfactor;
		temp(2,2) = zfactor;
		return temp;
	}
	//---------------------------
	//Rotations
	//---------------------------
	//Returns a matrix used for X axis rotation.
	static const Matrix3D<T>  RotateXAng(T angle)
	{
		//Just convert it to rads
		return rotateXRad(angle*(M_PI/180));
	}
	//Returns a matrix used for Z axis rotation.
	static const Matrix3D<T>  RotateXRad(T radian)
	{

		Matrix3D<T> temp;
		temp.LoadIdentity();
		temp(1,1) = cos(radian);
		temp(1,2) = sin(radian);
		temp(2,1) = -sin(radian);
		temp(2,2) = cos(radian);
		return temp;
	}
	//Returns a matrix used for Y axis rotation.
	static const Matrix3D<T>  RotateYAng(T angle)
	{
		//Just convert it to rads
		return rotateZRad(angle*(M_PI/180));
	}
	//Returns a matrix used for Y axis rotation.
	static const Matrix3D<T>  RotateYRad(T radian)
	{
		Matrix3D<T> temp;
		temp.LoadIdentity();
		temp(0,0) = cos(radian);
		temp(0,2) = sin(radian);
		temp(2,0) = -sin(radian);
		temp(2,2) = cos(radian);
		return temp;
	}
	//Returns a matrix used for Z axis rotation.
	static const Matrix3D<T>  RotateZAng(T angle)
	{
		//Just convert it to rads
		return rotateZRad(angle*(M_PI/180));
	}
	//Returns a matrix used for Z axis rotation.
	static const Matrix3D<T>  RotateZRad(T radian)
	{
		Matrix3D<T> temp;
		temp.LoadIdentity();
		temp(0,0) = cos(radian);
		temp(0,1) = sin(radian);
		temp(1,0) = -sin(radian);
		temp(1,1) = cos(radian);
		return temp;
	}
	//Returns a matrix used for traslation
	static const Matrix3D<T> Translate(T x,T y, T z)
	{
		Matrix3D<T> temp;
		temp.LoadIdentity();
		temp(3,0) = x;
		temp(3,1) = y;
		temp(3,2) = z;
		return temp;
	}
	~Matrix3D()
	{
	}
};

typedef Matrix3D<double> Matrix3Dd;
typedef Matrix3D<float> Matrix3Df;
typedef Matrix3D<int> Matrix3Di;
typedef Matrix3D<unsigned int> Matrix3Dui;

Quaternion.hpp

#pragma once

#include <iostream>

#include "Matrix.hpp"
#include "Vector.hpp"

template<class T>
class Quaternion
{
public:
	T w,x,y,z;
	Quaternion(const T &w, const T &x, const T &y, const T &z): w(w), x(x), y(y), z(z) {};
	Quaternion(const T &x, const T &y, const T &z): w(T()), x(x), y(y), z(z) {};
	Quaternion(const Vector3D<T> &vec): w(T()), x(vec.x()), y(vec.y()), z(vec.z()) {};
	Quaternion(const T &real): w(r), x(T()), y(T()), z(T()) {};
	Quaternion(): w(T()), x(T()), y(T()), z(T()) {};
	//Creates quaternion from matrix
	Quaternion(const Matrix<T> &mat)
	{
		if ( mat.rows() == 3 && mat.columns() == 3 ||
			mat.rows() == 4 && mat.columns() == 4)
		{
			if ( mat.trace() > (T)0)
			{
				T S = 0.5 / sqrt(mat.trace());
				w = (T)0.25 / S;
				x = ( mat(2,2) - mat(1,2)) * S;
				y = ( mat(0,2) - mat(2,0)) * S;
				z = ( mat(1,0) - mat(0,1)) * S;
			}
			else
			{
				//find out which column has the greatest value.
				unsigned int i = 0;
				if ( mat(1,1) > mat(0,0))
				{
					i = 1;
				}
				if ( mat(2,2) > mat(1,1))
				{
					i = 2;
				}
				//depending on the largest column, calculate the quaternion
				switch (i)
				{
				case 0:
					{
						T S = mat.trace();
						x = (T)0.5 / S;
						y = ( mat(0,1) + mat(1,0) ) / S;
						z = ( mat(0,2) + mat(2,0) ) / S;
						w = ( mat (1,2) + mat(2,1) ) / S;
						break;
					}
				case 1:
					{
						T S = mat.trace();
						x = ( mat(0,1) + mat(1,0) ) / S;
						y = (T)0.5 / S;
						z = ( mat (1,2) + mat(2,1) ) / S;
						w = ( mat(0,2) + mat(2,0) ) / S;
						break;
					}
				case 2:
					{
						T S = mat.trace();
						x = ( mat(0,2) + mat(2,0) ) / S;
						y = ( mat (1,2) + mat(2,1) ) / S;
						z = (T)0.5 / S;
						w = ( mat(0,1) + mat(1,0) ) / S;
						break;
					}
				}
			}
		}
		else
		{
			throw std::invalid_argument("Only 3x3 and 4x4 matrices may be converted "
				"into quaternions.");
		}
	}
	//Creates a quaternion from the given axis and angle(in degrees)
	Quaternion(Vector3D<T> axis, T angle)
	{
		Quaternion<T> temp = AxisAngle(axis,angle);
		x = temp.x;
		y = temp.y;
		z = temp.z;
		w = temp.w;
	}
	Quaternion(const Quaternion<T> &quat): w(quat.w), x(quat.x), y(quat.y), z(quat.z) {};
	Quaternion<T>& operator=(const Quaternion &quat) 
	{
		w=quat.w; x=quat.x; y=quat.y; z=quat.z; return *this;
	}
	Quaternion<T> operator-() const 
	{
		return Quaternion<T>(-w, -x, -y, -z);
	}

	T Squared() const 
	{
		return (T)w*w + x*x + y*y + z*z;
	}
	T Norm() const
	{
		return (T)(w*w + x*x + y*y + z*z);
	}
	T Length() const
	{
		return ( sqrt( Norm() ) );
	}
	Quaternion<T> Normalized() const
	{
		return Quaternion<T>(w/Length(),x/Length(),y/Length(),z/Length());
	}
	Quaternion<T> Conjugate() const
	{
		return Quaternion<T>(w, -x, -y, -z);
	}
	Quaternion<T> Inverse() const
	{
		return Quaternion<T>(Conjugate() *=((T)1/Norm()));
	}
	T DotProduct(const Quaternion<T> & quat) const
	{
		return (w*quat.w + x*quat.x + y*quat.y + z*quat.z);
	}
	Quaternion<T> Exponent() const
	{
		Quaternion<T> quat;
		T theta = sqrt(Vector3D<T>(x,y,z).DotProduct(Vector3D<T>(x,y,z))),
			sinTheta = sin(theta);
		quat.w = cos(theta);
		if(std::abs(sinTheta) > (T)0)
		{
			quat.x = x/sinTheta*theta;
			quat.y = y/sinTheta*theta;
			quat.z = z/sinTheta*theta;
		}
		else
		{
			quat.x = x;
			quat.y = y;
			quat.z = z;
		}
		return quat;
	}
	Quaternion<T> Power(const T t) const
	{
		return Quaternion<T>((Log()*t).Exponent());
	}
	Quaternion<T> Log() const
	{
		Quaternion<T> quat;
		quat.w = 0;
		T theta = acos(w),sinTheta = sin(theta);

		if(std::abs(sinTheta) > 0)
		{
			quat.x = x/sinTheta*theta;
			quat.y = y/sinTheta*theta;
			quat.z = z/sinTheta*theta;
		}
		else
		{
			quat.x = x;
			quat.y = y;
			quat.z = z;
		}
		return quat;
	}
	Quaternion<T> Slerp(const Quaternion<T> &quat, const T t) const
	{
		if(DotProduct(quat) >= (T)0)
		{
			return (*this) * ((Inverse() * quat).Power(t));
		}
		else
		{
			return (*this) * ((Inverse()* (T)-1 * quat).Power(t));
		}
	}

	//Returns the quaternion as a rotation matrix
	Matrix3D<T> RotationMatrix() const
	{
		Matrix3D<T> result;
		result(0,0) = (T)1 - (T)2 * (y*y + z*z);
		result(0,1) = (T)2 * (x*y - z*w);
		result(0,2) = (T)2 * (x*z + y*w);

		result(1,0) = (T)2 * (x*z + y*w);
		result(1,1) = (T)1 - (T)2 * (x*x + z*z);
		result(1,2) = (T)2 * (y*z - x*w);

		result(2,0) = (T)2 * (x*z - y*w);
		result(2,1) = (T)2 * (y*z + x*w);
		result(2,2) = (T)1 - (T)2 * (x*x + y*y);

		result(0,3) = result(1,3) = result(2,3) = result(3,0) = result(3,1) = result(3,2) = (T)0;
		result(3,3) = 1;
		return result;
	}

	static const Quaternion<T> Euler(T pitch, T yaw, T roll)
	{
		Quaternion<T> qx, qy, qz;
		qx = AxisAngle(Vector3D<T>::Xaxis(),pitch);
		qy = AxisAngle(Vector3D<T>::Yaxis(),yaw);
		qz = AxisAngle(Vector3D<T>::Zaxis(),roll);
		return ((qx * qy) * qz);
	}
	static const Quaternion<T> AxisAngle(const Vector3D<T> &axis, T angle)
	{
		T sina = sin( angle*M_PI/180 / 2);
		T cosa = cos( angle*M_PI/180 / 2);
		Quaternion<T> temp;
		temp.x = axis.x() * sina;
		temp.y = axis.y() * sina;
		temp.z = axis.z() * sina;
		temp.w = cosa;
		temp = temp.Normalized();
		return temp;
	}
public:
	//operators
	Quaternion& operator+=(const T &real) 
	{ w += real; return *this; }
	Quaternion& operator+=(const Quaternion &quat) 
	{ w += quat.w; x += quat.x; y += quat.y; z += quat.z; return *this; }
	Quaternion& operator-=(const T &real) 
	{ w -= real; return *this; }
	Quaternion& operator-=(const Quaternion &quat) 
	{ w -= quat.w; x -= quat.x; y -= quat.y; z -= quat.z; return *this; }
	Quaternion& operator*=(const T &real) 
	{ w *= real; x *= real; y *= real; z *= real; return *this; }
	Quaternion& operator*=(const Quaternion &quat) 
	{
		Quaternion<T> old(*this);
		w = old.w*quat.w - old.x*quat.x - old.y*quat.y - old.z*quat.z;
		x = old.w*quat.x + old.x*quat.w + old.y*quat.z - old.z*quat.y;
		y = old.w*quat.y + old.y*quat.w + old.z*quat.x - old.x*quat.z;
		z = old.w*quat.z + old.z*quat.w + old.x*quat.y - old.y*quat.x;
		return *this;
	}
	Vector3D<T> operator*(const Vector3D<T> &vec)
	{
		return vec*(*this);
	}

	Quaternion operator+(const T &real) const { return Quaternion(*this) += real; }
	Quaternion operator+(const Quaternion &quat) const { return Quaternion(*this) += quat; }
	Quaternion operator-(const T &real) const { return Quaternion(*this) -= real; }
	Quaternion operator-(const Quaternion &quat) const { return Quaternion(*this) -= quat; }
	Quaternion operator*(const T &real) const { return Quaternion(*this) *= real; }
	Quaternion operator*(const Quaternion &quat) const { return Quaternion(*this) *= quat; }
	Quaternion operator/(const T &real) const { return Quaternion(*this) /= real; }
	Quaternion operator/(const Quaternion &quat) const { return Quaternion(*this) /= quat; }

	bool operator==(const Quaternion &quat) const 
	{ return (w == quat.w) && (x == quat.x) && (y == quat.y) && (z == quat.z); }
	bool operator!=(const Quaternion &quat) const { return !operator==(q); }

	template<class T> friend Quaternion<T> operator+(const T &real, const Quaternion<T> &quat);
	template<class T> friend Quaternion<T> operator-(const T &real, const Quaternion<T> &quat);
	template<class T> friend Quaternion<T> operator*(const T &real, const Quaternion<T> &quat);
	template<class T> friend Quaternion<T> operator/(const T &real, const Quaternion<T> &quat);

	template<class T> friend std::ostream& operator<<(std::ostream &os, const Quaternion<T> &quat);
};
// Friend functions need to be outside the actual class definition
template<class T>
Quaternion<T> operator+(const T &real, const Quaternion<T> &quat) 
{
	return quat+real;
}

template<class T>
Quaternion<T> operator-(const T &real, const Quaternion<T> &quat)
{
	return Quaternion<T>(real-quat.w, quat.x, quat.y, quat.z);
}

template<class T>
Quaternion<T> operator*(const T &real, const Quaternion<T> &quat) 
{
	return quat*real;
}

//Multiplying a vector and a quat returns the rotated vector
template<class T>
Vector3D<T> operator*(const Vector3D<T> &vec, const Quaternion<T> &quat) 
{
	Vector3D<T> vn(vec);
	//vn = vn.Unit();
	Quaternion<T> vecquat, resQuat;
	vecquat.x = vn.x();
	vecquat.y = vn.y();
	vecquat.z = vn.z();
	vecquat.w = (T)0;
	resQuat = vecquat * quat.Conjugate();
	resQuat = (quat) * resQuat;
	return (Vector3D<T>(resQuat.x,resQuat.y,resQuat.z));
}

template<class T>
Quaternion<T> operator/(const T &real, const Quaternion<T> &quat)
{
	T n(quat.Squared());
	return Quaternion<T>(real*quat.w/n, -real*quat.x/n, -real*quat.y/n, -real*quat.z/n);
}

template<class T>
std::ostream& operator<<(std::ostream &os, const Quaternion<T> &quat)
{ 
	os << "(";
	os << quat.w;
	(quat.x < T()) ? (os << " - " << (-quat.x) << "i") : (os << " + " << quat.x << "i");
	(quat.y < T()) ? (os << " - " << (-quat.y) << "j") : (os << " + " << quat.y << "j");
	(quat.z < T()) ? (os << " - " << (-quat.z) << "k") : (os << " + " << quat.z << "k");
	os << ")" << std::endl;
	return os;
}

typedef Quaternion<double> Quaterniond;
typedef Quaternion<float> Quaternionf;
typedef Quaternion<int> Quaternioni;
typedef Quaternion<unsigned int> Quaternionui;

Vector.hpp

#pragma once

#include "Matrix.hpp"

template <class T>
class Vector2D : public Matrix<T>
{
public:
	Vector2D() : Matrix<T>(1,3)
	{
		operator()(0,2) = 1;
	}
	Vector2D(T x, T y) : Matrix<T>(1,3)
	{
		operator()(0,0) = x;
		operator()(0,1) = y;
		operator()(0,2) = 1;
	}
	Vector2D(T angle) : Matrix<T>(1,3)
	{
		operator()(0,0) = (T)cos(angle);
		operator()(0,1) = (T)sin(angle);
		operator()(0,2) = 1;
	}
	const static Vector2D<T> Xaxis()
	{
		return Vector2D<T>(1,0);
	}
	const static Vector2D<T> Yaxis()
	{
		return Vector2D<T>(0,1);
	}

	T x() const { return operator()(0,0);}
	T& x() { return operator()(0,0);}
	T y() const { return operator()(0,1);}
	T& y() { return operator()(0,1);}
	T w() const { return operator()(0,2);}
	T& w() { return operator()(0,2);}

	Vector2D<T> operator- ()
	{
		return Vector2D<T>(-x(),-y());
	}
	//Returns vector length
	T Length() const
	{
		return sqrt(x()*x() + y()*y());
	}
	//Returns normalized vector
	Vector2D<T> Unit() const
	{
		Vector2D<T> temp(*this);
		temp.x() /= Length();
		temp.y() /= Length();
		return temp;
	}
	//Returns dot product of two vectors
	T DotProduct(Vector2D<T> other) const
	{
		return x()*other.x() + y()*other.y();
	}
	//Returns the vector Projected onto this one.
	Vector2D<T> Project(Vector2D<T> other) const
	{
		Vector2D<T> temp(*this);
		temp = temp.Unit();
		temp *= DotProduct(other);
		return temp;
	}
	//Returns the signed area of two vectors
	//divide this by two to get the area of a triangle.
	T Wedgeproduct(Vector2D<T> other) const
	{
		return x()*other.y() - y()*other.x();
	}
	//Returns the vector perpendicular to this one
	//It is preferred that it "points" to the "left" of the vector
	//so that the perpendicular of the X axis is the Y axis.
	Vector2D<T> Perpendicular() const
	{
		Vector2D<T> temp(*this);
		temp.x() = -y();
		temp.y() = x();
		return temp;
	}
	//Returns the current vector as an angle. 
	//Counter clock-wise with the x axis being angle 0
	T Angle() const
	{
		return (T)Radian()*180/M_PI;
	}
	//Returns the current vector as a radian. 
	//Counter clock-wise with the x axis being angle 0
	T Radian() const
	{
		return (T)atan2(y(),x());
	}

	//Returns the angle between this vector and another
	//Counter clock-wise with the x axis being angle 0
	T Angle(Vector2D<T> other) const
	{
		return (T)Radian(other)*180/M_PI;
	}
	//Returns the radian between this vector and another
	//Counter clock-wise with the x axis being angle 0
	T Radian(Vector2D<T> other) const
	{
		return (T)atan2(other.y(),other.x()) - (T)atan2(y(),x());
	}
	~Vector2D()
	{
	}

};


typedef Vector2D<double> Vector2Dd;
typedef Vector2D<float> Vector2Df;
typedef Vector2D<int> Vector2Di;
typedef Vector2D<unsigned int> Vector2Dui;


template <class T>
class Vector3D : public Matrix<T>
{
public:
	Vector3D() : Matrix<T>(1,4)
	{
		operator()(0,3) = 1;
	}
	Vector3D(T x, T y, T z) : Matrix<T>(1,4)
	{
		operator()(0,0) = x;
		operator()(0,1) = y;
		operator()(0,2) = z;
		operator()(0,3) = 1;
	}
	//Create vector from pitch and yaw angles(in degrees).
	//The X axis is the origin of all rotations mathematically
	Vector3D(T yaw,T pitch) : Matrix<T>(1,4)
	{
		operator()(0,0) = cos(pitch*M_PI/180)*cos(yaw*M_PI/180);
		operator()(0,1) = cos(pitch*M_PI/180)*sin(yaw*M_PI/180);
		operator()(0,2) = sin(pitch*M_PI/180);
		operator()(0,3) = 1;
	}
	const static Vector3D<T> Xaxis()
	{
		return Vector3D<T>(1,0,0);
	}
	const static Vector3D<T> Yaxis()
	{
		return Vector3D<T>(0,1,0);
	}
	const static Vector3D<T> Zaxis()
	{
		return Vector3D<T>(0,0,1);
	}
	T x() const { return operator()(0,0);}
	T& x() { return operator()(0,0);}
	T y() const { return operator()(0,1);}
	T& y() { return operator()(0,1);}
	T z() const { return operator()(0,2);}
	T& z() { return operator()(0,2);}
	T w() const { return operator()(0,3);}
	T& w() { return operator()(0,3);}
	Vector3D<T> operator- ()
	{
		return Vector3D<T>(-x(),-y(),-z());
	}
	//Returns vector length
	T Length() const
	{
		return sqrt(x()*x() + y()*y() + z()*z());
	}
	//Returns normalized vector
	Vector3D<T> Unit() const
	{
		Vector3D<T> temp(*this);
		temp.x() /= Length();
		temp.y() /= Length();
		temp.z() /= Length();
		return temp;
	}
	//Returns dot product of two vectors
	T DotProduct(Vector3D<T> other) const
	{
		return x()*other.x() + y()*other.y() + z()*other.z();
	}
	//Returns the cross product vector of this vector to another
	Vector3D<T> CrossProduct(Vector3D<T> other) const
	{
		Vector3D<T> temp(*this);
		temp.x() = y()*other.z() - z() * other.y();
		temp.y() = z() * other.x() - x() * other.z();
		temp.z() = x() * other.y() - y() * other.x();
		return temp;
	}
	//Returns the angle between this vector and another
	T Angle(Vector3D<T> other) const
	{
		return (T)Radian(other)*180/M_PI;
	}
	//Returns the radian between this vector and another
	T Radian(Vector3D<T> other) const
	{
		return (T)acos((*this).Unit().DotProduct(other.Unit()));
	}
	~Vector3D()
	{
	}
};

typedef Vector3D<double> Vector3Dd;
typedef Vector3D<float> Vector3Df;
typedef Vector3D<int> Vector3Di;
typedef Vector3D<unsigned int> Vector3Dui;