Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2025 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file MSLCM_LC2013.cpp
15 : /// @author Daniel Krajzewicz
16 : /// @author Jakob Erdmann
17 : /// @author Friedemann Wesner
18 : /// @author Sascha Krieg
19 : /// @author Michael Behrisch
20 : /// @author Laura Bieker
21 : /// @author Leonhard Luecken
22 : /// @date Fri, 08.10.2013
23 : ///
24 : // A lane change model developed by J. Erdmann
25 : // based on the model of D. Krajzewicz developed between 2004 and 2011 (MSLCM_DK2004)
26 : /****************************************************************************/
27 : #include <config.h>
28 :
29 : #include <iostream>
30 : #include <utils/common/RandHelper.h>
31 : #include <utils/common/StringUtils.h>
32 : #include <microsim/transportables/MSPModel.h>
33 : #include <microsim/transportables/MSTransportableControl.h>
34 : #include <microsim/MSEdge.h>
35 : #include <microsim/MSLane.h>
36 : #include <microsim/MSLink.h>
37 : #include <microsim/MSDriverState.h>
38 : #include <microsim/MSNet.h>
39 : #include <microsim/MSStop.h>
40 : #include "MSLCHelper.h"
41 : #include "MSLCM_LC2013.h"
42 :
43 :
44 : // ===========================================================================
45 : // variable definitions
46 : // ===========================================================================
47 : #define LOOK_FORWARD 10.
48 :
49 : #define JAM_FACTOR 1.
50 :
51 : #define LCA_RIGHT_IMPATIENCE -1.
52 : #define CUT_IN_LEFT_SPEED_THRESHOLD 27.
53 :
54 : #define LOOK_AHEAD_MIN_SPEED 0.0
55 : #define LOOK_AHEAD_SPEED_MEMORY 0.9
56 :
57 : #define HELP_DECEL_FACTOR 1.0
58 :
59 : #define HELP_OVERTAKE (10.0 / 3.6)
60 : #define MIN_FALLBEHIND (7.0 / 3.6)
61 :
62 : #define RELGAIN_NORMALIZATION_MIN_SPEED 10.0
63 : #define URGENCY 2.0
64 : #define OPPOSITE_URGENCY 5.0
65 :
66 : #define KEEP_RIGHT_TIME 5.0 // the number of seconds after which a vehicle should move to the right lane
67 :
68 : #define KEEP_RIGHT_HEADWAY 2.0
69 : #define MAX_ONRAMP_LENGTH 200.
70 : #define TURN_LANE_DIST 200.0 // the distance at which a lane leading elsewhere is considered to be a turn-lane that must be avoided
71 :
72 : #define LC_RESOLUTION_SPEED_LAT 0.5 // the lateral speed (in m/s) for a standing vehicle which was unable to finish a continuous LC in time (in case mySpeedLatStanding==0), see #3771
73 :
74 : #define REACT_TO_STOPPED_DISTANCE 100
75 : #define BLOCKER_IS_BLOCKED_TIME_THRESHOLD 5 // the time after which a blocking neighbor is treated similar to a stopped vehicle
76 :
77 : // ===========================================================================
78 : // debug defines
79 : // ===========================================================================
80 : //#define DEBUG_CONSTRUCTOR
81 : //#define DEBUG_PATCH_SPEED
82 : //#define DEBUG_INFORMED
83 : //#define DEBUG_INFORMER
84 : //#define DEBUG_WANTS_CHANGE
85 : //#define DEBUG_SLOW_DOWN
86 : //#define DEBUG_COOPERATE
87 : //#define DEBUG_SAVE_BLOCKER_LENGTH
88 :
89 : //#define DEBUG_COND (myVehicle.getID() == "disabled")
90 : #define DEBUG_COND (myVehicle.isSelected())
91 : //#define DEBUG_COND (true)
92 :
93 : // ===========================================================================
94 : // member method definitions
95 : // ===========================================================================
96 3779305 : MSLCM_LC2013::MSLCM_LC2013(MSVehicle& v) :
97 : MSAbstractLaneChangeModel(v, LaneChangeModel::LC2013),
98 3779305 : mySpeedGainProbabilityLeft(0),
99 3779305 : mySpeedGainProbabilityRight(0),
100 3779305 : myKeepRightProbability(0),
101 3779305 : myLeadingBlockerLength(0),
102 3779305 : myLeftSpace(0),
103 3779305 : myLookAheadSpeed(LOOK_AHEAD_MIN_SPEED),
104 3779305 : myStrategicParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_STRATEGIC_PARAM, 1)),
105 3779305 : myCooperativeParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_COOPERATIVE_PARAM, 1)),
106 3779305 : mySpeedGainParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAIN_PARAM, 1)),
107 3779305 : myKeepRightParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_KEEPRIGHT_PARAM, 1)),
108 3779305 : myOppositeParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_OPPOSITE_PARAM, 1)),
109 3779305 : myLookaheadLeft(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_LOOKAHEADLEFT, 2.0)),
110 3779305 : mySpeedGainRight(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAINRIGHT, 0.1)),
111 3779305 : mySpeedGainLookahead(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAIN_LOOKAHEAD, 0)),
112 3779305 : mySpeedGainRemainTime(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAIN_REMAIN_TIME, 20)),
113 3779305 : mySpeedGainUrgency(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAIN_URGENCY, 50)),
114 3779305 : myRoundaboutBonus(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_COOPERATIVE_ROUNDABOUT, myCooperativeParam)),
115 3779305 : myCooperativeSpeed(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_COOPERATIVE_SPEED, myCooperativeParam)),
116 3779305 : myKeepRightAcceptanceTime(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_KEEPRIGHT_ACCEPTANCE_TIME, -1)),
117 3779305 : myOvertakeDeltaSpeedFactor(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_OVERTAKE_DELTASPEED_FACTOR, 0)),
118 7558610 : myExperimentalParam1(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_EXPERIMENTAL1, 0)) {
119 3779305 : initDerivedParameters();
120 : #ifdef DEBUG_CONSTRUCTOR
121 : if (DEBUG_COND) {
122 : std::cout << SIMTIME
123 : << " create lcModel veh=" << myVehicle.getID()
124 : << " lcStrategic=" << myStrategicParam
125 : << " lcCooperative=" << myCooperativeParam
126 : << " lcSpeedGain=" << mySpeedGainParam
127 : << " lcKeepRight=" << myKeepRightParam
128 : << "\n";
129 : }
130 : #endif
131 3779305 : }
132 :
133 7558450 : MSLCM_LC2013::~MSLCM_LC2013() {
134 3779225 : changed();
135 7558450 : }
136 :
137 :
138 : void
139 3788301 : MSLCM_LC2013::initDerivedParameters() {
140 3788301 : if (mySpeedGainParam <= 0) {
141 7096 : myChangeProbThresholdRight = std::numeric_limits<double>::max();
142 7096 : myChangeProbThresholdLeft = std::numeric_limits<double>::max();
143 : } else {
144 3781205 : myChangeProbThresholdRight = (0.2 / mySpeedGainRight) / mySpeedGainParam;
145 3781205 : myChangeProbThresholdLeft = 0.2 / mySpeedGainParam;
146 : }
147 3788301 : }
148 :
149 :
150 : bool
151 3406 : MSLCM_LC2013::debugVehicle() const {
152 3406 : return DEBUG_COND;
153 : }
154 :
155 :
156 : int
157 235400955 : MSLCM_LC2013::wantsChange(
158 : int laneOffset,
159 : MSAbstractLaneChangeModel::MSLCMessager& msgPass,
160 : int blocked,
161 : const std::pair<MSVehicle*, double>& leader,
162 : const std::pair<MSVehicle*, double>& follower,
163 : const std::pair<MSVehicle*, double>& neighLead,
164 : const std::pair<MSVehicle*, double>& neighFollow,
165 : const MSLane& neighLane,
166 : const std::vector<MSVehicle::LaneQ>& preb,
167 : MSVehicle** lastBlocked,
168 : MSVehicle** firstBlocked) {
169 :
170 : #ifdef DEBUG_WANTS_CHANGE
171 : if (DEBUG_COND) {
172 : std::cout << "\nWANTS_CHANGE\n" << SIMTIME
173 : << std::setprecision(gPrecision)
174 : << " veh=" << myVehicle.getID()
175 : << " lane=" << myVehicle.getLane()->getID()
176 : << " pos=" << myVehicle.getPositionOnLane()
177 : << " posLat=" << myVehicle.getLateralPositionOnLane()
178 : << " speed=" << myVehicle.getSpeed()
179 : << " considerChangeTo=" << (laneOffset == -1 ? "right" : "left")
180 : << "\n";
181 : }
182 : #endif
183 :
184 235400955 : const int result = _wantsChange(laneOffset, msgPass, blocked, leader, follower, neighLead, neighFollow, neighLane, preb, *lastBlocked, *firstBlocked);
185 :
186 : #ifdef DEBUG_WANTS_CHANGE
187 : if (DEBUG_COND) {
188 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " result=" << toString((LaneChangeAction)result) << " blocked=" << toString((LaneChangeAction)blocked) << "\n\n\n";
189 : }
190 : #endif
191 :
192 235400955 : return result;
193 : }
194 :
195 :
196 : double
197 534840602 : MSLCM_LC2013::patchSpeed(const double min, const double wanted, const double max, const MSCFModel& cfModel) {
198 :
199 : #ifdef DEBUG_PATCH_SPEED
200 : if (DEBUG_COND) {
201 : std::cout << "\nPATCH_SPEED\n"
202 : << SIMTIME
203 : << " veh=" << myVehicle.getID()
204 : << " lane=" << myVehicle.getLane()->getID()
205 : << " pos=" << myVehicle.getPositionOnLane()
206 : << " v=" << myVehicle.getSpeed()
207 : << " min=" << min
208 : << " wanted=" << wanted
209 : << " max=" << max
210 : << "\n";
211 : }
212 : #endif
213 :
214 : // negative min speed may be passed when using ballistic updated
215 534840602 : const double newSpeed = _patchSpeed(MAX2(min, 0.0), wanted, max, cfModel);
216 :
217 : #ifdef DEBUG_PATCH_SPEED
218 : if (DEBUG_COND) {
219 : const std::string patched = (wanted != newSpeed ? " patched=" + toString(newSpeed) : "");
220 : std::cout << patched
221 : << "\n";
222 : }
223 : #endif
224 :
225 534840602 : return newSpeed;
226 : }
227 :
228 :
229 : double
230 534840602 : MSLCM_LC2013::_patchSpeed(double min, const double wanted, double max, const MSCFModel& cfModel) {
231 534840602 : int state = myOwnState;
232 : #ifdef DEBUG_PATCH_SPEED
233 : if (DEBUG_COND) {
234 : std::cout
235 : << "\n" << SIMTIME << std::setprecision(gPrecision)
236 : << " patchSpeed state=" << toString((LaneChangeAction)state) << " myLCAccelerationAdvices=" << toString(myLCAccelerationAdvices)
237 : << "\n speed=" << myVehicle.getSpeed() << " min=" << min << " wanted=" << wanted
238 : << "\n myLeadingBlockerLength=" << myLeadingBlockerLength
239 : << "\n";
240 : }
241 : #endif
242 :
243 : // letting vehicles merge in at the end of the lane in case of counter-lane change, step#2
244 : double nVSafe = wanted;
245 : bool gotOne = false;
246 : // if we want to change and have a blocking leader and there is enough room for him in front of us
247 534840602 : if (myLeadingBlockerLength != 0) {
248 487495 : double space = myLeftSpace - myLeadingBlockerLength - POSITION_EPS;
249 : #ifdef DEBUG_PATCH_SPEED
250 : if (DEBUG_COND) {
251 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " myLeftSpace=" << myLeftSpace << " myLeadingBlockerLength=" << myLeadingBlockerLength << " space=" << space << "\n";
252 : }
253 : #endif
254 487495 : if (space > 0 && (myVehicle.getLane()->isNormal() || myVehicle.getCurrentEdge()->isRoundabout())) {
255 : // compute speed for decelerating towards a place which allows the blocking leader to merge in in front
256 358827 : const double vMinEmergency = myVehicle.getCarFollowModel().minNextSpeedEmergency(myVehicle.getSpeed(), &myVehicle);
257 358827 : double safe = cfModel.stopSpeed(&myVehicle, myVehicle.getSpeed(), space, MSCFModel::CalcReason::LANE_CHANGE);
258 : max = MIN2(max, MAX2(safe, vMinEmergency));
259 : // if we are approaching this place
260 358827 : if (safe < wanted) {
261 : // return this speed as the speed to use
262 44213 : if (safe < min) {
263 4692 : if (safe >= vMinEmergency) {
264 : // permit harder braking if needed and helpful
265 : min = MAX2(vMinEmergency, safe);
266 : }
267 : }
268 : #ifdef DEBUG_PATCH_SPEED
269 : if (DEBUG_COND) {
270 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " slowing down for leading blocker, safe=" << safe << (safe + NUMERICAL_EPS < min ? " (not enough)" : "") << "\n";
271 : }
272 : #endif
273 : nVSafe = MAX2(min, safe);
274 : gotOne = true;
275 : }
276 : }
277 : }
278 :
279 534840602 : const double coopWeight = MAX2(0.0, MIN2(1.0, myCooperativeSpeed));
280 556601431 : for (auto i : myLCAccelerationAdvices) {
281 : double a = i.first;
282 21760829 : double v = myVehicle.getSpeed() + ACCEL2SPEED(a);
283 :
284 21760829 : if (v >= min && v <= max && (MSGlobals::gSemiImplicitEulerUpdate
285 : // ballistic update: (negative speeds may appear, e.g. min<0, v<0), BUT:
286 : // XXX: LaneChanging returns -1 to indicate no restrictions, which leads to probs here (Leo), refs. #2577
287 : // As a quick fix, we just dismiss cases where v=-1
288 : // VERY rarely (whenever a requested help-acceleration is really indicated by v=-1)
289 : // this can lead to failing lane-change attempts, though)
290 256557 : || v != -1)) {
291 5251516 : if (i.second) {
292 : // own advice, no scaling needed
293 : nVSafe = MIN2(v, nVSafe);
294 : } else {
295 2119979 : nVSafe = MIN2(v * coopWeight + (1 - coopWeight) * wanted, nVSafe);
296 : }
297 : gotOne = true;
298 : #ifdef DEBUG_PATCH_SPEED
299 : if (DEBUG_COND) {
300 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " got nVSafe=" << nVSafe << " isOwn: " << i.second << " rawV=" << v << "\n";
301 : }
302 : #endif
303 : } else {
304 : if (v < min) {
305 : #ifdef DEBUG_PATCH_SPEED
306 : if (DEBUG_COND) {
307 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " ignoring low nVSafe=" << v << " min=" << min << "\n";
308 : }
309 : #endif
310 : } else {
311 : #ifdef DEBUG_PATCH_SPEED
312 : if (DEBUG_COND) {
313 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " ignoring high nVSafe=" << v << " max=" << max << "\n";
314 : }
315 : #endif
316 : }
317 : }
318 : }
319 : // myDontBrake is used in counter-lane-change situations with relief connection
320 534840602 : if (gotOne && !myDontBrake) {
321 : #ifdef DEBUG_PATCH_SPEED
322 : if (DEBUG_COND) {
323 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " got vSafe\n";
324 : }
325 : #endif
326 : return nVSafe;
327 : }
328 :
329 : // check whether the vehicle is blocked
330 530368470 : if ((state & LCA_WANTS_LANECHANGE) != 0 && (state & LCA_BLOCKED) != 0) {
331 13617607 : if ((state & LCA_STRATEGIC) != 0) {
332 : // necessary decelerations are controlled via vSafe. If there are
333 : // none it means we should speed up
334 : #ifdef DEBUG_PATCH_SPEED
335 : if (DEBUG_COND) {
336 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_WANTS_LANECHANGE (strat, no vSafe)\n";
337 : }
338 : #endif
339 2734107 : return (max + wanted) / 2.0;
340 10883500 : } else if ((state & LCA_COOPERATIVE) != 0) {
341 : // only minor adjustments in speed should be done
342 752387 : if ((state & LCA_BLOCKED_BY_LEADER) != 0) {
343 : #ifdef DEBUG_PATCH_SPEED
344 : if (DEBUG_COND) {
345 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_LEADER (coop)\n";
346 : }
347 : #endif
348 606231 : if (wanted >= 0.) {
349 606231 : return (MAX2(0., min) + wanted) / 2.0;
350 : } else {
351 : return wanted;
352 : }
353 : }
354 146156 : if ((state & LCA_BLOCKED_BY_FOLLOWER) != 0) {
355 : #ifdef DEBUG_PATCH_SPEED
356 : if (DEBUG_COND) {
357 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_FOLLOWER (coop)\n";
358 : }
359 : #endif
360 146156 : return (max + wanted) / 2.0;
361 : }
362 : //} else { // VARIANT_16
363 : // // only accelerations should be performed
364 : // if ((state & LCA_BLOCKED_BY_FOLLOWER) != 0) {
365 : // if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_FOLLOWER\n";
366 : // return (max + wanted) / 2.0;
367 : // }
368 : }
369 : }
370 :
371 : /*
372 : // decelerate if being a blocking follower
373 : // (and does not have to change lanes)
374 : if ((state & LCA_AMBLOCKINGFOLLOWER) != 0) {
375 : if (fabs(max - myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle)) < 0.001 && min == 0) { // !!! was standing
376 : if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGFOLLOWER (standing)\n";
377 : return 0;
378 : }
379 : if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGFOLLOWER\n";
380 :
381 : //return min; // VARIANT_3 (brakeStrong)
382 : return (min + wanted) / 2.0;
383 : }
384 : if ((state & LCA_AMBACKBLOCKER) != 0) {
385 : if (max <= myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle) && min == 0) { // !!! was standing
386 : if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBACKBLOCKER (standing)\n";
387 : //return min; VARIANT_9 (backBlockVSafe)
388 : return nVSafe;
389 : }
390 : }
391 : if ((state & LCA_AMBACKBLOCKER_STANDING) != 0) {
392 : if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBACKBLOCKER_STANDING\n";
393 : //return min;
394 : return nVSafe;
395 : }
396 : */
397 :
398 : // accelerate if being a blocking leader or blocking follower not able to brake
399 : // (and does not have to change lanes)
400 526881976 : if ((state & LCA_AMBLOCKINGLEADER) != 0 && myCooperativeSpeed >= 0) {
401 : #ifdef DEBUG_PATCH_SPEED
402 : if (DEBUG_COND) {
403 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGLEADER\n";
404 : }
405 : #endif
406 1446732 : return (max + wanted) / 2.0;
407 : }
408 :
409 : if ((state & LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0) {
410 : #ifdef DEBUG_PATCH_SPEED
411 : if (DEBUG_COND) {
412 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGFOLLOWER_DONTBRAKE\n";
413 : }
414 : #endif
415 : /*
416 : // VARIANT_4 (dontbrake)
417 : if (max <= myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle) && min == 0) { // !!! was standing
418 : return wanted;
419 : }
420 : return (min + wanted) / 2.0;
421 : */
422 : }
423 525435244 : if (!myVehicle.getLane()->getEdge().hasLaneChanger()) {
424 : // remove chaning information if on a road with a single lane
425 289220100 : changed();
426 : }
427 : return wanted;
428 : }
429 :
430 :
431 : void*
432 4310411 : MSLCM_LC2013::inform(void* info, MSVehicle* sender) {
433 : UNUSED_PARAMETER(sender);
434 : Info* pinfo = (Info*)info;
435 : assert(pinfo->first >= 0 || !MSGlobals::gSemiImplicitEulerUpdate);
436 4310411 : addLCSpeedAdvice(pinfo->first, false);
437 4310411 : myOwnState |= pinfo->second;
438 : #ifdef DEBUG_INFORMED
439 : if (DEBUG_COND) {
440 : std::cout << SIMTIME
441 : << " veh=" << myVehicle.getID()
442 : << " informedBy=" << sender->getID()
443 : << " info=" << pinfo->second
444 : << " vSafe=" << pinfo->first
445 : << "\n";
446 : }
447 : #endif
448 4310411 : delete pinfo;
449 4310411 : return (void*) true;
450 : }
451 :
452 : double
453 4228629 : MSLCM_LC2013::overtakeDistance(const MSVehicle* follower, const MSVehicle* leader, const double gap, double followerSpeed, double leaderSpeed) {
454 4228629 : followerSpeed = followerSpeed == INVALID_SPEED ? follower->getSpeed() : followerSpeed;
455 4228629 : leaderSpeed = leaderSpeed == INVALID_SPEED ? leader->getSpeed() : leaderSpeed;
456 : double overtakeDist = (gap // drive to back of leader
457 4228629 : + leader->getVehicleType().getLengthWithGap() // drive to front of leader
458 4228629 : + follower->getVehicleType().getLength() // follower back reaches leader front
459 4228629 : + leader->getCarFollowModel().getSecureGap( // save gap to leader
460 4228629 : leader, follower, leaderSpeed, followerSpeed, follower->getCarFollowModel().getMaxDecel()));
461 4228629 : return MAX2(overtakeDist, 0.);
462 : }
463 :
464 :
465 : double
466 4385066 : MSLCM_LC2013::informLeader(MSAbstractLaneChangeModel::MSLCMessager& msgPass,
467 : int blocked,
468 : int dir,
469 : const std::pair<MSVehicle*, double>& neighLead,
470 : double remainingSeconds) {
471 4385066 : double plannedSpeed = myVehicle.getSpeed();
472 4385066 : if (!isOpposite()) {
473 4290378 : plannedSpeed = MIN2(plannedSpeed,
474 4290378 : myVehicle.getCarFollowModel().stopSpeed(&myVehicle, myVehicle.getSpeed(), myLeftSpace - myLeadingBlockerLength));
475 : }
476 4998026 : for (auto i : myLCAccelerationAdvices) {
477 : const double a = i.first;
478 612960 : if (a >= -myVehicle.getCarFollowModel().getMaxDecel()) {
479 612940 : plannedSpeed = MIN2(plannedSpeed, myVehicle.getSpeed() + ACCEL2SPEED(a));
480 : }
481 : }
482 : #ifdef DEBUG_INFORMER
483 : if (DEBUG_COND) {
484 : std::cout << "\nINFORM_LEADER"
485 : << "\nspeed=" << myVehicle.getSpeed() << " planned=" << plannedSpeed << "\n";
486 : }
487 : #endif
488 :
489 4385066 : const MSVehicle* const nv = neighLead.first;
490 4385066 : if (nv == nullptr) {
491 : // not overtaking
492 : return plannedSpeed;
493 : }
494 4158315 : const double neighNextSpeed = nv->getSpeed() - ACCEL2SPEED(MAX2(1.0, -nv->getAcceleration()));
495 : double neighNextGap;
496 4158315 : if (MSGlobals::gSemiImplicitEulerUpdate) {
497 3926513 : neighNextGap = neighLead.second + SPEED2DIST(neighNextSpeed - plannedSpeed);
498 : } else {
499 231802 : neighNextGap = neighLead.second + SPEED2DIST((nv->getSpeed() + neighNextSpeed) / 2) - SPEED2DIST((myVehicle.getSpeed() + plannedSpeed) / 2);
500 : }
501 4158315 : if ((blocked & LCA_BLOCKED_BY_LEADER) != 0) {
502 3358453 : if (MSLCHelper::divergentRoute(myVehicle, *nv)) {
503 : //std::cout << SIMTIME << " ego=" << myVehicle.getID() << " ignoresDivergentBlockingLeader=" << nv->getID() << "\n";
504 : return plannedSpeed;
505 : }
506 : #ifdef DEBUG_INFORMER
507 : if (DEBUG_COND) {
508 : std::cout << " blocked by leader nv=" << nv->getID() << " nvSpeed=" << nv->getSpeed() << " needGap="
509 : << myVehicle.getCarFollowModel().getSecureGap(&myVehicle, nv, myVehicle.getSpeed(), nv->getSpeed(), nv->getCarFollowModel().getMaxDecel()) << "\n";
510 : }
511 : #endif
512 : // decide whether we want to overtake the leader or follow it
513 : double overtakeTime;
514 3355571 : const double overtakeDist = overtakeDistance(&myVehicle, nv, neighLead.second);
515 3355571 : const double dv = plannedSpeed - nv->getSpeed();
516 :
517 3355571 : if (dv > myOvertakeDeltaSpeedFactor * myVehicle.getLane()->getSpeedLimit()) {
518 1446695 : overtakeTime = overtakeDist / dv;
519 : } else if (nv->getWaitingSeconds() > BLOCKER_IS_BLOCKED_TIME_THRESHOLD
520 1083606 : && !isOpposite()
521 2966247 : && (myVehicle.getVehicleType().getLengthWithGap() + nv->getVehicleType().getLengthWithGap()) <= myLeftSpace) {
522 : // -> set overtakeTime to indicate possibility of overtaking (only if there is enough space)
523 941397 : overtakeTime = remainingSeconds - 1;
524 : } else {
525 : // -> set overtakeTime to something indicating impossibility of overtaking
526 967479 : overtakeTime = remainingSeconds + 1;
527 : }
528 :
529 : #ifdef DEBUG_INFORMER
530 : if (DEBUG_COND) {
531 : std::cout << SIMTIME << " informLeader() of " << myVehicle.getID()
532 : << "\nnv = " << nv->getID()
533 : << "\nplannedSpeed = " << plannedSpeed
534 : << "\nleaderSpeed = " << nv->getSpeed()
535 : << "\nmyLeftSpace = " << myLeftSpace
536 : << "\nremainingSeconds = " << remainingSeconds
537 : << "\novertakeDist = " << overtakeDist
538 : << "\novertakeTime = " << overtakeTime
539 : << std::endl;
540 : }
541 : #endif
542 :
543 3355571 : if ((dv < myOvertakeDeltaSpeedFactor * myVehicle.getLane()->getSpeedLimit()
544 : // overtaking on the right on an uncongested highway is forbidden (noOvertakeLCLeft)
545 2545329 : || (dir == LCA_MLEFT && !myVehicle.congested() && !myAllowOvertakingRight)
546 : // not enough space to overtake?
547 4934073 : || (MSGlobals::gSemiImplicitEulerUpdate && myLeftSpace - myLeadingBlockerLength - myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed()) < overtakeDist)
548 : // using brakeGap() without headway seems adequate in a situation where the obstacle (the lane end) is not moving [XXX implemented in branch ticket860, can be used in general if desired, refs. #2575] (Leo).
549 2387702 : || (!MSGlobals::gSemiImplicitEulerUpdate && myLeftSpace - myLeadingBlockerLength - myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed(), getCarFollowModel().getMaxDecel(), 0.) < overtakeDist)
550 : // not enough time to overtake? (skipped for a stopped leader [currently only for ballistic update XXX: check if appropriate for euler, too, refs. #2575] to ensure that it can be overtaken if only enough space is exists) (Leo)
551 2370549 : || (remainingSeconds < overtakeTime && (MSGlobals::gSemiImplicitEulerUpdate || !nv->isStopped())))
552 : // opposite driving and must overtake
553 4033248 : && (!neighLead.first->isStopped() || (isOpposite() && neighLead.second >= 0))) {
554 : // cannot overtake
555 1485198 : msgPass.informNeighLeader(new Info(std::numeric_limits<double>::max(), dir | LCA_AMBLOCKINGLEADER), &myVehicle);
556 : // slow down smoothly to follow leader
557 : // account for minor decelerations by the leader (dawdling)
558 4455594 : double targetSpeed = MAX3(myVehicle.getCarFollowModel().minNextSpeed(myVehicle.getSpeed(), &myVehicle),
559 1485198 : getCarFollowModel().followSpeed(&myVehicle, myVehicle.getSpeed(), neighNextGap, neighNextSpeed, nv->getCarFollowModel().getMaxDecel()),
560 : // avoid changing on intersection
561 1503907 : (myVehicle.getLane()->isNormal() || myVehicle.getCurrentEdge()->isRoundabout()) ? 0 : ACCEL2SPEED(myVehicle.getCarFollowModel().getMaxAccel()));
562 1485198 : if (targetSpeed < myVehicle.getSpeed()) {
563 : // slow down smoothly to follow leader
564 506689 : const double decel = remainingSeconds == 0. ? myVehicle.getCarFollowModel().getMaxDecel() :
565 506689 : MIN2(myVehicle.getCarFollowModel().getMaxDecel(),
566 506689 : MAX2(MIN_FALLBEHIND, (myVehicle.getSpeed() - targetSpeed) / remainingSeconds));
567 506689 : const double nextSpeed = MIN2(plannedSpeed, MAX2(0.0, myVehicle.getSpeed() - ACCEL2SPEED(decel)));
568 : #ifdef DEBUG_INFORMER
569 : if (DEBUG_COND) {
570 : std::cout << SIMTIME
571 : << " cannot overtake leader nv=" << nv->getID()
572 : << " dv=" << dv
573 : << " myLookAheadSpeed=" << myLookAheadSpeed
574 : << " myLeftSpace=" << myLeftSpace
575 : << " overtakeDist=" << overtakeDist
576 : << " overtakeTime=" << overtakeTime
577 : << " remainingSeconds=" << remainingSeconds
578 : << " currentGap=" << neighLead.second
579 : << " brakeGap=" << myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed(), getCarFollowModel().getMaxDecel(), 0.)
580 : << " neighNextSpeed=" << neighNextSpeed
581 : << " neighNextGap=" << neighNextGap
582 : << " targetSpeed=" << targetSpeed
583 : << " nextSpeed=" << nextSpeed
584 : << "\n";
585 : }
586 : #endif
587 506689 : addLCSpeedAdvice(nextSpeed);
588 506689 : return nextSpeed;
589 : } else {
590 : // leader is fast enough anyway
591 : #ifdef DEBUG_INFORMER
592 : if (DEBUG_COND) {
593 : std::cout << SIMTIME
594 : << " cannot overtake fast leader nv=" << nv->getID()
595 : << " dv=" << dv
596 : << " myLookAheadSpeed=" << myLookAheadSpeed
597 : << " myLeftSpace=" << myLeftSpace
598 : << " overtakeDist=" << overtakeDist
599 : << " myLeadingBlockerLength=" << myLeadingBlockerLength
600 : << " overtakeTime=" << overtakeTime
601 : << " remainingSeconds=" << remainingSeconds
602 : << " currentGap=" << neighLead.second
603 : << " neighNextSpeed=" << neighNextSpeed
604 : << " neighNextGap=" << neighNextGap
605 : << " targetSpeed=" << targetSpeed
606 : << "\n";
607 : }
608 : #endif
609 978509 : addLCSpeedAdvice(targetSpeed);
610 978509 : return plannedSpeed;
611 : }
612 : } else {
613 : // overtaking, leader should not accelerate
614 : #ifdef DEBUG_INFORMER
615 : if (DEBUG_COND) {
616 : std::cout << SIMTIME
617 : << " wants to overtake leader nv=" << nv->getID()
618 : << " dv=" << dv
619 : << " overtakeDist=" << overtakeDist
620 : << " remainingSeconds=" << remainingSeconds
621 : << " overtakeTime=" << overtakeTime
622 : << " currentGap=" << neighLead.second
623 : << " secureGap=" << nv->getCarFollowModel().getSecureGap(nv, &myVehicle, nv->getSpeed(), myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel())
624 : << "\n";
625 : }
626 : #endif
627 : // no need to pass a message if the neighbor is waiting/stuck anyway (but sending it would risk deadlock)
628 1870373 : if (nv->getWaitingSeconds() <= BLOCKER_IS_BLOCKED_TIME_THRESHOLD) {
629 835160 : msgPass.informNeighLeader(new Info(nv->getSpeed(), dir | LCA_AMBLOCKINGLEADER), &myVehicle);
630 : }
631 1870373 : return -1; // XXX: using -1 is ambiguous for the ballistic update! Currently this is being catched in patchSpeed() (Leo), consider returning INVALID_SPEED, refs. #2577
632 : }
633 : } else { // (remainUnblocked)
634 : // we are not blocked now. make sure we stay far enough from the leader
635 1599724 : const double targetSpeed = MAX2(
636 799862 : myVehicle.getCarFollowModel().minNextSpeed(myVehicle.getSpeed(), &myVehicle),
637 799862 : getCarFollowModel().followSpeed(&myVehicle, myVehicle.getSpeed(), neighNextGap, neighNextSpeed, nv->getCarFollowModel().getMaxDecel()));
638 799862 : addLCSpeedAdvice(targetSpeed);
639 : #ifdef DEBUG_INFORMER
640 : if (DEBUG_COND) {
641 : std::cout << " not blocked by leader nv=" << nv->getID()
642 : << " nvSpeed=" << nv->getSpeed()
643 : << " gap=" << neighLead.second
644 : << " neighNextSpeed=" << neighNextSpeed
645 : << " neighNextGap=" << neighNextGap
646 : << " needGap=" << myVehicle.getCarFollowModel().getSecureGap(&myVehicle, nv, myVehicle.getSpeed(), nv->getSpeed(), nv->getCarFollowModel().getMaxDecel())
647 : << " targetSpeed=" << targetSpeed
648 : << "\n";
649 : }
650 : #endif
651 : return MIN2(targetSpeed, plannedSpeed);
652 : }
653 : }
654 :
655 : void
656 2511653 : MSLCM_LC2013::informFollower(MSAbstractLaneChangeModel::MSLCMessager& msgPass,
657 : int blocked,
658 : int dir,
659 : const std::pair<MSVehicle*, double>& neighFollow,
660 : double remainingSeconds,
661 : double plannedSpeed) {
662 :
663 2511653 : MSVehicle* nv = neighFollow.first;
664 2511653 : const double plannedAccel = SPEED2ACCEL(MAX2(MIN2(getCarFollowModel().getMaxAccel(), plannedSpeed - myVehicle.getSpeed()), -getCarFollowModel().getMaxDecel()));
665 :
666 : #ifdef DEBUG_INFORMER
667 : if (DEBUG_COND) {
668 : std::cout << "\nINFORM_FOLLOWER"
669 : << "\nspeed=" << myVehicle.getSpeed() << " planned=" << plannedSpeed << "\n";
670 : }
671 :
672 : #endif
673 2511653 : if ((blocked & LCA_BLOCKED_BY_FOLLOWER) != 0 && nv != nullptr) {
674 1439235 : if (MSLCHelper::divergentRoute(myVehicle, *nv)) {
675 : //std::cout << SIMTIME << " ego=" << myVehicle.getID() << " ignoresDivergentBlockingFollower=" << nv->getID() << "\n";
676 : return;
677 : }
678 : #ifdef DEBUG_INFORMER
679 : if (DEBUG_COND) {
680 : std::cout << " blocked by follower nv=" << nv->getID() << " nvSpeed=" << nv->getSpeed() << " needGap="
681 : << nv->getCarFollowModel().getSecureGap(nv, &myVehicle, nv->getSpeed(), myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel()) << " planned=" << plannedSpeed << "\n";
682 : }
683 : #endif
684 :
685 : // are we fast enough to cut in without any help?
686 1439076 : if (MAX2(plannedSpeed, 0.) - nv->getSpeed() >= HELP_OVERTAKE) {
687 81932 : const double neededGap = nv->getCarFollowModel().getSecureGap(nv, &myVehicle, nv->getSpeed(), plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel());
688 81932 : if ((neededGap - neighFollow.second) / remainingSeconds < (MAX2(plannedSpeed, 0.) - nv->getSpeed())) {
689 : #ifdef DEBUG_INFORMER
690 : if (DEBUG_COND) {
691 : std::cout << " wants to cut in before nv=" << nv->getID() << " without any help." << "\nneededGap = " << neededGap << "\n";
692 : }
693 : #endif
694 : // follower might even accelerate but not to much
695 : // XXX: I don't understand this. The needed gap was determined for nv->getSpeed(), not for (plannedSpeed - HELP_OVERTAKE)?! (Leo), refs. #2578
696 70632 : msgPass.informNeighFollower(new Info(MAX2(plannedSpeed, 0.) - HELP_OVERTAKE, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
697 70632 : return;
698 : }
699 : }
700 :
701 : // decide whether we will request help to cut in before the follower or allow to be overtaken
702 :
703 : // PARAMETERS
704 : // assume other vehicle will assume the equivalent of 1 second of
705 : // maximum deceleration to help us (will probably be spread over
706 : // multiple seconds)
707 : // -----------
708 : const double helpDecel = nv->getCarFollowModel().getMaxDecel() * HELP_DECEL_FACTOR;
709 :
710 : // follower's new speed in next step
711 : double neighNewSpeed;
712 : // follower's new speed after 1s.
713 : double neighNewSpeed1s;
714 : // velocity difference, gap after follower-deceleration
715 : double dv, decelGap;
716 :
717 1368444 : if (MSGlobals::gSemiImplicitEulerUpdate) {
718 : // euler
719 1234003 : neighNewSpeed = MAX2(0., nv->getSpeed() - ACCEL2SPEED(helpDecel));
720 1234003 : neighNewSpeed1s = MAX2(0., nv->getSpeed() - helpDecel); // TODO: consider introduction of a configurable anticipationTime here (see far below in the !blocked part). Refs. #2578
721 : // change in the gap between ego and blocker over 1 second (not STEP!)
722 : // XXX: though here it is calculated as if it were one step!? (Leo) Refs. #2578
723 1234003 : dv = plannedSpeed - neighNewSpeed1s; // XXX: what is this quantity (if TS!=1)?
724 : // new gap between follower and self in case the follower does brake for 1s
725 : // XXX: if the step-length is not 1s., this is not the gap after 1s. deceleration!
726 : // And this formula overestimates the real gap. Isn't that problematic? (Leo)
727 : // Below, it seems that decelGap > secureGap is taken to indicate the possibility
728 : // to cut in within the next time-step. However, this is not the case, if TS<1s.,
729 : // since decelGap is (not exactly, though!) the gap after 1s. Refs. #2578
730 1234003 : decelGap = neighFollow.second + dv;
731 : } else {
732 : // ballistic
733 : // negative newSpeed-extrapolation possible, if stop lies within the next time-step
734 : // XXX: this code should work for the euler case as well, since gapExtrapolation() takes
735 : // care of this, but for TS!=1 we will have different behavior (see previous remark) Refs. #2578
736 134441 : neighNewSpeed = nv->getSpeed() - ACCEL2SPEED(helpDecel);
737 134441 : neighNewSpeed1s = nv->getSpeed() - helpDecel;
738 :
739 134441 : dv = myVehicle.getSpeed() - nv->getSpeed(); // current velocity difference
740 268882 : decelGap = getCarFollowModel().gapExtrapolation(1., neighFollow.second, myVehicle.getSpeed(),
741 134441 : nv->getSpeed(), plannedAccel, -helpDecel, myVehicle.getMaxSpeedOnLane(), nv->getMaxSpeedOnLane());
742 : }
743 :
744 1368444 : const double secureGap = nv->getCarFollowModel().getSecureGap(nv, &myVehicle, MAX2(neighNewSpeed1s, 0.),
745 1368444 : MAX2(plannedSpeed, 0.), myVehicle.getCarFollowModel().getMaxDecel());
746 :
747 1368444 : const double onRampThreshold = myVehicle.getLane()->getSpeedLimit() * 0.8 * myExperimentalParam1 * (1 - myVehicle.getImpatience());
748 :
749 : #ifdef DEBUG_INFORMER
750 : if (DEBUG_COND) {
751 : std::cout << SIMTIME
752 : << " speed=" << myVehicle.getSpeed()
753 : << " plannedSpeed=" << plannedSpeed
754 : << " threshold=" << onRampThreshold
755 : << " neighNewSpeed=" << neighNewSpeed
756 : << " neighNewSpeed1s=" << neighNewSpeed1s
757 : << " dv=" << dv
758 : << " gap=" << neighFollow.second
759 : << " decelGap=" << decelGap
760 : << " secureGap=" << secureGap
761 : << "\n";
762 : }
763 : #endif
764 : // prevent vehicles on an on ramp stopping the main flow
765 : if (dir == LCA_MLEFT
766 521635 : && myVehicle.getLane()->isAccelLane()
767 1432849 : && neighNewSpeed1s < onRampThreshold) {
768 : return;
769 : }
770 :
771 1349428 : if (decelGap > 0 && decelGap >= secureGap) {
772 : // XXX: This does not assure that the leader can cut in in the next step if TS < 1 (see above)
773 : // this seems to be supposed in the following (euler code)...?! (Leo) Refs. #2578
774 :
775 : // if the blocking follower brakes it could help
776 : // how hard does it actually need to be?
777 : // to be safe in the next step the following equation has to hold for the follower's vsafe:
778 : // vsafe <= followSpeed(gap=currentGap - SPEED2DIST(vsafe), ...)
779 : double vsafe, vsafe1;
780 :
781 95731 : if (MSGlobals::gSemiImplicitEulerUpdate) {
782 : // euler
783 : // we compute an upper bound on vsafe by doing the computation twice
784 85729 : vsafe1 = MAX2(neighNewSpeed, nv->getCarFollowModel().followSpeed(
785 85729 : nv, nv->getSpeed(), neighFollow.second + SPEED2DIST(plannedSpeed), plannedSpeed, getCarFollowModel().getMaxDecel()));
786 85729 : vsafe = MAX2(neighNewSpeed, nv->getCarFollowModel().followSpeed(
787 85729 : nv, nv->getSpeed(), neighFollow.second + SPEED2DIST(plannedSpeed - vsafe1), plannedSpeed, getCarFollowModel().getMaxDecel()));
788 : //assert(vsafe <= vsafe1); assertion does not hold for models with randomness in followSpeed (W99)
789 : } else {
790 : // ballistic
791 :
792 : // XXX: This block should actually do as well for euler update (TODO: test!), refs #2575
793 : // we compute an upper bound on vsafe
794 : // next step's gap without help deceleration (nv's speed assumed constant)
795 10002 : double nextGap = getCarFollowModel().gapExtrapolation(TS,
796 10002 : neighFollow.second, myVehicle.getSpeed(),
797 10002 : nv->getSpeed(), plannedAccel, 0,
798 10002 : myVehicle.getMaxSpeedOnLane(), nv->getMaxSpeedOnLane());
799 : #ifdef DEBUG_INFORMER
800 : if (DEBUG_COND) {
801 : std::cout << "nextGap=" << nextGap << " (without help decel) \n";
802 : }
803 : #endif
804 :
805 : // NOTE: the second argument of MIN2() can get larger than nv->getSpeed()
806 20004 : vsafe1 = MIN2(nv->getSpeed(), MAX2(neighNewSpeed,
807 10002 : nv->getCarFollowModel().followSpeed(nv,
808 10002 : nv->getSpeed(), nextGap,
809 : MAX2(0., plannedSpeed),
810 : getCarFollowModel().getMaxDecel())));
811 :
812 :
813 : // next step's gap with possibly less than maximal help deceleration (in case vsafe1 > neighNewSpeed)
814 10002 : double decel2 = SPEED2ACCEL(nv->getSpeed() - vsafe1);
815 10002 : nextGap = getCarFollowModel().gapExtrapolation(TS,
816 10002 : neighFollow.second, myVehicle.getSpeed(),
817 10002 : nv->getSpeed(), plannedAccel, -decel2,
818 10002 : myVehicle.getMaxSpeedOnLane(), nv->getMaxSpeedOnLane());
819 :
820 : // vsafe = MAX(neighNewSpeed, safe speed assuming next_gap)
821 : // Thus, the gap resulting from vsafe is larger or equal to next_gap
822 : // in contrast to the euler case, where nv's follow speed doesn't depend on the actual speed,
823 : // we need to assure, that nv doesn't accelerate
824 20004 : vsafe = MIN2(nv->getSpeed(), MAX2(neighNewSpeed,
825 10002 : nv->getCarFollowModel().followSpeed(nv,
826 10002 : nv->getSpeed(), nextGap,
827 : MAX2(0., plannedSpeed),
828 : getCarFollowModel().getMaxDecel())));
829 :
830 : assert(vsafe >= vsafe1 - NUMERICAL_EPS);
831 :
832 : #ifdef DEBUG_INFORMER
833 : if (DEBUG_COND) {
834 : std::cout << "nextGap=" << nextGap
835 : << " (with vsafe1 and help decel) \nvsafe1=" << vsafe1
836 : << " vsafe=" << vsafe
837 : << "\n";
838 : }
839 : #endif
840 :
841 : // For subsecond simulation, this might not lead to secure gaps for a long time,
842 : // we seek to establish a secure gap as soon as possible
843 10002 : double nextSecureGap = nv->getCarFollowModel().getSecureGap(nv, &myVehicle, vsafe, plannedSpeed, getCarFollowModel().getMaxDecel());
844 :
845 10002 : if (nextGap < nextSecureGap) {
846 : // establish a secureGap as soon as possible
847 : vsafe = neighNewSpeed;
848 : }
849 :
850 : #ifdef DEBUG_INFORMER
851 : if (DEBUG_COND) {
852 : std::cout << "nextGap=" << nextGap
853 : << " minNextSecureGap=" << nextSecureGap
854 : << " vsafe=" << vsafe << "\n";
855 : }
856 : #endif
857 :
858 : }
859 95731 : msgPass.informNeighFollower(
860 95731 : new Info(vsafe, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
861 :
862 : #ifdef DEBUG_INFORMER
863 : if (DEBUG_COND) {
864 : std::cout << " wants to cut in before nv=" << nv->getID()
865 : << " vsafe1=" << vsafe1 << " vsafe=" << vsafe
866 : << " newSecGap="
867 : << nv->getCarFollowModel().getSecureGap(nv, &myVehicle, vsafe,
868 : plannedSpeed,
869 : myVehicle.getCarFollowModel().getMaxDecel())
870 : << "\n";
871 : }
872 : #endif
873 1253697 : } else if ((MSGlobals::gSemiImplicitEulerUpdate && dv > 0 && dv * remainingSeconds > (secureGap - decelGap + POSITION_EPS))
874 1181958 : || (!MSGlobals::gSemiImplicitEulerUpdate && dv > 0 && dv * (remainingSeconds - 1) > secureGap - decelGap + POSITION_EPS)
875 : ) {
876 :
877 : // XXX: Alternative formulation (encapsulating differences of euler and ballistic) TODO: test, refs. #2575
878 : // double eventualGap = getCarFollowModel().gapExtrapolation(remainingSeconds - 1., decelGap, plannedSpeed, neighNewSpeed1s);
879 : // } else if (eventualGap > secureGap + POSITION_EPS) {
880 :
881 :
882 : // NOTE: This case corresponds to the situation, where some time is left to perform the lc
883 : // For the ballistic case this is interpreted as follows:
884 : // If the follower breaks with helpDecel for one second, this vehicle maintains the plannedSpeed,
885 : // and both continue with their speeds for remainingSeconds seconds the gap will suffice for a laneChange
886 : // For the euler case we had the following comment:
887 : // 'decelerating once is sufficient to open up a large enough gap in time', but:
888 : // XXX: 1) Decelerating *once* does not necessarily lead to the gap decelGap! (if TS<1s.) (Leo)
889 : // 2) Probably, the if() for euler should test for dv * (remainingSeconds-1) > ..., too ?!, refs. #2578
890 74673 : msgPass.informNeighFollower(new Info(neighNewSpeed, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
891 : #ifdef DEBUG_INFORMER
892 : if (DEBUG_COND) {
893 : std::cout << " wants to cut in before nv=" << nv->getID() << " (eventually)\n";
894 : }
895 : #endif
896 1179024 : } else if (dir == LCA_MRIGHT && !myAllowOvertakingRight && !nv->congested()) {
897 : // XXX: check if this requires a special treatment for the ballistic update, refs. #2575
898 : const double vhelp = MAX2(neighNewSpeed, HELP_OVERTAKE);
899 1307 : msgPass.informNeighFollower(new Info(vhelp, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
900 : #ifdef DEBUG_INFORMER
901 : if (DEBUG_COND) {
902 : std::cout << " wants to cut in before nv=" << nv->getID() << " (nv cannot overtake right)\n";
903 : }
904 : #endif
905 : } else {
906 1177717 : double vhelp = MAX2(nv->getSpeed(), myVehicle.getSpeed() + HELP_OVERTAKE);
907 : //if (dir == LCA_MRIGHT && myVehicle.getWaitingSeconds() > LCA_RIGHT_IMPATIENCE &&
908 : // nv->getSpeed() > myVehicle.getSpeed()) {
909 1177717 : if (nv->getSpeed() > myVehicle.getSpeed() &&
910 318468 : ((dir == LCA_MRIGHT && myVehicle.getWaitingSeconds() > LCA_RIGHT_IMPATIENCE) // NOTE: it might be considered to use myVehicle.getAccumulatedWaitingSeconds() > LCA_RIGHT_IMPATIENCE instead (Leo). Refs. #2578
911 206401 : || (dir == LCA_MLEFT && plannedSpeed > CUT_IN_LEFT_SPEED_THRESHOLD) // VARIANT_22 (slowDownLeft)
912 : // XXX this is a hack to determine whether the vehicles is on an on-ramp. This information should be retrieved from the network itself
913 206369 : || (dir == LCA_MLEFT && myVehicle.getLane()->getLength() > MAX_ONRAMP_LENGTH)
914 : )) {
915 : // let the follower slow down to increase the likelihood that later vehicles will be slow enough to help
916 : // follower should still be fast enough to open a gap
917 : // XXX: The probability for that success would be larger if the slow down of the appropriate following vehicle
918 : // would take place without the immediate follower slowing down. We might consider to model reactions of
919 : // vehicles that are not immediate followers. (Leo) -> see ticket #2532
920 364216 : vhelp = MAX2(neighNewSpeed, myVehicle.getSpeed() + HELP_OVERTAKE);
921 : #ifdef DEBUG_INFORMER
922 : if (DEBUG_COND) {
923 : // NOTE: the condition labeled "VARIANT_22" seems to imply that this could as well concern the *left* follower?! (Leo)
924 : // Further, vhelp might be larger than nv->getSpeed(), so the request issued below is not to slow down!? (see below) Refs. #2578
925 : std::cout << " wants right follower to slow down a bit\n";
926 : }
927 : #endif
928 364216 : if (MSGlobals::gSemiImplicitEulerUpdate) {
929 : // euler
930 311265 : if ((nv->getSpeed() - myVehicle.getSpeed()) / helpDecel < remainingSeconds) {
931 :
932 : #ifdef DEBUG_INFORMER
933 : if (DEBUG_COND) {
934 : // NOTE: the condition labeled "VARIANT_22" seems to imply that this could as well concern the *left* follower?! Refs. #2578
935 : std::cout << " wants to cut in before right follower nv=" << nv->getID() << " (eventually)\n";
936 : }
937 : #endif
938 : // XXX: I don't understand. This vhelp might be larger than nv->getSpeed() but the above condition seems to rely
939 : // on the reasoning that if nv breaks with helpDecel for remaining Seconds, nv will be so slow, that this
940 : // vehicle will be able to cut in. But nv might have overtaken this vehicle already (or am I missing sth?). (Leo)
941 : // Ad: To my impression, the intention behind allowing larger speeds for the blocking follower is to prevent a
942 : // situation, where an overlapping follower keeps blocking the ego vehicle. Refs. #2578
943 295403 : msgPass.informNeighFollower(new Info(vhelp, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
944 295403 : return;
945 : }
946 : } else {
947 :
948 : // ballistic (this block is a bit different to the logic in the euler part, but in general suited to work on euler as well.. must be tested <- TODO, refs. #2575)
949 : // estimate gap after remainingSeconds.
950 : // Assumptions:
951 : // (A1) leader continues with currentSpeed. (XXX: That might be wrong: Think of accelerating on an on-ramp or of a congested region ahead!)
952 : // (A2) follower breaks with helpDecel.
953 52951 : const double gapAfterRemainingSecs = getCarFollowModel().gapExtrapolation(
954 52951 : remainingSeconds, neighFollow.second, myVehicle.getSpeed(), nv->getSpeed(), 0, -helpDecel, myVehicle.getMaxSpeedOnLane(), nv->getMaxSpeedOnLane());
955 52951 : const double secureGapAfterRemainingSecs = nv->getCarFollowModel().getSecureGap(nv, &myVehicle,
956 52951 : MAX2(nv->getSpeed() - remainingSeconds * helpDecel, 0.), myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel());
957 52951 : if (gapAfterRemainingSecs >= secureGapAfterRemainingSecs) { // XXX: here it would be wise to check whether there is enough space for eventual braking if the maneuver doesn't succeed
958 : #ifdef DEBUG_INFORMER
959 : if (DEBUG_COND) {
960 : std::cout << " wants to cut in before follower nv=" << nv->getID() << " (eventually)\n";
961 : }
962 : #endif
963 : // NOTE: ballistic uses neighNewSpeed instead of vhelp, see my note above. (Leo)
964 : // TODO: recheck if this might cause suboptimal behaviour in some LC-situations. Refs. #2578
965 9256 : msgPass.informNeighFollower(new Info(neighNewSpeed, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
966 9256 : return;
967 : }
968 : }
969 :
970 :
971 : }
972 :
973 : #ifdef DEBUG_INFORMER
974 : if (DEBUG_COND) {
975 : std::cout << SIMTIME
976 : << " veh=" << myVehicle.getID()
977 : << " informs follower " << nv->getID()
978 : << " vhelp=" << vhelp
979 : << "\n";
980 : }
981 : #endif
982 :
983 873058 : msgPass.informNeighFollower(new Info(vhelp, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
984 : // This follower is supposed to overtake us. Slow down smoothly to allow this.
985 873058 : const double overtakeDist = overtakeDistance(nv, &myVehicle, neighFollow.second, vhelp, plannedSpeed);
986 : // speed difference to create a sufficiently large gap
987 873058 : const double needDV = overtakeDist / remainingSeconds;
988 : // make sure the deceleration is not to strong (XXX: should be assured in finalizeSpeed -> TODO: remove the MAX2 if agreed) -> prob with possibly non-existing maximal deceleration for som CF Models(?) Refs. #2578
989 975722 : addLCSpeedAdvice(MAX2(vhelp - needDV, myVehicle.getSpeed() - ACCEL2SPEED(myVehicle.getCarFollowModel().getMaxDecel())));
990 :
991 : #ifdef DEBUG_INFORMER
992 : if (DEBUG_COND) {
993 : std::cout << SIMTIME
994 : << " veh=" << myVehicle.getID()
995 : << " wants to be overtaken by=" << nv->getID()
996 : << " overtakeDist=" << overtakeDist
997 : << " vneigh=" << nv->getSpeed()
998 : << " vhelp=" << vhelp
999 : << " needDV=" << needDV
1000 : << " vsafe=" << myLCAccelerationAdvices.back().first
1001 : << "\n";
1002 : }
1003 : #endif
1004 : }
1005 1072418 : } else if (neighFollow.first != nullptr && (blocked & LCA_BLOCKED_BY_LEADER)) {
1006 : // we are not blocked by the follower now, make sure it remains that way
1007 567552 : const double vsafe = MSLCHelper::getSpeedPreservingSecureGap(myVehicle, *neighFollow.first, neighFollow.second, plannedSpeed);
1008 567552 : msgPass.informNeighFollower(new Info(vsafe, dir), &myVehicle);
1009 :
1010 : #ifdef DEBUG_INFORMER
1011 : if (DEBUG_COND) {
1012 : std::cout << " wants to cut in before non-blocking follower nv=" << nv->getID() << "\n";
1013 : }
1014 : #endif
1015 : }
1016 : }
1017 :
1018 :
1019 : void
1020 531903772 : MSLCM_LC2013::prepareStep() {
1021 531903772 : MSAbstractLaneChangeModel::prepareStep();
1022 : // keep information about strategic change direction
1023 531903772 : if (!isChangingLanes()) {
1024 531368642 : myOwnState = (myOwnState & LCA_STRATEGIC) ? (myOwnState & LCA_WANTS_LANECHANGE) : 0;
1025 : }
1026 531903772 : myLeadingBlockerLength = 0;
1027 531903772 : myLeftSpace = 0;
1028 : myLCAccelerationAdvices.clear();
1029 531903772 : myDontBrake = false;
1030 : // truncate to work around numerical instability between different builds
1031 531903772 : mySpeedGainProbabilityLeft = ceil(mySpeedGainProbabilityLeft * 100000.0) * 0.00001;
1032 531903772 : mySpeedGainProbabilityRight = ceil(mySpeedGainProbabilityRight * 100000.0) * 0.00001;
1033 531903772 : myKeepRightProbability = ceil(myKeepRightProbability * 100000.0) * 0.00001;
1034 531903772 : if (mySigma > 0 && !isChangingLanes()) {
1035 : // disturb lateral position directly
1036 1154 : const double maxDist = SPEED2DIST(myVehicle.getVehicleType().getMaxSpeedLat());
1037 1154 : const double oldPosLat = myVehicle.getLateralPositionOnLane();
1038 1154 : const double overlap = myVehicle.getLateralOverlap();
1039 : double scaledDelta;
1040 1154 : if (overlap > 0) {
1041 : // return to within lane boundary
1042 : scaledDelta = MIN2(overlap, maxDist);
1043 23 : if (myVehicle.getLateralPositionOnLane() > 0) {
1044 7 : scaledDelta *= -1;
1045 : }
1046 : } else {
1047 : // random drift
1048 1131 : double deltaPosLat = OUProcess::step(oldPosLat,
1049 1131 : myVehicle.getActionStepLengthSecs(),
1050 1131 : MAX2(NUMERICAL_EPS, (1 - mySigma) * 100), mySigma) - oldPosLat;
1051 1131 : deltaPosLat = MAX2(MIN2(deltaPosLat, maxDist), -maxDist);
1052 1131 : scaledDelta = deltaPosLat * myVehicle.getSpeed() / myVehicle.getLane()->getSpeedLimit();
1053 : }
1054 1154 : myVehicle.setLateralPositionOnLane(oldPosLat + scaledDelta);
1055 1154 : setSpeedLat(DIST2SPEED(scaledDelta));
1056 : } else {
1057 531902618 : resetSpeedLat();
1058 : }
1059 531903772 : }
1060 :
1061 :
1062 : void
1063 293729876 : MSLCM_LC2013::changed() {
1064 293729876 : myOwnState = 0;
1065 293729876 : mySpeedGainProbabilityLeft = 0;
1066 293729876 : mySpeedGainProbabilityRight = 0;
1067 293729876 : myKeepRightProbability = 0;
1068 293729876 : if (myVehicle.getBestLaneOffset() == 0) {
1069 : // if we are not yet on our best lane there might still be unseen blockers
1070 : // (during patchSpeed)
1071 293604381 : myLeadingBlockerLength = 0;
1072 293604381 : myLeftSpace = 0;
1073 : }
1074 293729876 : myLookAheadSpeed = LOOK_AHEAD_MIN_SPEED;
1075 : myLCAccelerationAdvices.clear();
1076 293729876 : myDontBrake = false;
1077 293729876 : myLeadingBlockerLength = 0;
1078 293729876 : }
1079 :
1080 :
1081 : void
1082 8073 : MSLCM_LC2013::resetState() {
1083 8073 : myOwnState = 0;
1084 8073 : mySpeedGainProbabilityLeft = 0;
1085 8073 : mySpeedGainProbabilityRight = 0;
1086 8073 : myKeepRightProbability = 0;
1087 8073 : myLeadingBlockerLength = 0;
1088 8073 : myLeftSpace = 0;
1089 8073 : myLookAheadSpeed = LOOK_AHEAD_MIN_SPEED;
1090 : myLCAccelerationAdvices.clear();
1091 8073 : myDontBrake = false;
1092 8073 : }
1093 :
1094 :
1095 : int
1096 235400955 : MSLCM_LC2013::_wantsChange(
1097 : int laneOffset,
1098 : MSAbstractLaneChangeModel::MSLCMessager& msgPass,
1099 : int blocked,
1100 : const std::pair<MSVehicle*, double>& leader,
1101 : const std::pair<MSVehicle*, double>& follower,
1102 : const std::pair<MSVehicle*, double>& neighLead,
1103 : const std::pair<MSVehicle*, double>& neighFollow,
1104 : const MSLane& neighLane,
1105 : const std::vector<MSVehicle::LaneQ>& preb,
1106 : MSVehicle* lastBlocked,
1107 : MSVehicle* firstBlocked) {
1108 : assert(laneOffset == 1 || laneOffset == -1);
1109 235400955 : const SUMOTime currentTime = MSNet::getInstance()->getCurrentTimeStep();
1110 : // compute bestLaneOffset
1111 : MSVehicle::LaneQ curr, neigh, best;
1112 : int bestLaneOffset = 0;
1113 : // What do these "dists" mean? Please comment. (Leo) Ad: I now think the following:
1114 : // currentDist is the distance that the vehicle can go on its route without having to
1115 : // change lanes from the current lane. neighDist as currentDist for the considered target lane (i.e., neigh)
1116 : // If this is true I suggest to put this into the docu of wantsChange()
1117 : double currentDist = 0;
1118 : double neighDist = 0;
1119 : int currIdx = 0;
1120 235400955 : const bool checkOpposite = &neighLane.getEdge() != &myVehicle.getLane()->getEdge();
1121 235400955 : const MSLane* prebLane = myVehicle.getLane();
1122 235400955 : if (prebLane->getEdge().isInternal()) {
1123 : // internal edges are not kept inside the bestLanes structure
1124 1113240 : if (isOpposite()) {
1125 553 : prebLane = prebLane->getNormalPredecessorLane();
1126 : } else {
1127 1112687 : prebLane = prebLane->getLinkCont()[0]->getLane();
1128 : }
1129 : }
1130 : // special case: vehicle considers changing to the opposite direction edge
1131 : const int prebOffset = laneOffset;
1132 410091128 : for (int p = 0; p < (int) preb.size(); ++p) {
1133 : //if (DEBUG_COND) {
1134 : // std::cout << " p=" << p << " prebLane=" << prebLane->getID() << " preb.p=" << preb[p].lane->getID() << "\n";
1135 : //}
1136 410091128 : if (preb[p].lane == prebLane && p + laneOffset >= 0) {
1137 : assert(p + prebOffset < (int)preb.size());
1138 : curr = preb[p];
1139 235400955 : neigh = preb[p + prebOffset];
1140 235400955 : currentDist = curr.length;
1141 235400955 : neighDist = neigh.length;
1142 235400955 : bestLaneOffset = curr.bestLaneOffset;
1143 235400955 : if (bestLaneOffset == 0 && preb[p + prebOffset].bestLaneOffset == 0 && !checkOpposite) {
1144 : #ifdef DEBUG_WANTS_CHANGE
1145 : if (DEBUG_COND) {
1146 : std::cout << STEPS2TIME(currentTime)
1147 : << " veh=" << myVehicle.getID()
1148 : << " bestLaneOffsetOld=" << bestLaneOffset
1149 : << " bestLaneOffsetNew=" << laneOffset
1150 : << "\n";
1151 : }
1152 : #endif
1153 : bestLaneOffset = prebOffset;
1154 : }
1155 235400955 : best = preb[p + bestLaneOffset];
1156 : currIdx = p;
1157 : break;
1158 : }
1159 : }
1160 : assert(curr.lane != nullptr);
1161 : assert(neigh.lane != nullptr);
1162 : assert(best.lane != nullptr);
1163 : // direction specific constants
1164 235400955 : const bool right = (laneOffset == -1);
1165 235400955 : const double posOnLane = getForwardPos();
1166 : double driveToNextStop = -std::numeric_limits<double>::max();
1167 235400955 : if (myVehicle.nextStopDist() < std::numeric_limits<double>::max()
1168 235400955 : && &myVehicle.getNextStop().lane->getEdge() == &myVehicle.getLane()->getEdge()) {
1169 : // vehicle can always drive up to stop distance
1170 : // @note this information is dynamic and thus not available in updateBestLanes()
1171 : // @note: nextStopDist was compute before the vehicle moved
1172 1656386 : driveToNextStop = myVehicle.nextStopDist();
1173 1656386 : const double stopPos = posOnLane + myVehicle.nextStopDist() - myVehicle.getLastStepDist();
1174 : #ifdef DEBUG_WANTS_CHANGE
1175 : if (DEBUG_COND) {
1176 : std::cout << SIMTIME << std::setprecision(gPrecision) << " veh=" << myVehicle.getID()
1177 : << " stopDist=" << myVehicle.nextStopDist()
1178 : << " lastDist=" << myVehicle.getLastStepDist()
1179 : << " stopPos=" << stopPos
1180 : << " currentDist=" << currentDist
1181 : << " neighDist=" << neighDist
1182 : << "\n";
1183 : }
1184 : #endif
1185 : currentDist = MAX2(currentDist, stopPos);
1186 : neighDist = MAX2(neighDist, stopPos);
1187 : }
1188 235400955 : const int lca = (right ? LCA_RIGHT : LCA_LEFT);
1189 : const int myLca = (right ? LCA_MRIGHT : LCA_MLEFT);
1190 : const int lcaCounter = (right ? LCA_LEFT : LCA_RIGHT);
1191 235400955 : bool changeToBest = (right && bestLaneOffset < 0) || (!right && bestLaneOffset > 0);
1192 : // keep information about being a leader/follower
1193 235400955 : int ret = (myOwnState & 0xffff0000);
1194 : int req = 0; // the request to change or stay
1195 :
1196 235400955 : ret = slowDownForBlocked(lastBlocked, ret);
1197 235400955 : if (lastBlocked != firstBlocked) {
1198 7220426 : ret = slowDownForBlocked(firstBlocked, ret);
1199 : }
1200 :
1201 : #ifdef DEBUG_WANTS_CHANGE
1202 : if (DEBUG_COND) {
1203 : std::cout << SIMTIME
1204 : << " veh=" << myVehicle.getID()
1205 : << " _wantsChange state=" << myOwnState
1206 : << " myLCAccelerationAdvices=" << toString(myLCAccelerationAdvices)
1207 : << " firstBlocked=" << Named::getIDSecure(firstBlocked)
1208 : << " lastBlocked=" << Named::getIDSecure(lastBlocked)
1209 : << " leader=" << Named::getIDSecure(leader.first)
1210 : << " leaderGap=" << leader.second
1211 : << " follower=" << Named::getIDSecure(follower.first)
1212 : << " followerGap=" << follower.second
1213 : << " neighLead=" << Named::getIDSecure(neighLead.first)
1214 : << " neighLeadGap=" << neighLead.second
1215 : << " neighFollow=" << Named::getIDSecure(neighFollow.first)
1216 : << " neighFollowGap=" << neighFollow.second
1217 : << "\n";
1218 : }
1219 : #endif
1220 :
1221 : // we try to estimate the distance which is necessary to get on a lane
1222 : // we have to get on in order to keep our route
1223 : // we assume we need something that depends on our velocity
1224 : // and compare this with the free space on our wished lane
1225 : //
1226 : // if the free space is somehow(<-?) less than the space we need, we should
1227 : // definitely try to get to the desired lane
1228 : //
1229 : // this rule forces our vehicle to change the lane if a lane changing is necessary soon
1230 :
1231 :
1232 : // we do not want the lookahead distance to change all the time so we let it decay slowly
1233 : // (in contrast, growth is applied instantaneously)
1234 235400955 : if (myVehicle.getSpeed() > myLookAheadSpeed) {
1235 40672558 : myLookAheadSpeed = myVehicle.getSpeed();
1236 : } else {
1237 : // memory decay factor for this action step
1238 194728397 : const double memoryFactor = 1. - (1. - LOOK_AHEAD_SPEED_MEMORY) * myVehicle.getActionStepLengthSecs();
1239 : assert(memoryFactor > 0.);
1240 194728397 : myLookAheadSpeed = MAX2(LOOK_AHEAD_MIN_SPEED,
1241 194728397 : (memoryFactor * myLookAheadSpeed + (1 - memoryFactor) * myVehicle.getSpeed()));
1242 : }
1243 235400955 : double laDist = myLookAheadSpeed * LOOK_FORWARD * myStrategicParam * (right ? 1 : myLookaheadLeft);
1244 235400955 : laDist += myVehicle.getVehicleType().getLengthWithGap() * 2.;
1245 235400955 : const bool hasStoppedLeader = leader.first != 0 && leader.first->isStopped() && leader.second < (currentDist - posOnLane);
1246 235400955 : const bool hasBidiLeader = myVehicle.getLane()->getBidiLane() != nullptr && MSLCHelper::isBidiLeader(leader.first, curr.bestContinuations);
1247 235400955 : const bool hasBidiNeighLeader = neighLane.getBidiLane() != nullptr && MSLCHelper::isBidiLeader(neighLead.first, neigh.bestContinuations);
1248 :
1249 235400955 : if (bestLaneOffset == 0 && hasBidiLeader) {
1250 : // getting out of the way is enough to clear the blockage
1251 : laDist = 0;
1252 235399620 : } else if (bestLaneOffset == 0 && hasStoppedLeader) {
1253 : // react to a stopped leader on the current lane
1254 : // The value of laDist is doubled below for the check whether the lc-maneuver can be taken out
1255 : // on the remaining distance (because the vehicle has to change back and forth). Therefore multiply with 0.5.
1256 69589 : laDist = 0.5 * (myVehicle.getVehicleType().getLengthWithGap()
1257 69589 : + leader.first->getVehicleType().getLengthWithGap()
1258 69589 : + leader.second);
1259 235330031 : } else if (bestLaneOffset == laneOffset && neighLead.first != 0 && (neighLead.first->isStopped() || hasBidiNeighLeader) && neighLead.second < (currentDist - posOnLane)) {
1260 : // react to a stopped leader on the target lane (if it is the bestLane)
1261 274083 : if (isOpposite()) {
1262 : // always allow changing back
1263 51417 : laDist = (myVehicle.getVehicleType().getLengthWithGap()
1264 51417 : + neighLead.first->getVehicleType().getLengthWithGap()
1265 51417 : + neighLead.second);
1266 222666 : } else if (!hasStoppedLeader &&
1267 203111 : ((neighLead.second + myVehicle.getVehicleType().getLengthWithGap() + neighLead.first->getVehicleType().getLengthWithGap()) < (currentDist - posOnLane)
1268 5315 : || hasBidiNeighLeader)) {
1269 : // do not change to the target lane until passing the stopped vehicle
1270 : // (unless the vehicle blocks our intended stopping position, then we have to wait anyway)
1271 : changeToBest = false;
1272 : }
1273 : }
1274 235400955 : if (myStrategicParam < 0) {
1275 : laDist = -1e3; // never perform strategic change
1276 : }
1277 :
1278 : // free space that is available for changing
1279 : //const double neighSpeed = (neighLead.first != 0 ? neighLead.first->getSpeed() :
1280 : // neighFollow.first != 0 ? neighFollow.first->getSpeed() :
1281 : // best.lane->getSpeedLimit());
1282 : // @note: while this lets vehicles change earlier into the correct direction
1283 : // it also makes the vehicles more "selfish" and prevents changes which are necessary to help others
1284 :
1285 :
1286 :
1287 : // Next we assign to roundabout edges a larger distance than to normal edges
1288 : // in order to decrease sense of lc urgency and induce higher usage of inner roundabout lanes.
1289 235400955 : const double roundaboutBonus = MSLCHelper::getRoundaboutDistBonus(myVehicle, myRoundaboutBonus, curr, neigh, best);
1290 235400955 : currentDist += roundaboutBonus;
1291 235400955 : neighDist += roundaboutBonus;
1292 :
1293 235400955 : const double usableDist = MAX2(currentDist - posOnLane - best.occupation * JAM_FACTOR, driveToNextStop);
1294 : //- (best.lane->getVehicleNumber() * neighSpeed)); // VARIANT 9 jfSpeed
1295 235400955 : const double maxJam = MAX2(preb[currIdx + prebOffset].occupation, preb[currIdx].occupation);
1296 235400955 : const double vMax = myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle);
1297 235400955 : const double neighVMax = neighLane.getVehicleMaxSpeed(&myVehicle);
1298 : // upper bound which will be restricted successively
1299 235400955 : double thisLaneVSafe = vMax;
1300 235400955 : const bool checkOverTakeRight = avoidOvertakeRight();
1301 :
1302 235400955 : double neighLeftPlace = MAX2(0.0, neighDist - posOnLane - maxJam);
1303 235400955 : if (neighLead.first != 0 && neighLead.first->isStopped()) {
1304 241492 : neighLeftPlace = MIN2(neighLeftPlace, neighLead.second);
1305 : }
1306 :
1307 : #ifdef DEBUG_WANTS_CHANGE
1308 : if (DEBUG_COND) {
1309 : std::cout << STEPS2TIME(currentTime)
1310 : << " veh=" << myVehicle.getID()
1311 : << " laSpeed=" << myLookAheadSpeed
1312 : << " laDist=" << laDist
1313 : << " currentDist=" << currentDist
1314 : << " usableDist=" << usableDist
1315 : << " bestLaneOffset=" << bestLaneOffset
1316 : << " best.occupation=" << best.occupation
1317 : << " best.length=" << best.length
1318 : << "\n roundaboutBonus=" << roundaboutBonus
1319 : << " maxJam=" << maxJam
1320 : << " neighDist=" << neighDist
1321 : << " neighLeftPlace=" << neighLeftPlace
1322 : << (hasBidiLeader ? " bidiLeader" : "")
1323 : << (hasBidiNeighLeader ? " bidiNeighLeader" : "")
1324 : << "\n";
1325 : }
1326 : #endif
1327 :
1328 : bool changeLeftToAvoidOvertakeRight = false;
1329 195074514 : if (changeToBest && bestLaneOffset == curr.bestLaneOffset
1330 246285813 : && currentDistDisallows(usableDist, bestLaneOffset, laDist)) {
1331 : /// @brief we urgently need to change lanes to follow our route
1332 4327193 : ret = ret | lca | LCA_STRATEGIC | LCA_URGENT;
1333 : } else {
1334 : // VARIANT_20 (noOvertakeRight)
1335 231073762 : if (neighLead.first != 0 && checkOverTakeRight && !right) {
1336 : // check for slower leader on the left. we should not overtake but
1337 : // rather move left ourselves (unless congested)
1338 : MSVehicle* nv = neighLead.first;
1339 11351722 : const double deltaV = MAX2(vMax - neighLane.getVehicleMaxSpeed(nv),
1340 11351722 : myVehicle.getSpeed() - nv->getSpeed());
1341 11351722 : if (deltaV > 0) {
1342 2291817 : const double vMaxDecel = getCarFollowModel().getSpeedAfterMaxDecel(myVehicle.getSpeed());
1343 2291817 : const double vSafeFollow = getCarFollowModel().followSpeed(
1344 2291817 : &myVehicle, myVehicle.getSpeed(), neighLead.second, nv->getSpeed(), nv->getCarFollowModel().getMaxDecel());
1345 2291817 : const double vStayBehind = nv->getSpeed() - HELP_OVERTAKE;
1346 : double vSafe;
1347 2291817 : if (vSafeFollow >= vMaxDecel) {
1348 : vSafe = vSafeFollow;
1349 : } else {
1350 : vSafe = MAX2(vMaxDecel, vStayBehind);
1351 : }
1352 2291817 : if (mySpeedGainProbabilityLeft < myChangeProbThresholdLeft) {
1353 1552336 : vSafe = MAX2(vSafe, nv->getSpeed());
1354 : }
1355 2291817 : thisLaneVSafe = MIN2(thisLaneVSafe, vSafe);
1356 2291817 : addLCSpeedAdvice(vSafe);
1357 : // only generate impulse for overtaking left shortly before braking would be necessary
1358 2291817 : const double deltaGapFuture = deltaV * 8;
1359 2291817 : const double vSafeFuture = getCarFollowModel().followSpeed(
1360 2291817 : &myVehicle, myVehicle.getSpeed(), neighLead.second - deltaGapFuture, nv->getSpeed(), nv->getCarFollowModel().getMaxDecel());
1361 2291817 : if (vSafeFuture < vSafe) {
1362 : const double relativeGain = deltaV / MAX2(vMax,
1363 1886706 : RELGAIN_NORMALIZATION_MIN_SPEED);
1364 1886706 : mySpeedGainProbabilityLeft += myVehicle.getActionStepLengthSecs() * relativeGain;
1365 : changeLeftToAvoidOvertakeRight = true;
1366 : }
1367 : #ifdef DEBUG_WANTS_CHANGE
1368 : if (DEBUG_COND) {
1369 : std::cout << STEPS2TIME(currentTime)
1370 : << " avoid overtaking on the right nv=" << nv->getID()
1371 : << " deltaV=" << deltaV
1372 : << " nvSpeed=" << nv->getSpeed()
1373 : << " speedGainL=" << mySpeedGainProbabilityLeft
1374 : << " speedGainR=" << mySpeedGainProbabilityRight
1375 : << " planned acceleration =" << myLCAccelerationAdvices.back().first
1376 : << "\n";
1377 : }
1378 : #endif
1379 : }
1380 : }
1381 231073762 : const bool currFreeUntilNeighEnd = leader.first == nullptr || neighDist - posOnLane <= leader.second;
1382 446808062 : const double overtakeDist = (leader.first == 0 || hasBidiLeader ? -1 :
1383 215734300 : leader.second + myVehicle.getVehicleType().getLength() + leader.first->getVehicleType().getLengthWithGap());
1384 231311108 : const double overtakeDist2 = (neighLead.first == 0 || !neighLead.first->isStopped() ? -1 :
1385 237346 : neighLead.second + myVehicle.getVehicleType().getLength() + neighLead.first->getVehicleType().getLengthWithGap());
1386 215739655 : if (leader.first != 0 && (leader.first->isStopped() || hasBidiLeader) && leader.second < REACT_TO_STOPPED_DISTANCE
1387 : // current destination leaves enough space to overtake the leader
1388 112539 : && MIN2(neighDist, currentDist) - posOnLane > overtakeDist
1389 : // maybe do not overtake on the right at high speed
1390 78558 : && (!checkOverTakeRight || !right)
1391 76872 : && myStrategicParam >= 0
1392 231150046 : && (neighLead.first == 0 || !neighLead.first->isStopped()
1393 : // neighboring stopped vehicle leaves enough space to overtake leader
1394 21419 : || neighLead.second > overtakeDist
1395 : // if we cannot pass neighLead before reaching leader we must find another free lane
1396 18581 : || (overtakeDist2 > leader.second && hasFreeLane(laneOffset, neighLead)))) {
1397 : // avoid becoming stuck behind a stopped leader
1398 58207 : currentDist = myVehicle.getPositionOnLane() + leader.second;
1399 : #ifdef DEBUG_WANTS_CHANGE
1400 : if (DEBUG_COND) {
1401 : std::cout << " veh=" << myVehicle.getID() << " overtake stopped leader=" << leader.first->getID()
1402 : << " overtakeDist=" << overtakeDist
1403 : << " overtakeDist2=" << overtakeDist
1404 : << " hasFreeLane=" << hasFreeLane(laneOffset, neighLead)
1405 : << " remaining=" << MIN2(neighDist, currentDist) - posOnLane
1406 : << "\n";
1407 : }
1408 : #endif
1409 58207 : ret = ret | lca | LCA_STRATEGIC | LCA_URGENT;
1410 231015555 : } else if (!changeToBest && currentDistDisallows(neighLeftPlace, abs(bestLaneOffset) + 2, laDist) && !hasBidiLeader) {
1411 : // the opposite lane-changing direction should be done than the one examined herein
1412 : // we'll check whether we assume we could change anyhow and get back in time...
1413 : //
1414 : // this rule prevents the vehicle from moving in opposite direction of the best lane
1415 : // unless the way till the end where the vehicle has to be on the best lane
1416 : // is long enough
1417 : #ifdef DEBUG_WANTS_CHANGE
1418 : if (DEBUG_COND) {
1419 : std::cout << " veh=" << myVehicle.getID() << " could not change back and forth in time (1) neighLeftPlace=" << neighLeftPlace << "\n";
1420 : }
1421 : #endif
1422 35894662 : ret = ret | LCA_STAY | LCA_STRATEGIC;
1423 195120893 : } else if (bestLaneOffset == 0 && (neighLeftPlace * 2. < laDist)) {
1424 : // the current lane is the best and a lane-changing would cause a situation
1425 : // of which we assume we will not be able to return to the lane we have to be on.
1426 : // this rule prevents the vehicle from leaving the current, best lane when it is
1427 : // close to this lane's end
1428 : #ifdef DEBUG_WANTS_CHANGE
1429 : if (DEBUG_COND) {
1430 : std::cout << " veh=" << myVehicle.getID() << " could not change back and forth in time (2) neighLeftPlace=" << neighLeftPlace << "\n";
1431 : }
1432 : #endif
1433 0 : ret = ret | LCA_STAY | LCA_STRATEGIC;
1434 : } else if (bestLaneOffset == 0
1435 3984362 : && (leader.first == 0 || !leader.first->isStopped())
1436 3974892 : && !hasBidiLeader
1437 3974889 : && neigh.bestContinuations.back()->getLinkCont().size() != 0
1438 2308846 : && roundaboutBonus == 0
1439 806695 : && !checkOpposite
1440 702534 : && ((myStrategicParam >= 0 && neighDist < TURN_LANE_DIST)
1441 : // lane changing cannot possibly help
1442 657412 : || (myStrategicParam < 0 && currFreeUntilNeighEnd))
1443 : ) {
1444 : // VARIANT_21 (stayOnBest)
1445 : // we do not want to leave the best lane for a lane which leads elsewhere
1446 : // unless our leader is stopped or we are approaching a roundabout
1447 : #ifdef DEBUG_WANTS_CHANGE
1448 : if (DEBUG_COND) {
1449 : std::cout << " veh=" << myVehicle.getID() << " does not want to leave the bestLane (neighDist=" << neighDist << ")\n";
1450 : }
1451 : #endif
1452 45200 : ret = ret | LCA_STAY | LCA_STRATEGIC;
1453 : }
1454 : }
1455 : // check for overriding TraCI requests
1456 : #ifdef DEBUG_WANTS_CHANGE
1457 : if (DEBUG_COND) {
1458 : std::cout << STEPS2TIME(currentTime) << " veh=" << myVehicle.getID() << " ret=" << toString((LaneChangeAction)ret);
1459 : }
1460 : #endif
1461 : // store state before canceling
1462 235400955 : getCanceledState(laneOffset) |= ret | blocked;
1463 235400955 : ret = myVehicle.influenceChangeDecision(ret);
1464 235400955 : if ((ret & lcaCounter) != 0) {
1465 : // we are not interested in traci requests for the opposite direction here
1466 30 : ret &= ~(LCA_TRACI | lcaCounter | LCA_URGENT);
1467 : }
1468 : #ifdef DEBUG_WANTS_CHANGE
1469 : if (DEBUG_COND) {
1470 : std::cout << " retAfterInfluence=" << toString((LaneChangeAction)ret) << "\n";
1471 : }
1472 : #endif
1473 :
1474 235400955 : if ((ret & LCA_STAY) != 0) {
1475 : // remove TraCI flags because it should not be included in "state-without-traci"
1476 36018649 : ret = getCanceledState(laneOffset);
1477 36018649 : return ret;
1478 : }
1479 199382306 : if ((ret & LCA_URGENT) != 0) {
1480 : // prepare urgent lane change maneuver
1481 : // save the left space
1482 4385120 : myLeftSpace = currentDist - posOnLane;
1483 4385120 : if (changeToBest && abs(bestLaneOffset) > 1 && myVehicle.getNumRemainingEdges() > 1) {
1484 : // there might be a vehicle which needs to counter-lane-change one lane further and we cannot see it yet
1485 616684 : myLeadingBlockerLength = MAX2(getExtraReservation(bestLaneOffset, neighDist - currentDist), myLeadingBlockerLength);
1486 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
1487 : if (DEBUG_COND) {
1488 : std::cout << " reserving space for unseen blockers myLeadingBlockerLength=" << myLeadingBlockerLength << "\n";
1489 : }
1490 : #endif
1491 : }
1492 :
1493 : // letting vehicles merge in at the end of the lane in case of counter-lane change, step#1
1494 : // if there is a leader and he wants to change to the opposite direction
1495 4385120 : const bool canContinue = curr.bestContinuations.size() > 1;
1496 4385120 : bool canReserve = MSLCHelper::updateBlockerLength(myVehicle, neighLead.first, lcaCounter, myLeftSpace - POSITION_EPS, canContinue, myLeadingBlockerLength);
1497 4385120 : if (firstBlocked != neighLead.first) {
1498 4129951 : canReserve &= MSLCHelper::updateBlockerLength(myVehicle, firstBlocked, lcaCounter, myLeftSpace - POSITION_EPS, canContinue, myLeadingBlockerLength);
1499 : }
1500 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
1501 : if (DEBUG_COND) {
1502 : std::cout << SIMTIME << " canReserve=" << canReserve << " canContinue=" << canContinue << "\n";
1503 : }
1504 : #endif
1505 4385120 : if (!canReserve && !isOpposite()) {
1506 : // we have a low-priority relief connection
1507 : // std::cout << SIMTIME << " veh=" << myVehicle.getID() << " cannotReserve for blockers\n";
1508 12184 : myDontBrake = canContinue;
1509 : }
1510 :
1511 4385120 : const int remainingLanes = MAX2(1, abs(bestLaneOffset));
1512 4385120 : const double urgency = isOpposite() ? OPPOSITE_URGENCY : URGENCY;
1513 4385120 : const double remainingSeconds = ((ret & LCA_TRACI) == 0 ?
1514 : //MAX2(STEPS2TIME(TS), (myLeftSpace-myLeadingBlockerLength) / MAX2(myLookAheadSpeed, NUMERICAL_EPS) / remainingLanes / urgency) :
1515 4577556 : MAX2(STEPS2TIME(TS), myLeftSpace / MAX2(myLookAheadSpeed, NUMERICAL_EPS) / remainingLanes / urgency) :
1516 220 : myVehicle.getInfluencer().changeRequestRemainingSeconds(currentTime));
1517 4385120 : if (!hasBidiNeighLeader) {
1518 4385066 : const double plannedSpeed = informLeader(msgPass, blocked, myLca, neighLead, remainingSeconds);
1519 : // NOTE: for the ballistic update case negative speeds may indicate a stop request,
1520 : // while informLeader returns -1 in that case. Refs. #2577
1521 4385066 : if (plannedSpeed >= 0 || (!MSGlobals::gSemiImplicitEulerUpdate && plannedSpeed != -1)) {
1522 : // maybe we need to deal with a blocking follower
1523 2514662 : const bool hasBidiNeighFollower = neighLane.getBidiLane() != nullptr && MSLCHelper::isBidiFollower(&myVehicle, neighFollow.first);
1524 : if (!hasBidiNeighFollower) {
1525 2511653 : informFollower(msgPass, blocked, myLca, neighFollow, remainingSeconds, plannedSpeed);
1526 : }
1527 : }
1528 : #ifdef DEBUG_WANTS_CHANGE
1529 : if (DEBUG_COND) {
1530 : std::cout << STEPS2TIME(currentTime)
1531 : << " veh=" << myVehicle.getID()
1532 : << " myLeftSpace=" << myLeftSpace
1533 : << " remainingSeconds=" << remainingSeconds
1534 : << " plannedSpeed=" << plannedSpeed
1535 : << "\n";
1536 : }
1537 : #endif
1538 : } else {
1539 : #ifdef DEBUG_WANTS_CHANGE
1540 : if (DEBUG_COND) {
1541 : std::cout << STEPS2TIME(currentTime)
1542 : << " veh=" << myVehicle.getID()
1543 : << " myLeftSpace=" << myLeftSpace
1544 : << " remainingSeconds=" << remainingSeconds
1545 : << " hasBidiNeighLeader\n";
1546 : }
1547 : #endif
1548 : }
1549 :
1550 :
1551 : // remove TraCI flags because it should not be included in "state-without-traci"
1552 4385120 : ret = getCanceledState(laneOffset);
1553 4385120 : return ret;
1554 : }
1555 :
1556 : // we wish to anticipate future speeds. This is difficult when the leading
1557 : // vehicles are still accelerating so we resort to comparing speeds for the near future (1s) in this case
1558 183489861 : const bool acceleratingLeader = (neighLead.first != 0 && neighLead.first->getAcceleration() > 0)
1559 284737396 : || (leader.first != 0 && leader.first->getAcceleration() > 0);
1560 194997186 : double neighLaneVSafe = MIN2(neighVMax, anticipateFollowSpeed(neighLead, neighDist, neighVMax, acceleratingLeader));
1561 194997186 : thisLaneVSafe = MIN2(thisLaneVSafe, anticipateFollowSpeed(leader, currentDist, vMax, acceleratingLeader));
1562 : //std::cout << SIMTIME << " veh=" << myVehicle.getID() << " thisLaneVSafe=" << thisLaneVSafe << " neighLaneVSafe=" << neighLaneVSafe << "\n";
1563 :
1564 :
1565 : // a high inconvenience prevents cooperative changes and the following things are inconvenient:
1566 : // - a desire to change in the opposite direction for speedGain
1567 : // - low anticipated speed on the neighboring lane
1568 : // - high occupancy on the neighboring lane while in a roundabout
1569 :
1570 : double inconvenience = laneOffset < 0
1571 194997186 : ? mySpeedGainProbabilityLeft / myChangeProbThresholdRight
1572 100705023 : : mySpeedGainProbabilityRight / myChangeProbThresholdLeft;
1573 :
1574 251360397 : const double relSpeedDiff = thisLaneVSafe == 0 ? 0 : (thisLaneVSafe - neighLaneVSafe) / MAX2(thisLaneVSafe, neighLaneVSafe);
1575 : inconvenience = MAX2(relSpeedDiff, inconvenience);
1576 : inconvenience = MIN2(1.0, inconvenience);
1577 :
1578 194997186 : const bool speedGainInconvenient = inconvenience > myCooperativeParam;
1579 194997186 : const bool neighOccupancyInconvenient = neigh.lane->getBruttoOccupancy() > curr.lane->getBruttoOccupancy();
1580 : #ifdef DEBUG_WANTS_CHANGE
1581 : if (DEBUG_COND) {
1582 : std::cout << STEPS2TIME(currentTime)
1583 : << " veh=" << myVehicle.getID()
1584 : << " speedGainL=" << mySpeedGainProbabilityLeft
1585 : << " speedGainR=" << mySpeedGainProbabilityRight
1586 : << " neighSpeedFactor=" << (thisLaneVSafe / neighLaneVSafe - 1)
1587 : << " inconvenience=" << inconvenience
1588 : << " speedInconv=" << speedGainInconvenient
1589 : << " occInconv=" << neighOccupancyInconvenient
1590 : << "\n";
1591 : }
1592 : #endif
1593 :
1594 : // VARIANT_15
1595 194997186 : if (roundaboutBonus > 0) {
1596 :
1597 : #ifdef DEBUG_WANTS_CHANGE
1598 : if (DEBUG_COND) {
1599 : std::cout << STEPS2TIME(currentTime)
1600 : << " veh=" << myVehicle.getID()
1601 : << " roundaboutBonus=" << roundaboutBonus
1602 : << " myLeftSpace=" << myLeftSpace
1603 : << "\n";
1604 : }
1605 : #endif
1606 : // try to use the inner lanes of a roundabout to increase throughput
1607 : // unless we are approaching the exit
1608 4688318 : if (lca == LCA_LEFT) {
1609 : // if inconvenience is not too high, request collaborative change (currently only for ballistic update)
1610 : // TODO: test this for euler update! Refs. #2575
1611 805446 : if (MSGlobals::gSemiImplicitEulerUpdate || !neighOccupancyInconvenient) {
1612 : // if(MSGlobals::gSemiImplicitEulerUpdate || !speedGainInconvenient){
1613 805446 : req = ret | lca | LCA_COOPERATIVE;
1614 : }
1615 : } else {
1616 : // if inconvenience is not too high, request collaborative change (currently only for ballistic update)
1617 3882872 : if (MSGlobals::gSemiImplicitEulerUpdate || neighOccupancyInconvenient) {
1618 : // if(MSGlobals::gSemiImplicitEulerUpdate || speedGainInconvenient){
1619 3882872 : req = ret | LCA_STAY | LCA_COOPERATIVE;
1620 : }
1621 : }
1622 4688318 : if (!cancelRequest(req, laneOffset)) {
1623 4688318 : return ret | req;
1624 : }
1625 : }
1626 :
1627 : // let's also regard the case where the vehicle is driving on a highway...
1628 : // in this case, we do not want to get to the dead-end of an on-ramp
1629 190308868 : if (right) {
1630 90409291 : if (bestLaneOffset == 0 && myVehicle.getLane()->getSpeedLimit() > 80. / 3.6 && myLookAheadSpeed > SUMO_const_haltingSpeed) {
1631 : #ifdef DEBUG_WANTS_CHANGE
1632 : if (DEBUG_COND) {
1633 : std::cout << " veh=" << myVehicle.getID() << " does not want to get stranded on the on-ramp of a highway\n";
1634 : }
1635 : #endif
1636 87560 : req = ret | LCA_STAY | LCA_STRATEGIC;
1637 87560 : if (!cancelRequest(req, laneOffset)) {
1638 : return ret | req;
1639 : }
1640 : }
1641 : }
1642 : // --------
1643 :
1644 : // -------- make place on current lane if blocking follower
1645 : //if (amBlockingFollowerPlusNB()) {
1646 : // std::cout << myVehicle.getID() << ", " << currentDistAllows(neighDist, bestLaneOffset, laDist)
1647 : // << " neighDist=" << neighDist
1648 : // << " currentDist=" << currentDist
1649 : // << "\n";
1650 : //}
1651 :
1652 : if (amBlockingFollowerPlusNB()
1653 137493 : && (!speedGainInconvenient)
1654 136968 : && ((myOwnState & myLca) != 0) // VARIANT_6 : counterNoHelp
1655 190266782 : && (changeToBest || currentDistAllows(neighDist, abs(bestLaneOffset) + 1, laDist))) {
1656 :
1657 : // VARIANT_2 (nbWhenChangingToHelp)
1658 : #ifdef DEBUG_COOPERATE
1659 : if (DEBUG_COND) {
1660 : std::cout << STEPS2TIME(currentTime)
1661 : << " veh=" << myVehicle.getID()
1662 : << " wantsChangeToHelp=" << (right ? "right" : "left")
1663 : << " state=" << myOwnState
1664 : << (((myOwnState & myLca) == 0) ? " (counter)" : "")
1665 : << "\n";
1666 : }
1667 : #endif
1668 45449 : req = ret | lca | LCA_COOPERATIVE | LCA_URGENT ;//| LCA_CHANGE_TO_HELP;
1669 45449 : if (!cancelRequest(req, laneOffset)) {
1670 45443 : if ((blocked & LCA_BLOCKED_BY_LEFT_FOLLOWER) && !right && mySpeedGainProbabilityLeft > mySpeedGainUrgency) {
1671 287 : MSVehicle* nv = neighFollow.first;
1672 287 : const bool hasBidiNeighFollower = neighLane.getBidiLane() != nullptr && MSLCHelper::isBidiFollower(&myVehicle, nv);
1673 287 : if (nv != nullptr && !hasBidiNeighFollower) {
1674 287 : const double helpSpeed = MAX2(nv->getCarFollowModel().minNextSpeed(nv->getSpeed(), nv), myVehicle.getSpeed() - 1);
1675 287 : msgPass.informNeighFollower(new Info(helpSpeed, myLca | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
1676 : }
1677 : }
1678 45443 : return ret | req;
1679 : }
1680 : }
1681 :
1682 : // --------
1683 :
1684 :
1685 : //// -------- security checks for krauss
1686 : //// (vsafe fails when gap<0)
1687 : //if ((blocked & LCA_BLOCKED) != 0) {
1688 : // return ret;
1689 : //}
1690 : //// --------
1691 :
1692 : // -------- higher speed
1693 : //if ((congested(neighLead.first) && neighLead.second < 20) || predInteraction(leader.first)) { //!!!
1694 : // return ret;
1695 : //}
1696 :
1697 190175890 : if (neighLane.getEdge().getPersons().size() > 0) {
1698 : // react to pedestrians
1699 47002 : adaptSpeedToPedestrians(myVehicle.getLane(), thisLaneVSafe);
1700 47002 : adaptSpeedToPedestrians(&neighLane, neighLaneVSafe);
1701 : }
1702 :
1703 190175890 : const double relativeGain = (neighLaneVSafe - thisLaneVSafe) / MAX2(neighLaneVSafe,
1704 190175890 : RELGAIN_NORMALIZATION_MIN_SPEED);
1705 :
1706 : #ifdef DEBUG_WANTS_CHANGE
1707 : if (DEBUG_COND) {
1708 : std::cout << STEPS2TIME(currentTime)
1709 : << " veh=" << myVehicle.getID()
1710 : << " currentDist=" << currentDist
1711 : << " neighDist=" << neighDist
1712 : << " thisVSafe=" << thisLaneVSafe
1713 : << " neighVSafe=" << neighLaneVSafe
1714 : << " relGain=" << toString(relativeGain, 8)
1715 : << "\n";
1716 : }
1717 : #endif
1718 :
1719 190175890 : if (right) {
1720 : // ONLY FOR CHANGING TO THE RIGHT
1721 90317604 : if (thisLaneVSafe - 5 / 3.6 > neighLaneVSafe) {
1722 : // ok, the current lane is faster than the right one...
1723 60571519 : mySpeedGainProbabilityRight *= pow(0.5, myVehicle.getActionStepLengthSecs());
1724 : //myKeepRightProbability /= 2.0;
1725 : } else {
1726 : // ok, the current lane is not (much) faster than the right one
1727 29746085 : mySpeedGainProbabilityRight += myVehicle.getActionStepLengthSecs() * relativeGain;
1728 :
1729 : // honor the obligation to keep right (Rechtsfahrgebot)
1730 29746085 : const double roadSpeedFactor = vMax / myVehicle.getLane()->getSpeedLimit(); // differse from speedFactor if vMax < speedLimit
1731 : double acceptanceTime;
1732 29746085 : if (myKeepRightAcceptanceTime == -1) {
1733 : // legacy behavior: scale acceptance time with current speed and
1734 : // use old hard-coded constant
1735 59489562 : acceptanceTime = 7 * roadSpeedFactor * MAX2(1.0, myVehicle.getSpeed());
1736 : } else {
1737 1304 : acceptanceTime = myKeepRightAcceptanceTime * roadSpeedFactor;
1738 1511 : if (follower.first != nullptr && follower.second < 2 * follower.first->getCarFollowModel().brakeGap(follower.first->getSpeed())) {
1739 : // reduce acceptanceTime if the follower vehicle is faster or wants to drive faster
1740 207 : if (follower.first->getSpeed() >= myVehicle.getSpeed()) {
1741 380 : acceptanceTime *= MAX2(1.0, myVehicle.getSpeed()) / MAX2(1.0, follower.first->getSpeed());
1742 190 : const double fRSF = follower.first->getLane()->getVehicleMaxSpeed(follower.first) / follower.first->getLane()->getSpeedLimit();
1743 190 : if (fRSF > roadSpeedFactor) {
1744 120 : acceptanceTime /= fRSF;
1745 : }
1746 : }
1747 : }
1748 : }
1749 29746085 : double fullSpeedGap = MAX2(0., neighDist - myVehicle.getCarFollowModel().brakeGap(vMax));
1750 29746085 : double fullSpeedDrivingSeconds = MIN2(acceptanceTime, fullSpeedGap / vMax);
1751 29746085 : if (neighLead.first != 0 && neighLead.first->getSpeed() < vMax) {
1752 23549202 : fullSpeedGap = MAX2(0., MIN2(fullSpeedGap,
1753 23549202 : neighLead.second - myVehicle.getCarFollowModel().getSecureGap(&myVehicle, neighLead.first,
1754 23549202 : vMax, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())));
1755 23549202 : fullSpeedDrivingSeconds = MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - neighLead.first->getSpeed()));
1756 : }
1757 : // stay on the current lane if we cannot overtake a slow leader on the right
1758 4604704 : if (checkOverTakeRight && leader.first != 0
1759 33834288 : && leader.first->getLane()->getVehicleMaxSpeed(leader.first) < vMax) {
1760 1284574 : fullSpeedGap = MIN2(fullSpeedGap, leader.second);
1761 1284574 : fullSpeedDrivingSeconds = MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - leader.first->getSpeed()));
1762 : }
1763 :
1764 29746085 : const double deltaProb = (myChangeProbThresholdRight * (fullSpeedDrivingSeconds / acceptanceTime) / KEEP_RIGHT_TIME);
1765 29746085 : myKeepRightProbability -= myVehicle.getActionStepLengthSecs() * deltaProb;
1766 :
1767 : //std::cout << STEPS2TIME(currentTime)
1768 : // << " veh=" << myVehicle.getID()
1769 : // << " acceptanceTime=" << acceptanceTime
1770 : // << " fullSpeedDrivingSeconds=" << fullSpeedDrivingSeconds
1771 : // << " dProb=" << deltaProb
1772 : // << " myKeepRightProbability=" << myKeepRightProbability
1773 : // << "\n";
1774 :
1775 : #ifdef DEBUG_WANTS_CHANGE
1776 : if (DEBUG_COND) {
1777 : std::cout << STEPS2TIME(currentTime)
1778 : << " veh=" << myVehicle.getID()
1779 : << " vMax=" << vMax
1780 : << " neighDist=" << neighDist
1781 : << " brakeGap=" << myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed())
1782 : << " leaderSpeed=" << (neighLead.first == 0 ? -1 : neighLead.first->getSpeed())
1783 : << " secGap=" << (neighLead.first == 0 ? -1 : myVehicle.getCarFollowModel().getSecureGap(&myVehicle, neighLead.first,
1784 : myVehicle.getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()))
1785 : << " acceptanceTime=" << acceptanceTime
1786 : << " fullSpeedGap=" << fullSpeedGap
1787 : << " fullSpeedDrivingSeconds=" << fullSpeedDrivingSeconds
1788 : << " dProb=" << deltaProb
1789 : << " myKeepRightProbability=" << myKeepRightProbability
1790 : << "\n";
1791 : }
1792 : #endif
1793 29746085 : if (myKeepRightProbability * myKeepRightParam < -myChangeProbThresholdRight) {
1794 3176637 : req = ret | lca | LCA_KEEPRIGHT;
1795 3176637 : if (!cancelRequest(req, laneOffset)) {
1796 3176212 : return ret | req;
1797 : }
1798 : }
1799 : }
1800 :
1801 : #ifdef DEBUG_WANTS_CHANGE
1802 : if (DEBUG_COND) {
1803 : std::cout << STEPS2TIME(currentTime)
1804 : << " veh=" << myVehicle.getID()
1805 : << " speed=" << myVehicle.getSpeed()
1806 : << " speedGainL=" << mySpeedGainProbabilityLeft
1807 : << " speedGainR=" << mySpeedGainProbabilityRight
1808 : << " thisLaneVSafe=" << thisLaneVSafe
1809 : << " neighLaneVSafe=" << neighLaneVSafe
1810 : << " relativeGain=" << relativeGain
1811 : << " blocked=" << blocked
1812 : << "\n";
1813 : }
1814 : #endif
1815 :
1816 87141392 : if (mySpeedGainProbabilityRight > myChangeProbThresholdRight
1817 87703147 : && neighDist / MAX2(.1, myVehicle.getSpeed()) > mySpeedGainRemainTime) { //./MAX2( .1, myVehicle.getSpeed())) { // -.1
1818 541412 : req = ret | lca | LCA_SPEEDGAIN;
1819 541412 : if (mySpeedGainProbabilityRight > mySpeedGainUrgency) {
1820 935 : req |= LCA_URGENT;
1821 : }
1822 541412 : if (!cancelRequest(req, laneOffset)) {
1823 541412 : return ret | req;
1824 : }
1825 : }
1826 : } else {
1827 : // ONLY FOR CHANGING TO THE LEFT
1828 99858286 : if (thisLaneVSafe > neighLaneVSafe) {
1829 : // this lane is better
1830 60076991 : mySpeedGainProbabilityLeft *= pow(0.5, myVehicle.getActionStepLengthSecs());
1831 39781295 : } else if (thisLaneVSafe == neighLaneVSafe) {
1832 19720388 : mySpeedGainProbabilityLeft *= pow(0.8, myVehicle.getActionStepLengthSecs());
1833 : } else {
1834 20060907 : mySpeedGainProbabilityLeft += myVehicle.getActionStepLengthSecs() * relativeGain;
1835 : }
1836 : // VARIANT_19 (stayRight)
1837 : //if (neighFollow.first != 0) {
1838 : // MSVehicle* nv = neighFollow.first;
1839 : // const double secGap = nv->getCarFollowModel().getSecureGap(nv, &myVehicle, nv->getSpeed(), myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel());
1840 : // if (neighFollow.second < secGap * KEEP_RIGHT_HEADWAY) {
1841 : // // do not change left if it would inconvenience faster followers
1842 : // return ret | LCA_STAY | LCA_SPEEDGAIN;
1843 : // }
1844 : //}
1845 :
1846 : #ifdef DEBUG_WANTS_CHANGE
1847 : if (DEBUG_COND) {
1848 : std::cout << STEPS2TIME(currentTime)
1849 : << " veh=" << myVehicle.getID()
1850 : << " speed=" << myVehicle.getSpeed()
1851 : << " speedGainL=" << mySpeedGainProbabilityLeft
1852 : << " speedGainR=" << mySpeedGainProbabilityRight
1853 : << " thisLaneVSafe=" << thisLaneVSafe
1854 : << " neighLaneVSafe=" << neighLaneVSafe
1855 : << " relativeGain=" << relativeGain
1856 : << " blocked=" << blocked
1857 : << "\n";
1858 : }
1859 : #endif
1860 :
1861 99858286 : if (mySpeedGainProbabilityLeft > myChangeProbThresholdLeft
1862 10414119 : && (relativeGain > NUMERICAL_EPS || changeLeftToAvoidOvertakeRight)
1863 115615122 : && neighDist / MAX2(.1, myVehicle.getSpeed()) > mySpeedGainRemainTime) { // .1
1864 7575883 : req = ret | lca | LCA_SPEEDGAIN;
1865 7575883 : if (mySpeedGainProbabilityLeft > mySpeedGainUrgency) {
1866 2255 : req |= LCA_URGENT;
1867 : }
1868 7575883 : if (!cancelRequest(req, laneOffset)) {
1869 7575787 : if ((req & LCA_URGENT) && (blocked & LCA_BLOCKED_BY_LEFT_FOLLOWER)) {
1870 2154 : MSVehicle* nv = neighFollow.first;
1871 2154 : const bool hasBidiNeighFollower = neighLane.getBidiLane() != nullptr && MSLCHelper::isBidiFollower(&myVehicle, nv);
1872 2154 : if (nv != nullptr && !hasBidiNeighFollower) {
1873 2154 : const double helpSpeed = MAX2(nv->getCarFollowModel().minNextSpeed(nv->getSpeed(), nv), myVehicle.getSpeed() - 1);
1874 2154 : msgPass.informNeighFollower(new Info(helpSpeed, myLca | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
1875 : }
1876 : }
1877 7575787 : return ret | req;
1878 : }
1879 : }
1880 : }
1881 : // --------
1882 178882479 : if (changeToBest && bestLaneOffset == curr.bestLaneOffset
1883 2620615 : && myStrategicParam >= 0
1884 2613933 : && relativeGain >= 0
1885 957281 : && (right ? mySpeedGainProbabilityRight : mySpeedGainProbabilityLeft) > 0) {
1886 : // change towards the correct lane, speedwise it does not hurt
1887 132966 : req = ret | lca | LCA_STRATEGIC;
1888 132966 : if (!cancelRequest(req, laneOffset)) {
1889 132966 : return ret | req;
1890 : }
1891 : }
1892 : #ifdef DEBUG_WANTS_CHANGE
1893 : if (DEBUG_COND) {
1894 : std::cout << STEPS2TIME(currentTime)
1895 : << " veh=" << myVehicle.getID()
1896 : << " speedGainL=" << mySpeedGainProbabilityLeft
1897 : << " speedGainR=" << mySpeedGainProbabilityRight
1898 : << " myKeepRightProbability=" << myKeepRightProbability
1899 : << " thisLaneVSafe=" << thisLaneVSafe
1900 : << " neighLaneVSafe=" << neighLaneVSafe
1901 : << "\n";
1902 : }
1903 : #endif
1904 :
1905 : return ret;
1906 : }
1907 :
1908 :
1909 : double
1910 389994372 : MSLCM_LC2013::anticipateFollowSpeed(const std::pair<MSVehicle*, double>& leaderDist, double dist, double vMax, bool acceleratingLeader) {
1911 389994372 : const MSVehicle* leader = leaderDist.first;
1912 389994372 : const double gap = leaderDist.second;
1913 : double futureSpeed;
1914 389994372 : if (acceleratingLeader) {
1915 : // XXX see #6562
1916 273137926 : const double maxSpeed1s = (myVehicle.getSpeed() + myVehicle.getCarFollowModel().getMaxAccel()
1917 273137926 : - ACCEL2SPEED(myVehicle.getCarFollowModel().getMaxAccel()));
1918 273137926 : if (leader == nullptr) {
1919 4850909 : if (hasBlueLight()) {
1920 : // can continue from any lane if necessary
1921 : futureSpeed = vMax;
1922 : } else {
1923 4850867 : futureSpeed = getCarFollowModel().followSpeed(&myVehicle, maxSpeed1s, dist, 0, 0);
1924 : }
1925 : } else {
1926 268287017 : futureSpeed = getCarFollowModel().followSpeed(&myVehicle, maxSpeed1s, gap, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel());
1927 : }
1928 : } else {
1929 : // onInsertion = true because the vehicle has already moved
1930 116856446 : if (leader == nullptr) {
1931 16757203 : if (hasBlueLight()) {
1932 : // can continue from any lane if necessary
1933 : futureSpeed = vMax;
1934 : } else {
1935 16755653 : futureSpeed = getCarFollowModel().maximumSafeStopSpeed(dist, getCarFollowModel().getMaxDecel(), myVehicle.getSpeed(), true);
1936 : }
1937 : } else {
1938 100099243 : futureSpeed = getCarFollowModel().maximumSafeFollowSpeed(gap, myVehicle.getSpeed(), leader->getSpeed(), leader->getCarFollowModel().getMaxDecel(), true);
1939 : }
1940 : }
1941 : futureSpeed = MIN2(vMax, futureSpeed);
1942 389994372 : if (leader != nullptr && gap > 0 && mySpeedGainLookahead > 0) {
1943 1004 : const double futureLeaderSpeed = acceleratingLeader ? leader->getLane()->getVehicleMaxSpeed(leader) : leader->getSpeed();
1944 1004 : const double deltaV = vMax - futureLeaderSpeed;
1945 1004 : if (deltaV > 0 && gap > 0) {
1946 986 : const double secGap = getCarFollowModel().getSecureGap(&myVehicle, leader, futureSpeed, leader->getSpeed(), getCarFollowModel().getMaxDecel());
1947 986 : const double fullSpeedGap = gap - secGap;
1948 986 : if (fullSpeedGap / deltaV < mySpeedGainLookahead) {
1949 : // anticipate future braking by computing the average
1950 : // speed over the next few seconds
1951 : const double gapClosingTime = MAX2(0.0, fullSpeedGap / deltaV);
1952 190 : const double foreCastTime = mySpeedGainLookahead * 2;
1953 : //if (DEBUG_COND) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " leader=" << leader->getID() << " gap=" << gap << " deltaV=" << deltaV << " futureSpeed=" << futureSpeed << " futureLeaderSpeed=" << futureLeaderSpeed;
1954 190 : futureSpeed = MIN2(futureSpeed, (gapClosingTime * futureSpeed + (foreCastTime - gapClosingTime) * futureLeaderSpeed) / foreCastTime);
1955 : //if (DEBUG_COND) std::cout << " newFutureSpeed=" << futureSpeed << "\n";
1956 : }
1957 : }
1958 : }
1959 389994372 : return futureSpeed;
1960 : }
1961 :
1962 :
1963 : int
1964 242621381 : MSLCM_LC2013::slowDownForBlocked(MSVehicle* blocked, int state) {
1965 : // if this vehicle is blocking someone in front, we maybe decelerate to let him in
1966 242621381 : if (blocked != nullptr) {
1967 17539755 : double gap = blocked->getPositionOnLane() - blocked->getVehicleType().getLength() - myVehicle.getPositionOnLane() - myVehicle.getVehicleType().getMinGap();
1968 : #ifdef DEBUG_SLOW_DOWN
1969 : if (DEBUG_COND) {
1970 : std::cout << SIMTIME
1971 : << " veh=" << myVehicle.getID()
1972 : << " blocked=" << Named::getIDSecure(blocked)
1973 : << " gap=" << gap
1974 : << "\n";
1975 : }
1976 : #endif
1977 17539755 : if (gap > POSITION_EPS) {
1978 : //const bool blockedWantsUrgentRight = (((*blocked)->getLaneChangeModel().getOwnState() & LCA_RIGHT != 0)
1979 : // && ((*blocked)->getLaneChangeModel().getOwnState() & LCA_URGENT != 0));
1980 :
1981 14047371 : if (myVehicle.getSpeed() < myVehicle.getCarFollowModel().getMaxDecel()
1982 : //|| blockedWantsUrgentRight // VARIANT_10 (helpblockedRight)
1983 : ) {
1984 12295310 : if (blocked->getSpeed() < SUMO_const_haltingSpeed) {
1985 6824557 : state |= LCA_AMBACKBLOCKER_STANDING;
1986 : } else {
1987 5470753 : state |= LCA_AMBACKBLOCKER;
1988 : }
1989 24590620 : addLCSpeedAdvice(getCarFollowModel().followSpeed(
1990 12295310 : &myVehicle, myVehicle.getSpeed(),
1991 12295310 : gap - POSITION_EPS, blocked->getSpeed(),
1992 : blocked->getCarFollowModel().getMaxDecel()), false);
1993 :
1994 : //(*blocked) = 0; // VARIANT_14 (furtherBlock)
1995 : #ifdef DEBUG_SLOW_DOWN
1996 : if (DEBUG_COND) {
1997 : std::cout << SIMTIME
1998 : << " veh=" << myVehicle.getID()
1999 : << " slowing down for"
2000 : << " blocked=" << Named::getIDSecure(blocked)
2001 : << " helpSpeed=" << myLCAccelerationAdvices.back().first
2002 : << "\n";
2003 : }
2004 : #endif
2005 : } /*else if ((*blocked)->getWaitingSeconds() > 30 && gap > myVehicle.getBrakeGap()) {
2006 : // experimental else-branch...
2007 :
2008 : state |= LCA_AMBACKBLOCKER;
2009 : addLCSpeedAdvice(getCarFollowModel().followSpeed(
2010 : &myVehicle, myVehicle.getSpeed(),
2011 : (gap - POSITION_EPS), (*blocked)->getSpeed(),
2012 : (*blocked)->getCarFollowModel().getMaxDecel()));
2013 : } */
2014 : }
2015 : }
2016 242621381 : return state;
2017 : }
2018 :
2019 :
2020 : void
2021 94004 : MSLCM_LC2013::adaptSpeedToPedestrians(const MSLane* lane, double& v) {
2022 94004 : if (lane->hasPedestrians()) {
2023 : #ifdef DEBUG_WANTS_CHANGE
2024 : if (DEBUG_COND) {
2025 : std::cout << SIMTIME << " adapt to pedestrians on lane=" << lane->getID() << "\n";
2026 : }
2027 : #endif
2028 20794 : PersonDist leader = lane->nextBlocking(myVehicle.getPositionOnLane(),
2029 20794 : myVehicle.getRightSideOnLane(), myVehicle.getRightSideOnLane() + myVehicle.getVehicleType().getWidth(),
2030 20794 : ceil(myVehicle.getSpeed() / myVehicle.getCarFollowModel().getMaxDecel()));
2031 20794 : if (leader.first != 0) {
2032 7757 : const double stopSpeed = myVehicle.getCarFollowModel().stopSpeed(&myVehicle, myVehicle.getSpeed(), leader.second - myVehicle.getVehicleType().getMinGap());
2033 12746 : v = MIN2(v, stopSpeed);
2034 : #ifdef DEBUG_WANTS_CHANGE
2035 : if (DEBUG_COND) {
2036 : std::cout << SIMTIME << " pedLeader=" << leader.first->getID() << " dist=" << leader.second << " v=" << v << "\n";
2037 : }
2038 : #endif
2039 : }
2040 : }
2041 94004 : }
2042 :
2043 :
2044 : double
2045 606062 : MSLCM_LC2013::computeSpeedLat(double latDist, double& maneuverDist, bool urgent) const {
2046 606062 : double result = MSAbstractLaneChangeModel::computeSpeedLat(latDist, maneuverDist, urgent);
2047 : #ifdef DEBUG_WANTS_CHANGE
2048 : if (DEBUG_COND) {
2049 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " myLeftSpace=" << myLeftSpace << " latDist=" << latDist << " maneuverDist=" << maneuverDist << " result=" << result << "\n";
2050 : }
2051 : #endif
2052 606062 : if (myLeftSpace > POSITION_EPS || !urgent) {
2053 492325 : double speedBound = myMaxSpeedLatStanding + myMaxSpeedLatFactor * myVehicle.getSpeed();
2054 492325 : if (isChangingLanes()) {
2055 : speedBound = MAX2(LC_RESOLUTION_SPEED_LAT, speedBound);
2056 : }
2057 492325 : result = MAX2(-speedBound, MIN2(speedBound, result));
2058 : }
2059 606062 : return result;
2060 : }
2061 :
2062 :
2063 : double
2064 499930096 : MSLCM_LC2013::getSafetyFactor() const {
2065 499930096 : return 1 / myAssertive;
2066 : }
2067 :
2068 : double
2069 8491228 : MSLCM_LC2013::getOppositeSafetyFactor() const {
2070 8491228 : return myOppositeParam <= 0 ? std::numeric_limits<double>::max() : 1 / myOppositeParam;
2071 : }
2072 :
2073 : bool
2074 142780 : MSLCM_LC2013::saveBlockerLength(double length, double foeLeftSpace) {
2075 142780 : const bool canReserve = MSLCHelper::canSaveBlockerLength(myVehicle, length, myLeftSpace);
2076 142780 : if (!isOpposite() && (canReserve || myLeftSpace > foeLeftSpace)) {
2077 130512 : myLeadingBlockerLength = MAX2(length, myLeadingBlockerLength);
2078 : #ifdef DEBUG_SAVE_BLOCKER_LENGTH
2079 : if (DEBUG_COND) {
2080 : std::cout << SIMTIME << " saveBlockerLength veh=" << myVehicle.getID() << " canReserve=" << canReserve << " myLeftSpace=" << myLeftSpace << " foeLeftSpace=" << foeLeftSpace << "\n";
2081 : }
2082 : #endif
2083 130512 : if (myLeftSpace == 0 && foeLeftSpace < 0) {
2084 : // called from opposite overtaking, myLeftSpace must be initialized
2085 127728 : myLeftSpace = myVehicle.getBestLanes()[myVehicle.getLane()->getIndex()].length - myVehicle.getPositionOnLane();
2086 : }
2087 130512 : return true;
2088 : } else {
2089 : return false;
2090 : }
2091 : }
2092 :
2093 :
2094 : bool
2095 25456 : MSLCM_LC2013::hasFreeLane(int laneOffset, const std::pair<MSVehicle*, double>& neighLeadStopped) const {
2096 25456 : if (neighLeadStopped.first == nullptr) {
2097 : return true;
2098 : }
2099 25456 : int dir = (laneOffset > 0 ? 1 : -1);
2100 25456 : const MSLane* neigh = myVehicle.getLane()->getParallelLane(laneOffset);
2101 25456 : if (dir > 0 && !neigh->allowsChangingLeft(myVehicle.getVClass())) {
2102 : return false;
2103 25456 : } else if (dir < 0 && !neigh->allowsChangingRight(myVehicle.getVClass())) {
2104 : return false;
2105 : }
2106 25456 : int nextOffset = laneOffset + dir;
2107 25456 : const MSLane* next = myVehicle.getLane()->getParallelLane(nextOffset);
2108 25456 : if (next == nullptr || !next->allowsVehicleClass(myVehicle.getVClass())) {
2109 17953 : return false;
2110 : }
2111 7503 : const double overtakeDist = neighLeadStopped.second + neighLeadStopped.first->getVehicleType().getLengthWithGap() + myVehicle.getLength() + POSITION_EPS;
2112 7503 : std::pair<MSVehicle* const, double> nextLead = next->getLeader(&myVehicle, myVehicle.getPositionOnLane(), myVehicle.getBestLanesContinuation(next), overtakeDist);
2113 8007 : return nextLead.first == nullptr || nextLead.second >= overtakeDist || hasFreeLane(nextOffset, nextLead);
2114 : }
2115 :
2116 :
2117 : std::string
2118 152 : MSLCM_LC2013::getParameter(const std::string& key) const {
2119 152 : if (key == toString(SUMO_ATTR_LCA_STRATEGIC_PARAM)) {
2120 36 : return toString(myStrategicParam);
2121 116 : } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_PARAM)) {
2122 36 : return toString(myCooperativeParam);
2123 80 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_PARAM)) {
2124 36 : return toString(mySpeedGainParam);
2125 44 : } else if (key == toString(SUMO_ATTR_LCA_KEEPRIGHT_PARAM)) {
2126 0 : return toString(myKeepRightParam);
2127 44 : } else if (key == toString(SUMO_ATTR_LCA_OPPOSITE_PARAM)) {
2128 0 : return toString(myOppositeParam);
2129 44 : } else if (key == toString(SUMO_ATTR_LCA_LOOKAHEADLEFT)) {
2130 0 : return toString(myLookaheadLeft);
2131 44 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAINRIGHT)) {
2132 0 : return toString(mySpeedGainRight);
2133 44 : } else if (key == toString(SUMO_ATTR_LCA_ASSERTIVE)) {
2134 0 : return toString(myAssertive);
2135 44 : } else if (key == toString(SUMO_ATTR_LCA_OVERTAKE_RIGHT)) {
2136 0 : return toString(myOvertakeRightParam);
2137 44 : } else if (key == toString(SUMO_ATTR_LCA_SIGMA)) {
2138 0 : return toString(mySigma);
2139 44 : } else if (key == toString(SUMO_ATTR_LCA_KEEPRIGHT_ACCEPTANCE_TIME)) {
2140 0 : return toString(myKeepRightAcceptanceTime);
2141 44 : } else if (key == toString(SUMO_ATTR_LCA_OVERTAKE_DELTASPEED_FACTOR)) {
2142 0 : return toString(myOvertakeDeltaSpeedFactor);
2143 44 : } else if (key == toString(SUMO_ATTR_LCA_STRATEGIC_LOOKAHEAD)) {
2144 0 : return toString(myStrategicLookahead);
2145 44 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_LOOKAHEAD)) {
2146 0 : return toString(mySpeedGainLookahead);
2147 44 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_REMAIN_TIME)) {
2148 0 : return toString(mySpeedGainRemainTime);
2149 44 : } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_ROUNDABOUT)) {
2150 0 : return toString(myRoundaboutBonus);
2151 44 : } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_SPEED)) {
2152 0 : return toString(myCooperativeSpeed);
2153 44 : } else if (key == toString(SUMO_ATTR_LCA_MAXSPEEDLATSTANDING)) {
2154 0 : return toString(myMaxSpeedLatStanding);
2155 44 : } else if (key == toString(SUMO_ATTR_LCA_MAXSPEEDLATFACTOR)) {
2156 0 : return toString(myMaxSpeedLatFactor);
2157 44 : } else if (key == toString(SUMO_ATTR_LCA_MAXDISTLATSTANDING)) {
2158 0 : return toString(myMaxDistLatStanding);
2159 : // access to internal state for debugging in sumo-gui (not documented since it may change at any time)
2160 44 : } else if (key == "speedGainProbabilityRight") {
2161 0 : return toString(mySpeedGainProbabilityRight);
2162 44 : } else if (key == "speedGainProbabilityLeft") {
2163 0 : return toString(mySpeedGainProbabilityLeft);
2164 44 : } else if (key == "keepRightProbability") {
2165 0 : return toString(-myKeepRightProbability);
2166 44 : } else if (key == "lookAheadSpeed") {
2167 0 : return toString(myLookAheadSpeed);
2168 : // motivation relative to threshold
2169 44 : } else if (key == "speedGainRP") {
2170 0 : return toString(mySpeedGainProbabilityRight / myChangeProbThresholdRight);
2171 44 : } else if (key == "speedGainLP") {
2172 0 : return toString(mySpeedGainProbabilityLeft / myChangeProbThresholdLeft);
2173 44 : } else if (key == "keepRightP") {
2174 0 : return toString(myKeepRightProbability * myKeepRightParam / -myChangeProbThresholdRight);
2175 : }
2176 176 : throw InvalidArgument("Parameter '" + key + "' is not supported for laneChangeModel of type '" + toString(myModel) + "'");
2177 : }
2178 :
2179 :
2180 : void
2181 11926 : MSLCM_LC2013::setParameter(const std::string& key, const std::string& value) {
2182 : double doubleValue;
2183 : try {
2184 11926 : doubleValue = StringUtils::toDouble(value);
2185 0 : } catch (NumberFormatException&) {
2186 0 : throw InvalidArgument("Setting parameter '" + key + "' requires a number for laneChangeModel of type '" + toString(myModel) + "'");
2187 0 : }
2188 11926 : if (key == toString(SUMO_ATTR_LCA_STRATEGIC_PARAM)) {
2189 3144 : myStrategicParam = doubleValue;
2190 8782 : } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_PARAM)) {
2191 0 : myCooperativeParam = doubleValue;
2192 8782 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_PARAM)) {
2193 0 : mySpeedGainParam = doubleValue;
2194 8782 : } else if (key == toString(SUMO_ATTR_LCA_KEEPRIGHT_PARAM)) {
2195 0 : myKeepRightParam = doubleValue;
2196 8782 : } else if (key == toString(SUMO_ATTR_LCA_OPPOSITE_PARAM)) {
2197 0 : myOppositeParam = doubleValue;
2198 8782 : } else if (key == toString(SUMO_ATTR_LCA_LOOKAHEADLEFT)) {
2199 0 : myLookaheadLeft = doubleValue;
2200 8782 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAINRIGHT)) {
2201 0 : mySpeedGainRight = doubleValue;
2202 8782 : } else if (key == toString(SUMO_ATTR_LCA_ASSERTIVE)) {
2203 0 : myAssertive = doubleValue;
2204 8782 : } else if (key == toString(SUMO_ATTR_LCA_OVERTAKE_RIGHT)) {
2205 0 : myOvertakeRightParam = doubleValue;
2206 8782 : } else if (key == toString(SUMO_ATTR_LCA_SIGMA)) {
2207 0 : mySigma = doubleValue;
2208 8782 : } else if (key == toString(SUMO_ATTR_LCA_KEEPRIGHT_ACCEPTANCE_TIME)) {
2209 0 : myKeepRightAcceptanceTime = doubleValue;
2210 8782 : } else if (key == toString(SUMO_ATTR_LCA_OVERTAKE_DELTASPEED_FACTOR)) {
2211 0 : myOvertakeDeltaSpeedFactor = doubleValue;
2212 8782 : } else if (key == toString(SUMO_ATTR_LCA_STRATEGIC_LOOKAHEAD)) {
2213 0 : myStrategicLookahead = doubleValue;
2214 8782 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_LOOKAHEAD)) {
2215 2926 : mySpeedGainLookahead = doubleValue;
2216 5856 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_REMAIN_TIME)) {
2217 2926 : mySpeedGainRemainTime = doubleValue;
2218 2930 : } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_ROUNDABOUT)) {
2219 0 : myRoundaboutBonus = doubleValue;
2220 2930 : } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_SPEED)) {
2221 0 : myCooperativeSpeed = doubleValue;
2222 2930 : } else if (key == toString(SUMO_ATTR_LCA_MAXSPEEDLATSTANDING)) {
2223 0 : myMaxSpeedLatStanding = doubleValue;
2224 2930 : } else if (key == toString(SUMO_ATTR_LCA_MAXSPEEDLATFACTOR)) {
2225 0 : myMaxSpeedLatFactor = doubleValue;
2226 2930 : } else if (key == toString(SUMO_ATTR_LCA_MAXDISTLATSTANDING)) {
2227 0 : myMaxDistLatStanding = doubleValue;
2228 : // access to internal state
2229 2930 : } else if (key == "speedGainProbabilityRight") {
2230 0 : mySpeedGainProbabilityRight = doubleValue;
2231 2930 : } else if (key == "speedGainProbabilityLeft") {
2232 0 : mySpeedGainProbabilityLeft = doubleValue;
2233 2930 : } else if (key == "keepRightProbability") {
2234 0 : myKeepRightProbability = -doubleValue;
2235 2930 : } else if (key == "lookAheadSpeed") {
2236 0 : myLookAheadSpeed = doubleValue;
2237 : } else {
2238 11720 : throw InvalidArgument("Setting parameter '" + key + "' is not supported for laneChangeModel of type '" + toString(myModel) + "'");
2239 : }
2240 8996 : initDerivedParameters();
2241 8996 : }
2242 :
2243 :
2244 : /****************************************************************************/
|
