46 #define DEBUGCOND (myNode.getID() == "disabled") 63 bool singleDirection =
false;
65 singleDirection =
true;
69 singleDirection =
true;
72 if (singleDirection) {
85 for (EdgeVector::const_iterator i = incoming.begin(); i != incoming.end(); ++i) {
86 double ia = (*i)->getAngleAtNode(&
myNode);
87 for (EdgeVector::const_iterator j = outgoing.begin(); j != outgoing.end(); ++j) {
88 double oa = (*j)->getAngleAtNode(&
myNode);
91 maxAngle =
MAX2(ad, maxAngle);
95 if (maxAngle > 22.5) {
103 if (ret.size() < 3) {
112 assert(l1[0].distanceTo2D(l1[1]) >= 100.);
113 assert(l2[0].distanceTo2D(l2[1]) >= 100.);
116 tmp.push_back(l1[1]);
118 tmp[1].set(-tmp[1].y(), tmp[1].x());
126 l2.erase(l2.begin(), l2.begin() + (l2.size() - tl2.size()));
143 const int cornerDetail = oc.
getInt(
"junctions.corner-detail");
144 const double sCurveStretch = oc.
getFloat(
"junctions.scurve-stretch");
145 const bool rectangularCut = oc.
getBool(
"rectangular-lane-cut");
146 const bool openDriveOutput = oc.
isSet(
"opendrive-output");
153 const double advanceStopLine = oc.
exists(
"opendrive-files") && oc.
isSet(
"opendrive-files") ? oc.
getFloat(
"opendrive.advance-stopline") : 0;
156 #ifdef DEBUG_NODE_SHAPE 158 std::cout <<
"\ncomputeNodeShapeDefault node " <<
myNode.
getID() <<
" simple=" << simpleContinuation <<
" radius=" << radius <<
"\n";
163 EdgeVector::const_iterator i;
165 std::map<NBEdge*, std::set<NBEdge*> > same;
175 if (newAll.size() < 2) {
184 std::map<NBEdge*, double> distances;
185 std::map<NBEdge*, bool> myExtended;
187 for (i = newAll.begin(); i != newAll.end(); ++i) {
188 EdgeVector::const_iterator cwi = i;
189 EdgeVector::const_iterator ccwi = i;
192 initNeighbors(newAll, i, geomsCW, geomsCCW, cwi, ccwi, cad, ccad);
193 assert(geomsCCW.find(*i) != geomsCCW.end());
194 assert(geomsCW.find(*ccwi) != geomsCW.end());
195 assert(geomsCW.find(*cwi) != geomsCW.end());
201 (simpleContinuation && fabs(ccad - cad) < (
double) 0.1)
204 || (!simpleContinuation && fabs(ccad - cad) <
DEG2RAD(22.5)))
208 if (myExtended.find(*ccwi) != myExtended.end()) {
209 p = geomsCCW[*ccwi][0];
210 p.
add(geomsCW[*ccwi][0]);
212 #ifdef DEBUG_NODE_SHAPE 214 std::cout <<
" extended: p=" << p <<
" angle=" << (ccad - cad) <<
"\n";
218 p = geomsCCW[*ccwi][0];
219 p.
add(geomsCW[*ccwi][0]);
220 p.
add(geomsCCW[*i][0]);
221 p.
add(geomsCW[*i][0]);
223 #ifdef DEBUG_NODE_SHAPE 225 std::cout <<
" unextended: p=" << p <<
" angle=" << (ccad - cad) <<
"\n";
231 geomsCCW[*i].nearest_offset_to_point2D(p),
232 geomsCW[*i].nearest_offset_to_point2D(p));
244 (*i)->setGeometry(g);
246 geomsCCW[*i] = (*i)->getCCWBoundaryLine(
myNode);
247 geomsCCW[*i].extrapolate(100);
248 geomsCW[*i] = (*i)->getCWBoundaryLine(
myNode);
249 geomsCW[*i].extrapolate(100);
252 myExtended[*i] =
true;
253 #ifdef DEBUG_NODE_SHAPE 255 std::cout <<
" extending (dist=" << dist <<
")\n";
259 if (!simpleContinuation) {
263 double radius2 = fabs(ccad - cad) * (*i)->getNumLanes();
265 radius2 =
MAX2(0.15, radius2);
268 #ifdef DEBUG_NODE_SHAPE 270 std::cout <<
" using radius=" << fabs(ccad - cad) * (*i)->getNumLanes() <<
" ccad=" << ccad <<
" cad=" << cad <<
"\n";
274 distances[*i] = dist;
280 const bool ccwCloser = ccad < cad;
282 const PositionVector& currGeom = ccwCloser ? geomsCCW[*i] : geomsCW[*i];
284 const PositionVector& currGeom2 = ccwCloser ? geomsCW[*i] : geomsCCW[*i];
286 const PositionVector& neighGeom = ccwCloser ? geomsCW[*ccwi] : geomsCCW[*cwi];
288 const PositionVector& neighGeom2 = ccwCloser ? geomsCCW[*cwi] : geomsCW[*ccwi];
289 #ifdef DEBUG_NODE_SHAPE 291 std::cout <<
" i=" << (*i)->getID() <<
" neigh=" << (*ccwi)->getID() <<
" neigh2=" << (*cwi)->getID() <<
"\n";
294 if (!simpleContinuation) {
297 #ifdef DEBUG_NODE_SHAPE 299 std::cout <<
" neigh intersects dist=" << distances[*i] <<
" currGeom=" << currGeom <<
" neighGeom=" << neighGeom <<
"\n";
302 if (*cwi != *ccwi && currGeom2.
intersects(neighGeom2)) {
305 const double farAngleDist = ccwCloser ? cad : ccad;
306 double a1 = distances[*i];
308 #ifdef DEBUG_NODE_SHAPE 310 std::cout <<
" neigh2 also intersects a1=" << a1 <<
" a2=" << a2 <<
" ccad=" <<
RAD2DEG(ccad) <<
" cad=" <<
RAD2DEG(cad) <<
" dist[cwi]=" << distances[*cwi] <<
" dist[ccwi]=" << distances[*ccwi] <<
" farAngleDist=" <<
RAD2DEG(farAngleDist) <<
" currGeom2=" << currGeom2 <<
" neighGeom2=" << neighGeom2 <<
"\n";
318 }
else if (farAngleDist <
DEG2RAD(135) || (fabs(
RAD2DEG(farAngleDist) - 180) > 1 && fabs(a2 - a1) < 10)) {
319 distances[*i] =
MAX2(a1, a2);
321 #ifdef DEBUG_NODE_SHAPE 323 std::cout <<
" a1=" << a1 <<
" a2=" << a2 <<
" dist=" << distances[*i] <<
"\n";
328 if (*cwi != *ccwi && currGeom2.
intersects(neighGeom2)) {
330 #ifdef DEBUG_NODE_SHAPE 332 std::cout <<
" neigh2 intersects dist=" << distances[*i] <<
" currGeom2=" << currGeom2 <<
" neighGeom2=" << neighGeom2 <<
"\n";
336 distances[*i] = 100 + radius;
337 #ifdef DEBUG_NODE_SHAPE 339 std::cout <<
" no intersects dist=" << distances[*i] <<
" currGeom=" << currGeom <<
" neighGeom=" << neighGeom <<
" currGeom2=" << currGeom2 <<
" neighGeom2=" << neighGeom2 <<
"\n";
348 distances[*i] = (double) 100.;
352 if (defaultRadius && sCurveStretch > 0) {
354 if (sCurveWidth > 0) {
355 const double sCurveRadius = radius + sCurveWidth /
SUMO_const_laneWidth * sCurveStretch * pow((*i)->getSpeed(), 2 + sCurveStretch) / 1000;
356 const double stretch = 100 + sCurveRadius - distances[*i];
358 distances[*i] += stretch;
360 const double shorten = distances[*i] - 100;
361 (*i)->shortenGeometryAtNode(&
myNode, shorten);
362 for (std::set<NBEdge*>::iterator k = same[*i].begin(); k != same[*i].end(); ++k) {
363 (*k)->shortenGeometryAtNode(&
myNode, shorten);
365 #ifdef DEBUG_NODE_SHAPE 367 std::cout <<
" stretching junction: sCurveWidth=" << sCurveWidth <<
" sCurveRadius=" << sCurveRadius <<
" stretch=" << stretch <<
" dist=" << distances[*i] <<
"\n";
375 for (i = newAll.begin(); i != newAll.end(); ++i) {
376 if (distances.find(*i) == distances.end()) {
383 const double minDistSum = 2 * (100 + radius);
384 for (i = newAll.begin(); i != newAll.end(); ++i) {
385 if (distances[*i] < 100 && (*i)->hasDefaultGeometryEndpointAtNode(&
myNode)) {
386 for (EdgeVector::const_iterator j = newAll.begin(); j != newAll.end(); ++j) {
387 if (distances[*j] > 100 && (*j)->hasDefaultGeometryEndpointAtNode(&
myNode) && distances[*i] + distances[*j] < minDistSum) {
389 if (angleDiff > 160 || angleDiff < 20) {
390 #ifdef DEBUG_NODE_SHAPE 392 std::cout <<
" increasing dist for i=" << (*i)->getID() <<
" because of j=" << (*j)->getID() <<
" jDist=" << distances[*j]
393 <<
" oldI=" << distances[*i] <<
" newI=" << minDistSum - distances[*j]
394 <<
" angleDiff=" << angleDiff
395 <<
" geomI=" << (*i)->getGeometry() <<
" geomJ=" << (*j)->getGeometry() <<
"\n";
398 distances[*i] = minDistSum - distances[*j];
408 for (i = newAll.begin(); i != newAll.end(); ++i) {
412 double offset = distances[*i];
413 if (!(*i)->hasDefaultGeometryEndpointAtNode(&
myNode)) {
415 if (advanceStopLine > 0 && offset < 100) {
416 #ifdef DEBUG_NODE_SHAPE 417 std::cout <<
" i=" << (*i)->getID() <<
" offset=" << offset <<
" advanceStopLine=" << advanceStopLine <<
"\n";
420 (*i)->extendGeometryAtNode(&
myNode, advanceStopLine);
421 for (std::set<NBEdge*>::iterator k = same[*i].begin(); k != same[*i].end(); ++k) {
422 (*k)->extendGeometryAtNode(&
myNode, advanceStopLine);
425 offset =
MAX2(100.0 - advanceStopLine, offset);
429 offset = (double) - .1;
433 if (i != newAll.begin()) {
441 #ifdef DEBUG_NODE_SHAPE 443 std::cout <<
" build stopLine for i=" << (*i)->getID() <<
" offset=" << offset <<
" dist=" << distances[*i] <<
" cwLength=" << cwBound.
length2D() <<
" ccwLength=" << ccwBound.
length2D() <<
" p=" << p <<
" p2=" << p2 <<
" ccwBound=" << ccwBound <<
" cwBound=" << cwBound <<
"\n";
446 (*i)->setNodeBorder(&
myNode, p, p2, rectangularCut);
447 for (std::set<NBEdge*>::iterator k = same[*i].begin(); k != same[*i].end(); ++k) {
448 (*k)->setNodeBorder(&
myNode, p, p2, rectangularCut);
452 ret.
append(
getSmoothCorner(geomsCW[*(newAll.end() - 1)], geomsCCW[*newAll.begin()], ret[-1], ret[0], cornerDetail));
460 double result = intersections[0];
461 for (std::vector<double>::iterator it = intersections.begin() + 1; it != intersections.end(); ++it) {
462 if (fabs(*it - offset) < fabs(result - offset)) {
474 if (cornerDetail > 0) {
476 begShape[-1] = begPoint;
477 endShape[0] = endPoint;
479 if (curve.size() > 2) {
480 curve.erase(curve.begin());
492 EdgeVector::const_iterator i, j;
497 geomsCCW[*i] = (*i)->getCCWBoundaryLine(
myNode);
499 WRITE_WARNING(std::string(
"While computing intersection geometry: ") + std::string(e.what()));
500 geomsCCW[*i] = (*i)->getGeometry();
503 geomsCW[*i] = (*i)->getCWBoundaryLine(
myNode);
505 WRITE_WARNING(std::string(
"While computing intersection geometry: ") + std::string(e.what()));
506 geomsCW[*i] = (*i)->getGeometry();
511 ? (*i)->getCCWBoundaryLine(
myNode)
512 : (*i)->getCWBoundaryLine(
myNode);
513 geomsCCW[*i].extrapolate2D(100,
true);
514 geomsCW[*i].extrapolate2D(100,
true);
517 geomsCCW[*j] = (*j)->getCCWBoundaryLine(
myNode);
518 geomsCW[*j] = (*j)->getCWBoundaryLine(
myNode);
521 ? (*j)->getCCWBoundaryLine(
myNode)
522 : (*j)->getCWBoundaryLine(
myNode);
523 geomsCCW[*j].extrapolate2D(100,
true);
524 geomsCW[*j].extrapolate2D(100,
true);
530 const double angleChangeLookahead = 35;
533 EdgeVector::const_iterator j;
539 const bool incoming = (*i)->getToNode() == &
myNode;
540 const bool incoming2 = (*j)->getToNode() == &
myNode;
541 const Position positionAtNode = (*i)->getGeometry()[incoming ? -1 : 0];
542 const Position positionAtNode2 = (*j)->getGeometry()[incoming2 ? -1 : 0];
545 const double angle1further = (g1.size() > 2 && g1[0].distanceTo2D(g1[1]) < angleChangeLookahead ?
547 const double angle2further = (g2.size() > 2 && g2[0].distanceTo2D(g2[1]) < angleChangeLookahead ?
551 const bool ambiguousGeometry = ((angleDiff > 0 && angleDiffFurther < 0) || (angleDiff < 0 && angleDiffFurther > 0));
552 const bool differentDirs = (incoming != incoming2);
557 if (fabs(angleDiff) <
DEG2RAD(20)) {
558 const bool isOpposite = differentDirs && foundOpposite.count(*i) == 0;
560 foundOpposite.insert(*i);
561 foundOpposite.insert(*j);
563 if (isOpposite || ambiguousGeometry ||
badIntersection(*i, *j, geomsCW[*i], geomsCCW[*j], 100)) {
565 for (std::set<NBEdge*>::iterator k = same[*i].begin(); k != same[*i].end(); ++k) {
571 for (std::set<NBEdge*>::iterator k = same[*j].begin(); k != same[*j].end(); ++k) {
579 #ifdef DEBUG_NODE_SHAPE 581 std::cout <<
" joinedSameDirectionEdges " << (*i)->getID() <<
" " << (*j)->getID() <<
" isOpposite=" << isOpposite <<
" ambiguousGeometry=" << ambiguousGeometry <<
"\n";
608 geom1 = geom1.reverse();
615 std::vector<double> distances = geom1.
distances(geom2,
true);
619 const bool onTop = maxDist -
POSITION_EPS < minDistanceThreshold;
620 const bool curvingTowards = geom1[0].distanceTo2D(geom2[0]) > minDistanceThreshold && minDist < minDistanceThreshold;
621 const bool intersects = e1cw.
intersects(e2ccw);
622 return onTop || curvingTowards || !intersects;
628 std::map<
NBEdge*, std::set<NBEdge*> >& same,
636 for (EdgeVector::iterator i2 = newAll.begin(); i2 != newAll.end(); ++i2) {
637 std::set<NBEdge*> other = same[*i2];
638 for (std::set<NBEdge*>::const_iterator j = other.begin(); j != other.end(); ++j) {
639 EdgeVector::iterator k = find(newAll.begin(), newAll.end(), *j);
640 if (k != newAll.end()) {
643 geomsCW[*i2] = geomsCW[*j];
648 geomsCCW[*i2] = geomsCCW[*j];
669 EdgeVector::const_iterator& cwi,
670 EdgeVector::const_iterator& ccwi,
673 const double twoPI = (double)(2 *
M_PI);
676 if (cwi == edges.end()) {
677 std::advance(cwi, -((
int)edges.size()));
680 if (ccwi == edges.begin()) {
681 std::advance(ccwi, edges.size() - 1);
686 const double angleCurCCW = geomsCCW[*current].angleAt2D(0);
687 const double angleCurCW = geomsCW[*current].angleAt2D(0);
688 const double angleCCW = geomsCW[*ccwi].angleAt2D(0);
689 const double angleCW = geomsCCW[*cwi].angleAt2D(0);
690 ccad = angleCCW - angleCurCCW;
694 cad = angleCurCW - angleCW;
704 #ifdef DEBUG_NODE_SHAPE 706 std::cout <<
"computeNodeShapeSmall node=" <<
myNode.
getID() <<
"\n";
710 EdgeVector::const_iterator i;
715 Position delta = edgebound1[1] - edgebound1[0];
716 delta.
set(-delta.
y(), delta.
x());
719 edgebound1.extrapolate2D(500);
721 if (cross.intersects(edgebound1)) {
722 Position np = cross.intersectionPosition2D(edgebound1);
726 if (cross.intersects(edgebound2)) {
727 Position np = cross.intersectionPosition2D(edgebound2);
731 (*i)->resetNodeBorder(&
myNode);
static double relAngle(double angle1, double angle2)
computes the relative angle between the two angles
static double getMinAngleDiff(double angle1, double angle2)
Returns the minimum distance (clockwise/counter-clockwise) between both angles.
LaneSpreadFunction getLaneSpreadFunction() const
Returns how this edge's lanes' lateral offset is computed.
static void initNeighbors(const EdgeVector &edges, const EdgeVector::const_iterator ¤t, 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.
void add(const Position &pos)
Adds the given position to this one.
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
double y() const
Returns the y-position.
The representation of a single edge during network building.
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
double x() const
Returns the x-position.
static T minValue(const std::vector< T > &v)
const NBNode & myNode
The node to compute the geometry for.
PositionVector reverse() const
reverse position vector
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.
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
bool badIntersection(const NBEdge *e1, const NBEdge *e2, const PositionVector &e1cw, const PositionVector &e2ccw, double distance)
#define WRITE_WARNING(msg)
static OptionsCont & getOptions()
Retrieves the options.
const EdgeVector & getOutgoingEdges() const
Returns this node's outgoing edges (The edges which start at this node)
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.
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
A point in 2D or 3D with translation and scaling methods.
bool exists(const std::string &name) const
Returns the information whether the named option is known.
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 ...
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.
double getDisplacementError() const
compute the displacement error during s-curve computation
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.
const PositionVector & getGeometry() const
Returns the geometry of the edge.
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.
const EdgeVector & getIncomingEdges() const
Returns this node's incoming edges (The edges which yield in this node)
std::vector< NBEdge * > EdgeVector
container for (sorted) edges
double getTotalWidth() const
Returns the combined width of all lanes of this edge.
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)
double angleAt2D(int pos) const
get angle in certain position of position vector
const Position & getPosition() const
Represents a single node (junction) during network building.
EdgeVector computeUniqueDirectionList(std::map< NBEdge *, std::set< NBEdge *> > &same, GeomsMap &geomsCCW, GeomsMap &geomsCW)
Joins edges and computes ccw/cw boundaries.
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.
static T maxValue(const std::vector< T > &v)
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.
void add(double xoff, double yoff, double zoff)
NBNode * getToNode() const
Returns the destination node of the edge.
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
void setz(double z)
set position z