More pathfinding bugfixes and refinements