Move matrix unittests to end of file, clean up too

The tests were too much in the way.
Move vector unittests to end of file