// -- 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 }
// -- Compare one node to another based on either f or g public int CompareTo(Node otherNode, char c) { if (c == 'f') return Convert.ToInt32(this.f - otherNode.f); if (c == 'g') return Convert.ToInt32(this.g - otherNode.g); else return -1; }
public Node[,] GetMap(string stringMap) { string[] lines = stringMap.Split('\n'); Node[,] mapList = new Node[lines[0].Length, lines.Length]; for (int y = 0; y < lines.Length; y++) { char[] squares = lines[y].ToCharArray(); for (int x = 0; x < squares.Length; x++) { int cost = moveMentCosts[squares[x]]; // Gets the cost to move to current current node mapList[x, y] = new Node(x, y, cost); if (squares[x] == '#') // Set to non-walkable mapList[x, y].SetWalkable(false); else if (squares[x] == 'A') // Set to start node start = mapList[x, y]; else if (squares[x] == 'B') // set to goal node goal = mapList[x, y]; } } return mapList; }
// -- 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 }
public void SetParent(Node parent) { this.parent = parent; }
// -- 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; }
// -- Return child nodes of input node (the nodes north, south, east, and wes on the map) private static List<Node> GenerateChildren(Node currentNode) { List<Node> children = new List<Node>(); int x = currentNode.GetX(); int y = currentNode.GetY(); if (NodeAtCoordinates(x - 1, y) != null) children.Add(NodeAtCoordinates(x - 1, y)); // West if (NodeAtCoordinates(x + 1, y) != null) children.Add(NodeAtCoordinates(x + 1, y)); // East if (NodeAtCoordinates(x, y - 1) != null) children.Add(NodeAtCoordinates(x, y - 1)); // North if (NodeAtCoordinates(x, y + 1) != null) children.Add(NodeAtCoordinates(x, y + 1)); // South return children; }
// -- Construct a list of the the nodes that are a part of the path form start to goal private static List<Node> ConstructPath(Node current) { List<Node> path = new List<Node>(); Node temp = current; while (temp.GetParent() != null) // As long as the current node has a parent, { path.Add(temp); // add it to the path list temp = temp.GetParent(); // and set temp to this node } return path; }
// -- Get manhatten distance from node a to node b public static int GetManhattenDistanceBetweenNodes(Node current, Node goal) { int x = current.GetX() - goal.GetX(); int y = current.GetY() - goal.GetY(); return (Math.Abs(x) + Math.Abs(y)); }
// -- 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 }