Improved use of pathfinder code

At least I think it's improved.