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) 19972005 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..max1]. 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
