Pathfinding bugfixes