Clean up, speed up

Small changes were made to speed up some parts of the code having to do
with matrices growing in size.