PriorityQueue.cs 2.55 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112
using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using System;

public class PriorityQueue<T> where T : IComparable<T>
{
    private List<T> data;    // 1 Indexed, element 0 is left empty

    public PriorityQueue()
    {
        data = new List<T>();
    }

    public T Push(T t)
    {
        int i = data.Count; // Starts at 1
        data.Add(t);

        for(;i > 2 && t.CompareTo(data[i/2]) < 0; i = (i / 2) - 1)  // Compare t to parent and swap if needed
        {
            // Swap parent to child position
            swap(i, (i / 2) - 1);
        }

        return t;
    }

    public T Pop()
    {
        if (data.Count <= 0)
            return default(T);
        T removed = data[0];    // Store what is removed to return

        percDown(0);

        return removed;
    }

    public T Peek()
    {
        if (data.Count <= 0)
            return default(T);
        return data[0];
    }

    public bool empty()
    {
        return data.Count <= 0;
    }

    public bool Remove(T r)
    {
        for (int i = 0; i < data.Count; i++)
        {
            if(r.CompareTo(data[i]) == 0)
            {
                percDown(i);
                return true;
            }
        }
        return false;
    }

    public String toString()
    {
        String str = "Priority Queue (" + (data.Count - 1) + ")\n";
        foreach (T t in data)
        {
            if (EqualityComparer<T>.Default.Equals(t, default(T)))
                str += "NULL";
            else
                str += t.ToString();
        }
        return str;
    }

    private void swap(int p1, int p2)
    {
        T temp = data[p1];
        data[p1] = data[p2];
        data[p2] = temp;
    }

    // Percolate down from i
    // Fix the structure in order of priority
    private void percDown(int i)
    {
        int il = (i * 2) + 1; // Index of left child
        int ir = il + 1;      // Index of right child
        data[i] = data[data.Count - 1]; // Move last element to removed position
        data.RemoveAt(data.Count - 1);  // Remove the copy of the previous last element

        while (il < data.Count) // Percolate down
        {
            if (ir >= data.Count || data[il].CompareTo(data[ir]) < 0)    // Left is higher priority
            {
                swap(i, il);
                i = il;
            }
            else    // Right is higher priority
            {
                swap(i, ir);
                i = ir;
            }

            // Calculate new child indexes
            il = (i * 2) + 1;
            ir = il + 1;
        }
    }
}