Rewrite matrix filling to use our own SparseMatrix class