Add magnitude function to vector3 class