69 #ifdef CHECK_MEMORY_LEAKS
71 #endif // CHECK_MEMORY_LEAKS
74 #define EXTEND_CROSSING_ANGLE_THRESHOLD 35.0 // degrees
76 #define SPLIT_CROSSING_WIDTH_THRESHOLD 1.5 // meters
77 #define SPLIT_CROSSING_ANGLE_THRESHOLD 5 // degrees
79 #define UNCONTROLLED_CROSSING_SPEED_THRESHOLD 13.89 // meters/second
96 EdgeVector* approaching,
NBEdge* currentOutgoing,
const bool buildCrossingsAndWalkingAreas) :
97 myApproaching(approaching), myCurrentOutgoing(currentOutgoing) {
101 std::set<int> approachedLanes;
103 const std::vector<NBEdge::Connection> conns = (*it)->getConnections();
104 for (std::vector<NBEdge::Connection>::const_iterator it_con = conns.begin(); it_con != conns.end(); ++it_con) {
106 approachedLanes.insert((*it_con).toLane);
117 && approachedLanes.count(i) == 0) {
130 assert(myApproaching->size() > src);
132 NBEdge* incomingEdge = (*myApproaching)[src];
136 std::vector<int> approachingLanes =
138 assert(approachingLanes.size() != 0);
139 std::deque<int>* approachedLanes = spread(approachingLanes, dest);
140 assert(approachedLanes->size() <= myAvailableLanes.size());
142 for (
unsigned int i = 0; i < approachedLanes->size(); i++) {
143 assert(approachedLanes->size() > i);
144 assert(approachingLanes.size() > i);
145 unsigned int approached = myAvailableLanes[(*approachedLanes)[i]];
147 incomingEdge->setConnection((
unsigned int) approachingLanes[i], myCurrentOutgoing,
150 delete approachedLanes;
157 std::deque<int>* ret =
new std::deque<int>();
158 unsigned int noLanes = (
unsigned int) approachingLanes.size();
162 ret->push_back(dest);
166 unsigned int noOutgoingLanes = (
unsigned int)myAvailableLanes.size();
168 ret->push_back(dest);
169 unsigned int noSet = 1;
172 while (noSet < noLanes) {
178 if (noOutgoingLanes == noSet) {
187 if (dest + loffset >= static_cast<int>(noOutgoingLanes)) {
190 for (
unsigned int i = 0; i < ret->size(); i++) {
191 (*ret)[i] = (*ret)[i] - 1;
196 ret->push_back(dest + loffset);
201 if (noOutgoingLanes == noSet) {
206 if (noSet < noLanes) {
209 if (dest < roffset) {
212 for (
unsigned int i = 0; i < ret->size(); i++) {
213 (*ret)[i] = (*ret)[i] + 1;
216 ret->push_front(dest - roffset);
241 myPosition(position),
243 myDistrict(district),
244 myHaveCustomPoly(false),
256 bool updateEdgeGeometries) {
263 if (updateEdgeGeometries) {
267 (*i)->setGeometry(geom);
272 (*i)->setGeometry(geom);
307 for (std::set<NBTrafficLightDefinition*>::const_iterator i = trafficLights.begin(); i != trafficLights.end(); ++i) {
319 if ((*i)->getID().find(
"joined") == 0) {
331 for (std::set<NBTrafficLightDefinition*>::iterator it = oldDefs.begin(); it != oldDefs.end(); ++it) {
333 if (dynamic_cast<NBOwnTLDef*>(orig) == 0) {
335 const std::vector<NBNode*>& nodes = orig->
getNodes();
336 while (!nodes.empty()) {
337 nodes.front()->removeTrafficLight(orig);
338 newDef->
addNode(nodes.front());
351 (*it)->shiftTLConnectionLaneIndex(edge, offset);
358 unsigned int ret = 0;
359 unsigned int pos = 0;
375 if ((*i)->isConnectedTo(dummy) && *i != dummy) {
376 incomingConnected.push_back(*i);
383 outgoingConnected.push_back(*i);
388 remapRemoved(tc, dummy, incomingConnected, outgoingConnected);
437 if (in->
getNumLanes() != (*opposite)->getNumLanes()) {
450 NBEdge* toE,
int toL,
int numPoints)
const {
457 bool noSpline =
false;
463 unsigned int noInitialPoints = 0;
477 Line cross(straightConn);
478 cross.
sub(cross.
p1().
x(), cross.
p1().
y());
480 center.
sub(cross.
p2());
485 if (angle < M_PI / 4. || angle > 7. / 4.*
M_PI) {
526 WRITE_WARNING(
"Could not use edge geometry for internal lane, node '" +
getID() +
"'.");
543 for (
int i = 0; i < (
int) init.size(); ++i) {
545 def[i * 3 + 1] = init[i].x();
547 def[i * 3 + 3] = init[i].y();
550 bezier(noInitialPoints, def, numPoints, ret_buf);
553 for (
int i = 0; i < (
int) numPoints; i++) {
554 Position current(ret_buf[i * 3 + 1], ret_buf[i * 3 + 3]);
555 if (prev != current) {
577 if (fromE == otherFromE) {
581 if (!
foes(otherFromE, otherToE, fromE, toE)) {
589 bool bothLeft = thisLeft && otherLeft;
590 if (c.
tlID !=
"" && !bothLeft) {
624 WRITE_WARNING(
"Junction '" +
getID() +
"' is too complicated (#links>64); will be set to unregulated.");
628 }
else if (numConnections == 0) {
666 if (mismatchThreshold >= 0
725 std::swap(inc1, inc2);
748 std::swap(out1, out2);
759 EdgeVector::reverse_iterator i;
761 NBEdge* currentOutgoing = *i;
764 const unsigned int numApproaching = (
unsigned int)approaching->size();
765 if (numApproaching != 0) {
766 ApproachingDivider divider(approaching, currentOutgoing, buildCrossingsAndWalkingAreas);
776 (*i)->markAsInLane2LaneState();
794 EdgeVector::const_iterator i = find(
myAllEdges.begin(),
800 for (; *i != currentOutgoing;) {
802 if ((*i)->getToNode() ==
this && (*i)->getTurnDestination() != currentOutgoing) {
803 std::vector<int> connLanes = (*i)->getConnectionLanes(currentOutgoing);
804 if (connLanes.size() != 0) {
805 approaching->push_back(*i);
835 unsigned int laneOff = 0;
836 for (EdgeVector::const_iterator i = which.begin(); i != which.end(); i++) {
838 laneOff += (*i)->getNumLanes();
867 unsigned int laneOff = 0;
868 for (EdgeVector::const_iterator i = which.begin(); i != which.end(); i++) {
870 laneOff += (*i)->getNumLanes();
885 unsigned int whichLaneOff,
unsigned int byLaneOff) {
889 bool changed =
false;
891 if (c.
replaceFrom(which, whichLaneOff, by, byLaneOff)) {
894 if (c.
replaceTo(which, whichLaneOff, by, byLaneOff)) {
908 for (NBConnectionVector::iterator k = prohibiting.begin(); k != prohibiting.end(); k++) {
910 sprohibiting.
replaceFrom(which, whichLaneOff, by, byLaneOff);
911 sprohibiting.
replaceTo(which, whichLaneOff, by, byLaneOff);
972 if (find(edges.begin(), edges.end(), e) != edges.end()) {
973 edges.erase(find(edges.begin(), edges.end(), e));
975 if (edges.size() == 0) {
991 mayDrive.
getTo() == 0 ||
993 mustStop.
getTo() == 0) {
995 WRITE_WARNING(
"Something went wrong during the building of a connection...");
999 conn.push_back(mayDrive);
1006 unsigned int size = (
unsigned int) edgeid.length();
1008 std::string
id = (*i)->
getID();
1009 if (
id.substr(0, size) == edgeid) {
1019 unsigned int size = (
unsigned int) edgeid.length();
1021 std::string
id = (*i)->
getID();
1022 if (
id.substr(0, size) == edgeid) {
1047 if (removeFromConnections) {
1049 (*i)->removeFromConnections(edge);
1059 EdgeVector::const_iterator i;
1061 NBNode* conn = (*i)->getFromNode();
1064 toAdd.
mul((
SUMOReal) 1.0 / sqrt(toAdd.
x()*toAdd.
x() + toAdd.
y()*toAdd.
y()));
1068 NBNode* conn = (*i)->getToNode();
1071 toAdd.
mul((
SUMOReal) 1.0 / sqrt(toAdd.
x()*toAdd.
x() + toAdd.
y()*toAdd.
y()));
1075 if (pos.
x() == 0 && pos.
y() == 0) {
1087 (*i)->invalidateConnections();
1095 (*i)->invalidateConnections();
1140 std::vector<NBEdge*>::const_iterator i = std::find(
myAllEdges.begin(),
myAllEdges.end(), from);
1150 const NBEdge*
const possProhibitedFrom,
const NBEdge*
const possProhibitedTo,
1151 bool regardNonSignalisedLowerPriority)
const {
1153 possProhibitedFrom, possProhibitedTo,
1154 regardNonSignalisedLowerPriority);
1160 const NBEdge*
const from2,
const NBEdge*
const to2)
const {
1169 assert(find(incoming.begin(), incoming.end(), removed) == incoming.end());
1170 bool changed =
true;
1176 for (NBConnectionProhibits::iterator i = blockedConnectionsTmp.begin(); i != blockedConnectionsTmp.end(); i++) {
1181 bool blockedChanged =
false;
1183 NBConnectionVector::const_iterator j;
1184 for (j = blocked.begin(); j != blocked.end(); j++) {
1186 if (sblocked.
getFrom() == removed || sblocked.
getTo() == removed) {
1187 blockedChanged =
true;
1191 for (j = blocked.begin(); blockedChanged && j != blocked.end(); j++) {
1193 if (sblocked.
getFrom() == removed && sblocked.
getTo() == removed) {
1197 }
else if (sblocked.
getFrom() == removed) {
1198 assert(sblocked.
getTo() != removed);
1199 for (EdgeVector::const_iterator k = incoming.begin(); k != incoming.end(); k++) {
1202 }
else if (sblocked.
getTo() == removed) {
1203 assert(sblocked.
getFrom() != removed);
1204 for (EdgeVector::const_iterator k = outgoing.begin(); k != outgoing.end(); k++) {
1211 if (blockedChanged) {
1212 blockedConnectionsNew[blocker] = newBlocked;
1217 if (blocker.
getFrom() == removed && blocker.
getTo() == removed) {
1222 }
else if (blocker.
getFrom() == removed) {
1223 assert(blocker.
getTo() != removed);
1225 for (EdgeVector::const_iterator k = incoming.begin(); k != incoming.end(); k++) {
1228 }
else if (blocker.
getTo() == removed) {
1229 assert(blocker.
getFrom() != removed);
1231 for (EdgeVector::const_iterator k = outgoing.begin(); k != outgoing.end(); k++) {
1235 blockedConnectionsNew[blocker] = blocked;
1249 if (outgoing == 0) {
1260 if (
abs((
int) angle) + 1 < 45) {
1267 EdgeVector::const_iterator i =
1270 while ((*i) != incoming) {
1271 if ((*i)->getFromNode() ==
this) {
1279 EdgeVector::const_iterator i =
1282 while ((*i) != incoming) {
1294 bool mayDefinitelyPass,
const std::string& tlID)
const {
1298 if (outgoing == 0) {
1307 if ((!incoming->
isInnerEdge() &&
mustBrake(incoming, outgoing, fromlane)) && !mayDefinitelyPass) {
1321 EdgeVector::const_iterator i;
1334 std::set<NBNode*> origSet;
1336 origSet.insert((*i)->getFromNode());
1338 if (origSet.size() < 2) {
1345 NBNode* origin = (*i)->getFromNode();
1347 EdgeVector::const_iterator j =
1356 if (!(*i)->expandableBy(*j)) {
1372 std::vector<std::pair<NBEdge*, NBEdge*> >
1375 std::vector<std::pair<NBEdge*, NBEdge*> > ret;
1379 std::pair<NBEdge*, NBEdge*>(
1385 NBNode* origin = (*i)->getFromNode();
1386 EdgeVector::const_iterator j =
1390 ret.push_back(std::pair<NBEdge*, NBEdge*>(*i, *j));
1412 if ((*i)->getToNode() == n) {
1427 back_inserter(edges));
1429 back_inserter(edges));
1430 for (EdgeVector::const_iterator j = edges.begin(); j != edges.end(); ++j) {
1441 for (EdgeVector::const_iterator k = edges2.begin(); k != edges2.end(); ++k) {
1442 if ((*k)->getFromNode()->isDistrict() || (*k)->getToNode()->isDistrict()) {
1466 std::cout <<
"guess crossings for " <<
getID() <<
"\n";
1472 std::vector<std::pair<NBEdge*, bool> > normalizedLanes;
1475 const std::vector<NBEdge::Lane>& lanes = edge->
getLanes();
1477 for (std::vector<NBEdge::Lane>::const_reverse_iterator it_l = lanes.rbegin(); it_l != lanes.rend(); ++it_l) {
1478 normalizedLanes.push_back(std::make_pair(edge, ((*it_l).permissions &
SVC_PEDESTRIAN) != 0));
1481 for (std::vector<NBEdge::Lane>::const_iterator it_l = lanes.begin(); it_l != lanes.end(); ++it_l) {
1482 normalizedLanes.push_back(std::make_pair(edge, ((*it_l).permissions &
SVC_PEDESTRIAN) != 0));
1487 int firstSidewalk = -1;
1488 for (
int i = 0; i < (
int)normalizedLanes.size(); ++i) {
1489 if (normalizedLanes[i].second) {
1494 if (firstSidewalk != -1) {
1496 std::vector<std::pair<NBEdge*, bool> > tmp;
1497 copy(normalizedLanes.begin() + firstSidewalk, normalizedLanes.end(), std::back_inserter(tmp));
1498 copy(normalizedLanes.begin(), normalizedLanes.begin() + firstSidewalk, std::back_inserter(tmp));
1499 normalizedLanes = tmp;
1502 bool hadCandidates =
false;
1503 for (
int i = 0; i < (
int)normalizedLanes.size(); ++i) {
1504 NBEdge* edge = normalizedLanes[i].first;
1505 const bool allowsPed = normalizedLanes[i].second;
1507 std::cout <<
" cands=" <<
toString(candidates) <<
" edge=" << edge->
getID() <<
" allowsPed=" << allowsPed <<
"\n";
1509 if (!allowsPed && (candidates.size() == 0 || candidates.back() != edge)) {
1510 candidates.push_back(edge);
1511 }
else if (allowsPed) {
1512 if (candidates.size() > 0) {
1513 hadCandidates =
true;
1519 if (hadCandidates) {
1532 std::cout <<
"checkCrossing candidates=" <<
toString(candidates) <<
"\n";
1534 if (candidates.size() == 0) {
1536 std::cout <<
"no crossing added (numCandidates=" << candidates.size() <<
")\n";
1542 for (
size_t i = 0; i < candidates.size(); ++i) {
1543 NBEdge* edge = candidates[i];
1548 std::cout <<
"no crossing added (found angle difference of " << fabs(angle - prevAngle) <<
" at i=" << i <<
"\n";
1554 std::cout <<
"no crossing added (uncontrolled, edge with speed=" << edge->
getSpeed() <<
")\n";
1560 if (candidates.size() == 1) {
1563 std::cout <<
"adding crossing: " <<
toString(candidates) <<
"\n";
1569 for (EdgeVector::iterator it = candidates.begin(); it != candidates.end(); ++it) {
1570 SUMOReal angle = (*it)->getCrossingAngle(
this);
1571 if (it != candidates.begin()) {
1572 NBEdge* prev = *(it - 1);
1580 prevPos = prev->
getLanes()[laneI].shape[-1];
1583 prevPos = prev->
getLanes()[laneI].shape[0];
1588 currPos = curr->
getLanes()[laneI].shape[0];
1591 currPos = curr->
getLanes()[laneI].shape[-1];
1597 <<
" prevAngle=" << prevAngle
1598 <<
" angle=" << angle
1599 <<
" intermediateWidth=" << intermediateWidth
1612 std::cout <<
"adding crossing: " <<
toString(candidates) <<
"\n";
1623 unsigned int noInternalNoSplits = 0;
1625 const std::vector<NBEdge::Connection>& elv = (*i)->getConnections();
1626 for (std::vector<NBEdge::Connection>::const_iterator k = elv.begin(); k != elv.end(); ++k) {
1627 if ((*k).toEdge == 0) {
1630 noInternalNoSplits++;
1633 unsigned int lno = 0;
1634 unsigned int splitNo = 0;
1636 (*i)->buildInnerEdges(*
this, noInternalNoSplits, lno, splitNo);
1638 if (buildCrossingsAndWalkingAreas) {
1643 if ((*it).prevWalkingArea ==
"" || (*it).nextWalkingArea ==
"") {
1658 unsigned int index = 0;
1661 if ((*it).edges.size() > 2) {
1662 WRITE_ERROR(
"Crossings across more than 2 edges not yet implemented (" +
toString((*it).edges) +
")");
1667 (*it).shape.clear();
1669 const int begDir = (edges.front()->getFromNode() ==
this ?
FORWARD :
BACKWARD);
1670 const int endDir = (edges.back()->getToNode() ==
this ?
FORWARD :
BACKWARD);
1671 NBEdge::Lane crossingBeg = edges.front()->getFirstNonPedestrianLane(begDir);
1672 NBEdge::Lane crossingEnd = edges.back()->getFirstNonPedestrianLane(endDir);
1679 (*it).shape.push_back(crossingBeg.
shape[begDir ==
FORWARD ? 0 : -1]);
1680 (*it).shape.push_back(crossingEnd.
shape[endDir ==
FORWARD ? -1 : 0]);
1689 unsigned int index = 0;
1692 std::cout <<
"build walkingAreas for " <<
getID() <<
":\n";
1695 std::vector<std::pair<NBEdge*, NBEdge::Lane> > normalizedLanes;
1698 const std::vector<NBEdge::Lane>& lanes = edge->
getLanes();
1700 for (std::vector<NBEdge::Lane>::const_reverse_iterator it_l = lanes.rbegin(); it_l != lanes.rend(); ++it_l) {
1703 normalizedLanes.
push_back(std::make_pair(edge, l));
1706 for (std::vector<NBEdge::Lane>::const_iterator it_l = lanes.begin(); it_l != lanes.end(); ++it_l) {
1710 normalizedLanes.
push_back(std::make_pair(edge, l));
1716 std::vector<std::pair<int, int> > waIndices;
1718 NBEdge* prevEdge = normalizedLanes.back().first;
1719 for (
int i = 0; i < (
int)normalizedLanes.size(); ++i) {
1720 NBEdge* edge = normalizedLanes[i].first;
1728 waIndices.push_back(std::make_pair(start, i - start));
1738 <<
" waI=" << waIndices.size() <<
" crossingBetween=" <<
crossingBetween(edge, prevEdge) <<
"\n";
1743 const int waNumLanes = (
int)normalizedLanes.size() - start;
1744 if (waIndices.size() == 0) {
1745 waIndices.push_back(std::make_pair(start, waNumLanes));
1747 std::cout <<
" single wa, end at wrap-around\n";
1750 if (waIndices.front().first == 0) {
1751 NBEdge* edge = normalizedLanes.front().first;
1752 NBEdge* prevEdge = normalizedLanes.back().first;
1755 waIndices.push_back(std::make_pair(start, waNumLanes));
1757 std::cout <<
" do not wrap around, turn-around in between\n";
1761 waIndices.front().first = start;
1762 waIndices.front().second = waNumLanes + waIndices.front().second;
1764 std::cout <<
" wrapping around\n";
1769 waIndices.push_back(std::make_pair(start, waNumLanes));
1771 std::cout <<
" end at wrap-around\n";
1777 std::cout <<
" normalizedLanes=" << normalizedLanes.size() <<
" waIndices:\n";
1778 for (
int i = 0; i < (
int)waIndices.size(); ++i) {
1779 std::cout <<
" " << waIndices[i].first <<
", " << waIndices[i].second <<
"\n";
1783 for (
int i = 0; i < (
int)waIndices.size(); ++i) {
1784 const bool buildExtensions = waIndices[i].second != (
int)normalizedLanes.size();
1785 const int start = waIndices[i].first;
1786 const int prev = start > 0 ? start - 1 : (
int)normalizedLanes.size() - 1;
1787 const int count = waIndices[i].second;
1788 const int end = (start + count) % normalizedLanes.size();
1792 std::cout <<
"build walkingArea " << wa.
id <<
" start=" << start <<
" end=" << end <<
" count=" << count <<
" prev=" << prev <<
":\n";
1797 bool connectsCrossing =
false;
1798 std::vector<Position> connectedPoints;
1800 if ((*it).edges.back() == normalizedLanes[end].first
1801 && (normalizedLanes[end].second.permissions &
SVC_PEDESTRIAN) == 0) {
1803 (*it).nextWalkingArea = wa.
id;
1804 endCrossingWidth = (*it).width;
1805 connectsCrossing =
true;
1806 connectedPoints.push_back((*it).shape[-1]);
1808 std::cout <<
" crossing " << (*it).id <<
" ends\n";
1811 if ((*it).edges.front() == normalizedLanes[prev].first
1812 && (normalizedLanes[prev].second.permissions &
SVC_PEDESTRIAN) == 0) {
1814 (*it).prevWalkingArea = wa.
id;
1816 startCrossingWidth = (*it).width;
1817 connectsCrossing =
true;
1821 connectedPoints.push_back((*it).shape[0]);
1823 std::cout <<
" crossing " << (*it).id <<
" starts\n";
1826 if (
gDebugFlag1) std::cout <<
" check connections to crossing " << (*it).id
1827 <<
" cFront=" << (*it).edges.front()->getID() <<
" cBack=" << (*it).edges.back()->getID()
1828 <<
" wEnd=" << normalizedLanes[end].first->getID() <<
" wStart=" << normalizedLanes[start].first->getID()
1829 <<
" wStartPrev=" << normalizedLanes[prev].first->getID()
1832 if (count < 2 && !connectsCrossing) {
1836 wa.
width = (endCrossingWidth + startCrossingWidth) / 2;
1838 std::set<NBEdge*> connected;
1839 for (
int j = 0; j < count; ++j) {
1840 const int nlI = (start + j) % normalizedLanes.size();
1841 NBEdge* edge = normalizedLanes[nlI].first;
1844 if (connected.count(edge) == 0) {
1852 connected.insert(edge);
1859 if (buildExtensions) {
1866 l = normalizedLanes[end > 0 ? end - 1 : normalizedLanes.size() - 1].second;
1873 NBEdge* e1 = *connected.begin();
1874 NBEdge* e2 = *(++connected.begin());
1881 int combinations = 0;
1882 for (std::vector<Position>::const_iterator it1 = connectedPoints.begin(); it1 != connectedPoints.end(); ++it1) {
1883 for (std::vector<Position>::const_iterator it2 = connectedPoints.begin(); it2 != connectedPoints.end(); ++it2) {
1893 std::cout <<
" combinations=" << combinations <<
" connectedPoints=" << connectedPoints <<
"\n";
1896 if (combinations > 0) {
1940 EdgeVector::const_iterator it1 = find(edges.begin(), edges.end(), e1);
1941 EdgeVector::const_iterator it2 = find(edges.begin(), edges.end(), e2);
1942 if (it1 != edges.end() && it2 != edges.end()) {
1958 while (it != it_end) {
1959 result.push_back(*it);
1980 if (
MAX2(angle0, angle1) <= 160) {
2008 if ((*it).id ==
id) {
2012 throw ProcessError(
"Request for unknown crossing '" +
id +
"'");
2019 (*it).tlLinkNo = startIndex++;