public void AStar(Vector3 start, Vector3 end, OnPathComputed callback)
{
// Reset all scores and sets membership
Reset();
Node startNode = getClosestNode(start);
Node endNode = getClosestNode(end);
if (startNode == null || endNode == null) {
callback(new Path());
return;
}
PriorityQueue<Node> openSet = new PriorityQueue<Node>();
openSet.Push(startNode); startNode.InOpenSet = true;
Dictionary<Node, Node> parentPath = new Dictionary<Node, Node>();
startNode.GScore = 0;
startNode.FScore = startNode.GScore + heuristic(startNode, endNode);
Path path = new Path();
Node current = startNode;
while (openSet.Count() > 0) {
current = openSet.Pop(); // automatically do the remove part
current.InOpenSet = false;
if (current.Id == endNode.Id) {
path.Add(current.Position);
while (parentPath.ContainsKey(current)) {
current = parentPath[current];
path.Add(current.Position);
}
path.Reverse();
callback(path);
return;
}
current.InClosedSet = true;
List<Node> neighbors = getNeighbors(current);
for (int i = 0; i < neighbors.Count; ++i) {
Node neighbor = neighbors[i];
if (neighbor.InClosedSet) continue;
float gAttempt = current.GScore + euclidian(current, neighbor);
if (!neighbor.InOpenSet || gAttempt <= neighbor.GScore) {
parentPath[neighbor] = current;
neighbor.GScore = gAttempt;
neighbor.FScore = neighbor.GScore + heuristic(neighbor, endNode);
if (!neighbor.InOpenSet) {
openSet.Push(neighbor);
neighbor.InOpenSet = true;
}
}
} // end loop neighbors
}
}