root / rgbdslam / gicp / ann_1.1.1 / src / pr_queue.h @ 9240aaa3
History | View | Annotate | Download (4.39 KB)
1 |
//----------------------------------------------------------------------
|
---|---|
2 |
// File: pr_queue.h
|
3 |
// Programmer: Sunil Arya and David Mount
|
4 |
// Description: Include file for priority queue and related
|
5 |
// structures.
|
6 |
// Last modified: 01/04/05 (Version 1.0)
|
7 |
//----------------------------------------------------------------------
|
8 |
// Copyright (c) 1997-2005 University of Maryland and Sunil Arya and
|
9 |
// David Mount. All Rights Reserved.
|
10 |
//
|
11 |
// This software and related documentation is part of the Approximate
|
12 |
// Nearest Neighbor Library (ANN). This software is provided under
|
13 |
// the provisions of the Lesser GNU Public License (LGPL). See the
|
14 |
// file ../ReadMe.txt for further information.
|
15 |
//
|
16 |
// The University of Maryland (U.M.) and the authors make no
|
17 |
// representations about the suitability or fitness of this software for
|
18 |
// any purpose. It is provided "as is" without express or implied
|
19 |
// warranty.
|
20 |
//----------------------------------------------------------------------
|
21 |
// History:
|
22 |
// Revision 0.1 03/04/98
|
23 |
// Initial release
|
24 |
//----------------------------------------------------------------------
|
25 |
|
26 |
#ifndef PR_QUEUE_H
|
27 |
#define PR_QUEUE_H
|
28 |
|
29 |
#include <ANN/ANNx.h> // all ANN includes |
30 |
#include <ANN/ANNperf.h> // performance evaluation |
31 |
|
32 |
//----------------------------------------------------------------------
|
33 |
// Basic types.
|
34 |
//----------------------------------------------------------------------
|
35 |
typedef void *PQinfo; // info field is generic pointer |
36 |
typedef ANNdist PQkey; // key field is distance |
37 |
|
38 |
//----------------------------------------------------------------------
|
39 |
// Priority queue
|
40 |
// A priority queue is a list of items, along with associated
|
41 |
// priorities. The basic operations are insert and extract_minimum.
|
42 |
//
|
43 |
// The priority queue is maintained using a standard binary heap.
|
44 |
// (Implementation note: Indexing is performed from [1..max] rather
|
45 |
// than the C standard of [0..max-1]. This simplifies parent/child
|
46 |
// computations.) User information consists of a void pointer,
|
47 |
// and the user is responsible for casting this quantity into whatever
|
48 |
// useful form is desired.
|
49 |
//
|
50 |
// Because the priority queue is so central to the efficiency of
|
51 |
// query processing, all the code is inline.
|
52 |
//----------------------------------------------------------------------
|
53 |
|
54 |
class ANNpr_queue { |
55 |
|
56 |
struct pq_node { // node in priority queue |
57 |
PQkey key; // key value
|
58 |
PQinfo info; // info field
|
59 |
}; |
60 |
int n; // number of items in queue |
61 |
int max_size; // maximum queue size |
62 |
pq_node *pq; // the priority queue (array of nodes)
|
63 |
|
64 |
public:
|
65 |
ANNpr_queue(int max) // constructor (given max size) |
66 |
{ |
67 |
n = 0; // initially empty |
68 |
max_size = max; // maximum number of items
|
69 |
pq = new pq_node[max+1]; // queue is array [1..max] of nodes |
70 |
} |
71 |
|
72 |
~ANNpr_queue() // destructor
|
73 |
{ delete [] pq; } |
74 |
|
75 |
ANNbool empty() // is queue empty?
|
76 |
{ if (n==0) return ANNtrue; else return ANNfalse; } |
77 |
|
78 |
ANNbool non_empty() // is queue nonempty?
|
79 |
{ if (n==0) return ANNfalse; else return ANNtrue; } |
80 |
|
81 |
void reset() // make existing queue empty |
82 |
{ n = 0; }
|
83 |
|
84 |
inline void insert( // insert item (inlined for speed) |
85 |
PQkey kv, // key value
|
86 |
PQinfo inf) // item info
|
87 |
{ |
88 |
if (++n > max_size) annError("Priority queue overflow.", ANNabort); |
89 |
register int r = n; |
90 |
while (r > 1) { // sift up new item |
91 |
register int p = r/2; |
92 |
ANN_FLOP(1) // increment floating ops |
93 |
if (pq[p].key <= kv) // in proper order |
94 |
break;
|
95 |
pq[r] = pq[p]; // else swap with parent
|
96 |
r = p; |
97 |
} |
98 |
pq[r].key = kv; // insert new item at final location
|
99 |
pq[r].info = inf; |
100 |
} |
101 |
|
102 |
inline void extr_min( // extract minimum (inlined for speed) |
103 |
PQkey &kv, // key (returned)
|
104 |
PQinfo &inf) // item info (returned)
|
105 |
{ |
106 |
kv = pq[1].key; // key of min item |
107 |
inf = pq[1].info; // information of min item |
108 |
register PQkey kn = pq[n--].key;// last item in queue |
109 |
register int p = 1; // p points to item out of position |
110 |
register int r = p<<1; // left child of p |
111 |
while (r <= n) { // while r is still within the heap |
112 |
ANN_FLOP(2) // increment floating ops |
113 |
// set r to smaller child of p
|
114 |
if (r < n && pq[r].key > pq[r+1].key) r++; |
115 |
if (kn <= pq[r].key) // in proper order |
116 |
break;
|
117 |
pq[p] = pq[r]; // else swap with child
|
118 |
p = r; // advance pointers
|
119 |
r = p<<1;
|
120 |
} |
121 |
pq[p] = pq[n+1]; // insert last item in proper place |
122 |
} |
123 |
}; |
124 |
|
125 |
#endif
|