MathLibrary cleanup and more testing

Cleaned up the math library a bit because the test/ catch cases were
causing errors with the google test framework. Tested all of the 2D & 3D
vector functions. Started testing the matrix functions but the
constructor for matrix 3x3 is not wanting to work how it should and I
can't figure out why so i'm going to work on todays tutorial for the
rest of the day.