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