Fixed Pathfinding bug

pathfinding now works in all cases.