Pathfinding Fix

Fixed a bug in pathfinding