From 6e7bb505c1e5e6fd9b95baca76874c1177d296da Mon Sep 17 00:00:00 2001 From: DennisOSRM Date: Fri, 30 Dec 2011 22:11:48 +0100 Subject: [PATCH] Edge-based via routes working again. Beware, it may be buggy or slow or even both. It does not yet work with via points on one-way streets. --- DataStructures/SearchEngine.h | 741 +++++++++++++++++++--------------- Plugins/RawRouteData.h | 4 - Plugins/ViaRoutePlugin.h | 20 +- 3 files changed, 427 insertions(+), 338 deletions(-) diff --git a/DataStructures/SearchEngine.h b/DataStructures/SearchEngine.h index e78b7e698..38c2267fd 100644 --- a/DataStructures/SearchEngine.h +++ b/DataStructures/SearchEngine.h @@ -33,384 +33,483 @@ or see http://www.gnu.org/licenses/agpl.txt. #include "../typedefs.h" struct _HeapData { - NodeID parent; - _HeapData( NodeID p ) : parent(p) { } -}; - -struct _ViaHeapData { - NodeID parent; - NodeID sourceNode; - _ViaHeapData(NodeID id) :parent(id), sourceNode(id) { } + NodeID parent; + _HeapData( NodeID p ) : parent(p) { } }; typedef boost::thread_specific_ptr > > HeapPtr; -typedef boost::thread_specific_ptr > > ViaHeapPtr; template class SearchEngine { private: - const GraphT * _graph; - NodeInformationHelpDesk * nodeHelpDesk; - std::vector * _names; - static HeapPtr _forwardHeap; - static HeapPtr _backwardHeap; - static ViaHeapPtr _forwardViaHeap; - static ViaHeapPtr _backwardViaHeap; - inline double absDouble(double input) { if(input < 0) return input*(-1); else return input;} + const GraphT * _graph; + NodeInformationHelpDesk * nodeHelpDesk; + std::vector * _names; + static HeapPtr _forwardHeap; + static HeapPtr _backwardHeap; + static HeapPtr _forwardHeap2; + static HeapPtr _backwardHeap2; + inline double absDouble(double input) { if(input < 0) return input*(-1); else return input;} public: - SearchEngine(GraphT * g, NodeInformationHelpDesk * nh, std::vector * n = new std::vector()) : _graph(g), nodeHelpDesk(nh), _names(n) {} - ~SearchEngine() {} + SearchEngine(GraphT * g, NodeInformationHelpDesk * nh, std::vector * n = new std::vector()) : _graph(g), nodeHelpDesk(nh), _names(n) {} + ~SearchEngine() {} - inline const void GetCoordinatesForNodeID(NodeID id, _Coordinate& result) const { - result.lat = nodeHelpDesk->getLatitudeOfNode(id); - result.lon = nodeHelpDesk->getLongitudeOfNode(id); - } + inline const void GetCoordinatesForNodeID(NodeID id, _Coordinate& result) const { + result.lat = nodeHelpDesk->getLatitudeOfNode(id); + result.lon = nodeHelpDesk->getLongitudeOfNode(id); + } - inline void InitializeThreadLocalStorageIfNecessary() { - if(!_forwardHeap.get()) { - _forwardHeap.reset(new BinaryHeap< NodeID, NodeID, int, _HeapData, UnorderedMapStorage >(nodeHelpDesk->getNumberOfNodes())); - } - else - _forwardHeap->Clear(); + inline void InitializeThreadLocalStorageIfNecessary() { + if(!_forwardHeap.get()) { + _forwardHeap.reset(new BinaryHeap< NodeID, NodeID, int, _HeapData, UnorderedMapStorage >(nodeHelpDesk->getNumberOfNodes())); + } + else + _forwardHeap->Clear(); - if(!_backwardHeap.get()) { - _backwardHeap.reset(new BinaryHeap< NodeID, NodeID, int, _HeapData, UnorderedMapStorage >(nodeHelpDesk->getNumberOfNodes())); - } - else - _backwardHeap->Clear(); - } + if(!_backwardHeap.get()) { + _backwardHeap.reset(new BinaryHeap< NodeID, NodeID, int, _HeapData, UnorderedMapStorage >(nodeHelpDesk->getNumberOfNodes())); + } + else + _backwardHeap->Clear(); + } - inline void InitializeThreadLocalViaStorageIfNecessary() { - if(!_forwardViaHeap.get()) { - _forwardViaHeap.reset(new BinaryHeap< NodeID, NodeID, int, _ViaHeapData, UnorderedMapStorage >(nodeHelpDesk->getNumberOfNodes())); - } - else - _forwardViaHeap->Clear(); + inline void InitializeThreadLocalViaStorageIfNecessary() { + if(!_forwardHeap2.get()) { + _forwardHeap2.reset(new BinaryHeap< NodeID, NodeID, int, _HeapData, UnorderedMapStorage >(nodeHelpDesk->getNumberOfNodes())); + } + else + _forwardHeap2->Clear(); - if(!_backwardViaHeap.get()) { - _backwardViaHeap.reset(new BinaryHeap< NodeID, NodeID, int, _ViaHeapData, UnorderedMapStorage >(nodeHelpDesk->getNumberOfNodes())); - } - else - _backwardViaHeap->Clear(); - } + if(!_backwardHeap2.get()) { + _backwardHeap2.reset(new BinaryHeap< NodeID, NodeID, int, _HeapData, UnorderedMapStorage >(nodeHelpDesk->getNumberOfNodes())); + } + else + _backwardHeap2->Clear(); + } - int ComputeViaRoute(std::vector & phantomNodesVector, std::vector<_PathData> & unpackedPath) { - BOOST_FOREACH(PhantomNodes & phantomNodePair, phantomNodesVector) { - if(!phantomNodePair.AtLeastOnePhantomNodeIsUINTMAX()) - return INT_MAX; - } + template + void _RemoveConsecutiveDuplicatesFromContainer(ContainerT & packedPath) { + //remove consecutive duplicates + typename ContainerT::iterator it; + // using default comparison: + it = std::unique(packedPath.begin(), packedPath.end()); + packedPath.resize(it - packedPath.begin()); + } - int distance1 = 0; - int distance2 = 0; + int ComputeViaRoute(std::vector & phantomNodesVector, std::vector<_PathData> & unpackedPath) { + BOOST_FOREACH(PhantomNodes & phantomNodePair, phantomNodesVector) { + if(!phantomNodePair.AtLeastOnePhantomNodeIsUINTMAX()) + return INT_MAX; + } + int distance1 = 0; + int distance2 = 0; - std::deque packedPath1; - std::deque packedPath2; + bool searchFrom1stStartNode(true); + bool searchFrom2ndStartNode(true); + NodeID middle1 = ( NodeID ) UINT_MAX; + NodeID middle2 = ( NodeID ) UINT_MAX; + std::deque packedPath1; + std::deque packedPath2; + //Get distance to next pair of target nodes. + BOOST_FOREACH(PhantomNodes & phantomNodePair, phantomNodesVector) { + InitializeThreadLocalStorageIfNecessary(); + InitializeThreadLocalViaStorageIfNecessary(); - //Get distance to next pair of target nodes. - BOOST_FOREACH(PhantomNodes & phantomNodePair, phantomNodesVector) { - InitializeThreadLocalViaStorageIfNecessary(); - NodeID middle1 = ( NodeID ) UINT_MAX; - NodeID middle2 = ( NodeID ) UINT_MAX; + int _localUpperbound1 = INT_MAX; + int _localUpperbound2 = INT_MAX; - int _upperbound1 = INT_MAX; - int _upperbound2 = INT_MAX; + _forwardHeap->Clear(); + _forwardHeap2->Clear(); + //insert new starting nodes into forward heap, adjusted by previous distances. + if(searchFrom1stStartNode) { + _forwardHeap->Insert(phantomNodePair.startPhantom.edgeBasedNode, distance1-phantomNodePair.startPhantom.weight1, phantomNodePair.startPhantom.edgeBasedNode); + _forwardHeap2->Insert(phantomNodePair.startPhantom.edgeBasedNode, distance2-phantomNodePair.startPhantom.weight1, phantomNodePair.startPhantom.edgeBasedNode); +// INFO("1,2)forw insert " << phantomNodePair.startPhantom.edgeBasedNode << " with weight " << phantomNodePair.startPhantom.weight1); +// } else { +// INFO("Skipping first start node"); + } + if(phantomNodePair.startPhantom.isBidirected() && searchFrom2ndStartNode) { + _forwardHeap->Insert(phantomNodePair.startPhantom.edgeBasedNode+1, distance1-phantomNodePair.startPhantom.weight2, phantomNodePair.startPhantom.edgeBasedNode+1); + _forwardHeap2->Insert(phantomNodePair.startPhantom.edgeBasedNode+1, distance2-phantomNodePair.startPhantom.weight2, phantomNodePair.startPhantom.edgeBasedNode+1); +// INFO("1)forw insert " << phantomNodePair.startPhantom.edgeBasedNode+1 << " with weight " << distance1-phantomNodePair.startPhantom.weight1); +// INFO("2)forw insert " << phantomNodePair.startPhantom.edgeBasedNode+1 << " with weight " << distance2-phantomNodePair.startPhantom.weight1); +// } else if(!searchFrom2ndStartNode) { +// INFO("Skipping second start node"); + } - assert(INT_MAX != distance1); + _backwardHeap->Clear(); + _backwardHeap2->Clear(); + //insert new backward nodes into backward heap, unadjusted. + _backwardHeap->Insert(phantomNodePair.targetPhantom.edgeBasedNode, phantomNodePair.targetPhantom.weight1, phantomNodePair.targetPhantom.edgeBasedNode); +// INFO("1)back insert " << phantomNodePair.targetPhantom.edgeBasedNode << " with weight " << phantomNodePair.targetPhantom.weight1); + if(phantomNodePair.targetPhantom.isBidirected() ) { +// INFO("2)back insert " << phantomNodePair.targetPhantom.edgeBasedNode+1 << " with weight " << phantomNodePair.targetPhantom.weight2); + _backwardHeap2->Insert(phantomNodePair.targetPhantom.edgeBasedNode+1, phantomNodePair.targetPhantom.weight2, phantomNodePair.targetPhantom.edgeBasedNode+1); + } + int offset = (phantomNodePair.startPhantom.isBidirected() ? std::max(phantomNodePair.startPhantom.weight1, phantomNodePair.startPhantom.weight2) : phantomNodePair.startPhantom.weight1) ; + offset += (phantomNodePair.targetPhantom.isBidirected() ? std::max(phantomNodePair.targetPhantom.weight1, phantomNodePair.targetPhantom.weight2) : phantomNodePair.targetPhantom.weight1) ; - _forwardViaHeap->Clear(); - //insert new starting nodes into forward heap, adjusted by previous distances. - _forwardViaHeap->Insert(phantomNodePair.startPhantom.edgeBasedNode, distance1-phantomNodePair.startPhantom.weight1, phantomNodePair.startPhantom.edgeBasedNode); - if(phantomNodePair.startPhantom.isBidirected() ) { - _forwardViaHeap->Insert(phantomNodePair.startPhantom.edgeBasedNode+1, distance1-phantomNodePair.startPhantom.weight2, phantomNodePair.startPhantom.edgeBasedNode+1); - } + //run two-Target Dijkstra routing step. + while(_forwardHeap->Size() + _backwardHeap->Size() > 0){ + if(_forwardHeap->Size() > 0){ + _RoutingStep(_forwardHeap, _backwardHeap, true, &middle1, &_localUpperbound1, 2*offset); + } + if(_backwardHeap->Size() > 0){ + _RoutingStep(_backwardHeap, _forwardHeap, false, &middle1, &_localUpperbound1, 2*offset); + } + } + if(_backwardHeap2->Size() > 0) { + while(_forwardHeap2->Size() + _backwardHeap2->Size() > 0){ + if(_forwardHeap2->Size() > 0){ + _RoutingStep(_forwardHeap2, _backwardHeap2, true, &middle2, &_localUpperbound2, 2*offset); + } + if(_backwardHeap2->Size() > 0){ + _RoutingStep(_backwardHeap2, _forwardHeap2, false, &middle2, &_localUpperbound2, 2*offset); + } + } + } +// INFO("upperbound1: " << _localUpperbound1 << ", distance1: " << distance1); +// INFO("upperbound2: " << _localUpperbound2 << ", distance2: " << distance2); - _backwardViaHeap->Clear(); - //insert new backward nodes into backward heap, unadjusted. - _backwardViaHeap->Insert(phantomNodePair.targetPhantom.edgeBasedNode, phantomNodePair.targetPhantom.weight1, phantomNodePair.targetPhantom.edgeBasedNode); - if(phantomNodePair.targetPhantom.isBidirected() ) { - _backwardViaHeap->Insert(phantomNodePair.targetPhantom.edgeBasedNode+1, phantomNodePair.targetPhantom.weight2, phantomNodePair.targetPhantom.edgeBasedNode+1); - } - int offset = (phantomNodePair.startPhantom.isBidirected() ? std::max(phantomNodePair.startPhantom.weight1, phantomNodePair.startPhantom.weight2) : phantomNodePair.startPhantom.weight1) ; - offset += (phantomNodePair.targetPhantom.isBidirected() ? std::max(phantomNodePair.targetPhantom.weight1, phantomNodePair.targetPhantom.weight2) : phantomNodePair.targetPhantom.weight1) ; + //No path found for both target nodes? + if(INT_MAX == _localUpperbound1 && INT_MAX == _localUpperbound2) { + return INT_MAX; + } + if(UINT_MAX == middle1) { + searchFrom1stStartNode = false; +// INFO("Next Search will not start from 1st"); + } else { +// INFO("Next Search will start from 1st"); + searchFrom1stStartNode = true; + } + if(UINT_MAX == middle2) { + searchFrom2ndStartNode = false; +// INFO("Next Search will not start from 2nd"); + } else { + searchFrom2ndStartNode = true; +// INFO("Next Search will start from 2nd"); + } + //Add distance of segments to current sums + if(INT_MAX == distance1 || INT_MAX == _localUpperbound1) { +// INFO("setting distance1 = 0"); + distance1 = 0; + } +// distance1 += _localUpperbound1; + if(INT_MAX == distance2 || INT_MAX == _localUpperbound2) { +// INFO("Setting distance2 = 0"); + distance2 = 0; + } +// distance2 += _localUpperbound2; - //run two-Target Dijkstra routing step. - //TODO +// INFO("distance1: " << distance1 << ", distance2: " << distance2); - //No path found for both target nodes? - if(INT_MAX == _upperbound1 && INT_MAX == _upperbound2) { - return INT_MAX; - } - - //Add distance of segments to current sums - if(INT_MAX == distance1 || INT_MAX == _upperbound1) - distance1 = 0; - distance1 += _upperbound1; - if(INT_MAX == distance2 || INT_MAX == _upperbound2) - distance2 = 0; - distance2 += _upperbound2; + //Was at most one of the two paths not found? + assert(!(INT_MAX == distance1 && INT_MAX == distance2)); - if(INT_MAX == distance1) - packedPath1.clear(); - if(INT_MAX == distance2) - packedPath2.clear(); +// INFO("middle1: " << middle1); - //Was one of the previous segments empty? - bool empty1 = (INT_MAX != distance1 && 0 == packedPath1.size() && 0 != packedPath2.size()); - bool empty2 = (INT_MAX != distance2 && 0 == packedPath2.size() && 0 != packedPath1.size()); - assert(!(empty1 && empty2)); - if(empty1) - packedPath1.insert(packedPath1.begin(), packedPath2.begin(), packedPath2.end()); - if(empty2) - packedPath2.insert(packedPath2.begin(), packedPath1.begin(), packedPath2.end()); - - //set packed paths to current paths. - NodeID pathNode = middle1; - std::deque temporaryPackedPath; - while(phantomNodePair.startPhantom.edgeBasedNode != pathNode && (!phantomNodePair.startPhantom.isBidirected() || phantomNodePair.startPhantom.edgeBasedNode+1 != pathNode) ) { - pathNode = _forwardHeap->GetData(pathNode).parent; - temporaryPackedPath.push_front(pathNode); - } - temporaryPackedPath.push_back(middle1); - pathNode = middle1; - while(phantomNodePair.targetPhantom.edgeBasedNode != pathNode && (!phantomNodePair.targetPhantom.isBidirected() || phantomNodePair.targetPhantom.edgeBasedNode+1 != pathNode)) { - pathNode = _backwardHeap->GetData(pathNode).parent; - temporaryPackedPath.push_back(pathNode); - } - packedPath1.insert(packedPath1.end(), temporaryPackedPath.begin(), temporaryPackedPath.end()); - //TODO: add via node turn instruction + //Unpack paths if they exist + std::deque temporaryPackedPath1; + std::deque temporaryPackedPath2; + if(INT_MAX != _localUpperbound1) { + _RetrievePackedPathFromHeap(_forwardHeap, _backwardHeap, phantomNodePair, middle1, temporaryPackedPath1); + } +// INFO("middle2: " << middle2); - pathNode = middle2; - temporaryPackedPath.clear(); - while(phantomNodePair.startPhantom.edgeBasedNode != pathNode && (!phantomNodePair.startPhantom.isBidirected() || phantomNodePair.startPhantom.edgeBasedNode+1 != pathNode) ) { - pathNode = _forwardHeap->GetData(pathNode).parent; - temporaryPackedPath.push_front(pathNode); - } - temporaryPackedPath.push_back(middle2); - pathNode = middle2; - while(phantomNodePair.targetPhantom.edgeBasedNode != pathNode && (!phantomNodePair.targetPhantom.isBidirected() || phantomNodePair.targetPhantom.edgeBasedNode+1 != pathNode)) { - pathNode = _backwardHeap->GetData(pathNode).parent; - temporaryPackedPath.push_back(pathNode); - } - //TODO: add via node turn instruction - packedPath2.insert(packedPath2.end(), temporaryPackedPath.begin(), temporaryPackedPath.end()); - } - - if(distance1 < distance2) { - _UnpackPath(packedPath1, unpackedPath); - } else { - _UnpackPath(packedPath2, unpackedPath); - } + if(INT_MAX != _localUpperbound2) { + _RetrievePackedPathFromHeap(_forwardHeap2, _backwardHeap2, phantomNodePair, middle2, temporaryPackedPath2); + } - return std::min(distance1, distance2); - } + //if one of the paths was not found, replace it with the other one. + if(0 == temporaryPackedPath1.size()) { + temporaryPackedPath1.insert(temporaryPackedPath1.end(), temporaryPackedPath2.begin(), temporaryPackedPath2.end()); + _localUpperbound1 = _localUpperbound2; + } + if(0 ==temporaryPackedPath2.size()) { + temporaryPackedPath2.insert(temporaryPackedPath2.end(), temporaryPackedPath1.begin(), temporaryPackedPath1.end()); + _localUpperbound2 = _localUpperbound1; + } - int ComputeRoute(PhantomNodes & phantomNodes, std::vector<_PathData> & path) { - int _upperbound = INT_MAX; - if(!phantomNodes.AtLeastOnePhantomNodeIsUINTMAX()) - return _upperbound; + assert(0 < temporaryPackedPath1.size() && 0 < temporaryPackedPath2.size()); - InitializeThreadLocalStorageIfNecessary(); - NodeID middle = ( NodeID ) UINT_MAX; - //insert start and/or target node of start edge - _forwardHeap->Insert(phantomNodes.startPhantom.edgeBasedNode, -phantomNodes.startPhantom.weight1, phantomNodes.startPhantom.edgeBasedNode); -// INFO("a) forw insert " << phantomNodes.startPhantom.edgeBasedNode << ", weight: " << -phantomNodes.startPhantom.weight1); - if(phantomNodes.startPhantom.isBidirected() ) { -// INFO("b) forw insert " << phantomNodes.startPhantom.edgeBasedNode+1 << ", weight: " << -phantomNodes.startPhantom.weight2); - _forwardHeap->Insert(phantomNodes.startPhantom.edgeBasedNode+1, -phantomNodes.startPhantom.weight2, phantomNodes.startPhantom.edgeBasedNode+1); - } - //insert start and/or target node of target edge id - _backwardHeap->Insert(phantomNodes.targetPhantom.edgeBasedNode, phantomNodes.targetPhantom.weight1, phantomNodes.targetPhantom.edgeBasedNode); -// INFO("c) back insert " << phantomNodes.targetPhantom.edgeBasedNode << ", weight: " << phantomNodes.targetPhantom.weight1); - if(phantomNodes.targetPhantom.isBidirected() ) { - _backwardHeap->Insert(phantomNodes.targetPhantom.edgeBasedNode+1, phantomNodes.targetPhantom.weight2, phantomNodes.targetPhantom.edgeBasedNode+1); -// INFO("d) back insert " << phantomNodes.targetPhantom.edgeBasedNode+1 << ", weight: " << phantomNodes.targetPhantom.weight2); - } - int offset = (phantomNodes.startPhantom.isBidirected() ? std::max(phantomNodes.startPhantom.weight1, phantomNodes.startPhantom.weight2) : phantomNodes.startPhantom.weight1) ; - offset += (phantomNodes.targetPhantom.isBidirected() ? std::max(phantomNodes.targetPhantom.weight1, phantomNodes.targetPhantom.weight2) : phantomNodes.targetPhantom.weight1) ; + //Plug paths together, s.t. end of packed path is begin of temporary packed path + if(0 < packedPath1.size() && 0 < packedPath2.size() ) { + if( *(temporaryPackedPath1.begin()) == *(temporaryPackedPath2.begin())) { + //both new route segments start with the same node, thus one of the packedPath must go. + assert( (packedPath1.size() == packedPath2.size() ) || (*(packedPath1.end()-1) != *(packedPath2.end()-1)) ); + if( *(packedPath1.end()-1) == *(temporaryPackedPath1.begin())) { + packedPath2.clear(); + packedPath2.insert(packedPath2.end(), packedPath1.begin(), packedPath1.end()); + distance2 = distance1; + } else { + packedPath1.clear(); + packedPath1.insert(packedPath1.end(), packedPath2.begin(), packedPath2.end()); + distance1 = distance2; + } + } else { + //packed paths 1 and 2 may need to switch. + if(*(packedPath1.end()-1) != *(temporaryPackedPath1.begin())) { + packedPath1.swap(packedPath2); + std::swap(distance1, distance2); + } + } + } + packedPath1.insert(packedPath1.end(), temporaryPackedPath1.begin(), temporaryPackedPath1.end()); + packedPath2.insert(packedPath2.end(), temporaryPackedPath2.begin(), temporaryPackedPath2.end()); - while(_forwardHeap->Size() + _backwardHeap->Size() > 0){ - if(_forwardHeap->Size() > 0){ - _RoutingStep(_forwardHeap, _backwardHeap, true, &middle, &_upperbound, 2*offset); - } - if(_backwardHeap->Size() > 0){ - _RoutingStep(_backwardHeap, _forwardHeap, false, &middle, &_upperbound, 2*offset); - } - } + distance1 += _localUpperbound1; + distance2 += _localUpperbound2; + } -// INFO("-> dist " << _upperbound); - if ( _upperbound == INT_MAX ) { - return _upperbound; - } - NodeID pathNode = middle; - deque packedPath; - while(phantomNodes.startPhantom.edgeBasedNode != pathNode && (!phantomNodes.startPhantom.isBidirected() || phantomNodes.startPhantom.edgeBasedNode+1 != pathNode) ) { - pathNode = _forwardHeap->GetData(pathNode).parent; - packedPath.push_front(pathNode); - } - packedPath.push_back(middle); - pathNode = middle; - while(phantomNodes.targetPhantom.edgeBasedNode != pathNode && (!phantomNodes.targetPhantom.isBidirected() || phantomNodes.targetPhantom.edgeBasedNode+1 != pathNode)) { - pathNode = _backwardHeap->GetData(pathNode).parent; - packedPath.push_back(pathNode); - } - _UnpackPath(packedPath, path); - return _upperbound; - } +// INFO("length path1: " << distance1); +// INFO("length path2: " << distance2); + if(distance1 <= distance2){ + //remove consecutive duplicates +// std::cout << "unclean 1: "; +// for(unsigned i = 0; i < packedPath1.size(); ++i) +// std::cout << packedPath1[i] << " "; +// std::cout << std::endl; + _RemoveConsecutiveDuplicatesFromContainer(packedPath1); +// std::cout << "cleaned 1: "; +// for(unsigned i = 0; i < packedPath1.size(); ++i) +// std::cout << packedPath1[i] << " "; +// std::cout << std::endl; + _UnpackPath(packedPath1, unpackedPath); + } else { +// std::cout << "unclean 2: "; +// for(unsigned i = 0; i < packedPath2.size(); ++i) +// std::cout << packedPath2[i] << " "; +// std::cout << std::endl; + _RemoveConsecutiveDuplicatesFromContainer(packedPath2); +// std::cout << "cleaned 2: "; +// for(unsigned i = 0; i < packedPath2.size(); ++i) +// std::cout << packedPath2[i] << " "; +// std::cout << std::endl; + _UnpackPath(packedPath2, unpackedPath); + } +// INFO("Found via route with distance " << std::min(distance1, distance2)); + return std::min(distance1, distance2); + } - inline void FindRoutingStarts(const _Coordinate & start, const _Coordinate & target, PhantomNodes & routingStarts) const { - nodeHelpDesk->FindRoutingStarts(start, target, routingStarts); - } + int ComputeRoute(PhantomNodes & phantomNodes, std::vector<_PathData> & path) { + int _upperbound = INT_MAX; + if(!phantomNodes.AtLeastOnePhantomNodeIsUINTMAX()) + return _upperbound; - inline void FindPhantomNodeForCoordinate(const _Coordinate & location, PhantomNode & result) const { - nodeHelpDesk->FindPhantomNodeForCoordinate(location, result); - } + InitializeThreadLocalStorageIfNecessary(); + NodeID middle = ( NodeID ) UINT_MAX; + //insert start and/or target node of start edge + _forwardHeap->Insert(phantomNodes.startPhantom.edgeBasedNode, -phantomNodes.startPhantom.weight1, phantomNodes.startPhantom.edgeBasedNode); + // INFO("a) forw insert " << phantomNodes.startPhantom.edgeBasedNode << ", weight: " << -phantomNodes.startPhantom.weight1); + if(phantomNodes.startPhantom.isBidirected() ) { + // INFO("b) forw insert " << phantomNodes.startPhantom.edgeBasedNode+1 << ", weight: " << -phantomNodes.startPhantom.weight2); + _forwardHeap->Insert(phantomNodes.startPhantom.edgeBasedNode+1, -phantomNodes.startPhantom.weight2, phantomNodes.startPhantom.edgeBasedNode+1); + } + //insert start and/or target node of target edge id + _backwardHeap->Insert(phantomNodes.targetPhantom.edgeBasedNode, phantomNodes.targetPhantom.weight1, phantomNodes.targetPhantom.edgeBasedNode); + // INFO("c) back insert " << phantomNodes.targetPhantom.edgeBasedNode << ", weight: " << phantomNodes.targetPhantom.weight1); + if(phantomNodes.targetPhantom.isBidirected() ) { + _backwardHeap->Insert(phantomNodes.targetPhantom.edgeBasedNode+1, phantomNodes.targetPhantom.weight2, phantomNodes.targetPhantom.edgeBasedNode+1); + // INFO("d) back insert " << phantomNodes.targetPhantom.edgeBasedNode+1 << ", weight: " << phantomNodes.targetPhantom.weight2); + } + int offset = (phantomNodes.startPhantom.isBidirected() ? std::max(phantomNodes.startPhantom.weight1, phantomNodes.startPhantom.weight2) : phantomNodes.startPhantom.weight1) ; + offset += (phantomNodes.targetPhantom.isBidirected() ? std::max(phantomNodes.targetPhantom.weight1, phantomNodes.targetPhantom.weight2) : phantomNodes.targetPhantom.weight1) ; - inline NodeID GetNameIDForOriginDestinationNodeID(NodeID s, NodeID t) const { - if(s == t) - return 0; + while(_forwardHeap->Size() + _backwardHeap->Size() > 0){ + if(_forwardHeap->Size() > 0){ + _RoutingStep(_forwardHeap, _backwardHeap, true, &middle, &_upperbound, 2*offset); + } + if(_backwardHeap->Size() > 0){ + _RoutingStep(_backwardHeap, _forwardHeap, false, &middle, &_upperbound, 2*offset); + } + } - EdgeID e = _graph->FindEdge(s, t); - if(e == UINT_MAX) - e = _graph->FindEdge( t, s ); - if(UINT_MAX == e) { - return 0; - } - assert(e != UINT_MAX); - const EdgeData ed = _graph->GetEdgeData(e); - return ed.via; - } + // INFO("-> dist " << _upperbound); + if ( _upperbound == INT_MAX ) { + return _upperbound; + } + std::deque packedPath; + _RetrievePackedPathFromHeap(_forwardHeap, _backwardHeap, phantomNodes, middle, packedPath); +// std::cout << "0: "; +// for(unsigned i = 0; i < packedPath.size(); ++i) +// std::cout << packedPath[i] << " "; +// std::cout << std::endl; + _UnpackPath(packedPath, path); + return _upperbound; + } - inline std::string GetEscapedNameForNameID(const NodeID nameID) const { - return ((nameID >= _names->size() || nameID == 0) ? std::string("") : HTMLEntitize(_names->at(nameID))); - } + inline void FindRoutingStarts(const _Coordinate & start, const _Coordinate & target, PhantomNodes & routingStarts) const { + nodeHelpDesk->FindRoutingStarts(start, target, routingStarts); + } - inline std::string GetEscapedNameForEdgeBasedEdgeID(const unsigned edgeID) const { + inline void FindPhantomNodeForCoordinate(const _Coordinate & location, PhantomNode & result) const { + nodeHelpDesk->FindPhantomNodeForCoordinate(location, result); + } - const unsigned nameID = _graph->GetEdgeData(edgeID).nameID1; - return GetEscapedNameForNameID(nameID); - } + inline NodeID GetNameIDForOriginDestinationNodeID(NodeID s, NodeID t) const { + if(s == t) + return 0; + + EdgeID e = _graph->FindEdge(s, t); + if(e == UINT_MAX) + e = _graph->FindEdge( t, s ); + if(UINT_MAX == e) { + return 0; + } + assert(e != UINT_MAX); + const EdgeData ed = _graph->GetEdgeData(e); + return ed.via; + } + + inline std::string GetEscapedNameForNameID(const NodeID nameID) const { + return ((nameID >= _names->size() || nameID == 0) ? std::string("") : HTMLEntitize(_names->at(nameID))); + } + + inline std::string GetEscapedNameForEdgeBasedEdgeID(const unsigned edgeID) const { + const unsigned nameID = _graph->GetEdgeData(edgeID).nameID1; + return GetEscapedNameForNameID(nameID); + } private: - inline void _RoutingStep(HeapPtr & _forwardHeap, HeapPtr & _backwardHeap, const bool & forwardDirection, NodeID *middle, int *_upperbound, const int edgeBasedOffset) const { - const NodeID node = _forwardHeap->DeleteMin(); - const int distance = _forwardHeap->GetKey(node); -// INFO((forwardDirection ? "[forw]" : "[back]") << " settled node " << node << " at distance " << distance); - if(_backwardHeap->WasInserted(node) ){ -// INFO((forwardDirection ? "[forw]" : "[back]") << " scanned node " << node << " in both directions"); - const int newDistance = _backwardHeap->GetKey(node) + distance; - if(newDistance < *_upperbound ){ - if(newDistance>=0 ) { -// INFO((forwardDirection ? "[forw]" : "[back]") << " -> node " << node << " is new middle at total distance " << newDistance); - *middle = node; - *_upperbound = newDistance; - } else { -// INFO((forwardDirection ? "[forw]" : "[back]") << " -> ignored " << node << " as new middle at total distance " << newDistance); - } - } - } + inline void _RetrievePackedPathFromHeap(HeapPtr & _fHeap, HeapPtr & _bHeap, PhantomNodes & phantomNodePair, const NodeID middle, std::deque& packedPath) { + NodeID pathNode = middle; + while(phantomNodePair.startPhantom.edgeBasedNode != pathNode && (!phantomNodePair.startPhantom.isBidirected() || phantomNodePair.startPhantom.edgeBasedNode+1 != pathNode) ) { + pathNode = _fHeap->GetData(pathNode).parent; + packedPath.push_front(pathNode); + } + packedPath.push_back(middle); + pathNode = middle; + while(phantomNodePair.targetPhantom.edgeBasedNode != pathNode && (!phantomNodePair.targetPhantom.isBidirected() || phantomNodePair.targetPhantom.edgeBasedNode+1 != pathNode)) { + pathNode = _bHeap->GetData(pathNode).parent; + packedPath.push_back(pathNode); + } +// std::cout << "unpacking: "; +// for(std::deque::iterator it = packedPath.begin(); it != packedPath.end(); ++it) +// std::cout << *it << " "; +// std::cout << std::endl; + } - if(distance-edgeBasedOffset > *_upperbound){ - _forwardHeap->DeleteAll(); - return; - } - for ( typename GraphT::EdgeIterator edge = _graph->BeginEdges( node ); edge < _graph->EndEdges(node); edge++ ) { - const EdgeData & data = _graph->GetEdgeData(edge); - bool backwardDirectionFlag = (!forwardDirection) ? data.forward : data.backward; - if(backwardDirectionFlag) { - const NodeID to = _graph->GetTarget(edge); - const int edgeWeight = data.distance; + inline void _RoutingStep(HeapPtr & _forwardHeap, HeapPtr & _backwardHeap, const bool & forwardDirection, NodeID *middle, int *_upperbound, const int edgeBasedOffset) const { + const NodeID node = _forwardHeap->DeleteMin(); + const int distance = _forwardHeap->GetKey(node); + // INFO((forwardDirection ? "[forw]" : "[back]") << " settled node " << node << " at distance " << distance); + if(_backwardHeap->WasInserted(node) ){ + // INFO((forwardDirection ? "[forw]" : "[back]") << " scanned node " << node << " in both directions"); + const int newDistance = _backwardHeap->GetKey(node) + distance; + if(newDistance < *_upperbound ){ + if(newDistance>=0 ) { + // INFO((forwardDirection ? "[forw]" : "[back]") << " -> node " << node << " is new middle at total distance " << newDistance); + *middle = node; + *_upperbound = newDistance; + } else { + // INFO((forwardDirection ? "[forw]" : "[back]") << " -> ignored " << node << " as new middle at total distance " << newDistance); + } + } + } - assert( edgeWeight > 0 ); + if(distance-edgeBasedOffset > *_upperbound){ + _forwardHeap->DeleteAll(); + return; + } - //Stalling - if(_forwardHeap->WasInserted( to )) { - if(_forwardHeap->GetKey( to ) + edgeWeight < distance) { - return; - } - } - } - } + for ( typename GraphT::EdgeIterator edge = _graph->BeginEdges( node ); edge < _graph->EndEdges(node); edge++ ) { + const EdgeData & data = _graph->GetEdgeData(edge); + bool backwardDirectionFlag = (!forwardDirection) ? data.forward : data.backward; + if(backwardDirectionFlag) { + const NodeID to = _graph->GetTarget(edge); + const int edgeWeight = data.distance; - for ( typename GraphT::EdgeIterator edge = _graph->BeginEdges( node ); edge < _graph->EndEdges(node); edge++ ) { - const EdgeData & data = _graph->GetEdgeData(edge); - bool forwardDirectionFlag = (forwardDirection ? data.forward : data.backward ); - if(forwardDirectionFlag) { + assert( edgeWeight > 0 ); - const NodeID to = _graph->GetTarget(edge); - const int edgeWeight = data.distance; + //Stalling + if(_forwardHeap->WasInserted( to )) { + if(_forwardHeap->GetKey( to ) + edgeWeight < distance) { + return; + } + } + } + } - assert( edgeWeight > 0 ); - const int toDistance = distance + edgeWeight; + for ( typename GraphT::EdgeIterator edge = _graph->BeginEdges( node ); edge < _graph->EndEdges(node); edge++ ) { + const EdgeData & data = _graph->GetEdgeData(edge); + bool forwardDirectionFlag = (forwardDirection ? data.forward : data.backward ); + if(forwardDirectionFlag) { - //New Node discovered -> Add to Heap + Node Info Storage - if ( !_forwardHeap->WasInserted( to ) ) { -// INFO((forwardDirection ? "[forw]" : "[back]") << " scanning edge (" << node << "," << to << ") with distance " << toDistance << ", edge length: " << data.distance); - _forwardHeap->Insert( to, toDistance, node ); - } - //Found a shorter Path -> Update distance - else if ( toDistance < _forwardHeap->GetKey( to ) ) { -// INFO((forwardDirection ? "[forw]" : "[back]") << " decrease and scanning edge (" << node << "," << to << ") from " << _forwardHeap->GetKey(to) << "to " << toDistance << ", edge length: " << data.distance); - _forwardHeap->GetData( to ).parent = node; - _forwardHeap->DecreaseKey( to, toDistance ); - //new parent - } - } - } - } + const NodeID to = _graph->GetTarget(edge); + const int edgeWeight = data.distance; - inline void _UnpackPath(std::deque & packedPath, std::vector<_PathData> & unpackedPath) const { - const unsigned sizeOfPackedPath = packedPath.size(); - SimpleStack > recursionStack(sizeOfPackedPath); + assert( edgeWeight > 0 ); + const int toDistance = distance + edgeWeight; - //We have to push the path in reverse order onto the stack because it's LIFO. - for(unsigned i = sizeOfPackedPath-1; i > 0; --i){ - recursionStack.push(std::make_pair(packedPath[i-1], packedPath[i])); - } + //New Node discovered -> Add to Heap + Node Info Storage + if ( !_forwardHeap->WasInserted( to ) ) { + // INFO((forwardDirection ? "[forw]" : "[back]") << " scanning edge (" << node << "," << to << ") with distance " << toDistance << ", edge length: " << data.distance); + _forwardHeap->Insert( to, toDistance, node ); + } + //Found a shorter Path -> Update distance + else if ( toDistance < _forwardHeap->GetKey( to ) ) { + // INFO((forwardDirection ? "[forw]" : "[back]") << " decrease and scanning edge (" << node << "," << to << ") from " << _forwardHeap->GetKey(to) << "to " << toDistance << ", edge length: " << data.distance); + _forwardHeap->GetData( to ).parent = node; + _forwardHeap->DecreaseKey( to, toDistance ); + //new parent + } + } + } + } - std::pair edge; - while(!recursionStack.empty()) { - edge = recursionStack.top(); - recursionStack.pop(); + inline void _UnpackPath(std::deque & packedPath, std::vector<_PathData> & unpackedPath) const { + const unsigned sizeOfPackedPath = packedPath.size(); + SimpleStack > recursionStack(sizeOfPackedPath); - typename GraphT::EdgeIterator smallestEdge = SPECIAL_EDGEID; - int smallestWeight = INT_MAX; - for(typename GraphT::EdgeIterator eit = _graph->BeginEdges(edge.first);eit < _graph->EndEdges(edge.first);++eit){ - const int weight = _graph->GetEdgeData(eit).distance; - if(_graph->GetTarget(eit) == edge.second && weight < smallestWeight && _graph->GetEdgeData(eit).forward){ - smallestEdge = eit; - smallestWeight = weight; - } - } + //We have to push the path in reverse order onto the stack because it's LIFO. + for(unsigned i = sizeOfPackedPath-1; i > 0; --i){ + recursionStack.push(std::make_pair(packedPath[i-1], packedPath[i])); + } - if(smallestEdge == SPECIAL_EDGEID){ - for(typename GraphT::EdgeIterator eit = _graph->BeginEdges(edge.second);eit < _graph->EndEdges(edge.second);++eit){ - const int weight = _graph->GetEdgeData(eit).distance; - if(_graph->GetTarget(eit) == edge.first && weight < smallestWeight && _graph->GetEdgeData(eit).backward){ - smallestEdge = eit; - smallestWeight = weight; - } - } - } + std::pair edge; + while(!recursionStack.empty()) { + edge = recursionStack.top(); + recursionStack.pop(); + // INFO("Unpacking edge (" << edge.first << "," << edge.second << ")"); - assert(smallestWeight != INT_MAX); + typename GraphT::EdgeIterator smallestEdge = SPECIAL_EDGEID; + int smallestWeight = INT_MAX; + for(typename GraphT::EdgeIterator eit = _graph->BeginEdges(edge.first);eit < _graph->EndEdges(edge.first);++eit){ + const int weight = _graph->GetEdgeData(eit).distance; + if(_graph->GetTarget(eit) == edge.second && weight < smallestWeight && _graph->GetEdgeData(eit).forward){ + smallestEdge = eit; + smallestWeight = weight; + } + } - const EdgeData& ed = _graph->GetEdgeData(smallestEdge); - if(ed.shortcut) {//unpack - const NodeID middle = ed.via; - //again, we need to this in reversed order - recursionStack.push(std::make_pair(middle, edge.second)); - recursionStack.push(std::make_pair(edge.first, middle)); - } else { - assert(!ed.shortcut); - unpackedPath.push_back(_PathData(ed.via, ed.nameID, ed.turnInstruction, ed.distance) ); - } - } - } + if(smallestEdge == SPECIAL_EDGEID){ + for(typename GraphT::EdgeIterator eit = _graph->BeginEdges(edge.second);eit < _graph->EndEdges(edge.second);++eit){ + const int weight = _graph->GetEdgeData(eit).distance; + if(_graph->GetTarget(eit) == edge.first && weight < smallestWeight && _graph->GetEdgeData(eit).backward){ + smallestEdge = eit; + smallestWeight = weight; + } + } + } + assert(smallestWeight != INT_MAX); + + const EdgeData& ed = _graph->GetEdgeData(smallestEdge); + if(ed.shortcut) {//unpack + const NodeID middle = ed.via; + //again, we need to this in reversed order + recursionStack.push(std::make_pair(middle, edge.second)); + recursionStack.push(std::make_pair(edge.first, middle)); + } else { + assert(!ed.shortcut); + unpackedPath.push_back(_PathData(ed.via, ed.nameID, ed.turnInstruction, ed.distance) ); + } + } + } }; template HeapPtr SearchEngine::_forwardHeap; template HeapPtr SearchEngine::_backwardHeap; -template ViaHeapPtr SearchEngine::_forwardViaHeap; -template ViaHeapPtr SearchEngine::_backwardViaHeap; +template HeapPtr SearchEngine::_forwardHeap2; +template HeapPtr SearchEngine::_backwardHeap2; #endif /* SEARCHENGINE_H_ */ diff --git a/Plugins/RawRouteData.h b/Plugins/RawRouteData.h index ea73048c0..8ace4070c 100644 --- a/Plugins/RawRouteData.h +++ b/Plugins/RawRouteData.h @@ -22,10 +22,6 @@ or see http://www.gnu.org/licenses/agpl.txt. #define RAWROUTEDATA_H_ struct RawRouteData { - void Resize() { - unsigned size = rawViaNodeCoordinates.size()-1; - segmentEndCoordinates.resize(size); - } std::vector< _PathData > computedRouted; std::vector< PhantomNodes > segmentEndCoordinates; std::vector< _Coordinate > rawViaNodeCoordinates; diff --git a/Plugins/ViaRoutePlugin.h b/Plugins/ViaRoutePlugin.h index 7e953b89d..8ddb4a017 100644 --- a/Plugins/ViaRoutePlugin.h +++ b/Plugins/ViaRoutePlugin.h @@ -109,7 +109,7 @@ public: } int vialat = static_cast(100000.*atof(textCoord[0].c_str())); int vialon = static_cast(100000.*atof(textCoord[1].c_str())); - INFO("[debug] via" << i << ": " << vialat << "," << vialon); +// INFO("[debug] via" << i << ": " << vialat << "," << vialon); _Coordinate viaCoord(vialat, vialon); if(false == checkCoord(viaCoord)) { reply = http::Reply::stockReply(http::Reply::badRequest); @@ -124,8 +124,6 @@ public: searchEngine->FindPhantomNodeForCoordinate( rawRoute.rawViaNodeCoordinates[i], phantomNodeVector[i]); } - rawRoute.Resize(); - unsigned distance = 0; //single route or via point routing if(0 == routeParameters.viaPoints.size()) { @@ -133,23 +131,16 @@ public: segmentPhantomNodes.startPhantom = phantomNodeVector[0]; segmentPhantomNodes.targetPhantom = phantomNodeVector[1]; distance = searchEngine->ComputeRoute(segmentPhantomNodes, rawRoute.computedRouted); - - //put segments at correct position of routes raw data - rawRoute.segmentEndCoordinates[0] = (segmentPhantomNodes); + rawRoute.segmentEndCoordinates.push_back(segmentPhantomNodes); } else { //Getting the shortest via path is a dynamic programming problem and is solved as such. - std::vector phantomNodes; for(unsigned i = 0; i < phantomNodeVector.size()-1; ++i) { PhantomNodes segmentPhantomNodes; segmentPhantomNodes.startPhantom = phantomNodeVector[i]; segmentPhantomNodes.targetPhantom = phantomNodeVector[i+1]; - phantomNodes.push_back(segmentPhantomNodes); + rawRoute.segmentEndCoordinates.push_back(segmentPhantomNodes); } - distance = searchEngine->ComputeViaRoute(phantomNodes, rawRoute.computedRouted); - - //put segments at correct position of routes raw data -// rawRoute.segmentEndCoordinates[i] = (segmentPhantomNodes); -// rawRoute.routeSegments[i] = path; + distance = searchEngine->ComputeViaRoute(rawRoute.segmentEndCoordinates, rawRoute.computedRouted); } if(INT_MAX == distance ) { DEBUG( "Error occurred, single path not found" ); @@ -199,7 +190,10 @@ public: PhantomNodes phantomNodes; phantomNodes.startPhantom = rawRoute.segmentEndCoordinates[0].startPhantom; +// INFO("Start location: " << phantomNodes.startPhantom.location) phantomNodes.targetPhantom = rawRoute.segmentEndCoordinates[rawRoute.segmentEndCoordinates.size()-1].targetPhantom; +// INFO("TargetLocation: " << phantomNodes.targetPhantom.location); +// INFO("Number of segments: " << rawRoute.segmentEndCoordinates.size()); desc->SetConfig(descriptorConfig); desc->Run(reply, rawRoute, phantomNodes, *searchEngine, distance);