37 #define MAGIC_OFFSET 1.
38 #define LOOK_FORWARD (double)10.
40 #define JAM_FACTOR (double)1.
43 #define LCA_RIGHT_IMPATIENCE (double)-1.
44 #define CUT_IN_LEFT_SPEED_THRESHOLD (double)27.
45 #define MAX_ONRAMP_LENGTH (double)200.
47 #define LOOK_AHEAD_MIN_SPEED 0.0
48 #define LOOK_AHEAD_SPEED_MEMORY 0.9
50 #define HELP_DECEL_FACTOR (double)1.0
52 #define HELP_OVERTAKE (double)(10.0 / 3.6)
53 #define MIN_FALLBEHIND (double)(7.0 / 3.6)
55 #define KEEP_RIGHT_HEADWAY (double)2.0
57 #define URGENCY (double)2.0
59 #define ROUNDABOUT_DIST_BONUS (double)100.0
61 #define KEEP_RIGHT_TIME (double)5.0 // the number of seconds after which a vehicle should move to the right lane
62 #define KEEP_RIGHT_ACCEPTANCE (double)7.0 // calibration factor for determining the desire to keep right
64 #define RELGAIN_NORMALIZATION_MIN_SPEED (double)10.0
66 #define TURN_LANE_DIST (double)200.0 // the distance at which a lane leading elsewhere is considered to be a turn-lane that must be avoided
67 #define GAIN_PERCEPTION_THRESHOLD 0.05 // the minimum relative speed gain which affects the behavior
69 #define SPEED_GAIN_MIN_SECONDS 20.0
71 #define ARRIVALPOS_LAT_THRESHOLD 100.0
74 #define LATGAP_SPEED_THRESHOLD (50 / 3.6)
77 #define LATGAP_SPEED_THRESHOLD2 (50 / 3.6)
80 #define SPEEDGAIN_DECAY_FACTOR 0.5
82 #define SPEEDGAIN_MEMORY_FACTOR 0.5
108 #define DEBUG_COND (myVehicle.isSelected())
120 mySpeedGainProbabilityRight(0),
121 mySpeedGainProbabilityLeft(0),
122 myKeepRightProbability(0),
123 myLeadingBlockerLength(0),
127 myCanChangeFully(true),
128 mySafeLatDistRight(0),
129 mySafeLatDistLeft(0),
142 myMinImpatience(myImpatience),
181 const std::vector<MSVehicle::LaneQ>& preb,
184 double& latDist,
double& maneuverDist,
int& blocked) {
187 const std::string changeType = laneOffset == -1 ?
"right" : (laneOffset == 1 ?
"left" :
"current");
189 #ifdef DEBUG_MANEUVER
198 <<
" considerChangeTo=" << changeType
205 leaders, followers, blockers,
206 neighLeaders, neighFollowers, neighBlockers,
208 lastBlocked, firstBlocked, latDist, maneuverDist, blocked);
210 result =
keepLatGap(result, leaders, followers, blockers,
211 neighLeaders, neighFollowers, neighBlockers,
212 neighLane, laneOffset, latDist, maneuverDist, blocked);
214 result |=
getLCA(result, latDist);
216 #if defined(DEBUG_MANEUVER) || defined(DEBUG_STATE)
217 double latDistTmp = latDist;
220 #if defined(DEBUG_MANEUVER) || defined(DEBUG_STATE)
222 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" maneuverDist=" << maneuverDist <<
" latDist=" << latDistTmp <<
" mySpeedPrev=" <<
mySpeedLat <<
" speedLat=" <<
DIST2SPEED(latDist) <<
" latDist2=" << latDist <<
"\n";
229 <<
" wantsChangeTo=" << changeType
230 <<
" latDist=" << latDist
231 <<
" maneuverDist=" << maneuverDist
239 <<
" wantsNoChangeTo=" << changeType
297 const double newSpeed =
_patchSpeed(
MAX2(min, 0.0), wanted, max, cfModel);
298 #ifdef DEBUG_PATCHSPEED
300 const std::string patched = (wanted != newSpeed ?
" patched=" +
toString(newSpeed) :
"");
307 <<
" wanted=" << wanted
330 #ifdef DEBUG_PATCHSPEED
340 #ifdef DEBUG_PATCHSPEED
342 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" slowing down for leading blocker, safe=" << safe << (safe +
NUMERICAL_EPS < min ?
" (not enough)" :
"") <<
"\n";
345 return MAX2(min, safe);
349 double nVSafe = wanted;
353 if (v >= min && v <= max) {
354 nVSafe =
MIN2(v, nVSafe);
356 #ifdef DEBUG_PATCHSPEED
358 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" got accel=" << (*i) <<
" nVSafe=" << nVSafe <<
"\n";
362 #ifdef DEBUG_PATCHSPEED
365 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" ignoring low nVSafe=" << v <<
" min=" << min <<
"\n";
369 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" ignoring high nVSafe=" << v <<
" max=" << max <<
"\n";
377 #ifdef DEBUG_PATCHSPEED
390 #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
395 return (max + wanted) / (double) 2.0;
399 #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
404 return (min + wanted) / (double) 2.0;
407 #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
412 return (max + wanted) / (double) 2.0;
453 #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
458 return (max + wanted) / (double) 2.0;
462 #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
482 if (pinfo->first >= 0) {
491 <<
" informedBy=" << sender->
getID()
492 <<
" info=" << pinfo->second
493 <<
" vSafe=" << pinfo->first
506 assert(cld.first != 0);
515 double remainingSeconds) {
521 plannedSpeed =
MIN2(plannedSpeed, v);
526 std::cout <<
" informLeader speed=" <<
myVehicle.
getSpeed() <<
" planned=" << plannedSpeed <<
"\n";
533 if (
gDebugFlag2) std::cout <<
" blocked by leader nv=" << nv->
getID() <<
" nvSpeed=" << nv->
getSpeed() <<
" needGap="
537 const double dv = plannedSpeed - nv->
getSpeed();
538 const double overtakeDist = (neighLead.second
550 || dv * remainingSeconds < overtakeDist) {
565 <<
" cannot overtake leader nv=" << nv->
getID()
567 <<
" remainingSeconds=" << remainingSeconds
568 <<
" targetSpeed=" << targetSpeed
569 <<
" nextSpeed=" << nextSpeed
580 <<
" cannot overtake fast leader nv=" << nv->
getID()
582 <<
" remainingSeconds=" << remainingSeconds
583 <<
" targetSpeed=" << targetSpeed
594 <<
" wants to overtake leader nv=" << nv->
getID()
596 <<
" remainingSeconds=" << remainingSeconds
597 <<
" currentGap=" << neighLead.second
599 <<
" overtakeDist=" << overtakeDist
609 }
else if (neighLead.first != 0) {
612 double dv, nextNVSpeed;
632 std::cout <<
" not blocked by leader nv=" << nv->
getID()
634 <<
" gap=" << neighLead.second
635 <<
" nextGap=" << neighLead.second - dv
637 <<
" targetSpeed=" << targetSpeed
641 return MIN2(targetSpeed, plannedSpeed);
653 double remainingSeconds,
654 double plannedSpeed) {
658 if (
gDebugFlag2) std::cout <<
" blocked by follower nv=" << nv->
getID() <<
" nvSpeed=" << nv->
getSpeed() <<
" needGap="
665 if ((neededGap - neighFollow.second) / remainingSeconds < (plannedSpeed - nv->
getSpeed())) {
668 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" without any help neededGap=" << neededGap <<
"\n";
687 const double neighNewSpeed1s =
MAX2(0., nv->
getSpeed() - helpDecel);
688 const double dv = plannedSpeed - neighNewSpeed1s;
690 const double decelGap = neighFollow.second + dv;
696 <<
" egoNV=" << plannedSpeed
697 <<
" nvNewSpeed=" << neighNewSpeed
698 <<
" nvNewSpeed1s=" << neighNewSpeed1s
699 <<
" deltaGap=" << dv
700 <<
" decelGap=" << decelGap
701 <<
" secGap=" << secureGap
705 if (decelGap > 0 && decelGap >= secureGap) {
720 std::cout <<
" wants to cut in before nv=" << nv->
getID()
721 <<
" vsafe1=" << vsafe1
722 <<
" vsafe=" << vsafe
727 }
else if (dv > 0 && dv * remainingSeconds > (secureGap - decelGap +
POSITION_EPS)) {
732 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" (eventually)\n";
740 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" (nv cannot overtake right)\n";
756 std::cout <<
" wants right follower to slow down a bit\n";
762 std::cout <<
" wants to cut in before right follower nv=" << nv->
getID() <<
" (eventually)\n";
771 const double overtakeDist = (neighFollow.second
777 const double needDV = overtakeDist / remainingSeconds;
785 <<
" wants to be overtaken by=" << nv->
getID()
786 <<
" overtakeDist=" << overtakeDist
788 <<
" vhelp=" << vhelp
789 <<
" needDV=" << needDV
795 }
else if (neighFollow.first != 0) {
805 std::cout <<
" wants to cut in before non-blocking follower nv=" << nv->
getID() <<
"\n";
813 const std::vector<CLeaderDist>& blockers,
814 double remainingSeconds) {
821 plannedSpeed =
MIN2(plannedSpeed, safe);
824 for (std::vector<CLeaderDist>::const_iterator it = blockers.begin(); it != blockers.end(); ++it) {
825 plannedSpeed =
MIN2(plannedSpeed,
informLeader(blocked, dir, *it, remainingSeconds));
833 const std::vector<CLeaderDist>& blockers,
834 double remainingSeconds,
835 double plannedSpeed) {
837 for (std::vector<CLeaderDist>::const_iterator it = blockers.begin(); it != blockers.end(); ++it) {
838 informFollower(blocked, dir, *it, remainingSeconds, plannedSpeed);
866 const double halfWidth =
getWidth() * 0.5;
876 std::vector<double> newExpectedSpeeds;
885 const std::vector<MSLane*>& lanes = currEdge->
getLanes();
886 for (std::vector<MSLane*>::const_iterator it_lane = lanes.begin(); it_lane != lanes.end(); ++it_lane) {
888 for (
int i = 0; i < subLanes; ++i) {
889 newExpectedSpeeds.push_back((*it_lane)->getVehicleMaxSpeed(&
myVehicle));
897 if (subLaneShift < std::numeric_limits<int>::max()) {
899 const int newI = i + subLaneShift;
900 if (newI > 0 && newI < (
int)newExpectedSpeeds.size()) {
918 const std::vector<MSLane*>& lanes = prevEdge->
getLanes();
919 for (std::vector<MSLane*>::const_iterator it_lane = lanes.begin(); it_lane != lanes.end(); ++it_lane) {
920 const MSLane* lane = *it_lane;
921 for (MSLinkCont::const_iterator it_link = lane->
getLinkCont().begin(); it_link != lane->
getLinkCont().end(); ++it_link) {
922 if (&((*it_link)->getLane()->getEdge()) == curEdge) {
924 const MSLane* target = (*it_link)->getLane();
925 const std::vector<MSLane*>& lanes2 = curEdge->
getLanes();
926 for (std::vector<MSLane*>::const_iterator it_lane2 = lanes2.begin(); it_lane2 != lanes2.end(); ++it_lane2) {
927 const MSLane* lane2 = *it_lane2;
928 if (lane2 == target) {
929 return prevShift + curShift;
940 return std::numeric_limits<int>::max();
970 #if defined(DEBUG_MANEUVER) || defined(DEBUG_STATE)
989 const std::vector<MSVehicle::LaneQ>& preb,
992 double& latDist,
double& maneuverDist,
int& blocked) {
997 int bestLaneOffset = 0;
998 double currentDist = 0;
999 double neighDist = 0;
1006 for (
int p = 0; p < (int) preb.size(); ++p) {
1007 if (preb[p].lane == prebLane && p + laneOffset >= 0) {
1008 assert(p + laneOffset < (
int)preb.size());
1010 neigh = preb[p + laneOffset];
1011 currentDist = curr.
length;
1012 neighDist = neigh.
length;
1013 bestLaneOffset = curr.bestLaneOffset;
1015 if (bestLaneOffset == 0 && preb[p + laneOffset].bestLaneOffset == 0) {
1016 #ifdef DEBUG_WANTSCHANGE
1020 <<
" bestLaneOffsetOld=" << bestLaneOffset
1021 <<
" bestLaneOffsetNew=" << laneOffset
1025 bestLaneOffset = laneOffset;
1031 double driveToNextStop = -std::numeric_limits<double>::max();
1040 #ifdef DEBUG_WANTS_CHANGE
1045 <<
" stopPos=" << stopPos
1046 <<
" currentDist=" << currentDist
1047 <<
" neighDist=" << neighDist
1051 currentDist =
MAX2(currentDist, stopPos);
1052 neighDist =
MAX2(neighDist, stopPos);
1055 const bool right = (laneOffset == -1);
1056 const bool left = (laneOffset == 1);
1059 const bool changeToBest = (right && bestLaneOffset < 0) || (left && bestLaneOffset > 0) || (laneOffset == 0 && bestLaneOffset == 0);
1085 #ifdef DEBUG_WANTSCHANGE
1092 <<
"\n leaders=" << leaders.
toString()
1093 <<
"\n followers=" << followers.
toString()
1094 <<
"\n blockers=" << blockers.
toString()
1095 <<
"\n neighLeaders=" << neighLeaders.
toString()
1096 <<
"\n neighFollowers=" << neighFollowers.
toString()
1097 <<
"\n neighBlockers=" << neighBlockers.
toString()
1098 <<
"\n changeToBest=" << changeToBest
1099 <<
" latLaneDist=" << latLaneDist
1107 if (lastBlocked != firstBlocked) {
1167 int roundaboutEdgesAhead = 0;
1169 if ((*it) !=
nullptr && (*it)->getEdge().isRoundabout()) {
1170 roundaboutEdgesAhead += 1;
1171 }
else if (roundaboutEdgesAhead > 0) {
1176 int roundaboutEdgesAheadNeigh = 0;
1178 if ((*it) !=
nullptr && (*it)->getEdge().isRoundabout()) {
1179 roundaboutEdgesAheadNeigh += 1;
1180 }
else if (roundaboutEdgesAheadNeigh > 0) {
1185 if (roundaboutEdgesAhead > 1) {
1189 if (roundaboutEdgesAhead > 0) {
1190 #ifdef DEBUG_ROUNDABOUTS
1192 std::cout <<
" roundaboutEdgesAhead=" << roundaboutEdgesAhead <<
" roundaboutEdgesAheadNeigh=" << roundaboutEdgesAheadNeigh <<
"\n";
1197 if (laneOffset != 0) {
1209 roundaboutEdgesAhead,
1214 if ((ret &
LCA_STAY) != 0 && latDist == 0) {
1224 if (changeToBest && abs(bestLaneOffset) > 1
1229 #ifdef DEBUG_WANTSCHANGE
1231 std::cout <<
" reserving space for unseen blockers myLeadingBlockerLength=" <<
myLeadingBlockerLength <<
"\n";
1240 if (*firstBlocked != neighLeadLongest) {
1243 std::vector<CLeaderDist> collectLeadBlockers;
1244 std::vector<CLeaderDist> collectFollowBlockers;
1245 int blockedFully = 0;
1248 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1249 leaders, followers, blockers,
1250 neighLeaders, neighFollowers, neighBlockers, &collectLeadBlockers, &collectFollowBlockers,
1251 false, gapFactor, &blockedFully);
1253 const double absLaneOffset = fabs(bestLaneOffset != 0 ? bestLaneOffset : latDist /
SUMO_const_laneWidth);
1254 const double remainingSeconds = ((ret &
LCA_TRACI) == 0 ?
1257 const double plannedSpeed =
informLeaders(blocked, myLca, collectLeadBlockers, remainingSeconds);
1259 if (plannedSpeed >= 0) {
1261 informFollowers(blocked, myLca, collectFollowBlockers, remainingSeconds, plannedSpeed);
1263 if (plannedSpeed > 0) {
1264 commitManoeuvre(blocked, blockedFully, leaders, neighLeaders, neighLane, maneuverDist);
1266 #if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE)
1273 <<
" remainingSeconds=" << remainingSeconds
1274 <<
" plannedSpeed=" << plannedSpeed
1284 if (roundaboutEdgesAhead > 1) {
1294 latDist = latLaneDist;
1295 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1296 leaders, followers, blockers,
1297 neighLeaders, neighFollowers, neighBlockers);
1312 const double inconvenience = (latLaneDist < 0
1326 && (changeToBest ||
currentDistAllows(neighDist, abs(bestLaneOffset) + 1, laDist))) {
1329 #ifdef DEBUG_COOPERATE
1336 <<
" wantsChangeToHelp=" << (right ?
"right" :
"left")
1346 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1347 leaders, followers, blockers,
1348 neighLeaders, neighFollowers, neighBlockers);
1372 const double vehWidth =
getWidth();
1374 const double leftVehSide = rightVehSide + vehWidth;
1376 double defaultNextSpeed = std::numeric_limits<double>::max();
1378 int leftmostOnEdge = (int)sublaneSides.size() - 1;
1379 while (leftmostOnEdge > 0 && sublaneSides[leftmostOnEdge] > leftVehSide) {
1382 int rightmostOnEdge = leftmostOnEdge;
1383 while (rightmostOnEdge > 0 && sublaneSides[rightmostOnEdge] > rightVehSide +
NUMERICAL_EPS) {
1385 #ifdef DEBUG_WANTSCHANGE
1387 std::cout <<
" adapted to current sublane=" << rightmostOnEdge <<
" defaultNextSpeed=" << defaultNextSpeed <<
"\n";
1388 std::cout <<
" sublaneSides[rightmostOnEdge]=" << sublaneSides[rightmostOnEdge] <<
" rightVehSide=" << rightVehSide <<
"\n";
1394 #ifdef DEBUG_WANTSCHANGE
1396 std::cout <<
" adapted to current sublane=" << rightmostOnEdge <<
" defaultNextSpeed=" << defaultNextSpeed <<
"\n";
1397 std::cout <<
" sublaneSides[rightmostOnEdge]=" << sublaneSides[rightmostOnEdge] <<
" rightVehSide=" << rightVehSide <<
"\n";
1400 double maxGain = -std::numeric_limits<double>::max();
1401 double maxGainRight = -std::numeric_limits<double>::max();
1402 double maxGainLeft = -std::numeric_limits<double>::max();
1403 double latDistNice = std::numeric_limits<double>::max();
1406 const double leftMax =
MAX2(
1409 assert(leftMax <= edge.
getWidth());
1410 int sublaneCompact =
MAX2(iMin, rightmostOnEdge - 1);
1412 #ifdef DEBUG_WANTSCHANGE
1414 <<
" checking sublanes rightmostOnEdge=" << rightmostOnEdge
1415 <<
" leftmostOnEdge=" << leftmostOnEdge
1417 <<
" leftMax=" << leftMax
1418 <<
" sublaneCompact=" << sublaneCompact
1421 for (
int i = iMin; i < (int)sublaneSides.size(); ++i) {
1422 if (sublaneSides[i] + vehWidth < leftMax) {
1428 while (vMin > 0 && j < (
int)sublaneSides.size() && sublaneSides[j] < sublaneSides[i] + vehWidth) {
1434 const double currentLatDist = sublaneSides[i] - rightVehSide;
1436 if (relativeGain > maxGain) {
1437 maxGain = relativeGain;
1440 latDist = currentLatDist;
1441 #ifdef DEBUG_WANTSCHANGE
1443 std::cout <<
" i=" << i <<
" newLatDist=" << latDist <<
" relGain=" << relativeGain <<
"\n";
1449 if (currentLatDist > 0
1454 latDist = currentLatDist;
1457 #ifdef DEBUG_WANTSCHANGE
1459 std::cout <<
" i=" << i <<
" rightmostOnEdge=" << rightmostOnEdge <<
" vMin=" << vMin <<
" relGain=" << relativeGain <<
" sublaneCompact=" << sublaneCompact <<
" curLatDist=" << currentLatDist <<
"\n";
1463 maxGainRight =
MAX2(maxGainRight, relativeGain);
1465 maxGainLeft =
MAX2(maxGainLeft, relativeGain);
1467 const double subAlignDist = sublaneSides[i] - rightVehSide;
1468 if (fabs(subAlignDist) < fabs(latDistNice)) {
1469 latDistNice = subAlignDist;
1470 #ifdef DEBUG_WANTSCHANGE
1472 <<
" nicest sublane=" << i
1473 <<
" side=" << sublaneSides[i]
1474 <<
" rightSide=" << rightVehSide
1475 <<
" latDistNice=" << latDistNice
1476 <<
" maxGainR=" << maxGainRight
1477 <<
" maxGainL=" << maxGainLeft
1484 if (maxGainRight != -std::numeric_limits<double>::max()) {
1485 #ifdef DEBUG_WANTSCHANGE
1491 #ifdef DEBUG_WANTSCHANGE
1497 if (maxGainLeft != -std::numeric_limits<double>::max()) {
1498 #ifdef DEBUG_WANTSCHANGE
1504 #ifdef DEBUG_WANTSCHANGE
1511 if ((fabs(maxGainRight) <
NUMERICAL_EPS || maxGainRight == -std::numeric_limits<double>::max())
1512 && (right || (alternatives &
LCA_RIGHT) == 0)) {
1515 if ((fabs(maxGainLeft) <
NUMERICAL_EPS || maxGainLeft == -std::numeric_limits<double>::max())
1516 && (left || (alternatives &
LCA_LEFT) == 0)) {
1521 #ifdef DEBUG_WANTSCHANGE
1524 <<
" defaultNextSpeed=" << defaultNextSpeed
1525 <<
" maxGain=" << maxGain
1526 <<
" maxGainRight=" << maxGainRight
1527 <<
" maxGainLeft=" << maxGainLeft
1528 <<
" latDist=" << latDist
1529 <<
" latDistNice=" << latDistNice
1530 <<
" sublaneCompact=" << sublaneCompact
1536 if (right && maxGainRight >= 0) {
1543 double fullSpeedDrivingSeconds =
MIN2(acceptanceTime, fullSpeedGap / vMax);
1545 if (neighLead.first != 0 && neighLead.first->getSpeed() < vMax) {
1546 fullSpeedGap =
MAX2(0.,
MIN2(fullSpeedGap,
1548 vMax, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())));
1549 fullSpeedDrivingSeconds =
MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - neighLead.first->getSpeed()));
1554 #ifdef DEBUG_WANTSCHANGE
1557 <<
" considering keepRight:"
1559 <<
" neighDist=" << neighDist
1561 <<
" leaderSpeed=" << (neighLead.first == 0 ? -1 : neighLead.first->getSpeed())
1563 myVehicle.
getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()))
1564 <<
" acceptanceTime=" << acceptanceTime
1565 <<
" fullSpeedGap=" << fullSpeedGap
1566 <<
" fullSpeedDrivingSeconds=" << fullSpeedDrivingSeconds
1567 <<
" dProb=" << deltaProb
1578 latDist = latLaneDist;
1579 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1580 leaders, followers, blockers,
1581 neighLeaders, neighFollowers, neighBlockers);
1587 #ifdef DEBUG_WANTSCHANGE
1592 <<
" neighDist=" << neighDist
1595 <<
" latDist=" << latDist
1604 int blockedFully = 0;
1605 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1606 leaders, followers, blockers,
1607 neighLeaders, neighFollowers, neighBlockers,
1608 nullptr,
nullptr,
false, 0, &blockedFully);
1617 #ifdef DEBUG_WANTSCHANGE
1622 <<
" latDist=" << latDist
1623 <<
" neighDist=" << neighDist
1626 <<
" stayInLane=" << stayInLane
1637 int blockedFully = 0;
1638 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1639 leaders, followers, blockers,
1640 neighLeaders, neighFollowers, neighBlockers,
1641 nullptr,
nullptr,
false, 0, &blockedFully);
1650 double latDistSublane = 0.;
1652 const double halfVehWidth =
getWidth() * 0.5;
1655 && bestLaneOffset == 0
1675 #ifdef DEBUG_WANTSCHANGE
1689 switch (turnInfo.second) {
1718 latDistSublane = latDistNice;
1721 latDistSublane = sublaneSides[sublaneCompact] - rightVehSide;
1729 #if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE) || defined(DEBUG_MANEUVER)
1734 <<
" latDist=" << latDist
1735 <<
" latDistSublane=" << latDistSublane
1736 <<
" relGainSublane=" <<
computeSpeedGain(latDistSublane, defaultNextSpeed)
1737 <<
" maneuverDist=" << maneuverDist
1747 #if defined(DEBUG_WANTSCHANGE)
1749 <<
" speedGain=" <<
computeSpeedGain(latDistSublane, defaultNextSpeed) <<
")\n";
1756 && ((myManeuverDist < 0 && latDistSublane > 0) || (
myManeuverDist > 0 && latDistSublane < 0))) {
1757 #if defined(DEBUG_WANTSCHANGE)
1759 std::cout <<
" aborting sublane change due to prior maneuver\n";
1764 latDist = latDistSublane;
1769 #ifdef DEBUG_WANTSCHANGE
1772 <<
" latDist=" << latDist
1780 #ifdef DEBUG_WANTSCHANGE
1786 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1787 leaders, followers, blockers,
1788 neighLeaders, neighFollowers, neighBlockers);
1815 #ifdef DEBUG_WANTSCHANGE
1832 if ((*blocked) !=
nullptr) {
1834 #ifdef DEBUG_SLOWDOWN
1858 (*blocked)->getCarFollowModel().getMaxDecel()));
1869 #ifdef DEBUG_SAVE_BLOCKER_LENGTH
1885 #ifdef DEBUG_SAVE_BLOCKER_LENGTH
1897 #ifdef DEBUG_SAVE_BLOCKER_LENGTH
1903 <<
" potential=" << potential
1919 <<
"vSafe=" << vSafe <<
" -> accel=" << accel <<
"\n";
1929 const MSLane* lane = lanes[laneIndex];
1931 assert(preb.size() == lanes.size());
1933 for (
int sublane = 0; sublane < (int)ahead.
numSublanes(); ++sublane) {
1934 const int edgeSublane = sublane + sublaneOffset;
1942 const MSVehicle* leader = ahead[sublane].first;
1943 const double gap = ahead[sublane].second;
1945 if (leader ==
nullptr) {
1954 #ifdef DEBUG_EXPECTED_SLSPEED
1956 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" updateExpectedSublaneSpeeds edgeSublane=" << edgeSublane <<
" leader=" << leader->
getID() <<
" gap=" << gap <<
" vSafe=" << vSafe <<
"\n";
1959 const double deltaV = vMax - leader->
getSpeed();
1960 if (deltaV > 0 && gap / deltaV < 5) {
1963 const double foreCastTime = 10;
1964 const double gapClosingTime = gap / deltaV;
1965 vSafe = (gapClosingTime * vSafe + (foreCastTime - gapClosingTime) * leader->
getSpeed()) / foreCastTime;
1972 double foeRight, foeLeft;
1976 if (leader.first != 0) {
1979 vSafe =
MIN2(vSafe, vSafePed);
1982 vSafe =
MIN2(vMax, vSafe);
1996 double result = std::numeric_limits<double>::max();
1999 const double vehWidth =
getWidth();
2001 const double leftVehSide = rightVehSide + vehWidth;
2002 for (
int i = 0; i < (int)sublaneSides.size(); ++i) {
2003 if (
overlap(rightVehSide, leftVehSide, sublaneSides[i], sublaneSides[i] + res)) {
2009 return result - defaultNextSpeed;
2016 double maxLength = -1;
2018 if (ldi[i].first != 0) {
2019 const double length = ldi[i].first->getVehicleType().getLength();
2020 if (length > maxLength) {
2033 double minSpeed = std::numeric_limits<double>::max();
2035 if (ldi[i].first != 0) {
2036 const double speed = ldi[i].first->getSpeed();
2037 if (speed < minSpeed) {
2055 std::vector<CLeaderDist>* collectLeadBlockers,
2056 std::vector<CLeaderDist>* collectFollowBlockers,
2057 bool keepLatGapManeuver,
2059 int* retBlockedFully) {
2061 if (!keepLatGapManeuver) {
2063 maneuverDist = latDist;
2066 latDist =
MAX2(
MIN2(latDist, maxDist), -maxDist);
2074 if (laneOffset != 0) {
2085 if (laneOffset != 0) {
2089 #ifdef DEBUG_BLOCKING
2101 }
else if (!forcedTraCIChange) {
2107 }
else if (!forcedTraCIChange) {
2113 #ifdef DEBUG_BLOCKING
2129 if (laneOffset != 0) {
2138 int blockedFully = 0;
2143 if (laneOffset != 0) {
2151 if (retBlockedFully !=
nullptr) {
2152 *retBlockedFully = blockedFully;
2159 blocked |= blockedFully;
2164 if (collectFollowBlockers !=
nullptr && collectLeadBlockers !=
nullptr) {
2166 for (std::vector<CLeaderDist>::const_iterator it2 = collectLeadBlockers->begin(); it2 != collectLeadBlockers->end(); ++it2) {
2167 for (std::vector<CLeaderDist>::iterator it = collectFollowBlockers->begin(); it != collectFollowBlockers->end();) {
2168 if ((*it2).first == (*it).first) {
2169 #ifdef DEBUG_BLOCKING
2171 std::cout <<
" removed follower " << (*it).first->getID() <<
" because it is already a leader\n";
2174 it = collectFollowBlockers->erase(it);
2188 double latDist,
double foeOffset,
bool leaders,
LaneChangeAction blockType,
2189 double& safeLatGapRight,
double& safeLatGapLeft,
2190 std::vector<CLeaderDist>* collectBlockers)
const {
2192 const double vehWidth =
getWidth();
2194 const double leftVehSide = rightVehSide + vehWidth;
2195 const double rightVehSideDest = rightVehSide + latDist;
2196 const double leftVehSideDest = leftVehSide + latDist;
2197 const double rightNoOverlap =
MIN2(rightVehSideDest, rightVehSide);
2198 const double leftNoOverlap =
MAX2(leftVehSideDest, leftVehSide);
2199 #ifdef DEBUG_BLOCKING
2201 std::cout <<
" checkBlockingVehicles"
2202 <<
" latDist=" << latDist
2203 <<
" foeOffset=" << foeOffset
2204 <<
" vehRight=" << rightVehSide
2205 <<
" vehLeft=" << leftVehSide
2206 <<
" rightNoOverlap=" << rightNoOverlap
2207 <<
" leftNoOverlap=" << leftNoOverlap
2208 <<
" destRight=" << rightVehSideDest
2209 <<
" destLeft=" << leftVehSideDest
2210 <<
" leaders=" << leaders
2216 for (
int i = 0; i < vehicles.
numSublanes(); ++i) {
2218 if (vehDist.first != 0 &&
myCFRelated.count(vehDist.first) == 0) {
2219 const MSVehicle* leader = vehDist.first;
2222 std::swap(leader, follower);
2225 double foeRight, foeLeft;
2227 const bool overlapBefore =
overlap(rightVehSide, leftVehSide, foeRight, foeLeft);
2228 const bool overlapDest =
overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft);
2229 const bool overlapAny =
overlap(rightNoOverlap, leftNoOverlap, foeRight, foeLeft);
2230 #ifdef DEBUG_BLOCKING
2232 std::cout <<
" foe=" << vehDist.first->getID()
2233 <<
" gap=" << vehDist.second
2235 <<
" foeRight=" << foeRight
2236 <<
" foeLeft=" << foeLeft
2237 <<
" overlapBefore=" << overlapBefore
2238 <<
" overlap=" << overlapAny
2239 <<
" overlapDest=" << overlapDest
2244 if (vehDist.second < 0) {
2245 if (overlapBefore && !overlapDest) {
2246 #ifdef DEBUG_BLOCKING
2248 std::cout <<
" ignoring current overlap to come clear\n";
2252 #ifdef DEBUG_BLOCKING
2258 if (collectBlockers ==
nullptr) {
2261 collectBlockers->push_back(vehDist);
2277 const double expectedGap =
MSCFModel::gapExtrapolation(timeTillAction, vehDist.second, leader->
getSpeed(), follower->
getSpeed(), leaderAccel, followerAccel, std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
2280 const double followerExpectedSpeed = follower->
getSpeed() + timeTillAction * followerAccel;
2281 const double leaderExpectedSpeed =
MAX2(0., leader->
getSpeed() + timeTillAction * leaderAccel);
2284 #if defined(DEBUG_ACTIONSTEPS) && defined(DEBUG_BLOCKING)
2286 std::cout <<
" timeTillAction=" << timeTillAction
2287 <<
" followerAccel=" << followerAccel
2288 <<
" followerExpectedSpeed=" << followerExpectedSpeed
2289 <<
" leaderAccel=" << leaderAccel
2290 <<
" leaderExpectedSpeed=" << leaderExpectedSpeed
2291 <<
"\n gap=" << vehDist.second
2292 <<
" gapChange=" << (expectedGap - vehDist.second)
2293 <<
" expectedGap=" << expectedGap
2294 <<
" expectedSecureGap=" << expectedSecureGap
2295 <<
" safeLatGapLeft=" << safeLatGapLeft
2296 <<
" safeLatGapRight=" << safeLatGapRight
2303 if (expectedGap < secureGap2) {
2305 if (foeRight > leftVehSide) {
2306 safeLatGapLeft =
MIN2(safeLatGapLeft, foeRight - leftVehSide);
2307 }
else if (foeLeft < rightVehSide) {
2308 safeLatGapRight =
MIN2(safeLatGapRight, rightVehSide - foeLeft);
2311 #ifdef DEBUG_BLOCKING
2313 std::cout <<
" blocked by " << vehDist.first->getID() <<
" gap=" << vehDist.second <<
" expectedGap=" << expectedGap
2314 <<
" expectedSecureGap=" << expectedSecureGap <<
" secGap2=" << secureGap2 <<
" safetyFactor=" <<
getSafetyFactor()
2315 <<
" safeLatGapLeft=" << safeLatGapLeft <<
" safeLatGapRight=" << safeLatGapRight
2319 result |= blockType;
2320 if (collectBlockers ==
nullptr) {
2323 collectBlockers->push_back(vehDist);
2325 #ifdef DEBUG_BLOCKING
2326 }
else if (
gDebugFlag2 && expectedGap < expectedSecureGap) {
2327 std::cout <<
" ignore blocker " << vehDist.first->getID() <<
" gap=" << vehDist.second <<
" expectedGap=" << expectedGap
2328 <<
" expectedSecureGap=" << expectedSecureGap <<
" secGap2=" << secureGap2 <<
" safetyFactor=" <<
getSafetyFactor() <<
"\n";
2346 const double leftVehSide = rightVehSide + vehWidth;
2347 #ifdef DEBUG_BLOCKING
2349 std::cout <<
" updateCFRelated foeOffset=" << foeOffset <<
" vehicles=" << vehicles.
toString() <<
"\n";
2352 for (
int i = 0; i < vehicles.
numSublanes(); ++i) {
2354 if (vehDist.first != 0 &&
myCFRelated.count(vehDist.first) == 0) {
2355 double foeRight, foeLeft;
2357 if (
overlap(rightVehSide, leftVehSide, foeRight, foeLeft) && (vehDist.second >= 0
2363 && -vehDist.second < vehDist.first->getVehicleType().getMinGap()
2366 #ifdef DEBUG_BLOCKING
2368 std::cout <<
" ignoring cfrelated foe=" << vehDist.first->getID() <<
" gap=" << vehDist.second
2370 <<
" foeOffset=" << foeOffset
2371 <<
" egoR=" << rightVehSide <<
" egoL=" << leftVehSide
2372 <<
" iR=" << foeRight <<
" iL=" << foeLeft
2387 assert(right <= left);
2388 assert(right2 <= left2);
2410 return changeReason;
2417 if (sd1.
state == 0) {
2419 }
else if (sd2.
state == 0) {
2429 #ifdef DEBUG_WANTSCHANGE
2435 <<
" dir1=" << sd1.
dir
2439 <<
" dir2=" << sd2.
dir
2440 <<
" reason1=" << reason1
2441 <<
" reason2=" << reason2
2447 if (reason1 < reason2) {
2449 return (!can1 && can2 && sd1.
sameDirection(sd2)) ? sd2 : sd1;
2451 }
else if (reason1 > reason2) {
2453 return (!can2 && can1 && sd1.
sameDirection(sd2)) ? sd1 : sd2;
2461 }
else if (sd2.
dir == 0) {
2466 assert(sd1.
dir == -1);
2467 assert(sd2.
dir == 1);
2470 }
else if (sd2.
latDist >= 0) {
2478 return can1 ? sd1 : sd2;
2501 const std::vector<MSVehicle::LaneQ>& preb,
2510 int roundaboutEdgesAhead,
2514 const bool right = (laneOffset == -1);
2515 const bool left = (laneOffset == 1);
2527 #ifdef DEBUG_STRATEGIC_CHANGE
2532 <<
" laDist=" << laDist
2533 <<
" currentDist=" << currentDist
2534 <<
" usableDist=" << usableDist
2535 <<
" bestLaneOffset=" << bestLaneOffset
2536 <<
" best.length=" << best.
length
2537 <<
" maxJam=" << maxJam
2538 <<
" neighLeftPlace=" << neighLeftPlace
2544 if (laneOffset != 0 && changeToBest && bestLaneOffset == curr.
bestLaneOffset
2547 latDist = latLaneDist;
2564 #ifdef DEBUG_STRATEGIC_CHANGE
2567 <<
" avoid overtaking on the right nv=" << nv->
getID()
2577 if (!changeToBest && (
currentDistDisallows(neighLeftPlace, abs(bestLaneOffset) + 2, laDist))) {
2584 #ifdef DEBUG_STRATEGIC_CHANGE
2586 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" could not change back and forth in time (1) neighLeftPlace=" << neighLeftPlace <<
"\n";
2592 && bestLaneOffset == 0
2595 && roundaboutEdgesAhead == 0
2600 #ifdef DEBUG_STRATEGIC_CHANGE
2602 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" does not want to leave the bestLane (neighDist=" << neighDist <<
")\n";
2607 && bestLaneOffset == 0
2613 #ifdef DEBUG_STRATEGIC_CHANGE
2615 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" does not want to get stranded on the on-ramp of a highway\n";
2629 MSLane* shadowPrev =
nullptr;
2631 if (*it ==
nullptr) {
2635 if (shadow ==
nullptr || currentShadowDist >= requiredDist) {
2638 if (shadowPrev !=
nullptr) {
2641 currentShadowDist += shadow->
getLength();
2642 shadowPrev = shadow;
2643 #ifdef DEBUG_STRATEGIC_CHANGE
2645 std::cout <<
" shadow=" << shadow->
getID() <<
" currentShadowDist=" << currentShadowDist <<
"\n";
2649 #ifdef DEBUG_STRATEGIC_CHANGE
2654 if (currentShadowDist < requiredDist && currentShadowDist < usableDist) {
2657 #ifdef DEBUG_STRATEGIC_CHANGE
2659 std::cout <<
" must change for shadowLane end latDist=" << latDist <<
" myLeftSpace=" <<
myLeftSpace <<
"\n";
2667 #if defined(DEBUG_STRATEGIC_CHANGE) || defined(DEBUG_TRACI)
2679 }
else if (((retTraCI &
LCA_RIGHT) != 0 && laneOffset < 0)
2680 || ((retTraCI &
LCA_LEFT) != 0 && laneOffset > 0)) {
2682 latDist = latLaneDist;
2685 #if defined(DEBUG_STRATEGIC_CHANGE) || defined(DEBUG_TRACI)
2687 std::cout <<
" reqAfterInfluence=" << ret <<
" ret=" << ret <<
"\n";
2711 double& maneuverDist,
2747 const bool stayInLane = laneOffset == 0 || ((state &
LCA_STRATEGIC) != 0 && (state &
LCA_STAY) != 0);
2748 const double oldLatDist = latDist;
2751 const double halfWidth =
getWidth() * 0.5;
2757 double surplusGapRight = oldCenter - halfWidth;
2759 #ifdef DEBUG_KEEP_LATGAP
2761 std::cout <<
"\n " <<
SIMTIME <<
" keepLatGap() laneOffset=" << laneOffset
2762 <<
" latDist=" << latDist
2765 <<
" gapFactor=" << gapFactor
2766 <<
" stayInLane=" << stayInLane <<
"\n"
2767 <<
" stayInEdge: surplusGapRight=" << surplusGapRight <<
" surplusGapLeft=" << surplusGapLeft <<
"\n";
2771 if (surplusGapLeft < 0 || surplusGapRight < 0) {
2781 if (laneOffset != 0) {
2786 #ifdef DEBUG_KEEP_LATGAP
2788 std::cout <<
" minGapLat: surplusGapRight=" << surplusGapRight <<
" surplusGapLeft=" << surplusGapLeft <<
"\n"
2797 if (stayInLane || laneOffset == 1) {
2803 if (stayInLane || laneOffset == -1) {
2809 #ifdef DEBUG_KEEP_LATGAP
2811 std::cout <<
" stayInLane: surplusGapRight=" << surplusGapRight <<
" surplusGapLeft=" << surplusGapLeft <<
"\n";
2815 if (surplusGapRight + surplusGapLeft < 0) {
2820 const double equalDeficit = 0.5 * (surplusGapLeft + surplusGapRight);
2821 if (surplusGapRight < surplusGapLeft) {
2823 const double delta =
MIN2(equalDeficit - surplusGapRight, physicalGapLeft);
2825 maneuverDist = delta;
2826 #ifdef DEBUG_KEEP_LATGAP
2828 std::cout <<
" insufficient latSpace, move left: delta=" << delta <<
"\n";
2833 const double delta =
MIN2(equalDeficit - surplusGapLeft, physicalGapRight);
2835 maneuverDist = -delta;
2836 #ifdef DEBUG_KEEP_LATGAP
2838 std::cout <<
" insufficient latSpace, move right: delta=" << delta <<
"\n";
2844 latDist =
MAX2(
MIN2(latDist, surplusGapLeft), -surplusGapRight);
2845 maneuverDist =
MAX2(
MIN2(maneuverDist, surplusGapLeft), -surplusGapRight);
2846 #ifdef DEBUG_KEEP_LATGAP
2848 std::cout <<
" adapted latDist=" << latDist <<
" maneuverDist=" << maneuverDist <<
" (old=" << oldLatDist <<
")\n";
2858 #ifdef DEBUG_KEEP_LATGAP
2860 std::cout <<
" traci influenced latDist=" << latDist <<
"\n";
2866 const bool traciChange = (state &
LCA_TRACI) != 0;
2867 if (nonSublaneChange && !traciChange) {
2869 #ifdef DEBUG_KEEP_LATGAP
2871 std::cout <<
" wanted changeToLeft oldLatDist=" << oldLatDist <<
", blocked latGap changeToRight\n";
2874 latDist = oldLatDist;
2877 #ifdef DEBUG_KEEP_LATGAP
2879 std::cout <<
" wanted changeToRight oldLatDist=" << oldLatDist <<
", blocked latGap changeToLeft\n";
2882 latDist = oldLatDist;
2892 #ifdef DEBUG_KEEP_LATGAP
2894 std::cout <<
" latDistUpdated=" << latDist <<
" oldLatDist=" << oldLatDist <<
"\n";
2897 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset, leaders, followers, blockers, neighLeaders, neighFollowers, neighBlockers,
nullptr,
nullptr, nonSublaneChange);
2911 #if defined(DEBUG_KEEP_LATGAP) || defined(DEBUG_STATE)
2913 std::cout <<
" latDist2=" << latDist
2927 double& surplusGapRight,
double& surplusGapLeft,
2928 bool saveMinGap,
double netOverlap,
2930 std::vector<CLeaderDist>* collectBlockers) {
2935 if (others[i].first != 0 && others[i].second <= 0
2937 && (netOverlap == 0 || others[i].second + others[i].first->getVehicleType().getMinGap() < netOverlap)) {
2941 double foeRight, foeLeft;
2943 const double foeCenter = foeRight + 0.5 * res;
2944 const double gap =
MIN2(fabs(foeRight - oldCenter), fabs(foeLeft - oldCenter)) - halfWidth;
2947 const double currentMinGap = desiredMinGap * gapFactor;
2958 #if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
2960 std::cout <<
" updateGaps"
2962 <<
" foe=" << foe->
getID()
2963 <<
" foeRight=" << foeRight
2964 <<
" foeLeft=" << foeLeft
2965 <<
" oldCenter=" << oldCenter
2966 <<
" gap=" << others[i].second
2967 <<
" latgap=" << gap
2968 <<
" currentMinGap=" << currentMinGap
2969 <<
" surplusGapRight=" << surplusGapRight
2970 <<
" surplusGapLeft=" << surplusGapLeft
2978 if (foeCenter < oldCenter) {
2980 surplusGapRight =
MIN3(surplusGapRight, gap - currentMinGap,
MAX2(currentMinGap, gap - foeManeuverDist));
2983 surplusGapLeft =
MIN3(surplusGapLeft, gap - currentMinGap,
MAX2(currentMinGap, gap - foeManeuverDist));
2986 if (foeCenter < oldCenter) {
2987 #if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
2989 std::cout <<
" new minimum rightGap=" << gap <<
"\n";
2994 #if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
2996 std::cout <<
" new minimum leftGap=" << gap <<
"\n";
3002 if (collectBlockers !=
nullptr) {
3004 if ((foeCenter < oldCenter && latDist < 0 && gap < (desiredMinGap - latDist))
3005 || (foeCenter > oldCenter && latDist > 0 && gap < (desiredMinGap + latDist))) {
3006 collectBlockers->push_back(others[i]);
3023 int currentDirection =
mySpeedLat >= 0 ? 1 : -1;
3024 int directionWish = latDist >= 0 ? 1 : -1;
3028 #ifdef DEBUG_MANEUVER
3032 <<
" computeSpeedLat()"
3033 <<
" currentDirection=" << currentDirection
3034 <<
" directionWish=" << directionWish
3040 if (directionWish == 1) {
3051 double speedAccelSafe = latDist * speedAccel >= 0 ? speedAccel : 0;
3059 if (maneuverDist * latDist > 0) {
3060 maneuverDist = fullLatDist;
3063 #ifdef DEBUG_MANEUVER
3068 <<
" latDist=" << latDist
3069 <<
" maneuverDist=" << maneuverDist
3072 <<
" fullLatDist=" << fullLatDist
3073 <<
" speedAccel=" << speedAccel
3074 <<
" speedDecel=" << speedDecel
3075 <<
" speedBound=" << speedBound
3079 if (speedDecel * speedAccel <= 0 && (
3081 (latDist >= 0 && speedAccel >= speedBound && speedBound >= speedDecel)
3082 || (latDist <= 0 && speedAccel <= speedBound && speedBound <= speedDecel))) {
3084 #ifdef DEBUG_MANEUVER
3086 std::cout <<
" computeSpeedLat a)\n";
3093 #ifdef DEBUG_MANEUVER
3095 std::cout <<
" computeSpeedLat b)\n";
3098 return speedAccelSafe;
3102 if ((fabs(minDistAccel) < fabs(fullLatDist)) || (fabs(minDistAccel - fullLatDist) <
NUMERICAL_EPS)) {
3103 #ifdef DEBUG_MANEUVER
3105 std::cout <<
" computeSpeedLat c)\n";
3110 #ifdef DEBUG_MANEUVER
3112 std::cout <<
" minDistAccel=" << minDistAccel <<
"\n";
3117 if ((fabs(minDistCurrent) < fabs(fullLatDist)) || (fabs(minDistCurrent - fullLatDist) <
NUMERICAL_EPS)) {
3118 #ifdef DEBUG_MANEUVER
3120 std::cout <<
" computeSpeedLat d)\n";
3127 #ifdef DEBUG_MANEUVER
3129 std::cout <<
" computeSpeedLat e)\n";
3132 return speedDecelSafe;
3141 double maneuverDist) {
3144 double secondsToLeaveLane;
3154 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3170 double nextLeftSpace;
3171 if (nextActionStepSpeed > 0.) {
3186 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3190 <<
" avoidArrivalSpeed=" << avoidArrivalSpeed
3193 <<
"\n nextLeftSpace=" << nextLeftSpace
3194 <<
" nextActionStepSpeed=" << nextActionStepSpeed
3195 <<
" nextActionStepRemainingSeconds=" << secondsToLeaveLane - timeTillActionStep
3205 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3209 <<
" secondsToLeave=" << secondsToLeaveLane
3231 const double vehWidth =
getWidth();
3233 const double leftVehSide = rightVehSide + vehWidth;
3234 const double rightVehSideDest = rightVehSide + latDist;
3235 const double leftVehSideDest = leftVehSide + latDist;
3236 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3238 std::cout <<
" commitFollowSpeed"
3239 <<
" latDist=" << latDist
3240 <<
" foeOffset=" << foeOffset
3241 <<
" vehRight=" << rightVehSide
3242 <<
" vehLeft=" << leftVehSide
3243 <<
" destRight=" << rightVehSideDest
3244 <<
" destLeft=" << leftVehSideDest
3250 if (vehDist.first != 0) {
3251 const MSVehicle* leader = vehDist.first;
3253 double foeRight, foeLeft;
3255 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3257 std::cout <<
" foe=" << vehDist.first->getID()
3258 <<
" gap=" << vehDist.second
3260 <<
" foeRight=" << foeRight
3261 <<
" foeLeft=" << foeLeft
3262 <<
" overlapBefore=" <<
overlap(rightVehSide, leftVehSide, foeRight, foeLeft)
3263 <<
" overlapDest=" <<
overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft)
3267 if (
overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft)) {
3271 speed =
MIN2(speed, vSafe);
3272 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3274 std::cout <<
" case1 vsafe=" << vSafe <<
" speed=" << speed <<
"\n";
3277 }
else if (
overlap(rightVehSide, leftVehSide, foeRight, foeLeft)) {
3282 speed =
MIN2(speed, vSafe);
3283 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3285 std::cout <<
" case2 vsafe=" << vSafe <<
" speed=" << speed <<
"\n";
3381 const std::pair<MSVehicle*, double>& leader,
3382 const std::pair<MSVehicle*, double>& neighLead,
3383 const std::pair<MSVehicle*, double>& neighFollow,
3385 const std::vector<MSVehicle::LaneQ>& preb,
3391 #ifdef DEBUG_WANTSCHANGE
3393 std::cout <<
"\nWANTS_CHANGE\n" <<
SIMTIME
3400 <<
" considerChangeTo=" << (laneOffset == -1 ?
"right" :
"left")
3414 double maneuverDist;
3417 leaders, followers, blockers,
3418 neighLeaders, neighFollowers, neighBlockers,
3420 lastBlocked, firstBlocked, latDist, maneuverDist, blocked);
3426 result |=
getLCA(result, latDist);
3428 #if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE)
3433 <<
" wantsChangeTo=" << (laneOffset == -1 ?
"right" :
"left")
3434 << ((result &
LCA_URGENT) ?
" (urgent)" :
"")
3440 << ((result &
LCA_TRACI) ?
" (traci)" :
"")