More reformatting and a bit of efficiency.

Tried to clean up the naive matrix implementation to make it a bit
faster. Previously it would end up hanging onto 2-3x the amount of
memory actually needed. Still a work in progress though.