/// <summary>
/// Used when WorldStateWithOD objects are put in the open list priority queue.
/// All other things being equal, prefers nodes where more agents have moved.
/// G is already preferred, but this helps when the last move was a WAIT at the
/// goal, which doesn't increment G.
/// </summary>
/// <param name="other"></param>
/// <returns></returns>
public override int CompareTo(IBinaryHeapItem other)
{
int res = base.CompareTo(other);
if (res != 0)
{
return(res);
}
var that = (WorldStateWithOD)other;
// Further tie-breaking
// Prefer more fully generated nodes:
// For the same F, they're probably closer to the goal.
// The goal isn't necessarily a fully expanded node.
// A*+OD may finish when all agents reached their goal even if it isn't a fully expanded state, and that's a nice feature!
// So we prefer more fully generated nodes just because it gives a more DFS-like behavior
// on the heuristic's fast path to the goal.
if (this.agentTurn == 0 && that.agentTurn != 0)
{
return(-1);
}
if (that.agentTurn == 0 && this.agentTurn != 0)
{
return(1);
}
return(that.agentTurn.CompareTo(this.agentTurn)); // Notice the order inversion - bigger is better.
}
/// <summary>
/// Uses Equality check only for removing from the queue.
/// Might cost O(n) if all items are in the queue and not the heap.
/// </summary>
/// <param name="item"></param>
/// <returns></returns>
public bool Remove
(
IBinaryHeapItem item
)
{
if (item.GetIndexInHeap() == BinaryHeap.REMOVED_FROM_HEAP) // Or from queue.
{
return(false);
}
bool removedFromQueue = false;
// Remove from the queue if it's there, keeping the order in the queue.
for (int i = 0; i < this.queue.Count; ++i)
{
IBinaryHeapItem temp = this.queue.Dequeue();
if (temp.Equals(item))
{
removedFromQueue = true;
temp.SetIndexInHeap(BinaryHeap.REMOVED_FROM_HEAP);
}
else
{
this.queue.Enqueue(temp);
}
}
if (removedFromQueue == true)
{
return(true);
}
return(this.heap.Remove(item));
}
public void Add(IBinaryHeapItem item)
{
if (this.queue.Count == 0)
{
if (this.heap.Count == 0)
{
this.heap.Add(item); // It's very cheap.
}
else
{
int compareRes = item.CompareTo(this.heap.Peek());
if (compareRes != -1) // Even if equal, respect the stable order, don't "cut the line".
{
this.heap.Add(item);
}
else
{
this.queue.Enqueue(item);
this.quickInsertionCount++;
}
}
}
else
{
int compareRes = item.CompareTo(this.queue.Peek());
if (compareRes == 1) // item is larger than the queue
{
this.heap.Add(item);
}
else //
{
if (compareRes == -1) // Item is smaller than the queue
{
while (this.queue.Count != 0)
{
IBinaryHeapItem fromQueue = this.queue.Dequeue();
this.heap.Add(fromQueue);
this.quickInsertionCount--;
this.quickInsertionsCancelled++;
}
}
this.queue.Enqueue(item);
this.quickInsertionCount++;
}
}
//// The last removed item is the parent of all items added until another item is removed,
//// or the same node that was last removed, partially expanded or deferred with increased cost.
//// Otherwise the inserted item is one that was already in the open list, and its cost was
//// was increased by one of the children of the last removed item. In this case, since the item
//// wasn't the min of the open list and the last removed item was, now, with its increased cost,
//// it certainly won't be smaller than the last removed item.
//// If a partially expanded or otherwise deferred node is re-inserted with an updated cost,
//// that must be done after all its children generated so far are inserted. Otherwise the
//// cost comparison with the last removed item, which would still be the same node, would have
//// incorrect results.
}
public IBinaryHeapItem Get
(
IBinaryHeapItem item
)
{
if (openNodes.ContainsKey(item))
{
return(openNodes[item]);
}
return(null);
}
/// <summary>
/// Assumes item was added to the open list in the past
/// </summary>
/// <param name="item"></param>
/// <returns></returns>
public bool Contains
(
IBinaryHeapItem item
)
{
if (openNodes.ContainsKey(item))
{
return(true);
}
return(false);
}
public override int CompareTo(IBinaryHeapItem other)
{
this.h += this.targetDeltaF;
WorldStateForPartialExpansion otherNode = (WorldStateForPartialExpansion)other;
otherNode.h += otherNode.targetDeltaF;
int res = base.CompareTo(other);
this.h -= this.targetDeltaF;
otherNode.h -= otherNode.targetDeltaF;
return(res);
}
/// <summary>
/// Adds a key and value to the heap.
/// </summary>
/// <param name="item">The item to add to the heap.</param>
public void Add(IBinaryHeapItem item)
{
if (item == null)
{
return;
}
if (_count == _capacity)
{
Capacity *= 2; // Automatically grows the array!
}
item.SetIndexInHeap(_count);
_data[_count] = item;
_count++;
UpHeap();
}
public int CompareTo(IBinaryHeapItem item)
{
AgentToCheckForCardinalConflicts other = (AgentToCheckForCardinalConflicts)item;
if (this.groupSize < other.groupSize)
{
return(-1);
}
else if (this.groupSize > other.groupSize)
{
return(1);
}
if (this.degree < other.degree)
{
return(-1);
}
else if (this.degree > other.degree)
{
return(1);
}
if (this.planCost < other.planCost)
{
return(-1);
}
else if (this.planCost > other.planCost)
{
return(1);
}
if (this.index < other.index)
{
return(-1);
}
else if (this.index > other.index)
{
return(1);
}
else
{
return(0);
}
}
private void DownHeap()
//helper function that performs down-heap bubbling - bubbles the root down to its correct place
// This is down if you imagine the heap as a down-growing tree:
// 0
// 1 2
// 3 4 5 6
{
_sorted = false;
int n;
int p = 0;
IBinaryHeapItem item = _data[p];
while (true)
{
int ch1 = Child1(p);
if (ch1 >= _count)
{
break;
}
int ch2 = Child2(p);
if (ch2 >= _count)
{
n = ch1;
}
else
{
n = _data[ch1].CompareTo(_data[ch2]) < 0 ? ch1 : ch2;
}
if (item.CompareTo(_data[n]) > 0)
{
_data[p] = _data[n]; // Swap child up
_data[p].SetIndexInHeap(p);
p = n;
}
else
{
break;
}
}
_data[p] = item; // Finally, place item at the base of the bubble-down chain
_data[p].SetIndexInHeap(p);
}
/// <summary>
/// Removes and returns the first item in the heap.
/// </summary>
/// <returns>The next item in the heap.</returns>
public IBinaryHeapItem Remove()
{
if (this._count == 0)
{
throw new InvalidOperationException("Cannot remove item, heap is empty.");
}
IBinaryHeapItem v = _data[0];
v.SetIndexInHeap(REMOVED_FROM_HEAP);
_count--;
if (this._count != 0)
{
_data[0] = _data[_count];
DownHeap();
}
_data[_count] = default(IBinaryHeapItem); // Clear the last node
return(v);
}
private void UpHeap()
//helper function that performs up-heap bubbling - bubbles the last item up to its correct place
// This is up if you imagine the heap as a down-growing tree:
// 0
// 1 2
// 3 4 5 6
{
_sorted = false;
int p = _count - 1;
IBinaryHeapItem item = _data[p];
int par = Parent(p);
while (par > -1 && item.CompareTo(_data[par]) < 0)
{
_data[p] = _data[par]; // Swap parent down
_data[p].SetIndexInHeap(p);
p = par;
par = Parent(p);
}
_data[p] = item; // Finally, place item at the base of the bubble-up chain
_data[p].SetIndexInHeap(p);
}
/// <summary>
/// Removes an item from the binary heap.
/// Assumes item is or was in the heap. Doesn't use Equality checks.
/// This will not remove duplicates.
/// </summary>
/// <param name="item">The item to be removed.</param>
/// <returns>Boolean true if the item was removed.</returns>
public bool Remove(IBinaryHeapItem item)
{
if (item == null)
{
return(false);
}
int child_index = item.GetIndexInHeap();
if (child_index == REMOVED_FROM_HEAP)
{
return(false);
}
_data[child_index].SetIndexInHeap(REMOVED_FROM_HEAP);
if (child_index == 0) // This seems unnecessary
{
Remove();
return(true);
}
IBinaryHeapItem to_remove = _data[child_index];
// Bubble to_remove up the heap
// If UpHeap received an index parameter instead of always starting from the last element,
// we could maybe remove some code duplication.
int father_index = Parent(child_index);
while (child_index != 0)
{
_data[child_index] = _data[father_index]; // Swap parent down
_data[child_index].SetIndexInHeap(child_index);
child_index = father_index;
father_index = Parent(child_index);
}
//we got to 0
_data[0] = to_remove;
Remove(); // Ignoring the returned value.
return(true);
}
/// <summary>
/// Gets an enumerator for the binary heap.
/// </summary>
/// <returns>An IEnumerator of type T.</returns>
//public GetEnumerator()
//{
// EnsureSort();
// for (int i = 0; i < _count; i++)
// {
// yield return _data[i];
// }
//}
//System.Collections.IEnumerator System.Collections.IEnumerable.GetEnumerator()
//{
// return GetEnumerator();
//}
/// <summary>
/// Checks to see if the binary heap contains the specified item.
/// Uses CompareTo, not the item's binary heap index.
/// First call runs in O(nlogn) time. Next calls are O(logn).
/// </summary>
/// <param name="item">The item to search the binary heap for.</param>
/// <returns>A boolean, true if binary heap contains item.</returns>
public bool Contains(IBinaryHeapItem item)
{
EnsureSort();
return(Array.BinarySearch <IBinaryHeapItem>(_data, 0, _count, item) >= 0);
}
/// <summary>
/// Assumes item was added to the open list in the past
/// </summary>
/// <param name="item"></param>
/// <returns></returns>
public bool Contains(IBinaryHeapItem item)
{
return(item.GetIndexInHeap() != BinaryHeap.REMOVED_FROM_HEAP);
}
