24#include <unordered_set>
40#ifdef AFBU_WRITE_QGIS_FILTERS
49#ifdef AFBU_DEBUG_LEVEL_2
50#define AFBU_DEBUG_LEVEL_1
53#ifdef AFBU_DEBUG_LEVEL_1
54#define AFBU_DEBUG_LEVEL_0
64template<
class E,
class N,
class V,
class M>
81template<
class E,
class N,
class V,
class M>
85 const double EPS = 0.009;
105 int numberOfLevels,
bool unbuildIsWarning,
107 const std::shared_ptr<const FlippedLookupTable> flippedLookup =
nullptr,
108 const bool havePermissions =
false,
const bool haveRestrictions =
false,
147 void init(
SUMOTime time,
const V*
const vehicle, std::vector<FlagInfo*>& flagInfos);
196 void putArcFlag(
ArcInfo* arcInfo,
const int sHARCLevel,
const bool isLeftOrLowerCell);
236 size_t numberOfBoundaryNodes);
250template<
class E,
class N,
class V,
class M>
252 if (myArcInfos.empty()) {
254 myArcInfos.push_back(
new ArcInfo(flippedEdge));
258 for (sHARCLevel = 0; sHARCLevel < myNumberOfLevels - 1; sHARCLevel++) {
259#ifdef AFBU_DEBUG_LEVEL_0
260 std::cout <<
"Starting computation of flags of level " << sHARCLevel <<
" (levels run from 0 to "
261 << myNumberOfLevels - 2 <<
")." << std::endl;
263#ifdef AFBU_DEBUG_LEVEL_2
264 if (sHARCLevel != 0) {
268 computeArcFlags(msTime, sHARCLevel, vehicle);
270#ifdef AFBU_DEBUG_LEVEL_0
271 std::cout <<
"Copying arc flags from the arc infos... " << std::endl;
274 for (
const ArcInfo* arcInfo : myArcInfos) {
275 flagInfos[index++]->arcFlags = arcInfo->arcFlags;
278#ifdef AFBU_DEBUG_LEVEL_0
279 std::cout <<
"Arc flags copied from the arc infos. " << std::endl;
284template<
class E,
class N,
class V,
class M>
287 assert(myFlippedPartition);
288 const std::vector<const Cell*>& levelCells = myFlippedPartition->getCellsAtLevel(sHARCLevel2PartitionLevel(sHARCLevel));
289#ifdef AFBU_DEBUG_LEVEL_0
292 for (
const Cell* cell : levelCells) {
293#ifdef AFBU_DEBUG_LEVEL_0
294 std::cout <<
"Starting to compute core flags of the " << i++ <<
"th cell..." << std::endl;
296#ifdef AFBU_DEBUG_LEVEL_2
297 if (cell->getNumber() == 4) {
301 computeArcFlags(msTime, sHARCLevel, cell, vehicle);
303#ifdef AFBU_DEBUG_LEVEL_0
304 std::cout <<
"Cleaning up after computeArcFlags..." << std::endl;
306 for (
ArcInfo* arcInfo : myArcInfos) {
307 arcInfo->effortsToBoundaryNodes.clear();
308 arcInfo->touched =
false;
310#ifdef AFBU_DEBUG_LEVEL_0
311 std::cout <<
"Cleaned up." << std::endl;
313#ifdef AFBU_DEBUG_LEVEL_2
317 }
catch (
const std::invalid_argument& e) {
318 std::cerr <<
"Exception: " << e.what() << std::endl;
323template<
class E,
class N,
class V,
class M>
325 const Cell* supercell = cell->getSupercell();
326 const std::unordered_set<const FlippedEdge<E, N, V>*>& boundaryEdges = cell->getOutgoingBoundaryEdges();
327 const std::vector<const FlippedNode<E, N, V>*>& boundaryNodes = cell->getBoundaryFromNodes();
328#ifdef AFBU_DEBUG_LEVEL_1
329 std::cout <<
"Number of boundary edges: " << boundaryEdges.size() << std::endl;
330 std::cout <<
"Number of boundary nodes: " << boundaryNodes.size() << std::endl;
331 std::cout <<
"Cell number: " << cell->getNumber() << std::endl;
332 std::cout <<
"Supercell number: " << supercell->getNumber() << std::endl;
336 initBoundaryEdges(boundaryEdges);
338#ifdef AFBU_DEBUG_LEVEL_0
343 assert(!boundaryEdge->isInternal());
344 ArcInfo* arcInfo = myArcInfos[boundaryEdge->getNumericalID()];
345 if (boundaryNode == boundaryEdge->getFromJunction()) {
350 std::vector<const FlippedEdge<E, N, V>*> into;
351#ifdef AFBU_DEBUG_LEVEL_2
352 std::vector<const FlippedEdge<E, N, V>*> into2;
354 if (myNode2EdgeRouter->computeNode2Edge(boundaryNode, boundaryEdge, vehicle, msTime, into)) {
355 double recomputedEffort = myNode2EdgeRouter->recomputeCostsNoLastEdge(into, vehicle, msTime);
357#ifdef AFBU_DEBUG_LEVEL_2
358 if (!into.empty() && myNode2EdgeRouter->compute(into[0], boundaryEdge, vehicle, msTime, into2)) {
359 double recomputedEffort2 = myNode2EdgeRouter->recomputeCosts(into2, vehicle, msTime);
361 std::cout <<
"node2Edge router succeeded, effort: " << recomputedEffort <<
", effort incl. last edge: " << recomputedEffort2 << std::endl;
362 assert(recomputedEffort <= recomputedEffort2);
367#ifdef AFBU_DEBUG_LEVEL_2
368 std::cout <<
"UNREACHABLE!" << std::endl;
373#ifdef AFBU_DEBUG_LEVEL_0
375 std::cout <<
"Initial distance computation spent " +
elapsedMs2string(timeSpent) +
"." << std::endl;
379 initSupercellEdges(supercell, boundaryEdges, boundaryNodes.size());
380#ifdef AFBU_DEBUG_LEVEL_0
381 std::cout <<
"Initialization of all supercell edges' labels and arc flag vectors done. Starting the centralized shortest path tree algorithm..." << std::endl;
383 if (myCentralizedSPTree->computeCentralizedSPTree(msTime, cell, vehicle)) {
384 computeArcFlagsAux(msTime, sHARCLevel, cell, vehicle);
386#ifdef AFBU_DEBUG_LEVEL_0
387 std::cout <<
"Centralized shortest path tree algorithm finished." << std::endl;
391template<
class E,
class N,
class V,
class M>
393 const Cell* supercell = cell->getSupercell();
394 const std::unordered_set<const FlippedEdge<E, N, V>*>& boundaryEdges = cell->getOutgoingBoundaryEdges();
395 const std::vector<const FlippedNode<E, N, V>*>& boundaryNodes = cell->getBoundaryFromNodes();
396#ifdef AFBU_DEBUG_LEVEL_1
397 std::cout <<
"Number of boundary edges: " << boundaryEdges.size() << std::endl;
398 std::cout <<
"Number of boundary nodes: " << boundaryNodes.size() << std::endl;
399 std::cout <<
"Cell number: " << cell->getNumber() << std::endl;
400 std::cout <<
"Supercell number: " << supercell->getNumber() << std::endl;
403 initBoundaryEdges(boundaryEdges);
404#ifdef AFBU_DEBUG_LEVEL_1
407 std::map<const FlippedEdge<E, N, V>*, std::vector<const FlippedEdge<E, N, V>*>> incomingEdgesOfOutgoingBoundaryEdges;
408 size_t numberOfBoundaryNodes = boundaryNodes.size();
410 incomingEdgesOfOutgoingBoundaryEdges[boundaryEdge] = std::vector<const FlippedEdge<E, N, V>*>(numberOfBoundaryNodes);
414 myNode2EdgeRouter->reset(vehicle);
415 if (myNode2EdgeRouter->computeNode2Edges(boundaryNode, boundaryEdges, vehicle, msTime)) {
416#ifdef AFBU_DEBUG_LEVEL_2
417 std::cout <<
"Node-to-edge router succeeded." << std::endl;
421 assert(!boundaryEdge->isInternal());
422 ArcInfo* arcInfo = myArcInfos[boundaryEdge->getNumericalID()];
423 if (boundaryNode == boundaryEdge->getFromJunction()) {
425 (incomingEdgesOfOutgoingBoundaryEdges[boundaryEdge])[index] =
nullptr;
428 double effort = (myNode2EdgeRouter->edgeInfo(boundaryEdge))->effort;
430 = (myNode2EdgeRouter->edgeInfo(boundaryEdge))->prev ?
431 (myNode2EdgeRouter->edgeInfo(boundaryEdge))->prev->edge :
nullptr;
432 (incomingEdgesOfOutgoingBoundaryEdges[boundaryEdge])[index] = incomingEdge;
437#ifdef AFBU_DEBUG_LEVEL_0
439 std::cout <<
"Initial distance computation spent " +
elapsedMs2string(timeSpent) +
"." << std::endl;
442 initSupercellEdges(supercell, boundaryEdges, boundaryNodes.size());
443#ifdef AFBU_DEBUG_LEVEL_0
444 std::cout <<
"Initialization of all supercell edges' labels and arc flag vectors done. Starting the centralized shortest path tree algorithm..." << std::endl;
446#ifdef AFBU_DEBUG_LEVEL_0
449 if (myCentralizedSPTree->computeCentralizedSPTree(msTime, cell, vehicle, incomingEdgesOfOutgoingBoundaryEdges)) {
450#ifdef AFBU_DEBUG_LEVEL_0
452 std::cout <<
"Centralized SP tree computation spent " +
elapsedMs2string(timeSpent) +
"." << std::endl;
454 computeArcFlagsAux(msTime, sHARCLevel, cell, vehicle);
456#ifdef AFBU_DEBUG_LEVEL_0
457 std::cout <<
"Centralized shortest path tree algorithm finished." << std::endl;
461template<
class E,
class N,
class V,
class M>
463 const Cell* supercell = cell->getSupercell();
464 std::pair<typename std::vector<const FlippedNode<E, N, V>*>::const_iterator,
465 typename std::vector<const FlippedNode<E, N, V>*>::const_iterator> supercellNodeIterators = supercell->nodeIterators();
466 typename std::vector<const FlippedNode<E, N, V>*>::const_iterator first = supercellNodeIterators.first;
467 typename std::vector<const FlippedNode<E, N, V>*>::const_iterator last = supercellNodeIterators.second;
468 typename std::vector<const FlippedNode<E, N, V>*>::const_iterator iter;
469 std::unordered_set<ArcInfo*> arcInfosOnAShortestPath;
470#ifdef AFBU_DEBUG_LEVEL_1
471 int numberOfSupercellEdges = 0;
473 for (iter = first; iter != last; iter++) {
474 const std::vector<const FlippedEdge<E, N, V>*> incomingEdges = (*iter)->getIncoming();
476 if (supercellEdge->isInternal()) {
479 if ((myNode2EdgeRouter->edgeInfo(supercellEdge))->prohibited
480 || myNode2EdgeRouter->isProhibited(supercellEdge, vehicle)) {
483#ifdef AFBU_DEBUG_LEVEL_1
484 numberOfSupercellEdges++;
486 ArcInfo* supercellArcInfo = myArcInfos[supercellEdge->getNumericalID()];
488 arcInfosOnAShortestPath.insert(supercellArcInfo);
491#ifdef AFBU_DEBUG_LEVEL_1
492 std::cout <<
"Number of supercell edges: " << numberOfSupercellEdges << std::endl;
495#ifdef AFBU_DEBUG_LEVEL_1
496 std::cout <<
"Identifying shortest paths..." << std::endl;
498#ifdef AFBU_DEBUG_LEVEL_0
501 std::unordered_set<ArcInfo*> erasedEdges;
502 for (
auto iter2 = arcInfosOnAShortestPath.begin(); iter2 != arcInfosOnAShortestPath.end();) {
503 const ArcInfo* arcInfo = *iter2;
504 assert(!arcInfo->
edge->isInternal());
505 assert(myNode2EdgeRouter);
506 assert(!(myNode2EdgeRouter->edgeInfo(arcInfo->
edge))->prohibited
507 && !myNode2EdgeRouter->isProhibited(arcInfo->
edge, vehicle));
510 bool onShortestPath =
false;
512 for (index = 0; index < numberOfBoundaryNodes; index++) {
517 if (effort1ToBoundaryNode == std::numeric_limits<double>::max()) {
521 double edgeEffort = myNode2EdgeRouter->getEffort(arcInfo->
edge, vehicle, sTime);
522 sTime += myNode2EdgeRouter->getTravelTime(arcInfo->
edge, vehicle, sTime, edgeEffort);
523 double oldEdgeEffort = edgeEffort;
524 double oldSTime = sTime;
527 assert(!follower.first->isInternal());
528 ArcInfo* followerInfo = myArcInfos[follower.first->getNumericalID()];
531 if ((myNode2EdgeRouter->edgeInfo(follower.first))->prohibited
532 || myNode2EdgeRouter->isProhibited(follower.first, vehicle)) {
533 myErrorMsgHandler->inform(
"Vehicle '" +
Named::getIDSecure(vehicle) +
"' is not allowed on source edge '" + followerInfo->
edge->getID() +
"'.");
543 if (effort2ToBoundaryNode == std::numeric_limits<double>::max()) {
549 myNode2EdgeRouter->updateViaEdgeCost(follower.second, vehicle,
550 sTime , edgeEffort, length);
552 if (effort1ToBoundaryNode + edgeEffort
553 <= effort2ToBoundaryNode
554 + EPS && effort1ToBoundaryNode + edgeEffort >= effort2ToBoundaryNode - EPS) {
555 onShortestPath =
true;
558 edgeEffort = oldEdgeEffort;
562 if (!onShortestPath) {
563 erasedEdges.insert(*iter2);
564 iter2 = arcInfosOnAShortestPath.erase(iter2);
570#ifdef AFBU_DEBUG_LEVEL_0
571 std::cout <<
"Edges clearly not on a shortest path have been removed. Number of edges on a shortest path is now: "
572 << arcInfosOnAShortestPath.size() << std::endl;
577 std::unordered_set<const FlippedEdge<E, N, V>*>* pEdgesInsideCell = cell->edgeSet(vehicle);
578 std::unordered_set<const FlippedEdge<E, N, V>*> edgesInsideCell = *pEdgesInsideCell;
579#ifdef AFBU_DEBUG_LEVEL_0
580 std::cout <<
"Adding all edges completely inside the cell to set of edges on a shortest path, cell no:"
581 << cell->getNumber() << std::endl;
584 ArcInfo* arcInfo = myArcInfos[edgeInsideCell->getNumericalID()];
585 if ((myNode2EdgeRouter->edgeInfo(edgeInsideCell))->prohibited
586 || myNode2EdgeRouter->isProhibited(edgeInsideCell, vehicle)) {
589 arcInfosOnAShortestPath.insert(arcInfo);
591 delete pEdgesInsideCell;
592#ifdef AFBU_DEBUG_LEVEL_0
593 std::cout <<
"Edges inside cell added." << std::endl;
595 std::cout <<
"Shortest path identification spent " +
elapsedMs2string(timeSpent) +
"." << std::endl;
598#ifdef AFBU_WRITE_QGIS_FILTERS
599 std::string qgisFilterString =
"id IN (";
600 std::unordered_set<const FlippedNode<E, N, V>*> nodesOnAShortestPath;
601 std::unordered_set<const FlippedNode<E, N, V>*> erasedNodes;
602 for (
const ArcInfo* arcInfo : arcInfosOnAShortestPath) {
603 assert(!(myNode2EdgeRouter->edgeInfo(arcInfo->edge))->prohibited
604 && !myNode2EdgeRouter->isProhibited(arcInfo->edge, vehicle));
605 nodesOnAShortestPath.insert(arcInfo->edge->getFromJunction());
606 nodesOnAShortestPath.insert(arcInfo->edge->getToJunction());
608 for (
const ArcInfo* erasedEdgeArcInfo : erasedEdges) {
609 erasedNodes.insert(erasedEdgeArcInfo->edge->getFromJunction());
610 erasedNodes.insert(erasedEdgeArcInfo->edge->getToJunction());
614 size_t numberOfNodesOnAShortestPath = nodesOnAShortestPath.size();
617 qgisFilterString += node->getID() + (k < numberOfNodesOnAShortestPath ?
", " :
"");
619 qgisFilterString +=
")";
620 std::ostringstream pathAndFileName;
621 pathAndFileName <<
"./filter_superset_nodes_cell_" << cell->getNumber() <<
"_supercell_" << supercell->getNumber() <<
".qqf";
623 file.open(pathAndFileName.str());
624 std::ostringstream content;
625 content <<
"<Query>" << qgisFilterString <<
"</Query>";
626 file << content.str();
630 qgisFilterString.clear();
631 qgisFilterString =
"id IN (";
633 size_t numberOfErasedNodes = erasedNodes.size();
636 qgisFilterString += node->getID() + (k < numberOfErasedNodes ?
", " :
"");
638 qgisFilterString +=
")";
639 pathAndFileName.str(
"");
640 pathAndFileName.clear();
641 pathAndFileName <<
"./filter_erased_nodes_cell_" << cell->getNumber() <<
"_supercell_" << supercell->getNumber() <<
".qqf";
643 file.open(pathAndFileName.str());
646 content <<
"<Query>" << qgisFilterString <<
"</Query>";
647 file << content.str();
651 for (
ArcInfo* arcInfo : arcInfosOnAShortestPath) {
652 putArcFlag(arcInfo, sHARCLevel, cell->isLeftOrLowerCell());
656template<
class E,
class N,
class V,
class M>
659 (arcInfo->
arcFlags)[sHARCLevel * 2 + (isLeftOrLowerCell ? 0 : 1)] = 1;
662template<
class E,
class N,
class V,
class M>
666 assert(!boundaryEdge->isInternal());
668 arcInfo = myArcInfos[boundaryEdge->getNumericalID()];
670 std::fill_n(std::back_inserter(arcInfo->
arcFlags),
671 myFlippedPartition->numberOfArcFlags(),
false);
677template<
class E,
class N,
class V,
class M>
679 const Cell* supercell,
681 size_t numberOfBoundaryNodes) {
682 std::pair<typename std::vector<const FlippedNode<E, N, V>*>::const_iterator,
683 typename std::vector<const FlippedNode<E, N, V>*>::const_iterator> supercellNodeIterators = supercell->nodeIterators();
684 typename std::vector<const FlippedNode<E, N, V>*>::const_iterator first = supercellNodeIterators.first;
685 typename std::vector<const FlippedNode<E, N, V>*>::const_iterator last = supercellNodeIterators.second;
686 typename std::vector<const FlippedNode<E, N, V>*>::const_iterator iter;
687 for (iter = first; iter != last; iter++) {
688 const std::vector<const FlippedEdge<E, N, V>*> incomingEdges = (*iter)->getIncoming();
690 if (supercellEdge->isInternal()) {
693 if (boundaryEdges.count(supercellEdge)) {
697 supercellArcInfo = myArcInfos[supercellEdge->getNumericalID()];
698 if (supercellArcInfo->
arcFlags.empty()) {
699 std::fill_n(std::back_inserter(supercellArcInfo->
arcFlags),
700 myFlippedPartition->numberOfArcFlags(),
false);
704 numberOfBoundaryNodes, std::numeric_limits<double>::max());
std::string elapsedMs2string(long long int t)
convert ms to string for log output
SUMOVehicleClass
Definition of vehicle classes to differ between different lane usage and authority types.
@ SVC_IGNORING
vehicles ignoring classes
Builds the flags for (multi-level) arc flag routing (Hilger et al.) in its multi-level variant (also ...
void initSupercellEdges(const Cell *supercell, const std::unordered_set< const FlippedEdge< E, N, V > * > &boundaryEdges, size_t numberOfBoundaryNodes)
Initialize the supercell edges.
void init(SUMOTime time, const V *const vehicle, std::vector< FlagInfo * > &flagInfos)
Initialize the arc flag build.
AFInfo< FlippedEdge< E, N, V > >::ArcInfo ArcInfo
std::vector< ArcInfo * > myArcInfos
The container of arc informations (for the centralized shortest path tree)
AFCentralizedSPTree< FlippedEdge< E, N, V >, FlippedNode< E, N, V >, V > * myCentralizedSPTree
A Dijkstra based centralized label-correcting algorithm.
SUMOAbstractRouter< FlippedEdge< E, N, V >, V >::Operation myFlippedOperation
The object's operation to perform on a backward graph with flipped edges.
MsgHandler *const myErrorMsgHandler
The handler for routing errors.
const std::shared_ptr< const FlippedLookupTable > myFlippedLookupTable
The lookup table for travel time heuristics.
const std::vector< FlippedEdge< E, N, V > * > & myFlippedEdges
The flipped edges.
SUMOAbstractRouter< FlippedEdge< E, N, V >, V >::Operation getFlippedOperation()
Returns the operation for a backward graph with flipped edges.
const int myNumberOfLevels
The number of levels.
AbstractLookupTable< FlippedEdge< E, N, V >, V > FlippedLookupTable
AFInfo< E >::FlagInfo FlagInfo
void computeArcFlagsAux(SUMOTime msTime, const int sHARCLevel, const Cell *cell, const V *const vehicle)
Helper method for computeArcFlags(), which computes the arc flags for a given cell.
void initBoundaryEdges(const std::unordered_set< const FlippedEdge< E, N, V > * > &boundaryEdges)
Initialize the boundary edges param[in] boundaryEdges The boundary edges.
int sHARCLevel2PartitionLevel(int sHARCLevel)
Converts a SHARC level number to a partition level number.
void setFlippedPartition(const KDTreePartition< FlippedEdge< E, N, V >, FlippedNode< E, N, V >, V > *flippedPartition)
Set the flipped partition param[in] flippedPartition The flipped partition.
const bool myHaveRestrictions
The boolean flag indicating whether edge restrictions need to be considered or not.
void putArcFlag(ArcInfo *arcInfo, const int sHARCLevel, const bool isLeftOrLowerCell)
Put the arc flag of the edge in arcInfo.
const KDTreePartition< FlippedEdge< E, N, V >, FlippedNode< E, N, V >, V > * myFlippedPartition
The partition for the backward graph with flipped edges.
KDTreePartition< FlippedEdge< E, N, V >, FlippedNode< E, N, V >, V >::Cell Cell
void computeArcFlagsNaive(SUMOTime msTime, const int sHARCLevel, const Cell *cell, const V *const vehicle)
Computes the arc flags for a given cell (naive version)
const std::shared_ptr< const FlippedLookupTable > getFlippedLookup()
Returns the lookup table for the backward graph with flipped edges.
const std::vector< FlippedEdge< E, N, V > * > * myProhibited
The list of explicitly prohibited edges.
AFBuild(const std::vector< FlippedEdge< E, N, V > * > &flippedEdges, const KDTreePartition< FlippedEdge< E, N, V >, FlippedNode< E, N, V >, V > *const flippedPartition, int numberOfLevels, bool unbuildIsWarning, typename SUMOAbstractRouter< FlippedEdge< E, N, V >, V >::Operation flippedOperation, const std::shared_ptr< const FlippedLookupTable > flippedLookup=nullptr, const bool havePermissions=false, const bool haveRestrictions=false, const std::vector< FlippedEdge< E, N, V > * > *toProhibit=nullptr)
Constructor.
void computeArcFlags(SUMOTime msTime, int sHARCLevel, const V *const vehicle)
Computes the arc flags for all cells at a given level.
const bool myHavePermissions
The boolean flag indicating whether edge permissions need to be considered or not.
const double EPS
Maximum difference of two path lengths considered to be equal.
int partitionLevel2SHARCLevel(int partitionLevel)
Converts a partition level number to a SHARC level number.
Node2EdgeRouter< FlippedEdge< E, N, V >, FlippedNode< E, N, V >, V, M > * myNode2EdgeRouter
The node-to-edge router (for a backward graph with flipped edges)
std::vector< bool > arcFlags
The arc flags.
std::vector< double > effortsToBoundaryNodes
The efforts to boundary nodes.
const E *const edge
The current edge.
static int sHARCLevel2PartitionLevel(int sHARCLevel, int numberOfPartitionLevels)
Converts a SHARC level number to a partition level number.
static int partitionLevel2SHARCLevel(int partitionLevel, int numberOfPartitionLevels)
Converts a partition level number to a SHARC level number.
The edge type representing backward edges with flipped nodes.
the node type representing nodes used for backward search
Partitions the router's network wrt a k-d tree subdivision scheme.
static std::string getIDSecure(const T *obj, const std::string &fallBack="NULL")
get an identifier for Named-like object which may be Null
virtual void prohibit(const std::vector< E * > &toProhibit)
static long getCurrentMillis()
Returns the current time in milliseconds.