41 #define DEBUGCOND (myNode.getID() == "C")
51 myRadius(node.getRadius()) {
63 bool singleDirection =
false;
65 singleDirection =
true;
69 singleDirection =
true;
72 #ifdef DEBUG_NODE_SHAPE
82 if (singleDirection) {
95 for (EdgeVector::const_iterator i = incoming.begin(); i != incoming.end(); ++i) {
96 double ia = (*i)->getAngleAtNode(&
myNode);
97 for (EdgeVector::const_iterator j = outgoing.begin(); j != outgoing.end(); ++j) {
98 double oa = (*j)->getAngleAtNode(&
myNode);
101 maxAngle =
MAX2(ad, maxAngle);
105 if (maxAngle > 22.5) {
113 if (ret.size() < 3) {
122 assert(l1[0].distanceTo2D(l1[1]) >=
EXT);
123 assert(l2[0].distanceTo2D(l2[1]) >=
EXT);
126 tmp.push_back(l1[1]);
128 tmp[1].set(-tmp[1].y(), tmp[1].x());
136 l2.erase(l2.begin(), l2.begin() + (l2.size() - tl2.size()));
154 const int cornerDetail = oc.
getInt(
"junctions.corner-detail");
155 const double sCurveStretch = oc.
getFloat(
"junctions.scurve-stretch");
156 const bool rectangularCut = oc.
getBool(
"rectangular-lane-cut");
157 const bool openDriveOutput = oc.
isSet(
"opendrive-output");
164 const double advanceStopLine = oc.
exists(
"opendrive-files") && oc.
isSet(
"opendrive-files") ? oc.
getFloat(
"opendrive.advance-stopline") : 0;
167 #ifdef DEBUG_NODE_SHAPE
169 std::cout <<
"\ncomputeNodeShapeDefault node " <<
myNode.
getID() <<
" simple=" << simpleContinuation <<
" useDefaultRadius=" << useDefaultRadius <<
" radius=" <<
myRadius <<
"\n";
174 EdgeVector::const_iterator i;
176 std::map<NBEdge*, std::set<NBEdge*> > same;
186 if (newAll.size() < 2) {
195 std::map<NBEdge*, double> distances;
196 std::map<NBEdge*, bool> myExtended;
198 for (i = newAll.begin(); i != newAll.end(); ++i) {
199 EdgeVector::const_iterator cwi = i;
200 EdgeVector::const_iterator ccwi = i;
203 initNeighbors(newAll, i, geomsCW, geomsCCW, cwi, ccwi, cad, ccad);
204 assert(geomsCCW.find(*i) != geomsCCW.end());
205 assert(geomsCW.find(*ccwi) != geomsCW.end());
206 assert(geomsCW.find(*cwi) != geomsCW.end());
212 (simpleContinuation && fabs(ccad - cad) < (
double) 0.1)
215 || (!simpleContinuation && fabs(ccad - cad) <
DEG2RAD(22.5)))
219 if (myExtended.find(*ccwi) != myExtended.end()) {
220 p = geomsCCW[*ccwi][0];
221 p.
add(geomsCW[*ccwi][0]);
223 #ifdef DEBUG_NODE_SHAPE
225 std::cout <<
" extended: p=" << p <<
" angle=" << (ccad - cad) <<
"\n";
229 p = geomsCCW[*ccwi][0];
230 p.
add(geomsCW[*ccwi][0]);
231 p.
add(geomsCCW[*i][0]);
232 p.
add(geomsCW[*i][0]);
234 #ifdef DEBUG_NODE_SHAPE
236 std::cout <<
" unextended: p=" << p <<
" angle=" << (ccad - cad) <<
"\n";
242 geomsCCW[*i].nearest_offset_to_point2D(p),
243 geomsCW[*i].nearest_offset_to_point2D(p));
255 (*i)->setGeometry(g);
257 geomsCCW[*i] = (*i)->getCCWBoundaryLine(
myNode);
258 geomsCCW[*i].extrapolate(
EXT);
259 geomsCW[*i] = (*i)->getCWBoundaryLine(
myNode);
260 geomsCW[*i].extrapolate(
EXT);
263 myExtended[*i] =
true;
264 #ifdef DEBUG_NODE_SHAPE
266 std::cout <<
" extending (dist=" << dist <<
")\n";
270 if (!simpleContinuation) {
274 double radius2 = fabs(ccad - cad) * (*i)->getNumLanes();
276 radius2 =
MAX2(0.15, radius2);
279 #ifdef DEBUG_NODE_SHAPE
281 std::cout <<
" using radius=" << fabs(ccad - cad) * (*i)->getNumLanes() <<
" ccad=" << ccad <<
" cad=" << cad <<
"\n";
285 distances[*i] = dist;
291 const bool ccwCloser = ccad < cad;
293 const PositionVector& currGeom = ccwCloser ? geomsCCW[*i] : geomsCW[*i];
295 const PositionVector& currGeom2 = ccwCloser ? geomsCW[*i] : geomsCCW[*i];
297 const PositionVector& neighGeom = ccwCloser ? geomsCW[*ccwi] : geomsCCW[*cwi];
299 const PositionVector& neighGeom2 = ccwCloser ? geomsCCW[*cwi] : geomsCW[*ccwi];
300 #ifdef DEBUG_NODE_SHAPE
302 std::cout <<
" i=" << (*i)->getID() <<
" neigh=" << (*ccwi)->getID() <<
" neigh2=" << (*cwi)->getID() <<
"\n";
305 if (!simpleContinuation) {
308 #ifdef DEBUG_NODE_SHAPE
310 std::cout <<
" neigh intersects dist=" << distances[*i] <<
" currGeom=" << currGeom <<
" neighGeom=" << neighGeom <<
"\n";
313 if (*cwi != *ccwi && currGeom2.
intersects(neighGeom2)) {
316 const double farAngleDist = ccwCloser ? cad : ccad;
317 double a1 = distances[*i];
319 #ifdef DEBUG_NODE_SHAPE
321 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";
329 }
else if (farAngleDist <
DEG2RAD(135) || (fabs(
RAD2DEG(farAngleDist) - 180) > 1 && fabs(a2 - a1) < 10)) {
330 distances[*i] =
MAX2(a1, a2);
332 #ifdef DEBUG_NODE_SHAPE
334 std::cout <<
" a1=" << a1 <<
" a2=" << a2 <<
" dist=" << distances[*i] <<
"\n";
339 if (*cwi != *ccwi && currGeom2.
intersects(neighGeom2)) {
341 #ifdef DEBUG_NODE_SHAPE
343 std::cout <<
" neigh2 intersects dist=" << distances[*i] <<
" currGeom2=" << currGeom2 <<
" neighGeom2=" << neighGeom2 <<
"\n";
348 #ifdef DEBUG_NODE_SHAPE
350 std::cout <<
" no intersects dist=" << distances[*i] <<
" currGeom=" << currGeom <<
" neighGeom=" << neighGeom <<
" currGeom2=" << currGeom2 <<
" neighGeom2=" << neighGeom2 <<
"\n";
359 distances[*i] = (double)
EXT;
363 if (useDefaultRadius && sCurveStretch > 0) {
365 if (sCurveWidth > 0) {
366 const double sCurveRadius =
myRadius + sCurveWidth /
SUMO_const_laneWidth * sCurveStretch * pow((*i)->getSpeed(), 2 + sCurveStretch) / 1000;
367 const double stretch =
EXT + sCurveRadius - distances[*i];
369 distances[*i] += stretch;
371 const double shorten = distances[*i] -
EXT;
372 (*i)->shortenGeometryAtNode(&
myNode, shorten);
373 for (std::set<NBEdge*>::iterator k = same[*i].begin(); k != same[*i].end(); ++k) {
374 (*k)->shortenGeometryAtNode(&
myNode, shorten);
376 #ifdef DEBUG_NODE_SHAPE
378 std::cout <<
" stretching junction: sCurveWidth=" << sCurveWidth <<
" sCurveRadius=" << sCurveRadius <<
" stretch=" << stretch <<
" dist=" << distances[*i] <<
"\n";
386 for (i = newAll.begin(); i != newAll.end(); ++i) {
387 if (distances.find(*i) == distances.end()) {
395 for (i = newAll.begin(); i != newAll.end(); ++i) {
396 if (distances[*i] <
EXT && (*i)->hasDefaultGeometryEndpointAtNode(&
myNode)) {
397 for (EdgeVector::const_iterator j = newAll.begin(); j != newAll.end(); ++j) {
398 if (distances[*j] >
EXT && (*j)->hasDefaultGeometryEndpointAtNode(&
myNode) && distances[*i] + distances[*j] < minDistSum) {
400 if (angleDiff > 160 || angleDiff < 20) {
401 #ifdef DEBUG_NODE_SHAPE
403 std::cout <<
" increasing dist for i=" << (*i)->getID() <<
" because of j=" << (*j)->getID() <<
" jDist=" << distances[*j]
404 <<
" oldI=" << distances[*i] <<
" newI=" << minDistSum - distances[*j]
405 <<
" angleDiff=" << angleDiff
406 <<
" geomI=" << (*i)->getGeometry() <<
" geomJ=" << (*j)->getGeometry() <<
"\n";
409 distances[*i] = minDistSum - distances[*j];
419 for (i = newAll.begin(); i != newAll.end(); ++i) {
423 double offset = distances[*i];
424 if (!(*i)->hasDefaultGeometryEndpointAtNode(&
myNode)) {
426 if (advanceStopLine > 0 && offset <
EXT) {
427 #ifdef DEBUG_NODE_SHAPE
428 std::cout <<
" i=" << (*i)->getID() <<
" offset=" << offset <<
" advanceStopLine=" << advanceStopLine <<
"\n";
431 (*i)->extendGeometryAtNode(&
myNode, advanceStopLine);
432 for (std::set<NBEdge*>::iterator k = same[*i].begin(); k != same[*i].end(); ++k) {
433 (*k)->extendGeometryAtNode(&
myNode, advanceStopLine);
436 offset =
MAX2(
EXT - advanceStopLine, offset);
440 offset = (double) - .1;
444 if (i != newAll.begin()) {
452 #ifdef DEBUG_NODE_SHAPE
454 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";
457 (*i)->setNodeBorder(&
myNode, p, p2, rectangularCut);
458 for (std::set<NBEdge*>::iterator k = same[*i].begin(); k != same[*i].end(); ++k) {
459 (*k)->setNodeBorder(&
myNode, p, p2, rectangularCut);
463 ret.
append(
getSmoothCorner(geomsCW[*(newAll.end() - 1)], geomsCCW[*newAll.begin()], ret[-1], ret[0], cornerDetail));
471 double result = intersections[0];
472 for (std::vector<double>::iterator it = intersections.begin() + 1; it != intersections.end(); ++it) {
473 if (fabs(*it - offset) < fabs(result - offset)) {
485 if (cornerDetail > 0) {
488 #ifdef DEBUG_SMOOTH_CORNERS
490 std::cout <<
" begLength=" << begShape2.
length2D() <<
" begSplit=" << begSplit <<
"\n";
494 begShape2 = begShape2.
splitAt(begSplit,
true).first;
500 #ifdef DEBUG_SMOOTH_CORNERS
502 std::cout <<
" endLength=" << endShape2.
length2D() <<
" endSplit=" << endSplit <<
"\n";
506 endShape2 = endShape2.
splitAt(endSplit,
true).second;
513 #ifdef DEBUG_SMOOTH_CORNERS
515 std::cout <<
"getSmoothCorner begPoint=" << begPoint <<
" endPoint=" << endPoint
516 <<
" begShape=" << begShape <<
" endShape=" << endShape
517 <<
" begShape2=" << begShape2 <<
" endShape2=" << endShape2
521 if (begShape2.size() < 2 || endShape2.size() < 2) {
525 NBNode* recordError =
nullptr;
526 #ifdef DEBUG_SMOOTH_CORNERS
528 std::cout <<
" angle=" <<
RAD2DEG(angle) <<
"\n";
539 #ifdef DEBUG_SMOOTH_CORNERS
541 std::cout <<
" curveLength=" << curve.
length2D() <<
" dist=" << begPoint.
distanceTo2D(endPoint) <<
" curvature=" << curvature <<
"\n";
544 if (curvature > 2 && angle >
DEG2RAD(85)) {
548 if (curve.size() > 2) {
549 curve.erase(curve.begin());
567 WRITE_WARNING(
"While computing intersection geometry at junction '" +
myNode.
getID() +
"': " + std::string(e.what()));
573 WRITE_WARNING(
"While computing intersection geometry at junction '" +
myNode.
getID() +
"': " + std::string(e.what()));
585 geomsCW[edge].extrapolate2D(
EXT,
true);
590 const double angleChangeLookahead = 35;
593 EdgeVector::const_iterator j;
599 const bool incoming = (*i)->getToNode() == &
myNode;
600 const bool incoming2 = (*j)->getToNode() == &
myNode;
601 const Position positionAtNode = (*i)->getGeometry()[incoming ? -1 : 0];
602 const Position positionAtNode2 = (*j)->getGeometry()[incoming2 ? -1 : 0];
605 const double angle1further = (g1.size() > 2 && g1[0].distanceTo2D(g1[1]) < angleChangeLookahead ?
607 const double angle2further = (g2.size() > 2 && g2[0].distanceTo2D(g2[1]) < angleChangeLookahead ?
611 const bool ambiguousGeometry = ((angleDiff > 0 && angleDiffFurther < 0) || (angleDiff < 0 && angleDiffFurther > 0));
612 const bool differentDirs = (incoming != incoming2);
617 #ifdef DEBUG_NODE_SHAPE
619 std::cout <<
" checkSameDirection " << (*i)->getID() <<
" " << (*j)->getID()
620 <<
" diffDirs=" << differentDirs
621 <<
" isOpposite=" << (differentDirs && foundOpposite.count(*i) == 0)
622 <<
" angleDiff=" << angleDiff
623 <<
" ambiguousGeometry=" << ambiguousGeometry
629 if (fabs(angleDiff) <
DEG2RAD(20)) {
630 const bool isOpposite = differentDirs && foundOpposite.count(*i) == 0;
632 foundOpposite.insert(*i);
633 foundOpposite.insert(*j);
637 for (std::set<NBEdge*>::iterator k = same[*i].begin(); k != same[*i].end(); ++k) {
643 for (std::set<NBEdge*>::iterator k = same[*j].begin(); k != same[*j].end(); ++k) {
651 #ifdef DEBUG_NODE_SHAPE
653 std::cout <<
" joinedSameDirectionEdges " << (*i)->getID() <<
" " << (*j)->getID() <<
" isOpposite=" << isOpposite <<
" ambiguousGeometry=" << ambiguousGeometry <<
"\n";
686 std::vector<double> distances = geom1.
distances(geom2,
true);
689 const bool curvingTowards = geom1[0].distanceTo2D(geom2[0]) > minDistanceThreshold && minDist < minDistanceThreshold;
690 const bool onTop = maxDist -
POSITION_EPS < minDistanceThreshold;
695 #ifdef DEBUG_NODE_SHAPE
697 std::cout <<
" badIntersect: onTop=" << onTop <<
" curveTo=" << curvingTowards <<
" intersects=" << intersects
698 <<
" geom1=" << geom1 <<
" geom2=" << geom2
699 <<
" intersectPos=" << intersect
703 return onTop || curvingTowards || !intersects;
709 std::map<
NBEdge*, std::set<NBEdge*> >& same,
717 auto e2NewAll = std::find(newAll.begin(), newAll.end(), e1);
718 #ifdef DEBUG_NODE_SHAPE
719 if (
DEBUGCOND) std::cout <<
"computeUniqueDirectionList e1=" << e1->getID()
720 <<
" deleted=" << (e2NewAll == newAll.end())
723 if (e2NewAll == newAll.end()) {
726 auto e1It = std::find(all.begin(), all.end(), e1);
732 for (
NBEdge* e2 : same[e1]) {
733 #ifdef DEBUG_NODE_SHAPE
735 std::cout <<
" e2=" << e2->getID() <<
"\n";
738 auto e2It = std::find(all.begin(), all.end(), e2);
739 if (e2It + 1 == bestCCW || (e2It == (all.end() - 1) && bestCCW == all.begin())) {
742 #ifdef DEBUG_NODE_SHAPE
744 std::cout <<
" bestCCW=" << e2->getID() <<
"\n";
747 }
else if (bestCW + 1 == e2It || (bestCW == (all.end() - 1) && e2It == all.begin())) {
750 #ifdef DEBUG_NODE_SHAPE
752 std::cout <<
" bestCW=" << e2->getID() <<
"\n";
758 if (bestCW != e1It) {
759 geomsCW[e1] = geomsCW[*bestCW];
762 if (bestCCW != e1It) {
763 geomsCCW[e1] = geomsCCW[*bestCCW];
767 for (
NBEdge* e2 : same[e1]) {
768 auto e2NewAll = std::find(newAll.begin(), newAll.end(), e2);
769 if (e2NewAll != newAll.end()) {
770 newAll.erase(e2NewAll);
774 #ifdef DEBUG_NODE_SHAPE
776 std::cout <<
" newAll:\n";
777 for (
NBEdge* e : newAll) {
778 std::cout <<
" " << e->getID() <<
" geomCCW=" << geomsCCW[e] <<
" geomsCW=" << geomsCW[e] <<
"\n";
790 EdgeVector::const_iterator& cwi,
791 EdgeVector::const_iterator& ccwi,
794 const double twoPI = (double)(2 *
M_PI);
797 if (cwi == edges.end()) {
798 std::advance(cwi, -((
int)edges.size()));
801 if (ccwi == edges.begin()) {
802 std::advance(ccwi, edges.size() - 1);
807 const double angleCurCCW = geomsCCW[*current].angleAt2D(0);
808 const double angleCurCW = geomsCW[*current].angleAt2D(0);
809 const double angleCCW = geomsCW[*ccwi].angleAt2D(0);
810 const double angleCW = geomsCCW[*cwi].angleAt2D(0);
811 ccad = angleCCW - angleCurCCW;
815 cad = angleCurCW - angleCW;
825 #ifdef DEBUG_NODE_SHAPE
827 std::cout <<
"computeNodeShapeSmall node=" <<
myNode.
getID() <<
"\n";
831 EdgeVector::const_iterator i;
836 Position delta = edgebound1[1] - edgebound1[0];
837 delta.
set(-delta.
y(), delta.
x());
840 edgebound1.extrapolate2D(500);
852 (*i)->resetNodeBorder(&
myNode);
863 const double radius = oc.
getFloat(
"default.junctions.radius");
864 const double smallRadius = oc.
getFloat(
"junctions.small-radius");
869 double maxRightAngle = 0;
870 double extraWidthRight = 0;
871 double maxLeftAngle = 0;
872 double extraWidthLeft = 0;
876 for (
int i = 0; i < in->getNumLanes(); i++) {
877 if ((in->getPermissions(i) & large) != 0) {
882 if ((in->getPermissions() & out->getPermissions() & large) != 0) {
887 in->getGeometry().angleAt2D(-2),
888 out->getGeometry().angleAt2D(0));
890 if (maxRightAngle < -angle) {
891 maxRightAngle = -angle;
895 if (maxLeftAngle < angle) {
896 maxLeftAngle = angle;
901 while (*pIn != out) {
902 extraWidthLeft += (*pIn)->getTotalWidth();
912 int wideLanesOut = 0;
913 for (
int i = 0; i < out->getNumLanes(); i++) {
914 if ((out->getPermissions(i) & large) != 0) {
918 laneDelta =
MAX2(laneDelta, abs(wideLanesOut - wideLanesIn));
924 double result = radius;
926 double maxTurnAngle = maxRightAngle;
927 double extraWidth = extraWidthRight;
928 if (maxRightAngle <
DEG2RAD(5)) {
929 maxTurnAngle = maxLeftAngle;
930 extraWidth = extraWidthLeft;
935 result =
MAX2(smallRadius, radius * tan(0.5 *
MIN2(0.5 *
M_PI, maxTurnAngle)) - extraWidth);
939 std::cout <<
"getDefaultRadius n=" <<
myNode.
getID() <<
" laneDelta=" << laneDelta
940 <<
" rightA=" <<
RAD2DEG(maxRightAngle)
941 <<
" leftA=" <<
RAD2DEG(maxLeftAngle)
942 <<
" maxA=" <<
RAD2DEG(maxTurnAngle)
943 <<
" extraWidth=" << extraWidth
944 <<
" result=" << result <<
"\n";
959 while (lane < e->getNumLanes() && (e->
getPermissions(lane) & exclude) == 0) {