...
while (openSet.Count > 0){
Waypoint current = openSet.Dequeue();
if (current == goal)
return ReconstructPath(cameFrom, current, start);
closedSet.Add(current);
foreach (Waypoint neighbor in current.edges){
if (closedSet.Contains(neighbor))
continue;
if (!openSet.Contains(neighbor))
openSet.Enqueue(neighbor, gScore[neighbor] +
Heuristic(neighbor, goal));
float tentative_gScore = gScore[current] + Heuristic(current,
neighbor);
if (tentative_gScore >= gScore[neighbor])
continue;
cameFrom[neighbor] = current;
gScore[neighbor] = tentative_gScore;
openSet.UpdatePriority(neighbor, gScore[neighbor] +
Heuristic(neighbor, goal));
}
}
return new List<Waypoint>();
}