Rewrite matrix filling to use our own SparseMatrix struct

Actually, this turned out to be much more code than I expected initially, so I doubt this should really be merged. Anyway, it's here for your consideration.