Fix pathfinding bug