Fix bug in pathfinder causing endless loop in some situations