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),
183 const std::vector<MSVehicle::LaneQ>& preb,
186 double& latDist,
double& maneuverDist,
int& blocked) {
189 const std::string changeType = laneOffset == -1 ?
"right" : (laneOffset == 1 ?
"left" :
"current");
191 #ifdef DEBUG_MANEUVER
201 <<
" considerChangeTo=" << changeType
208 leaders, followers, blockers,
209 neighLeaders, neighFollowers, neighBlockers,
211 lastBlocked, firstBlocked, latDist, maneuverDist, blocked);
213 result =
keepLatGap(result, leaders, followers, blockers,
214 neighLeaders, neighFollowers, neighBlockers,
215 neighLane, laneOffset, latDist, maneuverDist, blocked);
217 result |=
getLCA(result, latDist);
219 #if defined(DEBUG_MANEUVER) || defined(DEBUG_STATE)
220 double latDistTmp = latDist;
223 #if defined(DEBUG_MANEUVER) || defined(DEBUG_STATE)
225 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" maneuverDist=" << maneuverDist <<
" latDist=" << latDistTmp <<
" mySpeedPrev=" <<
mySpeedLat <<
" speedLat=" <<
DIST2SPEED(latDist) <<
" latDist2=" << latDist <<
"\n";
232 <<
" wantsChangeTo=" << changeType
233 <<
" latDist=" << latDist
234 <<
" maneuverDist=" << maneuverDist
242 <<
" wantsNoChangeTo=" << changeType
300 const double newSpeed =
_patchSpeed(
MAX2(min, 0.0), wanted, max, cfModel);
301 #ifdef DEBUG_PATCHSPEED
303 const std::string patched = (wanted != newSpeed ?
" patched=" +
toString(newSpeed) :
"");
310 <<
" wanted=" << wanted
333 #ifdef DEBUG_PATCHSPEED
343 #ifdef DEBUG_PATCHSPEED
345 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" slowing down for leading blocker, safe=" << safe << (safe +
NUMERICAL_EPS < min ?
" (not enough)" :
"") <<
"\n";
348 return MAX2(min, safe);
352 double nVSafe = wanted;
356 if (v >= min && v <= max) {
357 nVSafe =
MIN2(v, nVSafe);
359 #ifdef DEBUG_PATCHSPEED
361 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" got accel=" << (*i) <<
" nVSafe=" << nVSafe <<
"\n";
365 #ifdef DEBUG_PATCHSPEED
368 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" ignoring low nVSafe=" << v <<
" min=" << min <<
"\n";
372 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" ignoring high nVSafe=" << v <<
" max=" << max <<
"\n";
380 #ifdef DEBUG_PATCHSPEED
393 #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
398 return (max + wanted) / (double) 2.0;
402 #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
407 return (min + wanted) / (double) 2.0;
410 #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
415 return (max + wanted) / (double) 2.0;
456 #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
461 return (max + wanted) / (double) 2.0;
465 #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
485 if (pinfo->first >= 0) {
494 <<
" informedBy=" << sender->
getID()
495 <<
" info=" << pinfo->second
496 <<
" vSafe=" << pinfo->first
509 assert(cld.first != 0);
518 double remainingSeconds) {
524 plannedSpeed =
MIN2(plannedSpeed, v);
529 std::cout <<
" informLeader speed=" <<
myVehicle.
getSpeed() <<
" planned=" << plannedSpeed <<
"\n";
536 if (
gDebugFlag2) std::cout <<
" blocked by leader nv=" << nv->
getID() <<
" nvSpeed=" << nv->
getSpeed() <<
" needGap="
540 const double dv = plannedSpeed - nv->
getSpeed();
541 const double overtakeDist = (neighLead.second
553 || dv * remainingSeconds < overtakeDist) {
568 <<
" cannot overtake leader nv=" << nv->
getID()
570 <<
" remainingSeconds=" << remainingSeconds
571 <<
" targetSpeed=" << targetSpeed
572 <<
" nextSpeed=" << nextSpeed
583 <<
" cannot overtake fast leader nv=" << nv->
getID()
585 <<
" remainingSeconds=" << remainingSeconds
586 <<
" targetSpeed=" << targetSpeed
597 <<
" wants to overtake leader nv=" << nv->
getID()
599 <<
" remainingSeconds=" << remainingSeconds
600 <<
" currentGap=" << neighLead.second
602 <<
" overtakeDist=" << overtakeDist
612 }
else if (neighLead.first != 0) {
615 double dv, nextNVSpeed;
635 std::cout <<
" not blocked by leader nv=" << nv->
getID()
637 <<
" gap=" << neighLead.second
638 <<
" nextGap=" << neighLead.second - dv
640 <<
" targetSpeed=" << targetSpeed
644 return MIN2(targetSpeed, plannedSpeed);
656 double remainingSeconds,
657 double plannedSpeed) {
661 if (
gDebugFlag2) std::cout <<
" blocked by follower nv=" << nv->
getID() <<
" nvSpeed=" << nv->
getSpeed() <<
" needGap="
668 if ((neededGap - neighFollow.second) / remainingSeconds < (plannedSpeed - nv->
getSpeed())) {
671 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" without any help neededGap=" << neededGap <<
"\n";
690 const double neighNewSpeed1s =
MAX2(0., nv->
getSpeed() - helpDecel);
691 const double dv = plannedSpeed - neighNewSpeed1s;
693 const double decelGap = neighFollow.second + dv;
699 <<
" egoNV=" << plannedSpeed
700 <<
" nvNewSpeed=" << neighNewSpeed
701 <<
" nvNewSpeed1s=" << neighNewSpeed1s
702 <<
" deltaGap=" << dv
703 <<
" decelGap=" << decelGap
704 <<
" secGap=" << secureGap
708 if (decelGap > 0 && decelGap >= secureGap) {
723 std::cout <<
" wants to cut in before nv=" << nv->
getID()
724 <<
" vsafe1=" << vsafe1
725 <<
" vsafe=" << vsafe
730 }
else if (dv > 0 && dv * remainingSeconds > (secureGap - decelGap +
POSITION_EPS)) {
735 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" (eventually)\n";
743 std::cout <<
" wants to cut in before nv=" << nv->
getID() <<
" (nv cannot overtake right)\n";
759 std::cout <<
" wants right follower to slow down a bit\n";
765 std::cout <<
" wants to cut in before right follower nv=" << nv->
getID() <<
" (eventually)\n";
774 const double overtakeDist = (neighFollow.second
780 const double needDV = overtakeDist / remainingSeconds;
788 <<
" wants to be overtaken by=" << nv->
getID()
789 <<
" overtakeDist=" << overtakeDist
791 <<
" vhelp=" << vhelp
792 <<
" needDV=" << needDV
798 }
else if (neighFollow.first != 0) {
808 std::cout <<
" wants to cut in before non-blocking follower nv=" << nv->
getID() <<
"\n";
816 const std::vector<CLeaderDist>& blockers,
817 double remainingSeconds) {
829 plannedSpeed =
MIN2(plannedSpeed, safe);
831 for (std::vector<CLeaderDist>::const_iterator it = blockers.begin(); it != blockers.end(); ++it) {
832 plannedSpeed =
MIN2(plannedSpeed,
informLeader(blocked, dir, *it, remainingSeconds));
840 const std::vector<CLeaderDist>& blockers,
841 double remainingSeconds,
842 double plannedSpeed) {
844 for (std::vector<CLeaderDist>::const_iterator it = blockers.begin(); it != blockers.end(); ++it) {
845 informFollower(blocked, dir, *it, remainingSeconds, plannedSpeed);
873 const double halfWidth =
getWidth() * 0.5;
883 std::vector<double> newExpectedSpeeds;
892 const std::vector<MSLane*>& lanes = currEdge->
getLanes();
893 for (std::vector<MSLane*>::const_iterator it_lane = lanes.begin(); it_lane != lanes.end(); ++it_lane) {
895 for (
int i = 0; i < subLanes; ++i) {
896 newExpectedSpeeds.push_back((*it_lane)->getVehicleMaxSpeed(&
myVehicle));
904 if (subLaneShift < std::numeric_limits<int>::max()) {
906 const int newI = i + subLaneShift;
907 if (newI > 0 && newI < (
int)newExpectedSpeeds.size()) {
942 const std::vector<MSLane*>& lanes = prevEdge->
getLanes();
943 for (std::vector<MSLane*>::const_iterator it_lane = lanes.begin(); it_lane != lanes.end(); ++it_lane) {
944 const MSLane* lane = *it_lane;
945 for (MSLinkCont::const_iterator it_link = lane->
getLinkCont().begin(); it_link != lane->
getLinkCont().end(); ++it_link) {
946 if (&((*it_link)->getLane()->getEdge()) == curEdge) {
948 const MSLane* target = (*it_link)->getLane();
949 const std::vector<MSLane*>& lanes2 = curEdge->
getLanes();
950 for (std::vector<MSLane*>::const_iterator it_lane2 = lanes2.begin(); it_lane2 != lanes2.end(); ++it_lane2) {
951 const MSLane* lane2 = *it_lane2;
952 if (lane2 == target) {
953 return prevShift + curShift;
964 return std::numeric_limits<int>::max();
994 #if defined(DEBUG_MANEUVER) || defined(DEBUG_STATE)
1013 const std::vector<MSVehicle::LaneQ>& preb,
1016 double& latDist,
double& maneuverDist,
int& blocked) {
1021 int bestLaneOffset = 0;
1022 double currentDist = 0;
1023 double neighDist = 0;
1030 for (
int p = 0; p < (int) preb.size(); ++p) {
1031 if (preb[p].lane == prebLane && p + laneOffset >= 0) {
1032 assert(p + laneOffset < (
int)preb.size());
1034 neigh = preb[p + laneOffset];
1035 currentDist = curr.
length;
1036 neighDist = neigh.
length;
1039 if (bestLaneOffset == 0 && preb[p + laneOffset].bestLaneOffset == 0) {
1040 #ifdef DEBUG_WANTSCHANGE
1044 <<
" bestLaneOffsetOld=" << bestLaneOffset
1045 <<
" bestLaneOffsetNew=" << laneOffset
1049 bestLaneOffset = laneOffset;
1055 double driveToNextStop = -std::numeric_limits<double>::max();
1064 #ifdef DEBUG_WANTS_CHANGE
1069 <<
" stopPos=" << stopPos
1070 <<
" currentDist=" << currentDist
1071 <<
" neighDist=" << neighDist
1075 currentDist =
MAX2(currentDist, stopPos);
1076 neighDist =
MAX2(neighDist, stopPos);
1079 const bool right = (laneOffset == -1);
1080 const bool left = (laneOffset == 1);
1083 const bool changeToBest = (right && bestLaneOffset < 0) || (left && bestLaneOffset > 0) || (laneOffset == 0 && bestLaneOffset == 0);
1109 #ifdef DEBUG_WANTSCHANGE
1116 <<
"\n leaders=" << leaders.
toString()
1117 <<
"\n followers=" << followers.
toString()
1118 <<
"\n blockers=" << blockers.
toString()
1119 <<
"\n neighLeaders=" << neighLeaders.
toString()
1120 <<
"\n neighFollowers=" << neighFollowers.
toString()
1121 <<
"\n neighBlockers=" << neighBlockers.
toString()
1122 <<
"\n changeToBest=" << changeToBest
1123 <<
" latLaneDist=" << latLaneDist
1131 if (lastBlocked != firstBlocked) {
1191 int roundaboutEdgesAhead = 0;
1193 if ((*it) !=
nullptr && (*it)->getEdge().isRoundabout()) {
1194 roundaboutEdgesAhead += 1;
1195 }
else if (roundaboutEdgesAhead > 0) {
1200 int roundaboutEdgesAheadNeigh = 0;
1202 if ((*it) !=
nullptr && (*it)->getEdge().isRoundabout()) {
1203 roundaboutEdgesAheadNeigh += 1;
1204 }
else if (roundaboutEdgesAheadNeigh > 0) {
1209 if (roundaboutEdgesAhead > 1) {
1213 if (roundaboutEdgesAhead > 0) {
1214 #ifdef DEBUG_ROUNDABOUTS
1216 std::cout <<
" roundaboutEdgesAhead=" << roundaboutEdgesAhead <<
" roundaboutEdgesAheadNeigh=" << roundaboutEdgesAheadNeigh <<
"\n";
1221 if (laneOffset != 0) {
1233 roundaboutEdgesAhead,
1238 if ((ret &
LCA_STAY) != 0 && latDist == 0) {
1248 if (changeToBest && abs(bestLaneOffset) > 1
1253 #ifdef DEBUG_WANTSCHANGE
1255 std::cout <<
" reserving space for unseen blockers myLeadingBlockerLength=" <<
myLeadingBlockerLength <<
"\n";
1264 if (*firstBlocked != neighLeadLongest) {
1267 std::vector<CLeaderDist> collectLeadBlockers;
1268 std::vector<CLeaderDist> collectFollowBlockers;
1269 int blockedFully = 0;
1272 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1273 leaders, followers, blockers,
1274 neighLeaders, neighFollowers, neighBlockers, &collectLeadBlockers, &collectFollowBlockers,
1275 false, gapFactor, &blockedFully);
1277 const double absLaneOffset = fabs(bestLaneOffset != 0 ? bestLaneOffset : latDist /
SUMO_const_laneWidth);
1278 const double remainingSeconds = ((ret &
LCA_TRACI) == 0 ?
1281 const double plannedSpeed =
informLeaders(blocked, myLca, collectLeadBlockers, remainingSeconds);
1283 if (plannedSpeed >= 0) {
1285 informFollowers(blocked, myLca, collectFollowBlockers, remainingSeconds, plannedSpeed);
1287 if (plannedSpeed > 0) {
1288 commitManoeuvre(blocked, blockedFully, leaders, neighLeaders, neighLane, maneuverDist);
1290 #if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE)
1297 <<
" remainingSeconds=" << remainingSeconds
1298 <<
" plannedSpeed=" << plannedSpeed
1308 if (roundaboutEdgesAhead > 1) {
1318 latDist = latLaneDist;
1319 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1320 leaders, followers, blockers,
1321 neighLeaders, neighFollowers, neighBlockers);
1336 const double inconvenience = (latLaneDist < 0
1350 && (changeToBest ||
currentDistAllows(neighDist, abs(bestLaneOffset) + 1, laDist))) {
1353 #ifdef DEBUG_COOPERATE
1360 <<
" wantsChangeToHelp=" << (right ?
"right" :
"left")
1370 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1371 leaders, followers, blockers,
1372 neighLeaders, neighFollowers, neighBlockers);
1396 const double vehWidth =
getWidth();
1398 const double leftVehSide = rightVehSide + vehWidth;
1400 double defaultNextSpeed = std::numeric_limits<double>::max();
1402 int leftmostOnEdge = (int)sublaneSides.size() - 1;
1403 while (leftmostOnEdge > 0 && sublaneSides[leftmostOnEdge] > leftVehSide) {
1406 int rightmostOnEdge = leftmostOnEdge;
1407 while (rightmostOnEdge > 0 && sublaneSides[rightmostOnEdge] > rightVehSide +
NUMERICAL_EPS) {
1409 #ifdef DEBUG_WANTSCHANGE
1411 std::cout <<
" adapted to current sublane=" << rightmostOnEdge <<
" defaultNextSpeed=" << defaultNextSpeed <<
"\n";
1412 std::cout <<
" sublaneSides[rightmostOnEdge]=" << sublaneSides[rightmostOnEdge] <<
" rightVehSide=" << rightVehSide <<
"\n";
1418 #ifdef DEBUG_WANTSCHANGE
1420 std::cout <<
" adapted to current sublane=" << rightmostOnEdge <<
" defaultNextSpeed=" << defaultNextSpeed <<
"\n";
1421 std::cout <<
" sublaneSides[rightmostOnEdge]=" << sublaneSides[rightmostOnEdge] <<
" rightVehSide=" << rightVehSide <<
"\n";
1424 double maxGain = -std::numeric_limits<double>::max();
1425 double maxGainRight = -std::numeric_limits<double>::max();
1426 double maxGainLeft = -std::numeric_limits<double>::max();
1427 double latDistNice = std::numeric_limits<double>::max();
1430 const double leftMax =
MAX2(
1433 assert(leftMax <= edge.
getWidth());
1434 int sublaneCompact =
MAX2(iMin, rightmostOnEdge - 1);
1436 #ifdef DEBUG_WANTSCHANGE
1438 <<
" checking sublanes rightmostOnEdge=" << rightmostOnEdge
1439 <<
" leftmostOnEdge=" << leftmostOnEdge
1441 <<
" leftMax=" << leftMax
1442 <<
" sublaneCompact=" << sublaneCompact
1446 for (
int i = iMin; i < (int)sublaneSides.size(); ++i) {
1447 if (sublaneSides[i] + vehWidth < leftMax) {
1453 while (vMin > 0 && j < (
int)sublaneSides.size() && sublaneSides[j] < sublaneSides[i] + vehWidth) {
1459 if (laneOffset != 0 &&
overlap(sublaneSides[i], sublaneSides[i] + vehWidth, laneBoundary, laneBoundary)) {
1463 const double currentLatDist = sublaneSides[i] - rightVehSide;
1465 if (relativeGain > maxGain) {
1466 maxGain = relativeGain;
1469 latDist = currentLatDist;
1470 #ifdef DEBUG_WANTSCHANGE
1472 std::cout <<
" i=" << i <<
" newLatDist=" << latDist <<
" relGain=" << relativeGain <<
"\n";
1478 if (currentLatDist > 0
1483 latDist = currentLatDist;
1486 #ifdef DEBUG_WANTSCHANGE
1488 std::cout <<
" i=" << i <<
" rightmostOnEdge=" << rightmostOnEdge <<
" vMin=" << vMin <<
" relGain=" << relativeGain <<
" sublaneCompact=" << sublaneCompact <<
" curLatDist=" << currentLatDist <<
"\n";
1492 maxGainRight =
MAX2(maxGainRight, relativeGain);
1494 maxGainLeft =
MAX2(maxGainLeft, relativeGain);
1496 const double subAlignDist = sublaneSides[i] - rightVehSide;
1497 if (fabs(subAlignDist) < fabs(latDistNice)) {
1498 latDistNice = subAlignDist;
1499 #ifdef DEBUG_WANTSCHANGE
1501 <<
" nicest sublane=" << i
1502 <<
" side=" << sublaneSides[i]
1503 <<
" rightSide=" << rightVehSide
1504 <<
" latDistNice=" << latDistNice
1505 <<
" maxGainR=" << maxGainRight
1506 <<
" maxGainL=" << maxGainLeft
1513 if (maxGainRight != -std::numeric_limits<double>::max()) {
1514 #ifdef DEBUG_WANTSCHANGE
1520 #ifdef DEBUG_WANTSCHANGE
1526 if (maxGainLeft != -std::numeric_limits<double>::max()) {
1527 #ifdef DEBUG_WANTSCHANGE
1533 #ifdef DEBUG_WANTSCHANGE
1540 if ((fabs(maxGainRight) <
NUMERICAL_EPS || maxGainRight == -std::numeric_limits<double>::max())
1541 && (right || (alternatives &
LCA_RIGHT) == 0)) {
1544 if ((fabs(maxGainLeft) <
NUMERICAL_EPS || maxGainLeft == -std::numeric_limits<double>::max())
1545 && (left || (alternatives &
LCA_LEFT) == 0)) {
1550 #ifdef DEBUG_WANTSCHANGE
1553 <<
" defaultNextSpeed=" << defaultNextSpeed
1554 <<
" maxGain=" << maxGain
1555 <<
" maxGainRight=" << maxGainRight
1556 <<
" maxGainLeft=" << maxGainLeft
1557 <<
" latDist=" << latDist
1558 <<
" latDistNice=" << latDistNice
1559 <<
" sublaneCompact=" << sublaneCompact
1565 if (right && maxGainRight >= 0) {
1572 double fullSpeedDrivingSeconds =
MIN2(acceptanceTime, fullSpeedGap / vMax);
1574 if (neighLead.first != 0 && neighLead.first->getSpeed() < vMax) {
1575 fullSpeedGap =
MAX2(0.,
MIN2(fullSpeedGap,
1577 vMax, neighLead.first->
getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())));
1578 fullSpeedDrivingSeconds =
MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - neighLead.first->getSpeed()));
1583 #ifdef DEBUG_WANTSCHANGE
1586 <<
" considering keepRight:"
1588 <<
" neighDist=" << neighDist
1590 <<
" leaderSpeed=" << (neighLead.first == 0 ? -1 : neighLead.first->getSpeed())
1592 myVehicle.
getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()))
1593 <<
" acceptanceTime=" << acceptanceTime
1594 <<
" fullSpeedGap=" << fullSpeedGap
1595 <<
" fullSpeedDrivingSeconds=" << fullSpeedDrivingSeconds
1596 <<
" dProb=" << deltaProb
1607 latDist = latLaneDist;
1608 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1609 leaders, followers, blockers,
1610 neighLeaders, neighFollowers, neighBlockers);
1616 #ifdef DEBUG_WANTSCHANGE
1621 <<
" neighDist=" << neighDist
1624 <<
" latDist=" << latDist
1633 int blockedFully = 0;
1634 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1635 leaders, followers, blockers,
1636 neighLeaders, neighFollowers, neighBlockers,
1637 nullptr,
nullptr,
false, 0, &blockedFully);
1646 #ifdef DEBUG_WANTSCHANGE
1651 <<
" latDist=" << latDist
1652 <<
" neighDist=" << neighDist
1655 <<
" stayInLane=" << stayInLane
1666 int blockedFully = 0;
1667 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1668 leaders, followers, blockers,
1669 neighLeaders, neighFollowers, neighBlockers,
1670 nullptr,
nullptr,
false, 0, &blockedFully);
1679 double latDistSublane = 0.;
1681 const double halfVehWidth =
getWidth() * 0.5;
1684 && bestLaneOffset == 0
1704 #ifdef DEBUG_WANTSCHANGE
1718 switch (turnInfo.second) {
1738 latDistSublane = -halfLaneWidth + halfVehWidth -
getPosLat();
1741 latDistSublane = halfLaneWidth - halfVehWidth -
getPosLat();
1747 latDistSublane = latDistNice;
1750 latDistSublane = sublaneSides[sublaneCompact] - rightVehSide;
1758 #if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE) || defined(DEBUG_MANEUVER)
1763 <<
" latDist=" << latDist
1764 <<
" latDistSublane=" << latDistSublane
1765 <<
" relGainSublane=" <<
computeSpeedGain(latDistSublane, defaultNextSpeed)
1766 <<
" maneuverDist=" << maneuverDist
1776 #if defined(DEBUG_WANTSCHANGE)
1778 <<
" speedGain=" <<
computeSpeedGain(latDistSublane, defaultNextSpeed) <<
")\n";
1785 && ((myManeuverDist < 0 && latDistSublane > 0) || (
myManeuverDist > 0 && latDistSublane < 0))) {
1786 #if defined(DEBUG_WANTSCHANGE)
1788 std::cout <<
" aborting sublane change due to prior maneuver\n";
1793 latDist = latDistSublane;
1798 #ifdef DEBUG_WANTSCHANGE
1801 <<
" latDist=" << latDist
1809 #ifdef DEBUG_WANTSCHANGE
1815 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1816 leaders, followers, blockers,
1817 neighLeaders, neighFollowers, neighBlockers);
1844 #ifdef DEBUG_WANTSCHANGE
1861 if ((*blocked) !=
nullptr) {
1863 #ifdef DEBUG_SLOWDOWN
1887 (*blocked)->getCarFollowModel().getMaxDecel()));
1898 #ifdef DEBUG_SAVE_BLOCKER_LENGTH
1914 #ifdef DEBUG_SAVE_BLOCKER_LENGTH
1926 #ifdef DEBUG_SAVE_BLOCKER_LENGTH
1932 <<
" potential=" << potential
1948 <<
"vSafe=" << vSafe <<
" -> accel=" << accel <<
"\n";
1958 const MSLane* lane = lanes[laneIndex];
1960 assert(preb.size() == lanes.size());
1962 for (
int sublane = 0; sublane < (int)ahead.
numSublanes(); ++sublane) {
1963 const int edgeSublane = sublane + sublaneOffset;
1971 const MSVehicle* leader = ahead[sublane].first;
1972 const double gap = ahead[sublane].second;
1974 if (leader ==
nullptr) {
1983 #ifdef DEBUG_EXPECTED_SLSPEED
1985 std::cout <<
SIMTIME <<
" veh=" <<
myVehicle.
getID() <<
" updateExpectedSublaneSpeeds edgeSublane=" << edgeSublane <<
" leader=" << leader->
getID() <<
" gap=" << gap <<
" vSafe=" << vSafe <<
"\n";
1988 const double deltaV = vMax - leader->
getSpeed();
1989 if (deltaV > 0 && gap / deltaV < 5) {
1992 const double foreCastTime = 10;
1993 const double gapClosingTime = gap / deltaV;
1994 vSafe = (gapClosingTime * vSafe + (foreCastTime - gapClosingTime) * leader->
getSpeed()) / foreCastTime;
2001 double foeRight, foeLeft;
2005 if (leader.first != 0) {
2008 vSafe =
MIN2(vSafe, vSafePed);
2011 vSafe =
MIN2(vMax, vSafe);
2025 double result = std::numeric_limits<double>::max();
2028 const double vehWidth =
getWidth();
2030 const double leftVehSide = rightVehSide + vehWidth;
2031 for (
int i = 0; i < (int)sublaneSides.size(); ++i) {
2032 if (
overlap(rightVehSide, leftVehSide, sublaneSides[i], sublaneSides[i] + res)) {
2038 return result - defaultNextSpeed;
2045 double maxLength = -1;
2047 if (ldi[i].first != 0) {
2048 const double length = ldi[i].first->getVehicleType().getLength();
2049 if (length > maxLength) {
2062 double minSpeed = std::numeric_limits<double>::max();
2064 if (ldi[i].first != 0) {
2065 const double speed = ldi[i].first->getSpeed();
2066 if (speed < minSpeed) {
2084 std::vector<CLeaderDist>* collectLeadBlockers,
2085 std::vector<CLeaderDist>* collectFollowBlockers,
2086 bool keepLatGapManeuver,
2088 int* retBlockedFully) {
2090 if (!keepLatGapManeuver) {
2092 maneuverDist = latDist;
2095 latDist =
MAX2(
MIN2(latDist, maxDist), -maxDist);
2103 if (laneOffset != 0) {
2114 if (laneOffset != 0) {
2118 #ifdef DEBUG_BLOCKING
2130 }
else if (!forcedTraCIChange) {
2136 }
else if (!forcedTraCIChange) {
2142 #ifdef DEBUG_BLOCKING
2158 if (laneOffset != 0) {
2167 int blockedFully = 0;
2172 if (laneOffset != 0) {
2180 if (retBlockedFully !=
nullptr) {
2181 *retBlockedFully = blockedFully;
2188 blocked |= blockedFully;
2193 if (collectFollowBlockers !=
nullptr && collectLeadBlockers !=
nullptr) {
2195 for (std::vector<CLeaderDist>::const_iterator it2 = collectLeadBlockers->begin(); it2 != collectLeadBlockers->end(); ++it2) {
2196 for (std::vector<CLeaderDist>::iterator it = collectFollowBlockers->begin(); it != collectFollowBlockers->end();) {
2197 if ((*it2).first == (*it).first) {
2198 #ifdef DEBUG_BLOCKING
2200 std::cout <<
" removed follower " << (*it).first->getID() <<
" because it is already a leader\n";
2203 it = collectFollowBlockers->erase(it);
2217 double latDist,
double foeOffset,
bool leaders,
LaneChangeAction blockType,
2218 double& safeLatGapRight,
double& safeLatGapLeft,
2219 std::vector<CLeaderDist>* collectBlockers)
const {
2221 const double vehWidth =
getWidth();
2223 const double leftVehSide = rightVehSide + vehWidth;
2224 const double rightVehSideDest = rightVehSide + latDist;
2225 const double leftVehSideDest = leftVehSide + latDist;
2226 const double rightNoOverlap =
MIN2(rightVehSideDest, rightVehSide);
2227 const double leftNoOverlap =
MAX2(leftVehSideDest, leftVehSide);
2228 #ifdef DEBUG_BLOCKING
2230 std::cout <<
" checkBlockingVehicles"
2231 <<
" latDist=" << latDist
2232 <<
" foeOffset=" << foeOffset
2233 <<
" vehRight=" << rightVehSide
2234 <<
" vehLeft=" << leftVehSide
2235 <<
" rightNoOverlap=" << rightNoOverlap
2236 <<
" leftNoOverlap=" << leftNoOverlap
2237 <<
" destRight=" << rightVehSideDest
2238 <<
" destLeft=" << leftVehSideDest
2239 <<
" leaders=" << leaders
2245 for (
int i = 0; i < vehicles.
numSublanes(); ++i) {
2247 if (vehDist.first != 0 &&
myCFRelated.count(vehDist.first) == 0) {
2248 const MSVehicle* leader = vehDist.first;
2251 std::swap(leader, follower);
2254 double foeRight, foeLeft;
2256 const bool overlapBefore =
overlap(rightVehSide, leftVehSide, foeRight, foeLeft);
2257 const bool overlapDest =
overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft);
2258 const bool overlapAny =
overlap(rightNoOverlap, leftNoOverlap, foeRight, foeLeft);
2259 #ifdef DEBUG_BLOCKING
2261 std::cout <<
" foe=" << vehDist.first->getID()
2262 <<
" gap=" << vehDist.second
2264 <<
" foeRight=" << foeRight
2265 <<
" foeLeft=" << foeLeft
2266 <<
" overlapBefore=" << overlapBefore
2267 <<
" overlap=" << overlapAny
2268 <<
" overlapDest=" << overlapDest
2273 if (vehDist.second < 0) {
2274 if (overlapBefore && !overlapDest) {
2275 #ifdef DEBUG_BLOCKING
2277 std::cout <<
" ignoring current overlap to come clear\n";
2281 #ifdef DEBUG_BLOCKING
2287 if (collectBlockers ==
nullptr) {
2290 collectBlockers->push_back(vehDist);
2306 const double expectedGap =
MSCFModel::gapExtrapolation(timeTillAction, vehDist.second, leader->
getSpeed(), follower->
getSpeed(), leaderAccel, followerAccel, std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
2309 const double followerExpectedSpeed = follower->
getSpeed() + timeTillAction * followerAccel;
2310 const double leaderExpectedSpeed =
MAX2(0., leader->
getSpeed() + timeTillAction * leaderAccel);
2313 #if defined(DEBUG_ACTIONSTEPS) && defined(DEBUG_BLOCKING)
2315 std::cout <<
" timeTillAction=" << timeTillAction
2316 <<
" followerAccel=" << followerAccel
2317 <<
" followerExpectedSpeed=" << followerExpectedSpeed
2318 <<
" leaderAccel=" << leaderAccel
2319 <<
" leaderExpectedSpeed=" << leaderExpectedSpeed
2320 <<
"\n gap=" << vehDist.second
2321 <<
" gapChange=" << (expectedGap - vehDist.second)
2322 <<
" expectedGap=" << expectedGap
2323 <<
" expectedSecureGap=" << expectedSecureGap
2324 <<
" safeLatGapLeft=" << safeLatGapLeft
2325 <<
" safeLatGapRight=" << safeLatGapRight
2332 if (expectedGap < secureGap2) {
2334 if (foeRight > leftVehSide) {
2335 safeLatGapLeft =
MIN2(safeLatGapLeft, foeRight - leftVehSide);
2336 }
else if (foeLeft < rightVehSide) {
2337 safeLatGapRight =
MIN2(safeLatGapRight, rightVehSide - foeLeft);
2340 #ifdef DEBUG_BLOCKING
2342 std::cout <<
" blocked by " << vehDist.first->getID() <<
" gap=" << vehDist.second <<
" expectedGap=" << expectedGap
2343 <<
" expectedSecureGap=" << expectedSecureGap <<
" secGap2=" << secureGap2 <<
" safetyFactor=" <<
getSafetyFactor()
2344 <<
" safeLatGapLeft=" << safeLatGapLeft <<
" safeLatGapRight=" << safeLatGapRight
2348 result |= blockType;
2349 if (collectBlockers ==
nullptr) {
2352 #ifdef DEBUG_BLOCKING
2353 }
else if (
gDebugFlag2 && expectedGap < expectedSecureGap) {
2354 std::cout <<
" ignore blocker " << vehDist.first->getID() <<
" gap=" << vehDist.second <<
" expectedGap=" << expectedGap
2355 <<
" expectedSecureGap=" << expectedSecureGap <<
" secGap2=" << secureGap2 <<
" safetyFactor=" <<
getSafetyFactor() <<
"\n";
2358 if (collectBlockers !=
nullptr) {
2361 collectBlockers->push_back(vehDist);
2378 const double leftVehSide = rightVehSide + vehWidth;
2379 #ifdef DEBUG_BLOCKING
2381 std::cout <<
" updateCFRelated foeOffset=" << foeOffset <<
" vehicles=" << vehicles.
toString() <<
"\n";
2384 for (
int i = 0; i < vehicles.
numSublanes(); ++i) {
2386 if (vehDist.first != 0 &&
myCFRelated.count(vehDist.first) == 0) {
2387 double foeRight, foeLeft;
2389 if (
overlap(rightVehSide, leftVehSide, foeRight, foeLeft) && (vehDist.second >= 0
2395 && -vehDist.second < vehDist.first->getVehicleType().getMinGap()
2398 #ifdef DEBUG_BLOCKING
2400 std::cout <<
" ignoring cfrelated foe=" << vehDist.first->getID() <<
" gap=" << vehDist.second
2402 <<
" foeOffset=" << foeOffset
2403 <<
" egoR=" << rightVehSide <<
" egoL=" << leftVehSide
2404 <<
" iR=" << foeRight <<
" iL=" << foeLeft
2419 assert(right <= left);
2420 assert(right2 <= left2);
2442 return changeReason;
2449 if (sd1.
state == 0) {
2451 }
else if (sd2.
state == 0) {
2461 #ifdef DEBUG_WANTSCHANGE
2467 <<
" dir1=" << sd1.
dir
2471 <<
" dir2=" << sd2.
dir
2472 <<
" reason1=" << reason1
2473 <<
" reason2=" << reason2
2479 if (reason1 < reason2) {
2481 return (!can1 && can2 && sd1.
sameDirection(sd2)) ? sd2 : sd1;
2483 }
else if (reason1 > reason2) {
2485 return (!can2 && can1 && sd1.
sameDirection(sd2)) ? sd1 : sd2;
2493 }
else if (sd2.
dir == 0) {
2498 assert(sd1.
dir == -1);
2499 assert(sd2.
dir == 1);
2502 }
else if (sd2.
latDist >= 0) {
2510 return can1 ? sd1 : sd2;
2533 const std::vector<MSVehicle::LaneQ>& preb,
2542 int roundaboutEdgesAhead,
2546 const bool right = (laneOffset == -1);
2547 const bool left = (laneOffset == 1);
2559 #ifdef DEBUG_STRATEGIC_CHANGE
2564 <<
" laDist=" << laDist
2565 <<
" currentDist=" << currentDist
2566 <<
" usableDist=" << usableDist
2567 <<
" bestLaneOffset=" << bestLaneOffset
2568 <<
" best.length=" << best.
length
2569 <<
" maxJam=" << maxJam
2570 <<
" neighLeftPlace=" << neighLeftPlace
2576 if (laneOffset != 0 && changeToBest && bestLaneOffset == curr.
bestLaneOffset
2579 latDist = latLaneDist;
2596 #ifdef DEBUG_STRATEGIC_CHANGE
2599 <<
" avoid overtaking on the right nv=" << nv->
getID()
2609 if (!changeToBest && (
currentDistDisallows(neighLeftPlace, abs(bestLaneOffset) + 2, laDist))) {
2616 #ifdef DEBUG_STRATEGIC_CHANGE
2618 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" could not change back and forth in time (1) neighLeftPlace=" << neighLeftPlace <<
"\n";
2624 && bestLaneOffset == 0
2627 && roundaboutEdgesAhead == 0
2632 #ifdef DEBUG_STRATEGIC_CHANGE
2634 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" does not want to leave the bestLane (neighDist=" << neighDist <<
")\n";
2639 && bestLaneOffset == 0
2645 #ifdef DEBUG_STRATEGIC_CHANGE
2647 std::cout <<
" veh=" <<
myVehicle.
getID() <<
" does not want to get stranded on the on-ramp of a highway\n";
2661 MSLane* shadowPrev =
nullptr;
2663 if (*it ==
nullptr) {
2667 if (shadow ==
nullptr || currentShadowDist >= requiredDist) {
2670 if (shadowPrev !=
nullptr) {
2673 currentShadowDist += shadow->
getLength();
2674 shadowPrev = shadow;
2675 #ifdef DEBUG_STRATEGIC_CHANGE
2677 std::cout <<
" shadow=" << shadow->
getID() <<
" currentShadowDist=" << currentShadowDist <<
"\n";
2681 #ifdef DEBUG_STRATEGIC_CHANGE
2686 if (currentShadowDist < requiredDist && currentShadowDist < usableDist) {
2689 #ifdef DEBUG_STRATEGIC_CHANGE
2691 std::cout <<
" must change for shadowLane end latDist=" << latDist <<
" myLeftSpace=" <<
myLeftSpace <<
"\n";
2699 #if defined(DEBUG_STRATEGIC_CHANGE) || defined(DEBUG_TRACI)
2711 }
else if (((retTraCI &
LCA_RIGHT) != 0 && laneOffset < 0)
2712 || ((retTraCI &
LCA_LEFT) != 0 && laneOffset > 0)) {
2714 latDist = latLaneDist;
2717 #if defined(DEBUG_STRATEGIC_CHANGE) || defined(DEBUG_TRACI)
2719 std::cout <<
" reqAfterInfluence=" << ret <<
" ret=" << ret <<
"\n";
2743 double& maneuverDist,
2779 const bool stayInLane = laneOffset == 0 || ((state &
LCA_STRATEGIC) != 0 && (state &
LCA_STAY) != 0);
2780 const double oldLatDist = latDist;
2781 const double oldManeuverDist = maneuverDist;
2784 const double halfWidth =
getWidth() * 0.5;
2790 double surplusGapRight = oldCenter - halfWidth;
2792 #ifdef DEBUG_KEEP_LATGAP
2794 std::cout <<
"\n " <<
SIMTIME <<
" keepLatGap() laneOffset=" << laneOffset
2795 <<
" latDist=" << latDist
2796 <<
" maneuverDist=" << maneuverDist
2799 <<
" gapFactor=" << gapFactor
2800 <<
" stayInLane=" << stayInLane <<
"\n"
2801 <<
" stayInEdge: surplusGapRight=" << surplusGapRight <<
" surplusGapLeft=" << surplusGapLeft <<
"\n";
2805 if (surplusGapLeft < 0 || surplusGapRight < 0) {
2815 if (laneOffset != 0) {
2820 #ifdef DEBUG_KEEP_LATGAP
2822 std::cout <<
" minGapLat: surplusGapRight=" << surplusGapRight <<
" surplusGapLeft=" << surplusGapLeft <<
"\n"
2831 if (stayInLane || laneOffset == 1) {
2837 if (stayInLane || laneOffset == -1) {
2843 #ifdef DEBUG_KEEP_LATGAP
2845 std::cout <<
" stayInLane: surplusGapRight=" << surplusGapRight <<
" surplusGapLeft=" << surplusGapLeft <<
"\n";
2849 if (surplusGapRight + surplusGapLeft < 0) {
2854 const double equalDeficit = 0.5 * (surplusGapLeft + surplusGapRight);
2855 if (surplusGapRight < surplusGapLeft) {
2857 const double delta =
MIN2(equalDeficit - surplusGapRight, physicalGapLeft);
2859 maneuverDist = delta;
2860 #ifdef DEBUG_KEEP_LATGAP
2862 std::cout <<
" insufficient latSpace, move left: delta=" << delta <<
"\n";
2867 const double delta =
MIN2(equalDeficit - surplusGapLeft, physicalGapRight);
2869 maneuverDist = -delta;
2870 #ifdef DEBUG_KEEP_LATGAP
2872 std::cout <<
" insufficient latSpace, move right: delta=" << delta <<
"\n";
2878 latDist =
MAX2(
MIN2(latDist, surplusGapLeft), -surplusGapRight);
2879 maneuverDist =
MAX2(
MIN2(maneuverDist, surplusGapLeft), -surplusGapRight);
2880 if ((state &
LCA_KEEPRIGHT) != 0 && maneuverDist != oldManeuverDist) {
2882 latDist = oldLatDist;
2883 maneuverDist = oldManeuverDist;
2885 #ifdef DEBUG_KEEP_LATGAP
2887 std::cout <<
" adapted latDist=" << latDist <<
" maneuverDist=" << maneuverDist <<
" (old=" << oldLatDist <<
")\n";
2897 #ifdef DEBUG_KEEP_LATGAP
2899 std::cout <<
" traci influenced latDist=" << latDist <<
"\n";
2905 const bool traciChange = (state &
LCA_TRACI) != 0;
2906 if (nonSublaneChange && !traciChange) {
2908 #ifdef DEBUG_KEEP_LATGAP
2910 std::cout <<
" wanted changeToLeft oldLatDist=" << oldLatDist <<
", blocked latGap changeToRight\n";
2913 latDist = oldLatDist;
2916 #ifdef DEBUG_KEEP_LATGAP
2918 std::cout <<
" wanted changeToRight oldLatDist=" << oldLatDist <<
", blocked latGap changeToLeft\n";
2921 latDist = oldLatDist;
2931 #ifdef DEBUG_KEEP_LATGAP
2933 std::cout <<
" latDistUpdated=" << latDist <<
" oldLatDist=" << oldLatDist <<
"\n";
2936 blocked =
checkBlocking(neighLane, latDist, maneuverDist, laneOffset, leaders, followers, blockers, neighLeaders, neighFollowers, neighBlockers,
nullptr,
nullptr, nonSublaneChange);
2950 #if defined(DEBUG_KEEP_LATGAP) || defined(DEBUG_STATE)
2952 std::cout <<
" latDist2=" << latDist
2966 double& surplusGapRight,
double& surplusGapLeft,
2967 bool saveMinGap,
double netOverlap,
2969 std::vector<CLeaderDist>* collectBlockers) {
2974 if (others[i].first != 0 && others[i].second <= 0
2976 && (netOverlap == 0 || others[i].second + others[i].first->getVehicleType().getMinGap() < netOverlap)) {
2980 double foeRight, foeLeft;
2982 const double foeCenter = foeRight + 0.5 * res;
2983 const double gap =
MIN2(fabs(foeRight - oldCenter), fabs(foeLeft - oldCenter)) - halfWidth;
2986 const double currentMinGap = desiredMinGap * gapFactor;
2997 #if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
2999 std::cout <<
" updateGaps"
3001 <<
" foe=" << foe->
getID()
3002 <<
" foeRight=" << foeRight
3003 <<
" foeLeft=" << foeLeft
3004 <<
" oldCenter=" << oldCenter
3005 <<
" gap=" << others[i].second
3006 <<
" latgap=" << gap
3007 <<
" currentMinGap=" << currentMinGap
3008 <<
" surplusGapRight=" << surplusGapRight
3009 <<
" surplusGapLeft=" << surplusGapLeft
3017 if (foeCenter < oldCenter) {
3019 surplusGapRight =
MIN3(surplusGapRight, gap - currentMinGap,
MAX2(currentMinGap, gap - foeManeuverDist));
3022 surplusGapLeft =
MIN3(surplusGapLeft, gap - currentMinGap,
MAX2(currentMinGap, gap - foeManeuverDist));
3025 if (foeCenter < oldCenter) {
3026 #if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
3028 std::cout <<
" new minimum rightGap=" << gap <<
"\n";
3033 #if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
3035 std::cout <<
" new minimum leftGap=" << gap <<
"\n";
3041 if (collectBlockers !=
nullptr) {
3043 if ((foeCenter < oldCenter && latDist < 0 && gap < (desiredMinGap - latDist))
3044 || (foeCenter > oldCenter && latDist > 0 && gap < (desiredMinGap + latDist))) {
3045 collectBlockers->push_back(others[i]);
3062 int currentDirection =
mySpeedLat >= 0 ? 1 : -1;
3063 int directionWish = latDist >= 0 ? 1 : -1;
3067 #ifdef DEBUG_MANEUVER
3071 <<
" computeSpeedLat()"
3072 <<
" currentDirection=" << currentDirection
3073 <<
" directionWish=" << directionWish
3079 if (directionWish == 1) {
3090 double speedAccelSafe = latDist * speedAccel >= 0 ? speedAccel : 0;
3098 if (maneuverDist * latDist > 0) {
3099 maneuverDist = fullLatDist;
3102 #ifdef DEBUG_MANEUVER
3107 <<
" latDist=" << latDist
3108 <<
" maneuverDist=" << maneuverDist
3111 <<
" fullLatDist=" << fullLatDist
3112 <<
" speedAccel=" << speedAccel
3113 <<
" speedDecel=" << speedDecel
3114 <<
" speedBound=" << speedBound
3118 if (speedDecel * speedAccel <= 0 && (
3120 (latDist >= 0 && speedAccel >= speedBound && speedBound >= speedDecel)
3121 || (latDist <= 0 && speedAccel <= speedBound && speedBound <= speedDecel))) {
3123 #ifdef DEBUG_MANEUVER
3125 std::cout <<
" computeSpeedLat a)\n";
3132 #ifdef DEBUG_MANEUVER
3134 std::cout <<
" computeSpeedLat b)\n";
3137 return speedAccelSafe;
3141 if ((fabs(minDistAccel) < fabs(fullLatDist)) || (fabs(minDistAccel - fullLatDist) <
NUMERICAL_EPS)) {
3142 #ifdef DEBUG_MANEUVER
3144 std::cout <<
" computeSpeedLat c)\n";
3149 #ifdef DEBUG_MANEUVER
3151 std::cout <<
" minDistAccel=" << minDistAccel <<
"\n";
3156 if ((fabs(minDistCurrent) < fabs(fullLatDist)) || (fabs(minDistCurrent - fullLatDist) <
NUMERICAL_EPS)) {
3157 #ifdef DEBUG_MANEUVER
3159 std::cout <<
" computeSpeedLat d)\n";
3166 #ifdef DEBUG_MANEUVER
3168 std::cout <<
" computeSpeedLat e)\n";
3171 return speedDecelSafe;
3180 double maneuverDist) {
3183 double secondsToLeaveLane;
3193 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3209 double nextLeftSpace;
3210 if (nextActionStepSpeed > 0.) {
3225 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3229 <<
" avoidArrivalSpeed=" << avoidArrivalSpeed
3232 <<
"\n nextLeftSpace=" << nextLeftSpace
3233 <<
" nextActionStepSpeed=" << nextActionStepSpeed
3234 <<
" nextActionStepRemainingSeconds=" << secondsToLeaveLane - timeTillActionStep
3244 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3248 <<
" secondsToLeave=" << secondsToLeaveLane
3270 const double vehWidth =
getWidth();
3272 const double leftVehSide = rightVehSide + vehWidth;
3273 const double rightVehSideDest = rightVehSide + latDist;
3274 const double leftVehSideDest = leftVehSide + latDist;
3275 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3277 std::cout <<
" commitFollowSpeed"
3278 <<
" latDist=" << latDist
3279 <<
" foeOffset=" << foeOffset
3280 <<
" vehRight=" << rightVehSide
3281 <<
" vehLeft=" << leftVehSide
3282 <<
" destRight=" << rightVehSideDest
3283 <<
" destLeft=" << leftVehSideDest
3289 if (vehDist.first != 0) {
3290 const MSVehicle* leader = vehDist.first;
3292 double foeRight, foeLeft;
3294 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3296 std::cout <<
" foe=" << vehDist.first->getID()
3297 <<
" gap=" << vehDist.second
3299 <<
" foeRight=" << foeRight
3300 <<
" foeLeft=" << foeLeft
3301 <<
" overlapBefore=" <<
overlap(rightVehSide, leftVehSide, foeRight, foeLeft)
3302 <<
" overlapDest=" <<
overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft)
3306 if (
overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft)) {
3310 speed =
MIN2(speed, vSafe);
3311 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3313 std::cout <<
" case1 vsafe=" << vSafe <<
" speed=" << speed <<
"\n";
3316 }
else if (
overlap(rightVehSide, leftVehSide, foeRight, foeLeft)) {
3321 speed =
MIN2(speed, vSafe);
3322 #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3324 std::cout <<
" case2 vsafe=" << vSafe <<
" speed=" << speed <<
"\n";
3428 const std::pair<MSVehicle*, double>& leader,
3429 const std::pair<MSVehicle*, double>& neighLead,
3430 const std::pair<MSVehicle*, double>& neighFollow,
3432 const std::vector<MSVehicle::LaneQ>& preb,
3438 #ifdef DEBUG_WANTSCHANGE
3440 std::cout <<
"\nWANTS_CHANGE\n" <<
SIMTIME
3447 <<
" considerChangeTo=" << (laneOffset == -1 ?
"right" :
"left")
3461 double maneuverDist;
3464 leaders, followers, blockers,
3465 neighLeaders, neighFollowers, neighBlockers,
3467 lastBlocked, firstBlocked, latDist, maneuverDist, blocked);
3473 result |=
getLCA(result, latDist);
3475 #if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE)
3480 <<
" wantsChangeTo=" << (laneOffset == -1 ?
"right" :
"left")
3481 << ((result &
LCA_URGENT) ?
" (urgent)" :
"")
3487 << ((result &
LCA_TRACI) ?
" (traci)" :
"")