private void OnNodeUpdated(AStartNode node)
{
// Bubble the updated node up or down as appropriate
int parentIndex = node.QueueIndex >> 1;
if (parentIndex > 0 && HasHigherPriority(node, _nodes[parentIndex]))
{
CascadeUp(node);
}
else
{
//Note that CascadeDown will be called if parentNode == node (that is, node is the root)
CascadeDown(node);
}
}
public bool Contains(AStartNode node)
{
#if DEBUG
if (node == null)
{
throw new ArgumentNullException(nameof(node));
}
if (node.QueueIndex < 0 || node.QueueIndex >= _nodes.Length)
{
throw new InvalidOperationException("node.QueueIndex has been corrupted. Did you change it manually? Or add this node to another queue?");
}
#endif
return(_nodes[node.QueueIndex] == node);
}
public void UpdateNodePriority(AStartNode node, int newGCost)
{
#if DEBUG
if (node == null)
{
throw new ArgumentNullException(nameof(node));
}
if (!Contains(node))
{
throw new InvalidOperationException("Cannot call UpdatePriority() on a node which is not enqueued: " + node);
}
#endif
node.CostFromStart = newGCost;
OnNodeUpdated(node);
}
public bool TryDequeue(out AStartNode node)
{
#if DEBUG
if (_numNodes <= 0)
{
throw new InvalidOperationException("Cannot call Dequeue() on an empty queue");
}
if (!IsValidQueue())
{
throw new InvalidOperationException("Queue has been corrupted (Did you update a node priority manually instead of calling UpdatePriority()?" +
"Or add the same node to two different queues?)");
}
#endif
if (_numNodes == 0)
{
node = null;
return(false);
}
var returnMe = _nodes[1];
//If the node is already the last node, we can remove it immediately
if (_numNodes == 1)
{
_nodes[1] = null;
_numNodes = 0;
node = returnMe;
return(true);
}
//Swap the node with the last node
var formerLastNode = _nodes[_numNodes];
_nodes[1] = formerLastNode;
formerLastNode.QueueIndex = 1;
_nodes[_numNodes] = null;
_numNodes--;
//Now bubble formerLastNode (which is no longer the last node) down
CascadeDown(formerLastNode);
node = returnMe;
return(true);
}
private void CascadeUp(AStartNode node)
{
//aka Heapify-up
int parent;
if (node.QueueIndex > 1)
{
parent = node.QueueIndex >> 1;
var parentNode = _nodes[parent];
if (HasHigherOrEqualPriority(parentNode, node))
{
return;
}
//Node has lower priority value, so move parent down the heap to make room
_nodes[node.QueueIndex] = parentNode;
parentNode.QueueIndex = node.QueueIndex;
node.QueueIndex = parent;
}
else
{
return;
}
while (parent > 1)
{
parent >>= 1;
var parentNode = _nodes[parent];
if (HasHigherOrEqualPriority(parentNode, node))
{
break;
}
//Node has lower priority value, so move parent down the heap to make room
_nodes[node.QueueIndex] = parentNode;
parentNode.QueueIndex = node.QueueIndex;
node.QueueIndex = parent;
}
_nodes[node.QueueIndex] = node;
}
private bool HasHigherOrEqualPriority(AStartNode higher, AStartNode lower)
{
return(higher.Priority <= lower.Priority);
}
private void CascadeDown(AStartNode node)
{
//aka Heapify-down
int finalQueueIndex = node.QueueIndex;
int childLeftIndex = 2 * finalQueueIndex;
// If leaf node, we're done
if (childLeftIndex > _numNodes)
{
return;
}
// Check if the left-child is higher-priority than the current node
int childRightIndex = childLeftIndex + 1;
var childLeft = _nodes[childLeftIndex];
if (HasHigherPriority(childLeft, node))
{
// Check if there is a right child. If not, swap and finish.
if (childRightIndex > _numNodes)
{
node.QueueIndex = childLeftIndex;
childLeft.QueueIndex = finalQueueIndex;
_nodes[finalQueueIndex] = childLeft;
_nodes[childLeftIndex] = node;
return;
}
// Check if the left-child is higher-priority than the right-child
var childRight = _nodes[childRightIndex];
if (HasHigherPriority(childLeft, childRight))
{
// left is highest, move it up and continue
childLeft.QueueIndex = finalQueueIndex;
_nodes[finalQueueIndex] = childLeft;
finalQueueIndex = childLeftIndex;
}
else
{
// right is even higher, move it up and continue
childRight.QueueIndex = finalQueueIndex;
_nodes[finalQueueIndex] = childRight;
finalQueueIndex = childRightIndex;
}
}
// Not swapping with left-child, does right-child exist?
else if (childRightIndex > _numNodes)
{
return;
}
else
{
// Check if the right-child is higher-priority than the current node
var childRight = _nodes[childRightIndex];
if (HasHigherPriority(childRight, node))
{
childRight.QueueIndex = finalQueueIndex;
_nodes[finalQueueIndex] = childRight;
finalQueueIndex = childRightIndex;
}
// Neither child is higher-priority than current, so finish and stop.
else
{
return;
}
}
while (true)
{
childLeftIndex = 2 * finalQueueIndex;
// If leaf node, we're done
if (childLeftIndex > _numNodes)
{
node.QueueIndex = finalQueueIndex;
_nodes[finalQueueIndex] = node;
break;
}
// Check if the left-child is higher-priority than the current node
childRightIndex = childLeftIndex + 1;
childLeft = _nodes[childLeftIndex];
if (HasHigherPriority(childLeft, node))
{
// Check if there is a right child. If not, swap and finish.
if (childRightIndex > _numNodes)
{
node.QueueIndex = childLeftIndex;
childLeft.QueueIndex = finalQueueIndex;
_nodes[finalQueueIndex] = childLeft;
_nodes[childLeftIndex] = node;
break;
}
// Check if the left-child is higher-priority than the right-child
var childRight = _nodes[childRightIndex];
if (HasHigherPriority(childLeft, childRight))
{
// left is highest, move it up and continue
childLeft.QueueIndex = finalQueueIndex;
_nodes[finalQueueIndex] = childLeft;
finalQueueIndex = childLeftIndex;
}
else
{
// right is even higher, move it up and continue
childRight.QueueIndex = finalQueueIndex;
_nodes[finalQueueIndex] = childRight;
finalQueueIndex = childRightIndex;
}
}
// Not swapping with left-child, does right-child exist?
else if (childRightIndex > _numNodes)
{
node.QueueIndex = finalQueueIndex;
_nodes[finalQueueIndex] = node;
break;
}
else
{
// Check if the right-child is higher-priority than the current node
var childRight = _nodes[childRightIndex];
if (HasHigherPriority(childRight, node))
{
childRight.QueueIndex = finalQueueIndex;
_nodes[finalQueueIndex] = childRight;
finalQueueIndex = childRightIndex;
}
// Neither child is higher-priority than current, so finish and stop.
else
{
node.QueueIndex = finalQueueIndex;
_nodes[finalQueueIndex] = node;
break;
}
}
}
}
public static int EstimateDistance(AStartNode lhs, AStartNode rhs)
{
return
(Math.Abs(lhs.X - rhs.X) +
Math.Abs(lhs.Y - rhs.Y));
}