Fix safety tp check above world height
Performance tweak to fly check