public HashSet <T> AStar <T>(GraphAstar <T> graph, T start, T end, int[] h)
{
var a = new HashSet <T>();
Dictionary <T, T> cameFrom = new Dictionary <T, T>();
Dictionary <T, int> costSoFar = new Dictionary <T, int>();
var closed = new HashSet <T>();
var frontier = new PriorityQueue <T>();
frontier.Enqueue(start, 0);
cameFrom[start] = start;
costSoFar[start] = 0;
while (frontier.Count > 0)
{
var current = frontier.Dequeue();
if (current.Equals(end))
{
break;
}
foreach (var next in graph.AdjacencyList[current])
{
int newCost;
try
{
newCost = costSoFar[current]
+ graph.withCost[Tuple.Create(current, next)] + h[int.Parse(next.ToString())];
}
catch (Exception)
{
newCost = costSoFar[current]
+ graph.withCost[Tuple.Create(next, current)] + h[int.Parse(next.ToString())];
}
if (!costSoFar.ContainsKey(next) || newCost < costSoFar[next])
{
costSoFar[next] = newCost;
int priority = newCost + (Heuristic(graph.location[next], graph.location[end]) + h[int.Parse(current.ToString())]);
frontier.Enqueue(next, priority);
cameFrom[next] = current;
}
}
}
//search path reversely using memory [camefrom]
a.Add(end);
T curr = end;
while (!curr.Equals(start))
{
foreach (var item in cameFrom)
{
if (item.Key.Equals(curr))
{
a.Add(item.Value);
curr = item.Value;
}
}
}
return(a);
}
public void performAStar(int startingNode, int endingNode)
{
showHeuristic(endingNode);
vertices = new int[vertex.Count];
var graph = new GraphAstar <int>();
for (int i = 0; i < vertex.Count; i++)
{
vertices[i] = i + 1;
graph.setLocation(vertices[i], new Point(vertex[i + 1].X, vertex[i + 1].Y));
}
edge = new List <Tuple <int, int> >();
for (int i = 0; i < edgeCount; i++)
{
edge.Add(Tuple.Create(start[i], end[i]));
graph.setIndividualDistance(Tuple.Create(start[i], end[i]), Distance(new Point(vertex[start[i]].X, vertex[start[i]].Y), new Point(vertex[end[i]].X, vertex[end[i]].Y)));
}
string t = "";
foreach (var a in graph.withCost)
{
t += a.Key + "::" + a.Value;
}
graph.init(vertices, edge);
var algorithms = new GraphAlgorithms();
var nodes = algorithms.AStar(graph, startingNode, endingNode, heuristics).Reverse();
List <int> result = new List <int>();
foreach (var item in nodes)
{
result.Add(item);
}
NodesResult = result;
retrace(result, endingNode);
}