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.