Updated math. atan2 instead of own custom math