44 #define DEBUGCOND true 46 #define MIN_TURN_DIAMETER 2.0 58 if (!oc.
isSet(
"opendrive-output")) {
63 const bool origNames = oc.
getBool(
"output.original-names");
64 const bool lefthand = oc.
getBool(
"lefthand");
65 const double straightThresh =
DEG2RAD(oc.
getFloat(
"opendrive-output.straight-threshold"));
68 int edgeID = nc.
size() * 10;
73 device <<
"<?xml version=\"1.0\" encoding=\"utf-8\"?>\n";
75 time_t now = time(
nullptr);
76 std::string dstr(ctime(&now));
79 device.openTag(
"header");
80 device.writeAttr(
"revMajor",
"1");
81 device.writeAttr(
"revMinor",
"4");
82 device.writeAttr(
"name",
"");
83 device.writeAttr(
"version",
"1.00");
84 device.writeAttr(
"date", dstr.substr(0, dstr.length() - 1));
85 device.writeAttr(
"north", b.
ymax());
86 device.writeAttr(
"south", b.
ymin());
87 device.writeAttr(
"east", b.
xmax());
88 device.writeAttr(
"west", b.
xmin());
99 device.openTag(
"geoReference");
100 device.writePreformattedTag(
" <![CDATA[\n " 110 for (std::map<std::string, NBEdge*>::const_iterator i = ec.
begin(); i != ec.
end(); ++i) {
111 const NBEdge* e = (*i).second;
116 fromNodeID, toNodeID,
117 origNames, straightThresh);
123 for (std::map<std::string, NBNode*>::const_iterator i = nc.
begin(); i != nc.
end(); ++i) {
125 int connectionID = 0;
126 const int nID =
getID(n->
getID(), nodeMap, nodeID);
128 junctionOSS <<
" <junction name=\"" << n->
getID() <<
"\" id=\"" << nID <<
"\">\n";
130 std::vector<NBEdge*> incoming = (*i).second->getIncomingEdges();
132 std::reverse(incoming.begin(), incoming.end());
134 for (
NBEdge* inEdge : incoming) {
135 std::string centerMark =
"none";
136 const int inEdgeID =
getID(inEdge->getID(), edgeMap, edgeID);
138 const NBEdge* outEdge =
nullptr;
139 bool isOuterEdge =
true;
140 int lastFromLane = -1;
141 std::vector<NBEdge::Connection> parallel;
142 std::vector<NBEdge::Connection> connections = inEdge->
getConnections();
144 std::reverse(connections.begin(), connections.end());
147 assert(c.toEdge != 0);
148 if (outEdge != c.toEdge || c.fromLane == lastFromLane) {
149 if (outEdge !=
nullptr) {
154 getID(parallel.back().getInternalLaneID(), edgeMap, edgeID),
156 getID(outEdge->getID(), edgeMap, edgeID),
158 parallel, isOuterEdge, straightThresh, centerMark);
164 lastFromLane = c.fromLane;
165 parallel.push_back(c);
170 if (!parallel.empty()) {
171 if (!lefthand && (n->
geometryLike() || inEdge->isTurningDirectionAt(outEdge))) {
172 centerMark =
"solid";
175 getID(parallel.back().getInternalLaneID(), edgeMap, edgeID),
177 getID(outEdge->getID(), edgeMap, edgeID),
179 parallel, isOuterEdge, straightThresh, centerMark);
184 junctionOSS <<
" </junction>\n";
191 for (std::map<std::string, NBNode*>::const_iterator i = nc.
begin(); i != nc.
end(); ++i) {
195 int numConnections = 0;
196 for (std::vector<NBEdge*>::const_iterator j = incoming.begin(); j != incoming.end(); ++j) {
197 numConnections += (int)((*j)->getConnections().size());
199 if (numConnections == 0) {
202 for (std::vector<NBEdge*>::const_iterator j = incoming.begin(); j != incoming.end(); ++j) {
203 const NBEdge* inEdge = *j;
204 const std::vector<NBEdge::Connection>& elv = inEdge->
getConnections();
205 for (std::vector<NBEdge::Connection>::const_iterator k = elv.begin(); k != elv.end(); ++k) {
208 if (outEdge ==
nullptr) {
222 int edgeID,
int fromNodeID,
int toNodeID,
223 const bool origNames,
224 const double straightThresh) {
232 planViewOSS.
openTag(
"planView");
234 const std::vector<NBEdge::Lane>& lanes = e->
getLanes();
236 #ifdef DEBUG_SMOOTH_GEOM 238 std::cout <<
"write planview for edge " << e->
getID() <<
"\n";
264 device.
writeAttr(
"elementType",
"junction");
265 device.
writeAttr(
"elementId", fromNodeID);
270 device.
writeAttr(
"elementType",
"junction");
279 device <<
" <lateralProfile/>\n";
280 device <<
" <lanes>\n";
281 device <<
" <laneSection s=\"0\">\n";
284 device <<
" <right>\n";
287 device <<
" <link/>\n";
295 device <<
" <width sOffset=\"0\" a=\"" << e->
getLaneWidth(j) <<
"\" b=\"0\" c=\"0\" d=\"0\"/>\n";
296 std::string markType =
"broken";
307 device <<
" <roadMark sOffset=\"0\" type=\"" << markType <<
"\" weight=\"standard\" color=\"standard\" width=\"0.13\"/>\n";
308 device <<
" <speed sOffset=\"0\" max=\"" << lanes[j].speed <<
"\"/>\n";
309 device <<
" </lane>\n";
311 device <<
" </right>\n";
312 device <<
" </laneSection>\n";
313 device <<
" </lanes>\n";
314 device <<
" <objects/>\n";
315 device <<
" <signals/>\n";
317 device <<
" <userData code=\"sumoId\" value=\"" << e->
getID() <<
"\"/>\n";
327 if (outEdge !=
nullptr 331 || parallel.front().fromLane != 0
332 || parallel.front().toLane != 0)) {
333 parallel.insert(parallel.begin(),
NBEdge::Connection(0, const_cast<NBEdge*>(outEdge), 0,
false));
334 parallel.front().vmax = (inEdge->
getLanes()[0].speed + outEdge->
getLanes()[0].speed) / (
double) 2.0;
341 int edgeID,
int inEdgeID,
int outEdgeID,
343 const std::vector<NBEdge::Connection>& parallel,
344 const bool isOuterEdge,
345 const double straightThresh,
346 const std::string& centerMark) {
347 assert(parallel.size() != 0);
349 const NBEdge* outEdge = cLeft.toEdge;
355 double laneOffset = 0;
357 fallBackShape.push_back(begShape.back());
358 fallBackShape.push_back(endShape.front());
362 if (init.size() == 0) {
366 WRITE_WARNING(
"Could not compute smooth shape from lane '" + inEdge->
getLaneID(cLeft.fromLane) +
"' to lane '" + outEdge->
getLaneID(cLeft.toLane) +
"'. Use option 'junctions.scurve-stretch' or increase radius of junction '" + inEdge->
getToNode()->
getID() +
"' to fix this.");
373 if (init.size() != 0) {
374 length =
bezier(init, 12).length2D();
380 length =
bezier(init, 12).length2D();
383 junctionDevice <<
" <connection id=\"" << connectionID <<
"\" incomingRoad=\"" << inEdgeID <<
"\" connectingRoad=\"" << edgeID <<
"\" contactPoint=\"start\">\n";
399 device.
writeAttr(
"elementId", outEdgeID);
400 device.
writeAttr(
"contactPoint",
"start");
408 #ifdef DEBUG_SMOOTH_GEOM 410 std::cout <<
"write planview for internal edge " << cLeft.id <<
" init=" << init <<
" fallback=" << fallBackShape
411 <<
" begShape=" << begShape <<
" endShape=" << endShape
415 if (init.size() == 0) {
423 device <<
" <lateralProfile/>\n";
424 device <<
" <lanes>\n";
425 if (laneOffset != 0) {
426 device <<
" <laneOffset s=\"0\" a=\"" << laneOffset <<
"\" b=\"0\" c=\"0\" d=\"0\"/>\n";
428 device <<
" <laneSection s=\"0\">\n";
430 device <<
" <right>\n";
431 for (
int j = (
int)parallel.size(); --j >= 0;) {
436 device <<
" <link>\n";
437 device <<
" <predecessor id=\"" << fromIndex <<
"\"/>\n";
438 device <<
" <successor id=\"" << toIndex <<
"\"/>\n";
439 device <<
" </link>\n";
440 device <<
" <width sOffset=\"0\" a=\"" << outEdge->
getLaneWidth(c.
toLane) <<
"\" b=\"0\" c=\"0\" d=\"0\"/>\n";
441 std::string markType =
"broken";
447 }
else if (isOuterEdge && j > 0
462 device <<
" <roadMark sOffset=\"0\" type=\"" << markType <<
"\" weight=\"standard\" color=\"standard\" width=\"0.13\"/>\n";
463 device <<
" <speed sOffset=\"0\" max=\"" << c.
vmax <<
"\"/>\n";
464 device <<
" </lane>\n";
466 junctionDevice <<
" <laneLink from=\"" << fromIndex <<
"\" to=\"" << toIndex <<
"\"/>\n";
469 device <<
" </right>\n";
470 device <<
" </laneSection>\n";
471 device <<
" </lanes>\n";
472 device <<
" <objects/>\n";
473 device <<
" <signals/>\n";
475 junctionDevice <<
" </connection>\n";
483 for (
int j = 0; j < (int)shape.size() - 1; ++j) {
496 elevationDevice <<
" <elevation s=\"" << offset <<
"\" a=\"" << p.
z() <<
"\" b=\"" << (p2.
z() - p.
z()) /
MAX2(
POSITION_EPS, length) <<
"\" c=\"0\" d=\"0\"/>\n";
505 device <<
" <center>\n";
506 device <<
" <lane id=\"0\" type=\"none\" level=\"true\">\n";
507 device <<
" <link/>\n";
508 device <<
" <roadMark sOffset=\"0\" type=\"" << mark <<
"\" weight=\"standard\" color=\"standard\" width=\"" << markWidth <<
"\"/>\n";
509 device <<
" </lane>\n";
510 device <<
" </center>\n";
517 return map.
get(origID);
519 map.
insert(origID, lastID++);
526 switch (permissions) {
545 if (permissions ==
SVCAll) {
562 if (laneIndex == -1) {
564 laneIndex = lefthand ? 0 : (int)edge->
getNumLanes() - 1;
573 const int leftmost = lefthand ? 0 : (int)edge->
getNumLanes() - 1;
577 for (
int i = leftmost; i < laneIndex; i++) {
581 for (
int i = leftmost; i > laneIndex; i--) {
605 assert(init.size() == 3 || init.size() == 4);
616 init.
add(-p.
x(), -p.
y(), -p.
z());
620 double aU, bU, cU, dU;
621 double aV, bV, cV, dV;
622 double aZ, bZ, cZ, dZ;
625 if (init.size() == 3) {
628 bU = 2 * init[1].x() - 2 * init[0].x();
629 cU = init[0].x() - 2 * init[1].x() + init[2].x();
633 bV = 2 * init[1].y() - 2 * init[0].y();
634 cV = init[0].y() - 2 * init[1].y() + init[2].y();
639 bZ = (2 * initZ[1].z() - 2 * initZ[0].z()) / length;
640 cZ = (initZ[0].z() - 2 * initZ[1].z() + initZ[2].z()) / (length * length);
646 bU = 3 * init[1].x() - 3 * init[0].x();
647 cU = 3 * init[0].x() - 6 * init[1].x() + 3 * init[2].x();
648 dU = -init[0].x() + 3 * init[1].x() - 3 * init[2].x() + init[3].x();
651 bV = 3 * init[1].y() - 3 * init[0].y();
652 cV = 3 * init[0].y() - 6 * init[1].y() + 3 * init[2].y();
653 dV = -init[0].y() + 3 * init[1].y() - 3 * init[2].y() + init[3].y();
657 bZ = (3 * initZ[1].z() - 3 * initZ[0].z()) / length;
658 cZ = (3 * initZ[0].z() - 6 * initZ[1].z() + 3 * initZ[2].z()) / (length * length);
659 dZ = (-initZ[0].z() + 3 * initZ[1].z() - 3 * initZ[2].z() + initZ[3].z()) / (length * length * length);
682 elevationDevice.
openTag(
"elevation");
690 return offset + length;
696 #ifdef DEBUG_SMOOTH_GEOM 698 std::cout <<
"writeGeomSmooth\n n=" << shape.size() <<
" shape=" <<
toString(shape) <<
"\n";
702 const double longThresh = speed;
703 const double curveCutout = longThresh / 2;
705 assert(longThresh >= 2 * curveCutout);
706 assert(shape.size() > 2);
710 double maxAngleDiff = 0;
712 for (
int j = 1; j < (int)shape.size() - 1; ++j) {
720 maxAngleDiff =
MAX2(maxAngleDiff, dAngle);
721 #ifdef DEBUG_SMOOTH_GEOM 723 std::cout <<
" j=" << j <<
" dAngle=" <<
RAD2DEG(dAngle) <<
" length1=" << length1 <<
" length2=" << length2 <<
"\n";
726 if (dAngle > straightThresh
727 && (length1 > longThresh || j == 1)
728 && (length2 > longThresh || j == (
int)shape.size() - 2)) {
731 shape2.removeClosest(p1);
735 const int numPoints = (int)shape2.size();
736 #ifdef DEBUG_SMOOTH_GEOM 738 std::cout <<
" n=" << numPoints <<
" shape2=" <<
toString(shape2) <<
"\n";
742 if (maxAngleDiff < straightThresh) {
744 #ifdef DEBUG_SMOOTH_GEOM 746 std::cout <<
" special case: all lines. maxAngleDiff=" << maxAngleDiff <<
"\n";
754 for (
int j = 0; j < numPoints - 1; ++j) {
760 const double lineLength = line.
length2D();
761 if (lineLength >= longThresh) {
763 #ifdef DEBUG_SMOOTH_GEOM 765 std::cout <<
" writeLine=" <<
toString(line) <<
"\n";
772 if (j == 0 || j == numPoints - 2) {
775 begShape.
add(p0 - begShape.back());
776 }
else if (j == 1 || p0.
distanceTo2D(shape2[j - 1]) > longThresh) {
778 begShape.push_back(shape2[j - 1]);
779 begShape.push_back(p0);
782 begShape.push_back(shape2[j - 1]);
783 begShape.push_back(p1);
784 begShape.
add(p0 - begShape.back());
787 if (j == 0 || j == numPoints - 2) {
790 endShape.
add(p1 - endShape.front());
791 }
else if (j == numPoints - 3 || p1.
distanceTo2D(shape2[j + 2]) > longThresh) {
793 endShape.push_back(p1);
794 endShape.push_back(shape2[j + 2]);
797 endShape.push_back(p0);
798 endShape.push_back(shape2[j + 2]);
799 endShape.
add(p1 - endShape.front());
801 const double extrapolateLength =
MIN2((
double)25, lineLength / 4);
803 if (init.size() == 0) {
806 #ifdef DEBUG_SMOOTH_GEOM 808 std::cout <<
" writeLine lineLength=" << lineLength <<
" begShape" << j <<
"=" <<
toString(begShape) <<
" endShape" << j <<
"=" <<
toString(endShape) <<
" init" << j <<
"=" <<
toString(init) <<
"\n";
813 const double curveLength =
bezier(init, 12).length2D();
814 offset =
writeGeomPP3(device, elevationDevice, init, curveLength, offset);
815 #ifdef DEBUG_SMOOTH_GEOM 817 std::cout <<
" writeCurve lineLength=" << lineLength <<
" curveLength=" << curveLength <<
" begShape" << j <<
"=" <<
toString(begShape) <<
" endShape" << j <<
"=" <<
toString(endShape) <<
" init" << j <<
"=" <<
toString(init) <<
"\n";
832 double z = shape.size() == 0 ? 0 : shape[0].z();
833 for (
int i = 1; i < (int)shape.size(); ++i) {
839 device <<
" <elevationProfile>\n";
841 device <<
" <elevation s=\"0\" a=\"" << z <<
"\" b=\"0\" c=\"0\" d=\"0\"/>\n";
845 device <<
" </elevationProfile>\n";
855 assert(shape0.size() >= 2);
859 stopLine.push_back(to);
862 for (
int lane = 1; lane < e->
getNumLanes(); ++lane) {
The link is a partial left direction.
double vmax
maximum velocity
OutputDevice & writeAttr(const SumoXMLAttr attr, const T &val)
writes a named attribute
double ymin() const
Returns minimum y-coordinate.
double length2D() const
Returns the length.
double xmax() const
Returns maximum x-coordinate.
A structure which describes a connection between edges or lanes.
int toLane
The lane the connections yields in.
int numNormalConnections() const
return the number of lane-to-lane connections at this junction (excluding crossings) ...
static void writeEmptyCenterLane(OutputDevice &device, const std::string &mark, double markWidth)
std::map< std::string, NBNode * >::const_iterator begin() const
Returns the pointer to the begin of the stored nodes.
double z() const
Returns the z-position.
double distance2D(const Position &p, bool perpendicular=false) const
closest 2D-distance to point p (or -1 if perpendicular is true and the point is beyond this vector) ...
NBEdge * toEdge
The edge the connections yields in.
EdgeVector getIncomingEdges() const
Returns the list of incoming edges unsorted.
const Boundary & getConvBoundary() const
Returns the converted boundary.
std::map< std::string, NBNode * >::const_iterator end() const
Returns the pointer to the end of the stored nodes.
vehicle is a not electrified rail
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
int gPrecision
the precision for floating point outputs
Position positionAtOffset2D(double pos, double lateralOffset=0) const
Returns the position at the given length.
static void writeNormalEdge(OutputDevice &device, const NBEdge *e, int edgeID, int fromNodeID, int toNodeID, const bool origNames, const double straightThresh)
write normal edge to device
std::string getString() const
Returns the current content as a string.
double y() const
Returns the y-position.
int SVCPermissions
bitset where each bit declares whether a certain SVC may use this edge/lane
bool usingGeoProjection() const
Returns whether a transformation from geo to metric coordinates will be performed.
The representation of a single edge during network building.
double x() const
Returns the x-position.
void setPrecision(int precision=gPrecision)
Sets the precison or resets it to default.
double angleTo2D(const Position &other) const
returns the angle in the plane of the vector pointing from here to the other position ...
const std::vector< NBEdge::Lane > & getLanes() const
Returns the lane definitions.
std::map< std::string, NBEdge * >::const_iterator end() const
Returns the pointer to the end of the stored edges.
bool getBool(const std::string &name) const
Returns the boolean-value of the named option (only for Option_Bool)
const std::string & getProjString() const
Returns the original projection definition.
const std::string & getID() const
Returns the id.
bool isRailway(SVCPermissions permissions)
Returns whether an edge with the given permission is a railway edge.
const SVCPermissions SVCAll
all VClasses are allowed
A class that stores a 2D geometrical boundary.
The link is a (hard) left direction.
vehicle is a (possibly fast moving) electric rail
#define WRITE_WARNING(msg)
static OptionsCont & getOptions()
Retrieves the options.
LinkDirection
The different directions a link between two lanes may take (or a stream between two edges)...
bool isSet(const std::string &name, bool failOnNonExistant=true) const
Returns the information whether the named option is set.
void insert(const std::string str, const T key, bool checkDuplicates=true)
static void writeNetwork(const OptionsCont &oc, NBNetBuilder &nb)
Writes the network into a openDRIVE-file.
static double writeGeomPP3(OutputDevice &device, OutputDevice &elevationDevice, PositionVector init, double length, double offset=0)
write geometry as a single bezier curve (paramPoly3)
std::map< std::string, NBEdge * >::const_iterator begin() const
Returns the pointer to the begin of the stored edges.
static methods for processing the coordinates conversion for the current net
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
static bool writeGeomSmooth(const PositionVector &shape, double speed, OutputDevice &device, OutputDevice &elevationDevice, double straightThresh, double &length)
int size() const
Returns the number of nodes stored in this container.
std::string getLaneID(int lane) const
get Lane ID (Secure)
int getNumLanes() const
Returns the number of lanes.
int fromLane
The lane the connections starts at.
A point in 2D or 3D with translation and scaling methods.
NBEdgeCont & getEdgeCont()
static PositionVector getRightLaneBorder(const NBEdge *edge, int laneIndex=-1)
T get(const std::string &str) const
std::string getString(const std::string &name) const
Returns the string-value of the named option (only for Option_String)
bool geometryLike() const
whether this is structurally similar to a geometry node
Storage for edges, including some functionality operating on multiple edges.
double xmin() const
Returns minimum x-coordinate.
The link is a (hard) right direction.
const std::string & getStreetName() const
Returns the street name of this edge.
static PositionVector getLeftLaneBorder(const NBEdge *edge, int laneIndex=-1, double widthOffset=0)
get the left border of the given lane (the leftmost one by default)
static PositionVector bezierControlPoints(const PositionVector &begShape, const PositionVector &endShape, bool isTurnaround, double extrapolateBeg, double extrapolateEnd, bool &ok, NBNode *recordError=0, double straightThresh=DEG2RAD(5), int shapeFlag=0)
get bezier control points
static std::string escapeXML(const std::string &orig, const bool maskDoubleHyphen=false)
Replaces the standard escapes by their XML entities.
The link is a partial right direction.
double getFloat(const std::string &name) const
Returns the double-value of the named option (only for Option_Float)
SVCPermissions getPermissions(int lane=-1) const
get the union of allowed classes over all lanes or for a specific lane
void move2side(double amount)
move position vector to side using certain ammount
vehicle is a passenger car (a "normal" car)
LinkDirection getDirection(const NBEdge *const incoming, const NBEdge *const outgoing, bool leftHand=false) const
Returns the representation of the described stream's direction.
double getSpeed() const
Returns the speed allowed on this edge.
double getLaneWidth() const
Returns the default width of lanes of this edge.
void rotate2D(double angle)
static std::string getLaneType(SVCPermissions permissions)
static double writeGeomLines(const PositionVector &shape, OutputDevice &device, OutputDevice &elevationDevice, double offset=0)
write geometry as sequence of lines (sumo style)
const PositionVector & getLaneShape(int i) const
Returns the shape of the nth lane.
const EdgeVector & getIncomingEdges() const
Returns this node's incoming edges (The edges which yield in this node)
const std::vector< Connection > & getConnections() const
Returns the connections.
NBNodeCont & getNodeCont()
Returns a reference to the node container.
Instance responsible for building networks.
static OutputDevice & getDevice(const std::string &name)
Returns the described OutputDevice.
static Position sideOffset(const Position &beg, const Position &end, const double amount)
get a side position of position vector using a offset
A storage for options typed value containers)
double angleAt2D(int pos) const
get angle in certain position of position vector
static const GeoConvHelper & getFinal()
the coordinate transformation for writing the location element and for tracking the original coordina...
const Position getOffsetBase() const
Returns the network base.
static void addPedestrianConnection(const NBEdge *inEdge, const NBEdge *outEdge, std::vector< NBEdge::Connection > ¶llel)
bool isTurningDirectionAt(const NBEdge *const edge) const
Returns whether the given edge is the opposite direction to this edge.
Represents a single node (junction) during network building.
Static storage of an output device and its base (abstract) implementation.
bool closeTag(const std::string &comment="")
Closes the most recently opened tag and optionally adds a comment.
NBNode * getFromNode() const
Returns the origin node of the edge.
Container for nodes during the netbuilding process.
bool hasString(const std::string &str) const
static double angleDiff(const double angle1, const double angle2)
Returns the difference of the second angle to the first angle in radiants.
static void checkLaneGeometries(const NBEdge *e)
check if the lane geometries are compatible with OpenDRIVE assumptions (colinear stop line) ...
void add(double xoff, double yoff, double zoff)
double ymax() const
Returns maximum y-coordinate.
static int writeInternalEdge(OutputDevice &device, OutputDevice &junctionDevice, const NBEdge *inEdge, int nodeID, int edgeID, int inEdgeID, int outEdgeID, int connectionID, const std::vector< NBEdge::Connection > ¶llel, const bool isOuterEdge, const double straightThresh, const std::string ¢erMark)
write internal edge to device, return next connectionID
static int getID(const std::string &origID, StringBijection< int > &map, int &lastID)
NBNode * getToNode() const
Returns the destination node of the edge.
static void writeElevationProfile(const PositionVector &shape, OutputDevice &device, const OutputDevice_String &elevationDevice)
OutputDevice & openTag(const std::string &xmlElement)
Opens an XML tag.
void lf()
writes a line feed if applicable
An output device that encapsulates an ofstream.
void bezier(int npts, double b[], int cpts, double p[])