50 #define DEBUGCOND true 52 #define MIN_TURN_DIAMETER 2.0 64 if (!oc.
isSet(
"opendrive-output")) {
69 const bool origNames = oc.
getBool(
"output.original-names");
70 const double straightThresh =
DEG2RAD(oc.
getFloat(
"opendrive-output.straight-threshold"));
73 int edgeID = nc.
size() * 10;
78 device <<
"<?xml version=\"1.0\" encoding=\"utf-8\"?>\n";
81 std::string dstr(ctime(&now));
84 device.openTag(
"header");
85 device.writeAttr(
"revMajor",
"1");
86 device.writeAttr(
"revMinor",
"4");
87 device.writeAttr(
"name",
"");
88 device.writeAttr(
"version",
"1.00");
89 device.writeAttr(
"date", dstr.substr(0, dstr.length() - 1));
90 device.writeAttr(
"north", b.
ymax());
91 device.writeAttr(
"south", b.
ymin());
92 device.writeAttr(
"east", b.
xmax());
93 device.writeAttr(
"west", b.
xmin());
102 for (std::map<std::string, NBEdge*>::const_iterator i = ec.
begin(); i != ec.
end(); ++i) {
103 const NBEdge* e = (*i).second;
112 planViewOSS.
openTag(
"planView");
115 const std::vector<NBEdge::Lane>& lanes = e->
getLanes();
117 #ifdef DEBUG_SMOOTH_GEOM 119 std::cout <<
"write planview for edge " << e->
getID() <<
"\n";
134 device.openTag(
"road");
136 device.setPrecision(8);
139 device.writeAttr(
"id",
getID(e->
getID(), edgeMap, edgeID));
140 device.writeAttr(
"junction", -1);
143 if (hasPred || hasSucc) {
144 device.openTag(
"link");
146 device.openTag(
"predecessor");
147 device.writeAttr(
"elementType",
"junction");
152 device.openTag(
"successor");
153 device.writeAttr(
"elementType",
"junction");
159 device.openTag(
"type").writeAttr(
"s", 0).writeAttr(
"type",
"town").closeTag();
162 device <<
" <lateralProfile/>\n";
163 device <<
" <lanes>\n";
164 device <<
" <laneSection s=\"0\">\n";
166 device <<
" <right>\n";
169 device <<
" <link/>\n";
177 device <<
" <width sOffset=\"0\" a=\"" << e->
getLaneWidth(j) <<
"\" b=\"0\" c=\"0\" d=\"0\"/>\n";
178 std::string markType =
"broken";
182 device <<
" <roadMark sOffset=\"0\" type=\"" << markType <<
"\" weight=\"standard\" color=\"standard\" width=\"0.13\"/>\n";
183 device <<
" <speed sOffset=\"0\" max=\"" << lanes[j].speed <<
"\"/>\n";
184 device <<
" </lane>\n";
186 device <<
" </right>\n";
187 device <<
" </laneSection>\n";
188 device <<
" </lanes>\n";
189 device <<
" <objects/>\n";
190 device <<
" <signals/>\n";
192 device <<
" <userData code=\"sumoId\" value=\"" << e->
getID() <<
"\"/>\n";
200 for (std::map<std::string, NBNode*>::const_iterator i = nc.
begin(); i != nc.
end(); ++i) {
203 for (std::vector<NBEdge*>::const_iterator j = incoming.begin(); j != incoming.end(); ++j) {
204 const NBEdge* inEdge = *j;
205 const std::vector<NBEdge::Connection>& elv = inEdge->
getConnections();
206 for (std::vector<NBEdge::Connection>::const_iterator k = elv.begin(); k != elv.end(); ++k) {
219 fallBackShape.push_back(begShape.back());
220 fallBackShape.push_back(endShape.front());
224 if (init.size() == 0) {
231 length =
bezier(init, 12).length2D();
234 device.openTag(
"road");
236 device.setPrecision(8);
240 device.writeAttr(
"junction",
getID(n->
getID(), nodeMap, nodeID));
241 device.openTag(
"link");
242 device.openTag(
"predecessor");
243 device.writeAttr(
"elementType",
"road");
244 device.writeAttr(
"elementId",
getID(inEdge->
getID(), edgeMap, edgeID));
245 device.writeAttr(
"contactPoint",
"end");
247 device.openTag(
"successor");
248 device.writeAttr(
"elementType",
"road");
249 device.writeAttr(
"elementId",
getID(outEdge->
getID(), edgeMap, edgeID));
250 device.writeAttr(
"contactPoint",
"start");
253 device.openTag(
"type").writeAttr(
"s", 0).writeAttr(
"type",
"town").closeTag();
254 device.openTag(
"planView");
255 device.setPrecision(8);
257 #ifdef DEBUG_SMOOTH_GEOM 259 std::cout <<
"write planview for internal edge " << c.
getInternalLaneID() <<
" init=" << init <<
" fallback=" << fallBackShape <<
"\n";
262 if (init.size() == 0) {
270 device <<
" <lateralProfile/>\n";
271 device <<
" <lanes>\n";
272 device <<
" <laneSection s=\"0\">\n";
274 device <<
" <right>\n";
276 device <<
" <link>\n";
279 device <<
" </link>\n";
280 device <<
" <width sOffset=\"0\" a=\"" << width <<
"\" b=\"0\" c=\"0\" d=\"0\"/>\n";
281 device <<
" <roadMark sOffset=\"0\" type=\"none\" weight=\"standard\" color=\"standard\" width=\"0.13\"/>\n";
282 device <<
" </lane>\n";
283 device <<
" </right>\n";
284 device <<
" </laneSection>\n";
285 device <<
" </lanes>\n";
286 device <<
" <objects/>\n";
287 device <<
" <signals/>\n";
294 for (std::map<std::string, NBNode*>::const_iterator i = nc.
begin(); i != nc.
end(); ++i) {
298 int numConnections = 0;
299 for (std::vector<NBEdge*>::const_iterator j = incoming.begin(); j != incoming.end(); ++j) {
300 numConnections += (int)((*j)->getConnections().size());
302 if (numConnections == 0) {
305 device <<
" <junction name=\"" << n->
getID() <<
"\" id=\"" <<
getID(n->
getID(), nodeMap, nodeID) <<
"\">\n";
307 for (std::vector<NBEdge*>::const_iterator j = incoming.begin(); j != incoming.end(); ++j) {
308 const NBEdge* inEdge = *j;
309 const std::vector<NBEdge::Connection>& elv = inEdge->
getConnections();
310 for (std::vector<NBEdge::Connection>::const_iterator k = elv.begin(); k != elv.end(); ++k) {
316 device <<
" <connection id=\"" 317 << index <<
"\" incomingRoad=\"" <<
getID(inEdge->
getID(), edgeMap, edgeID)
318 <<
"\" connectingRoad=\"" 320 <<
"\" contactPoint=\"start\">\n";
324 device <<
" </connection>\n";
328 device <<
" </junction>\n";
338 for (
int j = 0; j < (int)shape.size() - 1; ++j) {
351 elevationDevice <<
" <elevation s=\"" << offset <<
"\" a=\"" << p.
z() <<
"\" b=\"" << (p2.
z() - p.
z()) /
MAX2(
POSITION_EPS, length) <<
"\" c=\"0\" d=\"0\"/>\n";
360 device <<
" <center>\n";
361 device <<
" <lane id=\"0\" type=\"none\" level=\"true\">\n";
362 device <<
" <link/>\n";
363 device <<
" <roadMark sOffset=\"0\" type=\"" << mark <<
"\" weight=\"standard\" color=\"standard\" width=\"" << markWidth <<
"\"/>\n";
364 device <<
" </lane>\n";
365 device <<
" </center>\n";
372 return map.
get(origID);
374 map.
insert(origID, lastID++);
381 switch (permissions) {
400 if (permissions ==
SVCAll) {
416 if (laneIndex == -1) {
428 double widthOffset = -(edge->
getLaneWidth(leftmost) / 2);
430 for (
int i = leftmost; i > laneIndex; i--) {
448 assert(init.size() == 3 || init.size() == 4);
459 init.
add(-p.
x(), -p.
y(), -p.
z());
463 double aU, bU, cU, dU;
464 double aV, bV, cV, dV;
465 double aZ, bZ, cZ, dZ;
468 if (init.size() == 3) {
471 bU = 2 * init[1].x() - 2 * init[0].x();
472 cU = init[0].x() - 2 * init[1].x() + init[2].x();
476 bV = 2 * init[1].y() - 2 * init[0].y();
477 cV = init[0].y() - 2 * init[1].y() + init[2].y();
482 bZ = (2 * initZ[1].z() - 2 * initZ[0].z()) / length;
483 cZ = (initZ[0].z() - 2 * initZ[1].z() + initZ[2].z()) / (length * length);
489 bU = 3 * init[1].x() - 3 * init[0].x();
490 cU = 3 * init[0].x() - 6 * init[1].x() + 3 * init[2].x();
491 dU = -init[0].x() + 3 * init[1].x() - 3 * init[2].x() + init[3].x();
494 bV = 3 * init[1].y() - 3 * init[0].y();
495 cV = 3 * init[0].y() - 6 * init[1].y() + 3 * init[2].y();
496 dV = -init[0].y() + 3 * init[1].y() - 3 * init[2].y() + init[3].y();
500 bZ = (3 * initZ[1].z() - 3 * initZ[0].z()) / length;
501 cZ = (3 * initZ[0].z() - 6 * initZ[1].z() + 3 * initZ[2].z()) / (length * length);
502 dZ = (-initZ[0].z() + 3 * initZ[1].z() - 3 * initZ[2].z() + initZ[3].z()) / (length * length * length);
525 elevationDevice.
openTag(
"elevation");
533 return offset + length;
539 #ifdef DEBUG_SMOOTH_GEOM 541 std::cout <<
"writeGeomSmooth\n n=" << shape.size() <<
" shape=" <<
toString(shape) <<
"\n";
545 const double longThresh = speed;
546 const double curveCutout = longThresh / 2;
548 assert(longThresh >= 2 * curveCutout);
549 assert(shape.size() > 2);
553 double maxAngleDiff = 0;
555 for (
int j = 1; j < (int)shape.size() - 1; ++j) {
563 maxAngleDiff =
MAX2(maxAngleDiff, dAngle);
564 #ifdef DEBUG_SMOOTH_GEOM 566 std::cout <<
" j=" << j <<
" dAngle=" <<
RAD2DEG(dAngle) <<
" length1=" << length1 <<
" length2=" << length2 <<
"\n";
569 if (dAngle > straightThresh
570 && (length1 > longThresh || j == 1)
571 && (length2 > longThresh || j == (
int)shape.size() - 2)) {
574 shape2.removeClosest(p1);
578 const int numPoints = (int)shape2.size();
579 #ifdef DEBUG_SMOOTH_GEOM 581 std::cout <<
" n=" << numPoints <<
" shape2=" <<
toString(shape2) <<
"\n";
585 if (maxAngleDiff < straightThresh) {
587 #ifdef DEBUG_SMOOTH_GEOM 589 std::cout <<
" special case: all lines. maxAngleDiff=" << maxAngleDiff <<
"\n";
597 for (
int j = 0; j < numPoints - 1; ++j) {
603 const double lineLength = line.
length2D();
604 if (lineLength >= longThresh) {
606 #ifdef DEBUG_SMOOTH_GEOM 608 std::cout <<
" writeLine=" <<
toString(line) <<
"\n";
615 if (j == 0 || j == numPoints - 2) {
618 begShape.
add(p0 - begShape.back());
619 }
else if (j == 1 || p0.
distanceTo2D(shape2[j - 1]) > longThresh) {
621 begShape.push_back(shape2[j - 1]);
622 begShape.push_back(p0);
625 begShape.push_back(shape2[j - 1]);
626 begShape.push_back(p1);
627 begShape.
add(p0 - begShape.back());
630 if (j == 0 || j == numPoints - 2) {
633 endShape.
add(p1 - endShape.front());
634 }
else if (j == numPoints - 3 || p1.
distanceTo2D(shape2[j + 2]) > longThresh) {
636 endShape.push_back(p1);
637 endShape.push_back(shape2[j + 2]);
640 endShape.push_back(p0);
641 endShape.push_back(shape2[j + 2]);
642 endShape.
add(p1 - endShape.front());
644 const double extrapolateLength =
MIN2((
double)25, lineLength / 4);
646 if (init.size() == 0) {
649 #ifdef DEBUG_SMOOTH_GEOM 651 std::cout <<
" writeLine lineLength=" << lineLength <<
" begShape" << j <<
"=" <<
toString(begShape) <<
" endShape" << j <<
"=" <<
toString(endShape) <<
" init" << j <<
"=" <<
toString(init) <<
"\n";
656 const double curveLength =
bezier(init, 12).length2D();
657 offset =
writeGeomPP3(device, elevationDevice, init, curveLength, offset);
658 #ifdef DEBUG_SMOOTH_GEOM 660 std::cout <<
" writeCurve lineLength=" << lineLength <<
" curveLength=" << curveLength <<
" begShape" << j <<
"=" <<
toString(begShape) <<
" endShape" << j <<
"=" <<
toString(endShape) <<
" init" << j <<
"=" <<
toString(init) <<
"\n";
675 double z = shape.size() == 0 ? 0 : shape[0].z();
676 for (
int i = 1; i < (int)shape.size(); ++i) {
682 device <<
" <elevationProfile>\n";
684 device <<
" <elevation s=\"0\" a=\"" << z <<
"\" b=\"0\" c=\"0\" d=\"0\"/>\n";
688 device <<
" </elevationProfile>\n";
698 assert(shape0.size() >= 2);
702 stopLine.push_back(to);
705 for (
int lane = 1; lane < e->
getNumLanes(); ++lane) {
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.
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.
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
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 & 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.
vehicle is a (possibly fast moving) electric rail
#define WRITE_WARNING(msg)
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.
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()
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)
Storage for edges, including some functionality operating on multiple edges.
double xmin() const
Returns minimum x-coordinate.
const std::string & getStreetName() const
Returns the street name of this edge.
static std::string escapeXML(const std::string &orig, const bool maskDoubleHyphen=false)
Replaces the standard escapes by their XML entities.
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)
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)
static PositionVector getLeftLaneBorder(const NBEdge *edge, int laneIndex=-1)
get the left border of the given lane (the leftmost one by default)
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...
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()
Closes the most recently opened tag.
NBNode * getFromNode() const
Returns the origin node of the edge.
Container for nodes during the netbuilding process.
std::string getInternalLaneID() const
get ID of internal lane
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 getID(const std::string &origID, StringBijection< int > &map, int &lastID)
static PositionVector bezierControlPoints(const PositionVector &begShape, const PositionVector &endShape, bool isTurnaround, double extrapolateBeg, double extrapolateEnd, bool &ok, NBNode *recordError=0, double straightThresh=DEG2RAD(5))
get bezier control points
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.
An output device that encapsulates an ofstream.
void bezier(int npts, double b[], int cpts, double p[])