Open-Harbor/osrm-backend
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 3 Wiki Activity

First (buggy) implementation of single via node alternative routes.

Browse Source 
Proof of concept which is imcomplete.
This commit is contained in:
DennisOSRM 2012-06-15 18:47:27 +02:00
parent fa62f70cdf
commit 7f0d40f459
7 changed files with 1008 additions and 479 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Algorithms/ObjectToBase64.h
View File

@ -43,7 +43,7 @@ typedef
template<class ToEncodeT>
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 "_"

2
DataStructures/QueryEdge.h
View File

@ -72,6 +72,4 @@ struct QueryEdge {
}
};
#endif /* QUERYEDGE_H_ */

530
DataStructures/SearchEngine.h
View File

@ -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<BinaryHeap< NodeID, NodeID, int, _HeapData, UnorderedMapStorage<NodeID, int> > > HeapPtr;
typedef boost::thread_specific_ptr<BinaryHeap< NodeID, NodeID, int, _HeapData, UnorderedMapStorage<NodeID, int> > > SearchEngineHeapPtr;
template<class EdgeData, class GraphT>
struct SearchEngineData {
typedef SearchEngineHeapPtr HeapPtr;
typedef GraphT Graph;
SearchEngineData(GraphT * g, NodeInformationHelpDesk * nh, std::vector<string> & n) :graph(g), nodeHelpDesk(nh), names(n) {}
const GraphT * graph;
NodeInformationHelpDesk * nodeHelpDesk;
std::vector<string> & 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<NodeID, int> >(nodeHelpDesk->getNumberOfNodes()));
}
else
forwardHeap->Clear();
if(!backwardHeap.get()) {
backwardHeap.reset(new BinaryHeap< NodeID, NodeID, int, _HeapData, UnorderedMapStorage<NodeID, int> >(nodeHelpDesk->getNumberOfNodes()));
}
else
backwardHeap->Clear();
}
inline void InitializeOrClearSecondThreadLocalStorage() {
if(!forwardHeap2.get()) {
forwardHeap2.reset(new BinaryHeap< NodeID, NodeID, int, _HeapData, UnorderedMapStorage<NodeID, int> >(nodeHelpDesk->getNumberOfNodes()));
}
else
forwardHeap2->Clear();
if(!backwardHeap2.get()) {
backwardHeap2.reset(new BinaryHeap< NodeID, NodeID, int, _HeapData, UnorderedMapStorage<NodeID, int> >(nodeHelpDesk->getNumberOfNodes()));
}
else
backwardHeap2->Clear();
}
};
template<class EdgeData, class GraphT>
class SearchEngine {
private:
const GraphT * _graph;
NodeInformationHelpDesk * nodeHelpDesk;
std::vector<string> & _names;
static HeapPtr _forwardHeap;
static HeapPtr _backwardHeap;
static HeapPtr _forwardHeap2;
static HeapPtr _backwardHeap2;
typedef SearchEngineData<EdgeData, GraphT> 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<string> & n) : _graph(g), nodeHelpDesk(nh), _names(n) {}
ShortestPathRouting<SearchEngineDataT> shortestPath;
AlternativeRouting<SearchEngineDataT> alternativePaths;
SearchEngine(GraphT * g, NodeInformationHelpDesk * nh, std::vector<string> & 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<NodeID, int> >(nodeHelpDesk->getNumberOfNodes()));
}
else
_forwardHeap->Clear();
if(!_backwardHeap.get()) {
_backwardHeap.reset(new BinaryHeap< NodeID, NodeID, int, _HeapData, UnorderedMapStorage<NodeID, int> >(nodeHelpDesk->getNumberOfNodes()));
}
else
_backwardHeap->Clear();
}
inline void InitializeThreadLocalViaStorageIfNecessary() {
if(!_forwardHeap2.get()) {
_forwardHeap2.reset(new BinaryHeap< NodeID, NodeID, int, _HeapData, UnorderedMapStorage<NodeID, int> >(nodeHelpDesk->getNumberOfNodes()));
}
else
_forwardHeap2->Clear();
if(!_backwardHeap2.get()) {
_backwardHeap2.reset(new BinaryHeap< NodeID, NodeID, int, _HeapData, UnorderedMapStorage<NodeID, int> >(nodeHelpDesk->getNumberOfNodes()));
}
else
_backwardHeap2->Clear();
}
template<class ContainerT>
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<PhantomNodes> & 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<NodeID> packedPath1;
std::deque<NodeID> 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<true>(_forwardHeap, _backwardHeap, &middle1, &_localUpperbound1, 2*offset);
}
if(_backwardHeap->Size() > 0){
_RoutingStep<false>(_backwardHeap, _forwardHeap, &middle1, &_localUpperbound1, 2*offset);
}
}
if(_backwardHeap2->Size() > 0) {
while(_forwardHeap2->Size() + _backwardHeap2->Size() > 0){
if(_forwardHeap2->Size() > 0){
_RoutingStep<true>(_forwardHeap2, _backwardHeap2, &middle2, &_localUpperbound2, 2*offset);
}
if(_backwardHeap2->Size() > 0){
_RoutingStep<false>(_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<NodeID> temporaryPackedPath1;
std::deque<NodeID> 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<true>(_forwardHeap, _backwardHeap, &middle, &_upperbound, 2*offset);
}
if(_backwardHeap->Size() > 0){
_RoutingStep<false>(_backwardHeap, _forwardHeap, &middle, &_upperbound, 2*offset);
}
}
// INFO("dist: " << _upperbound);
if ( _upperbound == INT_MAX ) {
return _upperbound;
}
std::deque<NodeID> 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<NodeID>& 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<NodeID>::iterator it = packedPath.begin(); it != packedPath.end(); ++it)
// std::cout << *it << " ";
// std::cout << std::endl;
}
template<bool forwardDirection>
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<NodeID> & packedPath, std::vector<_PathData> & unpackedPath) const {
const unsigned sizeOfPackedPath = packedPath.size();
std::stack<std::pair<NodeID, NodeID> > 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<NodeID, NodeID> 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<class EdgeData, class GraphT> HeapPtr SearchEngine<EdgeData, GraphT>::_forwardHeap;
template<class EdgeData, class GraphT> HeapPtr SearchEngine<EdgeData, GraphT>::_backwardHeap;
template<class EdgeData, class GraphT> HeapPtr SearchEngine<EdgeData, GraphT>::_forwardHeap2;
template<class EdgeData, class GraphT> HeapPtr SearchEngine<EdgeData, GraphT>::_backwardHeap2;
template<class EdgeData, class GraphT> SearchEngineHeapPtr SearchEngineData<EdgeData, GraphT>::forwardHeap;
template<class EdgeData, class GraphT> SearchEngineHeapPtr SearchEngineData<EdgeData, GraphT>::backwardHeap;
template<class EdgeData, class GraphT> SearchEngineHeapPtr SearchEngineData<EdgeData, GraphT>::forwardHeap2;
template<class EdgeData, class GraphT> SearchEngineHeapPtr SearchEngineData<EdgeData, GraphT>::backwardHeap2;
#endif /* SEARCHENGINE_H_ */

45
Plugins/ViaRoutePlugin.h
View File

@ -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<std::string> 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 <alternative.size(); ++i) {
_Coordinate current;
searchEngine->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" );
}

470
RoutingAlgorithms/AlternativePathRouting.h Normal file
View File

@ -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 <cmath>
#include "BasicRoutingInterface.h"
template<class QueryDataT>
class AlternativeRouting : private BasicRoutingInterface<QueryDataT>{
typedef BasicRoutingInterface<QueryDataT> super;
typedef std::pair<NodeID, int> 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<NodeID> alternativePath;
std::vector<NodeID> 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<RankedCandidateNode > 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 <s,..,v> and <v,..,t> 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<NodeID> 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 <s,..,v> and <v,..,t> 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 <s,..,v> 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 <s,..,v>: " << upperBoundFor_s_v_Path << " with middle node " << s_v_middle << std::endl;
//compute path <v,..,t> 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 <v,..,t>: " << 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<NodeID> packedShortestPath;
std::deque<NodeID> packed_s_v_path;
std::deque<NodeID> 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<NodeID, NodeID> UnpackEdge;
std::stack<UnpackEdge > 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") << "(" <<middleOfShortestPath << ") , via shrtcut: "
<< (IsViaEdgeShortCut ? "yes" : "no") << "(" << middleOfViaPath << ")" << std::endl;
if(IsShortestPathEdgeShortCut || IsViaEdgeShortCut) {
if(middleOfShortestPath == middleOfViaPath) { // unpack first segment
//put first segment of via edge on stack, else take the segment already available
std::cout << " unpacking first segment" << std::endl;
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);
//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;
} 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<NodeID> packedShortestPath;
std::deque<NodeID> 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<NodeID>& 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_ */

184
RoutingAlgorithms/BasicRoutingInterface.h Normal file
View File

@ -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 <cassert>
#include <climits>
template<class QueryDataT>
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<NodeID> & packedPath, std::vector<_PathData> & unpackedPath) const {
const unsigned sizeOfPackedPath = packedPath.size();
std::stack<std::pair<NodeID, NodeID> > 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<NodeID, NodeID> 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<NodeID>& 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<NodeID>::iterator it = packedPath.begin(); it != packedPath.end(); ++it)
// std::cout << *it << " ";
// std::cout << std::endl;
}
};
#endif /* BASICROUTINGINTERFACE_H_ */

254
RoutingAlgorithms/ShortestPathRouting.h Normal file
View File

@ -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 QueryDataT>
class ShortestPathRouting : public BasicRoutingInterface<QueryDataT>{
typedef BasicRoutingInterface<QueryDataT> super;
public:
ShortestPathRouting(QueryDataT & qd) : super(qd) {}
~ShortestPathRouting() {}
int operator()(std::vector<PhantomNodes> & 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<NodeID> packedPath1;
std::deque<NodeID> 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<NodeID> temporaryPackedPath1;
std::deque<NodeID> 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<class ContainerT>
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_ */