osrm-backend/RoutingAlgorithms/ShortestPathRouting.h

262 lines
14 KiB
C++

/*
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() {}
void operator()(std::vector<PhantomNodes> & phantomNodesVector, RawRouteData & rawRouteData) {
BOOST_FOREACH(PhantomNodes & phantomNodePair, phantomNodesVector) {
if(!phantomNodePair.AtLeastOnePhantomNodeIsUINTMAX()) {
rawRouteData.lengthOfShortestPath = rawRouteData.lengthOfAlternativePath = INT_MAX;
return;
}
}
int distance1 = 0;
int distance2 = 0;
bool searchFrom1stStartNode(true);
bool searchFrom2ndStartNode(true);
NodeID middle1 = ( NodeID ) UINT_MAX;
NodeID middle2 = ( NodeID ) UINT_MAX;
std::vector<NodeID> packedPath1;
std::vector<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) {
rawRouteData.lengthOfShortestPath = rawRouteData.lengthOfAlternativePath = INT_MAX;
return;
}
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::vector<NodeID> temporaryPackedPath1;
std::vector<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;
// std::cout << "cleaned 1: ";
// for(unsigned i = 0; i < packedPath1.size(); ++i)
// std::cout << packedPath1[i] << " ";
// std::cout << std::endl;
// super::UnpackPath(packedPath1, rawRouteData.computedShortestPath);
} else {
std::swap(packedPath1, packedPath2);
// 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);
}
_RemoveConsecutiveDuplicatesFromContainer(packedPath1);
super::UnpackPath(packedPath1, rawRouteData.computedShortestPath);
rawRouteData.lengthOfShortestPath = std::min(distance1, distance2);
// INFO("Found via route with distance " << std::min(distance1, distance2));
return;
}
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_ */