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) {
288 lbcLane = lane->
getLength() - distToDivergence;
289 lbcSibling = lbcLane;
296#ifdef MSLink_DEBUG_CROSSING_POINTS
297 std::cout <<
" adding same-origin foe" << sibling->
getID()
310 const MSEdge* target = &(it->getLane()->getEdge());
314 if (target == myTarget) {
316#ifdef MSLink_DEBUG_CROSSING_POINTS
317 std::cout <<
" sublaneFoeLink (same target): " << it->getViaLaneOrLane()->getID() <<
"\n";
322#ifdef MSLink_DEBUG_CROSSING_POINTS
323 std::cout <<
" sublaneFoeLink2 (other target: " << it->getViaLaneOrLane()->getID() <<
"\n";
364 double lbcSibling = 0;
378 lbcSibling += s[-1].distanceTo2D(s[-2]);
384 lbcLane += l[-1].distanceTo2D(l[-2]);
388#ifdef MSLink_DEBUG_CROSSING_POINTS_DETAILS
389 std::cout <<
" sameSource=" << sameSource <<
" minDist=" << minDist <<
" backDist=" << l.back().distanceTo2D(s.back()) <<
"\n";
391 if (l.back().distanceTo2D(s.back()) > minDist) {
398 std::vector<double> distances = l.
distances(s);
399#ifdef MSLink_DEBUG_CROSSING_POINTS
400 std::cout <<
" distances=" <<
toString(distances) <<
"\n";
402 assert(distances.size() == l.size() + s.size());
403 if (distances.back() > minDist && distances[l.size() - 1] > minDist) {
405 for (
int j = (
int)s.size() - 2; j >= 0; j--) {
406 const int i = j + (int)l.size();
407 const double segLength = s[j].distanceTo2D(s[j + 1]);
408 if (distances[i] > minDist) {
409 lbcSibling += segLength;
412 lbcSibling += segLength - (minDist - distances[i]) * segLength / (distances[i + 1] - distances[i]);
416 for (
int i = (
int)l.size() - 2; i >= 0; i--) {
417 const double segLength = l[i].distanceTo2D(l[i + 1]);
418 if (distances[i] > minDist) {
419 lbcLane += segLength;
422 lbcLane += segLength - (minDist - distances[i]) * segLength / (distances[i + 1] - distances[i]);
427 assert(lbcSibling >= -NUMERICAL_EPS);
428 assert(lbcLane >= -NUMERICAL_EPS);
430 const double distToDivergence1 = sibling->
getLength() - lbcSibling;
431 const double distToDivergence2 = lane->
getLength() - lbcLane;
432 const double distToDivergence =
MIN3(
433 MAX2(distToDivergence1, distToDivergence2),
435#ifdef MSLink_DEBUG_CROSSING_POINTS
436 std::cout <<
" distToDivergence=" << distToDivergence
437 <<
" distTD1=" << distToDivergence1
438 <<
" distTD2=" << distToDivergence2
439 <<
" length=" << length
440 <<
" sibLength=" << sibLength
443 return distToDivergence;
449 if (foe->
getLinkCont()[0]->getViaLane() !=
nullptr) {
451 return intersections.size() > 0;
459 const bool setRequest,
const double arrivalSpeedBraking,
const SUMOTime waitingTime,
double dist,
double latOffset) {
461#ifdef DEBUG_APPROACHING
465 std::cout <<
"'" << i->first->getID() <<
"'" << std::endl;
471 arrivalSpeedBraking, waitingTime, dist, approaching->
getSpeed(), latOffset));
478#ifdef DEBUG_APPROACHING
482 std::cout <<
"'" << i->first->getID() <<
"'" << std::endl;
500 if ((*i)->isBlockingAnyone()) {
511#ifdef DEBUG_APPROACHING
514 std::cout <<
"' Removing approaching vehicle '" << veh->
getID() <<
"'\nCurrently registered vehicles:" << std::endl;
516 std::cout <<
"'" << i->first->getID() <<
"'" << std::endl;
543 const double leaveSpeed,
const double vehicleLength)
const {
550 double impatience,
double decel,
SUMOTime waitingTime,
double posLat,
552#ifdef MSLink_DEBUG_OPENED
567 assert(
myLane != foeLink->getLane());
568 for (
const auto& it : foeLink->myApproachingVehicles) {
572 ((posLat < foe->getLateralPositionOnLane() + it.second.latOffset &&
myLane->
getIndex() > foeLink->myLane->getIndex())
575 && (arrivalTime > it.second.arrivalTime
579 if (
blockedByFoe(foe, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed,
false,
580 impatience, decel, waitingTime, ego)) {
581#ifdef MSLink_DEBUG_OPENED
583 std::cout <<
SIMTIME <<
" blocked by " << foe->
getID() <<
" arrival=" << arrivalTime <<
" foeArrival=" << it.second.arrivalTime <<
"\n";
586 if (collectFoes ==
nullptr) {
587#ifdef MSLink_DEBUG_OPENED
589 std::cout <<
" link=" <<
getViaLaneOrLane()->
getID() <<
" blocked by sublaneFoe=" << foe->
getID() <<
" foeLink=" << foeLink->getViaLaneOrLane()->getID() <<
" posLat=" << posLat <<
"\n";
594 collectFoes->push_back(it.first);
605 for (
const auto& it : foeLink->myApproachingVehicles) {
615 if (
blockedByFoe(foe, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed,
false,
616 impatience, decel, waitingTime, ego)) {
617#ifdef MSLink_DEBUG_OPENED
619 std::cout <<
SIMTIME <<
" blocked by sublane foe " << foe->
getID() <<
" arrival=" << arrivalTime <<
" foeArrival=" << it.second.arrivalTime <<
"\n";
622 if (collectFoes ==
nullptr) {
623#ifdef MSLink_DEBUG_OPENED
625 std::cout <<
" link=" <<
getViaLaneOrLane()->
getID() <<
" blocked by sublaneFoe2=" << foe->
getID() <<
" foeLink=" << foeLink->getViaLaneOrLane()->getID() <<
" posLat=" << posLat <<
"\n";
630 collectFoes->push_back(it.first);
641 return collectFoes ==
nullptr || collectFoes->size() == 0;
647#ifdef MSLink_DEBUG_OPENED
659 if (link->haveRed()) {
663#ifdef MSLink_DEBUG_OPENED
665 std::cout <<
" foeLink=" << link->getViaLaneOrLane()->getID() <<
" numApproaching=" << link->getApproaching().size() <<
"\n";
668 if (link->blockedAtTime(arrivalTime, leaveTime, arrivalSpeed, leaveSpeed,
myLane == link->getLane(),
669 impatience, decel, waitingTime, collectFoes, ego, lastWasContRed)) {
673 if (collectFoes !=
nullptr && collectFoes->size() > 0) {
682 bool sameTargetLane,
double impatience,
double decel,
SUMOTime waitingTime,
685#ifdef MSLink_DEBUG_OPENED
690 std::stringstream stream;
692 <<
" foeVeh=" << it.first->getID() <<
" (below ignore speed)"
695 std::cout << stream.str();
706 &&
blockedByFoe(it.first, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, sameTargetLane,
707 impatience, decel, waitingTime, ego)) {
708 if (collectFoes ==
nullptr) {
711 collectFoes->push_back(it.first);
721 SUMOTime arrivalTime,
SUMOTime leaveTime,
double arrivalSpeed,
double leaveSpeed,
722 bool sameTargetLane,
double impatience,
double decel,
SUMOTime waitingTime,
724#ifdef MSLink_DEBUG_OPENED
726 std::stringstream stream;
728 <<
" foeVeh=" << veh->
getID()
733 std::cout << stream.str();
740 assert(waitingTime > 0);
750 if (impatience > 0 && arrivalTime < avi.
arrivalTime) {
751#ifdef MSLink_DEBUG_OPENED
755 foeArrivalTime = (
SUMOTime)((1. - impatience) * (double)avi.
arrivalTime + impatience * (
double)fatb);
756#ifdef MSLink_DEBUG_OPENED
775#ifdef MSLink_DEBUG_OPENED
777 std::stringstream stream;
778 stream <<
" imp=" << impatience <<
" fAT2=" << foeArrivalTime <<
" fASb=" << foeArrivalSpeedBraking <<
" lA=" << lookAhead <<
" egoAT=" << arrivalTime <<
" egoLT=" << leaveTime <<
" egoLS=" << leaveSpeed <<
"\n";
779 std::cout << stream.str();
784 if (sameTargetLane && (arrivalTime - avi.
leavingTime < lookAhead
787#ifdef MSLink_DEBUG_OPENED
789 std::cout <<
" blocked (cannot follow)\n";
794 }
else if (foeArrivalTime > leaveTime + lookAhead) {
798#ifdef MSLink_DEBUG_OPENED
800 std::cout <<
" blocked (cannot lead)\n";
807#ifdef MSLink_DEBUG_OPENED
809 std::cout <<
" blocked (hard conflict)\n";
826 if (arrivalTime - arrivalTime %
DELTA_T == foeArrivalTime - foeArrivalTime %
DELTA_T) {
828 return foeArrivalTime;
832 const double dt =
STEPS2TIME(foeArrivalTime - arrivalTime);
833 const double d = dt * m;
834 const double a = dt * d / 2;
837 if (0.5 * v * v / m <= dist2) {
839 std::cout <<
" dist=" << dist <<
" dist2=" << dist2 <<
" at=" <<
STEPS2TIME(arrivalTime) <<
" m=" << m <<
" d=" << d <<
" a=" << a <<
" canBrakeToStop\n";
851 const double x = (sqrt(4 * (v - d) * (v - d) - 8 * m * a) * -0.5 - d + v) / m;
853#ifdef MSLink_DEBUG_OPENED
854 const double x2 = (sqrt(4 * (v - d) * (v - d) - 8 * m * a) * 0.5 - d + v) / m;
856 std::cout <<
SIMTIME <<
" dist=" << dist <<
" dist2=" << dist2 <<
" at=" <<
STEPS2TIME(arrivalTime) <<
" m=" << m <<
" d=" << d <<
" v=" << v <<
" a=" << a <<
" x=" << x <<
" x2=" << x2 <<
"\n";
859 fasb = v - (dt + x) * m;
867 if (link->blockedAtTime(arrivalTime, leaveTime, speed, speed,
myLane == link->getLane(), 0, decel, 0)) {
872 if (lane->getVehicleNumberWithPartials() > 0) {
880std::pair<const SUMOVehicle*, const MSLink*>
882 double closetDist = std::numeric_limits<double>::max();
884 const MSLink* foeLink =
nullptr;
886 for (
const auto& it : link->myApproachingVehicles) {
889 return std::make_pair(
nullptr, wrapAround);
890 }
else if (it.second.dist < closetDist) {
891 closetDist = it.second.dist;
892 if (it.second.willPass) {
899 return std::make_pair(closest, foeLink);
932 assert(pred2 !=
nullptr);
934 assert(predLink !=
nullptr);
958 assert(pred2 !=
nullptr);
960 assert(predLink !=
nullptr);
961 return predLink->
getState() == linkState;
975 std::vector<std::pair<SUMOTime, const SUMOVehicle*> > toSort;
977 toSort.push_back(std::make_pair(it.second.arrivalTime, it.first));
979 std::sort(toSort.begin(), toSort.end());
980 for (std::vector<std::pair<SUMOTime, const SUMOVehicle*> >::const_iterator it = toSort.begin(); it != toSort.end(); ++it) {
1003 while (lane !=
nullptr && lane->
isInternal()) {
1015 while (lane !=
nullptr && lane->
isInternal()) {
1030 double totalDist = 0.;
1031 bool foundCrossing =
false;
1032 while (via !=
nullptr) {
1038 foundCrossing =
true;
1045 if (foundCrossing) {
1056 for (foe_ix = 0; foe_ix != (int)
myFoeLanes.size(); ++foe_ix) {
1063#ifdef MSLink_DEBUG_CROSSING_POINTS
1070 if (dist == -10000.) {
1074#ifdef MSLink_DEBUG_CROSSING_POINTS
1076 <<
"' at distance " << dist <<
" (approach along '"
1123 const MSLink* link =
nullptr;
1124 while (lane !=
nullptr) {
1134 const MSLink* link =
this;
1160 std::cout <<
SIMTIME <<
" getLeaderInfo link=" <<
getViaLaneOrLane()->
getID() <<
" dist=" << dist <<
" isShadowLink=" << isShadowLink <<
"\n";
1166 std::cout <<
" ignore linkLeaders beyond red light\n";
1172 for (
int i = 0; i < (int)
myFoeLanes.size(); ++i) {
1179 const double crossingWidth = (sameTarget || sameSource) ? 0 : foeLane->
getWidth();
1185 std::cout <<
" distToCrossing=" << distToCrossing <<
" foeLane=" << foeLane->
getID() <<
" cWidth=" << crossingWidth
1189 <<
" contLane=" << contLane
1194 if (distToCrossing + crossingWidth < 0 && !sameTarget
1198 bool ignoreGreenCont =
false;
1199 bool foeIndirect =
false;
1204 if (entry !=
nullptr && entry->
haveGreen()
1205 && foeEntry !=
nullptr && foeEntry->
haveGreen()
1208 ignoreGreenCont =
true;
1213 std::cout <<
" ignore:noIntersection\n";
1228 const double leaderBackDist = foeDistToCrossing - leaderBack;
1229 const double l2 = ego !=
nullptr ? ego->
getLength() + 2 : 0;
1231 const bool pastTheCrossingPoint = leaderBackDist + foeCrossingWidth + sagitta < 0;
1234 const bool ignoreIndirectBicycleTurn = pastTheCrossingPoint && foeIsBicycleTurn;
1235 const bool cannotIgnore = ((contLane && !ignoreIndirectBicycleTurn) || sameTarget || sameSource) && ego !=
nullptr;
1236 const bool inTheWay = (((!pastTheCrossingPoint && distToCrossing > 0) || (sameTarget && distToCrossing > leaderBackDist - leader->
getLength()))
1244 std::cout <<
" candidate leader=" << leader->
getID()
1245 <<
" cannotIgnore=" << cannotIgnore
1246 <<
" fdtc=" << foeDistToCrossing
1247 <<
" lb=" << leaderBack
1248 <<
" lbd=" << leaderBackDist
1249 <<
" fcwidth=" << foeCrossingWidth
1251 <<
" sagitta=" << sagitta
1252 <<
" foePastCP=" << pastTheCrossingPoint
1253 <<
" inTheWay=" << inTheWay
1254 <<
" willPass=" << willPass
1256 <<
" ignoreGreenCont=" << ignoreGreenCont
1257 <<
" foeIndirect=" << foeIndirect
1258 <<
" foeBikeTurn=" << foeIsBicycleTurn
1259 <<
" isOpposite=" << isOpposite <<
"\n";
1261 if (leader == ego) {
1265 if (!inTheWay && ignoreGreenCont) {
1267 std::cout <<
" ignoreGreenCont\n";
1272 if (distToCrossing < -POSITION_EPS && !inTheWay
1273 && (ego ==
nullptr || !
MSGlobals::gComputeLC || distToCrossing < -ego->getVehicleType().getLength())) {
1275 std::cout <<
" ego entered conflict area\n";
1280 if ((!cannotIgnore || leader->
isStopped() || sameTarget)
1288 std::cout <<
" foe will not pass\n";
1297 && (!foeStrategicBlocked)) {
1304 const double latGap = (fabs(posLat - posLatLeader)
1309 <<
" sameSource=" << sameSource
1310 <<
" sameTarget=" << sameTarget
1311 <<
" foeLane=" << foeLane->
getID()
1312 <<
" leader=" << leader->
getID()
1315 <<
" egoLat=" << posLat
1316 <<
" leaderLat=" << posLatLeader
1317 <<
" leaderLatOffset=" << leader->
getLatOffset(foeLane)
1318 <<
" latGap=" << latGap
1319 <<
" maneuverDist=" << maneuverDist
1324 if (latGap > 0 && (latGap > maneuverDist || !sameTarget)
1332 if ((posLat > posLatLeader) == leaderFromRight) {
1335 std::cout <<
" ignored (same source) leaderFromRight=" << leaderFromRight <<
"\n";
1346 leaderFromRight = !leaderFromRight;
1348 if ((posLat > posLatLeader) == leaderFromRight
1355 std::cout <<
" ignored (different source) leaderFromRight=" << leaderFromRight <<
"\n";
1369 bool fromLeft =
true;
1370 if (ego ==
nullptr) {
1373 gap = leaderBackDist;
1377 distToCrossing += foeLane->
getWidth() / 2;
1378 if (gap + foeCrossingWidth < 0) {
1385 fromLeft = foeDistToCrossing > 0.5 * foeLane->
getLength();
1386 }
else if ((contLane && !sameSource && !ignoreIndirectBicycleTurn) || isOpposite) {
1387 gap = -std::numeric_limits<double>::max();
1390 std::cout <<
" distToCrossing=" << distToCrossing <<
" leader back=" << leaderBack <<
" backDist=" << leaderBackDist
1395 if (pastTheCrossingPoint && !sameTarget) {
1410 const bool stopAsap = leader->
isFrontOnLane(foeLane) ? cannotIgnore : (sameTarget || sameSource);
1412 std::cout <<
" leader=" << leader->
getID() <<
" contLane=" << contLane <<
" cannotIgnore=" << cannotIgnore <<
" stopAsap=" << stopAsap <<
" gap=" << gap <<
"\n";
1417 result.emplace_back(leader, gap, stopAsap ? -1 : distToCrossing, fromLeft, inTheWay);
1428 const double vehSideOffset = (foeDistToCrossing +
myLaneBefore->
getWidth() * 0.5 - vehWidth * 0.5
1437 result.emplace_back(
nullptr, -1, distToPeds);
1443 if (ego !=
nullptr) {
1451 const MSLane* foeLane = *it;
1455 if (leader == ego) {
1469 <<
" foeLane=" << foeLane->
getID()
1470 <<
" leader=" << leader->
getID()
1473 <<
" egoLat=" << posLat
1474 <<
" leaderLat=" << posLatLeader
1475 <<
" leaderLatOffset=" << leader->
getLatOffset(foeLane)
1477 <<
" foeIndex=" << foeLane->
getIndex()
1483 if ((posLat < posLatLeader && myInternalLaneBefore->
getIndex() > foeLane->
getIndex())
1486 std::cout <<
SIMTIME <<
" blocked by " << leader->
getID() <<
" (sublane split) foeLane=" << foeLane->
getID() <<
"\n";
1491 result.emplace_back(leader, gap, -1);
1507 double distToPeds = std::numeric_limits<double>::max();
1516#ifdef DEBUG_WALKINGAREA
1521 <<
" dist=" << dist <<
"\n";
1524 if (dist < ego->getVehicleType().getWidth() / 2 ||
isInFront(ego, egoPath, p)) {
1525 distToPeds =
MIN2(distToPeds, dist);
1526 if (collectBlockers !=
nullptr) {
1527 collectBlockers->push_back(p);
1531 if (distToPeds != std::numeric_limits<double>::max()) {
1533 result.emplace_back(
nullptr, -1, distToPeds);
1542#ifdef DEBUG_WALKINGAREA
1544 std::cout <<
" angleDiff=" <<
RAD2DEG(angleDiff) <<
"\n";
1547 if (angleDiff <
DEG2RAD(75)) {
1556 if (direction == -1) {
1558 }
else if (direction == 1) {
1583 if (before !=
nullptr && after !=
nullptr) {
1585 if (link->getLane() == after) {
1603 throw ProcessError(
"Zipper junctions with more than two conflicting lanes are not supported (at junction '"
1611 <<
" dist=" << dist <<
" ignoring foes (arrival in " <<
STEPS2TIME(arrivalTime - now) <<
")\n")
1617 <<
" egoAT=" << arrivalTime
1619 <<
" brakeGap=" << brakeGap
1620 <<
" vSafe=" << vSafe
1621 <<
" numFoes=" << collectFoes->size()
1625 for (
const auto& item : *collectFoes) {
1640 <<
" ignoring foe=" << foe->
getID()
1642 <<
" foeDist=" << avi.
dist
1643 <<
" foeDist2=" << foeDist
1644 <<
" foeSpeed=" << avi.
speed
1646 <<
" deltaDist=" << foeDist - dist
1669 const double uEnd =
MIN2(uMax, uAccel);
1670 const double uAvg = (avi.
speed + uEnd) / 2;
1671 const double tf0 = foeDist /
MAX2(NUMERICAL_EPS, uAvg);
1672 const double tf =
MAX2(1.0, ceil((tf0) /
TS) *
TS);
1677 const double vEnd =
MIN3(vMax, vAccel,
MAX2(uEnd, vDecel));
1678 const double vAvg = (ego->
getSpeed() + vEnd) / 2;
1679 const double te0 = dist /
MAX2(NUMERICAL_EPS, vAvg);
1680 const double te =
MAX2(1.0, ceil((te0) /
TS) *
TS);
1687 const double deltaGap = gap + tf * uAvg - safeGap - vAvg * tf;
1688 const double a = 2 * deltaGap / (tf * tf);
1694 const double w =
MIN2(1.0, te / 10);
1696 const double vZipper =
MAX3(vFollow, ego->
getSpeed() -
ACCEL2SPEED(maxDecel), w * vSafeGap + (1 - w) * vFollow);
1698 vSafe =
MIN2(vSafe, vZipper);
1701 <<
" foeDist=" << foeDist
1702 <<
" foeSpeed=" << avi.
speed
1706 <<
" uAccel=" << uAccel
1710 <<
" safeGap=" << safeGap
1714 <<
" dg=" << deltaGap
1715 <<
" aSafeGap=" << a
1717 <<
" vAccel=" << vAccel
1718 <<
" vDecel=" << vDecel
1720 <<
" vSafeGap=" << vSafeGap
1721 <<
" vFollow=" << vFollow
1723 <<
" maxDecel=" << maxDecel
1724 <<
" vZipper=" << vZipper
1725 <<
" vSafe=" << vSafe
1736 followDist > leaderDist &&
1754 for (
const MSLink* link : cand->getLinkCont()) {
1765 return fabs(posLat2 - posLat) < (width + width2) / 2;
1786 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
const double SUMO_const_haltingSpeed
the speed threshold at which vehicles are considered as halting
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].
double getMaxAccel() const
Get the vehicle type's maximum acceleration [m/s^2].
double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver's reaction time tau (i....
double getMaxDecel() const
Get the vehicle type's maximal comfortable deceleration [m/s^2].
virtual double followSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0, const CalcReason usage=CalcReason::CURRENT) const =0
Computes the vehicle's follow speed (no dawdling)
A road/street connecting two junctions.
const std::set< MSTransportable *, ComparatorNumericalIdLess > & 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.
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
const std::vector< IncomingLaneInfo > & getIncomingLanes() const
double getLength() const
Returns the lane's length.
const PositionVector & getShape() const
Returns this lane's shape.
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.
MSLane * getLogicalPredecessorLane() const
get the most likely precedecessor lane (sorted using by_connections_to_sorter). The result is cached ...
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
MSEdge & getEdge() const
Returns the lane's edge.
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.
const std::vector< MSLink * > & getLinkCont() const
returns the container with all links !!!
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...
const MSLane * getInternalLaneBefore() const
return myInternalLaneBefore (always 0 when compiled without internal lanes)
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
const ApproachInfos & getApproaching() const
return all approaching vehicles
void setApproaching(const SUMOVehicle *approaching, const SUMOTime arrivalTime, const double arrivalSpeed, const double leaveSpeed, const bool setRequest, const double arrivalSpeedBraking, const SUMOTime waitingTime, double dist, double latOffset)
Sets the information about an approaching vehicle.
const MSLane * myInternalLaneBefore
LinkState myState
The state of the link.
bool lastWasContState(LinkState linkState) const
whether this is a link past an internal junction where the entry to the junction currently has the gi...
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.
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 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
const MSLane * getLaneBefore() const
return the internalLaneBefore if it exists and the laneBefore otherwise
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
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.
MSLane * getViaLane() const
Returns the following inner lane.
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)
static SUMOTime computeFoeArrivalTimeBraking(SUMOTime arrivalTime, const SUMOVehicle *foe, SUMOTime foeArrivalTime, double impatience, double dist, double &fasb)
compute arrival time if foe vehicle is braking for ego
MSJunction * myJunction
the junction to which this link belongs
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, bool lastWasContRed=false) const
Returns the information whether this link is blocked Valid after the vehicles have set their requests...
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
MSLane * getViaLaneOrLane() const
return the via lane if it exists and the lane otherwise
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)
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.
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.
Position getPosition(const double) const
Return current position (x/y, cartesian)
const MSVehicleType & getVehicleType() const
Returns the object's "vehicle" type.
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.
const MSLane * getLane() const
Returns the lane the vehicle is on.
double getLateralPositionOnLane() const
Get the vehicle's lateral position on the lane.
double getSpeed() const
Returns the vehicle's current speed.
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
bool ignoreRed(const MSLink *link, bool canBrake) const
decide whether a red (or yellow light) may be ignore
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.
const std::string & getID() const
Returns the name of the vehicle type.
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 MSCFModel & getCarFollowModel() const
Returns the vehicle type's car following model definition (const version)
double getLength() const
Get vehicle's length [m].
const SUMOVTypeParameter & getParameter() const
const std::string & getID() const
Returns the id.
Static storage of an output device and its base (abstract) implementation.
OutputDevice & writeAttr(const SumoXMLAttr attr, const T &val)
writes a named attribute
OutputDevice & openTag(const std::string &xmlElement)
Opens an XML tag.
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 const MSVehicleType & getVehicleType() const =0
Returns the object's "vehicle" type.
virtual double getSpeed() const =0
Returns the object's current speed.
virtual const SUMOVehicleParameter & getParameter() const =0
Returns the vehicle's parameter (including departure definition)
virtual SumoRNG * getRNG() const =0
Returns the associated RNG for this object.
virtual bool isSelected() const =0
whether this object is selected in the GUI
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.