initial commit

engine/Math/Matrix.h

@@ -0,0 +1,866 @@
+#ifndef BLUECORE_MATRIX_H
+#define BLUECORE_MATRIX_H
+
+// system includes
+#include <algorithm>
+#include <cmath>
+
+// local includes
+#include "Scalar.h"
+#include "Vector.h"
+#include "Quaternion.h"
+
+namespace BlueCore
+{
+	template <class T>
+	class Matrix3x3Template
+	{
+	public:
+		T m[9];
+
+		/**
+		 * default constructor
+		 */
+		inline Matrix3x3Template()
+		{
+			identity();
+		}
+
+
+		/**
+		 * constructor from array
+		 */
+		template <class S>
+		inline Matrix3x3Template( const S a[9] )
+		{
+			m[0] = static_cast<T>(a[0]);
+			m[1] = static_cast<T>(a[1]);
+			m[2] = static_cast<T>(a[2]);
+
+			m[3] = static_cast<T>(a[3]);
+			m[4] = static_cast<T>(a[4]);
+			m[5] = static_cast<T>(a[5]);
+
+			m[6] = static_cast<T>(a[6]);
+			m[7] = static_cast<T>(a[7]);
+			m[8] = static_cast<T>(a[8]);
+		}
+		
+
+		/**
+		 * copy constructor
+		 */
+		template <class S>
+		inline Matrix3x3Template( const Matrix3x3Template<S> &a )
+		{
+			m[0] = static_cast<T>(a.m[0]);
+			m[1] = static_cast<T>(a.m[1]);
+			m[2] = static_cast<T>(a.m[2]);
+
+			m[3] = static_cast<T>(a.m[3]);
+			m[4] = static_cast<T>(a.m[4]);
+			m[5] = static_cast<T>(a.m[5]);
+
+			m[6] = static_cast<T>(a.m[6]);
+			m[7] = static_cast<T>(a.m[7]);
+			m[8] = static_cast<T>(a.m[8]);
+		}
+
+
+		/**
+		 * constructor from heading and up vectors
+		 */
+		inline Matrix3x3Template(
+			const Vector3Template<T> &h,
+			const Vector3Template<T> &u )
+		{
+			Vector3Template<T> a, b, c;
+			c = h.unit();
+			a = h.crossProduct( u ).unit();
+			b = c.crossProduct( a ).unit();
+			
+			m[0]  = a.x;
+			m[1]  = b.x;
+			m[2]  = c.x;
+			
+			m[3]  = a.y;
+			m[4]  = b.y;
+			m[5]  = c.y;
+			
+			m[6]  = a.z;
+			m[7]  = b.z;
+			m[8]  = c.z;
+		}
+		
+
+		/**
+		 * contructor from euler angles
+		 */
+		inline Matrix3x3Template( T a, T e, T t )
+		{
+			T ch = cos( a );
+			T sh = sin( a );
+			T ca = cos( e );
+			T sa = sin( e );
+			T cb = cos( t );
+			T sb = sin( t );
+			
+			m[0]  = ch * ca;
+			m[1]  = sh*sb - ch*sa*cb;
+			m[2]  = ch*sa*sb + sh*cb;
+
+			m[3]  = sa;
+			m[4]  = ca*cb;
+			m[5]  = -ca*sb;
+
+			m[6]  = -sh*ca;
+			m[7]  = sh*sa*cb + ch*sb;
+			m[8] = -sh*sa*sb + ch*cb;
+		}
+		
+				
+		/**
+		 * contructor from quaternion
+		 */
+		template <class S>
+		inline Matrix3x3Template( const QuaternionTemplate<S> &q )
+		{
+			T	xx = static_cast<T>(q.x*q.x),
+				xy = static_cast<T>(q.x*q.y),
+				xz = static_cast<T>(q.x*q.z),
+				xw = static_cast<T>(q.x*q.w),
+				
+				yy = static_cast<T>(q.y*q.y),
+				yz = static_cast<T>(q.y*q.z),
+				yw = static_cast<T>(q.y*q.w),
+				
+				zz = static_cast<T>(q.z*q.z),
+				zw = static_cast<T>(q.z*q.w);
+
+			m[0] = 1 - yy - yy - zz - zz;
+			m[1] = xy + xy + zw + zw;
+			m[2] = xz + xz - yw - yw;
+
+			m[3] = xy + xy - zw - zw;
+			m[4] = 1 - xx - xx - zz - zz;
+			m[5] = yz + yz + xw + xw;
+			
+			m[6] = xz + xz + yw + yw;
+			m[7] = yz + yz - xw - xw;
+			m[8] = 1 - xx - xx - yy - yy;
+		}
+		
+
+		/**
+		 * set matrix to identity matrix
+		 */
+		inline void identity()
+		{
+			m[0] = static_cast<T>(1);
+			m[1] = static_cast<T>(0);
+			m[2] = static_cast<T>(0);
+			
+			m[3] = static_cast<T>(0);
+			m[4] = static_cast<T>(1);
+			m[5] = static_cast<T>(0);
+			
+			m[6] = static_cast<T>(0);
+			m[7] = static_cast<T>(0);
+			m[8] = static_cast<T>(1);
+		}
+
+
+		/**
+		 * get transposed matrix
+		 */
+		inline Matrix3x3Template<T> transposed() const
+		{
+			Matrix3x3Template<T> a;
+			
+			a.m[0] = m[0]; a.m[3] = m[1]; a.m[6] = m[2];
+			a.m[1] = m[3]; a.m[4] = m[4]; a.m[7] = m[5];
+			a.m[2] = m[6]; a.m[5] = m[7]; a.m[8] = m[8];
+			
+			return a;
+		}
+
+
+		/**
+		 * transpose
+		 */
+		inline void transpose()
+		{
+			swap( m[3], m[1] );
+			swap( m[6], m[2] );
+			swap( m[7], m[5] );
+		}
+		
+
+		/**
+		 * determinate
+		 */
+		inline T determinant() const
+		{
+			return ( m[0]*m[4]*m[8] + m[1]*m[5]*m[6] + m[2]*m[3]*m[7] )
+				- ( m[6]*m[4]*m[2] + m[7]*m[5]*m[0] + m[8]*m[3]*m[1] );
+		}
+
+		inline Matrix3x3Template<T> inverted()
+		{
+			T det = determinant();
+			T one_over_det = 1.0f / det;
+
+			Matrix3x3Template<T> result;
+			result.m[0] = +(m[4] * m[8] - m[5] * m[7]) * one_over_det;
+			result.m[1] = -(m[1] * m[8] - m[2] * m[7]) * one_over_det;
+			result.m[2] = +(m[1] * m[5] - m[2] * m[4]) * one_over_det;
+			result.m[3] = -(m[3] * m[8] - m[5] * m[6]) * one_over_det;
+			result.m[4] = +(m[0] * m[8] - m[2] * m[6]) * one_over_det;
+			result.m[5] = -(m[0] * m[5] - m[2] * m[3]) * one_over_det;
+			result.m[6] = +(m[3] * m[7] - m[4] * m[6]) * one_over_det;
+			result.m[7] = -(m[0] * m[7] - m[1] * m[6]) * one_over_det;
+			result.m[8] = +(m[0] * m[4] - m[1] * m[3]) * one_over_det;
+
+			return result;
+		}
+		
+
+		/**
+		 * add/assign operator
+		 */
+		template <class S>
+		inline Matrix3x3Template<T> &operator += (
+			const Matrix3x3Template<S> &a )
+		{
+			m[0] += static_cast<T>(a.m[0]);
+			m[1] += static_cast<T>(a.m[1]);
+			m[2] += static_cast<T>(a.m[2]);
+			
+			m[3] += static_cast<T>(a.m[3]);
+			m[4] += static_cast<T>(a.m[4]);
+			m[5] += static_cast<T>(a.m[5]);
+			
+			m[6] += static_cast<T>(a.m[6]);
+			m[7] += static_cast<T>(a.m[7]);
+			m[8] += static_cast<T>(a.m[8]);
+	
+			return *this;
+		}	
+	
+
+		/**
+		 * matrix multiplication
+		 */
+		template <class S> 
+		inline Matrix3x3Template<T> operator * ( const Matrix3x3Template<S> &a )
+		{
+			Matrix3x3Template<T> result;
+
+			result.m[0] = m[0] * static_cast<T>(a.m[0])
+				+ m[3] * static_cast<T>(a.m[1])
+				+ m[6] * static_cast<T>(a.m[2]);
+				
+			result.m[1] = m[1] * static_cast<T>(a.m[0])
+				+ m[4] * static_cast<T>(a.m[1])
+				+ m[7] * static_cast<T>(a.m[2]);
+				
+			result.m[2] = m[2] * static_cast<T>(a.m[0])
+				+ m[5] * static_cast<T>(a.m[1])
+				+ m[8] * static_cast<T>(a.m[2]);
+
+
+			result.m[3] = m[0] * static_cast<T>(a.m[3])
+				+ m[3] * static_cast<T>(a.m[4])
+				+ m[6] * static_cast<T>(a.m[5]);
+				
+			result.m[4] = m[1] * static_cast<T>(a.m[3])
+				+ m[4] * static_cast<T>(a.m[4])
+				+ m[7] * static_cast<T>(a.m[5]);
+				
+			result.m[5] = m[2] * static_cast<T>(a.m[3])
+				+ m[5] * static_cast<T>(a.m[4])
+				+ m[8] * static_cast<T>(a.m[5]);
+
+
+			result.m[6] = m[0] * static_cast<T>(a.m[6])
+				+ m[3] * static_cast<T>(a.m[7])
+				+ m[6] * static_cast<T>(a.m[8]);
+				
+			result.m[7] = m[1] * static_cast<T>(a.m[6])
+				+ m[4] * static_cast<T>(a.m[7])
+				+ m[7] * static_cast<T>(a.m[8]);
+				
+			result.m[8] = m[2] * static_cast<T>(a.m[6])
+				+ m[5] * static_cast<T>(a.m[7])
+				+ m[8] * static_cast<T>(a.m[8]);
+			
+			return result;
+		}
+		
+
+		/**
+		 * matrix vector multiplication
+		 */
+		template <class S>
+		Vector3Template<T> operator * ( const Vector3Template<S>& a )
+		{
+			return Vector3Template<T>(
+				m[0] * static_cast<T>(a.x)
+				+ m[3] * static_cast<T>(a.y)
+				+ m[6] * static_cast<T>(a.z),
+				m[1] * static_cast<T>(a.x)
+				+ m[4] * static_cast<T>(a.y)
+				+ m[7] * static_cast<T>(a.z),
+				m[2] * static_cast<T>(a.x)
+				+ m[5] * static_cast<T>(a.y)
+				+ m[8] * static_cast<T>(a.z) );
+		}
+
+		/**
+		 * matrix scalar multiplication
+		 */
+		template <class S>
+		Matrix3x3Template<T> operator * ( const S a )
+		{
+			Matrix3x3Template<T> result;
+
+			result.m[0] = m[0] * static_cast<T>(a);
+			result.m[1] = m[1] * static_cast<T>(a);
+			result.m[2] = m[2] * static_cast<T>(a);
+			result.m[3] = m[3] * static_cast<T>(a);
+			result.m[4] = m[4] * static_cast<T>(a);
+			result.m[5] = m[5] * static_cast<T>(a);
+			result.m[6] = m[6] * static_cast<T>(a);
+			result.m[7] = m[7] * static_cast<T>(a);
+			result.m[8] = m[8] * static_cast<T>(a);
+
+			return result;
+		}
+
+		friend std::ostream &operator << ( std::ostream& os, Matrix3x3Template<T> m )
+		{
+			os << "( " << m.m[0] << ", " << m.m[1] << ", " << m.m[2] << " )" << std::endl;
+			os << "( " << m.m[3] << ", " << m.m[4] << ", " << m.m[5] << " )" << std::endl;
+			os << "( " << m.m[6] << ", " << m.m[7] << ", " << m.m[8] << " )" << std::endl;
+			return os;
+		}
+
+	};
+	
+	typedef Matrix3x3Template<float> Matrix3x3Float;
+	typedef Matrix3x3Template<double> Matrix3x3Double;
+	typedef Matrix3x3Template<Scalar> Matrix3x3;
+
+
+	template <class T>
+	class Matrix4x4Template
+	{
+
+	public:
+
+		T m[16];
+
+		/**
+		 * default constructor
+		 */
+		inline Matrix4x4Template()
+		{
+			identity();
+		}
+		
+		inline void identity()
+		{
+			m[0] = static_cast<T>(1);
+			m[1] = static_cast<T>(0);
+			m[2] = static_cast<T>(0);
+			m[3] = static_cast<T>(0);
+			
+			m[4] = static_cast<T>(0);
+			m[5] = static_cast<T>(1);
+			m[6] = static_cast<T>(0);
+			m[7] = static_cast<T>(0);
+			
+			m[8] = static_cast<T>(0);
+			m[9] = static_cast<T>(0);
+			m[10] = static_cast<T>(1);
+			m[11] = static_cast<T>(0);
+			
+			m[12] = static_cast<T>(0);
+			m[13] = static_cast<T>(0);
+			m[14] = static_cast<T>(0);
+			m[15] = static_cast<T>(1);
+		}		
+		
+		
+		/**
+		 * constructor from array
+		 */
+		inline Matrix4x4Template( const T a[16] )
+		{
+			m[0]  = a[0];  m[1]  = a[1];  m[2]  = a[2];  m[3]   = a[3];
+			m[4]  = a[4];  m[5]  = a[5];  m[6]  = a[6];  m[7]   = a[7];
+			m[8]  = a[8];  m[9]  = a[9];  m[10] = a[10]; m[11]  = a[11];
+			m[12] = a[12]; m[13] = a[13]; m[14] = a[14]; m[15]  = a[15];
+		}
+
+		/**
+		 * constructor from 3x3 matrix, add. element are set to zero
+		 */
+		inline Matrix4x4Template( const Matrix3x3Template<T> &a )
+		{
+			m[0]  = a[0];
+			m[1]  = a[1];
+			m[2]  = a[2];
+			m[3]  = static_cast<T>(0);
+			
+			m[4]  = a[3];
+			m[5]  = a[4];
+			m[6]  = a[5];
+			m[7]  = static_cast<T>(0);
+			
+			m[8]  = a[6];
+			m[9]  = a[7];
+			m[10] = a[8];
+			m[11] = static_cast<T>(0);
+			
+			m[12] = static_cast<T>(0);
+			m[13] = static_cast<T>(0);
+			m[14] = static_cast<T>(0);
+			m[15] = static_cast<T>(1);
+		}
+		
+		/**
+		 * constructor from 3x3 rotation matrix and translation vector
+		 */
+		inline Matrix4x4Template(
+			const Matrix3x3Template<T> &a,
+			const Vector3Template<T> &b )
+		{
+			m[0]  = a[0];
+			m[1]  = a[1];
+			m[2]  = a[2];
+			m[3]  = static_cast<T>(0);
+			
+			m[4]  = a[3];
+			m[5]  = a[4];
+			m[6]  = a[5];
+			m[7]  = static_cast<T>(0);
+			
+			m[8]  = a[6];
+			m[9]  = a[7];
+			m[10] = a[8];
+			m[11] = static_cast<T>(0);
+			
+			m[12] = b.x;
+			m[13] = b.y;
+			m[14] = b.z;
+			m[15] = static_cast<T>(1);
+		}
+		
+
+		/**
+		 * copy constructor
+		 */
+		inline Matrix4x4Template( const Matrix4x4Template<T> &a )
+		{
+			m[0]  = a.m[0];
+			m[1]  = a.m[1];
+			m[2]  = a.m[2];
+			m[3]  = a.m[3];
+
+			m[4]  = a.m[4];
+			m[5]  = a.m[5];
+			m[6]  = a.m[6];
+			m[7]  = a.m[7];
+
+			m[8]  = a.m[8];
+			m[9]  = a.m[9];
+			m[10] = a.m[10];
+			m[11] = a.m[11];
+
+			m[12] = a.m[12];
+			m[13] = a.m[13];
+			m[14] = a.m[14];
+			m[15] = a.m[15];
+		}
+
+
+		/**
+		 * constructor from quaternion
+		 */
+		inline Matrix4x4Template( const QuaternionTemplate<T> &q )
+		{
+			m[0] = 1 - 2*q.y*q.y - 2*q.z*q.z;
+			m[1] = 2*q.x*q.y - 2*q.z*q.w;
+			m[2] = 2*q.x*q.z + 2*q.y*q.w;
+			m[3] = static_cast<T>(0);
+			
+			m[4] = 2*q.x*q.y + 2*q.z*q.w;
+			m[5] = 1 - 2*q.x*q.x - 2*q.z*q.z;
+			m[6] = 2*q.y*q.z - 2*q.x*q.w;
+			m[7] = static_cast<T>(0);
+			
+			m[8] = 2*q.x*q.z - 2*q.y*q.w;
+			m[9] = 2*q.y*q.z + 2*q.x*q.w;
+			m[10] = 1 - 2*q.x*q.x - 2*q.y*q.y;
+			m[11] = static_cast<T>(0);
+
+			m[12] = static_cast<T>(0);
+			m[13] = static_cast<T>(0);
+			m[14] = static_cast<T>(0);
+			m[15] = static_cast<T>(1);
+		}
+
+
+		/**
+		 * constructor from quaternion and translation vector
+		 */
+		inline Matrix4x4Template(
+			const QuaternionTemplate<T> &q,
+			const Vector3Template<T> &a  )
+		{
+			m[0] = 1 - 2*q.y*q.y - 2*q.z*q.z;
+			m[1] = 2*q.x*q.y - 2*q.z*q.w;
+			m[2] = 2*q.x*q.z + 2*q.y*q.w;
+			m[3] = static_cast<T>(0);
+			
+			m[4] = 2*q.x*q.y + 2*q.z*q.w;
+			m[5] = 1 - 2*q.x*q.x - 2*q.z*q.z;
+			m[6] = 2*q.y*q.z - 2*q.x*q.w;
+			m[7] = static_cast<T>(0);
+			
+			m[8] = 2*q.x*q.z - 2*q.y*q.w;
+			m[9] = 2*q.y*q.z + 2*q.x*q.w;
+			m[10] = 1 - 2*q.x*q.x - 2*q.y*q.y;
+			m[11] = static_cast<T>(0);
+
+			m[12] = a.x;
+			m[13] = a.y;
+			m[14] = a.z;
+			m[15] = static_cast<T>(1);
+		}
+
+					
+		/**
+		 * constructor from heading, up and translation vectors
+		 */
+		inline Matrix4x4Template(
+			const Vector3Template<T> &h,
+			const Vector3Template<T> &u,
+			const Vector3Template<T> &t )
+		{
+			Vector3Template<T> a, b, c;
+			c = h.unit();
+			a = h.crossProduct( u ).unit();
+			b = c.crossProduct( a ).unit();
+			
+			m[0]  = a.x;
+			m[1]  = b.x;
+			m[2]  = c.x;
+			m[3]  = static_cast<T>(0);
+			
+			m[4]  = a.y;
+			m[5]  = b.y;
+			m[6]  = c.y;
+			m[7]  = static_cast<T>(0);
+			
+			m[8]  = a.z;
+			m[9]  = b.z;
+			m[10] = c.z;
+			m[11] = static_cast<T>(0);
+			
+			m[12] = t.x;
+			m[13] = t.y;
+			m[14] = t.z;
+			m[15] = static_cast<T>(1);
+		}
+
+
+		/**
+		 * contructor from euler angles
+		 */
+		inline Matrix4x4Template(
+			T a,
+			T e,
+			T t,
+			const Vector3Template<T> &tr )
+		{
+			T ch = cos( a );
+			T sh = sin( a );
+			T ca = cos( e );
+			T sa = sin( e );
+			T cb = cos( t );
+			T sb = sin( t );
+			
+			m[0]  = ch * ca;
+			m[1]  = sh*sb - ch*sa*cb;
+			m[2]  = ch*sa*sb + sh*cb;
+			m[3]  = static_cast<T>(0);
+			
+			m[4]  = sa;
+			m[5]  = ca*cb;
+			m[6]  = -ca*sb;
+			m[7]  = static_cast<T>(0);
+			
+			
+			m[8]  = -sh*ca;
+			m[9]  = sh*sa*cb + ch*sb;
+			m[10] = -sh*sa*sb + ch*cb;
+			m[11] = static_cast<T>(0);
+			
+			m[12] = tr.x;
+			m[13] = tr.y;
+			m[14] = tr.z;
+			m[15] = static_cast<T>(1);
+		}
+
+
+		/**
+		 * transpose this matrix
+		 */
+		inline void transpose()
+		{
+			std::swap( m[4], m[1] );
+			std::swap( m[8], m[2] );
+			std::swap( m[12], m[3] );
+
+			std::swap( m[9], m[6] );
+			std::swap( m[13], m[7] );
+			std::swap( m[14], m[11] );
+		}
+	
+
+		/**
+		 * get transposed matrix
+		 */
+		inline Matrix4x4Template<T> transposed()
+		{
+			Matrix4x4Template<T> a;
+			
+			a.m[0] = m[0]; a.m[4] = m[1]; a.m[8] = m[2]; a.m[12] = m[3];
+			a.m[1] = m[4]; a.m[5] = m[5]; a.m[9] = m[6]; a.m[13] = m[7];
+			a.m[2] = m[8]; a.m[6] = m[9]; a.m[10] = m[10]; a.m[14] = m[11];
+			a.m[3] = m[12]; a.m[7] = m[13]; a.m[11] = m[14]; a.m[15] = m[15];
+			
+			return a;
+		}		
+
+
+		/**
+		 * matrix multiplication
+		 */
+		template <class S> 
+		inline Matrix4x4Template<T> operator * ( const Matrix4x4Template<S> &a )
+		{
+			Matrix4x4Template<T> result;
+			
+			result.m[0] = m[0] * static_cast<T>(a.m[0])
+				+ m[4] * static_cast<T>(a.m[1])
+				+ m[8] * static_cast<T>(a.m[2])
+				+ m[12] * static_cast<T>(a.m[3]);
+				
+			result.m[1] = m[1] * static_cast<T>(a.m[0])
+				+ m[5] * static_cast<T>(a.m[1])
+				+ m[9] * static_cast<T>(a.m[2])
+				+ m[13] * static_cast<T>(a.m[3]);
+				
+			result.m[2] = m[2] * static_cast<T>(a.m[0])
+				+ m[6] * static_cast<T>(a.m[1])
+				+ m[10] * static_cast<T>(a.m[2])
+				+ m[14] * static_cast<T>(a.m[3]);
+
+			result.m[3] = m[3] * static_cast<T>(a.m[0])
+				+ m[7] * static_cast<T>(a.m[1])
+				+ m[11] * static_cast<T>(a.m[2])
+				+ m[15] * static_cast<T>(a.m[3]);
+
+
+			result.m[4] = m[0] * static_cast<T>(a.m[4])
+				+ m[4] * static_cast<T>(a.m[5])
+				+ m[8] * static_cast<T>(a.m[6])
+				+ m[12] * static_cast<T>(a.m[7]);
+				
+			result.m[5] = m[1] * static_cast<T>(a.m[4])
+				+ m[5] * static_cast<T>(a.m[5])
+				+ m[9] * static_cast<T>(a.m[6])
+				+ m[13] * static_cast<T>(a.m[7]);
+				
+			result.m[6] = m[2] * static_cast<T>(a.m[4])
+				+ m[6] * static_cast<T>(a.m[5])
+				+ m[10] * static_cast<T>(a.m[6])
+				+ m[14] * static_cast<T>(a.m[7]);
+
+			result.m[7] = m[3] * static_cast<T>(a.m[4])
+				+ m[7] * static_cast<T>(a.m[5])
+				+ m[11] * static_cast<T>(a.m[6])
+				+ m[15] * static_cast<T>(a.m[7]);
+
+
+			result.m[8] = m[0] * static_cast<T>(a.m[8])
+				+ m[4] * static_cast<T>(a.m[9])
+				+ m[8] * static_cast<T>(a.m[10])
+				+ m[12] * static_cast<T>(a.m[11]);
+				
+			result.m[9] = m[1] * static_cast<T>(a.m[8])
+				+ m[5] * static_cast<T>(a.m[9])
+				+ m[9] * static_cast<T>(a.m[10])
+				+ m[13] * static_cast<T>(a.m[11]);
+				
+			result.m[10] = m[2] * static_cast<T>(a.m[8])
+				+ m[6] * static_cast<T>(a.m[9])
+				+ m[10] * static_cast<T>(a.m[10])
+				+ m[14] * static_cast<T>(a.m[11]);
+
+			result.m[11] = m[3] * static_cast<T>(a.m[8])
+				+ m[7] * static_cast<T>(a.m[9])
+				+ m[11] * static_cast<T>(a.m[10])
+				+ m[15] * static_cast<T>(a.m[11]);
+
+
+			result.m[12] = m[0] * static_cast<T>(a.m[12])
+				+ m[4] * static_cast<T>(a.m[13])
+				+ m[8] * static_cast<T>(a.m[14])
+				+ m[12] * static_cast<T>(a.m[15]);
+				
+			result.m[13] = m[1] * static_cast<T>(a.m[12])
+				+ m[5] * static_cast<T>(a.m[13])
+				+ m[9] * static_cast<T>(a.m[14])
+				+ m[13] * static_cast<T>(a.m[15]);
+				
+			result.m[14] = m[2] * static_cast<T>(a.m[12])
+				+ m[6] * static_cast<T>(a.m[13])
+				+ m[10] * static_cast<T>(a.m[14])
+				+ m[14] * static_cast<T>(a.m[15]);
+
+			result.m[15] = m[3] * static_cast<T>(a.m[12])
+				+ m[7] * static_cast<T>(a.m[13])
+				+ m[11] * static_cast<T>(a.m[14])
+				+ m[15] * static_cast<T>(a.m[15]);
+			
+			return result;			
+		}
+		
+		/**
+		 * matrix multiplication
+		 */
+/*
+		template <class S> 
+		inline Matrix4x4Template<T> operator *= ( const Matrix4x4Template<S> &a )
+		{
+			Matrix4x4Template<T> result;
+			
+			result.m[0] = m[0] * static_cast<T>(a.m[0])
+				+ m[4] * static_cast<T>(a.m[1])
+				+ m[8] * static_cast<T>(a.m[2]);
+				+ m[12] * static_cast<T>(a.m[3]);
+				
+			result.m[1] = m[1] * static_cast<T>(a.m[0])
+				+ m[5] * static_cast<T>(a.m[1])
+				+ m[9] * static_cast<T>(a.m[2]);
+				+ m[13] * static_cast<T>(a.m[3]);
+				
+			result.m[2] = m[2] * static_cast<T>(a.m[0])
+				+ m[6] * static_cast<T>(a.m[1])
+				+ m[10] * static_cast<T>(a.m[2]);
+				+ m[14] * static_cast<T>(a.m[3]);
+
+			result.m[3] = m[3] * static_cast<T>(a.m[0])
+				+ m[7] * static_cast<T>(a.m[1])
+				+ m[11] * static_cast<T>(a.m[2]);
+				+ m[15] * static_cast<T>(a.m[3]);
+
+
+			result.m[4] = m[0] * static_cast<T>(a.m[4])
+				+ m[4] * static_cast<T>(a.m[5])
+				+ m[8] * static_cast<T>(a.m[6]);
+				+ m[12] * static_cast<T>(a.m[7]);
+				
+			result.m[5] = m[1] * static_cast<T>(a.m[4])
+				+ m[5] * static_cast<T>(a.m[5])
+				+ m[9] * static_cast<T>(a.m[6]);
+				+ m[13] * static_cast<T>(a.m[7]);
+				
+			result.m[6] = m[2] * static_cast<T>(a.m[4])
+				+ m[6] * static_cast<T>(a.m[5])
+				+ m[10] * static_cast<T>(a.m[6]);
+				+ m[14] * static_cast<T>(a.m[7]);
+
+			result.m[7] = m[3] * static_cast<T>(a.m[4])
+				+ m[7] * static_cast<T>(a.m[5])
+				+ m[11] * static_cast<T>(a.m[6]);
+				+ m[15] * static_cast<T>(a.m[7]);
+
+
+			result.m[8] = m[0] * static_cast<T>(a.m[8])
+				+ m[4] * static_cast<T>(a.m[9])
+				+ m[8] * static_cast<T>(a.m[10]);
+				+ m[12] * static_cast<T>(a.m[11]);
+				
+			result.m[9] = m[1] * static_cast<T>(a.m[8])
+				+ m[5] * static_cast<T>(a.m[9])
+				+ m[9] * static_cast<T>(a.m[10]);
+				+ m[13] * static_cast<T>(a.m[11]);
+				
+			result.m[10] = m[2] * static_cast<T>(a.m[8])
+				+ m[6] * static_cast<T>(a.m[9])
+				+ m[10] * static_cast<T>(a.m[10]);
+				+ m[14] * static_cast<T>(a.m[11]);
+
+			result.m[11] = m[3] * static_cast<T>(a.m[8])
+				+ m[7] * static_cast<T>(a.m[9])
+				+ m[11] * static_cast<T>(a.m[10]);
+				+ m[15] * static_cast<T>(a.m[11]);
+
+
+			result.m[12] = m[0] * static_cast<T>(a.m[12])
+				+ m[4] * static_cast<T>(a.m[13])
+				+ m[8] * static_cast<T>(a.m[14]);
+				+ m[12] * static_cast<T>(a.m[15]);
+				
+			result.m[13] = m[1] * static_cast<T>(a.m[12])
+				+ m[5] * static_cast<T>(a.m[13])
+				+ m[9] * static_cast<T>(a.m[14]);
+				+ m[13] * static_cast<T>(a.m[15]);
+				
+			result.m[14] = m[2] * static_cast<T>(a.m[12])
+				+ m[6] * static_cast<T>(a.m[13])
+				+ m[10] * static_cast<T>(a.m[14]);
+				+ m[14] * static_cast<T>(a.m[15]);
+
+			result.m[15] = m[3] * static_cast<T>(a.m[12])
+				+ m[7] * static_cast<T>(a.m[13])
+				+ m[11] * static_cast<T>(a.m[14]);
+				+ m[15] * static_cast<T>(a.m[15]);
+			
+			m = result.m;
+			
+			return *this;			
+		}
+	*/			
+		/**
+		 * matrix vector multiplication
+		 */
+		Vector3Template<T> operator * ( const Vector3Template<T> &a )
+		{
+			Vector3Template<T> result;
+			
+			result.x = m[0] * a.x + m[4] * a.y + m[8] * a.z + m[12];
+			result.y = m[1] * a.x + m[5] * a.y + m[9] * a.z + m[13];
+			result.z = m[2] * a.x + m[6] * a.y + m[11] * a.z + m[14];
+			
+			return result;
+		}
+
+		T *data()
+		{
+			return &m[0];
+		}
+	};
+	
+	typedef Matrix4x4Template<float> Matrix4x4Float;
+	typedef Matrix4x4Template<double> Matrix4x4Double;
+	typedef Matrix4x4Template<Scalar> Matrix4x4;
+	
+} // namespace bc
+
+#endif // BLUECORE_MATRIX_H

engine/Math/Plane.h

@@ -0,0 +1,51 @@
+#ifndef BLUECORE_PLANE_H
+#define BLUECORE_PLANE_H
+
+#include "Scalar.h"
+#include "Vector.h"
+
+namespace BlueCore
+{
+	template <class T>
+	class PlaneTemplate
+	{
+	public:
+		Vector3Template<T> _n;
+		T _d;
+
+		/**
+		 * contructor
+		 */
+		inline PlaneTemplate() : _d( 0. )
+		{
+		}
+		
+
+		/**
+		 * contructor
+		 */
+		inline PlaneTemplate(
+			Vector3Template<T> a,
+			Vector3Template<T> b,
+			Vector3Template<T> c )
+		{
+			_n = (a - b).cross(a - c).normalized(); 
+			_d = _n.dot( a );
+		}
+		
+
+		/**
+		 * distance
+		 */
+		inline T distance( const Vector3Template<T> a )
+		{
+			return _n.dot( a ) - _d;
+		}
+	};
+	
+	typedef PlaneTemplate<float> PlaneFloat;
+	typedef PlaneTemplate<double> PlaneDouble;
+	typedef PlaneTemplate<Scalar> Plane;
+}
+
+#endif

engine/Math/Point.h

@@ -0,0 +1,57 @@
+//------------------------------------------------------------------------------
+// Author:		Gero Mueller <gero.mueller@cloo.de>
+// Copyright:	(c) 2006 Gero Mueller
+// License:		MIT License
+//------------------------------------------------------------------------------
+
+#ifndef BLUECORE_POINT_H
+#define BLUECORE_POINT_H
+
+#include "rectangle.h"
+
+namespace BlueCore
+{
+	template<typename type>
+	struct Rectangle2DTemplate;
+	
+	template<typename type>
+	struct Point2DTemplate
+	{
+		type _x, _y;
+
+		Point2DTemplate() : _x(0), _y(0) 
+		{
+		}
+
+		Point2DTemplate( type x, type y ) : _x(x), _y(y) 
+		{
+		}
+		
+		bool in( const Rectangle2DTemplate<type> &rect );
+	};
+
+	#include "rectangle.h"
+
+	template<typename type>
+	bool Point2DTemplate<type>::in( const Rectangle2DTemplate<type> &rect )
+	{
+		if( _x < rect._x )
+			return false;
+
+		if( _x > (rect._x + rect._width) )
+			return false;
+
+		if( _y < rect._y )
+			return false;
+
+		if( _y > (rect._y + rect._height) )
+			return false;
+
+		return true;
+	}	
+
+	typedef Point2DTemplate<int> Point2D;
+
+};
+	
+#endif

engine/Math/Quaternion.h

@@ -0,0 +1,366 @@
+#ifndef BLUECORE_QUATERNION_H
+#define BLUECORE_QUATERNION_H
+
+#include "Vector.h"
+
+namespace BlueCore
+{
+	template <class T>
+	class QuaternionTemplate
+	{
+	public:
+		T w, x, y, z;
+
+
+		/**
+		 * constructor
+		 */
+		inline QuaternionTemplate()
+		{
+			w = static_cast<T>(1.0);
+			x = static_cast<T>(0.0);
+			y = static_cast<T>(0.0);
+			z = static_cast<T>(0.0);
+		}
+		
+
+		/**
+		 * contructor
+		 */
+		template <class S>
+		inline QuaternionTemplate( S w, S x, S y, S z )
+		{
+			this->w = static_cast<T>(w);
+			this->x = static_cast<T>(x);
+			this->y = static_cast<T>(y);
+			this->z = static_cast<T>(z);
+		}
+
+
+		/**
+		 * contructor
+		 */
+		template <class S>
+		inline QuaternionTemplate( Vector3Template<S> axis, S angle )
+		{
+			T half_angle = static_cast<T>(angle)/static_cast<T>(2.0);
+			T sin_half_angle = static_cast<T>( sin( half_angle ) );
+			
+			w = static_cast<T>( cos( half_angle ) );
+			x = sin_half_angle * static_cast<T>(axis.x);
+			y = sin_half_angle * static_cast<T>(axis.y);
+			z = sin_half_angle * static_cast<T>(axis.z);
+		}
+
+
+		/**
+		 * contructor
+		 */
+		template <class S>
+		inline QuaternionTemplate( S h, S a, S b )
+		{
+			T c1 = static_cast<T>( cos(h / 2.0) );
+			T c2 = static_cast<T>( cos(a / 2.0) );
+			T c3 = static_cast<T>( cos(b / 2.0) );
+			T s1 = static_cast<T>( sin(h / 2.0) );
+			T s2 = static_cast<T>( sin(a / 2.0) );
+			T s3 = static_cast<T>( sin(b / 2.0) );
+			
+			w = c1 * c2 * c3 - s1 * s2 * s3;
+			x = s1 * s2 * c3 + c1 * c2 * s3;
+			y = s1 * c2 * c3 + c1 * s2 * s3;
+			z = c1 * s2 * c3 - s1 * c2 * s3;
+		}
+
+
+		/**
+		 * identity
+		 */
+		inline void identity()
+		{
+			w = static_cast<T>(1.0);
+			x = static_cast<T>(0.0);
+			y = static_cast<T>(0.0);
+			z = static_cast<T>(0.0);
+		}	
+		
+
+		/**
+		 * operator +
+		 */
+		template <class S>
+		inline QuaternionTemplate<T> operator + (
+			const QuaternionTemplate<S> &a ) const
+		{
+			return QuaternionTemplate<T>(
+				w + static_cast<T>(a.w),
+				x + static_cast<T>(a.x),
+				y + static_cast<T>(a.y),
+				z + static_cast<T>(a.z) );
+		}
+
+
+		/**
+		 * operator +=
+		 */
+		template <class S>
+		inline QuaternionTemplate<T> &operator += (
+			const QuaternionTemplate<S> &a )
+		{
+			w += static_cast<T>(a.w);
+			x += static_cast<T>(a.x);
+			y += static_cast<T>(a.y);
+			z += static_cast<T>(a.z);
+	
+			return *this;
+		}	
+
+
+		/**
+		 * operator -
+		 */
+		template <class S>
+		inline QuaternionTemplate<T> operator - (
+			const QuaternionTemplate<S> &a ) const
+		{
+			return QuaternionTemplate<T>(
+				w - static_cast<T>(a.w),
+				x - static_cast<T>(a.x),
+				y - static_cast<T>(a.y),
+				z - static_cast<T>(a.z) );
+		}
+		
+
+		/**
+		 * operator *
+		 */
+		template <class S>
+		inline QuaternionTemplate<T> operator * (
+			const QuaternionTemplate<S> &a ) const
+		{
+			return QuaternionTemplate<T>(
+				w * static_cast<T>(a.w) -
+				x * static_cast<T>(a.x) -
+				y * static_cast<T>(a.y) -
+				z * static_cast<T>(a.z), 
+				w * static_cast<T>(a.x) +
+				x * static_cast<T>(a.w) +
+				y * static_cast<T>(a.z) -
+				z * static_cast<T>(a.y),
+				w * static_cast<T>(a.y) -
+				x * static_cast<T>(a.z) +
+				y * static_cast<T>(a.w) +
+				z * static_cast<T>(a.x),
+				w * static_cast<T>(a.z) +
+				x * static_cast<T>(a.y) -
+				y * static_cast<T>(a.x) +
+				z * static_cast<T>(a.w) );
+		}		
+		
+
+		/**
+		 * operator *=
+		 */
+		template <class S>
+		inline QuaternionTemplate<T> &operator *=(
+			const QuaternionTemplate<S> &a )
+		{
+			w = w * static_cast<T>(a.w) - x * static_cast<T>(a.x) -
+				y * static_cast<T>(a.y) - z * static_cast<T>(a.z);
+			x = w * static_cast<T>(a.x) + x * static_cast<T>(a.w) +
+				y * static_cast<T>(a.z) - z * static_cast<T>(a.y);
+			y = w * static_cast<T>(a.y) - x * static_cast<T>(a.z) +
+				y * static_cast<T>(a.w) + z * static_cast<T>(a.x);
+			z = w * static_cast<T>(a.z) + x * static_cast<T>(a.y) -
+				y * static_cast<T>(a.x) + z * static_cast<T>(a.w);
+			
+			return *this;
+		}	
+
+
+		/**
+		 * operator -=
+		 */
+		template <class S>
+		inline QuaternionTemplate<T> &operator -= (
+			const QuaternionTemplate<S> &a )
+		{
+			w -= static_cast<T>(a.w);
+			x -= static_cast<T>(a.x);
+			y -= static_cast<T>(a.y);
+			z -= static_cast<T>(a.z);
+	
+			return *this;
+		}	
+
+
+		/**
+		 * operator =
+		 */
+		template <class S>
+		inline QuaternionTemplate<T> &operator = (
+			const QuaternionTemplate<S> &a )
+		{
+			w = static_cast<T>(a.w);
+			x = static_cast<T>(a.x);
+			y = static_cast<T>(a.y);
+			z = static_cast<T>(a.z);
+	
+			return *this;
+		}
+
+
+		/**
+		 * unit
+		 */
+		inline QuaternionTemplate<T> unit() const
+		{
+			T d = 1/sqrt( w*w + x*x + y*y + z*z );
+			return QuaternionTemplate<T>( w * d, x * d, y * d, z * d );
+		}			
+
+
+		/**
+		 * inversed
+		 */
+		inline QuaternionTemplate<T> inversed() const
+		{
+			return QuaternionTemplate<T>( w, -x, -y, -z );
+		}			
+
+		/**
+		 * inversed
+		 */
+		inline void inverse()
+		{
+			x = -x;
+			y = -y;
+			z = -z;
+		}			
+
+
+		/**
+		 * normalize
+		 */
+		inline QuaternionTemplate<T> &normalize()
+		{
+			T d = 1/sqrt( w*w + x*x + y*y + z*z );
+			w *= d;
+			x *= d;
+			y *= d;
+			z *= d;
+	
+			return *this;
+		}
+		
+
+		/**
+		 * apply
+		 */
+		template <class S>
+		inline const Vector3Template<T> apply(
+			const Vector3Template<S> &a ) const
+		{
+			T	xx = x*x, xy = x*y, xz = x*z, xw = x*w,
+				yy = y*y, yz = y*z, yw = y*w,
+				zz = z*z, zw = z*w;
+				
+			return Vector3Template<T>(
+				2.0 * ( static_cast<T>(a.x) * ( 0.5 - yy - zz ) +
+				static_cast<T>(a.y) * ( xy - zw ) +
+				static_cast<T>(a.z) * ( xz + yw ) ),
+				
+				2.0 * ( static_cast<T>(a.x) * ( xy + zw ) +
+				static_cast<T>(a.y) * ( 0.5 - xx - zz ) +
+				static_cast<T>(a.z) * ( yz - xw ) ),
+				
+				2.0 * ( static_cast<T>(a.x) * ( xz - yw ) +
+				static_cast<T>(a.y) * ( yz + xw ) +
+				static_cast<T>(a.z) * ( 0.5 - xx - yy ) ) );
+		}
+
+
+		/**
+		 * apply
+		 */
+		template <class S>
+		inline const Vector3Template<T> operator * (
+			const Vector3Template<S> &a ) const
+		{
+			T	xx = x*x, xy = x*y, xz = x*z, xw = x*w,
+				yy = y*y, yz = y*z, yw = y*w,
+				zz = z*z, zw = z*w;
+				
+			return Vector3Template<T>(
+				2.0 * ( static_cast<T>(a.x) * ( 0.5 - yy - zz ) +
+				static_cast<T>(a.y) * ( xy - zw ) +
+				static_cast<T>(a.z) * ( xz + yw ) ),
+				
+				2.0 * ( static_cast<T>(a.x) * ( xy + zw ) +
+				static_cast<T>(a.y) * ( 0.5 - xx - zz ) +
+				static_cast<T>(a.z) * ( yz - xw ) ),
+				
+				2.0 * ( static_cast<T>(a.x) * ( xz - yw ) +
+				static_cast<T>(a.y) * ( yz + xw ) +
+				static_cast<T>(a.z) * ( 0.5 - xx - yy ) ) );
+		}
+
+
+		/**
+		 * applyInversed
+		 */
+		template <class S>
+		inline Vector3Template<T> applyInversed(
+			const Vector3Template<S> &a ) const
+		{
+			T	xx = x*x, xy = x*y, xz = x*z, xw = -x*w,
+				yy = y*y, yz = y*z, yw = -y*w,
+				zz = z*z, zw = -z*w;
+				
+			return Vector3Template<T>(
+				2.0 * ( static_cast<T>(a.x) * ( 0.5 - yy - zz ) +
+				static_cast<T>(a.y) * ( xy - zw ) +
+				static_cast<T>(a.z) * ( xz + yw ) ),
+				
+				2.0 * ( static_cast<T>(a.x) * ( xy + zw ) +
+				static_cast<T>(a.y) * ( 0.5 - xx - zz ) +
+				static_cast<T>(a.z) * ( yz - xw ) ),
+				
+				2.0 * ( static_cast<T>(a.x) * ( xz - yw ) +
+				static_cast<T>(a.y) * ( yz + xw ) +
+				static_cast<T>(a.z) * ( 0.5 - xx - yy ) ) );
+		}
+		
+		/**
+		 * transform from ode to gl coodinates
+		 */
+		inline QuaternionTemplate<T> toGl() const
+		{
+			return QuaternionTemplate<T>( w, x, z, -y );
+		}
+
+		/**
+		 * transform from gl to ode coodinates
+		 */
+		inline QuaternionTemplate<T> toOde() const
+		{
+			return QuaternionTemplate<T>( w, x, -z, y );
+		}
+		
+		inline QuaternionTemplate<T> slerp( const QuaternionTemplate<T> &q, const Scalar &t )
+		{
+			Scalar phi = acos(w*q.w + x*q.x + y*q.y + z*q.z);
+			Scalar s = 1 / sin(phi);
+			Scalar a = sin(phi*(1-t)) * s; 
+			Scalar b = sin(phi*t) * s;
+
+			return QuaternionTemplate<T>( a*w+b*q.w, a*x+b*q.x, a*y+b*q.y, a*z+b*q.z );
+		}
+	};
+	
+	typedef QuaternionTemplate<float> QuaternionFloat;
+	typedef QuaternionTemplate<double> QuaternionDouble;
+	typedef QuaternionTemplate<Scalar> Quaternion;
+}
+
+#endif

engine/Math/Ray.h

@@ -0,0 +1,119 @@
+//------------------------------------------------------------------------------
+// Author:		Gero Mueller <gero.mueller@cloo.de>
+// Copyright:	(c) 2006 Gero Mueller
+// License:		MIT License
+//------------------------------------------------------------------------------
+
+#ifndef BLUECORE_RAY_H
+#define BLUECORE_RAY_H
+
+#include "scalar.h"
+#include "vector.h"
+
+namespace BlueCore
+{
+	template <class T>
+	class RayTemplate
+	{
+		Vector3Template<T> point, direction;
+
+	public:
+	
+		/**
+		 * normalize the vector to length 1
+		 */
+		inline RayTemplate()
+		{
+		}
+
+
+		/**
+		 * constructor from point and direction
+		 */
+		template <class S>
+		inline RayTemplate(
+			const Vector3Template<S> &point,
+			const Vector3Template<S> &direction )
+		{
+			this->point = point;
+			this->direction = direction.normalized();
+		}
+
+
+		/**
+		 * set the point
+		 */
+		template <class S>
+		inline setPoint( const Vector3Template<S> &point )
+		{
+			this->point = point;
+		}
+
+
+		/**
+		 * set the direction
+		 */
+		template <class S>
+		inline setDirection( const Vector3Template<S> &direction )
+		{
+			this->direction = direction.normalized();
+		}
+
+
+		/**
+		 * get point
+		 */
+		inline const Vector3Template<T> &getPoint() const
+		{
+			return point;
+		}
+
+
+		/**
+		 * get direction
+		 */
+		inline const Vector3Template<T> &getDirection() const
+		{
+			return direction;
+		}
+
+
+		/**
+		 * checks if a point lies on the ray
+		 */
+		template <class S>
+		inline bool contains( const Vector3Template<T> &a )
+		{
+			return (a - point).parallel( direction );
+		}
+		
+		/**
+		 * calculate the distance between a point and the ray
+		 */
+		template <class S>
+		inline T distance( const Vector3Template<S> &a ) const
+		{
+			T t = direction.dot( a - point );
+			Vector3Template<T> b = at(t) - a;
+			return b.length();
+		}
+
+
+		/**
+		 * gets the position at distance t
+		 */
+		template <class S>
+		inline Vector3Template<S> at( const T a ) const
+		{
+			return Vector3Template<T>( point + direction * a );
+		}
+
+
+	};
+	
+	typedef RayTemplate<float> RayFloat;
+	typedef RayTemplate<double> RayDouble;
+	typedef RayTemplate<Scalar> Ray;
+}
+
+#endif

engine/Math/Rectangle.h

@@ -0,0 +1,91 @@
+//------------------------------------------------------------------------------
+// Author:		Gero Mueller <gero.mueller@cloo.de>
+// Copyright:	(c) 2006 Gero Mueller
+// License:		MIT License
+//------------------------------------------------------------------------------
+
+#ifndef BLUECORE_RECTANGLE_H
+#define BLUECORE_RECTANGLE_H
+
+#include "point.h"
+
+namespace BlueCore
+{
+	template<typename type>
+	struct Rectangle2DTemplate
+	{
+		type _x, _y, _width, _height;
+		
+		Rectangle2DTemplate() :
+			_x(0),
+			_y(0),
+			_width(0),
+			_height(0)
+		{
+		} 
+
+		Rectangle2DTemplate( int x, int y, int width, int height ) :
+			_x(x),
+			_y(y),
+			_width(width),
+			_height(height) 
+		{
+		}
+
+		Rectangle2DTemplate( const Point2DTemplate<type> &topLeft, const Point2DTemplate<type> &bottomRight ) :
+			_x(topLeft._x),
+			_y(topLeft._y),
+			_width(bottomRight._x - topLeft._x),
+			_height(bottomRight._y - topLeft._y)
+		{
+		} 
+		
+		Rectangle2DTemplate &operator=(const Rectangle2DTemplate<type> &rect)
+		{
+			_x = rect._x;
+			_y = rect._y;
+			_width = rect._width;
+			_height = rect._height;
+			
+			return *this;
+		}
+		
+		bool overlaps( const Rectangle2DTemplate<type> &rect )
+		{
+			if( (rect._x + rect._width) < _x )
+				return false; 
+
+			if( rect._x > (_x + _width) )
+				return false;
+
+			if( rect._y < (_y - _height) )
+				return false; 
+
+			if( (rect._y - rect._height) > _y )
+				return false;
+			
+			return true;				
+		}
+		
+		bool contains( const Point2DTemplate<type> &point ) const
+		{
+			if( point._x < _x )
+				return false;
+	
+			if( point._y < _y )
+				return false;
+	
+			if( point._x > (_x + _width) )
+				return false;
+	
+			if( point._y > (_y + _height) )
+				return false;
+				
+			return true;
+		}		
+	};
+	
+	typedef Rectangle2DTemplate<int> Rectangle2D;
+}
+	
+#endif

engine/Math/Scalar.h

@@ -0,0 +1,19 @@
+//------------------------------------------------------------------------------
+// Author:		Gero Mueller <gero.mueller@cloo.de>
+// Copyright:	(c) 2006 Gero Mueller
+// License:		MIT License
+//------------------------------------------------------------------------------
+
+#ifndef BLUECORE_COMMON_H
+#define BLUECORE_COMMON_H
+
+namespace BlueCore
+{
+	/* Scalar defines the default floating point type */
+	typedef double Scalar;
+	
+	const Scalar Pi = 3.141592653589793;
+	
+} // namespace bc
+
+#endif // BLUECORE_COMMON_H

engine/Math/Transformation.h

@@ -0,0 +1,51 @@
+#ifndef BLUECORE_TRANSFORMATION_H
+#define BLUECORE_TRANSFORMATION_H
+
+#include "Quaternion.h"
+#include "Vector.h"
+
+namespace BlueCore
+{
+template<class T> class TransformationTemplate
+{
+  public:
+
+    QuaternionTemplate<T> rotation;
+    Vector3Template<T> translation;
+
+    TransformationTemplate()
+    {
+    }
+
+    template<class S> TransformationTemplate(
+        const QuaternionTemplate<S> &rot) :
+      rotation(rot)
+    {
+    }
+
+    template<class S> TransformationTemplate(
+        const Vector3Template<S> &trans) :
+      translation(trans)
+    {
+    }
+
+    template<class R, class S> TransformationTemplate(
+        const QuaternionTemplate<S> &rot,
+        const Vector3Template<S> &trans) :
+      rotation(rot), translation(trans)
+    {
+    }
+
+    template<class S> Vector3Template<T> transform(const Vector3Template<S> &v)
+    {
+      return rotation.apply(v) + translation;
+    }
+};
+
+typedef TransformationTemplate<float> TransformationFloat;
+typedef TransformationTemplate<double> TransformationDouble;
+typedef TransformationTemplate<Scalar> Transformation;
+
+} // namespace BlueCore
+
+#endif /*TRANSFORMATION_H_*/

engine/Math/Vector.h

@@ -0,0 +1,432 @@
+//------------------------------------------------------------------------------
+// Author:		Gero Mueller <gero.mueller@cloo.de>
+// Copyright:	(c) 2006 Gero Mueller
+// License:		MIT License
+//------------------------------------------------------------------------------
+
+#ifndef BLUECORE_VECTOR_H
+#define BLUECORE_VECTOR_H
+
+// system includes
+#include <cmath>
+#include <ostream>
+
+// project includes
+#include "Scalar.h"
+
+namespace BlueCore
+{
+	template <class T>
+	class Vector3Template
+	{
+	public:
+	
+		T x, y, z;
+
+		/**
+		 * Default Contructor
+		 */
+		inline Vector3Template()
+		{
+			x = static_cast<T>(0);
+			y = static_cast<T>(0);
+			z = static_cast<T>(0);
+		}
+		
+		/**
+		 * Constructor from three values
+		 */
+		template <class S>
+		inline Vector3Template( S x, S y, S z )
+		{
+			this->x = static_cast<T>(x);
+			this->y = static_cast<T>(y);
+			this->z = static_cast<T>(z);
+		}
+
+		/**
+		 * copy contructor
+		 */
+		template <class S>
+		inline Vector3Template( Vector3Template<S> &a )
+		{
+			x = static_cast<T>(a.x);
+			y = static_cast<T>(a.y);
+			z = static_cast<T>(a.z);
+		}
+		
+
+		/**
+		 * add operataor
+		 */
+		template <class S>
+		inline Vector3Template<T> operator + (
+			const Vector3Template<S> &a ) const
+		{
+			return Vector3Template<T>(
+				x + static_cast<T>(a.x),
+				y + static_cast<T>(a.y),
+				z + static_cast<T>(a.z) );
+		}
+
+
+		/**
+		 * zero the vector
+		 */
+		inline void zero()
+		{
+			x = static_cast<T>(0);
+			y = static_cast<T>(0);
+			z = static_cast<T>(0);
+		}
+
+		/**
+		 * zero the vector
+		 */
+		inline bool isZero() const
+		{
+			if( x != 0.0 )
+				return false; 
+
+			if( y != 0.0 )
+				return false; 
+
+			if( z != 0.0 )
+				return false;
+				
+			return true; 
+		}
+
+
+		/**
+		 * add-assign operator
+		 */
+		template <class S>
+		inline Vector3Template<T> &operator += (
+			const Vector3Template<S> &a )
+		{
+			x += static_cast<T>(a.x);
+			y += static_cast<T>(a.y);
+			z += static_cast<T>(a.z);
+	
+			return *this;
+		}	
+
+
+		/**
+		 * sub operator
+		 */
+		template <class S>
+		inline Vector3Template<T> operator - (
+			const Vector3Template<S> &a ) const
+		{
+			return Vector3Template<T>(
+				x - static_cast<T>(a.x),
+				y - static_cast<T>(a.y),
+				z - static_cast<T>(a.z)
+			);
+		}
+
+
+		/**
+		 * sub-assign operator
+		 */
+		template <class S>
+		inline Vector3Template<T> &operator -= (
+			const Vector3Template<S> &a )
+		{
+			x -= static_cast<T>(a.x);
+			y -= static_cast<T>(a.y);
+			z -= static_cast<T>(a.z);
+	
+			return *this;
+		}	
+
+
+		/**
+		 * multiply by scalar operator
+		 */
+		template <class S>
+		inline Vector3Template<T> operator * ( const S a ) const
+		{
+			return Vector3Template<T>(
+				x * static_cast<T>(a),
+				y * static_cast<T>(a),
+				z * static_cast<T>(a)
+			);
+		}
+
+
+		/**
+		 * multiply-assign by scalar operator
+		 */
+		template <class S>
+		inline Vector3Template<T> &operator *= ( const S a )
+		{
+			x *= static_cast<T>(a);
+			y *= static_cast<T>(a);
+			z *= static_cast<T>(a);
+	
+			return *this;
+		}	
+
+
+		/**
+		 * divide by scalar operator
+		 */
+		template <class S>
+		inline Vector3Template<T> operator / ( const S a ) const
+		{
+			return Vector3Template<T>(
+				x / static_cast<T>(a),
+				y / static_cast<T>(a),
+				z / static_cast<T>(a)
+			);
+		}
+
+
+		/**
+		 * divide-assign by scalar operator
+		 */
+		template <class S>
+		inline Vector3Template<T> &operator /= ( const S a )
+		{
+			x /= static_cast<T>(a);
+			y /= static_cast<T>(a);
+			z /= static_cast<T>(a);
+	
+			return *this;
+		}	
+
+
+		/**
+		 * assign operator
+		 */
+		template <class S>
+		inline Vector3Template<T> operator = (
+			const Vector3Template<S> &a )
+		{
+			x = static_cast<T>(a.x);
+			y = static_cast<T>(a.y);
+			z = static_cast<T>(a.z);
+	
+			return *this;
+		}
+
+		inline Vector3Template<T> operator - (void) const
+		{
+			return Vector3Template<T>( -x, -y, -z );
+		}
+
+		/**
+		 * compare operator
+		 */
+		template <class S>
+		inline bool operator == ( const Vector3Template<S> &a )
+		{
+			if( x != static_cast<T>(a.x) )
+				return false;
+				
+			if( y != static_cast<T>(a.y) )
+				return false;
+
+			if( z != static_cast<T>(a.z) )
+				return false;
+	
+			return true;
+		}
+		
+
+		/**
+		 * anti compare operator
+		 */
+		template <class S>
+		inline bool operator != ( const Vector3Template<S> &a )
+		{
+			if( x != static_cast<T>(a.x) )
+				return true;
+				
+			if( y != static_cast<T>(a.y) )
+				return true;
+
+			if( z != static_cast<T>(a.z) )
+				return true;
+	
+			return false;
+		}
+
+
+		/**
+		 * return the length of the vector
+		 */
+		inline T length() const
+		{
+			return static_cast<T>( sqrt(x*x + y*y + z*z) );
+		}
+		
+
+		/**
+		 * length squared
+		 */
+		inline T length2() const
+		{
+			return x*x + y*y + z*z;
+		}
+	
+
+		/**
+		 * calculate the cross product
+		 */
+		template <class S>
+		inline Vector3Template<T> cross(
+			const Vector3Template<S> &a ) const
+		{
+			return Vector3Template<T>(
+				y * static_cast<T>(a.z) - z * static_cast<T>(a.y),
+				z * static_cast<T>(a.x) - x * static_cast<T>(a.z),
+				x * static_cast<T>(a.y) - y * static_cast<T>(a.x)
+			);
+		}
+	
+
+		/**
+		 * calculate dot product
+		 */
+		template <class S>
+		inline T dot( const Vector3Template<S> &a ) const
+		{
+			return (
+				x * static_cast<T>(a.x) +
+				y * static_cast<T>(a.y) +
+				z * static_cast<T>(a.z) );
+		}
+	
+
+		/**
+		 * check if vectors are nearly equal
+		 */
+		template <class S>
+		inline bool nearlyEquals(
+			const Vector3Template<S> &a,
+			T epsilon = 0.00001 ) const
+		{
+			if( fabs(x-static_cast<T>(a.x)) > epsilon )
+				return false;
+
+			if( fabs(y-static_cast<T>(a.y)) > epsilon )
+				return false;
+
+			if( fabs(z-static_cast<T>(a.z)) > epsilon )
+				return false;
+			
+			return true;
+		}
+	
+
+		/**
+		 * normalize the vector to length 1
+		 */
+		inline void normalize()
+		{
+			T a = length();
+			if( a == 0 )
+				return;
+				
+			x /= a;
+			y /= a;
+			z /= a;
+		}
+
+	
+		/**
+		 * get normalized vector
+		 */
+		inline Vector3Template<T> normalized() const
+		{
+			T a = length();
+			if( a == 0 )
+				return Vector3Template<T>( 0.0, 0.0, 1.0 );
+			return Vector3Template<T>( x / a, y / a, z / a );
+		}
+
+		/**
+		 * check if vectors are parallel
+		 */
+		inline bool parallel( const Vector3Template<T> &a ) const
+		{
+			return ( (static_cast<T>(a.x) / x) ==
+				(static_cast<T>(a.y) / y) ==
+				(static_cast<T>(a.z) / z) );
+		}
+
+
+		/**
+		 * check if vectors are parallel
+		 */
+		template <class S>
+		inline bool nearlyParallel(
+			const Vector3Template<S> &a,
+			T epsilon = 0.00001 ) const
+		{
+			T ax = static_cast<T>(a.x) / x;
+			T ay = static_cast<T>(a.y) / y;
+			T az = static_cast<T>(a.z) / z;
+
+			if( fabs(ax - ay) > epsilon	)
+				return false;
+
+			if( fabs(ax - az) > epsilon	)
+				return false;
+
+			if( fabs(ay - az) > epsilon	)
+				return false;
+
+			return true;
+		}
+
+		/**
+		 * check if vectors are orthogonal
+		 */
+		inline bool orthogonal( const Vector3Template<T> &a ) const
+		{
+			return ( dot(a) == static_cast<T>(0) );
+		}
+
+
+		/**
+		 * check if vectors are orthogonal
+		 */
+		template <class S>
+		inline bool nearlyOrthogonal(
+			const Vector3Template<S> &a,
+			T epsilon = 0.00001 ) const
+		{
+			return ( dot(a) < epsilon );
+		}		
+
+		T& operator[] (unsigned int i)
+		{
+			if( i == 0 )
+				return x;
+			else if( i == 1 )
+				return y;
+			else if( i == 2 )
+				return z;
+			else
+				throw "index out of bound";
+		}
+
+		friend std::ostream &operator << ( std::ostream& os, Vector3Template<T> v )
+		{
+			os << "( " << v.x << ", " << v.y << ", " << v.z << " )";
+			return os;
+		}
+
+	};
+	
+	typedef Vector3Template<float> Vector3Float;
+	typedef Vector3Template<double> Vector3Double;
+	typedef Vector3Template<Scalar> Vector3;
+}
+
+#endif