SUMO - Simulation of Urban MObility
NBNodeShapeComputer.cpp
Go to the documentation of this file.
1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2001-2017 German Aerospace Center (DLR) and others.
4 /****************************************************************************/
5 //
6 // This program and the accompanying materials
7 // are made available under the terms of the Eclipse Public License v2.0
8 // which accompanies this distribution, and is available at
9 // http://www.eclipse.org/legal/epl-v20.html
10 //
11 /****************************************************************************/
19 // This class computes shapes of junctions
20 /****************************************************************************/
21 
22 
23 // ===========================================================================
24 // included modules
25 // ===========================================================================
26 #ifdef _MSC_VER
27 #include <windows_config.h>
28 #else
29 #include <config.h>
30 #endif
31 
32 #include <algorithm>
33 #include <iterator>
36 #include <utils/geom/GeomHelper.h>
37 #include <utils/common/StdDefs.h>
40 #include <utils/common/ToString.h>
42 #include "NBNode.h"
43 #include "NBNodeShapeComputer.h"
44 
45 //#define DEBUG_NODE_SHAPE
46 #define DEBUGCOND (myNode.getID() == "disabled")
47 
48 // ===========================================================================
49 // method definitions
50 // ===========================================================================
52  : myNode(node) {}
53 
54 
56 
57 
60  PositionVector ret;
61  // check whether the node is a dead end node or a node where only turning is possible
62  // in this case, we will use "computeNodeShapeSmall"
63  bool singleDirection = false;
64  if (myNode.myAllEdges.size() == 1) {
65  singleDirection = true;
66  }
67  if (myNode.myAllEdges.size() == 2 && myNode.getIncomingEdges().size() == 1) {
68  if (myNode.getIncomingEdges()[0]->isTurningDirectionAt(myNode.getOutgoingEdges()[0])) {
69  singleDirection = true;
70  }
71  }
72  if (singleDirection) {
73  return computeNodeShapeSmall();
74  }
75  // check whether the node is a just something like a geometry
76  // node (one in and one out or two in and two out, pair-wise continuations)
77  // also in this case "computeNodeShapeSmall" is used
78  bool geometryLike = myNode.isSimpleContinuation();
79  if (geometryLike) {
80  // additionally, the angle between the edges must not be larger than 45 degrees
81  // (otherwise, we will try to compute the shape in a different way)
82  const EdgeVector& incoming = myNode.getIncomingEdges();
83  const EdgeVector& outgoing = myNode.getOutgoingEdges();
84  double maxAngle = 0.;
85  for (EdgeVector::const_iterator i = incoming.begin(); i != incoming.end(); ++i) {
86  double ia = (*i)->getAngleAtNode(&myNode);
87  for (EdgeVector::const_iterator j = outgoing.begin(); j != outgoing.end(); ++j) {
88  double oa = (*j)->getAngleAtNode(&myNode);
89  double ad = GeomHelper::getMinAngleDiff(ia, oa);
90  if (22.5 >= ad) {
91  maxAngle = MAX2(ad, maxAngle);
92  }
93  }
94  }
95  if (maxAngle > 22.5) {
96  return computeNodeShapeSmall();
97  }
98  }
99 
100  //
101  ret = computeNodeShapeDefault(geometryLike);
102  // fail fall-back: use "computeNodeShapeSmall"
103  if (ret.size() < 3) {
104  ret = computeNodeShapeSmall();
105  }
106  return ret;
107 }
108 
109 
110 void
112  assert(l1[0].distanceTo2D(l1[1]) >= 100.);
113  assert(l2[0].distanceTo2D(l2[1]) >= 100.);
114  PositionVector tmp;
115  tmp.push_back(PositionVector::positionAtOffset2D(l1[0], l1[1], 100));
116  tmp.push_back(l1[1]);
117  tmp[1].sub(tmp[0]);
118  tmp[1].set(-tmp[1].y(), tmp[1].x());
119  tmp[1].add(tmp[0]);
120  tmp.extrapolate2D(100);
121  if (l2.intersects(tmp[0], tmp[1])) {
122  const double offset = l2.intersectsAtLengths2D(tmp)[0];
123  if (l2.length2D() - offset > POSITION_EPS) {
124  PositionVector tl2 = l2.getSubpart2D(offset, l2.length2D());
125  tl2.extrapolate2D(100);
126  l2.erase(l2.begin(), l2.begin() + (l2.size() - tl2.size()));
127  l2[0] = tl2[0];
128  }
129  }
130 }
131 
132 
135  // if we have less than two edges, we can not compute the node's shape this way
136  if (myNode.myAllEdges.size() < 2) {
137  return PositionVector();
138  }
139  // magic values
140  const OptionsCont& oc = OptionsCont::getOptions();
141  const bool defaultRadius = myNode.getRadius() == NBNode::UNSPECIFIED_RADIUS;
142  const double radius = (defaultRadius ? oc.getFloat("default.junctions.radius") : myNode.getRadius());
143  const int cornerDetail = oc.getInt("junctions.corner-detail");
144  const double sCurveStretch = oc.getFloat("junctions.scurve-stretch");
145  const bool rectangularCut = oc.getBool("rectangular-lane-cut");
146  const bool openDriveOutput = oc.isSet("opendrive-output");
147 
148  // Extend geometries to move the stop line forward.
149  // In OpenDrive the junction starts whenever the geometry changes. Stop
150  // line information is not given or ambiguous (sign positions at most)
151  // In SUMO, stop lines are where the junction starts. This is computed
152  // heuristically from intersecting the junctions roads geometries.
153  const double advanceStopLine = oc.exists("opendrive-files") && oc.isSet("opendrive-files") ? oc.getFloat("opendrive.advance-stopline") : 0;
154 
155 
156 #ifdef DEBUG_NODE_SHAPE
157  if (DEBUGCOND) {
158  std::cout << "\ncomputeNodeShapeDefault node " << myNode.getID() << " simple=" << simpleContinuation << " radius=" << radius << "\n";
159  }
160 #endif
161 
162  // initialise
163  EdgeVector::const_iterator i;
164  // edges located in the value-vector have the same direction as the key edge
165  std::map<NBEdge*, std::set<NBEdge*> > same;
166  // the counter-clockwise boundary of the edge regarding possible same-direction edges
167  GeomsMap geomsCCW;
168  // the clockwise boundary of the edge regarding possible same-direction edges
169  GeomsMap geomsCW;
170  // check which edges are parallel
171  joinSameDirectionEdges(same, geomsCCW, geomsCW);
172  // compute unique direction list
173  EdgeVector newAll = computeUniqueDirectionList(same, geomsCCW, geomsCW);
174  // if we have only two "directions", let's not compute the geometry using this method
175  if (newAll.size() < 2) {
176  return PositionVector();
177  }
178 
179  // All geoms are outoing from myNode.
180  // for every direction in newAll we compute the offset at which the
181  // intersection ends and the edge starts. This value is saved in 'distances'
182  // If the geometries need to be extended to get an intersection, this is
183  // recorded in 'myExtended'
184  std::map<NBEdge*, double> distances;
185  std::map<NBEdge*, bool> myExtended;
186 
187  for (i = newAll.begin(); i != newAll.end(); ++i) {
188  EdgeVector::const_iterator cwi = i;
189  EdgeVector::const_iterator ccwi = i;
190  double ccad;
191  double cad;
192  initNeighbors(newAll, i, geomsCW, geomsCCW, cwi, ccwi, cad, ccad);
193  assert(geomsCCW.find(*i) != geomsCCW.end());
194  assert(geomsCW.find(*ccwi) != geomsCW.end());
195  assert(geomsCW.find(*cwi) != geomsCW.end());
196 
197  // there are only 2 directions and they are almost parallel
198  if (*cwi == *ccwi &&
199  (
200  // no change in lane numbers, even low angles still give a good intersection
201  (simpleContinuation && fabs(ccad - cad) < (double) 0.1)
202  // lane numbers change, a direct intersection could be far away from the node position
203  // so we use a larger threshold
204  || (!simpleContinuation && fabs(ccad - cad) < DEG2RAD(22.5)))
205  ) {
206  // compute the mean position between both edges ends ...
207  Position p;
208  if (myExtended.find(*ccwi) != myExtended.end()) {
209  p = geomsCCW[*ccwi][0];
210  p.add(geomsCW[*ccwi][0]);
211  p.mul(0.5);
212 #ifdef DEBUG_NODE_SHAPE
213  if (DEBUGCOND) {
214  std::cout << " extended: p=" << p << " angle=" << (ccad - cad) << "\n";
215  }
216 #endif
217  } else {
218  p = geomsCCW[*ccwi][0];
219  p.add(geomsCW[*ccwi][0]);
220  p.add(geomsCCW[*i][0]);
221  p.add(geomsCW[*i][0]);
222  p.mul(0.25);
223 #ifdef DEBUG_NODE_SHAPE
224  if (DEBUGCOND) {
225  std::cout << " unextended: p=" << p << " angle=" << (ccad - cad) << "\n";
226  }
227 #endif
228  }
229  // ... compute the distance to this point ...
230  double dist = MAX2(
231  geomsCCW[*i].nearest_offset_to_point2D(p),
232  geomsCW[*i].nearest_offset_to_point2D(p));
233  if (dist < 0) {
234  // ok, we have the problem that even the extrapolated geometry
235  // does not reach the point
236  // in this case, the geometry has to be extenden... too bad ...
237  // ... let's append the mean position to the geometry
238  PositionVector g = (*i)->getGeometry();
239  if (myNode.hasIncoming(*i)) {
241  } else {
243  }
244  (*i)->setGeometry(g);
245  // and rebuild previous information
246  geomsCCW[*i] = (*i)->getCCWBoundaryLine(myNode);
247  geomsCCW[*i].extrapolate(100);
248  geomsCW[*i] = (*i)->getCWBoundaryLine(myNode);
249  geomsCW[*i].extrapolate(100);
250  // the distance is now = zero (the point we have appended)
251  distances[*i] = 100;
252  myExtended[*i] = true;
253 #ifdef DEBUG_NODE_SHAPE
254  if (DEBUGCOND) {
255  std::cout << " extending (dist=" << dist << ")\n";
256  }
257 #endif
258  } else {
259  if (!simpleContinuation) {
260  dist += radius;
261  } else {
262  // if the angles change, junction should have some size to avoid degenerate shape
263  double radius2 = fabs(ccad - cad) * (*i)->getNumLanes();
264  if (radius2 > NUMERICAL_EPS || openDriveOutput) {
265  radius2 = MAX2(0.15, radius2);
266  }
267  dist += radius2;
268 #ifdef DEBUG_NODE_SHAPE
269  if (DEBUGCOND) {
270  std::cout << " using radius=" << fabs(ccad - cad) * (*i)->getNumLanes() << " ccad=" << ccad << " cad=" << cad << "\n";
271  }
272 #endif
273  }
274  distances[*i] = dist;
275  }
276 
277  } else {
278  // the angles are different enough to compute the intersection of
279  // the outer boundaries directly (or there are more than 2 directions). The "nearer" neighbar causes the furthest distance
280  const bool ccwCloser = ccad < cad;
281  // the border facing the closer neighbor
282  const PositionVector& currGeom = ccwCloser ? geomsCCW[*i] : geomsCW[*i];
283  // the border facing the far neighbor
284  const PositionVector& currGeom2 = ccwCloser ? geomsCW[*i] : geomsCCW[*i];
285  // the border of the closer neighbor
286  const PositionVector& neighGeom = ccwCloser ? geomsCW[*ccwi] : geomsCCW[*cwi];
287  // the border of the far neighbor
288  const PositionVector& neighGeom2 = ccwCloser ? geomsCCW[*cwi] : geomsCW[*ccwi];
289 #ifdef DEBUG_NODE_SHAPE
290  if (DEBUGCOND) {
291  std::cout << " i=" << (*i)->getID() << " neigh=" << (*ccwi)->getID() << " neigh2=" << (*cwi)->getID() << "\n";
292  }
293 #endif
294  if (!simpleContinuation) {
295  if (currGeom.intersects(neighGeom)) {
296  distances[*i] = radius + closestIntersection(currGeom, neighGeom, 100);
297 #ifdef DEBUG_NODE_SHAPE
298  if (DEBUGCOND) {
299  std::cout << " neigh intersects dist=" << distances[*i] << " currGeom=" << currGeom << " neighGeom=" << neighGeom << "\n";
300  }
301 #endif
302  if (*cwi != *ccwi && currGeom2.intersects(neighGeom2)) {
303  // also use the second intersection point
304  // but prevent very large node shapes
305  const double farAngleDist = ccwCloser ? cad : ccad;
306  double a1 = distances[*i];
307  double a2 = radius + closestIntersection(currGeom2, neighGeom2, 100);
308 #ifdef DEBUG_NODE_SHAPE
309  if (DEBUGCOND) {
310  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";
311  }
312 #endif
313  //if (RAD2DEG(farAngleDist) < 175) {
314  // distances[*i] = MAX2(a1, MIN2(a2, a1 + 180 - RAD2DEG(farAngleDist)));
315  //}
316  if (ccad > DEG2RAD(90. + 45.) && cad > DEG2RAD(90. + 45.)) {
317  // do nothing.
318  } else if (farAngleDist < DEG2RAD(135) || (fabs(RAD2DEG(farAngleDist) - 180) > 1 && fabs(a2 - a1) < 10)) {
319  distances[*i] = MAX2(a1, a2);
320  }
321 #ifdef DEBUG_NODE_SHAPE
322  if (DEBUGCOND) {
323  std::cout << " a1=" << a1 << " a2=" << a2 << " dist=" << distances[*i] << "\n";
324  }
325 #endif
326  }
327  } else {
328  if (*cwi != *ccwi && currGeom2.intersects(neighGeom2)) {
329  distances[*i] = radius + currGeom2.intersectsAtLengths2D(neighGeom2)[0];
330 #ifdef DEBUG_NODE_SHAPE
331  if (DEBUGCOND) {
332  std::cout << " neigh2 intersects dist=" << distances[*i] << " currGeom2=" << currGeom2 << " neighGeom2=" << neighGeom2 << "\n";
333  }
334 #endif
335  } else {
336  distances[*i] = 100 + radius;
337 #ifdef DEBUG_NODE_SHAPE
338  if (DEBUGCOND) {
339  std::cout << " no intersects dist=" << distances[*i] << " currGeom=" << currGeom << " neighGeom=" << neighGeom << " currGeom2=" << currGeom2 << " neighGeom2=" << neighGeom2 << "\n";
340  }
341 #endif
342  }
343  }
344  } else {
345  if (currGeom.intersects(neighGeom)) {
346  distances[*i] = currGeom.intersectsAtLengths2D(neighGeom)[0];
347  } else {
348  distances[*i] = (double) 100.;
349  }
350  }
351  }
352  if (defaultRadius && sCurveStretch > 0) {
353  double sCurveWidth = myNode.getDisplacementError();
354  if (sCurveWidth > 0) {
355  const double sCurveRadius = radius + sCurveWidth / SUMO_const_laneWidth * sCurveStretch * pow((*i)->getSpeed(), 2 + sCurveStretch) / 1000;
356  const double stretch = 100 + sCurveRadius - distances[*i];
357  if (stretch > 0) {
358  distances[*i] += stretch;
359  // fixate extended geometry for repeated computation
360  const double shorten = distances[*i] - 100;
361  (*i)->shortenGeometryAtNode(&myNode, shorten);
362  for (std::set<NBEdge*>::iterator k = same[*i].begin(); k != same[*i].end(); ++k) {
363  (*k)->shortenGeometryAtNode(&myNode, shorten);
364  }
365 #ifdef DEBUG_NODE_SHAPE
366  if (DEBUGCOND) {
367  std::cout << " stretching junction: sCurveWidth=" << sCurveWidth << " sCurveRadius=" << sCurveRadius << " stretch=" << stretch << " dist=" << distances[*i] << "\n";
368  }
369 #endif
370  }
371  }
372  }
373  }
374 
375  for (i = newAll.begin(); i != newAll.end(); ++i) {
376  if (distances.find(*i) == distances.end()) {
377  assert(false);
378  distances[*i] = 100;
379  }
380  }
381  // prevent inverted node shapes
382  // (may happen with near-parallel edges)
383  const double minDistSum = 2 * (100 + radius);
384  for (i = newAll.begin(); i != newAll.end(); ++i) {
385  if (distances[*i] < 100 && (*i)->hasDefaultGeometryEndpointAtNode(&myNode)) {
386  for (EdgeVector::const_iterator j = newAll.begin(); j != newAll.end(); ++j) {
387  if (distances[*j] > 100 && (*j)->hasDefaultGeometryEndpointAtNode(&myNode) && distances[*i] + distances[*j] < minDistSum) {
388  const double angleDiff = fabs(NBHelpers::relAngle((*i)->getAngleAtNode(&myNode), (*j)->getAngleAtNode(&myNode)));
389  if (angleDiff > 160 || angleDiff < 20) {
390 #ifdef DEBUG_NODE_SHAPE
391  if (DEBUGCOND) {
392  std::cout << " increasing dist for i=" << (*i)->getID() << " because of j=" << (*j)->getID() << " jDist=" << distances[*j]
393  << " oldI=" << distances[*i] << " newI=" << minDistSum - distances[*j]
394  << " angleDiff=" << angleDiff
395  << " geomI=" << (*i)->getGeometry() << " geomJ=" << (*j)->getGeometry() << "\n";
396  }
397 #endif
398  distances[*i] = minDistSum - distances[*j];
399  }
400  }
401  }
402  }
403  }
404 
405 
406  // build
407  PositionVector ret;
408  for (i = newAll.begin(); i != newAll.end(); ++i) {
409  const PositionVector& ccwBound = geomsCCW[*i];
410  const PositionVector& cwBound = geomsCW[*i];
411  //double offset = MIN3(distances[*i], cwBound.length2D() - POSITION_EPS, ccwBound.length2D() - POSITION_EPS);
412  double offset = distances[*i];
413  if (!(*i)->hasDefaultGeometryEndpointAtNode(&myNode)) {
414  // for non geometry-endpoints, only shorten but never extend the geometry
415  if (advanceStopLine > 0 && offset < 100) {
416 #ifdef DEBUG_NODE_SHAPE
417  std::cout << " i=" << (*i)->getID() << " offset=" << offset << " advanceStopLine=" << advanceStopLine << "\n";
418 #endif
419  // fixate extended geometry for repeated computation
420  (*i)->extendGeometryAtNode(&myNode, advanceStopLine);
421  for (std::set<NBEdge*>::iterator k = same[*i].begin(); k != same[*i].end(); ++k) {
422  (*k)->extendGeometryAtNode(&myNode, advanceStopLine);
423  }
424  }
425  offset = MAX2(100.0 - advanceStopLine, offset);
426  }
427  if (offset == -1) {
428  WRITE_WARNING("Fixing offset for edge '" + (*i)->getID() + "' at node '" + myNode.getID() + ".");
429  offset = (double) - .1;
430  }
431  Position p = ccwBound.positionAtOffset2D(offset);
432  p.setz(myNode.getPosition().z());
433  if (i != newAll.begin()) {
434  ret.append(getSmoothCorner(geomsCW[*(i - 1)].reverse(), ccwBound, ret[-1], p, cornerDetail));
435  }
436  ret.push_back_noDoublePos(p);
437  //
438  Position p2 = cwBound.positionAtOffset2D(offset);
439  p2.setz(myNode.getPosition().z());
440  ret.push_back_noDoublePos(p2);
441 #ifdef DEBUG_NODE_SHAPE
442  if (DEBUGCOND) {
443  std::cout << " build stopLine for i=" << (*i)->getID() << " offset=" << offset << " dist=" << distances[*i] << " cwLength=" << cwBound.length2D() << " ccwLength=" << ccwBound.length2D() << " p=" << p << " p2=" << p2 << " ccwBound=" << ccwBound << " cwBound=" << cwBound << "\n";
444  }
445 #endif
446  (*i)->setNodeBorder(&myNode, p, p2, rectangularCut);
447  for (std::set<NBEdge*>::iterator k = same[*i].begin(); k != same[*i].end(); ++k) {
448  (*k)->setNodeBorder(&myNode, p, p2, rectangularCut);
449  }
450  }
451  // final curve segment
452  ret.append(getSmoothCorner(geomsCW[*(newAll.end() - 1)], geomsCCW[*newAll.begin()], ret[-1], ret[0], cornerDetail));
453  return ret;
454 }
455 
456 
457 double
458 NBNodeShapeComputer::closestIntersection(const PositionVector& geom1, const PositionVector& geom2, double offset) {
459  std::vector<double> intersections = geom1.intersectsAtLengths2D(geom2);
460  double result = intersections[0];
461  for (std::vector<double>::iterator it = intersections.begin() + 1; it != intersections.end(); ++it) {
462  if (fabs(*it - offset) < fabs(result - offset)) {
463  result = *it;
464  }
465  }
466  return result;
467 }
468 
469 
472  const Position& begPoint, const Position& endPoint, int cornerDetail) {
473  PositionVector ret;
474  if (cornerDetail > 0) {
475  begShape = begShape.reverse();
476  begShape[-1] = begPoint;
477  endShape[0] = endPoint;
478  PositionVector curve = myNode.computeSmoothShape(begShape, endShape, cornerDetail + 2, false, 25, 25);
479  if (curve.size() > 2) {
480  curve.erase(curve.begin());
481  curve.pop_back();
482  ret = curve;
483  }
484  }
485  return ret;
486 }
487 
488 void
489 NBNodeShapeComputer::joinSameDirectionEdges(std::map<NBEdge*, std::set<NBEdge*> >& same,
490  GeomsMap& geomsCCW,
491  GeomsMap& geomsCW) {
492  EdgeVector::const_iterator i, j;
493  // compute boundary lines and extend it by 100m
494  for (i = myNode.myAllEdges.begin(); i != myNode.myAllEdges.end() - 1; i++) {
495  // store current edge's boundary as current ccw/cw boundary
496  try {
497  geomsCCW[*i] = (*i)->getCCWBoundaryLine(myNode);
498  } catch (InvalidArgument& e) {
499  WRITE_WARNING(std::string("While computing intersection geometry: ") + std::string(e.what()));
500  geomsCCW[*i] = (*i)->getGeometry();
501  }
502  try {
503  geomsCW[*i] = (*i)->getCWBoundaryLine(myNode);
504  } catch (InvalidArgument& e) {
505  WRITE_WARNING(std::string("While computing intersection geometry: ") + std::string(e.what()));
506  geomsCW[*i] = (*i)->getGeometry();
507  }
508  // extend the boundary by extroplating it by 100m
509  PositionVector g1 =
510  myNode.hasIncoming(*i)
511  ? (*i)->getCCWBoundaryLine(myNode)
512  : (*i)->getCWBoundaryLine(myNode);
513  geomsCCW[*i].extrapolate2D(100, true);
514  geomsCW[*i].extrapolate2D(100, true);
515  //
516  for (j = i + 1; j != myNode.myAllEdges.end(); j++) {
517  geomsCCW[*j] = (*j)->getCCWBoundaryLine(myNode);
518  geomsCW[*j] = (*j)->getCWBoundaryLine(myNode);
519  PositionVector g2 =
520  myNode.hasIncoming(*j)
521  ? (*j)->getCCWBoundaryLine(myNode)
522  : (*j)->getCWBoundaryLine(myNode);
523  geomsCCW[*j].extrapolate2D(100, true);
524  geomsCW[*j].extrapolate2D(100, true);
525  }
526  }
527  // compute same (edges where an intersection doesn't work well
528  // (always check an edge and its cw neightbor)
529  // distance to look ahead for a misleading angle
530  const double angleChangeLookahead = 35;
531  EdgeSet foundOpposite;
532  for (i = myNode.myAllEdges.begin(); i != myNode.myAllEdges.end(); i++) {
533  EdgeVector::const_iterator j;
534  if (i == myNode.myAllEdges.end() - 1) {
535  j = myNode.myAllEdges.begin();
536  } else {
537  j = i + 1;
538  }
539  const bool incoming = (*i)->getToNode() == &myNode;
540  const bool incoming2 = (*j)->getToNode() == &myNode;
541  const Position positionAtNode = (*i)->getGeometry()[incoming ? -1 : 0];
542  const Position positionAtNode2 = (*j)->getGeometry()[incoming2 ? -1 : 0];
543  const PositionVector g1 = incoming ? (*i)->getCCWBoundaryLine(myNode) : (*i)->getCWBoundaryLine(myNode);
544  const PositionVector g2 = incoming ? (*j)->getCCWBoundaryLine(myNode) : (*j)->getCWBoundaryLine(myNode);
545  const double angle1further = (g1.size() > 2 && g1[0].distanceTo2D(g1[1]) < angleChangeLookahead ?
546  g1.angleAt2D(1) : g1.angleAt2D(0));
547  const double angle2further = (g2.size() > 2 && g2[0].distanceTo2D(g2[1]) < angleChangeLookahead ?
548  g2.angleAt2D(1) : g2.angleAt2D(0));
549  const double angleDiff = GeomHelper::angleDiff(g1.angleAt2D(0), g2.angleAt2D(0));
550  const double angleDiffFurther = GeomHelper::angleDiff(angle1further, angle2further);
551  const bool ambiguousGeometry = ((angleDiff > 0 && angleDiffFurther < 0) || (angleDiff < 0 && angleDiffFurther > 0));
552  const bool differentDirs = (incoming != incoming2);
553  //if (ambiguousGeometry) {
554  // @todo: this warning would be helpful in many cases. However, if angle and angleFurther jump between 179 and -179 it is misleading
555  // WRITE_WARNING("Ambigous angles at junction '" + myNode.getID() + "' for edges '" + (*i)->getID() + "' and '" + (*j)->getID() + "'.");
556  //}
557  if (fabs(angleDiff) < DEG2RAD(20)) {
558  const bool isOpposite = differentDirs && foundOpposite.count(*i) == 0;
559  if (isOpposite) {
560  foundOpposite.insert(*i);
561  foundOpposite.insert(*j);
562  }
563  if (isOpposite || ambiguousGeometry || badIntersection(*i, *j, geomsCW[*i], geomsCCW[*j], 100)) {
564  // maintain equivalence relation for all members of the equivalence class
565  for (std::set<NBEdge*>::iterator k = same[*i].begin(); k != same[*i].end(); ++k) {
566  if (*j != *k) {
567  same[*k].insert(*j);
568  same[*j].insert(*k);
569  }
570  }
571  for (std::set<NBEdge*>::iterator k = same[*j].begin(); k != same[*j].end(); ++k) {
572  if (*i != *k) {
573  same[*k].insert(*i);
574  same[*i].insert(*k);
575  }
576  }
577  same[*i].insert(*j);
578  same[*j].insert(*i);
579 #ifdef DEBUG_NODE_SHAPE
580  if (DEBUGCOND) {
581  std::cout << " joinedSameDirectionEdges " << (*i)->getID() << " " << (*j)->getID() << " isOpposite=" << isOpposite << " ambiguousGeometry=" << ambiguousGeometry << "\n";
582  }
583 #endif
584  }
585  }
586  }
587 }
588 
589 
590 bool
592  const PositionVector& e1cw, const PositionVector& e2ccw,
593  double distance) {
594  // check whether the two edges are on top of each other. In that case they should be joined
595  // also, if they never touch along their common length
596  const double commonLength = MIN3(distance, e1->getGeometry().length(), e2->getGeometry().length());
597  PositionVector geom1 = e1->getGeometry();
598  PositionVector geom2 = e2->getGeometry();
599  // shift to make geom the centerline of the edge regardless of spreadtype
601  geom1.move2side(e1->getTotalWidth() / 2);
602  }
604  geom2.move2side(e2->getTotalWidth() / 2);
605  }
606  // always let geometry start at myNode
607  if (e1->getToNode() == &myNode) {
608  geom1 = geom1.reverse();
609  }
610  if (e2->getToNode() == &myNode) {
611  geom2 = geom2.reverse();
612  }
613  geom1 = geom1.getSubpart2D(0, commonLength);
614  geom2 = geom2.getSubpart2D(0, commonLength);
615  std::vector<double> distances = geom1.distances(geom2, true);
616  const double minDistanceThreshold = (e1->getTotalWidth() + e2->getTotalWidth()) / 2 + POSITION_EPS;
617  const double minDist = VectorHelper<double>::minValue(distances);
618  const double maxDist = VectorHelper<double>::maxValue(distances);
619  const bool onTop = maxDist - POSITION_EPS < minDistanceThreshold;
620  const bool curvingTowards = geom1[0].distanceTo2D(geom2[0]) > minDistanceThreshold && minDist < minDistanceThreshold;
621  const bool intersects = e1cw.intersects(e2ccw);
622  return onTop || curvingTowards || !intersects;
623 }
624 
625 
628  std::map<NBEdge*, std::set<NBEdge*> >& same,
629  GeomsMap& geomsCCW,
630  GeomsMap& geomsCW) {
631  // store relationships
632  EdgeVector newAll = myNode.myAllEdges;
633  bool changed = true;
634  while (changed) {
635  changed = false;
636  for (EdgeVector::iterator i2 = newAll.begin(); i2 != newAll.end(); ++i2) {
637  std::set<NBEdge*> other = same[*i2];
638  for (std::set<NBEdge*>::const_iterator j = other.begin(); j != other.end(); ++j) {
639  EdgeVector::iterator k = find(newAll.begin(), newAll.end(), *j);
640  if (k != newAll.end()) {
641  if (myNode.hasIncoming(*i2)) {
642  if (!myNode.hasIncoming(*j)) {
643  geomsCW[*i2] = geomsCW[*j];
644  computeSameEnd(geomsCW[*i2], geomsCCW[*i2]);
645  }
646  } else {
647  if (myNode.hasIncoming(*j)) {
648  geomsCCW[*i2] = geomsCCW[*j];
649  computeSameEnd(geomsCW[*i2], geomsCCW[*i2]);
650  }
651  }
652  newAll.erase(k);
653  changed = true;
654  }
655  }
656  if (changed) {
657  break;
658  }
659  }
660  }
661  return newAll;
662 }
663 
664 
665 void
666 NBNodeShapeComputer::initNeighbors(const EdgeVector& edges, const EdgeVector::const_iterator& current,
667  GeomsMap& geomsCW,
668  GeomsMap& geomsCCW,
669  EdgeVector::const_iterator& cwi,
670  EdgeVector::const_iterator& ccwi,
671  double& cad,
672  double& ccad) {
673  const double twoPI = (double)(2 * M_PI);
674  cwi = current;
675  cwi++;
676  if (cwi == edges.end()) {
677  std::advance(cwi, -((int)edges.size())); // set to edges.begin();
678  }
679  ccwi = current;
680  if (ccwi == edges.begin()) {
681  std::advance(ccwi, edges.size() - 1); // set to edges.end() - 1;
682  } else {
683  ccwi--;
684  }
685 
686  const double angleCurCCW = geomsCCW[*current].angleAt2D(0);
687  const double angleCurCW = geomsCW[*current].angleAt2D(0);
688  const double angleCCW = geomsCW[*ccwi].angleAt2D(0);
689  const double angleCW = geomsCCW[*cwi].angleAt2D(0);
690  ccad = angleCCW - angleCurCCW;
691  while (ccad < 0.) {
692  ccad += twoPI;
693  }
694  cad = angleCurCW - angleCW;
695  while (cad < 0.) {
696  cad += twoPI;
697  }
698 }
699 
700 
701 
704 #ifdef DEBUG_NODE_SHAPE
705  if (DEBUGCOND) {
706  std::cout << "computeNodeShapeSmall node=" << myNode.getID() << "\n";
707  }
708 #endif
709  PositionVector ret;
710  EdgeVector::const_iterator i;
711  for (i = myNode.myAllEdges.begin(); i != myNode.myAllEdges.end(); i++) {
712  // compute crossing with normal
713  PositionVector edgebound1 = (*i)->getCCWBoundaryLine(myNode).getSubpartByIndex(0, 2);
714  PositionVector edgebound2 = (*i)->getCWBoundaryLine(myNode).getSubpartByIndex(0, 2);
715  Position delta = edgebound1[1] - edgebound1[0];
716  delta.set(-delta.y(), delta.x()); // rotate 90 degrees
717  PositionVector cross(myNode.getPosition(), myNode.getPosition() + delta);
718  cross.extrapolate2D(500);
719  edgebound1.extrapolate2D(500);
720  edgebound2.extrapolate2D(500);
721  if (cross.intersects(edgebound1)) {
722  Position np = cross.intersectionPosition2D(edgebound1);
723  np.set(np.x(), np.y(), myNode.getPosition().z());
724  ret.push_back_noDoublePos(np);
725  }
726  if (cross.intersects(edgebound2)) {
727  Position np = cross.intersectionPosition2D(edgebound2);
728  np.set(np.x(), np.y(), myNode.getPosition().z());
729  ret.push_back_noDoublePos(np);
730  }
731  (*i)->resetNodeBorder(&myNode);
732  }
733  return ret;
734 }
735 
736 
737 
738 /****************************************************************************/
static double relAngle(double angle1, double angle2)
computes the relative angle between the two angles
Definition: NBHelpers.cpp:53
static double getMinAngleDiff(double angle1, double angle2)
Returns the minimum distance (clockwise/counter-clockwise) between both angles.
Definition: GeomHelper.cpp:167
LaneSpreadFunction getLaneSpreadFunction() const
Returns how this edge&#39;s lanes&#39; lateral offset is computed.
Definition: NBEdge.h:684
static void initNeighbors(const EdgeVector &edges, const EdgeVector::const_iterator &current, GeomsMap &geomsCW, GeomsMap &geomsCCW, EdgeVector::const_iterator &cwi, EdgeVector::const_iterator &ccwi, double &cad, double &ccad)
Initialize neighbors and angles.
double length2D() const
Returns the length.
int getInt(const std::string &name) const
Returns the int-value of the named option (only for Option_Integer)
void append(const PositionVector &v, double sameThreshold=2.0)
double z() const
Returns the z-position.
Definition: Position.h:72
void add(const Position &pos)
Adds the given position to this one.
Definition: Position.h:132
PositionVector getSmoothCorner(PositionVector begShape, PositionVector endShape, const Position &begPoint, const Position &endPoint, int cornerDetail)
Compute smoothed corner shape.
Position positionAtOffset2D(double pos, double lateralOffset=0) const
Returns the position at the given length.
const double SUMO_const_laneWidth
Definition: StdDefs.h:49
double y() const
Returns the y-position.
Definition: Position.h:67
The representation of a single edge during network building.
Definition: NBEdge.h:70
PositionVector computeNodeShapeDefault(bool simpleContinuation)
Computes the node geometry Edges with the same direction are grouped. Then the node geometry is built...
static const double UNSPECIFIED_RADIUS
unspecified lane width
Definition: NBNode.h:209
double x() const
Returns the x-position.
Definition: Position.h:62
static T minValue(const std::vector< T > &v)
Definition: VectorHelper.h:107
const NBNode & myNode
The node to compute the geometry for.
T MAX2(T a, T b)
Definition: StdDefs.h:73
PositionVector reverse() const
reverse position vector
#define RAD2DEG(x)
Definition: GeomHelper.h:45
bool getBool(const std::string &name) const
Returns the boolean-value of the named option (only for Option_Bool)
const std::string & getID() const
Returns the id.
Definition: Named.h:74
std::map< NBEdge *, PositionVector > GeomsMap
std::vector< double > distances(const PositionVector &s, bool perpendicular=false) const
distances of all my points to s and all of s points to myself
void set(double x, double y)
set positions x and y
Definition: Position.h:92
bool badIntersection(const NBEdge *e1, const NBEdge *e2, const PositionVector &e1cw, const PositionVector &e2ccw, double distance)
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:199
static OptionsCont & getOptions()
Retrieves the options.
Definition: OptionsCont.cpp:64
const EdgeVector & getOutgoingEdges() const
Returns this node&#39;s outgoing edges (The edges which start at this node)
Definition: NBNode.h:254
NBNodeShapeComputer(const NBNode &node)
Constructor.
bool isSet(const std::string &name, bool failOnNonExistant=true) const
Returns the information whether the named option is set.
EdgeVector myAllEdges
Vector of incoming and outgoing edges.
Definition: NBNode.h:734
void extrapolate2D(const double val, const bool onlyFirst=false)
extrapolate position vector in two dimensions (Z is ignored)
void push_front_noDoublePos(const Position &p)
insert in front a non double position
std::set< NBEdge * > EdgeSet
container for unique edges
Definition: NBCont.h:50
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:45
A list of positions.
bool exists(const std::string &name) const
Returns the information whether the named option is known.
#define POSITION_EPS
Definition: config.h:175
std::vector< double > intersectsAtLengths2D(const PositionVector &other) const
For all intersections between this vector and other, return the 2D-length of the subvector from this ...
#define DEG2RAD(x)
Definition: GeomHelper.h:44
PositionVector getSubpartByIndex(int beginIndex, int count) const
get subpart of a position vector using index and a cout
PositionVector compute()
Computes the shape of the assigned junction.
double getFloat(const std::string &name) const
Returns the double-value of the named option (only for Option_Float)
void move2side(double amount)
move position vector to side using certain ammount
bool hasIncoming(const NBEdge *const e) const
Returns whether the given edge ends at this node.
Definition: NBNode.cpp:1208
double getDisplacementError() const
compute the displacement error during s-curve computation
Definition: NBNode.h:541
PositionVector computeSmoothShape(const PositionVector &begShape, const PositionVector &endShape, int numPoints, bool isTurnaround, double extrapolateBeg, double extrapolateEnd, NBNode *recordError=0) const
Compute a smooth curve between the given geometries.
Definition: NBNode.cpp:460
const PositionVector & getGeometry() const
Returns the geometry of the edge.
Definition: NBEdge.h:602
PositionVector getSubpart2D(double beginOffset, double endOffset) const
get subpart of a position vector in two dimensions (Z is ignored)
double length() const
Returns the length.
~NBNodeShapeComputer()
Destructor.
PositionVector computeNodeShapeSmall()
Computes the node geometry using normals.
double getRadius() const
Returns the turning radius of this node.
Definition: NBNode.h:271
const EdgeVector & getIncomingEdges() const
Returns this node&#39;s incoming edges (The edges which yield in this node)
Definition: NBNode.h:249
#define M_PI
Definition: odrSpiral.cpp:40
std::vector< NBEdge * > EdgeVector
container for (sorted) edges
Definition: NBCont.h:40
double getTotalWidth() const
Returns the combined width of all lanes of this edge.
Definition: NBEdge.cpp:2935
void joinSameDirectionEdges(std::map< NBEdge *, std::set< NBEdge *> > &same, GeomsMap &geomsCCW, GeomsMap &geomsCW)
Joins edges and computes ccw/cw boundaries.
A storage for options typed value containers)
Definition: OptionsCont.h:98
double angleAt2D(int pos) const
get angle in certain position of position vector
const Position & getPosition() const
Definition: NBNode.h:241
#define DEBUGCOND
Represents a single node (junction) during network building.
Definition: NBNode.h:74
EdgeVector computeUniqueDirectionList(std::map< NBEdge *, std::set< NBEdge *> > &same, GeomsMap &geomsCCW, GeomsMap &geomsCW)
Joins edges and computes ccw/cw boundaries.
T MIN3(T a, T b, T c)
Definition: StdDefs.h:80
#define NUMERICAL_EPS
Definition: config.h:151
void push_back_noDoublePos(const Position &p)
insert in back a non double position
void computeSameEnd(PositionVector &l1, PositionVector &l2)
void mul(double val)
Multiplies both positions with the given value.
Definition: Position.h:112
static T maxValue(const std::vector< T > &v)
Definition: VectorHelper.h:97
double closestIntersection(const PositionVector &geom1, const PositionVector &geom2, double offset)
return the intersection point closest to the given offset
static double angleDiff(const double angle1, const double angle2)
Returns the difference of the second angle to the first angle in radiants.
Definition: GeomHelper.cpp:173
void add(double xoff, double yoff, double zoff)
NBNode * getToNode() const
Returns the destination node of the edge.
Definition: NBEdge.h:433
bool intersects(const Position &p1, const Position &p2) const
Returns the information whether this list of points interesects the given line.
bool isSimpleContinuation(bool checkLaneNumbers=true) const
check if node is a simple continuation
Definition: NBNode.cpp:431
void setz(double z)
set position z
Definition: Position.h:87