diff --git a/Algorithms/ObjectToBase64.h b/Algorithms/ObjectToBase64.h index 6c6e6784f..fab161b21 100644 --- a/Algorithms/ObjectToBase64.h +++ b/Algorithms/ObjectToBase64.h @@ -43,7 +43,7 @@ typedef template static void EncodeObjectToBase64(const ToEncodeT & object, std::string& encoded) { - assert(0 == encoded.length()); + encoded.clear(); char * pointerToOriginalObject = (char *)&object; encoded = std::string(base64_t(pointerToOriginalObject), base64_t(pointerToOriginalObject+sizeof(ToEncodeT))); //replace "+" with "-" and "/" with "_" diff --git a/DataStructures/QueryEdge.h b/DataStructures/QueryEdge.h index 7f7135950..aab61f9c7 100644 --- a/DataStructures/QueryEdge.h +++ b/DataStructures/QueryEdge.h @@ -72,6 +72,4 @@ struct QueryEdge { } }; - - #endif /* QUERYEDGE_H_ */ diff --git a/DataStructures/SearchEngine.h b/DataStructures/SearchEngine.h index 83dbd5385..a02fc7e7e 100644 --- a/DataStructures/SearchEngine.h +++ b/DataStructures/SearchEngine.h @@ -30,6 +30,10 @@ or see http://www.gnu.org/licenses/agpl.txt. #include "BinaryHeap.h" #include "NodeInformationHelpDesk.h" #include "PhantomNodes.h" +#include "../RoutingAlgorithms/AlternativePathRouting.h" +#include "../RoutingAlgorithms/BasicRoutingInterface.h" +#include "../RoutingAlgorithms/ShortestPathRouting.h" + #include "../Util/StringUtil.h" #include "../typedefs.h" @@ -38,501 +42,111 @@ struct _HeapData { _HeapData( NodeID p ) : parent(p) { } }; -typedef boost::thread_specific_ptr > > HeapPtr; +typedef boost::thread_specific_ptr > > SearchEngineHeapPtr; + +template +struct SearchEngineData { + typedef SearchEngineHeapPtr HeapPtr; + typedef GraphT Graph; + SearchEngineData(GraphT * g, NodeInformationHelpDesk * nh, std::vector & n) :graph(g), nodeHelpDesk(nh), names(n) {} + const GraphT * graph; + NodeInformationHelpDesk * nodeHelpDesk; + std::vector & names; + static HeapPtr forwardHeap; + static HeapPtr backwardHeap; + static HeapPtr forwardHeap2; + static HeapPtr backwardHeap2; + + inline void InitializeOrClearFirstThreadLocalStorage() { + 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(); + } + + inline void InitializeOrClearSecondThreadLocalStorage() { + if(!forwardHeap2.get()) { + forwardHeap2.reset(new BinaryHeap< NodeID, NodeID, int, _HeapData, UnorderedMapStorage >(nodeHelpDesk->getNumberOfNodes())); + } + else + forwardHeap2->Clear(); + + if(!backwardHeap2.get()) { + backwardHeap2.reset(new BinaryHeap< NodeID, NodeID, int, _HeapData, UnorderedMapStorage >(nodeHelpDesk->getNumberOfNodes())); + } + else + backwardHeap2->Clear(); + } +}; template class SearchEngine { private: - const GraphT * _graph; - NodeInformationHelpDesk * nodeHelpDesk; - std::vector & _names; - static HeapPtr _forwardHeap; - static HeapPtr _backwardHeap; - static HeapPtr _forwardHeap2; - static HeapPtr _backwardHeap2; + typedef SearchEngineData SearchEngineDataT; + SearchEngineDataT _queryData; + inline double absDouble(double input) { if(input < 0) return input*(-1); else return input;} public: - SearchEngine(GraphT * g, NodeInformationHelpDesk * nh, std::vector & n) : _graph(g), nodeHelpDesk(nh), _names(n) {} + ShortestPathRouting shortestPath; + AlternativeRouting alternativePaths; + + SearchEngine(GraphT * g, NodeInformationHelpDesk * nh, std::vector & n) : + _queryData(g, nh, n), + shortestPath(_queryData), + alternativePaths(_queryData) + {} ~SearchEngine() {} 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(); - - if(!_backwardHeap.get()) { - _backwardHeap.reset(new BinaryHeap< NodeID, NodeID, int, _HeapData, UnorderedMapStorage >(nodeHelpDesk->getNumberOfNodes())); - } - else - _backwardHeap->Clear(); - } - - inline void InitializeThreadLocalViaStorageIfNecessary() { - if(!_forwardHeap2.get()) { - _forwardHeap2.reset(new BinaryHeap< NodeID, NodeID, int, _HeapData, UnorderedMapStorage >(nodeHelpDesk->getNumberOfNodes())); - } - else - _forwardHeap2->Clear(); - - if(!_backwardHeap2.get()) { - _backwardHeap2.reset(new BinaryHeap< NodeID, NodeID, int, _HeapData, UnorderedMapStorage >(nodeHelpDesk->getNumberOfNodes())); - } - else - _backwardHeap2->Clear(); - } - - 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 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; - - 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(); - - int _localUpperbound1 = INT_MAX; - int _localUpperbound2 = INT_MAX; - - _forwardHeap->Clear(); - _forwardHeap2->Clear(); - //insert new starting nodes into forward heap, adjusted by previous distances. - if(searchFrom1stStartNode) { - _forwardHeap->Insert(phantomNodePair.startPhantom.edgeBasedNode, -phantomNodePair.startPhantom.weight1, phantomNodePair.startPhantom.edgeBasedNode); - _forwardHeap2->Insert(phantomNodePair.startPhantom.edgeBasedNode, -phantomNodePair.startPhantom.weight1, phantomNodePair.startPhantom.edgeBasedNode); -// INFO("a 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, -phantomNodePair.startPhantom.weight2, phantomNodePair.startPhantom.edgeBasedNode+1); - _forwardHeap2->Insert(phantomNodePair.startPhantom.edgeBasedNode+1, -phantomNodePair.startPhantom.weight2, phantomNodePair.startPhantom.edgeBasedNode+1); -// INFO("b 1,2)forw insert " << phantomNodePair.startPhantom.edgeBasedNode+1 << " with weight " << -phantomNodePair.startPhantom.weight1); -// } else if(!searchFrom2ndStartNode) { -// INFO("Skipping second start node"); - } - - _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) ; - - //run two-Target Dijkstra routing step. - while(_forwardHeap->Size() + _backwardHeap->Size() > 0){ - if(_forwardHeap->Size() > 0){ - _RoutingStep(_forwardHeap, _backwardHeap, &middle1, &_localUpperbound1, 2*offset); - } - if(_backwardHeap->Size() > 0){ - _RoutingStep(_backwardHeap, _forwardHeap, &middle1, &_localUpperbound1, 2*offset); - } - } - if(_backwardHeap2->Size() > 0) { - while(_forwardHeap2->Size() + _backwardHeap2->Size() > 0){ - if(_forwardHeap2->Size() > 0){ - _RoutingStep(_forwardHeap2, _backwardHeap2, &middle2, &_localUpperbound2, 2*offset); - } - if(_backwardHeap2->Size() > 0){ - _RoutingStep(_backwardHeap2, _forwardHeap2, &middle2, &_localUpperbound2, 2*offset); - } - } - } -// INFO("upperbound1: " << _localUpperbound1 << ", distance1: " << distance1); -// INFO("upperbound2: " << _localUpperbound2 << ", distance2: " << distance2); - - //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"); - } - - //Was at most one of the two paths not found? - assert(!(INT_MAX == distance1 && INT_MAX == distance2)); - -// INFO("middle1: " << middle1); - - //Unpack paths if they exist - std::deque temporaryPackedPath1; - std::deque temporaryPackedPath2; - if(INT_MAX != _localUpperbound1) { - _RetrievePackedPathFromHeap(_forwardHeap, _backwardHeap, middle1, temporaryPackedPath1); -// INFO("temporaryPackedPath1 ends with " << *(temporaryPackedPath1.end()-1) ); - } -// INFO("middle2: " << middle2); - - if(INT_MAX != _localUpperbound2) { - _RetrievePackedPathFromHeap(_forwardHeap2, _backwardHeap2, middle2, temporaryPackedPath2); -// INFO("temporaryPackedPath2 ends with " << *(temporaryPackedPath2.end()-1) ); - } - - //if one of the paths was not found, replace it with the other one. - if(0 == temporaryPackedPath1.size()) { -// INFO("Deleting path 1"); - temporaryPackedPath1.insert(temporaryPackedPath1.end(), temporaryPackedPath2.begin(), temporaryPackedPath2.end()); - _localUpperbound1 = _localUpperbound2; - } - if(0 == temporaryPackedPath2.size()) { -// INFO("Deleting path 2"); - temporaryPackedPath2.insert(temporaryPackedPath2.end(), temporaryPackedPath1.begin(), temporaryPackedPath1.end()); - _localUpperbound2 = _localUpperbound1; - } - - assert(0 < temporaryPackedPath1.size() && 0 < temporaryPackedPath2.size()); - - //Plug paths together, s.t. end of packed path is begin of temporary packed path - if(0 < packedPath1.size() && 0 < packedPath2.size() ) { -// INFO("Both paths are non-empty"); - if( *(temporaryPackedPath1.begin()) == *(temporaryPackedPath2.begin())) { -// INFO("both paths start with the same node:" << *(temporaryPackedPath1.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())) { -// INFO("Deleting packedPath2 that ends with " << *(packedPath2.end()-1) << ", other ends with " << *(packedPath1.end()-1)); - packedPath2.clear(); - packedPath2.insert(packedPath2.end(), packedPath1.begin(), packedPath1.end()); - distance2 = distance1; -// INFO("packedPath2 now ends with " << *(packedPath2.end()-1)); - } else { -// INFO("Deleting path1 that ends with " << *(packedPath1.end()-1) << ", other ends with " << *(packedPath2.end()-1)); - packedPath1.clear(); - packedPath1.insert(packedPath1.end(), packedPath2.begin(), packedPath2.end()); - distance1 = distance2; -// INFO("Path1 now ends with " << *(packedPath1.end()-1)); - } - } else { - //packed paths 1 and 2 may need to switch. - if(*(packedPath1.end()-1) != *(temporaryPackedPath1.begin())) { -// INFO("Switching"); - packedPath1.swap(packedPath2); - std::swap(distance1, distance2); - } - } - } - packedPath1.insert(packedPath1.end(), temporaryPackedPath1.begin(), temporaryPackedPath1.end()); - packedPath2.insert(packedPath2.end(), temporaryPackedPath2.begin(), temporaryPackedPath2.end()); - - if( (packedPath1.back() == packedPath2.back()) && phantomNodePair.targetPhantom.isBidirected() ) { -// INFO("both paths end in same direction on bidirected edge, make sure start only start with : " << packedPath1.back()); - - NodeID lastNodeID = packedPath2.back(); - searchFrom1stStartNode &= !(lastNodeID == phantomNodePair.targetPhantom.edgeBasedNode+1); - searchFrom2ndStartNode &= !(lastNodeID == phantomNodePair.targetPhantom.edgeBasedNode); -// INFO("Next search from node " << phantomNodePair.targetPhantom.edgeBasedNode << ": " << (searchFrom1stStartNode ? "yes" : "no") ); -// INFO("Next search from node " << phantomNodePair.targetPhantom.edgeBasedNode+1 << ": " << (searchFrom2ndStartNode ? "yes" : "no") ); - } - - distance1 += _localUpperbound1; - distance2 += _localUpperbound2; - } - -// 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); - } - - int ComputeRoute(PhantomNodes & phantomNodes, std::vector<_PathData> & path) { - int _upperbound = INT_MAX; - if(!phantomNodes.AtLeastOnePhantomNodeIsUINTMAX()) - return _upperbound; - - 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) ; - while(_forwardHeap->Size() + _backwardHeap->Size() > 0){ - if(_forwardHeap->Size() > 0){ - _RoutingStep(_forwardHeap, _backwardHeap, &middle, &_upperbound, 2*offset); - } - if(_backwardHeap->Size() > 0){ - _RoutingStep(_backwardHeap, _forwardHeap, &middle, &_upperbound, 2*offset); - } - } -// INFO("dist: " << _upperbound); - if ( _upperbound == INT_MAX ) { - return _upperbound; - } - std::deque packedPath; - _RetrievePackedPathFromHeap(_forwardHeap, _backwardHeap, middle, packedPath); - - //Setting weights to correspond with that of the actual chosen path -// if(packedPath[0] == phantomNodes.startPhantom.edgeBasedNode && phantomNodes.startPhantom.isBidirected()) { -// INFO("Setting weight1=" << phantomNodes.startPhantom.weight1 << " to that of weight2=" << phantomNodes.startPhantom.weight2); -// phantomNodes.startPhantom.weight1 = phantomNodes.startPhantom.weight2; -// } else { -// INFO("Setting weight2=" << phantomNodes.startPhantom.weight2 << " to that of weight1=" << phantomNodes.startPhantom.weight1); -// phantomNodes.startPhantom.weight2 = phantomNodes.startPhantom.weight1; -// } -// std::cout << "0: "; -// for(unsigned i = 0; i < packedPath.size(); ++i) -// std::cout << packedPath[i] << " "; -// std::cout << std::endl; - - _UnpackPath(packedPath, path); - return _upperbound; + result.lat = _queryData.nodeHelpDesk->getLatitudeOfNode(id); + result.lon = _queryData.nodeHelpDesk->getLongitudeOfNode(id); } inline void FindRoutingStarts(const _Coordinate & start, const _Coordinate & target, PhantomNodes & routingStarts) const { - nodeHelpDesk->FindRoutingStarts(start, target, routingStarts); + _queryData.nodeHelpDesk->FindRoutingStarts(start, target, routingStarts); } inline void FindPhantomNodeForCoordinate(const _Coordinate & location, PhantomNode & result) const { - nodeHelpDesk->FindPhantomNodeForCoordinate(location, result); + _queryData.nodeHelpDesk->FindPhantomNodeForCoordinate(location, result); } inline NodeID GetNameIDForOriginDestinationNodeID(const NodeID s, const NodeID t) const { if(s == t) return 0; - EdgeID e = _graph->FindEdge(s, t); + EdgeID e = _queryData.graph->FindEdge(s, t); if(e == UINT_MAX) - e = _graph->FindEdge( t, s ); + e = _queryData.graph->FindEdge( t, s ); if(UINT_MAX == e) { return 0; } assert(e != UINT_MAX); - const EdgeData ed = _graph->GetEdgeData(e); + const EdgeData ed = _queryData.graph->GetEdgeData(e); return ed.via; } inline std::string GetEscapedNameForNameID(const unsigned nameID) const { - return ((nameID >= _names.size() || nameID == 0) ? std::string("") : HTMLEntitize(_names.at(nameID))); + return ((nameID >= _queryData.names.size() || nameID == 0) ? std::string("") : HTMLEntitize(_queryData.names.at(nameID))); } inline std::string GetEscapedNameForEdgeBasedEdgeID(const unsigned edgeID) const { - const unsigned nameID = _graph->GetEdgeData(edgeID).nameID1; + const unsigned nameID = _queryData.graph->GetEdgeData(edgeID).nameID1; return GetEscapedNameForNameID(nameID); } -private: - inline void _RetrievePackedPathFromHeap(HeapPtr & _fHeap, HeapPtr & _bHeap, const NodeID middle, std::deque& packedPath) { - NodeID pathNode = middle; - if(_fHeap->GetData(pathNode).parent != middle) { - do { - pathNode = _fHeap->GetData(pathNode).parent; - packedPath.push_front(pathNode); - }while(pathNode != _fHeap->GetData(pathNode).parent); - } - packedPath.push_back(middle); - pathNode = middle; - if(_bHeap->GetData(pathNode).parent != middle) { - do{ - pathNode = _bHeap->GetData(pathNode).parent; - packedPath.push_back(pathNode); - } while (pathNode != _bHeap->GetData(pathNode).parent); - } -// std::cout << "unpacking: "; -// for(std::deque::iterator it = packedPath.begin(); it != packedPath.end(); ++it) -// std::cout << *it << " "; -// std::cout << std::endl; - } - - template - inline void _RoutingStep(HeapPtr & _forwardHeap, HeapPtr & _backwardHeap, 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); - } - } - } - - 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; - - assert( edgeWeight > 0 ); - - //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 forwardDirectionFlag = (forwardDirection ? data.forward : data.backward ); - if(forwardDirectionFlag) { - - const NodeID to = _graph->GetTarget(edge); - const int edgeWeight = data.distance; - - assert( edgeWeight > 0 ); - const int toDistance = distance + edgeWeight; - - //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 - } - } - } - } - - inline void _UnpackPath(std::deque & packedPath, std::vector<_PathData> & unpackedPath) const { - const unsigned sizeOfPackedPath = packedPath.size(); - std::stack > recursionStack; - - //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])); - } - - std::pair edge; - while(!recursionStack.empty()) { - edge = recursionStack.top(); - recursionStack.pop(); -// INFO("Unpacking edge (" << edge.first << "," << edge.second << ")"); - - 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){ -// INFO("1smallest " << eit << ", " << weight); - smallestEdge = eit; - smallestWeight = weight; - } - } - - 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){ -// INFO("2smallest " << eit << ", " << weight); - smallestEdge = eit; - smallestWeight = weight; - } - } - } - assert(smallestWeight != INT_MAX); - - const EdgeData& ed = _graph->GetEdgeData(smallestEdge); - if(ed.shortcut) {//unpack - const NodeID middle = ed.id; - //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.id, nodeHelpDesk->getNameIndexFromEdgeID(ed.id), nodeHelpDesk->getTurnInstructionFromEdgeID(ed.id), ed.distance) ); - } - } - } }; -template HeapPtr SearchEngine::_forwardHeap; -template HeapPtr SearchEngine::_backwardHeap; -template HeapPtr SearchEngine::_forwardHeap2; -template HeapPtr SearchEngine::_backwardHeap2; +template SearchEngineHeapPtr SearchEngineData::forwardHeap; +template SearchEngineHeapPtr SearchEngineData::backwardHeap; + +template SearchEngineHeapPtr SearchEngineData::forwardHeap2; +template SearchEngineHeapPtr SearchEngineData::backwardHeap2; #endif /* SEARCHENGINE_H_ */ diff --git a/Plugins/ViaRoutePlugin.h b/Plugins/ViaRoutePlugin.h index 04a17aa71..efec305aa 100644 --- a/Plugins/ViaRoutePlugin.h +++ b/Plugins/ViaRoutePlugin.h @@ -82,10 +82,6 @@ public: RawRouteData rawRoute; rawRoute.checkSum = nodeHelpDesk->GetCheckSum(); bool checksumOK = ((unsigned)atoi(routeParameters.options.Find("checksum").c_str()) == rawRoute.checkSum); -// if(!checksumOK) { -// INFO((unsigned)atoi(routeParameters.options.Find("checksum").c_str()) << "!=" << rawRoute.checkSum); -// INFO("mismatching checksum"); -// } std::vector textCoord; for(unsigned i = 0; i < routeParameters.viaPoints.size(); ++i) { textCoord.resize(0); @@ -117,23 +113,36 @@ public: searchEngine->FindPhantomNodeForCoordinate( rawRoute.rawViaNodeCoordinates[i], phantomNodeVector[i]); } unsigned distance = 0; - //single route or via point routing - if(2 == rawRoute.rawViaNodeCoordinates.size()) { + for(unsigned i = 0; i < phantomNodeVector.size()-1; ++i) { PhantomNodes segmentPhantomNodes; - segmentPhantomNodes.startPhantom = phantomNodeVector[0]; - segmentPhantomNodes.targetPhantom = phantomNodeVector[1]; - distance = searchEngine->ComputeRoute(segmentPhantomNodes, rawRoute.computedRouted); + segmentPhantomNodes.startPhantom = phantomNodeVector[i]; + segmentPhantomNodes.targetPhantom = phantomNodeVector[i+1]; rawRoute.segmentEndCoordinates.push_back(segmentPhantomNodes); - } else { - //Getting the shortest via path is a dynamic programming problem and is solved as such. - for(unsigned i = 0; i < phantomNodeVector.size()-1; ++i) { - PhantomNodes segmentPhantomNodes; - segmentPhantomNodes.startPhantom = phantomNodeVector[i]; - segmentPhantomNodes.targetPhantom = phantomNodeVector[i+1]; - rawRoute.segmentEndCoordinates.push_back(segmentPhantomNodes); - } - distance = searchEngine->ComputeViaRoute(rawRoute.segmentEndCoordinates, rawRoute.computedRouted); } + distance = searchEngine->shortestPath(rawRoute.segmentEndCoordinates, rawRoute.computedRouted); + + std::vector<_PathData> alternative; + if(1 == rawRoute.segmentEndCoordinates.size()) { + INFO("Checking for alternative paths"); + int distance2 = searchEngine->alternativePaths(rawRoute.segmentEndCoordinates[0], alternative); + } + std::cout << "latitude,longitude" << std::endl; + for(unsigned i = 0; i < rawRoute.computedRouted.size(); ++i) { + _Coordinate current; + searchEngine->GetCoordinatesForNodeID(rawRoute.computedRouted[i].node, current); + std::cout << current.lat/100000. << "," << current.lon/100000. << std::endl; + } + std::cout << std::endl; + + std::cout << "latitude,longitude" << std::endl; + for(unsigned i = 0; i GetCoordinatesForNodeID(alternative[i].node, current); + std::cout << current.lat/100000. << "," << current.lon/100000. << std::endl; + } + std::cout << std::endl; + + if(INT_MAX == distance ) { DEBUG( "Error occurred, single path not found" ); } diff --git a/RoutingAlgorithms/AlternativePathRouting.h b/RoutingAlgorithms/AlternativePathRouting.h new file mode 100644 index 000000000..9e7cdf011 --- /dev/null +++ b/RoutingAlgorithms/AlternativePathRouting.h @@ -0,0 +1,470 @@ +/* + open source routing machine + Copyright (C) Dennis Luxen, others 2010 + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU AFFERO General Public License as published by +the Free Software Foundation; either version 3 of the License, or +any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU Affero General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +or see http://www.gnu.org/licenses/agpl.txt. + */ + +#ifndef ALTERNATIVEROUTES_H_ +#define ALTERNATIVEROUTES_H_ + +#include + +#include "BasicRoutingInterface.h" + +template +class AlternativeRouting : private BasicRoutingInterface{ + typedef BasicRoutingInterface super; + typedef std::pair PreselectedNode; + + struct RankedCandidateNode { + RankedCandidateNode(NodeID n, int l, int s) : node(n), length(l), sharing(s) {} + NodeID node; + int length; + int sharing; + const bool operator<(const RankedCandidateNode& other) const { + return (2*length + sharing) < (2*other.length + other.sharing); + } + }; +public: + + AlternativeRouting(QueryDataT & qd) : super(qd) { } + + ~AlternativeRouting() {} + + int operator()(const PhantomNodes & phantomNodePair, std::vector<_PathData> & unpackedPath) { + std::vector alternativePath; + std::vector viaNodeCandidates; + + int _lengthOfShortestPath = INT_MAX; + + INFO("Checking for alternative between (" << phantomNodePair.startPhantom.location << ") and (" << phantomNodePair.targetPhantom.location << ")"); + + typename QueryDataT::HeapPtr & forwardHeap = super::_queryData.forwardHeap; + typename QueryDataT::HeapPtr & backwardHeap = super::_queryData.backwardHeap; + typename QueryDataT::HeapPtr & forwardHeap2 = super::_queryData.forwardHeap2; + typename QueryDataT::HeapPtr & backwardHea2 = super::_queryData.backwardHeap2; + + //Initialize Queues + super::_queryData.InitializeOrClearFirstThreadLocalStorage(); + int _upperBound = INT_MAX; + NodeID middle = UINT_MAX; + forwardHeap->Insert(phantomNodePair.startPhantom.edgeBasedNode, -phantomNodePair.startPhantom.weight1, phantomNodePair.startPhantom.edgeBasedNode); + if(phantomNodePair.startPhantom.isBidirected() ) { + forwardHeap->Insert(phantomNodePair.startPhantom.edgeBasedNode+1, -phantomNodePair.startPhantom.weight2, phantomNodePair.startPhantom.edgeBasedNode+1); + } + backwardHeap->Insert(phantomNodePair.targetPhantom.edgeBasedNode, phantomNodePair.targetPhantom.weight1, phantomNodePair.targetPhantom.edgeBasedNode); + if(phantomNodePair.targetPhantom.isBidirected() ) { + backwardHeap->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) ; + + //exploration from s and t until deletemin/(1+epsilon) > _lengthOfShortestPath + while(forwardHeap->Size() + backwardHeap->Size() > 0){ + if(forwardHeap->Size() > 0){ + AlternativeRoutingStep(forwardHeap, backwardHeap, &middle, &_upperBound, 2*offset, true, viaNodeCandidates); + } + if(backwardHeap->Size() > 0){ + AlternativeRoutingStep(backwardHeap, forwardHeap, &middle, &_upperBound, 2*offset, false, viaNodeCandidates); + } + } + std::sort(viaNodeCandidates.begin(), viaNodeCandidates.end()); + int size = std::unique(viaNodeCandidates.begin(), viaNodeCandidates.end())- viaNodeCandidates.begin(); + std::cout << "middle: " << middle << ", other: "; + for(unsigned i = 0; i < size; ++i) + if(middle != viaNodeCandidates[i]) + std::cout << viaNodeCandidates[i] << " "; + std::cout << std::endl; + viaNodeCandidates.resize(size); + INFO("found " << viaNodeCandidates.size() << " nodes in search space intersection"); + + INFO("upper bound: " << _upperBound); + //ch-pruning of via nodes in both search spaces + + std::vector< PreselectedNode> nodesThatPassPreselection; + + BOOST_FOREACH(const NodeID node, viaNodeCandidates) { + if(node == middle) + continue; + +// std::cout << "via path over " << node << std::endl; + int sharing = approximateAmountOfSharing(middle, node, forwardHeap, backwardHeap); + + int length1 = forwardHeap->GetKey(node); + int length2 = backwardHeap->GetKey(node); +// std::cout << " length: " << length1+length2 << std::endl; + bool lengthPassed = (length1+length2 < _upperBound*1.25); +// std::cout << " length passed: " << (lengthPassed ? "yes" : "no") << std::endl; +// std::cout << " apx-sharing: " << sharing << std::endl; + bool sharingPassed = (sharing <= _upperBound*0.8); +// std::cout << " apx-sharing passed: " << ( sharingPassed ? "yes" : "no") << std::endl; + bool stretchPassed = length1+length2 - sharing < 1.25*(_upperBound-sharing); +// std::cout << " apx-stretch passed: " << ( stretchPassed ? "yes" : "no") << std::endl; + + if(lengthPassed && sharingPassed && stretchPassed) + nodesThatPassPreselection.push_back(std::make_pair(node, length1+length2)); + } + + std::vector rankedCandidates; + + INFO(nodesThatPassPreselection.size() << " out of " << viaNodeCandidates.size() << " passed preselection"); + //prioritizing via nodes + BOOST_FOREACH(const PreselectedNode node, nodesThatPassPreselection) { + int lengthOfViaPath = 0; + int sharingOfViaPath = 0; + + computeLengthAndSharingOfViaPath(phantomNodePair, node, &lengthOfViaPath, &sharingOfViaPath, offset, middle); + rankedCandidates.push_back(RankedCandidateNode(node.first, lengthOfViaPath, sharingOfViaPath)); + } + + std::sort(rankedCandidates.begin(), rankedCandidates.end()); + + NodeID selectedViaNode = UINT_MAX; + + BOOST_FOREACH(const RankedCandidateNode candidate, rankedCandidates){ + //TODO: select first admissable + //TODO: conduct T-Test +// selectedViaNode = candidate.node; + } + + selectedViaNode = rankedCandidates[0].node; + + //TODO: compute and unpack and by exploring search spaces from v and intersecting against queues + //TODO: Same (co-)routines necessary as for computing length and sharing + + retrievePackedViaPath(forwardHeap, backwardHeap, forwardHeap, backwardHea2, selectedViaNode, unpackedPath); + + return 0; + } + +private: + inline void retrievePackedViaPath(typename QueryDataT::HeapPtr & _forwardHeap1, typename QueryDataT::HeapPtr & _backwardHeap1, typename QueryDataT::HeapPtr & _forwardHeap2, typename QueryDataT::HeapPtr & _backwardHeap2, const NodeID viaNode, std::vector<_PathData> & unpackedPath) { + //unpack s,v + std::deque packed_s_v_path, packed_v_t_path; + std::cout << "1" << std::endl; + //super::RetrievePackedPathFromHeap(_forwardHeap1, _backwardHeap2, viaNode, packed_s_v_path); + std::cout << "2" << std::endl; + super::RetrievePackedPathFromHeap(_forwardHeap2, _backwardHeap1, viaNode, packed_v_t_path); + std::cout << "3" << std::endl; + packed_s_v_path.insert(packed_s_v_path.end(),packed_v_t_path.begin(), packed_v_t_path.end() ); + std::cout << "4" << std::endl; + + for(unsigned i = 0; i < packed_s_v_path.size(); ++i) + std::cout << packed_s_v_path[i] << " " << std::endl; + std::cout << std::endl; + + super::UnpackPath(packed_s_v_path, unpackedPath); + + } + + inline void computeLengthAndSharingOfViaPath(const PhantomNodes & phantomNodePair, const PreselectedNode& node, int *lengthOfViaPath, int *sharingOfViaPath, const int offset, const NodeID middleOfShortestPath) { + //compute and unpack and by exploring search spaces from v and intersecting against queues + //only half-searches have to be done at this stage + std::cout << "deep check for via path " << node.first << std::endl; + + super::_queryData.InitializeOrClearSecondThreadLocalStorage(); + + typename QueryDataT::HeapPtr & existingForwardHeap = super::_queryData.forwardHeap; + typename QueryDataT::HeapPtr & existingBackwardHeap = super::_queryData.backwardHeap; + + typename QueryDataT::HeapPtr & newForwardHeap = super::_queryData.forwardHeap2; + typename QueryDataT::HeapPtr & newBackwardHeap = super::_queryData.backwardHeap2; + + NodeID s_v_middle = UINT_MAX; + int upperBoundFor_s_v_Path = INT_MAX;//existingForwardHeap->GetKey(node.first); + + //compute path by reusing forward search from s + newBackwardHeap->Insert(node.first, 0, node.first); + while(newBackwardHeap->Size() > 0){ + super::RoutingStep(newBackwardHeap, existingForwardHeap, &s_v_middle, &upperBoundFor_s_v_Path, 2*offset, false); + } + std::cout << " length of : " << upperBoundFor_s_v_Path << " with middle node " << s_v_middle << std::endl; + + //compute path by reusing backward search from t + NodeID v_t_middle = UINT_MAX; + int upperBoundFor_v_t_Path = INT_MAX;//existingBackwardHeap->GetKey(node.first); + newForwardHeap->Insert(node.first, 0, node.first); + while(newForwardHeap->Size() > 0){ + super::RoutingStep(newForwardHeap, existingBackwardHeap, &v_t_middle, &upperBoundFor_v_t_Path, 2*offset, true); + } + std::cout << " length of : " << upperBoundFor_v_t_Path << " with middle node " << v_t_middle << std::endl; + + *lengthOfViaPath = upperBoundFor_s_v_Path+upperBoundFor_v_t_Path; + std::cout << " exact length of via path: " << *lengthOfViaPath << std::endl; + + std::deque packedShortestPath; + std::deque packed_s_v_path; + std::deque packed_v_t_path; + //retrieve packed paths + std::cout << " retrieving packed path for middle nodes " << middleOfShortestPath << "," << s_v_middle << "," << v_t_middle << " (shorstest, sv, vt)" << std::endl; + super::RetrievePackedPathFromHeap(existingForwardHeap, existingBackwardHeap, middleOfShortestPath, packedShortestPath); + super::RetrievePackedPathFromHeap(existingForwardHeap, newBackwardHeap, s_v_middle, packed_s_v_path); + super::RetrievePackedPathFromHeap(newForwardHeap, existingBackwardHeap, v_t_middle,packed_v_t_path); + + std::cout << "packed sv: "; + for(unsigned i = 0; i < packed_s_v_path.size(); ++i) { + std::cout << packed_s_v_path[i] << " "; + } + std::cout << std::endl; + std::cout << "packed vt: "; + for(unsigned i = 0; i < packed_v_t_path.size(); ++i) { + std::cout << packed_v_t_path[i] << " "; + } + std::cout << std::endl; + std::cout << "packed shortest: "; + for(unsigned i = 0; i < packedShortestPath.size(); ++i) { + std::cout << packedShortestPath[i] << " "; + } + std::cout << std::endl; + + + + typedef std::pair UnpackEdge; + std::stack unpackStack; + + //TODO: partial unpacking, compute sharing + //First partially unpack s-->v until paths deviate, note length of common path. + std::cout << "length of packed sv-path: " << packed_s_v_path.size() << ", length of packed shortest path: " << packedShortestPath.size() << std::endl; + for(unsigned i = 0, lengthOfPackedPath = std::min(packed_s_v_path.size(), packedShortestPath.size() ) - 1; (i < lengthOfPackedPath) && unpackStack.empty(); ++i ) { + std::cout << " checking indices [" << i << "] and [" << (i+1) << "]" << std::endl; + if(packed_s_v_path[i] == packedShortestPath[i] && packed_s_v_path[i+1] == packedShortestPath[i+1]) { + typename QueryDataT::Graph::EdgeIterator edgeID = super::_queryData.graph->FindEdgeInEitherDirection(packed_s_v_path[i], packed_s_v_path[i+1]); + *sharingOfViaPath += super::_queryData.graph->GetEdgeData(edgeID).distance; + } else { + if(packed_s_v_path[i] == packedShortestPath[i]) { + unpackStack.push(std::make_pair(packed_s_v_path[i], packed_s_v_path[i+1])); + unpackStack.push(std::make_pair(packedShortestPath[i], packedShortestPath[i+1])); + } + } + } + while(!unpackStack.empty()) { + UnpackEdge shortestPathEdge = unpackStack.top(); unpackStack.pop(); + UnpackEdge viaPathEdge = unpackStack.top(); unpackStack.pop(); + std::cout << " unpacking edges (" << shortestPathEdge.first << "," << shortestPathEdge.second << ") and (" << viaPathEdge.first << "," << viaPathEdge.second << ")" << std::endl; + typename QueryDataT::Graph::EdgeIterator edgeIDInShortestPath = super::_queryData.graph->FindEdgeInEitherDirection(shortestPathEdge.first, shortestPathEdge.second); + typename QueryDataT::Graph::EdgeIterator edgeIDInViaPath = super::_queryData.graph->FindEdgeInEitherDirection(viaPathEdge.first, viaPathEdge.second); + std::cout << " ids are " << edgeIDInShortestPath << " (shortest) and " << edgeIDInViaPath << " (via)" << std::endl; + bool IsShortestPathEdgeShortCut = super::_queryData.graph->GetEdgeData(edgeIDInShortestPath).shortcut; + bool IsViaEdgeShortCut = super::_queryData.graph->GetEdgeData(edgeIDInViaPath).shortcut; + + const NodeID middleOfShortestPath = !IsShortestPathEdgeShortCut ? UINT_MAX : super::_queryData.graph->GetEdgeData(edgeIDInShortestPath).id; + const NodeID middleOfViaPath = !IsViaEdgeShortCut ? UINT_MAX : super::_queryData.graph->GetEdgeData(edgeIDInViaPath).id; + + if(IsShortestPathEdgeShortCut || IsViaEdgeShortCut) { + if(middleOfShortestPath != middleOfViaPath) { // unpack first segment + //put first segment of via edge on stack, else take the segment already available + if(IsViaEdgeShortCut) + unpackStack.push(std::make_pair(viaPathEdge.first, middleOfViaPath)); + else + unpackStack.push(viaPathEdge); + + //put first segment of shortest path edge on stack if not a shortcut, else take the segment already available + if(IsShortestPathEdgeShortCut) + unpackStack.push(std::make_pair(shortestPathEdge.first, middleOfShortestPath)); + else + unpackStack.push(shortestPathEdge); + + } else { // unpack second segment + if(IsViaEdgeShortCut) + unpackStack.push(std::make_pair(middleOfViaPath, viaPathEdge.second)); + else + unpackStack.push(viaPathEdge); + + //put first segment of shortest path edge on stack if not a shortcut, else take the segment already available + if(IsShortestPathEdgeShortCut) + unpackStack.push(std::make_pair(middleOfShortestPath, shortestPathEdge.second )); + else + unpackStack.push(shortestPathEdge); + + //add length of first segment to amount of sharing + typename QueryDataT::Graph::EdgeIterator edgeIDInViaPath = super::_queryData.graph->FindEdgeInEitherDirection(viaPathEdge.first, viaPathEdge.second); + *sharingOfViaPath += super::_queryData.graph->GetEdgeData(edgeIDInViaPath).distance; + } + } + } + std::cout << "sharing of SV-Path: " << *sharingOfViaPath << std::endl; + + //Second, partially unpack v-->t in reverse until paths deviate and note lengths + unsigned viaPathIndex = packed_v_t_path.size()-1; + unsigned shortestPathIndex = packedShortestPath.size() -1; + std::cout << "length of packed vt-path: " << packed_v_t_path.size() << ", length of packed shortest path: " << packedShortestPath.size() << std::endl; + for( ; viaPathIndex>0 && shortestPathIndex>0; ) { +// std::cout << " checking indices [" << shortestPathIndex << "] and [" << (shortestPathIndex-1) << "] (shortest) as well as [" << shortestPathIndex << "] and [" << (shortestPathIndex-1) << "]" << std::endl; + if(packed_v_t_path[viaPathIndex-1] == packedShortestPath[shortestPathIndex-1] && packed_v_t_path[viaPathIndex] == packedShortestPath[shortestPathIndex]) { + typename QueryDataT::Graph::EdgeIterator edgeID = super::_queryData.graph->FindEdgeInEitherDirection(packed_v_t_path[viaPathIndex-1], packed_v_t_path[viaPathIndex]); +// std::cout << "Id of edge (" << packed_v_t_path[viaPathIndex-1] << "," << packed_v_t_path[viaPathIndex] << ") : " << edgeID << std::endl; + *sharingOfViaPath += super::_queryData.graph->GetEdgeData(edgeID).distance; + } else { + if(packed_v_t_path[viaPathIndex] == packedShortestPath[shortestPathIndex]) { + unpackStack.push(std::make_pair(packed_v_t_path[viaPathIndex-1], packed_v_t_path[viaPathIndex])); + unpackStack.push(std::make_pair(packedShortestPath[shortestPathIndex-1], packedShortestPath[shortestPathIndex])); + } + } + --viaPathIndex; --shortestPathIndex; + } + + while(!unpackStack.empty()) { + UnpackEdge shortestPathEdge = unpackStack.top(); unpackStack.pop(); + UnpackEdge viaPathEdge = unpackStack.top(); unpackStack.pop(); + std::cout << " unpacking edges (" << shortestPathEdge.first << "," << shortestPathEdge.second << ") and (" << viaPathEdge.first << "," << viaPathEdge.second << ")" << std::endl; + typename QueryDataT::Graph::EdgeIterator edgeIDInShortestPath = super::_queryData.graph->FindEdgeInEitherDirection(shortestPathEdge.first, shortestPathEdge.second); +// std::cout << "!" << std::endl; + typename QueryDataT::Graph::EdgeIterator edgeIDInViaPath = super::_queryData.graph->FindEdgeInEitherDirection(viaPathEdge.first, viaPathEdge.second); + std::cout << " ids are " << edgeIDInShortestPath << " (shortest) and " << edgeIDInViaPath << " (via)" << std::endl; + bool IsShortestPathEdgeShortCut = super::_queryData.graph->GetEdgeData(edgeIDInShortestPath).shortcut; + bool IsViaEdgeShortCut = super::_queryData.graph->GetEdgeData(edgeIDInViaPath).shortcut; + + const NodeID middleOfShortestPath = !IsShortestPathEdgeShortCut ? UINT_MAX : super::_queryData.graph->GetEdgeData(edgeIDInShortestPath).id; + const NodeID middleOfViaPath = !IsViaEdgeShortCut ? UINT_MAX : super::_queryData.graph->GetEdgeData(edgeIDInViaPath).id; + + std::cout << " shortest shrtcut: " << (IsShortestPathEdgeShortCut ? "yes": "no") << "(" <FindEdgeInEitherDirection(viaPathEdge.first, viaPathEdge.second); + *sharingOfViaPath += super::_queryData.graph->GetEdgeData(edgeIDInViaPath).distance; + + } else { // unpack second segment + std::cout << " unpacking second segment" << std::endl; + if(IsViaEdgeShortCut) + unpackStack.push(std::make_pair(middleOfViaPath, viaPathEdge.second)); + else + unpackStack.push(viaPathEdge); + + //put first segment of shortest path edge on stack if not a shortcut, else take the segment already available + if(IsShortestPathEdgeShortCut) + unpackStack.push(std::make_pair(middleOfShortestPath, shortestPathEdge.second )); + else + unpackStack.push(shortestPathEdge); + } + } + } + std::cout << "sharing of SVT-Path: " << *sharingOfViaPath << std::endl; + + } + + inline int approximateAmountOfSharing(const NodeID middleNodeIDOfShortestPath, const NodeID middleNodeIDOfAlternativePath, typename QueryDataT::HeapPtr & _forwardHeap, typename QueryDataT::HeapPtr & _backwardHeap) { + std::deque packedShortestPath; + std::deque packedAlternativePath; + + super::RetrievePackedPathFromHeap(_forwardHeap, _backwardHeap, middleNodeIDOfShortestPath, packedShortestPath); + super::RetrievePackedPathFromHeap(_forwardHeap, _backwardHeap, middleNodeIDOfAlternativePath, packedAlternativePath); + + int sharing = 0; + + int aindex = 0; + //compute forward sharing + while( (packedAlternativePath[aindex] == packedShortestPath[aindex]) && (packedAlternativePath[aindex+1] == packedShortestPath[aindex+1]) ) { + // INFO("retrieving edge (" << packedAlternativePath[aindex] << "," << packedAlternativePath[aindex+1] << ")"); + typename QueryDataT::Graph::EdgeIterator edgeID = super::_queryData.graph->FindEdgeInEitherDirection(packedAlternativePath[aindex], packedAlternativePath[aindex+1]); + sharing += super::_queryData.graph->GetEdgeData(edgeID).distance; + ++aindex; + } + + aindex = packedAlternativePath.size()-1; + int bindex = packedShortestPath.size()-1; + //compute backward sharing + while( (packedAlternativePath[aindex] == packedShortestPath[bindex]) && (packedAlternativePath[aindex-1] == packedShortestPath[bindex-1]) ) { + typename QueryDataT::Graph::EdgeIterator edgeID = super::_queryData.graph->FindEdgeInEitherDirection(packedAlternativePath[aindex], packedAlternativePath[aindex-1]); + sharing += super::_queryData.graph->GetEdgeData(edgeID).distance; + --aindex; --bindex; + } + + return sharing; + } + + inline void AlternativeRoutingStep(typename QueryDataT::HeapPtr & _forwardHeap, typename QueryDataT::HeapPtr & _backwardHeap, NodeID *middle, int *_upperbound, const int edgeBasedOffset, const bool forwardDirection, std::vector& searchSpaceIntersection) const { + const NodeID node = _forwardHeap->DeleteMin(); + + const int distance = _forwardHeap->GetKey(node); + if(_backwardHeap->WasInserted(node) ){ + searchSpaceIntersection.push_back(node); + + const int newDistance = _backwardHeap->GetKey(node) + distance; + if(newDistance < *_upperbound ){ + if(newDistance>=0 ) { + INFO("upper bound decrease to: " << newDistance); + *middle = node; + *_upperbound = newDistance; + } + } + } + + //0.8 implies an epsilon of 25% + if((distance-edgeBasedOffset)*0.8 > *_upperbound){ + _forwardHeap->DeleteAll(); + return; + } + + for ( typename QueryDataT::Graph::EdgeIterator edge = super::_queryData.graph->BeginEdges( node ); edge < super::_queryData.graph->EndEdges(node); edge++ ) { + const typename QueryDataT::Graph::EdgeData & data = super::_queryData.graph->GetEdgeData(edge); + bool forwardDirectionFlag = (forwardDirection ? data.forward : data.backward ); + if(forwardDirectionFlag) { + + const NodeID to = super::_queryData.graph->GetTarget(edge); + const int edgeWeight = data.distance; + + assert( edgeWeight > 0 ); + const int toDistance = distance + edgeWeight; + + //New Node discovered -> Add to Heap + Node Info Storage + if ( !_forwardHeap->WasInserted( to ) ) { + _forwardHeap->Insert( to, toDistance, node ); + + } + //Found a shorter Path -> Update distance + else if ( toDistance < _forwardHeap->GetKey( to ) ) { + _forwardHeap->GetData( to ).parent = node; + _forwardHeap->DecreaseKey( to, toDistance ); + //new parent + } + } + } + } + + void pruneViaNodeCandidates() { + + } + + unsigned computeApproximatedOverlap(const NodeID s, const NodeID t, const NodeID v) { + + } + + unsigned computeOverlap(const NodeID s, const NodeID t, const NodeID v) { + return 0; + } + +}; + + +#endif /* ALTERNATIVEROUTES_H_ */ diff --git a/RoutingAlgorithms/BasicRoutingInterface.h b/RoutingAlgorithms/BasicRoutingInterface.h new file mode 100644 index 000000000..e1228256b --- /dev/null +++ b/RoutingAlgorithms/BasicRoutingInterface.h @@ -0,0 +1,184 @@ +/* + open source routing machine + Copyright (C) Dennis Luxen, others 2010 + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU AFFERO General Public License as published by +the Free Software Foundation; either version 3 of the License, or +any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU Affero General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +or see http://www.gnu.org/licenses/agpl.txt. + */ + + + +#ifndef BASICROUTINGINTERFACE_H_ +#define BASICROUTINGINTERFACE_H_ + +#include +#include + +template +class BasicRoutingInterface { +protected: + QueryDataT & _queryData; +public: + BasicRoutingInterface(QueryDataT & qd) : _queryData(qd) { } + virtual ~BasicRoutingInterface(){ }; + + inline void RoutingStep(typename QueryDataT::HeapPtr & _forwardHeap, typename QueryDataT::HeapPtr & _backwardHeap, NodeID *middle, int *_upperbound, const int edgeBasedOffset, const bool forwardDirection) 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, upper bound: " << *_upperbound); + 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); + } + } + } + + if(distance-edgeBasedOffset > *_upperbound){ + _forwardHeap->DeleteAll(); + return; + } + + for ( typename QueryDataT::Graph::EdgeIterator edge = _queryData.graph->BeginEdges( node ); edge < _queryData.graph->EndEdges(node); edge++ ) { + const typename QueryDataT::Graph::EdgeData & data = _queryData.graph->GetEdgeData(edge); + bool backwardDirectionFlag = (!forwardDirection) ? data.forward : data.backward; + if(backwardDirectionFlag) { + const NodeID to = _queryData.graph->GetTarget(edge); + const int edgeWeight = data.distance; + + assert( edgeWeight > 0 ); + + //Stalling + if(_forwardHeap->WasInserted( to )) { + if(_forwardHeap->GetKey( to ) + edgeWeight < distance) { + return; + } + } + } + } + + for ( typename QueryDataT::Graph::EdgeIterator edge = _queryData.graph->BeginEdges( node ); edge < _queryData.graph->EndEdges(node); edge++ ) { + const typename QueryDataT::Graph::EdgeData & data = _queryData.graph->GetEdgeData(edge); + bool forwardDirectionFlag = (forwardDirection ? data.forward : data.backward ); + if(forwardDirectionFlag) { + + const NodeID to = _queryData.graph->GetTarget(edge); + const int edgeWeight = data.distance; + + assert( edgeWeight > 0 ); + const int toDistance = distance + edgeWeight; + + //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 + } + } + } + } + + inline void UnpackPath(std::deque & packedPath, std::vector<_PathData> & unpackedPath) const { + + const unsigned sizeOfPackedPath = packedPath.size(); + std::stack > recursionStack; + + //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])); + } + + std::pair edge; + while(!recursionStack.empty()) { + edge = recursionStack.top(); + recursionStack.pop(); +// INFO("Unpacking edge (" << edge.first << "," << edge.second << ")"); + + typename QueryDataT::Graph::EdgeIterator smallestEdge = SPECIAL_EDGEID; + int smallestWeight = INT_MAX; + for(typename QueryDataT::Graph::EdgeIterator eit = _queryData.graph->BeginEdges(edge.first);eit < _queryData.graph->EndEdges(edge.first);++eit){ + const int weight = _queryData.graph->GetEdgeData(eit).distance; +// INFO("Checking edge (" << edge.first << "/" << _queryData.graph->GetTarget(eit) << ")"); + if(_queryData.graph->GetTarget(eit) == edge.second && weight < smallestWeight && _queryData.graph->GetEdgeData(eit).forward){ +// INFO("1smallest " << eit << ", " << weight); + smallestEdge = eit; + smallestWeight = weight; + } + } + + if(smallestEdge == SPECIAL_EDGEID){ + for(typename QueryDataT::Graph::EdgeIterator eit = _queryData.graph->BeginEdges(edge.second);eit < _queryData.graph->EndEdges(edge.second);++eit){ + const int weight = _queryData.graph->GetEdgeData(eit).distance; +// INFO("Checking edge (" << edge.first << "/" << _queryData.graph->GetTarget(eit) << ")"); + if(_queryData.graph->GetTarget(eit) == edge.first && weight < smallestWeight && _queryData.graph->GetEdgeData(eit).backward){ +// INFO("2smallest " << eit << ", " << weight); + smallestEdge = eit; + smallestWeight = weight; + } + } + } + assert(smallestWeight != INT_MAX); + + const typename QueryDataT::Graph::EdgeData& ed = _queryData.graph->GetEdgeData(smallestEdge); + if(ed.shortcut) {//unpack + const NodeID middle = ed.id; + //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.id, _queryData.nodeHelpDesk->getNameIndexFromEdgeID(ed.id), _queryData.nodeHelpDesk->getTurnInstructionFromEdgeID(ed.id), ed.distance) ); + } + } + } + + inline void RetrievePackedPathFromHeap(typename QueryDataT::HeapPtr & _fHeap, typename QueryDataT::HeapPtr & _bHeap, const NodeID middle, std::deque& packedPath) { + NodeID pathNode = middle; + if(_fHeap->GetData(pathNode).parent != middle) { + do { + pathNode = _fHeap->GetData(pathNode).parent; + + packedPath.push_front(pathNode); + }while(pathNode != _fHeap->GetData(pathNode).parent); + } + packedPath.push_back(middle); + pathNode = middle; + if(_bHeap->GetData(pathNode).parent != middle) { + do{ + pathNode = _bHeap->GetData(pathNode).parent; + packedPath.push_back(pathNode); + } while (pathNode != _bHeap->GetData(pathNode).parent); + } +// std::cout << "unpacking: "; +// for(std::deque::iterator it = packedPath.begin(); it != packedPath.end(); ++it) +// std::cout << *it << " "; +// std::cout << std::endl; + } +}; + + +#endif /* BASICROUTINGINTERFACE_H_ */ diff --git a/RoutingAlgorithms/ShortestPathRouting.h b/RoutingAlgorithms/ShortestPathRouting.h new file mode 100644 index 000000000..1ab90cab7 --- /dev/null +++ b/RoutingAlgorithms/ShortestPathRouting.h @@ -0,0 +1,254 @@ +/* + open source routing machine + Copyright (C) Dennis Luxen, others 2010 + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU AFFERO General Public License as published by +the Free Software Foundation; either version 3 of the License, or +any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU Affero General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +or see http://www.gnu.org/licenses/agpl.txt. + */ + + + +#ifndef SHORTESTPATHROUTING_H_ +#define SHORTESTPATHROUTING_H_ + +#include "BasicRoutingInterface.h" + +template +class ShortestPathRouting : public BasicRoutingInterface{ + typedef BasicRoutingInterface super; +public: + ShortestPathRouting(QueryDataT & qd) : super(qd) {} + + ~ShortestPathRouting() {} + + int operator()(std::vector & phantomNodesVector, std::vector<_PathData> & unpackedPath) { + BOOST_FOREACH(PhantomNodes & phantomNodePair, phantomNodesVector) { + if(!phantomNodePair.AtLeastOnePhantomNodeIsUINTMAX()) + return INT_MAX; + } + int distance1 = 0; + int distance2 = 0; + + bool searchFrom1stStartNode(true); + bool searchFrom2ndStartNode(true); + NodeID middle1 = ( NodeID ) UINT_MAX; + NodeID middle2 = ( NodeID ) UINT_MAX; + std::deque packedPath1; + std::deque packedPath2; + + typename QueryDataT::HeapPtr & forwardHeap = super::_queryData.forwardHeap; + typename QueryDataT::HeapPtr & backwardHeap = super::_queryData.backwardHeap; + + typename QueryDataT::HeapPtr & forwardHeap2 = super::_queryData.forwardHeap2; + typename QueryDataT::HeapPtr & backwardHeap2 = super::_queryData.backwardHeap2; + + + //Get distance to next pair of target nodes. + BOOST_FOREACH(PhantomNodes & phantomNodePair, phantomNodesVector) { + super::_queryData.InitializeOrClearFirstThreadLocalStorage(); + super::_queryData.InitializeOrClearSecondThreadLocalStorage(); + + int _localUpperbound1 = INT_MAX; + int _localUpperbound2 = INT_MAX; + + //insert new starting nodes into forward heap, adjusted by previous distances. + if(searchFrom1stStartNode) { + forwardHeap->Insert(phantomNodePair.startPhantom.edgeBasedNode, -phantomNodePair.startPhantom.weight1, phantomNodePair.startPhantom.edgeBasedNode); + forwardHeap2->Insert(phantomNodePair.startPhantom.edgeBasedNode, -phantomNodePair.startPhantom.weight1, phantomNodePair.startPhantom.edgeBasedNode); +// INFO("a 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, -phantomNodePair.startPhantom.weight2, phantomNodePair.startPhantom.edgeBasedNode+1); + forwardHeap2->Insert(phantomNodePair.startPhantom.edgeBasedNode+1, -phantomNodePair.startPhantom.weight2, phantomNodePair.startPhantom.edgeBasedNode+1); +// INFO("b 1,2)forw insert " << phantomNodePair.startPhantom.edgeBasedNode+1 << " with weight " << -phantomNodePair.startPhantom.weight1); +// } else if(!searchFrom2ndStartNode) { +// INFO("Skipping second start node"); + } + +// 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) ; + + //run two-Target Dijkstra routing step. + while(forwardHeap->Size() + backwardHeap->Size() > 0){ + if(forwardHeap->Size() > 0){ + super::RoutingStep(forwardHeap, backwardHeap, &middle1, &_localUpperbound1, 2*offset, true); + } + if(backwardHeap->Size() > 0){ + super::RoutingStep(backwardHeap, forwardHeap, &middle1, &_localUpperbound1, 2*offset, false); + } + } + if(backwardHeap2->Size() > 0) { + while(forwardHeap2->Size() + backwardHeap2->Size() > 0){ + if(forwardHeap2->Size() > 0){ + super::RoutingStep(forwardHeap2, backwardHeap2, &middle2, &_localUpperbound2, 2*offset, true); + } + if(backwardHeap2->Size() > 0){ + super::RoutingStep(backwardHeap2, forwardHeap2, &middle2, &_localUpperbound2, 2*offset, false); + } + } + } +// INFO("upperbound1: " << _localUpperbound1 << ", distance1: " << distance1); +// INFO("upperbound2: " << _localUpperbound2 << ", distance2: " << distance2); + + //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"); + } + + //Was at most one of the two paths not found? + assert(!(INT_MAX == distance1 && INT_MAX == distance2)); + +// INFO("middle1: " << middle1); + + //Unpack paths if they exist + std::deque temporaryPackedPath1; + std::deque temporaryPackedPath2; + if(INT_MAX != _localUpperbound1) { + super::RetrievePackedPathFromHeap(forwardHeap, backwardHeap, middle1, temporaryPackedPath1); +// INFO("temporaryPackedPath1 ends with " << *(temporaryPackedPath1.end()-1) ); + } +// INFO("middle2: " << middle2); + + if(INT_MAX != _localUpperbound2) { + super::RetrievePackedPathFromHeap(forwardHeap2, backwardHeap2, middle2, temporaryPackedPath2); +// INFO("temporaryPackedPath2 ends with " << *(temporaryPackedPath2.end()-1) ); + } + + //if one of the paths was not found, replace it with the other one. + if(0 == temporaryPackedPath1.size()) { +// INFO("Deleting path 1"); + temporaryPackedPath1.insert(temporaryPackedPath1.end(), temporaryPackedPath2.begin(), temporaryPackedPath2.end()); + _localUpperbound1 = _localUpperbound2; + } + if(0 == temporaryPackedPath2.size()) { +// INFO("Deleting path 2"); + temporaryPackedPath2.insert(temporaryPackedPath2.end(), temporaryPackedPath1.begin(), temporaryPackedPath1.end()); + _localUpperbound2 = _localUpperbound1; + } + + assert(0 < temporaryPackedPath1.size() && 0 < temporaryPackedPath2.size()); + + //Plug paths together, s.t. end of packed path is begin of temporary packed path + if(0 < packedPath1.size() && 0 < packedPath2.size() ) { +// INFO("Both paths are non-empty"); + if( *(temporaryPackedPath1.begin()) == *(temporaryPackedPath2.begin())) { +// INFO("both paths start with the same node:" << *(temporaryPackedPath1.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())) { +// INFO("Deleting packedPath2 that ends with " << *(packedPath2.end()-1) << ", other ends with " << *(packedPath1.end()-1)); + packedPath2.clear(); + packedPath2.insert(packedPath2.end(), packedPath1.begin(), packedPath1.end()); + distance2 = distance1; +// INFO("packedPath2 now ends with " << *(packedPath2.end()-1)); + } else { +// INFO("Deleting path1 that ends with " << *(packedPath1.end()-1) << ", other ends with " << *(packedPath2.end()-1)); + packedPath1.clear(); + packedPath1.insert(packedPath1.end(), packedPath2.begin(), packedPath2.end()); + distance1 = distance2; +// INFO("Path1 now ends with " << *(packedPath1.end()-1)); + } + } else { + //packed paths 1 and 2 may need to switch. + if(*(packedPath1.end()-1) != *(temporaryPackedPath1.begin())) { +// INFO("Switching"); + packedPath1.swap(packedPath2); + std::swap(distance1, distance2); + } + } + } + packedPath1.insert(packedPath1.end(), temporaryPackedPath1.begin(), temporaryPackedPath1.end()); + packedPath2.insert(packedPath2.end(), temporaryPackedPath2.begin(), temporaryPackedPath2.end()); + + if( (packedPath1.back() == packedPath2.back()) && phantomNodePair.targetPhantom.isBidirected() ) { +// INFO("both paths end in same direction on bidirected edge, make sure start only start with : " << packedPath1.back()); + + NodeID lastNodeID = packedPath2.back(); + searchFrom1stStartNode &= !(lastNodeID == phantomNodePair.targetPhantom.edgeBasedNode+1); + searchFrom2ndStartNode &= !(lastNodeID == phantomNodePair.targetPhantom.edgeBasedNode); +// INFO("Next search from node " << phantomNodePair.targetPhantom.edgeBasedNode << ": " << (searchFrom1stStartNode ? "yes" : "no") ); +// INFO("Next search from node " << phantomNodePair.targetPhantom.edgeBasedNode+1 << ": " << (searchFrom2ndStartNode ? "yes" : "no") ); + } + + distance1 += _localUpperbound1; + distance2 += _localUpperbound2; + } + +// 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; + super::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; + super::UnpackPath(packedPath2, unpackedPath); + } +// INFO("Found via route with distance " << std::min(distance1, distance2)); + return std::min(distance1, distance2); + } +private: + 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()); + } +}; + +#endif /* SHORTESTPATHROUTING_H_ */