Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2025 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file NBNodeShapeComputer.cpp
15 : /// @author Daniel Krajzewicz
16 : /// @author Jakob Erdmann
17 : /// @author Michael Behrisch
18 : /// @date Sept 2002
19 : ///
20 : // This class computes shapes of junctions
21 : /****************************************************************************/
22 : #include <config.h>
23 :
24 : #include <algorithm>
25 : #include <iterator>
26 : #include <utils/geom/PositionVector.h>
27 : #include <utils/options/OptionsCont.h>
28 : #include <utils/geom/GeomHelper.h>
29 : #include <utils/common/StdDefs.h>
30 : #include <utils/common/MsgHandler.h>
31 : #include <utils/common/UtilExceptions.h>
32 : #include <utils/common/ToString.h>
33 : #include <utils/iodevices/OutputDevice.h>
34 : #include "NBNode.h"
35 : #include "NBAlgorithms.h"
36 : #include "NBNodeShapeComputer.h"
37 :
38 : //#define DEBUG_NODE_SHAPE
39 : //#define DEBUG_SMOOTH_CORNERS
40 : //#define DEBUG_RADIUS
41 : #define DEBUGCOND (myNode.getID() == "C")
42 :
43 :
44 : #define EXT2 10.0
45 :
46 : // foot and bicycle paths as well as pure service roads should not get large junctions
47 : // railways also do have have junctions with sharp turns so can be excluded
48 : const SVCPermissions NBNodeShapeComputer::SVC_LARGE_TURN(
49 : SVCAll & ~(SVC_BICYCLE | SVC_PEDESTRIAN | SVC_DELIVERY | SVC_RAIL_CLASSES));
50 :
51 : // ===========================================================================
52 : // method definitions
53 : // ===========================================================================
54 78199 : NBNodeShapeComputer::NBNodeShapeComputer(const NBNode& node) :
55 78199 : myNode(node),
56 78199 : myRadius(node.getRadius()) {
57 78199 : if (node.getEdges().size() > 4 && !NBNodeTypeComputer::isRailwayNode(&node)) {
58 19547 : EXT = 50;
59 : } else {
60 58652 : EXT = 100;
61 : }
62 78199 : }
63 :
64 :
65 78199 : NBNodeShapeComputer::~NBNodeShapeComputer() {}
66 :
67 :
68 : const PositionVector
69 78199 : NBNodeShapeComputer::compute(bool forceSmall) {
70 : #ifdef DEBUG_NODE_SHAPE
71 : if (DEBUGCOND) {
72 : // annotate edges edges to make their ordering visible
73 : int i = 0;
74 : for (NBEdge* e : myNode.getEdges()) {
75 : e->setStreetName(toString(i));
76 : i++;
77 : }
78 : }
79 : #endif
80 : // check whether the node is a dead end node or a node where only turning is possible
81 : // in this case, we will use "computeNodeShapeSmall"
82 78199 : if (myNode.getEdges().size() == 1 || forceSmall) {
83 9940 : return computeNodeShapeSmall();
84 : }
85 68259 : if (myNode.getEdges().size() == 2 && myNode.getIncomingEdges().size() == 1) {
86 21827 : if (myNode.getIncomingEdges()[0]->isTurningDirectionAt(myNode.getOutgoingEdges()[0])) {
87 10971 : return computeNodeShapeSmall();
88 : }
89 : }
90 57288 : const bool geometryLike = myNode.isSimpleContinuation(true, true);
91 57288 : const PositionVector& ret = computeNodeShapeDefault(geometryLike);
92 : // fail fall-back: use "computeNodeShapeSmall"
93 57288 : if (ret.size() < 3) {
94 83 : return computeNodeShapeSmall();
95 : }
96 : return ret;
97 57288 : }
98 :
99 :
100 : void
101 75552 : NBNodeShapeComputer::computeSameEnd(PositionVector& l1, PositionVector& l2) {
102 : assert(l1[0].distanceTo2D(l1[1]) >= EXT);
103 : assert(l2[0].distanceTo2D(l2[1]) >= EXT);
104 75552 : PositionVector tmp;
105 75552 : tmp.push_back(PositionVector::positionAtOffset2D(l1[0], l1[1], EXT));
106 75552 : tmp.push_back(l1[1]);
107 75552 : tmp[1].sub(tmp[0]);
108 75552 : tmp[1].set(-tmp[1].y(), tmp[1].x());
109 75552 : tmp[1].add(tmp[0]);
110 75552 : tmp.extrapolate2D(EXT);
111 75552 : if (l2.intersects(tmp[0], tmp[1])) {
112 75489 : const double offset = l2.intersectsAtLengths2D(tmp)[0];
113 75489 : if (l2.length2D() - offset > POSITION_EPS) {
114 75489 : PositionVector tl2 = l2.getSubpart2D(offset, l2.length2D());
115 75489 : tl2.extrapolate2D(EXT);
116 75489 : l2.erase(l2.begin(), l2.begin() + (l2.size() - tl2.size()));
117 75489 : l2[0] = tl2[0];
118 75489 : }
119 : }
120 75552 : }
121 :
122 :
123 : const PositionVector
124 57288 : NBNodeShapeComputer::computeNodeShapeDefault(bool simpleContinuation) {
125 : // if we have less than two edges, we can not compute the node's shape this way
126 57288 : if (myNode.getEdges().size() < 2) {
127 0 : return PositionVector();
128 : }
129 : // magic values
130 57288 : const OptionsCont& oc = OptionsCont::getOptions();
131 57288 : const double defaultRadius = getDefaultRadius(oc);
132 57288 : const bool useDefaultRadius = myNode.getRadius() == NBNode::UNSPECIFIED_RADIUS || myNode.getRadius() == defaultRadius;
133 57288 : myRadius = (useDefaultRadius ? defaultRadius : myNode.getRadius());
134 57288 : double smallRadius = useDefaultRadius ? oc.getFloat("junctions.small-radius") : myRadius;
135 57288 : const int cornerDetail = oc.getInt("junctions.corner-detail");
136 57288 : const double sCurveStretch = oc.getFloat("junctions.scurve-stretch");
137 57288 : const bool useEndpoints = oc.getBool("junctions.endpoint-shape");
138 57288 : const bool rectangularCut = oc.getBool("rectangular-lane-cut");
139 57288 : const bool openDriveOutput = oc.isSet("opendrive-output");
140 :
141 : // Extend geometries to move the stop line forward.
142 : // In OpenDrive the junction starts whenever the geometry changes. Stop
143 : // line information is not given or ambiguous (sign positions at most)
144 : // In SUMO, stop lines are where the junction starts. This is computed
145 : // heuristically from intersecting the junctions roads geometries.
146 109903 : const double advanceStopLine = oc.exists("opendrive-files") && oc.isSet("opendrive-files") ? oc.getFloat("opendrive.advance-stopline") : 0;
147 :
148 :
149 : #ifdef DEBUG_NODE_SHAPE
150 : if (DEBUGCOND) {
151 : std::cout << "\ncomputeNodeShapeDefault node " << myNode.getID() << " simple=" << simpleContinuation << " useDefaultRadius=" << useDefaultRadius << " radius=" << myRadius << "\n";
152 : }
153 : #endif
154 :
155 : // initialise
156 57288 : EdgeVector::const_iterator i;
157 : // edges located in the value-vector have the same direction as the key edge
158 : std::map<NBEdge*, std::set<NBEdge*, ComparatorIdLess> > same;
159 : // the counter-clockwise boundary of the edge regarding possible same-direction edges
160 : GeomsMap geomsCCW;
161 : // the clockwise boundary of the edge regarding possible same-direction edges
162 : GeomsMap geomsCW;
163 57288 : EdgeVector usedEdges = myNode.getEdges();
164 57288 : computeEdgeBoundaries(usedEdges, geomsCCW, geomsCW);
165 :
166 : // check which edges are parallel
167 57288 : joinSameDirectionEdges(usedEdges, same, useEndpoints);
168 : // compute unique direction list
169 57288 : EdgeVector newAll = computeUniqueDirectionList(usedEdges, same, geomsCCW, geomsCW);
170 : // if we have only two "directions", let's not compute the geometry using this method
171 57288 : if (newAll.size() < 2) {
172 83 : return PositionVector();
173 : }
174 :
175 : // All geoms are outgoing from myNode.
176 : // for every direction in newAll we compute the offset at which the
177 : // intersection ends and the edge starts. This value is saved in 'distances'
178 : // If the geometries need to be extended to get an intersection, this is
179 : // recorded in 'myExtended'
180 : std::map<NBEdge*, double> distances;
181 : std::map<NBEdge*, double> distances2;
182 : std::map<NBEdge*, bool> myExtended;
183 :
184 221739 : for (i = newAll.begin(); i != newAll.end(); ++i) {
185 164534 : EdgeVector::const_iterator cwi = i;
186 164534 : EdgeVector::const_iterator ccwi = i;
187 : double ccad;
188 : double cad;
189 164534 : initNeighbors(newAll, i, geomsCW, geomsCCW, cwi, ccwi, cad, ccad);
190 : assert(geomsCCW.find(*i) != geomsCCW.end());
191 : assert(geomsCW.find(*ccwi) != geomsCW.end());
192 : assert(geomsCW.find(*cwi) != geomsCW.end());
193 :
194 : // there are only 2 directions and they are almost parallel
195 164534 : if (*cwi == *ccwi &&
196 : (
197 : // no change in lane numbers, even low angles still give a good intersection
198 28990 : (simpleContinuation && fabs(ccad - cad) < (double) 0.1)
199 : // lane numbers change, a direct intersection could be far away from the node position
200 : // so we use a larger threshold
201 12210 : || (!simpleContinuation && fabs(ccad - cad) < DEG2RAD(22.5)))
202 : ) {
203 : // compute the mean position between both edges ends ...
204 : Position p;
205 27634 : if (myExtended.find(*ccwi) != myExtended.end()) {
206 0 : p = geomsCCW[*ccwi][0];
207 0 : p.add(geomsCW[*ccwi][0]);
208 : p.mul(0.5);
209 : #ifdef DEBUG_NODE_SHAPE
210 : if (DEBUGCOND) {
211 : std::cout << " extended: p=" << p << " angle=" << (ccad - cad) << "\n";
212 : }
213 : #endif
214 : } else {
215 27634 : p = geomsCCW[*ccwi][0];
216 27634 : p.add(geomsCW[*ccwi][0]);
217 27634 : p.add(geomsCCW[*i][0]);
218 27634 : p.add(geomsCW[*i][0]);
219 : p.mul(0.25);
220 : #ifdef DEBUG_NODE_SHAPE
221 : if (DEBUGCOND) {
222 : std::cout << " unextended: p=" << p << " angle=" << (ccad - cad) << "\n";
223 : }
224 : #endif
225 : }
226 : // ... compute the distance to this point ...
227 55268 : double dist = MAX2(
228 27634 : geomsCCW[*i].nearest_offset_to_point2D(p),
229 27634 : geomsCW[*i].nearest_offset_to_point2D(p));
230 27634 : if (dist < 0) {
231 0 : if (isRailway((*i)->getPermissions())) {
232 : // better not mess up bidi geometries
233 0 : return PositionVector();
234 : }
235 : // ok, we have the problem that even the extrapolated geometry
236 : // does not reach the point
237 : // in this case, the geometry has to be extenden... too bad ...
238 : // ... let's append the mean position to the geometry
239 0 : PositionVector g = (*i)->getGeometry();
240 0 : if (myNode.hasIncoming(*i)) {
241 0 : g.push_back_noDoublePos(p);
242 : } else {
243 0 : g.push_front_noDoublePos(p);
244 : }
245 0 : (*i)->setGeometry(g);
246 : // and rebuild previous information
247 0 : geomsCCW[*i] = (*i)->getCCWBoundaryLine(myNode);
248 0 : geomsCCW[*i].extrapolate(EXT);
249 0 : geomsCW[*i] = (*i)->getCWBoundaryLine(myNode);
250 0 : geomsCW[*i].extrapolate(EXT);
251 : // the distance is now = zero (the point we have appended)
252 0 : distances[*i] = EXT;
253 0 : myExtended[*i] = true;
254 : #ifdef DEBUG_NODE_SHAPE
255 : if (DEBUGCOND) {
256 : std::cout << " extending (dist=" << dist << ")\n";
257 : }
258 : #endif
259 0 : } else {
260 27634 : if (!simpleContinuation) {
261 9484 : dist += myRadius;
262 : } else {
263 : // if the angles change, junction should have some size to avoid degenerate shape
264 18150 : double radius2 = fabs(ccad - cad) * (*i)->getNumLanes();
265 18150 : if (radius2 > NUMERICAL_EPS || openDriveOutput) {
266 : radius2 = MAX2(0.15, radius2);
267 : }
268 18150 : if (myNode.getCrossings().size() > 0) {
269 50 : double width = myNode.getCrossings()[0]->customWidth;
270 50 : if (width == NBEdge::UNSPECIFIED_WIDTH) {
271 56 : width = OptionsCont::getOptions().getFloat("default.crossing-width");
272 : }
273 50 : radius2 = MAX2(radius2, width / 2);
274 : }
275 18150 : if (!useDefaultRadius) {
276 4 : radius2 = MAX2(radius2, myRadius);
277 : }
278 18150 : dist += radius2;
279 : #ifdef DEBUG_NODE_SHAPE
280 : if (DEBUGCOND) {
281 : std::cout << " using radius=" << radius2 << " ccad=" << ccad << " cad=" << cad << "\n";
282 : }
283 : #endif
284 : }
285 27634 : distances[*i] = dist;
286 : }
287 :
288 : } else {
289 : // the angles are different enough to compute the intersection of
290 : // the outer boundaries directly (or there are more than 2 directions). The "nearer" neighbor causes the furthest distance
291 136900 : const bool ccwCloser = ccad < cad;
292 136900 : const bool cwLargeTurn = needsLargeTurn(*i, *cwi, same);
293 136900 : const bool ccwLargeTurn = needsLargeTurn(*i, *ccwi, same);
294 136900 : const bool neighLargeTurn = ccwCloser ? ccwLargeTurn : cwLargeTurn;
295 136900 : const bool neigh2LargeTurn = ccwCloser ? cwLargeTurn : ccwLargeTurn;
296 : // the border facing the closer neighbor
297 136900 : const PositionVector& currGeom = ccwCloser ? geomsCCW[*i] : geomsCW[*i];
298 : // the border facing the far neighbor
299 136900 : const PositionVector& currGeom2 = ccwCloser ? geomsCW[*i] : geomsCCW[*i];
300 : // the border of the closer neighbor
301 136900 : const PositionVector& neighGeom = ccwCloser ? geomsCW[*ccwi] : geomsCCW[*cwi];
302 : // the border of the far neighbor
303 136900 : const PositionVector& neighGeom2 = ccwCloser ? geomsCCW[*cwi] : geomsCW[*ccwi];
304 : // whether the current edge/direction spans a divided road
305 273800 : const bool keepBothDistances = isDivided(*i, same[*i], geomsCCW[*i], geomsCW[*i]);
306 : #ifdef DEBUG_NODE_SHAPE
307 : if (DEBUGCOND) {
308 : std::cout << " i=" << (*i)->getID() << " neigh=" << (*ccwi)->getID() << " neigh2=" << (*cwi)->getID() << "\n";
309 : std::cout << " ccwCloser=" << ccwCloser << " divided=" << keepBothDistances
310 : << "\n currGeom=" << currGeom << " neighGeom=" << neighGeom
311 : << "\n currGeom2=" << currGeom2 << " neighGeom2=" << neighGeom2
312 : << "\n";
313 : }
314 : #endif
315 136900 : if (!simpleContinuation) {
316 125994 : if (useEndpoints && !(*i)->hasDefaultGeometryEndpointAtNode(&myNode)) {
317 16 : distances[*i] = EXT;
318 125978 : } else if (currGeom.intersects(neighGeom)) {
319 125425 : distances[*i] = (neighLargeTurn ? myRadius : smallRadius) + closestIntersection(currGeom, neighGeom, EXT);
320 : #ifdef DEBUG_NODE_SHAPE
321 : if (DEBUGCOND) {
322 : std::cout << " neigh intersects dist=" << distances[*i] << " currGeom=" << currGeom << " neighGeom=" << neighGeom << "\n";
323 : }
324 : #endif
325 125425 : if (*cwi != *ccwi && currGeom2.intersects(neighGeom2)) {
326 : // also use the second intersection point
327 : // but prevent very large node shapes
328 120062 : const double farAngleDist = ccwCloser ? cad : ccad;
329 120062 : double a1 = distances[*i];
330 120062 : double a2 = (neigh2LargeTurn ? myRadius : smallRadius) + closestIntersection(currGeom2, neighGeom2, EXT);
331 : #ifdef DEBUG_NODE_SHAPE
332 : if (DEBUGCOND) {
333 : std::cout << " neigh2 also intersects a1=" << a1 << " a2=" << a2 << " ccad=" << RAD2DEG(ccad) << " cad=" << RAD2DEG(cad) << " dist[cwi]=" << distances[*cwi] << " dist[ccwi]=" << distances[*ccwi] << " farAngleDist=" << RAD2DEG(farAngleDist) << " currGeom2=" << currGeom2 << " neighGeom2=" << neighGeom2 << "\n";
334 : }
335 : #endif
336 : //if (RAD2DEG(farAngleDist) < 175) {
337 : // distances[*i] = MAX2(a1, MIN2(a2, a1 + 180 - RAD2DEG(farAngleDist)));
338 : //}
339 120062 : if (a2 <= EXT) {
340 4662 : if (keepBothDistances) {
341 51 : if (ccwCloser) {
342 14 : distances2[*i] = a2;
343 : } else {
344 37 : distances[*i] = a2;
345 37 : distances2[*i] = a1;
346 : }
347 : } else {
348 4611 : distances[*i] = MAX2(a1, a2);
349 : }
350 28506 : } else if (ccad > DEG2RAD(90. + 45.) && cad > DEG2RAD(90. + 45.)
351 119426 : && (fabs(ccad - cad) > DEG2RAD(10)
352 1174 : || MAX2(ccad, cad) > DEG2RAD(160)
353 176 : || (a2 - a1) > 7
354 : // keep roundabouts nodes small
355 169 : || myNode.isRoundabout())) {
356 : #ifdef DEBUG_NODE_SHAPE
357 : if (DEBUGCOND) {
358 : std::cout << " ignore a2\n";
359 : }
360 : #endif
361 : // do nothing.
362 111533 : } else if (farAngleDist < DEG2RAD(135) || (fabs(RAD2DEG(farAngleDist) - 180) > 1 && fabs(a2 - a1) < 10)) {
363 87377 : if (keepBothDistances) {
364 262 : if (ccwCloser) {
365 129 : distances2[*i] = a2;
366 : } else {
367 133 : distances[*i] = a2;
368 133 : distances2[*i] = a1;
369 : }
370 : } else {
371 87115 : distances[*i] = MAX2(a1, a2);
372 : }
373 : }
374 : #ifdef DEBUG_NODE_SHAPE
375 : if (DEBUGCOND) {
376 : std::cout << " a1=" << a1 << " a2=" << a2 << " keepBoth=" << keepBothDistances << " dist=" << distances[*i] << "\n";
377 : }
378 : #endif
379 : }
380 : } else {
381 553 : if (*cwi != *ccwi && currGeom2.intersects(neighGeom2)) {
382 485 : distances[*i] = (neigh2LargeTurn ? myRadius : smallRadius) + currGeom2.intersectsAtLengths2D(neighGeom2)[0];
383 : #ifdef DEBUG_NODE_SHAPE
384 : if (DEBUGCOND) {
385 : std::cout << " neigh2 intersects dist=" << distances[*i] << " currGeom2=" << currGeom2 << " neighGeom2=" << neighGeom2 << "\n";
386 : }
387 : #endif
388 : } else {
389 68 : distances[*i] = EXT + myRadius;
390 : #ifdef DEBUG_NODE_SHAPE
391 : if (DEBUGCOND) {
392 : std::cout << " no intersects dist=" << distances[*i] << " currGeom=" << currGeom << " neighGeom=" << neighGeom << " currGeom2=" << currGeom2 << " neighGeom2=" << neighGeom2 << "\n";
393 : }
394 : #endif
395 : }
396 : }
397 : } else {
398 10906 : if (currGeom.intersects(neighGeom)) {
399 10878 : distances[*i] = currGeom.intersectsAtLengths2D(neighGeom)[0];
400 : } else {
401 28 : distances[*i] = (double) EXT;
402 : }
403 : }
404 : }
405 164534 : if (useDefaultRadius && sCurveStretch > 0) {
406 32 : double sCurveWidth = myNode.getDisplacementError();
407 32 : if (sCurveWidth > 0) {
408 2 : const double sCurveRadius = myRadius + sCurveWidth / SUMO_const_laneWidth * sCurveStretch * pow((*i)->getSpeed(), 2 + sCurveStretch) / 1000;
409 2 : const double stretch = EXT + sCurveRadius - distances[*i];
410 2 : if (stretch > 0) {
411 2 : distances[*i] += stretch;
412 : // fixate extended geometry for repeated computation
413 2 : const double shorten = distances[*i] - EXT;
414 2 : (*i)->shortenGeometryAtNode(&myNode, shorten);
415 2 : for (std::set<NBEdge*>::iterator k = same[*i].begin(); k != same[*i].end(); ++k) {
416 0 : (*k)->shortenGeometryAtNode(&myNode, shorten);
417 : }
418 : #ifdef DEBUG_NODE_SHAPE
419 : if (DEBUGCOND) {
420 : std::cout << " stretching junction: sCurveWidth=" << sCurveWidth << " sCurveRadius=" << sCurveRadius << " stretch=" << stretch << " dist=" << distances[*i] << "\n";
421 : }
422 : #endif
423 : }
424 : }
425 : }
426 : }
427 :
428 221739 : for (NBEdge* const edge : newAll) {
429 164534 : if (distances.find(edge) == distances.end()) {
430 : assert(false);
431 0 : distances[edge] = EXT;
432 : }
433 : }
434 : // because of lane spread right the crossing point may be identical to the junction center and thus the distance is exactly EXT
435 57205 : const double off = EXT - NUMERICAL_EPS;
436 : // prevent inverted node shapes
437 : // (may happen with near-parallel edges)
438 57205 : const double minDistSum = 2 * (EXT + myRadius);
439 221739 : for (NBEdge* const edge : newAll) {
440 164534 : if (distances[edge] < off && edge->hasDefaultGeometryEndpointAtNode(&myNode)) {
441 7696 : for (EdgeVector::const_iterator j = newAll.begin(); j != newAll.end(); ++j) {
442 5756 : if (distances[*j] > off && (*j)->hasDefaultGeometryEndpointAtNode(&myNode) && distances[edge] + distances[*j] < minDistSum) {
443 395 : const double angleDiff = fabs(NBHelpers::relAngle(edge->getAngleAtNode(&myNode), (*j)->getAngleAtNode(&myNode)));
444 395 : if (angleDiff > 160 || angleDiff < 20) {
445 : #ifdef DEBUG_NODE_SHAPE
446 : if (DEBUGCOND) {
447 : std::cout << " increasing dist for i=" << edge->getID() << " because of j=" << (*j)->getID() << " jDist=" << distances[*j]
448 : << " oldI=" << distances[edge] << " newI=" << minDistSum - distances[*j]
449 : << " angleDiff=" << angleDiff
450 : << " geomI=" << edge->getGeometry() << " geomJ=" << (*j)->getGeometry() << "\n";
451 : }
452 : #endif
453 166 : distances[edge] = minDistSum - distances[*j];
454 : }
455 : }
456 : }
457 : }
458 : }
459 :
460 :
461 : // build
462 57205 : PositionVector ret;
463 221739 : for (i = newAll.begin(); i != newAll.end(); ++i) {
464 164534 : const PositionVector& ccwBound = geomsCCW[*i];
465 164534 : const PositionVector& cwBound = geomsCW[*i];
466 : //double offset = MIN3(distances[*i], cwBound.length2D() - POSITION_EPS, ccwBound.length2D() - POSITION_EPS);
467 164534 : double offset = distances[*i];
468 313 : double offset2 = distances2.count(*i) != 0 ? distances2[*i] : offset;
469 164534 : if (offset != offset2) {
470 : // keep rectangular cuts if the difference is small or the roads aren't
471 : // really divided by much (unless the angle is very different)
472 626 : const double dWidth = divisionWidth(*i, same[*i],
473 313 : ccwBound.positionAtOffset2D(offset),
474 313 : cwBound.positionAtOffset2D(offset2));
475 313 : const double angle = RAD2DEG(GeomHelper::angleDiff(ccwBound.angleAt2D(0), cwBound.angleAt2D(0)));
476 313 : const double oDelta = fabs(offset - offset2);
477 : //std::cout << " i=" << (*i)->getID() << " offset=" << offset << " offset2=" << offset2 << " dWidth=" << dWidth << " angle=" << angle << " same=" << joinNamedToStringSorting(same[*i], ",") << "\n";
478 313 : if ((((oDelta < 5 || dWidth < 10) && fabs(angle) < 30)) || (fabs(angle) < 5 && myNode.getType() != SumoXMLNodeType::RAIL_CROSSING)) {
479 : #ifdef DEBUG_NODE_SHAPE
480 : std::cout << " i=" << (*i)->getID() << " offset=" << offset << " offset2=" << offset2 << " dWidth=" << dWidth << " angle=" << angle << " same=" << joinNamedToStringSorting(same[*i], ",") << "\n";
481 : #endif
482 : offset = MAX2(offset, offset2);
483 : offset2 = offset;
484 : }
485 : }
486 164534 : if (!(*i)->hasDefaultGeometryEndpointAtNode(&myNode)) {
487 : // for non geometry-endpoints, only shorten but never extend the geometry
488 16378 : if (advanceStopLine > 0 && offset < EXT) {
489 : #ifdef DEBUG_NODE_SHAPE
490 : std::cout << " i=" << (*i)->getID() << " offset=" << offset << " advanceStopLine=" << advanceStopLine << "\n";
491 : #endif
492 : // fixate extended geometry for repeated computation
493 0 : (*i)->extendGeometryAtNode(&myNode, advanceStopLine);
494 0 : for (std::set<NBEdge*>::iterator k = same[*i].begin(); k != same[*i].end(); ++k) {
495 0 : (*k)->extendGeometryAtNode(&myNode, advanceStopLine);
496 : }
497 : }
498 16378 : offset = MAX2(EXT - advanceStopLine, offset);
499 : offset2 = MAX2(EXT - advanceStopLine, offset2);
500 : }
501 164534 : if (offset == -1) {
502 0 : WRITE_WARNINGF(TL("Fixing offset for edge '%' at node '%."), (*i)->getID(), myNode.getID());
503 : offset = -.1;
504 : offset2 = -.1;
505 : }
506 164534 : Position p = ccwBound.positionAtOffset2D(offset);
507 164534 : p.setz(myNode.getPosition().z());
508 164534 : if (i != newAll.begin()) {
509 321987 : ret.append(getSmoothCorner(geomsCW[*(i - 1)], ccwBound, ret[-1], p, cornerDetail));
510 : }
511 164534 : Position p2 = cwBound.positionAtOffset2D(offset2);
512 164534 : p2.setz(myNode.getPosition().z());
513 : //ret.append(getEdgeCuts(*i, geomsCCW, geomsCW, offset, offset2, same));
514 164534 : ret.push_back_noDoublePos(p);
515 164534 : ret.push_back_noDoublePos(p2);
516 : #ifdef DEBUG_NODE_SHAPE
517 : if (DEBUGCOND) {
518 : std::cout << " build stopLine for i=" << (*i)->getID() << " offset=" << offset << " offset2=" << offset2 << " dist=" << distances[*i] << " cwLength=" << cwBound.length2D() << " ccwLength=" << ccwBound.length2D() << " p=" << p << " p2=" << p2 << " ccwBound=" << ccwBound << " cwBound=" << cwBound << "\n";
519 : }
520 : #endif
521 164534 : (*i)->setNodeBorder(&myNode, p, p2, rectangularCut);
522 242012 : for (std::set<NBEdge*>::iterator k = same[*i].begin(); k != same[*i].end(); ++k) {
523 77478 : (*k)->setNodeBorder(&myNode, p, p2, rectangularCut);
524 : }
525 : }
526 : // final curve segment
527 171615 : ret.append(getSmoothCorner(geomsCW[*(newAll.end() - 1)], geomsCCW[*newAll.begin()], ret[-1], ret[0], cornerDetail));
528 : #ifdef DEBUG_NODE_SHAPE
529 : if (DEBUGCOND) {
530 : std::cout << " final shape=" << ret << "\n";
531 : }
532 : #endif
533 : return ret;
534 114493 : }
535 :
536 :
537 : double
538 245487 : NBNodeShapeComputer::closestIntersection(const PositionVector& geom1, const PositionVector& geom2, double offset) {
539 245487 : std::vector<double> intersections = geom1.intersectsAtLengths2D(geom2);
540 245487 : double result = intersections[0];
541 260254 : for (std::vector<double>::iterator it = intersections.begin() + 1; it != intersections.end(); ++it) {
542 14767 : if (fabs(*it - offset) < fabs(result - offset)) {
543 : result = *it;
544 : }
545 : }
546 245487 : return result;
547 245487 : }
548 :
549 : bool
550 273800 : NBNodeShapeComputer::needsLargeTurn(NBEdge* e1, NBEdge* e2,
551 : std::map<NBEdge*, std::set<NBEdge*, ComparatorIdLess> >& same) const {
552 273800 : const SVCPermissions p1 = e1->getPermissions();
553 273800 : const SVCPermissions p2 = e2->getPermissions();
554 273800 : if ((p1 & p2 & SVC_LARGE_TURN) != 0) {
555 : // note: would could also check whether there is actually a connection
556 : // between those edges
557 : return true;
558 : }
559 : // maybe edges in the same direction need a large turn
560 167607 : for (NBEdge* e2s : same[e2]) {
561 43648 : if ((p1 & e2s->getPermissions() & SVC_LARGE_TURN) != 0
562 43648 : && (e1->getToNode() == e2s->getFromNode() || e2s->getToNode() == e1->getFromNode())) {
563 : return true;
564 : }
565 74424 : for (NBEdge* e1s : same[e1]) {
566 31775 : if ((e2s->getPermissions() & e1s->getPermissions() & SVC_LARGE_TURN) != 0
567 31775 : && (e2s->getToNode() == e1s->getFromNode() || e1s->getToNode() == e2s->getFromNode())) {
568 : return true;
569 : }
570 : }
571 : }
572 166065 : for (NBEdge* e1s : same[e1]) {
573 42571 : if ((p2 & e1s->getPermissions() & SVC_LARGE_TURN) != 0
574 42571 : && (e2->getToNode() == e1s->getFromNode() || e1s->getToNode() == e2->getFromNode())) {
575 : return true;
576 : }
577 : }
578 : //std::cout << " e1=" << e1->getID() << " e2=" << e2->getID() << " sameE1=" << toString(same[e1]) << " sameE2=" << toString(same[e2]) << "\n";
579 : return false;
580 : }
581 :
582 : PositionVector
583 164534 : NBNodeShapeComputer::getSmoothCorner(PositionVector begShape, PositionVector endShape,
584 : const Position& begPoint, const Position& endPoint, int cornerDetail) {
585 164534 : PositionVector ret;
586 164534 : if (cornerDetail > 0) {
587 127797 : PositionVector begShape2 = begShape.reverse().getSubpart2D(EXT2, begShape.length());
588 127797 : const double begSplit = begShape2.nearest_offset_to_point2D(begPoint, false);
589 : #ifdef DEBUG_SMOOTH_CORNERS
590 : if (DEBUGCOND) {
591 : std::cout << " begLength=" << begShape2.length2D() << " begSplit=" << begSplit << "\n";
592 : }
593 : #endif
594 127797 : if (begSplit > POSITION_EPS && begSplit < begShape2.length2D() - POSITION_EPS) {
595 118221 : begShape2 = begShape2.splitAt(begSplit, true).first;
596 : } else {
597 : return ret;
598 : }
599 118221 : PositionVector endShape2 = endShape.getSubpart(0, endShape.length() - EXT2);
600 118221 : const double endSplit = endShape2.nearest_offset_to_point2D(endPoint, false);
601 : #ifdef DEBUG_SMOOTH_CORNERS
602 : if (DEBUGCOND) {
603 : std::cout << " endLength=" << endShape2.length2D() << " endSplit=" << endSplit << "\n";
604 : }
605 : #endif
606 118221 : if (endSplit > POSITION_EPS && endSplit < endShape2.length2D() - POSITION_EPS) {
607 112986 : endShape2 = endShape2.splitAt(endSplit, true).second;
608 : } else {
609 : return ret;
610 : }
611 : // flatten z to junction z level
612 225972 : begShape2 = begShape2.interpolateZ(myNode.getPosition().z(), myNode.getPosition().z());
613 225972 : endShape2 = endShape2.interpolateZ(myNode.getPosition().z(), myNode.getPosition().z());
614 : #ifdef DEBUG_SMOOTH_CORNERS
615 : if (DEBUGCOND) {
616 : std::cout << "getSmoothCorner begPoint=" << begPoint << " endPoint=" << endPoint
617 : << " begShape=" << begShape << " endShape=" << endShape
618 : << " begShape2=" << begShape2 << " endShape2=" << endShape2
619 : << "\n";
620 : }
621 : #endif
622 112986 : if (begShape2.size() < 2 || endShape2.size() < 2) {
623 : return ret;
624 : }
625 112986 : const double angle = GeomHelper::angleDiff(begShape2.angleAt2D(-2), endShape2.angleAt2D(0));
626 : NBNode* recordError = nullptr;
627 : #ifdef DEBUG_SMOOTH_CORNERS
628 : if (DEBUGCOND) {
629 : std::cout << " angle=" << RAD2DEG(angle) << "\n";
630 : }
631 : recordError = const_cast<NBNode*>(&myNode);
632 : #endif
633 : // fill highly acute corners
634 : //if (fabs(angle) > DEG2RAD(135)) {
635 : // return ret;
636 : //}
637 112986 : PositionVector curve = myNode.computeSmoothShape(begShape2, endShape2, cornerDetail + 2, false, 25, 25, recordError, NBNode::AVOID_WIDE_LEFT_TURN);
638 : //PositionVector curve = myNode.computeSmoothShape(begShape2, endShape2, cornerDetail + 2, false, 25, 25, recordError, 0);
639 112986 : const double curvature = curve.length2D() / MAX2(NUMERICAL_EPS, begPoint.distanceTo2D(endPoint));
640 : #ifdef DEBUG_SMOOTH_CORNERS
641 : if (DEBUGCOND) {
642 : std::cout << " curve=" << curve << " curveLength=" << curve.length2D() << " dist=" << begPoint.distanceTo2D(endPoint) << " curvature=" << curvature << "\n";
643 : }
644 : #endif
645 112986 : if (curvature > 2 && angle > DEG2RAD(85)) {
646 : // simplify dubious inside corner shape
647 : return ret;
648 : }
649 111514 : if (curve.size() > 2) {
650 : curve.erase(curve.begin());
651 : curve.pop_back();
652 : ret = curve;
653 : }
654 127797 : }
655 : return ret;
656 0 : }
657 :
658 : void
659 57288 : NBNodeShapeComputer::computeEdgeBoundaries(const EdgeVector& edges,
660 : GeomsMap& geomsCCW,
661 : GeomsMap& geomsCW) {
662 : // compute boundary lines and extend it by EXT m
663 299525 : for (NBEdge* const edge : edges) {
664 : // store current edge's boundary as current ccw/cw boundary
665 : try {
666 484474 : geomsCCW[edge] = edge->getCCWBoundaryLine(myNode);
667 0 : } catch (InvalidArgument& e) {
668 0 : WRITE_WARNING("While computing intersection geometry at junction '" + myNode.getID() + "': " + std::string(e.what()));
669 0 : geomsCCW[edge] = edge->getGeometry();
670 0 : }
671 : try {
672 484474 : geomsCW[edge] = edge->getCWBoundaryLine(myNode);
673 0 : } catch (InvalidArgument& e) {
674 0 : WRITE_WARNING("While computing intersection geometry at junction '" + myNode.getID() + "': " + std::string(e.what()));
675 0 : geomsCW[edge] = edge->getGeometry();
676 0 : }
677 : // ensure the boundary is valid
678 242237 : if (geomsCCW[edge].length2D() < NUMERICAL_EPS) {
679 0 : geomsCCW[edge] = edge->getGeometry();
680 : }
681 242237 : if (geomsCW[edge].length2D() < NUMERICAL_EPS) {
682 0 : geomsCW[edge] = edge->getGeometry();
683 : }
684 : // cut off all parts beyond EXT to avoid issues with curved-back roads
685 484486 : geomsCCW[edge] = geomsCCW[edge].getSubpart2D(0, MAX2(EXT, edge->getTotalWidth()));
686 484486 : geomsCW[edge] = geomsCW[edge].getSubpart2D(0, MAX2(EXT, edge->getTotalWidth()));
687 : // extend the boundary by extrapolating it by EXT m towards the junction
688 242237 : geomsCCW[edge].extrapolate2D(EXT, true);
689 242237 : geomsCW[edge].extrapolate2D(EXT, true);
690 : // ensure minimum length by extending it away from the junction
691 242237 : geomsCCW[edge].extrapolate(EXT2, false, true);
692 242237 : geomsCW[edge].extrapolate(EXT2, false, true);
693 242237 : if (geomsCCW[edge].isNAN() || geomsCW[edge].isNAN()) {
694 0 : WRITE_WARNING("While computing intersection geometry at junction '" + myNode.getID() + "': found invalid boundary line for edge '" + edge->getID() + "'.")
695 : }
696 242237 : if (geomsCCW[edge].isNAN()) {
697 0 : geomsCCW[edge] = edge->getGeometry();
698 : }
699 242237 : if (geomsCW[edge].isNAN()) {
700 0 : geomsCW[edge] = edge->getGeometry();
701 : }
702 : }
703 57288 : }
704 :
705 : void
706 57288 : NBNodeShapeComputer::joinSameDirectionEdges(const EdgeVector& edges, std::map<NBEdge*, std::set<NBEdge*, ComparatorIdLess> >& same, bool useEndpoints) {
707 : // compute same (edges where an intersection doesn't work well
708 : // (always check an edge and its cw neighbor)
709 : const double angleChangeLookahead = 35; // distance to look ahead for a misleading angle
710 109784 : const bool isXodr = OptionsCont::getOptions().exists("opendrive-files") && OptionsCont::getOptions().isSet("opendrive-files");
711 : EdgeSet foundOpposite;
712 299525 : for (EdgeVector::const_iterator i = edges.begin(); i != edges.end(); i++) {
713 : EdgeVector::const_iterator j;
714 242237 : if (i == edges.end() - 1) {
715 : j = edges.begin();
716 : } else {
717 : j = i + 1;
718 : }
719 16 : if (useEndpoints
720 16 : && !(*i)->hasDefaultGeometryEndpointAtNode(&myNode)
721 242253 : && !(*j)->hasDefaultGeometryEndpointAtNode(&myNode)) {
722 16 : continue;
723 : }
724 242221 : const bool incoming = (*i)->getToNode() == &myNode;
725 242221 : const bool incoming2 = (*j)->getToNode() == &myNode;
726 : const bool differentDirs = (incoming != incoming2);
727 278124 : const bool sameGeom = (*i)->getGeometry() == (differentDirs ? (*j)->getGeometry().reverse() : (*j)->getGeometry());
728 242221 : const PositionVector g1 = incoming ? (*i)->getCCWBoundaryLine(myNode) : (*i)->getCWBoundaryLine(myNode);
729 242221 : const PositionVector g2 = incoming ? (*j)->getCCWBoundaryLine(myNode) : (*j)->getCWBoundaryLine(myNode);
730 242221 : const double angle1further = (g1.size() > 2 && g1[0].distanceTo2D(g1[1]) < angleChangeLookahead ?
731 242221 : g1.angleAt2D(1) : g1.angleAt2D(0));
732 242221 : const double angle2further = (g2.size() > 2 && g2[0].distanceTo2D(g2[1]) < angleChangeLookahead ?
733 242221 : g2.angleAt2D(1) : g2.angleAt2D(0));
734 242221 : const double angleDiff = GeomHelper::angleDiff(g1.angleAt2D(0), g2.angleAt2D(0));
735 242221 : const double angleDiffFurther = GeomHelper::angleDiff(angle1further, angle2further);
736 242221 : const bool ambiguousGeometry = ((angleDiff > 0 && angleDiffFurther < 0) || (angleDiff < 0 && angleDiffFurther > 0));
737 : //if (ambiguousGeometry) {
738 : // @todo: this warning would be helpful in many cases. However, if angle and angleFurther jump between 179 and -179 it is misleading
739 : // WRITE_WARNINGF(TL("Ambiguous angles at junction '%' for edges '%' and '%'."), myNode.getID(), (*i)->getID(), (*j)->getID());
740 : //}
741 : #ifdef DEBUG_NODE_SHAPE
742 : if (DEBUGCOND) {
743 : std::cout << " checkSameDirection " << (*i)->getID() << " " << (*j)->getID()
744 : << " sameGeom=" << sameGeom
745 : << " diffDirs=" << differentDirs
746 : << " isOpposite=" << (differentDirs && foundOpposite.count(*i) == 0)
747 : << " angleDiff=" << angleDiff
748 : << " ambiguousGeometry=" << ambiguousGeometry
749 : << " badInsersection=" << badIntersection(*i, *j, EXT)
750 : << "\n";
751 :
752 : }
753 : #endif
754 242221 : if (sameGeom || fabs(angleDiff) < DEG2RAD(20)) {
755 79658 : const bool isOpposite = differentDirs && foundOpposite.count(*i) == 0;
756 : if (isOpposite) {
757 : foundOpposite.insert(*i);
758 : foundOpposite.insert(*j);
759 : }
760 79658 : if (isOpposite || ambiguousGeometry || (!isXodr && badIntersection(*i, *j, EXT))) {
761 : // maintain equivalence relation for all members of the equivalence class
762 82241 : for (std::set<NBEdge*>::iterator k = same[*i].begin(); k != same[*i].end(); ++k) {
763 4563 : if (*j != *k) {
764 4505 : same[*k].insert(*j);
765 4505 : same[*j].insert(*k);
766 : }
767 : }
768 82434 : for (std::set<NBEdge*>::iterator k = same[*j].begin(); k != same[*j].end(); ++k) {
769 4756 : if (*i != *k) {
770 4698 : same[*k].insert(*i);
771 4698 : same[*i].insert(*k);
772 : }
773 : }
774 77678 : same[*i].insert(*j);
775 77678 : same[*j].insert(*i);
776 : #ifdef DEBUG_NODE_SHAPE
777 : if (DEBUGCOND) {
778 : std::cout << " joinedSameDirectionEdges " << (*i)->getID() << " " << (*j)->getID() << " isOpposite=" << isOpposite << " ambiguousGeometry=" << ambiguousGeometry << "\n";
779 : }
780 : #endif
781 : }
782 : }
783 242221 : }
784 57288 : }
785 :
786 :
787 : bool
788 4795 : NBNodeShapeComputer::badIntersection(const NBEdge* e1, const NBEdge* e2, double distance) {
789 : // check whether the two edges are on top of each other. In that case they should be joined
790 : // also, if they never touch along their common length
791 4795 : const double commonLength = MIN3(distance, e1->getGeometry().length(), e2->getGeometry().length());
792 : PositionVector geom1 = e1->getGeometry();
793 : PositionVector geom2 = e2->getGeometry();
794 : // shift to make geom the centerline of the edge regardless of spreadtype
795 4795 : if (e1->getLaneSpreadFunction() == LaneSpreadFunction::RIGHT) {
796 1012 : geom1.move2side(e1->getTotalWidth() / 2);
797 : }
798 4795 : if (e2->getLaneSpreadFunction() == LaneSpreadFunction::RIGHT) {
799 967 : geom2.move2side(e2->getTotalWidth() / 2);
800 : }
801 : // always let geometry start at myNode
802 4795 : if (e1->getToNode() == &myNode) {
803 3748 : geom1 = geom1.reverse();
804 : }
805 4795 : if (e2->getToNode() == &myNode) {
806 5146 : geom2 = geom2.reverse();
807 : }
808 9590 : geom1 = geom1.getSubpart2D(0, commonLength);
809 9590 : geom2 = geom2.getSubpart2D(0, commonLength);
810 : double endAngleDiff = 0;
811 4795 : if (geom1.size() >= 2 && geom2.size() >= 2) {
812 4795 : endAngleDiff = fabs(RAD2DEG(GeomHelper::angleDiff(
813 : geom1.angleAt2D((int)geom1.size() - 2),
814 : geom2.angleAt2D((int)geom2.size() - 2))));
815 : }
816 4795 : const double minDistanceThreshold = (e1->getTotalWidth() + e2->getTotalWidth()) / 2 + POSITION_EPS;
817 4795 : std::vector<double> distances = geom1.distances(geom2, true);
818 4795 : std::vector<double> distances2 = geom1.distances(geom2);
819 : const double minDist = VectorHelper<double>::minValue(distances2);
820 : const double maxDist = VectorHelper<double>::maxValue(distances);
821 4795 : const bool curvingTowards = geom1[0].distanceTo2D(geom2[0]) > minDistanceThreshold && minDist < minDistanceThreshold;
822 4795 : const bool onTop = (maxDist - POSITION_EPS < minDistanceThreshold) && endAngleDiff < 30;
823 4795 : const bool bothDefault = e1->hasDefaultGeometryEndpointAtNode(&myNode) && e2->hasDefaultGeometryEndpointAtNode(&myNode);
824 4795 : const bool neverTouch = minDist > minDistanceThreshold * 2 && !bothDefault;
825 4795 : geom1.extrapolate2D(EXT);
826 4795 : geom2.extrapolate2D(EXT);
827 4795 : Position intersect = geom1.intersectionPosition2D(geom2);
828 3540 : const bool intersects = intersect != Position::INVALID && geom1.distance2D(intersect) < POSITION_EPS;
829 : #ifdef DEBUG_NODE_SHAPE
830 : if (DEBUGCOND) {
831 : std::cout << " badIntersect: onTop=" << onTop << " curveTo=" << curvingTowards << " intersects=" << intersects
832 : << " endAngleDiff=" << endAngleDiff
833 : << " geom1=" << geom1 << " geom2=" << geom2
834 : << " distances=" << toString(distances) << " minDist=" << minDist << " maxDist=" << maxDist << " thresh=" << minDistanceThreshold
835 : << " neverTouch=" << neverTouch
836 : << " intersectPos=" << intersect
837 : << "\n";
838 : }
839 : #endif
840 9590 : return onTop || curvingTowards || !intersects || neverTouch;
841 4795 : }
842 :
843 :
844 : EdgeVector
845 57288 : NBNodeShapeComputer::computeUniqueDirectionList(
846 : const EdgeVector& all,
847 : std::map<NBEdge*, std::set<NBEdge*, ComparatorIdLess> >& same,
848 : GeomsMap& geomsCCW,
849 : GeomsMap& geomsCW) {
850 : // store relationships
851 57288 : EdgeVector newAll = all;
852 299525 : for (NBEdge* e1 : all) {
853 : // determine which of the edges marks the outer boundary
854 242237 : auto e2NewAll = std::find(newAll.begin(), newAll.end(), e1);
855 : #ifdef DEBUG_NODE_SHAPE
856 : if (DEBUGCOND) std::cout << "computeUniqueDirectionList e1=" << e1->getID()
857 : << " deleted=" << (e2NewAll == newAll.end())
858 : << " same=" << joinNamedToStringSorting(same[e1], ',') << "\n";
859 : #endif
860 242237 : if (e2NewAll == newAll.end()) {
861 77620 : continue;
862 : }
863 164617 : auto e1It = std::find(all.begin(), all.end(), e1);
864 : auto bestCCW = e1It;
865 : auto bestCW = e1It;
866 : bool changed = true;
867 405669 : while (changed) {
868 : changed = false;
869 400990 : for (NBEdge* e2 : same[e1]) {
870 : #ifdef DEBUG_NODE_SHAPE
871 : if (DEBUGCOND) {
872 : std::cout << " e2=" << e2->getID() << "\n";
873 : }
874 : #endif
875 159938 : auto e2It = std::find(all.begin(), all.end(), e2);
876 159938 : if (e2It + 1 == bestCCW || (e2It == (all.end() - 1) && bestCCW == all.begin())) {
877 : bestCCW = e2It;
878 : changed = true;
879 : #ifdef DEBUG_NODE_SHAPE
880 : if (DEBUGCOND) {
881 : std::cout << " bestCCW=" << e2->getID() << "\n";
882 : }
883 : #endif
884 158911 : } else if (bestCW + 1 == e2It || (bestCW == (all.end() - 1) && e2It == all.begin())) {
885 : bestCW = e2It;
886 : changed = true;
887 : #ifdef DEBUG_NODE_SHAPE
888 : if (DEBUGCOND) {
889 : std::cout << " bestCW=" << e2->getID() << "\n";
890 : }
891 : #endif
892 : }
893 : }
894 : }
895 164617 : if (bestCW != e1It) {
896 74703 : geomsCW[e1] = geomsCW[*bestCW];
897 74703 : computeSameEnd(geomsCW[e1], geomsCCW[e1]);
898 : }
899 164617 : if (bestCCW != e1It) {
900 849 : geomsCCW[e1] = geomsCCW[*bestCCW];
901 849 : computeSameEnd(geomsCW[e1], geomsCCW[e1]);
902 : }
903 : // clean up
904 242237 : for (NBEdge* e2 : same[e1]) {
905 77620 : auto e2NewAllIt = std::find(newAll.begin(), newAll.end(), e2);
906 77620 : if (e2NewAllIt != newAll.end()) {
907 : newAll.erase(e2NewAllIt);
908 : }
909 : }
910 : }
911 : #ifdef DEBUG_NODE_SHAPE
912 : if (DEBUGCOND) {
913 : std::cout << " newAll:\n";
914 : for (NBEdge* e : newAll) {
915 : std::cout << " " << e->getID() << " geomCCW=" << geomsCCW[e] << " geomsCW=" << geomsCW[e] << "\n";
916 : }
917 : }
918 : #endif
919 57288 : return newAll;
920 0 : }
921 :
922 :
923 : void
924 164534 : NBNodeShapeComputer::initNeighbors(const EdgeVector& edges, const EdgeVector::const_iterator& current,
925 : GeomsMap& geomsCW,
926 : GeomsMap& geomsCCW,
927 : EdgeVector::const_iterator& cwi,
928 : EdgeVector::const_iterator& ccwi,
929 : double& cad,
930 : double& ccad) {
931 : const double twoPI = (double)(2 * M_PI);
932 164534 : cwi = current;
933 : cwi++;
934 164534 : if (cwi == edges.end()) {
935 57205 : std::advance(cwi, -((int)edges.size())); // set to edges.begin();
936 : }
937 164534 : ccwi = current;
938 164534 : if (ccwi == edges.begin()) {
939 57205 : std::advance(ccwi, edges.size() - 1); // set to edges.end() - 1;
940 : } else {
941 : ccwi--;
942 : }
943 :
944 164534 : const double angleCurCCW = geomsCCW[*current].angleAt2D(0);
945 164534 : const double angleCurCW = geomsCW[*current].angleAt2D(0);
946 164534 : const double angleCCW = geomsCW[*ccwi].angleAt2D(0);
947 164534 : const double angleCW = geomsCCW[*cwi].angleAt2D(0);
948 164534 : ccad = angleCCW - angleCurCCW;
949 223294 : while (ccad < 0.) {
950 58760 : ccad += twoPI;
951 : }
952 164534 : cad = angleCurCW - angleCW;
953 223294 : while (cad < 0.) {
954 58760 : cad += twoPI;
955 : }
956 164534 : }
957 :
958 :
959 :
960 : const PositionVector
961 20994 : NBNodeShapeComputer::computeNodeShapeSmall() {
962 : #ifdef DEBUG_NODE_SHAPE
963 : if (DEBUGCOND) {
964 : std::cout << "computeNodeShapeSmall node=" << myNode.getID() << "\n";
965 : }
966 : #endif
967 20994 : PositionVector ret;
968 53133 : for (NBEdge* e : myNode.getEdges()) {
969 : // compute crossing with normal
970 32139 : PositionVector edgebound1 = e->getCCWBoundaryLine(myNode).getSubpartByIndex(0, 2);
971 32139 : PositionVector edgebound2 = e->getCWBoundaryLine(myNode).getSubpartByIndex(0, 2);
972 32139 : Position delta = edgebound1[1] - edgebound1[0];
973 : delta.set(-delta.y(), delta.x()); // rotate 90 degrees
974 32139 : PositionVector cross(myNode.getPosition(), myNode.getPosition() + delta);
975 32139 : cross.extrapolate2D(500);
976 32139 : edgebound1.extrapolate2D(500);
977 32139 : edgebound2.extrapolate2D(500);
978 32139 : if (cross.intersects(edgebound1)) {
979 32139 : Position np = cross.intersectionPosition2D(edgebound1);
980 32139 : np.set(np.x(), np.y(), myNode.getPosition().z());
981 32139 : ret.push_back_noDoublePos(np);
982 : }
983 32139 : if (cross.intersects(edgebound2)) {
984 32135 : Position np = cross.intersectionPosition2D(edgebound2);
985 32135 : np.set(np.x(), np.y(), myNode.getPosition().z());
986 32135 : ret.push_back_noDoublePos(np);
987 : }
988 32139 : e->resetNodeBorder(&myNode);
989 32139 : }
990 20994 : return ret;
991 0 : }
992 :
993 :
994 : double
995 57288 : NBNodeShapeComputer::getDefaultRadius(const OptionsCont& oc) {
996 : // look for incoming/outgoing edge pairs that do not go straight and allow wide vehicles
997 : // (connection information is not available yet)
998 : // @TODO compute the radius for each pair of neighboring edge intersections in computeNodeShapeDefault rather than use the maximum
999 57288 : const double radius = oc.getFloat("default.junctions.radius");
1000 57288 : const double smallRadius = oc.getFloat("junctions.small-radius");
1001 : double maxRightAngle = 0; // rad
1002 : double extraWidthRight = 0; // m
1003 : double maxLeftAngle = 0; // rad
1004 : double extraWidthLeft = 0; // m
1005 : int laneDelta = 0;
1006 : int totalWideLanesIn = 0;
1007 177953 : for (NBEdge* in : myNode.getIncomingEdges()) {
1008 : int wideLanesIn = 0;
1009 277869 : for (int i = 0; i < in->getNumLanes(); i++) {
1010 157204 : if ((in->getPermissions(i) & SVC_LARGE_TURN) != 0) {
1011 103078 : wideLanesIn++;
1012 : }
1013 : }
1014 120665 : totalWideLanesIn += wideLanesIn;
1015 426829 : for (NBEdge* out : myNode.getOutgoingEdges()) {
1016 306164 : if ((in->getPermissions() & out->getPermissions() & SVC_LARGE_TURN) != 0) {
1017 194304 : if (myNode.getDirection(in, out) == LinkDirection::TURN) {
1018 53102 : continue;
1019 : };
1020 141202 : const double angle = GeomHelper::angleDiff(
1021 141202 : in->getGeometry().angleAt2D(-2),
1022 : out->getGeometry().angleAt2D(0));
1023 141202 : if (angle < 0) {
1024 65781 : if (maxRightAngle < -angle) {
1025 : maxRightAngle = -angle;
1026 40664 : extraWidthRight = MAX2(getExtraWidth(in, SVC_LARGE_TURN), getExtraWidth(out, SVC_LARGE_TURN));
1027 : }
1028 : } else {
1029 75421 : if (maxLeftAngle < angle) {
1030 : maxLeftAngle = angle;
1031 : // all edges clockwise between in and out count as extra width
1032 : extraWidthLeft = 0;
1033 35960 : EdgeVector::const_iterator pIn = std::find(myNode.getEdges().begin(), myNode.getEdges().end(), in);
1034 35960 : NBContHelper::nextCW(myNode.getEdges(), pIn);
1035 93314 : while (*pIn != out) {
1036 57354 : extraWidthLeft += (*pIn)->getTotalWidth();
1037 : #ifdef DEBUG_RADIUS
1038 : if (DEBUGCOND) {
1039 : std::cout << " in=" << in->getID() << " out=" << out->getID() << " extra=" << (*pIn)->getID() << " extraWidthLeft=" << extraWidthLeft << "\n";
1040 : }
1041 : #endif
1042 57354 : NBContHelper::nextCW(myNode.getEdges(), pIn);
1043 : }
1044 : }
1045 : }
1046 : int wideLanesOut = 0;
1047 341113 : for (int i = 0; i < out->getNumLanes(); i++) {
1048 199911 : if ((out->getPermissions(i) & SVC_LARGE_TURN) != 0) {
1049 186483 : wideLanesOut++;
1050 : }
1051 : }
1052 : #ifdef DEBUG_RADIUS
1053 : if (DEBUGCOND) {
1054 : std::cout << " in=" << in->getID() << " out=" << out->getID() << " wideLanesIn=" << wideLanesIn << " wideLanesOut=" << wideLanesOut << "\n";
1055 : }
1056 : #endif
1057 141202 : laneDelta = MAX2(laneDelta, abs(wideLanesOut - wideLanesIn));
1058 : }
1059 : }
1060 : }
1061 : // special case: on/off-ramp
1062 57288 : if (myNode.getOutgoingEdges().size() == 1 || myNode.getIncomingEdges().size() == 1) {
1063 : int totalWideLanesOut = 0;
1064 53870 : for (NBEdge* out : myNode.getOutgoingEdges()) {
1065 74349 : for (int i = 0; i < out->getNumLanes(); i++) {
1066 43645 : if ((out->getPermissions(i) & SVC_LARGE_TURN) != 0) {
1067 25836 : totalWideLanesOut++;
1068 : }
1069 : }
1070 : }
1071 23166 : if (totalWideLanesIn == totalWideLanesOut) {
1072 : // use total laneDelta instead of individual edge lane delta
1073 : laneDelta = 0;
1074 : }
1075 : }
1076 : // changing the number of wide-vehicle lanes on a straight segment requires a larger junction to allow for smooth driving
1077 : // otherwise we can reduce the radius according to the angle
1078 : double result = radius;
1079 : // left turns are assumed to cross additional edges and thus du not determine the required radius in most cases
1080 : double maxTurnAngle = maxRightAngle;
1081 : double extraWidth = extraWidthRight;
1082 57288 : if (maxRightAngle < DEG2RAD(5)) {
1083 : maxTurnAngle = maxLeftAngle;
1084 : extraWidth = extraWidthLeft;
1085 : }
1086 57288 : const double minRadius = maxTurnAngle >= DEG2RAD(30) ? MIN2(smallRadius, radius) : smallRadius;
1087 57288 : if (laneDelta == 0 || maxTurnAngle >= DEG2RAD(30) || myNode.isConstantWidthTransition()) {
1088 : // subtract radius gained from extra lanes
1089 : // do not increase radius for turns that are sharper than a right angle
1090 54028 : result = radius * tan(0.5 * MIN2(0.5 * M_PI, maxTurnAngle)) - extraWidth;
1091 : }
1092 : result = MAX2(minRadius, result);
1093 : #ifdef DEBUG_RADIUS
1094 : if (DEBUGCOND) {
1095 : std::cout << "getDefaultRadius n=" << myNode.getID()
1096 : << " r=" << radius << " sr=" << smallRadius
1097 : << " mr=" << minRadius
1098 : << " laneDelta=" << laneDelta
1099 : << " rightA=" << RAD2DEG(maxRightAngle)
1100 : << " leftA=" << RAD2DEG(maxLeftAngle)
1101 : << " maxA=" << RAD2DEG(maxTurnAngle)
1102 : << " extraWidth=" << extraWidth
1103 : << " result=" << result << "\n";
1104 : }
1105 : #endif
1106 57288 : return result;
1107 : }
1108 :
1109 :
1110 : bool
1111 136900 : NBNodeShapeComputer::isDivided(const NBEdge* e, std::set<NBEdge*, ComparatorIdLess> same, const PositionVector& ccw, const PositionVector& cw) const {
1112 136900 : if (same.size() < 2) {
1113 : return false;
1114 : }
1115 : std::set<Position> endPoints;
1116 1778 : endPoints.insert(e->getEndpointAtNode(&myNode));
1117 3547 : for (NBEdge* s : same) {
1118 5316 : endPoints.insert(s->getEndpointAtNode(&myNode));
1119 : }
1120 889 : if (endPoints.size() > 1) {
1121 743 : std::vector<double> distances = ccw.distances(cw, true);
1122 743 : double width = e->getTotalWidth();
1123 3047 : for (const NBEdge* e2 : same) {
1124 2304 : width += e2->getTotalWidth();
1125 : }
1126 : const double maxDist = VectorHelper<double>::maxValue(distances);
1127 743 : const double maxDivider = maxDist - width;
1128 743 : return maxDivider >= 5;
1129 743 : }
1130 : return false;
1131 : }
1132 :
1133 :
1134 : double
1135 81328 : NBNodeShapeComputer::getExtraWidth(const NBEdge* e, SVCPermissions exclude) {
1136 : double result = 0;
1137 : int lane = 0;
1138 81328 : while (lane < e->getNumLanes() && e->getPermissions(lane) == 0) {
1139 : // ignore forbidden lanes out the outside
1140 0 : lane++;
1141 : }
1142 90249 : while (lane < e->getNumLanes() && (e->getPermissions(lane) & exclude) == 0) {
1143 8921 : result += e->getLaneWidth(lane);
1144 8921 : lane++;
1145 : }
1146 81328 : return result;
1147 : }
1148 :
1149 :
1150 : double
1151 313 : NBNodeShapeComputer::divisionWidth(const NBEdge* e, std::set<NBEdge*, ComparatorIdLess> same, const Position& p, const Position& p2) {
1152 : double result = p.distanceTo2D(p2);
1153 313 : result -= e->getTotalWidth();
1154 1466 : for (NBEdge* e2 : same) {
1155 1153 : result -= e2->getTotalWidth();
1156 : }
1157 313 : return MAX2(0.0, result);
1158 : }
1159 :
1160 : /****************************************************************************/
|
