借助heap数据结构实现。
以小顶heap为例（说明值越小优先级越高，如距离），代码如下：
1 // PriorityQueue.java 2 // Java Spatial Index Library 3 // Copyright (C) 2008 aled@users.sourceforge.net 4 // 5 // This library is free software; you can redistribute it and/or 6 // modify it under the terms of the GNU Lesser General Public 7 // License as published by the Free Software Foundation; either 8 // version 2.1 of the License, or (at your option) any later version. 9 // 10 // This library is distributed in the hope that it will be useful, 11 // but WITHOUT ANY WARRANTY; without even the implied warranty of 12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 // Lesser General Public License for more details. 14 // 15 // You should have received a copy of the GNU Lesser General Public 16 // License along with this library; if not, write to the Free Software 17 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 18 19 package buaa.act.ucar.moi; 20 21 import java.io.Serializable; 22 23 import gnu.trove.list.array.TFloatArrayList; 24 import gnu.trove.list.array.TIntArrayList; 25 26 /** 27 *28 * Priority Queue that stores values as ints and priorities as floats. Uses a 29 * Heap to sort the priorities; the values are sorted "in step" with the 30 * priorities. 31 *
32 *33 * A Heap is simply an array that is kept semi sorted; in particular if the 34 * elements of the array are arranged in a tree structure; ie 35 *
36 * 37 *38 * 00 39 * / \ 40 * 01 02 41 * / \ / \ 42 * 03 04 05 06 43 * /\ /\ /\ /\ 44 * 07 08 09 10 11 12 13 14 45 *46 *47 * then each parent is kept sorted with respect to it's immediate children. E.g. 48 * 00 < 01, 00 < 02, 02 < 05, 02 < 06 49 *
50 *51 * This means that the array appears to be sorted, as long as we only ever look 52 * at element 0. 53 *
54 *55 * Inserting new elements is much faster than if the entire array was kept 56 * sorted; a new element is appended to the array, and then recursively swapped 57 * with each parent to maintain the "parent is sorted w.r.t it's children" 58 * property. 59 *
60 *61 * To return the "next" value it is necessary to remove the root element. The 62 * last element in the array is placed in the root of the tree, and is 63 * recursively swapped with one of it's children until the "parent is sorted 64 * w.r.t it's children" property is restored. 65 *
66 *67 * Random access is slow (eg for deleting a particular value), and is not 68 * implemented here - if this functionality is required, then a heap probably 69 * isn't the right data structure. 70 *
71 */ 72 public class PriorityQueue implements Serializable { 73 private static final long serialVersionUID = -5653506138757217673L; 74 public static final boolean SORT_ORDER_ASCENDING = true; 75 public static final boolean SORT_ORDER_DESCENDING = false; 76 77 private TIntArrayList values = null; 78 private TFloatArrayList priorities = null; 79 private boolean sortOrder = SORT_ORDER_ASCENDING; 80 81 private static boolean INTERNAL_CONSISTENCY_CHECKING = false; 82 83 public PriorityQueue(boolean sortOrder) { 84 this(sortOrder, 10); 85 } 86 87 public PriorityQueue(boolean sortOrder, int initialCapacity) { 88 this.sortOrder = sortOrder; 89 values = new TIntArrayList(initialCapacity); 90 priorities = new TFloatArrayList(initialCapacity); 91 } 92 93 /** 94 * @param p1 95 * @param p2 96 * @return true if p1 has an earlier sort order than p2. 97 */ 98 private boolean sortsEarlierThan(float p1, float p2) { 99 if (sortOrder == SORT_ORDER_ASCENDING) {100 return p1 < p2;101 }102 return p2 < p1;103 }104 105 // to insert a value, append it to the arrays, then106 // reheapify by promoting it to the correct place.107 public void insert(int value, float priority) {108 values.add(value);109 priorities.add(priority);110 111 promote(values.size() - 1, value, priority);112 }113 114 private void promote(int index, int value, float priority) {115 // Consider the index to be a "hole"; i.e. don't swap priorities/values116 // when moving up the tree, simply copy the parent into the hole and117 // then consider the parent to be the hole.118 // Finally, copy the value/priority into the hole.119 while (index > 0) {120 int parentIndex = (index - 1) / 2;121 float parentPriority = priorities.get(parentIndex);122 123 if (sortsEarlierThan(parentPriority, priority)) {124 break;125 }126 127 // copy the parent entry into the current index.128 values.set(index, values.get(parentIndex));129 priorities.set(index, parentPriority);130 index = parentIndex;131 }132 133 values.set(index, value);134 priorities.set(index, priority);135 136 if (INTERNAL_CONSISTENCY_CHECKING) {137 check();138 }139 }140 141 public int size() {142 return values.size();143 }144 145 public void clear() {146 values.clear();147 priorities.clear();148 }149 150 public void reset() {151 values.reset();152 priorities.reset();153 }154 155 public int getValue() {156 return values.get(0);157 }158 159 public float getPriority() {160 return priorities.get(0);161 }162 163 private void demote(int index, int value, float priority) {164 int childIndex = (index * 2) + 1; // left child165 166 while (childIndex < values.size()) {167 float childPriority = priorities.get(childIndex);168 169 if (childIndex + 1 < values.size()) {170 float rightPriority = priorities.get(childIndex + 1);171 if (sortsEarlierThan(rightPriority, childPriority)) {172 childPriority = rightPriority;173 childIndex++; // right child174 }175 }176 177 if (sortsEarlierThan(childPriority, priority)) {178 priorities.set(index, childPriority);179 values.set(index, values.get(childIndex));180 index = childIndex;181 childIndex = (index * 2) + 1;182 } else {183 break;184 }185 }186 187 values.set(index, value);188 priorities.set(index, priority);189 }190 191 // get the value with the lowest priority192 // creates a "hole" at the root of the tree.193 // The algorithm swaps the hole with the appropriate child, until194 // the last entry will fit correctly into the hole (ie is lower195 // priority than its children)196 public int pop() {197 int ret = values.get(0);198 199 // record the value/priority of the last entry200 int lastIndex = values.size() - 1;201 int tempValue = values.get(lastIndex);202 float tempPriority = priorities.get(lastIndex);203 204 values.removeAt(lastIndex);205 priorities.removeAt(lastIndex);206 207 if (lastIndex > 0) {208 demote(0, tempValue, tempPriority);209 }210 211 if (INTERNAL_CONSISTENCY_CHECKING) {212 check();213 }214 215 return ret;216 }217 218 public void setSortOrder(boolean sortOrder) {219 if (this.sortOrder != sortOrder) {220 this.sortOrder = sortOrder;221 // reheapify the arrays222 for (int i = (values.size() / 2) - 1; i >= 0; i--) {223 demote(i, values.get(i), priorities.get(i));224 }225 }226 if (INTERNAL_CONSISTENCY_CHECKING) {227 check();228 }229 }230 231 private void check() {232 // for each entry, check that the child entries have a lower or equal233 // priority234 int lastIndex = values.size() - 1;235 236 for (int i = 0; i < values.size() / 2; i++) {237 float currentPriority = priorities.get(i);238 239 int leftIndex = (i * 2) + 1;240 if (leftIndex <= lastIndex) {241 float leftPriority = priorities.get(leftIndex);242 if (sortsEarlierThan(leftPriority, currentPriority)) {243 System.err.println("Internal error in PriorityQueue");244 }245 }246 247 int rightIndex = (i * 2) + 2;248 if (rightIndex <= lastIndex) {249 float rightPriority = priorities.get(rightIndex);250 if (sortsEarlierThan(rightPriority, currentPriority)) {251 System.err.println("Internal error in PriorityQueue");252 }253 }254 }255 }256 }