// -- Print the map of nodes, with start (A) and goal (B)
public static void PrintMap(Node[,] map, Node start, Node goal)
{
string s = "";
for (int y = 0; y < map.GetLength(1); y++)
{
for (int x = 0; x < map.GetLength(0); x++)
{
if (!map[x, y].IsWalkable()) // If the current node (x,y) is not wakable (a wall/obstacle)
s += "#";
else if (start.GetX() == x && start.GetY() == y) // If current node is start, print 'A'
s += "A";
else if (goal.GetX() == x && goal.GetY() == y) // If current node is goal, print 'A'
s += "B";
else if (map[x, y].GetCost() == 100) // If current node is water, cost = 100, print 'w'
s += "w";
else if (map[x, y].GetCost() == 50) // If current node is moutain, cost = 50, print 'm'
s += "m";
else if (map[x, y].GetCost() == 10) // If current node is water, cost = 10, print 'f'
s += "f";
else if (map[x, y].GetCost() == 5) // If current node is water, cost = 5, print 'g'
s += "g";
else if (map[x, y].GetCost() == 1) // If current node is water, cost = 1, print 'r'
s += "r";
else
s += ".";
}
s += "\n";
}
Console.WriteLine("Map:\n" + s); // Print the map
}
// -- Print the map with the nodes in the open list (*) and in the closed list(x)
public static void PrintMapWithOpenAndClosed(Node[,] map, Node start, Node goal, Node current, List<Node> open, List<Node> closed)
{
string s = "";
for (int y = 0; y < map.GetLength(1); y++)
{
for (int x = 0; x < map.GetLength(0); x++)
{
if (map[x, y] == current) // If current node is the position we are in, print 'o'
s += "o";
else if (start.GetX() == x && start.GetY() == y) // If current node is start, print 'A'
s += "A";
else if (goal.GetX() == x && goal.GetY() == y) // If current node is goal, print 'B'
s += "B";
else if (open.Contains(map[x, y])) // If current node is in the open list, print '*'
s += "*";
else if (closed.Contains(map[x, y])) // If current node is in the open list, print 'x'
s += "x";
else if (!map[x, y].IsWalkable()) // If the current node (x,y) is not wakable (a wall/obstacle)
s += "#";
else if (map[x, y].GetCost() == 100) // If current node is water, cost = 100, print 'w'
s += "w";
else if (map[x, y].GetCost() == 50) // If current node is moutain, cost = 50, print 'm'
s += "m";
else if (map[x, y].GetCost() == 10) // If current node is water, cost = 10, print 'f'
s += "f";
else if (map[x, y].GetCost() == 5) // If current node is water, cost = 5, print 'g'
s += "g";
else if (map[x, y].GetCost() == 1) // If current node is water, cost = 1, print 'r'
s += "r";
else
s += ".";
}
s += "\n";
}
Console.WriteLine("Map with open('*') and closed('x'):\n" + s); // Print the map
}
// // -- Print the map with the path from A to B, pluss which nodes are in the open list (*) and in the closed list(x)
public static void PrintMapWithOpenAndClosedWithPath(Node[,] map, List<Node> path, Node start, Node goal, Node current, List<Node> open, List<Node> closed)
{
string s = "";
for (int y = 0; y < map.GetLength(1); y++)
{
for (int x = 0; x < map.GetLength(0); x++)
{
bool isInResult = false;
foreach (Node node in path) // Check if the current node (x,y) is a part of the path
{
if (node.GetX() == x && node.GetY() == y)
isInResult = true;
}
if (isInResult) // If the current node (x,y) is a part of the path, represent it as 'O'
s += "O";
else if (start.GetX() == x && start.GetY() == y) // If current node is start, print 'A'
s += "A";
else if (goal.GetX() == x && goal.GetY() == y) // If current node is goal, print 'B'
s += "B";
else if (open.Contains(map[x, y])) // If current node is in the open list, print '*'
s += "*";
else if (closed.Contains(map[x, y])) // If current node is in the open list, print 'x'
s += "x";
else if (!map[x, y].IsWalkable()) // If the current node (x,y) is not wakable (a wall/obstacle)
s += "#";
else if (map[x, y].GetCost() == 100) // If current node is water, cost = 100, print 'w'
s += "w";
else if (map[x, y].GetCost() == 50) // If current node is moutain, cost = 50, print 'm'
s += "m";
else if (map[x, y].GetCost() == 10) // If current node is water, cost = 10, print 'f'
s += "f";
else if (map[x, y].GetCost() == 5) // If current node is water, cost = 5, print 'g'
s += "g";
else if (map[x, y].GetCost() == 1) // If current node is water, cost = 1, print 'r'
s += "r";
else
s += ".";
}
s += "\n";
}
Console.WriteLine("Map with open('*'), closed('X') and path('O'):\n" + s); // Print the map
}
// -- Print the map with the path from A to B
public static void PrintResultMap(Node[,] map, List<Node> result, Node start, Node goal)
{
string s = "";
for (int y = 0; y < map.GetLength(1); y++)
{
for (int x = 0; x < map.GetLength(0); x++)
{
bool isInResult = false;
foreach (Node node in result) // Check if the current node (x,y) is a part of the path
{
if (node.GetX() == x && node.GetY() == y)
isInResult = true;
}
if (isInResult) // If the current node (x,y) is a part of the path, represent it as 'O'
s += "O";
else if (!map[x, y].IsWalkable()) // If the current node (x,y) is not wakable (a wall/obstacle)
s += "#";
else if (start.GetX() == x && start.GetY() == y) // If current node is start, print 'A'
s += "A";
else if (goal.GetX() == x && goal.GetY() == y) // If current node is goal, print 'A'
s += "B";
else if (map[x, y].GetCost() == 100) // If current node is water, cost = 100, print 'w'
s += "w";
else if (map[x, y].GetCost() == 50) // If current node is moutain, cost = 50, print 'm'
s += "m";
else if (map[x, y].GetCost() == 10) // If current node is water, cost = 10, print 'f'
s += "f";
else if (map[x, y].GetCost() == 5) // If current node is water, cost = 5, print 'g'
s += "g";
else if (map[x, y].GetCost() == 1) // If current node is water, cost = 1, print 'r'
s += "r";
else
s += ".";
}
s += "\n";
}
Console.WriteLine("Map with path:\n" + s); // Print the map
}
// -- A*: returns list of nodes that are part of the shortest path from A to B using A*
public static List<Node> RunAStar(Node[,] map, Node start, Node goal)
{
allNodes = GetListFromArray(map);
List<Node> open = new List<Node>();
List<Node> closed = new List<Node>();
for (int y = 0; y < map.GetLength(1); y++)
{
for (int x = 0; x < map.GetLength(0); x++)
{
if (map[x, y].IsWalkable())
{
map[x, y].SetG(Int32.MaxValue); // Set start value of g = infinity (Int.MaxValue = 2 147 483 647)
map[x, y].UpdateF(); // Update f (f = g+h)
}
}
}
open.Add(start); // Add start to the open list
start.SetG(0); // Set g of the starting node = 0
start.SetH(GetManhattenDistanceBetweenNodes(start, goal)); // And h = manhattendistance to goal
start.UpdateF();
while (open.Count > 0)
{
// Set current node to the first element in the open list (which is ascending sorted on f)
Node currentNode = open.ElementAt(0);
open.RemoveAt(0); // Remove it from open
closed.Add(currentNode); // Add it to the closed list
if (currentNode == goal) // If goal is found, the search is complete
{
List<Node> path = ConstructPath(currentNode);
// Print the map with the path from A to B, and show which nodes are in open (*), and closed (x)
//Visualization.PrintMapWithOpenAndClosedWithPath(map, path, start, goal, currentNode, open, closed);
return path; // return the path from start to goal
}
// Genereate the children of this node (the nodes south, north, east and wes for current node
List<Node> children = GenerateChildren(currentNode);
foreach (Node child in children)
{
if (child == goal) // If goal is found, the search is complete
{
List<Node> path = ConstructPath(currentNode);
// Print the map with the path from A to B, and show which nodes are in open (*), and closed (x)
Visualization.PrintMapWithOpenAndClosedWithPath(map, path, start, goal, currentNode, open, closed);
return path; // return the path from start to goal
}
if (!child.IsWalkable()) // If child is not walkable (a obsticle or wall), skip it
continue;
double tentativeG = currentNode.GetG() + child.GetCost(); // calculate tentative g for child
if (open.Contains(child)) // If child is in the open list
{
if (child.GetG() < tentativeG) // and current g < tentativeG, skip it
continue;
}
else if (closed.Contains(child)) // If child is in closed
{
if (child.GetG() < tentativeG) // and current g < tentativeG, skip it
continue;
closed.Remove(child); // Remove child from open list
open.Add(child); // and add it to closed list
}
else // If child is in neither of the lists
{
open.Add(child); // Add it to the open list
child.SetH(GetManhattenDistanceBetweenNodes(child, goal));
child.UpdateF();
}
child.SetG(tentativeG);
child.UpdateF();
child.SetParent(currentNode); // Set parent of child to current node
}
closed.Add(currentNode); // We're done with current node, so add it to closed
open = GetListSortedOnF(open); // Sort the open list scending on f
//Visualization.PrintMapWithOpenAndClosed(map, start, goal, currentNode, open, closed);
}
return null; // No path was found
}
// -- Return a List<Node> representation of a 2D array of nodes (the map)
private static List<Node> GetListFromArray(Node[,] nodes)
{
List<Node> list = new List<Node>();
for (int x = 0; x < nodes.GetLength(0); x++)
{
for (int y = 0; y < nodes.GetLength(1); y++)
{
list.Add(nodes[x, y]);
}
}
return list;
}
// -- Dijkstra's: returns list of nodes that are part of the shortest path from A to B using Dijkstra's algorithm
public static List<Node> RunDijkstra(Node[,] map, Node start, Node goal)
{
allNodes = GetListFromArray(map);
List<Node> open = new List<Node>(); // List sorted ascending on g
List<Node> closed = new List<Node>();
for (int y = 0; y < map.GetLength(1); y++)
{
for (int x = 0; x < map.GetLength(0); x++)
{
if (map[x, y].IsWalkable())
{
map[x, y].SetG(Int32.MaxValue); // Set g value of all nodes to infinity (Int.MaxValue = 2 147 483 647)
open.Add(map[x, y]); // and add them to the open list
}
}
}
start.SetG(0); // Set g of starting node = 0
while (open.Count > 0)
{
open = GetListSortedOnG(open); // Sort list ascending on g
Node currentNode = open.ElementAt(0); // Pick the first element from open list
open.RemoveAt(0); // and remove it from the lists
closed.Add(currentNode);
// Genereate the children of this node (the nodes south, north, east and wes for current node
List<Node> children = GenerateChildren(currentNode);
foreach (Node child in children)
{
double tentativeG = currentNode.GetG() + child.GetCost(); // calculate tentative g for child
if (tentativeG < child.GetG()) // If tenative g < current g of child
{
child.SetG(tentativeG); // Set g of child = tentative g
child.SetParent(currentNode); // Set current node to parent of child
}
if (child == goal) // If gould node is founde
{
List<Node> path = ConstructPath(currentNode);
//Visualization.PrintMapWithOpenAndClosedWithPath(map, path, start, goal, currentNode, open, closed);
return path; // Return path from start to goal
}
}
//Visualization.PrintMapWithOpenAndClosed(map, start, goal, currentNode, open, closed);
}
return null; // No path was found
}
// -- BFS: returns list of nodes that are part of the shortest path from A to B using BFS
public static List<Node> RunBFS(Node[,] map, Node start, Node goal)
{
allNodes = GetListFromArray(map);
List<Node> open = new List<Node>(); // FIFO queue
List<Node> closed = new List<Node>();
for (int y = 0; y < map.GetLength(1); y++)
{
for (int x = 0; x < map.GetLength(0); x++)
{
map[x, y].SetG(Int32.MaxValue); // Set g value of all nodes to infinity (Int.MaxValue = 2 147 483 647)
}
}
start.SetG(0); // Set distance g to starting node = 0
open.Add(start);
while (open.Count > 0)
{
Node currentNode = open.ElementAt(0); // Pick the first element from open list
open.RemoveAt(0); // and remove it from the lists
closed.Add(currentNode);
// Genereate the children of this node (the nodes south, north, east and wes for current node
List<Node> children = GenerateChildren(currentNode);
foreach (Node child in children)
{
if (!child.IsWalkable()) // If child is not walkable (a obsticle or wall), skip it
continue;
if (child.GetG() > Int32.MaxValue-1) // If g value of child = infinity, in other words, not touched yet
{
// Set g (g of current node + the cost of moving from current node to child)
child.SetG(currentNode.GetG() + child.GetCost());
child.SetParent(currentNode); // Set parent to current ndoe
open.Add(child);
}
if (child == goal) // If the goal node is found
{
List<Node> path = ConstructPath(currentNode);
//Visualization.PrintMapWithOpenAndClosedWithPath(map, path, start, goal, currentNode, open, closed);
return path; // return the path from start to goal
}
}
//Visualization.PrintMapWithOpenAndClosed(map, start, goal, currentNode, open, closed);
}
return null; // No path was found
}