/// <summary> /// Finds the shortest path from the start node to the goal node /// </summary> /// <param name="AStartNode">Start node</param> /// <param name="AGoalNode">Goal node</param> public void FindPath(AStarNode AStartNode, AStarNode AGoalNode) { FStartNode = AStartNode; FGoalNode = AGoalNode; FOpenList.Add(FStartNode); int i = 0; while (FOpenList.Count > 0 && i < 2000) { // Get the node with the lowest TotalCost AStarNode NodeCurrent = (AStarNode)FOpenList.Pop(); // If the node is the goal copy the path to the solution array if (NodeCurrent.IsGoal()) { while (NodeCurrent != null) { FSolution.Insert(0, NodeCurrent); NodeCurrent = NodeCurrent.Parent; } break; } // Get successors to the current node NodeCurrent.GetSuccessors(FSuccessors); foreach (AStarNode NodeSuccessor in FSuccessors) { // Test if the currect successor node is on the open list, if it is and // the TotalCost is higher, we will throw away the current successor. AStarNode NodeOpen = null; if (FOpenList.Contains(NodeSuccessor)) { NodeOpen = (AStarNode)FOpenList[FOpenList.IndexOf(NodeSuccessor)]; } if ((NodeOpen != null) && (NodeSuccessor.TotalCost > NodeOpen.TotalCost)) { continue; } // Test if the currect successor node is on the closed list, if it is and // the TotalCost is higher, we will throw away the current successor. AStarNode NodeClosed = null; if (FClosedList.Contains(NodeSuccessor)) { NodeClosed = (AStarNode)FClosedList[FClosedList.IndexOf(NodeSuccessor)]; } if ((NodeClosed != null) && (NodeSuccessor.TotalCost > NodeClosed.TotalCost)) { continue; } // Remove the old successor from the open list FOpenList.Remove(NodeOpen); // Remove the old successor from the closed list FClosedList.Remove(NodeClosed); // Add the current successor to the open list FOpenList.Push(NodeSuccessor); } // Add the current node to the closed list FClosedList.Add(NodeCurrent); i++; } if (i == 2000) { m_pathPossible = false; } }
/// <summary> /// Finds the shortest path from the start node to the goal node /// </summary> /// <param name="AStartNode">Start node</param> /// <param name="AGoalNode">Goal node</param> public void FindPath(AStarNode AStartNode, AStarNode AGoalNode, System.Threading.CancellationTokenSource cancel, int maxIter = int.MaxValue) { FStartNode = AStartNode; // FGoalNode = AGoalNode; int iter = 0; FOpenList.Add(FStartNode); while (FOpenList.Count > 0) { if (cancel.IsCancellationRequested || (iter++ > maxIter)) { break; } // Get the node with the lowest TotalCost AStarNode NodeCurrent = (AStarNode)FOpenList.Pop(); // If the node is the goal copy the path to the solution array if (NodeCurrent.IsGoal()) { while (NodeCurrent != null) { FSolution.Insert(0, NodeCurrent); NodeCurrent = NodeCurrent.Parent; } break; } // Get successors to the current node NodeCurrent.GetSuccessors(FSuccessors); foreach (AStarNode NodeSuccessor in FSuccessors) { // Test if the currect successor node is on the open list, if it is and // the TotalCost is higher, we will throw away the current successor. AStarNode NodeOpen = null; if (FOpenList.Contains(NodeSuccessor)) { NodeOpen = (AStarNode)FOpenList[FOpenList.IndexOf(NodeSuccessor)]; } if ((NodeOpen != null) && (NodeSuccessor.TotalCost > NodeOpen.TotalCost)) { continue; } // Test if the currect successor node is on the closed list, if it is and // the TotalCost is higher, we will throw away the current successor. AStarNode NodeClosed = null; if (FClosedList.Contains(NodeSuccessor)) { NodeClosed = (AStarNode)FClosedList[FClosedList.IndexOf(NodeSuccessor)]; } if ((NodeClosed != null) && (NodeSuccessor.TotalCost > NodeClosed.TotalCost)) { continue; } // Remove the old successor from the open list FOpenList.Remove(NodeOpen); // Remove the old successor from the closed list FClosedList.Remove(NodeClosed); // Add the current successor to the open list FOpenList.Push(NodeSuccessor); } // Add the current node to the closed list FClosedList.Add(NodeCurrent); } }