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 MAGIC_OFFSET 1.
48 : #define LOOK_FORWARD 10.
49 :
50 : #define JAM_FACTOR 1.
51 :
52 : #define LCA_RIGHT_IMPATIENCE -1.
53 : #define CUT_IN_LEFT_SPEED_THRESHOLD 27.
54 :
55 : #define LOOK_AHEAD_MIN_SPEED 0.0
56 : #define LOOK_AHEAD_SPEED_MEMORY 0.9
57 :
58 : #define HELP_DECEL_FACTOR 1.0
59 :
60 : #define HELP_OVERTAKE (10.0 / 3.6)
61 : #define MIN_FALLBEHIND (7.0 / 3.6)
62 :
63 : #define RELGAIN_NORMALIZATION_MIN_SPEED 10.0
64 : #define URGENCY 2.0
65 : #define OPPOSITE_URGENCY 5.0
66 :
67 : #define KEEP_RIGHT_TIME 5.0 // the number of seconds after which a vehicle should move to the right lane
68 :
69 : #define KEEP_RIGHT_HEADWAY 2.0
70 : #define MAX_ONRAMP_LENGTH 200.
71 : #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
72 :
73 : #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
74 :
75 : #define REACT_TO_STOPPED_DISTANCE 100
76 : #define BLOCKER_IS_BLOCKED_TIME_THRESHOLD 5 // the time after which a blocking neighbor is treated similar to a stopped vehicle
77 :
78 : // ===========================================================================
79 : // debug defines
80 : // ===========================================================================
81 : //#define DEBUG_CONSTRUCTOR
82 : //#define DEBUG_PATCH_SPEED
83 : //#define DEBUG_INFORMED
84 : //#define DEBUG_INFORMER
85 : //#define DEBUG_WANTS_CHANGE
86 : //#define DEBUG_SLOW_DOWN
87 : //#define DEBUG_COOPERATE
88 : //#define DEBUG_SAVE_BLOCKER_LENGTH
89 :
90 : //#define DEBUG_COND (myVehicle.getID() == "disabled")
91 : #define DEBUG_COND (myVehicle.isSelected())
92 : //#define DEBUG_COND (true)
93 :
94 : // ===========================================================================
95 : // member method definitions
96 : // ===========================================================================
97 3831252 : MSLCM_LC2013::MSLCM_LC2013(MSVehicle& v) :
98 : MSAbstractLaneChangeModel(v, LaneChangeModel::LC2013),
99 3831252 : mySpeedGainProbabilityLeft(0),
100 3831252 : mySpeedGainProbabilityRight(0),
101 3831252 : myKeepRightProbability(0),
102 3831252 : myLeadingBlockerLength(0),
103 3831252 : myLeftSpace(0),
104 3831252 : myLookAheadSpeed(LOOK_AHEAD_MIN_SPEED),
105 3831252 : myStrategicParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_STRATEGIC_PARAM, 1)),
106 3831252 : myCooperativeParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_COOPERATIVE_PARAM, 1)),
107 3831252 : mySpeedGainParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAIN_PARAM, 1)),
108 3831252 : myKeepRightParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_KEEPRIGHT_PARAM, 1)),
109 3831252 : myOppositeParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_OPPOSITE_PARAM, 1)),
110 3831252 : myLookaheadLeft(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_LOOKAHEADLEFT, 2.0)),
111 3831252 : mySpeedGainRight(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAINRIGHT, 0.1)),
112 3831252 : mySpeedGainLookahead(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAIN_LOOKAHEAD, 0)),
113 3831252 : mySpeedGainRemainTime(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAIN_REMAIN_TIME, 20)),
114 3831252 : mySpeedGainUrgency(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAIN_URGENCY, 50)),
115 3831252 : myRoundaboutBonus(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_COOPERATIVE_ROUNDABOUT, myCooperativeParam)),
116 3831252 : myCooperativeSpeed(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_COOPERATIVE_SPEED, myCooperativeParam)),
117 3831252 : myKeepRightAcceptanceTime(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_KEEPRIGHT_ACCEPTANCE_TIME, -1)),
118 3831252 : myOvertakeDeltaSpeedFactor(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_OVERTAKE_DELTASPEED_FACTOR, 0)),
119 7662504 : myExperimentalParam1(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_EXPERIMENTAL1, 0)) {
120 3831252 : initDerivedParameters();
121 : #ifdef DEBUG_CONSTRUCTOR
122 : if (DEBUG_COND) {
123 : std::cout << SIMTIME
124 : << " create lcModel veh=" << myVehicle.getID()
125 : << " lcStrategic=" << myStrategicParam
126 : << " lcCooperative=" << myCooperativeParam
127 : << " lcSpeedGain=" << mySpeedGainParam
128 : << " lcKeepRight=" << myKeepRightParam
129 : << "\n";
130 : }
131 : #endif
132 3831252 : }
133 :
134 7662360 : MSLCM_LC2013::~MSLCM_LC2013() {
135 3831180 : changed();
136 7662360 : }
137 :
138 :
139 : void
140 3840320 : MSLCM_LC2013::initDerivedParameters() {
141 3840320 : if (mySpeedGainParam <= 0) {
142 7096 : myChangeProbThresholdRight = std::numeric_limits<double>::max();
143 7096 : myChangeProbThresholdLeft = std::numeric_limits<double>::max();
144 : } else {
145 3833224 : myChangeProbThresholdRight = (0.2 / mySpeedGainRight) / mySpeedGainParam;
146 3833224 : myChangeProbThresholdLeft = 0.2 / mySpeedGainParam;
147 : }
148 3840320 : }
149 :
150 :
151 : bool
152 3395 : MSLCM_LC2013::debugVehicle() const {
153 3395 : return DEBUG_COND;
154 : }
155 :
156 :
157 : int
158 235610578 : MSLCM_LC2013::wantsChange(
159 : int laneOffset,
160 : MSAbstractLaneChangeModel::MSLCMessager& msgPass,
161 : int blocked,
162 : const std::pair<MSVehicle*, double>& leader,
163 : const std::pair<MSVehicle*, double>& follower,
164 : const std::pair<MSVehicle*, double>& neighLead,
165 : const std::pair<MSVehicle*, double>& neighFollow,
166 : const MSLane& neighLane,
167 : const std::vector<MSVehicle::LaneQ>& preb,
168 : MSVehicle** lastBlocked,
169 : MSVehicle** firstBlocked) {
170 :
171 : #ifdef DEBUG_WANTS_CHANGE
172 : if (DEBUG_COND) {
173 : std::cout << "\nWANTS_CHANGE\n" << SIMTIME
174 : << std::setprecision(gPrecision)
175 : << " veh=" << myVehicle.getID()
176 : << " lane=" << myVehicle.getLane()->getID()
177 : << " pos=" << myVehicle.getPositionOnLane()
178 : << " posLat=" << myVehicle.getLateralPositionOnLane()
179 : << " speed=" << myVehicle.getSpeed()
180 : << " considerChangeTo=" << (laneOffset == -1 ? "right" : "left")
181 : << "\n";
182 : }
183 : #endif
184 :
185 235610578 : const int result = _wantsChange(laneOffset, msgPass, blocked, leader, follower, neighLead, neighFollow, neighLane, preb, *lastBlocked, *firstBlocked);
186 :
187 : #ifdef DEBUG_WANTS_CHANGE
188 : if (DEBUG_COND) {
189 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " result=" << toString((LaneChangeAction)result) << " blocked=" << toString((LaneChangeAction)blocked) << "\n\n\n";
190 : }
191 : #endif
192 :
193 235610578 : return result;
194 : }
195 :
196 :
197 : double
198 522910385 : MSLCM_LC2013::patchSpeed(const double min, const double wanted, const double max, const MSCFModel& cfModel) {
199 :
200 : #ifdef DEBUG_PATCH_SPEED
201 : if (DEBUG_COND) {
202 : std::cout << "\nPATCH_SPEED\n"
203 : << SIMTIME
204 : << " veh=" << myVehicle.getID()
205 : << " lane=" << myVehicle.getLane()->getID()
206 : << " pos=" << myVehicle.getPositionOnLane()
207 : << " v=" << myVehicle.getSpeed()
208 : << " min=" << min
209 : << " wanted=" << wanted
210 : << " max=" << max
211 : << "\n";
212 : }
213 : #endif
214 :
215 : // negative min speed may be passed when using ballistic updated
216 522910385 : const double newSpeed = _patchSpeed(MAX2(min, 0.0), wanted, max, cfModel);
217 :
218 : #ifdef DEBUG_PATCH_SPEED
219 : if (DEBUG_COND) {
220 : const std::string patched = (wanted != newSpeed ? " patched=" + toString(newSpeed) : "");
221 : std::cout << patched
222 : << "\n";
223 : }
224 : #endif
225 :
226 522910385 : return newSpeed;
227 : }
228 :
229 :
230 : double
231 522910385 : MSLCM_LC2013::_patchSpeed(double min, const double wanted, double max, const MSCFModel& cfModel) {
232 522910385 : int state = myOwnState;
233 : #ifdef DEBUG_PATCH_SPEED
234 : if (DEBUG_COND) {
235 : std::cout
236 : << "\n" << SIMTIME << std::setprecision(gPrecision)
237 : << " patchSpeed state=" << toString((LaneChangeAction)state) << " myLCAccelerationAdvices=" << toString(myLCAccelerationAdvices)
238 : << "\n speed=" << myVehicle.getSpeed() << " min=" << min << " wanted=" << wanted
239 : << "\n myLeadingBlockerLength=" << myLeadingBlockerLength
240 : << "\n";
241 : }
242 : #endif
243 :
244 : // letting vehicles merge in at the end of the lane in case of counter-lane change, step#2
245 : double nVSafe = wanted;
246 : bool gotOne = false;
247 : // if we want to change and have a blocking leader and there is enough room for him in front of us
248 522910385 : if (myLeadingBlockerLength != 0) {
249 577057 : double space = myLeftSpace - myLeadingBlockerLength - MAGIC_OFFSET - myVehicle.getVehicleType().getMinGap();
250 : #ifdef DEBUG_PATCH_SPEED
251 : if (DEBUG_COND) {
252 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " myLeftSpace=" << myLeftSpace << " myLeadingBlockerLength=" << myLeadingBlockerLength << " space=" << space << "\n";
253 : }
254 : #endif
255 577057 : if (space > 0 && (myVehicle.getLane()->isNormal() || myVehicle.getCurrentEdge()->isRoundabout())) { // XXX space > -MAGIC_OFFSET
256 : // compute speed for decelerating towards a place which allows the blocking leader to merge in in front
257 394731 : const double vMinEmergency = myVehicle.getCarFollowModel().minNextSpeedEmergency(myVehicle.getSpeed(), &myVehicle);
258 394731 : double safe = cfModel.stopSpeed(&myVehicle, myVehicle.getSpeed(), space, MSCFModel::CalcReason::LANE_CHANGE);
259 : max = MIN2(max, MAX2(safe, vMinEmergency));
260 : // if we are approaching this place
261 394731 : if (safe < wanted) {
262 : // return this speed as the speed to use
263 42822 : if (safe < min) {
264 5976 : if (safe >= vMinEmergency) {
265 : // permit harder braking if needed and helpful
266 : min = MAX2(vMinEmergency, safe);
267 : }
268 : }
269 : #ifdef DEBUG_PATCH_SPEED
270 : if (DEBUG_COND) {
271 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " slowing down for leading blocker, safe=" << safe << (safe + NUMERICAL_EPS < min ? " (not enough)" : "") << "\n";
272 : }
273 : #endif
274 : nVSafe = MAX2(min, safe);
275 : gotOne = true;
276 : }
277 : }
278 : }
279 :
280 522910385 : const double coopWeight = MAX2(0.0, MIN2(1.0, myCooperativeSpeed));
281 544964209 : for (auto i : myLCAccelerationAdvices) {
282 : double a = i.first;
283 22053824 : double v = myVehicle.getSpeed() + ACCEL2SPEED(a);
284 :
285 22053824 : if (v >= min && v <= max && (MSGlobals::gSemiImplicitEulerUpdate
286 : // ballistic update: (negative speeds may appear, e.g. min<0, v<0), BUT:
287 : // XXX: LaneChanging returns -1 to indicate no restrictions, which leads to probs here (Leo), refs. #2577
288 : // As a quick fix, we just dismiss cases where v=-1
289 : // VERY rarely (whenever a requested help-acceleration is really indicated by v=-1)
290 : // this can lead to failing lane-change attempts, though)
291 251713 : || v != -1)) {
292 5286600 : if (i.second) {
293 : // own advice, no scaling needed
294 : nVSafe = MIN2(v, nVSafe);
295 : } else {
296 2140974 : nVSafe = MIN2(v * coopWeight + (1 - coopWeight) * wanted, nVSafe);
297 : }
298 : gotOne = true;
299 : #ifdef DEBUG_PATCH_SPEED
300 : if (DEBUG_COND) {
301 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " got nVSafe=" << nVSafe << " isOwn: " << i.second << " rawV=" << v << "\n";
302 : }
303 : #endif
304 : } else {
305 : if (v < min) {
306 : #ifdef DEBUG_PATCH_SPEED
307 : if (DEBUG_COND) {
308 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " ignoring low nVSafe=" << v << " min=" << min << "\n";
309 : }
310 : #endif
311 : } else {
312 : #ifdef DEBUG_PATCH_SPEED
313 : if (DEBUG_COND) {
314 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " ignoring high nVSafe=" << v << " max=" << max << "\n";
315 : }
316 : #endif
317 : }
318 : }
319 : }
320 : // myDontBrake is used in counter-lane-change situations with relief connection
321 522910385 : if (gotOne && !myDontBrake) {
322 : #ifdef DEBUG_PATCH_SPEED
323 : if (DEBUG_COND) {
324 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " got vSafe\n";
325 : }
326 : #endif
327 : return nVSafe;
328 : }
329 :
330 : // check whether the vehicle is blocked
331 518412801 : if ((state & LCA_WANTS_LANECHANGE) != 0 && (state & LCA_BLOCKED) != 0) {
332 13610077 : if ((state & LCA_STRATEGIC) != 0) {
333 : // necessary decelerations are controlled via vSafe. If there are
334 : // none it means we should speed up
335 : #ifdef DEBUG_PATCH_SPEED
336 : if (DEBUG_COND) {
337 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_WANTS_LANECHANGE (strat, no vSafe)\n";
338 : }
339 : #endif
340 2752315 : return (max + wanted) / 2.0;
341 10857762 : } else if ((state & LCA_COOPERATIVE) != 0) {
342 : // only minor adjustments in speed should be done
343 719744 : if ((state & LCA_BLOCKED_BY_LEADER) != 0) {
344 : #ifdef DEBUG_PATCH_SPEED
345 : if (DEBUG_COND) {
346 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_LEADER (coop)\n";
347 : }
348 : #endif
349 571717 : if (wanted >= 0.) {
350 571717 : return (MAX2(0., min) + wanted) / 2.0;
351 : } else {
352 : return wanted;
353 : }
354 : }
355 148027 : if ((state & LCA_BLOCKED_BY_FOLLOWER) != 0) {
356 : #ifdef DEBUG_PATCH_SPEED
357 : if (DEBUG_COND) {
358 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_FOLLOWER (coop)\n";
359 : }
360 : #endif
361 148027 : return (max + wanted) / 2.0;
362 : }
363 : //} else { // VARIANT_16
364 : // // only accelerations should be performed
365 : // if ((state & LCA_BLOCKED_BY_FOLLOWER) != 0) {
366 : // if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_FOLLOWER\n";
367 : // return (max + wanted) / 2.0;
368 : // }
369 : }
370 : }
371 :
372 : /*
373 : // decelerate if being a blocking follower
374 : // (and does not have to change lanes)
375 : if ((state & LCA_AMBLOCKINGFOLLOWER) != 0) {
376 : if (fabs(max - myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle)) < 0.001 && min == 0) { // !!! was standing
377 : if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGFOLLOWER (standing)\n";
378 : return 0;
379 : }
380 : if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGFOLLOWER\n";
381 :
382 : //return min; // VARIANT_3 (brakeStrong)
383 : return (min + wanted) / 2.0;
384 : }
385 : if ((state & LCA_AMBACKBLOCKER) != 0) {
386 : if (max <= myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle) && min == 0) { // !!! was standing
387 : if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBACKBLOCKER (standing)\n";
388 : //return min; VARIANT_9 (backBlockVSafe)
389 : return nVSafe;
390 : }
391 : }
392 : if ((state & LCA_AMBACKBLOCKER_STANDING) != 0) {
393 : if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBACKBLOCKER_STANDING\n";
394 : //return min;
395 : return nVSafe;
396 : }
397 : */
398 :
399 : // accelerate if being a blocking leader or blocking follower not able to brake
400 : // (and does not have to change lanes)
401 514940742 : if ((state & LCA_AMBLOCKINGLEADER) != 0 && myCooperativeSpeed >= 0) {
402 : #ifdef DEBUG_PATCH_SPEED
403 : if (DEBUG_COND) {
404 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGLEADER\n";
405 : }
406 : #endif
407 1457626 : return (max + wanted) / 2.0;
408 : }
409 :
410 : if ((state & LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0) {
411 : #ifdef DEBUG_PATCH_SPEED
412 : if (DEBUG_COND) {
413 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGFOLLOWER_DONTBRAKE\n";
414 : }
415 : #endif
416 : /*
417 : // VARIANT_4 (dontbrake)
418 : if (max <= myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle) && min == 0) { // !!! was standing
419 : return wanted;
420 : }
421 : return (min + wanted) / 2.0;
422 : */
423 : }
424 513483116 : if (!myVehicle.getLane()->getEdge().hasLaneChanger()) {
425 : // remove chaning information if on a road with a single lane
426 287528860 : changed();
427 : }
428 : return wanted;
429 : }
430 :
431 :
432 : void*
433 4362891 : MSLCM_LC2013::inform(void* info, MSVehicle* sender) {
434 : UNUSED_PARAMETER(sender);
435 : Info* pinfo = (Info*)info;
436 : assert(pinfo->first >= 0 || !MSGlobals::gSemiImplicitEulerUpdate);
437 4362891 : addLCSpeedAdvice(pinfo->first, false);
438 4362891 : myOwnState |= pinfo->second;
439 : #ifdef DEBUG_INFORMED
440 : if (DEBUG_COND) {
441 : std::cout << SIMTIME
442 : << " veh=" << myVehicle.getID()
443 : << " informedBy=" << sender->getID()
444 : << " info=" << pinfo->second
445 : << " vSafe=" << pinfo->first
446 : << "\n";
447 : }
448 : #endif
449 4362891 : delete pinfo;
450 4362891 : return (void*) true;
451 : }
452 :
453 : double
454 4285609 : MSLCM_LC2013::overtakeDistance(const MSVehicle* follower, const MSVehicle* leader, const double gap, double followerSpeed, double leaderSpeed) {
455 4285609 : followerSpeed = followerSpeed == INVALID_SPEED ? follower->getSpeed() : followerSpeed;
456 4285609 : leaderSpeed = leaderSpeed == INVALID_SPEED ? leader->getSpeed() : leaderSpeed;
457 : double overtakeDist = (gap // drive to back of leader
458 4285609 : + leader->getVehicleType().getLengthWithGap() // drive to front of leader
459 4285609 : + follower->getVehicleType().getLength() // follower back reaches leader front
460 4285609 : + leader->getCarFollowModel().getSecureGap( // save gap to leader
461 4285609 : leader, follower, leaderSpeed, followerSpeed, follower->getCarFollowModel().getMaxDecel()));
462 4285609 : return MAX2(overtakeDist, 0.);
463 : }
464 :
465 :
466 : double
467 4428704 : MSLCM_LC2013::informLeader(MSAbstractLaneChangeModel::MSLCMessager& msgPass,
468 : int blocked,
469 : int dir,
470 : const std::pair<MSVehicle*, double>& neighLead,
471 : double remainingSeconds) {
472 4428704 : double plannedSpeed = myVehicle.getSpeed();
473 4428704 : if (!isOpposite()) {
474 4329716 : plannedSpeed = MIN2(plannedSpeed,
475 4329716 : myVehicle.getCarFollowModel().stopSpeed(&myVehicle, myVehicle.getSpeed(), myLeftSpace - myLeadingBlockerLength));
476 : }
477 5061695 : for (auto i : myLCAccelerationAdvices) {
478 : const double a = i.first;
479 632991 : if (a >= -myVehicle.getCarFollowModel().getMaxDecel()) {
480 632970 : plannedSpeed = MIN2(plannedSpeed, myVehicle.getSpeed() + ACCEL2SPEED(a));
481 : }
482 : }
483 : #ifdef DEBUG_INFORMER
484 : if (DEBUG_COND) {
485 : std::cout << "\nINFORM_LEADER"
486 : << "\nspeed=" << myVehicle.getSpeed() << " planned=" << plannedSpeed << "\n";
487 : }
488 : #endif
489 :
490 4428704 : const MSVehicle* const nv = neighLead.first;
491 4428704 : if (nv == nullptr) {
492 : // not overtaking
493 : return plannedSpeed;
494 : }
495 4203275 : const double neighNextSpeed = nv->getSpeed() - ACCEL2SPEED(MAX2(1.0, -nv->getAcceleration()));
496 : double neighNextGap;
497 4203275 : if (MSGlobals::gSemiImplicitEulerUpdate) {
498 3976083 : neighNextGap = neighLead.second + SPEED2DIST(neighNextSpeed - plannedSpeed);
499 : } else {
500 227192 : neighNextGap = neighLead.second + SPEED2DIST((nv->getSpeed() + neighNextSpeed) / 2) - SPEED2DIST((myVehicle.getSpeed() + plannedSpeed) / 2);
501 : }
502 4203275 : if ((blocked & LCA_BLOCKED_BY_LEADER) != 0) {
503 3399852 : if (MSLCHelper::divergentRoute(myVehicle, *nv)) {
504 : //std::cout << SIMTIME << " ego=" << myVehicle.getID() << " ignoresDivergentBlockingLeader=" << nv->getID() << "\n";
505 : return plannedSpeed;
506 : }
507 : #ifdef DEBUG_INFORMER
508 : if (DEBUG_COND) {
509 : std::cout << " blocked by leader nv=" << nv->getID() << " nvSpeed=" << nv->getSpeed() << " needGap="
510 : << myVehicle.getCarFollowModel().getSecureGap(&myVehicle, nv, myVehicle.getSpeed(), nv->getSpeed(), nv->getCarFollowModel().getMaxDecel()) << "\n";
511 : }
512 : #endif
513 : // decide whether we want to overtake the leader or follow it
514 : double overtakeTime;
515 3396768 : const double overtakeDist = overtakeDistance(&myVehicle, nv, neighLead.second);
516 3396768 : const double dv = plannedSpeed - nv->getSpeed();
517 :
518 3396768 : if (dv > myOvertakeDeltaSpeedFactor * myVehicle.getLane()->getSpeedLimit()) {
519 1465792 : overtakeTime = overtakeDist / dv;
520 : } else if (nv->getWaitingSeconds() > BLOCKER_IS_BLOCKED_TIME_THRESHOLD
521 1088183 : && !isOpposite()
522 2993785 : && (myVehicle.getVehicleType().getLengthWithGap() + nv->getVehicleType().getLengthWithGap()) <= myLeftSpace) {
523 : // -> set overtakeTime to indicate possibility of overtaking (only if there is enough space)
524 948384 : overtakeTime = remainingSeconds - 1;
525 : } else {
526 : // -> set overtakeTime to something indicating impossibility of overtaking
527 982592 : overtakeTime = remainingSeconds + 1;
528 : }
529 :
530 : #ifdef DEBUG_INFORMER
531 : if (DEBUG_COND) {
532 : std::cout << SIMTIME << " informLeader() of " << myVehicle.getID()
533 : << "\nnv = " << nv->getID()
534 : << "\nplannedSpeed = " << plannedSpeed
535 : << "\nleaderSpeed = " << nv->getSpeed()
536 : << "\nmyLeftSpace = " << myLeftSpace
537 : << "\nremainingSeconds = " << remainingSeconds
538 : << "\novertakeDist = " << overtakeDist
539 : << "\novertakeTime = " << overtakeTime
540 : << std::endl;
541 : }
542 : #endif
543 :
544 3396768 : if ((dv < myOvertakeDeltaSpeedFactor * myVehicle.getLane()->getSpeedLimit()
545 : // overtaking on the right on an uncongested highway is forbidden (noOvertakeLCLeft)
546 2577456 : || (dir == LCA_MLEFT && !myVehicle.congested() && !myAllowOvertakingRight)
547 : // not enough space to overtake?
548 5001866 : || (MSGlobals::gSemiImplicitEulerUpdate && myLeftSpace - myLeadingBlockerLength - myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed()) < overtakeDist)
549 : // 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).
550 2418594 : || (!MSGlobals::gSemiImplicitEulerUpdate && myLeftSpace - myLeadingBlockerLength - myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed(), getCarFollowModel().getMaxDecel(), 0.) < overtakeDist)
551 : // 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)
552 2405436 : || (remainingSeconds < overtakeTime && (MSGlobals::gSemiImplicitEulerUpdate || !nv->isStopped())))
553 : // opposite driving and must overtake
554 4080142 : && (!neighLead.first->isStopped() || (isOpposite() && neighLead.second >= 0))) {
555 : // cannot overtake
556 1499670 : msgPass.informNeighLeader(new Info(std::numeric_limits<double>::max(), dir | LCA_AMBLOCKINGLEADER), &myVehicle);
557 : // slow down smoothly to follow leader
558 : // account for minor decelerations by the leader (dawdling)
559 4499010 : double targetSpeed = MAX3(myVehicle.getCarFollowModel().minNextSpeed(myVehicle.getSpeed(), &myVehicle),
560 1499670 : getCarFollowModel().followSpeed(&myVehicle, myVehicle.getSpeed(), neighNextGap, neighNextSpeed, nv->getCarFollowModel().getMaxDecel()),
561 : // avoid changing on intersection
562 1518443 : (myVehicle.getLane()->isNormal() || myVehicle.getCurrentEdge()->isRoundabout()) ? 0 : ACCEL2SPEED(myVehicle.getCarFollowModel().getMaxAccel()));
563 1499670 : if (targetSpeed < myVehicle.getSpeed()) {
564 : // slow down smoothly to follow leader
565 505287 : const double decel = remainingSeconds == 0. ? myVehicle.getCarFollowModel().getMaxDecel() :
566 505287 : MIN2(myVehicle.getCarFollowModel().getMaxDecel(),
567 505287 : MAX2(MIN_FALLBEHIND, (myVehicle.getSpeed() - targetSpeed) / remainingSeconds));
568 505287 : const double nextSpeed = MIN2(plannedSpeed, MAX2(0.0, myVehicle.getSpeed() - ACCEL2SPEED(decel)));
569 : #ifdef DEBUG_INFORMER
570 : if (DEBUG_COND) {
571 : std::cout << SIMTIME
572 : << " cannot overtake leader nv=" << nv->getID()
573 : << " dv=" << dv
574 : << " myLookAheadSpeed=" << myLookAheadSpeed
575 : << " myLeftSpace=" << myLeftSpace
576 : << " overtakeDist=" << overtakeDist
577 : << " overtakeTime=" << overtakeTime
578 : << " remainingSeconds=" << remainingSeconds
579 : << " currentGap=" << neighLead.second
580 : << " brakeGap=" << myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed(), getCarFollowModel().getMaxDecel(), 0.)
581 : << " neighNextSpeed=" << neighNextSpeed
582 : << " neighNextGap=" << neighNextGap
583 : << " targetSpeed=" << targetSpeed
584 : << " nextSpeed=" << nextSpeed
585 : << "\n";
586 : }
587 : #endif
588 505287 : addLCSpeedAdvice(nextSpeed);
589 505287 : return nextSpeed;
590 : } else {
591 : // leader is fast enough anyway
592 : #ifdef DEBUG_INFORMER
593 : if (DEBUG_COND) {
594 : std::cout << SIMTIME
595 : << " cannot overtake fast leader nv=" << nv->getID()
596 : << " dv=" << dv
597 : << " myLookAheadSpeed=" << myLookAheadSpeed
598 : << " myLeftSpace=" << myLeftSpace
599 : << " overtakeDist=" << overtakeDist
600 : << " myLeadingBlockerLength=" << myLeadingBlockerLength
601 : << " overtakeTime=" << overtakeTime
602 : << " remainingSeconds=" << remainingSeconds
603 : << " currentGap=" << neighLead.second
604 : << " neighNextSpeed=" << neighNextSpeed
605 : << " neighNextGap=" << neighNextGap
606 : << " targetSpeed=" << targetSpeed
607 : << "\n";
608 : }
609 : #endif
610 994383 : addLCSpeedAdvice(targetSpeed);
611 994383 : return plannedSpeed;
612 : }
613 : } else {
614 : // overtaking, leader should not accelerate
615 : #ifdef DEBUG_INFORMER
616 : if (DEBUG_COND) {
617 : std::cout << SIMTIME
618 : << " wants to overtake leader nv=" << nv->getID()
619 : << " dv=" << dv
620 : << " overtakeDist=" << overtakeDist
621 : << " remainingSeconds=" << remainingSeconds
622 : << " overtakeTime=" << overtakeTime
623 : << " currentGap=" << neighLead.second
624 : << " secureGap=" << nv->getCarFollowModel().getSecureGap(nv, &myVehicle, nv->getSpeed(), myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel())
625 : << "\n";
626 : }
627 : #endif
628 : // no need to pass a message if the neighbor is waiting/stuck anyway (but sending it would risk deadlock)
629 1897098 : if (nv->getWaitingSeconds() <= BLOCKER_IS_BLOCKED_TIME_THRESHOLD) {
630 845674 : msgPass.informNeighLeader(new Info(nv->getSpeed(), dir | LCA_AMBLOCKINGLEADER), &myVehicle);
631 : }
632 1897098 : 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
633 : }
634 : } else { // (remainUnblocked)
635 : // we are not blocked now. make sure we stay far enough from the leader
636 1606846 : const double targetSpeed = MAX2(
637 803423 : myVehicle.getCarFollowModel().minNextSpeed(myVehicle.getSpeed(), &myVehicle),
638 803423 : getCarFollowModel().followSpeed(&myVehicle, myVehicle.getSpeed(), neighNextGap, neighNextSpeed, nv->getCarFollowModel().getMaxDecel()));
639 803423 : addLCSpeedAdvice(targetSpeed);
640 : #ifdef DEBUG_INFORMER
641 : if (DEBUG_COND) {
642 : std::cout << " not blocked by leader nv=" << nv->getID()
643 : << " nvSpeed=" << nv->getSpeed()
644 : << " gap=" << neighLead.second
645 : << " neighNextSpeed=" << neighNextSpeed
646 : << " neighNextGap=" << neighNextGap
647 : << " needGap=" << myVehicle.getCarFollowModel().getSecureGap(&myVehicle, nv, myVehicle.getSpeed(), nv->getSpeed(), nv->getCarFollowModel().getMaxDecel())
648 : << " targetSpeed=" << targetSpeed
649 : << "\n";
650 : }
651 : #endif
652 : return MIN2(targetSpeed, plannedSpeed);
653 : }
654 : }
655 :
656 : void
657 2528599 : MSLCM_LC2013::informFollower(MSAbstractLaneChangeModel::MSLCMessager& msgPass,
658 : int blocked,
659 : int dir,
660 : const std::pair<MSVehicle*, double>& neighFollow,
661 : double remainingSeconds,
662 : double plannedSpeed) {
663 :
664 2528599 : MSVehicle* nv = neighFollow.first;
665 2528599 : const double plannedAccel = SPEED2ACCEL(MAX2(MIN2(getCarFollowModel().getMaxAccel(), plannedSpeed - myVehicle.getSpeed()), -getCarFollowModel().getMaxDecel()));
666 :
667 : #ifdef DEBUG_INFORMER
668 : if (DEBUG_COND) {
669 : std::cout << "\nINFORM_FOLLOWER"
670 : << "\nspeed=" << myVehicle.getSpeed() << " planned=" << plannedSpeed << "\n";
671 : }
672 :
673 : #endif
674 2528599 : if ((blocked & LCA_BLOCKED_BY_FOLLOWER) != 0 && nv != nullptr) {
675 1462302 : if (MSLCHelper::divergentRoute(myVehicle, *nv)) {
676 : //std::cout << SIMTIME << " ego=" << myVehicle.getID() << " ignoresDivergentBlockingFollower=" << nv->getID() << "\n";
677 : return;
678 : }
679 : #ifdef DEBUG_INFORMER
680 : if (DEBUG_COND) {
681 : std::cout << " blocked by follower nv=" << nv->getID() << " nvSpeed=" << nv->getSpeed() << " needGap="
682 : << nv->getCarFollowModel().getSecureGap(nv, &myVehicle, nv->getSpeed(), myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel()) << " planned=" << plannedSpeed << "\n";
683 : }
684 : #endif
685 :
686 : // are we fast enough to cut in without any help?
687 1462184 : if (MAX2(plannedSpeed, 0.) - nv->getSpeed() >= HELP_OVERTAKE) {
688 82549 : const double neededGap = nv->getCarFollowModel().getSecureGap(nv, &myVehicle, nv->getSpeed(), plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel());
689 82549 : if ((neededGap - neighFollow.second) / remainingSeconds < (MAX2(plannedSpeed, 0.) - nv->getSpeed())) {
690 : #ifdef DEBUG_INFORMER
691 : if (DEBUG_COND) {
692 : std::cout << " wants to cut in before nv=" << nv->getID() << " without any help." << "\nneededGap = " << neededGap << "\n";
693 : }
694 : #endif
695 : // follower might even accelerate but not to much
696 : // XXX: I don't understand this. The needed gap was determined for nv->getSpeed(), not for (plannedSpeed - HELP_OVERTAKE)?! (Leo), refs. #2578
697 71829 : msgPass.informNeighFollower(new Info(MAX2(plannedSpeed, 0.) - HELP_OVERTAKE, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
698 71829 : return;
699 : }
700 : }
701 :
702 : // decide whether we will request help to cut in before the follower or allow to be overtaken
703 :
704 : // PARAMETERS
705 : // assume other vehicle will assume the equivalent of 1 second of
706 : // maximum deceleration to help us (will probably be spread over
707 : // multiple seconds)
708 : // -----------
709 : const double helpDecel = nv->getCarFollowModel().getMaxDecel() * HELP_DECEL_FACTOR;
710 :
711 : // follower's new speed in next step
712 : double neighNewSpeed;
713 : // follower's new speed after 1s.
714 : double neighNewSpeed1s;
715 : // velocity difference, gap after follower-deceleration
716 : double dv, decelGap;
717 :
718 1390355 : if (MSGlobals::gSemiImplicitEulerUpdate) {
719 : // euler
720 1261815 : neighNewSpeed = MAX2(0., nv->getSpeed() - ACCEL2SPEED(helpDecel));
721 1261815 : neighNewSpeed1s = MAX2(0., nv->getSpeed() - helpDecel); // TODO: consider introduction of a configurable anticipationTime here (see far below in the !blocked part). Refs. #2578
722 : // change in the gap between ego and blocker over 1 second (not STEP!)
723 : // XXX: though here it is calculated as if it were one step!? (Leo) Refs. #2578
724 1261815 : dv = plannedSpeed - neighNewSpeed1s; // XXX: what is this quantity (if TS!=1)?
725 : // new gap between follower and self in case the follower does brake for 1s
726 : // XXX: if the step-length is not 1s., this is not the gap after 1s. deceleration!
727 : // And this formula overestimates the real gap. Isn't that problematic? (Leo)
728 : // Below, it seems that decelGap > secureGap is taken to indicate the possibility
729 : // to cut in within the next time-step. However, this is not the case, if TS<1s.,
730 : // since decelGap is (not exactly, though!) the gap after 1s. Refs. #2578
731 1261815 : decelGap = neighFollow.second + dv;
732 : } else {
733 : // ballistic
734 : // negative newSpeed-extrapolation possible, if stop lies within the next time-step
735 : // XXX: this code should work for the euler case as well, since gapExtrapolation() takes
736 : // care of this, but for TS!=1 we will have different behavior (see previous remark) Refs. #2578
737 128540 : neighNewSpeed = nv->getSpeed() - ACCEL2SPEED(helpDecel);
738 128540 : neighNewSpeed1s = nv->getSpeed() - helpDecel;
739 :
740 128540 : dv = myVehicle.getSpeed() - nv->getSpeed(); // current velocity difference
741 257080 : decelGap = getCarFollowModel().gapExtrapolation(1., neighFollow.second, myVehicle.getSpeed(),
742 128540 : nv->getSpeed(), plannedAccel, -helpDecel, myVehicle.getMaxSpeedOnLane(), nv->getMaxSpeedOnLane());
743 : }
744 :
745 1390355 : const double secureGap = nv->getCarFollowModel().getSecureGap(nv, &myVehicle, MAX2(neighNewSpeed1s, 0.),
746 1390355 : MAX2(plannedSpeed, 0.), myVehicle.getCarFollowModel().getMaxDecel());
747 :
748 1390355 : const double onRampThreshold = myVehicle.getLane()->getSpeedLimit() * 0.8 * myExperimentalParam1 * (1 - myVehicle.getImpatience());
749 :
750 : #ifdef DEBUG_INFORMER
751 : if (DEBUG_COND) {
752 : std::cout << SIMTIME
753 : << " speed=" << myVehicle.getSpeed()
754 : << " plannedSpeed=" << plannedSpeed
755 : << " threshold=" << onRampThreshold
756 : << " neighNewSpeed=" << neighNewSpeed
757 : << " neighNewSpeed1s=" << neighNewSpeed1s
758 : << " dv=" << dv
759 : << " gap=" << neighFollow.second
760 : << " decelGap=" << decelGap
761 : << " secureGap=" << secureGap
762 : << "\n";
763 : }
764 : #endif
765 : // prevent vehicles on an on ramp stopping the main flow
766 : if (dir == LCA_MLEFT
767 536322 : && myVehicle.getLane()->isAccelLane()
768 1453785 : && neighNewSpeed1s < onRampThreshold) {
769 : return;
770 : }
771 :
772 1371339 : if (decelGap > 0 && decelGap >= secureGap) {
773 : // XXX: This does not assure that the leader can cut in in the next step if TS < 1 (see above)
774 : // this seems to be supposed in the following (euler code)...?! (Leo) Refs. #2578
775 :
776 : // if the blocking follower brakes it could help
777 : // how hard does it actually need to be?
778 : // to be safe in the next step the following equation has to hold for the follower's vsafe:
779 : // vsafe <= followSpeed(gap=currentGap - SPEED2DIST(vsafe), ...)
780 : double vsafe, vsafe1;
781 :
782 94025 : if (MSGlobals::gSemiImplicitEulerUpdate) {
783 : // euler
784 : // we compute an upper bound on vsafe by doing the computation twice
785 84075 : vsafe1 = MAX2(neighNewSpeed, nv->getCarFollowModel().followSpeed(
786 84075 : nv, nv->getSpeed(), neighFollow.second + SPEED2DIST(plannedSpeed), plannedSpeed, getCarFollowModel().getMaxDecel()));
787 84075 : vsafe = MAX2(neighNewSpeed, nv->getCarFollowModel().followSpeed(
788 84075 : nv, nv->getSpeed(), neighFollow.second + SPEED2DIST(plannedSpeed - vsafe1), plannedSpeed, getCarFollowModel().getMaxDecel()));
789 : //assert(vsafe <= vsafe1); assertion does not hold for models with randomness in followSpeed (W99)
790 : } else {
791 : // ballistic
792 :
793 : // XXX: This block should actually do as well for euler update (TODO: test!), refs #2575
794 : // we compute an upper bound on vsafe
795 : // next step's gap without help deceleration (nv's speed assumed constant)
796 9950 : double nextGap = getCarFollowModel().gapExtrapolation(TS,
797 9950 : neighFollow.second, myVehicle.getSpeed(),
798 9950 : nv->getSpeed(), plannedAccel, 0,
799 9950 : myVehicle.getMaxSpeedOnLane(), nv->getMaxSpeedOnLane());
800 : #ifdef DEBUG_INFORMER
801 : if (DEBUG_COND) {
802 : std::cout << "nextGap=" << nextGap << " (without help decel) \n";
803 : }
804 : #endif
805 :
806 : // NOTE: the second argument of MIN2() can get larger than nv->getSpeed()
807 19900 : vsafe1 = MIN2(nv->getSpeed(), MAX2(neighNewSpeed,
808 9950 : nv->getCarFollowModel().followSpeed(nv,
809 9950 : nv->getSpeed(), nextGap,
810 : MAX2(0., plannedSpeed),
811 : getCarFollowModel().getMaxDecel())));
812 :
813 :
814 : // next step's gap with possibly less than maximal help deceleration (in case vsafe1 > neighNewSpeed)
815 9950 : double decel2 = SPEED2ACCEL(nv->getSpeed() - vsafe1);
816 9950 : nextGap = getCarFollowModel().gapExtrapolation(TS,
817 9950 : neighFollow.second, myVehicle.getSpeed(),
818 9950 : nv->getSpeed(), plannedAccel, -decel2,
819 9950 : myVehicle.getMaxSpeedOnLane(), nv->getMaxSpeedOnLane());
820 :
821 : // vsafe = MAX(neighNewSpeed, safe speed assuming next_gap)
822 : // Thus, the gap resulting from vsafe is larger or equal to next_gap
823 : // in contrast to the euler case, where nv's follow speed doesn't depend on the actual speed,
824 : // we need to assure, that nv doesn't accelerate
825 19900 : vsafe = MIN2(nv->getSpeed(), MAX2(neighNewSpeed,
826 9950 : nv->getCarFollowModel().followSpeed(nv,
827 9950 : nv->getSpeed(), nextGap,
828 : MAX2(0., plannedSpeed),
829 : getCarFollowModel().getMaxDecel())));
830 :
831 : assert(vsafe >= vsafe1 - NUMERICAL_EPS);
832 :
833 : #ifdef DEBUG_INFORMER
834 : if (DEBUG_COND) {
835 : std::cout << "nextGap=" << nextGap
836 : << " (with vsafe1 and help decel) \nvsafe1=" << vsafe1
837 : << " vsafe=" << vsafe
838 : << "\n";
839 : }
840 : #endif
841 :
842 : // For subsecond simulation, this might not lead to secure gaps for a long time,
843 : // we seek to establish a secure gap as soon as possible
844 9950 : double nextSecureGap = nv->getCarFollowModel().getSecureGap(nv, &myVehicle, vsafe, plannedSpeed, getCarFollowModel().getMaxDecel());
845 :
846 9950 : if (nextGap < nextSecureGap) {
847 : // establish a secureGap as soon as possible
848 : vsafe = neighNewSpeed;
849 : }
850 :
851 : #ifdef DEBUG_INFORMER
852 : if (DEBUG_COND) {
853 : std::cout << "nextGap=" << nextGap
854 : << " minNextSecureGap=" << nextSecureGap
855 : << " vsafe=" << vsafe << "\n";
856 : }
857 : #endif
858 :
859 : }
860 94025 : msgPass.informNeighFollower(
861 94025 : new Info(vsafe, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
862 :
863 : #ifdef DEBUG_INFORMER
864 : if (DEBUG_COND) {
865 : std::cout << " wants to cut in before nv=" << nv->getID()
866 : << " vsafe1=" << vsafe1 << " vsafe=" << vsafe
867 : << " newSecGap="
868 : << nv->getCarFollowModel().getSecureGap(nv, &myVehicle, vsafe,
869 : plannedSpeed,
870 : myVehicle.getCarFollowModel().getMaxDecel())
871 : << "\n";
872 : }
873 : #endif
874 1277314 : } else if ((MSGlobals::gSemiImplicitEulerUpdate && dv > 0 && dv * remainingSeconds > (secureGap - decelGap + POSITION_EPS))
875 1203303 : || (!MSGlobals::gSemiImplicitEulerUpdate && dv > 0 && dv * (remainingSeconds - 1) > secureGap - decelGap + POSITION_EPS)
876 : ) {
877 :
878 : // XXX: Alternative formulation (encapsulating differences of euler and ballistic) TODO: test, refs. #2575
879 : // double eventualGap = getCarFollowModel().gapExtrapolation(remainingSeconds - 1., decelGap, plannedSpeed, neighNewSpeed1s);
880 : // } else if (eventualGap > secureGap + POSITION_EPS) {
881 :
882 :
883 : // NOTE: This case corresponds to the situation, where some time is left to perform the lc
884 : // For the ballistic case this is interpreted as follows:
885 : // If the follower breaks with helpDecel for one second, this vehicle maintains the plannedSpeed,
886 : // and both continue with their speeds for remainingSeconds seconds the gap will suffice for a laneChange
887 : // For the euler case we had the following comment:
888 : // 'decelerating once is sufficient to open up a large enough gap in time', but:
889 : // XXX: 1) Decelerating *once* does not necessarily lead to the gap decelGap! (if TS<1s.) (Leo)
890 : // 2) Probably, the if() for euler should test for dv * (remainingSeconds-1) > ..., too ?!, refs. #2578
891 76968 : msgPass.informNeighFollower(new Info(neighNewSpeed, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
892 : #ifdef DEBUG_INFORMER
893 : if (DEBUG_COND) {
894 : std::cout << " wants to cut in before nv=" << nv->getID() << " (eventually)\n";
895 : }
896 : #endif
897 1200346 : } else if (dir == LCA_MRIGHT && !myAllowOvertakingRight && !nv->congested()) {
898 : // XXX: check if this requires a special treatment for the ballistic update, refs. #2575
899 : const double vhelp = MAX2(neighNewSpeed, HELP_OVERTAKE);
900 1310 : msgPass.informNeighFollower(new Info(vhelp, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
901 : #ifdef DEBUG_INFORMER
902 : if (DEBUG_COND) {
903 : std::cout << " wants to cut in before nv=" << nv->getID() << " (nv cannot overtake right)\n";
904 : }
905 : #endif
906 : } else {
907 1199036 : double vhelp = MAX2(nv->getSpeed(), myVehicle.getSpeed() + HELP_OVERTAKE);
908 : //if (dir == LCA_MRIGHT && myVehicle.getWaitingSeconds() > LCA_RIGHT_IMPATIENCE &&
909 : // nv->getSpeed() > myVehicle.getSpeed()) {
910 1199036 : if (nv->getSpeed() > myVehicle.getSpeed() &&
911 319644 : ((dir == LCA_MRIGHT && myVehicle.getWaitingSeconds() > LCA_RIGHT_IMPATIENCE) // NOTE: it might be considered to use myVehicle.getAccumulatedWaitingSeconds() > LCA_RIGHT_IMPATIENCE instead (Leo). Refs. #2578
912 210213 : || (dir == LCA_MLEFT && plannedSpeed > CUT_IN_LEFT_SPEED_THRESHOLD) // VARIANT_22 (slowDownLeft)
913 : // XXX this is a hack to determine whether the vehicles is on an on-ramp. This information should be retrieved from the network itself
914 210181 : || (dir == LCA_MLEFT && myVehicle.getLane()->getLength() > MAX_ONRAMP_LENGTH)
915 : )) {
916 : // let the follower slow down to increase the likelihood that later vehicles will be slow enough to help
917 : // follower should still be fast enough to open a gap
918 : // XXX: The probability for that success would be larger if the slow down of the appropriate following vehicle
919 : // would take place without the immediate follower slowing down. We might consider to model reactions of
920 : // vehicles that are not immediate followers. (Leo) -> see ticket #2532
921 369474 : vhelp = MAX2(neighNewSpeed, myVehicle.getSpeed() + HELP_OVERTAKE);
922 : #ifdef DEBUG_INFORMER
923 : if (DEBUG_COND) {
924 : // NOTE: the condition labeled "VARIANT_22" seems to imply that this could as well concern the *left* follower?! (Leo)
925 : // Further, vhelp might be larger than nv->getSpeed(), so the request issued below is not to slow down!? (see below) Refs. #2578
926 : std::cout << " wants right follower to slow down a bit\n";
927 : }
928 : #endif
929 369474 : if (MSGlobals::gSemiImplicitEulerUpdate) {
930 : // euler
931 316531 : if ((nv->getSpeed() - myVehicle.getSpeed()) / helpDecel < remainingSeconds) {
932 :
933 : #ifdef DEBUG_INFORMER
934 : if (DEBUG_COND) {
935 : // NOTE: the condition labeled "VARIANT_22" seems to imply that this could as well concern the *left* follower?! Refs. #2578
936 : std::cout << " wants to cut in before right follower nv=" << nv->getID() << " (eventually)\n";
937 : }
938 : #endif
939 : // XXX: I don't understand. This vhelp might be larger than nv->getSpeed() but the above condition seems to rely
940 : // on the reasoning that if nv breaks with helpDecel for remaining Seconds, nv will be so slow, that this
941 : // vehicle will be able to cut in. But nv might have overtaken this vehicle already (or am I missing sth?). (Leo)
942 : // Ad: To my impression, the intention behind allowing larger speeds for the blocking follower is to prevent a
943 : // situation, where an overlapping follower keeps blocking the ego vehicle. Refs. #2578
944 300942 : msgPass.informNeighFollower(new Info(vhelp, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
945 300942 : return;
946 : }
947 : } else {
948 :
949 : // 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)
950 : // estimate gap after remainingSeconds.
951 : // Assumptions:
952 : // (A1) leader continues with currentSpeed. (XXX: That might be wrong: Think of accelerating on an on-ramp or of a congested region ahead!)
953 : // (A2) follower breaks with helpDecel.
954 52943 : const double gapAfterRemainingSecs = getCarFollowModel().gapExtrapolation(
955 52943 : remainingSeconds, neighFollow.second, myVehicle.getSpeed(), nv->getSpeed(), 0, -helpDecel, myVehicle.getMaxSpeedOnLane(), nv->getMaxSpeedOnLane());
956 52943 : const double secureGapAfterRemainingSecs = nv->getCarFollowModel().getSecureGap(nv, &myVehicle,
957 52943 : MAX2(nv->getSpeed() - remainingSeconds * helpDecel, 0.), myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel());
958 52943 : 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
959 : #ifdef DEBUG_INFORMER
960 : if (DEBUG_COND) {
961 : std::cout << " wants to cut in before follower nv=" << nv->getID() << " (eventually)\n";
962 : }
963 : #endif
964 : // NOTE: ballistic uses neighNewSpeed instead of vhelp, see my note above. (Leo)
965 : // TODO: recheck if this might cause suboptimal behaviour in some LC-situations. Refs. #2578
966 9253 : msgPass.informNeighFollower(new Info(neighNewSpeed, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
967 9253 : return;
968 : }
969 : }
970 :
971 :
972 : }
973 :
974 : #ifdef DEBUG_INFORMER
975 : if (DEBUG_COND) {
976 : std::cout << SIMTIME
977 : << " veh=" << myVehicle.getID()
978 : << " informs follower " << nv->getID()
979 : << " vhelp=" << vhelp
980 : << "\n";
981 : }
982 : #endif
983 :
984 888841 : msgPass.informNeighFollower(new Info(vhelp, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
985 : // This follower is supposed to overtake us. Slow down smoothly to allow this.
986 888841 : const double overtakeDist = overtakeDistance(nv, &myVehicle, neighFollow.second, vhelp, plannedSpeed);
987 : // speed difference to create a sufficiently large gap
988 888841 : const double needDV = overtakeDist / remainingSeconds;
989 : // 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
990 988225 : addLCSpeedAdvice(MAX2(vhelp - needDV, myVehicle.getSpeed() - ACCEL2SPEED(myVehicle.getCarFollowModel().getMaxDecel())));
991 :
992 : #ifdef DEBUG_INFORMER
993 : if (DEBUG_COND) {
994 : std::cout << SIMTIME
995 : << " veh=" << myVehicle.getID()
996 : << " wants to be overtaken by=" << nv->getID()
997 : << " overtakeDist=" << overtakeDist
998 : << " vneigh=" << nv->getSpeed()
999 : << " vhelp=" << vhelp
1000 : << " needDV=" << needDV
1001 : << " vsafe=" << myLCAccelerationAdvices.back().first
1002 : << "\n";
1003 : }
1004 : #endif
1005 : }
1006 1066297 : } else if (neighFollow.first != nullptr && (blocked & LCA_BLOCKED_BY_LEADER)) {
1007 : // we are not blocked by the follower now, make sure it remains that way
1008 571902 : const double vsafe = MSLCHelper::getSpeedPreservingSecureGap(myVehicle, *neighFollow.first, neighFollow.second, plannedSpeed);
1009 571902 : msgPass.informNeighFollower(new Info(vsafe, dir), &myVehicle);
1010 :
1011 : #ifdef DEBUG_INFORMER
1012 : if (DEBUG_COND) {
1013 : std::cout << " wants to cut in before non-blocking follower nv=" << nv->getID() << "\n";
1014 : }
1015 : #endif
1016 : }
1017 : }
1018 :
1019 :
1020 : void
1021 520474438 : MSLCM_LC2013::prepareStep() {
1022 520474438 : MSAbstractLaneChangeModel::prepareStep();
1023 : // keep information about strategic change direction
1024 520474438 : if (!isChangingLanes()) {
1025 519939496 : myOwnState = (myOwnState & LCA_STRATEGIC) ? (myOwnState & LCA_WANTS_LANECHANGE) : 0;
1026 : }
1027 520474438 : myLeadingBlockerLength = 0;
1028 520474438 : myLeftSpace = 0;
1029 : myLCAccelerationAdvices.clear();
1030 520474438 : myDontBrake = false;
1031 : // truncate to work around numerical instability between different builds
1032 520474438 : mySpeedGainProbabilityLeft = ceil(mySpeedGainProbabilityLeft * 100000.0) * 0.00001;
1033 520474438 : mySpeedGainProbabilityRight = ceil(mySpeedGainProbabilityRight * 100000.0) * 0.00001;
1034 520474438 : myKeepRightProbability = ceil(myKeepRightProbability * 100000.0) * 0.00001;
1035 520474438 : if (mySigma > 0 && !isChangingLanes()) {
1036 : // disturb lateral position directly
1037 1154 : const double maxDist = SPEED2DIST(myVehicle.getVehicleType().getMaxSpeedLat());
1038 1154 : const double oldPosLat = myVehicle.getLateralPositionOnLane();
1039 1154 : const double overlap = myVehicle.getLateralOverlap();
1040 : double scaledDelta;
1041 1154 : if (overlap > 0) {
1042 : // return to within lane boundary
1043 : scaledDelta = MIN2(overlap, maxDist);
1044 23 : if (myVehicle.getLateralPositionOnLane() > 0) {
1045 7 : scaledDelta *= -1;
1046 : }
1047 : } else {
1048 : // random drift
1049 1131 : double deltaPosLat = OUProcess::step(oldPosLat,
1050 1131 : myVehicle.getActionStepLengthSecs(),
1051 1131 : MAX2(NUMERICAL_EPS, (1 - mySigma) * 100), mySigma) - oldPosLat;
1052 1131 : deltaPosLat = MAX2(MIN2(deltaPosLat, maxDist), -maxDist);
1053 1131 : scaledDelta = deltaPosLat * myVehicle.getSpeed() / myVehicle.getLane()->getSpeedLimit();
1054 : }
1055 1154 : myVehicle.setLateralPositionOnLane(oldPosLat + scaledDelta);
1056 1154 : setSpeedLat(DIST2SPEED(scaledDelta));
1057 : } else {
1058 520473284 : resetSpeedLat();
1059 : }
1060 520474438 : }
1061 :
1062 :
1063 : void
1064 292092845 : MSLCM_LC2013::changed() {
1065 292092845 : myOwnState = 0;
1066 292092845 : mySpeedGainProbabilityLeft = 0;
1067 292092845 : mySpeedGainProbabilityRight = 0;
1068 292092845 : myKeepRightProbability = 0;
1069 292092845 : if (myVehicle.getBestLaneOffset() == 0) {
1070 : // if we are not yet on our best lane there might still be unseen blockers
1071 : // (during patchSpeed)
1072 291668573 : myLeadingBlockerLength = 0;
1073 291668573 : myLeftSpace = 0;
1074 : }
1075 292092845 : myLookAheadSpeed = LOOK_AHEAD_MIN_SPEED;
1076 : myLCAccelerationAdvices.clear();
1077 292092845 : myDontBrake = false;
1078 292092845 : }
1079 :
1080 :
1081 : void
1082 7884 : MSLCM_LC2013::resetState() {
1083 7884 : myOwnState = 0;
1084 7884 : mySpeedGainProbabilityLeft = 0;
1085 7884 : mySpeedGainProbabilityRight = 0;
1086 7884 : myKeepRightProbability = 0;
1087 7884 : myLeadingBlockerLength = 0;
1088 7884 : myLeftSpace = 0;
1089 7884 : myLookAheadSpeed = LOOK_AHEAD_MIN_SPEED;
1090 : myLCAccelerationAdvices.clear();
1091 7884 : myDontBrake = false;
1092 7884 : }
1093 :
1094 :
1095 : int
1096 235610578 : 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 235610578 : 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 235610578 : const bool checkOpposite = &neighLane.getEdge() != &myVehicle.getLane()->getEdge();
1121 235610578 : const MSLane* prebLane = myVehicle.getLane();
1122 235610578 : if (prebLane->getEdge().isInternal()) {
1123 : // internal edges are not kept inside the bestLanes structure
1124 1112821 : if (isOpposite()) {
1125 557 : prebLane = prebLane->getNormalPredecessorLane();
1126 : } else {
1127 1112264 : prebLane = prebLane->getLinkCont()[0]->getLane();
1128 : }
1129 : }
1130 : // special case: vehicle considers changing to the opposite direction edge
1131 : const int prebOffset = laneOffset;
1132 410591302 : 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 410591302 : if (preb[p].lane == prebLane && p + laneOffset >= 0) {
1137 : assert(p + prebOffset < (int)preb.size());
1138 : curr = preb[p];
1139 235610578 : neigh = preb[p + prebOffset];
1140 235610578 : currentDist = curr.length;
1141 235610578 : neighDist = neigh.length;
1142 235610578 : bestLaneOffset = curr.bestLaneOffset;
1143 235610578 : 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 235610578 : 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 235610578 : const bool right = (laneOffset == -1);
1165 235610578 : const double posOnLane = getForwardPos();
1166 : double driveToNextStop = -std::numeric_limits<double>::max();
1167 235610578 : if (myVehicle.nextStopDist() < std::numeric_limits<double>::max()
1168 235610578 : && &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 1656551 : driveToNextStop = myVehicle.nextStopDist();
1173 1656551 : 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 235610578 : 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 235610578 : bool changeToBest = (right && bestLaneOffset < 0) || (!right && bestLaneOffset > 0);
1192 : // keep information about being a leader/follower
1193 235610578 : int ret = (myOwnState & 0xffff0000);
1194 : int req = 0; // the request to change or stay
1195 :
1196 235610578 : ret = slowDownForBlocked(lastBlocked, ret);
1197 235610578 : if (lastBlocked != firstBlocked) {
1198 7354556 : 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 235610578 : if (myVehicle.getSpeed() > myLookAheadSpeed) {
1235 40681759 : myLookAheadSpeed = myVehicle.getSpeed();
1236 : } else {
1237 : // memory decay factor for this action step
1238 194928819 : const double memoryFactor = 1. - (1. - LOOK_AHEAD_SPEED_MEMORY) * myVehicle.getActionStepLengthSecs();
1239 : assert(memoryFactor > 0.);
1240 194928819 : myLookAheadSpeed = MAX2(LOOK_AHEAD_MIN_SPEED,
1241 194928819 : (memoryFactor * myLookAheadSpeed + (1 - memoryFactor) * myVehicle.getSpeed()));
1242 : }
1243 235610578 : double laDist = myLookAheadSpeed * LOOK_FORWARD * myStrategicParam * (right ? 1 : myLookaheadLeft);
1244 235610578 : laDist += myVehicle.getVehicleType().getLengthWithGap() * 2.;
1245 235610578 : const bool hasStoppedLeader = leader.first != 0 && leader.first->isStopped() && leader.second < (currentDist - posOnLane);
1246 235610578 : const bool hasBidiLeader = myVehicle.getLane()->getBidiLane() != nullptr && MSLCHelper::isBidiLeader(leader.first, curr.bestContinuations);
1247 235610578 : const bool hasBidiNeighLeader = neighLane.getBidiLane() != nullptr && MSLCHelper::isBidiLeader(neighLead.first, neigh.bestContinuations);
1248 :
1249 235610578 : if (bestLaneOffset == 0 && hasBidiLeader) {
1250 : // getting out of the way is enough to clear the blockage
1251 : laDist = 0;
1252 235608898 : } 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 69530 : laDist = 0.5 * (myVehicle.getVehicleType().getLengthWithGap()
1257 69530 : + leader.first->getVehicleType().getLengthWithGap()
1258 69530 : + leader.second);
1259 235539368 : } 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 271537 : if (isOpposite()) {
1262 : // always allow changing back
1263 52356 : laDist = (myVehicle.getVehicleType().getLengthWithGap()
1264 52356 : + neighLead.first->getVehicleType().getLengthWithGap()
1265 52356 : + neighLead.second);
1266 219181 : } else if (!hasStoppedLeader &&
1267 199599 : ((neighLead.second + myVehicle.getVehicleType().getLengthWithGap() + neighLead.first->getVehicleType().getLengthWithGap()) < (currentDist - posOnLane)
1268 4681 : || 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 235610578 : 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 235610578 : const double roundaboutBonus = MSLCHelper::getRoundaboutDistBonus(myVehicle, myRoundaboutBonus, curr, neigh, best);
1290 235610578 : currentDist += roundaboutBonus;
1291 235610578 : neighDist += roundaboutBonus;
1292 :
1293 235610578 : const double usableDist = MAX2(currentDist - posOnLane - best.occupation * JAM_FACTOR, driveToNextStop);
1294 : //- (best.lane->getVehicleNumber() * neighSpeed)); // VARIANT 9 jfSpeed
1295 235610578 : const double maxJam = MAX2(preb[currIdx + prebOffset].occupation, preb[currIdx].occupation);
1296 235610578 : const double vMax = myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle);
1297 235610578 : const double neighVMax = neighLane.getVehicleMaxSpeed(&myVehicle);
1298 : // upper bound which will be restricted successively
1299 235610578 : double thisLaneVSafe = vMax;
1300 235610578 : const bool checkOverTakeRight = avoidOvertakeRight();
1301 :
1302 235610578 : double neighLeftPlace = MAX2(0.0, neighDist - posOnLane - maxJam);
1303 235610578 : if (neighLead.first != 0 && neighLead.first->isStopped()) {
1304 243689 : 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 195218871 : if (changeToBest && bestLaneOffset == curr.bestLaneOffset
1330 246556654 : && currentDistDisallows(usableDist, bestLaneOffset, laDist)) {
1331 : /// @brief we urgently need to change lanes to follow our route
1332 4370683 : ret = ret | lca | LCA_STRATEGIC | LCA_URGENT;
1333 : } else {
1334 : // VARIANT_20 (noOvertakeRight)
1335 231239895 : 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 11334336 : const double deltaV = MAX2(vMax - neighLane.getVehicleMaxSpeed(nv),
1340 11334336 : myVehicle.getSpeed() - nv->getSpeed());
1341 11334336 : if (deltaV > 0) {
1342 2284844 : const double vMaxDecel = getCarFollowModel().getSpeedAfterMaxDecel(myVehicle.getSpeed());
1343 2284844 : const double vSafeFollow = getCarFollowModel().followSpeed(
1344 2284844 : &myVehicle, myVehicle.getSpeed(), neighLead.second, nv->getSpeed(), nv->getCarFollowModel().getMaxDecel());
1345 2284844 : const double vStayBehind = nv->getSpeed() - HELP_OVERTAKE;
1346 : double vSafe;
1347 2284844 : if (vSafeFollow >= vMaxDecel) {
1348 : vSafe = vSafeFollow;
1349 : } else {
1350 : vSafe = MAX2(vMaxDecel, vStayBehind);
1351 : }
1352 2284844 : if (mySpeedGainProbabilityLeft < myChangeProbThresholdLeft) {
1353 1544906 : vSafe = MAX2(vSafe, nv->getSpeed());
1354 : }
1355 2284844 : thisLaneVSafe = MIN2(thisLaneVSafe, vSafe);
1356 2284844 : addLCSpeedAdvice(vSafe);
1357 : // only generate impulse for overtaking left shortly before braking would be necessary
1358 2284844 : const double deltaGapFuture = deltaV * 8;
1359 2284844 : const double vSafeFuture = getCarFollowModel().followSpeed(
1360 2284844 : &myVehicle, myVehicle.getSpeed(), neighLead.second - deltaGapFuture, nv->getSpeed(), nv->getCarFollowModel().getMaxDecel());
1361 2284844 : if (vSafeFuture < vSafe) {
1362 : const double relativeGain = deltaV / MAX2(vMax,
1363 1887945 : RELGAIN_NORMALIZATION_MIN_SPEED);
1364 1887945 : 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 231239895 : const bool currFreeUntilNeighEnd = leader.first == nullptr || neighDist - posOnLane <= leader.second;
1382 447129226 : const double overtakeDist = (leader.first == 0 || hasBidiLeader ? -1 :
1383 215889331 : leader.second + myVehicle.getVehicleType().getLength() + leader.first->getVehicleType().getLengthWithGap());
1384 231479438 : const double overtakeDist2 = (neighLead.first == 0 || !neighLead.first->isStopped() ? -1 :
1385 239543 : neighLead.second + myVehicle.getVehicleType().getLength() + neighLead.first->getVehicleType().getLengthWithGap());
1386 215895211 : 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 113254 : && MIN2(neighDist, currentDist) - posOnLane > overtakeDist
1389 : // maybe do not overtake on the right at high speed
1390 78717 : && (!checkOverTakeRight || !right)
1391 77032 : && myStrategicParam >= 0
1392 231316339 : && (neighLead.first == 0 || !neighLead.first->isStopped()
1393 : // neighboring stopped vehicle leaves enough space to overtake leader
1394 21466 : || neighLead.second > overtakeDist
1395 : // if we cannot pass neighLead before reaching leader we must find another free lane
1396 18611 : || (overtakeDist2 > leader.second && hasFreeLane(laneOffset, neighLead)))) {
1397 : // avoid becoming stuck behind a stopped leader
1398 58334 : 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 58334 : ret = ret | lca | LCA_STRATEGIC | LCA_URGENT;
1410 231181561 : } 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 35930041 : ret = ret | LCA_STAY | LCA_STRATEGIC;
1423 195251520 : } 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 3965813 : && (leader.first == 0 || !leader.first->isStopped())
1436 3956319 : && !hasBidiLeader
1437 3956316 : && neigh.bestContinuations.back()->getLinkCont().size() != 0
1438 2282483 : && roundaboutBonus == 0
1439 800074 : && !checkOpposite
1440 696243 : && ((myStrategicParam >= 0 && neighDist < TURN_LANE_DIST)
1441 : // lane changing cannot possibly help
1442 652543 : || (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 43780 : 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 235610578 : getCanceledState(laneOffset) |= ret | blocked;
1463 235610578 : ret = myVehicle.influenceChangeDecision(ret);
1464 235610578 : 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 235610578 : if ((ret & LCA_STAY) != 0) {
1475 : // remove TraCI flags because it should not be included in "state-without-traci"
1476 36052608 : ret = getCanceledState(laneOffset);
1477 36052608 : return ret;
1478 : }
1479 199557970 : if ((ret & LCA_URGENT) != 0) {
1480 : // prepare urgent lane change maneuver
1481 : // save the left space
1482 4428737 : myLeftSpace = currentDist - posOnLane;
1483 4428737 : 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 655170 : 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 4428737 : const bool canContinue = curr.bestContinuations.size() > 1;
1496 4428737 : bool canReserve = MSLCHelper::updateBlockerLength(myVehicle, neighLead.first, lcaCounter, myLeftSpace - MAGIC_OFFSET, canContinue, myLeadingBlockerLength);
1497 4428737 : if (firstBlocked != neighLead.first) {
1498 4178154 : canReserve &= MSLCHelper::updateBlockerLength(myVehicle, firstBlocked, lcaCounter, myLeftSpace - MAGIC_OFFSET, 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 4428737 : if (!canReserve && !isOpposite()) {
1506 : // we have a low-priority relief connection
1507 : // std::cout << SIMTIME << " veh=" << myVehicle.getID() << " cannotReserve for blockers\n";
1508 4594 : myDontBrake = canContinue;
1509 : }
1510 :
1511 4428737 : const int remainingLanes = MAX2(1, abs(bestLaneOffset));
1512 4428737 : const double urgency = isOpposite() ? OPPOSITE_URGENCY : URGENCY;
1513 4428737 : const double remainingSeconds = ((ret & LCA_TRACI) == 0 ?
1514 : //MAX2(STEPS2TIME(TS), (myLeftSpace-myLeadingBlockerLength) / MAX2(myLookAheadSpeed, NUMERICAL_EPS) / remainingLanes / urgency) :
1515 4618294 : MAX2(STEPS2TIME(TS), myLeftSpace / MAX2(myLookAheadSpeed, NUMERICAL_EPS) / remainingLanes / urgency) :
1516 220 : myVehicle.getInfluencer().changeRequestRemainingSeconds(currentTime));
1517 4428737 : if (!hasBidiNeighLeader) {
1518 4428704 : 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 4428704 : if (plannedSpeed >= 0 || (!MSGlobals::gSemiImplicitEulerUpdate && plannedSpeed != -1)) {
1522 : // maybe we need to deal with a blocking follower
1523 2531575 : const bool hasBidiNeighFollower = neighLane.getBidiLane() != nullptr && MSLCHelper::isBidiFollower(&myVehicle, neighFollow.first);
1524 : if (!hasBidiNeighFollower) {
1525 2528599 : 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 4428737 : ret = getCanceledState(laneOffset);
1553 4428737 : 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 183601550 : const bool acceleratingLeader = (neighLead.first != 0 && neighLead.first->getAcceleration() > 0)
1559 284944069 : || (leader.first != 0 && leader.first->getAcceleration() > 0);
1560 195129233 : double neighLaneVSafe = MIN2(neighVMax, anticipateFollowSpeed(neighLead, neighDist, neighVMax, acceleratingLeader));
1561 195129233 : 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 195129233 : ? mySpeedGainProbabilityLeft / myChangeProbThresholdRight
1572 100718575 : : mySpeedGainProbabilityRight / myChangeProbThresholdLeft;
1573 :
1574 251527391 : const double relSpeedDiff = thisLaneVSafe == 0 ? 0 : (thisLaneVSafe - neighLaneVSafe) / MAX2(thisLaneVSafe, neighLaneVSafe);
1575 : inconvenience = MAX2(relSpeedDiff, inconvenience);
1576 : inconvenience = MIN2(1.0, inconvenience);
1577 :
1578 195129233 : const bool speedGainInconvenient = inconvenience > myCooperativeParam;
1579 195129233 : 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 195129233 : 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 4663841 : 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 771221 : if (MSGlobals::gSemiImplicitEulerUpdate || !neighOccupancyInconvenient) {
1612 : // if(MSGlobals::gSemiImplicitEulerUpdate || !speedGainInconvenient){
1613 771221 : req = ret | lca | LCA_COOPERATIVE;
1614 : }
1615 : } else {
1616 : // if inconvenience is not too high, request collaborative change (currently only for ballistic update)
1617 3892620 : if (MSGlobals::gSemiImplicitEulerUpdate || neighOccupancyInconvenient) {
1618 : // if(MSGlobals::gSemiImplicitEulerUpdate || speedGainInconvenient){
1619 3892620 : req = ret | LCA_STAY | LCA_COOPERATIVE;
1620 : }
1621 : }
1622 4663841 : if (!cancelRequest(req, laneOffset)) {
1623 4663841 : 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 190465392 : if (right) {
1630 90518038 : 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 87399 : req = ret | LCA_STAY | LCA_STRATEGIC;
1637 87399 : 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 155122 : && (!speedGainInconvenient)
1654 154597 : && ((myOwnState & myLca) != 0) // VARIANT_6 : counterNoHelp
1655 190429002 : && (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 50984 : req = ret | lca | LCA_COOPERATIVE | LCA_URGENT ;//| LCA_CHANGE_TO_HELP;
1669 50984 : if (!cancelRequest(req, laneOffset)) {
1670 50978 : 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 50978 : 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 190327040 : if (neighLane.getEdge().getPersons().size() > 0) {
1698 : // react to pedestrians
1699 47891 : adaptSpeedToPedestrians(myVehicle.getLane(), thisLaneVSafe);
1700 47891 : adaptSpeedToPedestrians(&neighLane, neighLaneVSafe);
1701 : }
1702 :
1703 190327040 : const double relativeGain = (neighLaneVSafe - thisLaneVSafe) / MAX2(neighLaneVSafe,
1704 190327040 : 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 190327040 : if (right) {
1720 : // ONLY FOR CHANGING TO THE RIGHT
1721 90426604 : if (thisLaneVSafe - 5 / 3.6 > neighLaneVSafe) {
1722 : // ok, the current lane is faster than the right one...
1723 60627963 : 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 29798641 : mySpeedGainProbabilityRight += myVehicle.getActionStepLengthSecs() * relativeGain;
1728 :
1729 : // honor the obligation to keep right (Rechtsfahrgebot)
1730 29798641 : const double roadSpeedFactor = vMax / myVehicle.getLane()->getSpeedLimit(); // differse from speedFactor if vMax < speedLimit
1731 : double acceptanceTime;
1732 29798641 : if (myKeepRightAcceptanceTime == -1) {
1733 : // legacy behavior: scale acceptance time with current speed and
1734 : // use old hard-coded constant
1735 59594674 : 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 29798641 : double fullSpeedGap = MAX2(0., neighDist - myVehicle.getCarFollowModel().brakeGap(vMax));
1750 29798641 : double fullSpeedDrivingSeconds = MIN2(acceptanceTime, fullSpeedGap / vMax);
1751 29798641 : if (neighLead.first != 0 && neighLead.first->getSpeed() < vMax) {
1752 23605195 : fullSpeedGap = MAX2(0., MIN2(fullSpeedGap,
1753 23605195 : neighLead.second - myVehicle.getCarFollowModel().getSecureGap(&myVehicle, neighLead.first,
1754 23605195 : vMax, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())));
1755 23605195 : 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 4599128 : if (checkOverTakeRight && leader.first != 0
1759 33881971 : && leader.first->getLane()->getVehicleMaxSpeed(leader.first) < vMax) {
1760 1283071 : fullSpeedGap = MIN2(fullSpeedGap, leader.second);
1761 1283071 : fullSpeedDrivingSeconds = MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - leader.first->getSpeed()));
1762 : }
1763 :
1764 29798641 : const double deltaProb = (myChangeProbThresholdRight * (fullSpeedDrivingSeconds / acceptanceTime) / KEEP_RIGHT_TIME);
1765 29798641 : 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 29798641 : if (myKeepRightProbability * myKeepRightParam < -myChangeProbThresholdRight) {
1794 3181266 : req = ret | lca | LCA_KEEPRIGHT;
1795 3181266 : if (!cancelRequest(req, laneOffset)) {
1796 3180841 : 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 87245763 : if (mySpeedGainProbabilityRight > myChangeProbThresholdRight
1817 87810427 : && neighDist / MAX2(.1, myVehicle.getSpeed()) > mySpeedGainRemainTime) { //./MAX2( .1, myVehicle.getSpeed())) { // -.1
1818 544311 : req = ret | lca | LCA_SPEEDGAIN;
1819 544311 : if (mySpeedGainProbabilityRight > mySpeedGainUrgency) {
1820 935 : req |= LCA_URGENT;
1821 : }
1822 544311 : if (!cancelRequest(req, laneOffset)) {
1823 544311 : return ret | req;
1824 : }
1825 : }
1826 : } else {
1827 : // ONLY FOR CHANGING TO THE LEFT
1828 99900436 : if (thisLaneVSafe > neighLaneVSafe) {
1829 : // this lane is better
1830 60117811 : mySpeedGainProbabilityLeft *= pow(0.5, myVehicle.getActionStepLengthSecs());
1831 39782625 : } else if (thisLaneVSafe == neighLaneVSafe) {
1832 19715407 : mySpeedGainProbabilityLeft *= pow(0.8, myVehicle.getActionStepLengthSecs());
1833 : } else {
1834 20067218 : 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 99900436 : if (mySpeedGainProbabilityLeft > myChangeProbThresholdLeft
1862 10419357 : && (relativeGain > NUMERICAL_EPS || changeLeftToAvoidOvertakeRight)
1863 115664890 : && neighDist / MAX2(.1, myVehicle.getSpeed()) > mySpeedGainRemainTime) { // .1
1864 7577771 : req = ret | lca | LCA_SPEEDGAIN;
1865 7577771 : if (mySpeedGainProbabilityLeft > mySpeedGainUrgency) {
1866 2291 : req |= LCA_URGENT;
1867 : }
1868 7577771 : if (!cancelRequest(req, laneOffset)) {
1869 7577655 : if ((req & LCA_URGENT) && (blocked & LCA_BLOCKED_BY_LEFT_FOLLOWER)) {
1870 2190 : MSVehicle* nv = neighFollow.first;
1871 2190 : const bool hasBidiNeighFollower = neighLane.getBidiLane() != nullptr && MSLCHelper::isBidiFollower(&myVehicle, nv);
1872 2190 : if (nv != nullptr && !hasBidiNeighFollower) {
1873 2190 : const double helpSpeed = MAX2(nv->getCarFollowModel().minNextSpeed(nv->getSpeed(), nv), myVehicle.getSpeed() - 1);
1874 2190 : msgPass.informNeighFollower(new Info(helpSpeed, myLca | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
1875 : }
1876 : }
1877 7577655 : return ret | req;
1878 : }
1879 : }
1880 : }
1881 : // --------
1882 179024233 : if (changeToBest && bestLaneOffset == curr.bestLaneOffset
1883 2638667 : && myStrategicParam >= 0
1884 2631976 : && relativeGain >= 0
1885 961782 : && (right ? mySpeedGainProbabilityRight : mySpeedGainProbabilityLeft) > 0) {
1886 : // change towards the correct lane, speedwise it does not hurt
1887 134294 : req = ret | lca | LCA_STRATEGIC;
1888 134294 : if (!cancelRequest(req, laneOffset)) {
1889 134294 : 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 390258466 : MSLCM_LC2013::anticipateFollowSpeed(const std::pair<MSVehicle*, double>& leaderDist, double dist, double vMax, bool acceleratingLeader) {
1911 390258466 : const MSVehicle* leader = leaderDist.first;
1912 390258466 : const double gap = leaderDist.second;
1913 : double futureSpeed;
1914 390258466 : if (acceleratingLeader) {
1915 : // XXX see #6562
1916 273260204 : const double maxSpeed1s = (myVehicle.getSpeed() + myVehicle.getCarFollowModel().getMaxAccel()
1917 273260204 : - ACCEL2SPEED(myVehicle.getCarFollowModel().getMaxAccel()));
1918 273260204 : if (leader == nullptr) {
1919 4857621 : if (hasBlueLight()) {
1920 : // can continue from any lane if necessary
1921 : futureSpeed = vMax;
1922 : } else {
1923 4857579 : futureSpeed = getCarFollowModel().followSpeed(&myVehicle, maxSpeed1s, dist, 0, 0);
1924 : }
1925 : } else {
1926 268402583 : futureSpeed = getCarFollowModel().followSpeed(&myVehicle, maxSpeed1s, gap, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel());
1927 : }
1928 : } else {
1929 : // onInsertion = true because the vehicle has already moved
1930 116998262 : if (leader == nullptr) {
1931 16770079 : if (hasBlueLight()) {
1932 : // can continue from any lane if necessary
1933 : futureSpeed = vMax;
1934 : } else {
1935 16768504 : futureSpeed = getCarFollowModel().maximumSafeStopSpeed(dist, getCarFollowModel().getMaxDecel(), myVehicle.getSpeed(), true);
1936 : }
1937 : } else {
1938 100228183 : futureSpeed = getCarFollowModel().maximumSafeFollowSpeed(gap, myVehicle.getSpeed(), leader->getSpeed(), leader->getCarFollowModel().getMaxDecel(), true);
1939 : }
1940 : }
1941 : futureSpeed = MIN2(vMax, futureSpeed);
1942 390258466 : 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 390258466 : return futureSpeed;
1960 : }
1961 :
1962 :
1963 : int
1964 242965134 : MSLCM_LC2013::slowDownForBlocked(MSVehicle* blocked, int state) {
1965 : // if this vehicle is blocking someone in front, we maybe decelerate to let him in
1966 242965134 : if (blocked != nullptr) {
1967 17832041 : 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 17832041 : if (gap > POSITION_EPS) {
1978 : //const bool blockedWantsUrgentRight = (((*blocked)->getLaneChangeModel().getOwnState() & LCA_RIGHT != 0)
1979 : // && ((*blocked)->getLaneChangeModel().getOwnState() & LCA_URGENT != 0));
1980 :
1981 14241589 : if (myVehicle.getSpeed() < myVehicle.getCarFollowModel().getMaxDecel()
1982 : //|| blockedWantsUrgentRight // VARIANT_10 (helpblockedRight)
1983 : ) {
1984 12509105 : if (blocked->getSpeed() < SUMO_const_haltingSpeed) {
1985 6915672 : state |= LCA_AMBACKBLOCKER_STANDING;
1986 : } else {
1987 5593433 : state |= LCA_AMBACKBLOCKER;
1988 : }
1989 25018210 : addLCSpeedAdvice(getCarFollowModel().followSpeed(
1990 12509105 : &myVehicle, myVehicle.getSpeed(),
1991 12509105 : 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 242965134 : return state;
2017 : }
2018 :
2019 :
2020 : void
2021 95782 : MSLCM_LC2013::adaptSpeedToPedestrians(const MSLane* lane, double& v) {
2022 95782 : 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 20793 : PersonDist leader = lane->nextBlocking(myVehicle.getPositionOnLane(),
2029 20793 : myVehicle.getRightSideOnLane(), myVehicle.getRightSideOnLane() + myVehicle.getVehicleType().getWidth(),
2030 20793 : ceil(myVehicle.getSpeed() / myVehicle.getCarFollowModel().getMaxDecel()));
2031 20793 : 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 95782 : }
2042 :
2043 :
2044 : double
2045 605845 : MSLCM_LC2013::computeSpeedLat(double latDist, double& maneuverDist, bool urgent) const {
2046 605845 : 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 605845 : if (myLeftSpace > POSITION_EPS || !urgent) {
2053 492248 : double speedBound = myMaxSpeedLatStanding + myMaxSpeedLatFactor * myVehicle.getSpeed();
2054 492248 : if (isChangingLanes()) {
2055 : speedBound = MAX2(LC_RESOLUTION_SPEED_LAT, speedBound);
2056 : }
2057 492248 : result = MAX2(-speedBound, MIN2(speedBound, result));
2058 : }
2059 605845 : return result;
2060 : }
2061 :
2062 :
2063 : double
2064 500215397 : MSLCM_LC2013::getSafetyFactor() const {
2065 500215397 : return 1 / myAssertive;
2066 : }
2067 :
2068 : double
2069 9143582 : MSLCM_LC2013::getOppositeSafetyFactor() const {
2070 9143582 : return myOppositeParam <= 0 ? std::numeric_limits<double>::max() : 1 / myOppositeParam;
2071 : }
2072 :
2073 : bool
2074 182783 : MSLCM_LC2013::saveBlockerLength(double length, double foeLeftSpace) {
2075 182783 : const bool canReserve = MSLCHelper::canSaveBlockerLength(myVehicle, length, myLeftSpace);
2076 182783 : if (!isOpposite() && (canReserve || myLeftSpace > foeLeftSpace)) {
2077 178147 : 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 178147 : if (myLeftSpace == 0 && foeLeftSpace < 0) {
2084 : // called from opposite overtaking, myLeftSpace must be initialized
2085 171633 : myLeftSpace = myVehicle.getBestLanes()[myVehicle.getLane()->getIndex()].length - myVehicle.getPositionOnLane();
2086 : }
2087 178147 : return true;
2088 : } else {
2089 : return false;
2090 : }
2091 : }
2092 :
2093 :
2094 : bool
2095 25475 : MSLCM_LC2013::hasFreeLane(int laneOffset, const std::pair<MSVehicle*, double>& neighLeadStopped) const {
2096 25475 : if (neighLeadStopped.first == nullptr) {
2097 : return true;
2098 : }
2099 25475 : int dir = (laneOffset > 0 ? 1 : -1);
2100 25475 : const MSLane* neigh = myVehicle.getLane()->getParallelLane(laneOffset);
2101 25475 : if (dir > 0 && !neigh->allowsChangingLeft(myVehicle.getVClass())) {
2102 : return false;
2103 25475 : } else if (dir < 0 && !neigh->allowsChangingRight(myVehicle.getVClass())) {
2104 : return false;
2105 : }
2106 25475 : int nextOffset = laneOffset + dir;
2107 25475 : const MSLane* next = myVehicle.getLane()->getParallelLane(nextOffset);
2108 25475 : if (next == nullptr || !next->allowsVehicleClass(myVehicle.getVClass())) {
2109 17995 : return false;
2110 : }
2111 7480 : const double overtakeDist = neighLeadStopped.second + neighLeadStopped.first->getVehicleType().getLengthWithGap() + myVehicle.getLength() + POSITION_EPS;
2112 7480 : std::pair<MSVehicle* const, double> nextLead = next->getLeader(&myVehicle, myVehicle.getPositionOnLane(), myVehicle.getBestLanesContinuation(next), overtakeDist);
2113 7981 : 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 12018 : MSLCM_LC2013::setParameter(const std::string& key, const std::string& value) {
2182 : double doubleValue;
2183 : try {
2184 12018 : 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 12018 : if (key == toString(SUMO_ATTR_LCA_STRATEGIC_PARAM)) {
2189 3176 : myStrategicParam = doubleValue;
2190 8842 : } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_PARAM)) {
2191 0 : myCooperativeParam = doubleValue;
2192 8842 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_PARAM)) {
2193 0 : mySpeedGainParam = doubleValue;
2194 8842 : } else if (key == toString(SUMO_ATTR_LCA_KEEPRIGHT_PARAM)) {
2195 0 : myKeepRightParam = doubleValue;
2196 8842 : } else if (key == toString(SUMO_ATTR_LCA_OPPOSITE_PARAM)) {
2197 0 : myOppositeParam = doubleValue;
2198 8842 : } else if (key == toString(SUMO_ATTR_LCA_LOOKAHEADLEFT)) {
2199 0 : myLookaheadLeft = doubleValue;
2200 8842 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAINRIGHT)) {
2201 0 : mySpeedGainRight = doubleValue;
2202 8842 : } else if (key == toString(SUMO_ATTR_LCA_ASSERTIVE)) {
2203 0 : myAssertive = doubleValue;
2204 8842 : } else if (key == toString(SUMO_ATTR_LCA_OVERTAKE_RIGHT)) {
2205 0 : myOvertakeRightParam = doubleValue;
2206 8842 : } else if (key == toString(SUMO_ATTR_LCA_SIGMA)) {
2207 0 : mySigma = doubleValue;
2208 8842 : } else if (key == toString(SUMO_ATTR_LCA_KEEPRIGHT_ACCEPTANCE_TIME)) {
2209 0 : myKeepRightAcceptanceTime = doubleValue;
2210 8842 : } else if (key == toString(SUMO_ATTR_LCA_OVERTAKE_DELTASPEED_FACTOR)) {
2211 0 : myOvertakeDeltaSpeedFactor = doubleValue;
2212 8842 : } else if (key == toString(SUMO_ATTR_LCA_STRATEGIC_LOOKAHEAD)) {
2213 0 : myStrategicLookahead = doubleValue;
2214 8842 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_LOOKAHEAD)) {
2215 2946 : mySpeedGainLookahead = doubleValue;
2216 5896 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_REMAIN_TIME)) {
2217 2946 : mySpeedGainRemainTime = doubleValue;
2218 2950 : } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_ROUNDABOUT)) {
2219 0 : myRoundaboutBonus = doubleValue;
2220 2950 : } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_SPEED)) {
2221 0 : myCooperativeSpeed = doubleValue;
2222 2950 : } else if (key == toString(SUMO_ATTR_LCA_MAXSPEEDLATSTANDING)) {
2223 0 : myMaxSpeedLatStanding = doubleValue;
2224 2950 : } else if (key == toString(SUMO_ATTR_LCA_MAXSPEEDLATFACTOR)) {
2225 0 : myMaxSpeedLatFactor = doubleValue;
2226 2950 : } else if (key == toString(SUMO_ATTR_LCA_MAXDISTLATSTANDING)) {
2227 0 : myMaxDistLatStanding = doubleValue;
2228 : // access to internal state
2229 2950 : } else if (key == "speedGainProbabilityRight") {
2230 0 : mySpeedGainProbabilityRight = doubleValue;
2231 2950 : } else if (key == "speedGainProbabilityLeft") {
2232 0 : mySpeedGainProbabilityLeft = doubleValue;
2233 2950 : } else if (key == "keepRightProbability") {
2234 0 : myKeepRightProbability = -doubleValue;
2235 2950 : } else if (key == "lookAheadSpeed") {
2236 0 : myLookAheadSpeed = doubleValue;
2237 : } else {
2238 11800 : throw InvalidArgument("Setting parameter '" + key + "' is not supported for laneChangeModel of type '" + toString(myModel) + "'");
2239 : }
2240 9068 : initDerivedParameters();
2241 9068 : }
2242 :
2243 :
2244 : /****************************************************************************/
|
