Added utility functions (util.hpp).

Added default tolerance value for the matrix equal function to make it
similar in usage to the one defined for vectors.
Update CMakeLists.txt

added util stuff
Merge pull request #45 from steiner-/master

small last minute additions