Add magnitude function to vector3. Something strange with Junit passing when it shouldn't. TODO: Fix.