Priority Queue

NativePriorityQueue.cs

using System;
using System.Collections;
using System.Collections.Generic;
using System.Diagnostics;
using Unity.Collections;
using Unity.Collections.LowLevel.Unsafe;
using Unity.Mathematics;
using UnityEngine;

[NativeContainer]
//[NativeContainerSupportsMinMaxWriteRestriction]
[DebuggerDisplay("Length = {" + nameof(Length) + "}")]
[DebuggerTypeProxy(typeof(NativePriorityQueueDebugView<>))]
public struct NativePriorityQueue<T> : IDisposable
    where T : struct, IComparable
{
    private NativeList<ItemNode<T>> list;
    internal int m_Length;

    internal int CurrentHighIndex;
    internal int CurrentLowIndex;

#if ENABLE_UNITY_COLLECTIONS_CHECKS
    //internal int m_MinIndex;
    //internal int m_MaxIndex;
    internal AtomicSafetyHandle m_Safety;
    [NativeSetClassTypeToNullOnSchedule]
    internal DisposeSentinel m_DisposeSentinel;
#endif

    public NativePriorityQueue(Allocator allocator) : this(200, allocator)
    { }

    public NativePriorityQueue(int cap, Allocator allocator)
    {
        list = new NativeList<ItemNode<T>>(cap, allocator);
        CurrentHighIndex = 0;
        CurrentLowIndex = -1;
        m_Length = 0;

#if ENABLE_UNITY_COLLECTIONS_CHECKS
        //m_MinIndex = 0;
        //m_MaxIndex = 0 - 1;
        DisposeSentinel.Create(out m_Safety, out m_DisposeSentinel, 0, allocator);
#endif
    }

    public int Length => m_Length;
    public bool IsCreated => list.IsCreated;
    
    public void Enqueue(T Item, double priority)
    {
        var item = new ItemNode<T> { Item = Item, priority = priority, NextHigh = -1, NextLow = -1, Index = list.Length };
        list.Add(item);
        m_Length++;
        ProcessEnqueue(ref item);
    }

    public void EnqueueNoResize(T Item, double priority)
    {
        var item = new ItemNode<T> { Item = Item, priority = priority, NextHigh = -1, NextLow = -1, Index = list.Length };
        list.AddNoResize(item);
        m_Length++;
        ProcessEnqueue(ref item);
    }

    private void ProcessEnqueue(ref ItemNode<T> item)
    {
        //ItemNode<T> currentHigh = item;
        // Set the currentHighIndex if item has higher priority
        //if (CurrentHighIndex > -1)
        //{
        ItemNode<T> currentHigh = list[CurrentHighIndex];
            if (currentHigh.priority < item.priority)
            {
                item.NextLow = currentHigh.Index;
                CurrentHighIndex = item.Index;
                list[item.Index] = item;
                currentHigh = item;
            }
        //}

        //if (CurrentHighIndex <= -1)
        //{
            // We are dealing with the first entry
            //var test = math.select(currentHigh.Index, item.Index, CurrentHighIndex == -1);
            //CurrentHighIndex = test;
            //currentHigh = item;
        //}

        //CurrentHighIndex = currentHigh.Index;

        ItemNode<T> nextNode;
        while(true)
        {
            if (currentHigh.NextLow != -1)
            {
                // move to the next node
                nextNode = list[currentHigh.NextLow];
                nextNode.NextHigh = currentHigh.Index;
                list[currentHigh.NextLow] = nextNode;
                currentHigh = nextNode;
                continue;
            }

            CurrentLowIndex = currentHigh.Index;
            // CurrentHighIndex = math.select(currentHigh.Index, item.Index, CurrentHighIndex == -1);
            break;
        }
    }

    public T Dequeue()
    {
        int currentWorkingIndex = CurrentLowIndex;
        //int li = list.Length - 1; // last index (before removal)
        ItemNode<T> frontItem = list[currentWorkingIndex];
        //list[0] = list[li];

        //list.RemoveAtSwapBack(li);
        m_Length--;

        //--li; // last index after removal;
        //int pi = 0;
        //while (true)
        //{
        //    int ci = math.mad(pi, 2, 1);
        //    //int ci = pi * 2 + 1; // left child index of parent
        //    if (ci > li) break; // no children so done
        //    int rc = ci + 1; // right child
        //    //if (rc <= li && list[rc].CompareTo(list[ci]) < 0) // if there is a rc (ci + 1), and it is smaller than left child, use the rc instead
        //    //    ci = rc;
        //    ci = math.select(ci, rc, rc <= li && list[rc].CompareTo(list[ci]) < 0);
        //    if (list[pi].CompareTo(list[ci]) <= 0) break; // parent is smaller than (or equal to) smallest child so done
        //    ItemNode<T> tmp = list[pi]; list[pi] = list[ci]; list[ci] = tmp;
        //    pi = ci;
        //}

        

        if (frontItem.NextHigh != -1)
        {
            ItemNode<T> nextItem = list[frontItem.NextHigh];
            CurrentLowIndex = frontItem.NextHigh;
            //HighValue = nextItem.priority;
            nextItem.NextLow = -1;
            list[frontItem.NextHigh] = nextItem;
        }
        //else
        //{
        CurrentHighIndex = math.select(CurrentHighIndex, -1, CurrentHighIndex == -1);
        //}
        
        frontItem.NextHigh = -1;
        frontItem.NextLow = -1;
        list[currentWorkingIndex] = frontItem;
        
        return frontItem.Item;
    }

    public ItemNode<T>[] ToArray()
    {
        return list.ToArray();
    }

    public void Dispose()
    {
#if ENABLE_UNITY_COLLECTIONS_CHECKS
        DisposeSentinel.Dispose(ref m_Safety, ref m_DisposeSentinel);
#endif
        list.Dispose();
    }
}

public struct ItemNode<T> : IComparable<ItemNode<T>> where T : struct
{
    public T Item;
    public double priority;
    public int NextHigh;
    public int NextLow;
    public int Index;

    public int CompareTo(ItemNode<T> other)
    {
        return priority.CompareTo(other.priority);
    }
}

internal sealed class NativePriorityQueueDebugView<T>
    where T : struct, IComparable
{
    private NativePriorityQueue<T> array;

    public NativePriorityQueueDebugView(NativePriorityQueue<T> array)
    {
        this.array = array;
    }

    public ItemNode<T>[] Items
    {
        get
        {
            return array.ToArray();
        }
    }
}

TestQueueSystem.cs

using System.Collections;
using System.Collections.Generic;
using Unity.Collections;
using Unity.Entities;
using UnityEngine;

public class TestQueueSystem : SystemBase
{
    protected override void OnCreate()
    {
        RequireSingletonForUpdate<SettingsComponent>();
    }
    protected override void OnUpdate()
    {
        //Enabled = false;

        

        Entities.ForEach((in SettingsComponent settings) =>
        {
            var test = new NativePriorityQueue<int>(50000, Allocator.Temp);

            for (int i = 0; i < 500; i++)
            {
                test.EnqueueNoResize(i, i);
            }
            //Debug.Log(test.Dequeue());
            //Debug.Log(test.Dequeue());
            //Debug.Log(test.Dequeue());
            while (test.Length > 0)
            {
                //Debug.Log(test.Length);
                //Debug.Log(test.Dequeue());
                test.Dequeue();
            }
            test.Dispose();
        })
        .WithoutBurst()    
        .Run();

        
    }
}