/// <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)
{
int totalCost = (AStarHabbo.Map.GetLength(0) * AStarHabbo.Map.GetLength(1)) ^ 2;
FStartNode = AStartNode;
FGoalNode = AGoalNode;
int i = 0;
FOpenList.Add(FStartNode);
while (FOpenList.Count > 0)
{
i++;
if (i > totalCost)
{
break;
}
else
{
// 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);
}
}
}
