借助heap数据结构实现。
以小顶heap为例(说明值越小优先级越高,如距离),代码如下:
![](https://i-blog.csdnimg.cn/blog_migrate/8f900a89c6347c561fdf2122f13be562.gif)
![](https://i-blog.csdnimg.cn/blog_migrate/961ddebeb323a10fe0623af514929fc1.gif)
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 * <p> 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 * </p> 32 * <p> 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 * </p> 36 * 37 * <pre> 38 * 00 39 * / \ 40 * 01 02 41 * / \ / \ 42 * 03 04 05 06 43 * /\ /\ /\ /\ 44 * 07 08 09 10 11 12 13 14 45 * </pre> 46 * <p> 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 * </p> 50 * <p> 51 * This means that the array appears to be sorted, as long as we only ever look 52 * at element 0. 53 * </p> 54 * <p> 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 * </p> 60 * <p> 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 * </p> 66 * <p> 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 * </p> 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, then 106 // 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/values 116 // when moving up the tree, simply copy the parent into the hole and 117 // 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 child 165 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 child 174 } 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 priority 192 // creates a "hole" at the root of the tree. 193 // The algorithm swaps the hole with the appropriate child, until 194 // the last entry will fit correctly into the hole (ie is lower 195 // priority than its children) 196 public int pop() { 197 int ret = values.get(0); 198 199 // record the value/priority of the last entry 200 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 arrays 222 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 equal 233 // priority 234 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 }