Skip to content

Instantly share code, notes, and snippets.

@tgjones
Created October 20, 2014 14:51
Show Gist options
  • Save tgjones/06ddde4d9f7794d3883a to your computer and use it in GitHub Desktop.
Save tgjones/06ddde4d9f7794d3883a to your computer and use it in GitHub Desktop.
The Matrix Inverted
#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;
}
#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
#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
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment