Moved matrix functions to math.h