public bool Contains(Cell node) => _nodes[node.QueueIndex] == node;
private bool HasHigherOrEqualPriority(Cell higher, Cell lower) => higher.F <= lower.F;
private void CascadeDown(Cell node)
{
//aka Heapify-down
var finalQueueIndex = node.QueueIndex;
var childLeftIndex = 2 * finalQueueIndex;
// If leaf node, we're done
if (childLeftIndex > Count)
{
return;
}
// Check if the left-child is higher-priority than the current node
var 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 > Count)
{
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 > Count)
{
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 > Count)
{
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 > Count)
{
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 > Count)
{
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;
}
}
}
}
private bool HasHigherPriority(Cell higher, Cell lower) => higher.F < lower.F;
public void UpdatePriority(Cell node, double priority)
{
node.F = priority;
OnNodeUpdated(node);
}