52 #define DEBUG_COND2(obj) (obj->isSelected())
56 #define DEBUG_COND_ZIPPER (ego->isSelected())
65 #define INVALID_TIME -1000
68 #define JM_CROSSING_GAP_DEFAULT 10
71 #define DIVERGENCE_MIN_WIDTH 2.5
73 #define NO_INTERSECTION 10000.0
79 double length,
double foeVisibilityDistance,
bool keepClear,
83 myLaneBefore(predLane),
93 myFoeVisibilityDistance(foeVisibilityDistance),
97 myKeepClear(keepClear),
99 myInternalLaneBefore(nullptr),
103 myWalkingAreaFoe(nullptr),
104 myWalkingAreaFoeExit(nullptr),
105 myHavePedestrianCrossingFoe(false),
106 myParallelRight(nullptr),
107 myParallelLeft(nullptr),
108 myAmIndirect(indirect),
109 myRadius(std::numeric_limits<double>::max()),
110 myJunction(nullptr) {
119 const double dist = from.back().distanceTo2D(to.front());
125 myLateralShift = (from.back().distanceTo2D(to.front()) < dist) ? dist : -dist;
140 const std::vector<MSLink*>& foeLinks,
141 const std::vector<MSLane*>& foeLanes,
142 MSLane* internalLaneBefore) {
152 for (std::vector<MSLane*>::const_iterator it_lane = foeLanes.begin(); it_lane != foeLanes.end(); ++it_lane) {
160 if (internalLaneBefore !=
nullptr) {
162 lane = internalLaneBefore;
168 #ifdef MSLink_DEBUG_CROSSING_POINTS
171 if (lane !=
nullptr) {
172 const bool beforeInternalJunction = lane->
getLinkCont()[0]->getViaLaneOrLane()->getEdge().isInternal();
177 for (std::vector<const MSLane*>::const_iterator it_lane =
myFoeLanes.begin(); it_lane !=
myFoeLanes.end(); ++it_lane) {
180 if (sameTarget && !beforeInternalJunction && !
contIntersect(lane, *it_lane)) {
183 const MSLane* sibling = *it_lane;
185 if (lane->
getShape().back().distanceTo2D(sibling->
getShape().back()) >= minDist) {
189 #ifdef MSLink_DEBUG_CROSSING_POINTS
190 std::cout <<
" " << lane->
getID() <<
" dummy merge with indirect" << (*it_lane)->getID() <<
"\n";
194 #ifdef MSLink_DEBUG_CROSSING_POINTS
195 std::cout <<
" " << lane->
getID() <<
" dummy merge with " << (*it_lane)->getID() <<
"\n";
203 #ifdef MSLink_DEBUG_CROSSING_POINTS
205 <<
" " << lane->
getID()
206 <<
" merges with " << (*it_lane)->getID()
207 <<
" nextLane " << lane->
getLinkCont()[0]->getViaLaneOrLane()->getID()
215 #ifdef MSLink_DEBUG_CROSSING_POINTS_DETAILS
216 std::cout <<
" intersections1=" <<
toString(intersections1) <<
"\n";
218 bool haveIntersection =
true;
219 if (intersections1.size() == 0) {
221 haveIntersection =
false;
222 }
else if (intersections1.size() > 1) {
223 std::sort(intersections1.begin(), intersections1.end());
225 std::vector<double> intersections2 = (*it_lane)->getShape().intersectsAtLengths2D(lane->
getShape());
226 #ifdef MSLink_DEBUG_CROSSING_POINTS_DETAILS
227 std::cout <<
" intersections2=" <<
toString(intersections2) <<
"\n";
229 if (intersections2.size() == 0) {
230 intersections2.push_back(0);
231 }
else if (intersections2.size() > 1) {
232 std::sort(intersections2.begin(), intersections2.end());
234 if (haveIntersection) {
236 intersections1.back() -= (*it_lane)->getWidth() / 2;
237 intersections2.back() -= lane->
getWidth() / 2;
239 intersections1.back() =
MAX2(0.0, intersections1.back());
240 intersections2.back() =
MAX2(0.0, intersections2.back());
244 intersections2.back() = (*it_lane)->interpolateGeometryPosToLanePos(intersections2.back());
249 intersections1.back() = 0;
254 lane->
getLength() - intersections1.back(),
255 (*it_lane)->getLength() - intersections2.back()));
257 #ifdef MSLink_DEBUG_CROSSING_POINTS
259 <<
" intersection of " << lane->
getID()
261 <<
" with " << (*it_lane)->getID()
262 <<
" totalLength=" << (*it_lane)->getLength()
274 const MSLane* sibling = it->getViaLane();
275 if (sibling != lane && sibling !=
nullptr) {
277 if (lane->
getShape().front().distanceTo2D(sibling->
getShape().front()) >= minDist) {
286 #ifdef MSLink_DEBUG_CROSSING_POINTS
287 std::cout <<
" adding same-origin foe" << sibling->
getID()
300 const MSEdge* target = &(it->getLane()->getEdge());
304 if (target == myTarget) {
306 #ifdef MSLink_DEBUG_CROSSING_POINTS
307 std::cout <<
" sublaneFoeLink (same target): " << it->getViaLaneOrLane()->getID() <<
"\n";
312 #ifdef MSLink_DEBUG_CROSSING_POINTS
313 std::cout <<
" sublaneFoeLink2 (other target: " << it->getViaLaneOrLane()->getID() <<
"\n";
354 double lbcSibling = 0;
368 lbcSibling += s[-1].distanceTo2D(s[-2]);
374 lbcLane += l[-1].distanceTo2D(l[-2]);
378 #ifdef MSLink_DEBUG_CROSSING_POINTS_DETAILS
379 std::cout <<
" sameSource=" << sameSource <<
" minDist=" << minDist <<
" backDist=" << l.back().distanceTo2D(s.back()) <<
"\n";
381 if (l.back().distanceTo2D(s.back()) > minDist) {
388 std::vector<double> distances = l.
distances(s);
389 #ifdef MSLink_DEBUG_CROSSING_POINTS
390 std::cout <<
" distances=" <<
toString(distances) <<
"\n";
392 assert(distances.size() == l.size() + s.size());
393 if (distances.back() > minDist && distances[l.size() - 1] > minDist) {
395 for (
int j = (
int)s.size() - 2; j >= 0; j--) {
396 const int i = j + (int)l.size();
397 const double segLength = s[j].distanceTo2D(s[j + 1]);
398 if (distances[i] > minDist) {
399 lbcSibling += segLength;
402 lbcSibling += segLength - (minDist - distances[i]) * segLength / (distances[i + 1] - distances[i]);
406 for (
int i = (
int)l.size() - 2; i >= 0; i--) {
407 const double segLength = l[i].distanceTo2D(l[i + 1]);
408 if (distances[i] > minDist) {
409 lbcLane += segLength;
412 lbcLane += segLength - (minDist - distances[i]) * segLength / (distances[i + 1] - distances[i]);
417 assert(lbcSibling >= -NUMERICAL_EPS);
418 assert(lbcLane >= -NUMERICAL_EPS);
420 const double distToDivergence1 = sibling->
getLength() - lbcSibling;
421 const double distToDivergence2 = lane->
getLength() - lbcLane;
422 const double distToDivergence =
MIN3(
423 MAX2(distToDivergence1, distToDivergence2),
425 #ifdef MSLink_DEBUG_CROSSING_POINTS
426 std::cout <<
" distToDivergence=" << distToDivergence
427 <<
" distTD1=" << distToDivergence1
428 <<
" distTD2=" << distToDivergence2
429 <<
" length=" << length
430 <<
" sibLength=" << sibLength
433 return distToDivergence;
439 if (foe->
getLinkCont()[0]->getViaLane() !=
nullptr) {
441 return intersections.size() > 0;
449 const bool setRequest,
const SUMOTime arrivalTimeBraking,
const double arrivalSpeedBraking,
const SUMOTime waitingTime,
double dist,
double latOffset) {
451 #ifdef DEBUG_APPROACHING
455 std::cout <<
"'" << i->first->getID() <<
"'" << std::endl;
461 arrivalTimeBraking, arrivalSpeedBraking, waitingTime, dist, approaching->
getSpeed(), latOffset));
468 #ifdef DEBUG_APPROACHING
472 std::cout <<
"'" << i->first->getID() <<
"'" << std::endl;
490 if ((*i)->isBlockingAnyone()) {
501 #ifdef DEBUG_APPROACHING
504 std::cout <<
"' Removing approaching vehicle '" << veh->
getID() <<
"'\nCurrently registered vehicles:" << std::endl;
506 std::cout <<
"'" << i->first->getID() <<
"'" << std::endl;
520 return ApproachingVehicleInformation(
INVALID_TIME,
INVALID_TIME, 0, 0,
false,
INVALID_TIME, 0, 0, 0, 0, 0);
531 const double leaveSpeed,
const double vehicleLength)
const {
532 return arrivalTime +
TIME2STEPS((
getLength() + vehicleLength) /
MAX2(0.5 * (arrivalSpeed + leaveSpeed), NUMERICAL_EPS));
538 double impatience,
double decel,
SUMOTime waitingTime,
double posLat,
550 assert(
myLane != foeLink->getLane());
551 for (
const auto& it : foeLink->myApproachingVehicles) {
555 ((posLat < foe->getLateralPositionOnLane() + it.second.latOffset &&
myLane->
getIndex() > foeLink->myLane->getIndex())
558 && (arrivalTime > it.second.arrivalTime
562 if (
blockedByFoe(foe, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed,
false,
563 impatience, decel, waitingTime, ego)) {
564 #ifdef MSLink_DEBUG_OPENED
566 std::cout <<
SIMTIME <<
" blocked by " << foe->
getID() <<
" arrival=" << arrivalTime <<
" foeArrival=" << it.second.arrivalTime <<
"\n";
569 if (collectFoes ==
nullptr) {
570 #ifdef MSLink_DEBUG_OPENED
572 std::cout <<
" link=" <<
getViaLaneOrLane()->
getID() <<
" blocked by sublaneFoe=" << foe->
getID() <<
" foeLink=" << foeLink->getViaLaneOrLane()->getID() <<
" posLat=" << posLat <<
"\n";
577 collectFoes->push_back(it.first);
588 for (
const auto& it : foeLink->myApproachingVehicles) {
598 if (
blockedByFoe(foe, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed,
false,
599 impatience, decel, waitingTime, ego)) {
600 #ifdef MSLink_DEBUG_OPENED
602 std::cout <<
SIMTIME <<
" blocked by sublane foe " << foe->
getID() <<
" arrival=" << arrivalTime <<
" foeArrival=" << it.second.arrivalTime <<
"\n";
605 if (collectFoes ==
nullptr) {
606 #ifdef MSLink_DEBUG_OPENED
608 std::cout <<
" link=" <<
getViaLaneOrLane()->
getID() <<
" blocked by sublaneFoe2=" << foe->
getID() <<
" foeLink=" << foeLink->getViaLaneOrLane()->getID() <<
" posLat=" << posLat <<
"\n";
613 collectFoes->push_back(it.first);
624 return collectFoes ==
nullptr || collectFoes->size() == 0;
630 #ifdef MSLink_DEBUG_OPENED
641 if (link->haveRed()) {
645 #ifdef MSLink_DEBUG_OPENED
647 std::cout <<
" foeLink=" << link->getViaLaneOrLane()->getID() <<
" numApproaching=" << link->getApproaching().size() <<
"\n";
650 if (link->blockedAtTime(arrivalTime, leaveTime, arrivalSpeed, leaveSpeed,
myLane == link->getLane(),
651 impatience, decel, waitingTime, collectFoes, ego)) {
655 if (collectFoes !=
nullptr && collectFoes->size() > 0) {
664 bool sameTargetLane,
double impatience,
double decel,
SUMOTime waitingTime,
667 #ifdef MSLink_DEBUG_OPENED
672 std::stringstream stream;
674 <<
" foeVeh=" << it.first->getID() <<
" (below ignore speed)"
677 std::cout << stream.str();
687 &&
blockedByFoe(it.first, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, sameTargetLane,
688 impatience, decel, waitingTime, ego)) {
689 if (collectFoes ==
nullptr) {
692 collectFoes->push_back(it.first);
702 SUMOTime arrivalTime,
SUMOTime leaveTime,
double arrivalSpeed,
double leaveSpeed,
703 bool sameTargetLane,
double impatience,
double decel,
SUMOTime waitingTime,
705 #ifdef MSLink_DEBUG_OPENED
707 std::stringstream stream;
709 <<
" foeVeh=" << veh->
getID()
714 std::cout << stream.str();
721 assert(waitingTime > 0);
736 #ifdef MSLink_DEBUG_OPENED
738 std::stringstream stream;
739 stream <<
" imp=" << impatience <<
" fATb=" << avi.
arrivalTimeBraking <<
" fAT2=" << foeArrivalTime <<
" lA=" << lookAhead <<
" egoAT=" << arrivalTime <<
" egoLT=" << leaveTime <<
"\n";
740 std::cout << stream.str();
745 if (sameTargetLane && (arrivalTime - avi.
leavingTime < lookAhead
748 #ifdef MSLink_DEBUG_OPENED
750 std::cout <<
" blocked (cannot follow)\n";
755 }
else if (foeArrivalTime > leaveTime + lookAhead) {
759 #ifdef MSLink_DEBUG_OPENED
761 std::cout <<
" blocked (cannot lead)\n";
768 #ifdef MSLink_DEBUG_OPENED
770 std::cout <<
" blocked (hard conflict)\n";
782 if (link->blockedAtTime(arrivalTime, leaveTime, speed, speed,
myLane == link->getLane(), 0, decel, 0)) {
787 if (lane->getVehicleNumberWithPartials() > 0) {
795 std::pair<const SUMOVehicle*, const MSLink*>
797 double closetDist = std::numeric_limits<double>::max();
799 const MSLink* foeLink =
nullptr;
801 for (
const auto& it : link->myApproachingVehicles) {
804 return std::make_pair(
nullptr, wrapAround);
805 }
else if (it.second.dist < closetDist) {
806 closetDist = it.second.dist;
807 if (it.second.willPass) {
814 return std::make_pair(closest, foeLink);
847 assert(pred2 !=
nullptr);
849 assert(predLink !=
nullptr);
873 assert(pred2 !=
nullptr);
875 assert(predLink !=
nullptr);
890 std::vector<std::pair<SUMOTime, const SUMOVehicle*> > toSort;
892 toSort.push_back(std::make_pair(it.second.arrivalTime, it.first));
894 std::sort(toSort.begin(), toSort.end());
895 for (std::vector<std::pair<SUMOTime, const SUMOVehicle*> >::const_iterator it = toSort.begin(); it != toSort.end(); ++it) {
919 while (lane !=
nullptr && lane->
isInternal()) {
931 while (lane !=
nullptr && lane->
isInternal()) {
946 double totalDist = 0.;
947 bool foundCrossing =
false;
948 while (via !=
nullptr) {
954 foundCrossing =
true;
972 for (foe_ix = 0; foe_ix != (int)
myFoeLanes.size(); ++foe_ix) {
979 #ifdef MSLink_DEBUG_CROSSING_POINTS
986 if (dist == -10000.) {
990 #ifdef MSLink_DEBUG_CROSSING_POINTS
992 <<
"' at distance " << dist <<
" (approach along '"
1039 const MSLink* link =
nullptr;
1040 while (lane !=
nullptr) {
1050 const MSLink* link =
this;
1076 std::cout <<
SIMTIME <<
" getLeaderInfo link=" <<
getViaLaneOrLane()->
getID() <<
" dist=" << dist <<
" isShadowLink=" << isShadowLink <<
"\n";
1079 for (
int i = 0; i < (int)
myFoeLanes.size(); ++i) {
1086 const double crossingWidth = (sameTarget || sameSource) ? 0 : foeLane->
getWidth();
1092 std::cout <<
" distToCrossing=" << distToCrossing <<
" foeLane=" << foeLane->
getID() <<
" cWidth=" << crossingWidth
1096 <<
" contLane=" << contLane
1101 if (distToCrossing + crossingWidth < 0 && !sameTarget
1105 bool ignoreGreenCont =
false;
1106 bool foeIndirect =
false;
1111 if (entry !=
nullptr && entry->
haveGreen()
1112 && foeEntry !=
nullptr && foeEntry->
haveGreen()
1115 ignoreGreenCont =
true;
1120 std::cout <<
" ignore:noIntersection\n";
1135 const double leaderBackDist = foeDistToCrossing - leaderBack;
1136 const double l2 = ego !=
nullptr ? ego->
getLength() + 2 : 0;
1138 const bool pastTheCrossingPoint = leaderBackDist + foeCrossingWidth + sagitta < 0;
1141 const bool ignoreIndirectBicycleTurn = pastTheCrossingPoint && foeIsBicycleTurn;
1142 const bool cannotIgnore = ((contLane && !ignoreIndirectBicycleTurn) || sameTarget || sameSource) && ego !=
nullptr;
1143 const bool inTheWay = (((!pastTheCrossingPoint && distToCrossing > 0) || (sameTarget && distToCrossing > leaderBackDist - leader->
getLength()))
1151 std::cout <<
" candidate leader=" << leader->
getID()
1152 <<
" cannotIgnore=" << cannotIgnore
1153 <<
" fdtc=" << foeDistToCrossing
1154 <<
" lb=" << leaderBack
1155 <<
" lbd=" << leaderBackDist
1156 <<
" fcwidth=" << foeCrossingWidth
1158 <<
" sagitta=" << sagitta
1159 <<
" foePastCP=" << pastTheCrossingPoint
1160 <<
" inTheWay=" << inTheWay
1161 <<
" willPass=" << willPass
1163 <<
" ignoreGreenCont=" << ignoreGreenCont
1164 <<
" foeIndirect=" << foeIndirect
1165 <<
" foeBikeTurn=" << foeIsBicycleTurn
1166 <<
" isOpposite=" << isOpposite <<
"\n";
1168 if (leader == ego) {
1172 if (!inTheWay && ignoreGreenCont) {
1174 std::cout <<
" ignoreGreenCont\n";
1179 if (distToCrossing < -POSITION_EPS && !inTheWay
1180 && (ego ==
nullptr || !
MSGlobals::gComputeLC || distToCrossing < -ego->getVehicleType().getLength())) {
1182 std::cout <<
" ego entered conflict area\n";
1187 if ((!cannotIgnore || leader->
isStopped() || sameTarget)
1195 std::cout <<
" foe will not pass\n";
1204 && (!foeStrategicBlocked)) {
1208 const double latGap = (fabs(posLat - posLatLeader)
1213 <<
" sameSource=" << sameSource
1214 <<
" sameTarget=" << sameTarget
1215 <<
" foeLane=" << foeLane->
getID()
1216 <<
" leader=" << leader->
getID()
1219 <<
" egoLat=" << posLat
1220 <<
" leaderLat=" << posLatLeader
1221 <<
" leaderLatOffset=" << leader->
getLatOffset(foeLane)
1222 <<
" latGap=" << latGap
1223 <<
" maneuverDist=" << maneuverDist
1228 if (latGap > 0 && (latGap > maneuverDist || !sameTarget)
1236 if ((posLat > posLatLeader) == leaderFromRight) {
1239 std::cout <<
" ignored (same source) leaderFromRight=" << leaderFromRight <<
"\n";
1250 leaderFromRight = !leaderFromRight;
1252 if ((posLat > posLatLeader) == leaderFromRight
1259 std::cout <<
" ignored (different source) leaderFromRight=" << leaderFromRight <<
"\n";
1273 bool fromLeft =
true;
1274 if (ego ==
nullptr) {
1277 gap = leaderBackDist;
1281 distToCrossing += foeLane->
getWidth() / 2;
1282 if (gap + foeCrossingWidth < 0) {
1289 fromLeft = foeDistToCrossing > 0.5 * foeLane->
getLength();
1290 }
else if ((contLane && !sameSource && !ignoreIndirectBicycleTurn) || isOpposite) {
1291 gap = -std::numeric_limits<double>::max();
1294 std::cout <<
" distToCrossing=" << distToCrossing <<
" leader back=" << leaderBack <<
" backDist=" << leaderBackDist
1299 if (pastTheCrossingPoint && !sameTarget) {
1308 gap = -std::numeric_limits<double>::max();
1314 const bool stopAsap = leader->
isFrontOnLane(foeLane) ? cannotIgnore : (sameTarget || sameSource);
1316 std::cout <<
" leader=" << leader->
getID() <<
" contLane=" << contLane <<
" cannotIgnore=" << cannotIgnore <<
" stopAsap=" << stopAsap <<
"\n";
1321 result.emplace_back(leader, gap, stopAsap ? -1 : distToCrossing, fromLeft, inTheWay);
1332 const double vehSideOffset = (foeDistToCrossing +
myLaneBefore->
getWidth() * 0.5 - vehWidth * 0.5
1341 result.emplace_back(
nullptr, -1, distToPeds);
1347 if (ego !=
nullptr) {
1355 const MSLane* foeLane = *it;
1359 if (leader == ego) {
1373 <<
" foeLane=" << foeLane->
getID()
1374 <<
" leader=" << leader->
getID()
1377 <<
" egoLat=" << posLat
1378 <<
" leaderLat=" << posLatLeader
1379 <<
" leaderLatOffset=" << leader->
getLatOffset(foeLane)
1381 <<
" foeIndex=" << foeLane->
getIndex()
1387 if ((posLat < posLatLeader && myInternalLaneBefore->
getIndex() > foeLane->
getIndex())
1390 std::cout <<
SIMTIME <<
" blocked by " << leader->
getID() <<
" (sublane split) foeLane=" << foeLane->
getID() <<
"\n";
1395 result.emplace_back(leader, gap, -1);
1411 double distToPeds = std::numeric_limits<double>::max();
1420 #ifdef DEBUG_WALKINGAREA
1425 <<
" dist=" << dist <<
"\n";
1428 if (dist < ego->getVehicleType().getWidth() / 2 ||
isInFront(ego, egoPath, p)) {
1429 distToPeds =
MIN2(distToPeds, dist);
1430 if (collectBlockers !=
nullptr) {
1431 collectBlockers->push_back(p);
1435 if (distToPeds != std::numeric_limits<double>::max()) {
1446 #ifdef DEBUG_WALKINGAREA
1448 std::cout <<
" angleDiff=" <<
RAD2DEG(angleDiff) <<
"\n";
1451 if (angleDiff <
DEG2RAD(75)) {
1460 if (direction == -1) {
1462 }
else if (direction == 1) {
1487 if (before !=
nullptr && after !=
nullptr) {
1489 if (link->getLane() == after) {
1507 throw ProcessError(
"Zipper junctions with more than two conflicting lanes are not supported (at junction '"
1515 <<
" dist=" << dist <<
" ignoring foes (arrival in " <<
STEPS2TIME(arrivalTime - now) <<
")\n")
1521 <<
" egoAT=" << arrivalTime
1523 <<
" brakeGap=" << brakeGap
1524 <<
" vSafe=" << vSafe
1525 <<
" numFoes=" << collectFoes->size()
1529 for (
const auto& item : *collectFoes) {
1544 <<
" ignoring foe=" << foe->
getID()
1546 <<
" foeDist=" << avi.
dist
1547 <<
" foeDist2=" << foeDist
1548 <<
" foeSpeed=" << avi.
speed
1550 <<
" deltaDist=" << foeDist - dist
1573 const double uEnd =
MIN2(uMax, uAccel);
1574 const double uAvg = (avi.
speed + uEnd) / 2;
1575 const double tf0 = foeDist /
MAX2(NUMERICAL_EPS, uAvg);
1576 const double tf =
MAX2(1.0, ceil((tf0) /
TS) *
TS);
1581 const double vEnd =
MIN3(vMax, vAccel,
MAX2(uEnd, vDecel));
1582 const double vAvg = (ego->
getSpeed() + vEnd) / 2;
1583 const double te0 = dist /
MAX2(NUMERICAL_EPS, vAvg);
1584 const double te =
MAX2(1.0, ceil((te0) /
TS) *
TS);
1591 const double deltaGap = gap + tf * uAvg - safeGap - vAvg * tf;
1592 const double a = 2 * deltaGap / (tf * tf);
1598 const double w =
MIN2(1.0, te / 10);
1600 const double vZipper =
MAX3(vFollow, ego->
getSpeed() -
ACCEL2SPEED(maxDecel), w * vSafeGap + (1 - w) * vFollow);
1602 vSafe =
MIN2(vSafe, vZipper);
1605 <<
" foeDist=" << foeDist
1606 <<
" foeSpeed=" << avi.
speed
1610 <<
" uAccel=" << uAccel
1614 <<
" safeGap=" << safeGap
1618 <<
" dg=" << deltaGap
1619 <<
" aSafeGap=" << a
1621 <<
" vAccel=" << vAccel
1622 <<
" vDecel=" << vDecel
1624 <<
" vSafeGap=" << vSafeGap
1625 <<
" vFollow=" << vFollow
1627 <<
" maxDecel=" << maxDecel
1628 <<
" vZipper=" << vZipper
1629 <<
" vSafe=" << vSafe
1640 followDist > leaderDist &&
1658 for (
const MSLink* link : cand->getLinkCont()) {
1669 return fabs(posLat2 - posLat) < (width + width2) / 2;
1690 if (
id == foe->
getID()) {
#define JM_CROSSING_GAP_DEFAULT
#define DIVERGENCE_MIN_WIDTH
#define DEBUG_COND_ZIPPER
std::string time2string(SUMOTime t)
convert SUMOTime to string
@ SVC_BICYCLE
vehicle is a bicycle
const int VEHPARS_JUNCTIONMODEL_PARAMS_SET
LinkDirection
The different directions a link between two lanes may take (or a stream between two edges)....
@ PARTLEFT
The link is a partial left direction.
@ RIGHT
The link is a (hard) right direction.
@ LEFT
The link is a (hard) left direction.
@ STRAIGHT
The link is a straight direction.
@ PARTRIGHT
The link is a partial right direction.
LinkState
The right-of-way state of a link between two lanes used when constructing a NBTrafficLightLogic,...
@ LINKSTATE_ALLWAY_STOP
This is an uncontrolled, all-way stop link.
@ LINKSTATE_STOP
This is an uncontrolled, minor link, has to stop.
@ LINKSTATE_TL_GREEN_MAJOR
The link has green light, may pass.
@ LINKSTATE_ZIPPER
This is an uncontrolled, zipper-merge link.
@ LINKSTATE_TL_OFF_BLINKING
The link is controlled by a tls which is off and blinks, has to brake.
@ LINKSTATE_TL_RED
The link has red light (must brake)
@ LINKSTATE_TL_GREEN_MINOR
The link has green light, has to brake.
@ LINKSTATE_TL_OFF_NOSIGNAL
The link is controlled by a tls which is off, not blinking, may pass.
@ SUMO_ATTR_JM_IGNORE_FOE_SPEED
@ SUMO_ATTR_JM_IGNORE_IDS
@ SUMO_ATTR_JM_IGNORE_TYPES
@ SUMO_ATTR_JM_IGNORE_FOE_PROB
@ SUMO_ATTR_JM_CROSSING_GAP
@ SUMO_ATTR_JM_TIMEGAP_MINOR
bool gDebugFlag1
global utility flags for debugging
const double INVALID_DOUBLE
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
static double angleDiff(const double angle1, const double angle2)
Returns the difference of the second angle to the first angle in radiants.
bool isStrategicBlocked() const
double getManeuverDist() const
Returns the remaining unblocked distance for the current maneuver. (only used by sublane model)
double getSpeedLat() const
return the lateral speed of the current lane change maneuver
virtual bool isSelected() const
whether this vehicle is selected in the GUI
double getLength() const
Returns the vehicle's length.
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
bool isStopped() const
Returns whether the vehicle is at a stop.
double estimateSpeedAfterDistance(const double dist, const double v, const double accel) const
virtual double getSecureGap(const MSVehicle *const, const MSVehicle *const, const double speed, const double leaderSpeed, const double leaderMaxDecel) const
Returns the minimum gap to reserve if the leader is braking at maximum (>=0)
double getEmergencyDecel() const
Get the vehicle type's maximal phisically possible deceleration [m/s^2].
virtual double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver's reaction time tau (i....
virtual double followSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0) const =0
Computes the vehicle's follow speed (no dawdling)
double getMaxAccel() const
Get the vehicle type's maximum acceleration [m/s^2].
double getMaxDecel() const
Get the vehicle type's maximal comfortable deceleration [m/s^2].
A road/street connecting two junctions.
const std::set< MSTransportable * > & getPersons() const
Returns this edge's persons set.
const std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
const MSJunction * getFromJunction() const
bool isInternal() const
return whether this edge is an internal edge
static double gLateralResolution
static bool gComputeLC
whether the simulationLoop is in the lane changing phase
static bool gLefthand
Whether lefthand-drive is being simulated.
static SUMOTime gIgnoreJunctionBlocker
static bool gSublane
whether sublane simulation is enabled (sublane model or continuous lanechanging)
static bool gUsingInternalLanes
Information whether the simulation regards internal lanes.
The base class for an intersection.
AnyVehicleIterator is a structure, which manages the iteration through all vehicles on the lane,...
Representation of a lane in the micro simulation.
const std::vector< MSLink * > & getLinkCont() const
returns the container with all links !!!
MSLane * getParallelLane(int offset, bool includeOpposite=true) const
Returns the lane with the given offset parallel to this one or 0 if it does not exist.
AnyVehicleIterator anyVehiclesEnd() const
end iterator for iterating over all vehicles touching this lane in downstream direction
const MSLink * getEntryLink() const
Returns the entry link if this is an internal lane, else nullptr.
const MSLink * getLinkTo(const MSLane *const) const
returns the link to the given lane or nullptr, if it is not connected
double getLength() const
Returns the lane's length.
double getVehicleMaxSpeed(const SUMOTrafficObject *const veh) const
Returns the lane's maximum speed, given a vehicle's speed limit adaptation.
int getIndex() const
Returns the lane's index.
const std::vector< IncomingLaneInfo > & getIncomingLanes() const
MSLane * getLogicalPredecessorLane() const
get the most likely precedecessor lane (sorted using by_connections_to_sorter). The result is cached ...
MSEdge & getEdge() const
Returns the lane's edge.
const PositionVector & getShape() const
Returns this lane's shape.
double interpolateGeometryPosToLanePos(double geometryPos) const
AnyVehicleIterator anyVehiclesBegin() const
begin iterator for iterating over all vehicles touching this lane in downstream direction
MSLane * getOpposite() const
return the neighboring opposite direction lane for lane changing or nullptr
const MSLane * getNormalPredecessorLane() const
get normal lane leading to this internal lane, for normal lanes, the lane itself is returned
double getWidth() const
Returns the lane's width.
bool fromInternalLane() const
return whether the fromLane of this link is an internal lane
void writeApproaching(OutputDevice &od, const std::string fromLaneID) const
write information about all approaching vehicles to the given output device
bool isIndirect() const
whether this link is the start of an indirect turn
double computeDistToDivergence(const MSLane *lane, const MSLane *sibling, double minDist, bool sameSource) const
compute point of divergence for geomatries with a common start or end
double getLengthBeforeCrossing(const MSLane *foeLane) const
Returns the internal length from the beginning of the link's internal lane before to the crossing wit...
LinkState getState() const
Returns the current state of the link.
void checkWalkingAreaFoe(const MSVehicle *ego, const MSLane *foeLane, std::vector< const MSPerson * > *collectBlockers, LinkLeaders &result) const
check for persons on walkingarea in the path of ego vehicle
bool hasApproachingFoe(SUMOTime arrivalTime, SUMOTime leaveTime, double speed, double decel) const
Returns the information whether a vehicle is approaching on one of the link's foe streams.
std::vector< std::pair< double, double > > myLengthsBehindCrossing
const bool myAmIndirect
whether this connection is an indirect turning movement
std::vector< MSLink * > mySublaneFoeLinks
static const SUMOTime myLookaheadTime
ApproachInfos myApproachingVehicles
double myFoeVisibilityDistance
distance from which an approaching vehicle is able to see all relevant foes and may accelerate if the...
MSLink * computeParallelLink(int direction)
int myIndex
The position within this respond.
bool myHasFoes
Whether any foe links exist.
bool isInFront(const MSVehicle *ego, const PositionVector &egoPath, const MSPerson *p) const
whether the given person is in front of the car
MSLane * getViaLane() const
Returns the following inner lane.
const MSLane * myInternalLaneBefore
LinkState myState
The state of the link.
void initParallelLinks()
initialize parallel links (to be called after all links are loaded)
void setTLState(LinkState state, SUMOTime t)
Sets the current tl-state.
static const SUMOTime myLookaheadTimeZipper
MSLane * getLane() const
Returns the connected lane.
void setApproaching(const SUMOVehicle *approaching, const SUMOTime arrivalTime, const double arrivalSpeed, const double leaveSpeed, const bool setRequest, const SUMOTime arrivalTimeBraking, const double arrivalSpeedBraking, const SUMOTime waitingTime, double dist, double latOffset)
Sets the information about an approaching vehicle.
MSLane * getViaLaneOrLane() const
return the via lane if it exists and the lane otherwise
bool isConflictEntryLink() const
return whether this link enters the conflict area (not a continuation link)
double myRadius
the turning radius for this link or doublemax for straight links
int getIndex() const
Returns the respond index (for visualization)
std::vector< const SUMOVehicle * > BlockingFoes
bool havePriority() const
Returns whether this link is a major link.
bool blockedAtTime(SUMOTime arrivalTime, SUMOTime leaveTime, double arrivalSpeed, double leaveSpeed, bool sameTargetLane, double impatience, double decel, SUMOTime waitingTime, BlockingFoes *collectFoes=nullptr, const SUMOTrafficObject *ego=nullptr) const
Returns the information whether this link is blocked Valid after the vehicles have set their requests...
bool blockedByFoe(const SUMOVehicle *veh, const ApproachingVehicleInformation &avi, SUMOTime arrivalTime, SUMOTime leaveTime, double arrivalSpeed, double leaveSpeed, bool sameTargetLane, double impatience, double decel, SUMOTime waitingTime, const SUMOTrafficObject *ego) const
double getZipperSpeed(const MSVehicle *ego, const double dist, double vSafe, SUMOTime arrivalTime, BlockingFoes *collectFoes) const
return the speed at which ego vehicle must approach the zipper link
const LinkLeaders getLeaderInfo(const MSVehicle *ego, double dist, std::vector< const MSPerson * > *collectBlockers=0, bool isShadowLink=false) const
Returns all potential link leaders (vehicles on foeLanes) Valid during the planMove() phase.
bool isEntryLink() const
return whether the toLane of this link is an internal lane and fromLane is a normal lane
ApproachingVehicleInformation getApproaching(const SUMOVehicle *veh) const
const MSTrafficLightLogic * myLogic
the controlling logic or 0
static bool lateralOverlap(double posLat, double width, double posLat2, double width2)
check whether the given vehicle positions overlap laterally
std::vector< MSLink * > myFoeLinks
const MSLane * getLaneBefore() const
return the internalLaneBefore if it exists and the laneBefore otherwise
bool isInternalJunctionLink() const
return whether the fromLane and the toLane of this link are internal lanes
bool isExitLink() const
return whether the fromLane of this link is an internal lane and toLane is a normal lane
std::vector< const MSLane * > myFoeLanes
std::vector< LinkLeader > LinkLeaders
void clearState()
Remove all approaching vehicles before quick-loading state.
bool willHaveBlockedFoe() const
MSLane * myLane
The lane behind the junction approached by this link.
bool lastWasContMajorGreen() const
whether this is a link past an internal junction which currently has green major
double getInternalLengthsAfter() const
Returns the cumulative length of all internal lanes after this link.
std::string getDescription() const
get string description for this link
bool hasFoes() const
Returns whether this link belongs to a junction where more than one edge is incoming.
LinkState myLastGreenState
The last green state of the link (minor or major)
MSJunction * myJunction
the junction to which this link belongs
const MSLink * getCorrespondingEntryLink() const
returns the corresponding entry link for exitLinks to a junction.
void setRequestInformation(int index, bool hasFoes, bool isCont, const std::vector< MSLink * > &foeLinks, const std::vector< MSLane * > &foeLanes, MSLane *internalLaneBefore=0)
Sets the request information.
void removeApproaching(const SUMOVehicle *veh)
removes the vehicle from myApproachingVehicles
bool contIntersect(const MSLane *lane, const MSLane *foe)
check if the lane intersects with a foe cont-lane
bool isExitLinkAfterInternalJunction() const
return whether the fromLane of this link is an internal lane and its incoming lane is also an interna...
LinkState getLastGreenState() const
Returns the last green state of the link.
std::pair< const SUMOVehicle *, const MSLink * > getFirstApproachingFoe(const MSLink *wrapAround) const
get the foe vehicle that is closest to the intersection or nullptr along with the foe link This funct...
std::vector< MSLink * > mySublaneFoeLinks2
MSLink * getParallelLink(int direction) const
return the link that is parallel to this lane or 0
static bool ignoreFoe(const SUMOTrafficObject *ego, const SUMOVehicle *foe)
MSLane *const myInternalLane
The following junction-internal lane if used.
void addBlockedLink(MSLink *link)
double myLateralShift
lateral shift to be applied when passing this link
double getInternalLengthsBefore() const
Returns the cumulative length of all internal lanes before this link.
const MSLane * myWalkingAreaFoe
walkingArea that must be checked when entering the intersection
static bool couldBrakeForLeader(double followDist, double leaderDist, const MSVehicle *follow, const MSVehicle *leader)
whether follower could stay behind leader (possibly by braking)
const ApproachInfos & getApproaching() const
return all approaching vehicles
bool opened(SUMOTime arrivalTime, double arrivalSpeed, double leaveSpeed, double vehicleLength, double impatience, double decel, SUMOTime waitingTime, double posLat=0, BlockingFoes *collectFoes=nullptr, bool ignoreRed=false, const SUMOTrafficObject *ego=nullptr) const
Returns the information whether the link may be passed.
const MSLane * myWalkingAreaFoeExit
walkingArea that must be checked when leaving the intersection
MSLane * myLaneBefore
The lane approaching this link.
std::set< MSLink * > myBlockedFoeLinks
bool lastWasContMajor() const
whether this is a link past an internal junction which currently has priority
double getLengthsBeforeCrossing(const MSLane *foeLane) const
Returns the sum of the lengths along internal lanes following this link to the crossing with the give...
bool myHavePedestrianCrossingFoe
whether on of myFoeLanes is a crossing
SUMOTime myLastStateChange
The time of the last state change.
LinkDirection myDirection
An abstract (hopefully human readable) definition of the link's direction.
bool checkContOff() const
figure out whether the cont status remains in effect when switching off the tls
const MSLink * getCorrespondingExitLink() const
returns the corresponding exit link for entryLinks to a junction.
static bool unsafeMergeSpeeds(double leaderSpeed, double followerSpeed, double leaderDecel, double followerDecel)
return whether the given vehicles may NOT merge safely
SUMOTime getLeaveTime(const SUMOTime arrivalTime, const double arrivalSpeed, const double leaveSpeed, const double vehicleLength) const
return the expected time at which the given vehicle will clear the link
MSLink * getOppositeDirectionLink() const
return the link that is the opposite entry link to this one
MSLink(MSLane *predLane, MSLane *succLane, MSLane *via, LinkDirection dir, LinkState state, double length, double foeVisibilityDistance, bool keepClear, MSTrafficLightLogic *logic, int tlLinkIdx, bool indirect)
Constructor for simulation which uses internal lanes.
std::vector< MSLane * > mySublaneFoeLanes
LinkDirection getDirection() const
Returns the direction the vehicle passing this link take.
const MSLane * getInternalLaneBefore() const
return myInternalLaneBefore (always 0 when compiled without internal lanes)
bool haveRed() const
Returns whether this link is blocked by a red (or redyellow) traffic light.
double getLength() const
Returns the length of this link.
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
SUMOTime getCurrentTimeStep() const
Returns the current simulation step.
bool hasPersons() const
Returns whether persons are simulated.
virtual MSTransportableControl & getPersonControl()
Returns the person control.
virtual bool blockedAtDist(const MSLane *lane, double vehSide, double vehWidth, double oncomingGap, std::vector< const MSPerson * > *collectBlockers)
whether a pedestrian is blocking the crossing of lane for the given vehicle bondaries
static const double SAFETY_GAP
The parent class for traffic light logics.
MSPModel * getMovementModel()
Returns the default movement model for this kind of transportables.
const MSVehicleType & getVehicleType() const
Returns the object's "vehicle" type.
Position getPosition(const double) const
Return current position (x/y, cartesian)
Representation of a vehicle in the micro simulation.
bool willStop() const
Returns whether the vehicle will stop on the current edge.
SUMOTime getLastActionTime() const
Returns the time of the vehicle's last action point.
SUMOTime getWaitingTime() const
Returns the SUMOTime waited (speed was lesser than 0.1m/s)
bool isActive() const
Returns whether the current simulation step is an action point for the vehicle.
bool isFrontOnLane(const MSLane *lane) const
Returns the information whether the front of the vehicle is on the given lane.
MSAbstractLaneChangeModel & getLaneChangeModel()
Position getPosition(const double offset=0) const
Return current position (x/y, cartesian)
double getBackPositionOnLane(const MSLane *lane) const
Get the vehicle's position relative to the given lane.
double getLatOffset(const MSLane *lane) const
Get the offset that that must be added to interpret myState.myPosLat for the given lane.
double getLateralPositionOnLane() const
Get the vehicle's lateral position on the lane.
double getSpeed() const
Returns the vehicle's current speed.
const MSLane * getLane() const
Returns the lane the vehicle is on.
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
double getAngle() const
Returns the vehicle's direction in radians.
double getWidth() const
Get the width which vehicles of this class shall have when being drawn.
SUMOVehicleClass getVehicleClass() const
Get this vehicle type's vehicle class.
double getMinGap() const
Get the free space in front of vehicles of this class.
double getMaxSpeedLat() const
Get vehicle's maximum lateral speed [m/s].
const std::string & getID() const
Returns the name of the vehicle type.
double getLength() const
Get vehicle's length [m].
const MSCFModel & getCarFollowModel() const
Returns the vehicle type's car following model definition (const version)
const SUMOVTypeParameter & getParameter() const
const std::string & getID() const
Returns the id.
Static storage of an output device and its base (abstract) implementation.
OutputDevice & openTag(const std::string &xmlElement)
Opens an XML tag.
OutputDevice & writeAttr(const SumoXMLAttr attr, const T &val)
writes a named attribute
bool closeTag(const std::string &comment="")
Closes the most recently opened tag and optionally adds a comment.
virtual const std::string getParameter(const std::string &key, const std::string defaultValue="") const
Returns the value for a given key.
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
double angleTo2D(const Position &other) const
returns the angle in the plane of the vector pointing from here to the other position
double length2D() const
Returns the length.
std::vector< double > intersectsAtLengths2D(const PositionVector &other) const
For all intersections between this vector and other, return the 2D-length of the subvector from this ...
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)
std::vector< double > distances(const PositionVector &s, bool perpendicular=false) const
distances of all my points to s and all of s points to myself
void move2side(double amount, double maxExtension=100)
move position vector to side using certain ammount
double angleAt2D(int pos) const
get angle in certain position of position vector
PositionVector reverse() const
reverse position vector
static double rand(SumoRNG *rng=nullptr)
Returns a random real number in [0, 1)
Representation of a vehicle, person, or container.
virtual double getSpeed() const =0
Returns the object's current speed.
virtual SumoRNG * getRNG() const =0
Returns the associated RNG for this object.
virtual const MSVehicleType & getVehicleType() const =0
Returns the object's "vehicle" type.
virtual const SUMOVehicleParameter & getParameter() const =0
Returns the vehicle's parameter (including departure definition)
double getJMParam(const SumoXMLAttr attr, const double defaultValue) const
Returns the named value from the map, or the default if it is not contained there.
Representation of a vehicle.
virtual double getLateralPositionOnLane() const =0
Get the vehicle's lateral position on the lane.
Structure representing possible vehicle parameter.
bool wasSet(int what) const
Returns whether the given parameter was set.