CmPriorityQueue.h

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of NVIDIA CORPORATION nor the names of its
//    contributors may be used to endorse or promote products derived
//    from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// Copyright (c) 2008-2022 NVIDIA Corporation. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
 
#ifndef CM_PRIORITY_QUEUE_H
#define CM_PRIORITY_QUEUE_H
 
#include "foundation/PxBasicTemplates.h"
#include "foundation/PxAllocator.h"
#include "foundation/PxMemory.h"
 
namespace physx
{
namespace Cm
{
    template<class Element, class Comparator = PxLess<Element> >
    class PriorityQueueBase : protected Comparator // inherit so that stateless comparators take no space
    {
    public:
        PriorityQueueBase(const Comparator& less, Element* elements) : Comparator(less), mHeapSize(0), mDataPtr(elements)
        {
        }
        
        ~PriorityQueueBase() 
        {
        }
        
        PX_FORCE_INLINE const Element top() const
        {
            return mDataPtr[0];
        }
 
        PX_FORCE_INLINE Element top()
        {
            return mDataPtr[0];
        }
        
        PX_FORCE_INLINE bool empty() const
        {
            return (mHeapSize == 0);
        }
        
        PX_FORCE_INLINE void clear()
        {
            mHeapSize = 0;
        }  
 
        PX_FORCE_INLINE void push(const Element& value)
        {
            PxU32 newIndex;
            PxU32 parentIndex = parent(mHeapSize);
 
            for (newIndex = mHeapSize; newIndex > 0 && compare(value, mDataPtr[parentIndex]); newIndex = parentIndex, parentIndex= parent(newIndex)) 
            {
                mDataPtr[ newIndex ] = mDataPtr[parentIndex];
            }
            mDataPtr[newIndex] = value; 
            mHeapSize++;
            PX_ASSERT(valid());
        }
 
        PX_FORCE_INLINE Element pop()
        {
            PX_ASSERT(mHeapSize > 0);
            PxU32 i, child;
            //try to avoid LHS
            PxU32 tempHs = mHeapSize-1;
            mHeapSize = tempHs;
            Element min = mDataPtr[0];
            Element last = mDataPtr[tempHs];
            
            for (i = 0; (child = left(i)) < tempHs; i = child) 
            {
                /* Find highest priority child */
                const PxU32 rightChild = child + 1;
            
                child += ((rightChild < tempHs) & compare((mDataPtr[rightChild]), (mDataPtr[child]))) ? 1 : 0;
 
                if(compare(last, mDataPtr[child]))
                    break;
 
                mDataPtr[i] = mDataPtr[child];
            }
            mDataPtr[ i ] = last;
            
            PX_ASSERT(valid());
            return min;
        } 
 
        bool valid() const
        {
            const Element& min = mDataPtr[0];
            for(PxU32 i=1; i<mHeapSize; ++i)
            {
                if(compare(mDataPtr[i], min))
                    return false;
            }
 
            return true;
        }
 
        PxU32 size() const
        {
            return mHeapSize;
        }
 
    protected:
 
        PxU32 mHeapSize;
        Element* mDataPtr;
        
        PX_FORCE_INLINE bool compare(const Element& a, const Element& b) const
        {
            return Comparator::operator()(a,b);
        }
 
        static PX_FORCE_INLINE PxU32 left(PxU32 nodeIndex) 
        {
            return (nodeIndex << 1) + 1;
        }
        
        static PX_FORCE_INLINE PxU32 parent(PxU32 nodeIndex) 
        {
            return (nodeIndex - 1) >> 1;
        }
    private:
        PriorityQueueBase<Element, Comparator>& operator = (const PriorityQueueBase<Element, Comparator>);
    };
 
    template <typename Element, PxU32 Capacity, typename Comparator>
    class InlinePriorityQueue : public PriorityQueueBase<Element, Comparator>
    {
        Element mData[Capacity];
    public:
        InlinePriorityQueue(const Comparator& less = Comparator()) : PriorityQueueBase<Element, Comparator>(less, mData)
        {
        }
 
        PX_FORCE_INLINE void push(Element& elem)
        {
            PX_ASSERT(this->mHeapSize < Capacity);
            PriorityQueueBase<Element, Comparator>::push(elem);
        }
    private:
        InlinePriorityQueue<Element, Capacity, Comparator>& operator = (const InlinePriorityQueue<Element, Capacity, Comparator>);
    };
 
    template <typename Element, typename Comparator, typename Alloc = typename physx::PxAllocatorTraits<Element>::Type>
    class PriorityQueue : public PriorityQueueBase<Element, Comparator>, protected Alloc
    {
        PxU32 mCapacity;
    public:
        PriorityQueue(const Comparator& less = Comparator(), PxU32 initialCapacity = 0, Alloc alloc = Alloc()) 
            : PriorityQueueBase<Element, Comparator>(less, NULL), Alloc(alloc), mCapacity(initialCapacity)
        {
            if(initialCapacity > 0)
                this->mDataPtr = reinterpret_cast<Element*>(Alloc::allocate(sizeof(Element)*initialCapacity, __FILE__, __LINE__));
        }
 
        ~PriorityQueue()
        {
            if(this->mDataPtr)
                this->deallocate(this->mDataPtr);
        }
 
        PX_FORCE_INLINE void push(Element& elem)
        {
            if(this->mHeapSize == mCapacity)
            {
                reserve((this->mHeapSize+1)*2);
            }
            PriorityQueueBase<Element, Comparator>::push(elem);
        }
 
        PX_FORCE_INLINE PxU32 capacity()
        {
            return mCapacity;
        }
 
        PX_FORCE_INLINE void reserve(const PxU32 newCapacity)
        {
            if(newCapacity > mCapacity)
            {
                Element* newElems = reinterpret_cast<Element*>(Alloc::allocate(sizeof(Element)*newCapacity, __FILE__, __LINE__));
                if(this->mDataPtr)
                {
                    physx::PxMemCopy(newElems, this->mDataPtr, sizeof(Element) * this->mHeapSize);
                    Alloc::deallocate(this->mDataPtr);
                }
                this->mDataPtr = newElems;
                mCapacity = newCapacity;
            }
        }
 
    private:
        PriorityQueue<Element, Comparator, Alloc>& operator = (const PriorityQueue<Element, Comparator, Alloc>);
    };
    
}
}
 
#endif