383 lines
16 KiB
C++
383 lines
16 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 STRONGLYCONNECTEDCOMPONENTS_H_
|
|
#define STRONGLYCONNECTEDCOMPONENTS_H_
|
|
|
|
#include <cassert>
|
|
|
|
#include <stack>
|
|
#include <vector>
|
|
|
|
#include <boost/foreach.hpp>
|
|
#include <boost/make_shared.hpp>
|
|
#include <boost/unordered_map.hpp>
|
|
|
|
#include <gdal/gdal.h>
|
|
#include <gdal/ogrsf_frmts.h>
|
|
|
|
#include "../DataStructures/DeallocatingVector.h"
|
|
#include "../DataStructures/DynamicGraph.h"
|
|
#include "../DataStructures/ImportEdge.h"
|
|
#include "../DataStructures/NodeCoords.h"
|
|
#include "../DataStructures/Percent.h"
|
|
#include "../DataStructures/Restriction.h"
|
|
|
|
|
|
// Strongly connected components using Tarjan's Algorithm
|
|
|
|
class TarjanSCC {
|
|
private:
|
|
struct _NodeBasedEdgeData {
|
|
int distance;
|
|
unsigned edgeBasedNodeID;
|
|
unsigned nameID:31;
|
|
bool shortcut:1;
|
|
bool forward:1;
|
|
bool backward:1;
|
|
bool roundabout:1;
|
|
bool ignoreInGrid:1;
|
|
short type;
|
|
bool isAccessRestricted;
|
|
};
|
|
|
|
struct _EdgeBasedEdgeData {
|
|
int distance;
|
|
unsigned via;
|
|
unsigned nameID;
|
|
bool forward;
|
|
bool backward;
|
|
short turnInstruction;
|
|
};
|
|
|
|
typedef DynamicGraph< _NodeBasedEdgeData > _NodeBasedDynamicGraph;
|
|
typedef _NodeBasedDynamicGraph::InputEdge _NodeBasedEdge;
|
|
std::vector<NodeInfo> inputNodeInfoList;
|
|
unsigned numberOfTurnRestrictions;
|
|
boost::shared_ptr<_NodeBasedDynamicGraph> _nodeBasedGraph;
|
|
boost::unordered_map<NodeID, bool> _barrierNodes;
|
|
boost::unordered_map<NodeID, bool> _trafficLights;
|
|
|
|
typedef std::pair<NodeID, NodeID> RestrictionSource;
|
|
typedef std::pair<NodeID, bool> RestrictionTarget;
|
|
typedef std::vector<RestrictionTarget> EmanatingRestrictionsVector;
|
|
typedef boost::unordered_map<RestrictionSource, unsigned > RestrictionMap;
|
|
std::vector<EmanatingRestrictionsVector> _restrictionBucketVector;
|
|
RestrictionMap _restrictionMap;
|
|
|
|
|
|
public:
|
|
struct EdgeBasedNode {
|
|
bool operator<(const EdgeBasedNode & other) const {
|
|
return other.id < id;
|
|
}
|
|
bool operator==(const EdgeBasedNode & other) const {
|
|
return id == other.id;
|
|
}
|
|
NodeID id;
|
|
int lat1;
|
|
int lat2;
|
|
int lon1;
|
|
int lon2:31;
|
|
bool belongsToTinyComponent:1;
|
|
NodeID nameID;
|
|
unsigned weight:31;
|
|
bool ignoreInGrid:1;
|
|
};
|
|
|
|
private:
|
|
DeallocatingVector<EdgeBasedNode> edgeBasedNodes;
|
|
|
|
struct TarjanNode {
|
|
TarjanNode() : index(UINT_MAX), lowlink(UINT_MAX), onStack(false) {}
|
|
unsigned index;
|
|
unsigned lowlink;
|
|
bool onStack;
|
|
};
|
|
|
|
struct TarjanStackFrame {
|
|
explicit TarjanStackFrame(NodeID _v, NodeID p) : v(_v), parent(p) {}
|
|
NodeID v;
|
|
NodeID parent;
|
|
};
|
|
public:
|
|
TarjanSCC(int nodes, std::vector<NodeBasedEdge> & inputEdges, std::vector<NodeID> & bn, std::vector<NodeID> & tl, std::vector<_Restriction> & irs, std::vector<NodeInfo> & nI) : inputNodeInfoList(nI), numberOfTurnRestrictions(irs.size()) {
|
|
BOOST_FOREACH(_Restriction & restriction, irs) {
|
|
std::pair<NodeID, NodeID> restrictionSource = std::make_pair(restriction.fromNode, restriction.viaNode);
|
|
unsigned index;
|
|
RestrictionMap::iterator restrIter = _restrictionMap.find(restrictionSource);
|
|
if(restrIter == _restrictionMap.end()) {
|
|
index = _restrictionBucketVector.size();
|
|
_restrictionBucketVector.resize(index+1);
|
|
_restrictionMap[restrictionSource] = index;
|
|
} else {
|
|
index = restrIter->second;
|
|
//Map already contains an is_only_*-restriction
|
|
if(_restrictionBucketVector.at(index).begin()->second)
|
|
continue;
|
|
else if(restriction.flags.isOnly){
|
|
//We are going to insert an is_only_*-restriction. There can be only one.
|
|
_restrictionBucketVector.at(index).clear();
|
|
}
|
|
}
|
|
|
|
_restrictionBucketVector.at(index).push_back(std::make_pair(restriction.toNode, restriction.flags.isOnly));
|
|
}
|
|
|
|
BOOST_FOREACH(NodeID id, bn) {
|
|
_barrierNodes[id] = true;
|
|
}
|
|
BOOST_FOREACH(NodeID id, tl) {
|
|
_trafficLights[id] = true;
|
|
}
|
|
|
|
DeallocatingVector< _NodeBasedEdge > edges;
|
|
for ( std::vector< NodeBasedEdge >::const_iterator i = inputEdges.begin(); i != inputEdges.end(); ++i ) {
|
|
|
|
_NodeBasedEdge edge;
|
|
if(!i->isForward()) {
|
|
edge.source = i->target();
|
|
edge.target = i->source();
|
|
edge.data.backward = i->isForward();
|
|
edge.data.forward = i->isBackward();
|
|
} else {
|
|
edge.source = i->source();
|
|
edge.target = i->target();
|
|
edge.data.forward = i->isForward();
|
|
edge.data.backward = i->isBackward();
|
|
}
|
|
if(edge.source == edge.target)
|
|
continue;
|
|
|
|
edge.data.distance = (std::max)((int)i->weight(), 1 );
|
|
assert( edge.data.distance > 0 );
|
|
edge.data.shortcut = false;
|
|
edge.data.roundabout = i->isRoundabout();
|
|
edge.data.ignoreInGrid = i->ignoreInGrid();
|
|
edge.data.nameID = i->name();
|
|
edge.data.type = i->type();
|
|
edge.data.isAccessRestricted = i->isAccessRestricted();
|
|
edge.data.edgeBasedNodeID = edges.size();
|
|
edges.push_back( edge );
|
|
if( edge.data.backward ) {
|
|
std::swap( edge.source, edge.target );
|
|
edge.data.forward = i->isBackward();
|
|
edge.data.backward = i->isForward();
|
|
edge.data.edgeBasedNodeID = edges.size();
|
|
edges.push_back( edge );
|
|
}
|
|
}
|
|
std::vector<NodeBasedEdge>().swap(inputEdges);
|
|
std::sort( edges.begin(), edges.end() );
|
|
_nodeBasedGraph = boost::make_shared<_NodeBasedDynamicGraph>( nodes, edges );
|
|
}
|
|
|
|
void Run() {
|
|
Percent p(_nodeBasedGraph->GetNumberOfNodes());
|
|
|
|
const char *pszDriverName = "ESRI Shapefile";
|
|
OGRSFDriver *poDriver;
|
|
|
|
OGRRegisterAll();
|
|
|
|
poDriver = OGRSFDriverRegistrar::GetRegistrar()->GetDriverByName(
|
|
pszDriverName );
|
|
if( poDriver == NULL )
|
|
{
|
|
printf( "%s driver not available.\n", pszDriverName );
|
|
exit( 1 );
|
|
}
|
|
OGRDataSource *poDS;
|
|
|
|
poDS = poDriver->CreateDataSource( "component.shp", NULL );
|
|
if( poDS == NULL ) {
|
|
printf( "Creation of output file failed.\n" );
|
|
exit( 1 );
|
|
}
|
|
|
|
OGRLayer *poLayer;
|
|
|
|
poLayer = poDS->CreateLayer( "component", NULL, wkbLineString, NULL );
|
|
if( poLayer == NULL ) {
|
|
printf( "Layer creation failed.\n" );
|
|
exit( 1 );
|
|
}
|
|
|
|
|
|
//The following is a hack to distinguish between stuff that happens before the recursive call and stuff that happens after
|
|
std::stack<std::pair<bool, TarjanStackFrame> > recursionStack; //true = stuff before, false = stuff after call
|
|
std::stack<NodeID> tarjanStack;
|
|
std::vector<unsigned> componentsIndex(_nodeBasedGraph->GetNumberOfNodes(), UINT_MAX);
|
|
std::vector<NodeID> vectorOfComponentSizes;
|
|
std::vector<TarjanNode> tarjanNodes(_nodeBasedGraph->GetNumberOfNodes());
|
|
unsigned currentComponent = 0, sizeOfCurrentComponent = 0;
|
|
int index = 0;
|
|
for(NodeID node = 0, endNodes = _nodeBasedGraph->GetNumberOfNodes(); node < endNodes; ++node) {
|
|
if(UINT_MAX == componentsIndex[node]) {
|
|
recursionStack.push(std::make_pair(true, TarjanStackFrame(node,node)) );
|
|
}
|
|
|
|
while(!recursionStack.empty()) {
|
|
bool beforeRecursion = recursionStack.top().first;
|
|
TarjanStackFrame currentFrame = recursionStack.top().second;
|
|
NodeID v = currentFrame.v;
|
|
// INFO("popping node " << v << (beforeRecursion ? " before " : " after ") << "recursion");
|
|
recursionStack.pop();
|
|
|
|
if(beforeRecursion) {
|
|
//Mark frame to handle tail of recursion
|
|
recursionStack.push(std::make_pair(false, currentFrame));
|
|
|
|
//Mark essential information for SCC
|
|
tarjanNodes[v].index = index;
|
|
tarjanNodes[v].lowlink = index;
|
|
tarjanStack.push(v);
|
|
tarjanNodes[v].onStack = true;
|
|
++index;
|
|
// INFO("pushing " << v << " onto tarjan stack, idx[" << v << "]=" << tarjanNodes[v].index << ", lowlink["<< v << "]=" << tarjanNodes[v].lowlink);
|
|
|
|
//Traverse outgoing edges
|
|
for(_NodeBasedDynamicGraph::EdgeIterator e2 = _nodeBasedGraph->BeginEdges(v); e2 < _nodeBasedGraph->EndEdges(v); ++e2) {
|
|
_NodeBasedDynamicGraph::NodeIterator vprime = _nodeBasedGraph->GetTarget(e2);
|
|
// INFO("traversing edge (" << v << "," << vprime << ")");
|
|
if(UINT_MAX == tarjanNodes[vprime].index) {
|
|
|
|
recursionStack.push(std::make_pair(true,TarjanStackFrame(vprime, v)));
|
|
} else {
|
|
// INFO("Node " << vprime << " is already explored");
|
|
if(tarjanNodes[vprime].onStack) {
|
|
unsigned newLowlink = std::min(tarjanNodes[v].lowlink, tarjanNodes[vprime].index);
|
|
// INFO("Setting lowlink[" << v << "] from " << tarjanNodes[v].lowlink << " to " << newLowlink);
|
|
tarjanNodes[v].lowlink = newLowlink;
|
|
// } else {
|
|
// INFO("But node " << vprime << " is not on stack");
|
|
}
|
|
}
|
|
}
|
|
} else {
|
|
|
|
// INFO("we are at the end of recursion and checking node " << v);
|
|
{ // setting lowlink in its own scope so it does not pollute namespace
|
|
// NodeID parent = (UINT_MAX == tarjanNodes[v].parent ? v : tarjanNodes[v].parent );
|
|
// INFO("parent=" << currentFrame.parent);
|
|
// INFO("tarjanNodes[" << v << "].lowlink=" << tarjanNodes[v].lowlink << ", tarjanNodes[" << currentFrame.parent << "].lowlink=" << tarjanNodes[currentFrame.parent].lowlink);
|
|
//Note the index shift by 1 compared to the recursive version
|
|
tarjanNodes[currentFrame.parent].lowlink = std::min(tarjanNodes[currentFrame.parent].lowlink, tarjanNodes[v].lowlink);
|
|
// INFO("Setting tarjanNodes[" << currentFrame.parent <<"].lowlink=" << tarjanNodes[currentFrame.parent].lowlink);
|
|
}
|
|
// INFO("tarjanNodes[" << v << "].lowlink=" << tarjanNodes[v].lowlink << ", tarjanNodes[" << v << "].index=" << tarjanNodes[v].index);
|
|
|
|
//after recursion, lets do cycle checking
|
|
//Check if we found a cycle. This is the bottom part of the recursion
|
|
if(tarjanNodes[v].lowlink == tarjanNodes[v].index) {
|
|
NodeID vprime;
|
|
do {
|
|
// INFO("identified component " << currentComponent << ": " << tarjanStack.top());
|
|
vprime = tarjanStack.top(); tarjanStack.pop();
|
|
tarjanNodes[vprime].onStack = false;
|
|
componentsIndex[vprime] = currentComponent;
|
|
++sizeOfCurrentComponent;
|
|
} while( v != vprime);
|
|
vectorOfComponentSizes.push_back(sizeOfCurrentComponent);
|
|
if(sizeOfCurrentComponent > 1000)
|
|
INFO("large component [" << currentComponent << "]=" << sizeOfCurrentComponent);
|
|
++currentComponent;
|
|
sizeOfCurrentComponent = 0;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
INFO("identified: " << vectorOfComponentSizes.size() << " many components, marking small components");
|
|
|
|
int singleCounter = 0;
|
|
for(unsigned i = 0; i < vectorOfComponentSizes.size(); ++i){
|
|
if(1 == vectorOfComponentSizes[i])
|
|
++singleCounter;
|
|
}
|
|
INFO("identified " << singleCounter << " SCCs of size 1");
|
|
|
|
p.reinit(_nodeBasedGraph->GetNumberOfNodes());
|
|
for(_NodeBasedDynamicGraph::NodeIterator u = 0; u < _nodeBasedGraph->GetNumberOfNodes(); ++u ) {
|
|
for(_NodeBasedDynamicGraph::EdgeIterator e1 = _nodeBasedGraph->BeginEdges(u); e1 < _nodeBasedGraph->EndEdges(u); ++e1) {
|
|
_NodeBasedDynamicGraph::NodeIterator v = _nodeBasedGraph->GetTarget(e1);
|
|
|
|
if(_nodeBasedGraph->GetEdgeData(e1).type != SHRT_MAX) {
|
|
assert(e1 != UINT_MAX);
|
|
assert(u != UINT_MAX);
|
|
assert(v != UINT_MAX);
|
|
//edges that end on bollard nodes may actually be in two distinct components
|
|
if(std::min(vectorOfComponentSizes[componentsIndex[u]], vectorOfComponentSizes[componentsIndex[v]]) < 10) {
|
|
|
|
//INFO("(" << inputNodeInfoList[u].lat/100000. << ";" << inputNodeInfoList[u].lon/100000. << ") -> (" << inputNodeInfoList[v].lat/100000. << ";" << inputNodeInfoList[v].lon/100000. << ")");
|
|
OGRLineString lineString;
|
|
lineString.addPoint(inputNodeInfoList[u].lon/100000., inputNodeInfoList[u].lat/100000.);
|
|
lineString.addPoint(inputNodeInfoList[v].lon/100000., inputNodeInfoList[v].lat/100000.);
|
|
|
|
OGRFeature *poFeature;
|
|
poFeature = OGRFeature::CreateFeature( poLayer->GetLayerDefn() );
|
|
poFeature->SetGeometry( &lineString );
|
|
if( poLayer->CreateFeature( poFeature ) != OGRERR_NONE )
|
|
{
|
|
ERR( "Failed to create feature in shapefile.\n" );
|
|
}
|
|
OGRFeature::DestroyFeature( poFeature );
|
|
}
|
|
}
|
|
}
|
|
}
|
|
OGRDataSource::DestroyDataSource( poDS );
|
|
std::vector<NodeID>().swap(vectorOfComponentSizes);
|
|
std::vector<NodeID>().swap(componentsIndex);
|
|
|
|
}
|
|
private:
|
|
unsigned CheckForEmanatingIsOnlyTurn(const NodeID u, const NodeID v) const {
|
|
std::pair < NodeID, NodeID > restrictionSource = std::make_pair(u, v);
|
|
RestrictionMap::const_iterator restrIter = _restrictionMap.find(restrictionSource);
|
|
if (restrIter != _restrictionMap.end()) {
|
|
unsigned index = restrIter->second;
|
|
BOOST_FOREACH(RestrictionSource restrictionTarget, _restrictionBucketVector.at(index)) {
|
|
if(restrictionTarget.second) {
|
|
return restrictionTarget.first;
|
|
}
|
|
}
|
|
}
|
|
return UINT_MAX;
|
|
}
|
|
bool CheckIfTurnIsRestricted(const NodeID u, const NodeID v, const NodeID w) const {
|
|
//only add an edge if turn is not a U-turn except it is the end of dead-end street.
|
|
std::pair < NodeID, NodeID > restrictionSource = std::make_pair(u, v);
|
|
RestrictionMap::const_iterator restrIter = _restrictionMap.find(restrictionSource);
|
|
if (restrIter != _restrictionMap.end()) {
|
|
unsigned index = restrIter->second;
|
|
BOOST_FOREACH(RestrictionTarget restrictionTarget, _restrictionBucketVector.at(index)) {
|
|
if(w == restrictionTarget.first)
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
};
|
|
|
|
#endif /* STRONGLYCONNECTEDCOMPONENTS_H_ */
|