diff --git a/src/SimpleMath/SimpleMath.h b/src/SimpleMath/SimpleMath.h
index 6182884..dd815bc 100644
--- a/src/SimpleMath/SimpleMath.h
+++ b/src/SimpleMath/SimpleMath.h
@@ -6,32 +6,37 @@
 #include <cstring> // for memcpy
 #include <cmath>
 #include <limits>
+#include <type_traits>
 
 namespace SimpleMath {
 
 //
 // Forward Declarations
 //
+enum {
+        Dynamic = -1
+};
+
+template <typename ScalarType, int NumRows = Dynamic, int NumCols = Dynamic>
+struct Matrix;
+
 template <typename Derived>
 struct CommaInitializer;
 
-template <typename ScalarType, int NumRows, int NumCols>
-struct Fixed;
-
-template <typename ScalarType, int NumRows, int NumCols>
-struct Dynamic;
-
 template <typename Derived, typename ScalarType, int NumRows = -1, int NumCols = -1>
 struct Block;
 
 template <typename Derived, typename ScalarType, int NumRows, int NumCols>
 struct Transpose;
 
-//template <typename Derived>
-//class HouseholderQR;
+typedef Matrix<float, 3, 3> Matrix33f;
+typedef Matrix<float, 3, 1> Vector3f;
 
-//template <typename Derived>
-//class ColPivHouseholderQR;
+template <typename Derived, typename ScalarType, int NumRows, int NumCols>
+class HouseholderQR;
+
+template <typename Derived, typename ScalarType, int NumRows, int NumCols>
+class ColPivHouseholderQR;
 
 
 //
@@ -42,38 +47,50 @@ template <typename Derived, typename ScalarType, int Rows, int Cols>
 struct MatrixBase {
     typedef MatrixBase<Derived, ScalarType, Rows, Cols> MatrixType;
 	typedef ScalarType value_type;
+	
+	template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
+	Derived& operator=(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) {
+		if (static_cast<const void*>(this) != static_cast<const void*>(&other)) {
+			for (size_t i = 0; i < other.rows(); i++) {
+				for (size_t j = 0; j < other.cols(); j++) {
+					this->operator()(i,j) = other(i,j);
+				}
+			}
+		}
 
-	template <typename OtherDerived>
-	Derived& operator=(const OtherDerived& other) {
-		Derived result (other.rows(), other.cols());
+		return *this;
+	}
+
+	//
+	// operators with scalars
+	//
+	Derived operator*(const double& scalar) const {
+		Derived result (rows(), cols());
 
 		unsigned int i,j;
 		for (i = 0; i < rows(); i++) {
 			for (j = 0; j < cols(); j++) {
-				this->operator()(i,j) = other(i,j);
+				result (i,j) = operator()(i,j) * static_cast<ScalarType>(scalar);
+  			}
+		}
+		return result;
+	}
+
+	Derived operator*(const float& scalar) const {
+		Derived result (rows(), cols());
+
+		unsigned int i,j;
+		for (i = 0; i < rows(); i++) {
+			for (j = 0; j < cols(); j++) {
+				result (i,j) = operator()(i,j) * static_cast<ScalarType>(scalar);
 			}
 		}
-		
 		return result;
 	}
 
 	//
 	// operators with other matrices
 	//
-	template <typename OtherDerived>
-	void operator=(const OtherDerived& other) {
-		assert (cols() == other.cols());
-		assert (rows() == other.rows());
-
-		std::cout << "here!!" << std::endl;
-
-		for (int i = 0, nr = rows(); i < nr; ++i) {
-			for (int j = 0, nc = cols(); j < nc; ++j) {
-				operator()(i,j) = other(i,j);
-			}
-		}
-	}
-	
 	template <typename OtherDerived>
 	bool operator==(const OtherDerived& other) const {
 		unsigned int i,j;
@@ -112,22 +129,22 @@ struct MatrixBase {
 		return result;
 	}
 
-    template <typename OtherDerived>
-	Derived operator*(const OtherDerived& other) const {
-	    MatrixBase<Derived, ScalarType,
-	    OtherDerived
-		Derived result (Fixed<ScalarType, Rows, Cols>());
+    template<typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
+    Matrix<ScalarType, Rows, OtherCols>
+    operator*(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols> &other) const {
+        Matrix<ScalarType, Rows, OtherCols> result(Matrix<ScalarType, Rows, OtherCols>::Zero(rows(), other.cols()));
 
-		unsigned int i,j,k;
-		for (i = 0; i < rows(); i++) {
-			for (j = 0; j < other.cols(); j++) {
-				for (k = 0; k < other.rows(); k++) {
-					result (i,j) += operator()(i,k) * other(k,j);
-				}
-			}
-		}
-		return result;
-	}
+        unsigned int i, j, k;
+        for (i = 0; i < rows(); i++) {
+            for (j = 0; j < other.cols(); j++) {
+                for (k = 0; k < other.rows(); k++) {
+                    result(i, j) += operator()(i, k) * other(k, j);
+                }
+            }
+        }
+
+        return result;
+    }
 
 	template <typename OtherDerived>
 	Derived operator*=(const OtherDerived& other) {
@@ -143,11 +160,11 @@ struct MatrixBase {
 			}
 		}
 
-  		return *this;
+  	return *this;
 	}
 
 	void set(const ScalarType& v0) {
-		static_assert(cols() * rows() == 1);
+		static_assert(cols() * rows() == 1, "Invalid matrix size");
 		data()[0] = v0;
 	}
 
@@ -177,21 +194,6 @@ struct MatrixBase {
 		data()[3] = v3;
 	}
 
-	//
-	// operators with scalars
-	//
-	Derived operator*(const ScalarType& scalar) const {
-		Derived result (rows(), cols());
-
-		unsigned int i,j;
-		for (i = 0; i < rows(); i++) {
-			for (j = 0; j < cols(); j++) {
-				result (i,j) = operator()(i,j) * scalar;
-			}
-		}
-		return result;
-	}
-
     size_t rows() const {
 		return static_cast<const Derived*>(this)->rows();
 	}
@@ -207,14 +209,20 @@ struct MatrixBase {
 	}
 
 	const ScalarType& operator[](const size_t& i) const {
-		static_assert(Cols == 1);
+        assert(cols() == 1);
 		return static_cast<const Derived*>(this)->operator()(i,0);
 	}
 	ScalarType& operator[](const size_t& i) {
-		static_assert(Cols == 1);
+	    assert(cols() == 1);
 		return static_cast<Derived*>(this)->operator()(i,0);
 	}
 
+	operator ScalarType() const {
+		assert ( static_cast<const Derived*>(this)->cols() == 1
+				&& static_cast<const Derived*>(this)->rows() == 1);
+		return static_cast<const Derived*>(this)->operator()(0,0);
+	}
+
 	//
 	// Numerical functions
 	//
@@ -291,26 +299,17 @@ struct MatrixBase {
 		return result;
 	}
 
-	Derived inverse() const {
-	    assert(false);
-
-	    Derived result(cols(), rows());
-
-        return result;
-	}
-
-	/*
     Derived inverse() const {
-        return colPivHouseholderQr().inverse();
+      return colPivHouseholderQr().inverse();
     }
 
-    const HouseholderQR<Derived> householderQr() const {
-        return HouseholderQR<Derived>(*this);
+    const HouseholderQR<Derived, ScalarType, Rows, Cols> householderQr() const {
+        return HouseholderQR<Derived, ScalarType, Rows, Cols>(*this);
     }
-    const ColPivHouseholderQR<Derived> colPivHouseholderQr() const {
-        return ColPivHouseholderQR<Derived>(*this);
+
+    const ColPivHouseholderQR<Derived, ScalarType, Rows, Cols> colPivHouseholderQr() const {
+        return ColPivHouseholderQR<Derived, ScalarType, Rows, Cols>(*this);
     }
-    */
 
 	ScalarType* data() {
 		return static_cast<Derived*>(this)->data();
@@ -323,7 +322,7 @@ struct MatrixBase {
 	//
 	// Special Constructors
 	//
-	static Derived Zero(size_t NumRows = Rows, size_t NumCols = Cols) {
+	static Derived Zero(int NumRows = (Rows == Dynamic) ? 1 : Rows, int NumCols = (Cols == Dynamic) ? 1 : Cols) {
 		Derived result (NumRows, NumCols);
 
 		for (size_t i = 0; i < NumRows; i++) {
@@ -336,12 +335,10 @@ struct MatrixBase {
 	}
 
 	static Derived Identity(size_t NumRows = Rows, size_t NumCols = Cols) {
-		Derived result (NumRows, NumCols);
+		Derived result (Derived::Zero(NumRows, NumCols));
 
 		for (size_t i = 0; i < NumRows; i++) {
-			for (size_t j = 0; j < NumCols; j++) {
-				result(i,j) = static_cast<ScalarType>(1.0);
-			}
+				result(i,i) = static_cast<ScalarType>(1.0);
 		}
 
 		return result;
@@ -407,20 +404,17 @@ struct MatrixBase {
 	//
 	// Transpose
 	//
-	Transpose<Derived, ScalarType, Rows, Cols> transpose() {
-		return Transpose<Derived, ScalarType, Rows, Cols>(static_cast<Derived*>(this));
+	Transpose<Derived, ScalarType, Cols, Rows> transpose() {
+		return Transpose<Derived, ScalarType, Cols, Rows>(static_cast<Derived*>(this));
 	}
 
-	const Transpose<const Derived, ScalarType, Rows, Cols> transpose() const {
-		return Transpose<const Derived, ScalarType, Rows, Cols>(static_cast<const Derived*>(this));
-	}
-
-	template <typename OtherDerived>
-	Transpose<OtherDerived, ScalarType, Rows, Cols> transpose() {
-		return Transpose<OtherDerived, ScalarType, Rows, Cols>(static_cast<Derived*>(this));
+	const Transpose<const Derived, ScalarType, Cols, Rows> transpose() const {
+		return Transpose<const Derived, ScalarType, Cols, Rows>(static_cast<const Derived*>(this));
 	}
 };
 
+
+
 template <typename Derived, typename ScalarType, int Rows, int Cols>
 inline Derived operator*(const ScalarType& scalar, const MatrixBase<Derived, ScalarType, Rows, Cols> &matrix) {
     return matrix * scalar;
@@ -436,6 +430,326 @@ inline Derived operator/(const MatrixBase<Derived, ScalarType, Rows, Cols> &matr
     return matrix * (1.0 / scalar);
 }
 
+template <typename ScalarType, int SizeAtCompileTime, int NumRows, int NumCols>
+struct Storage;
+
+template <typename ScalarType, int SizeAtCompileTime, int NumCols>
+struct Storage<ScalarType, SizeAtCompileTime, -1, NumCols> : public Storage<ScalarType, 0, -1, -1> {};
+
+// fixed storage
+template <typename ScalarType, int SizeAtCompileTime, int NumRows, int NumCols>
+struct Storage {
+    ScalarType mData[SizeAtCompileTime];
+
+    Storage() {}
+
+	Storage(int rows, int cols) {
+		resize(rows, cols);
+	}
+
+    size_t rows() const { return NumRows; }
+
+    size_t cols() const { return NumCols; }
+
+    void resize(int num_rows, int num_cols) {
+            // Resizing of fixed size matrices not allowed
+			assert (num_rows == NumRows && num_cols == NumCols);
+    }
+
+    ScalarType& coeff(int row_index, int col_index) {
+        assert (row_index >= 0 && row_index <= NumRows);
+        assert (col_index >= 0 && col_index <= NumCols);
+        return mData[row_index * NumCols + col_index];
+    }
+
+    const ScalarType& coeff(int row_index, int col_index) const {
+        assert (row_index >= 0 && row_index <= NumRows);
+        assert (col_index >= 0 && col_index <= NumCols);
+        return mData[row_index * NumCols + col_index];
+    }
+};
+
+template <typename ScalarType, int NumCols>
+struct Storage<ScalarType, 0, Dynamic, NumCols> {
+    ScalarType* mData = nullptr;
+    int mRows = 0;
+    int mCols = 0;
+
+    Storage() {}
+
+    Storage(int rows, int cols) {
+        resize(rows, cols);
+    }
+
+    size_t rows() const { return mRows; }
+    size_t cols() const { return mCols; }
+
+    void resize(int num_rows, int num_cols) {
+        if (mRows != num_rows || mCols != num_cols) {
+            if (mData != nullptr) {
+                delete[] mData;
+            }
+
+            mData = new ScalarType[num_rows * num_cols];
+            mRows = num_rows;
+            mCols = num_cols;
+        }
+    }
+
+    ScalarType& coeff(int row_index, int col_index) {
+        assert (row_index >= 0 && row_index <= mRows);
+        assert (col_index >= 0 && col_index <= mCols);
+        return mData[row_index * mCols + col_index];
+    }
+    const ScalarType& coeff(int row_index, int col_index) const {
+        assert (row_index >= 0 && row_index <= mRows);
+        assert (col_index >= 0 && col_index <= mCols);
+        return mData[row_index * mCols + col_index];
+    }
+};
+
+
+template <typename ScalarType>
+struct Storage<ScalarType, 0, Dynamic, Dynamic> {
+    ScalarType* mData = nullptr;
+    int mRows = 0;
+    int mCols = 0;
+
+    Storage() {}
+
+    Storage(int rows, int cols) {
+        resize(rows, cols);
+    }
+
+    size_t rows() const { return mRows; }
+    size_t cols() const { return mCols; }
+
+    void resize(int num_rows, int num_cols) {
+        if (mRows != num_rows || mCols != num_cols) {
+            if (mData != nullptr) {
+                delete[] mData;
+            }
+
+            mData = new ScalarType[num_rows * num_cols];
+            mRows = num_rows;
+            mCols = num_cols;
+        }
+    }
+
+    ScalarType& coeff(int row_index, int col_index) {
+        assert (row_index >= 0 && row_index <= mRows);
+        assert (col_index >= 0 && col_index <= mCols);
+        return mData[row_index * mCols + col_index];
+    }
+    const ScalarType& coeff(int row_index, int col_index) const {
+        assert (row_index >= 0 && row_index <= mRows);
+        assert (col_index >= 0 && col_index <= mCols);
+        return mData[row_index * mCols + col_index];
+    }
+    };
+
+
+template <typename ScalarType, int NumRows, int NumCols>
+struct Matrix : public MatrixBase<Matrix<ScalarType, NumRows, NumCols>, ScalarType, NumRows, NumCols>{
+	enum {
+		RowsAtCompileTime = (NumCols == Dynamic || NumRows == Dynamic) ? -1 : NumRows,
+		ColsAtCompileTime = (NumCols == Dynamic || NumRows == Dynamic) ? -1 : NumCols,
+		SizeAtCompileTime = (NumRows != Dynamic && NumCols != Dynamic) ? NumRows * NumCols : 0
+	};
+
+    Storage<ScalarType, SizeAtCompileTime, RowsAtCompileTime, ColsAtCompileTime> mStorage;
+
+	Matrix() :
+		mStorage (
+				SizeAtCompileTime / ColsAtCompileTime,
+                SizeAtCompileTime / RowsAtCompileTime
+		) {}
+
+    explicit Matrix(int rows, int cols) :
+        mStorage(rows, cols) {}
+
+    explicit Matrix (size_t rows, size_t cols) :
+        mStorage(rows, cols) {}
+
+    template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
+	Matrix(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) {
+	    mStorage.resize(other.rows(), other.cols());
+
+		for (size_t i = 0; i < rows(); i++) {
+			for (size_t j = 0; j < cols(); j++) {
+				this->operator()(i,j) = other(i,j);
+			}
+		}
+	}
+
+    //
+    // Constructor for vectors
+    //
+
+    Matrix (
+            const ScalarType& v0
+    ) {
+        static_assert (NumRows * NumCols == 1, "Invalid matrix size");
+
+        operator()(0,0) = v0;
+    }
+
+    Matrix (
+            const ScalarType& v0,
+            const ScalarType& v1
+    ) {
+        static_assert (NumRows * NumCols == 2, "Invalid matrix size");
+
+        operator()(0,0) = v0;
+        operator()(1,0) = v1;
+    }
+
+    Matrix (
+            const ScalarType& v0,
+            const ScalarType& v1,
+            const ScalarType& v2
+    ) {
+        static_assert (NumRows * NumCols == 3, "Invalid matrix size");
+
+        operator()(0,0) = v0;
+        operator()(1,0) = v1;
+        operator()(2,0) = v2;
+    }
+
+    Matrix (
+            const ScalarType& v0,
+            const ScalarType& v1,
+            const ScalarType& v2,
+            const ScalarType& v3
+    ) {
+        static_assert (NumRows * NumCols == 4, "Invalid matrix size");
+
+        operator()(0,0) = v0;
+        operator()(1,0) = v1;
+        operator()(2,0) = v2;
+        operator()(3,0) = v3;
+    }
+
+    //
+    // Constructor for matrices
+    //
+    Matrix (
+            const ScalarType& v00,
+            const ScalarType& v01,
+            const ScalarType& v02,
+            const ScalarType& v10,
+            const ScalarType& v11,
+            const ScalarType& v12,
+            const ScalarType& v20,
+            const ScalarType& v21,
+            const ScalarType& v22
+    ) {
+        static_assert (NumRows == 3 && NumCols == 3, "Invalid matrix size");
+
+        operator()(0,0) = v00;
+        operator()(0,1) = v01;
+        operator()(0,2) = v02;
+
+        operator()(1,0) = v10;
+        operator()(1,1) = v11;
+        operator()(1,2) = v12;
+
+        operator()(2,0) = v20;
+        operator()(2,1) = v21;
+        operator()(2,2) = v22;
+    }
+
+    Matrix (
+            const ScalarType& v00,
+            const ScalarType& v01,
+            const ScalarType& v02,
+            const ScalarType& v03,
+            const ScalarType& v10,
+            const ScalarType& v11,
+            const ScalarType& v12,
+            const ScalarType& v13,
+            const ScalarType& v20,
+            const ScalarType& v21,
+            const ScalarType& v22,
+            const ScalarType& v23,
+            const ScalarType& v30,
+            const ScalarType& v31,
+            const ScalarType& v32,
+            const ScalarType& v33
+    ) {
+        static_assert (NumRows == 4 && NumCols == 4, "Invalid matrix size");
+
+        operator()(0,0) = v00;
+        operator()(0,1) = v01;
+        operator()(0,2) = v02;
+        operator()(0,3) = v03;
+
+        operator()(1,0) = v10;
+        operator()(1,1) = v11;
+        operator()(1,2) = v12;
+        operator()(1,3) = v13;
+
+        operator()(2,0) = v20;
+        operator()(2,1) = v21;
+        operator()(2,2) = v22;
+        operator()(2,3) = v23;
+
+        operator()(3,0) = v30;
+        operator()(3,1) = v31;
+        operator()(3,2) = v32;
+        operator()(3,3) = v33;
+    }
+
+    template <typename OtherDerived>
+	Matrix& operator+=(const OtherDerived& other) {
+		assert (NumRows == other.rows() && NumCols == other.cols() && "Error: matrix dimensions do not match!");
+
+		for (size_t i = 0; i < rows(); i++) {
+			for (size_t j = 0; j < cols(); j++) {
+				this->operator()(i,j) += other(i,j);
+			}
+		}
+		return *this;
+	}
+
+	template <typename OtherDerived>
+	Matrix& operator-=(const OtherDerived& other) {
+		assert (rows() == other.rows() && cols() == other.cols() && "Error: matrix dimensions do not match!");
+
+		for (size_t i = 0; i < rows(); i++) {
+			for (size_t j = 0; j < cols(); j++) {
+				this->operator()(i,j) -= other(i,j);
+			}
+		}
+		return *this;
+	}
+
+	ScalarType& operator()(const size_t& i, const size_t& j) {
+		return mStorage.coeff(i, j);
+	}
+
+	ScalarType* data() {
+        return mStorage.mData;
+	}
+
+	const ScalarType* data() const {
+        return mStorage.mData;
+	}
+
+	const ScalarType& operator()(const size_t& i, const size_t& j) const {
+	    return mStorage.coeff(i, j);
+	}
+
+	size_t cols() const {
+	    return mStorage.cols();
+	}
+
+	size_t rows() const {
+	    return mStorage.rows();
+	}
+};
+	
+
 //
 // CommaInitializer
 //
@@ -512,8 +826,7 @@ struct CommaInitializer {
 //
 template <typename Derived, typename ScalarType, int NumRows, int NumCols>
 struct Transpose : public MatrixBase<Transpose<Derived, ScalarType, NumRows, NumCols>, ScalarType, NumRows, NumCols> {
-	typedef MatrixBase<Derived, ScalarType, NumCols, NumRows> MatrixType;
-	Derived* mTransposeSource;	
+	Derived* mTransposeSource;
 
 	Transpose(Derived* transpose_source) :
 		mTransposeSource(transpose_source)
@@ -523,15 +836,35 @@ struct Transpose : public MatrixBase<Transpose<Derived, ScalarType, NumRows, Num
 		mTransposeSource(other.mTransposeSource)
 	{ }
 
-	template <typename OtherDerived>
-	Transpose& operator=(const OtherDerived& other) {
-		unsigned int i,j;
-		for (i = 0; i < rows(); i++) {
-			for (j = 0; j < cols(); j++) {
-				this->operator()(i,j) = other(i,j);
+  template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
+  Matrix<ScalarType, NumRows, OtherCols> operator*(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) const {
+		Matrix<ScalarType, NumRows, OtherCols> result (Matrix<ScalarType, NumRows, OtherCols>::Zero(rows(), other.cols()));
+
+		unsigned int i,j,k;
+		unsigned int nrows = rows();
+		unsigned int other_ncols = other.cols();
+		unsigned int other_nrows = other.rows();
+
+		for (i = 0; i < nrows; i++) {
+			for (j = 0; j < other_ncols; j++) {
+				for (k = 0; k < other_nrows; k++) {
+					result (i,j) += operator()(i,k) * other(k,j);
+				}
 			}
 		}
-		
+		return result;
+	}
+
+	template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
+	Transpose& operator=(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) {
+		if (static_cast<const void*>(this) != static_cast<const void*>(&other)) {
+			for (size_t i = 0; i < other.rows(); i++) {
+				for (size_t j = 0; j < other.cols(); j++) {
+					this->operator()(i,j) = other(i,j);
+				}
+			}
+		}
+
 		return *this;
 	}
 
@@ -587,21 +920,32 @@ struct Block : public MatrixBase<Block<Derived, ScalarType, NumRows, NumCols>, S
 		col_index(other.col_index)
 	{ }
 
-	template <typename OtherDerived>
-	Derived& operator=(const OtherDerived& other) {
+	Block& operator=(const Block &other) {
 		unsigned int i,j;
 		for (i = 0; i < rows(); i++) {
 			for (j = 0; j < cols(); j++) {
 				this->operator()(i,j) = other(i,j);
 			}
 		}
-		
-		return *mBlockSource;
+
+		return *this;
 	}
-	
-	template <typename OtherDerived>
-	Dynamic<ScalarType, -1, -1> operator*(const OtherDerived& other) {
-		Dynamic<ScalarType, -1, -1> result (Dynamic<ScalarType, -1, -1>::Zero(rows(), other.cols()));
+
+	template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
+	Block& operator=(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) {
+		unsigned int i,j;
+		for (i = 0; i < rows(); i++) {
+			for (j = 0; j < cols(); j++) {
+				this->operator()(i,j) = other(i,j);
+			}
+		}
+
+		return *this;
+	}
+
+	template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
+	Matrix<ScalarType, NumRows, OtherCols> operator*(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) const {
+		Matrix<ScalarType, NumRows, OtherCols> result (rows(), other.cols());
 
 		unsigned int i,j,k;
 		for (i = 0; i < rows(); i++) {
@@ -614,7 +958,6 @@ struct Block : public MatrixBase<Block<Derived, ScalarType, NumRows, NumCols>, S
 		return result;
 	}
 
-
 	size_t rows() const {
 		return nrows;
 	}
@@ -629,10 +972,16 @@ struct Block : public MatrixBase<Block<Derived, ScalarType, NumRows, NumCols>, S
 		return static_cast<Derived*>(mBlockSource)->operator()(row_index + i,col_index + j);
 	}
 
-	template <typename OtherDerived>
-	Dynamic<ScalarType, -1, -1> operator+(const OtherDerived& other) {
-		Dynamic<ScalarType, -1, -1> result (*this);
-		result += other;
+	template <typename OtherDerived, typename OtherScalarType, int OtherRows, int OtherCols>
+	Matrix<ScalarType, NumRows, OtherCols> operator+(const MatrixBase<OtherDerived, OtherScalarType, OtherRows, OtherCols>& other) const {
+		Matrix<ScalarType, NumRows, OtherCols> result (rows(), other.cols());
+
+		unsigned int i,j,k;
+		for (i = 0; i < rows(); i++) {
+			for (j = 0; j < other.cols(); j++) {
+				result (i,j) = operator()(i,j) + other(i,j);
+			}
+		}
 		return result;
 	}
 
@@ -650,700 +999,338 @@ struct Block : public MatrixBase<Block<Derived, ScalarType, NumRows, NumCols>, S
 	}
 };
 
-//
-// Fixed Matrices (i.e. size defined at compile time)
-//
-template <typename val_type, int NumRows, int NumCols>
-struct Fixed : public MatrixBase<Fixed<val_type, NumRows, NumCols>, val_type, NumRows, NumCols> {
-	typedef Fixed<val_type, NumRows, NumCols> matrix_type;
-	typedef val_type value_type;
-
-	val_type mData[NumRows * NumCols];
-
-	Fixed() {
-		for (size_t i = 0; i < NumRows * NumCols; i++) {
-			mData[i] = static_cast<val_type> (0.);
-		}
-	}
-
-	Fixed (const Fixed &other) {
-		assert (NumRows == other.rows() && NumCols == other.cols() && "Error: matrix dimensions do not match!");
-		memcpy (mData, other.data(), sizeof (val_type) * rows() * cols());
-	}
-
-	template <typename OtherDerived>
-	Fixed(const OtherDerived &other) {
-		std::cout << "FCC" << std::endl;
-		assert (other.rows() == NumRows && other.cols() == NumCols);
-
-		for (size_t i = 0; i < rows(); i++) {
-			for (size_t j = 0; j < cols(); j++) {
-				this->operator()(i,j) = other(i,j);
-			}
-		}
-	}
-
-	template <typename OtherDerived>
-	Fixed& operator=(const OtherDerived& other) {
-		if (static_cast<const void*>(this) != static_cast<const void*>(&other)) {
-			std::cout << "Fixed AO" << std::endl;
-
-			for (size_t i = 0; i < other.rows(); i++) {
-				for (size_t j = 0; j < other.cols(); j++) {
-					this->operator()(i,j) = other(i,j);
-				}
-			}
-		}
-
-		return *this;
-	}
-
-	explicit Fixed (size_t rows, size_t cols) {
-		assert (rows == NumRows);
-		assert (cols == cols);
-	}
-
-
-	//
-	// Constructor for vectors
-	//
-
-	Fixed (
-			const val_type& v0
-			) {
-		static_assert (NumRows * NumCols == 1);
-
-		mData[0] = v0;
-	}
-
-	Fixed (
-			const val_type& v0,
-			const val_type& v1
-			) {
-		static_assert (NumRows * NumCols == 2);
-
-		mData[0] = v0;
-		mData[1] = v1;
-	}
-
-	Fixed (
-			const val_type& v0,
-			const val_type& v1,
-			const val_type& v2
-			) {
-		static_assert (NumRows * NumCols == 3);
-
-		mData[0] = v0;
-		mData[1] = v1;
-		mData[2] = v2;
-	}
-
-	Fixed (
-			const val_type& v0,
-			const val_type& v1,
-			const val_type& v2,
-			const val_type& v3
-			) {
-		static_assert (NumRows * NumCols == 4);
-
-		mData[0] = v0;
-		mData[1] = v1;
-		mData[2] = v2;
-		mData[3] = v3;
-	}
-
-	//
-	// Constructor for matrices
-	//
-	Fixed (
-			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
-			) {
-		static_assert (NumRows == 3 && NumCols == 3);
-
-		operator()(0,0) = v00;
-		operator()(0,1) = v01;
-		operator()(0,2) = v02;
-
-		operator()(1,0) = v10;
-		operator()(1,1) = v11;
-		operator()(1,2) = v12;
-
-		operator()(2,0) = v20;
-		operator()(2,1) = v21;
-		operator()(2,2) = v22;
-	}
-
-	Fixed (
-			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
-			) {
-		static_assert (NumRows == 4 && NumCols == 4);
-
-		operator()(0,0) = v00;
-		operator()(0,1) = v01;
-		operator()(0,2) = v02;
-		operator()(0,3) = v03;
-
-		operator()(1,0) = v10;
-		operator()(1,1) = v11;
-		operator()(1,2) = v12;
-		operator()(1,3) = v13;
-
-		operator()(2,0) = v20;
-		operator()(2,1) = v21;
-		operator()(2,2) = v22;
-		operator()(2,3) = v23;
-
-		operator()(3,0) = v30;
-		operator()(3,1) = v31;
-		operator()(3,2) = v32;
-		operator()(3,3) = v33;
-	}
-
-
-
-	template <typename OtherDerived>
-	Fixed& operator+=(const OtherDerived& other) {
-		assert (NumRows == other.rows() && NumCols == other.cols() && "Error: matrix dimensions do not match!");
-
-		for (size_t i = 0; i < rows(); i++) {
-			for (size_t j = 0; j < cols(); j++) {
-				this->operator()(i,j) += other(i,j);
-			}
-		}
-		return *this;
-	}
-
-	template <typename OtherDerived>
-	Fixed& operator-=(const OtherDerived& other) {
-		assert (NumRows == other.rows() && NumCols == other.cols() && "Error: matrix dimensions do not match!");
-
-		for (size_t i = 0; i < rows(); i++) {
-			for (size_t j = 0; j < cols(); j++) {
-				this->operator()(i,j) -= other(i,j);
-			}
-		}
-		return *this;
-	}
-
-	val_type& operator()(const size_t& i, const size_t& j) {
-		return mData[i*NumCols + j];
-	}
-
-	val_type* data() {
-		return mData;
-	}
-
-	const val_type* data() const {
-		return mData;
-	}
-
-	const val_type& operator()(const size_t& i, const size_t& j) const {
-		return mData[i*NumCols + j];
-	}
-
-	size_t cols() const {
-		return NumCols;
-	}
-
-	size_t rows() const {
-		return NumRows;
-	}
-};
-
-//
-// Dynamic Matrices
-//
-template <typename ScalarType, int Rows = -1, int Cols = -1>
-struct Dynamic : public MatrixBase<Dynamic<ScalarType, -1, -1>, ScalarType, -1, -1> {
-	typedef Dynamic<ScalarType> matrix_type;
-
-	ScalarType* mData;
-	int mNumRows;
-	int mNumCols;
-
-	Dynamic() : 
-		mData (NULL),
-		mNumRows (0),
-		mNumCols (0)
-	{
-	}
-
-	Dynamic(int num_rows, int num_cols) :
-		mNumRows (num_rows),
-		mNumCols (num_cols) {
-		mData = new ScalarType[mNumRows * mNumCols];
-
-		for (size_t i = 0; i < (size_t) mNumRows * (size_t) mNumCols; i++) {
-			mData[i] = static_cast<ScalarType> (0.);
-		}
-	}
-
-	~Dynamic() {
-		if (mData != NULL) {
-			delete[] mData;
-			mData = NULL;
-		}
-	}
-
-	Dynamic(const Dynamic &other) :
-		mNumRows (other.rows()),
-		mNumCols (other.cols()) {
-		mData = new ScalarType[mNumRows * mNumCols];
-		assert (mNumRows == (int) other.rows() && mNumCols == (int) other.cols() && "Error: matrix dimensions do not match!");
-		memcpy (mData, other.data(), sizeof (ScalarType) * rows() * cols());
-	}
-
-	template <typename OtherDerived>
-	Dynamic(const OtherDerived &other) :
-		mNumRows (other.rows()),
-		mNumCols (other.cols()) {
-		mData = new ScalarType[mNumRows * mNumCols];
-		assert (mNumRows == other.rows() && mNumCols == other.cols() && "Error: matrix dimensions do not match!");
-		for (size_t i = 0; i < rows(); i++) {
-			for (size_t j = 0; j < cols(); j++) {
-				this->operator()(i,j) = other(i,j);
-			}
-		}
-	}
-
-	Dynamic& operator=(const Dynamic &other) {
-		if (static_cast<const void*>(this) != static_cast<const void*>(&other)) {
-			assert (mData != NULL);
-			delete[] mData;
-
-			mNumRows = other.rows();
-			mNumCols = other.cols();
-			mData = new ScalarType[mNumRows * mNumCols];
-			
-			for (size_t i = 0; i < rows(); i++) {
-				for (size_t j = 0; j < cols(); j++) {
-					this->operator()(i,j) = other(i,j);
-				}
-			}
-		}
-
-		return *this;
-	}
-
-	template <typename OtherDerived>
-	Dynamic& operator=(const OtherDerived &other) {
-		if (static_cast<const void*>(this) != static_cast<const void*>(&other)) {
-			assert (mData != NULL);
-			delete[] mData;
-
-			mNumRows = other.rows();
-			mNumCols = other.cols();
-			mData = new ScalarType[mNumRows * mNumCols];
-			
-			for (size_t i = 0; i < rows(); i++) {
-				for (size_t j = 0; j < cols(); j++) {
-					this->operator()(i,j) = other(i,j);
-				}
-			}
-		}
-
-		return *this;
-	}
-
-	template <typename OtherDerived>
-	Dynamic& operator+=(const OtherDerived& other) {
-		assert (mNumRows == (int) other.rows() && mNumCols == (int) other.cols() && "Error: matrix dimensions do not match!");
-
-		for (size_t i = 0; i < rows(); i++) {
-			for (size_t j = 0; j < cols(); j++) {
-				this->operator()(i,j) += other(i,j);
-			}
-		}
-		
-		return *this;
-	}
-
-	template <typename OtherDerived>
-	Dynamic& operator-=(const OtherDerived& other) {
-		assert (mNumRows == (int) other.rows() && mNumCols == (int) other.cols() && "Error: matrix dimensions do not match!");
-
-		for (size_t i = 0; i < rows(); i++) {
-			for (size_t j = 0; j < cols(); j++) {
-				this->operator()(i,j) -= other(i,j);
-			}
-		}
-		
-		return *this;
-	}
-
-	ScalarType& operator[](const size_t& i) {
-		return mData[i];
-	}
-
-	ScalarType& operator()(const size_t& i, const size_t& j) {
-		return mData[i*cols() + j];
-	}
-
-	const ScalarType& operator()(const size_t& i, const size_t& j) const {
-		return mData[i*cols() + j];
-	}
-
-	ScalarType* data() {
-		return mData;
-	}
-
-	const ScalarType* data() const {
-		return mData;
-	}
-
-	size_t cols() const {
-		return mNumCols;
-	}
-
-	size_t rows() const {
-		return mNumRows;
-	}
-};
-
 //
 // QR Decomposition
 //
-    template <typename matrix_type>
-    class HouseholderQR {
-    public:
-        typedef typename matrix_type::value_type value_type;
-        HouseholderQR() :
-                mIsFactorized(false)
-        {}
+template <typename Derived, typename ScalarType, int NumRows, int NumCols>
+class HouseholderQR {
+public:
+    typedef MatrixBase<Derived, ScalarType, NumRows, NumCols> MatrixType;
+    typedef typename MatrixType::value_type value_type;
 
-    private:
-        typedef Dynamic<value_type> VectorXd;
-        typedef Dynamic<value_type> MatrixXXd;
+    HouseholderQR() :
+            mIsFactorized(false)
+    {}
 
-        bool mIsFactorized;
-        MatrixXXd mQ;
-        MatrixXXd mR;
+private:
+    typedef Matrix<value_type> VectorXd;
+    typedef Matrix<value_type> MatrixXXd;
+	typedef Matrix<ScalarType, NumRows, 1> ColumnVector;
 
-    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<value_type>::Identity (mR.rows(), mR.rows());
+    bool mIsFactorized;
+    Matrix<ScalarType, NumRows, NumRows> mQ;
+    Derived mR;
 
-            for (unsigned int i = 0; i < mR.cols(); i++) {
-                unsigned int block_rows = mR.rows() - i;
-                unsigned int block_cols = mR.cols() - i;
+public:
+    HouseholderQR(const MatrixType &matrix) :
+            mIsFactorized(false),
+            mQ(matrix.rows(), matrix.rows())
+    {
+        compute(matrix);
+    }
+    HouseholderQR compute(const MatrixType &matrix) {
+        mR = matrix;
+        mQ = MatrixType::Identity (mR.rows(), mR.rows());
 
-                MatrixXXd current_block = mR.block(i,i, block_rows, block_cols);
-                VectorXd column = current_block.block(0, 0, block_rows, 1);
+        for (unsigned int i = 0; i < mR.cols(); i++) {
+            unsigned int block_rows = mR.rows() - i;
+            unsigned int block_cols = mR.cols() - i;
 
-                value_type alpha = - column.norm();
-                if (current_block(0,0) < 0) {
-                    alpha = - alpha;
-                }
+            MatrixXXd current_block = mR.block(i,i, block_rows, block_cols);
+            VectorXd column = current_block.block(0, 0, block_rows, 1);
 
-                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;
+            value_type alpha = - column.norm();
+            if (current_block(0,0) < 0) {
+                alpha = - alpha;
             }
 
-            mIsFactorized = true;
+            VectorXd v = current_block.block(0, 0, block_rows, 1);
+            v[0] = v[0] - alpha;
 
-            return *this;
-        }
-        Dynamic<value_type> solve (
-                const Dynamic<value_type> &rhs
-        ) const {
-            assert (mIsFactorized);
+            MatrixXXd Q (MatrixXXd::Identity(mR.rows(), mR.rows()));
 
-            VectorXd y = mQ.transpose() * rhs;
-            VectorXd x = VectorXd::Zero(mR.cols());
+            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));
 
-            for (int i = mR.cols() - 1; i >= 0; --i) {
-                value_type z = y[i];
+            mR = Q * mR;
 
-                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);
+            // 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.;
             }
 
-            return x;
+            mQ = mQ * Q;
         }
-        Dynamic<value_type> inverse() const {
-            assert (mIsFactorized);
 
-            VectorXd rhs_temp = VectorXd::Zero(mQ.cols());
-            MatrixXXd result (mQ.cols(), mQ.cols());
+        mIsFactorized = true;
 
-            for (unsigned int i = 0; i < mQ.cols(); i++) {
-                rhs_temp[i] = 1.;
+        return *this;
+    }
+	ColumnVector solve (
+            const ColumnVector &rhs
+    ) const {
+        assert (mIsFactorized);
 
-                result.block(0, i, mQ.cols(), 1) = solve(rhs_temp);
+		ColumnVector y = mQ.transpose() * rhs;
+		ColumnVector x = ColumnVector::Zero(mR.cols());
 
-                rhs_temp[i] = 0.;
+        int ncols = mR.cols();
+        for (int i = ncols - 1; i >= 0; i--) {
+            value_type z = y[i];
+
+            for (unsigned int j = i + 1; j < ncols; j++) {
+                z = z - x[j] * mR(i,j);
             }
 
-            return result;
+            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);
         }
-        Dynamic<value_type> householderQ () const {
-            return mQ;
-        }
-        Dynamic<value_type> matrixR () const {
-            return mR;
-        }
-    };
 
-template <typename matrix_type>
+        return x;
+    }
+    Derived 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;
+    }
+    Matrix<value_type> householderQ () const {
+        return mQ;
+    }
+    Derived matrixR () const {
+        return mR;
+    }
+};
+
+template <typename Derived, typename ScalarType, int NumRows, int NumCols>
 class ColPivHouseholderQR {
-    public:
-        typedef typename matrix_type::value_type value_type;
-    private:
-        typedef Dynamic<value_type> MatrixXXd;
-        typedef Dynamic<value_type> VectorXd;
+public:
+    typedef MatrixBase<Derived, ScalarType, NumRows, NumCols> MatrixType;
+    typedef typename MatrixType::value_type value_type;
 
-        bool mIsFactorized;
-        MatrixXXd mQ;
-        MatrixXXd mR;
-        unsigned int *mPermutations;
-        value_type mThreshold;
-        unsigned int mRank;
 
-    public:
-        ColPivHouseholderQR():
-                mIsFactorized(false) {
-            mPermutations = new unsigned int[1];
-        }
+private:
+    typedef Matrix<value_type> VectorXd;
+    typedef Matrix<value_type> MatrixXXd;
+	typedef Matrix<ScalarType, NumRows, 1> ColumnVector;
 
-        ColPivHouseholderQR (const ColPivHouseholderQR& other) {
+    bool mIsFactorized;
+    Matrix<ScalarType, NumRows, NumRows> mQ;
+    Derived 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;
         }
 
-        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 MatrixType &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 MatrixType &matrix) {
+        mR = matrix;
+        mQ = MatrixType::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();
+                }
             }
 
-            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<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;
+            if (col_norm_max < mThreshold) {
+                // if all entries of the column is close to zero, we bail out
+                break;
             }
 
-            mIsFactorized = true;
 
-            return *this;
-        }
-        Dynamic<value_type> solve (
-                const Dynamic<value_type> &rhs
-        ) const {
-            assert (mIsFactorized);
+            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;
 
-            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);
+                unsigned int temp_index = mPermutations[i];
+                mPermutations[i] = mPermutations[col_index_norm_max];
+                mPermutations[col_index_norm_max] = temp_index;
             }
 
-            return x;
-        }
-        Dynamic<value_type> inverse() const {
-            assert (mIsFactorized);
+			MatrixXXd current_block = mR.block(i,i, block_rows, block_cols);
+            VectorXd column = current_block.block(0, 0, block_rows, 1);
 
-            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.;
+            value_type alpha = - column.norm();
+            if (current_block(0,0) < 0) {
+                alpha = - alpha;
             }
 
-            return result;
-        }
+            VectorXd v = current_block.block(0, 0, block_rows, 1);
+            v[0] = v[0] - alpha;
 
-        Dynamic<value_type> householderQ () const {
-            return mQ;
-        }
-        Dynamic<value_type> matrixR () const {
-            return mR;
-        }
-        Dynamic<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;
-        }
+            MatrixXXd Q (MatrixXXd::Identity(mR.rows(), mR.rows()));
 
-        unsigned int rank() const {
-            value_type abs_threshold = fabs(mR(0,0)) * mThreshold;
+            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)));
 
-            for (unsigned int i = 1; i < mR.cols(); i++) {
-                if (fabs(mR(i,i)) < abs_threshold)
-                    return i;
+            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.;
             }
 
-            return mR.cols();
+            mQ = mQ * Q;
         }
-    };
 
+        mIsFactorized = true;
+
+        return *this;
+    }
+	ColumnVector solve (
+            const ColumnVector &rhs
+    ) const {
+        assert (mIsFactorized);
+
+		ColumnVector y = mQ.transpose() * rhs;
+		ColumnVector x = ColumnVector::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;
+    }
+    Derived inverse() const {
+        assert (mIsFactorized);
+
+        VectorXd rhs_temp = VectorXd::Zero(mQ.cols());
+        Derived 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;
+    }
+
+    Matrix<value_type> householderQ () const {
+        return mQ;
+    }
+    Derived matrixR () const {
+        return mR;
+    }
+    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();
+    }
+};
 
 //
 // OpenGL Matrices and Quaternions
 //
 
-typedef Fixed<float, 3, 1> Vector3f;
-typedef Fixed<float, 3, 3> Matrix33f;
+namespace GL {
 
-typedef Fixed<float, 4, 1> Vector4f;
-typedef Fixed<float, 4, 4> Matrix44f;
+typedef Matrix<float, 3, 1> Vector3f;
+typedef Matrix<float, 3, 3> Matrix33f;
+
+typedef Matrix<float, 4, 1> Vector4f;
+typedef Matrix<float, 4, 4> Matrix44f;
 
 inline Matrix33f RotateMat33 (float rot_deg, float x, float y, float z) {
 	float c = cosf (rot_deg * M_PI / 180.f);
@@ -1466,10 +1453,11 @@ inline Matrix44f LookAt(
 			);
 }
 
-/** Quaternion 
- *
- * order: x,y,z,w
- */
+//
+// Quaternion
+//
+// order: x,y,z,w
+
 class Quaternion : public Vector4f {
 	public:
 		Quaternion () :
@@ -1481,7 +1469,7 @@ class Quaternion : public Vector4f {
 		Quaternion (float x, float y, float z, float w):
 			Vector4f (x, y, z, w)
 		{}
-		/** This function is equivalent to multiplicate their corresponding rotation matrices */
+		// 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],
@@ -1627,7 +1615,7 @@ class Quaternion : public Vector4f {
 						+(*this)[3] * (*this)[3] - (*this)[0] * (*this)[0]
 						)
 					);
-		}
+		};
 
 		Matrix33f toMatrix() const {
 			float x = (*this)[0];
@@ -1668,6 +1656,8 @@ class Quaternion : public Vector4f {
 		}
 };
 
+} /* namespace GL */
+
 
 //
 // Stream operators
diff --git a/src/math_types.h b/src/math_types.h
index f744b51..8c793dc 100644
--- a/src/math_types.h
+++ b/src/math_types.h
@@ -3,24 +3,24 @@
 
 #include "SimpleMath/SimpleMath.h"
 
-typedef SimpleMath::Fixed<float, 2, 1> Vector2f;
-typedef SimpleMath::Fixed<float, 2, 2> Matrix22f;
+typedef SimpleMath::Matrix<float, 2, 1> Vector2f;
+typedef SimpleMath::Matrix<float, 2, 2> Matrix22f;
 
-typedef SimpleMath::Fixed<float, 3, 1> Vector3f;
-typedef SimpleMath::Fixed<float, 3, 3> Matrix33f;
+typedef SimpleMath::Matrix<float, 3, 1> Vector3f;
+typedef SimpleMath::Matrix<float, 3, 3> Matrix33f;
 
-typedef SimpleMath::Fixed<float, 4, 1> Vector4f;
-typedef SimpleMath::Fixed<float, 4, 4> Matrix44f;
+typedef SimpleMath::Matrix<float, 4, 1> Vector4f;
+typedef SimpleMath::Matrix<float, 4, 4> Matrix44f;
 
-typedef SimpleMath::Quaternion Quaternion;
+typedef SimpleMath::GL::Quaternion Quaternion;
 
-typedef SimpleMath::Dynamic<float> VectorNf;
-typedef SimpleMath::Dynamic<float> MatrixNNf;
+typedef SimpleMath::Matrix<float> VectorNf;
+typedef SimpleMath::Matrix<float> MatrixNNf;
 
-typedef SimpleMath::Fixed<double, 3, 1> Vector3d;
-typedef SimpleMath::Fixed<double, 3, 3> Matrix33d;
+typedef SimpleMath::Matrix<double, 3, 1> Vector3d;
+typedef SimpleMath::Matrix<double, 3, 3> Matrix33d;
 
-typedef SimpleMath::Dynamic<double> VectorNd;
-typedef SimpleMath::Dynamic<double> MatrixNNd;
+typedef SimpleMath::Matrix<double> VectorNd;
+typedef SimpleMath::Matrix<double> MatrixNNd;
 
 #endif
diff --git a/src/modules/RenderModule.cc b/src/modules/RenderModule.cc
index a1bb16d..17ead42 100644
--- a/src/modules/RenderModule.cc
+++ b/src/modules/RenderModule.cc
@@ -10,7 +10,7 @@
 #include "imgui/imgui.h"
 #include "imgui_dock.h"
 
-using namespace SimpleMath;
+using namespace SimpleMath::GL;
 
 struct Renderer;
 
diff --git a/src/modules/RenderUtils.cc b/src/modules/RenderUtils.cc
index 9b6c595..215c6d0 100644
--- a/src/modules/RenderUtils.cc
+++ b/src/modules/RenderUtils.cc
@@ -12,7 +12,7 @@
 #include "imgui/imgui.h"
 #include "imgui_dock.h"
 
-using namespace SimpleMath;
+using namespace SimpleMath::GL;
 
 typedef tinygltf::TinyGLTF GLTFLoader;
 
diff --git a/src/modules/TestModule.cc b/src/modules/TestModule.cc
index c0814d7..f17290b 100644
--- a/src/modules/TestModule.cc
+++ b/src/modules/TestModule.cc
@@ -46,10 +46,10 @@ void handle_mouse (struct module_state *state) {
 		view_dir = (poi - eye).normalized();
 		Vector3f right = camera_rot_inv.block<1,3>(0,0).transpose();
 		right = view_dir.cross (Vector3f (0.f, 1.f, 0.f));
-		Matrix33f rot_matrix_y = SimpleMath::RotateMat33(
+		Matrix33f rot_matrix_y = SimpleMath::GL::RotateMat33(
 				gGuiInputState->mousedY * 0.4f,
 				right[0], right[1], right[2]);
-		Matrix33f rot_matrix_x = SimpleMath::RotateMat33(
+		Matrix33f rot_matrix_x = SimpleMath::GL::RotateMat33(
 				gGuiInputState->mousedX * 0.4f,
 				0.f, 1.f, 0.f);
 		poi = eye + rot_matrix_x * rot_matrix_y * view_dir;