/// <summary> /// Initializes the pathfinding list and assigns all necessary variables. /// </summary> /// <param name="graph"></param> /// <param name="start"></param> /// <param name="end"></param> /// <param name="heuristic"></param> public virtual void Init(IGraph <T> graph, T start, T end, Heuristic <T> heuristic) { this.graph = graph; this.start = start; this.end = end; this.heuristic = heuristic; //Initialize the open and closed lists pathFindingList = new PathfindingList <T>(); pathFindingList.Build(graph, start, end, heuristic); this.current = pathFindingList.SmallestElement(); //pathFindingList[start] = startRecord; }
public List <Connection> pathfindDijkstra(Graph graph, Node start, Node to) { int count = 0; NodeRecord startRecord = new NodeRecord(); startRecord.node = start; startRecord.connection = null; startRecord.costSoFar = 0f; Node endNode = null; NodeRecord endNodeRecord = new NodeRecord(); float endNodeCost; List <Connection> output = new List <Connection>(); PathfindingList open = new PathfindingList(); open.nodeRecords.Add(startRecord); PathfindingList closed = new PathfindingList(); NodeRecord current = new NodeRecord(); while (open.length() > 0) { current = open.smallestElement(); Debug.Log("open.length"); Debug.Log(open.length()); Debug.Log("count = "); Debug.Log(count); Debug.Log("current = " + current); Debug.Log("current.node = " + open.smallestElement().node); count++; if (current.node == to) { Debug.Log("success"); break; } connections = graph.getConnections(current.node); foreach (Connection connection in connections) { endNode = connection.to; endNodeCost = current.costSoFar + connection.getCost(); if (closed.contains(endNode)) { Debug.Log("continue"); Debug.Log("continue"); Debug.Log("continue"); continue; } else if (open.contains(endNode)) { endNodeRecord = open.find(endNode); if (endNodeRecord != null && endNodeRecord.costSoFar < endNodeCost) { continue; } } else { endNodeRecord = new NodeRecord(); endNodeRecord.node = endNode; } endNodeRecord.costSoFar = endNodeCost; endNodeRecord.connection = connection; if (!open.contains(endNode)) { open.nodeRecords.Add(endNodeRecord); } } open.nodeRecords.Remove(current); closed.nodeRecords.Add(current); Debug.Log("open.length"); Debug.Log("open.length"); Debug.Log("open.length"); Debug.Log("open.length"); Debug.Log(open.length()); } //Debug.Log(to); //Debug.Log(current.node); if (current.node != to) { return(null); } else { while (current.node != start) { output.Add(current.connection); Node fromNode = current.connection.from; current = closed.find(current.connection.from); } } output.Reverse(); return(output); }
public List <Connection> PathfindAStar(Graph aGraph, GameObject start, GameObject end, Heuristic myHeuristic) { // Set up the start record. NodeRecord StartRecord = new NodeRecord(); StartRecord.Node = start; StartRecord.Connection = null; StartRecord.CostSoFar = 0; StartRecord.EstimatedTotalCost = myHeuristic.Estimate(start, end); // Create the lists. PathfindingList OpenList = new PathfindingList(); PathfindingList ClosedList = new PathfindingList(); // Add the start record to the open list. OpenList.AddNodeRecord(StartRecord); // Iterate through and process each node. NodeRecord CurrentRecord = null; List <Connection> Connections; while (OpenList.GetSize() > 0) { // Find the smallest element in the open list (using the estimatedTotalCost). CurrentRecord = OpenList.GetSmallestElement(); // If it is the goal node, then terminate. if (CurrentRecord.Node.Equals(end)) { break; } // Otherwise get its outgoing connections. Connections = aGraph.GetConnections(CurrentRecord.Node); // Loop through each connection in turn. GameObject EndNode; float EndNodeCost; NodeRecord EndNodeRecord; float EndNodeHeuristic; foreach (Connection aConnection in Connections) { // Get the cost estimate for the end node. EndNode = aConnection.ToNode; EndNodeCost = CurrentRecord.CostSoFar + aConnection.Cost; // If the node is closed we may have to skip, or remove it from the closed list. if (ClosedList.Contains(EndNode)) { // Here we find the record in the closed list corresponding to the endNode. EndNodeRecord = ClosedList.Find(EndNode); // If we didn’t find a shorter route, skip. if (EndNodeRecord.CostSoFar <= EndNodeCost) { continue; } // Otherwise remove it from the closed list. ClosedList.RemoveNodeRecord(EndNodeRecord); // We can use the node’s old cost values to calculate its heuristic without calling // the possibly expensive heuristic function. EndNodeHeuristic = EndNodeRecord.EstimatedTotalCost - EndNodeRecord.CostSoFar; } // Skip if the node is open and we’ve not found a better route. else if (OpenList.Contains(EndNode)) { // Here we find the record in the open list corresponding to the endNode. EndNodeRecord = OpenList.Find(EndNode); // If our route is no better, then skip. if (EndNodeRecord.CostSoFar <= EndNodeCost) { continue; } // We can use the node’s old cost values to calculate its heuristic without calling // the possibly expensive heuristic function. EndNodeHeuristic = EndNodeRecord.EstimatedTotalCost - EndNodeRecord.CostSoFar; } // Otherwise we know we’ve got an unvisited node, so make a record for it. else { EndNodeRecord = new NodeRecord(); EndNodeRecord.Node = EndNode; // We’ll need to calculate the heuristic value using the function, since we don’t have an existing record to use. EndNodeHeuristic = myHeuristic.Estimate(EndNode, end); } // We’re here if we need to update the node Update the cost, estimate and connection. EndNodeRecord.CostSoFar = EndNodeCost; EndNodeRecord.Connection = aConnection; EndNodeRecord.EstimatedTotalCost = EndNodeCost + EndNodeHeuristic; // And add it to the open list. if (!(OpenList.Contains(EndNode))) { OpenList.AddNodeRecord(EndNodeRecord); } } //#END: Looping through Connections. // We’ve finished looking at the connections for the current node, so add it to the closed list // and remove it from the open list OpenList.RemoveNodeRecord(CurrentRecord); ClosedList.AddNodeRecord(CurrentRecord); } // We’re here if we’ve either found the goal, or if we’ve no more nodes to search, find which. List <Connection> tempList = new List <Connection>(); if (!CurrentRecord.Node.Equals(end)) { // We’ve run out of nodes without finding the goal, so there’s no solution return(tempList); } else { while (!CurrentRecord.Node.Equals(start)) { tempList.Add(CurrentRecord.Connection); CurrentRecord = ClosedList.Find(CurrentRecord.Connection.FromNode); } // The path is in the wrong order. Reverse the path, and return it. List <Connection> tempList2 = new List <Connection>(); for (int i = (tempList.Count - 1); i >= 0; i--) { tempList2.Add(tempList[i]); } return(tempList2); } }
public List <Connection> PathfindAStar(Graph graph, GameObject start, GameObject end, Heuristic heuristic) { NodeRecord startRecord = new NodeRecord(); startRecord.Node = start; startRecord.Connection = null; startRecord.CostSoFar = 0; startRecord.EstimatedTotalCost = heuristic.Estimate(start, end); PathfindingList openList = new PathfindingList(); PathfindingList closedList = new PathfindingList(); openList.AddRecord(startRecord); NodeRecord currentRecord = null; List <Connection> connections; while (openList.GetSize() > 0) { currentRecord = openList.GetSmallestElement(); if (currentRecord.Node.Equals(end)) { break; } connections = graph.GetConnections(currentRecord.Node); GameObject endNode; float endNodeCost; NodeRecord endNodeRecord; float endNodeHeuristic; foreach (Connection aConnection in connections) { endNode = aConnection.ToNode; endNodeCost = currentRecord.CostSoFar + aConnection.Cost; if (closedList.Contains(endNode)) { endNodeRecord = closedList.Find(endNode); if (endNodeRecord.CostSoFar <= endNodeCost) { continue; } closedList.RemoveRecord(endNodeRecord); endNodeHeuristic = endNodeRecord.EstimatedTotalCost - endNodeRecord.CostSoFar; } else if (openList.Contains(endNode)) { endNodeRecord = openList.Find(endNode); if (endNodeRecord.CostSoFar <= endNodeCost) { continue; } endNodeHeuristic = endNodeRecord.EstimatedTotalCost - endNodeRecord.CostSoFar; } else { endNodeRecord = new NodeRecord(); endNodeRecord.Node = endNode; endNodeHeuristic = heuristic.Estimate(endNode, end); } endNodeRecord.CostSoFar = endNodeCost; endNodeRecord.Connection = aConnection; endNodeRecord.EstimatedTotalCost = endNodeCost + endNodeHeuristic; if (!openList.Contains(endNode)) { openList.AddRecord(endNodeRecord); } } openList.RemoveRecord(currentRecord); closedList.AddRecord(currentRecord); } List <Connection> tempList = new List <Connection>(); if (!currentRecord.Node.Equals(end)) { return(tempList); } else { while (!currentRecord.Node.Equals(start)) { tempList.Add(currentRecord.Connection); if (currentRecord.Connection == null) { Debug.Log("Current Record " + currentRecord.ToString() + " is null"); } if (currentRecord.Connection.FromNode == null) { Debug.Log("Current FromNode " + currentRecord.Connection.FromNode.gameObject.name + " is null"); } currentRecord = closedList.Find(currentRecord.Connection.FromNode); } // Reverse path and return List <Connection> tempList2 = new List <Connection>(); for (int i = tempList.Count - 1; i >= 0; i--) { tempList2.Add(tempList[i]); } return(tempList2); } }
public static List <Connection> pathfind(Graph graph, Node start, Node goal) { // Initialize the record for the start node. NodeRecord startRecord = new NodeRecord(); startRecord.node = start; startRecord.connection = null; startRecord.costSoFar = 0; // Initialize the open and closed lists PathfindingList open = new PathfindingList(); open.add(startRecord); PathfindingList closed = new PathfindingList(); // Iterate through processing each node NodeRecord current = new NodeRecord(); while (open.length() > 0) { // Find the smallest element in the open list current = open.smallestElement(); // If it is the goal node, then terminate if (current.node == goal) { break; } // Otherwise get its outgoing connections. List <Connection> connections = graph.getConnections(current.node); // Loop through each connection in turn. foreach (Connection connection in connections) { // Get the cost estimate for the end node Node endNode = connection.getToNode(); float endNodeCost = current.costSoFar + connection.getCost(); NodeRecord endNodeRecord = new NodeRecord(); // Skip if the node is closed if (closed.contains(endNode)) { continue; } // ... or if it is open and we've found a worse route else if (open.contains(endNode)) { // Here we find the record in the open list // corresponding to the endNode endNodeRecord = open.find(endNode); if (endNodeRecord != null && endNodeRecord.costSoFar < endNodeCost) { continue; } } // Otherwise we know we've got an unvisited node, so make a // record for it else { endNodeRecord = new NodeRecord(); endNodeRecord.node = endNode; } // We're here if we need to update the node. Update the // cost and connection. endNodeRecord.costSoFar = endNodeCost; endNodeRecord.connection = connection; // And add it to the open list if (!open.contains(endNode)) { open.add(endNodeRecord); } } // We've finished looking at the connections for the current // node, so add it to the closed list and remove it from the // open list. open.remove(current); closed.add(current); } // We're here if we've either found the goal, or if we've no more // nodes to search. Find which. if (current.node != goal) { // We've run out of nodes without finding the goal, so there's // no solution return(null); } else { // Compile the list of connections in the path. List <Connection> path = new List <Connection>(); // Work back along the path, accumulating connections while (current.node != start) { path.Add(current.connection); //current = current.connection.getFromNode(); // << This is Millington. This doesn't work because current needs to be a full NodeRecord. Node fromNode = current.connection.getFromNode(); current = closed.find(fromNode); } // Reverse the path and return it. path.Reverse(); return(path); } }
public static List <Connection> pathfind(IGraph graph, Node start, Node goal) { NodeRecord startRecord = new NodeRecord(); startRecord.node = start; startRecord.connection = null; startRecord.costSoFar = 0; PathfindingList open = new PathfindingList(); open.add(startRecord); PathfindingList closed = new PathfindingList(); NodeRecord current = new NodeRecord(); while (open.length() > 0) { current = open.smallestElement(); if (current.node == goal) { break; } List <Connection> connections = graph.getConnections(current.node); foreach (Connection connection in connections) { Node endNode = connection.getToNode(); float endNodeCost = current.costSoFar + connection.getCost(); NodeRecord endNodeRecord = new NodeRecord(); if (closed.contains(endNode)) { continue; } else if (open.contains(endNode)) { endNodeRecord = open.find(endNode); if (endNodeRecord != null && endNodeRecord.costSoFar < endNodeCost) { continue; } } else { endNodeRecord = new NodeRecord(); endNodeRecord.node = endNode; } endNodeRecord.costSoFar = endNodeCost; endNodeRecord.connection = connection; if (!open.contains(endNode)) { open.add(endNodeRecord); } } open.remove(current); closed.add(current); } if (current.node != goal) { return(null); } else { List <Connection> path = new List <Connection>(); while (current.node != start) { path.Add(current.connection); Node fromNode = current.connection.getFromNode(); current = closed.find(fromNode); } path.Reverse(); return(path); } }
public static List <Connection> pathfind(Graph graph, Node start, Node goal) { // initialize record for start node. NodeRecord startRecord = new NodeRecord(); startRecord.node = start; startRecord.connection = null; startRecord.costSoFar = 0; // initialize open and closed lists PathfindingList open = new PathfindingList(); open.add(startRecord); PathfindingList closed = new PathfindingList(); // iterate through processing each node NodeRecord current = new NodeRecord(); while (open.length() > 0) { // find smallest element in open list current = open.smallestElement(); // if its goal node, terminate if (current.node == goal) { break; } // otherwise List <Connection> connections = graph.getConnections(current.node); foreach (Connection connection in connections) { // get cost estimate for end node Node endNode = connection.getToNode(); float endNodeCost = current.costSoFar + connection.getCost(); NodeRecord endNodeRecord = new NodeRecord(); // skip if node is closed if (closed.contains(endNode)) { continue; } // or else if (open.contains(endNode)) { endNodeRecord = open.find(endNode); if (endNodeRecord != null && endNodeRecord.costSoFar < endNodeCost) { continue; } } else { endNodeRecord = new NodeRecord(); endNodeRecord.node = endNode; } endNodeRecord.costSoFar = endNodeCost; endNodeRecord.connection = connection; if (!open.contains(endNode)) { open.add(endNodeRecord); } } open.remove(current); closed.add(current); } if (current.node != goal) { return(null); } else { // compile list of connections in path List <Connection> path = new List <Connection>(); // accumulate connections while (current.node != start) { path.Add(current.connection); Node fromNode = current.connection.getFromNode(); current = closed.find(fromNode); } // reverse path and return it path.Reverse(); return(path); } }