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