1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2001-2019 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials
5 // are made available under the terms of the Eclipse Public License v2.0
6 // which accompanies this distribution, and is available at
7 // http://www.eclipse.org/legal/epl-v20.html
8 // SPDX-License-Identifier: EPL-2.0
9 /****************************************************************************/
10 /// @file NBNode.cpp
11 /// @author Daniel Krajzewicz
12 /// @author Jakob Erdmann
13 /// @author Sascha Krieg
14 /// @author Michael Behrisch
15 /// @date Tue, 20 Nov 2001
16 /// @version $Id$
17 ///
18 // The representation of a single node
19 /****************************************************************************/
20
21
22 // ===========================================================================
23 // included modules
24 // ===========================================================================
25 #include <config.h>
26
27 #include <string>
28 #include <map>
29 #include <cassert>
30 #include <algorithm>
31 #include <vector>
32 #include <deque>
33 #include <set>
34 #include <cmath>
35 #include <iterator>
36 #include <utils/common/UtilExceptions.h>
37 #include <utils/common/StringUtils.h>
38 #include <utils/options/OptionsCont.h>
39 #include <utils/geom/GeomHelper.h>
40 #include <utils/common/MsgHandler.h>
41 #include <utils/common/StdDefs.h>
42 #include <utils/common/ToString.h>
43 #include <utils/geom/GeoConvHelper.h>
44 #include <utils/iodevices/OutputDevice.h>
45 #include <iomanip>
46 #include "NBNode.h"
47 #include "NBAlgorithms.h"
48 #include "NBNodeCont.h"
49 #include "NBNodeShapeComputer.h"
50 #include "NBEdgeCont.h"
51 #include "NBTypeCont.h"
52 #include "NBHelpers.h"
53 #include "NBDistrict.h"
54 #include "NBContHelper.h"
55 #include "NBRequest.h"
56 #include "NBOwnTLDef.h"
57 #include "NBLoadedSUMOTLDef.h"
58 #include "NBTrafficLightLogicCont.h"
59 #include "NBTrafficLightDefinition.h"
60
61 // allow to extend a crossing across multiple edges
62 #define EXTEND_CROSSING_ANGLE_THRESHOLD 35.0 // degrees
63 // create intermediate walking areas if either of the following thresholds is exceeded
64 #define SPLIT_CROSSING_WIDTH_THRESHOLD 1.5 // meters
65 #define SPLIT_CROSSING_ANGLE_THRESHOLD 5 // degrees
66
67 // minimum length for a weaving section at a combined on-off ramp
68 #define MIN_WEAVE_LENGTH 20.0
69
70 //#define DEBUG_CONNECTION_GUESSING
71 //#define DEBUG_SMOOTH_GEOM
72 //#define DEBUG_PED_STRUCTURES
73 //#define DEBUG_EDGE_SORTING
74 //#define DEBUGCOND true
75 #define DEBUG_NODE_ID "F"
76 #define DEBUGCOND (getID() == DEBUG_NODE_ID)
77 #define DEBUGCOND2(obj) ((obj != 0 && (obj)->getID() == DEBUG_NODE_ID))
78
79 // ===========================================================================
80 // static members
81 // ===========================================================================
82 const int NBNode::FORWARD(1);
83 const int NBNode::BACKWARD(-1);
84 const double NBNode::UNSPECIFIED_RADIUS = -1;
85 const int NBNode::AVOID_WIDE_LEFT_TURN(1);
86 const int NBNode::AVOID_WIDE_RIGHT_TURN(2);
87 const int NBNode::FOUR_CONTROL_POINTS(4);
88 const int NBNode::AVOID_INTERSECTING_LEFT_TURNS(8);
89
90 // ===========================================================================
91 // method definitions
92 // ===========================================================================
93 /* -------------------------------------------------------------------------
94 * NBNode::ApproachingDivider-methods
95 * ----------------------------------------------------------------------- */
ApproachingDivider(const EdgeVector & approaching,NBEdge * currentOutgoing)96 NBNode::ApproachingDivider::ApproachingDivider(
97 const EdgeVector& approaching, NBEdge* currentOutgoing) :
98 myApproaching(approaching),
99 myCurrentOutgoing(currentOutgoing),
100 myIsBikeEdge(currentOutgoing->getPermissions() == SVC_BICYCLE) {
101 // collect lanes which are expliclity targeted
102 std::set<int> approachedLanes;
103 for (const NBEdge* const approachingEdge : myApproaching) {
104 for (const NBEdge::Connection& con : approachingEdge->getConnections()) {
105 if (con.toEdge == myCurrentOutgoing) {
106 approachedLanes.insert(con.toLane);
107 }
108 }
109 }
110 // compute the indices of lanes that should be targeted (excluding pedestrian
111 // lanes that will be connected from walkingAreas and forbidden lanes)
112 // if the lane is targeted by an explicitly set connection we need
113 // to make it available anyway
114 for (int i = 0; i < currentOutgoing->getNumLanes(); ++i) {
115 if ((currentOutgoing->getPermissions(i) == SVC_PEDESTRIAN
116 // don't consider bicycle lanes as targets unless the target
117 // edge is exclusively for bicycles
118 || (currentOutgoing->getPermissions(i) == SVC_BICYCLE && !myIsBikeEdge)
119 || isForbidden(currentOutgoing->getPermissions(i)))
120 && approachedLanes.count(i) == 0) {
121 continue;
122 }
123 myAvailableLanes.push_back(i);
124 }
125 }
126
127
~ApproachingDivider()128 NBNode::ApproachingDivider::~ApproachingDivider() {}
129
130
131 void
execute(const int src,const int dest)132 NBNode::ApproachingDivider::execute(const int src, const int dest) {
133 assert((int)myApproaching.size() > src);
134 // get the origin edge
135 NBEdge* incomingEdge = myApproaching[src];
136 if (incomingEdge->getStep() == NBEdge::LANES2LANES_DONE || incomingEdge->getStep() == NBEdge::LANES2LANES_USER) {
137 return;
138 }
139 if (myAvailableLanes.size() == 0) {
140 return;
141 }
142 std::vector<int> approachingLanes = incomingEdge->getConnectionLanes(myCurrentOutgoing, myIsBikeEdge || incomingEdge->getPermissions() == SVC_BICYCLE);
143 if (approachingLanes.size() == 0) {
144 return;
145 }
146 #ifdef DEBUG_CONNECTION_GUESSING
147 if (DEBUGCOND2(incomingEdge->getToNode())) {
148 std::cout << "Bre:ex src=" << src << " dest=" << dest << " in=" << incomingEdge->getID() << " apLanes=" << toString(approachingLanes) << "\n";
149 }
150
151 #endif
152 std::deque<int>* approachedLanes = spread(approachingLanes, dest);
153 assert(approachedLanes->size() <= myAvailableLanes.size());
154 // set lanes
155 for (int i = 0; i < (int)approachedLanes->size(); i++) {
156 assert((int)approachingLanes.size() > i);
157 int approached = myAvailableLanes[(*approachedLanes)[i]];
158 incomingEdge->setConnection(approachingLanes[i], myCurrentOutgoing, approached, NBEdge::L2L_COMPUTED);
159 }
160 delete approachedLanes;
161 }
162
163
164 std::deque<int>*
spread(const std::vector<int> & approachingLanes,int dest) const165 NBNode::ApproachingDivider::spread(const std::vector<int>& approachingLanes, int dest) const {
166 std::deque<int>* ret = new std::deque<int>();
167 const int numLanes = (int)approachingLanes.size();
168 // when only one lane is approached, we check, whether the double-value
169 // is assigned more to the left or right lane
170 if (numLanes == 1) {
171 ret->push_back(dest);
172 return ret;
173 }
174
175 const int numOutgoingLanes = (int)myAvailableLanes.size();
176 //
177 ret->push_back(dest);
178 int noSet = 1;
179 int roffset = 1;
180 int loffset = 1;
181 while (noSet < numLanes) {
182 // It may be possible, that there are not enough lanes the source
183 // lanes may be divided on
184 // In this case, they remain unset
185 // !!! this is only a hack. It is possible, that this yields in
186 // uncommon divisions
187 if (numOutgoingLanes == noSet) {
188 return ret;
189 }
190
191 // as due to the conversion of double->uint the numbers will be lower
192 // than they should be, we try to append to the left side first
193 //
194 // check whether the left boundary of the approached street has
195 // been overridden; if so, move all lanes to the right
196 if (dest + loffset >= numOutgoingLanes) {
197 loffset -= 1;
198 roffset += 1;
199 for (int i = 0; i < (int)ret->size(); i++) {
200 (*ret)[i] = (*ret)[i] - 1;
201 }
202 }
203 // append the next lane to the left of all edges
204 // increase the position (destination edge)
205 ret->push_back(dest + loffset);
206 noSet++;
207 loffset += 1;
208
209 // as above
210 if (numOutgoingLanes == noSet) {
211 return ret;
212 }
213
214 // now we try to append the next lane to the right side, when needed
215 if (noSet < numLanes) {
216 // check whether the right boundary of the approached street has
217 // been overridden; if so, move all lanes to the right
218 if (dest < roffset) {
219 loffset += 1;
220 roffset -= 1;
221 for (int i = 0; i < (int)ret->size(); i++) {
222 (*ret)[i] = (*ret)[i] + 1;
223 }
224 }
225 ret->push_front(dest - roffset);
226 noSet++;
227 roffset += 1;
228 }
229 }
230 return ret;
231 }
232
233
Crossing(const NBNode * _node,const EdgeVector & _edges,double _width,bool _priority,int _customTLIndex,int _customTLIndex2,const PositionVector & _customShape)234 NBNode::Crossing::Crossing(const NBNode* _node, const EdgeVector& _edges, double _width, bool _priority, int _customTLIndex, int _customTLIndex2, const PositionVector& _customShape) :
235 Parameterised(),
236 node(_node),
237 edges(_edges),
238 customWidth(_width),
239 width(_width),
240 priority(_priority),
241 customShape(_customShape),
242 tlLinkIndex(_customTLIndex),
243 tlLinkIndex2(_customTLIndex2),
244 customTLIndex(_customTLIndex),
245 customTLIndex2(_customTLIndex2),
246 valid(true) {
247 }
248
249 /* -------------------------------------------------------------------------
250 * NBNode-methods
251 * ----------------------------------------------------------------------- */
NBNode(const std::string & id,const Position & position,SumoXMLNodeType type)252 NBNode::NBNode(const std::string& id, const Position& position,
253 SumoXMLNodeType type) :
254 Named(StringUtils::convertUmlaute(id)),
255 myPosition(position),
256 myType(type),
257 myDistrict(nullptr),
258 myHaveCustomPoly(false),
259 myRequest(nullptr),
260 myRadius(UNSPECIFIED_RADIUS),
261 myKeepClear(OptionsCont::getOptions().getBool("default.junctions.keep-clear")),
262 myRightOfWay(SUMOXMLDefinitions::RightOfWayValues.get(OptionsCont::getOptions().getString("default.right-of-way"))),
263 myFringeType(FRINGE_TYPE_DEFAULT),
264 myDiscardAllCrossings(false),
265 myCrossingsLoadedFromSumoNet(0),
266 myDisplacementError(0),
267 myIsBentPriority(false),
268 myTypeWasGuessed(false) {
269 if (!SUMOXMLDefinitions::isValidNetID(myID)) {
270 throw ProcessError("Invalid node id '" + myID + "'.");
271 }
272 }
273
274
NBNode(const std::string & id,const Position & position,NBDistrict * district)275 NBNode::NBNode(const std::string& id, const Position& position, NBDistrict* district) :
276 Named(StringUtils::convertUmlaute(id)),
277 myPosition(position),
278 myType(district == nullptr ? NODETYPE_UNKNOWN : NODETYPE_DISTRICT),
279 myDistrict(district),
280 myHaveCustomPoly(false),
281 myRequest(nullptr),
282 myRadius(UNSPECIFIED_RADIUS),
283 myKeepClear(OptionsCont::getOptions().getBool("default.junctions.keep-clear")),
284 myRightOfWay(SUMOXMLDefinitions::RightOfWayValues.get(OptionsCont::getOptions().getString("default.right-of-way"))),
285 myFringeType(FRINGE_TYPE_DEFAULT),
286 myDiscardAllCrossings(false),
287 myCrossingsLoadedFromSumoNet(0),
288 myDisplacementError(0),
289 myIsBentPriority(false),
290 myTypeWasGuessed(false) {
291 if (!SUMOXMLDefinitions::isValidNetID(myID)) {
292 throw ProcessError("Invalid node id '" + myID + "'.");
293 }
294 }
295
296
~NBNode()297 NBNode::~NBNode() {
298 delete myRequest;
299 }
300
301
302 void
reinit(const Position & position,SumoXMLNodeType type,bool updateEdgeGeometries)303 NBNode::reinit(const Position& position, SumoXMLNodeType type,
304 bool updateEdgeGeometries) {
305 myPosition = position;
306 // patch type
307 myType = type;
308 if (!isTrafficLight(myType)) {
309 removeTrafficLights();
310 }
311 if (updateEdgeGeometries) {
312 for (EdgeVector::iterator i = myIncomingEdges.begin(); i != myIncomingEdges.end(); i++) {
313 PositionVector geom = (*i)->getGeometry();
314 geom[-1] = myPosition;
315 (*i)->setGeometry(geom);
316 }
317 for (EdgeVector::iterator i = myOutgoingEdges.begin(); i != myOutgoingEdges.end(); i++) {
318 PositionVector geom = (*i)->getGeometry();
319 geom[0] = myPosition;
320 (*i)->setGeometry(geom);
321 }
322 }
323 }
324
325
326
327 // ----------- Applying offset
328 void
reshiftPosition(double xoff,double yoff)329 NBNode::reshiftPosition(double xoff, double yoff) {
330 myPosition.add(xoff, yoff, 0);
331 myPoly.add(xoff, yoff, 0);
332 for (auto& wacs : myWalkingAreaCustomShapes) {
333 wacs.shape.add(xoff, yoff, 0);
334 }
335 }
336
337
338 void
mirrorX()339 NBNode::mirrorX() {
340 myPosition.mul(1, -1);
341 myPoly.mirrorX();
342 // mirror pre-computed geometty of crossings and walkingareas
343 for (auto c : myCrossings) {
344 c->shape.mirrorX();
345 }
346 for (std::vector<WalkingArea>::iterator it_wa = myWalkingAreas.begin(); it_wa != myWalkingAreas.end(); ++it_wa) {
347 (*it_wa).shape.mirrorX();
348 }
349 }
350
351
352 // ----------- Methods for dealing with assigned traffic lights
353 void
addTrafficLight(NBTrafficLightDefinition * tlDef)354 NBNode::addTrafficLight(NBTrafficLightDefinition* tlDef) {
355 myTrafficLights.insert(tlDef);
356 // rail signals receive a temporary traffic light in order to set connection tl-linkIndex
357 if (!isTrafficLight(myType) && myType != NODETYPE_RAIL_SIGNAL && myType != NODETYPE_RAIL_CROSSING) {
358 myType = NODETYPE_TRAFFIC_LIGHT;
359 }
360 }
361
362
363 void
removeTrafficLight(NBTrafficLightDefinition * tlDef)364 NBNode::removeTrafficLight(NBTrafficLightDefinition* tlDef) {
365 tlDef->removeNode(this);
366 myTrafficLights.erase(tlDef);
367 }
368
369
370 void
removeTrafficLights()371 NBNode::removeTrafficLights() {
372 std::set<NBTrafficLightDefinition*> trafficLights = myTrafficLights; // make a copy because we will modify the original
373 for (std::set<NBTrafficLightDefinition*>::const_iterator i = trafficLights.begin(); i != trafficLights.end(); ++i) {
374 removeTrafficLight(*i);
375 }
376 }
377
378
379 void
invalidateTLS(NBTrafficLightLogicCont & tlCont,bool removedConnections,bool addedConnections)380 NBNode::invalidateTLS(NBTrafficLightLogicCont& tlCont, bool removedConnections, bool addedConnections) {
381 if (isTLControlled()) {
382 std::set<NBTrafficLightDefinition*> oldDefs(myTrafficLights);
383 for (std::set<NBTrafficLightDefinition*>::iterator it = oldDefs.begin(); it != oldDefs.end(); ++it) {
384 NBTrafficLightDefinition* orig = *it;
385 if (dynamic_cast<NBLoadedSUMOTLDef*>(orig) != nullptr) {
386 dynamic_cast<NBLoadedSUMOTLDef*>(orig)->registerModifications(removedConnections, addedConnections);
387 } else if (dynamic_cast<NBOwnTLDef*>(orig) == nullptr) {
388 NBTrafficLightDefinition* newDef = new NBOwnTLDef(orig->getID(), orig->getOffset(), orig->getType());
389 const std::vector<NBNode*>& nodes = orig->getNodes();
390 while (!nodes.empty()) {
391 newDef->addNode(nodes.front());
392 nodes.front()->removeTrafficLight(orig);
393 }
394 tlCont.removeFully(orig->getID());
395 tlCont.insert(newDef);
396 }
397 }
398 }
399 }
400
401
402 void
shiftTLConnectionLaneIndex(NBEdge * edge,int offset,int threshold)403 NBNode::shiftTLConnectionLaneIndex(NBEdge* edge, int offset, int threshold) {
404 for (std::set<NBTrafficLightDefinition*>::iterator it = myTrafficLights.begin(); it != myTrafficLights.end(); ++it) {
405 (*it)->shiftTLConnectionLaneIndex(edge, offset, threshold);
406 }
407 }
408
409 // ----------- Prunning the input
410 int
removeSelfLoops(NBDistrictCont & dc,NBEdgeCont & ec,NBTrafficLightLogicCont & tc)411 NBNode::removeSelfLoops(NBDistrictCont& dc, NBEdgeCont& ec, NBTrafficLightLogicCont& tc) {
412 int ret = 0;
413 int pos = 0;
414 EdgeVector::const_iterator j = myIncomingEdges.begin();
415 while (j != myIncomingEdges.end()) {
416 // skip edges which are only incoming and not outgoing
417 if (find(myOutgoingEdges.begin(), myOutgoingEdges.end(), *j) == myOutgoingEdges.end()) {
418 ++j;
419 ++pos;
420 continue;
421 }
422 // an edge with both its origin and destination being the current
423 // node should be removed
424 NBEdge* dummy = *j;
425 WRITE_WARNING(" Removing self-looping edge '" + dummy->getID() + "'");
426 // get the list of incoming edges connected to the self-loop
427 EdgeVector incomingConnected = dummy->getIncomingEdges();;
428 // get the list of outgoing edges connected to the self-loop
429 EdgeVector outgoingConnected = dummy->getConnectedEdges();
430 // let the self-loop remap its connections
431 dummy->remapConnections(incomingConnected);
432 remapRemoved(tc, dummy, incomingConnected, outgoingConnected);
433 // delete the self-loop
434 ec.erase(dc, dummy);
435 j = myIncomingEdges.begin() + pos;
436 ++ret;
437 }
438 return ret;
439 }
440
441
442 // -----------
443 void
addIncomingEdge(NBEdge * edge)444 NBNode::addIncomingEdge(NBEdge* edge) {
445 assert(edge != 0);
446 if (find(myIncomingEdges.begin(), myIncomingEdges.end(), edge) == myIncomingEdges.end()) {
447 myIncomingEdges.push_back(edge);
448 myAllEdges.push_back(edge);
449 }
450 }
451
452
453 void
addOutgoingEdge(NBEdge * edge)454 NBNode::addOutgoingEdge(NBEdge* edge) {
455 assert(edge != 0);
456 if (find(myOutgoingEdges.begin(), myOutgoingEdges.end(), edge) == myOutgoingEdges.end()) {
457 myOutgoingEdges.push_back(edge);
458 myAllEdges.push_back(edge);
459 }
460 }
461
462
463 bool
isSimpleContinuation(bool checkLaneNumbers) const464 NBNode::isSimpleContinuation(bool checkLaneNumbers) const {
465 // one in, one out->continuation
466 if (myIncomingEdges.size() == 1 && myOutgoingEdges.size() == 1) {
467 // both must have the same number of lanes
468 return !checkLaneNumbers || ((*(myIncomingEdges.begin()))->getNumLanes() == (*(myOutgoingEdges.begin()))->getNumLanes());
469 }
470 // two in and two out and both in reverse direction
471 if (myIncomingEdges.size() == 2 && myOutgoingEdges.size() == 2) {
472 for (EdgeVector::const_iterator i = myIncomingEdges.begin(); i != myIncomingEdges.end(); i++) {
473 NBEdge* in = *i;
474 EdgeVector::const_iterator opposite = find_if(myOutgoingEdges.begin(), myOutgoingEdges.end(), NBContHelper::opposite_finder(in));
475 // must have an opposite edge
476 if (opposite == myOutgoingEdges.end()) {
477 return false;
478 }
479 // both must have the same number of lanes
480 NBContHelper::nextCW(myOutgoingEdges, opposite);
481 if (checkLaneNumbers && in->getNumLanes() != (*opposite)->getNumLanes()) {
482 return false;
483 }
484 }
485 return true;
486 }
487 // nope
488 return false;
489 }
490
491
492 PositionVector
computeSmoothShape(const PositionVector & begShape,const PositionVector & endShape,int numPoints,bool isTurnaround,double extrapolateBeg,double extrapolateEnd,NBNode * recordError,int shapeFlag) const493 NBNode::computeSmoothShape(const PositionVector& begShape,
494 const PositionVector& endShape,
495 int numPoints,
496 bool isTurnaround,
497 double extrapolateBeg,
498 double extrapolateEnd,
499 NBNode* recordError,
500 int shapeFlag) const {
501
502 bool ok = true;
503 PositionVector init = bezierControlPoints(begShape, endShape, isTurnaround, extrapolateBeg, extrapolateEnd, ok, recordError, DEG2RAD(5), shapeFlag);
504 #ifdef DEBUG_SMOOTH_GEOM
505 if (DEBUGCOND) {
506 std::cout << "computeSmoothShape node " << getID() << " init=" << init << "\n";
507 }
508 #endif
509 if (init.size() == 0) {
510 PositionVector ret;
511 ret.push_back(begShape.back());
512 ret.push_back(endShape.front());
513 return ret;
514 } else {
515 return init.bezier(numPoints).smoothedZFront();
516 }
517 }
518
519 PositionVector
bezierControlPoints(const PositionVector & begShape,const PositionVector & endShape,bool isTurnaround,double extrapolateBeg,double extrapolateEnd,bool & ok,NBNode * recordError,double straightThresh,int shapeFlag)520 NBNode::bezierControlPoints(
521 const PositionVector& begShape,
522 const PositionVector& endShape,
523 bool isTurnaround,
524 double extrapolateBeg,
525 double extrapolateEnd,
526 bool& ok,
527 NBNode* recordError,
528 double straightThresh,
529 int shapeFlag) {
530
531 const Position beg = begShape.back();
532 const Position end = endShape.front();
533 const double dist = beg.distanceTo2D(end);
534 PositionVector init;
535 if (dist < POSITION_EPS || beg.distanceTo2D(begShape[-2]) < POSITION_EPS || end.distanceTo2D(endShape[1]) < POSITION_EPS) {
536 #ifdef DEBUG_SMOOTH_GEOM
537 if (DEBUGCOND2(recordError)) std::cout << " bezierControlPoints failed beg=" << beg << " end=" << end
538 << " dist=" << dist
539 << " distBegLast=" << beg.distanceTo2D(begShape[-2])
540 << " distEndFirst=" << end.distanceTo2D(endShape[1])
541 << "\n";
542 #endif
543 // typically, this node a is a simpleContinuation. see also #2539
544 return init;
545 } else {
546 init.push_back(beg);
547 if (isTurnaround) {
548 // turnarounds:
549 // - end of incoming lane
550 // - position between incoming/outgoing end/begin shifted by the distance orthogonally
551 // - begin of outgoing lane
552 Position center = PositionVector::positionAtOffset2D(beg, end, beg.distanceTo2D(end) / (double) 2.);
553 center.sub(beg.y() - end.y(), end.x() - beg.x());
554 init.push_back(center);
555 } else {
556 const double angle = GeomHelper::angleDiff(begShape.angleAt2D(-2), endShape.angleAt2D(0));
557 PositionVector endShapeBegLine(endShape[0], endShape[1]);
558 PositionVector begShapeEndLineRev(begShape[-1], begShape[-2]);
559 endShapeBegLine.extrapolate2D(100, true);
560 begShapeEndLineRev.extrapolate2D(100, true);
561 if (fabs(angle) < M_PI / 4.) {
562 // very low angle: could be an s-shape or a straight line
563 const double displacementAngle = GeomHelper::angleDiff(begShape.angleAt2D(-2), beg.angleTo2D(end));
564 const double bendDeg = RAD2DEG(fabs(displacementAngle - angle));
565 const double halfDistance = dist / 2;
566 if (fabs(displacementAngle) <= straightThresh && fabs(angle) <= straightThresh) {
567 #ifdef DEBUG_SMOOTH_GEOM
568 if (DEBUGCOND2(recordError)) std::cout << " bezierControlPoints identified straight line beg=" << beg << " end=" << end
569 << " angle=" << RAD2DEG(angle) << " displacementAngle=" << RAD2DEG(displacementAngle) << "\n";
570 #endif
571 return PositionVector();
572 } else if (bendDeg > 22.5 && pow(bendDeg / 45, 2) / dist > 0.13) {
573 // do not allow s-curves with extreme bends
574 // (a linear dependency is to restrictive at low displacementAngles and too permisive at high angles)
575 #ifdef DEBUG_SMOOTH_GEOM
576 if (DEBUGCOND2(recordError)) std::cout << " bezierControlPoints found extreme s-curve, falling back to straight line beg=" << beg << " end=" << end
577 << " angle=" << RAD2DEG(angle) << " displacementAngle=" << RAD2DEG(displacementAngle)
578 << " dist=" << dist << " bendDeg=" << bendDeg << " bd2=" << pow(bendDeg / 45, 2)
579 << " displacementError=" << sin(displacementAngle) * dist
580 << " begShape=" << begShape << " endShape=" << endShape << "\n";
581 #endif
582 ok = false;
583 if (recordError != nullptr) {
584 recordError->myDisplacementError = MAX2(recordError->myDisplacementError, (double)fabs(sin(displacementAngle) * dist));
585 }
586 return PositionVector();
587 } else {
588 const double endLength = begShape[-2].distanceTo2D(begShape[-1]);
589 const double off1 = endLength + MIN2(extrapolateBeg, halfDistance);
590 init.push_back(PositionVector::positionAtOffset2D(begShapeEndLineRev[1], begShapeEndLineRev[0], off1));
591 const double off2 = 100. - MIN2(extrapolateEnd, halfDistance);
592 init.push_back(PositionVector::positionAtOffset2D(endShapeBegLine[0], endShapeBegLine[1], off2));
593 #ifdef DEBUG_SMOOTH_GEOM
594 if (DEBUGCOND2(recordError)) std::cout << " bezierControlPoints found s-curve beg=" << beg << " end=" << end
595 << " angle=" << RAD2DEG(angle) << " displacementAngle=" << RAD2DEG(displacementAngle)
596 << " halfDistance=" << halfDistance << "\n";
597 #endif
598 }
599 } else {
600 // turning
601 // - end of incoming lane
602 // - intersection of the extrapolated lanes
603 // - begin of outgoing lane
604 // attention: if there is no intersection, use a straight line
605 Position intersect = endShapeBegLine.intersectionPosition2D(begShapeEndLineRev);
606 if (intersect == Position::INVALID) {
607 #ifdef DEBUG_SMOOTH_GEOM
608 if (DEBUGCOND2(recordError)) {
609 std::cout << " bezierControlPoints failed beg=" << beg << " end=" << end << " intersect=" << intersect
610 << " endShapeBegLine=" << endShapeBegLine
611 << " begShapeEndLineRev=" << begShapeEndLineRev
612 << "\n";
613 }
614 #endif
615 ok = false;
616 if (recordError != nullptr) {
617 // it's unclear if this error can be solved via stretching the intersection.
618 recordError->myDisplacementError = MAX2(recordError->myDisplacementError, (double)1.0);
619 }
620 return PositionVector();
621 }
622 const double minControlLength = MIN2((double)1.0, dist / 2);
623 const double distBeg = intersect.distanceTo2D(beg);
624 const double distEnd = intersect.distanceTo2D(end);
625 const bool lengthenBeg = distBeg <= minControlLength;
626 const bool lengthenEnd = distEnd <= minControlLength;
627 if (lengthenBeg && lengthenEnd) {
628 #ifdef DEBUG_SMOOTH_GEOM
629 if (DEBUGCOND2(recordError)) std::cout << " bezierControlPoints failed beg=" << beg << " end=" << end << " intersect=" << intersect
630 << " distBeg=" << distBeg << " distEnd=" << distEnd << "\n";
631 #endif
632 if (recordError != nullptr) {
633 // This should be fixable with minor stretching
634 recordError->myDisplacementError = MAX2(recordError->myDisplacementError, (double)1.0);
635 }
636 ok = false;
637 return PositionVector();
638 } else if ((shapeFlag & FOUR_CONTROL_POINTS)) {
639 init.push_back(begShapeEndLineRev.positionAtOffset2D(100 - extrapolateBeg));
640 init.push_back(endShapeBegLine.positionAtOffset2D(100 - extrapolateEnd));
641 } else if (lengthenBeg || lengthenEnd) {
642 init.push_back(begShapeEndLineRev.positionAtOffset2D(100 - minControlLength));
643 init.push_back(endShapeBegLine.positionAtOffset2D(100 - minControlLength));
644 } else if ((shapeFlag & AVOID_WIDE_LEFT_TURN) != 0
645 // there are two reasons for enabling special geometry rules:
646 // 1) sharp edge angles which could cause overshoot
647 // 2) junction geometries with a large displacement between opposite left turns
648 // which would cause the default geometry to overlap
649 && ((shapeFlag & AVOID_INTERSECTING_LEFT_TURNS) != 0
650 || (angle > DEG2RAD(95) && (distBeg > 20 || distEnd > 20)))) {
651 //std::cout << " bezierControlPoints intersect=" << intersect << " dist=" << dist << " distBeg=" << distBeg << " distEnd=" << distEnd << " angle=" << RAD2DEG(angle) << " flag=" << shapeFlag << "\n";
652 const double factor = ((shapeFlag & AVOID_INTERSECTING_LEFT_TURNS) == 0 ? 1
653 : MIN2(0.6, 16 / dist));
654 init.push_back(begShapeEndLineRev.positionAtOffset2D(100 - MIN2(distBeg * factor / 1.2, dist * factor / 1.8)));
655 init.push_back(endShapeBegLine.positionAtOffset2D(100 - MIN2(distEnd * factor / 1.2, dist * factor / 1.8)));
656 } else if ((shapeFlag & AVOID_WIDE_RIGHT_TURN) != 0 && angle < DEG2RAD(-95) && (distBeg > 20 || distEnd > 20)) {
657 //std::cout << " bezierControlPoints intersect=" << intersect << " distBeg=" << distBeg << " distEnd=" << distEnd << "\n";
658 init.push_back(begShapeEndLineRev.positionAtOffset2D(100 - MIN2(distBeg / 1.4, dist / 2)));
659 init.push_back(endShapeBegLine.positionAtOffset2D(100 - MIN2(distEnd / 1.4, dist / 2)));
660 } else {
661 double z;
662 const double z1 = begShapeEndLineRev.positionAtOffset2D(begShapeEndLineRev.nearest_offset_to_point2D(intersect)).z();
663 const double z2 = endShapeBegLine.positionAtOffset2D(endShapeBegLine.nearest_offset_to_point2D(intersect)).z();
664 const double z3 = 0.5 * (beg.z() + end.z());
665 // if z1 and z2 are on the same side in regard to z3 then we
666 // can use their avarage. Otherwise, the intersection in 3D
667 // is not good and we are better of using z3
668 if ((z1 <= z3 && z2 <= z3) || (z1 >= z3 && z2 >= z3)) {
669 z = 0.5 * (z1 + z2);
670 } else {
671 z = z3;
672 }
673 intersect.set(intersect.x(), intersect.y(), z);
674 init.push_back(intersect);
675 }
676 }
677 }
678 init.push_back(end);
679 }
680 return init;
681 }
682
683
684 PositionVector
computeInternalLaneShape(NBEdge * fromE,const NBEdge::Connection & con,int numPoints,NBNode * recordError,int shapeFlag) const685 NBNode::computeInternalLaneShape(NBEdge* fromE, const NBEdge::Connection& con, int numPoints, NBNode* recordError, int shapeFlag) const {
686 if (con.fromLane >= fromE->getNumLanes()) {
687 throw ProcessError("Connection '" + con.getDescription(fromE) + "' starts at a non-existant lane.");
688 }
689 if (con.toLane >= con.toEdge->getNumLanes()) {
690 throw ProcessError("Connection '" + con.getDescription(fromE) + "' targets a non-existant lane.");
691 }
692 PositionVector fromShape = fromE->getLaneShape(con.fromLane);
693 PositionVector toShape = con.toEdge->getLaneShape(con.toLane);
694 PositionVector ret;
695 bool useCustomShape = con.customShape.size() > 0;
696 if (useCustomShape) {
697 // ensure that the shape starts and ends at the intersection boundary
698 PositionVector startBorder = fromE->getNodeBorder(this);
699 if (startBorder.size() == 0) {
700 startBorder = fromShape.getOrthogonal(fromShape.back(), 1, true);
701 }
702 PositionVector tmp = NBEdge::startShapeAt(con.customShape, this, startBorder);
703 if (tmp.size() < 2) {
704 WRITE_WARNING("Could not use custom shape for connection " + con.getDescription(fromE));
705 useCustomShape = false;
706 } else {
707 if (tmp.length2D() > con.customShape.length2D() + POSITION_EPS) {
708 // shape was lengthened at the start, make sure it attaches at the center of the lane
709 tmp[0] = fromShape.back();
710 } else if (recordError != nullptr) {
711 const double offset = tmp[0].distanceTo2D(fromShape.back());
712 if (offset > fromE->getLaneWidth(con.fromLane) / 2) {
713 WRITE_WARNING("Custom shape has distance " + toString(offset) + " to incoming lane for connection " + con.getDescription(fromE));
714 }
715 }
716 PositionVector endBorder = con.toEdge->getNodeBorder(this);
717 if (endBorder.size() == 0) {
718 endBorder = toShape.getOrthogonal(toShape.front(), 1, false);
719 }
720 ret = NBEdge::startShapeAt(tmp.reverse(), this, endBorder).reverse();
721 if (ret.size() < 2) {
722 WRITE_WARNING("Could not use custom shape for connection " + con.getDescription(fromE));
723 useCustomShape = false;
724 } else if (ret.length2D() > tmp.length2D() + POSITION_EPS) {
725 // shape was lengthened at the end, make sure it attaches at the center of the lane
726 ret[-1] = toShape.front();
727 } else if (recordError != nullptr) {
728 const double offset = ret[-1].distanceTo2D(toShape.front());
729 if (offset > con.toEdge->getLaneWidth(con.toLane) / 2) {
730 WRITE_WARNING("Custom shape has distance " + toString(offset) + " to outgoing lane for connection " + con.getDescription(fromE));
731 }
732 }
733 }
734 }
735 if (!useCustomShape) {
736 displaceShapeAtWidthChange(fromE, con, fromShape, toShape);
737 double extrapolateBeg = 5. * fromE->getNumLanes();
738 double extrapolateEnd = 5. * con.toEdge->getNumLanes();
739 LinkDirection dir = getDirection(fromE, con.toEdge);
740 if (dir == LINKDIR_LEFT || dir == LINKDIR_TURN) {
741 shapeFlag += AVOID_WIDE_LEFT_TURN;
742 }
743 #ifdef DEBUG_SMOOTH_GEOM
744 if (DEBUGCOND) {
745 std::cout << "computeInternalLaneShape node " << getID() << " fromE=" << fromE->getID() << " toE=" << con.toEdge->getID() << "\n";
746 }
747 #endif
748 ret = computeSmoothShape(fromShape, toShape,
749 numPoints, fromE->getTurnDestination() == con.toEdge,
750 extrapolateBeg, extrapolateEnd, recordError, shapeFlag);
751 }
752 const NBEdge::Lane& lane = fromE->getLaneStruct(con.fromLane);
753 if (lane.endOffset > 0) {
754 PositionVector beg = lane.shape.getSubpart(lane.shape.length() - lane.endOffset, lane.shape.length());;
755 beg.append(ret);
756 ret = beg;
757 }
758 if (con.toEdge->isBidiRail() && con.toEdge->getTurnDestination(true)->getEndOffset() > 0) {
759 PositionVector end = toShape.getSubpart(0, con.toEdge->getTurnDestination(true)->getEndOffset());
760 ret.append(end);
761 }
762 return ret;
763 }
764
765
766 bool
isConstantWidthTransition() const767 NBNode::isConstantWidthTransition() const {
768 return (myIncomingEdges.size() == 1
769 && myOutgoingEdges.size() == 1
770 && myIncomingEdges[0]->getNumLanes() != myOutgoingEdges[0]->getNumLanes()
771 && myIncomingEdges[0]->getTotalWidth() == myOutgoingEdges[0]->getTotalWidth());
772 }
773
774 void
displaceShapeAtWidthChange(const NBEdge * from,const NBEdge::Connection & con,PositionVector & fromShape,PositionVector & toShape) const775 NBNode::displaceShapeAtWidthChange(const NBEdge* from, const NBEdge::Connection& con,
776 PositionVector& fromShape, PositionVector& toShape) const {
777 if (isConstantWidthTransition()) {
778 // displace shapes
779 NBEdge* in = myIncomingEdges[0];
780 NBEdge* out = myOutgoingEdges[0];
781 double outCenter = out->getLaneWidth(con.toLane) / 2;
782 for (int i = 0; i < con.toLane; ++i) {
783 outCenter += out->getLaneWidth(i);
784 }
785 double inCenter = in->getLaneWidth(con.fromLane) / 2;
786 for (int i = 0; i < con.fromLane; ++i) {
787 inCenter += in->getLaneWidth(i);
788 }
789 //std::cout << "displaceShapeAtWidthChange inCenter=" << inCenter << " outCenter=" << outCenter << "\n";
790 try {
791 if (in->getNumLanes() > out->getNumLanes()) {
792 // shift toShape so the internal lane ends straight at the displaced entry point
793 toShape.move2side(outCenter - inCenter);
794 } else {
795 // shift fromShape so the internal lane starts straight at the displaced exit point
796 fromShape.move2side(inCenter - outCenter);
797
798 }
799 } catch (InvalidArgument&) { }
800 } else {
801 SVCPermissions fromP = from->getPermissions(con.fromLane);
802 SVCPermissions toP = con.toEdge->getPermissions(con.toLane);
803 if ((fromP & toP) == SVC_BICYCLE && (fromP | toP) != SVC_BICYCLE) {
804 double shift = (from->getLaneWidth(con.fromLane) - con.toEdge->getLaneWidth(con.toLane)) / 2;
805 if (toP == SVC_BICYCLE) {
806 // let connection to dedicated bicycle lane start on the right side of a mixed lane for straight an right-going connections
807 // (on the left side for left turns)
808 // XXX indirect left turns should also start on the right side
809 LinkDirection dir = getDirection(from, con.toEdge);
810 if (dir == LINKDIR_LEFT || dir == LINKDIR_PARTLEFT || dir == LINKDIR_TURN) {
811 fromShape.move2side(-shift);
812 } else {
813 fromShape.move2side(shift);
814 }
815 } else if (fromP == SVC_BICYCLE) {
816 // let connection from dedicated bicycle end on the right side of a mixed lane
817 toShape.move2side(-shift);
818 }
819 }
820 }
821 }
822
823 bool
needsCont(const NBEdge * fromE,const NBEdge * otherFromE,const NBEdge::Connection & c,const NBEdge::Connection & otherC) const824 NBNode::needsCont(const NBEdge* fromE, const NBEdge* otherFromE,
825 const NBEdge::Connection& c, const NBEdge::Connection& otherC) const {
826 const NBEdge* toE = c.toEdge;
827 const NBEdge* otherToE = otherC.toEdge;
828
829 if (myType == NODETYPE_RIGHT_BEFORE_LEFT || myType == NODETYPE_ALLWAY_STOP) {
830 return false;
831 }
832 LinkDirection d1 = getDirection(fromE, toE);
833 const bool thisRight = (d1 == LINKDIR_RIGHT || d1 == LINKDIR_PARTRIGHT);
834 const bool rightTurnConflict = (thisRight &&
835 NBNode::rightTurnConflict(fromE, toE, c.fromLane, otherFromE, otherToE, otherC.fromLane));
836 if (thisRight && !rightTurnConflict) {
837 return false;
838 }
839 if (!(foes(otherFromE, otherToE, fromE, toE) || myRequest == nullptr || rightTurnConflict)) {
840 // if they do not cross, no waiting place is needed
841 return false;
842 }
843 LinkDirection d2 = getDirection(otherFromE, otherToE);
844 if (d2 == LINKDIR_TURN) {
845 return false;
846 }
847 const bool thisLeft = (d1 == LINKDIR_LEFT || d1 == LINKDIR_TURN);
848 const bool otherLeft = (d2 == LINKDIR_LEFT || d2 == LINKDIR_TURN);
849 const bool bothLeft = thisLeft && otherLeft;
850 if (fromE == otherFromE && !thisRight) {
851 // ignore same edge links except for right-turns
852 return false;
853 }
854 if (thisRight && d2 != LINKDIR_STRAIGHT) {
855 return false;
856 }
857 if (c.tlID != "" && !bothLeft) {
858 assert(myTrafficLights.size() > 0 || myType == NODETYPE_RAIL_CROSSING || myType == NODETYPE_RAIL_SIGNAL);
859 for (std::set<NBTrafficLightDefinition*>::const_iterator it = myTrafficLights.begin(); it != myTrafficLights.end(); ++it) {
860 if ((*it)->needsCont(fromE, toE, otherFromE, otherToE)) {
861 return true;
862 }
863 }
864 return false;
865 }
866 if (fromE->getJunctionPriority(this) > 0 && otherFromE->getJunctionPriority(this) > 0) {
867 return mustBrake(fromE, toE, c.fromLane, c.toLane, false);
868 }
869 return false;
870 }
871
872 bool
tlsContConflict(const NBEdge * from,const NBEdge::Connection & c,const NBEdge * foeFrom,const NBEdge::Connection & foe) const873 NBNode::tlsContConflict(const NBEdge* from, const NBEdge::Connection& c,
874 const NBEdge* foeFrom, const NBEdge::Connection& foe) const {
875 return (foe.haveVia && isTLControlled() && c.tlLinkIndex >= 0 && foe.tlLinkIndex >= 0
876 && !foeFrom->isTurningDirectionAt(foe.toEdge)
877 && foes(from, c.toEdge, foeFrom, foe.toEdge)
878 && !needsCont(foeFrom, from, foe, c));
879 }
880
881
882 void
removeJoinedTrafficLights()883 NBNode::removeJoinedTrafficLights() {
884 std::set<NBTrafficLightDefinition*> trafficLights = myTrafficLights; // make a copy because we will modify the original
885 for (std::set<NBTrafficLightDefinition*>::const_iterator i = trafficLights.begin(); i != trafficLights.end(); ++i) {
886 // if this is the only controlled node we keep the tlDef as it is to generate a warning later
887 if ((*i)->getNodes().size() > 1) {
888 myTrafficLights.erase(*i);
889 (*i)->removeNode(this);
890 (*i)->setParticipantsInformation();
891 (*i)->setTLControllingInformation();
892 }
893 }
894 }
895
896 void
computeLogic(const NBEdgeCont & ec,OptionsCont & oc)897 NBNode::computeLogic(const NBEdgeCont& ec, OptionsCont& oc) {
898 delete myRequest; // possibly recomputation step
899 myRequest = nullptr;
900 if (myIncomingEdges.size() == 0 || myOutgoingEdges.size() == 0) {
901 // no logic if nothing happens here
902 myType = NODETYPE_DEAD_END;
903 removeJoinedTrafficLights();
904 return;
905 }
906 // check whether the node was set to be unregulated by the user
907 if (oc.getBool("keep-nodes-unregulated") || oc.isInStringVector("keep-nodes-unregulated.explicit", getID())
908 || (oc.getBool("keep-nodes-unregulated.district-nodes") && (isNearDistrict() || isDistrict()))) {
909 myType = NODETYPE_NOJUNCTION;
910 return;
911 }
912 // compute the logic if necessary or split the junction
913 if (myType != NODETYPE_NOJUNCTION && myType != NODETYPE_DISTRICT && myType != NODETYPE_TRAFFIC_LIGHT_NOJUNCTION) {
914 // build the request
915 myRequest = new NBRequest(ec, this, myAllEdges, myIncomingEdges, myOutgoingEdges, myBlockedConnections);
916 // check whether it is not too large
917 int numConnections = numNormalConnections();
918 if (numConnections >= SUMO_MAX_CONNECTIONS) {
919 // yep -> make it untcontrolled, warn
920 delete myRequest;
921 myRequest = nullptr;
922 if (myType == NODETYPE_TRAFFIC_LIGHT) {
923 myType = NODETYPE_TRAFFIC_LIGHT_NOJUNCTION;
924 } else {
925 myType = NODETYPE_NOJUNCTION;
926 }
927 WRITE_WARNING("Junction '" + getID() + "' is too complicated (" + toString(numConnections)
928 + " connections, max " + toString(SUMO_MAX_CONNECTIONS) + "); will be set to " + toString(myType));
929 } else if (numConnections == 0) {
930 delete myRequest;
931 myRequest = nullptr;
932 myType = NODETYPE_DEAD_END;
933 removeJoinedTrafficLights();
934 } else {
935 myRequest->buildBitfieldLogic();
936 }
937 }
938 }
939
940
941 void
computeLogic2(bool checkLaneFoes)942 NBNode::computeLogic2(bool checkLaneFoes) {
943 if (myRequest != nullptr) {
944 myRequest->computeLogic(checkLaneFoes);
945 }
946 }
947
948
949 bool
writeLogic(OutputDevice & into) const950 NBNode::writeLogic(OutputDevice& into) const {
951 if (myRequest) {
952 myRequest->writeLogic(into);
953 return true;
954 }
955 return false;
956 }
957
958
959 const std::string
getFoes(int linkIndex) const960 NBNode::getFoes(int linkIndex) const {
961 if (myRequest == nullptr) {
962 return "";
963 } else {
964 return myRequest->getFoes(linkIndex);
965 }
966 }
967
968
969 const std::string
getResponse(int linkIndex) const970 NBNode::getResponse(int linkIndex) const {
971 if (myRequest == nullptr) {
972 return "";
973 } else {
974 return myRequest->getResponse(linkIndex);
975 }
976 }
977
978
979 void
computeNodeShape(double mismatchThreshold)980 NBNode::computeNodeShape(double mismatchThreshold) {
981 if (myHaveCustomPoly) {
982 return;
983 }
984 if (myIncomingEdges.size() == 0 && myOutgoingEdges.size() == 0) {
985 // may be an intermediate step during network editing
986 myPoly.clear();
987 myPoly.push_back(myPosition);
988 return;
989 }
990 try {
991 NBNodeShapeComputer computer(*this);
992 myPoly = computer.compute();
993 if (myRadius == UNSPECIFIED_RADIUS && !OptionsCont::getOptions().isDefault("default.junctions.radius")) {
994 myRadius = computer.getRadius();
995 }
996 if (myPoly.size() > 0) {
997 PositionVector tmp = myPoly;
998 tmp.push_back_noDoublePos(tmp[0]); // need closed shape
999 if (mismatchThreshold >= 0
1000 && !tmp.around(myPosition)
1001 && tmp.distance2D(myPosition) > mismatchThreshold) {
1002 WRITE_WARNING("Shape for junction '" + myID + "' has distance " + toString(tmp.distance2D(myPosition)) + " to its given position");
1003 }
1004 }
1005 } catch (InvalidArgument&) {
1006 WRITE_WARNING("For junction '" + getID() + "': could not compute shape.");
1007 // make sure our shape is not empty because our XML schema forbids empty attributes
1008 myPoly.clear();
1009 myPoly.push_back(myPosition);
1010 }
1011 }
1012
1013
1014 void
computeLanes2Lanes()1015 NBNode::computeLanes2Lanes() {
1016 // special case a):
1017 // one in, one out, the outgoing has one lane more
1018 if (myIncomingEdges.size() == 1 && myOutgoingEdges.size() == 1) {
1019 NBEdge* in = myIncomingEdges[0];
1020 NBEdge* out = myOutgoingEdges[0];
1021 // check if it's not the turnaround
1022 if (in->getTurnDestination() == out) {
1023 // will be added later or not...
1024 return;
1025 }
1026 #ifdef DEBUG_CONNECTION_GUESSING
1027 if (DEBUGCOND) {
1028 std::cout << "l2l node=" << getID() << " specialCase a\n";
1029 }
1030 #endif
1031 const int inOffset = MAX2(0, in->getFirstNonPedestrianLaneIndex(FORWARD, true));
1032 const int outOffset = MAX2(0, out->getFirstNonPedestrianLaneIndex(FORWARD, true));
1033 if (in->getStep() <= NBEdge::LANES2EDGES
1034 && in->getNumLanes() - inOffset == out->getNumLanes() - outOffset - 1
1035 && in != out
1036 && in->isConnectedTo(out)) {
1037 for (int i = inOffset; i < in->getNumLanes(); ++i) {
1038 in->setConnection(i, out, i - inOffset + outOffset + 1, NBEdge::L2L_COMPUTED);
1039 }
1040 in->setConnection(inOffset, out, outOffset, NBEdge::L2L_COMPUTED);
1041 return;
1042 }
1043 }
1044 // special case b):
1045 // two in, one out, the outgoing has the same number of lanes as the sum of the incoming
1046 // --> highway on-ramp
1047 if (myIncomingEdges.size() == 2 && myOutgoingEdges.size() == 1) {
1048 NBEdge* out = myOutgoingEdges[0];
1049 NBEdge* in1 = myIncomingEdges[0];
1050 NBEdge* in2 = myIncomingEdges[1];
1051 const int outOffset = MAX2(0, out->getFirstNonPedestrianLaneIndex(FORWARD, true));
1052 int in1Offset = MAX2(0, in1->getFirstNonPedestrianLaneIndex(FORWARD, true));
1053 int in2Offset = MAX2(0, in2->getFirstNonPedestrianLaneIndex(FORWARD, true));
1054 if (in1->getNumLanes() + in2->getNumLanes() - in1Offset - in2Offset == out->getNumLanes() - outOffset
1055 && (in1->getStep() <= NBEdge::LANES2EDGES)
1056 && (in2->getStep() <= NBEdge::LANES2EDGES)
1057 && in1 != out
1058 && in2 != out
1059 && in1->isConnectedTo(out)
1060 && in2->isConnectedTo(out)
1061 && in1->getSpecialLane(SVC_BICYCLE) == -1
1062 && in2->getSpecialLane(SVC_BICYCLE) == -1
1063 && out->getSpecialLane(SVC_BICYCLE) == -1
1064 && isLongEnough(out, MIN_WEAVE_LENGTH)) {
1065 #ifdef DEBUG_CONNECTION_GUESSING
1066 if (DEBUGCOND) {
1067 std::cout << "l2l node=" << getID() << " specialCase b\n";
1068 }
1069 #endif
1070 // for internal: check which one is the rightmost
1071 double a1 = in1->getAngleAtNode(this);
1072 double a2 = in2->getAngleAtNode(this);
1073 double ccw = GeomHelper::getCCWAngleDiff(a1, a2);
1074 double cw = GeomHelper::getCWAngleDiff(a1, a2);
1075 if (ccw > cw) {
1076 std::swap(in1, in2);
1077 std::swap(in1Offset, in2Offset);
1078 }
1079 in1->addLane2LaneConnections(in1Offset, out, outOffset, in1->getNumLanes() - in1Offset, NBEdge::L2L_VALIDATED, true);
1080 in2->addLane2LaneConnections(in2Offset, out, in1->getNumLanes() + outOffset - in1Offset, in2->getNumLanes() - in2Offset, NBEdge::L2L_VALIDATED, true);
1081 return;
1082 }
1083 }
1084 // special case c):
1085 // one in, two out, the incoming has the same number of lanes or only 1 lane less than the sum of the outgoing lanes
1086 // --> highway off-ramp
1087 if (myIncomingEdges.size() == 1 && myOutgoingEdges.size() == 2) {
1088 NBEdge* in = myIncomingEdges[0];
1089 NBEdge* out1 = myOutgoingEdges[0];
1090 NBEdge* out2 = myOutgoingEdges[1];
1091 const int inOffset = MAX2(0, in->getFirstNonPedestrianLaneIndex(FORWARD, true));
1092 int out1Offset = MAX2(0, out1->getFirstNonPedestrianLaneIndex(FORWARD, true));
1093 int out2Offset = MAX2(0, out2->getFirstNonPedestrianLaneIndex(FORWARD, true));
1094 const int deltaLaneSum = (out2->getNumLanes() + out1->getNumLanes() - out1Offset - out2Offset) - (in->getNumLanes() - inOffset);
1095 if ((deltaLaneSum == 0 || (deltaLaneSum == 1 && in->getPermissionVariants(inOffset, in->getNumLanes()).size() == 1))
1096 && (in->getStep() <= NBEdge::LANES2EDGES)
1097 && in != out1
1098 && in != out2
1099 && in->isConnectedTo(out1)
1100 && in->isConnectedTo(out2)
1101 && !in->isTurningDirectionAt(out1)
1102 && !in->isTurningDirectionAt(out2)
1103 ) {
1104 #ifdef DEBUG_CONNECTION_GUESSING
1105 if (DEBUGCOND) {
1106 std::cout << "l2l node=" << getID() << " specialCase c\n";
1107 }
1108 #endif
1109 // for internal: check which one is the rightmost
1110 if (NBContHelper::relative_outgoing_edge_sorter(in)(out2, out1)) {
1111 std::swap(out1, out2);
1112 std::swap(out1Offset, out2Offset);
1113 }
1114 in->addLane2LaneConnections(inOffset, out1, out1Offset, out1->getNumLanes() - out1Offset, NBEdge::L2L_VALIDATED, true);
1115 in->addLane2LaneConnections(out1->getNumLanes() + inOffset - out1Offset - deltaLaneSum, out2, out2Offset, out2->getNumLanes() - out2Offset, NBEdge::L2L_VALIDATED, false);
1116 return;
1117 }
1118 }
1119 // special case d):
1120 // one in, one out, the outgoing has one lane less and node has type 'zipper'
1121 if (myIncomingEdges.size() == 1 && myOutgoingEdges.size() == 1 && myType == NODETYPE_ZIPPER) {
1122 NBEdge* in = myIncomingEdges[0];
1123 NBEdge* out = myOutgoingEdges[0];
1124 // check if it's not the turnaround
1125 if (in->getTurnDestination() == out) {
1126 // will be added later or not...
1127 return;
1128 }
1129 #ifdef DEBUG_CONNECTION_GUESSING
1130 if (DEBUGCOND) {
1131 std::cout << "l2l node=" << getID() << " specialCase d\n";
1132 }
1133 #endif
1134 const int inOffset = MAX2(0, in->getFirstNonPedestrianLaneIndex(FORWARD, true));
1135 const int outOffset = MAX2(0, out->getFirstNonPedestrianLaneIndex(FORWARD, true));
1136 if (in->getStep() <= NBEdge::LANES2EDGES
1137 && in->getNumLanes() - inOffset == out->getNumLanes() - outOffset + 1
1138 && in != out
1139 && in->isConnectedTo(out)) {
1140 for (int i = inOffset; i < in->getNumLanes(); ++i) {
1141 in->setConnection(i, out, MIN2(outOffset + i, out->getNumLanes() - 1), NBEdge::L2L_COMPUTED, true);
1142 }
1143 return;
1144 }
1145 }
1146 // special case f):
1147 // one in, one out, out has reduced or same number of lanes
1148 if (myIncomingEdges.size() == 1 && myOutgoingEdges.size() == 1) {
1149 NBEdge* in = myIncomingEdges[0];
1150 NBEdge* out = myOutgoingEdges[0];
1151 // check if it's not the turnaround
1152 if (in->getTurnDestination() == out) {
1153 // will be added later or not...
1154 return;
1155 }
1156 #ifdef DEBUG_CONNECTION_GUESSING
1157 if (DEBUGCOND) {
1158 std::cout << "l2l node=" << getID() << " specialCase f\n";
1159 }
1160 #endif
1161 int inOffset = MAX2(0, in->getFirstNonPedestrianLaneIndex(FORWARD, true));
1162 const int outOffset = MAX2(0, out->getFirstNonPedestrianLaneIndex(FORWARD, true));
1163 const int reduction = (in->getNumLanes() - inOffset) - (out->getNumLanes() - outOffset);
1164 if (in->getStep() <= NBEdge::LANES2EDGES
1165 && reduction >= 0
1166 && in != out
1167 && in->isConnectedTo(out)) {
1168 // in case of reduced lane number, let the rightmost lanse end
1169 inOffset += reduction;
1170 for (int i = outOffset; i < out->getNumLanes(); ++i) {
1171 in->setConnection(i + inOffset - outOffset, out, i, NBEdge::L2L_COMPUTED);
1172 }
1173 //std::cout << " special case f at node=" << getID() << " inOffset=" << inOffset << " outOffset=" << outOffset << "\n";
1174 return;
1175 }
1176 }
1177
1178 // go through this node's outgoing edges
1179 // for every outgoing edge, compute the distribution of the node's
1180 // incoming edges on this edge when approaching this edge
1181 // the incoming edges' steps will then also be marked as LANE2LANE_RECHECK...
1182 EdgeVector approaching;
1183 for (NBEdge* currentOutgoing : myOutgoingEdges) {
1184 // get the information about edges that do approach this edge
1185 getEdgesThatApproach(currentOutgoing, approaching);
1186 const int numApproaching = (int)approaching.size();
1187 if (numApproaching != 0) {
1188 ApproachingDivider divider(approaching, currentOutgoing);
1189 Bresenham::compute(÷r, numApproaching, divider.numAvailableLanes());
1190 }
1191 #ifdef DEBUG_CONNECTION_GUESSING
1192 if (DEBUGCOND) {
1193 std::cout << "l2l node=" << getID() << " bresenham:\n";
1194 for (NBEdge* e : myIncomingEdges) {
1195 const std::vector<NBEdge::Connection>& elv = e->getConnections();
1196 for (std::vector<NBEdge::Connection>::const_iterator k = elv.begin(); k != elv.end(); ++k) {
1197 std::cout << " " << e->getID() << "_" << (*k).fromLane << " -> " << (*k).toEdge->getID() << "_" << (*k).toLane << "\n";
1198 }
1199 }
1200 }
1201 #endif
1202 int bikeLaneTarget = currentOutgoing->getSpecialLane(SVC_BICYCLE);
1203
1204 // ensure that all modes have a connection if possible
1205 for (NBEdge* incoming : myIncomingEdges) {
1206 if (incoming->getConnectionLanes(currentOutgoing).size() > 0 && incoming->getStep() <= NBEdge::LANES2LANES_DONE) {
1207 // no connections are needed for pedestrians during this step
1208 // no satisfaction is possible if the outgoing edge disallows
1209 SVCPermissions unsatisfied = incoming->getPermissions() & currentOutgoing->getPermissions() & ~SVC_PEDESTRIAN;
1210 //std::cout << "initial unsatisfied modes from edge=" << incoming->getID() << " toEdge=" << currentOutgoing->getID() << " deadModes=" << getVehicleClassNames(unsatisfied) << "\n";
1211 const std::vector<NBEdge::Connection>& elv = incoming->getConnections();
1212 for (std::vector<NBEdge::Connection>::const_iterator k = elv.begin(); k != elv.end(); ++k) {
1213 const NBEdge::Connection& c = *k;
1214 if (c.toEdge == currentOutgoing && c.toLane >= 0) {
1215 const SVCPermissions satisfied = (incoming->getPermissions(c.fromLane) & c.toEdge->getPermissions(c.toLane));
1216 //std::cout << " from=" << c.fromLane << " to=" << c.toEdge->getID() << "_" << c.toLane << " satisfied=" << getVehicleClassNames(satisfied) << "\n";
1217 unsatisfied &= ~satisfied;
1218 }
1219 }
1220 if (unsatisfied != 0) {
1221 #ifdef DEBUG_CONNECTION_GUESSING
1222 if (DEBUGCOND) {
1223 std::cout << " unsatisfied modes from edge=" << incoming->getID() << " toEdge=" << currentOutgoing->getID() << " deadModes=" << getVehicleClassNames(unsatisfied) << "\n";
1224 }
1225 #endif
1226 int fromLane = 0;
1227 while (unsatisfied != 0 && fromLane < incoming->getNumLanes()) {
1228 if ((incoming->getPermissions(fromLane) & unsatisfied) != 0) {
1229 for (int toLane = 0; toLane < currentOutgoing->getNumLanes(); ++toLane) {
1230 const SVCPermissions satisfied = incoming->getPermissions(fromLane) & currentOutgoing->getPermissions(toLane) & unsatisfied;
1231 if (satisfied != 0 && !incoming->getLaneStruct(fromLane).connectionsDone) {
1232 incoming->setConnection((int)fromLane, currentOutgoing, toLane, NBEdge::L2L_COMPUTED);
1233 #ifdef DEBUG_CONNECTION_GUESSING
1234 if (DEBUGCOND) {
1235 std::cout << " new connection from=" << fromLane << " to=" << currentOutgoing->getID() << "_" << toLane << " satisfies=" << getVehicleClassNames(satisfied) << "\n";
1236 }
1237 #endif
1238 unsatisfied &= ~satisfied;
1239 }
1240 }
1241 }
1242 fromLane++;
1243 }
1244 #ifdef DEBUG_CONNECTION_GUESSING
1245 if (DEBUGCOND) {
1246 if (unsatisfied != 0) {
1247 std::cout << " still unsatisfied modes from edge=" << incoming->getID() << " toEdge=" << currentOutgoing->getID() << " deadModes=" << getVehicleClassNames(unsatisfied) << "\n";
1248 }
1249 }
1250 #endif
1251 }
1252 }
1253 // prevent dead-end bicycle lanes (they were excluded by the ApproachingDivider)
1254 // and the bicycle mode might already be satisfied by other lanes
1255 // assume that left-turns and turn-arounds are better satisfied from lanes to the left
1256 LinkDirection dir = getDirection(incoming, currentOutgoing);
1257 if (incoming->getStep() <= NBEdge::LANES2LANES_DONE
1258 && ((bikeLaneTarget >= 0 && dir != LINKDIR_TURN)
1259 || dir == LINKDIR_RIGHT || dir == LINKDIR_PARTRIGHT || dir == LINKDIR_STRAIGHT)) {
1260 bool builtConnection = false;
1261 for (int i = 0; i < (int)incoming->getNumLanes(); i++) {
1262 if (incoming->getPermissions(i) == SVC_BICYCLE
1263 && incoming->getConnectionsFromLane(i, currentOutgoing).size() == 0) {
1264 // find a dedicated bike lane as target
1265 if (bikeLaneTarget >= 0) {
1266 incoming->setConnection(i, currentOutgoing, bikeLaneTarget, NBEdge::L2L_COMPUTED);
1267 builtConnection = true;
1268 } else {
1269 // use any lane that allows bicycles
1270 for (int i2 = 0; i2 < (int)currentOutgoing->getNumLanes(); i2++) {
1271 if ((currentOutgoing->getPermissions(i2) & SVC_BICYCLE) != 0) {
1272 // possibly a double-connection
1273 // XXX could use 'true' here but this requires additional work on tls generation
1274 incoming->setConnection(i, currentOutgoing, i2, NBEdge::L2L_COMPUTED, false);
1275 builtConnection = true;
1276 break;
1277 }
1278 }
1279 }
1280 }
1281 }
1282 if (!builtConnection && bikeLaneTarget >= 0
1283 && incoming->getConnectionsFromLane(-1, currentOutgoing, bikeLaneTarget).size() == 0) {
1284 // find origin lane that allows bicycles
1285 int start = 0;
1286 int end = (int)incoming->getNumLanes();
1287 int inc = 1;
1288 if (dir == LINKDIR_TURN || dir == LINKDIR_LEFT || dir == LINKDIR_PARTLEFT) {
1289 std::swap(start, end);
1290 inc = -1;
1291 }
1292 for (int i = start; i < end; i += inc) {
1293 if ((incoming->getPermissions(i) & SVC_BICYCLE) != 0) {
1294 incoming->setConnection(i, currentOutgoing, bikeLaneTarget, NBEdge::L2L_COMPUTED);
1295 break;
1296 }
1297 }
1298 }
1299 }
1300 }
1301 }
1302 // special case e): rail_crossing
1303 // there should only be straight connections here
1304 if (myType == NODETYPE_RAIL_CROSSING) {
1305 for (EdgeVector::const_iterator i = myIncomingEdges.begin(); i != myIncomingEdges.end(); i++) {
1306 const std::vector<NBEdge::Connection> cons = (*i)->getConnections();
1307 for (std::vector<NBEdge::Connection>::const_iterator k = cons.begin(); k != cons.end(); ++k) {
1308 if (getDirection(*i, (*k).toEdge) == LINKDIR_TURN) {
1309 (*i)->removeFromConnections((*k).toEdge);
1310 }
1311 }
1312 }
1313 }
1314
1315 // ... but we may have the case that there are no outgoing edges
1316 // In this case, we have to mark the incoming edges as being in state
1317 // LANE2LANE( not RECHECK) by hand
1318 if (myOutgoingEdges.size() == 0) {
1319 for (NBEdge* incoming : myIncomingEdges) {
1320 incoming->markAsInLane2LaneState();
1321 }
1322 }
1323
1324 #ifdef DEBUG_CONNECTION_GUESSING
1325 if (DEBUGCOND) {
1326 std::cout << "final connections at " << getID() << "\n";
1327 for (NBEdge* e : myIncomingEdges) {
1328 const std::vector<NBEdge::Connection>& elv = e->getConnections();
1329 for (std::vector<NBEdge::Connection>::const_iterator k = elv.begin(); k != elv.end(); ++k) {
1330 std::cout << " " << e->getID() << "_" << (*k).fromLane << " -> " << (*k).toEdge->getID() << "_" << (*k).toLane << "\n";
1331 }
1332 }
1333 }
1334 #endif
1335 }
1336
1337 bool
isLongEnough(NBEdge * out,double minLength)1338 NBNode::isLongEnough(NBEdge* out, double minLength) {
1339 double seen = out->getLoadedLength();
1340 while (seen < minLength) {
1341 // advance along trivial continuations
1342 if (out->getToNode()->getOutgoingEdges().size() != 1
1343 || out->getToNode()->getIncomingEdges().size() != 1) {
1344 return false;
1345 } else {
1346 out = out->getToNode()->getOutgoingEdges()[0];
1347 seen += out->getLoadedLength();
1348 }
1349 }
1350 return true;
1351 }
1352
1353
1354 void
getEdgesThatApproach(NBEdge * currentOutgoing,EdgeVector & approaching)1355 NBNode::getEdgesThatApproach(NBEdge* currentOutgoing, EdgeVector& approaching) {
1356 // get the position of the node to get the approaching nodes of
1357 EdgeVector::const_iterator i = std::find(myAllEdges.begin(),
1358 myAllEdges.end(), currentOutgoing);
1359 // get the first possible approaching edge
1360 NBContHelper::nextCW(myAllEdges, i);
1361 // go through the list of edges clockwise and add the edges
1362 approaching.clear();
1363 for (; *i != currentOutgoing;) {
1364 // check only incoming edges
1365 if ((*i)->getToNode() == this && (*i)->getTurnDestination() != currentOutgoing) {
1366 std::vector<int> connLanes = (*i)->getConnectionLanes(currentOutgoing);
1367 if (connLanes.size() != 0) {
1368 approaching.push_back(*i);
1369 }
1370 }
1371 NBContHelper::nextCW(myAllEdges, i);
1372 }
1373 }
1374
1375
1376 void
replaceOutgoing(NBEdge * which,NBEdge * by,int laneOff)1377 NBNode::replaceOutgoing(NBEdge* which, NBEdge* by, int laneOff) {
1378 // replace the edge in the list of outgoing nodes
1379 EdgeVector::iterator i = std::find(myOutgoingEdges.begin(), myOutgoingEdges.end(), which);
1380 if (i != myOutgoingEdges.end()) {
1381 (*i) = by;
1382 i = std::find(myAllEdges.begin(), myAllEdges.end(), which);
1383 (*i) = by;
1384 }
1385 // replace the edge in connections of incoming edges
1386 for (i = myIncomingEdges.begin(); i != myIncomingEdges.end(); ++i) {
1387 (*i)->replaceInConnections(which, by, laneOff);
1388 }
1389 // replace within the connetion prohibition dependencies
1390 replaceInConnectionProhibitions(which, by, 0, laneOff);
1391 }
1392
1393
1394 void
replaceOutgoing(const EdgeVector & which,NBEdge * by)1395 NBNode::replaceOutgoing(const EdgeVector& which, NBEdge* by) {
1396 // replace edges
1397 int laneOff = 0;
1398 for (EdgeVector::const_iterator i = which.begin(); i != which.end(); i++) {
1399 replaceOutgoing(*i, by, laneOff);
1400 laneOff += (*i)->getNumLanes();
1401 }
1402 // removed double occurences
1403 removeDoubleEdges();
1404 // check whether this node belongs to a district and the edges
1405 // must here be also remapped
1406 if (myDistrict != nullptr) {
1407 myDistrict->replaceOutgoing(which, by);
1408 }
1409 }
1410
1411
1412 void
replaceIncoming(NBEdge * which,NBEdge * by,int laneOff)1413 NBNode::replaceIncoming(NBEdge* which, NBEdge* by, int laneOff) {
1414 // replace the edge in the list of incoming nodes
1415 EdgeVector::iterator i = std::find(myIncomingEdges.begin(), myIncomingEdges.end(), which);
1416 if (i != myIncomingEdges.end()) {
1417 (*i) = by;
1418 i = std::find(myAllEdges.begin(), myAllEdges.end(), which);
1419 (*i) = by;
1420 }
1421 // replace within the connetion prohibition dependencies
1422 replaceInConnectionProhibitions(which, by, laneOff, 0);
1423 }
1424
1425
1426 void
replaceIncoming(const EdgeVector & which,NBEdge * by)1427 NBNode::replaceIncoming(const EdgeVector& which, NBEdge* by) {
1428 // replace edges
1429 int laneOff = 0;
1430 for (EdgeVector::const_iterator i = which.begin(); i != which.end(); i++) {
1431 replaceIncoming(*i, by, laneOff);
1432 laneOff += (*i)->getNumLanes();
1433 }
1434 // removed double occurences
1435 removeDoubleEdges();
1436 // check whether this node belongs to a district and the edges
1437 // must here be also remapped
1438 if (myDistrict != nullptr) {
1439 myDistrict->replaceIncoming(which, by);
1440 }
1441 }
1442
1443
1444
1445 void
replaceInConnectionProhibitions(NBEdge * which,NBEdge * by,int whichLaneOff,int byLaneOff)1446 NBNode::replaceInConnectionProhibitions(NBEdge* which, NBEdge* by,
1447 int whichLaneOff, int byLaneOff) {
1448 // replace in keys
1449 NBConnectionProhibits::iterator j = myBlockedConnections.begin();
1450 while (j != myBlockedConnections.end()) {
1451 bool changed = false;
1452 NBConnection c = (*j).first;
1453 if (c.replaceFrom(which, whichLaneOff, by, byLaneOff)) {
1454 changed = true;
1455 }
1456 if (c.replaceTo(which, whichLaneOff, by, byLaneOff)) {
1457 changed = true;
1458 }
1459 if (changed) {
1460 myBlockedConnections[c] = (*j).second;
1461 myBlockedConnections.erase(j);
1462 j = myBlockedConnections.begin();
1463 } else {
1464 j++;
1465 }
1466 }
1467 // replace in values
1468 for (j = myBlockedConnections.begin(); j != myBlockedConnections.end(); j++) {
1469 NBConnectionVector& prohibiting = (*j).second;
1470 for (NBConnectionVector::iterator k = prohibiting.begin(); k != prohibiting.end(); k++) {
1471 NBConnection& sprohibiting = *k;
1472 sprohibiting.replaceFrom(which, whichLaneOff, by, byLaneOff);
1473 sprohibiting.replaceTo(which, whichLaneOff, by, byLaneOff);
1474 }
1475 }
1476 }
1477
1478
1479
1480 void
removeDoubleEdges()1481 NBNode::removeDoubleEdges() {
1482 // check incoming
1483 for (int i = 0; myIncomingEdges.size() > 0 && i < (int)myIncomingEdges.size() - 1; i++) {
1484 int j = i + 1;
1485 while (j < (int)myIncomingEdges.size()) {
1486 if (myIncomingEdges[i] == myIncomingEdges[j]) {
1487 myIncomingEdges.erase(myIncomingEdges.begin() + j);
1488 } else {
1489 j++;
1490 }
1491 }
1492 }
1493 // check outgoing
1494 for (int i = 0; myOutgoingEdges.size() > 0 && i < (int)myOutgoingEdges.size() - 1; i++) {
1495 int j = i + 1;
1496 while (j < (int)myOutgoingEdges.size()) {
1497 if (myOutgoingEdges[i] == myOutgoingEdges[j]) {
1498 myOutgoingEdges.erase(myOutgoingEdges.begin() + j);
1499 } else {
1500 j++;
1501 }
1502 }
1503 }
1504 // check all
1505 for (int i = 0; myAllEdges.size() > 0 && i < (int)myAllEdges.size() - 1; i++) {
1506 int j = i + 1;
1507 while (j < (int)myAllEdges.size()) {
1508 if (myAllEdges[i] == myAllEdges[j]) {
1509 myAllEdges.erase(myAllEdges.begin() + j);
1510 } else {
1511 j++;
1512 }
1513 }
1514 }
1515 }
1516
1517
1518 bool
hasIncoming(const NBEdge * const e) const1519 NBNode::hasIncoming(const NBEdge* const e) const {
1520 return std::find(myIncomingEdges.begin(), myIncomingEdges.end(), e) != myIncomingEdges.end();
1521 }
1522
1523
1524 bool
hasOutgoing(const NBEdge * const e) const1525 NBNode::hasOutgoing(const NBEdge* const e) const {
1526 return std::find(myOutgoingEdges.begin(), myOutgoingEdges.end(), e) != myOutgoingEdges.end();
1527 }
1528
1529
1530 NBEdge*
getOppositeIncoming(NBEdge * e) const1531 NBNode::getOppositeIncoming(NBEdge* e) const {
1532 EdgeVector edges = myIncomingEdges;
1533 if (find(edges.begin(), edges.end(), e) != edges.end()) {
1534 edges.erase(find(edges.begin(), edges.end(), e));
1535 }
1536 if (edges.size() == 0) {
1537 return nullptr;
1538 }
1539 if (e->getToNode() == this) {
1540 sort(edges.begin(), edges.end(), NBContHelper::edge_opposite_direction_sorter(e, this, false));
1541 } else {
1542 sort(edges.begin(), edges.end(), NBContHelper::edge_similar_direction_sorter(e));
1543 }
1544 return edges[0];
1545 }
1546
1547
1548 void
addSortedLinkFoes(const NBConnection & mayDrive,const NBConnection & mustStop)1549 NBNode::addSortedLinkFoes(const NBConnection& mayDrive,
1550 const NBConnection& mustStop) {
1551 if (mayDrive.getFrom() == nullptr ||
1552 mayDrive.getTo() == nullptr ||
1553 mustStop.getFrom() == nullptr ||
1554 mustStop.getTo() == nullptr) {
1555
1556 WRITE_WARNING("Something went wrong during the building of a connection...");
1557 return; // !!! mark to recompute connections
1558 }
1559 NBConnectionVector conn = myBlockedConnections[mustStop];
1560 conn.push_back(mayDrive);
1561 myBlockedConnections[mustStop] = conn;
1562 }
1563
1564
1565 NBEdge*
getPossiblySplittedIncoming(const std::string & edgeid)1566 NBNode::getPossiblySplittedIncoming(const std::string& edgeid) {
1567 int size = (int) edgeid.length();
1568 for (EdgeVector::iterator i = myIncomingEdges.begin(); i != myIncomingEdges.end(); i++) {
1569 std::string id = (*i)->getID();
1570 if (id.substr(0, size) == edgeid) {
1571 return *i;
1572 }
1573 }
1574 return nullptr;
1575 }
1576
1577
1578 NBEdge*
getPossiblySplittedOutgoing(const std::string & edgeid)1579 NBNode::getPossiblySplittedOutgoing(const std::string& edgeid) {
1580 int size = (int) edgeid.length();
1581 for (EdgeVector::iterator i = myOutgoingEdges.begin(); i != myOutgoingEdges.end(); i++) {
1582 std::string id = (*i)->getID();
1583 if (id.substr(0, size) == edgeid) {
1584 return *i;
1585 }
1586 }
1587 return nullptr;
1588 }
1589
1590
1591 void
removeEdge(NBEdge * edge,bool removeFromConnections)1592 NBNode::removeEdge(NBEdge* edge, bool removeFromConnections) {
1593 EdgeVector::iterator i = std::find(myAllEdges.begin(), myAllEdges.end(), edge);
1594 if (i != myAllEdges.end()) {
1595 myAllEdges.erase(i);
1596 i = std::find(myOutgoingEdges.begin(), myOutgoingEdges.end(), edge);
1597 if (i != myOutgoingEdges.end()) {
1598 myOutgoingEdges.erase(i);
1599 } else {
1600 i = std::find(myIncomingEdges.begin(), myIncomingEdges.end(), edge);
1601 if (i != myIncomingEdges.end()) {
1602 myIncomingEdges.erase(i);
1603 } else {
1604 // edge must have been either incoming or outgoing
1605 assert(false);
1606 }
1607 }
1608 if (removeFromConnections) {
1609 for (i = myAllEdges.begin(); i != myAllEdges.end(); ++i) {
1610 (*i)->removeFromConnections(edge);
1611 }
1612 }
1613 // invalidate controlled connections for loaded traffic light plans
1614 for (std::set<NBTrafficLightDefinition*>::iterator i = myTrafficLights.begin(); i != myTrafficLights.end(); ++i) {
1615 (*i)->replaceRemoved(edge, -1, nullptr, -1);
1616 }
1617 }
1618 }
1619
1620
1621 Position
getEmptyDir() const1622 NBNode::getEmptyDir() const {
1623 Position pos(0, 0);
1624 EdgeVector::const_iterator i;
1625 for (i = myIncomingEdges.begin(); i != myIncomingEdges.end(); i++) {
1626 NBNode* conn = (*i)->getFromNode();
1627 Position toAdd = conn->getPosition();
1628 toAdd.sub(myPosition);
1629 toAdd.mul((double) 1.0 / sqrt(toAdd.x()*toAdd.x() + toAdd.y()*toAdd.y()));
1630 pos.add(toAdd);
1631 }
1632 for (i = myOutgoingEdges.begin(); i != myOutgoingEdges.end(); i++) {
1633 NBNode* conn = (*i)->getToNode();
1634 Position toAdd = conn->getPosition();
1635 toAdd.sub(myPosition);
1636 toAdd.mul((double) 1.0 / sqrt(toAdd.x()*toAdd.x() + toAdd.y()*toAdd.y()));
1637 pos.add(toAdd);
1638 }
1639 pos.mul((double) - 1.0 / (myIncomingEdges.size() + myOutgoingEdges.size()));
1640 if (pos.x() == 0 && pos.y() == 0) {
1641 pos = Position(1, 0);
1642 }
1643 pos.norm2d();
1644 return pos;
1645 }
1646
1647
1648
1649 void
invalidateIncomingConnections()1650 NBNode::invalidateIncomingConnections() {
1651 for (EdgeVector::const_iterator i = myIncomingEdges.begin(); i != myIncomingEdges.end(); i++) {
1652 (*i)->invalidateConnections();
1653 }
1654 }
1655
1656
1657 void
invalidateOutgoingConnections()1658 NBNode::invalidateOutgoingConnections() {
1659 for (EdgeVector::const_iterator i = myOutgoingEdges.begin(); i != myOutgoingEdges.end(); i++) {
1660 (*i)->invalidateConnections();
1661 }
1662 }
1663
1664
1665 bool
mustBrake(const NBEdge * const from,const NBEdge * const to,int fromLane,int toLane,bool includePedCrossings) const1666 NBNode::mustBrake(const NBEdge* const from, const NBEdge* const to, int fromLane, int toLane, bool includePedCrossings) const {
1667 // unregulated->does not need to brake
1668 if (myRequest == nullptr) {
1669 return false;
1670 }
1671 // vehicles which do not have a following lane must always decelerate to the end
1672 if (to == nullptr) {
1673 return true;
1674 }
1675 // check whether any other connection on this node prohibits this connection
1676 return myRequest->mustBrake(from, to, fromLane, toLane, includePedCrossings);
1677 }
1678
1679 bool
mustBrakeForCrossing(const NBEdge * const from,const NBEdge * const to,const NBNode::Crossing & crossing) const1680 NBNode::mustBrakeForCrossing(const NBEdge* const from, const NBEdge* const to, const NBNode::Crossing& crossing) const {
1681 return NBRequest::mustBrakeForCrossing(this, from, to, crossing);
1682 }
1683
1684
1685 bool
rightTurnConflict(const NBEdge * from,const NBEdge * to,int fromLane,const NBEdge * prohibitorFrom,const NBEdge * prohibitorTo,int prohibitorFromLane,bool lefthand)1686 NBNode::rightTurnConflict(const NBEdge* from, const NBEdge* to, int fromLane,
1687 const NBEdge* prohibitorFrom, const NBEdge* prohibitorTo, int prohibitorFromLane,
1688 bool lefthand) {
1689 if (from != prohibitorFrom) {
1690 return false;
1691 }
1692 if (from->isTurningDirectionAt(to)
1693 || prohibitorFrom->isTurningDirectionAt(prohibitorTo)) {
1694 // XXX should warn if there are any non-turning connections left of this
1695 return false;
1696 }
1697 // conflict if to is between prohibitorTo and from when going clockwise
1698 if (to->getStartAngle() == prohibitorTo->getStartAngle()) {
1699 // reduce rounding errors
1700 return false;
1701 }
1702 const LinkDirection d1 = from->getToNode()->getDirection(from, to);
1703 // must be a right turn to qualify as rightTurnConflict
1704 if (d1 == LINKDIR_STRAIGHT) {
1705 // no conflict for straight going connections
1706 // XXX actually this should check the main direction (which could also
1707 // be a turn)
1708 return false;
1709 } else {
1710 const LinkDirection d2 = prohibitorFrom->getToNode()->getDirection(prohibitorFrom, prohibitorTo);
1711 if (d1 == LINKDIR_LEFT || d1 == LINKDIR_PARTLEFT) {
1712 // check for leftTurnConflicht
1713 lefthand = !lefthand;
1714 if (d2 == LINKDIR_RIGHT || d1 == LINKDIR_PARTRIGHT) {
1715 // assume that the left-turning bicycle goes straight at first
1716 // and thus gets precedence over a right turning vehicle
1717 return false;
1718 }
1719 }
1720 if ((!lefthand && fromLane <= prohibitorFromLane) ||
1721 (lefthand && fromLane >= prohibitorFromLane)) {
1722 return false;
1723 }
1724 const double toAngleAtNode = fmod(to->getStartAngle() + 180, (double)360.0);
1725 const double prohibitorToAngleAtNode = fmod(prohibitorTo->getStartAngle() + 180, (double)360.0);
1726 return (lefthand != (GeomHelper::getCWAngleDiff(from->getEndAngle(), toAngleAtNode) <
1727 GeomHelper::getCWAngleDiff(from->getEndAngle(), prohibitorToAngleAtNode)));
1728 }
1729 }
1730
1731
1732 bool
turnFoes(const NBEdge * from,const NBEdge * to,int fromLane,const NBEdge * from2,const NBEdge * to2,int fromLane2,bool lefthand) const1733 NBNode::turnFoes(const NBEdge* from, const NBEdge* to, int fromLane,
1734 const NBEdge* from2, const NBEdge* to2, int fromLane2,
1735 bool lefthand) const {
1736 UNUSED_PARAMETER(lefthand);
1737 if (from != from2 || to == to2 || fromLane == fromLane2) {
1738 return false;
1739 }
1740 if (from->isTurningDirectionAt(to)
1741 || from2->isTurningDirectionAt(to2)) {
1742 // XXX should warn if there are any non-turning connections left of this
1743 return false;
1744 }
1745 bool result = false;
1746 EdgeVector::const_iterator it = std::find(myAllEdges.begin(), myAllEdges.end(), from);
1747 if (fromLane < fromLane2) {
1748 // conflict if 'to' comes before 'to2' going clockwise starting at 'from'
1749 while (*it != to2) {
1750 if (*it == to) {
1751 result = true;
1752 }
1753 NBContHelper::nextCW(myAllEdges, it);
1754 }
1755 } else {
1756 // conflict if 'to' comes before 'to2' going counter-clockwise starting at 'from'
1757 while (*it != to2) {
1758 if (*it == to) {
1759 result = true;
1760 }
1761 NBContHelper::nextCCW(myAllEdges, it);
1762 }
1763 }
1764 /*
1765 if (result) {
1766 std::cout << "turnFoes node=" << getID()
1767 << " from=" << from->getLaneID(fromLane)
1768 << " to=" << to->getID()
1769 << " from2=" << from2->getLaneID(fromLane2)
1770 << " to2=" << to2->getID()
1771 << "\n";
1772 }
1773 */
1774 return result;
1775 }
1776
1777
1778 bool
isLeftMover(const NBEdge * const from,const NBEdge * const to) const1779 NBNode::isLeftMover(const NBEdge* const from, const NBEdge* const to) const {
1780 // when the junction has only one incoming edge, there are no
1781 // problems caused by left blockings
1782 if (myIncomingEdges.size() == 1 || myOutgoingEdges.size() == 1) {
1783 return false;
1784 }
1785 double fromAngle = from->getAngleAtNode(this);
1786 double toAngle = to->getAngleAtNode(this);
1787 double cw = GeomHelper::getCWAngleDiff(fromAngle, toAngle);
1788 double ccw = GeomHelper::getCCWAngleDiff(fromAngle, toAngle);
1789 std::vector<NBEdge*>::const_iterator i = std::find(myAllEdges.begin(), myAllEdges.end(), from);
1790 do {
1791 NBContHelper::nextCW(myAllEdges, i);
1792 } while ((!hasOutgoing(*i) || from->isTurningDirectionAt(*i)) && *i != from);
1793 return cw < ccw && (*i) == to && myOutgoingEdges.size() > 2;
1794 }
1795
1796
1797 bool
forbids(const NBEdge * const possProhibitorFrom,const NBEdge * const possProhibitorTo,const NBEdge * const possProhibitedFrom,const NBEdge * const possProhibitedTo,bool regardNonSignalisedLowerPriority) const1798 NBNode::forbids(const NBEdge* const possProhibitorFrom, const NBEdge* const possProhibitorTo,
1799 const NBEdge* const possProhibitedFrom, const NBEdge* const possProhibitedTo,
1800 bool regardNonSignalisedLowerPriority) const {
1801 return myRequest != nullptr && myRequest->forbids(possProhibitorFrom, possProhibitorTo,
1802 possProhibitedFrom, possProhibitedTo,
1803 regardNonSignalisedLowerPriority);
1804 }
1805
1806
1807 bool
foes(const NBEdge * const from1,const NBEdge * const to1,const NBEdge * const from2,const NBEdge * const to2) const1808 NBNode::foes(const NBEdge* const from1, const NBEdge* const to1,
1809 const NBEdge* const from2, const NBEdge* const to2) const {
1810 return myRequest != nullptr && myRequest->foes(from1, to1, from2, to2);
1811 }
1812
1813
1814 void
remapRemoved(NBTrafficLightLogicCont & tc,NBEdge * removed,const EdgeVector & incoming,const EdgeVector & outgoing)1815 NBNode::remapRemoved(NBTrafficLightLogicCont& tc,
1816 NBEdge* removed, const EdgeVector& incoming,
1817 const EdgeVector& outgoing) {
1818 assert(find(incoming.begin(), incoming.end(), removed) == incoming.end());
1819 bool changed = true;
1820 while (changed) {
1821 changed = false;
1822 NBConnectionProhibits blockedConnectionsTmp = myBlockedConnections;
1823 NBConnectionProhibits blockedConnectionsNew;
1824 // remap in connections
1825 for (NBConnectionProhibits::iterator i = blockedConnectionsTmp.begin(); i != blockedConnectionsTmp.end(); i++) {
1826 const NBConnection& blocker = (*i).first;
1827 const NBConnectionVector& blocked = (*i).second;
1828 // check the blocked connections first
1829 // check whether any of the blocked must be changed
1830 bool blockedChanged = false;
1831 NBConnectionVector newBlocked;
1832 NBConnectionVector::const_iterator j;
1833 for (j = blocked.begin(); j != blocked.end(); j++) {
1834 const NBConnection& sblocked = *j;
1835 if (sblocked.getFrom() == removed || sblocked.getTo() == removed) {
1836 blockedChanged = true;
1837 }
1838 }
1839 // adapt changes if so
1840 for (j = blocked.begin(); blockedChanged && j != blocked.end(); j++) {
1841 const NBConnection& sblocked = *j;
1842 if (sblocked.getFrom() == removed && sblocked.getTo() == removed) {
1843 /* for(EdgeVector::const_iterator k=incoming.begin(); k!=incoming.end(); k++) {
1844 !!! newBlocked.push_back(NBConnection(*k, *k));
1845 }*/
1846 } else if (sblocked.getFrom() == removed) {
1847 assert(sblocked.getTo() != removed);
1848 for (EdgeVector::const_iterator k = incoming.begin(); k != incoming.end(); k++) {
1849 newBlocked.push_back(NBConnection(*k, sblocked.getTo()));
1850 }
1851 } else if (sblocked.getTo() == removed) {
1852 assert(sblocked.getFrom() != removed);
1853 for (EdgeVector::const_iterator k = outgoing.begin(); k != outgoing.end(); k++) {
1854 newBlocked.push_back(NBConnection(sblocked.getFrom(), *k));
1855 }
1856 } else {
1857 newBlocked.push_back(NBConnection(sblocked.getFrom(), sblocked.getTo()));
1858 }
1859 }
1860 if (blockedChanged) {
1861 blockedConnectionsNew[blocker] = newBlocked;
1862 changed = true;
1863 }
1864 // if the blocked were kept
1865 else {
1866 if (blocker.getFrom() == removed && blocker.getTo() == removed) {
1867 changed = true;
1868 /* for(EdgeVector::const_iterator k=incoming.begin(); k!=incoming.end(); k++) {
1869 !!! blockedConnectionsNew[NBConnection(*k, *k)] = blocked;
1870 }*/
1871 } else if (blocker.getFrom() == removed) {
1872 assert(blocker.getTo() != removed);
1873 changed = true;
1874 for (EdgeVector::const_iterator k = incoming.begin(); k != incoming.end(); k++) {
1875 blockedConnectionsNew[NBConnection(*k, blocker.getTo())] = blocked;
1876 }
1877 } else if (blocker.getTo() == removed) {
1878 assert(blocker.getFrom() != removed);
1879 changed = true;
1880 for (EdgeVector::const_iterator k = outgoing.begin(); k != outgoing.end(); k++) {
1881 blockedConnectionsNew[NBConnection(blocker.getFrom(), *k)] = blocked;
1882 }
1883 } else {
1884 blockedConnectionsNew[blocker] = blocked;
1885 }
1886 }
1887 }
1888 myBlockedConnections = blockedConnectionsNew;
1889 }
1890 // remap in traffic lights
1891 tc.remapRemoved(removed, incoming, outgoing);
1892 }
1893
1894
1895 LinkDirection
getDirection(const NBEdge * const incoming,const NBEdge * const outgoing,bool leftHand) const1896 NBNode::getDirection(const NBEdge* const incoming, const NBEdge* const outgoing, bool leftHand) const {
1897 // ok, no connection at all -> dead end
1898 if (outgoing == nullptr) {
1899 return LINKDIR_NODIR;
1900 }
1901 if (incoming->getJunctionPriority(this) == NBEdge::ROUNDABOUT && outgoing->getJunctionPriority(this) == NBEdge::ROUNDABOUT) {
1902 return LINKDIR_STRAIGHT;
1903 }
1904 // turning direction
1905 if (incoming->isTurningDirectionAt(outgoing)) {
1906 return leftHand ? LINKDIR_TURN_LEFTHAND : LINKDIR_TURN;
1907 }
1908 // get the angle between incoming/outgoing at the junction
1909 const double angle = NBHelpers::normRelAngle(incoming->getAngleAtNode(this), outgoing->getAngleAtNode(this));
1910 // ok, should be a straight connection
1911 if (abs((int) angle) + 1 < 45) {
1912 // check whether there is a straighter edge
1913 EdgeVector::const_iterator i =
1914 std::find(myOutgoingEdges.begin(), myOutgoingEdges.end(), outgoing);
1915 if (leftHand) {
1916 NBContHelper::nextCCW(myOutgoingEdges, i);
1917 } else {
1918 NBContHelper::nextCW(myOutgoingEdges, i);
1919 }
1920 const double angle2 = NBHelpers::normRelAngle(incoming->getAngleAtNode(this), (*i)->getAngleAtNode(this));
1921 if (fabs(angle2) < fabs(angle) && fabs(angle2 - angle) > 5.) {
1922 if (angle2 > angle) {
1923 return LINKDIR_PARTLEFT;
1924 } else {
1925 return LINKDIR_PARTRIGHT;
1926 }
1927 } else {
1928 return LINKDIR_STRAIGHT;
1929 }
1930 }
1931
1932 // check for left and right, first
1933 SVCPermissions vehPerm = incoming->getPermissions() & outgoing->getPermissions();
1934 if (vehPerm != SVC_PEDESTRIAN) {
1935 vehPerm &= ~SVC_PEDESTRIAN;
1936 }
1937 if (angle > 0) {
1938 // check whether any other edge goes further to the right
1939 EdgeVector::const_iterator i =
1940 std::find(myAllEdges.begin(), myAllEdges.end(), outgoing);
1941 if (leftHand) {
1942 NBContHelper::nextCCW(myAllEdges, i);
1943 } else {
1944 NBContHelper::nextCW(myAllEdges, i);
1945 }
1946 while ((*i) != incoming) {
1947 if ((*i)->getFromNode() == this
1948 && !incoming->isTurningDirectionAt(*i)
1949 && (vehPerm & (*i)->getPermissions()) != 0) {
1950 //std::cout << incoming->getID() << " -> " << outgoing->getID() << " partRight because auf " << (*i)->getID() << "\n";
1951 return LINKDIR_PARTRIGHT;
1952 }
1953 if (leftHand) {
1954 NBContHelper::nextCCW(myAllEdges, i);
1955 } else {
1956 NBContHelper::nextCW(myAllEdges, i);
1957 }
1958 }
1959 return LINKDIR_RIGHT;
1960 }
1961 // check whether any other edge goes further to the left
1962 EdgeVector::const_iterator i =
1963 std::find(myAllEdges.begin(), myAllEdges.end(), outgoing);
1964 if (leftHand) {
1965 NBContHelper::nextCW(myAllEdges, i);
1966 } else {
1967 NBContHelper::nextCCW(myAllEdges, i);
1968 }
1969 while ((*i) != incoming) {
1970 if ((*i)->getFromNode() == this
1971 && !incoming->isTurningDirectionAt(*i)
1972 && (vehPerm & (*i)->getPermissions()) != 0) {
1973 //std::cout << incoming->getID() << " -> " << outgoing->getID() << " partLeft because auf " << (*i)->getID() << " turn=" << incoming->getTurnDestination(true) << "\n";
1974 return LINKDIR_PARTLEFT;
1975 }
1976 if (leftHand) {
1977 NBContHelper::nextCW(myAllEdges, i);
1978 } else {
1979 NBContHelper::nextCCW(myAllEdges, i);
1980 }
1981 }
1982 return LINKDIR_LEFT;
1983 }
1984
1985
1986 LinkState
getLinkState(const NBEdge * incoming,NBEdge * outgoing,int fromlane,int toLane,bool mayDefinitelyPass,const std::string & tlID) const1987 NBNode::getLinkState(const NBEdge* incoming, NBEdge* outgoing, int fromlane, int toLane,
1988 bool mayDefinitelyPass, const std::string& tlID) const {
1989 if (myType == NODETYPE_RAIL_CROSSING && isRailway(incoming->getPermissions())) {
1990 return LINKSTATE_MAJOR; // the trains must run on time
1991 }
1992 if (tlID != "") {
1993 return mustBrake(incoming, outgoing, fromlane, toLane, true) ? LINKSTATE_TL_OFF_BLINKING : LINKSTATE_TL_OFF_NOSIGNAL;
1994 }
1995 if (outgoing == nullptr) { // always off
1996 return LINKSTATE_TL_OFF_NOSIGNAL;
1997 }
1998 if (myType == NODETYPE_RIGHT_BEFORE_LEFT) {
1999 return LINKSTATE_EQUAL; // all the same
2000 }
2001 if (myType == NODETYPE_ALLWAY_STOP) {
2002 return LINKSTATE_ALLWAY_STOP; // all drive, first one to arrive may drive first
2003 }
2004 if (myType == NODETYPE_ZIPPER && mustBrake(incoming, outgoing, fromlane, toLane, false)) {
2005 return LINKSTATE_ZIPPER;
2006 }
2007 if (!mayDefinitelyPass
2008 && mustBrake(incoming, outgoing, fromlane, toLane, true)
2009 // legacy mode
2010 && (!incoming->isInternal() || getDirection(incoming, outgoing) != LINKDIR_STRAIGHT)
2011 // avoid linkstate minor at pure railway nodes
2012 && (incoming->getPriority() != outgoing->getPriority() || !NBNodeTypeComputer::isRailwayNode(this))) {
2013 return myType == NODETYPE_PRIORITY_STOP ? LINKSTATE_STOP : LINKSTATE_MINOR; // minor road
2014 }
2015 // traffic lights are not regarded here
2016 return LINKSTATE_MAJOR;
2017 }
2018
2019 bool
checkIsRemovable() const2020 NBNode::checkIsRemovable() const {
2021 std::string reason;
2022 return checkIsRemovableReporting(reason);
2023 }
2024
2025 bool
checkIsRemovableReporting(std::string & reason) const2026 NBNode::checkIsRemovableReporting(std::string& reason) const {
2027 // check whether this node is included in a traffic light or crossing
2028 if (myTrafficLights.size() != 0) {
2029 reason = "TLS";
2030 return false;
2031 }
2032 if (myType == NODETYPE_RAIL_SIGNAL) {
2033 reason = "rail_signal";
2034 return false;
2035 }
2036 if (myCrossings.size() != 0) {
2037 reason = "crossing";
2038 return false;
2039 }
2040 EdgeVector::const_iterator i;
2041 // one in, one out -> just a geometry ...
2042 if (myOutgoingEdges.size() == 1 && myIncomingEdges.size() == 1) {
2043 // ... if types match ...
2044 if (!myIncomingEdges[0]->expandableBy(myOutgoingEdges[0], reason)) {
2045 reason = "edges incompatible: " + reason;
2046 return false;
2047 }
2048 if (myIncomingEdges[0]->getTurnDestination(true) == myOutgoingEdges[0]) {
2049 reason = "turnaround";
2050 return false;
2051 }
2052 return true;
2053 }
2054 // two in, two out -> may be something else
2055 if (myOutgoingEdges.size() == 2 && myIncomingEdges.size() == 2) {
2056 // check whether the origin nodes of the incoming edges differ
2057 std::set<NBNode*> origSet;
2058 for (i = myIncomingEdges.begin(); i != myIncomingEdges.end(); i++) {
2059 origSet.insert((*i)->getFromNode());
2060 }
2061 if (origSet.size() < 2) {
2062 return false;
2063 }
2064 // check whether this node is an intermediate node of
2065 // a two-directional street
2066 for (i = myIncomingEdges.begin(); i != myIncomingEdges.end(); i++) {
2067 // each of the edges must have an opposite direction edge
2068 NBEdge* opposite = (*i)->getTurnDestination(true);
2069 if (opposite != nullptr) {
2070 // the other outgoing edges must be the continuation of the current
2071 NBEdge* continuation = opposite == myOutgoingEdges.front() ? myOutgoingEdges.back() : myOutgoingEdges.front();
2072 // check whether the types allow joining
2073 if (!(*i)->expandableBy(continuation, reason)) {
2074 reason = "edges incompatible: " + reason;
2075 return false;
2076 }
2077 } else {
2078 // ok, at least one outgoing edge is not an opposite
2079 // of an incoming one
2080 reason = "not opposites";
2081 return false;
2082 }
2083 }
2084 return true;
2085 }
2086 // ok, a real node
2087 reason = "intersection";
2088 return false;
2089 }
2090
2091
2092 std::vector<std::pair<NBEdge*, NBEdge*> >
getEdgesToJoin() const2093 NBNode::getEdgesToJoin() const {
2094 assert(checkIsRemovable());
2095 std::vector<std::pair<NBEdge*, NBEdge*> > ret;
2096 // one in, one out-case
2097 if (myOutgoingEdges.size() == 1 && myIncomingEdges.size() == 1) {
2098 ret.push_back(
2099 std::pair<NBEdge*, NBEdge*>(
2100 myIncomingEdges[0], myOutgoingEdges[0]));
2101 return ret;
2102 }
2103 // two in, two out-case
2104 for (EdgeVector::const_iterator i = myIncomingEdges.begin(); i != myIncomingEdges.end(); i++) {
2105 // join with the edge that is not a turning direction
2106 NBEdge* opposite = (*i)->getTurnDestination(true);
2107 assert(opposite != 0);
2108 NBEdge* continuation = opposite == myOutgoingEdges.front() ? myOutgoingEdges.back() : myOutgoingEdges.front();
2109 ret.push_back(std::pair<NBEdge*, NBEdge*>(*i, continuation));
2110 }
2111 return ret;
2112 }
2113
2114
2115 const PositionVector&
getShape() const2116 NBNode::getShape() const {
2117 return myPoly;
2118 }
2119
2120
2121 void
setCustomShape(const PositionVector & shape)2122 NBNode::setCustomShape(const PositionVector& shape) {
2123 myPoly = shape;
2124 myHaveCustomPoly = (myPoly.size() > 1);
2125 if (myHaveCustomPoly) {
2126 for (EdgeVector::iterator i = myAllEdges.begin(); i != myAllEdges.end(); i++) {
2127 (*i)->resetNodeBorder(this);
2128 }
2129 }
2130 }
2131
2132
2133 NBEdge*
getConnectionTo(NBNode * n) const2134 NBNode::getConnectionTo(NBNode* n) const {
2135 for (EdgeVector::const_iterator i = myOutgoingEdges.begin(); i != myOutgoingEdges.end(); i++) {
2136 if ((*i)->getToNode() == n) {
2137 return (*i);
2138 }
2139 }
2140 return nullptr;
2141 }
2142
2143
2144 bool
isNearDistrict() const2145 NBNode::isNearDistrict() const {
2146 if (isDistrict()) {
2147 return false;
2148 }
2149 EdgeVector edges;
2150 copy(getIncomingEdges().begin(), getIncomingEdges().end(),
2151 back_inserter(edges));
2152 copy(getOutgoingEdges().begin(), getOutgoingEdges().end(),
2153 back_inserter(edges));
2154 for (EdgeVector::const_iterator j = edges.begin(); j != edges.end(); ++j) {
2155 NBEdge* t = *j;
2156 NBNode* other = nullptr;
2157 if (t->getToNode() == this) {
2158 other = t->getFromNode();
2159 } else {
2160 other = t->getToNode();
2161 }
2162 EdgeVector edges2;
2163 copy(other->getIncomingEdges().begin(), other->getIncomingEdges().end(), back_inserter(edges2));
2164 copy(other->getOutgoingEdges().begin(), other->getOutgoingEdges().end(), back_inserter(edges2));
2165 for (EdgeVector::const_iterator k = edges2.begin(); k != edges2.end(); ++k) {
2166 if ((*k)->getFromNode()->isDistrict() || (*k)->getToNode()->isDistrict()) {
2167 return true;
2168 }
2169 }
2170 }
2171 return false;
2172 }
2173
2174
2175 bool
isDistrict() const2176 NBNode::isDistrict() const {
2177 return myType == NODETYPE_DISTRICT;
2178 }
2179
2180
2181 int
guessCrossings()2182 NBNode::guessCrossings() {
2183 #ifdef DEBUG_PED_STRUCTURES
2184 gDebugFlag1 = DEBUGCOND;
2185 #endif
2186 int numGuessed = 0;
2187 if (myCrossings.size() > 0 || myDiscardAllCrossings) {
2188 // user supplied crossings, do not guess
2189 return numGuessed;
2190 }
2191 if (gDebugFlag1) {
2192 std::cout << "guess crossings for " << getID() << "\n";
2193 }
2194 EdgeVector allEdges = getEdgesSortedByAngleAtNodeCenter();
2195 // check for pedestrial lanes going clockwise around the node
2196 std::vector<std::pair<NBEdge*, bool> > normalizedLanes;
2197 for (EdgeVector::const_iterator it = allEdges.begin(); it != allEdges.end(); ++it) {
2198 NBEdge* edge = *it;
2199 const std::vector<NBEdge::Lane>& lanes = edge->getLanes();
2200 if (edge->getFromNode() == this) {
2201 for (std::vector<NBEdge::Lane>::const_reverse_iterator it_l = lanes.rbegin(); it_l != lanes.rend(); ++it_l) {
2202 normalizedLanes.push_back(std::make_pair(edge, ((*it_l).permissions & SVC_PEDESTRIAN) != 0));
2203 }
2204 } else {
2205 for (std::vector<NBEdge::Lane>::const_iterator it_l = lanes.begin(); it_l != lanes.end(); ++it_l) {
2206 normalizedLanes.push_back(std::make_pair(edge, ((*it_l).permissions & SVC_PEDESTRIAN) != 0));
2207 }
2208 }
2209 }
2210 // do we even have a pedestrian lane?
2211 int firstSidewalk = -1;
2212 for (int i = 0; i < (int)normalizedLanes.size(); ++i) {
2213 if (normalizedLanes[i].second) {
2214 firstSidewalk = i;
2215 break;
2216 }
2217 }
2218 int hadCandidates = 0;
2219 std::vector<int> connectedCandidates; // number of crossings that were built for each connected candidate
2220 if (firstSidewalk != -1) {
2221 // rotate lanes to ensure that the first one allows pedestrians
2222 std::vector<std::pair<NBEdge*, bool> > tmp;
2223 copy(normalizedLanes.begin() + firstSidewalk, normalizedLanes.end(), std::back_inserter(tmp));
2224 copy(normalizedLanes.begin(), normalizedLanes.begin() + firstSidewalk, std::back_inserter(tmp));
2225 normalizedLanes = tmp;
2226 // find candidates
2227 EdgeVector candidates;
2228 for (int i = 0; i < (int)normalizedLanes.size(); ++i) {
2229 NBEdge* edge = normalizedLanes[i].first;
2230 const bool allowsPed = normalizedLanes[i].second;
2231 if (gDebugFlag1) {
2232 std::cout << " cands=" << toString(candidates) << " edge=" << edge->getID() << " allowsPed=" << allowsPed << "\n";
2233 }
2234 if (!allowsPed && (candidates.size() == 0 || candidates.back() != edge)) {
2235 candidates.push_back(edge);
2236 } else if (allowsPed) {
2237 if (candidates.size() > 0) {
2238 if (hadCandidates > 0 || forbidsPedestriansAfter(normalizedLanes, i)) {
2239 hadCandidates++;
2240 const int n = checkCrossing(candidates);
2241 numGuessed += n;
2242 if (n > 0) {
2243 connectedCandidates.push_back(n);
2244 }
2245 }
2246 candidates.clear();
2247 }
2248 }
2249 }
2250 if (hadCandidates > 0 && candidates.size() > 0) {
2251 // avoid wrapping around to the same sidewalk
2252 hadCandidates++;
2253 const int n = checkCrossing(candidates);
2254 numGuessed += n;
2255 if (n > 0) {
2256 connectedCandidates.push_back(n);
2257 }
2258 }
2259 }
2260 // Avoid duplicate crossing between the same pair of walkingareas
2261 if (gDebugFlag1) {
2262 std::cout << " hadCandidates=" << hadCandidates << " connectedCandidates=" << toString(connectedCandidates) << "\n";
2263 }
2264 if (hadCandidates == 2 && connectedCandidates.size() == 2) {
2265 // One or both of them might be split: remove the one with less splits
2266 if (connectedCandidates.back() <= connectedCandidates.front()) {
2267 numGuessed -= connectedCandidates.back();
2268 myCrossings.erase(myCrossings.end() - connectedCandidates.back(), myCrossings.end());
2269 } else {
2270 numGuessed -= connectedCandidates.front();
2271 myCrossings.erase(myCrossings.begin(), myCrossings.begin() + connectedCandidates.front());
2272 }
2273 }
2274 std::sort(myCrossings.begin(), myCrossings.end(), NBNodesEdgesSorter::crossing_by_junction_angle_sorter(this, myAllEdges));
2275 if (gDebugFlag1) {
2276 std::cout << "guessedCrossings:\n";
2277 for (auto crossing : myCrossings) {
2278 std::cout << " edges=" << toString(crossing->edges) << "\n";
2279 }
2280 }
2281 return numGuessed;
2282 }
2283
2284
2285 int
checkCrossing(EdgeVector candidates)2286 NBNode::checkCrossing(EdgeVector candidates) {
2287 if (gDebugFlag1) {
2288 std::cout << "checkCrossing candidates=" << toString(candidates) << "\n";
2289 }
2290 if (candidates.size() == 0) {
2291 if (gDebugFlag1) {
2292 std::cout << "no crossing added (numCandidates=" << candidates.size() << ")\n";
2293 }
2294 return 0;
2295 } else {
2296 // check whether the edges may be part of a common crossing due to having similar angle
2297 double prevAngle = -100000; // dummy
2298 for (int i = 0; i < (int)candidates.size(); ++i) {
2299 NBEdge* edge = candidates[i];
2300 double angle = edge->getCrossingAngle(this);
2301 // edges should be sorted by angle but this only holds true approximately
2302 if (i > 0 && fabs(NBHelpers::relAngle(angle, prevAngle)) > EXTEND_CROSSING_ANGLE_THRESHOLD) {
2303 if (gDebugFlag1) {
2304 std::cout << "no crossing added (found angle difference of " << fabs(NBHelpers::relAngle(angle, prevAngle)) << " at i=" << i << "\n";
2305 }
2306 return 0;
2307 }
2308 if (!isTLControlled() && myType != NODETYPE_RAIL_CROSSING && edge->getSpeed() > OptionsCont::getOptions().getFloat("crossings.guess.speed-threshold")) {
2309 if (gDebugFlag1) {
2310 std::cout << "no crossing added (uncontrolled, edge with speed > " << edge->getSpeed() << ")\n";
2311 }
2312 return 0;
2313 }
2314 prevAngle = angle;
2315 }
2316 if (candidates.size() == 1 || getType() == NODETYPE_RAIL_CROSSING) {
2317 addCrossing(candidates, NBEdge::UNSPECIFIED_WIDTH, isTLControlled());
2318 if (gDebugFlag1) {
2319 std::cout << "adding crossing: " << toString(candidates) << "\n";
2320 }
2321 return 1;
2322 } else {
2323 // check for intermediate walking areas
2324 double prevAngle = -100000; // dummy
2325 for (EdgeVector::iterator it = candidates.begin(); it != candidates.end(); ++it) {
2326 double angle = (*it)->getCrossingAngle(this);
2327 if (it != candidates.begin()) {
2328 NBEdge* prev = *(it - 1);
2329 NBEdge* curr = *it;
2330 Position prevPos, currPos;
2331 int laneI;
2332 // compute distance between candiate edges
2333 double intermediateWidth = 0;
2334 if (prev->getToNode() == this) {
2335 laneI = prev->getNumLanes() - 1;
2336 prevPos = prev->getLanes()[laneI].shape[-1];
2337 } else {
2338 laneI = 0;
2339 prevPos = prev->getLanes()[laneI].shape[0];
2340 }
2341 intermediateWidth -= 0.5 * prev->getLaneWidth(laneI);
2342 if (curr->getFromNode() == this) {
2343 laneI = curr->getNumLanes() - 1;
2344 currPos = curr->getLanes()[laneI].shape[0];
2345 } else {
2346 laneI = 0;
2347 currPos = curr->getLanes()[laneI].shape[-1];
2348 }
2349 intermediateWidth -= 0.5 * curr->getLaneWidth(laneI);
2350 intermediateWidth += currPos.distanceTo2D(prevPos);
2351 if (gDebugFlag1) {
2352 std::cout
2353 << " prevAngle=" << prevAngle
2354 << " angle=" << angle
2355 << " intermediateWidth=" << intermediateWidth
2356 << "\n";
2357 }
2358 if (fabs(NBHelpers::relAngle(prevAngle, angle)) > SPLIT_CROSSING_ANGLE_THRESHOLD
2359 || (intermediateWidth > SPLIT_CROSSING_WIDTH_THRESHOLD)) {
2360 return checkCrossing(EdgeVector(candidates.begin(), it))
2361 + checkCrossing(EdgeVector(it, candidates.end()));
2362 }
2363 }
2364 prevAngle = angle;
2365 }
2366 addCrossing(candidates, NBEdge::UNSPECIFIED_WIDTH, isTLControlled());
2367 if (gDebugFlag1) {
2368 std::cout << "adding crossing: " << toString(candidates) << "\n";
2369 }
2370 return 1;
2371 }
2372 }
2373 }
2374
2375
2376 bool
checkCrossingDuplicated(EdgeVector edges)2377 NBNode::checkCrossingDuplicated(EdgeVector edges) {
2378 // sort edge vector
2379 std::sort(edges.begin(), edges.end());
2380 // iterate over crossing to find a crossing with the same edges
2381 for (auto crossing : myCrossings) {
2382 // sort edges of crossing before compare
2383 EdgeVector edgesOfCrossing = crossing->edges;
2384 std::sort(edgesOfCrossing.begin(), edgesOfCrossing.end());
2385 if (edgesOfCrossing == edges) {
2386 return true;
2387 }
2388 }
2389 return false;
2390 }
2391
2392
2393 bool
forbidsPedestriansAfter(std::vector<std::pair<NBEdge *,bool>> normalizedLanes,int startIndex)2394 NBNode::forbidsPedestriansAfter(std::vector<std::pair<NBEdge*, bool> > normalizedLanes, int startIndex) {
2395 for (int i = startIndex; i < (int)normalizedLanes.size(); ++i) {
2396 if (!normalizedLanes[i].second) {
2397 return true;
2398 }
2399 }
2400 return false;
2401 }
2402
2403
2404 void
buildCrossingsAndWalkingAreas()2405 NBNode::buildCrossingsAndWalkingAreas() {
2406 buildCrossings();
2407 buildWalkingAreas(OptionsCont::getOptions().getInt("junctions.corner-detail"));
2408 // ensure that all crossings are properly connected
2409 for (auto crossing : myCrossings) {
2410 if (crossing->prevWalkingArea == "" || crossing->nextWalkingArea == "" || !crossing->valid) {
2411 if (crossing->valid) {
2412 WRITE_WARNING("Discarding invalid crossing '" + crossing->id + "' at junction '" + getID() + "' with edges '" + toString(crossing->edges) + "' (no walkingarea found).");
2413 }
2414 for (WalkingArea& wa : myWalkingAreas) {
2415 std::vector<std::string>::iterator it_nc = std::find(wa.nextCrossings.begin(), wa.nextCrossings.end(), crossing->id);
2416 if (it_nc != wa.nextCrossings.end()) {
2417 wa.nextCrossings.erase(it_nc);
2418 }
2419 }
2420 crossing->valid = false;
2421 crossing->prevWalkingArea = "";
2422 crossing->nextWalkingArea = "";
2423 }
2424 }
2425 }
2426
2427 std::vector<NBNode::Crossing*>
getCrossings() const2428 NBNode::getCrossings() const {
2429 std::vector<Crossing*> result;
2430 for (Crossing* const c : myCrossings) {
2431 if (c->valid) {
2432 result.push_back(c);
2433 }
2434 }
2435 //if (myCrossings.size() > 0) {
2436 // std::cout << "valid crossings at " << getID() << "\n";
2437 // for (std::vector<NBNode::Crossing*>::const_iterator it = result.begin(); it != result.end(); ++it) {
2438 // std::cout << " " << toString((*it)->edges) << "\n";
2439 // }
2440 //}
2441 return result;
2442 }
2443
2444
2445 void
discardAllCrossings(bool rejectAll)2446 NBNode::discardAllCrossings(bool rejectAll) {
2447 for (auto c : myCrossings) {
2448 delete c;
2449 }
2450 myCrossings.clear();
2451 // also discard all further crossings
2452 if (rejectAll) {
2453 myDiscardAllCrossings = true;
2454 }
2455 }
2456
2457
2458 void
discardWalkingareas()2459 NBNode::discardWalkingareas() {
2460 myWalkingAreas.clear();
2461 }
2462
2463
2464 void
buildInnerEdges()2465 NBNode::buildInnerEdges() {
2466 // myDisplacementError is computed during this operation. reset first
2467 myDisplacementError = 0;
2468 // build inner edges for vehicle movements across the junction
2469 int noInternalNoSplits = 0;
2470 for (EdgeVector::const_iterator i = myIncomingEdges.begin(); i != myIncomingEdges.end(); i++) {
2471 const std::vector<NBEdge::Connection>& elv = (*i)->getConnections();
2472 for (std::vector<NBEdge::Connection>::const_iterator k = elv.begin(); k != elv.end(); ++k) {
2473 if ((*k).toEdge == nullptr) {
2474 continue;
2475 }
2476 noInternalNoSplits++;
2477 }
2478 }
2479 int lno = 0;
2480 int splitNo = 0;
2481 for (EdgeVector::const_iterator i = myIncomingEdges.begin(); i != myIncomingEdges.end(); i++) {
2482 (*i)->buildInnerEdges(*this, noInternalNoSplits, lno, splitNo);
2483 }
2484 }
2485
2486
2487 int
buildCrossings()2488 NBNode::buildCrossings() {
2489 #ifdef DEBUG_PED_STRUCTURES
2490 gDebugFlag1 = DEBUGCOND;
2491 #endif
2492 if (gDebugFlag1) {
2493 std::cout << "build crossings for " << getID() << ":\n";
2494 }
2495 if (myDiscardAllCrossings) {
2496 myCrossings.clear();
2497 }
2498 int index = 0;
2499 const double defaultWidth = OptionsCont::getOptions().getFloat("default.crossing-width");
2500 for (auto c : myCrossings) {
2501 c->valid = true;
2502 if (!isTLControlled()) {
2503 c->tlID = ""; // reset for Netedit, set via setCrossingTLIndices()
2504 }
2505 c->id = ":" + getID() + "_c" + toString(index++);
2506 c->width = (c->customWidth == NBEdge::UNSPECIFIED_WIDTH) ? defaultWidth : c->customWidth;
2507 // reset fields, so repeated computation (Netedit) will sucessfully perform the checks
2508 // in buildWalkingAreas (split crossings) and buildInnerEdges (sanity check)
2509 c->nextWalkingArea = "";
2510 c->prevWalkingArea = "";
2511 EdgeVector& edges = c->edges;
2512 if (gDebugFlag1) {
2513 std::cout << " crossing=" << c->id << " edges=" << toString(edges);
2514 }
2515 // sorting the edges in the right way is imperative. We want to sort
2516 // them by getAngleAtNodeToCenter() but need to be extra carefull to avoid wrapping around 0 somewhere in between
2517 std::sort(edges.begin(), edges.end(), NBContHelper::edge_by_angle_to_nodeShapeCentroid_sorter(this));
2518 if (gDebugFlag1) {
2519 std::cout << " sortedEdges=" << toString(edges) << "\n";
2520 };
2521 // rotate the edges so that the largest relative angle difference comes at the end
2522 double maxAngleDiff = 0;
2523 int maxAngleDiffIndex = 0; // index before maxDist
2524 for (int i = 0; i < (int) edges.size(); i++) {
2525 double diff = NBHelpers::relAngle(edges[i]->getAngleAtNodeToCenter(this),
2526 edges[(i + 1) % edges.size()]->getAngleAtNodeToCenter(this));
2527 if (diff < 0) {
2528 diff += 360;
2529 }
2530 if (gDebugFlag1) {
2531 std::cout << " i=" << i << " a1=" << edges[i]->getAngleAtNodeToCenter(this) << " a2=" << edges[(i + 1) % edges.size()]->getAngleAtNodeToCenter(this) << " diff=" << diff << "\n";
2532 }
2533 if (diff > maxAngleDiff) {
2534 maxAngleDiff = diff;
2535 maxAngleDiffIndex = i;
2536 }
2537 }
2538 if (maxAngleDiff > 2 && maxAngleDiff < 360 - 2) {
2539 // if the angle differences is too small, we better not rotate
2540 std::rotate(edges.begin(), edges.begin() + (maxAngleDiffIndex + 1) % edges.size(), edges.end());
2541 if (gDebugFlag1) {
2542 std::cout << " rotatedEdges=" << toString(edges);
2543 }
2544 }
2545 // reverse to get them in CCW order (walking direction around the node)
2546 std::reverse(edges.begin(), edges.end());
2547 if (gDebugFlag1) {
2548 std::cout << " finalEdges=" << toString(edges) << "\n";
2549 }
2550 // compute shape
2551 c->shape.clear();
2552 const int begDir = (edges.front()->getFromNode() == this ? FORWARD : BACKWARD);
2553 const int endDir = (edges.back()->getToNode() == this ? FORWARD : BACKWARD);
2554 if (edges.front()->getFirstNonPedestrianLaneIndex(begDir) < 0
2555 || edges.back()->getFirstNonPedestrianLaneIndex(endDir) < 0) {
2556 // invalid crossing
2557 WRITE_WARNING("Discarding invalid crossing '" + c->id + "' at junction '" + getID() + "' with edges '" + toString(c->edges) + "' (no vehicle lanes to cross).");
2558 c->valid = false;
2559 } else if (c->customShape.size() != 0) {
2560 c->shape = c->customShape;
2561 } else {
2562 NBEdge::Lane crossingBeg = edges.front()->getFirstNonPedestrianLane(begDir);
2563 NBEdge::Lane crossingEnd = edges.back()->getFirstNonPedestrianLane(endDir);
2564 crossingBeg.width = (crossingBeg.width == NBEdge::UNSPECIFIED_WIDTH ? SUMO_const_laneWidth : crossingBeg.width);
2565 crossingEnd.width = (crossingEnd.width == NBEdge::UNSPECIFIED_WIDTH ? SUMO_const_laneWidth : crossingEnd.width);
2566 crossingBeg.shape.move2side(begDir * crossingBeg.width / 2);
2567 crossingEnd.shape.move2side(endDir * crossingEnd.width / 2);
2568 crossingBeg.shape.extrapolate(c->width / 2);
2569 crossingEnd.shape.extrapolate(c->width / 2);
2570 // check if after all changes shape are NAN (in these case, discard)
2571 if (crossingBeg.shape.isNAN() || crossingEnd.shape.isNAN()) {
2572 WRITE_WARNING("Discarding invalid crossing '" + c->id + "' at junction '" + getID() + "' with edges '" + toString(c->edges) + "' (Invalid shape).");
2573 c->valid = false;
2574 } else {
2575 c->shape.push_back(crossingBeg.shape[begDir == FORWARD ? 0 : -1]);
2576 c->shape.push_back(crossingEnd.shape[endDir == FORWARD ? -1 : 0]);
2577 }
2578 }
2579 }
2580 return index;
2581 }
2582
2583
2584 void
buildWalkingAreas(int cornerDetail)2585 NBNode::buildWalkingAreas(int cornerDetail) {
2586 #ifdef DEBUG_PED_STRUCTURES
2587 gDebugFlag1 = DEBUGCOND;
2588 #endif
2589 int index = 0;
2590 myWalkingAreas.clear();
2591 if (gDebugFlag1) {
2592 std::cout << "build walkingAreas for " << getID() << ":\n";
2593 }
2594 if (myAllEdges.size() == 0) {
2595 return;
2596 }
2597 EdgeVector allEdges = getEdgesSortedByAngleAtNodeCenter();
2598 // shapes are all pointing away from the intersection
2599 std::vector<std::pair<NBEdge*, NBEdge::Lane> > normalizedLanes;
2600 for (EdgeVector::const_iterator it = allEdges.begin(); it != allEdges.end(); ++it) {
2601 NBEdge* edge = *it;
2602 const std::vector<NBEdge::Lane>& lanes = edge->getLanes();
2603 if (edge->getFromNode() == this) {
2604 for (std::vector<NBEdge::Lane>::const_reverse_iterator it_l = lanes.rbegin(); it_l != lanes.rend(); ++it_l) {
2605 NBEdge::Lane l = *it_l;
2606 l.shape = l.shape.getSubpartByIndex(0, 2);
2607 l.width = (l.width == NBEdge::UNSPECIFIED_WIDTH ? SUMO_const_laneWidth : l.width);
2608 normalizedLanes.push_back(std::make_pair(edge, l));
2609 }
2610 } else {
2611 for (std::vector<NBEdge::Lane>::const_iterator it_l = lanes.begin(); it_l != lanes.end(); ++it_l) {
2612 NBEdge::Lane l = *it_l;
2613 l.shape = l.shape.reverse();
2614 l.shape = l.shape.getSubpartByIndex(0, 2);
2615 l.width = (l.width == NBEdge::UNSPECIFIED_WIDTH ? SUMO_const_laneWidth : l.width);
2616 normalizedLanes.push_back(std::make_pair(edge, l));
2617 }
2618 }
2619 }
2620 //if (gDebugFlag1) std::cout << " normalizedLanes=" << normalizedLanes.size() << "\n";
2621 // collect [start,count[ indices in normalizedLanes that belong to a walkingArea
2622 std::vector<std::pair<int, int> > waIndices;
2623 int start = -1;
2624 NBEdge* prevEdge = normalizedLanes.back().first;
2625 for (int i = 0; i < (int)normalizedLanes.size(); ++i) {
2626 NBEdge* edge = normalizedLanes[i].first;
2627 NBEdge::Lane& l = normalizedLanes[i].second;
2628 if (start == -1) {
2629 if ((l.permissions & SVC_PEDESTRIAN) != 0) {
2630 start = i;
2631 }
2632 } else {
2633 if ((l.permissions & SVC_PEDESTRIAN) == 0 || crossingBetween(edge, prevEdge)) {
2634 waIndices.push_back(std::make_pair(start, i - start));
2635 if ((l.permissions & SVC_PEDESTRIAN) != 0) {
2636 start = i;
2637 } else {
2638 start = -1;
2639 }
2640
2641 }
2642 }
2643 if (gDebugFlag1) std::cout << " i=" << i << " edge=" << edge->getID() << " start=" << start << " ped=" << ((l.permissions & SVC_PEDESTRIAN) != 0)
2644 << " waI=" << waIndices.size() << " crossingBetween=" << crossingBetween(edge, prevEdge) << "\n";
2645 prevEdge = edge;
2646 }
2647 // deal with wrap-around issues
2648 if (start != - 1) {
2649 const int waNumLanes = (int)normalizedLanes.size() - start;
2650 if (waIndices.size() == 0) {
2651 waIndices.push_back(std::make_pair(start, waNumLanes));
2652 if (gDebugFlag1) {
2653 std::cout << " single wa, end at wrap-around\n";
2654 }
2655 } else {
2656 if (waIndices.front().first == 0) {
2657 NBEdge* edge = normalizedLanes.front().first;
2658 NBEdge* prevEdge = normalizedLanes.back().first;
2659 if (crossingBetween(edge, prevEdge)) {
2660 // do not wrap-around if there is a crossing in between
2661 waIndices.push_back(std::make_pair(start, waNumLanes));
2662 if (gDebugFlag1) {
2663 std::cout << " do not wrap around, turn-around in between\n";
2664 }
2665 } else {
2666 // first walkingArea wraps around
2667 waIndices.front().first = start;
2668 waIndices.front().second = waNumLanes + waIndices.front().second;
2669 if (gDebugFlag1) {
2670 std::cout << " wrapping around\n";
2671 }
2672 }
2673 } else {
2674 // last walkingArea ends at the wrap-around
2675 waIndices.push_back(std::make_pair(start, waNumLanes));
2676 if (gDebugFlag1) {
2677 std::cout << " end at wrap-around\n";
2678 }
2679 }
2680 }
2681 }
2682 if (gDebugFlag1) {
2683 std::cout << " normalizedLanes=" << normalizedLanes.size() << " waIndices:\n";
2684 for (int i = 0; i < (int)waIndices.size(); ++i) {
2685 std::cout << " " << waIndices[i].first << ", " << waIndices[i].second << "\n";
2686 }
2687 }
2688 // build walking areas connected to a sidewalk
2689 for (int i = 0; i < (int)waIndices.size(); ++i) {
2690 const bool buildExtensions = waIndices[i].second != (int)normalizedLanes.size();
2691 const int start = waIndices[i].first;
2692 const int prev = start > 0 ? start - 1 : (int)normalizedLanes.size() - 1;
2693 const int count = waIndices[i].second;
2694 const int end = (start + count) % normalizedLanes.size();
2695
2696 WalkingArea wa(":" + getID() + "_w" + toString(index++), 1);
2697 if (gDebugFlag1) {
2698 std::cout << "build walkingArea " << wa.id << " start=" << start << " end=" << end << " count=" << count << " prev=" << prev << ":\n";
2699 }
2700 double endCrossingWidth = 0;
2701 double startCrossingWidth = 0;
2702 PositionVector endCrossingShape;
2703 PositionVector startCrossingShape;
2704 // check for connected crossings
2705 bool connectsCrossing = false;
2706 std::vector<Position> connectedPoints;
2707 for (auto c : getCrossings()) {
2708 if (gDebugFlag1) {
2709 std::cout << " crossing=" << c->id << " sortedEdges=" << toString(c->edges) << "\n";
2710 }
2711 if (c->edges.back() == normalizedLanes[end].first
2712 && (normalizedLanes[end].second.permissions & SVC_PEDESTRIAN) == 0) {
2713 // crossing ends
2714 if (c->nextWalkingArea != "") {
2715 WRITE_WARNING("Invalid pedestrian topology at junction '" + getID()
2716 + "'; crossing '" + c->id
2717 + "' targets '" + c->nextWalkingArea
2718 + "' and '" + wa.id + "'.");
2719 c->valid = false;
2720 }
2721 c->nextWalkingArea = wa.id;
2722 if ((int)c->edges.size() < wa.minPrevCrossingEdges) {
2723 // if there are multiple crossings, use the shape of the one that crosses fewer edges
2724 endCrossingWidth = c->width;
2725 endCrossingShape = c->shape;
2726 wa.width = MAX2(wa.width, endCrossingWidth);
2727 connectsCrossing = true;
2728 connectedPoints.push_back(c->shape[-1]);
2729 wa.minPrevCrossingEdges = (int)c->edges.size();
2730 }
2731 if (gDebugFlag1) {
2732 std::cout << " crossing " << c->id << " ends\n";
2733 }
2734 }
2735 if (c->edges.front() == normalizedLanes[prev].first
2736 && (normalizedLanes[prev].second.permissions & SVC_PEDESTRIAN) == 0) {
2737 // crossing starts
2738 if (c->prevWalkingArea != "") {
2739 WRITE_WARNING("Invalid pedestrian topology at junction '" + getID()
2740 + "'; crossing '" + c->id
2741 + "' is targeted by '" + c->prevWalkingArea
2742 + "' and '" + wa.id + "'.");
2743 c->valid = false;
2744 }
2745 c->prevWalkingArea = wa.id;
2746 wa.nextCrossings.push_back(c->id);
2747 if ((int)c->edges.size() < wa.minNextCrossingEdges) {
2748 // if there are multiple crossings, use the shape of the one that crosses fewer edges
2749 startCrossingWidth = c->width;
2750 startCrossingShape = c->shape;
2751 wa.width = MAX2(wa.width, startCrossingWidth);
2752 connectsCrossing = true;
2753 connectedPoints.push_back(c->shape[0]);
2754 wa.minNextCrossingEdges = (int)c->edges.size();
2755 }
2756 if (gDebugFlag1) {
2757 std::cout << " crossing " << c->id << " starts\n";
2758 }
2759 }
2760 if (gDebugFlag1) std::cout << " check connections to crossing " << c->id
2761 << " cFront=" << c->edges.front()->getID() << " cBack=" << c->edges.back()->getID()
2762 << " wEnd=" << normalizedLanes[end].first->getID() << " wStart=" << normalizedLanes[start].first->getID()
2763 << " wStartPrev=" << normalizedLanes[prev].first->getID()
2764 << "\n";
2765 }
2766 if (count < 2 && !connectsCrossing) {
2767 // not relevant for walking
2768 if (gDebugFlag1) {
2769 std::cout << " not relevant for walking: count=" << count << " connectsCrossing=" << connectsCrossing << "\n";
2770 }
2771 continue;
2772 }
2773 // build shape and connections
2774 std::set<NBEdge*, ComparatorIdLess> connected;
2775 for (int j = 0; j < count; ++j) {
2776 const int nlI = (start + j) % normalizedLanes.size();
2777 NBEdge* edge = normalizedLanes[nlI].first;
2778 NBEdge::Lane l = normalizedLanes[nlI].second;
2779 wa.width = MAX2(wa.width, l.width);
2780 if (connected.count(edge) == 0) {
2781 if (edge->getFromNode() == this) {
2782 wa.nextSidewalks.push_back(edge->getSidewalkID());
2783 connectedPoints.push_back(edge->getLaneShape(0)[0]);
2784 } else {
2785 wa.prevSidewalks.push_back(edge->getSidewalkID());
2786 connectedPoints.push_back(edge->getLaneShape(0)[-1]);
2787 }
2788 connected.insert(edge);
2789 }
2790 l.shape.move2side(-l.width / 2);
2791 wa.shape.push_back(l.shape[0]);
2792 l.shape.move2side(l.width);
2793 wa.shape.push_back(l.shape[0]);
2794 }
2795 if (buildExtensions) {
2796 // extension at starting crossing
2797 if (startCrossingShape.size() > 0) {
2798 if (gDebugFlag1) {
2799 std::cout << " extension at startCrossing shape=" << startCrossingShape << "\n";
2800 }
2801 startCrossingShape.move2side(startCrossingWidth / 2);
2802 wa.shape.push_front_noDoublePos(startCrossingShape[0]); // right corner
2803 startCrossingShape.move2side(-startCrossingWidth);
2804 wa.shape.push_front_noDoublePos(startCrossingShape[0]); // left corner goes first
2805 }
2806 // extension at ending crossing
2807 if (endCrossingShape.size() > 0) {
2808 if (gDebugFlag1) {
2809 std::cout << " extension at endCrossing shape=" << endCrossingShape << "\n";
2810 }
2811 endCrossingShape.move2side(endCrossingWidth / 2);
2812 wa.shape.push_back_noDoublePos(endCrossingShape[-1]);
2813 endCrossingShape.move2side(-endCrossingWidth);
2814 wa.shape.push_back_noDoublePos(endCrossingShape[-1]);
2815 }
2816 }
2817 if (connected.size() == 2 && !connectsCrossing && wa.nextSidewalks.size() == 1 && wa.prevSidewalks.size() == 1
2818 && normalizedLanes.size() == 2) {
2819 // do not build a walkingArea since a normal connection exists
2820 NBEdge* e1 = *connected.begin();
2821 NBEdge* e2 = *(++connected.begin());
2822 if (e1->hasConnectionTo(e2, 0, 0) || e2->hasConnectionTo(e1, 0, 0)) {
2823 if (gDebugFlag1) {
2824 std::cout << " not building a walkingarea since normal connections exist\n";
2825 }
2826 continue;
2827 }
2828 }
2829 // build smooth inner curve (optional)
2830 if (cornerDetail > 0) {
2831 int smoothEnd = end;
2832 int smoothPrev = prev;
2833 // extend to green verge
2834 if (endCrossingWidth > 0 && normalizedLanes[smoothEnd].second.permissions == 0) {
2835 smoothEnd = (smoothEnd + 1) % normalizedLanes.size();
2836 }
2837 if (startCrossingWidth > 0 && normalizedLanes[smoothPrev].second.permissions == 0) {
2838 if (smoothPrev == 0) {
2839 smoothPrev = (int)normalizedLanes.size() - 1;
2840 } else {
2841 smoothPrev--;
2842 }
2843 }
2844 PositionVector begShape = normalizedLanes[smoothEnd].second.shape;
2845 begShape = begShape.reverse();
2846 //begShape.extrapolate(endCrossingWidth);
2847 begShape.move2side(normalizedLanes[smoothEnd].second.width / 2);
2848 PositionVector endShape = normalizedLanes[smoothPrev].second.shape;
2849 endShape.move2side(normalizedLanes[smoothPrev].second.width / 2);
2850 //endShape.extrapolate(startCrossingWidth);
2851 PositionVector curve;
2852 if ((normalizedLanes[smoothEnd].first->getPermissions() & normalizedLanes[smoothPrev].first->getPermissions() &
2853 ~SVC_PEDESTRIAN) != 0) {
2854 curve = computeSmoothShape(begShape, endShape, cornerDetail + 2, false, 25, 25);
2855 } else {
2856 const double extend = MIN2(10.0, begShape.back().distanceTo2D(endShape.front()) / 2);
2857 curve = computeSmoothShape(begShape, endShape, cornerDetail + 2, false, extend, extend, nullptr, FOUR_CONTROL_POINTS);
2858 }
2859 if (gDebugFlag1) std::cout
2860 << " end=" << smoothEnd << " prev=" << smoothPrev
2861 << " endCrossingWidth=" << endCrossingWidth << " startCrossingWidth=" << startCrossingWidth
2862 << " begShape=" << begShape << " endShape=" << endShape << " smooth curve=" << curve << "\n";
2863 if (curve.size() > 2) {
2864 curve.erase(curve.begin());
2865 curve.pop_back();
2866 if (endCrossingWidth > 0) {
2867 wa.shape.pop_back();
2868 }
2869 if (startCrossingWidth > 0) {
2870 wa.shape.erase(wa.shape.begin());
2871 }
2872 wa.shape.append(curve, 0);
2873 }
2874 }
2875 // apply custom shapes
2876 if (myWalkingAreaCustomShapes.size() > 0) {
2877 for (auto wacs : myWalkingAreaCustomShapes) {
2878 // every edge in wasc.edges must be part of connected
2879 if (wacs.shape.size() != 0 && includes(connected, wacs.edges)) {
2880 wa.shape = wacs.shape;
2881 wa.hasCustomShape = true;
2882 }
2883 }
2884 }
2885 // determine length (average of all possible connections)
2886 double lengthSum = 0;
2887 int combinations = 0;
2888 for (std::vector<Position>::const_iterator it1 = connectedPoints.begin(); it1 != connectedPoints.end(); ++it1) {
2889 for (std::vector<Position>::const_iterator it2 = connectedPoints.begin(); it2 != connectedPoints.end(); ++it2) {
2890 const Position& p1 = *it1;
2891 const Position& p2 = *it2;
2892 if (p1 != p2) {
2893 lengthSum += p1.distanceTo2D(p2);
2894 combinations += 1;
2895 }
2896 }
2897 }
2898 if (gDebugFlag1) {
2899 std::cout << " combinations=" << combinations << " connectedPoints=" << connectedPoints << "\n";
2900 }
2901 wa.length = POSITION_EPS;
2902 if (combinations > 0) {
2903 wa.length = MAX2(POSITION_EPS, lengthSum / combinations);
2904 }
2905 myWalkingAreas.push_back(wa);
2906 }
2907 // build walkingAreas between split crossings
2908 std::vector<Crossing*> validCrossings = getCrossings();
2909 for (std::vector<Crossing*>::iterator it = validCrossings.begin(); it != validCrossings.end(); ++it) {
2910 Crossing& prev = **it;
2911 Crossing& next = (it != validCrossings.begin() ? **(it - 1) :** (validCrossings.end() - 1));
2912 if (gDebugFlag1) {
2913 std::cout << " checkIntermediate: prev=" << prev.id << " next=" << next.id << " prev.nextWA=" << prev.nextWalkingArea << "\n";
2914 }
2915 if (prev.nextWalkingArea == "") {
2916 if (next.prevWalkingArea != "" || &prev == &next) {
2917 WRITE_WARNING("Invalid pedestrian topology: crossing '" + prev.id + "' across '" + toString(prev.edges) + "' has no target.");
2918 prev.valid = false;
2919 continue;
2920 }
2921 WalkingArea wa(":" + getID() + "_w" + toString(index++), prev.width);
2922 prev.nextWalkingArea = wa.id;
2923 wa.nextCrossings.push_back(next.id);
2924 next.prevWalkingArea = wa.id;
2925 // back of previous crossing
2926 PositionVector tmp = prev.shape;
2927 tmp.move2side(-prev.width / 2);
2928 wa.shape.push_back(tmp[-1]);
2929 tmp.move2side(prev.width);
2930 wa.shape.push_back(tmp[-1]);
2931 // front of next crossing
2932 tmp = next.shape;
2933 tmp.move2side(prev.width / 2);
2934 wa.shape.push_back(tmp[0]);
2935 tmp.move2side(-prev.width);
2936 wa.shape.push_back(tmp[0]);
2937 // apply custom shapes
2938 if (myWalkingAreaCustomShapes.size() > 0) {
2939 std::set<NBEdge*, ComparatorIdLess> crossed(prev.edges.begin(), prev.edges.end());
2940 crossed.insert(next.edges.begin(), next.edges.end());
2941 for (auto wacs : myWalkingAreaCustomShapes) {
2942 // every edge in wacs.edges must be part of crossed
2943 if (wacs.shape.size() != 0 && wacs.edges.size() > 1 && includes(crossed, wacs.edges)) {
2944 wa.shape = wacs.shape;
2945 wa.hasCustomShape = true;
2946 }
2947 }
2948 }
2949 // length (special case)
2950 wa.length = MAX2(POSITION_EPS, prev.shape.back().distanceTo2D(next.shape.front()));
2951 myWalkingAreas.push_back(wa);
2952 if (gDebugFlag1) {
2953 std::cout << " build wa=" << wa.id << "\n";
2954 }
2955 }
2956 }
2957 }
2958
2959 bool
includes(const std::set<NBEdge *,ComparatorIdLess> & super,const std::set<const NBEdge *,ComparatorIdLess> & sub)2960 NBNode::includes(const std::set<NBEdge*, ComparatorIdLess>& super,
2961 const std::set<const NBEdge*, ComparatorIdLess>& sub) {
2962 // for some reason std::include does not work reliably
2963 for (const NBEdge* e : sub) {
2964 if (super.count(const_cast<NBEdge*>(e)) == 0) {
2965 return false;
2966 }
2967 }
2968 return true;
2969 }
2970
2971
2972 bool
crossingBetween(const NBEdge * e1,const NBEdge * e2) const2973 NBNode::crossingBetween(const NBEdge* e1, const NBEdge* e2) const {
2974 if (e1 == e2) {
2975 return false;
2976 }
2977 if (myAllEdges.size() > 3) {
2978 // pedestrian scramble
2979 return false;
2980 }
2981 for (auto c : getCrossings()) {
2982 const EdgeVector& edges = c->edges;
2983 EdgeVector::const_iterator it1 = std::find(edges.begin(), edges.end(), e1);
2984 EdgeVector::const_iterator it2 = std::find(edges.begin(), edges.end(), e2);
2985 if (it1 != edges.end() && it2 != edges.end()) {
2986 return true;
2987 }
2988 }
2989 return false;
2990 }
2991
2992
2993 EdgeVector
edgesBetween(const NBEdge * e1,const NBEdge * e2) const2994 NBNode::edgesBetween(const NBEdge* e1, const NBEdge* e2) const {
2995 EdgeVector result;
2996 EdgeVector::const_iterator it = std::find(myAllEdges.begin(), myAllEdges.end(), e1);
2997 assert(it != myAllEdges.end());
2998 NBContHelper::nextCW(myAllEdges, it);
2999 EdgeVector::const_iterator it_end = std::find(myAllEdges.begin(), myAllEdges.end(), e2);
3000 assert(it_end != myAllEdges.end());
3001 while (it != it_end) {
3002 result.push_back(*it);
3003 NBContHelper::nextCW(myAllEdges, it);
3004 }
3005 return result;
3006 }
3007
3008
3009 void
addWalkingAreaShape(EdgeVector edges,const PositionVector & shape)3010 NBNode::addWalkingAreaShape(EdgeVector edges, const PositionVector& shape) {
3011 WalkingAreaCustomShape wacs;
3012 wacs.edges.insert(edges.begin(), edges.end());
3013 wacs.shape = shape;
3014 myWalkingAreaCustomShapes.push_back(wacs);
3015 }
3016
3017
3018 bool
geometryLike() const3019 NBNode::geometryLike() const {
3020 return geometryLike(myIncomingEdges, myOutgoingEdges);
3021 }
3022
3023 bool
geometryLike(const EdgeVector & incoming,const EdgeVector & outgoing) const3024 NBNode::geometryLike(const EdgeVector& incoming, const EdgeVector& outgoing) const {
3025 if (incoming.size() == 1 && outgoing.size() == 1) {
3026 return true;
3027 }
3028 if (incoming.size() == 2 && outgoing.size() == 2) {
3029 // check whether the incoming and outgoing edges are pairwise (near) parallel and
3030 // thus the only cross-connections could be turn-arounds
3031 NBEdge* in0 = incoming[0];
3032 NBEdge* in1 = incoming[1];
3033 NBEdge* out0 = outgoing[0];
3034 NBEdge* out1 = outgoing[1];
3035 if ((in0->isTurningDirectionAt(out0) || in0->isTurningDirectionAt(out1))
3036 && (in1->isTurningDirectionAt(out0) || in1->isTurningDirectionAt(out1))) {
3037 return true;
3038 }
3039 for (EdgeVector::const_iterator it = incoming.begin(); it != incoming.end(); ++it) {
3040 NBEdge* inEdge = *it;
3041 double angle0 = fabs(NBHelpers::relAngle(inEdge->getAngleAtNode(this), out0->getAngleAtNode(this)));
3042 double angle1 = fabs(NBHelpers::relAngle(inEdge->getAngleAtNode(this), out1->getAngleAtNode(this)));
3043 if (MAX2(angle0, angle1) <= 160) {
3044 // neither of the outgoing edges is parallel to inEdge
3045 return false;
3046 }
3047 }
3048 return true;
3049 }
3050 return false;
3051 }
3052
3053 void
setRoundabout()3054 NBNode::setRoundabout() {
3055 if (myType == NODETYPE_RIGHT_BEFORE_LEFT) {
3056 myType = NODETYPE_PRIORITY;
3057 }
3058 }
3059
3060
3061 NBNode::Crossing*
addCrossing(EdgeVector edges,double width,bool priority,int tlIndex,int tlIndex2,const PositionVector & customShape,bool fromSumoNet)3062 NBNode::addCrossing(EdgeVector edges, double width, bool priority, int tlIndex, int tlIndex2,
3063 const PositionVector& customShape, bool fromSumoNet) {
3064 Crossing* c = new Crossing(this, edges, width, priority, tlIndex, tlIndex2, customShape);
3065 myCrossings.push_back(c);
3066 if (fromSumoNet) {
3067 myCrossingsLoadedFromSumoNet += 1;
3068 }
3069 return c;
3070 }
3071
3072
3073 void
removeCrossing(const EdgeVector & edges)3074 NBNode::removeCrossing(const EdgeVector& edges) {
3075 EdgeSet edgeSet(edges.begin(), edges.end());
3076 for (std::vector<Crossing*>::iterator it = myCrossings.begin(); it != myCrossings.end();) {
3077 EdgeSet edgeSet2((*it)->edges.begin(), (*it)->edges.end());
3078 if (edgeSet == edgeSet2) {
3079 delete *it;
3080 it = myCrossings.erase(it);
3081 } else {
3082 ++it;
3083 }
3084 }
3085 }
3086
3087
3088 NBNode::Crossing*
getCrossing(const std::string & id) const3089 NBNode::getCrossing(const std::string& id) const {
3090 for (auto c : myCrossings) {
3091 if (c->id == id) {
3092 return c;
3093 }
3094 }
3095 throw ProcessError("Request for unknown crossing '" + id + "'");
3096 }
3097
3098
3099 NBNode::Crossing*
getCrossing(const EdgeVector & edges,bool hardFail) const3100 NBNode::getCrossing(const EdgeVector& edges, bool hardFail) const {
3101 EdgeSet edgeSet(edges.begin(), edges.end());
3102 for (auto it : myCrossings) {
3103 EdgeSet edgeSet2(it->edges.begin(), it->edges.end());
3104 if (edgeSet == edgeSet2) {
3105 return it;
3106 }
3107 }
3108 if (!hardFail) {
3109 return nullptr;
3110 } else {
3111 throw ProcessError("Request for unknown crossing for the given Edges");
3112 }
3113 }
3114
3115
3116 bool
setCrossingTLIndices(const std::string & tlID,int startIndex)3117 NBNode::setCrossingTLIndices(const std::string& tlID, int startIndex) {
3118 bool usedCustom = false;
3119 for (auto c : getCrossings()) {
3120 c->tlLinkIndex = startIndex++;
3121 c->tlID = tlID;
3122 if (c->customTLIndex != -1) {
3123 usedCustom |= (c->tlLinkIndex != c->customTLIndex);
3124 c->tlLinkIndex = c->customTLIndex;
3125 }
3126 c->tlLinkIndex2 = c->customTLIndex2;
3127 }
3128 return usedCustom;
3129 }
3130
3131
3132 int
numNormalConnections() const3133 NBNode::numNormalConnections() const {
3134 if (myRequest == nullptr) {
3135 // could be an uncontrolled type
3136 int result = 0;
3137 for (const NBEdge* const edge : myIncomingEdges) {
3138 result += (int)edge->getConnections().size();
3139 }
3140 return result;
3141 } else {
3142 return myRequest->getSizes().second;
3143 }
3144 }
3145
3146
3147 int
getConnectionIndex(const NBEdge * from,const NBEdge::Connection & con) const3148 NBNode::getConnectionIndex(const NBEdge* from, const NBEdge::Connection& con) const {
3149 int result = 0;
3150 for (EdgeVector::const_iterator i = myIncomingEdges.begin(); i != myIncomingEdges.end(); i++) {
3151 const std::vector<NBEdge::Connection>& elv = (*i)->getConnections();
3152 for (std::vector<NBEdge::Connection>::const_iterator k = elv.begin(); k != elv.end(); ++k) {
3153 const NBEdge::Connection& cand = *k;
3154 if (*i == from
3155 && cand.fromLane == con.fromLane
3156 && cand.toLane == con.toLane
3157 && cand.toEdge == con.toEdge) {
3158 return result;
3159 };
3160 result++;
3161 }
3162 }
3163 return -1;
3164 }
3165
3166 Position
getCenter() const3167 NBNode::getCenter() const {
3168 /* Conceptually, the center point would be identical with myPosition.
3169 * However, if the shape is influenced by custom geometry endpoints of the adjoining edges,
3170 * myPosition may fall outside the shape. In this case it is better to use
3171 * the center of the shape
3172 **/
3173 PositionVector tmp = myPoly;
3174 tmp.closePolygon();
3175 //std::cout << getID() << " around=" << tmp.around(myPosition) << " dist=" << tmp.distance2D(myPosition) << "\n";
3176 if (tmp.size() < 3 || tmp.around(myPosition) || tmp.distance2D(myPosition) < POSITION_EPS) {
3177 return myPosition;
3178 } else {
3179 return myPoly.getPolygonCenter();
3180 }
3181 }
3182
3183
3184 EdgeVector
getEdgesSortedByAngleAtNodeCenter() const3185 NBNode::getEdgesSortedByAngleAtNodeCenter() const {
3186 EdgeVector result = myAllEdges;
3187 if (gDebugFlag1) {
3188 std::cout << " angles:\n";
3189 for (EdgeVector::const_iterator it = result.begin(); it != result.end(); ++it) {
3190 std::cout << " edge=" << (*it)->getID() << " edgeAngle=" << (*it)->getAngleAtNode(this) << " angleToShape=" << (*it)->getAngleAtNodeToCenter(this) << "\n";
3191 }
3192 std::cout << " allEdges before: " << toString(result) << "\n";
3193 }
3194 sort(result.begin(), result.end(), NBContHelper::edge_by_angle_to_nodeShapeCentroid_sorter(this));
3195 // let the first edge in myAllEdges remain the first
3196 if (gDebugFlag1) {
3197 std::cout << " allEdges sorted: " << toString(result) << "\n";
3198 }
3199 rotate(result.begin(), std::find(result.begin(), result.end(), *myAllEdges.begin()), result.end());
3200 if (gDebugFlag1) {
3201 std::cout << " allEdges rotated: " << toString(result) << "\n";
3202 }
3203 return result;
3204 }
3205
3206
3207 std::string
getNodeIDFromInternalLane(const std::string id)3208 NBNode::getNodeIDFromInternalLane(const std::string id) {
3209 // this relies on the fact that internal ids always have the form
3210 // :<nodeID>_<part1>_<part2>
3211 // i.e. :C_3_0, :C_c1_0 :C_w0_0
3212 assert(id[0] == ':');
3213 std::string::size_type sep_index = id.rfind('_');
3214 if (sep_index == std::string::npos) {
3215 WRITE_ERROR("Invalid lane id '" + id + "' (missing '_').");
3216 return "";
3217 }
3218 sep_index = id.substr(0, sep_index).rfind('_');
3219 if (sep_index == std::string::npos) {
3220 WRITE_ERROR("Invalid lane id '" + id + "' (missing '_').");
3221 return "";
3222 }
3223 return id.substr(1, sep_index - 1);
3224 }
3225
3226
3227 void
avoidOverlap()3228 NBNode::avoidOverlap() {
3229 // simple case: edges with LANESPREAD_CENTER and a (possible) turndirection at the same node
3230 for (EdgeVector::iterator it = myIncomingEdges.begin(); it != myIncomingEdges.end(); it++) {
3231 NBEdge* edge = *it;
3232 NBEdge* turnDest = edge->getTurnDestination(true);
3233 if (turnDest != nullptr) {
3234 edge->shiftPositionAtNode(this, turnDest);
3235 turnDest->shiftPositionAtNode(this, edge);
3236 }
3237 }
3238 // @todo: edges in the same direction with sharp angles starting/ending at the same position
3239 }
3240
3241
3242 bool
isTrafficLight(SumoXMLNodeType type)3243 NBNode::isTrafficLight(SumoXMLNodeType type) {
3244 return type == NODETYPE_TRAFFIC_LIGHT
3245 || type == NODETYPE_TRAFFIC_LIGHT_NOJUNCTION
3246 || type == NODETYPE_TRAFFIC_LIGHT_RIGHT_ON_RED;
3247 }
3248
3249
3250 bool
rightOnRedConflict(int index,int foeIndex) const3251 NBNode::rightOnRedConflict(int index, int foeIndex) const {
3252 if (myType == NODETYPE_TRAFFIC_LIGHT_RIGHT_ON_RED) {
3253 for (std::set<NBTrafficLightDefinition*>::const_iterator i = myTrafficLights.begin(); i != myTrafficLights.end(); ++i) {
3254 if ((*i)->rightOnRedConflict(index, foeIndex)) {
3255 return true;
3256 }
3257 }
3258 }
3259 return false;
3260 }
3261
3262
3263 void
sortEdges(bool useNodeShape)3264 NBNode::sortEdges(bool useNodeShape) {
3265 if (myAllEdges.size() == 0) {
3266 return;
3267 }
3268 EdgeVector allEdgesOriginal = myAllEdges;
3269 EdgeVector& allEdges = myAllEdges;
3270 EdgeVector& incoming = myIncomingEdges;
3271 EdgeVector& outgoing = myOutgoingEdges;
3272
3273 // sort the edges by angle (this is the canonical sorting)
3274 std::sort(allEdges.begin(), allEdges.end(), NBNodesEdgesSorter::edge_by_junction_angle_sorter(this));
3275 std::sort(incoming.begin(), incoming.end(), NBNodesEdgesSorter::edge_by_junction_angle_sorter(this));
3276 std::sort(outgoing.begin(), outgoing.end(), NBNodesEdgesSorter::edge_by_junction_angle_sorter(this));
3277 std::vector<NBEdge*>::iterator j;
3278 for (j = allEdges.begin(); j != allEdges.end() - 1 && j != allEdges.end(); ++j) {
3279 NBNodesEdgesSorter::swapWhenReversed(this, j, j + 1);
3280 }
3281 if (allEdges.size() > 1 && j != allEdges.end()) {
3282 NBNodesEdgesSorter::swapWhenReversed(this, allEdges.end() - 1, allEdges.begin());
3283 }
3284
3285 // sort again using additional geometry information
3286 NBEdge* firstOfAll = allEdges.front();
3287 NBEdge* firstOfIncoming = incoming.size() > 0 ? incoming.front() : 0;
3288 NBEdge* firstOfOutgoing = outgoing.size() > 0 ? outgoing.front() : 0;
3289 // sort by the angle between the node shape center and the point where the edge meets the node shape
3290 sort(allEdges.begin(), allEdges.end(), NBContHelper::edge_by_angle_to_nodeShapeCentroid_sorter(this));
3291 sort(incoming.begin(), incoming.end(), NBContHelper::edge_by_angle_to_nodeShapeCentroid_sorter(this));
3292 sort(outgoing.begin(), outgoing.end(), NBContHelper::edge_by_angle_to_nodeShapeCentroid_sorter(this));
3293 // let the first edge remain the first
3294 rotate(allEdges.begin(), std::find(allEdges.begin(), allEdges.end(), firstOfAll), allEdges.end());
3295 if (firstOfIncoming != nullptr) {
3296 rotate(incoming.begin(), std::find(incoming.begin(), incoming.end(), firstOfIncoming), incoming.end());
3297 }
3298 if (firstOfOutgoing != nullptr) {
3299 rotate(outgoing.begin(), std::find(outgoing.begin(), outgoing.end(), firstOfOutgoing), outgoing.end());
3300 }
3301 #ifdef DEBUG_EDGE_SORTING
3302 if (DEBUGCOND) {
3303 std::cout << "sortedEdges:\n";
3304 for (NBEdge* e : allEdges) {
3305 std::cout << " " << e->getID()
3306 << " angleToCenter=" << e->getAngleAtNodeToCenter(this)
3307 << " junctionAngle=" << e->getAngleAtNode(this) << "\n";
3308 }
3309 }
3310 #endif
3311
3312 // fixing some pathological all edges orderings
3313 // if every of the edges a,b,c has a turning edge a',b',c' the all edges ordering should be a,a',b,b',c,c'
3314 if (incoming.size() == outgoing.size() && incoming.front() == allEdges.front()) {
3315 std::vector<NBEdge*>::const_iterator in, out;
3316 std::vector<NBEdge*> allTmp;
3317 for (in = incoming.begin(), out = outgoing.begin(); in != incoming.end(); ++in, ++out) {
3318 if ((*in)->isTurningDirectionAt(*out)) {
3319 allTmp.push_back(*in);
3320 allTmp.push_back(*out);
3321 } else {
3322 break;
3323 }
3324 }
3325 if (allTmp.size() == allEdges.size()) {
3326 allEdges = allTmp;
3327 }
3328 }
3329 // sort the crossings
3330 std::sort(myCrossings.begin(), myCrossings.end(), NBNodesEdgesSorter::crossing_by_junction_angle_sorter(this, allEdges));
3331 //if (crossings.size() > 0) {
3332 // std::cout << " crossings at " << getID() << "\n";
3333 // for (std::vector<NBNode::Crossing*>::iterator it = crossings.begin(); it != crossings.end(); ++it) {
3334 // std::cout << " " << toString((*it)->edges) << "\n";
3335 // }
3336 //}
3337
3338 if (useNodeShape && myAllEdges != allEdgesOriginal) {
3339 // sorting order changed after node shape was computed.
3340 computeNodeShape(-1);
3341 for (NBEdge* e : myAllEdges) {
3342 e->computeEdgeShape();
3343 }
3344 }
3345 }
3346
3347 std::vector<std::pair<Position, std::string> >
getEndPoints() const3348 NBNode::getEndPoints() const {
3349 // using a set would be nicer but we want to have some slack in position identification
3350 std::vector<std::pair<Position, std::string> >result;
3351 for (NBEdge* e : myAllEdges) {
3352 Position pos = this == e->getFromNode() ? e->getGeometry().front() : e->getGeometry().back();
3353 const std::string origID = e->getParameter(this == e->getFromNode() ? "origFrom" : "origTo");
3354 bool unique = true;
3355 for (const auto& pair : result) {
3356 if (pos.almostSame(pair.first) || (origID != "" && pair.second == origID)) {
3357 unique = false;
3358 break;
3359 }
3360 }
3361 if (unique) {
3362 result.push_back(std::make_pair(pos, origID));
3363 }
3364 }
3365 return result;
3366 }
3367
3368 /****************************************************************************/
3369
3370