42 #define DEBUGCOND (myNode.getID() == "C")
52 myRadius(node.getRadius()) {
64 bool singleDirection =
false;
66 singleDirection =
true;
70 singleDirection =
true;
73 #ifdef DEBUG_NODE_SHAPE
83 if (singleDirection) {
96 for (EdgeVector::const_iterator i = incoming.begin(); i != incoming.end(); ++i) {
97 double ia = (*i)->getAngleAtNode(&
myNode);
98 for (EdgeVector::const_iterator j = outgoing.begin(); j != outgoing.end(); ++j) {
99 double oa = (*j)->getAngleAtNode(&
myNode);
102 maxAngle =
MAX2(ad, maxAngle);
106 if (maxAngle > 22.5) {
114 if (ret.size() < 3) {
123 assert(l1[0].distanceTo2D(l1[1]) >=
EXT);
124 assert(l2[0].distanceTo2D(l2[1]) >=
EXT);
127 tmp.push_back(l1[1]);
129 tmp[1].set(-tmp[1].y(), tmp[1].x());
137 l2.erase(l2.begin(), l2.begin() + (l2.size() - tl2.size()));
155 const int cornerDetail = oc.
getInt(
"junctions.corner-detail");
156 const double sCurveStretch = oc.
getFloat(
"junctions.scurve-stretch");
157 const bool rectangularCut = oc.
getBool(
"rectangular-lane-cut");
158 const bool openDriveOutput = oc.
isSet(
"opendrive-output");
165 const double advanceStopLine = oc.
exists(
"opendrive-files") && oc.
isSet(
"opendrive-files") ? oc.
getFloat(
"opendrive.advance-stopline") : 0;
168 #ifdef DEBUG_NODE_SHAPE
170 std::cout <<
"\ncomputeNodeShapeDefault node " <<
myNode.
getID() <<
" simple=" << simpleContinuation <<
" useDefaultRadius=" << useDefaultRadius <<
" radius=" <<
myRadius <<
"\n";
175 EdgeVector::const_iterator i;
177 std::map<NBEdge*, std::set<NBEdge*> > same;
187 if (newAll.size() < 2) {
196 std::map<NBEdge*, double> distances;
197 std::map<NBEdge*, bool> myExtended;
199 for (i = newAll.begin(); i != newAll.end(); ++i) {
200 EdgeVector::const_iterator cwi = i;
201 EdgeVector::const_iterator ccwi = i;
204 initNeighbors(newAll, i, geomsCW, geomsCCW, cwi, ccwi, cad, ccad);
205 assert(geomsCCW.find(*i) != geomsCCW.end());
206 assert(geomsCW.find(*ccwi) != geomsCW.end());
207 assert(geomsCW.find(*cwi) != geomsCW.end());
213 (simpleContinuation && fabs(ccad - cad) < (
double) 0.1)
216 || (!simpleContinuation && fabs(ccad - cad) <
DEG2RAD(22.5)))
220 if (myExtended.find(*ccwi) != myExtended.end()) {
221 p = geomsCCW[*ccwi][0];
222 p.
add(geomsCW[*ccwi][0]);
224 #ifdef DEBUG_NODE_SHAPE
226 std::cout <<
" extended: p=" << p <<
" angle=" << (ccad - cad) <<
"\n";
230 p = geomsCCW[*ccwi][0];
231 p.
add(geomsCW[*ccwi][0]);
232 p.
add(geomsCCW[*i][0]);
233 p.
add(geomsCW[*i][0]);
235 #ifdef DEBUG_NODE_SHAPE
237 std::cout <<
" unextended: p=" << p <<
" angle=" << (ccad - cad) <<
"\n";
243 geomsCCW[*i].nearest_offset_to_point2D(p),
244 geomsCW[*i].nearest_offset_to_point2D(p));
256 (*i)->setGeometry(g);
258 geomsCCW[*i] = (*i)->getCCWBoundaryLine(
myNode);
259 geomsCCW[*i].extrapolate(
EXT);
260 geomsCW[*i] = (*i)->getCWBoundaryLine(
myNode);
261 geomsCW[*i].extrapolate(
EXT);
264 myExtended[*i] =
true;
265 #ifdef DEBUG_NODE_SHAPE
267 std::cout <<
" extending (dist=" << dist <<
")\n";
271 if (!simpleContinuation) {
275 double radius2 = fabs(ccad - cad) * (*i)->getNumLanes();
277 radius2 =
MAX2(0.15, radius2);
280 #ifdef DEBUG_NODE_SHAPE
282 std::cout <<
" using radius=" << fabs(ccad - cad) * (*i)->getNumLanes() <<
" ccad=" << ccad <<
" cad=" << cad <<
"\n";
286 distances[*i] = dist;
292 const bool ccwCloser = ccad < cad;
294 const PositionVector& currGeom = ccwCloser ? geomsCCW[*i] : geomsCW[*i];
296 const PositionVector& currGeom2 = ccwCloser ? geomsCW[*i] : geomsCCW[*i];
298 const PositionVector& neighGeom = ccwCloser ? geomsCW[*ccwi] : geomsCCW[*cwi];
300 const PositionVector& neighGeom2 = ccwCloser ? geomsCCW[*cwi] : geomsCW[*ccwi];
301 #ifdef DEBUG_NODE_SHAPE
303 std::cout <<
" i=" << (*i)->getID() <<
" neigh=" << (*ccwi)->getID() <<
" neigh2=" << (*cwi)->getID() <<
"\n";
306 if (!simpleContinuation) {
309 #ifdef DEBUG_NODE_SHAPE
311 std::cout <<
" neigh intersects dist=" << distances[*i] <<
" currGeom=" << currGeom <<
" neighGeom=" << neighGeom <<
"\n";
314 if (*cwi != *ccwi && currGeom2.
intersects(neighGeom2)) {
317 const double farAngleDist = ccwCloser ? cad : ccad;
318 double a1 = distances[*i];
320 #ifdef DEBUG_NODE_SHAPE
322 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";
330 }
else if (farAngleDist <
DEG2RAD(135) || (fabs(
RAD2DEG(farAngleDist) - 180) > 1 && fabs(a2 - a1) < 10)) {
331 distances[*i] =
MAX2(a1, a2);
333 #ifdef DEBUG_NODE_SHAPE
335 std::cout <<
" a1=" << a1 <<
" a2=" << a2 <<
" dist=" << distances[*i] <<
"\n";
340 if (*cwi != *ccwi && currGeom2.
intersects(neighGeom2)) {
342 #ifdef DEBUG_NODE_SHAPE
344 std::cout <<
" neigh2 intersects dist=" << distances[*i] <<
" currGeom2=" << currGeom2 <<
" neighGeom2=" << neighGeom2 <<
"\n";
349 #ifdef DEBUG_NODE_SHAPE
351 std::cout <<
" no intersects dist=" << distances[*i] <<
" currGeom=" << currGeom <<
" neighGeom=" << neighGeom <<
" currGeom2=" << currGeom2 <<
" neighGeom2=" << neighGeom2 <<
"\n";
360 distances[*i] = (double)
EXT;
364 if (useDefaultRadius && sCurveStretch > 0) {
366 if (sCurveWidth > 0) {
367 const double sCurveRadius =
myRadius + sCurveWidth /
SUMO_const_laneWidth * sCurveStretch * pow((*i)->getSpeed(), 2 + sCurveStretch) / 1000;
368 const double stretch =
EXT + sCurveRadius - distances[*i];
370 distances[*i] += stretch;
372 const double shorten = distances[*i] -
EXT;
373 (*i)->shortenGeometryAtNode(&
myNode, shorten);
374 for (std::set<NBEdge*>::iterator k = same[*i].begin(); k != same[*i].end(); ++k) {
375 (*k)->shortenGeometryAtNode(&
myNode, shorten);
377 #ifdef DEBUG_NODE_SHAPE
379 std::cout <<
" stretching junction: sCurveWidth=" << sCurveWidth <<
" sCurveRadius=" << sCurveRadius <<
" stretch=" << stretch <<
" dist=" << distances[*i] <<
"\n";
387 for (i = newAll.begin(); i != newAll.end(); ++i) {
388 if (distances.find(*i) == distances.end()) {
396 for (i = newAll.begin(); i != newAll.end(); ++i) {
397 if (distances[*i] <
EXT && (*i)->hasDefaultGeometryEndpointAtNode(&
myNode)) {
398 for (EdgeVector::const_iterator j = newAll.begin(); j != newAll.end(); ++j) {
399 if (distances[*j] >
EXT && (*j)->hasDefaultGeometryEndpointAtNode(&
myNode) && distances[*i] + distances[*j] < minDistSum) {
401 if (angleDiff > 160 || angleDiff < 20) {
402 #ifdef DEBUG_NODE_SHAPE
404 std::cout <<
" increasing dist for i=" << (*i)->getID() <<
" because of j=" << (*j)->getID() <<
" jDist=" << distances[*j]
405 <<
" oldI=" << distances[*i] <<
" newI=" << minDistSum - distances[*j]
406 <<
" angleDiff=" << angleDiff
407 <<
" geomI=" << (*i)->getGeometry() <<
" geomJ=" << (*j)->getGeometry() <<
"\n";
410 distances[*i] = minDistSum - distances[*j];
420 for (i = newAll.begin(); i != newAll.end(); ++i) {
424 double offset = distances[*i];
425 if (!(*i)->hasDefaultGeometryEndpointAtNode(&
myNode)) {
427 if (advanceStopLine > 0 && offset <
EXT) {
428 #ifdef DEBUG_NODE_SHAPE
429 std::cout <<
" i=" << (*i)->getID() <<
" offset=" << offset <<
" advanceStopLine=" << advanceStopLine <<
"\n";
432 (*i)->extendGeometryAtNode(&
myNode, advanceStopLine);
433 for (std::set<NBEdge*>::iterator k = same[*i].begin(); k != same[*i].end(); ++k) {
434 (*k)->extendGeometryAtNode(&
myNode, advanceStopLine);
437 offset =
MAX2(
EXT - advanceStopLine, offset);
441 offset = (double) - .1;
445 if (i != newAll.begin()) {
453 #ifdef DEBUG_NODE_SHAPE
455 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";
458 (*i)->setNodeBorder(&
myNode, p, p2, rectangularCut);
459 for (std::set<NBEdge*>::iterator k = same[*i].begin(); k != same[*i].end(); ++k) {
460 (*k)->setNodeBorder(&
myNode, p, p2, rectangularCut);
464 ret.
append(
getSmoothCorner(geomsCW[*(newAll.end() - 1)], geomsCCW[*newAll.begin()], ret[-1], ret[0], cornerDetail));
472 double result = intersections[0];
473 for (std::vector<double>::iterator it = intersections.begin() + 1; it != intersections.end(); ++it) {
474 if (fabs(*it - offset) < fabs(result - offset)) {
486 if (cornerDetail > 0) {
489 #ifdef DEBUG_SMOOTH_CORNERS
491 std::cout <<
" begLength=" << begShape2.
length2D() <<
" begSplit=" << begSplit <<
"\n";
495 begShape2 = begShape2.
splitAt(begSplit,
true).first;
501 #ifdef DEBUG_SMOOTH_CORNERS
503 std::cout <<
" endLength=" << endShape2.
length2D() <<
" endSplit=" << endSplit <<
"\n";
507 endShape2 = endShape2.
splitAt(endSplit,
true).second;
514 #ifdef DEBUG_SMOOTH_CORNERS
516 std::cout <<
"getSmoothCorner begPoint=" << begPoint <<
" endPoint=" << endPoint
517 <<
" begShape=" << begShape <<
" endShape=" << endShape
518 <<
" begShape2=" << begShape2 <<
" endShape2=" << endShape2
522 if (begShape2.size() < 2 || endShape2.size() < 2) {
526 NBNode* recordError =
nullptr;
527 #ifdef DEBUG_SMOOTH_CORNERS
529 std::cout <<
" angle=" <<
RAD2DEG(angle) <<
"\n";
531 recordError = const_cast<NBNode*>(&
myNode);
540 #ifdef DEBUG_SMOOTH_CORNERS
542 std::cout <<
" curveLength=" << curve.
length2D() <<
" dist=" << begPoint.
distanceTo2D(endPoint) <<
" curvature=" << curvature <<
"\n";
545 if (curvature > 2 && angle >
DEG2RAD(85)) {
549 if (curve.size() > 2) {
550 curve.erase(curve.begin());
568 WRITE_WARNING(
"While computing intersection geometry at junction '" +
myNode.
getID() +
"': " + std::string(e.what()));
574 WRITE_WARNING(
"While computing intersection geometry at junction '" +
myNode.
getID() +
"': " + std::string(e.what()));
586 geomsCW[edge].extrapolate2D(
EXT,
true);
591 const double angleChangeLookahead = 35;
594 EdgeVector::const_iterator j;
600 const bool incoming = (*i)->getToNode() == &
myNode;
601 const bool incoming2 = (*j)->getToNode() == &
myNode;
602 const Position positionAtNode = (*i)->getGeometry()[incoming ? -1 : 0];
603 const Position positionAtNode2 = (*j)->getGeometry()[incoming2 ? -1 : 0];
606 const double angle1further = (g1.size() > 2 && g1[0].distanceTo2D(g1[1]) < angleChangeLookahead ?
608 const double angle2further = (g2.size() > 2 && g2[0].distanceTo2D(g2[1]) < angleChangeLookahead ?
612 const bool ambiguousGeometry = ((angleDiff > 0 && angleDiffFurther < 0) || (angleDiff < 0 && angleDiffFurther > 0));
613 const bool differentDirs = (incoming != incoming2);
618 #ifdef DEBUG_NODE_SHAPE
620 std::cout <<
" checkSameDirection " << (*i)->getID() <<
" " << (*j)->getID()
621 <<
" diffDirs=" << differentDirs
622 <<
" isOpposite=" << (differentDirs && foundOpposite.count(*i) == 0)
623 <<
" angleDiff=" << angleDiff
624 <<
" ambiguousGeometry=" << ambiguousGeometry
630 if (fabs(angleDiff) <
DEG2RAD(20)) {
631 const bool isOpposite = differentDirs && foundOpposite.count(*i) == 0;
633 foundOpposite.insert(*i);
634 foundOpposite.insert(*j);
638 for (std::set<NBEdge*>::iterator k = same[*i].begin(); k != same[*i].end(); ++k) {
644 for (std::set<NBEdge*>::iterator k = same[*j].begin(); k != same[*j].end(); ++k) {
652 #ifdef DEBUG_NODE_SHAPE
654 std::cout <<
" joinedSameDirectionEdges " << (*i)->getID() <<
" " << (*j)->getID() <<
" isOpposite=" << isOpposite <<
" ambiguousGeometry=" << ambiguousGeometry <<
"\n";
679 geom1 = geom1.reverse();
687 std::vector<double> distances = geom1.distances(geom2,
true);
690 const bool curvingTowards = geom1[0].distanceTo2D(geom2[0]) > minDistanceThreshold && minDist < minDistanceThreshold;
691 const bool onTop = maxDist -
POSITION_EPS < minDistanceThreshold;
692 geom1.extrapolate2D(
EXT);
694 Position intersect = geom1.intersectionPosition2D(geom2);
696 #ifdef DEBUG_NODE_SHAPE
698 std::cout <<
" badIntersect: onTop=" << onTop <<
" curveTo=" << curvingTowards <<
" intersects=" << intersects
699 <<
" geom1=" << geom1 <<
" geom2=" << geom2
700 <<
" intersectPos=" << intersect
704 return onTop || curvingTowards || !intersects;
710 std::map<
NBEdge*, std::set<NBEdge*> >& same,
718 auto e2NewAll = std::find(newAll.begin(), newAll.end(), e1);
719 #ifdef DEBUG_NODE_SHAPE
720 if (
DEBUGCOND) std::cout <<
"computeUniqueDirectionList e1=" << e1->getID()
721 <<
" deleted=" << (e2NewAll == newAll.end())
724 if (e2NewAll == newAll.end()) {
727 auto e1It = std::find(all.begin(), all.end(), e1);
733 for (
NBEdge* e2 : same[e1]) {
734 #ifdef DEBUG_NODE_SHAPE
736 std::cout <<
" e2=" << e2->getID() <<
"\n";
739 auto e2It = std::find(all.begin(), all.end(), e2);
740 if (e2It + 1 == bestCCW || (e2It == (all.end() - 1) && bestCCW == all.begin())) {
743 #ifdef DEBUG_NODE_SHAPE
745 std::cout <<
" bestCCW=" << e2->getID() <<
"\n";
748 }
else if (bestCW + 1 == e2It || (bestCW == (all.end() - 1) && e2It == all.begin())) {
751 #ifdef DEBUG_NODE_SHAPE
753 std::cout <<
" bestCW=" << e2->getID() <<
"\n";
759 if (bestCW != e1It) {
760 geomsCW[e1] = geomsCW[*bestCW];
763 if (bestCCW != e1It) {
764 geomsCCW[e1] = geomsCCW[*bestCCW];
768 for (
NBEdge* e2 : same[e1]) {
769 auto e2NewAll = std::find(newAll.begin(), newAll.end(), e2);
770 if (e2NewAll != newAll.end()) {
771 newAll.erase(e2NewAll);
775 #ifdef DEBUG_NODE_SHAPE
777 std::cout <<
" newAll:\n";
778 for (
NBEdge* e : newAll) {
779 std::cout <<
" " << e->getID() <<
" geomCCW=" << geomsCCW[e] <<
" geomsCW=" << geomsCW[e] <<
"\n";
791 EdgeVector::const_iterator& cwi,
792 EdgeVector::const_iterator& ccwi,
795 const double twoPI = (double)(2 *
M_PI);
798 if (cwi == edges.end()) {
799 std::advance(cwi, -((
int)edges.size()));
802 if (ccwi == edges.begin()) {
803 std::advance(ccwi, edges.size() - 1);
808 const double angleCurCCW = geomsCCW[*current].angleAt2D(0);
809 const double angleCurCW = geomsCW[*current].angleAt2D(0);
810 const double angleCCW = geomsCW[*ccwi].angleAt2D(0);
811 const double angleCW = geomsCCW[*cwi].angleAt2D(0);
812 ccad = angleCCW - angleCurCCW;
816 cad = angleCurCW - angleCW;
826 #ifdef DEBUG_NODE_SHAPE
828 std::cout <<
"computeNodeShapeSmall node=" <<
myNode.
getID() <<
"\n";
832 EdgeVector::const_iterator i;
837 Position delta = edgebound1[1] - edgebound1[0];
838 delta.
set(-delta.
y(), delta.
x());
841 edgebound1.extrapolate2D(500);
843 if (cross.intersects(edgebound1)) {
844 Position np = cross.intersectionPosition2D(edgebound1);
848 if (cross.intersects(edgebound2)) {
849 Position np = cross.intersectionPosition2D(edgebound2);
853 (*i)->resetNodeBorder(&
myNode);
864 const double radius = oc.
getFloat(
"default.junctions.radius");
865 const double smallRadius = oc.
getFloat(
"junctions.small-radius");
870 double maxRightAngle = 0;
871 double extraWidthRight = 0;
872 double maxLeftAngle = 0;
873 double extraWidthLeft = 0;
877 for (
int i = 0; i < in->getNumLanes(); i++) {
878 if ((in->getPermissions(i) & large) != 0) {
883 if ((in->getPermissions() & out->getPermissions() & large) != 0) {
888 in->getGeometry().angleAt2D(-2),
889 out->getGeometry().angleAt2D(0));
891 if (maxRightAngle < -angle) {
892 maxRightAngle = -angle;
896 if (maxLeftAngle < angle) {
897 maxLeftAngle = angle;
902 while (*pIn != out) {
903 extraWidthLeft += (*pIn)->getTotalWidth();
913 int wideLanesOut = 0;
914 for (
int i = 0; i < out->getNumLanes(); i++) {
915 if ((out->getPermissions(i) & large) != 0) {
919 laneDelta =
MAX2(laneDelta, abs(wideLanesOut - wideLanesIn));
925 double result = radius;
927 double maxTurnAngle = maxRightAngle;
928 double extraWidth = extraWidthRight;
929 if (maxRightAngle <
DEG2RAD(5)) {
930 maxTurnAngle = maxLeftAngle;
931 extraWidth = extraWidthLeft;
936 result =
MAX2(smallRadius, radius * tan(0.5 *
MIN2(0.5 *
M_PI, maxTurnAngle)) - extraWidth);
940 std::cout <<
"getDefaultRadius n=" <<
myNode.
getID() <<
" laneDelta=" << laneDelta
941 <<
" rightA=" <<
RAD2DEG(maxRightAngle)
942 <<
" leftA=" <<
RAD2DEG(maxLeftAngle)
943 <<
" maxA=" <<
RAD2DEG(maxTurnAngle)
944 <<
" extraWidth=" << extraWidth
945 <<
" result=" << result <<
"\n";
960 while (lane < e->getNumLanes() && (e->
getPermissions(lane) & exclude) == 0) {