private Couple <int> AStarGraphSearch()
{
Console.WriteLine("A* Graph Search:");
SortedList <AStarGraphNode> openedNodes = new SortedList <AStarGraphNode>()
{
new AStarGraphNode(distanceMethod, 0, startPosition)
};
SortedList <AStarGraphNode> closedNodes = new SortedList <AStarGraphNode>();
int minSteps = -1;
int examinedNodes = 0;
while (openedNodes.Count > 0)
{
examinedNodes++;
AStarGraphNode currentNode = openedNodes[0];
openedNodes.RemoveAt(0);
closedNodes.Add(currentNode);
if (IsGoal(currentNode))
{
minSteps = currentNode.DistanceFromRoot;
break;
}
foreach (AStarGraphNode node in GetSuccessors(currentNode))
{
Pair <bool, AStarGraphNode> resultsClosed = ListContains(closedNodes, node);
Pair <bool, AStarGraphNode> resultsOpened = ListContains(openedNodes, node);
AStarGraphNode foundedNode = (resultsClosed.First) ? resultsClosed.Second : resultsOpened.Second;
if ((resultsOpened.First || resultsClosed.First) && node.DistanceFromRoot < foundedNode.DistanceFromRoot)
{
foundedNode.UpdateDistance(node.DistanceFromRoot);
}
else if (!(resultsOpened.First || resultsClosed.First))
{
openedNodes.Add(node);
}
}
}
return(new Couple <int>(minSteps, examinedNodes));
}
private Pair <bool, AStarGraphNode> ListContains(SortedList <AStarGraphNode> list, AStarGraphNode node)
{
Pair <bool, AStarGraphNode> results = new Pair <bool, AStarGraphNode>(false, null);
foreach (AStarGraphNode currentNode in list)
{
if (currentNode.Coorditates == node.Coorditates)
{
results = new Pair <bool, AStarGraphNode>(true, currentNode);
break;
}
}
return(results);
}