Moved matrix and vector functions into matrix.h and matrix.c