Made it simpler to switch between old and new SimpleMath

src/SimpleMathOld/SimpleMath.h

@@ -0,0 +1,11 @@
+#ifndef _SIMPLEMATH_H
+#define _SIMPLEMATH_H
+
+#include "SimpleMathFixed.h"
+#include "SimpleMathDynamic.h"
+#include "SimpleMathMixed.h"
+#include "SimpleMathQR.h"
+#include "SimpleMathCommaInitializer.h"
+#include "SimpleMathGL.h"
+
+#endif /* _SIMPLEMATH_H */

src/SimpleMathOld/SimpleMathBlock.h

@@ -0,0 +1,194 @@
+/**
+ * This is a highly inefficient math library. It was conceived by Martin
+ * Felis <martin.felis@iwr.uni-heidelberg.de> while he was compiling code
+ * that uses a highly efficient math library.
+ *
+ * It is intended to be used as a fast compiling substitute for the
+ * blazingly fast Eigen3 library and tries to mimic its API to a certain
+ * extend.
+ *
+ * Feel free to use it wherever you like. However, no guarantees are given
+ * that this code does what it says it would.
+ */
+
+#ifndef SIMPLEMATHBLOCK_H
+#define SIMPLEMATHBLOCK_H
+
+#include <cstdlib>
+#include <cmath>
+#include <iostream>
+#include <assert.h>
+
+#include "compileassert.h"
+
+// #include "SimpleMathQR.h"
+
+/** \brief Namespace for a highly inefficient math library
+ *
+ */
+namespace SimpleMath {
+
+/** \brief Namespace for fixed size elements
+ */
+// forward declaration
+template <typename val_type, unsigned int nrows, unsigned int ncols>
+class Matrix;
+
+template <typename matrix_type, typename val_type>
+class Block {
+	public:
+		typedef val_type value_type;
+
+		Block() :
+			mParentRows(0),
+			mParentCols(0),
+			mParentRowStart(0),
+			mParentColStart(0)
+		{ }
+		Block (const matrix_type &matrix, const unsigned int row_start, const unsigned int col_start, const unsigned int row_count, const unsigned int col_count) :
+			mParentRows (matrix.rows()),
+			mParentCols (matrix.cols()),
+			mParentRowStart (row_start),
+			mParentColStart (col_start),
+			mRowCount (row_count),
+			mColCount (col_count),
+			mTransposed (false) {
+				assert (mParentRows >= mParentRowStart + mRowCount);
+				assert (mParentCols >= mParentColStart + mColCount);
+
+				// without the following line we could not create blocks from const
+				// matrices
+				mParentMatrix = const_cast<matrix_type*>(&matrix);
+			}
+
+		// copy data from the other block into this
+		Block& operator=(const Block &other) {
+			if (this != &other) {
+				if (mRowCount != other.rows() || mColCount != other.cols()) {
+					std::cerr << "Error: cannot assign blocks of different size (left is " << mRowCount << "x" << mColCount << " right is " << other.rows() << "x" << other.cols() << ")!" << std::endl;
+					abort();
+				}
+				
+				value_type* temp_values = new value_type [mRowCount * mColCount];
+
+				for (unsigned int i = 0; i < mRowCount; i++) {
+					for (unsigned int j = 0; j < mColCount; j++) {
+						temp_values[i * mColCount + j] = static_cast<value_type>(other(i,j));
+					}
+				}
+
+				for (unsigned int i = 0; i < mRowCount; i++) {
+					for (unsigned int j = 0; j < mColCount; j++) {
+						(*this)(i,j) = temp_values[i * mColCount + j];
+					}
+				}
+
+				delete[] temp_values;
+			}
+
+			return *this;
+		}
+
+		template <typename other_matrix_type>
+		// copy data from the other block into this
+		Block& operator=(const other_matrix_type &other) {
+			if (mRowCount != other.rows() || mColCount != other.cols()) {
+				std::cerr << "Error: cannot assign blocks of different size (left is " << mRowCount << "x" << mColCount << " right is " << other.rows() << "x" << other.cols() << ")!" << std::endl;
+				abort();
+			}
+
+			value_type *temp_values = new value_type[mRowCount * mColCount];
+
+			for (unsigned int i = 0; i < mRowCount; i++) {
+				for (unsigned int j = 0; j < mColCount; j++) {
+					temp_values[i * mColCount + j] = static_cast<value_type>(other(i,j));
+				}
+			}
+
+			for (unsigned int i = 0; i < mRowCount; i++) {
+				for (unsigned int j = 0; j < mColCount; j++) {
+					(*this)(i,j) = temp_values[i * mColCount + j];
+				}
+			}
+			
+			delete[] temp_values;
+
+			return *this;
+		}
+
+		unsigned int rows() const {
+			if (!mTransposed)
+				return mRowCount;
+
+			return mColCount;
+		}
+		unsigned int cols() const {
+			if (!mTransposed)
+				return mColCount;
+			
+			return mRowCount;
+		}
+		const val_type& operator() (const unsigned int i, const unsigned int j) const {
+
+			if (!mTransposed) {
+				assert (i < mRowCount);
+				assert (j < mColCount);
+				return (*mParentMatrix) (i + mParentRowStart, j + mParentColStart);
+			}
+
+			return (*mParentMatrix) (j + mParentRowStart, i + mParentColStart);
+		}
+
+		val_type& operator() (const unsigned int i, const unsigned int j) {
+			if (!mTransposed) {
+				assert (i < mRowCount);
+				assert (j < mColCount);
+				return (*mParentMatrix) (i + mParentRowStart, j + mParentColStart);
+			}
+
+			assert (j < mRowCount);
+			assert (i < mColCount);
+			return (*mParentMatrix) (j + mParentRowStart, i + mParentColStart);
+		}
+
+		Block transpose() const {
+			Block result (*this);
+			result.mTransposed = mTransposed ^ true;
+			return result;
+		}
+
+	private:
+		matrix_type *mParentMatrix;
+		const unsigned int mParentRows;
+		const unsigned int mParentCols;
+		const unsigned int mParentRowStart;
+		const unsigned int mParentColStart;
+		const unsigned int mRowCount;
+		const unsigned int mColCount;
+		bool mTransposed;
+};
+
+template <typename matrix_type, typename val_type>
+inline std::ostream& operator<<(std::ostream& output, const Block<matrix_type, val_type> &block) {
+	unsigned int i,j;
+	for (i = 0; i < block.rows(); i++) {
+		output << "[ ";
+		for (j = 0; j < block.cols(); j++) {
+			output << block(i,j);
+
+			if (j < block.cols() - 1)
+				output << ", ";
+		}
+		output << " ]";
+		
+		if (block.rows() > 1 && i < block.rows() - 1)
+			output << std::endl;
+	}
+
+	return output;
+}
+
+
+}
+
+#endif /* SIMPLEMATHBLOCK_H */

src/SimpleMathOld/SimpleMathCholesky.h

@@ -0,0 +1,94 @@
+#ifndef _SIMPLE_MATH_CHOLESKY_H
+#define _SIMPLE_MATH_CHOLESKY_H
+
+#include <iostream>
+#include <limits>
+
+#include "SimpleMathDynamic.h"
+
+namespace SimpleMath {
+
+	template <typename matrix_type>
+	class LLT {
+		public:
+			typedef typename matrix_type::value_type value_type;	
+
+		private:
+			LLT () {}
+
+			typedef Dynamic::Matrix<value_type> MatrixXXd;
+			typedef Dynamic::Matrix<value_type> VectorXd;
+			
+			bool mIsFactorized;
+			MatrixXXd mL;
+
+		public:
+			LLT (const matrix_type &matrix) :
+				mIsFactorized(false),
+				mL(matrix) {
+					compute();
+			}
+			LLT compute() {
+				for (int i = 0; i < mL.rows(); i++) {
+					for (int j = 0; j < mL.rows(); j++) {
+						if (j > i) {
+							mL(i,j) = 0.;
+							continue;
+						}
+						double s = mL(i,j);
+						for (int k = 0; k < j; k++) {
+							s = s - mL(i,k) * mL(j,k);
+						}
+						if (i > j) {
+							mL(i,j) = s / mL(j,j);
+						} else if (s > 0.) {
+							mL (i,i) = sqrt (s);
+						} else {
+							std::cerr << "Error computing Cholesky decomposition: matrix not symmetric positive definite!" << std::endl;
+							assert (false);
+						}
+					}
+				}
+
+				mIsFactorized = true;
+
+				return *this;
+			}
+			Dynamic::Matrix<value_type> solve (
+					const Dynamic::Matrix<value_type> &rhs
+					) const {
+				assert (mIsFactorized);
+
+				VectorXd y (mL.rows());
+				for (unsigned int i = 0; i < mL.rows(); i++) {
+					double temp = rhs[i];
+
+					for (unsigned int j = 0; j < i; j++) {
+						temp = temp - mL(i,j) * y[j];
+					}
+
+					y[i] = temp / mL(i,i);
+				}
+
+				VectorXd x (mL.rows());
+				for (int i = mL.rows() - 1; i >= 0; i--) {
+					double temp = y[i];
+
+					for (unsigned int j = i + 1; j < mL.rows(); j++) {
+						temp = temp - mL(j, i) * x[j];
+					}
+
+					x[i] = temp / mL(i,i);
+				}
+
+				return x;
+			}
+			Dynamic::Matrix<value_type> matrixL() const {
+				return mL;
+			}
+	};
+
+}
+
+/* _SIMPLE_MATH_CHOLESKY_H */
+#endif

src/SimpleMathOld/SimpleMathCommaInitializer.h

@@ -0,0 +1,69 @@
+#ifndef _SIMPLE_MATH_COMMA_INITIALIZER_H
+#define _SIMPLE_MATH_COMMA_INITIALIZER_H
+
+#include <iostream>
+#include <limits>
+
+#include "SimpleMathFixed.h"
+#include "SimpleMathDynamic.h"
+
+namespace SimpleMath {
+
+	template <typename matrix_type>
+	class CommaInitializer {
+		public:
+			typedef typename matrix_type::value_type value_type;	
+
+			CommaInitializer(matrix_type &matrix, const value_type &value) :
+				mElementWasAdded (false) {
+				assert (matrix.cols() > 0 && matrix.rows() > 0);
+				mParentMatrix = &matrix;
+				mRowIndex = 0;
+				mColIndex = 0;
+				(*mParentMatrix)(mRowIndex, mColIndex) = value;
+			}
+			CommaInitializer(matrix_type &matrix, unsigned int row_index, unsigned int col_index) :
+				mRowIndex (row_index),
+				mColIndex (col_index),
+				mElementWasAdded (false) {
+				assert (matrix.cols() > 0 && matrix.rows() > 0);
+				mParentMatrix = &matrix;
+				mRowIndex = row_index;
+				mColIndex = col_index;
+			}
+			~CommaInitializer() {
+				if (!mElementWasAdded 
+						&& (mColIndex + 1 < mParentMatrix->cols() || mRowIndex + 1 < mParentMatrix->rows())) {
+					std::cerr << "Error: too few elements passed to CommaInitializer! Expected " << mParentMatrix->size() << " but was given " << mRowIndex * mParentMatrix->cols() + mColIndex + 1 << std::endl;
+					abort();
+				}
+			}
+			CommaInitializer<matrix_type> operator, (const value_type &value) {
+				mColIndex++;
+				if (mColIndex >= mParentMatrix->cols()) {
+						mRowIndex++;
+						mColIndex = 0;
+				}
+				if (mRowIndex == mParentMatrix->rows() && mColIndex == 0 ) {
+					std::cerr << "Error: too many elements passed to CommaInitializer! Expected " << mParentMatrix->size() << " but was given " << mRowIndex * mParentMatrix->cols() + mColIndex + 1 << std::endl;
+					abort();
+				}
+				(*mParentMatrix)(mRowIndex, mColIndex) = value;
+				mElementWasAdded = true;
+
+				return CommaInitializer (*mParentMatrix, mRowIndex, mColIndex);
+			}
+
+		private:
+			CommaInitializer() {}
+
+			matrix_type *mParentMatrix;
+			unsigned int mRowIndex;
+			unsigned int mColIndex;
+			bool mElementWasAdded;
+	};
+
+}
+
+/* _SIMPLE_MATH_COMMA_INITIALIZER_H */
+#endif

src/SimpleMathOld/SimpleMathDynamic.h

@@ -0,0 +1,678 @@
+/**
+ * This is a highly inefficient math library. It was conceived by Martin
+ * Felis <martin.felis@iwr.uni-heidelberg.de> while he was compiling code
+ * that uses a highly efficient math library.
+ *
+ * It is intended to be used as a fast compiling substitute for the
+ * blazingly fast Eigen3 library and tries to mimic its API to a certain
+ * extend.
+ *
+ * Feel free to use it wherever you like. However, no guarantees are given
+ * that this code does what it says it would.
+ */
+
+#ifndef SIMPLEMATHDYNAMIC_H
+#define SIMPLEMATHDYNAMIC_H
+
+#include <sstream>
+#include <cstdlib>
+#include <assert.h>
+#include <algorithm>
+
+#include "compileassert.h"
+#include "SimpleMathBlock.h"
+
+/** \brief Namespace for a highly inefficient math library
+ *
+ */
+namespace SimpleMath {
+
+template <typename matrix_type>
+class LLT;
+
+template <typename matrix_type>
+class HouseholderQR;
+
+template <typename matrix_type>
+class ColPivHouseholderQR;
+
+template <typename matrix_type>
+class CommaInitializer;
+
+namespace Fixed {
+	template <typename val_type, unsigned int ncols, unsigned int nrows> class Matrix;
+}
+
+
+/** \brief Namespace for elements of varying size.
+ */
+namespace Dynamic {
+
+// forward declaration
+template <typename val_type>
+class Matrix;
+
+/** \brief Class for both matrices and vectors.
+ */
+template <typename val_type>
+class Matrix {
+	public:
+		typedef Matrix<val_type> matrix_type;
+		typedef val_type value_type;
+
+		Matrix() :
+			nrows (0),
+			ncols (0),
+			mapped_data (false),
+			mData (NULL) {};
+		Matrix(unsigned int rows) :
+			nrows (rows),
+			ncols (1),
+			mapped_data (false) {
+				mData = new val_type[rows];
+			}
+		Matrix(unsigned int rows, unsigned int cols) :
+			nrows (rows),
+			ncols (cols),
+			mapped_data (false) {
+				mData = new val_type[rows * cols];
+			}
+		Matrix(unsigned int rows, unsigned int cols, val_type *data_ptr) :
+			nrows (rows),
+			ncols (cols),
+			mapped_data (true) {
+				mData = data_ptr;
+			}
+	
+		unsigned int rows() const {
+			return nrows;
+		}
+
+		unsigned int cols() const {
+			return ncols;
+		}
+
+		unsigned int size() const {
+			return nrows * ncols;
+		}
+		void resize (unsigned int rows, unsigned int cols=1) {
+			if (nrows * ncols > 0 && mData != NULL && mapped_data == false) {
+				delete[] mData;
+			}
+
+			nrows = rows;
+			ncols = cols;
+
+			mData = new val_type[nrows * ncols];
+		}
+
+		void conservativeResize (unsigned int rows, unsigned int cols = 1) {
+			Matrix <val_type> result = Matrix<val_type>::Zero(rows, cols);
+
+			unsigned int arows = std::min (rows, nrows);
+			unsigned int acols = std::min (cols, ncols);
+
+			for (unsigned int i = 0; i < arows; i++) {
+				for (unsigned int j = 0; j < acols; j++) {
+					result(i,j) = (*this)(i,j);
+				}
+			}
+
+			*this = result;	
+		}
+
+		Matrix(const Matrix &matrix) :
+			nrows (matrix.nrows),
+			ncols (matrix.ncols),
+			mapped_data (false) {
+			unsigned int i;
+		
+			mData = new val_type[nrows * ncols];
+
+			for (i = 0; i < nrows * ncols; i++)
+				mData[i] = matrix.mData[i];
+		}
+		Matrix& operator=(const Matrix &matrix) {
+			if (this != &matrix) {
+				if (!mapped_data) {
+					delete[] mData;
+
+					nrows = matrix.nrows;
+					ncols = matrix.ncols;
+					mapped_data = false;
+
+					mData = new val_type[nrows * ncols];
+
+					unsigned int i;
+					for (i = 0; i < nrows * ncols; i++)
+						mData[i] = matrix.mData[i];
+				} else {
+					// we overwrite any existing data
+					nrows = matrix.nrows;
+					ncols = matrix.ncols;
+					mapped_data = true;
+
+					unsigned int i;
+					for (i = 0; i < nrows * ncols; i++)
+						mData[i] = matrix.mData[i];
+				}
+			}
+			return *this;
+		}
+
+		CommaInitializer<matrix_type> operator<< (const val_type& value) {
+			return CommaInitializer<matrix_type> (*this, value);
+		}
+
+		// conversion different val_types
+		template <typename other_type>
+		Matrix (const Matrix<other_type> &matrix) :
+			nrows (matrix.rows()),
+			ncols (matrix.cols()),
+			mapped_data(false) {
+
+			mData = new val_type[nrows * ncols];
+
+			for (unsigned int i = 0; i < nrows; i++) {
+				for (unsigned int j = 0; j < ncols; j++) {
+					(*this)(i,j) = static_cast<val_type>(matrix(i,j));
+				}
+			}
+		}
+
+		// conversion from a fixed size matrix
+		template <typename other_type, unsigned int fnrows, unsigned int fncols>
+		Matrix (const Fixed::Matrix<other_type, fnrows, fncols> &fixed_matrix) :
+			nrows (fnrows),
+			ncols (fncols),
+			mapped_data (false),
+			mData (NULL) {
+				mData = new val_type[nrows * ncols];
+
+				for (unsigned int i = 0; i < nrows; i++) {
+					for (unsigned int j = 0; j < ncols; j++) {
+						(*this)(i,j) = static_cast<val_type>(fixed_matrix(i,j));
+					}
+				}
+			}
+
+		template <typename other_matrix_type>
+		Matrix (const Block<other_matrix_type, value_type> &block) :
+			nrows(block.rows()),
+			ncols(block.cols()),
+			mapped_data (false) {
+				mData = new val_type[nrows * ncols];
+
+				for (unsigned int i = 0; i < nrows; i++) {
+					for (unsigned int j = 0; j < ncols; j++) {
+						(*this)(i,j) = static_cast<val_type>(block(i,j));
+					}
+				}
+
+			}
+
+		~Matrix() {
+			if (nrows * ncols > 0 && mData != NULL && mapped_data == false) {
+				delete[] mData;
+				mData = NULL;
+			}
+
+			nrows = 0;
+			ncols = 0;
+		};
+
+		// comparison
+		bool operator==(const Matrix &matrix) const {
+			if (nrows != matrix.nrows || ncols != matrix.ncols)
+				return false;
+
+			for (unsigned int i = 0; i < nrows * ncols; i++) {
+				if (mData[i] != matrix.mData[i])
+					return false;
+			}
+			return true;
+		}
+		bool operator!=(const Matrix &matrix) const {
+			if (nrows != matrix.nrows || ncols != matrix.ncols)
+				return true;
+
+			for (unsigned int i = 0; i < nrows * ncols; i++) {
+				if (mData[i] != matrix.mData[i])
+					return true;
+			}
+			return false;
+		}
+
+		// access operators
+		const val_type& operator[](const unsigned int &index) const {
+			assert (index	>= 0);
+			assert (index < nrows * ncols);
+			return mData[index];
+		};
+		val_type& operator[](const unsigned int &index) {
+			assert (index	>= 0 && index < nrows * ncols);
+			return mData[index];
+		}
+
+		const val_type& operator()(const unsigned int &row, const unsigned int &col) const {
+			if (!(row	>= 0 && row < nrows && col >= 0 && col < ncols)) {
+				std::cout << "row = " << row << " col = " << col << std::endl;
+				std::cout << "nrows = " << nrows << " ncols = " << ncols << std::endl;
+				std::cout << "invalid read = " << mData[100000] << std::endl;
+			}
+			assert (row	>= 0 && row < nrows && col >= 0 && col < ncols);
+			return mData[row*ncols + col];
+		};
+		val_type& operator()(const unsigned int &row, const unsigned int &col) {
+			assert (row	>= 0 && row < nrows && col >= 0 && col < ncols);
+			return mData[row*ncols + col];
+		};
+		
+		void zero() {
+			for (unsigned int i = 0; i < ncols * nrows; i++)
+				mData[i] = 0.;
+		}
+		void setZero() {
+			zero();
+		}
+
+		val_type norm() const {
+			return sqrt(this->squaredNorm());
+		}
+
+		void normalize() {
+			val_type length = this->norm();
+
+			for (unsigned int i = 0; i < ncols * nrows; i++)
+				mData[i] /= length;
+		}
+
+		Matrix<val_type> normalized() const {
+			return Matrix<val_type> (*this) / this->norm();
+		}
+
+		Matrix<val_type> cross(const Matrix<val_type> &other_vector) {
+			assert (nrows * ncols == 3);
+			assert (other_vector.nrows * other_vector.ncols == 3);
+
+			Matrix<val_type> result (3, 1);
+			result[0] = mData[1] * other_vector[2] - mData[2] * other_vector[1];
+			result[1] = mData[2] * other_vector[0] - mData[0] * other_vector[2];
+			result[2] = mData[0] * other_vector[1] - mData[1] * other_vector[0];
+
+			return result;
+		}
+
+		val_type trace() const {
+			assert (rows() == cols());
+			val_type result = 0.;
+
+			for (unsigned int i = 0; i < rows(); i++) {
+				result += operator()(i,i);
+			}
+
+			return result;
+		}
+
+		val_type mean() const {
+			assert (rows() == 1 || cols() == 1);
+
+			val_type result = 0.;
+			for (unsigned int i = 0; i < rows() * cols(); i++) {
+				result += operator[](i);
+			}
+
+			return result / static_cast<val_type>(rows() * cols());
+		}
+
+		static matrix_type Zero() {
+			matrix_type result;
+			result.setZero();
+			return result;
+		}
+
+		static matrix_type Zero(int rows, int cols = 1) {
+			matrix_type result (rows, cols);
+			result.setZero();
+			return result;
+		}
+
+		static matrix_type Constant (int rows, val_type value) {
+			matrix_type result (rows, 1);
+			unsigned int i;
+			for (i = 0; i < result.size(); i++)
+				result[i] = value;
+			
+			return result;
+		}
+
+		static matrix_type Constant (int rows, int cols, val_type value) {
+			matrix_type result (rows, cols);
+			unsigned int i;
+			for (i = 0; i < result.size(); i++)
+				result[i] = value;
+			
+			return result;
+		}
+
+		static matrix_type Random (int rows, int cols = 1) {
+			matrix_type result (rows, cols);
+			for (int i = 0; i < rows; i++) {
+				for (int j = 0; j < cols; j++) {
+					result(i,j) = (static_cast<value_type>(rand()) / static_cast<value_type>(RAND_MAX)) * 2.0 - 1.0;
+				}
+			}
+
+			return result;
+		}
+
+		static matrix_type Identity (int rows, int cols = 1) {
+			assert (rows == cols);
+
+			matrix_type result (rows, cols);
+			result.identity();
+
+			return result;
+		}
+
+		void identity() {
+			assert (nrows == ncols);
+
+			setZero();
+			for (unsigned int i = 0; i < ncols; i++)
+				mData[i * ncols + i] = 1.;
+		}
+
+		void random() {
+			for (unsigned int i = 0; i < nrows * ncols; i++) 
+				mData[i] = static_cast<val_type> (rand()) / static_cast<val_type> (RAND_MAX);
+		}
+
+		val_type squaredNorm() const {
+			assert (ncols == 1 || nrows == 1);
+			val_type result = 0;
+
+			for (unsigned int i = 0; i < nrows * ncols; i++)
+				result += mData[i] * mData[i];
+
+			return result;
+		}
+
+		val_type dot(const matrix_type &matrix) const {
+			assert (ncols == 1 || nrows == 1);
+			val_type result = 0;
+
+			for (unsigned int i = 0; i < nrows * ncols; i++)
+				result += mData[i] * matrix[i];
+
+			return result;
+		}
+
+		// Blocks
+		Block<matrix_type, val_type>
+			block (unsigned int row_start, unsigned int col_start, unsigned int row_count, unsigned int col_count) {
+				return Block<matrix_type, val_type>(*this, row_start, col_start, row_count, col_count);
+			}
+
+		template <unsigned int row_count, unsigned int col_count>
+		Block<matrix_type, val_type>
+			block (unsigned int row_start, unsigned int col_start) {
+				return Block<matrix_type, val_type>(*this, row_start, col_start, row_count, col_count);
+			}
+
+		Block<matrix_type, val_type>
+			block (unsigned int row_start, unsigned int col_start, unsigned int row_count, unsigned int col_count) const {
+				return Block<matrix_type, val_type>(*this, row_start, col_start, row_count, col_count);
+			}
+
+		template <unsigned int row_count, unsigned int col_count>
+		Block<matrix_type, val_type>
+			block (unsigned int row_start, unsigned int col_start) const {
+				return Block<matrix_type, val_type>(*this, row_start, col_start, row_count, col_count);
+			}
+
+		// row and col accessors
+		Block<matrix_type, val_type>
+		col(unsigned int index) const {
+			return Block<matrix_type, val_type>(*this, 0, index, rows(), 1);
+		}
+
+		Block<matrix_type, val_type>
+		col(unsigned int index) {
+			return Block<matrix_type, val_type>(*this, 0, index, rows(), 1);
+		}
+
+		Block<matrix_type, val_type>
+		row(unsigned int index) const {
+			return Block<matrix_type, val_type>(*this, index, 0, 1, cols());
+		}
+
+		Block<matrix_type, val_type>
+		row(unsigned int index) {
+			return Block<matrix_type, val_type>(*this, index, 0, 1, cols());
+		}
+
+		// Operators with scalars
+		void operator*=(const val_type &scalar) {
+			for (unsigned int i = 0; i < nrows * ncols; i++)
+				mData[i] *= scalar;
+		};
+		void operator/=(const val_type &scalar) {
+			for (unsigned int i = 0; i < nrows * ncols; i++)
+				mData[i] /= scalar;
+		}
+		Matrix operator/(const val_type& scalar) const {
+			matrix_type result (*this);
+
+			for (unsigned int i = 0; i < nrows * ncols; i++)
+				result[i] /= scalar;
+
+			return result;
+		}
+
+		// Operators with other matrices
+		Matrix operator+(const Matrix &matrix) const {
+			matrix_type result (*this);
+
+			for (unsigned int i = 0; i < nrows * ncols; i++)
+				result[i] += matrix[i];
+
+			return result;
+		}
+		void operator+=(const matrix_type &matrix) {
+			for (unsigned int i = 0; i < nrows * ncols; i++)
+				mData[i] += matrix.mData[i];
+		}
+		Matrix operator-(const Matrix &matrix) const {
+			matrix_type result (*this);
+
+			for (unsigned int i = 0; i < nrows * ncols; i++)
+				result[i] -= matrix[i];
+
+			return result;
+		}
+		void operator-=(const Matrix &matrix) {
+			for (unsigned int i = 0; i < nrows * ncols; i++)
+				mData[i] -= matrix.mData[i];
+		}
+
+		Matrix<val_type> operator*(const Matrix<val_type> &other_matrix) const {
+			assert (ncols == other_matrix.nrows);
+
+			Matrix<val_type> result(nrows, other_matrix.ncols);
+			
+			result.setZero();
+
+			unsigned int i,j, k;
+			for (i = 0; i < nrows; i++) {
+				for (j = 0; j < other_matrix.cols(); j++) {
+					for (k = 0; k < other_matrix.rows(); k++) {
+						result(i,j) += mData[i * ncols + k] * other_matrix(k,j);
+					}
+				}
+			}
+			
+			return result;
+		}
+
+		template <unsigned int _nrows, unsigned int _ncols>
+		Matrix<val_type> operator*(const Fixed::Matrix<val_type, _nrows, _ncols> &other_matrix) const {
+			assert (ncols == other_matrix.rows());
+
+			Matrix<val_type> result(nrows, other_matrix.cols());
+			
+			result.setZero();
+
+			unsigned int i,j, k;
+			for (i = 0; i < nrows; i++) {
+				for (j = 0; j < other_matrix.cols(); j++) {
+					for (k = 0; k < other_matrix.rows(); k++) {
+						result(i,j) += mData[i * ncols + k] * other_matrix(k,j);
+					}
+				}
+			}
+			
+			return result;
+		}
+
+		Matrix<val_type> operator*(const Block<matrix_type, val_type> &other_matrix) const {
+			assert (ncols == other_matrix.rows());
+
+			Matrix<val_type> result(nrows, other_matrix.cols());
+			
+			result.setZero();
+
+			unsigned int i,j, k;
+			for (i = 0; i < nrows; i++) {
+				for (j = 0; j < other_matrix.cols(); j++) {
+					for (k = 0; k < other_matrix.rows(); k++) {
+						result(i,j) += mData[i * ncols + k] * other_matrix(k,j);
+					}
+				}
+			}
+			
+			return result;
+		}
+
+		void operator*=(const Matrix &matrix) {
+			matrix_type temp (*this);
+			*this = temp * matrix;
+		}
+
+		// Special operators
+		val_type *data(){
+			return mData;
+		}
+
+		// regular transpose of a 6 dimensional matrix
+		Matrix<val_type> transpose() const {
+			Matrix<val_type> result(ncols, nrows);
+	
+			for (unsigned int i = 0; i < nrows; i++) {
+				for (unsigned int j = 0; j < ncols; j++) {
+					result(j,i) = mData[i * ncols + j];
+				}
+			}
+
+			return result;
+		}
+
+		operator val_type() {
+
+			assert (nrows == 1);
+			assert (nrows == 1);
+
+			return mData[0];
+		}
+
+		Matrix operator-() const {
+			return *this * -1.0;
+		};
+
+		Matrix inverse() const {
+			return colPivHouseholderQr().inverse();
+		}
+
+		const LLT<matrix_type> llt() const {
+			return LLT<matrix_type>(*this);
+		}
+
+		const HouseholderQR<matrix_type> householderQr() const {
+			return HouseholderQR<matrix_type>(*this);
+		}
+		const ColPivHouseholderQR<matrix_type> colPivHouseholderQr() const {
+			return ColPivHouseholderQR<matrix_type>(*this);
+		}
+
+	private:
+		unsigned int nrows;
+		unsigned int ncols;
+		bool mapped_data;
+
+		val_type* mData;
+};
+
+template <typename val_type>
+inline Matrix<val_type> operator*(val_type scalar, const Matrix<val_type> &matrix) {
+	Matrix<val_type> result (matrix);
+
+	for (unsigned int i = 0; i < matrix.rows() * matrix.cols(); i++)
+		result.data()[i] *= scalar;
+
+	return result;
+}
+
+template <typename val_type, typename other_type>
+inline Matrix<val_type> operator*(const Matrix<val_type> &matrix, other_type scalar) {
+	Matrix<val_type> result (matrix);
+
+	for (unsigned int i = 0; i < matrix.rows() * matrix.cols(); i++)
+		result.data()[i] *= static_cast<val_type>(scalar);
+
+	return result;
+}
+
+template <typename val_type>
+inline std::ostream& operator<<(std::ostream& output, const Matrix<val_type> &matrix) {
+	size_t max_width = 0;
+	size_t out_width = output.width();
+
+	// get the widest number
+	for (size_t i = 0; i < matrix.rows(); i++) {
+		for (size_t j = 0; j < matrix.cols(); j++) {
+			std::stringstream out_stream;
+			out_stream << matrix(i,j);
+			max_width = std::max (out_stream.str().size(),max_width);
+		}
+	}
+
+	// overwrite width if it was explicitly prescribed
+	if (out_width != 0) {
+		max_width = out_width;
+	}
+
+	for (unsigned int i = 0; i < matrix.rows(); i++) {
+		output.width(0);
+		output << "[ ";
+		output.width(out_width);
+		for (unsigned int j = 0; j < matrix.cols(); j++) {
+			std::stringstream out_stream;
+			out_stream.width (max_width);
+			out_stream << matrix(i,j);
+			output << out_stream.str();
+
+			if (j < matrix.cols() - 1)
+				output << ", ";
+		}
+		output << " ]";
+		
+		if (matrix.rows() > 1 && i < matrix.rows() - 1)
+			output << std::endl;
+	}
+	return output;}
+
+}
+
+}
+
+#endif /* SIMPLEMATHDYNAMIC_H */

src/SimpleMathOld/SimpleMathFixed.h

@@ -0,0 +1,899 @@
+/**
+ * This is a highly inefficient math library. It was conceived by Martin
+ * Felis <martin.felis@iwr.uni-heidelberg.de> while he was compiling code
+ * that uses a highly efficient math library.
+ *
+ * It is intended to be used as a fast compiling substitute for the
+ * blazingly fast Eigen3 library and tries to mimic its API to a certain
+ * extend.
+ *
+ * Feel free to use it wherever you like. However, no guarantees are given
+ * that this code does what it says it would.
+ */
+
+#ifndef SIMPLEMATHFIXED_H
+#define SIMPLEMATHFIXED_H
+
+#include <sstream>
+#include <cstdlib>
+#include <cmath>
+#include <assert.h>
+#include <algorithm>
+#include <string.h>
+
+#include "compileassert.h"
+#include "SimpleMathBlock.h"
+
+/** \brief Namespace for a highly inefficient math library
+ *
+ */
+namespace SimpleMath {
+
+template <typename matrix_type>
+class LLT;
+
+template <typename matrix_type>
+class HouseholderQR;
+
+template <typename matrix_type>
+class ColPivHouseholderQR;
+
+template <typename matrix_type>
+class CommaInitializer;
+
+namespace Dynamic {
+template <typename val_type> class Matrix;
+}
+
+/** \brief Namespace for fixed size elements
+ */
+namespace Fixed {
+
+// forward declaration
+template <typename val_type, unsigned int nrows, unsigned int ncols>
+class Matrix;
+
+/** \brief Fixed size matrix class
+ */
+template <typename val_type, unsigned int nrows, unsigned int ncols>
+class Matrix {
+	public:
+		typedef Matrix<val_type, nrows, ncols> matrix_type;
+		typedef val_type value_type;
+
+		unsigned int rows() const {
+			return nrows;
+		}
+
+		unsigned int cols() const {
+			return ncols;
+		}
+
+		unsigned int size() const {
+			return nrows * ncols;
+		}
+
+		Matrix() {};
+		Matrix(const Matrix &matrix) {
+			unsigned int i;
+			for (i = 0; i < nrows * ncols; i++)
+				mData[i] = matrix.mData[i];
+		}
+		Matrix(unsigned int _nrows, unsigned int _ncols, const value_type* values) {
+			assert(nrows == _nrows);
+			assert(ncols == _ncols);
+			memcpy (mData, values, sizeof(value_type) * nrows * ncols);
+		}
+
+		Matrix& operator=(const Matrix &matrix) {
+			if (this != &matrix) {
+				unsigned int i;
+				for (i = 0; i < nrows * ncols; i++)
+					mData[i] = matrix.mData[i];
+			}
+			return *this;
+		}
+
+		// conversion different val_types
+
+		template <typename other_matrix_type>
+		Matrix (const Block<other_matrix_type, val_type> &block) {
+			assert (nrows == block.rows());
+			assert (ncols == block.cols());
+
+			for (unsigned int i = 0; i < nrows; i++) {
+				for (unsigned int j = 0; j < ncols; j++) {
+					(*this)(i,j) = static_cast<val_type>(block(i,j));
+				}
+			}
+		}
+		template <typename other_matrix_type>
+		Matrix& operator= (const Block<other_matrix_type, val_type> &block) {
+			assert (nrows == block.rows());
+			assert (ncols == block.cols());
+
+			for (unsigned int i = 0; i < nrows; i++) {
+				for (unsigned int j = 0; j < ncols; j++) {
+					(*this)(i,j) = static_cast<value_type>(block(i,j));
+				}
+			}
+
+			return *this;
+		}
+
+		template <typename other_type>
+		Matrix (const Matrix<other_type, nrows, ncols> &matrix) {
+			for (unsigned int i = 0; i < nrows; i++) {
+				for (unsigned int j = 0; j < ncols; j++) {
+					(*this)(i,j) = static_cast<val_type>(matrix(i,j));
+				}
+			}
+		}
+
+		template <typename other_type>
+		Matrix& operator=(const Matrix<other_type, nrows, ncols> &matrix) {
+			for (unsigned int i = 0; i < nrows; i++) {
+				for (unsigned int j = 0; j < ncols; j++) {
+					(*this)(i,j) = static_cast<val_type>(matrix(i,j));
+				}
+			}
+
+			return *this;
+		}
+		
+		CommaInitializer<matrix_type> operator<< (const val_type& value) {
+			return CommaInitializer<matrix_type> (*this, value);
+		}
+
+		// conversion Dynamic->Fixed
+		Matrix(const Dynamic::Matrix<val_type> &dynamic_matrix);
+		Matrix& operator=(const Dynamic::Matrix<val_type> &dynamic_matrix);
+
+	 	~Matrix() {};
+
+		Matrix (
+				const val_type &v00, const val_type &v01, const val_type &v02
+				) {
+			assert (nrows == 3);
+			assert (ncols == 1);
+
+			mData[0] = v00;
+			mData[1] = v01;
+			mData[2] = v02;
+		}
+
+		void set(
+				const val_type &v00, const val_type &v01, const val_type &v02
+				) {
+			COMPILE_ASSERT (nrows * ncols == 3);
+
+			mData[0] = v00;
+			mData[1] = v01;
+			mData[2] = v02;
+		}
+
+		Matrix (
+				const val_type &v00, const val_type &v01, const val_type &v02,
+				const val_type &v10, const val_type &v11, const val_type &v12,
+				const val_type &v20, const val_type &v21, const val_type &v22
+				) {
+			COMPILE_ASSERT (nrows == 3);
+			COMPILE_ASSERT (ncols == 3);
+
+			mData[0] = v00;
+			mData[1] = v01;
+			mData[2] = v02;
+
+			mData[1 * 3 + 0] = v10;
+			mData[1 * 3 + 1] = v11;
+			mData[1 * 3 + 2] = v12;
+			
+			mData[2 * 3 + 0] = v20;
+			mData[2 * 3 + 1] = v21;
+			mData[2 * 3 + 2] = v22;
+		}
+
+		void set(
+				const val_type v00, const val_type v01, const val_type v02,
+				const val_type v10, const val_type v11, const val_type v12,
+				const val_type v20, const val_type v21, const val_type v22
+				) {
+			COMPILE_ASSERT (nrows == 3);
+			COMPILE_ASSERT (ncols == 3);
+
+			mData[0] = v00;
+			mData[1] = v01;
+			mData[2] = v02;
+
+			mData[1 * 3 + 0] = v10;
+			mData[1 * 3 + 1] = v11;
+			mData[1 * 3 + 2] = v12;
+
+			mData[2 * 3 + 0] = v20;
+			mData[2 * 3 + 1] = v21;
+			mData[2 * 3 + 2] = v22;
+		}
+
+		Matrix (
+				const val_type &v00, const val_type &v01, const val_type &v02, const val_type &v03
+				) {
+			assert (nrows == 4);
+			assert (ncols == 1);
+
+			mData[0] = v00;
+			mData[1] = v01;
+			mData[2] = v02;
+			mData[3] = v03;
+		}
+
+		void set(
+				const val_type &v00, const val_type &v01, const val_type &v02, const val_type &v03
+				) {
+			COMPILE_ASSERT (nrows * ncols == 4);
+
+			mData[0] = v00;
+			mData[1] = v01;
+			mData[2] = v02;
+			mData[3] = v03;
+		}
+
+		Matrix (
+				const val_type &v00, const val_type &v01, const val_type &v02, const val_type &v03,
+				const val_type &v10, const val_type &v11, const val_type &v12, const val_type &v13,
+				const val_type &v20, const val_type &v21, const val_type &v22, const val_type &v23,
+				const val_type &v30, const val_type &v31, const val_type &v32, const val_type &v33
+				) {
+			COMPILE_ASSERT (nrows == 4);
+			COMPILE_ASSERT (ncols == 4);
+
+			mData[0] = v00;
+			mData[1] = v01;
+			mData[2] = v02;
+			mData[3] = v03;
+
+			mData[1 * 4 + 0] = v10;
+			mData[1 * 4 + 1] = v11;
+			mData[1 * 4 + 2] = v12;
+			mData[1 * 4 + 3] = v13;
+			
+			mData[2 * 4 + 0] = v20;
+			mData[2 * 4 + 1] = v21;
+			mData[2 * 4 + 2] = v22;
+			mData[2 * 4 + 3] = v23;
+
+			mData[3 * 4 + 0] = v30;
+			mData[3 * 4 + 1] = v31;
+			mData[3 * 4 + 2] = v32;
+			mData[3 * 4 + 3] = v33;
+		}
+
+		void set(
+				const val_type &v00, const val_type &v01, const val_type &v02, const val_type &v03,
+				const val_type &v10, const val_type &v11, const val_type &v12, const val_type &v13,
+				const val_type &v20, const val_type &v21, const val_type &v22, const val_type &v23,
+				const val_type &v30, const val_type &v31, const val_type &v32, const val_type &v33
+				) {
+			COMPILE_ASSERT (nrows == 4);
+			COMPILE_ASSERT (ncols == 4);
+
+			mData[0] = v00;
+			mData[1] = v01;
+			mData[2] = v02;
+			mData[3] = v03;
+
+			mData[1 * 4 + 0] = v10;
+			mData[1 * 4 + 1] = v11;
+			mData[1 * 4 + 2] = v12;
+			mData[1 * 4 + 3] = v13;
+			
+			mData[2 * 4 + 0] = v20;
+			mData[2 * 4 + 1] = v21;
+			mData[2 * 4 + 2] = v22;
+			mData[2 * 4 + 3] = v23;
+
+			mData[3 * 4 + 0] = v30;
+			mData[3 * 4 + 1] = v31;
+			mData[3 * 4 + 2] = v32;
+			mData[3 * 4 + 3] = v33;
+		}
+
+		Matrix (
+				const val_type &v00, const val_type &v01, const val_type &v02,
+				const val_type &v03, const val_type &v04, const val_type &v05
+				) {
+			COMPILE_ASSERT (nrows == 6);
+			COMPILE_ASSERT (ncols == 1);
+
+			mData[0] = v00;
+			mData[1] = v01;
+			mData[2] = v02;
+			mData[3] = v03;
+			mData[4] = v04;
+			mData[5] = v05;
+		}
+
+		void set(
+				const val_type &v00, const val_type &v01, const val_type &v02,
+				const val_type &v03, const val_type &v04, const val_type &v05
+				) {
+			COMPILE_ASSERT (nrows * ncols == 6);
+
+			mData[0] = v00;
+			mData[1] = v01;
+			mData[2] = v02;
+			mData[3] = v03;
+			mData[4] = v04;
+			mData[5] = v05;
+		}
+
+		Matrix (
+				const val_type &v00, const val_type &v01, const val_type &v02,
+				const val_type &v03, const val_type &v04, const val_type &v05,
+
+				const val_type &v10, const val_type &v11, const val_type &v12,
+				const val_type &v13, const val_type &v14, const val_type &v15,
+
+				const val_type &v20, const val_type &v21, const val_type &v22,
+				const val_type &v23, const val_type &v24, const val_type &v25,
+
+				const val_type &v30, const val_type &v31, const val_type &v32,
+				const val_type &v33, const val_type &v34, const val_type &v35,
+
+				const val_type &v40, const val_type &v41, const val_type &v42,
+				const val_type &v43, const val_type &v44, const val_type &v45,
+
+				const val_type &v50, const val_type &v51, const val_type &v52,
+				const val_type &v53, const val_type &v54, const val_type &v55
+				) {
+			COMPILE_ASSERT (nrows == 6);
+			COMPILE_ASSERT (ncols == 6);
+
+			mData[0] = v00;
+			mData[1] = v01;
+			mData[2] = v02;
+			mData[3] = v03;
+			mData[4] = v04;
+			mData[5] = v05;
+
+			mData[6 + 0] = v10;
+			mData[6 + 1] = v11;
+			mData[6 + 2] = v12;
+			mData[6 + 3] = v13;
+			mData[6 + 4] = v14;
+			mData[6 + 5] = v15;
+
+			mData[12 + 0] = v20;
+			mData[12 + 1] = v21;
+			mData[12 + 2] = v22;
+			mData[12 + 3] = v23;
+			mData[12 + 4] = v24;
+			mData[12 + 5] = v25;
+
+			mData[18 + 0] = v30;
+			mData[18 + 1] = v31;
+			mData[18 + 2] = v32;
+			mData[18 + 3] = v33;
+			mData[18 + 4] = v34;
+			mData[18 + 5] = v35;
+
+			mData[24 + 0] = v40;
+			mData[24 + 1] = v41;
+			mData[24 + 2] = v42;
+			mData[24 + 3] = v43;
+			mData[24 + 4] = v44;
+			mData[24 + 5] = v45;
+
+			mData[30 + 0] = v50;
+			mData[30 + 1] = v51;
+			mData[30 + 2] = v52;
+			mData[30 + 3] = v53;
+			mData[30 + 4] = v54;
+			mData[30 + 5] = v55;
+		};
+
+		void set(
+				const val_type v00, const val_type v01, const val_type v02,
+				const val_type v03, const val_type v04, const val_type v05,
+
+				const val_type v10, const val_type v11, const val_type v12,
+				const val_type v13, const val_type v14, const val_type v15,
+
+				const val_type v20, const val_type v21, const val_type v22,
+				const val_type v23, const val_type v24, const val_type v25,
+
+				const val_type v30, const val_type v31, const val_type v32,
+				const val_type v33, const val_type v34, const val_type v35,
+
+				const val_type v40, const val_type v41, const val_type v42,
+				const val_type v43, const val_type v44, const val_type v45,
+
+				const val_type v50, const val_type v51, const val_type v52,
+				const val_type v53, const val_type v54, const val_type v55
+				) {
+			COMPILE_ASSERT (nrows == 6);
+			COMPILE_ASSERT (ncols == 6);
+
+			mData[0] = v00;
+			mData[1] = v01;
+			mData[2] = v02;
+			mData[3] = v03;
+			mData[4] = v04;
+			mData[5] = v05;
+
+			mData[6 + 0] = v10;
+			mData[6 + 1] = v11;
+			mData[6 + 2] = v12;
+			mData[6 + 3] = v13;
+			mData[6 + 4] = v14;
+			mData[6 + 5] = v15;
+
+			mData[12 + 0] = v20;
+			mData[12 + 1] = v21;
+			mData[12 + 2] = v22;
+			mData[12 + 3] = v23;
+			mData[12 + 4] = v24;
+			mData[12 + 5] = v25;
+
+			mData[18 + 0] = v30;
+			mData[18 + 1] = v31;
+			mData[18 + 2] = v32;
+			mData[18 + 3] = v33;
+			mData[18 + 4] = v34;
+			mData[18 + 5] = v35;
+
+			mData[24 + 0] = v40;
+			mData[24 + 1] = v41;
+			mData[24 + 2] = v42;
+			mData[24 + 3] = v43;
+			mData[24 + 4] = v44;
+			mData[24 + 5] = v45;
+
+			mData[30 + 0] = v50;
+			mData[30 + 1] = v51;
+			mData[30 + 2] = v52;
+			mData[30 + 3] = v53;
+			mData[30 + 4] = v54;
+			mData[30 + 5] = v55;
+		}
+		
+		// comparison
+		bool operator==(const Matrix &matrix) const {
+			for (unsigned int i = 0; i < nrows * ncols; i++) {
+				if (mData[i] != matrix.mData[i])
+					return false;
+			}
+			return true;
+		}
+		bool operator!=(const Matrix &matrix) const {
+			for (unsigned int i = 0; i < nrows * ncols; i++) {
+				if (mData[i] != matrix.mData[i])
+					return true;
+			}
+			return false;
+		}
+
+		// access operators
+		const val_type& operator[](const unsigned int &index) const {
+			assert (index	>= 0 && index < nrows * ncols);
+			return mData[index];
+		};
+		val_type& operator[](const unsigned int &index) {
+			assert (index	>= 0 && index < nrows * ncols);
+			return mData[index];
+		}
+
+		const val_type& operator()(const unsigned int &row, const unsigned int &col) const {
+			assert (row	>= 0 && row < nrows && col	>= 0 && col < ncols);
+			return mData[row*ncols + col];
+		};
+		val_type& operator()(const unsigned int &row, const unsigned int &col) {
+			assert (row	>= 0 && row < nrows && col	>= 0 && col < ncols);
+			return mData[row*ncols + col];
+		};
+		
+		void zero() {
+			for (unsigned int i = 0; i < ncols * nrows; i++)
+				mData[i] = 0.;
+		}
+		void setZero() {
+			zero();
+		}
+
+		val_type norm() const {
+			return sqrt(this->squaredNorm());
+		}
+
+		matrix_type normalize() {
+			val_type length = this->norm();
+
+			for (unsigned int i = 0; i < ncols * nrows; i++)
+				mData[i] /= length;
+
+			return *this;
+		}
+
+		matrix_type normalized() const {
+			return matrix_type (*this) / this->norm();
+		}
+
+		Matrix<val_type, 3, 1> cross(const Matrix<val_type, 3, 1> &other_vector) const {
+			COMPILE_ASSERT (nrows * ncols == 3);
+
+			Matrix<val_type, 3, 1> result;
+			result[0] = mData[1] * other_vector[2] - mData[2] * other_vector[1];
+			result[1] = mData[2] * other_vector[0] - mData[0] * other_vector[2];
+			result[2] = mData[0] * other_vector[1] - mData[1] * other_vector[0];
+
+			return result;
+		}
+
+		val_type trace() const {
+			COMPILE_ASSERT(nrows == ncols);
+			val_type result = 0.;
+
+			for (unsigned int i = 0; i < rows(); i++) {
+				result += operator()(i,i);
+			}
+
+			return result;
+		}
+
+		val_type mean() const {
+			COMPILE_ASSERT(nrows == 1 || ncols == 1);
+
+			val_type result = 0.;
+			for (unsigned int i = 0; i < rows() * cols(); i++) {
+				result += operator[](i);
+			}
+
+			return result / static_cast<val_type>(nrows * ncols);
+		}
+
+		static matrix_type Zero() {
+			matrix_type result;
+			result.setZero();
+			return result;
+		}
+
+		static matrix_type Zero(int ignore_me) {
+			matrix_type result;
+			result.setZero();
+			return result;
+		}
+
+		static matrix_type Zero(int ignore_me, int ignore_me_too) {
+			matrix_type result;
+			result.setZero();
+			return result;
+		}
+
+		static matrix_type Identity() {
+			matrix_type result;
+			result.identity();
+			return result;
+		}
+
+		static matrix_type Identity(int ignore_me, int ignore_me_too) {
+			matrix_type result;
+			result.identity();
+			return result;
+		}
+
+		static matrix_type Random (int rows = 1, int cols = 1) {
+			matrix_type result;
+			for (int i = 0; i < nrows; i++) {
+				for (int j = 0; j < ncols; j++) {
+					result(i,j) = (static_cast<value_type>(rand()) / static_cast<value_type>(RAND_MAX)) * 2.0 - 1.0;
+				}
+			}
+
+			return result;
+		}
+
+		void identity() {
+			COMPILE_ASSERT (nrows == ncols);
+
+			setZero();
+			for (unsigned int i = 0; i < ncols; i++)
+				mData[i * ncols + i] = 1.;
+		}
+
+		void random() {
+			for (unsigned int i = 0; i < nrows * ncols; i++) 
+				mData[i] = static_cast<val_type> (rand()) / static_cast<val_type> (RAND_MAX);
+		}
+
+		val_type squaredNorm() const {
+			COMPILE_ASSERT (ncols == 1 || nrows == 1);
+			val_type result = 0;
+
+			for (unsigned int i = 0; i < nrows * ncols; i++)
+				result += mData[i] * mData[i];
+
+			return result;
+		}
+
+		val_type dot(const matrix_type &matrix) const {
+			COMPILE_ASSERT (ncols == 1 || nrows == 1);
+			val_type result = 0;
+
+			for (unsigned int i = 0; i < nrows * ncols; i++)
+				result += mData[i] * matrix[i];
+
+			return result;
+		}
+
+		// Blocks using block(i,j,r,c) syntax
+		Block<matrix_type, val_type>
+			block (unsigned int row_start, unsigned int col_start, unsigned int row_count, unsigned int col_count) {
+				return Block<matrix_type, val_type>(*this, row_start, col_start, row_count, col_count);
+			}
+
+		const Block<matrix_type, val_type>
+			block (unsigned int row_start, unsigned int col_start, unsigned int row_count, unsigned int col_count) const {
+				return Block<matrix_type, val_type>(*this, row_start, col_start, row_count, col_count);
+			}
+
+		// Blocks using block<r,c>(i,j) syntax
+		template <unsigned int block_row_count, unsigned int block_col_count>
+		Block<matrix_type, val_type>
+			block (unsigned int row_start, unsigned int col_start) {
+				return Block<matrix_type, val_type>(*this, row_start, col_start, block_row_count, block_col_count);
+			}
+
+		template <unsigned int block_row_count, unsigned int block_col_count>
+		const Block<matrix_type, val_type>
+			block (unsigned int row_start, unsigned int col_start) const {
+				return Block<matrix_type, val_type>(*this, row_start, col_start, block_row_count, block_col_count);
+			}
+
+		// row and col accessors
+		Block<matrix_type, val_type>
+		col(unsigned int index) const {
+			return Block<matrix_type, val_type>(*this, 0, index, rows(), 1);
+		}
+
+		Block<matrix_type, val_type>
+		col(unsigned int index) {
+			return Block<matrix_type, val_type>(*this, 0, index, rows(), 1);
+		}
+
+		Block<matrix_type, val_type>
+		row(unsigned int index) const {
+			return Block<matrix_type, val_type>(*this, index, 0, 1, cols());
+		}
+
+		Block<matrix_type, val_type>
+		row(unsigned int index) {
+			return Block<matrix_type, val_type>(*this, index, 0, 1, cols());
+		}
+
+		// Operators with scalars
+		void operator*=(const val_type &scalar) {
+			for (unsigned int i = 0; i < nrows * ncols; i++)
+				mData[i] *= scalar;
+		};
+		void operator/=(const val_type &scalar) {
+			for (unsigned int i = 0; i < nrows * ncols; i++)
+				mData[i] /= scalar;
+		}
+		Matrix operator/(const val_type& scalar) const {
+			matrix_type result (*this);
+
+			for (unsigned int i = 0; i < nrows * ncols; i++)
+				result[i] /= scalar;
+
+			return result;
+		}
+
+		// Operators with other matrices
+		Matrix operator+(const Matrix &matrix) const {
+			matrix_type result (*this);
+
+			for (unsigned int i = 0; i < nrows * ncols; i++)
+				result[i] += matrix[i];
+
+			return result;
+		}
+		void operator+=(const matrix_type &matrix) {
+			for (unsigned int i = 0; i < nrows * ncols; i++)
+				mData[i] += matrix.mData[i];
+		}
+		Matrix operator-(const Matrix &matrix) const {
+			matrix_type result (*this);
+
+			for (unsigned int i = 0; i < nrows * ncols; i++)
+				result[i] -= matrix[i];
+
+			return result;
+		}
+		void operator-=(const Matrix &matrix) {
+			for (unsigned int i = 0; i < nrows * ncols; i++)
+				mData[i] -= matrix.mData[i];
+		}
+
+		template <unsigned int other_rows, unsigned int other_cols>
+		Matrix<val_type, nrows, other_cols> operator*(const Matrix<val_type, other_rows, other_cols> &matrix) const {
+			COMPILE_ASSERT (ncols == other_rows);
+
+			Matrix<val_type, nrows, other_cols> result;
+			
+			result.setZero();
+
+			unsigned int i,j, k;
+			for (i = 0; i < nrows; i++) {
+				for (j = 0; j < other_cols; j++) {
+					for (k = 0; k < other_rows; k++) {
+						result(i,j) += mData[i * ncols + k] * matrix(k,j);
+					}
+				}
+			}
+			
+			return result;
+		}
+
+		// multiplication with dynamic sized matrix
+		template <typename other_type>
+		Dynamic::Matrix<val_type> operator*(const Dynamic::Matrix<other_type> &other_matrix) {
+			assert (ncols == other_matrix.rows());
+
+			Dynamic::Matrix<val_type> result(nrows, other_matrix.cols());
+			
+			result.setZero();
+
+			unsigned int i,j, k;
+			for (i = 0; i < nrows; i++) {
+				for (j = 0; j < other_matrix.cols(); j++) {
+					for (k = 0; k < other_matrix.rows(); k++) {
+						result(i,j) += mData[i * ncols + k] * static_cast<val_type>(other_matrix(k,j));
+					}
+				}
+			}
+			
+			return result;
+		}
+
+		// Multiplication with a block
+		template <typename other_matrix_type>
+		Dynamic::Matrix<val_type> operator*(const Block<other_matrix_type, val_type> &block) {
+			assert (ncols == block.rows());
+
+			Dynamic::Matrix<val_type> result(nrows, block.cols());
+
+			result.setZero();
+
+			unsigned int i,j, k;
+			for (i = 0; i < nrows; i++) {
+				for (j = 0; j < block.cols(); j++) {
+					for (k = 0; k < block.rows(); k++) {
+						result(i,j) += mData[i * ncols + k] * static_cast<val_type>(block(k,j));
+					}
+				}
+			}
+
+			return result;
+		}
+
+
+		void operator*=(const Matrix &matrix) {
+			matrix_type temp (*this);
+			*this = temp * matrix;
+		}
+
+		// Special operators
+		val_type *data(){
+			return mData;
+		}
+
+		const val_type *data() const{
+			return mData;
+		}
+
+		// regular transpose of a 6 dimensional matrix
+		Matrix<val_type, ncols, nrows> transpose() const {
+			Matrix<val_type, ncols, nrows> result;
+	
+			for (unsigned int i = 0; i < nrows; i++) {
+				for (unsigned int j = 0; j < ncols; j++) {
+					result(j,i) = mData[i * ncols + j];
+				}
+			}
+
+			return result;
+		}
+
+		operator val_type() {
+			COMPILE_ASSERT (nrows == 1);
+			COMPILE_ASSERT (nrows == 1);
+
+			return mData[0];
+		}
+
+		Matrix operator-() const {
+			return *this * -1.;
+		}
+
+		Matrix inverse() const {
+			return colPivHouseholderQr().inverse();
+		}
+
+		const LLT<matrix_type> llt() const {
+			return LLT<matrix_type>(*this);
+		}
+
+		const HouseholderQR<matrix_type> householderQr() const {
+			return HouseholderQR<matrix_type>(*this);
+		}
+		const ColPivHouseholderQR<matrix_type> colPivHouseholderQr() const {
+			return ColPivHouseholderQR<matrix_type>(*this);
+		}
+
+	private:
+		val_type mData[nrows * ncols];
+};
+
+template <typename val_type, unsigned int nrows, unsigned int ncols>
+inline Matrix<val_type, nrows, ncols> operator*(val_type scalar, const Matrix<val_type, nrows, ncols> &matrix) {
+	Matrix<val_type, nrows, ncols> result (matrix);
+
+	for (unsigned int i = 0; i < nrows * ncols; i++)
+		result.data()[i] *= scalar;
+
+	return result;
+}
+
+template <typename val_type, typename other_type, unsigned int nrows, unsigned int ncols>
+inline Matrix<val_type, nrows, ncols> operator*(const Matrix<val_type, nrows, ncols> &matrix, other_type scalar) {
+	Matrix<val_type, nrows, ncols> result (matrix);
+
+	for (unsigned int i = 0; i < nrows * ncols; i++)
+		result.data()[i] *= static_cast<val_type> (scalar);
+
+	return result;
+}
+
+template <typename val_type, unsigned int nrows, unsigned int ncols>
+inline std::ostream& operator<<(std::ostream& output, const Matrix<val_type, nrows, ncols> &matrix) {
+	size_t max_width = 0;
+	size_t out_width = output.width();
+
+	// get the widest number
+	for (size_t i = 0; i < matrix.rows(); i++) {
+		for (size_t j = 0; j < matrix.cols(); j++) {
+			std::stringstream out_stream;
+			out_stream << matrix(i,j);
+			max_width = std::max (out_stream.str().size(),max_width);
+		}
+	}
+
+	// overwrite width if it was explicitly prescribed
+	if (out_width != 0) {
+		max_width = out_width;
+	}
+
+	for (unsigned int i = 0; i < matrix.rows(); i++) {
+		output.width(0);
+		output << "[ ";
+		output.width(out_width);
+		for (unsigned int j = 0; j < matrix.cols(); j++) {
+			std::stringstream out_stream;
+			out_stream.width (max_width);
+			out_stream << matrix(i,j);
+			output << out_stream.str();
+
+			if (j < matrix.cols() - 1)
+				output << ", ";
+		}
+		output << " ]";
+		
+		if (matrix.rows() > 1 && i < matrix.rows() - 1)
+			output << std::endl;
+	}
+	return output;
+}
+
+}
+
+}
+
+#endif /* SIMPLEMATHFIXED_H */

src/SimpleMathOld/SimpleMathGL.h

@@ -0,0 +1,348 @@
+#ifndef _SIMPLEMATHGL_H_
+#define _SIMPLEMATHGL_H_
+
+#include "SimpleMath.h"
+#include <cmath>
+
+namespace SimpleMath {
+
+typedef SimpleMath::Fixed::Matrix<float, 3, 1> Vector3f;
+typedef SimpleMath::Fixed::Matrix<float, 3, 3> Matrix33f;
+
+typedef SimpleMath::Fixed::Matrix<float, 4, 1> Vector4f;
+typedef SimpleMath::Fixed::Matrix<float, 4, 4> Matrix44f;
+
+namespace GL {
+
+inline Matrix33f RotateMat33 (float rot_deg, float x, float y, float z) {
+	float c = cosf (rot_deg * M_PI / 180.f);
+	float s = sinf (rot_deg * M_PI / 180.f);
+	return Matrix33f (
+			x * x * (1.0f - c) + c,
+			y * x * (1.0f - c) + z * s,
+			x * z * (1.0f - c) - y * s,
+
+			x * y * (1.0f - c) - z * s,
+			y * y * (1.0f - c) + c,
+			y * z * (1.0f - c) + x * s,
+
+			x * z * (1.0f - c) + y * s,
+			y * z * (1.0f - c) - x * s,
+			z * z * (1.0f - c) + c
+
+			);
+}
+
+
+inline Matrix44f RotateMat44 (float rot_deg, float x, float y, float z) {
+	float c = cosf (rot_deg * M_PI / 180.f);
+	float s = sinf (rot_deg * M_PI / 180.f);
+	return Matrix44f (
+			x * x * (1.0f - c) + c,
+			y * x * (1.0f - c) + z * s,
+			x * z * (1.0f - c) - y * s,
+			0.f, 
+
+			x * y * (1.0f - c) - z * s,
+			y * y * (1.0f - c) + c,
+			y * z * (1.0f - c) + x * s,
+			0.f,
+
+			x * z * (1.0f - c) + y * s,
+			y * z * (1.0f - c) - x * s,
+			z * z * (1.0f - c) + c,
+			0.f,
+
+			0.f, 0.f, 0.f, 1.f
+			);
+}
+
+inline Matrix44f TranslateMat44 (float x, float y, float z) {
+	return Matrix44f (
+			1.f, 0.f, 0.f, 0.f,
+			0.f, 1.f, 0.f, 0.f,
+			0.f, 0.f, 1.f, 0.f,
+			  x,   y,   z, 1.f
+			);
+}
+
+inline Matrix44f ScaleMat44 (float x, float y, float z) {
+	return Matrix44f (
+			  x, 0.f, 0.f, 0.f,
+			0.f,   y, 0.f, 0.f,
+			0.f, 0.f,   z, 0.f,
+			0.f, 0.f, 0.f, 1.f
+			);
+}
+
+inline Matrix44f Ortho(float left, float right,
+		float bottom, float top,
+		float near, float far) {
+	float tx = -(right + left) / (right - left);
+	float ty = -(top + bottom) / (top - bottom);
+	float tz = -(far + near) / (far - near);
+	return Matrix44f(
+			2.0f / (right - left), 0.0f, 0.0f, 0.0f,
+			0, 2.0f / (top - bottom), 0.0f, 0.0f,
+			0.0f, 0.0f, -2.0f / (far - near), 0.0f,
+			tx, ty, tz, 1.0f
+			);
+}
+
+inline Matrix44f Perspective(float fovy, float aspect,
+		float near, float far) {
+	float x = (fovy * M_PI / 180.0) / 2.0f;
+	float f = cos(x) / sin(x);
+
+	return Matrix44f(
+			f / aspect, 0.0f, 0.0f, 0.0f,
+			0.0f, f, 0.0f, 0.0f,
+			0.0f, 0.0f, (far + near) / (near - far), -1.0f,
+			0.0f, 0.0f, (2.0f * far * near) / (near - far), 0.0f
+			);
+}
+
+inline Matrix44f Frustum(float left, float right,
+		float bottom, float top,
+		float near, float far) {
+	float A = (right + left) / (right - left);
+	float B = (top + bottom) / (top - bottom);
+	float C = -(far + near) / (far - near);
+	float D = - (2.0f * far * near) / (far - near);
+
+	return Matrix44f(
+			2.0f * near / (right - left), 0.0f, 0.0f, 0.0f,
+			0.0f, 2.0f * near / (top - bottom), 0.0f, 0.0f,
+			A, B, C, -1.0f,
+			0.0f, 0.0f, D, 0.0f
+			);
+}
+
+inline Matrix44f LookAt(
+		const Vector3f& eye,
+		const Vector3f& poi,
+		const Vector3f& up) {
+	Vector3f d = (poi - eye).normalized();
+	Vector3f s = d.cross(up.normalized()).normalized();
+	Vector3f u = s.cross(d).normalized();
+
+	return TranslateMat44(-eye[0], -eye[1], -eye[2]) * Matrix44f(
+			s[0], u[0], -d[0], 0.0f,
+			s[1], u[1], -d[1], 0.0f,
+			s[2], u[2], -d[2], 0.0f,
+			0.0f, 0.0f, 0.0f, 1.0f
+			);
+}
+
+/** Quaternion 
+ *
+ * order: x,y,z,w
+ */
+class Quaternion : public Vector4f {
+	public:
+		Quaternion () :
+			Vector4f (0.f, 0.f, 0.f, 1.f)
+		{}
+		Quaternion (const Vector4f vec4) :
+			Vector4f (vec4)
+		{}
+		Quaternion (float x, float y, float z, float w):
+			Vector4f (x, y, z, w)
+		{}
+		/** This function is equivalent to multiplicate their corresponding rotation matrices */
+		Quaternion operator* (const Quaternion &q) const {
+			return Quaternion (
+					(*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1],
+					(*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2],
+					(*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0],
+					(*this)[3] * q[3] - (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2]
+					);
+		}
+		Quaternion& operator*=(const Quaternion &q) {
+			set (
+					(*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1],
+					(*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2],
+					(*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0],
+					(*this)[3] * q[3] - (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2]
+					);
+			return *this;
+		}
+
+		static Quaternion fromGLRotate (float angle, float x, float y, float z) {
+			float st = sinf (angle * M_PI / 360.f);
+			return Quaternion (
+						st * x,
+						st * y,
+						st * z,
+						cosf (angle * M_PI / 360.f)
+						);
+		}
+
+		Quaternion normalize() {
+			return Vector4f::normalize();
+		}
+
+		Quaternion slerp (float alpha, const Quaternion &quat) const {
+			// check whether one of the two has 0 length
+			float s = sqrt (squaredNorm() * quat.squaredNorm());
+
+			// division by 0.f is unhealthy!
+			assert (s != 0.f);
+
+			float angle = acos (dot(quat) / s);
+			if (angle == 0.f || std::isnan(angle)) {
+				return *this;
+			}
+			assert(!std::isnan(angle));
+
+			float d = 1.f / sinf (angle);
+			float p0 = sinf ((1.f - alpha) * angle);
+			float p1 = sinf (alpha * angle);
+
+			if (dot (quat) < 0.f) {
+				return Quaternion( ((*this) * p0 - quat * p1) * d);
+			}
+			return Quaternion( ((*this) * p0 + quat * p1) * d);
+		}
+
+		Matrix44f toGLMatrix() const {
+			float x = (*this)[0];
+			float y = (*this)[1];
+			float z = (*this)[2];
+			float w = (*this)[3];
+			return Matrix44f (
+					1 - 2*y*y - 2*z*z,
+					2*x*y + 2*w*z,
+					2*x*z - 2*w*y,
+					0.f,
+
+					2*x*y - 2*w*z,
+					1 - 2*x*x - 2*z*z,
+					2*y*z + 2*w*x,
+					0.f,
+
+					2*x*z + 2*w*y,
+					2*y*z - 2*w*x,
+					1 - 2*x*x - 2*y*y,
+					0.f,
+					
+					0.f,
+					0.f,
+					0.f,
+					1.f);
+		}
+
+		static Quaternion fromGLMatrix(const Matrix44f &mat) {
+			float w = sqrt (1.f + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5f;
+			return Quaternion (
+					-(mat(2,1) - mat(1,2)) / (w * 4.f),
+					-(mat(0,2) - mat(2,0)) / (w * 4.f),
+					-(mat(1,0) - mat(0,1)) / (w * 4.f),
+					w);
+		}
+
+		static Quaternion fromMatrix (const Matrix33f &mat) {
+			float w = sqrt (1.f + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5f;
+			return Quaternion (
+					(mat(2,1) - mat(1,2)) / (w * 4.f),
+					(mat(0,2) - mat(2,0)) / (w * 4.f),
+					(mat(1,0) - mat(0,1)) / (w * 4.f),
+					w);
+		}
+
+		static Quaternion fromAxisAngle (const Vector3f &axis, double angle_rad) {
+			double d = axis.norm();
+			double s2 = std::sin (angle_rad * 0.5) / d;
+			return Quaternion (
+					axis[0] * s2,
+					axis[1] * s2,
+					axis[2] * s2,
+					std::cos(angle_rad * 0.5)
+					);
+		}
+
+		static Quaternion fromEulerZYX (const Vector3f &zyx_angles) {
+			return Quaternion::fromAxisAngle (Vector3f (0., 0., 1.), zyx_angles[0])
+				* Quaternion::fromAxisAngle (Vector3f (0., 1., 0.), zyx_angles[1])
+				* Quaternion::fromAxisAngle (Vector3f (1., 0., 0.), zyx_angles[2]); 
+		}
+
+		static Quaternion fromEulerYXZ (const Vector3f &yxz_angles) {
+			return Quaternion::fromAxisAngle (Vector3f (0., 1., 0.), yxz_angles[0])
+				* Quaternion::fromAxisAngle (Vector3f (1., 0., 0.), yxz_angles[1])
+				* Quaternion::fromAxisAngle (Vector3f (0., 0., 1.), yxz_angles[2]);
+		}
+
+		static Quaternion fromEulerXYZ (const Vector3f &xyz_angles) {
+			return Quaternion::fromAxisAngle (Vector3f (0., 0., 01.), xyz_angles[2]) 
+				* Quaternion::fromAxisAngle (Vector3f (0., 1., 0.), xyz_angles[1])
+				* Quaternion::fromAxisAngle (Vector3f (1., 0., 0.), xyz_angles[0]);
+		}
+ 
+		Vector3f toEulerZYX () const {
+			return Vector3f (1.0f, 2.0f, 3.0f
+					);
+		}
+
+		Vector3f toEulerYXZ() const {
+			return Vector3f (
+					atan2 (-2.f * (*this)[0] * (*this)[2] + 2.f * (*this)[3] * (*this)[1],
+						(*this)[2] * (*this)[2] - (*this)[1] * (*this)[1]
+						-(*this)[0] * (*this)[0] + (*this)[3] * (*this)[3]),
+					asin (2.f * (*this)[1] * (*this)[2] + 2.f * (*this)[3] * (*this)[0]),
+					atan2 (-2.f * (*this)[0] * (*this)[1] + 2.f * (*this)[3] * (*this)[2],
+						(*this)[1] * (*this)[1] - (*this)[2] * (*this)[2]
+						+(*this)[3] * (*this)[3] - (*this)[0] * (*this)[0]
+						)
+					);
+		}
+
+		Matrix33f toMatrix() const {
+			float x = (*this)[0];
+			float y = (*this)[1];
+			float z = (*this)[2];
+			float w = (*this)[3];
+			return Matrix33f (
+					1 - 2*y*y - 2*z*z,
+					2*x*y - 2*w*z,
+					2*x*z + 2*w*y,
+
+					2*x*y + 2*w*z,
+					1 - 2*x*x - 2*z*z,
+					2*y*z - 2*w*x,
+
+					2*x*z - 2*w*y,
+					2*y*z + 2*w*x,
+					1 - 2*x*x - 2*y*y
+			);
+		}
+
+		Quaternion conjugate() const {
+			return Quaternion (
+					-(*this)[0],
+					-(*this)[1],
+					-(*this)[2],
+					(*this)[3]);
+		}
+
+		Vector3f rotate (const Vector3f &vec) const {
+			Vector3f vn (vec);
+			Quaternion vec_quat (vn[0], vn[1], vn[2], 0.f), res_quat;
+
+			res_quat = (*this) * vec_quat;
+			res_quat = res_quat * conjugate();
+
+			return Vector3f (res_quat[0], res_quat[1], res_quat[2]);
+		}
+};
+
+// namespace GL
+}
+
+// namespace SimpleMath
+}
+
+/* _SIMPLEMATHGL_H_ */
+#endif
+
+

src/SimpleMathOld/SimpleMathMap.h

@@ -0,0 +1,22 @@
+#ifndef SIMPLEMATHMAP_H
+#define SIMPLEMATHMAP_H
+
+#include "compileassert.h"
+
+namespace SimpleMath {
+
+/** \brief \brief Wraps a varying size matrix type around existing data
+ *
+ * \warning If you create a matrix using the map function and then assign
+ *  a bigger matrix you invalidate the original memory!
+ *
+ */
+template < typename MatrixType >
+MatrixType Map (typename MatrixType::value_type *data, unsigned int rows, unsigned int cols) {
+	return MatrixType (rows, cols, data);
+}
+
+}
+
+// SIMPLEMATHMAP_H
+#endif

src/SimpleMathOld/SimpleMathMixed.h

@@ -0,0 +1,138 @@
+/**
+ * This is a highly inefficient math library. It was conceived by Martin
+ * Felis <martin.felis@iwr.uni-heidelberg.de> while he was compiling code
+ * that uses a highly efficient math library.
+ *
+ * It is intended to be used as a fast compiling substitute for the
+ * blazingly fast Eigen3 library and tries to mimic its API to a certain
+ * extend.
+ *
+ * Feel free to use it wherever you like. However, no guarantees are given
+ * that this code does what it says it would.
+ */
+
+#ifndef SIMPLEMATHMIXED_H
+#define SIMPLEMATHMIXED_H
+
+#include <sstream>
+#include <cstdlib>
+#include <assert.h>
+#include <iostream>
+
+#include "compileassert.h"
+
+/** \brief Namespace for a highly inefficient math library
+ *
+ */
+namespace SimpleMath {
+
+// conversion Dynamic->Fixed
+template <typename val_type, unsigned int nrows, unsigned int ncols>
+inline Fixed::Matrix<val_type, nrows, ncols>::Matrix(const Dynamic::Matrix<val_type> &dynamic_matrix) {
+	if (dynamic_matrix.cols() != ncols 
+		|| dynamic_matrix.rows() != nrows) {
+		std::cerr << "Error: cannot assign a dynamic sized matrix of size " << dynamic_matrix.rows() << "x" << dynamic_matrix.cols() << " to a fixed size matrix of size " << nrows << "x" << ncols << "!" << std::endl;
+		abort();
+	}
+	
+	for (unsigned int i = 0; i < nrows * ncols; i++) {
+		mData[i] = dynamic_matrix[i];
+	}
+}
+
+template <typename val_type, unsigned int nrows, unsigned int ncols>
+inline Fixed::Matrix<val_type, nrows, ncols>& Fixed::Matrix<val_type, nrows, ncols>::operator=(const Dynamic::Matrix<val_type> &dynamic_matrix) {
+	if (dynamic_matrix.cols() != ncols 
+		|| dynamic_matrix.rows() != nrows) {
+		std::cerr << "Error: cannot assign a dynamic sized matrix of size " << dynamic_matrix.rows() << "x" << dynamic_matrix.cols() << " to a fixed size matrix of size " << nrows << "x" << ncols << "!" << std::endl;
+		abort();
+	}
+	
+	for (unsigned int i = 0; i < nrows * ncols; i++) {
+		mData[i] = dynamic_matrix[i];
+	}
+
+	return *this;
+}
+
+// multiplication
+template <typename val_type, unsigned int nrows, unsigned int ncols>
+inline Dynamic::Matrix<val_type> operator*(
+		const Fixed::Matrix<val_type, nrows, ncols> &matrix_a,
+		const Dynamic::Matrix<val_type> &matrix_b) {
+	assert (matrix_a.cols() == matrix_b.rows());
+
+	Dynamic::Matrix<val_type> result (nrows, matrix_b.cols());
+
+	result.setZero();
+
+	unsigned int i,j, k;
+	for (i = 0; i < nrows; i++) {
+		for (j = 0; j < matrix_b.cols(); j++) {
+			for (k = 0; k < matrix_b.rows(); k++) {
+				result(i,j) += matrix_a(i,k) * matrix_b(k,j);
+			}
+		}
+	}
+
+	return result;
+}
+
+template <typename val_type, unsigned int nrows, unsigned int ncols>
+inline Dynamic::Matrix<val_type> operator*(
+		const Dynamic::Matrix<val_type> &matrix_a,
+		const Fixed::Matrix<val_type, nrows, ncols> &matrix_b) {
+	assert (matrix_a.cols() == matrix_b.rows());
+	
+	Dynamic::Matrix<val_type> result (matrix_a.rows(), ncols);
+
+	result.setZero();
+
+	unsigned int i,j, k;
+	for (i = 0; i < matrix_a.rows(); i++) {
+		for (j = 0; j < matrix_b.cols(); j++) {
+			for (k = 0; k < matrix_b.rows(); k++) {
+				result(i,j) += matrix_a(i,k) * matrix_b(k,j);
+			}
+		}
+	}
+
+	return result;
+}
+
+// equality
+template <typename val_type, unsigned int nrows, unsigned int ncols>
+inline bool operator==(
+		const Fixed::Matrix<val_type, nrows, ncols> &matrix_a,
+		const Dynamic::Matrix<val_type> &matrix_b) {
+	assert (nrows == matrix_a.rows());
+	assert (ncols == matrix_a.cols());
+
+	unsigned int i;
+	for (i = 0; i < matrix_a.size(); i++) {
+		if (matrix_a[i] != matrix_b[i])
+			return false;
+	}
+
+	return true;
+}
+
+template <typename val_type, unsigned int nrows, unsigned int ncols>
+inline bool operator==(
+		const Dynamic::Matrix<val_type> &matrix_b,
+		const Fixed::Matrix<val_type, nrows, ncols> &matrix_a) {
+	assert (nrows == matrix_a.rows());
+	assert (ncols == matrix_a.cols());
+
+	unsigned int i;
+	for (i = 0; i < matrix_a.size(); i++) {
+		if (matrix_a[i] != matrix_b[i])
+			return false;
+	}
+
+	return true;
+}
+
+}
+
+#endif /* SIMPLEMATHMIXED_H */

src/SimpleMathOld/SimpleMathQR.h

@@ -0,0 +1,324 @@
+#ifndef _SIMPLE_MATH_QR_H
+#define _SIMPLE_MATH_QR_H
+
+#include <iostream>
+#include <limits>
+
+#include "SimpleMathFixed.h"
+#include "SimpleMathDynamic.h"
+#include "SimpleMathBlock.h"
+
+namespace SimpleMath {
+
+	template <typename matrix_type>
+	class HouseholderQR {
+		public:
+			typedef typename matrix_type::value_type value_type;	
+			HouseholderQR() :
+				mIsFactorized(false)
+			{}
+
+		private:
+			typedef Dynamic::Matrix<value_type> MatrixXXd;
+			typedef Dynamic::Matrix<value_type> VectorXd;
+			
+			bool mIsFactorized;
+			MatrixXXd mQ;
+			MatrixXXd mR;
+
+		public:
+			HouseholderQR(const matrix_type &matrix) :
+				mIsFactorized(false),
+				mQ(matrix.rows(), matrix.rows())
+				{
+					compute(matrix);
+			}
+			HouseholderQR compute(const matrix_type &matrix) {
+				mR = matrix;
+				mQ = Dynamic::Matrix<value_type>::Identity (mR.rows(), mR.rows());
+
+				for (unsigned int i = 0; i < mR.cols(); i++) {
+					unsigned int block_rows = mR.rows() - i;
+					unsigned int block_cols = mR.cols() - i;
+
+					MatrixXXd current_block = mR.block(i,i, block_rows, block_cols);
+					VectorXd column = current_block.block(0, 0, block_rows, 1);
+
+					value_type alpha = - column.norm();
+					if (current_block(0,0) < 0) {
+						alpha = - alpha;
+					}
+
+					VectorXd v = current_block.block(0, 0, block_rows, 1);
+					v[0] = v[0] - alpha;
+
+					MatrixXXd Q (MatrixXXd::Identity(mR.rows(), mR.rows()));
+					Q.block(i, i, block_rows, block_rows) = MatrixXXd (Q.block(i, i, block_rows, block_rows))
+						- MatrixXXd(v * v.transpose() / (v.squaredNorm() * 0.5));
+
+					mR = Q * mR;
+
+					// Normalize so that we have positive diagonal elements
+					if (mR(i,i) < 0) {
+						mR.block(i,i,block_rows, block_cols) = MatrixXXd(mR.block(i,i,block_rows, block_cols)) * -1.;
+						Q.block(i,i,block_rows, block_rows) = MatrixXXd(Q.block(i,i,block_rows, block_rows)) * -1.;
+					}
+
+					mQ = mQ * Q;
+				}
+
+				mIsFactorized = true;
+
+				return *this;
+			}
+			Dynamic::Matrix<value_type> solve (
+					const Dynamic::Matrix<value_type> &rhs
+					) const {
+				assert (mIsFactorized);
+
+				VectorXd y = mQ.transpose() * rhs;
+				VectorXd x = VectorXd::Zero(mR.cols());
+
+				for (int i = mR.cols() - 1; i >= 0; --i) {
+					value_type z = y[i];
+
+					for (unsigned int j = i + 1; j < mR.cols(); j++) {
+						z = z - x[j] * mR(i,j);
+					}
+
+					if (fabs(mR(i,i)) < std::numeric_limits<value_type>::epsilon() * 10) {
+						std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero!" << std::endl;
+						abort();
+					}
+					x[i] = z / mR(i,i);
+				}
+
+				return x;
+			}
+			Dynamic::Matrix<value_type> inverse() const {
+				assert (mIsFactorized);
+
+				VectorXd rhs_temp = VectorXd::Zero(mQ.cols());
+				MatrixXXd result (mQ.cols(), mQ.cols());
+
+				for (unsigned int i = 0; i < mQ.cols(); i++) {
+					rhs_temp[i] = 1.;
+
+					result.block(0, i, mQ.cols(), 1) = solve(rhs_temp);
+
+					rhs_temp[i] = 0.;
+				}
+
+				return result;
+			}
+			Dynamic::Matrix<value_type> householderQ () const {
+				return mQ;
+			}
+			Dynamic::Matrix<value_type> matrixR () const {
+				return mR;
+			}
+	};
+
+	template <typename matrix_type>
+	class ColPivHouseholderQR {
+		public:
+			typedef typename matrix_type::value_type value_type;	
+		private:
+			typedef Dynamic::Matrix<value_type> MatrixXXd;
+			typedef Dynamic::Matrix<value_type> VectorXd;
+			
+			bool mIsFactorized;
+			MatrixXXd mQ;
+			MatrixXXd mR;
+			unsigned int *mPermutations;
+			value_type mThreshold;
+			unsigned int mRank;
+
+		public:
+			ColPivHouseholderQR():
+				mIsFactorized(false) {
+					mPermutations = new unsigned int[1];
+				}
+
+			ColPivHouseholderQR (const ColPivHouseholderQR& other) {
+				mIsFactorized = other.mIsFactorized;
+				mQ = other.mQ;
+				mR = other.mR;
+				mPermutations = new unsigned int[mQ.cols()];
+				mThreshold = other.mThreshold;
+				mRank = other.mRank;
+			}
+
+			ColPivHouseholderQR& operator= (const ColPivHouseholderQR& other) {
+				if (this != &other) {
+					mIsFactorized = other.mIsFactorized;
+					mQ = other.mQ;
+					mR = other.mR;
+					delete[] mPermutations;
+					mPermutations = new unsigned int[mQ.cols()];
+					mThreshold = other.mThreshold;
+					mRank = other.mRank;
+				}
+				
+				return *this;
+			}
+
+			ColPivHouseholderQR(const matrix_type &matrix) :
+				mIsFactorized(false),
+				mQ(matrix.rows(), matrix.rows()),
+				mThreshold (std::numeric_limits<value_type>::epsilon() * matrix.cols()) {
+					mPermutations = new unsigned int [matrix.cols()];
+					for (unsigned int i = 0; i < matrix.cols(); i++) {
+						mPermutations[i] = i;
+					}
+					compute(matrix);
+			}
+			~ColPivHouseholderQR() {
+				delete[] mPermutations;
+			}
+
+			ColPivHouseholderQR& setThreshold (const value_type& threshold) {
+				mThreshold = threshold;
+				
+				return *this;
+			}
+			ColPivHouseholderQR& compute(const matrix_type &matrix) {
+				mR = matrix;
+				mQ = Dynamic::Matrix<value_type>::Identity (mR.rows(), mR.rows());
+
+				for (unsigned int i = 0; i < mR.cols(); i++) {
+					unsigned int block_rows = mR.rows() - i;
+					unsigned int block_cols = mR.cols() - i;
+
+					// find and swap the column with the highest norm
+					unsigned int col_index_norm_max = i;
+					value_type col_norm_max = VectorXd(mR.block(i,i, block_rows, 1)).squaredNorm();
+
+					for (unsigned int j = i + 1; j < mR.cols(); j++) {
+						VectorXd column = mR.block(i, j, block_rows, 1);
+						
+						if (column.squaredNorm() > col_norm_max) {
+							col_index_norm_max = j;
+							col_norm_max = column.squaredNorm();
+						}
+					}
+
+					if (col_norm_max < mThreshold) {
+						// if all entries of the column is close to zero, we bail out
+						break;
+					}
+
+
+					if (col_index_norm_max != i) {
+						VectorXd temp_col = mR.block(0, i, mR.rows(), 1);
+						mR.block(0,i,mR.rows(),1) = mR.block(0, col_index_norm_max, mR.rows(), 1);
+						mR.block(0, col_index_norm_max, mR.rows(), 1) = temp_col;
+
+						unsigned int temp_index = mPermutations[i];
+						mPermutations[i] = mPermutations[col_index_norm_max];
+						mPermutations[col_index_norm_max] = temp_index;
+					}
+
+					MatrixXXd current_block = mR.block(i,i, block_rows, block_cols);
+					VectorXd column = current_block.block(0, 0, block_rows, 1);
+
+					value_type alpha = - column.norm();
+					if (current_block(0,0) < 0) {
+						alpha = - alpha;
+					}
+
+					VectorXd v = current_block.block(0, 0, block_rows, 1);
+					v[0] = v[0] - alpha;
+
+					MatrixXXd Q (MatrixXXd::Identity(mR.rows(), mR.rows()));
+
+					Q.block(i, i, block_rows, block_rows) = MatrixXXd (Q.block(i, i, block_rows, block_rows))
+						- MatrixXXd(v * v.transpose() / (v.squaredNorm() * static_cast<value_type>(0.5)));
+
+					mR = Q * mR;
+
+					// Normalize so that we have positive diagonal elements
+					if (mR(i,i) < 0) {
+						mR.block(i,i,block_rows, block_cols) = MatrixXXd(mR.block(i,i,block_rows, block_cols)) * -1.;
+						Q.block(i,i,block_rows, block_rows) = MatrixXXd(Q.block(i,i,block_rows, block_rows)) * -1.;
+					}
+
+					mQ = mQ * Q;
+				}
+
+				mIsFactorized = true;
+
+				return *this;
+			}
+			Dynamic::Matrix<value_type> solve (
+					const Dynamic::Matrix<value_type> &rhs
+					) const {
+				assert (mIsFactorized);
+
+				VectorXd y = mQ.transpose() * rhs;
+				VectorXd x = VectorXd::Zero(mR.cols());
+
+				for (int i = mR.cols() - 1; i >= 0; --i) {
+					value_type z = y[i];
+
+					for (unsigned int j = i + 1; j < mR.cols(); j++) {
+						z = z - x[mPermutations[j]] * mR(i,j);
+					}
+
+					if (fabs(mR(i,i)) < std::numeric_limits<value_type>::epsilon() * 10) {
+						std::cerr << "HouseholderQR: Cannot back-substitute as diagonal element is near zero!" << std::endl;
+						abort();
+					}
+					x[mPermutations[i]] = z / mR(i,i);
+				}
+
+				return x;
+			}
+			Dynamic::Matrix<value_type> inverse() const {
+				assert (mIsFactorized);
+
+				VectorXd rhs_temp = VectorXd::Zero(mQ.cols());
+				MatrixXXd result (mQ.cols(), mQ.cols());
+
+				for (unsigned int i = 0; i < mQ.cols(); i++) {
+					rhs_temp[i] = 1.;
+
+					result.block(0, i, mQ.cols(), 1) = solve(rhs_temp);
+
+					rhs_temp[i] = 0.;
+				}
+
+				return result;
+			}
+
+			Dynamic::Matrix<value_type> householderQ () const {
+				return mQ;
+			}
+			Dynamic::Matrix<value_type> matrixR () const {
+				return mR;
+			}
+			Dynamic::Matrix<value_type> matrixP () const {
+				MatrixXXd P = MatrixXXd::Identity(mR.cols(), mR.cols());
+				MatrixXXd identity = MatrixXXd::Identity(mR.cols(), mR.cols());
+				for (unsigned int i = 0; i < mR.cols(); i++) {
+					P.block(0,i,mR.cols(),1) = identity.block(0,mPermutations[i], mR.cols(), 1);
+				}
+				return P;
+			}
+
+			unsigned int rank() const {
+				value_type abs_threshold = fabs(mR(0,0)) * mThreshold;
+
+				for (unsigned int i = 1; i < mR.cols(); i++) {
+					if (fabs(mR(i,i)) < abs_threshold)
+						return i;
+				}
+
+				return mR.cols();
+			}
+	};
+
+}
+
+/* _SIMPLE_MATH_QR_H */
+#endif

src/SimpleMathOld/compileassert.h

@@ -0,0 +1,39 @@
+#ifndef _COMPILE_ASSERT_H
+#define _COMPILE_ASSERT_H
+
+/* 
+ * This is a simple compile time assertion tool taken from:
+ *   http://blogs.msdn.com/b/abhinaba/archive/2008/10/27/c-c-compile-time-asserts.aspx
+ * written by Abhinaba Basu!
+ *
+ * Thanks!
+ */
+
+#ifdef __cplusplus
+
+#define JOIN( X, Y ) JOIN2(X,Y)
+#define JOIN2( X, Y ) X##Y
+
+namespace static_assert_compat
+{
+    template <bool> struct STATIC_ASSERT_FAILURE;
+    template <> struct STATIC_ASSERT_FAILURE<true> { enum { value = 1 }; };
+
+    template<int x> struct static_assert_test{};
+}
+
+#define COMPILE_ASSERT(x) \
+    typedef ::static_assert_compat::static_assert_test<\
+        sizeof(::static_assert_compat::STATIC_ASSERT_FAILURE< (bool)( x ) >)>\
+            JOIN(_static_assert_typedef, __LINE__)
+
+#else // __cplusplus
+
+#define COMPILE_ASSERT(x) extern int __dummy[(int)x]
+
+#endif // __cplusplus
+
+#define VERIFY_EXPLICIT_CAST(from, to) COMPILE_ASSERT(sizeof(from) == sizeof(to)) 
+
+// _COMPILE_ASSERT_H_
+#endif

src/math_types.h

@@ -1,6 +1,8 @@
 #ifndef MESHUP_CONFIG_H
 #define MESHUP_CONFIG_H
 
+#ifdef NEW_SIMPLE_MATH
+
 #include "SimpleMath/SimpleMath.h"
 
 typedef SimpleMath::Matrix<float, 2, 1> Vector2f;
@@ -23,4 +25,31 @@ typedef SimpleMath::Matrix<double, 3, 3> Matrix33d;
 typedef SimpleMath::Matrix<double> VectorNd;
 typedef SimpleMath::Matrix<double> MatrixNNd;
 
+#else
+
+#include "SimpleMathOld/SimpleMath.h"
+
+typedef SimpleMath::Fixed::Matrix<float, 2, 1> Vector2f;
+typedef SimpleMath::Fixed::Matrix<float, 2, 2> Matrix22f;
+
+typedef SimpleMath::Fixed::Matrix<float, 3, 1> Vector3f;
+typedef SimpleMath::Fixed::Matrix<float, 3, 3> Matrix33f;
+
+typedef SimpleMath::Fixed::Matrix<float, 4, 1> Vector4f;
+typedef SimpleMath::Fixed::Matrix<float, 4, 4> Matrix44f;
+
+typedef SimpleMath::GL::Quaternion Quaternion;
+
+typedef SimpleMath::Dynamic::Matrix<float> VectorNf;
+typedef SimpleMath::Dynamic::Matrix<float> MatrixNNf;
+
+typedef SimpleMath::Fixed::Matrix<double, 3, 1> Vector3d;
+typedef SimpleMath::Fixed::Matrix<double, 3, 3> Matrix33d;
+
+typedef SimpleMath::Dynamic::Matrix<double> VectorNd;
+typedef SimpleMath::Dynamic::Matrix<double> MatrixNNd;
+
+#endif
+
+
 #endif