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