public void UpdateUnvisitedNeighbors(Node node, Node finishNode, List <List <Node> > grid) { SimplePriorityQueue <Node> unvisitedNeighbors = GetUnvisitedNeighbors(node, grid); foreach (Node neighbor in unvisitedNeighbors) { neighbor.GFunction = node.GFunction + 1 + neighbor.Weight; neighbor.PreviousNode = node; neighbor.HFunction = CalculateHFunction(neighbor, finishNode); int fFunction = neighbor.GFunction + neighbor.HFunction; if (OpenSet.Contains(neighbor)) { if (fFunction < OpenSet.GetPriority(neighbor)) { OpenSet.UpdatePriority(neighbor, fFunction); } } else { OpenSet.Enqueue(neighbor, fFunction); } } }