Add matrix mathematics, move old matrix to array2d, fix tests