SUMO - Simulation of Urban MObility
|
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