void reinsertIntoOpenList(WorldState node) { //if (this.openList.Contains(node)) // Node is partially expanded in the open list. Need to restart its expansion //{ // this.openList.Remove(node); // node.Clear(); //} this.openList.Remove(node); node.Clear(); this.openList.Add(node); // Re-insert into open list }
/// <summary> /// Returns whether the node was inserted into the open list. /// </summary> /// <param name="currentNode"></param> /// <returns></returns> protected virtual bool ProcessGeneratedNode(WorldState currentNode) { if (currentNode.h + currentNode.g <= this.maxCost) // Assuming h is an admissable heuristic, no need to generate nodes that won't get us to the goal // within the budget { if (instance.parameters.ContainsKey("ID-ConflictAvoidance")) { // Accumulating the conflicts count from parent to child // We're counting conflicts along the entire path, so the parent's conflicts count is added to the child's: currentNode.cbsInternalConflicts = new Dictionary <int, int>(currentNode.prevStep.cbsInternalConflicts); currentNode.conflictTimes = new Dictionary <int, List <int> >(); foreach (var kvp in currentNode.prevStep.conflictTimes) { currentNode.conflictTimes[kvp.Key] = new List <int>(kvp.Value); } currentNode.conflictTimesBias = new Dictionary <int, List <int> >(); foreach (var kvp in currentNode.prevStep.conflictTimesBias) { currentNode.conflictTimesBias[kvp.Key] = new List <int>(kvp.Value); } currentNode.conflictProbability = new Dictionary <int, List <double> >(); foreach (var kvp in currentNode.prevStep.conflictProbability) { currentNode.conflictProbability[kvp.Key] = new List <double>(kvp.Value); } currentNode.UpdateConflictCounts( ((IReadOnlyDictionary <TimedMove, List <int> >)instance.parameters["ID-ConflictAvoidance"])); // We're counting conflicts along the entire path, so the parent's conflicts count // is added to the child's. currentNode.potentialConflictsCount = currentNode.cbsInternalConflicts.Count; // FIXME: The above code duplication with the CBS CAT. Some of the vars above are actually from CBS now. } if (instance.parameters.ContainsKey(CBS.CAT)) { // Accumulating the conflicts count from parent to child. // We're counting conflicts along the entire path, so the parent's conflicts count is added to the child's: currentNode.cbsInternalConflicts = new Dictionary <int, int>(currentNode.prevStep.cbsInternalConflicts); currentNode.conflictTimes = new Dictionary <int, List <int> >(); foreach (var kvp in currentNode.prevStep.conflictTimes) { currentNode.conflictTimes[kvp.Key] = new List <int>(kvp.Value); } currentNode.conflictTimesBias = new Dictionary <int, List <int> >(); foreach (var kvp in currentNode.prevStep.conflictTimesBias) { currentNode.conflictTimesBias[kvp.Key] = new List <int>(kvp.Value); } currentNode.conflictProbability = new Dictionary <int, List <double> >(); foreach (var kvp in currentNode.prevStep.conflictProbability) { currentNode.conflictProbability[kvp.Key] = new List <double>(kvp.Value); } currentNode.UpdateConflictCounts( ((IReadOnlyDictionary <TimedMove, List <int> >)instance.parameters[CBS.CAT])); // Count one for every agent the path conflicts with any number of times: currentNode.cbsInternalConflictsCount = currentNode.cbsInternalConflicts.Count; } // If in closed list - only reopen if F is lower or node is otherwise preferred if (this.closedList.ContainsKey(currentNode) == true) { ++this.closedListHits; WorldState inClosedList = this.closedList[currentNode]; // Notice the agents may have gotten to their location from a different direction in this node. // Since the nodes are equal, give them both the max of their H bool improvedHOfThisNode = false; bool improvedHOfOldNode = false; if (currentNode.h < inClosedList.h) { currentNode.hBonus += inClosedList.h - currentNode.h; currentNode.h = inClosedList.h; improvedHOfThisNode = true; } if (inClosedList.h < currentNode.h) { inClosedList.hBonus += currentNode.h - inClosedList.h; inClosedList.h = currentNode.h; improvedHOfOldNode = true; } byte hackData = 0; int compareVal = currentNode.CompareTo(inClosedList); if (compareVal == -1) // Enables re-trying a node with different paths for the agents { this.reopened++; this.closedList.Remove(inClosedList); this.openList.Remove(inClosedList); // Items are searched for in the heap using their binaryHeapIndex, which is only initialized when they're put into it, // and not their hash or their Equals or CompareTo methods, so it's important to call Remove with inClosedList, // which might be in the heap, and not currentNode, which may be Equal to it, but was never in the heap so it // doesn't have a binaryHeapIndex initialized. if (improvedHOfThisNode) { ++reopenedWithOldH; } } else if (improvedHOfOldNode) { // Reinsert old node with new higher F, if it's still in the open list. // This pushes it further back in the open list so it certainly won't be smaller than the currently expanded node, so monotonicity is maintained. if (this.openList.Remove(inClosedList)) // Cheap if it isn't there { inClosedList.Clear(); this.openList.Add(inClosedList); ++noReopenHUpdates; } } } if (this.closedList.ContainsKey(currentNode) == false) { this.closedList.Add(currentNode, currentNode); this.generated++; // Reopened nodes are also recounted here. this.openList.Add(currentNode); currentNode.expandedCountWhenGenerated = this.expanded; if (this.debug) { Debug.Print("Generated node {0}", currentNode); } return(true); } else { if (this.debug) { Debug.Print("NOT generating node {0}. It already exists.", currentNode); } } // What if in open list? This implementation immediately puts _generated_ nodes in the closed list, // so it only needs to check it and not the open list. // That actually makes a lot of sense: membership tests in heaps are expensive, and in hashtables are cheap. // This way we only need to _search_ the open list if we encounter a node that was already visited. } return(false); }