[Pathfinding] More work done. Questionable completeness.