Matrix.h reworked