Matrix and Vector C++ classes extended from the ones in this book: http://shop.oreilly.com/product/9780596804831.do

Matrix.hpp

#pragma once
#include "Vector.hpp"

template <typename T>
struct Matrix2 {
    Matrix2()
    {
        x.x = 1; x.y = 0;
        y.x = 0; y.y = 1;
    }
    Matrix2(const T* m)
    {
        x.x = m[0]; x.y = m[1];
        y.x = m[2]; y.y = m[3];
    }
    vec2 x;
    vec2 y;
};

template <typename T>
struct Matrix3 {
    Matrix3()
    {
        x.x = 1; x.y = 0; x.z = 0;
        y.x = 0; y.y = 1; y.z = 0;
        z.x = 0; z.y = 0; z.z = 1;
    }
    Matrix3(const T* m)
    {
        x.x = m[0]; x.y = m[1]; x.z = m[2];
        y.x = m[3]; y.y = m[4]; y.z = m[5];
        z.x = m[6]; z.y = m[7]; z.z = m[8];
    }
    Matrix3 Transposed() const
    {
        Matrix3 m;
        m.x.x = x.x; m.x.y = y.x; m.x.z = z.x;
        m.y.x = x.y; m.y.y = y.y; m.y.z = z.y;
        m.z.x = x.z; m.z.y = y.z; m.z.z = z.z;
        return m;
    }
    const T* Pointer() const
    {
        return &x.x;
    }
    vec3 x;
    vec3 y;
    vec3 z;
};

template <typename T>
struct Matrix4 {
    Matrix4()
    {
        x.x = 1; x.y = 0; x.z = 0; x.w = 0;
        y.x = 0; y.y = 1; y.z = 0; y.w = 0;
        z.x = 0; z.y = 0; z.z = 1; z.w = 0;
        w.x = 0; w.y = 0; w.z = 0; w.w = 1;
    }
    Matrix4(const Matrix3<T>& m)
    {
        x.x = m.x.x; x.y = m.x.y; x.z = m.x.z; x.w = 0;
        y.x = m.y.x; y.y = m.y.y; y.z = m.y.z; y.w = 0;
        z.x = m.z.x; z.y = m.z.y; z.z = m.z.z; z.w = 0;
        w.x = 0; w.y = 0; w.z = 0; w.w = 1;
    }
    Matrix4(const T* m)
    {
        x.x = m[0];  x.y = m[1];  x.z = m[2];  x.w = m[3];
        y.x = m[4];  y.y = m[5];  y.z = m[6];  y.w = m[7];
        z.x = m[8];  z.y = m[9];  z.z = m[10]; z.w = m[11];
        w.x = m[12]; w.y = m[13]; w.z = m[14]; w.w = m[15];
    }
    Matrix4 operator * (const Matrix4& b) const
    {
        Matrix4 m;
        m.x.x = x.x * b.x.x + x.y * b.y.x + x.z * b.z.x + x.w * b.w.x;
        m.x.y = x.x * b.x.y + x.y * b.y.y + x.z * b.z.y + x.w * b.w.y;
        m.x.z = x.x * b.x.z + x.y * b.y.z + x.z * b.z.z + x.w * b.w.z;
        m.x.w = x.x * b.x.w + x.y * b.y.w + x.z * b.z.w + x.w * b.w.w;
        m.y.x = y.x * b.x.x + y.y * b.y.x + y.z * b.z.x + y.w * b.w.x;
        m.y.y = y.x * b.x.y + y.y * b.y.y + y.z * b.z.y + y.w * b.w.y;
        m.y.z = y.x * b.x.z + y.y * b.y.z + y.z * b.z.z + y.w * b.w.z;
        m.y.w = y.x * b.x.w + y.y * b.y.w + y.z * b.z.w + y.w * b.w.w;
        m.z.x = z.x * b.x.x + z.y * b.y.x + z.z * b.z.x + z.w * b.w.x;
        m.z.y = z.x * b.x.y + z.y * b.y.y + z.z * b.z.y + z.w * b.w.y;
        m.z.z = z.x * b.x.z + z.y * b.y.z + z.z * b.z.z + z.w * b.w.z;
        m.z.w = z.x * b.x.w + z.y * b.y.w + z.z * b.z.w + z.w * b.w.w;
        m.w.x = w.x * b.x.x + w.y * b.y.x + w.z * b.z.x + w.w * b.w.x;
        m.w.y = w.x * b.x.y + w.y * b.y.y + w.z * b.z.y + w.w * b.w.y;
        m.w.z = w.x * b.x.z + w.y * b.y.z + w.z * b.z.z + w.w * b.w.z;
        m.w.w = w.x * b.x.w + w.y * b.y.w + w.z * b.z.w + w.w * b.w.w;
        return m;
    }
    Vector4<T> operator * (const Vector4<T>& b) const
    {
        Vector4<T> v;
		
		v.x = x.x * b.x + y.x * b.y + z.x * b.z + w.x * b.w;
		v.y = x.y * b.x + y.y * b.y + z.y * b.z + w.y * b.w;
		v.z = x.z * b.x + y.z * b.y + z.z * b.z + w.z * b.w;
		v.w = x.w * b.x + y.w * b.y + z.w * b.z + w.w * b.w;
		
        return v;
    }
    Matrix4& operator *= (const Matrix4& b)
    {
        Matrix4 m = *this * b;
        return (*this = m);
    }
    Matrix4 Transposed() const
    {
        Matrix4 m;
        m.x.x = x.x; m.x.y = y.x; m.x.z = z.x; m.x.w = w.x;
        m.y.x = x.y; m.y.y = y.y; m.y.z = z.y; m.y.w = w.y;
        m.z.x = x.z; m.z.y = y.z; m.z.z = z.z; m.z.w = w.z;
        m.w.x = x.w; m.w.y = y.w; m.w.z = z.w; m.w.w = w.w;
        return m;
    }
    Matrix3<T> ToMat3() const
    {
        Matrix3<T> m;
        m.x.x = x.x; m.y.x = y.x; m.z.x = z.x;
        m.x.y = x.y; m.y.y = y.y; m.z.y = z.y;
        m.x.z = x.z; m.y.z = y.z; m.z.z = z.z;
        return m;
    }
    const T* Pointer() const
    {
        return &x.x;
    }
	void Invert() 
	{
		x.x = y.z * z.w * w.y - y.w * z.z * w.y + y.w * z.y * w.z - y.y * z.w * w.z - y.z * z.y * w.w + y.y * z.z * w.w; 
		x.y = x.w * z.z * w.y - x.z * z.w * w.y - x.w * z.y * w.z + x.y * z.w * w.z + x.z * z.y * w.w - x.y * z.z * w.w; 
		x.z = x.z * y.w * w.y - x.w * y.z * w.y + x.w * y.y * w.z - x.y * y.w * w.z - x.z * y.y * w.w + x.y * y.z * w.w; 
		x.w = x.w * y.z * z.y - x.z * y.w * z.y - x.w * y.y * z.z + x.y * y.w * z.z + x.z * y.y * z.w - x.y * y.z * z.w; 
		y.x = y.w * z.z * w.x - y.z * z.w * w.x - y.w * z.x * w.z + y.x * z.w * w.z + y.z * z.x * w.w - y.x * z.z * w.w; 
		y.y = x.z * z.w * w.x - x.w * z.z * w.x + x.w * z.x * w.z - x.x * z.w * w.z - x.z * z.x * w.w + x.x * z.z * w.w; 
		y.z = x.w * y.z * w.x - x.z * y.w * w.x - x.w * y.x * w.z + x.x * y.w * w.z + x.z * y.x * w.w - x.x * y.z * w.w; 
		y.w = x.z * y.w * z.x - x.w * y.z * z.x + x.w * y.x * z.z - x.x * y.w * z.z - x.z * y.x * z.w + x.x * y.z * z.w; 
		z.x = y.y * z.w * w.x - y.w * z.y * w.x + y.w * z.x * w.y - y.x * z.w * w.y - y.y * z.x * w.w + y.x * z.y * w.w; 
		z.y = x.w * z.y * w.x - x.y * z.w * w.x - x.w * z.x * w.y + x.x * z.w * w.y + x.y * z.x * w.w - x.x * z.y * w.w; 
		z.z = x.y * y.w * w.x - x.w * y.y * w.x + x.w * y.x * w.y - x.x * y.w * w.y - x.y * y.x * w.w + x.x * y.y * w.w; 
		z.w = x.w * y.y * z.x - x.y * y.w * z.x - x.w * y.x * z.y + x.x * y.w * z.y + x.y * y.x * z.w - x.x * y.y * z.w; 
		w.x = y.z * z.y * w.x - y.y * z.z * w.x - y.z * z.x * w.y + y.x * z.z * w.y + y.y * z.x * w.z - y.x * z.y * w.z; 
		w.y = x.y * z.z * w.x - x.z * z.y * w.x + x.z * z.x * w.y - x.x * z.z * w.y - x.y * z.x * w.z + x.x * z.y * w.z; 
		w.z = x.z * y.y * w.x - x.y * y.z * w.x - x.z * y.x * w.y + x.x * y.z * w.y + x.y * y.x * w.z - x.x * y.y * w.z; 
		w.w = x.y * y.z * z.x - x.z * y.y * z.x + x.z * y.x * z.y - x.x * y.z * z.y - x.y * y.x * z.z + x.x * y.y * z.z; 
		
		T d = Determinant();
		
		x = x / d;
		y = y / d;
		z = z / d;
		w = w / d;
	}
	
	T Determinant() 
	{
		T value;
		value =
		x.w * y.z * z.y * w.x - x.z * y.w * z.y * w.x - x.w * y.y * z.z * w.x + x.y * y.w * z.z * w.x +
		x.z * y.y * z.w * w.x - x.y * y.z * z.w * w.x - x.w * y.z * z.x * w.y + x.z * y.w * z.x * w.y +
		x.w * y.x * z.z * w.y - x.x * y.w * z.z * w.y - x.z * y.x * z.w * w.y + x.x * y.z * z.w * w.y +
		x.w * y.y * z.x * w.z - x.y * y.w * z.x * w.z - x.w * y.x * z.y * w.z + x.x * y.w * z.y * w.z +
		x.y * y.x * z.w * w.z - x.x * y.y * z.w * w.z - x.z * y.y * z.x * w.w + x.y * y.z * z.x * w.w +
		x.z * y.x * z.y * w.w - x.x * y.z * z.y * w.w - x.y * y.x * z.z * w.w + x.x * y.y * z.z * w.w; 
		return value;
	} 
    static Matrix4<T> Identity()
    {
        return Matrix4();
    }
    static Matrix4<T> Translate( T x, T y, T z )
    {
        Matrix4 m;
		
		m.w.x = x; m.w.y = y; m.w.z = z; 
		
        return m;
    }
    
    vec3 translation()
    {
        vec4 tx = *this * vec4( 0, 0, 0, 1 );
        
        return vec3( tx.x, tx.y, tx.z );
    }
    
    static Matrix4<T> Scale( T s )
    {
		return Scale( s, s, s );
    }
    static Matrix4<T> Scale( T x, T y, T z )
    {
        Matrix4 m;
		
        m.x.x = x;
		m.y.y = y; 
		m.z.z = z;
		
        return m;
    }
    static Matrix4<T> Rotate( T degrees )
    {
		return RotateZ( degrees );
    }
	static Matrix4<T> RotateX( T rs, T rc )
	{
		Matrix4 m;
		
		m.y.y = rc; m.y.z = rs;
		m.z.y = -rs; m.z.z = rc;
		
		return m;
	}
	static Matrix4<T> RotateX( T degrees )
	{
		//Vector2<T> sc = Vector2<T>::SinCos( degrees );
		
		T radians = degrees * 3.14159f / 180.0f;
        T s = std::sin( radians );
        T c = std::cos( radians );
		
		return RotateX( s, c );
		//return RotateX( sc.x, sc.y );
	}
	static Matrix4<T> RotateY( T rs, T rc )
	{
		Matrix4 m;
		
		m.x.x = rc; m.x.z = -rs;
		m.z.x = rs; m.z.z = rc;
		
		return m;
	}
	static Matrix4<T> RotateY( T degrees )
	{
		T radians = degrees * 3.14159f / 180.0f;
        T s = std::sin( radians );
        T c = std::cos( radians );
		
		return RotateY( s, c );
	}
	static Matrix4<T> RotateZ( T rs, T rc )
	{
		Matrix4 m;
		
		m.x.x = rc; m.x.y = rs;
		m.y.x = -rs; m.y.y = rc;
		
		return m;
	}
	static Matrix4<T> RotateZ( T degrees )
	{
		T radians = degrees * 3.14159f / 180.0f;
        T s = std::sin( radians );
        T c = std::cos( radians );
		
		return RotateZ( s, c );
	}
	static Matrix4<T> ShearZ( T dx, T dy )
	{
		Matrix4 m;
		
		m.x.z = dx;
		m.y.z = dy;
		
		return m;
	}
    static Matrix4<T> Frustum(T left, T right, T bottom, T top, T near, T far)
    {
        T a = 2 * near / (right - left);
        T b = 2 * near / (top - bottom);
        T c = (right + left) / (right - left);
        T d = (top + bottom) / (top - bottom);
        T e = - (far + near) / (far - near);
        T f = -2 * far * near / (far - near);
        Matrix4 m;
        m.x.x = a; m.x.y = 0; m.x.z = 0; m.x.w = 0;
        m.y.x = 0; m.y.y = b; m.y.z = 0; m.y.w = 0;
        m.z.x = c; m.z.y = d; m.z.z = e; m.z.w = -1;
        m.w.x = 0; m.w.y = 0; m.w.z = f; m.w.w = 1;
        return m;
    }
    static Matrix4<T> LookAt(const Vector3<T>& eye,
                             const Vector3<T>& target,
                             const Vector3<T>& up)
    {
        Vector3<T> z = (eye - target).Normalized();
        Vector3<T> x = up.Cross(z).Normalized();
        Vector3<T> y = z.Cross(x).Normalized();
        
        Matrix4<T> m;
        m.x = Vector4<T>(x, 0);
        m.y = Vector4<T>(y, 0);
        m.z = Vector4<T>(z, 0);
        m.w = Vector4<T>(0, 0, 0, 1);
        
        Vector4<T> eyePrime = m * Vector4<T>(-eye, 1);
        m = m.Transposed();
        m.w = eyePrime;
        
        return m;
    }
	
    void unserialise( std::istream & stream )
    {
        std::string semicolon;
        
        x.unserialise( stream );
        stream >> std::skipws >> semicolon;
        y.unserialise( stream );
        stream >> std::skipws >> semicolon;
        z.unserialise( stream );
        stream >> std::skipws >> semicolon;
        w.unserialise( stream );
    }
    
    void serialise( std::ostream & stream )
    {
        x.serialise( stream );
        stream << "; ";
        y.serialise( stream );
        stream << "; ";
        z.serialise( stream );
        stream << "; ";
        w.serialise( stream );
    }
    
    
	void dump( std::ostream & stream )
	{
		x.dump( stream );
		y.dump( stream );
		z.dump( stream );
		w.dump( stream );
	}
	
    vec4 x;
    vec4 y;
    vec4 z;
    vec4 w;
};

typedef Matrix2<float> mat2;
typedef Matrix3<float> mat3;
typedef Matrix4<float> mat4;

Vector.hpp

#pragma once
#include <cmath>
#include <iostream>

const float Pi = 4 * std::atan(1.0f);
const float TwoPi = 2 * Pi;

template <typename T>
struct Vector2 {
    Vector2() {}
    Vector2(T x, T y) : x(x), y(y) {}
    T Dot(const Vector2& v) const
    {
        return x * v.x + y * v.y;
    }
    T Cross(const Vector2& v) const
    {
        return x * v.y - y * v.x;
    }
    Vector2 operator+(const Vector2& v) const
    {
        return Vector2(x + v.x, y + v.y);
    }
    Vector2 operator-(const Vector2& v) const
    {
        return Vector2(x - v.x, y - v.y);
    }
    Vector2 operator-() const
    {
        return Vector2(-x, -y);
    }
    void operator+=(const Vector2& v)
    {
        *this = Vector2(x + v.x, y + v.y);
    }
    void operator-=(const Vector2& v)
    {
        *this = Vector2(x - v.x, y - v.y);
    }
    Vector2 operator/(float s) const
    {
        return Vector2(x / s, y / s);
    }
    Vector2 operator*(float s) const
    {
        return Vector2(x * s, y * s);
    }
    void operator/=(float s)
    {
        *this = Vector2(x / s, y / s);
    }
    void operator*=(float s)
    {
        *this = Vector2(x * s, y * s);
    }
    Vector2 &Normalize()
    {
        float s = 1.0f / Length();
        x *= s;
        y *= s;
        
        return *this;
    }
    Vector2 Normalized() const 
    {
        Vector2 v = *this;
        v.Normalize();
        return v;
    }
    T LengthSquared() const
    {
        return x * x + y * y;
    }
    T Length() const
    {
        return sqrt(LengthSquared());
    }
    Vector2 Bisector( const Vector2& v) const
    {
        return ( Normalized() + v.Normalized()).Normalized();
    }
    T intersectionCosine( const Vector2& v) const
    {
        return Normalized().Dot( v.Normalized());
    }
    Vector2 &Clockwise()
    {
        *this = Vector2( y, -x );
        return *this;
    }
    Vector2 &AntiClockwise()
    {
        *this = Vector2( -y, x );
        return *this;
    }
    Vector2 &Reflect()
    {
        *this = Vector2( -x, -y );
        return *this;
    }
    operator Vector2<float>() const
    {
        return Vector2<float>(x, y);
    }
    bool operator==(const Vector2& v) const
    {
        return x == v.x && y == v.y;
    }
    Vector2 Lerp(float t, const Vector2& v) const
    {
        return Vector2(x * (1 - t) + v.x * t,
                       y * (1 - t) + v.y * t);
    }
	static Vector2 SinCos( T degrees )
	{
		T radians = degrees * 3.14159f / 180.0f;
        
		return Vector2( std::sin( radians ), std::cos( radians ));
	}
    bool withinEpsilson(const Vector2& v, float epsilon) const
    {
        return(( fabsf( v.x - x ) < epsilon ) && ( fabsf( v.y - y ) < epsilon ));
    }
    
    friend std::ostream& operator<< (std::ostream& out, const Vector2& v )
    {
        out << v.x << ", " << v.y;
        
        return out;
    }
    
    std::ostream & serialise( std::ostream & stream )
    {
        stream << x << ", " << y;
        
        return stream;
    }
    
    void dump( std::ostream & stream )
	{
        serialise( stream );
		stream << "\n";
	}
    
    template <typename P>
    P* Write(P* pData)
    {
        Vector2* pVector = (Vector2*) pData;
        *pVector++ = *this;
        return (P*) pVector;
    }
    T x;
    T y;
};

template <typename T>
struct Vector3 {
    Vector3() {}
    Vector3(T x, T y, T z) : x(x), y(y), z(z) {}
    void Normalize()
    {
        float s = 1.0f / std::sqrt(x * x + y * y + z * z);
        x *= s;
        y *= s;
        z *= s;
    }
    Vector3 Normalized() const 
    {
        Vector3 v = *this;
        v.Normalize();
        return v;
    }
    Vector3 Cross(const Vector3& v) const
    {
        return Vector3(y * v.z - z * v.y,
                       z * v.x - x * v.z,
                       x * v.y - y * v.x);
    }
    T Dot(const Vector3& v) const
    {
        return x * v.x + y * v.y + z * v.z;
    }
	T Length() const
    {
        return powf( x * x + y * y + z * z, 1.0f / 3.0f );
    }
    Vector3 operator+(const Vector3& v) const
    {
        return Vector3(x + v.x, y + v.y,  z + v.z);
    }
    
    void operator+=(const Vector3& v)
    {
        x += v.x;
        y += v.y;
        z += v.z;
    }
    void operator-=(const Vector3& v)
    {
        x -= v.x;
        y -= v.y;
        z -= v.z;
    }
    void operator/=(T s)
    {
        x /= s;
        y /= s;
        z /= s;
    }
    Vector3 operator-(const Vector3& v) const
    {
        return Vector3(x - v.x, y - v.y,  z - v.z);
    }
    Vector3 operator-() const
    {
        return Vector3(-x, -y, -z);
    }
    Vector3 operator+(const T s) const
    {
        return Vector3(x + s, y + s,  z + s);
    }
    Vector3 operator-(const T s) const
    {
        return Vector3(x - s, y - s,  z - s);
    }
    Vector3 operator*(T s) const
    {
        return Vector3(x * s, y * s, z * s);
    }
    Vector3 operator/(T s) const
    {
        return Vector3(x / s, y / s, z / s);
    }
    bool operator==(const Vector3& v) const
    {
        return x == v.x && y == v.y && z == v.z;
    }
    Vector3 Lerp(float t, const Vector3& v) const
    {
        return Vector3(x * (1 - t) + v.x * t,
                       y * (1 - t) + v.y * t,
                       z * (1 - t) + v.z * t);
    }
    const T* Pointer() const
    {
        return &x;
    }
    
    void serialise( std::ostream & stream )
    {
        stream << x << ", " << y << ", " << z;
    }
    
    void dump( std::ostream & stream )
	{
        serialise( stream );
		stream << "\n";
	}
    
    template <typename P>
    P* Write(P* pData)
    {
        Vector3<T>* pVector = (Vector3<T>*) pData;
        *pVector++ = *this;
        return (P*) pVector;
    }
    T x;
    T y;
    T z;
};

template <typename T>
struct Vector4 {
    Vector4() {}
    Vector4(T x, T y, T z, T w) : x(x), y(y), z(z), w(w) {}
    Vector4(const Vector3<T>& v, T w) : x(v.x), y(v.y), z(v.z), w(w) {}
    T Dot(const Vector4& v) const
    {
        return x * v.x + y * v.y + z * v.z + w * v.w;
    }
    Vector4 Lerp(float t, const Vector4& v) const
    {
        return Vector4(x * (1 - t) + v.x * t,
                       y * (1 - t) + v.y * t,
                       z * (1 - t) + v.z * t,
                       w * (1 - t) + v.w * t);
    }
    const T* Pointer() const
    {
        return &x;
    }
	
	Vector4 operator+(const Vector4& v) const
    {
        return Vector4(x + v.x, y + v.y,  z + v.z, w + v.w);
    }
    
    Vector4 operator-(const Vector4& v) const
    {
        return Vector4(x - v.x, y - v.y, z - v.z, w - v.w);
    }
	
	Vector4 operator/(float s) const
    {
        return Vector4(x / s, y / s, z / s, w / s);
    }
	
	Vector4 operator*(float s) const
    {
        return Vector4(x * s, y * s, z * s, w * s);
    }
	
    void unserialise( std::istream & stream )
    {
        std::string comma;
        
        stream >> std::skipws >> x;
        stream >> std::skipws >> comma;
        stream >> std::skipws >> y;
        stream >> std::skipws >> comma;
        stream >> std::skipws >> z;
        stream >> std::skipws >> comma;
        stream >> std::skipws >> w;
    }
    
    void serialise( std::ostream & stream )
    {
        stream << x << ", " << y << ", " << z << ", " << w;
    }
    
	void dump( std::ostream & stream )
	{
        serialise( stream );
		stream << "\n";
	}
    
    static Vector4<T>Zero()
    {
        return Vector4( 0, 0, 0, 0 );
    }
	
    T x;
    T y;
    T z;
    T w;
};

typedef Vector2<bool> bvec2;

typedef Vector2<int> ivec2;
typedef Vector3<int> ivec3;
typedef Vector4<int> ivec4;

typedef Vector2<float> vec2;
typedef Vector3<float> vec3;
typedef Vector4<float> vec4;