/// <summary> /// Finds the shortest path from the start node to the goal node /// </summary> /// <param name="startNode">Start node</param> /// <param name="goalNode">Goal node</param> public void FindPath(AStarNode startNode, AStarNode goalNode) { m_StartNode = startNode; m_GoalNode = goalNode; int loopBreak = 0; m_OpenList.Add(m_StartNode); while (m_OpenList.Count > 0 && loopBreak < LOOPBREAKER) { // Get the node with the lowest TotalCost AStarNode NodeCurrent = (AStarNode)m_OpenList.Pop(); // If the node is the goal copy the path to the solution array if (NodeCurrent.IsGoal()) { while (NodeCurrent != null) { m_Solution.Insert(0, NodeCurrent); NodeCurrent = NodeCurrent.parent; } break; } // Get successors to the current node NodeCurrent.GetSuccessors(m_Successors); foreach (AStarNode NodeSuccessor in m_Successors) { // 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 (m_OpenList.Contains(NodeSuccessor)) { NodeOpen = (AStarNode)m_OpenList[m_OpenList.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 (m_ClosedList.Contains(NodeSuccessor)) { NodeClosed = (AStarNode)m_ClosedList[m_ClosedList.IndexOf(NodeSuccessor)]; } if ((NodeClosed != null) && (NodeSuccessor.totalCost > NodeClosed.totalCost)) { continue; } // Remove the old successor from the open list m_OpenList.Remove(NodeOpen); // Remove the old successor from the closed list m_ClosedList.Remove(NodeClosed); // Add the current successor to the open list m_OpenList.Push(NodeSuccessor); } // Add the current node to the closed list m_ClosedList.Add(NodeCurrent); loopBreak += 1; } }