SVN checkout 11/12/2010
[monav:monav.git] / plugins / contractionhierarchies / dynamicgraph.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 DYNAMICGRAPH_H_INCLUDED
21 #define DYNAMICGRAPH_H_INCLUDED
22
23 #include <vector>
24 #include <algorithm>
25 #include <limits>
26 #include "utils/bithelpers.h"
27
28 template< typename EdgeData>
29 class DynamicGraph {
30         public:
31                 typedef unsigned NodeIterator;
32                 typedef unsigned EdgeIterator;
33
34                 class InputEdge {
35                         public:
36                                 NodeIterator source;
37                                 NodeIterator target;
38                                 EdgeData data;
39                                 bool operator<( const InputEdge& right ) const {
40                                         if ( source != right.source )
41                                                 return source < right.source;
42                                         return target < right.target;
43                                 }
44                 };
45
46                 DynamicGraph( int nodes, const std::vector< InputEdge > &graph )
47                 {
48                         m_numNodes = nodes;
49                         m_numEdges = ( EdgeIterator ) graph.size();
50                         m_nodes.reserve( m_numNodes );
51                         m_nodes.resize( m_numNodes );
52                         EdgeIterator edge = 0;
53                         EdgeIterator position = 0;
54                         for ( NodeIterator node = 0; node < m_numNodes; ++node ) {
55                                 EdgeIterator lastEdge = edge;
56                                 while ( edge < m_numEdges && graph[edge].source == node ) {
57                                         ++edge;
58                                 }
59                                 m_nodes[node].firstEdge = position;
60                                 m_nodes[node].edges = edge - lastEdge;
61                                 position += m_nodes[node].edges;
62                         }
63                         m_edges.reserve( position * 1.2 );
64                         m_edges.resize( position );
65                         qDebug() << "Contraction Hiearchies: dynamic graph usage:" << m_numEdges << m_edges.size() << m_edges.capacity();
66                         edge = 0;
67                         for ( NodeIterator node = 0; node < m_numNodes; ++node ) {
68                                 for ( EdgeIterator i = m_nodes[node].firstEdge, e = m_nodes[node].firstEdge + m_nodes[node].edges; i != e; ++i ) {
69                                         m_edges[i].target = graph[edge].target;
70                                         m_edges[i].data = graph[edge].data;
71                                         edge++;
72                                 }
73                         }
74                 }
75
76                 ~DynamicGraph()
77                 {
78                         qDebug() << "Contraction Hiearchies: dynamic graph usage:" << m_numEdges << m_edges.size() << m_edges.capacity();
79                 }
80
81                 unsigned GetNumberOfNodes() const
82                 {
83                         return m_numNodes;
84                 }
85
86                 unsigned GetNumberOfEdges() const
87                 {
88                         return m_numEdges;
89                 }
90
91                 unsigned GetOutDegree( const NodeIterator &n ) const
92                 {
93                         return m_nodes[n].edges;
94                 }
95
96                 NodeIterator GetTarget( const EdgeIterator &e ) const
97                 {
98                         return NodeIterator( m_edges[e].target );
99                 }
100
101                 EdgeData &GetEdgeData( const EdgeIterator &e )
102                 {
103                         return m_edges[e].data;
104                 }
105
106                 const EdgeData &GetEdgeData( const EdgeIterator &e ) const
107                 {
108                         return m_edges[e].data;
109                 }
110
111                 EdgeIterator BeginEdges( const NodeIterator &n ) const
112                 {
113                         //assert( EndEdges( n ) - EdgeIterator( _nodes[n].firstEdge ) <= 100 );
114                         return EdgeIterator( m_nodes[n].firstEdge );
115                 }
116
117                 EdgeIterator EndEdges( const NodeIterator &n ) const
118                 {
119                         return EdgeIterator( m_nodes[n].firstEdge + m_nodes[n].edges );
120                 }
121
122                 //adds an edge. Invalidates edge iterators for the source node
123                 EdgeIterator InsertEdge( const NodeIterator &from, const NodeIterator &to, const EdgeData &data )
124                 {
125                         Node &node = m_nodes[from];
126                         EdgeIterator newFirstEdge = node.edges + node.firstEdge;
127                         if ( newFirstEdge >= m_edges.size() || !isDummy( newFirstEdge ) ) {
128                                 if ( node.firstEdge != 0 && isDummy( node.firstEdge - 1 ) ) {
129                                         node.firstEdge--;
130                                         m_edges[node.firstEdge] = m_edges[node.firstEdge + node.edges];
131                                 } else {
132                                         EdgeIterator newFirstEdge = ( EdgeIterator ) m_edges.size();
133                                         unsigned newSize = node.edges * 1.2 + 2;
134                                         EdgeIterator requiredCapacity = newSize + m_edges.size();
135                                         EdgeIterator oldCapacity = m_edges.capacity();
136                                         if ( requiredCapacity >= oldCapacity ) {
137                                                 m_edges.reserve( requiredCapacity * 1.1 );
138                                                 qDebug() << "Contraction Hiearchies: increased graph size:" << m_edges.capacity();
139                                         }
140                                         m_edges.resize( m_edges.size() + newSize );
141                                         for ( EdgeIterator i = 0; i < node.edges; ++i ) {
142                                                 m_edges[newFirstEdge + i ] = m_edges[node.firstEdge + i];
143                                                 makeDummy( node.firstEdge + i );
144                                         }
145                                         for ( EdgeIterator i = node.edges + 1; i < newSize; i++ )
146                                                 makeDummy( newFirstEdge + i );
147                                         node.firstEdge = newFirstEdge;
148                                 }
149                         }
150                         Edge &edge = m_edges[node.firstEdge + node.edges];
151                         edge.target = to;
152                         edge.data = data;
153                         m_numEdges++;
154                         node.edges++;
155                         return EdgeIterator( node.firstEdge + node.edges );
156                 }
157
158                 //removes an edge. Invalidates edge iterators for the source node
159                 void DeleteEdge( const NodeIterator source, const EdgeIterator &e ) {
160                         Node &node = m_nodes[source];
161                         --m_numEdges;
162                         --node.edges;
163                         const unsigned last = node.firstEdge + node.edges;
164                         //swap with last edge
165                         m_edges[e] = m_edges[last];
166                         makeDummy( last );
167                 }
168
169                 //removes all edges (source,target)
170                 int DeleteEdgesTo( const NodeIterator source, const NodeIterator target )
171                 {
172                         int deleted = 0;
173                         for ( EdgeIterator i = BeginEdges( source ), iend = EndEdges( source ); i < iend - deleted; ++i ) {
174                                 if ( m_edges[i].target == target ) {
175                                         do {
176                                                 deleted++;
177                                                 m_edges[i] = m_edges[iend - deleted];
178                                                 makeDummy( iend - deleted );
179                                         } while ( i < iend - deleted && m_edges[i].target == target );
180                                 }
181                         }
182
183                         #pragma omp atomic
184                         m_numEdges -= deleted;
185                         m_nodes[source].edges -= deleted;
186
187                         return deleted;
188                 }
189
190                 //searches for a specific edge
191                 EdgeIterator FindEdge( const NodeIterator &from, const NodeIterator &to ) const
192                 {
193                         for ( EdgeIterator i = BeginEdges( from ), iend = EndEdges( from ); i != iend; ++i ) {
194                                 if ( m_edges[i].target == to ) {
195                                         return i;
196                                 }
197                         }
198                         return EndEdges( from );
199                 }
200
201         protected:
202
203                 bool isDummy( EdgeIterator edge ) const
204                 {
205                         return m_edges[edge].target == std::numeric_limits< NodeIterator >::max();
206                 }
207
208                 void makeDummy( EdgeIterator edge )
209                 {
210                         m_edges[edge].target = std::numeric_limits< NodeIterator >::max();
211                 }
212
213                 struct Node {
214                         //index of the first edge
215                         EdgeIterator firstEdge;
216                         //amount of edges
217                         unsigned edges;
218                 };
219
220                 struct Edge {
221                         NodeIterator target;
222                         EdgeData data;
223                 };
224
225                 NodeIterator m_numNodes;
226                 EdgeIterator m_numEdges;
227
228                 std::vector< Node > m_nodes;
229                 std::vector< Edge > m_edges;
230 };
231
232 #endif // DYNAMICGRAPH_H_INCLUDED