Statistics
| Branch: | Revision:

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