small last minute additions

Added utility functions (util.hpp) + test routines.

Misc:
Added default tolerance value for the matrix equal function to make it
similar in usage to the one defined for vectors.