/// <summary>
/// Performs a step in the solution for the next open node.
/// </summary>
/// <param name="nextNode">Next open node to solve.</param>
private void ProcessNextSearchStep(Node <T> nextNode)
{
_traceManager.WriteLine("ProcessNextSearchStep", "path");
List <Node <T> > successors;
T pos = nextNode.Position;
if (!nextNode.IsRootNode)
{
T parentPoint = nextNode.Parent.Position;
successors = _searchGrid.GetSuccessorsWithDirectionMinusParent(pos, parentPoint);
}
else
{
successors = _searchGrid.GetSuccessorsWithDirection(pos);
}
_traceManager.WriteLine("Successors of " + nextNode, "path");
_traceManager.DisplayContentsOfNodeList(successors, "path");
Node <T> openNode = null;
Node <T> closedNode = null;
foreach (Node <T> successor in successors)
{
bool foundOpen = false;
bool foundClosed = false;
// in this case, the "cost" is added to g
// but I think I will keep cost even across all squares
float newg = nextNode.G + _searchGrid.GetPathingWeight(successor.Position);
// check to see if this node exists already on the open list
if (_sortedOpenList.ContainsKey(successor.Position))
{
foundOpen = true;
openNode = _sortedOpenList[successor.Position];
}
if (foundOpen)
{
if (openNode.G <= newg)
{
// ignore this successor because another solution is better/at least as good
continue;
}
}
if (_closedList.ContainsKey(successor.Position))
{
foundClosed = true;
closedNode = _closedList[successor.Position];
}
if (foundClosed)
{
if (closedNode.G <= newg)
{
// ignore this successor because another solution is better/at least as good
continue;
}
}
successor.Parent = nextNode;
successor.G = newg;
successor.H = GoalDistanceEstimate(_goalNode.Position, successor.Position);
successor.F = successor.G + successor.H;
_traceManager.WriteLine("Node added to open list - " + successor.ToString(), "path");
if (foundClosed)
{
_closedList.Remove(closedNode.Position);
}
if (foundOpen)
{
_openList.Remove(openNode);
_sortedOpenList.Remove(openNode.Position);
}
PushHeap(successor);
}
// now that all the successors have been deal with add nextNode onto closed list
_closedList.Add(nextNode.Position, nextNode);
}