Rearranged Math Classes

Added Matrix4f class
Removed some debug code