More linear algebra crorrections
Add matrix transform function
Hacking at refactoring to matrices