The Matrix Inverted

main.cpp

#include <iostream>
#include <assert.h>

#include "Matrix.h"

int main(int argc, const char * argv[])
{
    auto original = Matrix<4>
        (3.0f, 0.0f, 2.0f, -1.0f,
         1.0f, 2.0f, 0.0f, -2.0f,
         4.0f, 0.0f, 6.0f, -3.0f,
         5.0f, 0.0f, 2.0f, 0.0f);
    
    auto inverted = Matrix<4>::invert(original);
    
    assert((original * inverted).approximatelyEquals(Matrix<4>::identity()));
    
    return 0;
}

Matrix.h

#ifndef MatrixInversion_Matrix_h
#define MatrixInversion_Matrix_h

template <int Order>
class Matrix
{
public:
	// Static methods
    static Matrix<Order> identity();
	static Matrix<Order> invert(const Matrix<Order>& m);
	
	// Constructors
    template <typename... Arguments>
    Matrix(Arguments... values);
    
	Matrix();
    
    // Public methods
    bool approximatelyEquals(const Matrix<Order> &rhs) const;
	
	// Operators
	Matrix<Order> operator*(const Matrix<Order>& rhs) const;
	float operator()(const int i, const int j) const;
	float& operator()(const int i, const int j);
	
protected:
	// Protected data
	float m[Order * Order];
};

#include "Matrix.inl"

#endif

Matrix.inl

#ifndef MatrixInversion_Matrix_inl
#define MatrixInversion_Matrix_inl

#include <math.h>

template <int Order>
Matrix<Order>
Matrix<Order>::identity()
{
	Matrix<Order> r;
	for (int i = 0; i < Order; i++)
		for (int j = 0; j < Order; j++)
			r(i, j) = (i == j) ? 1 : 0;
	return r;
}

template <unsigned Order>
Matrix<Order - 1> getSubmatrix(Matrix<Order> src, int row, int col)
{
	int colCount = 0, rowCount = 0;
	
	Matrix<Order - 1> dest;
	for (int i = 0; i < Order; i++)
	{
		if (i != row)
		{
			colCount = 0;
			for (int j = 0; j < Order; j++)
			{
				if (j != col)
				{
					dest(rowCount, colCount) = src(i, j);
					colCount++;
				}
			}
			rowCount++;
		}
	}
	
	return dest;
}

// Forward declaration, so it can be used by calculateMinor
template <unsigned Order>
double calculateDeterminant(Matrix<Order> mat);

template <unsigned Order>
double calculateMinor(Matrix<Order> src, int row, int col)
{
    auto minorSubmatrix = getSubmatrix<Order>(src, row, col);
    return calculateDeterminant<Order - 1>(minorSubmatrix);
}

template <unsigned Order>
double calculateDeterminant(Matrix<Order> mat)
{
    float det = 0.0f;
    
    for (int i = 0; i < Order; i++)
    {
        // Get minor of element (0, i)
        float minor = calculateMinor<Order>(mat, 0, i);
        
        // If this is an odd-numbered row, negate the value.
        float factor = (i % 2 == 1) ? -1.0f : 1.0f;
        
        det += factor * mat(0, i) * minor;
    }
    
    return det;
}

// Template specialization for 2x2 matrix
template <>
double calculateDeterminant<2>(Matrix<2> mat)
{
    return mat(0, 0) * mat(1, 1) - mat(0, 1) * mat(1, 0);
}

template <int Order>
Matrix<Order>
Matrix<Order>::invert(const Matrix<Order>& m)
{
	// Calculate the inverse of the determinant of m.
    float det = calculateDeterminant<Order>(m);
	float inverseDet = 1.0f / det;
	
	Matrix<Order> result;
    
    for (int j = 0; j < Order; j++)
		for (int i = 0; i < Order; i++)
		{
            // Get minor of element (j, i) - not (i, j) because
            // this is where the transpose happens.
			float minor = calculateMinor<Order>(m, j, i);
            
            // Multiply by (−1)^{i+j}
            float factor = ((i + j) % 2 == 1) ? -1.0f : 1.0f;
            float cofactor = minor * factor;
            
			result(i, j) = inverseDet * cofactor;
		}
    
	return result;
}

template <int Order>
bool
Matrix<Order>::approximatelyEquals(const Matrix<Order> &rhs) const
{
    const float EPSILON = 0.0001f;
    for (int i = 0; i < Order; i++)
        for (int j = 0; j < Order; j++)
            if (fabs((*this)(i, j) - rhs(i, j)) > EPSILON)
                return false;
    return true;
}

template <int Order>
template <typename... Arguments>
Matrix<Order>::Matrix(Arguments... values)
    : m { values... }
{
    static_assert(sizeof...(Arguments) == Order * Order,
                  "Incorrect number of arguments");
}

template <int Order>
Matrix<Order>::Matrix()
{
    for (int i = 0; i < Order; i++)
        for (int j = 0; j < Order; j++)
			(*this)(i, j) = 0;
}

template <int Order>
Matrix<Order>
Matrix<Order>::operator*(const Matrix<Order>& rhs) const
{
	Matrix<Order> r;
	for (int i = 0; i < Order; i++)
		for (int j = 0; j < Order; j++)
			for (int k = 0; k < Order; k++)
				r(i, j) += (*this)(i, k) * rhs(k, j);
	return r;
}

template <int Order>
float
Matrix<Order>::operator()(const int i, const int j) const
{
    return m[(i * Order) + j];
}

template <int Order>
float&
Matrix<Order>::operator()(const int i, const int j)
{
    return m[(i * Order) + j];
}

#endif