Added supprt for diagonal pathing.
Implemented a simple algorithm.