public int astar(Vertex start, Vertex goal, Heuristic h) {
int count;
Collection<Vertex> vertices_;
Comparator<Vertex> comp;
PriorityQueue<Vertex> fringe;
count = 0;
vertices_ = vertices.values();
comp =
new Comparator<Vertex>() {
public int compare(Vertex i, Vertex j) {
return (i.cost + i.estimate - j.cost - j.estimate);
}
}; // use estimates
fringe = new PriorityQueue<Vertex>(20, comp);
for (Vertex v : vertices_) {
v.visited = false;
v.cost = 999999;
}
start.cost = 0;
start.parent = null;
fringe.add(start);
while (!fringe.isEmpty()) {
Vertex v = fringe.remove(); // lowest-cost
count++; // count nodes
if (v.equals(goal)) { // if the goal is found, quit
break;
}
if (!v.visited) {
v.visited = true;
for (Edge e : v.edges) {
int newcost = v.cost + e.cost;
if (newcost < e.target.cost) {
e.target.cost = newcost;
e.target.parent = v;
e.target.estimate = h.fn(e.target, goal); // use heuristic
fringe.add(e.target);
}
}
}
}
return count;
}
