root / rgbdslam / gicp / ann_1.1.1 / src / pr_queue.h @ 9240aaa3
History | View | Annotate | Download (4.39 KB)
1 | 9240aaa3 | Alex | //----------------------------------------------------------------------
|
---|---|---|---|
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 |