Root/monav/contractionhierarchies/contractionhierarchiesclient.cpp

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
13{
14}
15 without even the implied warranty of
16MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17GNU General Public License for more details.
18
19You should have received a copy of the GNU General Public License
20along with MoNav. If not, see <http://www.gnu.org/licenses/>.
21*/
22
23#include "contractionhierarchiesclient.h"
24#include "utils/qthelpers.h"
25#include <QtDebug>
26#include <stack>
27#ifndef NOGUI
28    #include <QMessageBox>
29#endif
30
31ContractionHierarchiesClient::ContractionHierarchiesClient()
32{
33    m_heapForward = NULL;
34    m_heapBackward = NULL;
35}
36
37ContractionHierarchiesClient::~ContractionHierarchiesClient()
38{
39    unload();
40}
41
42
43QString ContractionHierarchiesClient::GetName()
44{
45    return "Contraction Hierarchies";
46}
47
48void ContractionHierarchiesClient::SetInputDirectory( const QString& dir )
49{
50    m_directory = dir;
51}
52
53void ContractionHierarchiesClient::ShowSettings()
54{
55#ifndef NOGUI
56    QMessageBox::information( NULL, "Settings", "No settings available" );
57#endif
58}
59
60void ContractionHierarchiesClient::unload()
61{
62    if ( m_heapForward != NULL )
63        delete m_heapForward;
64    m_heapForward = NULL;
65    if ( m_heapBackward != NULL )
66        delete m_heapBackward;
67    m_heapBackward = NULL;
68    m_types.clear();
69}
70
71bool ContractionHierarchiesClient::LoadData()
72{
73    QString filename = fileInDirectory( m_directory,"Contraction Hierarchies" );
74    unload();
75
76    if ( !m_graph.loadGraph( filename, 1024 * 1024 * 4 ) )
77        return false;
78
79    m_namesFile.setFileName( filename + "_names" );
80    if ( !openQFile( &m_namesFile, QIODevice::ReadOnly ) )
81        return false;
82    m_names = ( const char* ) m_namesFile.map( 0, m_namesFile.size() );
83    if ( m_names == NULL )
84        return false;
85    m_namesFile.close();
86
87    m_heapForward = new Heap( m_graph.numberOfNodes() );
88    m_heapBackward = new Heap( m_graph.numberOfNodes() );
89
90    QFile typeFile( filename + "_types" );
91    if ( !openQFile( &typeFile, QIODevice::ReadOnly ) )
92        return false;
93
94    QByteArray buffer = typeFile.readAll();
95    QString types = QString::fromUtf8( buffer.constData() );
96    m_types = types.split( ';' );
97
98    return true;
99}
100
101bool ContractionHierarchiesClient::GetRoute( double* distance, QVector< Node>* pathNodes, QVector< Edge >* pathEdges, const IGPSLookup::Result& source, const IGPSLookup::Result& target )
102{
103    m_heapForward->Clear();
104    m_heapBackward->Clear();
105
106    *distance = computeRoute( source, target, pathNodes, pathEdges );
107    if ( *distance == std::numeric_limits< int >::max() )
108        return false;
109
110    // is it shorter to drive along the edge?
111    if ( target.source == source.source && target.target == source.target && source.edgeID == target.edgeID ) {
112        EdgeIterator targetEdge = m_graph.findEdge( target.source, target.target, target.edgeID );
113        double onEdgeDistance = fabs( target.percentage - source.percentage ) * targetEdge.distance();
114        if ( onEdgeDistance < *distance ) {
115            if ( ( targetEdge.forward() && targetEdge.backward() ) || source.percentage < target.percentage ) {
116                pathNodes->clear();
117                pathEdges->clear();
118                pathNodes->push_back( source.nearestPoint );
119
120                QVector< Node > tempNodes;
121                if ( targetEdge.unpacked() )
122                    m_graph.path( targetEdge, &tempNodes, pathEdges, target.target == targetEdge.target() );
123                else
124                    pathEdges->push_back( targetEdge.description() );
125
126                if ( target.previousWayCoordinates < source.previousWayCoordinates ) {
127                    for ( unsigned pathID = target.previousWayCoordinates; pathID < source.previousWayCoordinates; pathID++ )
128                        pathNodes->push_back( tempNodes[pathID - 1] );
129                    std::reverse( pathNodes->begin() + 1, pathNodes->end() );
130                } else {
131                    for ( unsigned pathID = source.previousWayCoordinates; pathID < target.previousWayCoordinates; pathID++ )
132                        pathNodes->push_back( tempNodes[pathID - 1] );
133                }
134
135                pathNodes->push_back( target.nearestPoint );
136                pathEdges->front().length = pathNodes->size() - 1;
137                *distance = onEdgeDistance;
138            }
139        }
140    }
141
142    *distance /= 10;
143    return true;
144}
145
146bool ContractionHierarchiesClient::GetName( QString* result, unsigned name )
147{
148    *result = QString::fromUtf8( m_names + name );
149    return true;
150}
151
152bool ContractionHierarchiesClient::GetNames( QVector< QString >* result, QVector< unsigned > names )
153{
154    result->resize( names.size() );
155    for ( int i = 0; i < names.size(); i++ )
156        ( *result )[i] = QString::fromUtf8( m_names + names[i] );
157    return true;
158}
159
160bool ContractionHierarchiesClient::GetType( QString* result, unsigned type )
161{
162    *result = m_types[type];
163    return true;
164}
165
166bool ContractionHierarchiesClient::GetTypes( QVector< QString >* result, QVector< unsigned > types )
167{
168    result->resize( types.size() );
169    for ( int i = 0; i < types.size(); i++ )
170        ( *result )[i] = m_types[types[i]];
171    return true;
172}
173
174template< class EdgeAllowed, class StallEdgeAllowed >
175void ContractionHierarchiesClient::computeStep( Heap* heapForward, Heap* heapBackward, const EdgeAllowed& edgeAllowed, const StallEdgeAllowed& stallEdgeAllowed, NodeIterator* middle, int* targetDistance ) {
176
177    const NodeIterator node = heapForward->DeleteMin();
178    const int distance = heapForward->GetKey( node );
179
180    if ( heapForward->GetData( node ).stalled )
181        return;
182
183    if ( heapBackward->WasInserted( node ) && !heapBackward->GetData( node ).stalled ) {
184        const int newDistance = heapBackward->GetKey( node ) + distance;
185        if ( newDistance < *targetDistance ) {
186            *middle = node;
187            *targetDistance = newDistance;
188        }
189    }
190
191    if ( distance > *targetDistance ) {
192        heapForward->DeleteAll();
193        return;
194    }
195    for ( EdgeIterator edge = m_graph.edges( node ); edge.hasEdgesLeft(); ) {
196        m_graph.unpackNextEdge( &edge );
197        const NodeIterator to = edge.target();
198        const int edgeWeight = edge.distance();
199        assert( edgeWeight > 0 );
200        const int toDistance = distance + edgeWeight;
201
202        if ( stallEdgeAllowed( edge.forward(), edge.backward() ) && heapForward->WasInserted( to ) ) {
203            const int shorterDistance = heapForward->GetKey( to ) + edgeWeight;
204            if ( shorterDistance < distance ) {
205                //perform a bfs starting at node
206                //only insert nodes when a sub-optimal path can be proven
207                //insert node into the stall queue
208                heapForward->GetKey( node ) = shorterDistance;
209                heapForward->GetData( node ).stalled = true;
210                m_stallQueue.push( node );
211
212                while ( !m_stallQueue.empty() ) {
213                    //get node from the queue
214                    const NodeIterator stallNode = m_stallQueue.front();
215                    m_stallQueue.pop();
216                    const int stallDistance = heapForward->GetKey( stallNode );
217
218                    //iterate over outgoing edges
219                    for ( EdgeIterator stallEdge = m_graph.edges( stallNode ); stallEdge.hasEdgesLeft(); ) {
220                        m_graph.unpackNextEdge( &stallEdge );
221                        //is edge outgoing/reached/stalled?
222                        if ( !edgeAllowed( stallEdge.forward(), stallEdge.backward() ) )
223                            continue;
224                        const NodeIterator stallTo = stallEdge.target();
225                        if ( !heapForward->WasInserted( stallTo ) )
226                            continue;
227                        if ( heapForward->GetData( stallTo ).stalled == true )
228                            continue;
229
230                        const int stallToDistance = stallDistance + stallEdge.distance();
231                        //sub-optimal path found -> insert stallTo
232                        if ( stallToDistance < heapForward->GetKey( stallTo ) ) {
233                            if ( heapForward->WasRemoved( stallTo ) )
234                                heapForward->GetKey( stallTo ) = stallToDistance;
235                            else
236                                heapForward->DecreaseKey( stallTo, stallToDistance );
237
238                            m_stallQueue.push( stallTo );
239                            heapForward->GetData( stallTo ).stalled = true;
240                        }
241                    }
242                }
243                break;
244            }
245        }
246
247        if ( edgeAllowed( edge.forward(), edge.backward() ) ) {
248            //New Node discovered -> Add to Heap + Node Info Storage
249            if ( !heapForward->WasInserted( to ) )
250                heapForward->Insert( to, toDistance, node );
251
252            //Found a shorter Path -> Update distance
253            else if ( toDistance <= heapForward->GetKey( to ) ) {
254                heapForward->DecreaseKey( to, toDistance );
255                //new parent + unstall
256                heapForward->GetData( to ).parent = node;
257                heapForward->GetData( to ).stalled = false;
258            }
259        }
260    }
261}
262
263int ContractionHierarchiesClient::computeRoute( const IGPSLookup::Result& source, const IGPSLookup::Result& target, QVector< Node>* pathNodes, QVector< Edge >* pathEdges ) {
264    EdgeIterator sourceEdge = m_graph.findEdge( source.source, source.target, source.edgeID );
265    unsigned sourceWeight = sourceEdge.distance();
266    EdgeIterator targetEdge = m_graph.findEdge( target.source, target.target, target.edgeID );
267    unsigned targetWeight = targetEdge.distance();
268
269    //insert source into heap
270    m_heapForward->Insert( source.target, sourceWeight - sourceWeight * source.percentage, source.target );
271    if ( sourceEdge.backward() && sourceEdge.forward() && source.target != source.source )
272        m_heapForward->Insert( source.source, sourceWeight * source.percentage, source.source );
273
274    //insert target into heap
275    m_heapBackward->Insert( target.source, targetWeight * target.percentage, target.source );
276    if ( targetEdge.backward() && targetEdge.forward() && target.target != target.source )
277        m_heapBackward->Insert( target.target, targetWeight - targetWeight * target.percentage, target.target );
278
279    int targetDistance = std::numeric_limits< int >::max();
280    NodeIterator middle = ( NodeIterator ) 0;
281    AllowForwardEdge forward;
282    AllowBackwardEdge backward;
283
284    while ( m_heapForward->Size() + m_heapBackward->Size() > 0 ) {
285
286        if ( m_heapForward->Size() > 0 )
287            computeStep( m_heapForward, m_heapBackward, forward, backward, &middle, &targetDistance );
288
289        if ( m_heapBackward->Size() > 0 )
290            computeStep( m_heapBackward, m_heapForward, backward, forward, &middle, &targetDistance );
291
292    }
293
294    if ( targetDistance == std::numeric_limits< int >::max() )
295        return std::numeric_limits< int >::max();
296
297    std::stack< NodeIterator > stack;
298    NodeIterator pathNode = middle;
299    while ( true ) {
300        NodeIterator parent = m_heapForward->GetData( pathNode ).parent;
301        stack.push( pathNode );
302        if ( parent == pathNode )
303            break;
304        pathNode = parent;
305    }
306
307    pathNodes->push_back( source.nearestPoint );
308    bool reverseSourceDescription = pathNode != source.target;
309    if ( source.source == source.target && sourceEdge.backward() && sourceEdge.forward() && source.percentage < 0.5 )
310        reverseSourceDescription = !reverseSourceDescription;
311    if ( sourceEdge.unpacked() ) {
312        bool unpackSourceForward = source.target != sourceEdge.target() ? reverseSourceDescription : !reverseSourceDescription;
313        m_graph.path( sourceEdge, pathNodes, pathEdges, unpackSourceForward );
314        if ( reverseSourceDescription ) {
315            pathNodes->remove( 1, pathNodes->size() - 1 - source.previousWayCoordinates );
316        } else {
317            pathNodes->remove( 1, source.previousWayCoordinates - 1 );
318        }
319    } else {
320        pathNodes->push_back( m_graph.node( pathNode ) );
321        pathEdges->push_back( sourceEdge.description() );
322    }
323    pathEdges->front().length = pathNodes->size() - 1;
324
325    while ( stack.size() > 1 ) {
326        const NodeIterator node = stack.top();
327        stack.pop();
328        unpackEdge( node, stack.top(), true, pathNodes, pathEdges );
329    }
330
331    pathNode = middle;
332    while ( true ) {
333        NodeIterator parent = m_heapBackward->GetData( pathNode ).parent;
334        if ( parent == pathNode )
335            break;
336        unpackEdge( parent, pathNode, false, pathNodes, pathEdges );
337        pathNode = parent;
338    }
339
340    int begin = pathNodes->size();
341    bool reverseTargetDescription = pathNode != target.source;
342    if ( target.source == target.target && targetEdge.backward() && targetEdge.forward() && target.percentage > 0.5 )
343        reverseSourceDescription = !reverseSourceDescription;
344    if ( targetEdge.unpacked() ) {
345        bool unpackTargetForward = target.target != targetEdge.target() ? reverseTargetDescription : !reverseTargetDescription;
346        m_graph.path( targetEdge, pathNodes, pathEdges, unpackTargetForward );
347        if ( reverseTargetDescription ) {
348            pathNodes->resize( pathNodes->size() - target.previousWayCoordinates );
349        } else {
350            pathNodes->resize( begin + target.previousWayCoordinates - 1 );
351        }
352    } else {
353        pathEdges->push_back( targetEdge.description() );
354    }
355    pathNodes->push_back( target.nearestPoint );
356    pathEdges->back().length = pathNodes->size() - begin;
357
358    return targetDistance;
359}
360
361bool ContractionHierarchiesClient::unpackEdge( const NodeIterator source, const NodeIterator target, bool forward, QVector< Node >* pathNodes, QVector< Edge >* pathEdges ) {
362    EdgeIterator shortestEdge;
363
364    unsigned distance = std::numeric_limits< unsigned >::max();
365    for ( EdgeIterator edge = m_graph.edges( source ); edge.hasEdgesLeft(); ) {
366        m_graph.unpackNextEdge( &edge );
367        if ( edge.target() != target )
368            continue;
369        if ( forward && !edge.forward() )
370            continue;
371        if ( !forward && !edge.backward() )
372            continue;
373        if ( edge.distance() > distance )
374            continue;
375        distance = edge.distance();
376        shortestEdge = edge;
377    }
378
379    if ( shortestEdge.unpacked() ) {
380        m_graph.path( shortestEdge, pathNodes, pathEdges, forward );
381        return true;
382    }
383
384    if ( !shortestEdge.shortcut() ) {
385        pathEdges->push_back( shortestEdge.description() );
386        if ( forward )
387            pathNodes->push_back( m_graph.node( target ).coordinate );
388        else
389            pathNodes->push_back( m_graph.node( source ).coordinate );
390        return true;
391    }
392
393    const NodeIterator middle = shortestEdge.middle();
394
395    if ( forward ) {
396        unpackEdge( middle, source, false, pathNodes, pathEdges );
397        unpackEdge( middle, target, true, pathNodes, pathEdges );
398        return true;
399    } else {
400        unpackEdge( middle, target, false, pathNodes, pathEdges );
401        unpackEdge( middle, source, true, pathNodes, pathEdges );
402        return true;
403    }
404}
405
406Q_EXPORT_PLUGIN2( contractionhierarchiesclient, ContractionHierarchiesClient )
407
408

Archive Download this file

Branches:
master



interactive