Adding diagonal option for pathfinder