Root/monav/contractionhierarchies/binaryheap.h

1/*
2Copyright 2010 Christian Vetter veaac.fdirct@gmail.com
3
4This file is part of MoNav.
5
6MoNav is free software: you can redistribute it and/or modify
7it under the terms of the GNU General Public License as published by
8the Free Software Foundation, either version 3 of the License, or
9(at your option) any later version.
10
11MoNav is distributed in the hope that it will be useful,
12but WITHOUT ANY WARRANTY; without even the implied warranty of
13MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14GNU General Public License for more details.
15
16You should have received a copy of the GNU General Public License
17along with MoNav. If not, see <http://www.gnu.org/licenses/>.
18*/
19
20#ifndef BINARYHEAP_H_INCLUDED
21#define BINARYHEAP_H_INCLUDED
22
23//Not compatible with non contiguous node ids
24
25#include <cassert>
26#include <vector>
27#include <QHash>
28
29template< typename NodeID, typename Key >
30class ArrayStorage {
31    public:
32
33        ArrayStorage( size_t size ) :
34                positions( new Key[size] )
35        {
36        }
37
38        ~ArrayStorage()
39        {
40            delete[] positions;
41        }
42
43        Key &operator[]( NodeID node )
44        {
45            return positions[node];
46        }
47
48        void clear() {}
49
50    private:
51        Key* positions;
52};
53
54template< typename NodeID, typename Key >
55class MapStorage {
56    public:
57
58        MapStorage( size_t )
59        {
60        }
61
62        Key &operator[]( NodeID node )
63        {
64            return nodes[node];
65        }
66
67        void clear()
68        {
69            nodes.clear();
70        }
71
72    private:
73        QHash< NodeID, Key > nodes;
74
75};
76
77template < typename NodeID, typename Key, typename Weight, typename Data, typename IndexStorage = ArrayStorage< NodeID, Key > >
78class BinaryHeap {
79    private:
80        BinaryHeap( const BinaryHeap& right );
81        void operator=( const BinaryHeap& right );
82    public:
83        typedef Weight WeightType;
84        typedef Data DataType;
85
86        BinaryHeap( size_t maxID )
87                : nodeIndex( maxID ) {
88            Clear();
89        }
90
91        void Clear() {
92            heap.resize( 1 );
93            insertedNodes.clear();
94            nodeIndex.clear();
95            heap[0].weight = 0;
96        }
97
98        Key Size() const {
99            return ( Key )( heap.size() - 1 );
100        }
101
102        void Insert( NodeID node, Weight weight, const Data &data ) {
103            HeapElement element;
104            element.index = ( NodeID ) insertedNodes.size();
105            element.weight = weight;
106            const Key key = ( Key ) heap.size();
107            heap.push_back( element );
108            insertedNodes.push_back( HeapNode( node, key, weight, data ) );
109            nodeIndex[node] = element.index;
110            Upheap( key );
111            CheckHeap();
112        }
113
114        Data& GetData( NodeID node ) {
115            const Key index = nodeIndex[node];
116            return insertedNodes[index].data;
117        }
118
119        Weight& GetKey( NodeID node ) {
120            const Key index = nodeIndex[node];
121            return insertedNodes[index].weight;
122        }
123
124        bool WasRemoved( NodeID node ) {
125            assert( WasInserted( node ) );
126            const Key index = nodeIndex[node];
127            return insertedNodes[index].key == 0;
128        }
129
130        bool WasInserted( NodeID node ) {
131            const Key index = nodeIndex[node];
132            if ( index >= ( Key ) insertedNodes.size() )
133                return false;
134            return insertedNodes[index].node == node;
135        }
136
137        NodeID Min() const {
138            assert( heap.size() > 1 );
139            return insertedNodes[heap[1].index].node;
140        }
141
142        NodeID DeleteMin() {
143            assert( heap.size() > 1 );
144            const Key removedIndex = heap[1].index;
145            heap[1] = heap[heap.size()-1];
146            heap.pop_back();
147            if ( heap.size() > 1 )
148                Downheap( 1 );
149            insertedNodes[removedIndex].key = 0;
150            CheckHeap();
151            return insertedNodes[removedIndex].node;
152        }
153
154        void DeleteAll() {
155            for ( typename std::vector< HeapElement >::iterator i = heap.begin() + 1, iend = heap.end(); i != iend; ++i )
156                insertedNodes[i->index].key = 0;
157            heap.resize( 1 );
158            heap[0].weight = 0;
159        }
160
161
162        void DecreaseKey( NodeID node, Weight weight ) {
163            const Key index = nodeIndex[node];
164            Key key = insertedNodes[index].key;
165            assert ( key != 0 );
166
167            insertedNodes[index].weight = weight;
168            heap[key].weight = weight;
169            Upheap( key );
170            CheckHeap();
171        }
172
173    private:
174        class HeapNode {
175            public:
176                HeapNode() {
177                }
178                HeapNode( NodeID n, Key k, Weight w, Data d )
179                        : node( n ), key( k ), weight( w ), data( d ) {
180                }
181
182                NodeID node;
183                Key key;
184                Weight weight;
185                Data data;
186        };
187        struct HeapElement {
188            Key index;
189            Weight weight;
190        };
191
192        std::vector< HeapNode > insertedNodes;
193        std::vector< HeapElement > heap;
194        IndexStorage nodeIndex;
195
196        void Downheap( Key key ) {
197            const Key droppingIndex = heap[key].index;
198            const Weight weight = heap[key].weight;
199            Key nextKey = key << 1;
200            while ( nextKey < ( Key ) heap.size() ) {
201                const Key nextKeyOther = nextKey + 1;
202                if ( ( nextKeyOther < ( Key ) heap.size() ) )
203                    if ( heap[nextKey].weight > heap[nextKeyOther].weight )
204                        nextKey = nextKeyOther;
205
206                if ( weight <= heap[nextKey].weight )
207                    break;
208
209                heap[key] = heap[nextKey];
210                insertedNodes[heap[key].index].key = key;
211                key = nextKey;
212                nextKey <<= 1;
213            }
214            heap[key].index = droppingIndex;
215            heap[key].weight = weight;
216            insertedNodes[droppingIndex].key = key;
217        }
218
219        void Upheap( Key key ) {
220            const Key risingIndex = heap[key].index;
221            const Weight weight = heap[key].weight;
222            Key nextKey = key >> 1;
223            while ( heap[nextKey].weight > weight ) {
224                assert( nextKey != 0 );
225                heap[key] = heap[nextKey];
226                insertedNodes[heap[key].index].key = key;
227                key = nextKey;
228                nextKey >>= 1;
229            }
230            heap[key].index = risingIndex;
231            heap[key].weight = weight;
232            insertedNodes[risingIndex].key = key;
233        }
234        
235        void CheckHeap() {
236            /*for ( Key i = 2; i < heap.size(); ++i ) {
237                assert( heap[i].weight >= heap[i >> 1].weight );
238            }*/
239        }
240};
241
242#endif //#ifndef BINARYHEAP_H_INCLUDED
243

Archive Download this file

Branches:
master



interactive