SUMO - Simulation of Urban MObility
NWWriter_OpenDrive.cpp
Go to the documentation of this file.
1 /****************************************************************************/
8 // Exporter writing networks using the openDRIVE format
9 /****************************************************************************/
10 // SUMO, Simulation of Urban MObility; see http://sumo.dlr.de/
11 // Copyright (C) 2011-2017 DLR (http://www.dlr.de/) and contributors
12 /****************************************************************************/
13 //
14 // This file is part of SUMO.
15 // SUMO is free software: you can redistribute it and/or modify
16 // it under the terms of the GNU General Public License as published by
17 // the Free Software Foundation, either version 3 of the License, or
18 // (at your option) any later version.
19 //
20 /****************************************************************************/
21 
22 
23 // ===========================================================================
24 // included modules
25 // ===========================================================================
26 #ifdef _MSC_VER
27 #include <windows_config.h>
28 #else
29 #include <config.h>
30 #endif
31 
32 #include <ctime>
33 #include "NWWriter_OpenDrive.h"
36 #include <netbuild/NBEdge.h>
37 #include <netbuild/NBEdgeCont.h>
38 #include <netbuild/NBNode.h>
39 #include <netbuild/NBNodeCont.h>
40 #include <netbuild/NBNetBuilder.h>
43 #include <utils/geom/bezier.h>
45 #include <utils/common/StdDefs.h>
48 
49 //#define DEBUG_SMOOTH_GEOM
50 #define DEBUGCOND true
51 
52 #define MIN_TURN_DIAMETER 2.0
53 
54 
55 // ===========================================================================
56 // method definitions
57 // ===========================================================================
58 // ---------------------------------------------------------------------------
59 // static methods
60 // ---------------------------------------------------------------------------
61 void
63  // check whether an opendrive-file shall be generated
64  if (!oc.isSet("opendrive-output")) {
65  return;
66  }
67  const NBNodeCont& nc = nb.getNodeCont();
68  const NBEdgeCont& ec = nb.getEdgeCont();
69  const bool origNames = oc.getBool("output.original-names");
70  const double straightThresh = DEG2RAD(oc.getFloat("opendrive-output.straight-threshold"));
71  // some internal mapping containers
72  int nodeID = 1;
73  int edgeID = nc.size() * 10; // distinct from node ids
74  StringBijection<int> edgeMap;
75  StringBijection<int> nodeMap;
76  //
77  OutputDevice& device = OutputDevice::getDevice(oc.getString("opendrive-output"));
78  device << "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n";
79  device.openTag("OpenDRIVE");
80  time_t now = time(0);
81  std::string dstr(ctime(&now));
83  // write header
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());
94  /* @note obsolete in 1.4
95  device.writeAttr("maxRoad", ec.size());
96  device.writeAttr("maxJunc", nc.size());
97  device.writeAttr("maxPrg", 0);
98  */
99  device.closeTag();
100 
101  // write normal edges (road)
102  for (std::map<std::string, NBEdge*>::const_iterator i = ec.begin(); i != ec.end(); ++i) {
103  const NBEdge* e = (*i).second;
104 
105  // buffer output because some fields are computed out of order
106  OutputDevice_String elevationOSS(false, 3);
107  elevationOSS.setPrecision(8);
108  OutputDevice_String planViewOSS(false, 2);
109  planViewOSS.setPrecision(8);
110  double length = 0;
111 
112  planViewOSS.openTag("planView");
113  planViewOSS.setPrecision(8); // geometry hdg requires higher precision
114  // for the shape we need to use the leftmost border of the leftmost lane
115  const std::vector<NBEdge::Lane>& lanes = e->getLanes();
117 #ifdef DEBUG_SMOOTH_GEOM
118  if (DEBUGCOND) {
119  std::cout << "write planview for edge " << e->getID() << "\n";
120  }
121 #endif
122 
123  if (ls.size() == 2 || e->getPermissions() == SVC_PEDESTRIAN) {
124  // foot paths may contain sharp angles
125  length = writeGeomLines(ls, planViewOSS, elevationOSS);
126  } else {
127  bool ok = writeGeomSmooth(ls, e->getSpeed(), planViewOSS, elevationOSS, straightThresh, length);
128  if (!ok) {
129  WRITE_WARNING("Could not compute smooth shape for edge '" + e->getID() + "'.");
130  }
131  }
132  planViewOSS.closeTag();
133 
134  device.openTag("road");
135  device.writeAttr("name", StringUtils::escapeXML(e->getStreetName()));
136  device.setPrecision(8); // length requires higher precision
137  device.writeAttr("length", MAX2(POSITION_EPS, length));
138  device.setPrecision(gPrecision);
139  device.writeAttr("id", getID(e->getID(), edgeMap, edgeID));
140  device.writeAttr("junction", -1);
141  const bool hasSucc = e->getConnections().size() > 0;
142  const bool hasPred = e->getIncomingEdges().size() > 0;
143  if (hasPred || hasSucc) {
144  device.openTag("link");
145  if (hasPred) {
146  device.openTag("predecessor");
147  device.writeAttr("elementType", "junction");
148  device.writeAttr("elementId", getID(e->getFromNode()->getID(), nodeMap, nodeID));
149  device.closeTag();
150  }
151  if (hasSucc) {
152  device.openTag("successor");
153  device.writeAttr("elementType", "junction");
154  device.writeAttr("elementId", getID(e->getToNode()->getID(), nodeMap, nodeID));
155  device.closeTag();
156  }
157  device.closeTag();
158  }
159  device.openTag("type").writeAttr("s", 0).writeAttr("type", "town").closeTag();
160  device << planViewOSS.getString();
161  writeElevationProfile(ls, device, elevationOSS);
162  device << " <lateralProfile/>\n";
163  device << " <lanes>\n";
164  device << " <laneSection s=\"0\">\n";
165  writeEmptyCenterLane(device, "solid", 0.13);
166  device << " <right>\n";
167  for (int j = e->getNumLanes(); --j >= 0;) {
168  device << " <lane id=\"-" << e->getNumLanes() - j << "\" type=\"" << getLaneType(e->getPermissions(j)) << "\" level=\"true\">\n";
169  device << " <link/>\n";
170  // this could be used for geometry-link junctions without u-turn,
171  // predecessor and sucessors would be lane indices,
172  // road predecessor / succesfors would be of type 'road' rather than
173  // 'junction'
174  //device << " <predecessor id=\"-1\"/>\n";
175  //device << " <successor id=\"-1\"/>\n";
176  //device << " </link>\n";
177  device << " <width sOffset=\"0\" a=\"" << e->getLaneWidth(j) << "\" b=\"0\" c=\"0\" d=\"0\"/>\n";
178  std::string markType = "broken";
179  if (j == 0) {
180  markType = "solid";
181  }
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";
185  }
186  device << " </right>\n";
187  device << " </laneSection>\n";
188  device << " </lanes>\n";
189  device << " <objects/>\n";
190  device << " <signals/>\n";
191  if (origNames) {
192  device << " <userData code=\"sumoId\" value=\"" << e->getID() << "\"/>\n";
193  }
194  device.closeTag();
196  }
197  device.lf();
198 
199  // write junction-internal edges (road). In OpenDRIVE these are called 'paths' or 'connecting roads'
200  for (std::map<std::string, NBNode*>::const_iterator i = nc.begin(); i != nc.end(); ++i) {
201  NBNode* n = (*i).second;
202  const std::vector<NBEdge*>& incoming = (*i).second->getIncomingEdges();
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) {
207  const NBEdge::Connection& c = *k;
208  const NBEdge* outEdge = c.toEdge;
209  if (outEdge == 0) {
210  continue;
211  }
212  const double width = c.toEdge->getLaneWidth(c.toLane);
213  const PositionVector begShape = getLeftLaneBorder(inEdge, c.fromLane);
214  const PositionVector endShape = getLeftLaneBorder(outEdge, c.toLane);
215  //std::cout << "computing reference line for internal lane " << c.getInternalLaneID() << " begLane=" << inEdge->getLaneShape(c.fromLane) << " endLane=" << outEdge->getLaneShape(c.toLane) << "\n";
216 
217  double length;
218  PositionVector fallBackShape;
219  fallBackShape.push_back(begShape.back());
220  fallBackShape.push_back(endShape.front());
221  const bool turnaround = inEdge->isTurningDirectionAt(outEdge);
222  bool ok = true;
223  PositionVector init = NBNode::bezierControlPoints(begShape, endShape, turnaround, 25, 25, ok, 0, straightThresh);
224  if (init.size() == 0) {
225  length = fallBackShape.length2D();
226  // problem with turnarounds is known, method currently returns 'ok' (#2539)
227  if (!ok) {
228  WRITE_WARNING("Could not compute smooth shape from lane '" + inEdge->getLaneID(c.fromLane) + "' to lane '" + outEdge->getLaneID(c.toLane) + "'. Use option 'junctions.scurve-stretch' or increase radius of junction '" + inEdge->getToNode()->getID() + "' to fix this.");
229  }
230  } else {
231  length = bezier(init, 12).length2D();
232  }
233 
234  device.openTag("road");
235  device.writeAttr("name", c.getInternalLaneID());
236  device.setPrecision(8); // length requires higher precision
237  device.writeAttr("length", MAX2(POSITION_EPS, length));
238  device.setPrecision(gPrecision);
239  device.writeAttr("id", getID(c.getInternalLaneID(), edgeMap, edgeID));
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");
246  device.closeTag();
247  device.openTag("successor");
248  device.writeAttr("elementType", "road");
249  device.writeAttr("elementId", getID(outEdge->getID(), edgeMap, edgeID));
250  device.writeAttr("contactPoint", "start");
251  device.closeTag();
252  device.closeTag();
253  device.openTag("type").writeAttr("s", 0).writeAttr("type", "town").closeTag();
254  device.openTag("planView");
255  device.setPrecision(8); // geometry hdg requires higher precision
256  OutputDevice_String elevationOSS(false, 3);
257 #ifdef DEBUG_SMOOTH_GEOM
258  if (DEBUGCOND) {
259  std::cout << "write planview for internal edge " << c.getInternalLaneID() << " init=" << init << " fallback=" << fallBackShape << "\n";
260  }
261 #endif
262  if (init.size() == 0) {
263  writeGeomLines(fallBackShape, device, elevationOSS);
264  } else {
265  writeGeomPP3(device, elevationOSS, init, length);
266  }
267  device.setPrecision(gPrecision);
268  device.closeTag();
269  writeElevationProfile(fallBackShape, device, elevationOSS);
270  device << " <lateralProfile/>\n";
271  device << " <lanes>\n";
272  device << " <laneSection s=\"0\">\n";
273  writeEmptyCenterLane(device, "none", 0);
274  device << " <right>\n";
275  device << " <lane id=\"-1\" type=\"" << getLaneType(outEdge->getPermissions(c.toLane)) << "\" level=\"true\">\n";
276  device << " <link>\n";
277  device << " <predecessor id=\"-" << inEdge->getNumLanes() - c.fromLane << "\"/>\n";
278  device << " <successor id=\"-" << outEdge->getNumLanes() - c.toLane << "\"/>\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";
288  device.closeTag();
289  }
290  }
291  }
292 
293  // write junctions (junction)
294  for (std::map<std::string, NBNode*>::const_iterator i = nc.begin(); i != nc.end(); ++i) {
295  NBNode* n = (*i).second;
296  const std::vector<NBEdge*>& incoming = n->getIncomingEdges();
297  // check if any connections must be written
298  int numConnections = 0;
299  for (std::vector<NBEdge*>::const_iterator j = incoming.begin(); j != incoming.end(); ++j) {
300  numConnections += (int)((*j)->getConnections().size());
301  }
302  if (numConnections == 0) {
303  continue;
304  }
305  device << " <junction name=\"" << n->getID() << "\" id=\"" << getID(n->getID(), nodeMap, nodeID) << "\">\n";
306  int index = 0;
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) {
311  const NBEdge::Connection& c = *k;
312  const NBEdge* outEdge = c.toEdge;
313  if (outEdge == 0) {
314  continue;
315  }
316  device << " <connection id=\""
317  << index << "\" incomingRoad=\"" << getID(inEdge->getID(), edgeMap, edgeID)
318  << "\" connectingRoad=\""
319  << getID(c.getInternalLaneID(), edgeMap, edgeID)
320  << "\" contactPoint=\"start\">\n";
321  device << " <laneLink from=\"-" << inEdge->getNumLanes() - c.fromLane
322  << "\" to=\"-1" // every connection has its own edge
323  << "\"/>\n";
324  device << " </connection>\n";
325  ++index;
326  }
327  }
328  device << " </junction>\n";
329  }
330 
331  device.closeTag();
332  device.close();
333 }
334 
335 
336 double
337 NWWriter_OpenDrive::writeGeomLines(const PositionVector& shape, OutputDevice& device, OutputDevice& elevationDevice, double offset) {
338  for (int j = 0; j < (int)shape.size() - 1; ++j) {
339  const Position& p = shape[j];
340  const Position& p2 = shape[j + 1];
341  const double hdg = shape.angleAt2D(j);
342  const double length = p.distanceTo2D(p2);
343  device.openTag("geometry");
344  device.writeAttr("s", offset);
345  device.writeAttr("x", p.x());
346  device.writeAttr("y", p.y());
347  device.writeAttr("hdg", hdg);
348  device.writeAttr("length", length);
349  device.openTag("line").closeTag();
350  device.closeTag();
351  elevationDevice << " <elevation s=\"" << offset << "\" a=\"" << p.z() << "\" b=\"" << (p2.z() - p.z()) / MAX2(POSITION_EPS, length) << "\" c=\"0\" d=\"0\"/>\n";
352  offset += length;
353  }
354  return offset;
355 }
356 
357 
358 void
359 NWWriter_OpenDrive::writeEmptyCenterLane(OutputDevice& device, const std::string& mark, double markWidth) {
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";
366 }
367 
368 
369 int
370 NWWriter_OpenDrive::getID(const std::string& origID, StringBijection<int>& map, int& lastID) {
371  if (map.hasString(origID)) {
372  return map.get(origID);
373  }
374  map.insert(origID, lastID++);
375  return lastID - 1;
376 }
377 
378 
379 std::string
381  switch (permissions) {
382  case SVC_PEDESTRIAN:
383  return "sidewalk";
384  //case (SVC_BICYCLE | SVC_PEDESTRIAN):
385  // WRITE_WARNING("Ambiguous lane type (biking+driving) for road '" + roadID + "'");
386  // return "sidewalk";
387  case SVC_BICYCLE:
388  return "biking";
389  case 0:
390  // ambiguous
391  return "none";
392  case SVC_RAIL:
393  case SVC_RAIL_URBAN:
394  case SVC_RAIL_ELECTRIC:
395  return "rail";
396  case SVC_TRAM:
397  return "tram";
398  default: {
399  // complex permissions
400  if (permissions == SVCAll) {
401  return "driving";
402  } else if (isRailway(permissions)) {
403  return "rail";
404  } else if ((permissions & SVC_PASSENGER) != 0) {
405  return "driving";
406  } else {
407  return "restricted";
408  }
409  }
410  }
411 }
412 
413 
415 NWWriter_OpenDrive::getLeftLaneBorder(const NBEdge* edge, int laneIndex) {
416  if (laneIndex == -1) {
417  // leftmost lane
418  laneIndex = (int)edge->getNumLanes() - 1;
419  }
421  // PositionVector result = edge->getLaneShape(laneIndex);
422  // (and the moveo2side)
423  // However, the lanes in SUMO have a small lateral gap (SUMO_const_laneOffset) to account for markings
424  // In OpenDRIVE this gap does not exists so we have to do all lateral
425  // computations based on the reference line
426  // This assumes that the 'stop line' for all lanes is colinear!
427  const int leftmost = (int)edge->getNumLanes() - 1;
428  double widthOffset = -(edge->getLaneWidth(leftmost) / 2);
429  // collect lane widths from left border of edge to left border of lane to connect to
430  for (int i = leftmost; i > laneIndex; i--) {
431  widthOffset += edge->getLaneWidth(i);
432  }
433  PositionVector result = edge->getLaneShape(leftmost);
434  try {
435  result.move2side(widthOffset);
436  } catch (InvalidArgument&) { }
437  return result;
438 }
439 
440 
441 double
443  OutputDevice& device,
444  OutputDevice& elevationDevice,
445  PositionVector init,
446  double length,
447  double offset) {
448  assert(init.size() == 3 || init.size() == 4);
449 
450  // avoid division by 0
451  length = MAX2(POSITION_EPS, length);
452 
453  const Position p = init.front();
454  const double hdg = init.angleAt2D(0);
455 
456  // backup elevation values
457  const PositionVector initZ = init;
458  // translate to u,v coordinates
459  init.add(-p.x(), -p.y(), -p.z());
460  init.rotate2D(-hdg);
461 
462  // parametric coefficients
463  double aU, bU, cU, dU;
464  double aV, bV, cV, dV;
465  double aZ, bZ, cZ, dZ;
466 
467  // unfactor the Bernstein polynomials of degree 2 (or 3) and collect the coefficients
468  if (init.size() == 3) {
469  //f(x, a, b ,c) = a + (2*b - 2*a)*x + (a - 2*b + c)*x*x
470  aU = init[0].x();
471  bU = 2 * init[1].x() - 2 * init[0].x();
472  cU = init[0].x() - 2 * init[1].x() + init[2].x();
473  dU = 0;
474 
475  aV = init[0].y();
476  bV = 2 * init[1].y() - 2 * init[0].y();
477  cV = init[0].y() - 2 * init[1].y() + init[2].y();
478  dV = 0;
479 
480  // elevation is not parameteric on [0:1] but on [0:length]
481  aZ = initZ[0].z();
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);
484  dZ = 0;
485 
486  } else {
487  // f(x, a, b, c, d) = a + (x*((3*b) - (3*a))) + ((x*x)*((3*a) + (3*c) - (6*b))) + ((x*x*x)*((3*b) - (3*c) - a + d))
488  aU = init[0].x();
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();
492 
493  aV = init[0].y();
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();
497 
498  // elevation is not parameteric on [0:1] but on [0:length]
499  aZ = initZ[0].z();
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);
503  }
504 
505  device.openTag("geometry");
506  device.writeAttr("s", offset);
507  device.writeAttr("x", p.x());
508  device.writeAttr("y", p.y());
509  device.writeAttr("hdg", hdg);
510  device.writeAttr("length", length);
511 
512  device.openTag("paramPoly3");
513  device.writeAttr("aU", aU);
514  device.writeAttr("bU", bU);
515  device.writeAttr("cU", cU);
516  device.writeAttr("dU", dU);
517  device.writeAttr("aV", aV);
518  device.writeAttr("bV", bV);
519  device.writeAttr("cV", cV);
520  device.writeAttr("dV", dV);
521  device.closeTag();
522  device.closeTag();
523 
524  // write elevation
525  elevationDevice.openTag("elevation");
526  elevationDevice.writeAttr("s", offset);
527  elevationDevice.writeAttr("a", aZ);
528  elevationDevice.writeAttr("b", bZ);
529  elevationDevice.writeAttr("c", cZ);
530  elevationDevice.writeAttr("d", dZ);
531  elevationDevice.closeTag();
532 
533  return offset + length;
534 }
535 
536 
537 bool
538 NWWriter_OpenDrive::writeGeomSmooth(const PositionVector& shape, double speed, OutputDevice& device, OutputDevice& elevationDevice, double straightThresh, double& length) {
539 #ifdef DEBUG_SMOOTH_GEOM
540  if (DEBUGCOND) {
541  std::cout << "writeGeomSmooth\n n=" << shape.size() << " shape=" << toString(shape) << "\n";
542  }
543 #endif
544  bool ok = true;
545  const double longThresh = speed; // 16.0; // make user-configurable (should match the sampling rate of the source data)
546  const double curveCutout = longThresh / 2; // 8.0; // make user-configurable (related to the maximum turning rate)
547  // the length of the segment that is added for cutting a corner can be bounded by 2*curveCutout (prevent the segment to be classified as 'long')
548  assert(longThresh >= 2 * curveCutout);
549  assert(shape.size() > 2);
550  // add intermediate points wherever there is a strong angular change between long segments
551  // assume the geometry is simplified so as not to contain consecutive colinear points
552  PositionVector shape2 = shape;
553  double maxAngleDiff = 0;
554  double offset = 0;
555  for (int j = 1; j < (int)shape.size() - 1; ++j) {
556  //const double hdg = shape.angleAt2D(j);
557  const Position& p0 = shape[j - 1];
558  const Position& p1 = shape[j];
559  const Position& p2 = shape[j + 1];
560  const double dAngle = fabs(GeomHelper::angleDiff(p0.angleTo2D(p1), p1.angleTo2D(p2)));
561  const double length1 = p0.distanceTo2D(p1);
562  const double length2 = p1.distanceTo2D(p2);
563  maxAngleDiff = MAX2(maxAngleDiff, dAngle);
564 #ifdef DEBUG_SMOOTH_GEOM
565  if (DEBUGCOND) {
566  std::cout << " j=" << j << " dAngle=" << RAD2DEG(dAngle) << " length1=" << length1 << " length2=" << length2 << "\n";
567  }
568 #endif
569  if (dAngle > straightThresh
570  && (length1 > longThresh || j == 1)
571  && (length2 > longThresh || j == (int)shape.size() - 2)) {
572  shape2.insertAtClosest(shape.positionAtOffset2D(offset + length1 - MIN2(length1 - POSITION_EPS, curveCutout)));
573  shape2.insertAtClosest(shape.positionAtOffset2D(offset + length1 + MIN2(length2 - POSITION_EPS, curveCutout)));
574  shape2.removeClosest(p1);
575  }
576  offset += length1;
577  }
578  const int numPoints = (int)shape2.size();
579 #ifdef DEBUG_SMOOTH_GEOM
580  if (DEBUGCOND) {
581  std::cout << " n=" << numPoints << " shape2=" << toString(shape2) << "\n";
582  }
583 #endif
584 
585  if (maxAngleDiff < straightThresh) {
586  length = writeGeomLines(shape2, device, elevationDevice, 0);
587 #ifdef DEBUG_SMOOTH_GEOM
588  if (DEBUGCOND) {
589  std::cout << " special case: all lines. maxAngleDiff=" << maxAngleDiff << "\n";
590  }
591 #endif
592  return ok;
593  }
594 
595  // write the long segments as lines, short segments as curves
596  offset = 0;
597  for (int j = 0; j < numPoints - 1; ++j) {
598  const Position& p0 = shape2[j];
599  const Position& p1 = shape2[j + 1];
600  PositionVector line;
601  line.push_back(p0);
602  line.push_back(p1);
603  const double lineLength = line.length2D();
604  if (lineLength >= longThresh) {
605  offset = writeGeomLines(line, device, elevationDevice, offset);
606 #ifdef DEBUG_SMOOTH_GEOM
607  if (DEBUGCOND) {
608  std::cout << " writeLine=" << toString(line) << "\n";
609  }
610 #endif
611  } else {
612  // find control points
613  PositionVector begShape;
614  PositionVector endShape;
615  if (j == 0 || j == numPoints - 2) {
616  // keep the angle of the first/last segment but end at the front of the shape
617  begShape = line;
618  begShape.add(p0 - begShape.back());
619  } else if (j == 1 || p0.distanceTo2D(shape2[j - 1]) > longThresh) {
620  // use the previous segment if it is long or the first one
621  begShape.push_back(shape2[j - 1]);
622  begShape.push_back(p0);
623  } else {
624  // end at p0 with mean angle of the previous and current segment
625  begShape.push_back(shape2[j - 1]);
626  begShape.push_back(p1);
627  begShape.add(p0 - begShape.back());
628  }
629 
630  if (j == 0 || j == numPoints - 2) {
631  // keep the angle of the first/last segment but start at the end of the shape
632  endShape = line;
633  endShape.add(p1 - endShape.front());
634  } else if (j == numPoints - 3 || p1.distanceTo2D(shape2[j + 2]) > longThresh) {
635  // use the next segment if it is long or the final one
636  endShape.push_back(p1);
637  endShape.push_back(shape2[j + 2]);
638  } else {
639  // start at p1 with mean angle of the current and next segment
640  endShape.push_back(p0);
641  endShape.push_back(shape2[j + 2]);
642  endShape.add(p1 - endShape.front());
643  }
644  const double extrapolateLength = MIN2((double)25, lineLength / 4);
645  PositionVector init = NBNode::bezierControlPoints(begShape, endShape, false, extrapolateLength, extrapolateLength, ok, 0, straightThresh);
646  if (init.size() == 0) {
647  // could not compute control points, write line
648  offset = writeGeomLines(line, device, elevationDevice, offset);
649 #ifdef DEBUG_SMOOTH_GEOM
650  if (DEBUGCOND) {
651  std::cout << " writeLine lineLength=" << lineLength << " begShape" << j << "=" << toString(begShape) << " endShape" << j << "=" << toString(endShape) << " init" << j << "=" << toString(init) << "\n";
652  }
653 #endif
654  } else {
655  // write bezier
656  const double curveLength = bezier(init, 12).length2D();
657  offset = writeGeomPP3(device, elevationDevice, init, curveLength, offset);
658 #ifdef DEBUG_SMOOTH_GEOM
659  if (DEBUGCOND) {
660  std::cout << " writeCurve lineLength=" << lineLength << " curveLength=" << curveLength << " begShape" << j << "=" << toString(begShape) << " endShape" << j << "=" << toString(endShape) << " init" << j << "=" << toString(init) << "\n";
661  }
662 #endif
663  }
664  }
665  }
666  length = offset;
667  return ok;
668 }
669 
670 
671 void
673  // check if the shape is flat
674  bool flat = true;
675  double z = shape.size() == 0 ? 0 : shape[0].z();
676  for (int i = 1; i < (int)shape.size(); ++i) {
677  if (fabs(shape[i].z() - z) > NUMERICAL_EPS) {
678  flat = false;
679  break;
680  }
681  }
682  device << " <elevationProfile>\n";
683  if (flat) {
684  device << " <elevation s=\"0\" a=\"" << z << "\" b=\"0\" c=\"0\" d=\"0\"/>\n";
685  } else {
686  device << elevationDevice.getString();
687  }
688  device << " </elevationProfile>\n";
689 
690 }
691 
692 
693 void
695  if (e->getNumLanes() > 1) {
696  // compute 'stop line' of rightmost lane
697  const PositionVector shape0 = e->getLaneShape(0);
698  assert(shape0.size() >= 2);
699  const Position& from = shape0[-2];
700  const Position& to = shape0[-1];
701  PositionVector stopLine;
702  stopLine.push_back(to);
703  stopLine.push_back(to - PositionVector::sideOffset(from, to, -1000.0));
704  // endpoints of all other lanes should be on the stop line
705  for (int lane = 1; lane < e->getNumLanes(); ++lane) {
706  const double dist = stopLine.distance2D(e->getLaneShape(lane)[-1]);
707  if (dist > NUMERICAL_EPS) {
708  WRITE_WARNING("Uneven stop line at lane '" + e->getLaneID(lane) + "' (dist=" + toString(dist) + ") cannot be represented in OpenDRIVE.");
709  }
710  }
711  }
712 }
713 
714 /****************************************************************************/
715 
#define DEBUGCOND
OutputDevice & writeAttr(const SumoXMLAttr attr, const T &val)
writes a named attribute
Definition: OutputDevice.h:256
double ymin() const
Returns minimum y-coordinate.
Definition: Boundary.cpp:138
double length2D() const
Returns the length.
double xmax() const
Returns maximum x-coordinate.
Definition: Boundary.cpp:132
A structure which describes a connection between edges or lanes.
Definition: NBEdge.h:164
int toLane
The lane the connections yields in.
Definition: NBEdge.h:190
static void writeEmptyCenterLane(OutputDevice &device, const std::string &mark, double markWidth)
is a pedestrian
std::map< std::string, NBNode * >::const_iterator begin() const
Returns the pointer to the begin of the stored nodes.
Definition: NBNodeCont.h:114
double z() const
Returns the z-position.
Definition: Position.h:73
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.
Definition: NBEdge.h:187
EdgeVector getIncomingEdges() const
Returns the list of incoming edges unsorted.
Definition: NBEdge.cpp:1119
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.
Definition: NBNodeCont.h:119
vehicle is a not electrified rail
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:250
int gPrecision
the precision for floating point outputs
Definition: StdDefs.cpp:30
Position positionAtOffset2D(double pos, double lateralOffset=0) const
Returns the position at the given length.
vehicle is a bicycle
std::string getString() const
Returns the current content as a string.
double y() const
Returns the y-position.
Definition: Position.h:68
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.
Definition: NBEdge.h:71
double x() const
Returns the x-position.
Definition: Position.h:63
vehicle is a light rail
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 ...
Definition: Position.h:260
T MAX2(T a, T b)
Definition: StdDefs.h:70
const std::vector< NBEdge::Lane > & getLanes() const
Returns the lane definitions.
Definition: NBEdge.h:570
std::map< std::string, NBEdge * >::const_iterator end() const
Returns the pointer to the end of the stored edges.
Definition: NBEdgeCont.h:198
#define RAD2DEG(x)
Definition: GeomHelper.h:46
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.
Definition: Named.h:66
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.
Definition: Boundary.h:48
vehicle is a (possibly fast moving) electric rail
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:200
vehicle is a city rail
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.
Definition: NBEdgeCont.h:190
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:56
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.
Definition: NBNodeCont.h:229
std::string getLaneID(int lane) const
get Lane ID (Secure)
Definition: NBEdge.cpp:2679
int getNumLanes() const
Returns the number of lanes.
Definition: NBEdge.h:413
int fromLane
The lane the connections starts at.
Definition: NBEdge.h:184
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:46
NBEdgeCont & getEdgeCont()
Definition: NBNetBuilder.h:155
A list of positions.
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.
Definition: NBEdgeCont.h:66
T MIN2(T a, T b)
Definition: StdDefs.h:64
double xmin() const
Returns minimum x-coordinate.
Definition: Boundary.cpp:126
#define POSITION_EPS
Definition: config.h:175
const std::string & getStreetName() const
Returns the street name of this edge.
Definition: NBEdge.h:536
#define DEG2RAD(x)
Definition: GeomHelper.h:45
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
Definition: NBEdge.cpp:2913
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.
Definition: NBEdge.h:507
double getLaneWidth() const
Returns the default width of lanes of this edge.
Definition: NBEdge.h:523
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.
Definition: NBEdge.cpp:757
const EdgeVector & getIncomingEdges() const
Returns this node&#39;s incoming edges (The edges which yield in this node)
Definition: NBNode.h:240
const std::vector< Connection > & getConnections() const
Returns the connections.
Definition: NBEdge.h:834
NBNodeCont & getNodeCont()
Returns a reference to the node container.
Definition: NBNetBuilder.h:160
Instance responsible for building networks.
Definition: NBNetBuilder.h:114
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)
Definition: OptionsCont.h:99
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.
Definition: NBEdge.cpp:2367
Represents a single node (junction) during network building.
Definition: NBNode.h:75
Static storage of an output device and its base (abstract) implementation.
Definition: OutputDevice.h:71
bool closeTag()
Closes the most recently opened tag.
#define NUMERICAL_EPS
Definition: config.h:151
NBNode * getFromNode() const
Returns the origin node of the edge.
Definition: NBEdge.h:427
Container for nodes during the netbuilding process.
Definition: NBNodeCont.h:63
std::string getInternalLaneID() const
get ID of internal lane
Definition: NBEdge.cpp:80
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.
Definition: GeomHelper.cpp:174
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.
Definition: Boundary.cpp:144
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
Definition: NBNode.cpp:487
NBNode * getToNode() const
Returns the destination node of the edge.
Definition: NBEdge.h:434
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[])
Definition: bezier.cpp:97