/**
* Executes an A* search in a graph. It needs a graph, the start and end vertex as well as a
* heuristic distance measurer.
*/
public WeightedEdgeContainer<VERTEX_ID> startAStarSearch(
Graph<VERTEX_ID, VERTEX_VALUE, Integer> g,
VERTEX_ID start,
VERTEX_ID goal,
DistanceMeasurer<VERTEX_ID, VERTEX_VALUE, Integer> measurer) {
HashSet<VERTEX_ID> closedSet = new HashSet<>();
HashSet<VERTEX_ID> openSet = new HashSet<>();
HashMap<VERTEX_ID, VERTEX_ID> cameFrom = new HashMap<>();
// distance from the start from start along optimal path
HashMap<VERTEX_ID, Integer> g_score = new HashMap<>();
// distance from start to goal plus heuristic estimate
HashMap<VERTEX_ID, Double> f_score = new HashMap<>();
// heuristic score
HashMap<VERTEX_ID, Double> h_score = new HashMap<>();
g_score.put(start, 0);
h_score.put(start, measurer.measureDistance(g, start, goal));
f_score.put(start, h_score.get(start));
// create a deep copy
for (Vertex<VERTEX_ID, VERTEX_VALUE> v : g.getVertexSet()) openSet.add(v.getVertexId());
while (!openSet.isEmpty()) {
VERTEX_ID v = findLowest(openSet, f_score);
if (v.equals(goal)) {
return new WeightedEdgeContainer<>(g_score, cameFrom);
} else {
openSet.remove(v);
closedSet.add(v);
for (Edge<VERTEX_ID, Integer> y : g.getEdges(v)) {
boolean tentativeIsBetter = false;
if (closedSet.contains(y.getDestinationVertexID())) continue;
int tentativeGScore = g_score.get(v) + y.getValue();
Integer gScore = g_score.get(y);
if (!openSet.contains(y.getDestinationVertexID())) {
openSet.add(y.getDestinationVertexID());
tentativeIsBetter = true;
} else if (gScore == null || tentativeGScore < gScore) {
tentativeIsBetter = true;
} else {
tentativeIsBetter = false;
}
if (tentativeIsBetter) {
cameFrom.put(y.getDestinationVertexID(), v);
g_score.put(y.getDestinationVertexID(), tentativeGScore);
double dist = measurer.measureDistance(g, y.getDestinationVertexID(), goal);
h_score.put(y.getDestinationVertexID(), dist);
f_score.put(y.getDestinationVertexID(), tentativeGScore + dist);
}
}
}
}
return new WeightedEdgeContainer<>(g_score, cameFrom);
}
private double averageEdgeWeights(Graph<VERTEX_ID, VERTEX_VALUE, Integer> g, VERTEX_ID vertex) {
int sum = 0;
int count = 0;
Set<Edge<VERTEX_ID, Integer>> adjacents = g.getEdges(vertex);
if (adjacents == null) return 0;
for (Edge<VERTEX_ID, Integer> e : adjacents) {
sum += e.getValue();
count++;
}
if (count == 0) return 0;
else return sum / count;
}