Implemented waypointing, streamlined pushing code

Pushing now uses proper trig to determine angle instead of 8-direction
conditionals. Pushing is also based on waypoints instead of teleporting.