public List<Node> calcRoute()
{
Node start = new Node(startX, startY);
Node Goal = new Node(endX,endY);
if (start.Equals(Goal))
{
throw new Exception("Goal and Start are the same");
}
Node[,] came_from = new Node[20, 20];
int[,] g_score = InitMap();
int[,] f_score = InitMap();
g_score[start.y, start.x] = 0;
f_score[start.y, start.x] = g_score[start.y, start.x] + HeuresticFunction(start, Goal);
start.f_score = f_score[start.y, start.x];
List<Node> openset = new List<Node>();
openset.Add(start);
List<Node> closedset = new List<Node>();
while(openset.Any())
{
Node current = openset.Min();
if (current.Equals(Goal)) break;
openset.Remove(current);
closedset.Add(current);
Node[] myneihbours = neighbours(current);
foreach (Node neighbour in myneihbours)
{
int tentative_g_score = g_score[current.y, current.x] + 5;
if((!openset.Contains(neighbour) || tentative_g_score < g_score[neighbour.y,neighbour.x]) && neighbour.open)
{
came_from[neighbour.y, neighbour.x] = current;
g_score[neighbour.y, neighbour.x] = tentative_g_score;
f_score[neighbour.y, neighbour.x] = g_score[neighbour.y, neighbour.x] + HeuresticFunction(neighbour, Goal);
if (!openset.Contains(neighbour))
openset.Add(neighbour);
}
}
}
return path(came_from, Goal, start);
}
private Node[] neighbours(Node current)
{
return new Node[]{new Node(current.x - 1 , current.y), new Node(current.x + 1,current.y),
new Node(current.x, current.y - 1), new Node(current.x, current.y + 1)};
}
private int HeuresticFunction(Node from, Node to)
{
return (int)(Math.Pow((to.x - from.x), 2) + Math.Pow(to.y - from.y,2));
}
private List<Node> path(Node[,] came_from, Node goal,Node start)
{
List<Node> total_path = new List<Node>(){goal};
Node current = goal;
while (!current.Equals(start))
{
current = came_from[current.y, current.x];
total_path.Add(current);
}
return total_path;
}