Pathfinding fixes

Small but essential changes to the pathfinding. I still have to do a lot
of checking and debugging (see if routes get terminated for the right
reason) but for now it seems to work.