Update matrices.cpp

Added a few methods, improved consistency and some bug fixes.