/// <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(Trevor.CONFLICT_AVOIDANCE))
{
// Accumulating the conflicts count from parent to child
currentNode.UpdateConflictCounts(
((IReadOnlyDictionary <TimedMove, List <int> >)instance.parameters[Trevor.CONFLICT_AVOIDANCE]));
// 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;
}
if (instance.parameters.ContainsKey(CBS_LocalConflicts.INTERNAL_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.UpdateConflictCounts(
((IReadOnlyDictionary <TimedMove, List <int> >)instance.parameters[CBS_LocalConflicts.INTERNAL_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
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.h = inClosedList.h;
improvedHOfThisNode = true;
}
if (inClosedList.h < currentNode.h)
{
inClosedList.h = currentNode.h;
improvedHOfOldNode = true;
}
int compareVal = currentNode.CompareTo(inClosedList);
if (compareVal == -1) // This node has smaller f, or preferred due to other consideration.
// Since we equalised their h, a smaller f means smaller g.
{
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 closed 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
{
this.openList.Add(inClosedList);
++noReopenHUpdates;
}
}
}
if (this.closedList.ContainsKey(currentNode) == false)
{
this.closedList.Add(currentNode, currentNode);
this.generated++; // Reopned nodes are also recounted here.
this.openList.Add(currentNode);
currentNode.expandedCountWhenGenerated = this.expanded;
return(true);
}
// 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);
}
/// <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);
}
