PriorityQueue的Java实现

借助heap数据结构实现。

以小顶heap为例（说明值越小优先级越高，如距离），代码如下：

 //  PriorityQueue.java
 //  Java Spatial Index Library
 //  Copyright (C) 2008 aled@users.sourceforge.net
 //
 //  This library is free software; you can redistribute it and/or
 //  modify it under the terms of the GNU Lesser General Public
 //  License as published by the Free Software Foundation; either
 //  version 2.1 of the License, or (at your option) any later version.
 //
 //  This library is distributed in the hope that it will be useful,
 //  but WITHOUT ANY WARRANTY; without even the implied warranty of
 //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 //  Lesser General Public License for more details.
 //
 //  You should have received a copy of the GNU Lesser General Public
 //  License along with this library; if not, write to the Free Software
 //  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA

 package buaa.act.ucar.moi;

 import java.io.Serializable;

 import gnu.trove.list.array.TFloatArrayList;
 import gnu.trove.list.array.TIntArrayList;

 /**
  * <p>
  * Priority Queue that stores values as ints and priorities as floats. Uses a
  * Heap to sort the priorities; the values are sorted "in step" with the
  * priorities.
  * </p>
  * <p>
  * A Heap is simply an array that is kept semi sorted; in particular if the
  * elements of the array are arranged in a tree structure; ie
  * </p>
  *
  * <pre>
  *                                   00
  *                                 /     \
  *                             01          02
  *                            /  \        /  \
  *                          03    04    05    06
  *                          /\    /\    /\    /\
  *                        07 08 09 10 11 12 13 14
  * </pre>
  * <p>
  * then each parent is kept sorted with respect to it's immediate children. E.g.
  * 00 < 01, 00 < 02, 02 < 05, 02 < 06
  * </p>
  * <p>
  * This means that the array appears to be sorted, as long as we only ever look
  * at element 0.
  * </p>
  * <p>
  * Inserting new elements is much faster than if the entire array was kept
  * sorted; a new element is appended to the array, and then recursively swapped
  * with each parent to maintain the "parent is sorted w.r.t it's children"
  * property.
  * </p>
  * <p>
  * To return the "next" value it is necessary to remove the root element. The
  * last element in the array is placed in the root of the tree, and is
  * recursively swapped with one of it's children until the "parent is sorted
  * w.r.t it's children" property is restored.
  * </p>
  * <p>
  * Random access is slow (eg for deleting a particular value), and is not
  * implemented here - if this functionality is required, then a heap probably
  * isn't the right data structure.
  * </p>
  */
 public class PriorityQueue implements Serializable {
     private static final long serialVersionUID = -5653506138757217673L;
     public static final boolean SORT_ORDER_ASCENDING = true;
     public static final boolean SORT_ORDER_DESCENDING = false;

     private TIntArrayList values = null;
     private TFloatArrayList priorities = null;
     private boolean sortOrder = SORT_ORDER_ASCENDING;

     private static boolean INTERNAL_CONSISTENCY_CHECKING = false;

     public PriorityQueue(boolean sortOrder) {
         this(sortOrder, 10);
     }

     public PriorityQueue(boolean sortOrder, int initialCapacity) {
         this.sortOrder = sortOrder;
         values = new TIntArrayList(initialCapacity);
         priorities = new TFloatArrayList(initialCapacity);
     }

     /**
      * @param p1
      * @param p2
      * @return true if p1 has an earlier sort order than p2.
      */
     private boolean sortsEarlierThan(float p1, float p2) {
         if (sortOrder == SORT_ORDER_ASCENDING) {
             return p1 < p2;
         }
         return p2 < p1;
     }

     // to insert a value, append it to the arrays, then
     // reheapify by promoting it to the correct place.
     public void insert(int value, float priority) {
         values.add(value);
         priorities.add(priority);

         promote(values.size() - 1, value, priority);
     }

     private void promote(int index, int value, float priority) {
         // Consider the index to be a "hole"; i.e. don't swap priorities/values
         // when moving up the tree, simply copy the parent into the hole and
         // then consider the parent to be the hole.
         // Finally, copy the value/priority into the hole.
         while (index > 0) {
             int parentIndex = (index - 1) / 2;
             float parentPriority = priorities.get(parentIndex);

             if (sortsEarlierThan(parentPriority, priority)) {
                 break;
             }

             // copy the parent entry into the current index.
             values.set(index, values.get(parentIndex));
             priorities.set(index, parentPriority);
             index = parentIndex;
         }

         values.set(index, value);
         priorities.set(index, priority);

         if (INTERNAL_CONSISTENCY_CHECKING) {
             check();
         }
     }

     public int size() {
         return values.size();
     }

     public void clear() {
         values.clear();
         priorities.clear();
     }

     public void reset() {
         values.reset();
         priorities.reset();
     }

     public int getValue() {
         return values.get(0);
     }

     public float getPriority() {
         return priorities.get(0);
     }

     private void demote(int index, int value, float priority) {
         int childIndex = (index * 2) + 1; // left child

         while (childIndex < values.size()) {
             float childPriority = priorities.get(childIndex);

             if (childIndex + 1 < values.size()) {
                 float rightPriority = priorities.get(childIndex + 1);
                 if (sortsEarlierThan(rightPriority, childPriority)) {
                     childPriority = rightPriority;
                     childIndex++; // right child
                 }
             }

             if (sortsEarlierThan(childPriority, priority)) {
                 priorities.set(index, childPriority);
                 values.set(index, values.get(childIndex));
                 index = childIndex;
                 childIndex = (index * 2) + 1;
             } else {
                 break;
             }
         }

         values.set(index, value);
         priorities.set(index, priority);
     }

     // get the value with the lowest priority
     // creates a "hole" at the root of the tree.
     // The algorithm swaps the hole with the appropriate child, until
     // the last entry will fit correctly into the hole (ie is lower
     // priority than its children)
     public int pop() {
         int ret = values.get(0);

         // record the value/priority of the last entry
         int lastIndex = values.size() - 1;
         int tempValue = values.get(lastIndex);
         float tempPriority = priorities.get(lastIndex);

         values.removeAt(lastIndex);
         priorities.removeAt(lastIndex);

         if (lastIndex > 0) {
             demote(0, tempValue, tempPriority);
         }

         if (INTERNAL_CONSISTENCY_CHECKING) {
             check();
         }

         return ret;
     }

     public void setSortOrder(boolean sortOrder) {
         if (this.sortOrder != sortOrder) {
             this.sortOrder = sortOrder;
             // reheapify the arrays
             for (int i = (values.size() / 2) - 1; i >= 0; i--) {
                 demote(i, values.get(i), priorities.get(i));
             }
         }
         if (INTERNAL_CONSISTENCY_CHECKING) {
             check();
         }
     }

     private void check() {
         // for each entry, check that the child entries have a lower or equal
         // priority
         int lastIndex = values.size() - 1;

         for (int i = 0; i < values.size() / 2; i++) {
             float currentPriority = priorities.get(i);

             int leftIndex = (i * 2) + 1;
             if (leftIndex <= lastIndex) {
                 float leftPriority = priorities.get(leftIndex);
                 if (sortsEarlierThan(leftPriority, currentPriority)) {
                     System.err.println("Internal error in PriorityQueue");
                 }
             }

             int rightIndex = (i * 2) + 2;
             if (rightIndex <= lastIndex) {
                 float rightPriority = priorities.get(rightIndex);
                 if (sortsEarlierThan(rightPriority, currentPriority)) {
                     System.err.println("Internal error in PriorityQueue");
                 }
             }
         }
     }
 }