47 #define DEBUGCOND (myNode.getID() == "disabled") 64 bool singleDirection =
false;
66 singleDirection =
true;
70 singleDirection =
true;
73 if (singleDirection) {
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);
92 maxAngle =
MAX2(ad, maxAngle);
96 if (maxAngle > 22.5) {
104 if (ret.size() < 3) {
113 assert(l1[0].distanceTo2D(l1[1]) >= 100.);
114 assert(l2[0].distanceTo2D(l2[1]) >= 100.);
117 tmp.push_back(l1[1]);
119 tmp[1].set(-tmp[1].y(), tmp[1].x());
127 l2.erase(l2.begin(), l2.begin() + (l2.size() - tl2.size()));
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");
154 const double advanceStopLine = oc.
exists(
"opendrive-files") && oc.
isSet(
"opendrive-files") ? oc.
getFloat(
"opendrive.advance-stopline") : 0;
157 #ifdef DEBUG_NODE_SHAPE 159 std::cout <<
"\ncomputeNodeShapeDefault node " <<
myNode.
getID() <<
" simple=" << simpleContinuation <<
" radius=" << radius <<
"\n";
164 EdgeVector::const_iterator i;
166 std::map<NBEdge*, std::set<NBEdge*> > same;
176 if (newAll.size() < 2) {
185 std::map<NBEdge*, double> distances;
186 std::map<NBEdge*, bool> myExtended;
188 for (i = newAll.begin(); i != newAll.end(); ++i) {
189 EdgeVector::const_iterator cwi = i;
190 EdgeVector::const_iterator ccwi = i;
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());
202 (simpleContinuation && fabs(ccad - cad) < (
double) 0.1)
205 || (!simpleContinuation && fabs(ccad - cad) <
DEG2RAD(22.5)))
209 if (myExtended.find(*ccwi) != myExtended.end()) {
210 p = geomsCCW[*ccwi][0];
211 p.
add(geomsCW[*ccwi][0]);
213 #ifdef DEBUG_NODE_SHAPE 215 std::cout <<
" extended: p=" << p <<
" angle=" << (ccad - cad) <<
"\n";
219 p = geomsCCW[*ccwi][0];
220 p.
add(geomsCW[*ccwi][0]);
221 p.
add(geomsCCW[*i][0]);
222 p.
add(geomsCW[*i][0]);
224 #ifdef DEBUG_NODE_SHAPE 226 std::cout <<
" unextended: p=" << p <<
" angle=" << (ccad - cad) <<
"\n";
232 geomsCCW[*i].nearest_offset_to_point2D(p),
233 geomsCW[*i].nearest_offset_to_point2D(p));
245 (*i)->setGeometry(g);
247 geomsCCW[*i] = (*i)->getCCWBoundaryLine(
myNode);
248 geomsCCW[*i].extrapolate(100);
249 geomsCW[*i] = (*i)->getCWBoundaryLine(
myNode);
250 geomsCW[*i].extrapolate(100);
253 myExtended[*i] =
true;
254 #ifdef DEBUG_NODE_SHAPE 256 std::cout <<
" extending (dist=" << dist <<
")\n";
260 if (!simpleContinuation) {
264 double radius2 = fabs(ccad - cad) * (*i)->getNumLanes();
266 radius2 =
MAX2(0.15, radius2);
269 #ifdef DEBUG_NODE_SHAPE 271 std::cout <<
" using radius=" << fabs(ccad - cad) * (*i)->getNumLanes() <<
" ccad=" << ccad <<
" cad=" << cad <<
"\n";
275 distances[*i] = dist;
281 const bool ccwCloser = ccad < cad;
283 const PositionVector& currGeom = ccwCloser ? geomsCCW[*i] : geomsCW[*i];
285 const PositionVector& currGeom2 = ccwCloser ? geomsCW[*i] : geomsCCW[*i];
287 const PositionVector& neighGeom = ccwCloser ? geomsCW[*ccwi] : geomsCCW[*cwi];
289 const PositionVector& neighGeom2 = ccwCloser ? geomsCCW[*cwi] : geomsCW[*ccwi];
290 #ifdef DEBUG_NODE_SHAPE 292 std::cout <<
" i=" << (*i)->getID() <<
" neigh=" << (*ccwi)->getID() <<
" neigh2=" << (*cwi)->getID() <<
"\n";
295 if (!simpleContinuation) {
298 #ifdef DEBUG_NODE_SHAPE 300 std::cout <<
" neigh intersects dist=" << distances[*i] <<
" currGeom=" << currGeom <<
" neighGeom=" << neighGeom <<
"\n";
303 if (*cwi != *ccwi && currGeom2.
intersects(neighGeom2)) {
306 const double farAngleDist = ccwCloser ? cad : ccad;
307 double a1 = distances[*i];
309 #ifdef DEBUG_NODE_SHAPE 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";
319 }
else if (farAngleDist <
DEG2RAD(135) || (fabs(
RAD2DEG(farAngleDist) - 180) > 1 && fabs(a2 - a1) < 10)) {
320 distances[*i] =
MAX2(a1, a2);
322 #ifdef DEBUG_NODE_SHAPE 324 std::cout <<
" a1=" << a1 <<
" a2=" << a2 <<
" dist=" << distances[*i] <<
"\n";
329 if (*cwi != *ccwi && currGeom2.
intersects(neighGeom2)) {
331 #ifdef DEBUG_NODE_SHAPE 333 std::cout <<
" neigh2 intersects dist=" << distances[*i] <<
" currGeom2=" << currGeom2 <<
" neighGeom2=" << neighGeom2 <<
"\n";
337 distances[*i] = 100 + radius;
338 #ifdef DEBUG_NODE_SHAPE 340 std::cout <<
" no intersects dist=" << distances[*i] <<
" currGeom=" << currGeom <<
" neighGeom=" << neighGeom <<
" currGeom2=" << currGeom2 <<
" neighGeom2=" << neighGeom2 <<
"\n";
349 distances[*i] = (double) 100.;
353 if (defaultRadius && sCurveStretch > 0) {
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];
359 distances[*i] += stretch;
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);
366 #ifdef DEBUG_NODE_SHAPE 368 std::cout <<
" stretching junction: sCurveWidth=" << sCurveWidth <<
" sCurveRadius=" << sCurveRadius <<
" stretch=" << stretch <<
" dist=" << distances[*i] <<
"\n";
376 for (i = newAll.begin(); i != newAll.end(); ++i) {
377 if (distances.find(*i) == distances.end()) {
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) {
390 if (angleDiff > 160 || angleDiff < 20) {
391 #ifdef DEBUG_NODE_SHAPE 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";
399 distances[*i] = minDistSum - distances[*j];
409 for (i = newAll.begin(); i != newAll.end(); ++i) {
413 double offset = distances[*i];
414 if (!(*i)->hasDefaultGeometryEndpointAtNode(&
myNode)) {
416 if (advanceStopLine > 0 && offset < 100) {
417 #ifdef DEBUG_NODE_SHAPE 418 std::cout <<
" i=" << (*i)->getID() <<
" offset=" << offset <<
" advanceStopLine=" << advanceStopLine <<
"\n";
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);
426 offset =
MAX2(100.0 - advanceStopLine, offset);
430 offset = (double) - .1;
434 if (i != newAll.begin()) {
442 #ifdef DEBUG_NODE_SHAPE 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";
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);
453 ret.
append(
getSmoothCorner(geomsCW[*(newAll.end() - 1)], geomsCCW[*newAll.begin()], ret[-1], ret[0], cornerDetail));
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)) {
475 if (cornerDetail > 0) {
477 begShape[-1] = begPoint;
478 endShape[0] = endPoint;
480 if (curve.size() > 2) {
481 curve.erase(curve.begin());
493 EdgeVector::const_iterator i, j;
498 geomsCCW[*i] = (*i)->getCCWBoundaryLine(
myNode);
500 WRITE_WARNING(std::string(
"While computing intersection geometry: ") + std::string(e.what()));
501 geomsCCW[*i] = (*i)->getGeometry();
504 geomsCW[*i] = (*i)->getCWBoundaryLine(
myNode);
506 WRITE_WARNING(std::string(
"While computing intersection geometry: ") + std::string(e.what()));
507 geomsCW[*i] = (*i)->getGeometry();
512 ? (*i)->getCCWBoundaryLine(
myNode)
513 : (*i)->getCWBoundaryLine(
myNode);
514 geomsCCW[*i].extrapolate2D(100,
true);
515 geomsCW[*i].extrapolate2D(100,
true);
518 geomsCCW[*j] = (*j)->getCCWBoundaryLine(
myNode);
519 geomsCW[*j] = (*j)->getCWBoundaryLine(
myNode);
522 ? (*j)->getCCWBoundaryLine(
myNode)
523 : (*j)->getCWBoundaryLine(
myNode);
524 geomsCCW[*j].extrapolate2D(100,
true);
525 geomsCW[*j].extrapolate2D(100,
true);
531 const double angleChangeLookahead = 35;
534 EdgeVector::const_iterator j;
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];
546 const double angle1further = (g1.size() > 2 && g1[0].distanceTo2D(g1[1]) < angleChangeLookahead ?
548 const double angle2further = (g2.size() > 2 && g2[0].distanceTo2D(g2[1]) < angleChangeLookahead ?
552 const bool ambiguousGeometry = ((angleDiff > 0 && angleDiffFurther < 0) || (angleDiff < 0 && angleDiffFurther > 0));
553 const bool differentDirs = (incoming != incoming2);
558 if (fabs(angleDiff) <
DEG2RAD(20)) {
559 const bool isOpposite = differentDirs && foundOpposite.count(*i) == 0;
561 foundOpposite.insert(*i);
562 foundOpposite.insert(*j);
564 if (isOpposite || ambiguousGeometry ||
badIntersection(*i, *j, geomsCW[*i], geomsCCW[*j], 100)) {
566 for (std::set<NBEdge*>::iterator k = same[*i].begin(); k != same[*i].end(); ++k) {
572 for (std::set<NBEdge*>::iterator k = same[*j].begin(); k != same[*j].end(); ++k) {
580 #ifdef DEBUG_NODE_SHAPE 582 std::cout <<
" joinedSameDirectionEdges " << (*i)->getID() <<
" " << (*j)->getID() <<
" isOpposite=" << isOpposite <<
" ambiguousGeometry=" << ambiguousGeometry <<
"\n";
609 geom1 = geom1.reverse();
616 std::vector<double> distances = geom1.
distances(geom2,
true);
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;
629 std::map<
NBEdge*, std::set<NBEdge*> >& same,
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()) {
644 geomsCW[*i2] = geomsCW[*j];
649 geomsCCW[*i2] = geomsCCW[*j];
670 EdgeVector::const_iterator& cwi,
671 EdgeVector::const_iterator& ccwi,
674 const double twoPI = (double)(2 *
M_PI);
677 if (cwi == edges.end()) {
678 std::advance(cwi, -((
int)edges.size()));
681 if (ccwi == edges.begin()) {
682 std::advance(ccwi, edges.size() - 1);
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;
695 cad = angleCurCW - angleCW;
705 #ifdef DEBUG_NODE_SHAPE 707 std::cout <<
"computeNodeShapeSmall node=" <<
myNode.
getID() <<
"\n";
711 EdgeVector::const_iterator i;
716 Position delta = edgebound1[1] - edgebound1[0];
717 delta.
set(-delta.
y(), delta.
x());
720 edgebound1.extrapolate2D(500);
722 if (cross.intersects(edgebound1)) {
723 Position np = cross.intersectionPosition2D(edgebound1);
727 if (cross.intersects(edgebound2)) {
728 Position np = cross.intersectionPosition2D(edgebound2);
732 (*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