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;
                    }
                }
            }
        }
Esempio n. 8
0
 public static int EstimateDistance(AStartNode lhs, AStartNode rhs)
 {
     return
         (Math.Abs(lhs.X - rhs.X) +
          Math.Abs(lhs.Y - rhs.Y));
 }