SVN checkout 11/12/2010
[monav:monav.git] / plugins / contractionhierarchies / binaryheap.h
1 /*
2 Copyright 2010  Christian Vetter veaac.fdirct@gmail.com
3
4 This file is part of MoNav.
5
6 MoNav is free software: you can redistribute it and/or modify
7 it under the terms of the GNU General Public License as published by
8 the Free Software Foundation, either version 3 of the License, or
9 (at your option) any later version.
10
11 MoNav is distributed in the hope that it will be useful,
12 but WITHOUT ANY WARRANTY; without even the implied warranty of
13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14 GNU General Public License for more details.
15
16 You should have received a copy of the GNU General Public License
17 along 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
29 template< typename NodeID, typename Key >
30 class 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
54 template< typename NodeID, typename Key >
55 class 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
77 template < typename NodeID, typename Key, typename Weight, typename Data, typename IndexStorage = ArrayStorage< NodeID, Key > >
78 class 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