Avoid some unnecessary re-visits in A*

Nodes can appear multiple times in the priority queue `visit_next`. To
guarantee the so-called runtime optimality (no other general algorithm
can visit fewer nodes than A* given the same information), we must check
that we re-visit a node if and only if we found a better path to it.
This commit adds a guard to ensure that we don't visit nodes for which
better paths have already been seen.

This also includes a test that counts the number of times `edge_cost` is
called throughout a run of `astar`. This serves as a proxy for the
number of nodes visited, which cannot be accessed directly.
2 files changed