public AStarSearch(WeightedGraph<Position> graph, Position start, Position goal)
{
var frontier = new SimplePriorityQueue<Position>();
frontier.Enqueue(start, 0);
cameFrom[start] = start;
costSoFar[start] = 0;
while (frontier.Count > 0)
{
var current = frontier.Dequeue();
if(current.Equals(goal))
{
break;
}
foreach (var next in graph.Neighbors(current))
{
int newCost = costSoFar[current] + graph.Cost(current, next);
if (!costSoFar.ContainsKey(next) || newCost < costSoFar[next])
{
costSoFar[next] = newCost;
int priority = newCost + Heuristic(next, goal);
frontier.Enqueue(next, priority);
cameFrom[next] = current;
}
}
}
}
public AStarSearch(WeightedGraph <Location> graph, Location start, Location goal)
{
var frontier = new PriorityQueue <Location>();
frontier.Enqueue(start, 0);
cameFrom[start] = start;
costSoFar[start] = 0;
while (frontier.Count > 0)
{
var current = frontier.Dequeue();
if (current.Equals(goal))
{
break;
}
foreach (var next in graph.Neighbors(current))
{
double newCost = costSoFar[current]
+ graph.Cost(current, next);
if (!costSoFar.ContainsKey(next) ||
newCost < costSoFar[next])
{
costSoFar[next] = newCost;
double priority = newCost + Heuristic(next, goal);
frontier.Enqueue(next, priority);
cameFrom[next] = current;
}
}
}
}
public AStarSearch(WeightedGraph <Vector3Int> graph, Vector3Int start, Vector3Int destination)
{
var frontier = new PriorityQueue <Vector3Int>();
frontier.Enqueue(start, 0);
CameFrom[start] = start;
CostSoFar[start] = 0;
while (frontier.Count > 0)
{
var current = frontier.Dequeue();
if (current.Equals(destination))
{
Path.Add(current);
var rear = current;
while (CameFrom.ContainsKey(rear) && CameFrom[rear] != rear)
{
Path.Add(CameFrom[rear]);
rear = CameFrom[rear];
}
Path.Reverse();
break;
}
foreach (var next in graph.Neighbors(current))
{
var newCost = CostSoFar[current] + graph.Cost(current, next);
if (!CostSoFar.ContainsKey(next) || newCost < CostSoFar[next])
{
CostSoFar[next] = newCost;
var priority = newCost + Heuristic(next, destination);
frontier.Enqueue(next, priority);
CameFrom[next] = current;
}
}
}
}
