Returns a list of points that make up a continuous path
between the start
and goal
nodes in this.graph
.
Every point on the returned path was a walkable Node
of this.graph
.
Source
List<Node> pathFind(Node start, Node goal) {
Set<Node> Unvisited = new Set<Node>();
Map<Node, Node> Came_From = new Map<Node, Node>();
for (Node node in this.graph.allNodes) {
node._f = double.INFINITY;
Unvisited.add(node);
}
Node current = start;
current._f = 0.0;
while (Unvisited.isNotEmpty) {
for (Node neighbor in this.graph.getNeighbors(current, onlyWalkable: true)) {
if (!Unvisited.contains(neighbor)) {
continue;
}
double tentative_distance =
current._f + this.graph.heuristic(current, neighbor);
if (tentative_distance < neighbor._f) {
neighbor._f = tentative_distance;
Came_From[neighbor] = current;
}
}
Unvisited.remove(current);
// Goal node was visited
if (!Unvisited.contains(goal)) {
return _reconstruct_path(Came_From, current);
}
Node nodeWithSmallestDistance = Unvisited.first;
for (Node node in Unvisited) {
if (node._f < nodeWithSmallestDistance._f) {
nodeWithSmallestDistance = node;
}
}
// No connection between start and goal nodes exists.
if (nodeWithSmallestDistance._f == double.INFINITY) {
return [];
}
current = nodeWithSmallestDistance;
}
return [];
}