Pathfinding: Fix bug of map not generating if difference between z-coordinates is zero. Code cleanup.