Template Matrix class

Matrix.hpp

// Default type traits for class Matrix
template <class type>
class DefaultMatrixTraits 
{
public:
	static const type zero() { return (type)0; }
	static const type one() { return (type)1; }
	static const type two() { return (type)2; }
};

// Matrix
template <unsigned int rows, unsigned int columns, class _type=float, class _traits=DefaultMatrixTraits<_type> > 
class Matrix
{
public:
	typedef _type type;
	typedef _traits traits;

	Matrix(type arg1=traits::zero(), type arg2=traits::zero(), type arg3=traits::zero(), type arg4=traits::zero(), 
		type arg5=traits::zero(), type arg6=traits::zero(), type arg7=traits::zero(), type arg8=traits::zero(), 
		type arg9=traits::zero(), type arg10=traits::zero(), type arg11=traits::zero(), type arg12=traits::zero(),
		type arg13=traits::zero(), type arg14=traits::zero(), type arg15=traits::zero(), type arg16=traits::zero()) 
	{
		switch(rows * columns) {
			case 16: matrix[15] = arg16;
			case 15: matrix[14] = arg15;
			case 14: matrix[13] = arg14;
			case 13: matrix[12] = arg13;
			case 12: matrix[11] = arg12;
			case 11: matrix[10] = arg11;
			case 10: matrix[9] = arg10;
			case 9: matrix[8] = arg9;
			case 8: matrix[7] = arg8;
			case 7: matrix[6] = arg7;
			case 6: matrix[5] = arg6;
			case 5: matrix[4] = arg5;
			case 4: matrix[3] = arg4;
			case 3: matrix[2] = arg3;
			case 2: matrix[1] = arg2;
			case 1: matrix[0] = arg1;
		}
	}

	// Construction from value array
	Matrix(const type *values)
	{
		*this = values;
	}

	// Assignment of value array
	Matrix<rows, columns, type> operator=(const type *values) 
	{
		for(unsigned int i=0; i<rows*columns; ++i) {
			matrix[i] = values[i];
		}

		return *this;
	}

	// Element access
	type &operator()(const unsigned int row, const unsigned int column=0)
	{
		return matrix[row + column*rows]; 
	}

	type operator()(const unsigned int row, const unsigned int column=0) const
	{
		return matrix[row + column*rows]; 
	}

	// Comparison
	bool operator ==(const Matrix<rows, columns, type> &val) const
	{
		for(unsigned int i=0; i<rows*columns; ++i) {
			if(matrix[i] != val.matrix[i]) {
				return false;
			}
		}
		return true;
	}

	bool operator !=(const Matrix<rows, columns, type> &val) const
	{
		for(unsigned int i=0; i<rows*columns; ++i) {
			if(matrix[i] != val.matrix[i]) {
				return true;
			}
		}
		return false;
	}

	// Cast
	template <class other_type> operator Matrix<rows, columns, other_type>() 
	{
		Matrix<rows, columns, other_type> ret;

		for(unsigned int i=0; i<rows*columns; ++i) {
#pragma warning( push )
#pragma warning( disable : 4244 )
			ret(i) = (other_type)matrix[i];
#pragma warning( pop )
		}

		return ret;
	}

	// Addition / Subtraction
	void operator +=(const Matrix<rows, columns, type> &val)
	{
		for(unsigned int i=0; i<rows*columns; ++i) {
			matrix[i] += val.matrix[i];
		}			
	}

	void operator -=(const Matrix<rows, columns, type> &val)
	{
		for(unsigned int i=0; i<rows*columns; ++i) {
			matrix[i] -= val.matrix[i];
		}			
	}

	Matrix<rows, columns, type> operator +(const Matrix<rows, columns, type> &val) const
	{
		Matrix<rows, columns, type> ret;
		for(unsigned int i=0; i<rows*columns; ++i) {
			ret.matrix[i]+= matrix[i] + val.matrix[i];
		}		
		return ret;
	}

	friend Matrix<rows, columns, type> operator -(const Matrix<rows, columns, type> &a, const Matrix<rows, columns, type> &b)
	{
		Matrix<rows, columns, type> ret;
		for(unsigned int i=0; i<rows*columns; ++i) {
			ret.matrix[i] = a.matrix[i] - b.matrix[i];
		}		
		return ret;
	}

	// Prefix + and -
	Matrix<rows, columns, type> operator +() const
	{
		return *this;
	}

	Matrix<rows, columns, type> operator -() const
	{
		return -traits::one() * (*this);
	}
		
	// Scalar multiplication
	void operator *=(type scalar) { 
		for(unsigned int i=0; i<columns*rows;++i) 
		{
			matrix[i] *= scalar; 
		}
	}

	friend Matrix<rows, columns, type> operator *(const Matrix<rows, columns, type> &mat, const type scalar)
	{
		Matrix<rows, columns, type> ret;
		for(unsigned int i=0; i<rows*columns; ++i) {
			ret.matrix[i] = mat.matrix[i] * scalar; 
		}
		return ret;
	}

	friend Matrix<rows, columns, type> operator *(const type scalar, const Matrix<rows, columns, type> &mat)
	{
		Matrix<rows, columns, type> ret;
		for(unsigned int i=0; i<rows*columns; ++i) {
			ret.matrix[i] = mat.matrix[i] * scalar; 
		}
		return ret;
	}

	// Scalar division
	Matrix<rows, columns, type> operator /(const type scalar) const
	{
		Matrix<rows, columns, type> ret;
		type rec = traits::one() / scalar;
		for(unsigned int i=0; i<rows*columns; ++i) {
			ret.matrix[i] = matrix[i] * rec; 
		}
		return ret;
	}

	Matrix<rows, columns, type> operator /(const Matrix<rows, columns, type> &val) const
	{
		Matrix<rows, columns, type> ret;			
		for(unsigned int i=0; i<rows*columns; ++i) {
			ret.matrix[i] = matrix[i] / val.matrix[i]; 
		}
		return ret;
	}

	void operator /=(const type scalar) 
	{
		type rec = traits::one() / scalar;
		for(unsigned int i=0; i<rows*columns; ++i) {
			matrix[i] *= rec; 
		}
	}

	// Scaling
	void scale(Matrix<rows, columns, type> &val) 
	{
		for(unsigned int i=0; i<rows*columns; ++i) {
			matrix[i] *= val.matrix[i];
		}	
	}

	// Vector functions
	type length() const
	{ 
		type sqr_sum = traits::zero();
		for(unsigned int i=0; i<rows; ++i) {
			sqr_sum += matrix[i] * matrix[i];
		}
		return sqrt(sqr_sum);
	}

	Matrix<rows, columns, type> normalize() 
	{
		*this /= length();
		return *this;
	}

	// Matrix functions
	Matrix<rows, columns, type> setIdentity() 
	{
		for(unsigned int row=0; row < rows; ++row) {
			for(unsigned int column=0; column < columns; ++column) {
				(*this)(column, row) = (row == column) ? traits::one() : traits::zero();
			}
		}

		return *this;
	}

	Matrix<columns, rows, type> transpose() const
	{
		Matrix<columns, rows, type> ret;
		for(unsigned int row=0; row<rows; ++row) {
			for(unsigned int column=0; column<columns; ++column) {
				ret(column, row) = (*this)(row, column);
			}
		}
		return ret;
	}

	// basic_string output
	operator std::string() const
	{
		using boost::lexical_cast;
		using std::string;

		if(columns > 1 && rows > 1) {
			string ret = "Matrix:\n/ ";

			for(unsigned int row=0; row<rows; ++row) {
				if(row != 0) {
					ret += "| ";
				}

				for(unsigned int column=0; column<columns; ++column) {
					ret += lexical_cast<string>((*this)(row, column));
					if(column < columns - 1) {
						ret += " ";
					}
				}

				if(row < rows - 1) {
					ret += " |\n";
				}
			}

			ret += " /";

			return ret;
		}
		else if(rows == 1) {
			string ret = "Vector: ( ";

			for(unsigned int row=0; row<rows; ++row) {
				ret += lexical_cast<string>((*this)(row)) + " ";
			}

			ret += ")";

			return ret;
		}
		else {
			string ret = "Row Vector: ( ";

			for(unsigned int column=0; column<columns; ++column) {
				ret += lexical_cast<string>((*this)(1, column)) + " ";
			}

			ret += ")";

			return ret;
		}
	}

	operator std::wstring() const 
	{
		std::string str = (std::string)*this;
		return boost::lexical_cast<std::wstring>(str.c_str());
	}

private:
	type matrix[rows * columns];
};

// ostream output
template <unsigned int rows, unsigned int columns, class type>
std::ostream &operator <<(std::ostream &os, const Matrix<rows, columns, type> &mat)
{
	os << "Matrix:" << std::endl << "/ ";

	for(unsigned int row=0; row<rows; ++row) {
		if(row != 0) {
			os << "| ";
		}

		for(unsigned int column=0; column<columns; ++column) {
			os << mat(row, column);
			if(column < columns - 1) {
				os << " ";
			}
		}
			
		if(row < rows - 1) {
			os << " |" << std::endl;
		}
	}

	os << " /";

	return os;
}

// Column vector ostream output
template <unsigned int rows, class type>
std::ostream &operator <<(std::ostream &os, const Matrix<rows, 1, type> &vec)
{		
	os << "Vector: ( ";

	for(unsigned int row=0; row<rows; ++row) {
		os << vec(row) << " ";
	}

	os << ")";

	return os;
}

// Row vector ostream output
template <unsigned int columns, class type>
std::ostream &operator <<(std::ostream &os, const Matrix<1, columns, type> &vec)
{		
	os << "Row Vector: ( ";

	for(unsigned int column=0; column<columns; ++column) {
		os << vec(0, column) << " ";
	}

	os << ")";

	return os;
}

// Matrix Multiplication
template <class type, class a_type, class b_type, class traits, unsigned int a_rows, unsigned int a_columns_b_rows, unsigned int b_columns> 
inline Matrix<a_rows, b_columns, type, traits> matrix_mult(const Matrix<a_rows, a_columns_b_rows, a_type> &a, const Matrix<a_columns_b_rows, b_columns, b_type> &b) 
{ 
	Matrix<a_rows, b_columns, type, traits> result;

	for(unsigned int row=0; row<a_rows; ++row) {
		for(unsigned int column=0; column<b_columns; ++column) {
			result(row, column) = traits::zero();
				
			for(unsigned int i=0; i<a_columns_b_rows; ++i) {
				result(row, column) += a(row, i) * b(i, column);
			}
		}
	}

	return result;
}

template <class type, unsigned int a_rows, unsigned int a_columns_b_rows, unsigned int b_columns>
inline Matrix<a_rows, b_columns, type> operator *(const Matrix<a_rows, a_columns_b_rows, type> &a, const Matrix<a_columns_b_rows, b_columns, type> &b) 
{
	return matrix_mult<type, type, type, DefaultMatrixTraits<type> >(a, b);
}

// Typedefs	
typedef Matrix<2, 2> Matrix22;
typedef Matrix<2, 2, double> Matrix22d;
typedef Matrix<2, 2, int> Matrix22i;
typedef Matrix<3, 3> Matrix33;
typedef Matrix<3, 3, double> Matrix33d;
typedef Matrix<3, 3, int> Matrix33i;
typedef Matrix<4, 4> Matrix44;
typedef Matrix<4, 4, double> Matrix44d;
typedef Matrix<4, 4, int> Matrix44i;

typedef Matrix<2, 1> Vector2;	
typedef Matrix<2, 1, double> Vector2d;
typedef Matrix<2, 1, int> Vector2i;
typedef Matrix<3, 1> Vector3;	
typedef Matrix<3, 1, double> Vector3d;
typedef Matrix<3, 1, int> Vector3i;	
typedef Matrix<4, 1> Vector4;
typedef Matrix<4, 1, double> Vector4d;
typedef Matrix<4, 1, int> Vector4i;	

// Cross product
template <class type, class type_a, class type_b>
type cross_product(const Matrix<2, 1, type_a> &a, const Matrix<2, 1, type_b> &b) 
{
	return a(0) * b(1) - a(1) * b(0);
}

template <class type, class type_a, class type_b>
inline Matrix<3, 1, type> cross_product(const Matrix<3, 1, type_a> &a, const Matrix<3, 1, type_b> &b) 
{
	Matrix<3, 1, type> ret;
	ret(0) = a(1) * b(2) - a(2) * b(1);
	ret(1) = a(2) * b(0) - a(0) * b(2);
	ret(2) = a(0) * b(1) - a(1) * b(0);
	return ret;
}

template <class type> type cross(const Matrix<2, 1, type> &a, const Matrix<2, 1, type> &b) 
{
	return cross_product<type>(a, b);
}

template <class type>
inline Matrix<3, 1, type> cross(const Matrix<3, 1, type> &a, const Matrix<3, 1, type> &b) 
{
	return cross_product<type>(a, b);
}

// Dot product
template <class type, class type_a, class type_b, class traits, unsigned int dim>
inline type dot_product(const Matrix<dim, 1, type_a> &a, const Matrix<dim, 1, type_b> &b) 
{
	type ret = traits::zero();

	for(unsigned int i=0; i<dim; ++i) {
		ret += (type)a(i) * (type)b(i);
	}

	return ret;
}

template <class type, class type_a, class type_b, unsigned int dim>
inline type dot_product(const Matrix<dim, 1, type_a> &a, const Matrix<dim, 1, type_b> &b) 
{
	return dot_product<type, type_a, type_b, DefaultMatrixTraits<type>, dim>(a, b);
}

template <class type, unsigned int dim>
inline type operator *(const Matrix<dim, 1, type> &a, const Matrix<dim, 1, type> &b) 
{
	return dot_product<type>(a, b);
}

// Determinant
template <class type>
inline type det(const Matrix<1, 1, type> &m)
{
	return m(0, 0);
}

template <class type>
inline type det(const Matrix<2, 2, type> &m)
{
	return m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0);
}

template <class type>
inline type det(const Matrix<3, 3, type> &m)
{
	return m(0, 0) * m(1, 1) * m(2, 2) + m(0, 1) * m(1, 2) * m(2, 0) + m(0, 2) * m(1, 0) * m(2, 1)
		- m(0, 2) * m(1, 1) * m(2, 0) - m(0, 1) * m(1, 0) * m(2, 2) - m(0, 0) * m(1, 2) * m(2, 1);
}

template <class type, unsigned int dim, class traits>
inline type det(const Matrix<dim, dim, type, traits> &m)
{
	// Compute determinant recursively
	type sign = traits::one();
	type ret = traits::zero();

	for(unsigned int i = 0; i < dim; ++i) {
		// Construct sub matrix
		Matrix<dim - 1, dim - 1, type, traits> sub_matrix;

		for(unsigned int src_col = 1; src_col < dim; ++src_col) {
			const unsigned int dst_col = src_col - 1;

			for(unsigned int src_row = 0, dst_row=0; src_row < dim; ++src_row) {
				if(src_row != i) {
					sub_matrix(dst_row, dst_col) = m(src_row, src_col);
					++dst_row;
				}
			}
		}
			
		type a = m(i, 0);
		ret += sign * a * det(sub_matrix);
		sign *= -traits::one();
	}

	return ret;
}

// Cofactor matrix
template <class type, unsigned int dim, class traits>
inline Matrix<dim, dim, type, traits> cofactor(const Matrix<dim, dim, type, traits> &m)
{
	Matrix<dim, dim, type, traits> ret;
	type sign = traits::one();
		
	for(unsigned int row = 0; row < dim; ++row) {
		for(unsigned int column = 0; column < dim; ++column) {
			Matrix<dim - 1, dim - 1, type, traits> sub_matrix;				
				
			for(unsigned int src_row = 0, dst_row = 0; src_row < dim; ++src_row) {					
				if(src_row != row) {
					for(unsigned int src_column = 0, dst_column = 0; src_column < dim; ++src_column) {
						if(src_column != column) {								
							sub_matrix(dst_row, dst_column) = m(src_row, src_column);
							++dst_column;								
						}						
					}

					++dst_row;
				}					
			}
				
			ret(row, column) = sign * det(sub_matrix);
			sign *= -traits::one();
		}
		sign *= -traits::one();
	}
		
	return ret;
}

// Adjoint matrix
template <class type, unsigned int dim, class traits>
inline Matrix<dim, dim, type, traits> adjoint(const Matrix<dim, dim, type, traits> &m)
{
	Matrix<dim, dim, type, traits> ret;
	type sign = traits::one();
		
	for(unsigned int row = 0; row < dim; ++row) {
		for(unsigned int column = 0; column < dim; ++column) {
			Matrix<dim - 1, dim - 1, type, traits> sub_matrix;				
				
			for(unsigned int src_row = 0, dst_row = 0; src_row < dim; ++src_row) {					
				if(src_row != row) {
					for(unsigned int src_column = 0, dst_column = 0; src_column < dim; ++src_column) {
						if(src_column != column) {								
							sub_matrix(dst_row, dst_column) = m(src_row, src_column);
							++dst_column;								
						}						
					}

					++dst_row;
				}					
			}
				
			ret(column, row) = sign * det(sub_matrix);
			sign *= -traits::one();
		}
		sign *= -traits::one();
	}
		
	return ret;
}

// Matrix inverse
template <class type, class traits>
inline Matrix<1, 1, type, traits> inverse(const Matrix<1, 1, type, traits> &m)
{		
	return Matrix<1, 1, type, traits>(traits::one() / m(0));
}

template <class type, class traits>
inline Matrix<2, 2, type, traits> inverse(const Matrix<2, 2, type, traits> &m)
{
	const type one_over_det = traits::one() / det(m);
	const Matrix<2, 2, type, traits> ret(m(1, 1), -m(1, 0), -m(0, 1), m(0, 0));		
	return one_over_det * ret;
}

template <class type, class traits>
inline Matrix<3, 3, type, traits> inverse(const Matrix<3, 3, type, traits> &m)
{
	const type one_over_det = traits::one() / det(m);

	Matrix<3, 3, type, traits> ret;

	ret(0, 0) = m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1);
	ret(1, 0) = m(1, 2) * m(2, 0) - m(1, 0) * m(2, 2);
	ret(2, 0) = m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0);

	ret(0, 1) = m(0, 2) * m(2, 1) - m(0, 1) * m(2, 2);
	ret(1, 1) = m(0, 0) * m(2, 2) - m(0, 2) * m(2, 0);
	ret(2, 1) = m(0, 1) * m(2, 0) - m(0, 0) * m(2, 1);

	ret(0, 2) = m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1);
	ret(1, 2) = m(0, 2) * m(1, 0) - m(0, 0) * m(1, 2);
	ret(2, 2) = m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0);		

	return one_over_det * ret;
}

template <class type, unsigned int dim, class traits>
inline Matrix<dim, dim, type, traits> inverse(const Matrix<dim, dim, type, traits> &m)
{
	const type one_over_det = traits::one() / det(m);		
	const Matrix<dim, dim, type, traits> ret = adjoint(m);
	return one_over_det * ret;
}

// Angle
template <class type, unsigned int dim>
inline type angle_between(Matrix<dim, 1, type> a, Matrix<dim, 1, type> b) 
{
	float angle = acos(a.normalize() * b.normalize());
		
	// Check for NaNs and infinities
	return (angle != angle) ? 0.0f : angle;
}

// Scale
template <class type, unsigned int dim>
inline Matrix<dim, 1, type> scale_vec(Matrix<dim, 1, type> a, Matrix<dim, 1, type> b) 
{
	Matrix<dim, 1, type> ret;

	for(unsigned int i=0; i<dim; ++i) {
		ret(i) = a(i) * b(i);
	}

	return ret;
}

template <class type, unsigned int columns, unsigned int rows>
inline Matrix<columns, rows, type> lerp(Matrix<columns, rows, type> a, Matrix<columns, rows, type> b, type factor)
{
	return a + (b - a) * factor;		
}

// X Rotation Matrix
template <class type, class traits> 
Matrix<4, 4, type, traits> xRotationMatrix44(type angle) 
{
	Matrix<4, 4, type, traits> matrix;

	const type zero = traits::zero();
	const type one = traits::one();

	matrix(0, 0) = one;		matrix(0, 1) = zero;		matrix(0, 2) = zero;		matrix(0, 3) = zero;
	matrix(1, 0) = zero;	matrix(1, 1) = cos(angle);	matrix(1, 2) = -sin(angle);	matrix(1, 3) = zero;
	matrix(2, 0) = zero;	matrix(2, 1) = sin(angle);	matrix(2, 2) = cos(angle);	matrix(2, 3) = zero;
	matrix(3, 0) = zero;	matrix(3, 1) = zero;		matrix(3, 2) = zero;		matrix(3, 3) = one;

	return matrix;
}

template <class type> 
Matrix<4, 4, type> xRotationMatrix44(type angle) 
{
	return xRotationMatrix44<type, DefaultMatrixTraits<type> >(angle);
}

template <class type, class traits>
Matrix<3, 3, type, traits> xRotationMatrix33(type angle) 
{
	Matrix<3, 3, type, traits> matrix;

	const type zero = traits::zero();
	const type one = traits::one();

	matrix(0, 0) = one;		matrix(0, 1) = zero;		matrix(0, 2) = zero;
	matrix(1, 0) = zero;	matrix(1, 1) = cos(angle);	matrix(1, 2) = -sin(angle);
	matrix(2, 0) = zero;	matrix(2, 1) = sin(angle);	matrix(2, 2) = cos(angle);

	return matrix;
}	

template <class type>
Matrix<3, 3, type> xRotationMatrix33(type angle)
{
	return xRotationMatrix33<type, DefaultMatrixTraits<type> >(angle);
}

// Y Rotation Matrix
template <class type, class traits> 
Matrix<4, 4, type, traits> yRotationMatrix44(type angle) 
{
	Matrix<4, 4, type, traits> matrix;

	const type zero = traits::zero();
	const type one = traits::one();

	matrix(0, 0) = cos(angle);	matrix(0, 1) = zero;	matrix(0, 2) = sin(angle);	matrix(0, 3) = zero;
	matrix(1, 0) = zero;		matrix(1, 1) = one;		matrix(1, 2) = zero;		matrix(1, 3) = zero;
	matrix(2, 0) = -sin(angle);	matrix(2, 1) = zero;	matrix(2, 2) = cos(angle);	matrix(2, 3) = zero;
	matrix(3, 0) = zero;		matrix(3, 1) = zero;	matrix(3, 2) = zero;		matrix(3, 3) = one;

	return matrix;
}

template <class type> 
Matrix<4, 4, type> yRotationMatrix44(type angle)
{
	return yRotationMatrix44<type, DefaultMatrixTraits<type> >(angle);
}

template <class type, class traits> 
Matrix<3, 3, type, traits> yRotationMatrix33(type angle) 
{
	Matrix<3, 3, type, traits> matrix;

	const type zero = traits::zero();
	const type one = traits::one();

	matrix(0, 0) = cos(angle);	matrix(0, 1) = zero;	matrix(0, 2) = sin(angle);
	matrix(1, 0) = zero;		matrix(1, 1) = one;		matrix(1, 2) = zero;
	matrix(2, 0) = -sin(angle);	matrix(2, 1) = zero;	matrix(2, 2) = cos(angle);

	return matrix;
}

template <class type> 
Matrix<3, 3, type> yRotationMatrix33(type angle) 
{
	return yRotationMatrix33<type, DefaultMatrixTraits<type> >(angle);
}

// Z Rotation Matrix
template <class type, class traits> 
Matrix<4, 4, type, traits> zRotationMatrix44(type angle) 
{
	Matrix<4, 4, type, traits> matrix;

	const type zero = traits::zero();
	const type one = traits::one();

	matrix(0, 0) = cos(angle);	matrix(0, 1) = -sin(angle);		matrix(0, 2) = zero;	matrix(0, 3) = zero;
	matrix(1, 0) = sin(angle);	matrix(1, 1) = cos(angle);		matrix(1, 2) = zero;	matrix(1, 3) = zero;
	matrix(2, 0) = zero;		matrix(2, 1) = zero;			matrix(2, 2) = one;		matrix(2, 3) = zero;
	matrix(3, 0) = zero;		matrix(3, 1) = zero;			matrix(3, 2) = zero;	matrix(3, 3) = one;

	return matrix;
}

template <class type> 
Matrix<4, 4, type> zRotationMatrix44(type angle) 
{
	return zRotationMatrix44<type, DefaultMatrixTraits<type> >(angle);
}

template <class type, class traits> 
Matrix<3, 3, type, traits> zRotationMatrix33(type angle) 
{
	Matrix<3, 3, type, traits> matrix;

	const type zero = traits::zero();
	const type one = traits::one();

	matrix(0, 0) = cos(angle);	matrix(0, 1) = -sin(angle);		matrix(0, 2) = zero;
	matrix(1, 0) = sin(angle);	matrix(1, 1) = cos(angle);		matrix(1, 2) = zero;
	matrix(2, 0) = zero;		matrix(2, 1) = zero;			matrix(2, 2) = one;	

	return matrix;
}

template <class type> 
Matrix<3, 3, type> zRotationMatrix33(type angle) 
{
	return zRotationMatrix33<type, DefaultMatrixTraits<type> >(angle);
}

template <class type, class traits> 
Matrix<2, 2, type, traits> zRotationMatrix22(type angle) 
{
	Matrix<2, 2, type, traits> matrix;

	matrix(0, 0) = cos(angle);	matrix(0, 1) = -sin(angle);
	matrix(1, 0) = sin(angle);	matrix(1, 1) = cos(angle);

	return matrix;
}

template <class type> 
Matrix<2, 2, type> zRotationMatrix22(type angle) 
{
	return zRotationMatrix22<type, DefaultMatrixTraits<type> >(angle);
}

// Arbitrary Rotation Matrix
template <class type, class traits> 
Matrix<4, 4, type, traits> arbitaryRotationMatrix44(type angle, Matrix<3, 1, type, traits> axis) 
{
	Matrix<4, 4, type, traits> matrix;

	const type c = cos(angle);
	const type s = sin(angle);

	const type zero = traits::zero();
	const type one = traits::one();

	matrix(0, 0) = c + axis(0) * axis(0) * (zero-c);				
	matrix(0, 1) = axis(0) * axis(1) * (zero-c) - axis(2)*s;		
	matrix(0, 2) = axis(0) * axis(2) * (zero-c) + axis(1)*s;
	matrix(0, 3) = zero;

	matrix(1, 0) = axis(0) * axis(1) * (zero-c) + axis(2)*s;
	matrix(1, 1) = c + axis(1) * axis(1) * (1-c);
	matrix(1, 2) = axis(1) * axis(2) * (zero-c) - axis(0)*s;
	matrix(1, 3) = zero;

	matrix(2, 0) = axis(0) * axis(2) * (zero-c) - axis(1)*s;
	matrix(2, 1) = axis(1) * axis(2) * (zero-c) + axis(0)*s;
	matrix(2, 2) = c + axis(2) * axis(2) * (zero-c);
	matrix(2, 3) = zero;

	matrix(3, 0) = zero;
	matrix(3, 1) = zero;
	matrix(3, 2) = zero;
	matrix(3, 3) = one;

	return matrix;
}

template <class type>
Matrix<4, 4, type> arbitaryRotationMatrix44(type angle, Matrix<3, 1, type> axis) 
{
	return arbitaryRotationMatrix44<type, DefaultMatrixTraits<type> >(angle, axis);
}

template <class type>
Matrix<3, 3, type> arbitaryRotationMatrix33(type angle, Matrix<3, 1, type> axis) 
{
	Matrix<3, 3, type> matrix;

	const type c = cos(angle);
	const type s = sin(angle);

	matrix(0, 0) = c + axis(0) * axis(0) * (1-c);
	matrix(0, 1) = axis(0) * axis(1) * (1-c) - axis(2)*s;
	matrix(0, 2) = axis(0) * axis(2) * (1-c) + axis(1)*s;

	matrix(1, 0) = axis(0) * axis(1) * (1-c) + axis(2)*s;
	matrix(1, 1) = c + axis(1) * axis(1) * (1-c);
	matrix(1, 2) = axis(1) * axis(2) * (1-c) - axis(0)*s;

	matrix(2, 0) = axis(0) * axis(2) * (1-c) - axis(1)*s;
	matrix(2, 1) = axis(1) * axis(2) * (1-c) + axis(0)*s;
	matrix(2, 2) = c + axis(2) * axis(2)*(1-c);

	return matrix;
}

// Scaling matrices
template <class type, class traits>
Matrix<2, 2, type, traits> scaleMatrix22(Matrix<2, 1, type> scaling) 
{
	Matrix<2, 2, type, traits> matrix;

	const type zero = traits::zero();
	const type one = traits::one();

	matrix(0, 0) = scaling(0);	matrix(0, 1) = zero;
	matrix(1, 0) = zero;		matrix(1, 1) = scaling(1);

	return matrix;
}

template <class type>
Matrix<2, 2, type> scaleMatrix22(Matrix<2, 1, type> scaling) 
{
	return scaleMatrix22<type, DefaultMatrixTraits<type> >(scaling);
}

template <class type, class traits> 
Matrix<3, 3, type, traits> scaleMatrix33(Matrix<3, 1, type> scaling) 
{
	Matrix<3, 3, type, traits> matrix;

	const type zero = traits::zero();
	const type one = traits::one();

	matrix(0, 0) = scaling(0);	matrix(0, 1) = zero;			matrix(0, 2) = zero;
	matrix(1, 0) = zero;		matrix(1, 1) = scaling(1);		matrix(1, 2) = zero;
	matrix(2, 0) = zero;		matrix(2, 1) = zero;			matrix(2, 2) = scaling(2);	

	return matrix;
}
template <class type> 
Matrix<3, 3, type> scaleMatrix33(Matrix<3, 1, type> scaling) 
{
	return scaleMatrix33<type, DefaultMatrixTraits<type> >(scaling);
}

template <class type, class traits> 
Matrix<4, 4, type, traits> scaleMatrix44(Matrix<3, 1, type> scaling) 
{
	Matrix<4, 4, type, traits> matrix;

	const type zero = traits::zero();
	const type one = traits::one();

	matrix(0, 0) = scaling(0);	matrix(0, 1) = zero;			matrix(0, 2) = zero;			matrix(0, 3) = zero;
	matrix(1, 0) = zero;		matrix(1, 1) = scaling(1);		matrix(1, 2) = zero;			matrix(1, 3) = zero;
	matrix(2, 0) = zero;		matrix(2, 1) = zero;			matrix(2, 2) = scaling(2);		matrix(2, 3) = zero;
	matrix(3, 0) = zero;		matrix(3, 1) = zero;			matrix(3, 2) = zero;			matrix(3, 3) = one;

	return matrix;
}

template <class type> 
Matrix<4, 4, type> scaleMatrix44(Matrix<3, 1, type> scaling)
{
	return scaleMatrix44<type, DefaultMatrixTraits<type> >(scaling);
}

// Projection matrices
template <class type, class traits> 
Matrix<4, 4, type, traits> projectionMatrix(type fov, type aspect_ratio, type znear, type zfar) 
{
	Matrix<4, 4, type, traits> ret;
	ret.setIdentity();

	const type zero = traits::zero();
	const type one = traits::one();
	const type two = traits::two();

	const float scaley = one / tan(fov / two);
	const float scalex = scaley / aspect_ratio;

	// x = scalex
	ret(0, 0) = scalex;

	// y = scaley
	ret(1, 1) = scaley;

	// z = zfar/(zfar-znear)*z' -znear*zfar/(zfar-znear)*w'
	ret(2, 2) = zfar/(zfar-znear);	
	ret(2, 3) = -znear*zfar/(zfar-znear);	

	// w = z'
	ret(3, 2) = one;
	ret(3, 3) = zero;

	return ret;
}

template <class type> 
Matrix<4, 4, type> projectionMatrix(type fov, type aspect_ratio, type znear, type zfar) 
{
	return projectionMatrix<type, DefaultMatrixTraits<type> >(fov, aspect_ratio, znear, zfar);
}

template <class type> 
Matrix<4, 4, type> orthoMatrix(type left, type right, type bottom, type top, type znear, type zfar)
{
	Matrix<4, 4, type> ret;
	ret.setIdentity();

	// x = 2.0f/(right-left)*x' - (right+left)/(right-left)
	ret(0, 0) = 2.0f/(right-left); 
	ret(0, 3) = -(right+left)/(right-left);		

	// y = 2.0f/(top-bottom)*y' - (top+bottom)/(top-bottom)
	ret(1, 1) = 2.0f/(top-bottom); 
	ret(1, 3) = -(top+bottom)/(top-bottom);	

	// z = -2.0f/(zfar-znear)*z' - (zfar+znear)/(zfar-znear)
	ret(2, 2) = -2.0f/(zfar-znear);
	ret(2, 3) = -(zfar+znear)/(zfar-znear);

	return ret;
}

template <class type> 
Matrix<3, 3, type> translationMatrix2D(Matrix<2, 1, type> translation)
{
	Matrix<3, 3, type> ret;
	ret.setIdentity();

	ret(0, 2) = translation(0);
	ret(1, 2) = translation(1);

	return ret;
}

template <class type> 
Matrix<4, 4, type> translationMatrix3D(Matrix<3, 1, type> translation)
{
	Matrix<4, 4, type> ret;
	ret.setIdentity();
		
	ret(0, 3) = translation(0);
	ret(1, 3) = translation(1);
	ret(2, 3) = translation(2);

	return ret;
}