private static void IdentifySuccessors(IMapData Map, NavigationNode Node, INode Goal, NavigationNodeStack OpenedNodes, NavigationNodeStack ClosedNodes)
{
INode[] neighbors = GetNeighbors(Map, Node);
for (int i = 0; i < neighbors.Length; ++i)
{
INode neighborNode = neighbors[i];
INode jumpNode = Jump(Map, neighborNode, Node.Node, Goal, OpenedNodes);
if (jumpNode == null || IsNavigationNodeInList(jumpNode, ClosedNodes))
{
continue;
}
NavigationNode jumpNavNode = GetNavigationNode(jumpNode, OpenedNodes);
bool alreadyIsInOpenNodes = (jumpNavNode != null);
if (jumpNavNode == null)
{
jumpNavNode = GetOrAddNavigationNode(jumpNode, OpenedNodes);
}
Number heuristic = NavigationNode.CalculateHeuristicCost(Node.Node, jumpNode);
Number nextExactCost = Node.ExactCost + heuristic;
if (!alreadyIsInOpenNodes || nextExactCost < jumpNavNode.ExactCost)
{
jumpNavNode.SetExactCost(nextExactCost);
jumpNavNode.SetHeuristicCost(jumpNavNode.HeuristicCost != 0 ? jumpNavNode.HeuristicCost : NavigationNode.CalculateHeuristicCost(jumpNode, Goal));
jumpNavNode.SetParent(Node);
}
}
}
