55 #ifdef CHECK_MEMORY_LEAKS
57 #endif // CHECK_MEMORY_LEAKS
129 std::vector<OpenDriveEdge> innerEdges, outerEdges;
132 std::vector<std::string> files = oc.
getStringVector(
"opendrive-files");
133 for (std::vector<std::string>::const_iterator file = files.begin(); file != files.end(); ++file) {
135 WRITE_ERROR(
"Could not open opendrive file '" + *file +
"'.");
153 std::map<std::string, Boundary> posMap;
154 std::map<std::string, std::string> edge2junction;
156 for (std::vector<OpenDriveEdge>::iterator i = innerEdges.begin(); i != innerEdges.end(); ++i) {
160 if (posMap.find(e.
junction) == posMap.end()) {
166 for (std::map<std::string, Boundary>::iterator i = posMap.begin(); i != posMap.end(); ++i) {
168 throw ProcessError(
"Could not add node '" + (*i).first +
"'.");
172 for (std::vector<OpenDriveEdge>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
174 for (std::vector<OpenDriveLink>::iterator j = e.
links.begin(); j != e.
links.end(); ++j) {
181 if (edge2junction.find(l.
elementID) != edge2junction.end()) {
193 for (std::vector<OpenDriveEdge>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
195 for (std::vector<OpenDriveLink>::iterator j = e.
links.begin(); j != e.
links.end(); ++j) {
202 std::string id1 = e.
id;
207 std::string nid = id1 +
"." + id2;
212 throw ProcessError(
"Could not build node '" + nid +
"'.");
230 for (std::vector<OpenDriveEdge>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
232 if (e.
to != 0 && e.
from != 0) {
235 for (std::vector<OpenDriveEdge>::iterator j = innerEdges.begin(); j != innerEdges.end(); ++j) {
237 for (std::vector<OpenDriveLink>::iterator k = ie.
links.begin(); k != ie.
links.end(); ++k) {
243 std::string nid = edge2junction[ie.
id];
257 for (std::vector<OpenDriveEdge>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
260 std::string nid = e.
id +
".begin";
265 std::string nid = e.
id +
".end";
275 std::map<NBEdge*, std::map<int, int> > fromLaneMap;
276 std::map<NBEdge*, std::map<int, int> > toLaneMap;
278 for (std::vector<OpenDriveEdge>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
285 if (noLanesRight != 0 || noLanesLeft != 0) {
290 if (noLanesRight > 0) {
291 NBEdge* nbe =
new NBEdge(
"-" + e.
id, e.
from, e.
to,
"", speed, noLanesRight, priority, -1, -1, e.
geom,
"", lsf,
true);
293 throw ProcessError(
"Could not add edge '" + std::string(
"-") + e.
id +
"'.");
298 if (noLanesLeft > 0) {
299 NBEdge* nbe =
new NBEdge(e.
id, e.
to, e.
from,
"", speed, noLanesLeft, priority, -1, -1, e.
geom.
reverse(),
"", lsf,
true);
312 std::vector<Connection> connections;
315 for (std::vector<OpenDriveEdge>::iterator i = outerEdges.begin(); i != outerEdges.end(); ++i) {
317 for (std::vector<OpenDriveLink>::iterator j = e.
links.begin(); j != e.
links.end(); ++j) {
323 if (edge2junction.find(l.
elementID) != edge2junction.end()) {
331 if (p == outerEdges.end()) {
334 std::string id1 = e.
id;
335 std::string id2 = (*p).id;
339 std::string nid = id1 +
"." + id2;
350 for (std::vector<OpenDriveEdge>::iterator i = innerEdges.begin(); i != innerEdges.end(); ++i) {
352 std::string pred, succ;
354 for (std::vector<OpenDriveLink>::iterator j = e.
links.begin(); j != e.
links.end(); ++j) {
358 std::cout <<
"unsupported" << std::endl;
361 if (edge2junction.find(l.
elementID) != edge2junction.end()) {
363 std::cout <<
"unsupported" << std::endl;
368 std::cout <<
"double succ" << std::endl;
374 std::cout <<
"double pred" << std::endl;
382 std::cout <<
"Both dirs given!" << std::endl;
385 bool isReversed =
false;
392 if (succ ==
"" || pred ==
"") {
393 std::cout <<
"Missing edge." << std::endl;
398 std::vector<OpenDriveEdge>::iterator predEdge = std::find_if(outerEdges.begin(), outerEdges.end(),
edge_by_id_finder(pred));
399 if (predEdge == outerEdges.end()) {
402 std::vector<OpenDriveEdge>::iterator succEdge = std::find_if(outerEdges.begin(), outerEdges.end(),
edge_by_id_finder(succ));
403 if (succEdge == outerEdges.end()) {
406 NBEdge* fromEdge, *toEdge;
428 connections.push_back(c);
430 for (std::vector<Connection>::const_iterator i = connections.begin(); i != connections.end(); ++i) {
431 if ((*i).from == 0 || (*i).to == 0) {
432 std::cout <<
"Nope." << std::endl;
435 (*i).from->addEdge2EdgeConnection((*i).to);
436 std::map<int, int> fromMap = fromLaneMap[(*i).from];
437 std::map<int, int> toMap = fromLaneMap[(*i).to];
438 for (std::vector<std::pair<int, int> >::const_iterator j = (*i).lanes.begin(); j != (*i).lanes.end(); ++j) {
439 int fromLane = fromMap[(*j).first];
440 int toLane = toMap[(*j).second];
441 if (static_cast<unsigned int>(fromLane) >= (*i).from->getNumLanes() || fromLane < 0) {
442 std::cout <<
"False " << std::endl;
444 if (static_cast<unsigned int>(toLane) >= (*i).to->getNumLanes() || toLane < 0) {
445 std::cout <<
"False " << std::endl;
457 LinkType lt,
const std::string& via,
458 std::vector<NIImporter_OpenDrive::Connection> &connections) {
482 if (from == 0 && to == 0) {
486 connections.push_back(c);
493 std::vector<NIImporter_OpenDrive::Connection> &connections) {
496 if (fromEdge == 0 || !node->
hasIncoming(fromEdge)) {
503 if (fromEdge != 0 && toEdge != 0) {
508 connections.push_back(c);
512 if (fromEdge == 0 || !node->
hasOutgoing(fromEdge)) {
519 if (fromEdge != 0 && toEdge != 0) {
524 connections.push_back(c);
534 const std::vector<OpenDriveLane> &fromLanes = fromLS.
lanesByDir.find(fromLaneDir)->second;
536 const std::vector<OpenDriveLane> &toLanes = toLS.
lanesByDir.find(toLaneDir)->second;
540 for (std::vector<OpenDriveLane>::const_iterator i = fromLanes.begin(); i != fromLanes.end(); ++i) {
541 if ((*i).type !=
"driving") {
545 c.
lanes.push_back(std::make_pair((*i).id, (*i).successor));
548 c.
lanes.push_back(std::make_pair((*i).id, (*i).predecessor));
551 for (std::vector<OpenDriveLane>::const_iterator i = toLanes.begin(); i != toLanes.end(); ++i) {
552 if ((*i).type !=
"driving") {
556 c.
lanes.push_back(std::make_pair((*i).predecessor, (*i).id));
559 c.
lanes.push_back(std::make_pair((*i).successor, (*i).id));
571 setLaneConnections(from2via, from, fromAtBegin, fromLaneDir, via, viaIsReversed, viaLaneDir);
574 for (std::vector<std::pair<int, int> >::const_iterator i = from2via.
lanes.begin(); i != from2via.
lanes.end(); ++i) {
575 int fromLane = (*i).first;
576 int viaLane = (*i).second;
577 for (std::vector<std::pair<int, int> >::const_iterator j = via2to.
lanes.begin(); j != via2to.
lanes.end(); ++j) {
578 if ((*j).first == viaLane) {
579 c.
lanes.push_back(std::make_pair(fromLane, (*j).second));
592 if (!nc.
insert(
id, pos)) {
606 throw ProcessError(
"Could not find node '" + nodeID +
"'.");
609 if (e.
to != 0 && e.
to != n) {
628 const std::string& edgeID,
629 const std::string& nodeID) {
646 const std::string& edgeID,
647 const std::string& nodeID) {
663 for (std::vector<OpenDriveEdge>::iterator i = edges.begin(); i != edges.end(); ++i) {
665 for (std::vector<OpenDriveGeometry>::iterator j = e.
geometries.begin(); j != e.
geometries.end(); ++j) {
667 std::vector<Position> geom;
683 for (std::vector<Position>::iterator k = geom.begin(); k != geom.end(); ++k) {
687 for (
unsigned int j = 0; j < e.
geom.
size(); ++j) {
695 std::vector<Position>
698 std::vector<Position> ret;
705 std::vector<Position>
708 std::vector<Position> ret;
728 if (geo_posE - g.
s > g.
length) {
731 if (geo_posE - g.
s > g.
length) {
734 calcPointOnCurve(&endX, &endY, centerX, centerY, radius, geo_posE - geo_posS);
736 dist += (geo_posE - geo_posS);
737 if (curvature > 0.0) {
738 hdgE = g.
hdg + dist / fabs(radius);
740 hdgE = g.
hdg - dist / fabs(radius);
743 ret.push_back(
Position(startX, startY));
750 if (geo_posE - (g.
s + g.
length) < 0.001 && geo_posE - (g.
s + g.
length) > -0.001) {
758 std::vector<Position>
762 std::vector<Position> ret;
771 double x2 = normx * cos(hdg) - normy * sin(hdg);
772 double y2 = normx * sin(hdg) + normy * cos(hdg);
775 return Position(start.
x() + normx, start.
y() + normy);
792 normX = normX * cos(ad_hdg) + normY * sin(ad_hdg);
793 normY = tmpX * sin(ad_hdg) + normY * cos(ad_hdg);
796 normX = normX * cos(90 *
PI / 180) + turn * normY * sin(90 *
PI / 180);
797 normY = -1 * turn * tmpX * sin(90 *
PI / 180) + normY * cos(90 *
PI / 180);
799 normX = fabs(ad_radius) * normX;
800 normY = fabs(ad_radius) * normY;
810 double rotAngle = ad_length / fabs(ad_r);
811 double vx = *ad_x - ad_centerX;
812 double vy = *ad_y - ad_centerY;
822 vx = vx * cos(rotAngle) + turn * vy * sin(rotAngle);
823 vy = -1 * turn * tmpx * sin(rotAngle) + vy * cos(rotAngle);
824 *ad_x = vx + ad_centerX;
825 *ad_y = vy + ad_centerY;
832 std::vector<OpenDriveEdge> &innerEdges,
833 std::vector<OpenDriveEdge> &outerEdges)
834 :
GenericSAXHandler(openDriveTags, OPENDRIVE_TAG_NOTHING, openDriveAttrs, OPENDRIVE_ATTR_NOTHING,
"opendrive"),
835 myCurrentEdge(
"",
"", -1), myInnerEdges(innerEdges), myOuterEdges(outerEdges) {
851 if (majorVersion != 1 || minorVersion != 2) {
905 std::vector<SUMOReal> vals;
910 std::vector<SUMOReal> vals;
917 std::vector<SUMOReal> vals;
923 std::vector<SUMOReal> vals;
964 const std::string& chars) {
991 const std::string& elementID,
992 const std::string& contactPoint) {
995 if (elementType ==
"road") {
997 }
else if (elementType ==
"junction") {
1001 if (contactPoint ==
"start") {
1003 }
else if (contactPoint ==
"end") {