/// <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;
}
}
