Commits

Martin Felis committed dd1fad7

Upgraded SimpleMath

Comments (0)

Files changed (6)

include/rbdl/SimpleMath/SimpleMath.h

 #include "SimpleMathFixed.h"
 #include "SimpleMathDynamic.h"
 #include "SimpleMathMixed.h"
+#include "SimpleMathQR.h"
 
 typedef SimpleMath::Fixed::Matrix<int, 3, 1> Vector3i;
 

include/rbdl/SimpleMath/SimpleMathBlock.h

+/**
+ * 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[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];
+					}
+				}
+			}
+
+			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[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];
+				}
+			}
+
+			return *this;
+		}
+
+		unsigned int rows() const {
+			return mRowCount;
+		}
+		unsigned int cols() const {
+			return mColCount;
+		}
+		const val_type& operator() (const unsigned int i, const unsigned int j) const {
+			assert (i < mRowCount);
+			assert (j < mColCount);
+
+			if (!mTransposed) {
+				return (*mParentMatrix) (i + mParentRowStart, j + mParentColStart);
+			}
+
+			return (*mParentMatrix) (j + mParentRowStart, i + mParentColStart);
+		}
+
+		val_type& operator() (const unsigned int i, const unsigned int j) {
+			assert (i < mRowCount);
+			assert (j < mColCount);
+
+			if (!mTransposed) {
+				return (*mParentMatrix) (i + mParentRowStart, j + mParentColStart);
+			}
+
+			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 */

include/rbdl/SimpleMath/SimpleMathDynamic.h

 #include <assert.h>
 
 #include "compileassert.h"
+#include "SimpleMathBlock.h"
 
 /** \brief Namespace for a highly inefficient math library
  *
  */
 namespace SimpleMath {
 
+namespace Fixed {
+	template <typename val_type, unsigned int ncols, unsigned int nrows> class Matrix;
+}
+
 /** \brief Namespace for elements of varying size.
  */
 namespace Dynamic {
 template <typename val_type>
 class Matrix;
 
-/** \brief Block class that can be used to access blocks of a matrix.
- *
- * This class is a proxy class and only contains data on where to find the
- * desired information.
- *
- */
-template <typename val_type, unsigned int block_rows, unsigned int block_cols>
-class Block {
-	public:
-	Block () :
-		parent_nrows(0),
-		parent_ncols(0),
-		parent_row_index(0),
-		parent_col_index(0),
-		transposed(false),
-		parent(NULL)
-	{ }
-	Block (const Block& other) :
-		parent_nrows(other.parent_nrows),
-		parent_ncols(other.parent_ncols),
-		parent_row_index(other.parent_row_index),
-		parent_col_index(other.parent_col_index),
-		transposed(other.transposed),
-		parent(other.parent)
-	{ }
-
-	Block (
-			val_type *parent_data,
-			unsigned int parent_row_start,
-			unsigned int parent_col_start,
-			unsigned int parent_num_rows,
-			unsigned int parent_num_cols)
-	{
-		parent = parent_data;
-		parent_row_index = parent_row_start;
-		parent_col_index = parent_col_start;
-
-		parent_nrows = parent_num_rows;
-		parent_ncols = parent_num_cols;
-
-		transposed = false;
-	}
-
-	/** This operater is only used to copy the data from other into this
-	 *
-	 */
-	Block& operator=(const Block& other) {
-		if (this != &other) {
-			// copy the data, but we have to ensure, that the sizes match!
-
-			unsigned int i, j;
-			for (i = 0; i < block_rows; i++) {
-				for (j = 0; j < block_cols; j++) {
-					this->operator()(i,j) = other(i,j);
-				}
-			}
-		}
-
-		// copy data depending on other.transposed!
-
-		return *this;
-	}
-
-	/** This operater is only used to copy the data from other into this
-	 */
-	Block& operator=(const Matrix<val_type>& data_in) {
-		assert (parent != NULL);
-		// copy the data, but we have to ensure, that the sizes match!
-		assert (block_rows == data_in.rows());
-		assert (block_cols == data_in.cols());
-
-		if (!transposed) {
-			for (unsigned int i = 0; i < block_rows; i++) {
-				for (unsigned int j = 0; j < block_cols; j++) {
-					parent[parent_nrows * (i + parent_row_index) + j + parent_col_index] = data_in(i,j);
-				}
-			}
-		} else {
-			for (unsigned int i = 0; i < block_rows; i++) {
-				for (unsigned int j = 0; j < block_cols; j++) {
-					parent[parent_nrows * (j + parent_row_index) + i + parent_col_index] = data_in(i,j);
-				}
-			}
-		}
-
-		return *this;
-	}
-
-	Block transpose() {
-		assert (parent != NULL);
-		Block result (*this);
-		result.transposed = transposed ^ true;
-		return result;
-	}
-
-	const val_type& operator() (const unsigned int i, const unsigned int j) const {
-		assert (parent != NULL);
-		assert (i < block_rows);
-		assert (j < block_cols);
-
-		if (!transposed)
-			return parent[parent_nrows * (i + parent_row_index) + j + parent_col_index];
-	
-		return parent[parent_nrows * (j + parent_row_index) + i + parent_col_index];
-	}
-
-	val_type& operator() (const unsigned int i, const unsigned int j) {
-		assert (parent != NULL);
-		assert (i < block_rows);
-		assert (j < block_cols);
-
-		if (!transposed)
-			return parent[parent_nrows * (i + parent_row_index) + j + parent_col_index];
-	
-		return parent[parent_nrows * (j + parent_row_index) + i + parent_col_index];
-	}
-
-	// casting operators
-	operator Matrix<val_type>() {
-		if (!transposed) {
-			Matrix<val_type> result (block_rows, block_cols);
-
-			for (unsigned int i = 0; i < block_rows; i++) 
-				for (unsigned int j = 0; j < block_cols; j++)
-					result(i,j) = parent[parent_nrows * (i + parent_row_index) + j + parent_col_index];
-
-			return result;
-		} 
-
-		Matrix<val_type> result (block_cols, block_rows);
-
-		for (unsigned int i = 0; i < block_rows; i++) 
-			for (unsigned int j = 0; j < block_cols; j++)
-				result(j,i) = parent[parent_nrows * (i + parent_row_index) + j + parent_col_index];
-
-		return result;
-	}
-
-	unsigned int parent_nrows;
-	unsigned int parent_ncols;
-	unsigned int parent_row_index;
-	unsigned int parent_col_index;
-	bool transposed;
-
-	val_type *parent;
-};
-
 /** \brief Class for both matrices and vectors.
  */
 template <typename val_type>
 			mapped_data (false) {
 				mData = new val_type[rows * cols];
 			}
-
 		Matrix(unsigned int rows, unsigned int cols, val_type *data_ptr) :
 			nrows (rows),
 			ncols (cols),
 			return *this;
 		}
 
+		// 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));
+					}
+				}
+			}
+
+		Matrix (const Block<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;
 		}
 
 		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];
 		};
 			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);
+			}
 
-		// Block accessing functions
-		template <unsigned int blockrows, unsigned int blockcols>
-		Block<val_type, blockrows, blockcols> block (unsigned int i, unsigned int j) const {
-			assert (nrows >= blockrows);
-			assert (ncols >= blockcols);
-			return Block<val_type, blockrows, blockcols> (const_cast<val_type*> (this->mData), i, j, nrows, ncols);
-		}
+		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);
+			}
 
 		// Operators with scalars
 		void operator*=(const val_type &scalar) {
 			return mData[0];
 		}
 
+//		const HouseholderQR<matrix_type> colPivHouseholderQR() const {
+//			return HouseholderQR<matrix_type>(*this);
+//		}
+
 	private:
 		unsigned int nrows;
 		unsigned int ncols;
 		val_type* mData;
 };
 
-template <typename val_type, unsigned int blockrows, unsigned int blockcols>
-inline std::ostream& operator<<(std::ostream& output, const Block<val_type, blockrows, blockcols> &block) {
-	unsigned int i,j;
-	for (i = 0; i < blockrows; i++) {
-		output << "[ ";
-		for (j = 0; j < blockcols; j++) {
-			output << block(i,j);
-
-			if (j < blockcols - 1)
-				output << ", ";
-		}
-		output << " ]";
-
-		if (blockrows > 1 && i < blockrows - 1) 
-			output << std::endl;
-	}
-
-	return output;
-}
-
 template <typename val_type>
 inline Matrix<val_type> operator*(val_type scalar, const Matrix<val_type> &matrix) {
 	Matrix<val_type> result (matrix);
 	return result;
 }
 
-template <typename val_type>
-inline Matrix<val_type> operator*(const Matrix<val_type> &matrix, val_type scalar) {
+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] *= scalar;
+		result.data()[i] *= static_cast<val_type>(scalar);
 
 	return result;
 }

include/rbdl/SimpleMath/SimpleMathFixed.h

 
 #include "compileassert.h"
 
+#include "SimpleMathBlock.h"
+
 /** \brief Namespace for a highly inefficient math library
  *
  */
 template <typename val_type> class Matrix;
 }
 
+template <typename matrix_type>
+class HouseholderQR;
+
+template <typename matrix_type>
+class ColPivHouseholderQR;
+
 /** \brief Namespace for fixed size elements
  */
 namespace Fixed {
 template <typename val_type, unsigned int nrows, unsigned int ncols>
 class Matrix;
 
-/** \brief Block class that can be used to access blocks of a matrix
- *
- * This class is a proxy class and only contains data on where to find the
- * desired information.
- *
- */
-template <typename val_type, unsigned int block_rows, unsigned int block_cols>
-class Block {
-	public:
-	Block () :
-		parent_nrows(0),
-		parent_ncols(0),
-		parent_row_index(0),
-		parent_col_index(0),
-		transposed(false),
-		parent(NULL)
-	{ }
-	Block (const Block& other) :
-		parent_nrows(other.parent_nrows),
-		parent_ncols(other.parent_ncols),
-		parent_row_index(other.parent_row_index),
-		parent_col_index(other.parent_col_index),
-		transposed(other.transposed),
-		parent(other.parent)
-	{ }
-
-	Block (
-			val_type *parent_data,
-			unsigned int parent_row_start,
-			unsigned int parent_col_start,
-			unsigned int parent_num_rows,
-			unsigned int parent_num_cols)
-	{
-		parent = parent_data;
-		parent_row_index = parent_row_start;
-		parent_col_index = parent_col_start;
-
-		parent_nrows = parent_num_rows;
-		parent_ncols = parent_num_cols;
-
-		transposed = false;
-	}
-
-	/** This operater is only used to copy the data from other into this
-	 *
-	 */
-	Block& operator=(const Block& other) {
-		if (this != &other) {
-			// copy the data, but we have to ensure, that the sizes match!
-
-			unsigned int i, j;
-			for (i = 0; i < block_rows; i++) {
-				for (j = 0; j < block_cols; j++) {
-					this->operator()(i,j) = other(i,j);
-				}
-			}
-		}
-
-		// copy data depending on other.transposed!
-
-		return *this;
-	}
-
-	/** This operater is only used to copy the data from other into this
-	 */
-	template <unsigned int other_rows, unsigned int other_cols>
-	Block& operator=(const Matrix<val_type, other_rows, other_cols>& data_in) {
-		assert (parent != NULL);
-		// copy the data, but we have to ensure, that the sizes match!
-		COMPILE_ASSERT (block_rows == other_rows);
-		COMPILE_ASSERT (block_cols == other_cols);
-
-		if (!transposed) {
-			for (unsigned int i = 0; i < block_rows; i++) {
-				for (unsigned int j = 0; j < block_cols; j++) {
-					parent[parent_nrows * (i + parent_row_index) + j + parent_col_index] = data_in(i,j);
-				}
-			}
-		} else {
-			for (unsigned int i = 0; i < block_rows; i++) {
-				for (unsigned int j = 0; j < block_cols; j++) {
-					parent[parent_nrows * (j + parent_row_index) + i + parent_col_index] = data_in(i,j);
-				}
-			}
-		}
-
-		return *this;
-	}
-
-	Block transpose() {
-		assert (parent != NULL);
-		Block result (*this);
-		result.transposed = transposed ^ true;
-		return result;
-	}
-
-	const val_type& operator() (const unsigned int i, const unsigned int j) const {
-		assert (parent != NULL);
-		assert (i < block_rows);
-		assert (j < block_cols);
-
-		if (!transposed)
-			return parent[parent_nrows * (i + parent_row_index) + j + parent_col_index];
-	
-		return parent[parent_nrows * (j + parent_row_index) + i + parent_col_index];
-	}
-
-	val_type& operator() (const unsigned int i, const unsigned int j) {
-		assert (parent != NULL);
-		assert (i < block_rows);
-		assert (j < block_cols);
-
-		if (!transposed)
-			return parent[parent_nrows * (i + parent_row_index) + j + parent_col_index];
-	
-		return parent[parent_nrows * (j + parent_row_index) + i + parent_col_index];
-	}
-
-	// casting operators
-	template <unsigned int other_rows, unsigned int other_cols>
-	operator Matrix<val_type, other_rows, other_cols>() {
-
-		if (!transposed) {
-			assert (block_rows == other_rows);
-			assert (block_cols == other_cols);
-
-			Matrix<val_type, other_rows, other_cols> result;
-			for (unsigned int i = 0; i < block_rows; i++) 
-				for (unsigned int j = 0; j < block_cols; j++)
-					result(i,j) = parent[parent_nrows * (i + parent_row_index) + j + parent_col_index];
-
-			return result;
-		} 
-
-		assert (block_rows == other_cols);
-		assert (block_cols == other_rows);
-
-		Matrix<val_type, other_rows, other_cols> result;
-		for (unsigned int i = 0; i < block_rows; i++) 
-			for (unsigned int j = 0; j < block_cols; j++)
-				result(j,i) = parent[parent_nrows * (i + parent_row_index) + j + parent_col_index];
-
-		return result;
-	}
-
-	unsigned int parent_nrows;
-	unsigned int parent_ncols;
-	unsigned int parent_row_index;
-	unsigned int parent_col_index;
-	bool transposed;
-
-	val_type *parent;
-};
-
 /** \brief Fixed size matrix class
  */
 template <typename val_type, unsigned int nrows, unsigned int ncols>
 			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;
+		}
+
 		// conversion Dynamic->Fixed
 		Matrix(const Dynamic::Matrix<val_type> &dynamic_matrix);
 		Matrix& operator=(const Dynamic::Matrix<val_type> &dynamic_matrix);
 
-		~Matrix() {};
+	 	~Matrix() {};
 
 		Matrix (
 				const val_type &v00, const val_type &v01, const val_type &v02
 			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);
+			}
 
-		// Block accessing functions
-		template <unsigned int blockrows, unsigned int blockcols>
-		Block<val_type, blockrows, blockcols> block (unsigned int i, unsigned int j) const {
-			COMPILE_ASSERT (nrows >= blockrows);
-			COMPILE_ASSERT (ncols >= blockcols);
-			return Block<val_type, blockrows, blockcols> (const_cast<val_type*> (this->mData), i, j, nrows, ncols);
-		}
+		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);
+			}
 
 		// Operators with scalars
 		void operator*=(const val_type &scalar) {
 			return *this * -1.;
 		}
 
+		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 blockrows, unsigned int blockcols>
-inline std::ostream& operator<<(std::ostream& output, const Block<val_type, blockrows, blockcols> &block) {
-	unsigned int i,j;
-	for (i = 0; i < blockrows; i++) {
-		output << "[ ";
-		for (j = 0; j < blockcols; j++) {
-			output << block(i,j);
-
-			if (j < blockcols - 1)
-				output << ", ";
-		}
-		output << " ]";
-		
-		if (blockrows > 1 && i < blockrows - 1)
-			output << std::endl;
-	}
-
-	return output;
-}
-
 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);
 	return result;
 }
 
-template <typename val_type, unsigned int nrows, unsigned int ncols>
-inline Matrix<val_type, nrows, ncols> operator*(const Matrix<val_type, nrows, ncols> &matrix, val_type scalar) {
+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] *= scalar;
+		result.data()[i] *= static_cast<val_type> (scalar);
 
 	return result;
 }

include/rbdl/SimpleMath/SimpleMathMixed.h

 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: dimension mismatch!" << std::endl;
+		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();
 	}
 	
 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: dimension mismatch!" << std::endl;
+		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();
 	}
 	

include/rbdl/SimpleMath/SimpleMathQR.h

+#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;	
+
+		private:
+			HouseholderQR() {}
+
+			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()),
+				mR(matrix) {
+					compute();
+			}
+			HouseholderQR compute() {
+				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> matrixQ () 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:
+			ColPivHouseholderQR() {}
+
+			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(const matrix_type &matrix) :
+				mIsFactorized(false),
+				mQ(matrix.rows(), matrix.rows()),
+				mR(matrix),
+				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();
+			}
+			~ColPivHouseholderQR() {
+				delete[] mPermutations;
+			}
+
+			ColPivHouseholderQR& setThreshold (const value_type& threshold) {
+				mThreshold = threshold;
+				
+				return *this;
+			}
+			ColPivHouseholderQR& compute() {
+				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
+					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; 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(i, i, block_rows, 1);
+						mR.block(i,i,block_rows,1) = mR.block(i, col_index_norm_max, block_rows, 1);
+						mR.block(i, col_index_norm_max, block_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() * 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> matrixQ () 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