Single header file for templated matrices.

matrix.h

#ifndef _MATRIX_H
#define _MATRIX_H

#include <cmath>
#include <algorithm>
#include <cstring>

#include "vector.h"

namespace
{
    class Mat4f
    {
    public:
        union
        {
            float m[4][4];
            struct 
            {
                float m00, m01, m02, m03;
                float m10, m11, m12, m13;
                float m20, m21, m22, m23;
                float m30, m31, m32, m33;
            };
        };

        Mat4f(float mm00 = 1.f, float mm01 = 0.f, float mm02 = 0.f, float mm03 = 0.f,
            float mm10 = 0.f, float mm11 = 1.f, float mm12 = 0.f, float mm13 = 0.f,
            float mm20 = 0.f, float mm21 = 0.f, float mm22 = 1.f, float mm23 = 0.f,
            float mm30 = 0.f, float mm31 = 0.f, float mm32 = 0.f, float mm33 = 1.f)
            : m00(mm00), m01(mm01), m02(mm02), m03(mm03)
            , m10(mm10), m11(mm11), m12(mm12), m13(mm13)
            , m20(mm20), m21(mm21), m22(mm22), m23(mm23)
            , m30(mm30), m31(mm31), m32(mm32), m33(mm33)
        {
        }

        Mat4f(Mat4f const& o)
            : m00(o.m00), m01(o.m01), m02(o.m02), m03(o.m03)
            , m10(o.m10), m11(o.m11), m12(o.m12), m13(o.m13)
            , m20(o.m20), m21(o.m21), m22(o.m22), m23(o.m23)
            , m30(o.m30), m31(o.m31), m32(o.m32), m33(o.m33)
        {
        }

        Mat4f& operator = (Mat4f const& o)
        {
            for (int i = 0; i < 4; ++i)
                for (int j = 0; j < 4; ++j)
                    m[i][j] = o.m[i][j];
            return *this;
        }

        Mat4f operator-() const;
        Mat4f transpose() const;
        float  trace() const { return m00 + m11 + m22 + m33; }

        Mat4f& operator += (Mat4f const& o);
        Mat4f& operator -= (Mat4f const& o);
        Mat4f& operator *= (Mat4f const& o);
        Mat4f& operator *= (float c);
    };


    inline Mat4f Mat4f::operator -() const
    {
        Mat4f res = *this;
        for(int i=0;i<4;++i)
            for (int j=0;j<4;++j)
                res.m[i][j] = -m[i][j]; 
        return res;
    }

    inline Mat4f Mat4f::transpose() const
    {
        Mat4f res;
        for (int i=0;i<4;++i)
            for (int j=0;j<4;++j)
                res.m[j][i] = m[i][j];
        return res;
    }

    inline Mat4f& Mat4f::operator += (Mat4f const& o)
    {
        for(int i=0;i<4;++i)
            for (int j=0;j<4;++j)
                m[i][j] += o.m[i][j]; 
        return *this;
    }

    inline Mat4f& Mat4f::operator -= (Mat4f const& o)
    {
        for(int i=0;i<4;++i)
            for (int j=0;j<4;++j)
                m[i][j] -= o.m[i][j]; 
        return *this;
    }

    inline Mat4f& Mat4f::operator *= (Mat4f const& o)
    {
        Mat4f temp;
        for (int i=0;i<4;++i)
        {
            for (int j=0;j<4;++j)
            {
                temp.m[i][j] = 0.f;
                for (int k=0;k<4;++k)
                    temp.m[i][j] += m[i][k] * o.m[k][j];
            }
        }
        *this = temp;
        return *this;
    }

    inline Mat4f& Mat4f::operator *= (float c)
    {
        for(int i=0;i<4;++i)
            for (int j=0;j<4;++j)
                m[i][j] *= c; 
        return *this;
    }

    inline Mat4f operator+(Mat4f const& m1, Mat4f const& m2)
    {
        Mat4f res = m1;
        return res+=m2;
    }

    inline Mat4f operator-(Mat4f const& m1, Mat4f const& m2)
    {
        Mat4f res = m1;
        return res-=m2;
    }

    inline Mat4f operator*(Mat4f const& m1, Mat4f const& m2)
    {
        Mat4f res;
        for (int i=0;i<4;++i)
        {
            for (int j=0;j<4;++j)
            {
                res.m[i][j] = 0.f;
                for (int k=0;k<4;++k)
                    res.m[i][j] += m1.m[i][k]*m2.m[k][j];
            }
        }
        return res;
    }

    inline Mat4f operator*(Mat4f const& m, float c)
    {
        Mat4f res = m;
        return res*=c;
    }

    inline Mat4f operator*(float c, Mat4f const& m)
    {
        Mat4f res = m;
        return res*=c;
    }

    inline Vec4f operator*(Mat4f const& m, Vec4f const& v)
    {
        Vec4f res(0, 0, 0, 0);
        for (int i = 0; i < 4; ++i)
		{
            //res[i] = 0.f;
			for (int j = 0; j < 4; ++j)
                res[i] += m.m[i][j] * v[j];
        }

        return res;
    }

	inline Vec3f operator*(Mat4f const& m, Vec3f const& v)
	{
		return Vec3f(m * Vec4f(v));
	}

    inline Mat4f inverse(Mat4f const& m)
    {
        int indxc[4], indxr[4];
        int ipiv[4] = { 0, 0, 0, 0 };
        float minv[4][4];
        Mat4f temp = m;  
        memcpy(minv,  &temp.m[0][0], 4*4*sizeof(float));
        for (int i = 0; i < 4; i++) {
            int irow = -1, icol = -1;
            float big = 0.;
            // Choose pivot
            for (int j = 0; j < 4; j++) {
                if (ipiv[j] != 1) {
                    for (int k = 0; k < 4; k++) {
                        if (ipiv[k] == 0) {
                            if (fabsf(minv[j][k]) >= big) {
                                big = float(fabsf(minv[j][k]));
                                irow = j;
                                icol = k;
                            }
                        }
                        else if (ipiv[k] > 1)
                            return Mat4f();
                    }
                }
            }
            ++ipiv[icol];
            // Swap rows _irow_ and _icol_ for pivot
            if (irow != icol) {
                for (int k = 0; k < 4; ++k)
                    std::swap(minv[irow][k], minv[icol][k]);
            }
            indxr[i] = irow;
            indxc[i] = icol;
            if (minv[icol][icol] == 0.)
                return Mat4f();

            // Set $m[icol][icol]$ to one by scaling row _icol_ appropriately
            float pivinv = 1.f / minv[icol][icol];
            minv[icol][icol] = 1.f;
            for (int j = 0; j < 4; j++)
                minv[icol][j] *= pivinv;

            // Subtract this row from others to zero out their columns
            for (int j = 0; j < 4; j++) {
                if (j != icol) {
                    float save = minv[j][icol];
                    minv[j][icol] = 0;
                    for (int k = 0; k < 4; k++)
                        minv[j][k] -= minv[icol][k]*save;
                }
            }
        }
        // Swap columns to reflect permutation
        for (int j = 3; j >= 0; j--) {
            if (indxr[j] != indxc[j]) {
                for (int k = 0; k < 4; k++)
                    std::swap(minv[k][indxr[j]], minv[k][indxc[j]]);
            }
        }

        Mat4f result;
        std::memcpy(&result.m[0][0], minv, 4*4*sizeof(float));
        return result;
    }

	inline Mat4f translation(Vec3f const& v)
    {
        return Mat4f(1, 0, 0, v.x, 
                     0, 1, 0, v.y, 
                     0, 0, 1, v.z, 
                     0, 0, 0, 1);
    }

    inline Mat4f rotation_x(float ang)
    {
        return Mat4f(1,             0,              0, 0, 
                     0, std::cos(ang), -std::sin(ang), 0,
                     0, std::sin(ang),  std::cos(ang), 0,
                     0,             0,              0, 1);
    }

    inline Mat4f rotation_y(float ang)
    {
        return Mat4f( std::cos(ang), 0, std::sin(ang), 0,
                      0,             1,             0, 0,
                     -std::sin(ang), 0, std::cos(ang), 0,
                      0,             0,             0, 1);
    }

    inline Mat4f rotation_z(float ang)
    {
        return Mat4f(std::cos(ang), -std::sin(ang), 0, 0,
                     std::sin(ang),  std::cos(ang), 0, 0,
                                 0,              0, 1, 0,
                                 0,              0, 0, 1);
    }

    inline Mat4f rotation(const Vec3f& a, float ang)
    {
        float cos = std::cos(ang);
        float sin = std::sin(ang);
        return Mat4f(      cos + a.x * a.x * (1 - cos), a.x * a.y * (1 - cos) - a.z * sin, a.x * a.z * (1 - cos) + a.y * sin, 0,
                     a.y * a.x * (1 - cos) + a.z * sin,       cos + a.y * a.y * (1 - cos), a.y * a.z * (1 - cos) - a.x * sin, 0,
                     a.z * a.x * (1 - cos) - a.y * sin, a.z * a.y * (1 - cos) + a.x * sin,    cos + a.z * a.z * (1 - cos),    0,
					                                 0,                                 0,                              0,    1);
    }

    inline Mat4f scale(Vec3f const& v)
    {
        return Mat4f(v.x, 0, 0, 0, 0, v.y, 0, 0, 0, 0, v.z, 0, 0, 0, 0, 1.f);
    }

	/// Transform a point using a matrix
    inline Vec3f transform_point(Vec3f const& p, Mat4f const& m)
    {
        Vec3f res = m * p;
        res.x += m.m03;
        res.y += m.m13;
        res.z += m.m23;
        return res;
    }
}

#endif // _MATRIX_H