42 #define OPPOSITE_OVERTAKING_SAFE_TIMEGAP 0.0
43 #define OPPOSITE_OVERTAKING_SAFETYGAP_HEADWAY_FACTOR 0.0
44 #define OPPOSITE_OVERTAKING_SAFETY_FACTOR 1.2
46 #define OPPOSITE_OVERTAKING_MAX_LOOKAHEAD 150.0 // just a guess
47 #define OPPOSITE_OVERTAKING_MAX_LOOKAHEAD_EMERGENCY 1000.0 // just a guess
49 #define OPPOSITE_OVERTAKING_ONCOMING_LOOKAHEAD 200.0 // just a guess
64 #define DEBUG_COND (vehicle->isSelected())
76 firstBlocked(nullptr),
78 aheadNext(lane, nullptr, 0) {
84 lane->myTmpVehicles.insert(lane->myTmpVehicles.begin(), vehicle);
99 for (std::vector<MSLane*>::const_iterator lane = lanes->begin(); lane != lanes->end(); ++lane) {
101 myChanger.back().mayChangeRight = lane != lanes->begin();
102 myChanger.back().mayChangeLeft = (lane + 1) != lanes->end();
104 if ((*lane)->isInternal()) {
105 if (
myChanger.back().mayChangeRight && (*lane)->getLogicalPredecessorLane() == (*(lane - 1))->getLogicalPredecessorLane()) {
108 if (
myChanger.back().mayChangeLeft && (*lane)->getLogicalPredecessorLane() == (*(lane + 1))->getLogicalPredecessorLane()) {
129 const bool haveChanged =
change();
136 ce->lane->releaseVehicles();
148 ce->hoppedVeh =
nullptr;
149 ce->lastBlocked =
nullptr;
150 ce->firstBlocked =
nullptr;
152 ce->lane->getVehiclesSecure();
166 if (!vehHasChanged) {
189 ce->lane->swapAfterLaneChange(t);
190 ce->lane->releaseVehicles();
200 #ifdef DEBUG_CANDIDATE
201 std::cout <<
SIMTIME <<
" findCandidate() on edge " <<
myChanger.begin()->lane->getEdge().getID() << std::endl;
205 if (
veh(ce) ==
nullptr) {
208 #ifdef DEBUG_CANDIDATE
209 std::cout <<
" lane = " << ce->lane->getID() <<
"\n";
213 #ifdef DEBUG_CANDIDATE
219 assert(
veh(ce) != 0);
220 assert(
veh(max) != 0);
221 if (
veh(max)->getPositionOnLane() <
veh(ce)->getPositionOnLane()) {
222 #ifdef DEBUG_CANDIDATE
229 assert(
veh(max) != 0);
236 if (direction == 0) {
242 if (direction == -1) {
243 return myCandi->mayChangeRight && (
myCandi - 1)->lane->allowsVehicleClass(
veh(
myCandi)->getVehicleType().getVehicleClass());
244 }
else if (direction == 1) {
245 return myCandi->mayChangeLeft && (
myCandi + 1)->lane->allowsVehicleClass(
veh(
myCandi)->getVehicleType().getVehicleClass());
262 #ifdef DEBUG_ACTIONSTEPS
280 #ifdef DEBUG_ACTIONSTEPS
282 std::cout <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' skips regular change checks." << std::endl;
285 bool changed =
false;
309 for (
int i = 0; i < (int)
myChanger.size(); ++i) {
313 const std::vector<MSVehicle::LaneQ>& preb = vehicle->
getBestLanes();
324 (
myCandi - 1)->lastBlocked = vehicle;
325 if ((
myCandi - 1)->firstBlocked ==
nullptr) {
326 (
myCandi - 1)->firstBlocked = vehicle;
341 (
myCandi + 1)->lastBlocked = vehicle;
342 if ((
myCandi + 1)->firstBlocked ==
nullptr) {
343 (
myCandi + 1)->firstBlocked = vehicle;
387 <<
" veh=" << vehicle->
getID()
390 << ((newstate &
LCA_BLOCKED) != 0 ?
" (blocked)" :
"")
401 bool changed =
false;
403 const int dir = (state &
LCA_RIGHT) != 0 ? -1 : ((state &
LCA_LEFT) != 0 ? 1 : 0);
404 const bool execute = dir != 0 && ((state &
LCA_BLOCKED) == 0);
412 to->registerHop(vehicle);
434 to->registerHop(vehicle);
435 to->lane->requireCollisionCheck();
456 to->registerHop(vehicle);
460 from->registerHop(vehicle);
461 from->lane->requireCollisionCheck();
471 shadow->hoppedVeh = vehicle;
479 #ifdef DEBUG_CONTINUE_CHANGE
482 <<
" continueChange veh=" << vehicle->
getID()
484 <<
" dir=" << direction
485 <<
" pastMidpoint=" << pastMidpoint
497 std::pair<MSVehicle* const, double>
501 #ifdef DEBUG_SURROUNDING_VEHICLES
503 std::cout <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' looks for leader on lc-target lane '" << target->lane->getID() <<
"'." << std::endl;
509 #ifdef DEBUG_SURROUNDING_VEHICLES
511 if (neighLead != 0) {
512 std::cout <<
"Considering '" << neighLead->
getID() <<
"' at position " << neighLead->
getPositionOnLane() << std::endl;
523 if (target->hoppedVeh !=
nullptr) {
524 double hoppedPos = target->hoppedVeh->getPositionOnLane();
525 #ifdef DEBUG_SURROUNDING_VEHICLES
527 std::cout <<
"Considering hopped vehicle '" << target->hoppedVeh->getID() <<
"' at position " << hoppedPos << std::endl;
531 neighLead = target->hoppedVeh;
535 if (neighLead ==
nullptr) {
536 #ifdef DEBUG_SURROUNDING_VEHICLES
538 std::cout <<
"Looking for leader on consecutive lanes." << std::endl;
543 MSLane* targetLane = target->lane;
545 double leaderBack = targetLane->
getLength();
548 if (plBack < leaderBack &&
554 if (neighLead !=
nullptr) {
555 #ifdef DEBUG_SURROUNDING_VEHICLES
557 std::cout <<
" found leader=" << neighLead->
getID() <<
" (partial)\n";
566 if (seen > dist && !
myCandi->lane->isInternal()) {
567 #ifdef DEBUG_SURROUNDING_VEHICLES
569 std::cout <<
" found no leader within dist=" << dist <<
"\n";
572 return std::pair<MSVehicle* const, double>(static_cast<MSVehicle*>(
nullptr), -1);
576 std::pair<MSVehicle* const, double> result = target->lane->getLeaderOnConsecutive(dist, seen, speed, *
veh(
myCandi), bestLaneConts);
577 #ifdef DEBUG_SURROUNDING_VEHICLES
585 #ifdef DEBUG_SURROUNDING_VEHICLES
587 std::cout <<
" found leader=" << neighLead->
getID() <<
"\n";
595 std::pair<MSVehicle* const, double>
599 #ifdef DEBUG_SURROUNDING_VEHICLES
602 std::cout <<
SIMTIME <<
" veh '" << vehicle->
getID() <<
"' looks for follower on lc-target lane '" << target->lane->getID() <<
"'." << std::endl;
609 #ifdef DEBUG_SURROUNDING_VEHICLES
611 if (neighFollow != 0) {
612 std::cout <<
"veh(target) returns '" << neighFollow->
getID() <<
"' at position " << neighFollow->
getPositionOnLane() << std::endl;
614 std::cout <<
"veh(target) returns none." << std::endl;
620 #ifdef DEBUG_SURROUNDING_VEHICLES
622 if (
getCloserFollower(candiPos, neighFollow, target->hoppedVeh) != neighFollow) {
623 std::cout <<
"Hopped vehicle '" << target->hoppedVeh->getID() <<
"' at position " << target->hoppedVeh->getPositionOnLane() <<
" is closer." << std::endl;
632 #ifdef DEBUG_SURROUNDING_VEHICLES
635 if (partialBehind != 0 && partialBehind != neighFollow) {
636 std::cout <<
"'Partial behind'-vehicle '" << target->lane->getPartialBehind(candi)->
getID() <<
"' at position " << partialBehind->
getPositionOnLane() <<
" is closer." << std::endl;
641 neighFollow =
getCloserFollower(candiPos, neighFollow, target->lane->getPartialBehind(candi));
643 if (neighFollow ==
nullptr) {
645 #ifdef DEBUG_SURROUNDING_VEHICLES
647 if (consecutiveFollower.first == 0) {
648 std::cout <<
"no follower found." << std::endl;
650 std::cout <<
"found follower '" << consecutiveFollower.first->
getID() <<
"' on consecutive lanes." << std::endl;
654 return std::make_pair(const_cast<MSVehicle*>(consecutiveFollower.first), consecutiveFollower.second);
656 #ifdef DEBUG_SURROUNDING_VEHICLES
658 std::cout <<
"found follower '" << neighFollow->
getID() <<
"'." << std::endl;
662 return std::pair<MSVehicle* const, double>(neighFollow,
686 const std::pair<MSVehicle* const, double>& leader,
687 const std::vector<MSVehicle::LaneQ>& preb)
const {
691 if (neighLead.first !=
nullptr && neighLead.first == neighFollow.first) {
694 neighFollow.first = 0;
697 return checkChange(laneOffset, target->lane, leader, neighLead, neighFollow, preb);
704 const std::pair<MSVehicle* const, double>& leader,
705 const std::pair<MSVehicle* const, double>& neighLead,
706 const std::pair<MSVehicle* const, double>& neighFollow,
707 const std::vector<MSVehicle::LaneQ>& preb)
const {
711 #ifdef DEBUG_CHECK_CHANGE
714 <<
"\n" <<
SIMTIME <<
" checkChange() for vehicle '" << vehicle->
getID() <<
"'"
723 if (neighFollow.first !=
nullptr && neighFollow.second < 0) {
727 #ifdef DEBUG_CHECK_CHANGE
730 <<
" overlapping with follower..."
736 if (neighLead.first !=
nullptr && neighLead.second < 0) {
739 #ifdef DEBUG_CHECK_CHANGE
742 <<
" overlapping with leader..."
754 if ((blocked & blockedByFollower) == 0 && neighFollow.first !=
nullptr) {
762 const double vNextFollower = neighFollow.first->getSpeed() +
MAX2(0., tauRemainder * neighFollow.first->getAcceleration());
765 secureBackGap = neighFollow.first->getCarFollowModel().getSecureGap(vNextFollower,
767 #ifdef DEBUG_CHECK_CHANGE
770 <<
" backGap=" << neighFollow.second
771 <<
" vNextFollower=" << vNextFollower
772 <<
" vNextEgo=" << vNextLeader
773 <<
" secureGap=" << secureBackGap
780 blocked |= blockedByFollower;
785 if ((blocked & blockedByLeader) == 0 && neighLead.first !=
nullptr) {
794 const double vNextLeader = neighLead.first->getSpeed() +
MIN2(0., tauRemainder * neighLead.first->getAcceleration());
797 vNextLeader, neighLead.first->getCarFollowModel().getMaxDecel());
798 #ifdef DEBUG_CHECK_CHANGE
801 <<
" frontGap=" << neighFollow.second
802 <<
" vNextEgo=" << vNextFollower
803 <<
" vNextLeader=" << vNextLeader
804 <<
" secureGap=" << secureFrontGap
811 blocked |= blockedByLeader;
818 if (leader.first != 0) {
822 #ifdef DEBUG_CHECK_CHANGE
824 std::cout <<
SIMTIME <<
" pedestrian on road " + leader.first->getID() <<
" gap=" << gap <<
" brakeGap=" << brakeGap <<
"\n";
827 if (brakeGap > gap) {
828 blocked |= blockedByLeader;
829 #ifdef DEBUG_CHECK_CHANGE
831 std::cout <<
SIMTIME <<
" blocked by pedestrian " + leader.first->getID() <<
"\n";
838 if (leader.first !=
nullptr) {
844 laneOffset, msg, blocked, leader, neighLead, neighFollow, *targetLane, preb, &(
myCandi->lastBlocked), &(
myCandi->firstBlocked));
853 const double speed = vehicle->
getSpeed();
855 if (seen < dist || myCandi->lane->isInternal()) {
856 std::pair<MSVehicle* const, double> neighLead2 = targetLane->
getCriticalLeader(dist, seen, speed, *vehicle);
857 if (neighLead2.first !=
nullptr && neighLead2.first != neighLead.first) {
859 neighLead2.first->getSpeed(), neighLead2.first->getCarFollowModel().getMaxDecel());
861 #ifdef DEBUG_SURROUNDING_VEHICLES
863 std::cout <<
SIMTIME <<
" found critical leader=" << neighLead2.first->getID()
864 <<
" gap=" << neighLead2.second <<
" secGap=" << secureGap <<
" secGap2=" << secureGap2 <<
"\n";
867 if (neighLead2.second < secureGap2) {
868 state |= blockedByLeader;
876 state |= blockedByLeader;
888 if (estimatedLCDuration == -1) {
890 #ifdef DEBUG_CHECK_CHANGE
892 std::cout <<
SIMTIME <<
" checkChange() too slow to guarantee completion of continuous lane change."
893 <<
"\nestimatedLCDuration=" << estimatedLCDuration
894 <<
"\ndistToNeighLane=" << distToNeighLane
902 const double avgSpeed = 0.5 * (
906 const double space2change = avgSpeed * estimatedLCDuration;
909 #ifdef DEBUG_CHECK_CHANGE
911 std::cout <<
SIMTIME <<
" checkChange() checking continuous lane change..."
912 <<
"\ndistToNeighLane=" << distToNeighLane
913 <<
" estimatedLCDuration=" << estimatedLCDuration
914 <<
" space2change=" << space2change
915 <<
" avgSpeed=" << avgSpeed
924 MSLinkCont::const_iterator link =
MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
925 while (!nextLane->
isLinkEnd(link) && seen <= space2change) {
929 || (nextLane->
getEdge().
isInternal() && (*link)->getViaLaneOrLane()->getEdge().isInternal())
934 if ((*link)->getViaLane() ==
nullptr) {
937 nextLane = (*link)->getViaLaneOrLane();
942 #ifdef DEBUG_CHECK_CHANGE
944 std::cout <<
" available distance=" << seen << std::endl;
947 if (nextLane->
isLinkEnd(link) && seen < space2change) {
948 #ifdef DEBUG_CHECK_CHANGE
950 std::cout <<
SIMTIME <<
" checkChange insufficientSpace: seen=" << seen <<
" space2change=" << space2change <<
"\n";
959 const double speed = vehicle->
getSpeed();
964 MSLinkCont::const_iterator link =
MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
965 while (!nextLane->
isLinkEnd(link) && seen <= space2change && seen <= dist) {
966 nextLane = (*link)->getViaLaneOrLane();
968 if (targetLane ==
nullptr) {
972 std::pair<MSVehicle* const, double> neighLead2 = targetLane->
getLeader(vehicle, -seen, std::vector<MSLane*>());
973 if (neighLead2.first !=
nullptr && neighLead2.first != neighLead.first
975 vehicle->
getSpeed(), neighLead2.first->getSpeed(), neighLead2.first->getCarFollowModel().getMaxDecel()))) {
976 state |= blockedByLeader;
980 if ((*link)->getViaLane() ==
nullptr) {
990 const int oldstate = state;
993 #ifdef DEBUG_CHECK_CHANGE
996 <<
" veh=" << vehicle->
getID()
1011 if (laneOffset != 0) {
1033 bool oppositeChangeByTraci =
false;
1036 oppositeChangeByTraci =
true;
1039 if (!isOpposite && leader.first == 0 && !oppositeChangeByTraci) {
1045 if (!isOpposite && !oppositeChangeByTraci
1047 && leader.first != 0) {
1052 #ifdef DEBUG_CHANGE_OPPOSITE
1054 std::cout <<
" not overtaking leader " << leader.first->getID() <<
" that has blinker set\n";
1058 }
else if (leader.second < 0) {
1060 #ifdef DEBUG_CHANGE_OPPOSITE
1062 std::cout <<
" not overtaking leader " << leader.first->getID() <<
" with gap " << leader.second <<
"\n";
1076 int direction = isOpposite ? -1 : 1;
1077 std::pair<MSVehicle*, double> neighLead((
MSVehicle*)
nullptr, -1);
1080 double timeToOvertake;
1081 double spaceToOvertake;
1090 std::pair<MSVehicle*, double> overtaken;
1092 if (!isOpposite && !oppositeChangeByTraci) {
1094 if (overtaken.first == 0) {
1097 #ifdef DEBUG_CHANGE_OPPOSITE
1099 std::cout <<
" compute time/space to overtake for columnLeader=" << overtaken.first->getID() <<
" egoGap=" << overtaken.second <<
"\n";
1105 #ifdef DEBUG_CHANGE_OPPOSITE
1107 std::cout <<
" cannot changeOpposite due to upcoming stop (dist=" << vehicle->
nextStopDist() <<
" spaceToOvertake=" << spaceToOvertake <<
")\n";
1114 #ifdef DEBUG_CHANGE_OPPOSITE
1117 <<
" veh=" << vehicle->
getID()
1118 <<
" changeOpposite opposite=" << opposite->
getID()
1120 <<
" timeToOvertake=" << timeToOvertake
1121 <<
" spaceToOvertake=" << spaceToOvertake
1126 if (neighLead.first != 0) {
1127 const MSVehicle* oncoming = neighLead.first;
1133 const double surplusGap = neighLead.second - spaceToOvertake - timeToOvertake * oncomingSpeed - safetyGap;
1134 #ifdef DEBUG_CHANGE_OPPOSITE
1137 <<
" oncoming=" << oncoming->
getID()
1138 <<
" oncomingGap=" << neighLead.second
1139 <<
" leaderGap=" << leader.second
1140 <<
" safetyGap=" << safetyGap
1141 <<
" surplusGap=" << surplusGap
1145 if (surplusGap < 0) {
1147 #ifdef DEBUG_CHANGE_OPPOSITE
1149 std::cout <<
" cannot changeOpposite due to dangerous oncoming (surplusGap=" << surplusGap <<
")\n";
1153 #ifdef DEBUG_CHANGE_OPPOSITE
1156 std::cout <<
SIMTIME <<
" ego=" << vehicle->
getID() <<
" does not changeOpposite due to dangerous oncoming " << oncoming->
getID() <<
" (but the leader is also opposite)\n";
1164 }
else if (!oppositeChangeByTraci) {
1165 timeToOvertake = -1;
1167 spaceToOvertake = std::numeric_limits<double>::max();
1170 double gap = leader.second;
1171 while (leader.first !=
nullptr && leader.first->getLaneChangeModel().isOpposite() && dist > 0) {
1173 #ifdef DEBUG_CHANGE_OPPOSITE
1175 std::cout <<
SIMTIME <<
" ego=" << vehicle->
getID() <<
" opposite leader=" << leader.first->getID() <<
" gap=" << gap <<
" is driving against the flow\n";
1178 const double gapToLeaderFront = leader.second + leader.first->getVehicleType().getLengthWithGap();
1179 if (gapToLeaderFront < 0) {
1182 dist -= gapToLeaderFront;
1184 if (leader.first != 0) {
1185 gap += gapToLeaderFront;
1188 leader.second = gap;
1199 if (usableDist < spaceToOvertake) {
1202 assert(bestLaneConts.size() >= 1);
1203 std::vector<MSLane*>::const_iterator it = bestLaneConts.begin() + 1;
1204 while (usableDist < spaceToOvertake && it != bestLaneConts.end()) {
1205 #ifdef DEBUG_CHANGE_OPPOSITE
1207 std::cout <<
" usableDist=" << usableDist <<
" opposite=" <<
Named::getIDSecure((*it)->getOpposite()) <<
"\n";
1210 if ((*it)->getOpposite() ==
nullptr || !(*it)->getOpposite()->allowsVehicleClass(vehicle->
getVClass())) {
1215 if (*(it - 1) !=
nullptr) {
1224 #ifdef DEBUG_CHANGE_OPPOSITE
1226 std::cout <<
" stop lookahead at link=" << (link == 0 ?
"NULL" : link->
getViaLaneOrLane()->
getID()) <<
" state=" << (link == 0 ?
"?" :
toString(link->getState())) <<
" ignoreRed=" << vehicle->
ignoreRed(link,
true) <<
"\n";
1232 usableDist += (*it)->getLength();
1236 if (!isOpposite && usableDist < spaceToOvertake) {
1237 #ifdef DEBUG_CHANGE_OPPOSITE
1239 std::cout <<
" cannot changeOpposite due to insufficient space (seen=" << usableDist <<
" spaceToOvertake=" << spaceToOvertake <<
")\n";
1244 #ifdef DEBUG_CHANGE_OPPOSITE
1246 std::cout <<
" usableDist=" << usableDist <<
" spaceToOvertake=" << spaceToOvertake <<
" timeToOvertake=" << timeToOvertake <<
"\n";
1251 std::vector<MSVehicle::LaneQ> preb = vehicle->
getBestLanes();
1266 if (leader.first != 0) {
1267 if (!leader.first->getLaneChangeModel().isOpposite()) {
1273 laneQ.
length =
MIN2(laneQ.
length, leader.second / 2 + forwardPos - safetyGap);
1274 #ifdef DEBUG_CHANGE_OPPOSITE
1276 std::cout <<
SIMTIME <<
" found oncoming leader=" << oncoming->
getID() <<
" gap=" << leader.second <<
"\n";
1280 #ifdef DEBUG_CHANGE_OPPOSITE
1282 std::cout <<
SIMTIME <<
" opposite leader=" << leader.first->getID() <<
" gap=" << leader.second <<
" is driving against the flow\n";
1286 if (neighLead.first != 0) {
1288 if (overtaken.first == 0) {
1289 #ifdef DEBUG_CHANGE_OPPOSITE
1291 std::cout <<
SIMTIME <<
" ego=" << vehicle->
getID() <<
" did not find columnleader to overtake\n";
1295 const double remainingDist = laneQ.
length - forwardPos;
1297 #ifdef DEBUG_CHANGE_OPPOSITE
1299 std::cout <<
SIMTIME <<
" ego=" << vehicle->
getID() <<
" is overtaking " << overtaken.first->getID()
1300 <<
" remainingDist=" << remainingDist <<
" spaceToOvertake=" << spaceToOvertake <<
" timeToOvertake=" << timeToOvertake <<
"\n";
1303 if (remainingDist > spaceToOvertake) {
1312 #ifdef DEBUG_CHANGE_OPPOSITE
1314 std::cout <<
SIMTIME <<
" veh=" << vehicle->
getID() <<
" remaining dist=" << laneQ.
length - forwardPos <<
" forwardPos=" << forwardPos <<
" laneQ.length=" << laneQ.
length <<
"\n";
1319 int state =
checkChange(direction, opposite, leader, neighLead, neighFollow, preb);
1322 bool changingAllowed = (state &
LCA_BLOCKED) == 0;
1328 #ifdef DEBUG_CHANGE_OPPOSITE
1330 std::cout <<
SIMTIME <<
" changing to opposite veh=" << vehicle->
getID() <<
" dir=" << direction <<
" opposite=" <<
Named::getIDSecure(opposite) <<
" state=" << state <<
"\n";
1338 #ifdef DEBUG_CHANGE_OPPOSITE
1340 std::cout <<
SIMTIME <<
" not changing to opposite veh=" << vehicle->
getID() <<
" dir=" << direction
1358 const double v = vehicle->
getSpeed();
1362 const double g =
MAX2(0.0, (
1371 const double sign = -1;
1375 double t = (u - v - sqrt(4 * (u - v) * (u - v) + 8 * a * g) * sign * 0.5) / a;
1376 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1378 std::cout <<
" computeOvertakingTime v=" << v <<
" vMax=" << vMax <<
" u=" << u <<
" a=" << a <<
" d=" << d <<
" gap=" << gap <<
" g=" << g <<
" t=" << t
1379 <<
" distEgo=" << v* t + t* t* a * 0.5 <<
" distLead=" << g + u* t
1388 t = ceil(t /
TS) *
TS;
1391 const double timeToMaxSpeed = (vMax - v) / a;
1393 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1395 std::cout <<
" t=" << t <<
" tvMax=" << timeToMaxSpeed <<
"\n";
1398 if (t <= timeToMaxSpeed) {
1400 spaceToOvertake = v * t + t * t * a * 0.5;
1401 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1403 std::cout <<
" sto=" << spaceToOvertake <<
"\n";
1408 const double s = v * timeToMaxSpeed + timeToMaxSpeed * timeToMaxSpeed * a * 0.5;
1409 const double m = timeToMaxSpeed;
1412 t = (g - s + m * vMax) / (vMax - u);
1415 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1417 std::cout <<
" t2=" << t <<
"\n";
1420 timeToOvertake = std::numeric_limits<double>::max();
1421 spaceToOvertake = std::numeric_limits<double>::max();
1426 t = ceil(t /
TS) *
TS;
1429 spaceToOvertake = s + (t - m) * vMax;
1430 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1432 std::cout <<
" t2=" << t <<
" s=" << s <<
" sto=" << spaceToOvertake <<
" m=" << m <<
"\n";
1438 timeToOvertake *= safetyFactor;
1439 spaceToOvertake *= safetyFactor;
1440 #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
1442 if (safetyFactor != 1) {
1443 std::cout <<
" applying safetyFactor=" << safetyFactor
1444 <<
" tto=" << timeToOvertake <<
" sto=" << spaceToOvertake <<
"\n";
1453 std::pair<MSVehicle*, double>
1455 assert(leader.first != 0);
1460 std::pair<MSVehicle*, double> columnLeader = leader;
1461 double egoGap = leader.second;
1462 bool foundSpaceAhead =
false;
1463 double seen = leader.second + leader.first->getVehicleType().getLengthWithGap();
1465 if (maxLookAhead == std::numeric_limits<double>::max()) {
1470 #ifdef DEBUG_CHANGE_OPPOSITE
1472 std::cout <<
" getColumnleader vehicle=" << vehicle->
getID() <<
" leader=" << leader.first->getID() <<
" gap=" << leader.second <<
" maxLookAhead=" << maxLookAhead <<
"\n";
1476 while (!foundSpaceAhead) {
1477 const double requiredSpaceAfterLeader = (columnLeader.first->getCarFollowModel().getSecureGap(
1479 + columnLeader.first->getVehicleType().getMinGap()
1484 const bool checkTmpVehicles = (&columnLeader.first->getLane()->getEdge() == &source->
getEdge());
1485 std::pair<MSVehicle* const, double> leadLead = columnLeader.first->getLane()->getLeader(
1486 columnLeader.first, columnLeader.first->getPositionOnLane(), conts, requiredSpaceAfterLeader + mergeBrakeGap,
1489 #ifdef DEBUG_CHANGE_OPPOSITE
1491 std::cout <<
" leadLead=" <<
Named::getIDSecure(leadLead.first) <<
" gap=" << leadLead.second <<
"\n";
1494 if (leadLead.first ==
nullptr) {
1495 double availableSpace = columnLeader.first->getLane()->getLength() - columnLeader.first->getPositionOnLane();
1496 const double requiredSpace = safetyFactor * (requiredSpaceAfterLeader
1498 #ifdef DEBUG_CHANGE_OPPOSITE
1500 std::cout <<
" no direct leader found after columnLeader " << columnLeader.first->getID()
1501 <<
" availableSpace=" << availableSpace
1502 <<
" req1=" << requiredSpaceAfterLeader
1503 <<
" req2=" << requiredSpace / safetyFactor
1504 <<
" req3=" << requiredSpace
1508 if (availableSpace > requiredSpace) {
1509 foundSpaceAhead =
true;
1514 #ifdef DEBUG_CHANGE_OPPOSITE
1516 std::cout <<
" look for another leader on lane " <<
Named::getIDSecure(next) <<
"\n";
1519 while (next !=
nullptr && seen < maxLookAhead) {
1522 if (cand ==
nullptr) {
1524 if (availableSpace > requiredSpace) {
1525 foundSpaceAhead =
true;
1530 if (availableSpace > requiredSpace) {
1531 foundSpaceAhead =
true;
1538 if (!foundSpaceAhead) {
1539 return std::make_pair(
nullptr, -1);
1543 const double requiredSpace = safetyFactor * (requiredSpaceAfterLeader
1544 + vehicle->
getCarFollowModel().
getSecureGap(overtakingSpeed, leadLead.first->getSpeed(), leadLead.first->getCarFollowModel().getMaxDecel()));
1545 #ifdef DEBUG_CHANGE_OPPOSITE
1547 std::cout <<
" leader's leader " << leadLead.first->getID() <<
" space=" << leadLead.second
1548 <<
" req1=" << requiredSpaceAfterLeader
1549 <<
" req2=" << requiredSpace / safetyFactor
1550 <<
" req3=" << requiredSpace
1554 if (leadLead.second > requiredSpace) {
1555 foundSpaceAhead =
true;
1557 #ifdef DEBUG_CHANGE_OPPOSITE
1559 std::cout <<
" not enough space after columnLeader=" << columnLeader.first->getID() <<
" required=" << requiredSpace <<
"\n";
1562 seen +=
MAX2(0., leadLead.second) + leadLead.first->getVehicleType().getLengthWithGap();
1563 if (seen > maxLookAhead) {
1564 #ifdef DEBUG_CHANGE_OPPOSITE
1566 std::cout <<
" cannot changeOpposite due to insufficient free space after columnLeader (seen=" << seen <<
" columnLeader=" << columnLeader.first->getID() <<
")\n";
1569 return std::make_pair(
nullptr, -1);
1572 egoGap += columnLeader.first->getVehicleType().getLengthWithGap() + leadLead.second;
1573 columnLeader = leadLead;
1574 #ifdef DEBUG_CHANGE_OPPOSITE
1576 std::cout <<
" new columnLeader=" << columnLeader.first->getID() <<
"\n";
1582 columnLeader.second = egoGap;
1583 return columnLeader;
1589 for (
auto it = conts.begin(); it != conts.end(); ++it) {
1591 if (it + 1 != conts.end()) {