SUMO - Simulation of Urban MObility
NBAlgorithms.cpp
Go to the documentation of this file.
00001 /****************************************************************************/
00007 // Algorithms for network computation
00008 /****************************************************************************/
00009 // SUMO, Simulation of Urban MObility; see http://sumo.sourceforge.net/
00010 // Copyright (C) 2001-2012 DLR (http://www.dlr.de/) and contributors
00011 /****************************************************************************/
00012 //
00013 //   This file is part of SUMO.
00014 //   SUMO is free software: you can redistribute it and/or modify
00015 //   it under the terms of the GNU General Public License as published by
00016 //   the Free Software Foundation, either version 3 of the License, or
00017 //   (at your option) any later version.
00018 //
00019 /****************************************************************************/
00020 
00021 
00022 // ===========================================================================
00023 // included modules
00024 // ===========================================================================
00025 #ifdef _MSC_VER
00026 #include <windows_config.h>
00027 #else
00028 #include <config.h>
00029 #endif
00030 
00031 #include <sstream>
00032 #include <iostream>
00033 #include <cassert>
00034 #include <algorithm>
00035 #include <utils/common/MsgHandler.h>
00036 #include "NBEdge.h"
00037 #include "NBNodeCont.h"
00038 #include "NBTypeCont.h"
00039 #include "NBNode.h"
00040 #include "NBAlgorithms.h"
00041 
00042 #ifdef CHECK_MEMORY_LEAKS
00043 #include <foreign/nvwa/debug_new.h>
00044 #endif // CHECK_MEMORY_LEAKS
00045 
00046 
00047 // ===========================================================================
00048 // method definitions
00049 // ===========================================================================
00050 // ---------------------------------------------------------------------------
00051 // NBTurningDirectionsComputer
00052 // ---------------------------------------------------------------------------
00053 void 
00054 NBTurningDirectionsComputer::computeTurnDirections(NBNodeCont &nc) {
00055     for(std::map<std::string, NBNode*>::const_iterator i=nc.begin(); i!=nc.end(); ++i) {
00056         computeTurnDirectionsForNode(i->second);
00057     }
00058 }
00059 
00060 void 
00061 NBTurningDirectionsComputer::computeTurnDirectionsForNode(NBNode *node) {
00062     const std::vector<NBEdge*> &incoming = node->getIncomingEdges();
00063     const std::vector<NBEdge*> &outgoing = node->getOutgoingEdges();
00064     std::vector<Combination> combinations;
00065     for(std::vector<NBEdge*>::const_iterator j=outgoing.begin(); j!=outgoing.end(); ++j) {
00066         NBEdge *outedge = *j;
00067         for(std::vector<NBEdge*>::const_iterator k=incoming.begin(); k!=incoming.end(); ++k) {
00068             NBEdge* e = *k;
00069             if (e->getConnections().size()!=0 && !e->isConnectedTo(outedge)) {
00070                 // has connections, but not to outedge; outedge will not be the turn direction
00071                 //
00072                 // @todo: this seems to be needed due to legacy issues; actually, we could regard
00073                 //  such pairs, too, and it probably would increase the accuracy. But there is
00074                 //  no mechanism implemented, yet, which would avoid adding them as turnarounds though
00075                 //  no connection is specified.
00076                 continue;
00077             }
00078 
00079             // @todo: check whether NBHelpers::relAngle is properly defined and whether it should really be used, here
00080             SUMOReal angle = fabs(NBHelpers::relAngle(e->getAngleAtNode(node), outedge->getAngleAtNode(node)));
00081             if(angle<160) {
00082                 continue;
00083             }
00084             if(e->getFromNode()==outedge->getToNode()) {
00085                 // they connect the same nodes; should be the turnaround direction
00086                 // we'll assign a maximum number
00087                 //
00088                 // @todo: indeed, we have observed some pathological intersections
00089                 //  see "294831560" in OSM/adlershof. Here, several edges are connecting
00090                 //  same nodes. We have to do the angle check before...
00091                 //
00092                 // @todo: and well, there are some other as well, see plain import
00093                 //  of delphi_muenchen (elmar), intersection "59534191". Not that it would
00094                 //  be realistic in any means; we will warn, here.
00095                 angle += 360;
00096             }
00097             Combination c;
00098             c.from = e;
00099             c.to = outedge;
00100             c.angle = angle;
00101             combinations.push_back(c);
00102         }
00103     }
00104     // sort combinations so that the ones with the highest angle are at the begin
00105     std::sort(combinations.begin(), combinations.end(), combination_by_angle_sorter());
00106     std::set<NBEdge*> seen;
00107     bool haveWarned = false;
00108     for(std::vector<Combination>::const_iterator j=combinations.begin(); j!=combinations.end(); ++j) {
00109         if(seen.find((*j).from)!=seen.end() || seen.find((*j).to)!=seen.end() ) {
00110             // do not regard already set edges
00111             if((*j).angle>360&&!haveWarned) {
00112                 WRITE_WARNING("Ambiguity in turnarounds computation at node '" + node->getID() +"'.");
00113                 haveWarned = true;
00114             }
00115             continue;
00116         }
00117         // mark as seen
00118         seen.insert((*j).from);
00119         seen.insert((*j).to);
00120         // set turnaround information
00121         (*j).from->setTurningDestination((*j).to);
00122     }
00123 }
00124 
00125 
00126 // ---------------------------------------------------------------------------
00127 // NBNodesEdgesSorter
00128 // ---------------------------------------------------------------------------
00129 void 
00130 NBNodesEdgesSorter::sortNodesEdges(NBNodeCont &nc, bool leftHand) {
00131     for(std::map<std::string, NBNode*>::const_iterator i=nc.begin(); i!=nc.end(); ++i) {
00132         NBNode *n = (*i).second;
00133         if (n->myAllEdges.size() == 0) {
00134             continue;
00135         }
00136         std::vector<NBEdge*> &allEdges = (*i).second->myAllEdges;
00137         std::vector<NBEdge*> &incoming = (*i).second->myIncomingEdges;
00138         std::vector<NBEdge*> &outgoing = (*i).second->myOutgoingEdges;
00139         // sort the edges
00140         std::sort(allEdges.begin(), allEdges.end(), edge_by_junction_angle_sorter(n));
00141         std::sort(incoming.begin(), incoming.end(), edge_by_junction_angle_sorter(n));
00142         std::sort(outgoing.begin(), outgoing.end(), edge_by_junction_angle_sorter(n));
00143         std::vector<NBEdge*>::iterator j;
00144         for (j = allEdges.begin(); j != allEdges.end() - 1 && j != allEdges.end(); ++j) {
00145             swapWhenReversed(n, leftHand, j, j + 1);
00146         }
00147         if (allEdges.size() > 1 && j != allEdges.end()) {
00148             swapWhenReversed(n, leftHand, allEdges.end() - 1, allEdges.begin());
00149         }
00150     }
00151 }
00152 
00153 
00154 void
00155 NBNodesEdgesSorter::swapWhenReversed(const NBNode * const n, bool leftHand,
00156                                      const std::vector<NBEdge*>::iterator& i1,
00157                                      const std::vector<NBEdge*>::iterator& i2) {
00158     NBEdge* e1 = *i1;
00159     NBEdge* e2 = *i2;
00160     if (leftHand) {
00161         // @todo: check this; shouldn't it be "swap(*e1, *e2)"?
00162         std::swap(e1, e2);
00163     }
00164     // @todo: The difference between "isTurningDirectionAt" and "isTurnaround"
00165     //  is not nice. Maybe we could get rid of it if we would always mark edges
00166     //  as turnarounds, even if they do not have to be added, as mentioned in
00167     //  notes on NBTurningDirectionsComputer::computeTurnDirectionsForNode
00168     if (e2->getToNode() == n && e2->isTurningDirectionAt(n, e1)) {
00169         std::swap(*i1, *i2);
00170     }
00171 }
00172 
00173 
00174 // ---------------------------------------------------------------------------
00175 // NBNodeTypeComputer
00176 // ---------------------------------------------------------------------------
00177 void 
00178 NBNodeTypeComputer::computeNodeTypes(NBNodeCont &nc) {
00179     for(std::map<std::string, NBNode*>::const_iterator i=nc.begin(); i!=nc.end(); ++i) {
00180         NBNode *n = (*i).second;
00181         // the type may already be set from the data
00182         if (n->myType != NODETYPE_UNKNOWN) {
00183             continue;
00184         }
00185         // check whether the junction is not a real junction
00186         if (n->myIncomingEdges.size() == 1) {
00187             n->myType = NODETYPE_PRIORITY_JUNCTION;
00188             continue;
00189         }
00190         // @todo "isSimpleContinuation" should be revalidated
00191         if (n->isSimpleContinuation()) {
00192             n->myType = NODETYPE_PRIORITY_JUNCTION;
00193             continue;
00194         }
00195         // determine the type
00196         SumoXMLNodeType type = NODETYPE_RIGHT_BEFORE_LEFT;
00197         for (EdgeVector::const_iterator i = n->myIncomingEdges.begin(); i != n->myIncomingEdges.end(); i++) {
00198             for (EdgeVector::const_iterator j = i + 1; j != n->myIncomingEdges.end(); j++) {
00199                 // @todo "getOppositeIncoming" should probably be refactored into something the edge knows
00200                 if (n->getOppositeIncoming(*j) == *i && n->myIncomingEdges.size() > 2) {
00201                     continue;
00202                 }
00203                 // @todo check against a legal document
00204                 SUMOReal s1 = (*i)->getSpeed() * (SUMOReal) 3.6;
00205                 SUMOReal s2 = (*j)->getSpeed() * (SUMOReal) 3.6;
00206                 if(fabs(s1-s2)>(SUMOReal) 9.5 || s1>=(SUMOReal) 49. || s2>=(SUMOReal) 49.) {
00207                     type = NODETYPE_PRIORITY_JUNCTION;
00208                     break;
00209                 }
00210             }
00211         }
00212         // save type
00213         n->myType = type;
00214     }
00215 }
00216 
00217 
00218 // ---------------------------------------------------------------------------
00219 // NBEdgePriorityComputer
00220 // ---------------------------------------------------------------------------
00221 void
00222 NBEdgePriorityComputer::computeEdgePriorities(NBNodeCont &nc) {
00223     for(std::map<std::string, NBNode*>::const_iterator i=nc.begin(); i!=nc.end(); ++i) {
00224         NBNode *n = (*i).second;
00225         // preset all junction's edge priorities to zero
00226         for (EdgeVector::iterator j = n->myAllEdges.begin(); j != n->myAllEdges.end(); ++j) {
00227             (*j)->setJunctionPriority(n, 0);
00228         }
00229         // check if the junction is not a real junction
00230         if (n->myIncomingEdges.size() == 1 && n->myOutgoingEdges.size() == 1) {
00231             continue;
00232         }
00233         // compute the priorities on junction when needed
00234         if (n->myType != NODETYPE_RIGHT_BEFORE_LEFT) {
00235             setPriorityJunctionPriorities(*n);
00236         }
00237     }
00238 }
00239 
00240 
00241 void
00242 NBEdgePriorityComputer::setPriorityJunctionPriorities(NBNode &n) {
00243     if (n.myIncomingEdges.size() == 0 || n.myOutgoingEdges.size() == 0) {
00244         return;
00245     }
00246     EdgeVector incoming = n.myIncomingEdges;
00247     EdgeVector outgoing = n.myOutgoingEdges;
00248     // what we do want to have is to extract the pair of roads that are
00249     //  the major roads for this junction
00250     // let's get the list of incoming edges with the highest priority
00251     std::sort(incoming.begin(), incoming.end(), NBContHelper::edge_by_priority_sorter());
00252     EdgeVector bestIncoming;
00253     NBEdge* best = incoming[0];
00254     while (incoming.size() > 0 && samePriority(best, incoming[0])) {
00255         bestIncoming.push_back(*incoming.begin());
00256         incoming.erase(incoming.begin());
00257     }
00258     // now, let's get the list of best outgoing
00259     assert(outgoing.size() != 0);
00260     sort(outgoing.begin(), outgoing.end(), NBContHelper::edge_by_priority_sorter());
00261     EdgeVector bestOutgoing;
00262     best = outgoing[0];
00263     while (outgoing.size() > 0 && samePriority(best, outgoing[0])) { //->getPriority()==best->getPriority()) {
00264         bestOutgoing.push_back(*outgoing.begin());
00265         outgoing.erase(outgoing.begin());
00266     }
00267     // now, let's compute for each of the best incoming edges
00268     //  the incoming which is most opposite
00269     //  the outgoing which is most opposite
00270     EdgeVector::iterator i;
00271     std::map<NBEdge*, NBEdge*> counterIncomingEdges;
00272     std::map<NBEdge*, NBEdge*> counterOutgoingEdges;
00273     incoming = n.myIncomingEdges;
00274     outgoing = n.myOutgoingEdges;
00275     for (i = bestIncoming.begin(); i != bestIncoming.end(); ++i) {
00276         std::sort(incoming.begin(), incoming.end(), NBContHelper::edge_opposite_direction_sorter(*i, &n));
00277         counterIncomingEdges[*i] = *incoming.begin();
00278         std::sort(outgoing.begin(), outgoing.end(), NBContHelper::edge_opposite_direction_sorter(*i, &n));
00279         counterOutgoingEdges[*i] = *outgoing.begin();
00280     }
00281     // ok, let's try
00282     // 1) there is one best incoming road
00283     if (bestIncoming.size() == 1) {
00284         // let's mark this road as the best
00285         NBEdge* best1 = extractAndMarkFirst(n, bestIncoming);
00286         if(counterIncomingEdges.find(best1)!=counterIncomingEdges.end()) {
00287             // ok, look, what we want is the opposit of the straight continuation edge
00288             // but, what if such an edge does not exist? By now, we'll determine it
00289             // geometrically
00290             NBEdge *s = counterIncomingEdges.find(best1)->second;
00291             if(GeomHelper::getMinAngleDiff(best1->getAngleAtNode(&n), s->getAngleAtNode(&n))>180-45) {
00292                 s->setJunctionPriority(&n, 1);
00293             }
00294         }
00295         if (bestOutgoing.size() != 0) {
00296             // mark the best outgoing as the continuation
00297             sort(bestOutgoing.begin(), bestOutgoing.end(), NBContHelper::edge_similar_direction_sorter(best1));
00298             best1 = extractAndMarkFirst(n, bestOutgoing);
00299             if(counterOutgoingEdges.find(best1)!=counterOutgoingEdges.end()) {
00300                 NBEdge *s = counterOutgoingEdges.find(best1)->second;
00301                 if(GeomHelper::getMinAngleDiff(best1->getAngleAtNode(&n), s->getAngleAtNode(&n))>180-45) {
00302                     s->setJunctionPriority(&n, 1);
00303                 }
00304             }
00305         }
00306         return;
00307     }
00308 
00309     // ok, what we want to do in this case is to determine which incoming
00310     //  has the best continuation...
00311     // This means, when several incoming roads have the same priority,
00312     //  we want a (any) straight connection to be more priorised than a turning
00313     SUMOReal bestAngle = 0;
00314     NBEdge* bestFirst = 0;
00315     NBEdge* bestSecond = 0;
00316     bool hadBest = false;
00317     for (i = bestIncoming.begin(); i != bestIncoming.end(); ++i) {
00318         EdgeVector::iterator j;
00319         NBEdge* t1 = *i;
00320         SUMOReal angle1 = t1->getAngle() + 180;
00321         if (angle1 >= 360) {
00322             angle1 -= 360;
00323         }
00324         for (j = i + 1; j != bestIncoming.end(); ++j) {
00325             NBEdge* t2 = *j;
00326             SUMOReal angle2 = t2->getAngle() + 180;
00327             if (angle2 >= 360) {
00328                 angle2 -= 360;
00329             }
00330             SUMOReal angle = GeomHelper::getMinAngleDiff(angle1, angle2);
00331             if (!hadBest || angle > bestAngle) {
00332                 bestAngle = angle;
00333                 bestFirst = *i;
00334                 bestSecond = *j;
00335                 hadBest = true;
00336             }
00337         }
00338     }
00339     bestFirst->setJunctionPriority(&n, 1);
00340     sort(bestOutgoing.begin(), bestOutgoing.end(), NBContHelper::edge_similar_direction_sorter(bestFirst));
00341     if (bestOutgoing.size() != 0) {
00342         extractAndMarkFirst(n, bestOutgoing);
00343     }
00344     bestSecond->setJunctionPriority(&n, 1);
00345     sort(bestOutgoing.begin(), bestOutgoing.end(), NBContHelper::edge_similar_direction_sorter(bestSecond));
00346     if (bestOutgoing.size() != 0) {
00347         extractAndMarkFirst(n, bestOutgoing);
00348     }
00349 }
00350 
00351 
00352 NBEdge*
00353 NBEdgePriorityComputer::extractAndMarkFirst(NBNode &n, std::vector<NBEdge*>& s) {
00354     if (s.size() == 0) {
00355         return 0;
00356     }
00357     NBEdge* ret = s.front();
00358     s.erase(s.begin());
00359     ret->setJunctionPriority(&n, 1);
00360     return ret;
00361 }
00362 
00363 
00364 bool
00365 NBEdgePriorityComputer::samePriority(const NBEdge*const e1, const NBEdge*const e2) {
00366     if (e1 == e2) {
00367         return true;
00368     }
00369     if (e1->getPriority() != e2->getPriority()) {
00370         return false;
00371     }
00372     if ((int) e1->getSpeed() != (int) e2->getSpeed()) {
00373         return false;
00374     }
00375     return (int) e1->getNumLanes() == (int) e2->getNumLanes();
00376 }
00377 
00378 
00379 /****************************************************************************/
00380 
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Friends Defines