Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2013-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_SL2015.cpp
15 : /// @author Jakob Erdmann
16 : /// @date Tue, 06.10.2015
17 : ///
18 : // A lane change model for heterogeneous traffic (based on sub-lanes)
19 : /****************************************************************************/
20 : #include <config.h>
21 :
22 : #include <iostream>
23 : #include <utils/common/RandHelper.h>
24 : #include <utils/common/StringUtils.h>
25 : #include <microsim/MSEdge.h>
26 : #include <microsim/MSLane.h>
27 : #include <microsim/MSLink.h>
28 : #include <microsim/MSNet.h>
29 : #include <microsim/MSDriverState.h>
30 : #include <microsim/MSGlobals.h>
31 : #include <microsim/MSStop.h>
32 : #include <microsim/MSStoppingPlace.h>
33 : #include <microsim/transportables/MSTransportableControl.h>
34 : #include <microsim/transportables/MSPModel.h>
35 : #include "MSLCHelper.h"
36 : #include "MSLCM_SL2015.h"
37 :
38 : // ===========================================================================
39 : // variable definitions
40 : // ===========================================================================
41 : #define LOOK_FORWARD 10.
42 :
43 : #define JAM_FACTOR 1.
44 :
45 : #define LCA_RIGHT_IMPATIENCE -1.
46 : #define CUT_IN_LEFT_SPEED_THRESHOLD 27.
47 : #define MAX_ONRAMP_LENGTH 200.
48 :
49 : #define LOOK_AHEAD_MIN_SPEED 0.0
50 : #define LOOK_AHEAD_SPEED_MEMORY 0.9
51 :
52 : #define HELP_DECEL_FACTOR 1.0
53 :
54 : #define HELP_OVERTAKE (10.0 / 3.6)
55 : #define MIN_FALLBEHIND (7.0 / 3.6)
56 :
57 : #define URGENCY 2.0
58 :
59 : #define KEEP_RIGHT_TIME 5.0 // the number of seconds after which a vehicle should move to the right lane
60 :
61 : #define RELGAIN_NORMALIZATION_MIN_SPEED 10.0
62 :
63 : #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
64 : #define GAIN_PERCEPTION_THRESHOLD 0.05 // the minimum relative speed gain which affects the behavior
65 :
66 : #define ARRIVALPOS_LAT_THRESHOLD 100.0
67 :
68 : // the speed at which the desired lateral gap grows now further
69 : #define LATGAP_SPEED_THRESHOLD (50 / 3.6)
70 : // the speed at which the desired lateral gap shrinks now further.
71 : // @note: when setting LATGAP_SPEED_THRESHOLD = LATGAP_SPEED_THRESHOLD2, no speed-specif reduction of minGapLat is done
72 : #define LATGAP_SPEED_THRESHOLD2 (50 / 3.6)
73 :
74 : // intention to change decays over time
75 : #define SPEEDGAIN_DECAY_FACTOR 0.5
76 : // exponential averaging factor for expected sublane speeds
77 : #define SPEEDGAIN_MEMORY_FACTOR 0.5
78 :
79 : #define REACT_TO_STOPPED_DISTANCE 100
80 :
81 :
82 : // ===========================================================================
83 : // Debug flags
84 : // ===========================================================================
85 : //#define DEBUG_MANEUVER
86 : //#define DEBUG_WANTSCHANGE
87 : //#define DEBUG_DECISION
88 : //#define DEBUG_STRATEGIC_CHANGE
89 : //#define DEBUG_KEEP_LATGAP
90 : //#define DEBUG_STATE
91 : //#define DEBUG_ACTIONSTEPS
92 : //#define DEBUG_COMMITTED_SPEED
93 : //#define DEBUG_PATCHSPEED
94 : //#define DEBUG_INFORM
95 : //#define DEBUG_ROUNDABOUTS
96 : //#define DEBUG_COOPERATE
97 : //#define DEBUG_SLOWDOWN
98 : //#define DEBUG_SAVE_BLOCKER_LENGTH
99 : //#define DEBUG_BLOCKING
100 : //#define DEBUG_TRACI
101 : //#define DEBUG_EXPECTED_SLSPEED
102 : //#define DEBUG_SLIDING
103 : //#define DEBUG_COND (myVehicle.getID() == "moped.18" || myVehicle.getID() == "moped.16")
104 : //#define DEBUG_COND (myVehicle.getID() == "Togliatti_71_0")
105 : #define DEBUG_COND (myVehicle.isSelected())
106 : //#define DEBUG_COND (myVehicle.getID() == "pkw150478" || myVehicle.getID() == "pkw150494" || myVehicle.getID() == "pkw150289")
107 : //#define DEBUG_COND (myVehicle.getID() == "A" || myVehicle.getID() == "B") // fail change to left
108 : //#define DEBUG_COND (myVehicle.getID() == "disabled") // test stops_overtaking
109 : //#define DEBUG_COND true
110 :
111 :
112 : // ===========================================================================
113 : // member method definitions
114 : // ===========================================================================
115 852337 : MSLCM_SL2015::MSLCM_SL2015(MSVehicle& v) :
116 : MSAbstractLaneChangeModel(v, LaneChangeModel::SL2015),
117 852337 : mySpeedGainProbabilityRight(0),
118 852337 : mySpeedGainProbabilityLeft(0),
119 852337 : myKeepRightProbability(0),
120 852337 : myLeadingBlockerLength(0),
121 852337 : myLeftSpace(0),
122 852337 : myLookAheadSpeed(LOOK_AHEAD_MIN_SPEED),
123 852337 : myLastEdge(nullptr),
124 852337 : myCanChangeFully(true),
125 852337 : mySafeLatDistRight(0),
126 852337 : mySafeLatDistLeft(0),
127 852337 : myStrategicParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_STRATEGIC_PARAM, 1)),
128 852337 : myCooperativeParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_COOPERATIVE_PARAM, 1)),
129 852337 : mySpeedGainParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAIN_PARAM, 1)),
130 852337 : myKeepRightParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_KEEPRIGHT_PARAM, 1)),
131 852337 : myOppositeParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_OPPOSITE_PARAM, 1)),
132 852337 : mySublaneParam(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SUBLANE_PARAM, 1)),
133 : // by default use SUMO_ATTR_LCA_PUSHY. If that is not set, try SUMO_ATTR_LCA_PUSHYGAP
134 852337 : myMinGapLat(v.getVehicleType().getMinGapLat()),
135 852337 : myPushy(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_PUSHY,
136 1704577 : 1 - (v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_PUSHYGAP,
137 852337 : MAX2(NUMERICAL_EPS, myMinGapLat)) /
138 852337 : MAX2(NUMERICAL_EPS, myMinGapLat)))),
139 852337 : myImpatience(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_IMPATIENCE, 0)),
140 852337 : myMinImpatience(myImpatience),
141 852337 : myTimeToImpatience(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_TIME_TO_IMPATIENCE, std::numeric_limits<double>::max())),
142 852337 : myAccelLat(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_ACCEL_LAT, 1.0)),
143 852337 : myTurnAlignmentDist(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_TURN_ALIGNMENT_DISTANCE, 0.0)),
144 852337 : myLookaheadLeft(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_LOOKAHEADLEFT, 2.0)),
145 852337 : mySpeedGainRight(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAINRIGHT, 0.1)),
146 852337 : myLaneDiscipline(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_LANE_DISCIPLINE, 0.0)),
147 852337 : mySpeedGainLookahead(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAIN_LOOKAHEAD, 5)),
148 852337 : mySpeedGainRemainTime(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAIN_REMAIN_TIME, 20)),
149 852337 : myRoundaboutBonus(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_COOPERATIVE_ROUNDABOUT, myCooperativeParam)),
150 852337 : myCooperativeSpeed(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_COOPERATIVE_SPEED, myCooperativeParam)),
151 852337 : myKeepRightAcceptanceTime(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_KEEPRIGHT_ACCEPTANCE_TIME, -1)),
152 852337 : myOvertakeDeltaSpeedFactor(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_OVERTAKE_DELTASPEED_FACTOR, 0)),
153 852337 : mySigmaState(0) {
154 852337 : initDerivedParameters();
155 852337 : }
156 :
157 1704658 : MSLCM_SL2015::~MSLCM_SL2015() {
158 852329 : changed();
159 1704658 : }
160 :
161 :
162 : void
163 1129677 : MSLCM_SL2015::initDerivedParameters() {
164 1129677 : if (mySpeedGainParam <= 0) {
165 2235 : myChangeProbThresholdRight = std::numeric_limits<double>::max();
166 2235 : myChangeProbThresholdLeft = std::numeric_limits<double>::max();
167 : } else {
168 1127442 : myChangeProbThresholdRight = (0.2 / mySpeedGainRight) / mySpeedGainParam;
169 1127442 : myChangeProbThresholdLeft = 0.2 / mySpeedGainParam;
170 : }
171 1129677 : mySpeedLossProbThreshold = (-0.1 + (1 - mySublaneParam));
172 1129677 : }
173 :
174 :
175 : bool
176 15318 : MSLCM_SL2015::debugVehicle() const {
177 15318 : return DEBUG_COND;
178 : }
179 :
180 :
181 : int
182 118380750 : MSLCM_SL2015::wantsChangeSublane(
183 : int laneOffset,
184 : LaneChangeAction alternatives,
185 : const MSLeaderDistanceInfo& leaders,
186 : const MSLeaderDistanceInfo& followers,
187 : const MSLeaderDistanceInfo& blockers,
188 : const MSLeaderDistanceInfo& neighLeaders,
189 : const MSLeaderDistanceInfo& neighFollowers,
190 : const MSLeaderDistanceInfo& neighBlockers,
191 : const MSLane& neighLane,
192 : const std::vector<MSVehicle::LaneQ>& preb,
193 : MSVehicle** lastBlocked,
194 : MSVehicle** firstBlocked,
195 : double& latDist, double& maneuverDist, int& blocked) {
196 :
197 118380750 : gDebugFlag2 = DEBUG_COND;
198 200918815 : const std::string changeType = laneOffset == -1 ? "right" : (laneOffset == 1 ? "left" : "current");
199 :
200 : #ifdef DEBUG_MANEUVER
201 : if (gDebugFlag2) {
202 : std::cout << "\n" << SIMTIME
203 : << std::setprecision(gPrecision)
204 : << " veh=" << myVehicle.getID()
205 : << " lane=" << myVehicle.getLane()->getID()
206 : << " neigh=" << neighLane.getID()
207 : << " pos=" << myVehicle.getPositionOnLane()
208 : << " posLat=" << myVehicle.getLateralPositionOnLane()
209 : << " posLatError=" << mySigmaState
210 : << " speed=" << myVehicle.getSpeed()
211 : << " considerChangeTo=" << changeType
212 : << "\n";
213 : }
214 : #endif
215 :
216 118380750 : int result = _wantsChangeSublane(laneOffset,
217 : alternatives,
218 : leaders, followers, blockers,
219 : neighLeaders, neighFollowers, neighBlockers,
220 : neighLane, preb,
221 : lastBlocked, firstBlocked, latDist, maneuverDist, blocked);
222 :
223 118380750 : result = keepLatGap(result, leaders, followers, blockers,
224 : neighLeaders, neighFollowers, neighBlockers,
225 : neighLane, laneOffset, latDist, maneuverDist, blocked);
226 :
227 118380750 : result |= getLCA(result, latDist);
228 : // take into account lateral acceleration
229 : #if defined(DEBUG_MANEUVER) || defined(DEBUG_STATE)
230 : double latDistTmp = latDist;
231 : #endif
232 118380750 : latDist = SPEED2DIST(computeSpeedLat(latDist, maneuverDist, (result & LCA_URGENT) != 0));
233 : #if defined(DEBUG_MANEUVER) || defined(DEBUG_STATE)
234 : if (gDebugFlag2 && latDist != latDistTmp) {
235 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " maneuverDist=" << maneuverDist << " latDist=" << latDistTmp << " mySpeedPrev=" << mySpeedLat << " speedLat=" << DIST2SPEED(latDist) << " latDist2=" << latDist << "\n";
236 : }
237 :
238 : if (gDebugFlag2) {
239 : if (result & LCA_WANTS_LANECHANGE) {
240 : std::cout << SIMTIME
241 : << " veh=" << myVehicle.getID()
242 : << " wantsChangeTo=" << changeType
243 : << " latDist=" << latDist
244 : << " maneuverDist=" << maneuverDist
245 : << " state=" << toString((LaneChangeAction)result)
246 : << ((blocked & LCA_BLOCKED) ? " (blocked)" : "")
247 : << ((blocked & LCA_OVERLAPPING) ? " (overlap)" : "")
248 : << "\n\n";
249 : } else {
250 : std::cout << SIMTIME
251 : << " veh=" << myVehicle.getID()
252 : << " wantsNoChangeTo=" << changeType
253 : << " state=" << toString((LaneChangeAction)result)
254 : << "\n\n";
255 : }
256 : }
257 : #endif
258 118380750 : gDebugFlag2 = false;
259 118380750 : return result;
260 : }
261 :
262 : void
263 82595349 : MSLCM_SL2015::setOwnState(const int state) {
264 82595349 : MSAbstractLaneChangeModel::setOwnState(state);
265 82595349 : if (myVehicle.isActive()) {
266 82595333 : if ((state & (LCA_STRATEGIC | LCA_SPEEDGAIN)) != 0 && (state & LCA_BLOCKED) != 0) {
267 8542529 : myImpatience = MIN2(1.0, myImpatience + myVehicle.getActionStepLengthSecs() / myTimeToImpatience);
268 : } else {
269 : // impatience decays only to the driver-specific level
270 81395695 : myImpatience = MAX2(myMinImpatience, myImpatience - myVehicle.getActionStepLengthSecs() / myTimeToImpatience);
271 : }
272 : #ifdef DEBUG_STATE
273 : if (DEBUG_COND) {
274 : std::cout << SIMTIME << " veh=" << myVehicle.getID()
275 : << " setOwnState=" << toString((LaneChangeAction)state)
276 : << " myMinImpatience=" << myMinImpatience
277 : << " myImpatience=" << myImpatience
278 : << "\n";
279 : }
280 : #endif
281 82595333 : if ((state & LCA_STAY) != 0) {
282 72012116 : myCanChangeFully = true;
283 : // if (DEBUG_COND) {
284 : // std::cout << " myCanChangeFully=true\n";
285 : // }
286 : }
287 : }
288 82595349 : }
289 :
290 :
291 : void
292 4032540 : MSLCM_SL2015::updateSafeLatDist(const double travelledLatDist) {
293 4032540 : mySafeLatDistLeft -= travelledLatDist;
294 4032540 : mySafeLatDistRight += travelledLatDist;
295 :
296 4032540 : if (fabs(mySafeLatDistLeft) < NUMERICAL_EPS) {
297 54649 : mySafeLatDistLeft = 0.;
298 : }
299 4032540 : if (fabs(mySafeLatDistRight) < NUMERICAL_EPS) {
300 91898 : mySafeLatDistRight = 0.;
301 : }
302 4032540 : }
303 :
304 :
305 : double
306 84040982 : MSLCM_SL2015::patchSpeed(const double min, const double wanted, const double max, const MSCFModel& cfModel) {
307 84040982 : gDebugFlag2 = DEBUG_COND;
308 : // negative min speed may be passed when using ballistic updated
309 84040982 : const double newSpeed = _patchSpeed(MAX2(min, 0.0), wanted, max, cfModel);
310 : #ifdef DEBUG_PATCHSPEED
311 : if (gDebugFlag2) {
312 : const std::string patched = (wanted != newSpeed ? " patched=" + toString(newSpeed) : "");
313 : std::cout << SIMTIME
314 : << " veh=" << myVehicle.getID()
315 : << " lane=" << myVehicle.getLane()->getID()
316 : << " pos=" << myVehicle.getPositionOnLane()
317 : << " v=" << myVehicle.getSpeed()
318 : << " min=" << min
319 : << " wanted=" << wanted
320 : << " max=" << max
321 : << patched
322 : << "\n\n";
323 : }
324 : #endif
325 84040982 : gDebugFlag2 = false;
326 84040982 : return newSpeed;
327 : }
328 :
329 :
330 : double
331 84040982 : MSLCM_SL2015::_patchSpeed(double min, const double wanted, double max, const MSCFModel& cfModel) {
332 84040982 : if (wanted <= 0) {
333 : return wanted;
334 : }
335 :
336 79030979 : int state = myOwnState;
337 :
338 : double nVSafe = wanted;
339 : bool gotOne = false;
340 : // letting vehicles merge in at the end of the lane in case of counter-lane change, step#2
341 : // if we want to change and have a blocking leader and there is enough room for him in front of us
342 79030979 : if (myLeadingBlockerLength != 0) {
343 383405 : double space = myLeftSpace - myLeadingBlockerLength - POSITION_EPS;
344 : #ifdef DEBUG_PATCHSPEED
345 : if (gDebugFlag2) {
346 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " myLeadingBlockerLength=" << myLeadingBlockerLength << " space=" << space << "\n";
347 : }
348 : #endif
349 383405 : if (space >= 0) {
350 : // compute speed for decelerating towards a place which allows the blocking leader to merge in in front
351 357014 : double safe = cfModel.stopSpeed(&myVehicle, myVehicle.getSpeed(), space, MSCFModel::CalcReason::LANE_CHANGE);
352 : max = MIN2(max, safe);
353 : // if we are approaching this place
354 357014 : if (safe < wanted) {
355 48779 : if (safe < min) {
356 2293 : const double vMinEmergency = myVehicle.getCarFollowModel().minNextSpeedEmergency(myVehicle.getSpeed(), &myVehicle);
357 2293 : if (safe >= vMinEmergency) {
358 : // permit harder braking if needed and helpful
359 : min = MAX2(vMinEmergency, safe);
360 : }
361 : }
362 : #ifdef DEBUG_PATCHSPEED
363 : if (gDebugFlag2) {
364 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " slowing down for leading blocker, safe=" << safe << (safe + NUMERICAL_EPS < min ? " (not enough)" : "") << "\n";
365 : }
366 : #endif
367 : nVSafe = MAX2(min, safe);
368 : gotOne = true;
369 : }
370 : }
371 : }
372 79030979 : const double coopWeight = MAX2(0.0, MIN2(1.0, myCooperativeSpeed));
373 104841157 : for (auto i : myLCAccelerationAdvices) {
374 : double accel = i.first;
375 25810178 : double v = myVehicle.getSpeed() + ACCEL2SPEED(accel);
376 25810178 : if (v >= min && v <= max) {
377 9633309 : if (i.second & LCA_CHANGE_TO_HELP) {
378 4473767 : nVSafe = MIN2(v * coopWeight + (1 - coopWeight) * wanted, nVSafe);
379 : } else {
380 : // own advice, no scaling needed
381 : nVSafe = MIN2(v, nVSafe);
382 : }
383 : gotOne = true;
384 : #ifdef DEBUG_PATCHSPEED
385 : if (gDebugFlag2) {
386 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " got accel=" << accel << " nVSafe=" << nVSafe << "\n";
387 : }
388 : #endif
389 : } else {
390 : #ifdef DEBUG_PATCHSPEED
391 : if (v < min) {
392 : if (gDebugFlag2) {
393 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " ignoring low nVSafe=" << v << " (accel=" << accel << ") min=" << min << "\n";
394 : }
395 : } else {
396 : if (gDebugFlag2) {
397 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " ignoring high nVSafe=" << v << " (accel=" << accel << ") max=" << max << "\n";
398 : }
399 : }
400 : #endif
401 : }
402 : }
403 :
404 79030979 : if (gotOne && !myDontBrake) {
405 : #ifdef DEBUG_PATCHSPEED
406 : if (gDebugFlag2) {
407 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " got vSafe\n";
408 : }
409 : #endif
410 : return nVSafe;
411 : }
412 :
413 : // check whether the vehicle is blocked
414 76610608 : if ((state & LCA_WANTS_LANECHANGE) != 0 && (state & LCA_BLOCKED) != 0) {
415 5020389 : if ((state & LCA_STRATEGIC) != 0) {
416 : // necessary decelerations are controlled via vSafe. If there are
417 : // none it means we should speed up
418 : #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
419 : if (gDebugFlag2) {
420 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_WANTS_LANECHANGE (strat, no vSafe)\n";
421 : }
422 : #endif
423 614386 : return (max + wanted) / 2.0;
424 4406003 : } else if ((state & LCA_COOPERATIVE) != 0) {
425 : // only minor adjustments in speed should be done
426 442166 : if ((state & LCA_BLOCKED_BY_LEADER) != 0) {
427 : #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
428 : if (gDebugFlag2) {
429 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_LEADER (coop)\n";
430 : }
431 : #endif
432 404719 : return (min + wanted) / 2.0;
433 : }
434 37447 : if ((state & LCA_BLOCKED_BY_FOLLOWER) != 0) {
435 : #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
436 : if (gDebugFlag2) {
437 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_FOLLOWER (coop)\n";
438 : }
439 : #endif
440 37447 : return (max + wanted) / 2.0;
441 : }
442 : //} else { // VARIANT_16
443 : // // only accelerations should be performed
444 : // if ((state & LCA_BLOCKED_BY_FOLLOWER) != 0) {
445 : // if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_BLOCKED_BY_FOLLOWER\n";
446 : // return (max + wanted) / 2.0;
447 : // }
448 : }
449 : }
450 :
451 : /*
452 : // decelerate if being a blocking follower
453 : // (and does not have to change lanes)
454 : if ((state & LCA_AMBLOCKINGFOLLOWER) != 0) {
455 : if (fabs(max - myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle)) < 0.001 && min == 0) { // !!! was standing
456 : if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGFOLLOWER (standing)\n";
457 : return 0;
458 : }
459 : if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGFOLLOWER\n";
460 :
461 : //return min; // VARIANT_3 (brakeStrong)
462 : return (min + wanted) / 2.0;
463 : }
464 : if ((state & LCA_AMBACKBLOCKER) != 0) {
465 : if (max <= myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle) && min == 0) { // !!! was standing
466 : if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBACKBLOCKER (standing)\n";
467 : //return min; VARIANT_9 (backBlockVSafe)
468 : return nVSafe;
469 : }
470 : }
471 : if ((state & LCA_AMBACKBLOCKER_STANDING) != 0) {
472 : if (gDebugFlag2) std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBACKBLOCKER_STANDING\n";
473 : //return min;
474 : return nVSafe;
475 : }
476 : */
477 :
478 : // accelerate if being a blocking leader or blocking follower not able to brake
479 : // (and does not have to change lanes)
480 75554056 : if ((state & LCA_AMBLOCKINGLEADER) != 0) {
481 : #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
482 : if (gDebugFlag2) {
483 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGLEADER\n";
484 : }
485 : #endif
486 856206 : return (max + wanted) / 2.0;
487 : }
488 :
489 : if ((state & LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0) {
490 : #if defined(DEBUG_PATCHSPEED) || defined(DEBUG_STATE)
491 : if (gDebugFlag2) {
492 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " LCA_AMBLOCKINGFOLLOWER_DONTBRAKE\n";
493 : }
494 : #endif
495 : /*
496 : // VARIANT_4 (dontbrake)
497 : if (max <= myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle) && min == 0) { // !!! was standing
498 : return wanted;
499 : }
500 : return (min + wanted) / 2.0;
501 : */
502 : }
503 : return wanted;
504 : }
505 :
506 :
507 : void*
508 11954243 : MSLCM_SL2015::inform(void* info, MSVehicle* sender) {
509 : Info* pinfo = (Info*) info;
510 11954243 : if (pinfo->first >= 0) {
511 7468768 : addLCSpeedAdvice(pinfo->first, LCA_CHANGE_TO_HELP);
512 : }
513 : //myOwnState &= 0xffffffff; // reset all bits of MyLCAEnum but only those
514 11954243 : myOwnState |= pinfo->second;
515 : #ifdef DEBUG_INFORM
516 : if (gDebugFlag2 || DEBUG_COND || sender->getLaneChangeModel().debugVehicle()) {
517 : std::cout << SIMTIME
518 : << " veh=" << myVehicle.getID()
519 : << " informedBy=" << sender->getID()
520 : << " info=" << pinfo->second
521 : << " vSafe=" << pinfo->first
522 : << "\n";
523 : }
524 : #else
525 : UNUSED_PARAMETER(sender);
526 : #endif
527 11954243 : delete pinfo;
528 11954243 : return (void*) true;
529 : }
530 :
531 :
532 : void
533 11954243 : MSLCM_SL2015::msg(const CLeaderDist& cld, double speed, int state) {
534 : assert(cld.first != 0);
535 11954243 : ((MSVehicle*)cld.first)->getLaneChangeModel().inform(new Info(speed, state), &myVehicle);
536 11954243 : }
537 :
538 :
539 : double
540 7859615 : MSLCM_SL2015::informLeader(int blocked,
541 : int dir,
542 : const CLeaderDist& neighLead,
543 : double remainingSeconds) {
544 15719230 : double plannedSpeed = MIN2(myVehicle.getSpeed(),
545 7859615 : myVehicle.getCarFollowModel().stopSpeed(&myVehicle, myVehicle.getSpeed(), myLeftSpace - myLeadingBlockerLength));
546 28395329 : for (auto i : myLCAccelerationAdvices) {
547 20535714 : double v = myVehicle.getSpeed() + ACCEL2SPEED(i.first);
548 20535714 : if (v >= myVehicle.getSpeed() - ACCEL2SPEED(myVehicle.getCarFollowModel().getMaxDecel())) {
549 : plannedSpeed = MIN2(plannedSpeed, v);
550 : }
551 : }
552 : #ifdef DEBUG_INFORM
553 : if (gDebugFlag2) {
554 : std::cout << " informLeader speed=" << myVehicle.getSpeed() << " planned=" << plannedSpeed << "\n";
555 : }
556 : #endif
557 :
558 7859615 : if ((blocked & LCA_BLOCKED_BY_LEADER) != 0 && neighLead.first != 0) {
559 : const MSVehicle* nv = neighLead.first;
560 6070328 : if (MSLCHelper::divergentRoute(myVehicle, *nv)) {
561 : //std::cout << SIMTIME << " ego=" << myVehicle.getID() << " ignoresDivergentBlockingLeader=" << nv->getID() << "\n";
562 : return plannedSpeed;
563 : }
564 : #ifdef DEBUG_INFORM
565 : if (gDebugFlag2) std::cout << " blocked by leader nv=" << nv->getID() << " nvSpeed=" << nv->getSpeed() << " needGap="
566 : << myVehicle.getCarFollowModel().getSecureGap(&myVehicle, nv, myVehicle.getSpeed(), nv->getSpeed(), nv->getCarFollowModel().getMaxDecel()) << "\n";
567 : #endif
568 : // decide whether we want to overtake the leader or follow it
569 6064682 : const double dv = plannedSpeed - nv->getSpeed();
570 6064682 : const double overtakeDist = (neighLead.second // drive to back of follower
571 6064682 : + nv->getVehicleType().getLengthWithGap() // drive to front of follower
572 6064682 : + myVehicle.getVehicleType().getLength() // ego back reaches follower front
573 18194046 : + nv->getCarFollowModel().getSecureGap( // save gap to follower
574 6064682 : nv, &myVehicle, nv->getSpeed(), myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel()));
575 :
576 6064682 : if ((dv < myOvertakeDeltaSpeedFactor * myVehicle.getLane()->getSpeedLimit() + NUMERICAL_EPS
577 : // overtaking on the right on an uncongested highway is forbidden (noOvertakeLCLeft)
578 2007837 : || (dir == LCA_MLEFT && !myVehicle.congested() && !myAllowOvertakingRight)
579 : // not enough space to overtake? (we will start to brake when approaching a dead end)
580 3987822 : || myLeftSpace - myLeadingBlockerLength - myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed()) < overtakeDist
581 : // not enough time to overtake?
582 1854215 : || dv * remainingSeconds < overtakeDist)
583 6497497 : && (!neighLead.first->isStopped() || (isOpposite() && neighLead.second >= 0))) {
584 : // cannot overtake
585 4482852 : msg(neighLead, -1, dir | LCA_AMBLOCKINGLEADER);
586 : // slow down smoothly to follow leader
587 4482852 : const double targetSpeed = getCarFollowModel().followSpeed(
588 4482852 : &myVehicle, myVehicle.getSpeed(), neighLead.second, nv->getSpeed(), nv->getCarFollowModel().getMaxDecel());
589 4482852 : if (targetSpeed < myVehicle.getSpeed()) {
590 : // slow down smoothly to follow leader
591 1091590 : const double decel = ACCEL2SPEED(MIN2(myVehicle.getCarFollowModel().getMaxDecel(),
592 : MAX2(MIN_FALLBEHIND, (myVehicle.getSpeed() - targetSpeed) / remainingSeconds)));
593 : //const double nextSpeed = MAX2(0., MIN2(plannedSpeed, myVehicle.getSpeed() - decel));
594 845487 : const double nextSpeed = MIN2(plannedSpeed, MAX2(0.0, myVehicle.getSpeed() - decel));
595 : #ifdef DEBUG_INFORM
596 : if (gDebugFlag2) {
597 : std::cout << SIMTIME
598 : << " cannot overtake leader nv=" << nv->getID()
599 : << " dv=" << dv
600 : << " remainingSeconds=" << remainingSeconds
601 : << " targetSpeed=" << targetSpeed
602 : << " nextSpeed=" << nextSpeed
603 : << "\n";
604 : }
605 : #endif
606 845487 : addLCSpeedAdvice(nextSpeed, dir);
607 845487 : return nextSpeed;
608 : } else {
609 : // leader is fast enough anyway
610 : #ifdef DEBUG_INFORM
611 : if (gDebugFlag2) {
612 : std::cout << SIMTIME
613 : << " cannot overtake fast leader nv=" << nv->getID()
614 : << " dv=" << dv
615 : << " remainingSeconds=" << remainingSeconds
616 : << " targetSpeed=" << targetSpeed
617 : << "\n";
618 : }
619 : #endif
620 3637365 : addLCSpeedAdvice(targetSpeed, dir);
621 3637365 : return plannedSpeed;
622 : }
623 : } else {
624 : #ifdef DEBUG_INFORM
625 : if (gDebugFlag2) {
626 : std::cout << SIMTIME
627 : << " wants to overtake leader nv=" << nv->getID()
628 : << " dv=" << dv
629 : << " remainingSeconds=" << remainingSeconds
630 : << " currentGap=" << neighLead.second
631 : << " secureGap=" << nv->getCarFollowModel().getSecureGap(nv, &myVehicle, nv->getSpeed(), myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel())
632 : << " overtakeDist=" << overtakeDist
633 : << " leftSpace=" << myLeftSpace
634 : << " blockerLength=" << myLeadingBlockerLength
635 : << "\n";
636 : }
637 : #endif
638 : // overtaking, leader should not accelerate
639 1581830 : msg(neighLead, nv->getSpeed(), dir | LCA_AMBLOCKINGLEADER);
640 1581830 : return -1;
641 : }
642 1789287 : } else if (neighLead.first != 0) { // (remainUnblocked)
643 : // we are not blocked now. make sure we stay far enough from the leader
644 : const MSVehicle* nv = neighLead.first;
645 : double dv, nextNVSpeed;
646 1789287 : if (MSGlobals::gSemiImplicitEulerUpdate) {
647 : // XXX: the decrement (HELP_OVERTAKE) should be scaled with timestep length, I think.
648 : // It seems to function as an estimate nv's speed in the next simstep!? (so HELP_OVERTAKE should be an acceleration value.)
649 1500180 : nextNVSpeed = nv->getSpeed() - HELP_OVERTAKE; // conservative
650 1500180 : dv = SPEED2DIST(myVehicle.getSpeed() - nextNVSpeed);
651 : } else {
652 : // Estimate neigh's speed after actionstep length
653 : // @note The possible breaking can be underestimated by the formula, so this is a potential
654 : // source of collisions if actionsteplength>simsteplength.
655 : const double nvMaxDecel = HELP_OVERTAKE;
656 289107 : nextNVSpeed = nv->getSpeed() - nvMaxDecel * myVehicle.getActionStepLengthSecs(); // conservative
657 : // Estimated gap reduction until next action step if own speed stays constant
658 289107 : dv = SPEED2DIST(myVehicle.getSpeed() - nextNVSpeed);
659 : }
660 1789287 : const double targetSpeed = getCarFollowModel().followSpeed(
661 1789287 : &myVehicle, myVehicle.getSpeed(), neighLead.second - dv, nextNVSpeed, nv->getCarFollowModel().getMaxDecel());
662 1789287 : addLCSpeedAdvice(targetSpeed, dir);
663 : #ifdef DEBUG_INFORM
664 : if (gDebugFlag2) {
665 : std::cout << " not blocked by leader nv=" << nv->getID()
666 : << " nvSpeed=" << nv->getSpeed()
667 : << " gap=" << neighLead.second
668 : << " nextGap=" << neighLead.second - dv
669 : << " needGap=" << myVehicle.getCarFollowModel().getSecureGap(&myVehicle, nv, myVehicle.getSpeed(), nv->getSpeed(), nv->getCarFollowModel().getMaxDecel())
670 : << " targetSpeed=" << targetSpeed
671 : << "\n";
672 : }
673 : #endif
674 : return MIN2(targetSpeed, plannedSpeed);
675 : } else {
676 : // not overtaking
677 : return plannedSpeed;
678 : }
679 : }
680 :
681 :
682 : void
683 6087598 : MSLCM_SL2015::informFollower(int blocked,
684 : int dir,
685 : const CLeaderDist& neighFollow,
686 : double remainingSeconds,
687 : double plannedSpeed) {
688 :
689 6087598 : const MSVehicle* nv = neighFollow.first;
690 : // decide whether we will request help to cut in before the follower or allow to be overtaken
691 6087598 : if (nv != nullptr && MSLCHelper::unwillingToHelp(myVehicle, plannedSpeed, *nv)) {
692 : // @note: this check needs to come first because even if the follower is not blocking, getSpeedPreservingSecureGap may request a slow-down
693 : #ifdef DEBUG_INFORMER
694 : if (DEBUG_COND) {
695 : std::cout << "\n nv=" << nv->getID() << " not willing to help\n";
696 : }
697 : #endif
698 : return;
699 : }
700 :
701 5889718 : if ((blocked & LCA_BLOCKED_BY_FOLLOWER) != 0 && neighFollow.first != 0) {
702 4264195 : if (MSLCHelper::divergentRoute(myVehicle, *nv)) {
703 : //std::cout << SIMTIME << " ego=" << myVehicle.getID() << " ignoresDivergentBlockingFollower=" << nv->getID() << "\n";
704 : return;
705 : }
706 : #ifdef DEBUG_INFORM
707 : if (gDebugFlag2) std::cout << " blocked by follower nv=" << nv->getID() << " nvSpeed=" << nv->getSpeed() << " needGap="
708 : << nv->getCarFollowModel().getSecureGap(nv, &myVehicle, nv->getSpeed(), myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel()) << "\n";
709 : #endif
710 :
711 : // are we fast enough to cut in without any help?
712 4264038 : if (plannedSpeed - nv->getSpeed() >= HELP_OVERTAKE) {
713 267952 : const double neededGap = nv->getCarFollowModel().getSecureGap(nv, &myVehicle, nv->getSpeed(), plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel());
714 267952 : if ((neededGap - neighFollow.second) / remainingSeconds < (plannedSpeed - nv->getSpeed())) {
715 : #ifdef DEBUG_INFORM
716 : if (gDebugFlag2) {
717 : std::cout << " wants to cut in before nv=" << nv->getID() << " without any help neededGap=" << neededGap << "\n";
718 : }
719 : #endif
720 : // follower might even accelerate but not to much
721 246285 : msg(neighFollow, plannedSpeed - HELP_OVERTAKE, dir | LCA_AMBLOCKINGFOLLOWER);
722 246285 : return;
723 : }
724 : }
725 :
726 : // PARAMETERS
727 : // assume other vehicle will assume the equivalent of 1 second of
728 : // maximum deceleration to help us (will probably be spread over
729 : // multiple seconds)
730 : // -----------
731 : const double helpDecel = nv->getCarFollowModel().getMaxDecel() * HELP_DECEL_FACTOR ;
732 :
733 : // change in the gap between ego and blocker over 1 second (not STEP!)
734 4017753 : const double neighNewSpeed = MAX2(0., nv->getSpeed() - ACCEL2SPEED(helpDecel));
735 4017753 : const double neighNewSpeed1s = MAX2(0., nv->getSpeed() - helpDecel);
736 4017753 : const double dv = plannedSpeed - neighNewSpeed1s;
737 : // new gap between follower and self in case the follower does brake for 1s
738 4017753 : const double decelGap = neighFollow.second + dv;
739 4017753 : const double secureGap = nv->getCarFollowModel().getSecureGap(nv, &myVehicle, neighNewSpeed1s, plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel());
740 : #ifdef DEBUG_INFORM
741 : if (gDebugFlag2) {
742 : std::cout << SIMTIME
743 : << " egoV=" << myVehicle.getSpeed()
744 : << " egoNV=" << plannedSpeed
745 : << " nvNewSpeed=" << neighNewSpeed
746 : << " nvNewSpeed1s=" << neighNewSpeed1s
747 : << " deltaGap=" << dv
748 : << " decelGap=" << decelGap
749 : << " secGap=" << secureGap
750 : << "\n";
751 : }
752 : #endif
753 4017753 : if (decelGap > 0 && decelGap >= secureGap) {
754 : // if the blocking neighbor brakes it could actually help
755 : // how hard does it actually need to be?
756 : // to be safe in the next step the following equation has to hold:
757 : // vsafe <= followSpeed(gap=currentGap - SPEED2DIST(vsafe), ...)
758 : // we compute an upper bound on vsafe by doing the computation twice
759 999980 : const double vsafe1 = MAX2(neighNewSpeed, nv->getCarFollowModel().followSpeed(
760 999980 : nv, nv->getSpeed(), neighFollow.second + SPEED2DIST(plannedSpeed), plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel()));
761 999980 : const double vsafe = MAX2(neighNewSpeed, nv->getCarFollowModel().followSpeed(
762 999980 : nv, nv->getSpeed(), neighFollow.second + SPEED2DIST(plannedSpeed - vsafe1), plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel()));
763 : // the following assertion cannot be guaranteed because the CFModel handles small gaps differently, see MSCFModel::maximumSafeStopSpeed
764 : // assert(vsafe <= vsafe1);
765 999980 : msg(neighFollow, vsafe, dir | LCA_AMBLOCKINGFOLLOWER);
766 : #ifdef DEBUG_INFORM
767 : if (gDebugFlag2) {
768 : std::cout << " wants to cut in before nv=" << nv->getID()
769 : << " vsafe1=" << vsafe1
770 : << " vsafe=" << vsafe
771 : << " newSecGap=" << nv->getCarFollowModel().getSecureGap(nv, &myVehicle, vsafe, plannedSpeed, myVehicle.getCarFollowModel().getMaxDecel())
772 : << "\n";
773 : }
774 : #endif
775 4017753 : } else if (dv > 0 && dv * remainingSeconds > (secureGap - decelGap + POSITION_EPS)) {
776 : // decelerating once is sufficient to open up a large enough gap in time
777 179349 : msg(neighFollow, neighNewSpeed, dir | LCA_AMBLOCKINGFOLLOWER);
778 : #ifdef DEBUG_INFORM
779 : if (gDebugFlag2) {
780 : std::cout << " wants to cut in before nv=" << nv->getID() << " (eventually)\n";
781 : }
782 : #endif
783 2838424 : } else if (dir == LCA_MRIGHT && !myAllowOvertakingRight && !nv->congested()) {
784 : const double vhelp = MAX2(neighNewSpeed, HELP_OVERTAKE);
785 1656 : msg(neighFollow, vhelp, dir | LCA_AMBLOCKINGFOLLOWER);
786 : #ifdef DEBUG_INFORM
787 : if (gDebugFlag2) {
788 : std::cout << " wants to cut in before nv=" << nv->getID() << " (nv cannot overtake right)\n";
789 : }
790 : #endif
791 : } else {
792 2836768 : double vhelp = MAX2(nv->getSpeed(), myVehicle.getSpeed() + HELP_OVERTAKE);
793 2836768 : if (nv->getSpeed() > myVehicle.getSpeed() &&
794 432002 : ((dir == LCA_MRIGHT && myVehicle.getWaitingSeconds() > LCA_RIGHT_IMPATIENCE)
795 468734 : || (dir == LCA_MLEFT && plannedSpeed > CUT_IN_LEFT_SPEED_THRESHOLD) // VARIANT_22 (slowDownLeft)
796 : // XXX this is a hack to determine whether the vehicles is on an on-ramp. This information should be retrieved from the network itself
797 462819 : || (dir == LCA_MLEFT && myLeftSpace > MAX_ONRAMP_LENGTH)
798 : )) {
799 : // let the follower slow down to increase the likelyhood that later vehicles will be slow enough to help
800 : // follower should still be fast enough to open a gap
801 440338 : vhelp = MAX2(neighNewSpeed, myVehicle.getSpeed() + HELP_OVERTAKE);
802 : #ifdef DEBUG_INFORM
803 : if (gDebugFlag2) {
804 : std::cout << " wants right follower to slow down a bit\n";
805 : }
806 : #endif
807 440338 : if ((nv->getSpeed() - myVehicle.getSpeed()) / helpDecel < remainingSeconds) {
808 : #ifdef DEBUG_INFORM
809 : if (gDebugFlag2) {
810 : std::cout << " wants to cut in before right follower nv=" << nv->getID() << " (eventually)\n";
811 : }
812 : #endif
813 424979 : msg(neighFollow, vhelp, dir | LCA_AMBLOCKINGFOLLOWER);
814 424979 : return;
815 : }
816 : }
817 2411789 : msg(neighFollow, vhelp, dir | LCA_AMBLOCKINGFOLLOWER);
818 : // this follower is supposed to overtake us. slow down smoothly to allow this
819 2411789 : const double overtakeDist = (neighFollow.second // follower reaches ego back
820 2411789 : + myVehicle.getVehicleType().getLengthWithGap() // follower reaches ego front
821 2411789 : + nv->getVehicleType().getLength() // follower back at ego front
822 2411789 : + myVehicle.getCarFollowModel().getSecureGap( // follower has safe dist to ego
823 2411789 : &myVehicle, nv, plannedSpeed, vhelp, nv->getCarFollowModel().getMaxDecel()));
824 : // speed difference to create a sufficiently large gap
825 2411789 : const double needDV = overtakeDist / remainingSeconds;
826 : // make sure the deceleration is not to strong
827 2717190 : addLCSpeedAdvice(MAX2(vhelp - needDV, myVehicle.getSpeed() - ACCEL2SPEED(myVehicle.getCarFollowModel().getMaxDecel())), dir);
828 :
829 : #ifdef DEBUG_INFORM
830 : if (gDebugFlag2) {
831 : std::cout << SIMTIME
832 : << " veh=" << myVehicle.getID()
833 : << " wants to be overtaken by=" << nv->getID()
834 : << " overtakeDist=" << overtakeDist
835 : << " vneigh=" << nv->getSpeed()
836 : << " vhelp=" << vhelp
837 : << " needDV=" << needDV
838 : << " vsafe=" << myVehicle.getSpeed() + ACCEL2SPEED(myLCAccelerationAdvices.back().first)
839 : << "\n";
840 : }
841 : #endif
842 : }
843 1625523 : } else if (neighFollow.first != 0) {
844 1625523 : const double vsafe = MSLCHelper::getSpeedPreservingSecureGap(myVehicle, *neighFollow.first, neighFollow.second, plannedSpeed);
845 1625523 : msg(neighFollow, vsafe, dir | LCA_AMBLOCKINGFOLLOWER);
846 : #ifdef DEBUG_INFORM
847 : if (gDebugFlag2) {
848 : std::cout << " wants to cut in before non-blocking follower nv=" << neighFollow.first->getID() << "\n";
849 : }
850 : #endif
851 : }
852 : }
853 :
854 : double
855 2580392 : MSLCM_SL2015::informLeaders(int blocked, int dir,
856 : const std::vector<CLeaderDist>& blockers,
857 : double remainingSeconds) {
858 2580392 : double plannedSpeed = myVehicle.getSpeed();
859 2580392 : double space = myLeftSpace;
860 2580392 : if (myLeadingBlockerLength != 0) {
861 : // see patchSpeed @todo: refactor
862 380076 : space -= myLeadingBlockerLength - POSITION_EPS - myVehicle.getVehicleType().getMinGap();
863 380076 : if (space <= 0) {
864 : // ignore leading blocker
865 22319 : space = myLeftSpace;
866 : }
867 : }
868 2580392 : double safe = myVehicle.getCarFollowModel().stopSpeed(&myVehicle, myVehicle.getSpeed(), space);
869 : plannedSpeed = MIN2(plannedSpeed, safe);
870 :
871 10440007 : for (std::vector<CLeaderDist>::const_iterator it = blockers.begin(); it != blockers.end(); ++it) {
872 7859615 : plannedSpeed = MIN2(plannedSpeed, informLeader(blocked, dir, *it, remainingSeconds));
873 : }
874 2580392 : return plannedSpeed;
875 : }
876 :
877 :
878 : void
879 2182519 : MSLCM_SL2015::informFollowers(int blocked, int dir,
880 : const std::vector<CLeaderDist>& blockers,
881 : double remainingSeconds,
882 : double plannedSpeed) {
883 : // #3727
884 8270117 : for (std::vector<CLeaderDist>::const_iterator it = blockers.begin(); it != blockers.end(); ++it) {
885 6087598 : informFollower(blocked, dir, *it, remainingSeconds, plannedSpeed);
886 : }
887 2182519 : }
888 :
889 :
890 : void
891 83560801 : MSLCM_SL2015::prepareStep() {
892 83560801 : MSAbstractLaneChangeModel::prepareStep();
893 : // keep information about strategic change direction
894 83560801 : myOwnState = (myOwnState & (LCA_STRATEGIC | LCA_COOPERATIVE)) ? (myOwnState & LCA_WANTS_LANECHANGE) : 0;
895 : #ifdef DEBUG_INFORM
896 : if (debugVehicle()) {
897 : std::cout << SIMTIME
898 : << " veh=" << myVehicle.getID()
899 : << " prepareStep"
900 : << " myCanChangeFully=" << myCanChangeFully
901 : << "\n";
902 : }
903 : #endif
904 83560801 : myLeadingBlockerLength = 0;
905 83560801 : myLeftSpace = 0;
906 : myLCAccelerationAdvices.clear();
907 83560801 : myDontBrake = false;
908 : myCFRelated.clear();
909 83560801 : myCFRelatedReady = false;
910 83560801 : const double halfWidth = getWidth() * 0.5;
911 : // only permit changing within lane bounds but open up the range depending on the checked duration in _wantsChangeSublane()
912 83560801 : mySafeLatDistRight = myVehicle.getLane()->getWidth() * 0.5 + myVehicle.getLateralPositionOnLane() - halfWidth;
913 83560801 : mySafeLatDistLeft = myVehicle.getLane()->getWidth() * 0.5 - myVehicle.getLateralPositionOnLane() - halfWidth;
914 83560801 : if (isOpposite()) {
915 : std::swap(mySafeLatDistLeft, mySafeLatDistRight);
916 : }
917 : // truncate to work around numerical instability between different builds
918 83560801 : mySpeedGainProbabilityRight = ceil(mySpeedGainProbabilityRight * 100000.0) * 0.00001;
919 83560801 : mySpeedGainProbabilityLeft = ceil(mySpeedGainProbabilityLeft * 100000.0) * 0.00001;
920 83560801 : myKeepRightProbability = ceil(myKeepRightProbability * 100000.0) * 0.00001;
921 : // updated myExpectedSublaneSpeeds
922 : // XXX only do this when (sub)lane changing is possible
923 : std::vector<double> newExpectedSpeeds;
924 : #ifdef DEBUG_INFORM
925 : if (DEBUG_COND) {
926 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " myExpectedSublaneSpeeds=" << toString(myExpectedSublaneSpeeds) << "\n";
927 : }
928 : #endif
929 167121602 : if (myExpectedSublaneSpeeds.size() != myVehicle.getLane()->getEdge().getSubLaneSides().size()) {
930 : // initialize
931 1179596 : const MSEdge* currEdge = &myVehicle.getLane()->getEdge();
932 : const std::vector<MSLane*>& lanes = currEdge->getLanes();
933 3216197 : for (std::vector<MSLane*>::const_iterator it_lane = lanes.begin(); it_lane != lanes.end(); ++it_lane) {
934 2036601 : const int subLanes = MAX2(1, int(ceil((*it_lane)->getWidth() / MSGlobals::gLateralResolution)));
935 10832423 : for (int i = 0; i < subLanes; ++i) {
936 8795822 : newExpectedSpeeds.push_back((*it_lane)->getVehicleMaxSpeed(&myVehicle));
937 : }
938 : }
939 1179596 : if (currEdge->canChangeToOpposite()) {
940 11155 : MSLane* opposite = lanes.back()->getOpposite();
941 11155 : const int subLanes = MAX2(1, int(ceil(opposite->getWidth() / MSGlobals::gLateralResolution)));
942 61297 : for (int i = 0; i < subLanes; ++i) {
943 50142 : newExpectedSpeeds.push_back(lanes.back()->getVehicleMaxSpeed(&myVehicle));
944 : }
945 : }
946 1179596 : if (myExpectedSublaneSpeeds.size() > 0) {
947 : // copy old values
948 : assert(myLastEdge != 0);
949 658696 : if (myLastEdge->getSubLaneSides().size() == myExpectedSublaneSpeeds.size()) {
950 658696 : const int subLaneShift = computeSublaneShift(myLastEdge, currEdge);
951 658696 : if (subLaneShift < std::numeric_limits<int>::max()) {
952 2731725 : for (int i = 0; i < (int)myExpectedSublaneSpeeds.size(); ++i) {
953 2361914 : const int newI = i + subLaneShift;
954 2361914 : if (newI > 0 && newI < (int)newExpectedSpeeds.size()) {
955 1609882 : newExpectedSpeeds[newI] = myExpectedSublaneSpeeds[i];
956 : }
957 : }
958 : }
959 : }
960 : }
961 1179596 : myExpectedSublaneSpeeds = newExpectedSpeeds;
962 1179596 : myLastEdge = currEdge;
963 : }
964 : assert(myExpectedSublaneSpeeds.size() == myVehicle.getLane()->getEdge().getSubLaneSides().size());
965 83560801 : if (mySigma > 0) {
966 53576 : mySigmaState += getLateralDrift();
967 : }
968 83560801 : }
969 :
970 : double
971 53576 : MSLCM_SL2015::getLateralDrift() {
972 : //OUProcess::step(double state, double dt, double timeScale, double noiseIntensity)
973 53576 : const double deltaState = OUProcess::step(mySigmaState,
974 53576 : myVehicle.getActionStepLengthSecs(),
975 53576 : MAX2(NUMERICAL_EPS, (1 - mySigma) * 100), mySigma) - mySigmaState;
976 53576 : const double scaledDelta = deltaState * myVehicle.getSpeed() / myVehicle.getLane()->getSpeedLimit();
977 53576 : return scaledDelta;
978 : }
979 :
980 : double
981 96446116 : MSLCM_SL2015::getPosLat() {
982 96446116 : return myVehicle.getLateralPositionOnLane() + mySigmaState;
983 : }
984 :
985 : int
986 658696 : MSLCM_SL2015::computeSublaneShift(const MSEdge* prevEdge, const MSEdge* curEdge) {
987 : // find the first lane that targets the new edge
988 : int prevShift = 0;
989 1318467 : for (const MSLane* const lane : prevEdge->getLanes()) {
990 1915345 : for (const MSLink* const link : lane->getLinkCont()) {
991 1255574 : if (&link->getLane()->getEdge() == curEdge) {
992 : int curShift = 0;
993 : const MSLane* target = link->getLane();
994 : const std::vector<MSLane*>& lanes2 = curEdge->getLanes();
995 515937 : for (std::vector<MSLane*>::const_iterator it_lane2 = lanes2.begin(); it_lane2 != lanes2.end(); ++it_lane2) {
996 515937 : const MSLane* lane2 = *it_lane2;
997 515937 : if (lane2 == target) {
998 369811 : return prevShift + curShift;
999 : }
1000 146126 : MSLeaderInfo ahead(lane2->getWidth());
1001 146126 : curShift += ahead.numSublanes();
1002 146126 : }
1003 : assert(false);
1004 : }
1005 : }
1006 659771 : MSLeaderInfo ahead(lane->getWidth());
1007 659771 : prevShift -= ahead.numSublanes();
1008 659771 : }
1009 : return std::numeric_limits<int>::max();
1010 : }
1011 :
1012 :
1013 : void
1014 1242606 : MSLCM_SL2015::changed() {
1015 1242606 : if (!myCanChangeFully) {
1016 : // do not reset state yet so we can continue our maneuver but acknowledge
1017 : // a change to the right (movement should continue due to lane alignment desire)
1018 373608 : if (getManeuverDist() < 0) {
1019 166317 : myKeepRightProbability = 0;
1020 : }
1021 : #ifdef DEBUG_STATE
1022 : if (DEBUG_COND) {
1023 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " state not reset. maneuverDist=" << getManeuverDist() << "\n";
1024 : }
1025 : #endif
1026 373608 : return;
1027 : }
1028 868998 : myOwnState = 0;
1029 : // XX do not reset values for unfinished maneuvers
1030 868998 : mySpeedGainProbabilityRight = 0;
1031 868998 : mySpeedGainProbabilityLeft = 0;
1032 868998 : myKeepRightProbability = 0;
1033 :
1034 868998 : if (myVehicle.getBestLaneOffset() == 0) {
1035 : // if we are not yet on our best lane there might still be unseen blockers
1036 : // (during patchSpeed)
1037 848620 : myLeadingBlockerLength = 0;
1038 848620 : myLeftSpace = 0;
1039 : }
1040 868998 : myLookAheadSpeed = LOOK_AHEAD_MIN_SPEED;
1041 : myLCAccelerationAdvices.clear();
1042 868998 : myDontBrake = false;
1043 868998 : myLeadingBlockerLength = 0;
1044 : #if defined(DEBUG_MANEUVER) || defined(DEBUG_STATE)
1045 : if (DEBUG_COND) {
1046 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " changed()\n";
1047 : }
1048 : #endif
1049 : }
1050 :
1051 :
1052 : void
1053 3254 : MSLCM_SL2015::resetState() {
1054 3254 : myOwnState = 0;
1055 3254 : mySpeedGainProbabilityRight = 0;
1056 3254 : mySpeedGainProbabilityLeft = 0;
1057 3254 : myKeepRightProbability = 0;
1058 3254 : myLeadingBlockerLength = 0;
1059 3254 : myLeftSpace = 0;
1060 3254 : myLookAheadSpeed = LOOK_AHEAD_MIN_SPEED;
1061 : myLCAccelerationAdvices.clear();
1062 3254 : myDontBrake = false;
1063 3254 : }
1064 :
1065 :
1066 : int
1067 118384009 : MSLCM_SL2015::_wantsChangeSublane(
1068 : int laneOffset,
1069 : LaneChangeAction alternatives,
1070 : const MSLeaderDistanceInfo& leaders,
1071 : const MSLeaderDistanceInfo& followers,
1072 : const MSLeaderDistanceInfo& blockers,
1073 : const MSLeaderDistanceInfo& neighLeaders,
1074 : const MSLeaderDistanceInfo& neighFollowers,
1075 : const MSLeaderDistanceInfo& neighBlockers,
1076 : const MSLane& neighLane,
1077 : const std::vector<MSVehicle::LaneQ>& preb,
1078 : MSVehicle** lastBlocked,
1079 : MSVehicle** firstBlocked,
1080 : double& latDist, double& maneuverDist, int& blocked) {
1081 :
1082 118384009 : if (laneOffset != 0) {
1083 : // update mySafeLatDist w.r.t. the direction being checkd
1084 35845944 : const double halfWidth = getWidth() * 0.5;
1085 35845944 : double center = getVehicleCenter();
1086 35845944 : if (laneOffset < 0) {
1087 16193063 : mySafeLatDistRight = center - halfWidth;
1088 : } else {
1089 19652881 : mySafeLatDistLeft = getLeftBorder() - center - halfWidth;
1090 : }
1091 : }
1092 :
1093 118384009 : const SUMOTime currentTime = MSNet::getInstance()->getCurrentTimeStep();
1094 : // compute bestLaneOffset
1095 : MSVehicle::LaneQ curr, neigh, best;
1096 : int bestLaneOffset = 0;
1097 118384009 : double currentDist = 0;
1098 : double neighDist = 0;
1099 118384009 : const MSLane* prebLane = myVehicle.getLane();
1100 118384009 : if (prebLane->getEdge().isInternal()) {
1101 : // internal edges are not kept inside the bestLanes structure
1102 2408129 : if (isOpposite()) {
1103 585 : prebLane = prebLane->getNormalPredecessorLane();
1104 : } else {
1105 2407544 : prebLane = prebLane->getLinkCont()[0]->getLane();
1106 : }
1107 : }
1108 : // special case: vehicle considers changing to the opposite direction edge
1109 118384009 : const bool checkOpposite = &neighLane.getEdge() != &myVehicle.getLane()->getEdge();
1110 118384009 : const int prebOffset = (checkOpposite ? 0 : laneOffset);
1111 161357820 : for (int p = 0; p < (int) preb.size(); ++p) {
1112 161357820 : if (preb[p].lane == prebLane && p + laneOffset >= 0) {
1113 : assert(p + prebOffset < (int)preb.size());
1114 : curr = preb[p];
1115 118384009 : neigh = preb[p + prebOffset];
1116 118384009 : currentDist = curr.length;
1117 118384009 : neighDist = neigh.length;
1118 118384009 : bestLaneOffset = curr.bestLaneOffset;
1119 : // VARIANT_13 (equalBest)
1120 118384009 : if (bestLaneOffset == 0 && preb[p + prebOffset].bestLaneOffset == 0 && !checkOpposite) {
1121 : #ifdef DEBUG_WANTSCHANGE
1122 : if (gDebugFlag2) {
1123 : std::cout << STEPS2TIME(currentTime)
1124 : << " veh=" << myVehicle.getID()
1125 : << " bestLaneOffsetOld=" << bestLaneOffset
1126 : << " bestLaneOffsetNew=" << laneOffset
1127 : << "\n";
1128 : }
1129 : #endif
1130 : bestLaneOffset = prebOffset;
1131 : }
1132 118384009 : best = preb[p + bestLaneOffset];
1133 : break;
1134 : }
1135 : }
1136 : assert(curr.lane != nullptr);
1137 : assert(neigh.lane != nullptr);
1138 : assert(best.lane != nullptr);
1139 : double driveToNextStop = -std::numeric_limits<double>::max();
1140 : UNUSED_PARAMETER(driveToNextStop); // XXX use when computing usableDist
1141 118384009 : if (myVehicle.nextStopDist() < std::numeric_limits<double>::max()
1142 118384009 : && &myVehicle.getNextStop().lane->getEdge() == &myVehicle.getLane()->getEdge()) {
1143 : // vehicle can always drive up to stop distance
1144 : // @note this information is dynamic and thus not available in updateBestLanes()
1145 : // @note: nextStopDist was compute before the vehicle moved
1146 : driveToNextStop = myVehicle.nextStopDist();
1147 1981688 : const double stopPos = getForwardPos() + myVehicle.nextStopDist() - myVehicle.getLastStepDist();
1148 : #ifdef DEBUG_WANTS_CHANGE
1149 : if (DEBUG_COND) {
1150 : std::cout << SIMTIME << std::setprecision(gPrecision) << " veh=" << myVehicle.getID()
1151 : << " stopDist=" << myVehicle.nextStopDist()
1152 : << " lastDist=" << myVehicle.getLastStepDist()
1153 : << " stopPos=" << stopPos
1154 : << " currentDist=" << currentDist
1155 : << " neighDist=" << neighDist
1156 : << "\n";
1157 : }
1158 : #endif
1159 2594516 : currentDist = MAX2(currentDist, stopPos);
1160 : neighDist = MAX2(neighDist, stopPos);
1161 : }
1162 : // direction specific constants
1163 118384009 : const bool right = (laneOffset == -1);
1164 118384009 : const bool left = (laneOffset == 1);
1165 118384009 : const int myLca = (right ? LCA_MRIGHT : (left ? LCA_MLEFT : 0));
1166 102190946 : const int lcaCounter = (right ? LCA_LEFT : (left ? LCA_RIGHT : LCA_NONE));
1167 118384009 : const bool changeToBest = (right && bestLaneOffset < 0) || (left && bestLaneOffset > 0) || (laneOffset == 0 && bestLaneOffset == 0);
1168 : // keep information about being a leader/follower but remove information
1169 : // about previous lane change request or urgency
1170 118384009 : int ret = (myOwnState & 0xffff0000);
1171 :
1172 : // compute the distance when changing to the neighboring lane
1173 : // (ensure we do not lap into the line behind neighLane since there might be unseen blockers)
1174 : // minimum distance to move the vehicle fully onto the new lane
1175 118384009 : double latLaneDist = laneOffset == 0 ? 0. : myVehicle.lateralDistanceToLane(laneOffset);
1176 :
1177 : // VARIANT_5 (disableAMBACKBLOCKER1)
1178 : /*
1179 : if (leader.first != 0
1180 : && (myOwnState & LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0
1181 : && (leader.first->getLaneChangeModel().getOwnState() & LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0) {
1182 :
1183 : myOwnState &= (0xffffffff - LCA_AMBLOCKINGFOLLOWER_DONTBRAKE);
1184 : if (myVehicle.getSpeed() > SUMO_const_haltingSpeed) {
1185 : myOwnState |= LCA_AMBACKBLOCKER;
1186 : } else {
1187 : ret |= LCA_AMBACKBLOCKER;
1188 : myDontBrake = true;
1189 : }
1190 : }
1191 : */
1192 :
1193 : #ifdef DEBUG_WANTSCHANGE
1194 : if (gDebugFlag2) {
1195 : std::cout << STEPS2TIME(currentTime)
1196 : << " veh=" << myVehicle.getID()
1197 : << " myState=" << toString((LaneChangeAction)myOwnState)
1198 : << " firstBlocked=" << Named::getIDSecure(*firstBlocked)
1199 : << " lastBlocked=" << Named::getIDSecure(*lastBlocked)
1200 : << "\n leaders=" << leaders.toString()
1201 : << "\n followers=" << followers.toString()
1202 : << "\n blockers=" << blockers.toString()
1203 : << "\n neighLeaders=" << neighLeaders.toString()
1204 : << "\n neighFollowers=" << neighFollowers.toString()
1205 : << "\n neighBlockers=" << neighBlockers.toString()
1206 : << "\n changeToBest=" << changeToBest
1207 : << " latLaneDist=" << latLaneDist
1208 : << " alts=" << toString((LaneChangeAction)alternatives)
1209 : << "\n expectedSpeeds=" << toString(myExpectedSublaneSpeeds)
1210 : << std::endl;
1211 : }
1212 : #endif
1213 :
1214 118384009 : ret = slowDownForBlocked(lastBlocked, ret);
1215 : // VARIANT_14 (furtherBlock)
1216 118384009 : if (lastBlocked != firstBlocked) {
1217 118384009 : ret = slowDownForBlocked(firstBlocked, ret);
1218 : }
1219 :
1220 :
1221 : // we try to estimate the distance which is necessary to get on a lane
1222 : // we have to get on in order to keep our route
1223 : // we assume we need something that depends on our velocity
1224 : // and compare this with the free space on our wished lane
1225 : //
1226 : // if the free space is somehow less than the space we need, we should
1227 : // definitely try to get to the desired lane
1228 : //
1229 : // this rule forces our vehicle to change the lane if a lane changing is necessary soon
1230 : // lookAheadDistance:
1231 : // we do not want the lookahead distance to change all the time so we discrectize the speed a bit
1232 :
1233 : // VARIANT_18 (laHyst)
1234 118384009 : if (myVehicle.getSpeed() > myLookAheadSpeed) {
1235 20667733 : myLookAheadSpeed = myVehicle.getSpeed();
1236 : } else {
1237 : // FIXME: This strongly dependent on the value of TS, see LC2013 for the fix (l.1153, currently)
1238 97716276 : myLookAheadSpeed = MAX2(LOOK_AHEAD_MIN_SPEED,
1239 97716276 : (LOOK_AHEAD_SPEED_MEMORY * myLookAheadSpeed + (1 - LOOK_AHEAD_SPEED_MEMORY) * myVehicle.getSpeed()));
1240 : }
1241 : //myLookAheadSpeed = myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle);
1242 :
1243 : //double laDist = laSpeed > LOOK_FORWARD_SPEED_DIVIDER
1244 : // ? laSpeed * LOOK_FORWARD_FAR
1245 : // : laSpeed * LOOK_FORWARD_NEAR;
1246 118384009 : double laDist = myLookAheadSpeed * LOOK_FORWARD * myStrategicParam * (right ? 1 : myLookaheadLeft);
1247 118384009 : laDist += myVehicle.getVehicleType().getLengthWithGap() * 2.;
1248 : // aggressive drivers may elect to use reduced strategic lookahead to optimize speed
1249 : /*
1250 : if (mySpeedGainProbabilityRight > myChangeProbThresholdRight
1251 : || mySpeedGainProbabilityLeft > myChangeProbThresholdLeft) {
1252 : laDist *= MAX2(0.0, (1 - myPushy));
1253 : laDist *= MAX2(0,0, (1 - myAssertive));
1254 : laDist *= MAX2(0,0, (2 - mySpeedGainParam));
1255 : }
1256 : */
1257 :
1258 : // react to a stopped leader on the current lane
1259 118384009 : if (bestLaneOffset == 0 && leaders.hasStoppedVehicle()) {
1260 : // value is doubled for the check since we change back and forth
1261 : // laDist = 0.5 * (myVehicle.getVehicleType().getLengthWithGap() + leader.first->getVehicleType().getLengthWithGap());
1262 : // XXX determine length of longest stopped vehicle
1263 238436 : laDist = myVehicle.getVehicleType().getLengthWithGap();
1264 118145573 : } else if (checkOpposite && isOpposite() && neighLeaders.hasStoppedVehicle()) {
1265 : // compute exact distance to overtake stopped vehicle
1266 : laDist = 0;
1267 41725 : for (int i = 0; i < neighLeaders.numSublanes(); ++i) {
1268 33380 : CLeaderDist vehDist = neighLeaders[i];
1269 33380 : if (vehDist.first != nullptr && vehDist.first->isStopped()) {
1270 28963 : laDist = MAX2(laDist, myVehicle.getVehicleType().getMinGap() + vehDist.second + vehDist.first->getVehicleType().getLengthWithGap());
1271 : }
1272 : }
1273 8345 : laDist += myVehicle.getVehicleType().getLength();
1274 : }
1275 118384009 : if (myStrategicParam < 0) {
1276 : laDist = -1e3; // never perform strategic change
1277 : }
1278 :
1279 : // free space that is available for changing
1280 : //const double neighSpeed = (neighLead.first != 0 ? neighLead.first->getSpeed() :
1281 : // neighFollow.first != 0 ? neighFollow.first->getSpeed() :
1282 : // best.lane->getSpeedLimit());
1283 : // @note: while this lets vehicles change earlier into the correct direction
1284 : // it also makes the vehicles more "selfish" and prevents changes which are necessary to help others
1285 :
1286 118384009 : const double roundaboutBonus = MSLCHelper::getRoundaboutDistBonus(myVehicle, myRoundaboutBonus, curr, neigh, best);
1287 118384009 : currentDist += roundaboutBonus;
1288 118384009 : neighDist += roundaboutBonus;
1289 :
1290 118384009 : ret = checkStrategicChange(ret,
1291 : neighLane,
1292 : laneOffset,
1293 : leaders,
1294 : neighLeaders,
1295 : curr, neigh, best,
1296 : bestLaneOffset,
1297 : changeToBest,
1298 : currentDist,
1299 : neighDist,
1300 : laDist,
1301 : roundaboutBonus,
1302 : latLaneDist,
1303 : checkOpposite,
1304 : latDist);
1305 :
1306 :
1307 118384009 : if ((ret & LCA_STAY) != 0 && latDist == 0) {
1308 : // ensure that mySafeLatDistLeft / mySafeLatDistRight are up to date for the
1309 : // subsquent check with laneOffset = 0
1310 11345627 : const double center = myVehicle.getCenterOnEdge();
1311 11345627 : const double neighRight = getNeighRight(neighLane);
1312 11345627 : updateGaps(neighLeaders, neighRight, center, 1.0, mySafeLatDistRight, mySafeLatDistLeft);
1313 11345627 : updateGaps(neighFollowers, neighRight, center, 1.0, mySafeLatDistRight, mySafeLatDistLeft);
1314 : // remove TraCI flags because it should not be included in "state-without-traci"
1315 11345627 : ret = getCanceledState(laneOffset);
1316 11345627 : return ret;
1317 : }
1318 107038382 : if ((ret & LCA_URGENT) != 0) {
1319 : // prepare urgent lane change maneuver
1320 2392628 : if (changeToBest && abs(bestLaneOffset) > 1
1321 2650332 : && curr.bestContinuations.back()->getLinkCont().size() != 0
1322 : ) {
1323 : // there might be a vehicle which needs to counter-lane-change one lane further and we cannot see it yet
1324 50572 : const double reserve = MIN2(myLeftSpace - POSITION_EPS, getExtraReservation(bestLaneOffset, neighDist - currentDist));
1325 52021 : myLeadingBlockerLength = MAX2(reserve, myLeadingBlockerLength);
1326 : #ifdef DEBUG_WANTSCHANGE
1327 : if (gDebugFlag2) {
1328 : std::cout << " reserving space for unseen blockers myLeadingBlockerLength=" << myLeadingBlockerLength << "\n";
1329 : }
1330 : #endif
1331 : }
1332 :
1333 : // letting vehicles merge in at the end of the lane in case of counter-lane change, step#1
1334 : // if there is a leader and he wants to change to the opposite direction
1335 2580392 : MSVehicle* neighLeadLongest = const_cast<MSVehicle*>(getLongest(neighLeaders).first);
1336 2580392 : const bool canContinue = curr.bestContinuations.size() > 1;
1337 : #ifdef DEBUG_WANTSCHANGE
1338 : if (DEBUG_COND) {
1339 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " neighLeaders=" << neighLeaders.toString() << " longest=" << Named::getIDSecure(neighLeadLongest) << " firstBlocked=" << Named::getIDSecure(*firstBlocked) << "\n";
1340 : }
1341 : #endif
1342 2580392 : bool canReserve = MSLCHelper::updateBlockerLength(myVehicle, neighLeadLongest, lcaCounter, myLeftSpace - POSITION_EPS, canContinue, myLeadingBlockerLength);
1343 2580392 : if (*firstBlocked != neighLeadLongest && tieBrakeLeader(*firstBlocked)) {
1344 79401 : canReserve &= MSLCHelper::updateBlockerLength(myVehicle, *firstBlocked, lcaCounter, myLeftSpace - POSITION_EPS, canContinue, myLeadingBlockerLength);
1345 : }
1346 2580392 : if (!canReserve && !isOpposite()) {
1347 : // we have a low-priority relief connection
1348 : // std::cout << SIMTIME << " veh=" << myVehicle.getID() << " cannotReserve for blockers\n";
1349 30854 : myDontBrake = canContinue;
1350 : }
1351 :
1352 : std::vector<CLeaderDist> collectLeadBlockers;
1353 : std::vector<CLeaderDist> collectFollowBlockers;
1354 2580392 : int blockedFully = 0; // wether execution of the full maneuver is blocked
1355 2580392 : maneuverDist = latDist;
1356 2580392 : const double gapFactor = computeGapFactor(LCA_STRATEGIC);
1357 2580392 : blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1358 : leaders, followers, blockers,
1359 : neighLeaders, neighFollowers, neighBlockers, &collectLeadBlockers, &collectFollowBlockers,
1360 : false, gapFactor, &blockedFully);
1361 :
1362 2580392 : const double absLaneOffset = fabs(bestLaneOffset != 0 ? bestLaneOffset : latDist / SUMO_const_laneWidth);
1363 2580392 : const double remainingSeconds = ((ret & LCA_TRACI) == 0 ?
1364 3080915 : MAX2(STEPS2TIME(TS), myLeftSpace / MAX2(myLookAheadSpeed, NUMERICAL_EPS) / absLaneOffset / URGENCY) :
1365 184 : myVehicle.getInfluencer().changeRequestRemainingSeconds(currentTime));
1366 2580392 : const double plannedSpeed = informLeaders(blocked, myLca, collectLeadBlockers, remainingSeconds);
1367 : // coordinate with direct obstructions
1368 2580392 : if (plannedSpeed >= 0) {
1369 : // maybe we need to deal with a blocking follower
1370 2182519 : informFollowers(blocked, myLca, collectFollowBlockers, remainingSeconds, plannedSpeed);
1371 : }
1372 2182519 : if (plannedSpeed > 0) {
1373 1041576 : commitManoeuvre(blocked, blockedFully, leaders, neighLeaders, neighLane, maneuverDist);
1374 : }
1375 : #if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE)
1376 : if (gDebugFlag2) {
1377 : std::cout << STEPS2TIME(currentTime)
1378 : << " veh=" << myVehicle.getID()
1379 : << " myLeftSpace=" << myLeftSpace
1380 : << " changeFully=" << myCanChangeFully
1381 : << " blockedFully=" << toString((LaneChangeAction)blockedFully)
1382 : << " remainingSeconds=" << remainingSeconds
1383 : << " plannedSpeed=" << plannedSpeed
1384 : << " mySafeLatDistRight=" << mySafeLatDistRight
1385 : << " mySafeLatDistLeft=" << mySafeLatDistLeft
1386 : << "\n";
1387 : }
1388 : #endif
1389 : // remove TraCI flags because it should not be included in "state-without-traci"
1390 2580392 : ret = getCanceledState(laneOffset);
1391 : return ret;
1392 2580392 : }
1393 : // VARIANT_15
1394 104457990 : if (roundaboutBonus > 0) {
1395 :
1396 : #ifdef DEBUG_WANTS_CHANGE
1397 : if (DEBUG_COND) {
1398 : std::cout << STEPS2TIME(currentTime)
1399 : << " veh=" << myVehicle.getID()
1400 : << " roundaboutBonus=" << roundaboutBonus
1401 : << " myLeftSpace=" << myLeftSpace
1402 : << "\n";
1403 : }
1404 : #endif
1405 : // try to use the inner lanes of a roundabout to increase throughput
1406 : // unless we are approaching the exit
1407 5386727 : if (left) {
1408 538158 : ret |= LCA_COOPERATIVE;
1409 538158 : if (!cancelRequest(ret | LCA_LEFT, laneOffset)) {
1410 538158 : if ((ret & LCA_STAY) == 0) {
1411 538158 : latDist = latLaneDist;
1412 538158 : maneuverDist = latLaneDist;
1413 538158 : blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1414 : leaders, followers, blockers,
1415 : neighLeaders, neighFollowers, neighBlockers);
1416 : }
1417 538158 : return ret;
1418 : } else {
1419 : ret &= ~LCA_COOPERATIVE;
1420 : }
1421 : } else {
1422 4848569 : myKeepRightProbability = 0;
1423 : }
1424 : }
1425 :
1426 : // --------
1427 :
1428 : // -------- make place on current lane if blocking follower
1429 : //if (amBlockingFollowerPlusNB()) {
1430 : // std::cout << myVehicle.getID() << ", " << currentDistAllows(neighDist, bestLaneOffset, laDist)
1431 : // << " neighDist=" << neighDist
1432 : // << " currentDist=" << currentDist
1433 : // << "\n";
1434 : //}
1435 : const double inconvenience = (latLaneDist < 0
1436 103919832 : ? -mySpeedGainProbabilityRight / myChangeProbThresholdRight
1437 93329987 : : -mySpeedGainProbabilityLeft / myChangeProbThresholdLeft);
1438 : #ifdef DEBUG_COOPERATE
1439 : if (gDebugFlag2) {
1440 : std::cout << STEPS2TIME(currentTime)
1441 : << " veh=" << myVehicle.getID()
1442 : << " amBlocking=" << amBlockingFollowerPlusNB()
1443 : << " state=" << toString((LaneChangeAction)myOwnState)
1444 : << " myLca=" << toString((LaneChangeAction)myLca)
1445 : << " prevState=" << toString((LaneChangeAction)myPreviousState)
1446 : << " inconvenience=" << inconvenience
1447 : << " origLatDist=" << getManeuverDist()
1448 : << " wantsChangeToHelp=" << (right ? "right" : "left")
1449 : << " state=" << myOwnState
1450 : << "\n";
1451 : }
1452 : #endif
1453 :
1454 : if (laneOffset != 0
1455 21442662 : && ((amBlockingFollowerPlusNB()
1456 : // VARIANT_6 : counterNoHelp
1457 277953 : && ((myOwnState & myLca) != 0))
1458 21358176 : ||
1459 : // continue previous cooperative change
1460 21358176 : ((myPreviousState & LCA_COOPERATIVE) != 0
1461 89456 : && !myCanChangeFully
1462 : // change is in the right direction
1463 48645 : && (laneOffset * getManeuverDist() > 0)))
1464 86681 : && (inconvenience < myCooperativeParam)
1465 104004018 : && (changeToBest || currentDistAllows(neighDist, abs(bestLaneOffset) + 1, laDist))) {
1466 :
1467 : // VARIANT_2 (nbWhenChangingToHelp)
1468 : #ifdef DEBUG_COOPERATE
1469 : if (gDebugFlag2) {
1470 : std::cout << " wants cooperative change\n";
1471 : }
1472 : #endif
1473 :
1474 84186 : ret |= LCA_COOPERATIVE | LCA_URGENT ;//| LCA_CHANGE_TO_HELP;
1475 84186 : if (!cancelRequest(ret | getLCA(ret, latLaneDist), laneOffset)) {
1476 84186 : latDist = amBlockingFollowerPlusNB() ? latLaneDist : getManeuverDist();
1477 84186 : maneuverDist = latDist;
1478 84186 : blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1479 : leaders, followers, blockers,
1480 : neighLeaders, neighFollowers, neighBlockers);
1481 84186 : return ret;
1482 : } else {
1483 : ret &= ~(LCA_COOPERATIVE | LCA_URGENT);
1484 : }
1485 : }
1486 :
1487 : // --------
1488 :
1489 :
1490 : //// -------- security checks for krauss
1491 : //// (vsafe fails when gap<0)
1492 : //if ((blocked & LCA_BLOCKED) != 0) {
1493 : // return ret;
1494 : //}
1495 : //// --------
1496 :
1497 : // -------- higher speed
1498 : //if ((congested(neighLead.first) && neighLead.second < 20) || predInteraction(leader.first)) { //!!!
1499 : // return ret;
1500 : //}
1501 :
1502 : // iterate over all possible combinations of sublanes this vehicle might cover and check the potential speed
1503 103835646 : const MSEdge& edge = (isOpposite() ? myVehicle.getLane()->getParallelOpposite() : myVehicle.getLane())->getEdge();
1504 : const std::vector<double>& sublaneSides = edge.getSubLaneSides();
1505 : assert(sublaneSides.size() == myExpectedSublaneSpeeds.size());
1506 103835646 : const double vehWidth = getWidth();
1507 103835646 : const double rightVehSide = getVehicleCenter() - 0.5 * vehWidth;
1508 103835646 : const double leftVehSide = rightVehSide + vehWidth;
1509 : // figure out next speed when staying where we are
1510 : double defaultNextSpeed = std::numeric_limits<double>::max();
1511 : /// determine the leftmost and rightmost sublanes currently occupied
1512 103835646 : int leftmostOnEdge = (int)sublaneSides.size() - 1;
1513 299453946 : while (leftmostOnEdge > 0 && sublaneSides[leftmostOnEdge] > leftVehSide) {
1514 195618300 : leftmostOnEdge--;
1515 : }
1516 : int rightmostOnEdge = leftmostOnEdge;
1517 386437630 : while (rightmostOnEdge > 0 && sublaneSides[rightmostOnEdge] > rightVehSide + NUMERICAL_EPS) {
1518 282601984 : defaultNextSpeed = MIN2(defaultNextSpeed, myExpectedSublaneSpeeds[rightmostOnEdge]);
1519 : #ifdef DEBUG_WANTSCHANGE
1520 : if (gDebugFlag2) {
1521 : std::cout << " adapted to current sublane=" << rightmostOnEdge << " defaultNextSpeed=" << defaultNextSpeed << "\n";
1522 : std::cout << " sublaneSides[rightmostOnEdge]=" << sublaneSides[rightmostOnEdge] << " rightVehSide=" << rightVehSide << "\n";
1523 : }
1524 : #endif
1525 282601984 : rightmostOnEdge--;
1526 : }
1527 103835646 : defaultNextSpeed = MIN2(defaultNextSpeed, myExpectedSublaneSpeeds[rightmostOnEdge]);
1528 : #ifdef DEBUG_WANTSCHANGE
1529 : if (gDebugFlag2) {
1530 : std::cout << " adapted to current sublane=" << rightmostOnEdge << " defaultNextSpeed=" << defaultNextSpeed << "\n";
1531 : std::cout << " sublaneSides[rightmostOnEdge]=" << sublaneSides[rightmostOnEdge] << " rightVehSide=" << rightVehSide << "\n";
1532 : }
1533 : #endif
1534 : double maxGain = -std::numeric_limits<double>::max();
1535 : double maxGainRight = -std::numeric_limits<double>::max();
1536 : double maxGainLeft = -std::numeric_limits<double>::max();
1537 : double latDistNice = std::numeric_limits<double>::max();
1538 :
1539 103835646 : const int iMin = MIN2(myVehicle.getLane()->getRightmostSublane(), neighLane.getRightmostSublane());
1540 103835646 : double leftMax = MAX2(
1541 103835646 : myVehicle.getLane()->getRightSideOnEdge() + myVehicle.getLane()->getWidth(),
1542 : neighLane.getRightSideOnEdge() + neighLane.getWidth());
1543 103835646 : double rightMin = MIN2(myVehicle.getLane()->getRightSideOnEdge(), neighLane.getRightSideOnEdge());
1544 103835646 : if (checkOpposite || isOpposite()) {
1545 134510 : leftMax = getLeftBorder();
1546 : } else {
1547 : assert(leftMax <= edge.getWidth());
1548 : }
1549 103835646 : int sublaneCompact = MAX2(iMin, rightmostOnEdge - 1); // try to compactify to the right by default
1550 :
1551 103835646 : const double laneBoundary = laneOffset < 0 ? myVehicle.getLane()->getRightSideOnEdge() : neighLane.getRightSideOnEdge();
1552 : // if there is a neighboring lane we could change to, check sublanes on all lanes of the edge
1553 : // but restrict maneuver to the currently visible lanes (current, neigh) to ensure safety
1554 : // This way we can discover a fast lane beyond the immediate neighbor lane
1555 103835646 : const double maxLatDist = leftMax - leftVehSide;
1556 103835646 : const double minLatDist = rightMin - rightVehSide;
1557 103835646 : const int iStart = laneOffset == 0 ? iMin : 0;
1558 21358476 : const double rightEnd = laneOffset == 0 ? leftMax : (checkOpposite ? getLeftBorder() : edge.getWidth());
1559 : #ifdef DEBUG_WANTSCHANGE
1560 : if (gDebugFlag2) std::cout
1561 : << " checking sublanes rightmostOnEdge=" << rightmostOnEdge
1562 : << " rightEnd=" << rightEnd
1563 : << " leftmostOnEdge=" << leftmostOnEdge
1564 : << " iStart=" << iStart
1565 : << " iMin=" << iMin
1566 : << " sublaneSides=" << sublaneSides.size()
1567 : << " leftMax=" << leftMax
1568 : << " minLatDist=" << minLatDist
1569 : << " maxLatDist=" << maxLatDist
1570 : << " sublaneCompact=" << sublaneCompact
1571 : << "\n";
1572 : #endif
1573 764866368 : for (int i = iStart; i < (int)sublaneSides.size(); ++i) {
1574 661030722 : if (sublaneSides[i] + vehWidth < rightEnd) {
1575 : // i is the rightmost sublane and the left side of vehicles still fits on the edge,
1576 : // compute min speed of all sublanes covered by the vehicle in this case
1577 351052446 : double vMin = myExpectedSublaneSpeeds[i];
1578 : //std::cout << " i=" << i << "\n";
1579 : int j = i;
1580 1367785014 : while (vMin > 0 && j < (int)sublaneSides.size() && sublaneSides[j] < sublaneSides[i] + vehWidth) {
1581 1016732568 : vMin = MIN2(vMin, myExpectedSublaneSpeeds[j]);
1582 : #ifdef DEBUG_WANTSCHANGE
1583 : if (gDebugFlag2) {
1584 : //std::cout << " j=" << j << " vMin=" << vMin << " sublaneSides[j]=" << sublaneSides[j] << " leftVehSide=" << leftVehSide << " rightVehSide=" << rightVehSide << "\n";
1585 : }
1586 : #endif
1587 1016732568 : ++j;
1588 : }
1589 : // check whether the vehicle is between lanes
1590 351052446 : if (laneOffset != 0 && overlap(sublaneSides[i], sublaneSides[i] + vehWidth, laneBoundary, laneBoundary)) {
1591 40299650 : vMin *= (1 - myLaneDiscipline);
1592 : }
1593 498960021 : double relativeGain = (vMin - defaultNextSpeed) / MAX2(vMin, RELGAIN_NORMALIZATION_MIN_SPEED);
1594 351052446 : double currentLatDist = sublaneSides[i] - rightVehSide;
1595 351052446 : if ((laneOffset == 0 && (currentLatDist > maxLatDist || currentLatDist < minLatDist))
1596 351052446 : || (laneOffset < 0 && currentLatDist > maxLatDist)
1597 344048940 : || (laneOffset > 0 && currentLatDist < minLatDist)) {
1598 : #ifdef DEBUG_WANTSCHANGE
1599 : if (gDebugFlag2) {
1600 : std::cout << " i=" << i << " currentLatDist=" << currentLatDist << " outOfBounds\n";
1601 : }
1602 : #endif
1603 15484197 : continue;
1604 : }
1605 : currentLatDist = MIN2(MAX2(currentLatDist, minLatDist), maxLatDist);
1606 335568249 : if (currentLatDist > 0 && myVehicle.getLane()->getBidiLane() != nullptr) {
1607 : // penalize overtaking on the left if the lane is used in both
1608 : // directions
1609 540547 : relativeGain *= 0.5;
1610 : }
1611 : // @note only consider change if it is compatible with the current direction (same sign or laneOffset == 0)
1612 335568249 : if (relativeGain > maxGain && currentLatDist * laneOffset >= 0) {
1613 : maxGain = relativeGain;
1614 128461534 : if (maxGain > GAIN_PERCEPTION_THRESHOLD) {
1615 : sublaneCompact = i;
1616 7946319 : latDist = currentLatDist;
1617 : #ifdef DEBUG_WANTSCHANGE
1618 : if (gDebugFlag2) {
1619 : std::cout << " i=" << i << " vMin=" << vMin << " newLatDist=" << latDist << " relGain=" << relativeGain << "\n";
1620 : }
1621 : #endif
1622 : }
1623 : } else {
1624 : // if anticipated gains to the left are higher then to the right and current gains are equal, prefer left
1625 207106715 : if (currentLatDist > 0
1626 : //&& latDist < 0 // #7184 compensates for #7185
1627 139619661 : && mySpeedGainProbabilityLeft > mySpeedGainProbabilityRight
1628 36251224 : && relativeGain > GAIN_PERCEPTION_THRESHOLD
1629 3689866 : && maxGain - relativeGain < NUMERICAL_EPS) {
1630 2978703 : latDist = currentLatDist;
1631 : }
1632 : }
1633 : #ifdef DEBUG_WANTSCHANGE
1634 : if (gDebugFlag2) {
1635 : std::cout << " i=" << i << " rightmostOnEdge=" << rightmostOnEdge << " vMin=" << vMin << " relGain=" << relativeGain << " sublaneCompact=" << sublaneCompact << " curLatDist=" << currentLatDist << "\n";
1636 : }
1637 : #endif
1638 335568249 : if (currentLatDist < -NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) {
1639 : maxGainRight = MAX2(maxGainRight, relativeGain);
1640 167112676 : } else if (currentLatDist > NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) {
1641 : maxGainLeft = MAX2(maxGainLeft, relativeGain);
1642 : }
1643 335568249 : const double subAlignDist = sublaneSides[i] - rightVehSide;
1644 335568249 : if (fabs(subAlignDist) < fabs(latDistNice)) {
1645 : latDistNice = subAlignDist;
1646 : #ifdef DEBUG_WANTSCHANGE
1647 : if (gDebugFlag2) std::cout
1648 : << " nicest sublane=" << i
1649 : << " side=" << sublaneSides[i]
1650 : << " rightSide=" << rightVehSide
1651 : << " latDistNice=" << latDistNice
1652 : << " maxGainR=" << (maxGainRight == -std::numeric_limits<double>::max() ? "n/a" : toString(maxGainRight))
1653 : << " maxGainL=" << (maxGainLeft == -std::numeric_limits<double>::max() ? "n/a" : toString(maxGainLeft))
1654 : << "\n";
1655 : #endif
1656 : }
1657 : }
1658 : }
1659 : // updated change probabilities
1660 103835646 : if (maxGainRight != -std::numeric_limits<double>::max()) {
1661 : #ifdef DEBUG_WANTSCHANGE
1662 : if (gDebugFlag2) {
1663 : std::cout << " speedGainR_old=" << mySpeedGainProbabilityRight;
1664 : }
1665 : #endif
1666 99077351 : mySpeedGainProbabilityRight += myVehicle.getActionStepLengthSecs() * maxGainRight;
1667 : #ifdef DEBUG_WANTSCHANGE
1668 : if (gDebugFlag2) {
1669 : std::cout << " speedGainR_new=" << mySpeedGainProbabilityRight << "\n";
1670 : }
1671 : #endif
1672 : }
1673 103835646 : if (maxGainLeft != -std::numeric_limits<double>::max()) {
1674 : #ifdef DEBUG_WANTSCHANGE
1675 : if (gDebugFlag2) {
1676 : std::cout << " speedGainL_old=" << mySpeedGainProbabilityLeft;
1677 : }
1678 : #endif
1679 96555082 : mySpeedGainProbabilityLeft += myVehicle.getActionStepLengthSecs() * maxGainLeft;
1680 : #ifdef DEBUG_WANTSCHANGE
1681 : if (gDebugFlag2) {
1682 : std::cout << " speedGainL_new=" << mySpeedGainProbabilityLeft << "\n";
1683 : }
1684 : #endif
1685 : }
1686 : // decay if there is no reason for or against changing (only if we have enough information)
1687 103835646 : if ((fabs(maxGainRight) < NUMERICAL_EPS || maxGainRight == -std::numeric_limits<double>::max())
1688 98294565 : && (right || (alternatives & LCA_RIGHT) == 0)) {
1689 81456303 : mySpeedGainProbabilityRight *= pow(SPEEDGAIN_DECAY_FACTOR, myVehicle.getActionStepLengthSecs());
1690 : }
1691 103835646 : if ((fabs(maxGainLeft) < NUMERICAL_EPS || maxGainLeft == -std::numeric_limits<double>::max())
1692 94854103 : && (left || (alternatives & LCA_LEFT) == 0)) {
1693 74958712 : mySpeedGainProbabilityLeft *= pow(SPEEDGAIN_DECAY_FACTOR, myVehicle.getActionStepLengthSecs());
1694 : }
1695 :
1696 :
1697 : #ifdef DEBUG_WANTSCHANGE
1698 : if (gDebugFlag2) std::cout << SIMTIME
1699 : << " veh=" << myVehicle.getID()
1700 : << " defaultNextSpeed=" << defaultNextSpeed
1701 : << " maxGain=" << maxGain
1702 : << " maxGainRight=" << maxGainRight
1703 : << " maxGainLeft=" << maxGainLeft
1704 : << " probRight=" << mySpeedGainProbabilityRight
1705 : << " probLeft=" << mySpeedGainProbabilityLeft
1706 : << " latDist=" << latDist
1707 : << " latDistNice=" << latDistNice
1708 : << " sublaneCompact=" << sublaneCompact
1709 : << "\n";
1710 : #endif
1711 :
1712 103835646 : if (!left) {
1713 : // ONLY FOR CHANGING TO THE RIGHT
1714 : // start keepRight maneuver when no speed loss is expected and continue
1715 : // started maneuvers if the loss isn't too big
1716 92984839 : if (right && myVehicle.getSpeed() > 0 && (maxGainRight >= 0
1717 144072 : || ((myPreviousState & LCA_KEEPRIGHT) != 0 && maxGainRight >= -myKeepRightParam))) {
1718 : // honor the obligation to keep right (Rechtsfahrgebot)
1719 8526954 : const double vMax = myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle);
1720 8526954 : const double roadSpeedFactor = vMax / myVehicle.getLane()->getSpeedLimit(); // differse from speedFactor if vMax < speedLimit
1721 : double acceptanceTime;
1722 8526954 : if (myKeepRightAcceptanceTime == -1) {
1723 : // legacy behavior: scale acceptance time with current speed and
1724 : // use old hard-coded constant
1725 17053374 : acceptanceTime = 7 * roadSpeedFactor * MAX2(1.0, myVehicle.getSpeed());
1726 : } else {
1727 267 : acceptanceTime = myKeepRightAcceptanceTime * roadSpeedFactor;
1728 267 : if (followers.hasVehicles()) {
1729 : // reduce acceptanceTime if a follower vehicle is faster or wants to drive faster
1730 : double minFactor = 1.0;
1731 220 : for (int i = 0; i < followers.numSublanes(); ++i) {
1732 176 : CLeaderDist follower = followers[i];
1733 352 : if (follower.first != nullptr && follower.second < 2 * follower.first->getCarFollowModel().brakeGap(follower.first->getSpeed())) {
1734 176 : if (follower.first->getSpeed() >= myVehicle.getSpeed()) {
1735 352 : double factor = MAX2(1.0, myVehicle.getSpeed()) / MAX2(1.0, follower.first->getSpeed());
1736 176 : const double fRSF = follower.first->getLane()->getVehicleMaxSpeed(follower.first) / follower.first->getLane()->getSpeedLimit();
1737 176 : if (fRSF > roadSpeedFactor) {
1738 100 : factor /= fRSF;
1739 : }
1740 176 : if (factor < minFactor) {
1741 : minFactor = factor;
1742 : }
1743 : }
1744 : }
1745 : }
1746 44 : acceptanceTime *= minFactor;
1747 : }
1748 : }
1749 8526954 : double fullSpeedGap = MAX2(0., neighDist - myVehicle.getCarFollowModel().brakeGap(vMax));
1750 8526954 : double fullSpeedDrivingSeconds = MIN2(acceptanceTime, fullSpeedGap / vMax);
1751 8526954 : CLeaderDist neighLead = getSlowest(neighLeaders);
1752 8526954 : if (neighLead.first != 0 && neighLead.first->getSpeed() < vMax) {
1753 7087507 : fullSpeedGap = MAX2(0., MIN2(fullSpeedGap,
1754 7087507 : neighLead.second - myVehicle.getCarFollowModel().getSecureGap(&myVehicle, neighLead.first,
1755 7087507 : vMax, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel())));
1756 7087507 : fullSpeedDrivingSeconds = MIN2(fullSpeedDrivingSeconds, fullSpeedGap / (vMax - neighLead.first->getSpeed()));
1757 : }
1758 8526954 : const double deltaProb = (myChangeProbThresholdRight * (fullSpeedDrivingSeconds / acceptanceTime) / KEEP_RIGHT_TIME) * myVehicle.getActionStepLengthSecs();
1759 8526954 : const bool isSlide = preventSliding(latLaneDist);
1760 : // stay below threshold
1761 9598486 : if (!isSlide || !wantsKeepRight(myKeepRightProbability + deltaProb)) {
1762 8495537 : myKeepRightProbability += deltaProb;
1763 : }
1764 :
1765 : #ifdef DEBUG_WANTSCHANGE
1766 : if (gDebugFlag2) {
1767 : std::cout << STEPS2TIME(currentTime)
1768 : << " considering keepRight:"
1769 : << " vMax=" << vMax
1770 : << " neighDist=" << neighDist
1771 : << " brakeGap=" << myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed())
1772 : << " leaderSpeed=" << (neighLead.first == 0 ? -1 : neighLead.first->getSpeed())
1773 : << " secGap=" << (neighLead.first == 0 ? -1 : myVehicle.getCarFollowModel().getSecureGap(&myVehicle, neighLead.first,
1774 : myVehicle.getSpeed(), neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()))
1775 : << " acceptanceTime=" << acceptanceTime
1776 : << " fullSpeedGap=" << fullSpeedGap
1777 : << " fullSpeedDrivingSeconds=" << fullSpeedDrivingSeconds
1778 : << " dProb=" << deltaProb
1779 : << " isSlide=" << isSlide
1780 : << " keepRight=" << myKeepRightProbability
1781 : << " speedGainL=" << mySpeedGainProbabilityLeft
1782 : << "\n";
1783 : }
1784 : #endif
1785 8526954 : if (wantsKeepRight(myKeepRightProbability)
1786 : /*&& latLaneDist <= -NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()*/) {
1787 481705 : ret |= LCA_KEEPRIGHT;
1788 : assert(myVehicle.getLane()->getIndex() > neighLane.getIndex() || isOpposite());
1789 481705 : if (!cancelRequest(ret | LCA_RIGHT, laneOffset)) {
1790 480853 : latDist = latLaneDist;
1791 480853 : maneuverDist = latLaneDist;
1792 480853 : blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1793 : leaders, followers, blockers,
1794 : neighLeaders, neighFollowers, neighBlockers);
1795 480853 : return ret;
1796 : } else {
1797 : ret &= ~LCA_KEEPRIGHT;
1798 : }
1799 : }
1800 : }
1801 :
1802 92503986 : const double bidiRightFactor = myVehicle.getLane()->getBidiLane() == nullptr && !isOpposite() ? 1 : 0.05;
1803 : #ifdef DEBUG_WANTSCHANGE
1804 : if (gDebugFlag2) {
1805 : std::cout << STEPS2TIME(currentTime)
1806 : << " speedGainR=" << mySpeedGainProbabilityRight
1807 : << " speedGainL=" << mySpeedGainProbabilityLeft
1808 : << " neighDist=" << neighDist
1809 : << " neighTime=" << neighDist / MAX2(.1, myVehicle.getSpeed())
1810 : << " rThresh=" << myChangeProbThresholdRight
1811 : << " rThresh2=" << myChangeProbThresholdRight* bidiRightFactor
1812 : << " latDist=" << latDist
1813 : << "\n";
1814 : }
1815 : #endif
1816 :
1817 : // make changing on the right more attractive on bidi edges
1818 2692156 : if (latDist < 0 && mySpeedGainProbabilityRight >= myChangeProbThresholdRight * bidiRightFactor
1819 94518964 : && neighDist / MAX2(.1, myVehicle.getSpeed()) > mySpeedGainRemainTime) {
1820 973401 : ret |= LCA_SPEEDGAIN;
1821 973401 : if (!cancelRequest(ret | getLCA(ret, latDist), laneOffset)) {
1822 961566 : int blockedFully = 0;
1823 961566 : maneuverDist = latDist;
1824 961566 : blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1825 : leaders, followers, blockers,
1826 : neighLeaders, neighFollowers, neighBlockers,
1827 : nullptr, nullptr, false, 0, &blockedFully);
1828 : //commitManoeuvre(blocked, blockedFully, leaders, neighLeaders, neighLane);
1829 : return ret;
1830 : } else {
1831 : // @note: restore ret so subsequent calls to cancelRequest work correctly
1832 11835 : latDist = 0;
1833 : ret &= ~LCA_SPEEDGAIN;
1834 : }
1835 : }
1836 : }
1837 102393227 : if (!right || isOpposite()) {
1838 :
1839 93162789 : const bool stayInLane = myVehicle.getLateralPositionOnLane() + latDist < 0.5 * myVehicle.getLane()->getWidth();
1840 : #ifdef DEBUG_WANTSCHANGE
1841 : if (gDebugFlag2) {
1842 : std::cout << STEPS2TIME(currentTime)
1843 : << " speedGainL=" << mySpeedGainProbabilityLeft
1844 : << " speedGainR=" << mySpeedGainProbabilityRight
1845 : << " latDist=" << latDist
1846 : << " neighDist=" << neighDist
1847 : << " neighTime=" << neighDist / MAX2(.1, myVehicle.getSpeed())
1848 : << " lThresh=" << myChangeProbThresholdLeft
1849 : << " stayInLane=" << stayInLane
1850 : << "\n";
1851 : }
1852 : #endif
1853 :
1854 93162789 : if (latDist > 0 && mySpeedGainProbabilityLeft > myChangeProbThresholdLeft &&
1855 : // if we leave our lane, we should be able to stay in the new
1856 : // lane for some time
1857 4069288 : (stayInLane || neighDist / MAX2(.1, myVehicle.getSpeed()) > mySpeedGainRemainTime)) {
1858 2778896 : ret |= LCA_SPEEDGAIN;
1859 2778896 : if (!cancelRequest(ret + getLCA(ret, latDist), laneOffset)) {
1860 2759667 : int blockedFully = 0;
1861 2759667 : maneuverDist = latDist;
1862 2759667 : blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
1863 : leaders, followers, blockers,
1864 : neighLeaders, neighFollowers, neighBlockers,
1865 : nullptr, nullptr, false, 0, &blockedFully);
1866 : //commitManoeuvre(blocked, blockedFully, leaders, neighLeaders, neighLane);
1867 : return ret;
1868 : } else {
1869 19229 : latDist = 0;
1870 : ret &= ~LCA_SPEEDGAIN;
1871 : }
1872 : }
1873 : }
1874 :
1875 : double latDistSublane = 0.;
1876 99633560 : const double halfLaneWidth = myVehicle.getLane()->getWidth() * 0.5;
1877 99633560 : const double halfVehWidth = getWidth() * 0.5;
1878 99633560 : if (myVehicle.getParameter().arrivalPosLatProcedure != ArrivalPosLatDefinition::DEFAULT
1879 6099 : && myVehicle.getRoute().getLastEdge() == &myVehicle.getLane()->getEdge()
1880 4454 : && bestLaneOffset == 0
1881 99638014 : && (myVehicle.getArrivalPos() - getForwardPos()) < ARRIVALPOS_LAT_THRESHOLD) {
1882 : // vehicle is on its final edge, on the correct lane and close to
1883 : // its arrival position. Change to the desired lateral position
1884 868 : switch (myVehicle.getParameter().arrivalPosLatProcedure) {
1885 54 : case ArrivalPosLatDefinition::GIVEN:
1886 54 : latDistSublane = myVehicle.getParameter().arrivalPosLat - myVehicle.getLateralPositionOnLane();
1887 54 : break;
1888 814 : case ArrivalPosLatDefinition::RIGHT:
1889 814 : latDistSublane = -halfLaneWidth + halfVehWidth - myVehicle.getLateralPositionOnLane();
1890 814 : break;
1891 0 : case ArrivalPosLatDefinition::CENTER:
1892 0 : latDistSublane = -myVehicle.getLateralPositionOnLane();
1893 0 : break;
1894 0 : case ArrivalPosLatDefinition::LEFT:
1895 0 : latDistSublane = halfLaneWidth - halfVehWidth - myVehicle.getLateralPositionOnLane();
1896 0 : break;
1897 : default:
1898 : assert(false);
1899 : }
1900 : #ifdef DEBUG_WANTSCHANGE
1901 : if (gDebugFlag2) std::cout << SIMTIME
1902 : << " arrivalPosLatProcedure=" << (int)myVehicle.getParameter().arrivalPosLatProcedure
1903 : << " arrivalPosLat=" << myVehicle.getParameter().arrivalPosLat << "\n";
1904 : #endif
1905 :
1906 : } else {
1907 :
1908 99632692 : LatAlignmentDefinition align = getDesiredAlignment();
1909 99632692 : switch (align) {
1910 4153590 : case LatAlignmentDefinition::RIGHT:
1911 4153590 : latDistSublane = -halfLaneWidth + halfVehWidth - getPosLat();
1912 4153590 : break;
1913 152115 : case LatAlignmentDefinition::LEFT:
1914 152115 : latDistSublane = halfLaneWidth - halfVehWidth - getPosLat();
1915 152115 : break;
1916 92112290 : case LatAlignmentDefinition::CENTER:
1917 : case LatAlignmentDefinition::DEFAULT:
1918 92112290 : latDistSublane = -getPosLat();
1919 92112290 : break;
1920 : case LatAlignmentDefinition::NICE:
1921 : latDistSublane = latDistNice;
1922 : break;
1923 2916706 : case LatAlignmentDefinition::COMPACT:
1924 2916706 : latDistSublane = sublaneSides[sublaneCompact] - rightVehSide;
1925 2916706 : break;
1926 27680 : case LatAlignmentDefinition::ARBITRARY: {
1927 27680 : latDistSublane = myVehicle.getLateralPositionOnLane() - getPosLat();
1928 27680 : const double hLW = myVehicle.getLane()->getWidth() * 0.5;
1929 27680 : const double posLat = myVehicle.getLateralPositionOnLane();
1930 27680 : if (fabs(posLat) > hLW) {
1931 : // vehicle is not within it's current lane
1932 37 : if (posLat > 0) {
1933 32 : latDistSublane -= (posLat - hLW);
1934 : } else {
1935 5 : latDistSublane += (-posLat - hLW);
1936 : }
1937 : } else {
1938 27643 : const double edgeWidth = myVehicle.getCurrentEdge()->getWidth();
1939 27643 : if (getWidth() < edgeWidth) {
1940 27643 : if (rightVehSide < 0) {
1941 2957 : latDistSublane -= rightVehSide;
1942 24686 : } else if (leftVehSide > edgeWidth) {
1943 1416 : latDistSublane -= leftVehSide - edgeWidth;
1944 : }
1945 : }
1946 : }
1947 : break;
1948 : }
1949 441 : case LatAlignmentDefinition::GIVEN: {
1950 : // sublane alignment should not cause the vehicle to leave the lane
1951 441 : const double hw = myVehicle.getLane()->getWidth() / 2 - NUMERICAL_EPS;
1952 882 : const double offset = MAX2(-hw, MIN2(hw, myVehicle.getVehicleType().getPreferredLateralAlignmentOffset()));
1953 441 : latDistSublane = -getPosLat() + offset;
1954 : }
1955 441 : break;
1956 : default:
1957 : break;
1958 : }
1959 : }
1960 : // only factor in preferred lateral alignment if there is no speedGain motivation or it runs in the same direction
1961 99633560 : if (fabs(latDist) <= NUMERICAL_EPS * myVehicle.getActionStepLengthSecs() ||
1962 2920365 : latDistSublane * latDist > 0) {
1963 :
1964 : #if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE) || defined(DEBUG_MANEUVER)
1965 : if (gDebugFlag2) std::cout << SIMTIME
1966 : << " alignment=" << toString(myVehicle.getVehicleType().getPreferredLateralAlignment())
1967 : << " mySpeedGainR=" << mySpeedGainProbabilityRight
1968 : << " mySpeedGainL=" << mySpeedGainProbabilityLeft
1969 : << " latDist=" << latDist
1970 : << " latDistSublane=" << latDistSublane
1971 : << " relGainSublane=" << computeSpeedGain(latDistSublane, defaultNextSpeed)
1972 : << " maneuverDist=" << maneuverDist
1973 : << " myCanChangeFully=" << myCanChangeFully
1974 : << " myTurnAlignmentDist=" << myTurnAlignmentDist
1975 : << " nextTurn=" << myVehicle.getNextTurn().first << ":" << toString(myVehicle.getNextTurn().second)
1976 : << " prevState=" << toString((LaneChangeAction)myPreviousState)
1977 : << "\n";
1978 : #endif
1979 :
1980 10225523 : if ((latDistSublane < 0 && mySpeedGainProbabilityRight < mySpeedLossProbThreshold)
1981 96946698 : || (latDistSublane > 0 && mySpeedGainProbabilityLeft < mySpeedLossProbThreshold)
1982 192593558 : || computeSpeedGain(latDistSublane, defaultNextSpeed) < -mySublaneParam) {
1983 : // do not risk losing speed
1984 : #if defined(DEBUG_WANTSCHANGE)
1985 : if (gDebugFlag2) std::cout << " aborting sublane change to avoid speed loss (mySpeedLossProbThreshold=" << mySpeedLossProbThreshold
1986 : << " speedGain=" << computeSpeedGain(latDistSublane, defaultNextSpeed) << ")\n";
1987 : #endif
1988 : latDistSublane = 0;
1989 : }
1990 : // Ignore preferred lateral alignment if we are in the middle of an unfinished non-alignment maneuver into the opposite direction
1991 97898396 : if (!myCanChangeFully
1992 7700821 : && (myPreviousState & (LCA_STRATEGIC | LCA_COOPERATIVE | LCA_KEEPRIGHT | LCA_SPEEDGAIN)) != 0
1993 103491741 : && ((getManeuverDist() < 0 && latDistSublane > 0) || (getManeuverDist() > 0 && latDistSublane < 0))) {
1994 : #if defined(DEBUG_WANTSCHANGE)
1995 : if (gDebugFlag2) {
1996 : std::cout << " aborting sublane change due to prior maneuver\n";
1997 : }
1998 : #endif
1999 : latDistSublane = 0;
2000 : }
2001 97898396 : latDist = latDistSublane * (isOpposite() ? -1 : 1);
2002 : // XXX first compute preferred adaptation and then override with speed
2003 : // (this way adaptation is still done if changing for speedgain is
2004 : // blocked)
2005 97898396 : if (fabs(latDist) >= NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) {
2006 : #ifdef DEBUG_WANTSCHANGE
2007 : if (gDebugFlag2) std::cout << SIMTIME
2008 : << " adapting to preferred alignment=" << toString(myVehicle.getVehicleType().getPreferredLateralAlignment())
2009 : << " latDist=" << latDist
2010 : << "\n";
2011 : #endif
2012 8296461 : ret |= LCA_SUBLANE;
2013 : // include prior motivation when sublane-change is part of finishing an ongoing maneuver in the same direction
2014 8296461 : if (getPreviousManeuverDist() * latDist > 0) {
2015 951900 : int priorReason = (myPreviousState & LCA_CHANGE_REASONS & ~LCA_SUBLANE);
2016 951900 : ret |= priorReason;
2017 : #ifdef DEBUG_WANTSCHANGE
2018 : if (gDebugFlag2 && priorReason != 0) std::cout << " including prior reason " << toString((LaneChangeAction)priorReason)
2019 : << " prevManeuverDist=" << getPreviousManeuverDist() << "\n";
2020 : #endif
2021 : }
2022 8296461 : if (!cancelRequest(ret + getLCA(ret, latDist), laneOffset)) {
2023 8293343 : maneuverDist = latDist;
2024 8293343 : blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset,
2025 : leaders, followers, blockers,
2026 : neighLeaders, neighFollowers, neighBlockers);
2027 8293343 : return ret;
2028 : } else {
2029 3118 : ret &= ~LCA_SUBLANE;
2030 : }
2031 : } else {
2032 89601935 : return ret | LCA_SUBLANE | LCA_STAY;
2033 : }
2034 : }
2035 1738282 : latDist = 0;
2036 :
2037 :
2038 : // --------
2039 : /*
2040 : if (changeToBest && bestLaneOffset == curr.bestLaneOffset && laneOffset != 0
2041 : && (right
2042 : ? mySpeedGainProbabilityRight > MAX2(0., mySpeedGainProbabilityLeft)
2043 : : mySpeedGainProbabilityLeft > MAX2(0., mySpeedGainProbabilityRight))) {
2044 : // change towards the correct lane, speedwise it does not hurt
2045 : ret |= LCA_STRATEGIC;
2046 : if (!cancelRequest(ret, laneOffset)) {
2047 : latDist = latLaneDist;
2048 : blocked = checkBlocking(neighLane, latDist, laneOffset,
2049 : leaders, followers, blockers,
2050 : neighLeaders, neighFollowers, neighBlockers);
2051 : return ret;
2052 : }
2053 : }
2054 : */
2055 : #ifdef DEBUG_WANTSCHANGE
2056 : if (gDebugFlag2) {
2057 : std::cout << STEPS2TIME(currentTime)
2058 : << " veh=" << myVehicle.getID()
2059 : << " mySpeedGainR=" << mySpeedGainProbabilityRight
2060 : << " mySpeedGainL=" << mySpeedGainProbabilityLeft
2061 : << " myKeepRight=" << myKeepRightProbability
2062 : << "\n";
2063 : }
2064 : #endif
2065 1738282 : return ret;
2066 103835646 : }
2067 :
2068 :
2069 : int
2070 236768018 : MSLCM_SL2015::slowDownForBlocked(MSVehicle** blocked, int state) {
2071 : // if this vehicle is blocking someone in front, we maybe decelerate to let him in
2072 236768018 : if ((*blocked) != nullptr) {
2073 16243646 : double gap = (*blocked)->getPositionOnLane() - (*blocked)->getVehicleType().getLength() - myVehicle.getPositionOnLane() - myVehicle.getVehicleType().getMinGap();
2074 : #ifdef DEBUG_SLOWDOWN
2075 : if (gDebugFlag2) {
2076 : std::cout << SIMTIME
2077 : << " veh=" << myVehicle.getID()
2078 : << " blocked=" << Named::getIDSecure(*blocked)
2079 : << " gap=" << gap
2080 : << "\n";
2081 : }
2082 : #endif
2083 16243646 : if (gap > POSITION_EPS) {
2084 : //const bool blockedWantsUrgentRight = (((*blocked)->getLaneChangeModel().getOwnState() & LCA_RIGHT != 0)
2085 : // && ((*blocked)->getLaneChangeModel().getOwnState() & LCA_URGENT != 0));
2086 :
2087 12911944 : if (myVehicle.getSpeed() < myVehicle.getCarFollowModel().getMaxDecel()
2088 : //|| blockedWantsUrgentRight // VARIANT_10 (helpblockedRight)
2089 : ) {
2090 10966378 : if ((*blocked)->getSpeed() < SUMO_const_haltingSpeed) {
2091 6703100 : state |= LCA_AMBACKBLOCKER_STANDING;
2092 : } else {
2093 4263278 : state |= LCA_AMBACKBLOCKER;
2094 : }
2095 32899134 : const double targetSpeed = getCarFollowModel().followSpeed(
2096 10966378 : &myVehicle, myVehicle.getSpeed(), (gap - POSITION_EPS),
2097 10966378 : (*blocked)->getSpeed(), (*blocked)->getCarFollowModel().getMaxDecel());
2098 : #ifdef DEBUG_INFORM
2099 : if (gDebugFlag2) {
2100 : std::cout << " slowing down for blocked " << Named::getIDSecure(*blocked) << " targetSpeed=" << targetSpeed << "\n";
2101 : }
2102 : #endif
2103 10966378 : addLCSpeedAdvice(targetSpeed, LCA_CHANGE_TO_HELP);
2104 : //(*blocked) = 0; // VARIANT_14 (furtherBlock)
2105 : }
2106 : }
2107 : }
2108 236768018 : return state;
2109 : }
2110 :
2111 :
2112 : bool
2113 1219053 : MSLCM_SL2015::isBidi(const MSLane* lane) const {
2114 1219053 : if (!MSNet::getInstance()->hasBidiEdges()) {
2115 : return false;
2116 : }
2117 1219053 : if (lane == myVehicle.getLane()->getBidiLane()) {
2118 : return true;
2119 : }
2120 3892619 : for (const MSLane* cand : myVehicle.getBestLanesContinuation()) {
2121 2860215 : if (cand != nullptr && cand->getBidiLane() == lane) {
2122 : return true;
2123 : }
2124 : }
2125 : return false;
2126 : }
2127 :
2128 : void
2129 127486162 : MSLCM_SL2015::updateExpectedSublaneSpeeds(const MSLeaderDistanceInfo& ahead, int sublaneOffset, int laneIndex) {
2130 127486162 : const std::vector<MSLane*>& lanes = myVehicle.getLane()->getEdge().getLanes();
2131 127486162 : const std::vector<MSVehicle::LaneQ>& preb = myVehicle.getBestLanes();
2132 127486162 : const MSLane* lane = isOpposite() ? myVehicle.getLane()->getParallelOpposite() : lanes[laneIndex];
2133 127486162 : const MSLane* next = myVehicle.getBestLanesContinuation().size() > 1 ? myVehicle.getBestLanesContinuation()[1] : nullptr;
2134 105567866 : const MSLink* link = next != nullptr ? lane->getLinkTo(next) : nullptr;
2135 127486162 : const double shift = link != nullptr ? link->getLateralShift() + 0.5 * (lane->getWidth() - next->getWidth()) : 0;
2136 127486162 : const MSLane* bidi = myVehicle.getLane()->getBidiLane();
2137 127486162 : const double vMax = lane->getVehicleMaxSpeed(&myVehicle);
2138 : assert(preb.size() == lanes.size() || isOpposite());
2139 : #ifdef DEBUG_EXPECTED_SLSPEED
2140 : if (DEBUG_COND) {
2141 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " updateExpectedSublaneSpeeds opposite=" << isOpposite()
2142 : << " sublaneOffset=" << sublaneOffset << " laneIndex=" << laneIndex << " lane=" << lane->getID() << " ahead=" << ahead.toString() << "\n";
2143 : }
2144 : #endif
2145 :
2146 661917858 : for (int sublane = 0; sublane < (int)ahead.numSublanes(); ++sublane) {
2147 534431696 : const int edgeSublane = sublane + sublaneOffset;
2148 534431696 : if (edgeSublane >= (int)myExpectedSublaneSpeeds.size()) {
2149 : // this may happen if a sibling lane is wider than the changer lane
2150 170 : continue;
2151 : }
2152 534431526 : if (link != nullptr && lane->getWidth() > next->getWidth() + NUMERICAL_EPS && MSGlobals::gLateralResolution > 0 && sublaneEnds(sublane, next, shift)) {
2153 : // sublane does not continue, discourage from use
2154 2922554 : myExpectedSublaneSpeeds[edgeSublane] = 0;
2155 : #ifdef DEBUG_EXPECTED_SLSPEED
2156 : if (DEBUG_COND) {
2157 : std::cout << " updateExpectedSublaneSpeeds sublane=" << sublane << " doesNotContinue\n";
2158 : }
2159 : #endif
2160 2922554 : continue;
2161 531508972 : } else if (lane->allowsVehicleClass(myVehicle.getVehicleType().getVehicleClass())) {
2162 : // lane allowed, find potential leaders and compute safe speeds
2163 : // XXX anticipate future braking if leader has a lower speed than myVehicle
2164 525319702 : const MSVehicle* leader = ahead[sublane].first;
2165 525319702 : const double gap = ahead[sublane].second;
2166 : double vSafe;
2167 525319702 : if (leader == nullptr) {
2168 97621632 : if (hasBlueLight()) {
2169 : // can continue from any lane if necessary
2170 : vSafe = vMax;
2171 : } else {
2172 97391431 : const int prebIndex = isOpposite() ? (int)preb.size() - 1 : laneIndex;
2173 97391431 : const double dist = preb[prebIndex].length - myVehicle.getPositionOnLane();
2174 97391431 : vSafe = getCarFollowModel().followSpeed(&myVehicle, vMax, dist, 0, 0);
2175 : }
2176 427698070 : } else if (bidi != nullptr && leader->getLane()->getBidiLane() != nullptr && isBidi(leader->getLane())) {
2177 : // oncoming
2178 186649 : if (gap < (1 + mySpeedGainLookahead * 2) * (vMax + leader->getSpeed())) {
2179 : vSafe = 0;
2180 : } else {
2181 : vSafe = vMax;
2182 : }
2183 : #ifdef DEBUG_EXPECTED_SLSPEED
2184 : if (DEBUG_COND) {
2185 : std::cout << SIMTIME << " updateExpectedSublaneSpeeds sublane=" << sublane << " leader=" << leader->getID() << " bidi=" << bidi->getID() << " gap=" << gap << " vSafe=" << vSafe << "\n";
2186 : }
2187 : #endif
2188 : } else {
2189 427511421 : if (leader->getAcceleration() > 0.5 * leader->getCarFollowModel().getMaxAccel()) {
2190 : // assume that the leader will continue accelerating to its maximum speed
2191 49425985 : vSafe = leader->getLane()->getVehicleMaxSpeed(leader);
2192 : } else {
2193 378085436 : vSafe = getCarFollowModel().followSpeed(
2194 378085436 : &myVehicle, vMax, gap, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel());
2195 : #ifdef DEBUG_EXPECTED_SLSPEED
2196 : if (DEBUG_COND) {
2197 : std::cout << " updateExpectedSublaneSpeeds edgeSublane=" << edgeSublane << " leader=" << leader->getID() << " gap=" << gap << " vSafe=" << vSafe << "\n";
2198 : }
2199 : #endif
2200 378085436 : vSafe = forecastAverageSpeed(vSafe, vMax, gap, leader->getSpeed());
2201 : }
2202 : }
2203 : // take pedestrians into account
2204 525319702 : if (lane->getEdge().getPersons().size() > 0 && lane->hasPedestrians()) {
2205 : /// XXX this could be done faster by checking all sublanes at once (but would complicate the MSPModel API)
2206 : double foeRight, foeLeft;
2207 278387 : ahead.getSublaneBorders(sublane, 0, foeRight, foeLeft);
2208 : // get all leaders ahead or overlapping
2209 278387 : const PersonDist pedLeader = lane->nextBlocking(myVehicle.getPositionOnLane() - myVehicle.getVehicleType().getLength(), foeRight, foeLeft);
2210 278387 : if (pedLeader.first != 0) {
2211 80906 : const double pedGap = pedLeader.second - myVehicle.getVehicleType().getMinGap() - myVehicle.getVehicleType().getLength();
2212 : // we do not know the walking direction here so we take the pedestrian speed as 0
2213 80906 : vSafe = MIN2(getCarFollowModel().stopSpeed(&myVehicle, vMax, pedGap),
2214 : forecastAverageSpeed(vSafe, vMax, pedGap, 0));
2215 : #ifdef DEBUG_EXPECTED_SLSPEED
2216 : if (DEBUG_COND) {
2217 : std::cout << " updateExpectedSublaneSpeeds edgeSublane=" << edgeSublane << " pedLeader=" << pedLeader.first->getID() << " gap=" << pedGap << " vSafe=" << vSafe << "\n";
2218 : }
2219 : #endif
2220 : }
2221 : }
2222 : // take bidi pedestrians into account
2223 525319702 : if (bidi != nullptr && bidi->getEdge().getPersons().size() > 0 && bidi->hasPedestrians()) {
2224 : /// XXX this could be done faster by checking all sublanes at once (but would complicate the MSPModel API)
2225 : double foeRight, foeLeft;
2226 4277 : ahead.getSublaneBorders(sublane, 0, foeRight, foeLeft);
2227 4277 : const double foeRightBidi = bidi->getWidth() - foeLeft;
2228 4277 : const double foeLeftBidi = bidi->getWidth() - foeRight;
2229 : // get all leaders ahead or overlapping
2230 4277 : const double relativeBackPos = myVehicle.getLane()->getLength() - myVehicle.getPositionOnLane() + myVehicle.getLength();
2231 4277 : const double stopTime = ceil(myVehicle.getSpeed() / myVehicle.getCarFollowModel().getMaxDecel());
2232 4277 : PersonDist pedLeader = bidi->nextBlocking(relativeBackPos, foeRightBidi, foeLeftBidi, stopTime, true);
2233 4277 : if (pedLeader.first != 0) {
2234 719 : const double pedGap = pedLeader.second - myVehicle.getVehicleType().getMinGap() - myVehicle.getVehicleType().getLength();
2235 : // we do not know the walking direction here so we take the pedestrian speed as 0
2236 719 : vSafe = MIN2(getCarFollowModel().stopSpeed(&myVehicle, vMax, pedGap),
2237 : forecastAverageSpeed(vSafe, vMax, pedGap, 0));
2238 : #ifdef DEBUG_EXPECTED_SLSPEED
2239 : if (DEBUG_COND) {
2240 : std::cout << " updateExpectedSublaneSpeeds edgeSublane=" << edgeSublane << " pedLeader=" << pedLeader.first->getID() << " (bidi) gap=" << pedGap << " vSafe=" << vSafe << "\n";
2241 : }
2242 : #endif
2243 : }
2244 : }
2245 : vSafe = MIN2(vMax, vSafe);
2246 : // forget old data when on the opposite side
2247 525319702 : const double memoryFactor = isOpposite() ? 0 : pow(SPEEDGAIN_MEMORY_FACTOR, myVehicle.getActionStepLengthSecs());
2248 525319702 : myExpectedSublaneSpeeds[edgeSublane] = memoryFactor * myExpectedSublaneSpeeds[edgeSublane] + (1 - memoryFactor) * vSafe;
2249 : } else {
2250 : // lane forbidden
2251 6189270 : myExpectedSublaneSpeeds[edgeSublane] = -1;
2252 : #ifdef DEBUG_EXPECTED_SLSPEED
2253 : if (DEBUG_COND) {
2254 : std::cout << " updateExpectedSublaneSpeeds edgeSublane=" << edgeSublane << " lane " << lane->getID() << " forbidden\n";
2255 : }
2256 : #endif
2257 : }
2258 : }
2259 : // XXX deal with leaders on subsequent lanes based on preb
2260 127486162 : }
2261 :
2262 :
2263 : bool
2264 7299402 : MSLCM_SL2015::sublaneEnds(int i, const MSLane* next, double shift) {
2265 7299402 : const double side = i * MSGlobals::gLateralResolution + shift;
2266 : return ((side < -NUMERICAL_EPS
2267 13 : && (next->getParallelLane(-1) == nullptr || !next->getParallelLane(-1)->allowsVehicleClass(myVehicle.getVClass())))
2268 7299415 : || (side + MSGlobals::gLateralResolution > next->getWidth()
2269 6458632 : && (next->getParallelLane(1) == nullptr || !next->getParallelLane(1)->allowsVehicleClass(myVehicle.getVClass()))));
2270 : }
2271 :
2272 :
2273 : double
2274 378167061 : MSLCM_SL2015::forecastAverageSpeed(double vSafe, double vMax, double gap, double vLeader) const {
2275 378167061 : const double deltaV = vMax - vLeader;
2276 378167061 : if (deltaV > 0 && gap / deltaV < mySpeedGainLookahead && mySpeedGainLookahead > 0) {
2277 : // anticipate future braking by computing the average
2278 : // speed over the next few seconds
2279 221292969 : const double foreCastTime = mySpeedGainLookahead * 2;
2280 : const double gapClosingTime = MAX2(0.0, gap / deltaV);
2281 221292969 : const double vSafe2 = (gapClosingTime * vSafe + (foreCastTime - gapClosingTime) * vLeader) / foreCastTime;
2282 : #ifdef DEBUG_EXPECTED_SLSPEED
2283 : if (DEBUG_COND && vSafe2 != vSafe) {
2284 : std::cout << " foreCastTime=" << foreCastTime << " gapClosingTime=" << gapClosingTime << " extrapolated vSafe=" << vSafe2 << "\n";
2285 : }
2286 : #endif
2287 : vSafe = vSafe2;
2288 : }
2289 378167061 : return vSafe;
2290 : }
2291 :
2292 :
2293 : double
2294 94695162 : MSLCM_SL2015::computeSpeedGain(double latDistSublane, double defaultNextSpeed) const {
2295 : double result = std::numeric_limits<double>::max();
2296 94695162 : const std::vector<double>& sublaneSides = myVehicle.getLane()->getEdge().getSubLaneSides();
2297 94695162 : const double vehWidth = getWidth();
2298 94695162 : const double rightVehSide = myVehicle.getCenterOnEdge() - vehWidth * 0.5 + latDistSublane;
2299 94695162 : const double leftVehSide = rightVehSide + vehWidth;
2300 751664674 : for (int i = 0; i < (int)sublaneSides.size(); ++i) {
2301 656969512 : const double leftSide = i + 1 < (int)sublaneSides.size() ? sublaneSides[i + 1] : MAX2(myVehicle.getLane()->getEdge().getWidth(), sublaneSides[i] + POSITION_EPS);
2302 656969512 : if (overlap(rightVehSide, leftVehSide, sublaneSides[i], leftSide)) {
2303 353603433 : result = MIN2(result, myExpectedSublaneSpeeds[i]);
2304 : }
2305 : //std::cout << " i=" << i << " rightVehSide=" << rightVehSide << " leftVehSide=" << leftVehSide << " sublaneR=" << sublaneSides[i] << " sublaneL=" << leftSide << " overlap=" << overlap(rightVehSide, leftVehSide, sublaneSides[i], leftSide) << " speed=" << myExpectedSublaneSpeeds[i] << " result=" << result << "\n";
2306 : }
2307 94695162 : return result - defaultNextSpeed;
2308 94695162 : }
2309 :
2310 :
2311 : CLeaderDist
2312 2580392 : MSLCM_SL2015::getLongest(const MSLeaderDistanceInfo& ldi) const {
2313 : int iMax = -1;
2314 : double maxLength = -1;
2315 14167735 : for (int i = 0; i < ldi.numSublanes(); ++i) {
2316 11587343 : const MSVehicle* veh = ldi[i].first;
2317 11587343 : if (veh) {
2318 8918000 : const double length = veh->getVehicleType().getLength();
2319 8918000 : if (length > maxLength && tieBrakeLeader(veh)) {
2320 : maxLength = length;
2321 : iMax = i;
2322 : }
2323 : }
2324 : }
2325 2580392 : return iMax >= 0 ? ldi[iMax] : std::make_pair(nullptr, -1);
2326 : }
2327 :
2328 :
2329 : bool
2330 4243359 : MSLCM_SL2015::tieBrakeLeader(const MSVehicle* veh) const {
2331 : // tie braker if the leader is at the same lane position
2332 4243359 : return veh != nullptr && (veh->getPositionOnLane() != myVehicle.getPositionOnLane()
2333 19461 : || veh->getSpeed() < myVehicle.getSpeed()
2334 19358 : || &veh->getLane()->getEdge() != &myVehicle.getLane()->getEdge()
2335 19357 : || veh->getLane()->getIndex() > myVehicle.getLane()->getIndex());
2336 : }
2337 :
2338 :
2339 : CLeaderDist
2340 11386219 : MSLCM_SL2015::getSlowest(const MSLeaderDistanceInfo& ldi) {
2341 : int iMax = 0;
2342 : double minSpeed = std::numeric_limits<double>::max();
2343 57454920 : for (int i = 0; i < ldi.numSublanes(); ++i) {
2344 46068701 : if (ldi[i].first != 0) {
2345 41022932 : const double speed = ldi[i].first->getSpeed();
2346 41022932 : if (speed < minSpeed) {
2347 : minSpeed = speed;
2348 : iMax = i;
2349 : }
2350 : }
2351 : }
2352 11386219 : return ldi[iMax];
2353 : }
2354 :
2355 :
2356 : const MSVehicle*
2357 69083127 : MSLCM_SL2015::getStopped(const MSLeaderDistanceInfo& ldi) {
2358 69083127 : if (ldi.hasVehicles()) {
2359 298035061 : for (const MSVehicle* const v : ldi.getVehicles()) {
2360 241081226 : if (v != nullptr && v->isStopped()) {
2361 : return v;
2362 : }
2363 : }
2364 : }
2365 : return nullptr;
2366 : }
2367 :
2368 :
2369 : bool
2370 35506374 : MSLCM_SL2015::hasBidiLeader(const MSLeaderDistanceInfo& ldi, const std::vector<MSLane*>& conts) {
2371 35506374 : if (ldi.hasVehicles()) {
2372 163740944 : for (const MSVehicle* const v : ldi.getVehicles()) {
2373 132475530 : if (MSLCHelper::isBidiLeader(v, conts)) {
2374 : return true;
2375 : }
2376 : }
2377 : }
2378 : return false;
2379 : }
2380 :
2381 :
2382 : int
2383 20012987 : MSLCM_SL2015::checkBlocking(const MSLane& neighLane, double& latDist, double maneuverDist, int laneOffset,
2384 : const MSLeaderDistanceInfo& leaders,
2385 : const MSLeaderDistanceInfo& followers,
2386 : const MSLeaderDistanceInfo& /*blockers */,
2387 : const MSLeaderDistanceInfo& neighLeaders,
2388 : const MSLeaderDistanceInfo& neighFollowers,
2389 : const MSLeaderDistanceInfo& /* neighBlockers */,
2390 : std::vector<CLeaderDist>* collectLeadBlockers,
2391 : std::vector<CLeaderDist>* collectFollowBlockers,
2392 : bool keepLatGapManeuver,
2393 : double gapFactor,
2394 : int* retBlockedFully) {
2395 : // truncate latDist according to maxSpeedLat
2396 20012987 : const double maxDist = SPEED2DIST(getMaxSpeedLat2());
2397 20012987 : latDist = MAX2(MIN2(latDist, maxDist), -maxDist);
2398 20012987 : if (myVehicle.hasInfluencer() && myVehicle.getInfluencer().getLatDist() != 0 && myVehicle.getInfluencer().ignoreOverlap()) {
2399 : return 0;
2400 : }
2401 :
2402 20010611 : const double neighRight = getNeighRight(neighLane);
2403 20010611 : if (!myCFRelatedReady) {
2404 11932768 : updateCFRelated(followers, myVehicle.getLane()->getRightSideOnEdge(), false);
2405 11932768 : updateCFRelated(leaders, myVehicle.getLane()->getRightSideOnEdge(), true);
2406 11932768 : if (laneOffset != 0) {
2407 8266877 : updateCFRelated(neighFollowers, neighRight, false);
2408 8266877 : updateCFRelated(neighLeaders, neighRight, true);
2409 : }
2410 11932768 : myCFRelatedReady = true;
2411 : }
2412 :
2413 : // reduce latDist to avoid blockage with overlapping vehicles (no minGapLat constraints)
2414 20010611 : const double center = myVehicle.getCenterOnEdge();
2415 20010611 : updateGaps(leaders, myVehicle.getLane()->getRightSideOnEdge(), center, gapFactor, mySafeLatDistRight, mySafeLatDistLeft, false, 0, latDist, collectLeadBlockers);
2416 20010611 : updateGaps(followers, myVehicle.getLane()->getRightSideOnEdge(), center, gapFactor, mySafeLatDistRight, mySafeLatDistLeft, false, 0, latDist, collectFollowBlockers);
2417 20010611 : if (laneOffset != 0) {
2418 9906891 : updateGaps(neighLeaders, neighRight, center, gapFactor, mySafeLatDistRight, mySafeLatDistLeft, false, 0, latDist, collectLeadBlockers);
2419 9906891 : updateGaps(neighFollowers, neighRight, center, gapFactor, mySafeLatDistRight, mySafeLatDistLeft, false, 0, latDist, collectFollowBlockers);
2420 : }
2421 : #ifdef DEBUG_BLOCKING
2422 : if (gDebugFlag2) {
2423 : std::cout << " checkBlocking latDist=" << latDist << " mySafeLatDistRight=" << mySafeLatDistRight << " mySafeLatDistLeft=" << mySafeLatDistLeft << "\n";
2424 : }
2425 : #endif
2426 : // if we can move at least a little bit in the desired direction, do so (rather than block)
2427 20010611 : const bool forcedTraCIChange = (myVehicle.hasInfluencer()
2428 57516 : && myVehicle.getInfluencer().getLatDist() != 0
2429 20010903 : && myVehicle.getInfluencer().ignoreOverlap());
2430 20010611 : if (latDist < 0) {
2431 10300441 : if (mySafeLatDistRight <= NUMERICAL_EPS) {
2432 : return LCA_BLOCKED_RIGHT | LCA_OVERLAPPING;
2433 9334456 : } else if (!forcedTraCIChange) {
2434 12841770 : latDist = MAX2(latDist, -mySafeLatDistRight);
2435 : }
2436 : } else {
2437 9710170 : if (mySafeLatDistLeft <= NUMERICAL_EPS) {
2438 : return LCA_BLOCKED_LEFT | LCA_OVERLAPPING;
2439 7617961 : } else if (!forcedTraCIChange) {
2440 7617961 : latDist = MIN2(latDist, mySafeLatDistLeft);
2441 : }
2442 : }
2443 :
2444 16952417 : myCanChangeFully = (maneuverDist == 0 || latDist == maneuverDist);
2445 : #ifdef DEBUG_BLOCKING
2446 : if (gDebugFlag2) {
2447 : std::cout << " checkBlocking latDist=" << latDist << " maneuverDist=" << maneuverDist << "\n";
2448 : }
2449 : #endif
2450 : // destination sublanes must be safe
2451 : // intermediate sublanes must not be blocked by overlapping vehicles
2452 :
2453 : // XXX avoid checking the same leader multiple times
2454 : // XXX ensure that only changes within the same lane are undertaken if laneOffset = 0
2455 :
2456 : int blocked = 0;
2457 16952417 : blocked |= checkBlockingVehicles(&myVehicle, leaders, laneOffset, latDist, myVehicle.getLane()->getRightSideOnEdge(), true,
2458 : mySafeLatDistRight, mySafeLatDistLeft, collectLeadBlockers);
2459 16952417 : blocked |= checkBlockingVehicles(&myVehicle, followers, laneOffset, latDist, myVehicle.getLane()->getRightSideOnEdge(), false,
2460 : mySafeLatDistRight, mySafeLatDistLeft, collectFollowBlockers);
2461 16952417 : if (laneOffset != 0) {
2462 8617287 : blocked |= checkBlockingVehicles(&myVehicle, neighLeaders, laneOffset, latDist, neighRight, true,
2463 : mySafeLatDistRight, mySafeLatDistLeft, collectLeadBlockers);
2464 8617287 : blocked |= checkBlockingVehicles(&myVehicle, neighFollowers, laneOffset, latDist, neighRight, false,
2465 : mySafeLatDistRight, mySafeLatDistLeft, collectFollowBlockers);
2466 : }
2467 :
2468 : int blockedFully = 0;
2469 16952417 : blockedFully |= checkBlockingVehicles(&myVehicle, leaders, laneOffset, maneuverDist, myVehicle.getLane()->getRightSideOnEdge(), true,
2470 : mySafeLatDistRight, mySafeLatDistLeft, collectLeadBlockers);
2471 16952417 : blockedFully |= checkBlockingVehicles(&myVehicle, followers, laneOffset, maneuverDist, myVehicle.getLane()->getRightSideOnEdge(), false,
2472 : mySafeLatDistRight, mySafeLatDistLeft, collectFollowBlockers);
2473 16952417 : if (laneOffset != 0) {
2474 8617287 : blockedFully |= checkBlockingVehicles(&myVehicle, neighLeaders, laneOffset, maneuverDist, neighRight, true,
2475 : mySafeLatDistRight, mySafeLatDistLeft, collectLeadBlockers);
2476 8617287 : blockedFully |= checkBlockingVehicles(&myVehicle, neighFollowers, laneOffset, maneuverDist, neighRight, false,
2477 : mySafeLatDistRight, mySafeLatDistLeft, collectFollowBlockers);
2478 : }
2479 16952417 : if (retBlockedFully != nullptr) {
2480 5629621 : *retBlockedFully = blockedFully;
2481 : }
2482 : #ifdef DEBUG_BLOCKING
2483 : if (gDebugFlag2) {
2484 : std::cout << " blocked=" << blocked << " (" << toString((LaneChangeAction)blocked) << ") blockedFully=" << toString((LaneChangeAction)blockedFully)
2485 : << " canChangeFully=" << myCanChangeFully << " keepLatGapManeuver=" << keepLatGapManeuver << "\n";
2486 : }
2487 : #endif
2488 16952417 : if (blocked == 0 && !myCanChangeFully && myPushy == 0 && !keepLatGapManeuver) {
2489 : // aggressive drivers immediately start moving towards potential
2490 : // blockers and only check that the start of their maneuver (latDist) is safe. In
2491 : // contrast, cautious drivers need to check latDist and origLatDist to
2492 : // ensure that the maneuver can be finished without encroaching on other vehicles.
2493 8313511 : blocked |= blockedFully;
2494 : } else {
2495 : // XXX: in case of action step length > simulation step length, pushing may lead to collisions,
2496 : // because maneuver is continued until maneuverDist is reached (perhaps set maneuverDist=latDist)
2497 : }
2498 : #ifdef DEBUG_BLOCKING
2499 : if (gDebugFlag2) {
2500 : std::cout << " blocked2=" << blocked << " (" << toString((LaneChangeAction)blocked) << ")\n";
2501 : }
2502 : #endif
2503 16952417 : if (collectFollowBlockers != nullptr && collectLeadBlockers != nullptr) {
2504 : // prevent vehicles from being classified as leader and follower simultaneously
2505 9182858 : for (std::vector<CLeaderDist>::const_iterator it2 = collectLeadBlockers->begin(); it2 != collectLeadBlockers->end(); ++it2) {
2506 39288009 : for (std::vector<CLeaderDist>::iterator it = collectFollowBlockers->begin(); it != collectFollowBlockers->end();) {
2507 32211292 : if ((*it2).first == (*it).first) {
2508 : #ifdef DEBUG_BLOCKING
2509 : if (gDebugFlag2) {
2510 : std::cout << " removed follower " << (*it).first->getID() << " because it is already a leader\n";
2511 : }
2512 : #endif
2513 : it = collectFollowBlockers->erase(it);
2514 : } else {
2515 : ++it;
2516 : }
2517 : }
2518 : }
2519 : }
2520 : return blocked;
2521 : }
2522 :
2523 :
2524 : int
2525 102278816 : MSLCM_SL2015::checkBlockingVehicles(
2526 : const MSVehicle* ego, const MSLeaderDistanceInfo& vehicles,
2527 : int laneOffset, double latDist, double foeOffset, bool leaders,
2528 : double& safeLatGapRight, double& safeLatGapLeft,
2529 : std::vector<CLeaderDist>* collectBlockers) const {
2530 : // determine borders where safety/no-overlap conditions must hold
2531 : const LaneChangeAction blockType = (laneOffset == 0
2532 102278816 : ? (leaders ? LCA_BLOCKED_BY_LEADER : LCA_BLOCKED_BY_FOLLOWER)
2533 : : (laneOffset > 0
2534 68938296 : ? (leaders ? LCA_BLOCKED_BY_LEFT_LEADER : LCA_BLOCKED_BY_LEFT_FOLLOWER)
2535 30284328 : : (leaders ? LCA_BLOCKED_BY_RIGHT_LEADER : LCA_BLOCKED_BY_RIGHT_FOLLOWER)));
2536 102278816 : const double vehWidth = getWidth();
2537 102278816 : const double rightVehSide = ego->getRightSideOnEdge();
2538 102278816 : const double leftVehSide = rightVehSide + vehWidth;
2539 102278816 : const double rightVehSideDest = rightVehSide + latDist;
2540 102278816 : const double leftVehSideDest = leftVehSide + latDist;
2541 : const double rightNoOverlap = MIN2(rightVehSideDest, rightVehSide);
2542 : const double leftNoOverlap = MAX2(leftVehSideDest, leftVehSide);
2543 : #ifdef DEBUG_BLOCKING
2544 : if (gDebugFlag2) {
2545 : std::cout << " checkBlockingVehicles"
2546 : << " laneOffset=" << laneOffset
2547 : << " latDist=" << latDist
2548 : << " foeOffset=" << foeOffset
2549 : << " vehRight=" << rightVehSide
2550 : << " vehLeft=" << leftVehSide
2551 : << " rightNoOverlap=" << rightNoOverlap
2552 : << " leftNoOverlap=" << leftNoOverlap
2553 : << " destRight=" << rightVehSideDest
2554 : << " destLeft=" << leftVehSideDest
2555 : << " leaders=" << leaders
2556 : << " blockType=" << toString((LaneChangeAction) blockType)
2557 : << "\n";
2558 : }
2559 : #endif
2560 : int result = 0;
2561 521325479 : for (int i = 0; i < vehicles.numSublanes(); ++i) {
2562 431325774 : CLeaderDist vehDist = vehicles[i];
2563 431325774 : if (vehDist.first != 0 && myCFRelated.count(vehDist.first) == 0) {
2564 : const MSVehicle* leader = vehDist.first;
2565 : const MSVehicle* follower = ego;
2566 171519908 : if (!leaders) {
2567 : std::swap(leader, follower);
2568 : }
2569 : // only check the current stripe occupied by foe (transform into edge-coordinates)
2570 : double foeRight, foeLeft;
2571 171519908 : vehicles.getSublaneBorders(i, foeOffset, foeRight, foeLeft);
2572 171519908 : const bool overlapBefore = overlap(rightVehSide, leftVehSide, foeRight, foeLeft);
2573 171519908 : const bool overlapDest = overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft);
2574 171519908 : const bool overlapAny = overlap(rightNoOverlap, leftNoOverlap, foeRight, foeLeft);
2575 : #ifdef DEBUG_BLOCKING
2576 : if (gDebugFlag2) {
2577 : std::cout << " foe=" << vehDist.first->getID()
2578 : << " gap=" << vehDist.second
2579 : << " secGap=" << follower->getCarFollowModel().getSecureGap(follower, leader, follower->getSpeed(), leader->getSpeed(), leader->getCarFollowModel().getMaxDecel())
2580 : << " foeRight=" << foeRight
2581 : << " foeLeft=" << foeLeft
2582 : << " overlapBefore=" << overlapBefore
2583 : << " overlap=" << overlapAny
2584 : << " overlapDest=" << overlapDest
2585 : << "\n";
2586 : }
2587 : #endif
2588 171519908 : if (overlapAny) {
2589 44173400 : if (vehDist.second < 0) {
2590 11491543 : if (overlapBefore && !overlapDest && !outsideEdge()) {
2591 : #ifdef DEBUG_BLOCKING
2592 : if (gDebugFlag2) {
2593 : std::cout << " ignoring current overlap to come clear\n";
2594 : }
2595 : #endif
2596 : } else {
2597 : #ifdef DEBUG_BLOCKING
2598 : if (gDebugFlag2) {
2599 : std::cout << " overlap (" << toString((LaneChangeAction)blockType) << ")\n";
2600 : }
2601 : #endif
2602 11391151 : result |= (blockType | LCA_OVERLAPPING);
2603 11391151 : if (collectBlockers == nullptr) {
2604 12279111 : return result;
2605 : } else {
2606 4683184 : collectBlockers->push_back(vehDist);
2607 : }
2608 : }
2609 32681857 : } else if (overlapDest || !myCanChangeFully) {
2610 : // Estimate state after actionstep (follower may be accelerating!)
2611 : // A comparison between secure gap depending on the expected speeds and the extrapolated gap
2612 : // determines whether the s is blocking the lane change.
2613 : // (Note that the longitudinal state update has already taken effect before LC dynamics (thus "-TS" below), would be affected by #3665)
2614 :
2615 : // Use conservative estimate for time until next action step
2616 : // (XXX: how can the ego know the foe's action step length?)
2617 32416911 : const double timeTillAction = MAX2(follower->getActionStepLengthSecs(), leader->getActionStepLengthSecs()) - TS;
2618 : // Ignore decel for follower
2619 32416911 : const double followerAccel = MAX2(0., follower->getAcceleration());
2620 32416911 : const double leaderAccel = leader->getAcceleration();
2621 : // Expected gap after next actionsteps
2622 32416911 : const double expectedGap = MSCFModel::gapExtrapolation(timeTillAction, vehDist.second, leader->getSpeed(), follower->getSpeed(), leaderAccel, followerAccel, std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
2623 :
2624 : // Determine expected speeds and corresponding secure gap at the extrapolated timepoint
2625 32416911 : const double followerExpectedSpeed = follower->getSpeed() + timeTillAction * followerAccel;
2626 32416911 : const double leaderExpectedSpeed = MAX2(0., leader->getSpeed() + timeTillAction * leaderAccel);
2627 32416911 : const double expectedSecureGap = follower->getCarFollowModel().getSecureGap(follower, leader, followerExpectedSpeed, leaderExpectedSpeed, leader->getCarFollowModel().getMaxDecel());
2628 :
2629 : #if defined(DEBUG_ACTIONSTEPS) && defined(DEBUG_BLOCKING)
2630 : if (gDebugFlag2) {
2631 : std::cout << " timeTillAction=" << timeTillAction
2632 : << " followerAccel=" << followerAccel
2633 : << " followerExpectedSpeed=" << followerExpectedSpeed
2634 : << " leaderAccel=" << leaderAccel
2635 : << " leaderExpectedSpeed=" << leaderExpectedSpeed
2636 : << "\n gap=" << vehDist.second
2637 : << " gapChange=" << (expectedGap - vehDist.second)
2638 : << " expectedGap=" << expectedGap
2639 : << " expectedSecureGap=" << expectedSecureGap
2640 : << " safeLatGapLeft=" << safeLatGapLeft
2641 : << " safeLatGapRight=" << safeLatGapRight
2642 : << std::endl;
2643 : }
2644 : #endif
2645 :
2646 : // @note for euler-update, a different value for secureGap2 may be obtained when applying safetyFactor to followerDecel rather than secureGap
2647 32416911 : const double secureGap2 = expectedSecureGap * getSafetyFactor();
2648 32416911 : if (expectedGap < secureGap2) {
2649 : // Foe is a blocker. Update lateral safe gaps accordingly.
2650 6699055 : if (foeRight > leftVehSide) {
2651 4247566 : safeLatGapLeft = MIN2(safeLatGapLeft, foeRight - leftVehSide);
2652 4141931 : } else if (foeLeft < rightVehSide) {
2653 4807174 : safeLatGapRight = MIN2(safeLatGapRight, rightVehSide - foeLeft);
2654 : }
2655 :
2656 : #ifdef DEBUG_BLOCKING
2657 : if (gDebugFlag2) {
2658 : std::cout << " blocked by " << vehDist.first->getID() << " gap=" << vehDist.second << " expectedGap=" << expectedGap
2659 : << " expectedSecureGap=" << expectedSecureGap << " secGap2=" << secureGap2 << " safetyFactor=" << getSafetyFactor()
2660 : << " safeLatGapLeft=" << safeLatGapLeft << " safeLatGapRight=" << safeLatGapRight
2661 : << "\n";
2662 : }
2663 : #endif
2664 6699055 : result |= blockType;
2665 6699055 : if (collectBlockers == nullptr) {
2666 5571144 : return result;
2667 : }
2668 : #ifdef DEBUG_BLOCKING
2669 : } else if (gDebugFlag2 && expectedGap < expectedSecureGap) {
2670 : std::cout << " ignore blocker " << vehDist.first->getID() << " gap=" << vehDist.second << " expectedGap=" << expectedGap
2671 : << " expectedSecureGap=" << expectedSecureGap << " secGap2=" << secureGap2 << " safetyFactor=" << getSafetyFactor() << "\n";
2672 : #endif
2673 : }
2674 25717856 : if (collectBlockers != nullptr) {
2675 : // collect non-blocking followers as well to make sure
2676 : // they remain non-blocking
2677 6595396 : collectBlockers->push_back(vehDist);
2678 : }
2679 : }
2680 : }
2681 : }
2682 : }
2683 : return result;
2684 :
2685 : }
2686 :
2687 :
2688 : void
2689 40399290 : MSLCM_SL2015::updateCFRelated(const MSLeaderDistanceInfo& vehicles, double foeOffset, bool leaders) {
2690 : // to ensure that we do not ignore the wrong vehicles due to numerical
2691 : // instability we slightly reduce the width
2692 40399290 : const double vehWidth = myVehicle.getVehicleType().getWidth() - NUMERICAL_EPS;
2693 40399290 : const double rightVehSide = myVehicle.getCenterOnEdge() - 0.5 * vehWidth;
2694 40399290 : const double leftVehSide = rightVehSide + vehWidth;
2695 : #ifdef DEBUG_BLOCKING
2696 : if (gDebugFlag2) {
2697 : std::cout << " updateCFRelated foeOffset=" << foeOffset << " vehicles=" << vehicles.toString() << "\n";
2698 : }
2699 : #endif
2700 222521174 : for (int i = 0; i < vehicles.numSublanes(); ++i) {
2701 182121884 : CLeaderDist vehDist = vehicles[i];
2702 221350175 : if (vehDist.first != 0 && (myCFRelated.count(vehDist.first) == 0 || vehDist.second < 0)) {
2703 : double foeRight, foeLeft;
2704 111509953 : vehicles.getSublaneBorders(i, foeOffset, foeRight, foeLeft);
2705 : #ifdef DEBUG_BLOCKING
2706 : if (gDebugFlag2) {
2707 : std::cout << " foe=" << vehDist.first->getID() << " gap=" << vehDist.second
2708 : << " sublane=" << i
2709 : << " foeOffset=" << foeOffset
2710 : << " egoR=" << rightVehSide << " egoL=" << leftVehSide
2711 : << " iR=" << foeRight << " iL=" << foeLeft
2712 : << " egoV=" << myVehicle.getSpeed() << " foeV=" << vehDist.first->getSpeed()
2713 : << " egoE=" << myVehicle.getLane()->getEdge().getID() << " foeE=" << vehDist.first->getLane()->getEdge().getID()
2714 : << "\n";
2715 : }
2716 : #endif
2717 140980876 : if (overlap(rightVehSide, leftVehSide, foeRight, foeLeft) && !outsideEdge()
2718 : // account for imprecise driving by some models
2719 140980726 : && (vehDist.second >= vehDist.first->getVehicleType().getMinGap() * (1 - vehDist.first->getCarFollowModel().getCollisionMinGapFactor())
2720 : // avoid deadlock due to #3729
2721 7555950 : || (!leaders
2722 3232320 : && myVehicle.getPositionOnLane() >= myVehicle.getVehicleType().getLength()
2723 3038826 : && myVehicle.getSpeed() < SUMO_const_haltingSpeed
2724 2669822 : && vehDist.first->getSpeed() < SUMO_const_haltingSpeed
2725 2510712 : && -vehDist.second < vehDist.first->getVehicleType().getMinGap()
2726 2491800 : && &(myVehicle.getLane()->getEdge()) != &(vehDist.first->getLane()->getEdge()))
2727 : )) {
2728 : #ifdef DEBUG_BLOCKING
2729 : if (gDebugFlag2) {
2730 : std::cout << " ignoring cfrelated foe=" << vehDist.first->getID() << "\n";
2731 : }
2732 : #endif
2733 : myCFRelated.insert(vehDist.first);
2734 : } else {
2735 : const int erased = (int)myCFRelated.erase(vehDist.first);
2736 : #ifdef DEBUG_BLOCKING
2737 : if (gDebugFlag2 && erased > 0) {
2738 : std::cout << " restoring cfrelated foe=" << vehDist.first->getID() << "\n";
2739 : }
2740 : #else
2741 : UNUSED_PARAMETER(erased);
2742 : #endif
2743 : }
2744 : }
2745 : }
2746 40399290 : }
2747 :
2748 :
2749 : bool
2750 1451435788 : MSLCM_SL2015::overlap(double right, double left, double right2, double left2) {
2751 : assert(right <= left);
2752 : assert(right2 <= left2);
2753 1451435788 : return left2 >= right + NUMERICAL_EPS && left >= right2 + NUMERICAL_EPS;
2754 : }
2755 :
2756 :
2757 : int
2758 68510846 : MSLCM_SL2015::lowest_bit(int changeReason) {
2759 68510846 : if ((changeReason & LCA_STRATEGIC) != 0) {
2760 : return LCA_STRATEGIC;
2761 : }
2762 53920920 : if ((changeReason & LCA_COOPERATIVE) != 0) {
2763 : return LCA_COOPERATIVE;
2764 : }
2765 53209410 : if ((changeReason & LCA_SPEEDGAIN) != 0) {
2766 : return LCA_SPEEDGAIN;
2767 : }
2768 49453291 : if ((changeReason & LCA_KEEPRIGHT) != 0) {
2769 : return LCA_KEEPRIGHT;
2770 : }
2771 48796518 : if ((changeReason & LCA_TRACI) != 0) {
2772 4760 : return LCA_TRACI;
2773 : }
2774 : return changeReason;
2775 : }
2776 :
2777 :
2778 : MSLCM_SL2015::StateAndDist
2779 165076130 : MSLCM_SL2015::decideDirection(StateAndDist sd1, StateAndDist sd2) const {
2780 : // ignore dummy decisions (returned if mayChange() failes)
2781 165076130 : if (sd1.state == 0) {
2782 67423551 : return sd2;
2783 97652579 : } else if (sd2.state == 0) {
2784 63397156 : return sd1;
2785 : }
2786 : // LCA_SUBLANE is special because LCA_STAY|LCA_SUBLANE may override another LCA_SUBLANE command
2787 34255423 : const bool want1 = ((sd1.state & LCA_WANTS_LANECHANGE) != 0) || ((sd1.state & LCA_SUBLANE) != 0 && (sd1.state & LCA_STAY) != 0);
2788 34255423 : const bool want2 = ((sd2.state & LCA_WANTS_LANECHANGE) != 0) || ((sd2.state & LCA_SUBLANE) != 0 && (sd2.state & LCA_STAY) != 0);
2789 34255423 : const bool can1 = ((sd1.state & LCA_BLOCKED) == 0);
2790 34255423 : const bool can2 = ((sd2.state & LCA_BLOCKED) == 0);
2791 34255423 : int reason1 = lowest_bit(sd1.state & LCA_CHANGE_REASONS);
2792 34255423 : int reason2 = lowest_bit(sd2.state & LCA_CHANGE_REASONS);
2793 : #ifdef DEBUG_DECISION
2794 : if (DEBUG_COND) std::cout << SIMTIME
2795 : << " veh=" << myVehicle.getID()
2796 : << " state1=" << toString((LaneChangeAction)sd1.state)
2797 : << " want1=" << (sd1.state & LCA_WANTS_LANECHANGE)
2798 : << " dist1=" << sd1.latDist
2799 : << " dir1=" << sd1.dir
2800 : << " state2=" << toString((LaneChangeAction)sd2.state)
2801 : << " want2=" << (sd2.state & LCA_WANTS_LANECHANGE)
2802 : << " dist2=" << sd2.latDist
2803 : << " dir2=" << sd2.dir
2804 : << " reason1=" << toString((LaneChangeAction)reason1)
2805 : << " reason2=" << toString((LaneChangeAction)reason2)
2806 : << "\n";
2807 : #endif
2808 34255423 : if (want1) {
2809 33419808 : if (want2) {
2810 23096504 : if ((sd1.state & LCA_TRACI) != 0 && (sd2.state & LCA_TRACI) != 0) {
2811 : // influencer may assign LCA_WANTS_LANECHANGE despite latDist = 0
2812 1316 : if (sd1.latDist == 0 && sd2.latDist != 0) {
2813 96 : return sd2;
2814 1220 : } else if (sd2.latDist == 0 && sd1.latDist != 0) {
2815 16 : return sd1;
2816 : }
2817 : }
2818 : // decide whether right or left has higher priority (lower value in enum LaneChangeAction)
2819 23096392 : if (reason1 < reason2) {
2820 : //if (DEBUG_COND) std::cout << " " << (sd1.state & LCA_CHANGE_REASONS) << " < " << (sd2.state & LCA_CHANGE_REASONS) << "\n";
2821 468517 : return (!can1 && can2 && sd1.sameDirection(sd2)) ? sd2 : sd1;
2822 : //return sd1;
2823 22859807 : } else if (reason1 > reason2) {
2824 : //if (DEBUG_COND) std::cout << " " << (sd1.state & LCA_CHANGE_REASONS) << " > " << (sd2.state & LCA_CHANGE_REASONS) << "\n";
2825 11897198 : return (!can2 && can1 && sd1.sameDirection(sd2)) ? sd1 : sd2;
2826 : //return sd2;
2827 : } else {
2828 : // same priority.
2829 16890099 : if ((sd1.state & LCA_SUBLANE) != 0) {
2830 : // special treatment: prefer action with dir != 0
2831 16611245 : if (sd1.dir == 0) {
2832 15849464 : return sd2;
2833 761781 : } else if (sd2.dir == 0) {
2834 0 : return sd1;
2835 : } else {
2836 : // prefer action that knows more about the desired direction
2837 : // @note when deciding between right and left, right is always given as sd1
2838 : assert(sd1.dir == -1);
2839 : assert(sd2.dir == 1);
2840 761781 : if (sd1.latDist <= 0) {
2841 717525 : return sd1;
2842 44256 : } else if (sd2.latDist >= 0) {
2843 43402 : return sd2;
2844 : }
2845 : // when in doubt, prefer moving to the right
2846 854 : return sd1.latDist <= sd2.latDist ? sd1 : sd2;
2847 : }
2848 : } else {
2849 278854 : if (can1) {
2850 161436 : if (can2) {
2851 : // break strategic ties with tactial concerns
2852 104773 : if (reason1 == LCA_STRATEGIC) {
2853 2607 : if (sd1.latDist <= sd2.latDist) {
2854 4286 : return mySpeedGainProbabilityRight > mySpeedGainProbabilityLeft ? sd1 : sd2;
2855 : } else {
2856 3 : return mySpeedGainProbabilityRight > mySpeedGainProbabilityLeft ? sd2 : sd1;
2857 : }
2858 : } else {
2859 : // finish the shorter maneuver (i.e. continue the current maneuver)
2860 179841 : return fabs(sd1.maneuverDist) < fabs(sd2.maneuverDist) ? sd1 : sd2;
2861 : }
2862 : } else {
2863 56663 : return sd1;
2864 : }
2865 : } else {
2866 117418 : return sd2;
2867 : }
2868 : }
2869 : }
2870 : } else {
2871 10323304 : return sd1;
2872 : }
2873 : } else {
2874 835615 : return sd2;
2875 : }
2876 :
2877 : }
2878 :
2879 :
2880 : LaneChangeAction
2881 130516953 : MSLCM_SL2015::getLCA(int state, double latDist) {
2882 27146666 : return ((latDist == 0 || (state & LCA_CHANGE_REASONS) == 0)
2883 157663217 : ? LCA_NONE : (latDist < 0 ? LCA_RIGHT : LCA_LEFT));
2884 : }
2885 :
2886 :
2887 : int
2888 118384009 : MSLCM_SL2015::checkStrategicChange(int ret,
2889 : const MSLane& neighLane,
2890 : int laneOffset,
2891 : const MSLeaderDistanceInfo& leaders,
2892 : const MSLeaderDistanceInfo& neighLeaders,
2893 : const MSVehicle::LaneQ& curr,
2894 : const MSVehicle::LaneQ& neigh,
2895 : const MSVehicle::LaneQ& best,
2896 : int bestLaneOffset,
2897 : bool changeToBest,
2898 : double& currentDist,
2899 : double neighDist,
2900 : double laDist,
2901 : double roundaboutBonus,
2902 : double latLaneDist,
2903 : bool checkOpposite,
2904 : double& latDist
2905 : ) {
2906 118384009 : const bool right = (laneOffset == -1);
2907 : const bool left = (laneOffset == 1);
2908 :
2909 118384009 : const double forwardPos = getForwardPos();
2910 118384009 : if (laneOffset != 0) {
2911 35845944 : myLeftSpace = currentDist - forwardPos;
2912 : }
2913 118384009 : const double usableDist = (currentDist - forwardPos - best.occupation * JAM_FACTOR);
2914 : //- (best.lane->getVehicleNumber() * neighSpeed)); // VARIANT 9 jfSpeed
2915 118384009 : const double maxJam = MAX2(neigh.occupation, curr.occupation);
2916 118384009 : const double neighLeftPlace = MAX2(0., neighDist - forwardPos - maxJam);
2917 118384009 : const double overlap = myVehicle.getLateralOverlap();
2918 : // save the left space
2919 :
2920 : #ifdef DEBUG_STRATEGIC_CHANGE
2921 : if (gDebugFlag2) {
2922 : std::cout << SIMTIME
2923 : << " veh=" << myVehicle.getID()
2924 : << " forwardPos=" << forwardPos
2925 : << " laSpeed=" << myLookAheadSpeed
2926 : << " laDist=" << laDist
2927 : << " currentDist=" << currentDist
2928 : << " usableDist=" << usableDist
2929 : << " bestLaneOffset=" << bestLaneOffset
2930 : << " best.length=" << best.length
2931 : << " maxJam=" << maxJam
2932 : << " neighLeftPlace=" << neighLeftPlace
2933 : << " myLeftSpace=" << myLeftSpace
2934 : << " overlap=" << overlap
2935 : << "\n";
2936 : }
2937 : #endif
2938 :
2939 118384009 : if (laneOffset == 0) {
2940 82538065 : if (overlap > MAX2(POSITION_EPS, MSGlobals::gLateralResolution)
2941 364450 : && (getShadowLane() == nullptr || !getShadowLane()->allowsVehicleClass(myVehicle.getVClass()))
2942 82541593 : && getWidth() < myVehicle.getLane()->getWidth()) {
2943 : // @brief we urgently need to return to within lane bounds
2944 3441 : latDist = myVehicle.getLateralPositionOnLane() > 0 ? -overlap : overlap;
2945 3441 : ret |= LCA_STRATEGIC | LCA_URGENT;
2946 : #ifdef DEBUG_STRATEGIC_CHANGE
2947 : if (gDebugFlag2) {
2948 : std::cout << SIMTIME << " returnToLaneBounds\n";
2949 : }
2950 : #endif
2951 : //std::cout << SIMTIME << " veh=" << myVehicle.getID() << " overlap=" << overlap << " returnToLaneBounds\n";
2952 82534624 : } else if (myVehicle.getBestLanesContinuation().size() > 1 && myVehicle.getLane()->getWidth() > myVehicle.getBestLanesContinuation()[1]->getWidth()) {
2953 850979 : const MSLane* cur = myVehicle.getLane();
2954 850979 : const MSLane* next = myVehicle.getBestLanesContinuation()[1];
2955 850979 : const MSLink* link = cur->getLinkTo(next);
2956 850979 : const double distOnLane = cur->getLength() - myVehicle.getPositionOnLane();
2957 850979 : if (link != nullptr && getWidth() < next->getWidth() && distOnLane < 100) {
2958 846857 : double hwDiff = 0.5 * (cur->getWidth() - next->getWidth());
2959 846857 : double rightVehSide = myVehicle.getRightSideOnLane() + link->getLateralShift() - hwDiff;
2960 846857 : double leftVehSide = myVehicle.getLeftSideOnLane() + link->getLateralShift() - hwDiff;
2961 846857 : const double res = MSGlobals::gLateralResolution > 0 ? MSGlobals::gLateralResolution : next->getWidth();
2962 846857 : if (rightVehSide < -res && (next->getParallelLane(-1) == nullptr || !next->getParallelLane(-1)->allowsVehicleClass(myVehicle.getVClass()))) {
2963 23727 : latDist = -rightVehSide;
2964 23727 : myLeftSpace = distOnLane;
2965 23727 : ret |= LCA_STRATEGIC | LCA_URGENT;
2966 : #ifdef DEBUG_STRATEGIC_CHANGE
2967 : if (gDebugFlag2) {
2968 : std::cout << SIMTIME << " rightSublaneEnds rVSide=" << myVehicle.getRightSideOnLane()
2969 : << " shift=" << link->getLateralShift() << " rVSide2=" << rightVehSide << " myLeftSpace=" << myLeftSpace << " \n";
2970 : }
2971 : #endif
2972 823130 : } else if (leftVehSide > next->getWidth() + res && (next->getParallelLane(1) == nullptr || !next->getParallelLane(1)->allowsVehicleClass(myVehicle.getVClass()))) {
2973 32099 : latDist = -(leftVehSide - next->getWidth());
2974 32099 : myLeftSpace = distOnLane;
2975 32099 : ret |= LCA_STRATEGIC | LCA_URGENT;
2976 : #ifdef DEBUG_STRATEGIC_CHANGE
2977 : if (gDebugFlag2) {
2978 : std::cout << SIMTIME << " leftSublaneEnds lVSide=" << myVehicle.getLeftSideOnLane()
2979 : << " shift=" << link->getLateralShift() << " lVSide2=" << leftVehSide << " myLeftSpace=" << myLeftSpace << "\n";
2980 : }
2981 : #endif
2982 : }
2983 : }
2984 : }
2985 22402324 : } else if (laneOffset != 0 && changeToBest && bestLaneOffset == curr.bestLaneOffset
2986 41455772 : && currentDistDisallows(usableDist, bestLaneOffset, laDist)) {
2987 : /// @brief we urgently need to change lanes to follow our route
2988 2365447 : if (!mustOvertakeStopped(false, neighLane, neighLeaders, leaders, forwardPos, neighDist, right, latLaneDist, neigh.bestContinuations, currentDist, latDist)) {
2989 2312251 : latDist = latLaneDist;
2990 2312251 : ret |= LCA_STRATEGIC | LCA_URGENT;
2991 : #ifdef DEBUG_STRATEGIC_CHANGE
2992 : if (gDebugFlag2) {
2993 : std::cout << SIMTIME << " mustChangeToBest\n";
2994 : }
2995 : #endif
2996 : } else {
2997 : #ifdef DEBUG_STRATEGIC_CHANGE
2998 : if (gDebugFlag2) {
2999 : std::cout << " veh=" << myVehicle.getID() << " avoidStoppedNeigh\n";
3000 : }
3001 : #endif
3002 : }
3003 : } else {
3004 : // VARIANT_20 (noOvertakeRight)
3005 33480497 : const MSVehicle* const stoppedLeader = getStopped(neighLeaders);
3006 33480497 : if (left && avoidOvertakeRight(stoppedLeader, true) && neighLeaders.hasVehicles()) {
3007 : // check for slower leader on the left. we should not overtake but
3008 : // rather move left ourselves (unless congested)
3009 : // XXX only adapt as much as possible to get a lateral gap
3010 2859265 : CLeaderDist cld = getSlowest(neighLeaders);
3011 2859265 : const MSVehicle* nv = cld.first;
3012 2859265 : double deltaV = 0.;
3013 2859265 : double vSafe = 0.;
3014 2859265 : if (canOvertakeRight(nv, cld.second, myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle) - neighLane.getVehicleMaxSpeed(nv), HELP_OVERTAKE, vSafe, deltaV)) {
3015 557486 : addLCSpeedAdvice(vSafe, LCA_MLEFT);
3016 557486 : if (vSafe < myVehicle.getSpeed()) {
3017 175037 : mySpeedGainProbabilityRight += myVehicle.getActionStepLengthSecs() * myChangeProbThresholdLeft / 3;
3018 : }
3019 : #ifdef DEBUG_STRATEGIC_CHANGE
3020 : if (gDebugFlag2) {
3021 : std::cout << SIMTIME
3022 : << " avoid overtaking on the right nv=" << nv->getID()
3023 : << " nvSpeed=" << nv->getSpeed()
3024 : << " mySpeedGainProbabilityR=" << mySpeedGainProbabilityRight
3025 : << " plannedSpeed=" << myVehicle.getSpeed() + ACCEL2SPEED(myLCAccelerationAdvices.back().first)
3026 : << "\n";
3027 : }
3028 : #endif
3029 : }
3030 : }
3031 :
3032 : // handling reaction to stopped for opposite direction driving NYI
3033 33480497 : const bool noOpposites = &myVehicle.getLane()->getEdge() == &neighLane.getEdge();
3034 33480497 : if (laneOffset != 0 && myStrategicParam >= 0 && noOpposites && mustOvertakeStopped(true, neighLane, leaders, neighLeaders, forwardPos, neighDist, right, latLaneDist, curr.bestContinuations, currentDist, latDist)) {
3035 : #ifdef DEBUG_STRATEGIC_CHANGE
3036 : if (gDebugFlag2) {
3037 : std::cout << " veh=" << myVehicle.getID() << " mustOvertakeStopped\n";
3038 : }
3039 : #endif
3040 59490 : if (latDist == 0) {
3041 0 : ret |= LCA_STAY | LCA_STRATEGIC;
3042 : } else {
3043 59490 : ret |= LCA_STRATEGIC | LCA_URGENT;
3044 : }
3045 :
3046 33421007 : } else if (!changeToBest && (currentDistDisallows(neighLeftPlace, abs(bestLaneOffset) + 2, laDist))) {
3047 : // the opposite lane-changing direction should be done than the one examined herein
3048 : // we'll check whether we assume we could change anyhow and get back in time...
3049 : //
3050 : // this rule prevents the vehicle from moving in opposite direction of the best lane
3051 : // unless the way till the end where the vehicle has to be on the best lane
3052 : // is long enough
3053 : #ifdef DEBUG_STRATEGIC_CHANGE
3054 : if (gDebugFlag2) {
3055 : std::cout << " veh=" << myVehicle.getID() << " could not change back and forth in time (1) neighLeftPlace=" << neighLeftPlace << "\n";
3056 : }
3057 : #endif
3058 11283587 : ret |= LCA_STAY | LCA_STRATEGIC;
3059 : } else if (
3060 : laneOffset != 0
3061 22137420 : && bestLaneOffset == 0
3062 2097282 : && !leaders.hasStoppedVehicle()
3063 2077775 : && neigh.bestContinuations.back()->getLinkCont().size() != 0
3064 1354385 : && roundaboutBonus == 0
3065 1354385 : && !checkOpposite
3066 627184 : && neighDist < TURN_LANE_DIST
3067 22323829 : && myStrategicParam >= 0) {
3068 : // VARIANT_21 (stayOnBest)
3069 : // we do not want to leave the best lane for a lane which leads elsewhere
3070 : // unless our leader is stopped or we are approaching a roundabout
3071 : #ifdef DEBUG_STRATEGIC_CHANGE
3072 : if (gDebugFlag2) {
3073 : std::cout << " veh=" << myVehicle.getID() << " does not want to leave the bestLane (neighDist=" << neighDist << ")\n";
3074 : }
3075 : #endif
3076 182458 : ret |= LCA_STAY | LCA_STRATEGIC;
3077 : } else if (right
3078 21954962 : && bestLaneOffset == 0
3079 545686 : && myVehicle.getLane()->getSpeedLimit() > 80. / 3.6
3080 21977568 : && myLookAheadSpeed > SUMO_const_haltingSpeed
3081 : ) {
3082 : // let's also regard the case where the vehicle is driving on a highway...
3083 : // in this case, we do not want to get to the dead-end of an on-ramp
3084 : #ifdef DEBUG_STRATEGIC_CHANGE
3085 : if (gDebugFlag2) {
3086 : std::cout << " veh=" << myVehicle.getID() << " does not want to get stranded on the on-ramp of a highway\n";
3087 : }
3088 : #endif
3089 21826 : ret |= LCA_STAY | LCA_STRATEGIC;
3090 : }
3091 : }
3092 118384009 : if (laneOffset != 0 && (ret & LCA_URGENT) == 0 && getShadowLane() != nullptr &&
3093 : // ignore overlap if it goes in the correct direction
3094 1817318 : bestLaneOffset * myVehicle.getLateralPositionOnLane() <= 0) {
3095 : // no decision or decision to stay
3096 : // make sure to stay within lane bounds in case the shadow lane ends
3097 : //const double requiredDist = MAX2(2 * myVehicle.getLateralOverlap(), getSublaneWidth()) / SUMO_const_laneWidth * laDist;
3098 915742 : const double requiredDist = 2 * overlap / SUMO_const_laneWidth * laDist;
3099 915742 : double currentShadowDist = -myVehicle.getPositionOnLane();
3100 : MSLane* shadowPrev = nullptr;
3101 1966088 : for (std::vector<MSLane*>::const_iterator it = curr.bestContinuations.begin(); it != curr.bestContinuations.end(); ++it) {
3102 1730084 : if (*it == nullptr) {
3103 30534 : continue;
3104 : }
3105 1699550 : MSLane* shadow = getShadowLane(*it);
3106 1699550 : if (shadow == nullptr || currentShadowDist >= requiredDist) {
3107 : break;
3108 : }
3109 1019812 : if (shadowPrev != nullptr) {
3110 112025 : currentShadowDist += shadowPrev->getEdge().getInternalFollowingLengthTo(&shadow->getEdge(), myVehicle.getVClass());
3111 : }
3112 1019812 : currentShadowDist += shadow->getLength();
3113 : shadowPrev = shadow;
3114 : #ifdef DEBUG_STRATEGIC_CHANGE
3115 : if (gDebugFlag2) {
3116 : std::cout << " shadow=" << shadow->getID() << " currentShadowDist=" << currentShadowDist << "\n";
3117 : }
3118 : #endif
3119 : }
3120 : #ifdef DEBUG_STRATEGIC_CHANGE
3121 : if (gDebugFlag2) {
3122 : std::cout << " veh=" << myVehicle.getID() << " currentShadowDist=" << currentShadowDist << " requiredDist=" << requiredDist << " overlap=" << overlap << "\n";
3123 : }
3124 : #endif
3125 915742 : if (currentShadowDist < requiredDist && currentShadowDist < usableDist) {
3126 149256 : myLeftSpace = currentShadowDist;
3127 149256 : latDist = myVehicle.getLateralPositionOnLane() < 0 ? overlap : -overlap;
3128 : #ifdef DEBUG_STRATEGIC_CHANGE
3129 : if (gDebugFlag2) {
3130 : std::cout << " must change for shadowLane end latDist=" << latDist << " myLeftSpace=" << myLeftSpace << "\n";
3131 : }
3132 : #endif
3133 149256 : ret |= LCA_STRATEGIC | LCA_URGENT | LCA_STAY ;
3134 : }
3135 : }
3136 :
3137 : // check for overriding TraCI requests
3138 : #if defined(DEBUG_STRATEGIC_CHANGE) || defined(DEBUG_TRACI)
3139 : if (gDebugFlag2) {
3140 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " ret=" << ret;
3141 : }
3142 : #endif
3143 : // store state before canceling
3144 118384009 : getCanceledState(laneOffset) |= ret;
3145 118384009 : int retTraCI = myVehicle.influenceChangeDecision(ret);
3146 118384009 : if ((retTraCI & LCA_TRACI) != 0) {
3147 3952 : if ((retTraCI & LCA_STAY) != 0) {
3148 : ret = retTraCI;
3149 3484 : latDist = 0;
3150 468 : } else if (((retTraCI & LCA_RIGHT) != 0 && laneOffset < 0)
3151 408 : || ((retTraCI & LCA_LEFT) != 0 && laneOffset > 0)) {
3152 : ret = retTraCI;
3153 184 : latDist = latLaneDist;
3154 : }
3155 : }
3156 : #if defined(DEBUG_STRATEGIC_CHANGE) || defined(DEBUG_TRACI)
3157 : if (gDebugFlag2) {
3158 : std::cout << " reqAfterInfluence=" << toString((LaneChangeAction)retTraCI) << " ret=" << toString((LaneChangeAction)ret) << "\n";
3159 : }
3160 : #endif
3161 118384009 : return ret;
3162 : }
3163 :
3164 :
3165 : bool
3166 35602630 : MSLCM_SL2015::mustOvertakeStopped(bool checkCurrent, const MSLane& neighLane, const MSLeaderDistanceInfo& leaders, const MSLeaderDistanceInfo& neighLead,
3167 : double posOnLane, double neighDist, bool right, double latLaneDist, const std::vector<MSLane*>& conts, double& currentDist, double& latDist) {
3168 : bool mustOvertake = false;
3169 35602630 : const MSVehicle* const stoppedLeader = getStopped(leaders);
3170 35602630 : const bool checkOverTakeRight = avoidOvertakeRight(stoppedLeader, true);
3171 : int rightmost;
3172 : int leftmost;
3173 : const bool curHasStopped = stoppedLeader != nullptr;
3174 35602630 : const int dir = latLaneDist < 0 ? -1 : 1;
3175 35602630 : const MSLane* neighBeyond = neighLane.getParallelLane(dir);
3176 35602630 : const bool hasLaneBeyond = checkCurrent && neighBeyond != nullptr && neighBeyond->allowsVehicleClass(myVehicle.getVClass());
3177 : UNUSED_PARAMETER(hasLaneBeyond);
3178 35602630 : if (curHasStopped || hasBidiLeader(leaders, conts)) {
3179 195727 : leaders.getSubLanes(&myVehicle, 0, rightmost, leftmost);
3180 875688 : for (int i = rightmost; i <= leftmost; i++) {
3181 679961 : const CLeaderDist& leader = leaders[i];
3182 679961 : if (leader.first != 0 && (leader.first->isStopped() || MSLCHelper::isBidiLeader(leader.first, conts)) && leader.second < REACT_TO_STOPPED_DISTANCE) {
3183 552081 : const double overtakeDist = leader.second + myVehicle.getVehicleType().getLength() + leader.first->getVehicleType().getLengthWithGap();
3184 552081 : const double remaining = MIN2(neighDist, currentDist) - posOnLane;
3185 : #ifdef DEBUG_STRATEGIC_CHANGE
3186 : if (DEBUG_COND) {
3187 : std::cout << " overtakeDist=" << overtakeDist << " remaining=" << remaining
3188 : << " minDistToStopped=" << neighLead.getMinDistToStopped()
3189 : << " hasLaneBeyond=" << hasLaneBeyond
3190 : << "\n";
3191 : }
3192 : #endif
3193 : if (// current destination leaves enough space to overtake the leader
3194 : remaining > overtakeDist
3195 : // maybe do not overtake on the right at high speed
3196 141639 : && (!checkCurrent || !checkOverTakeRight || !right)
3197 692798 : && (!neighLead.hasStoppedVehicle() || neighLead.getMinDistToStopped() > overtakeDist /*|| (hasLaneBeyond && hasFreeLaneBeyond(neighBeyond, dir))*/)
3198 : //&& (neighLead.first == 0 || !neighLead.first->isStopped()
3199 : // // neighboring stopped vehicle leaves enough space to overtake leader
3200 : // || neighLead.second > overtakeDist))
3201 : ) {
3202 : // avoid becoming stuck behind a stopped leader
3203 112714 : currentDist = myVehicle.getPositionOnLane() + leader.second;
3204 112714 : myLeftSpace = currentDist - posOnLane;
3205 112714 : latDist = latLaneDist;
3206 : mustOvertake = true;
3207 : #ifdef DEBUG_STRATEGIC_CHANGE
3208 : if (DEBUG_COND) {
3209 : std::cout << " veh=" << myVehicle.getID()
3210 : << " overtake " << (leader.first->isStopped() ? "stopped" : "bidi") << " leader=" << leader.first->getID()
3211 : << " newCurrentDist=" << currentDist
3212 : << " overtakeDist=" << overtakeDist
3213 : << " remaining=" << remaining
3214 : << "\n";
3215 : }
3216 : #endif
3217 : }
3218 : }
3219 :
3220 : }
3221 : }
3222 35602630 : if (!mustOvertake && !curHasStopped && neighLead.hasStoppedVehicle()) {
3223 : // #todo fix this if the neigh lane has a different width
3224 146320 : const double offset = (latLaneDist < 0 ? -1 : 1) * myVehicle.getLane()->getWidth();
3225 125763 : neighLead.getSubLanes(&myVehicle, offset, rightmost, leftmost);
3226 624838 : for (int i = 0; i < leaders.numSublanes(); i++) {
3227 499075 : const CLeaderDist& leader = leaders[i];
3228 499075 : if (leader.first != 0 && leader.first->isStopped() && leader.second < REACT_TO_STOPPED_DISTANCE) {
3229 : mustOvertake = true;
3230 0 : if (i >= rightmost && i <= leftmost) {
3231 0 : latDist = myVehicle.getLateralOverlap() * (latLaneDist > 0 ? -1 : 1);
3232 0 : break;
3233 : }
3234 : }
3235 : }
3236 : }
3237 35602630 : return mustOvertake;
3238 : }
3239 :
3240 :
3241 : double
3242 120961142 : MSLCM_SL2015::computeGapFactor(int state) const {
3243 120961142 : return (state & LCA_STRATEGIC) != 0 ? MAX2(0.0, (1.0 - myPushy * (1 + 0.5 * myImpatience))) : 1.0;
3244 : }
3245 :
3246 :
3247 : int
3248 118380750 : MSLCM_SL2015::keepLatGap(int state,
3249 : const MSLeaderDistanceInfo& leaders,
3250 : const MSLeaderDistanceInfo& followers,
3251 : const MSLeaderDistanceInfo& blockers,
3252 : const MSLeaderDistanceInfo& neighLeaders,
3253 : const MSLeaderDistanceInfo& neighFollowers,
3254 : const MSLeaderDistanceInfo& neighBlockers,
3255 : const MSLane& neighLane,
3256 : int laneOffset,
3257 : double& latDist,
3258 : double& maneuverDist,
3259 : int& blocked) {
3260 :
3261 : /* @notes
3262 : * vehicles may need to compromise between fulfilling lane change objectives
3263 : * (LCA_STRATEGIC, LCA_SPEED etc) and maintaining lateral gap. The minimum
3264 : * acceptable lateral gap depends on
3265 : * - the cultural context (China vs Europe)
3266 : * - the driver agressiveness (willingness to encroach on other vehicles to force them to move laterally as well)
3267 : * - see @note in checkBlocking
3268 : * - the vehicle type (car vs motorcycle)
3269 : * - the current speed
3270 : * - the speed difference
3271 : * - the importance / urgency of the desired maneuver
3272 : *
3273 : * the object of this method is to evaluate the above circumstances and
3274 : * either:
3275 : * - allow the current maneuver (state, latDist)
3276 : * - to override the current maneuver with a distance-keeping maneuver
3277 : *
3278 : *
3279 : * laneChangeModel/driver parameters
3280 : * - bool pushy (willingness to encroach)
3281 : * - float minGap at 100km/h (to be interpolated for lower speeds (assume 0 at speed 0)
3282 : * - gapFactors (a factor for each of the change reasons
3283 : *
3284 : * further assumptions
3285 : * - the maximum of egoSpeed and deltaSpeed can be used when interpolating minGap
3286 : * - distance keeping to the edges of the road can be ignored (for now)
3287 : *
3288 : * currentMinGap = minGap * min(1.0, max(v, abs(v - vOther)) / 100) * gapFactor[lc_reason]
3289 : *
3290 : * */
3291 :
3292 : /// XXX to be made configurable
3293 118380750 : double gapFactor = computeGapFactor(state);
3294 118380750 : const double oldLatDist = latDist;
3295 118380750 : const double oldManeuverDist = maneuverDist;
3296 : /// passed state is without traci-influence but we need it here
3297 118380750 : const int traciState = myVehicle.influenceChangeDecision(state);
3298 :
3299 : // compute gaps after maneuver
3300 118380750 : const double halfWidth = getWidth() * 0.5;
3301 : // if the current maneuver is blocked we will stay where we are
3302 118380750 : const double oldCenter = myVehicle.getCenterOnEdge();
3303 : // surplus gaps. these are used to collect various constraints
3304 : // if they do not permit the desired maneuvre, should override it to better maintain distance
3305 : // stay within the current edge
3306 118380750 : double surplusGapRight = oldCenter - halfWidth;
3307 118380750 : double surplusGapLeft = getLeftBorder(laneOffset != 0) - oldCenter - halfWidth;
3308 : const bool stayInLane = (laneOffset == 0
3309 118380750 : || ((traciState & LCA_STRATEGIC) != 0
3310 : && (traciState & LCA_STAY) != 0
3311 : // permit wide vehicles to stay on the road
3312 11491942 : && (surplusGapLeft >= 0 && surplusGapRight >= 0)));
3313 :
3314 118380750 : if (isOpposite()) {
3315 : std::swap(surplusGapLeft, surplusGapRight);
3316 : }
3317 : #ifdef DEBUG_KEEP_LATGAP
3318 : if (gDebugFlag2) {
3319 : std::cout << "\n " << SIMTIME << " keepLatGap() laneOffset=" << laneOffset
3320 : << " latDist=" << latDist
3321 : << " maneuverDist=" << maneuverDist
3322 : << " state=" << toString((LaneChangeAction)state)
3323 : << " traciState=" << toString((LaneChangeAction)traciState)
3324 : << " blocked=" << toString((LaneChangeAction)blocked)
3325 : << " gapFactor=" << gapFactor
3326 : << " stayInLane=" << stayInLane << "\n"
3327 : << " stayInEdge: surplusGapRight=" << surplusGapRight << " surplusGapLeft=" << surplusGapLeft << "\n";
3328 : }
3329 : #endif
3330 : // staying within the edge overrides all minGap considerations
3331 118380750 : if (surplusGapLeft < 0 || surplusGapRight < 0) {
3332 : gapFactor = 0;
3333 : }
3334 :
3335 : // maintain gaps to vehicles on the current lane
3336 : // ignore vehicles that are too far behind
3337 118380750 : const double netOverlap = -myVehicle.getVehicleType().getLength() * 0.5;
3338 118380750 : updateGaps(leaders, myVehicle.getLane()->getRightSideOnEdge(), oldCenter, gapFactor, surplusGapRight, surplusGapLeft, true);
3339 118380750 : updateGaps(followers, myVehicle.getLane()->getRightSideOnEdge(), oldCenter, gapFactor, surplusGapRight, surplusGapLeft, true, netOverlap);
3340 :
3341 118380750 : if (laneOffset != 0) {
3342 : // maintain gaps to vehicles on the target lane
3343 35842685 : const double neighRight = getNeighRight(neighLane);
3344 35842685 : updateGaps(neighLeaders, neighRight, oldCenter, gapFactor, surplusGapRight, surplusGapLeft, true);
3345 35842685 : updateGaps(neighFollowers, neighRight, oldCenter, gapFactor, surplusGapRight, surplusGapLeft, true, netOverlap);
3346 : }
3347 : #ifdef DEBUG_KEEP_LATGAP
3348 : if (gDebugFlag2) {
3349 : std::cout << " minGapLat: surplusGapRight=" << surplusGapRight << " surplusGapLeft=" << surplusGapLeft << "\n"
3350 : << " lastGaps: right=" << myLastLateralGapRight << " left=" << myLastLateralGapLeft << "\n";
3351 : }
3352 : #endif
3353 : // we also need to track the physical gap, in addition to the psychological gap
3354 118380750 : double physicalGapLeft = myLastLateralGapLeft == NO_NEIGHBOR ? surplusGapLeft : myLastLateralGapLeft;
3355 118380750 : double physicalGapRight = myLastLateralGapRight == NO_NEIGHBOR ? surplusGapRight : myLastLateralGapRight;
3356 :
3357 118380750 : const double halfLaneWidth = myVehicle.getLane()->getWidth() * 0.5;
3358 118380750 : const double posLat = myVehicle.getLateralPositionOnLane() * (isOpposite() ? -1 : 1);
3359 118380750 : if (stayInLane || laneOffset == 1) {
3360 : // do not move past the right boundary of the current lane (traffic wasn't checked there)
3361 : // but assume it's ok to be where we are in case we are already beyond
3362 308470583 : surplusGapRight = MIN2(surplusGapRight, MAX2(0.0, halfLaneWidth + posLat - halfWidth));
3363 : physicalGapRight = MIN2(physicalGapRight, MAX2(0.0, halfLaneWidth + posLat - halfWidth));
3364 : }
3365 118380750 : if (stayInLane || laneOffset == -1) {
3366 : // do not move past the left boundary of the current lane (traffic wasn't checked there)
3367 : // but assume it's ok to be where we are in case we are already beyond
3368 288738715 : surplusGapLeft = MIN2(surplusGapLeft, MAX2(0.0, halfLaneWidth - posLat - halfWidth));
3369 : physicalGapLeft = MIN2(physicalGapLeft, MAX2(0.0, halfLaneWidth - posLat - halfWidth));
3370 : }
3371 : #ifdef DEBUG_KEEP_LATGAP
3372 : if (gDebugFlag2) {
3373 : std::cout << " stayInLane: surplusGapRight=" << surplusGapRight << " surplusGapLeft=" << surplusGapLeft << "\n";
3374 : }
3375 : #endif
3376 :
3377 118380750 : if (surplusGapRight + surplusGapLeft < 0) {
3378 : // insufficient lateral space to fulfill all requirements. apportion space proportionally
3379 3529072 : if ((state & LCA_CHANGE_REASONS) == 0) {
3380 31385 : state |= LCA_SUBLANE;
3381 : }
3382 3529072 : const double equalDeficit = 0.5 * (surplusGapLeft + surplusGapRight);
3383 3529072 : if (surplusGapRight < surplusGapLeft) {
3384 : // shift further to the left but no further than there is physical space
3385 1094399 : const double delta = MIN2(equalDeficit - surplusGapRight, physicalGapLeft);
3386 1094399 : latDist = delta;
3387 1094399 : maneuverDist = delta;
3388 : #ifdef DEBUG_KEEP_LATGAP
3389 : if (gDebugFlag2) {
3390 : std::cout << " insufficient latSpace, move left: delta=" << delta << "\n";
3391 : }
3392 : #endif
3393 : } else {
3394 : // shift further to the right but no further than there is physical space
3395 2434673 : const double delta = MIN2(equalDeficit - surplusGapLeft, physicalGapRight);
3396 2434673 : latDist = -delta;
3397 2434673 : maneuverDist = -delta;
3398 : #ifdef DEBUG_KEEP_LATGAP
3399 : if (gDebugFlag2) {
3400 : std::cout << " insufficient latSpace, move right: delta=" << delta << "\n";
3401 : }
3402 : #endif
3403 : }
3404 : } else {
3405 : // sufficient space. move as far as the gaps permit
3406 114851678 : latDist = MAX2(MIN2(latDist, surplusGapLeft), -surplusGapRight);
3407 114851678 : maneuverDist = MAX2(MIN2(maneuverDist, surplusGapLeft), -surplusGapRight);
3408 114851678 : if ((state & LCA_KEEPRIGHT) != 0 && maneuverDist != oldManeuverDist) {
3409 : // don't start keepRight unless it can be completed
3410 113660 : latDist = oldLatDist;
3411 113660 : maneuverDist = oldManeuverDist;
3412 : }
3413 : #ifdef DEBUG_KEEP_LATGAP
3414 : if (gDebugFlag2) {
3415 : std::cout << " adapted latDist=" << latDist << " maneuverDist=" << maneuverDist << " (old=" << oldLatDist << ")\n";
3416 : }
3417 : #endif
3418 : }
3419 : // take into account overriding traci sublane-request
3420 118380750 : if (myVehicle.hasInfluencer() && myVehicle.getInfluencer().getLatDist() != 0) {
3421 : // @note: the influence is reset in MSAbstractLaneChangeModel::setOwnState at the end of the lane-changing code for this vehicle
3422 1888 : latDist = myVehicle.getInfluencer().getLatDist();
3423 1888 : maneuverDist = myVehicle.getInfluencer().getLatDist();
3424 1888 : if (latDist < 0) {
3425 1972 : mySafeLatDistRight = MAX2(-latDist, mySafeLatDistRight);
3426 : } else {
3427 1432 : mySafeLatDistLeft = MAX2(latDist, mySafeLatDistLeft);
3428 : }
3429 1888 : state |= LCA_TRACI;
3430 : #ifdef DEBUG_KEEP_LATGAP
3431 : if (gDebugFlag2) {
3432 : std::cout << " traci influenced latDist=" << latDist << "\n";
3433 : }
3434 : #endif
3435 : }
3436 : // if we cannot move in the desired direction, consider the maneuver blocked anyway
3437 118380750 : const bool nonSublaneChange = (state & (LCA_STRATEGIC | LCA_COOPERATIVE | LCA_SPEEDGAIN | LCA_KEEPRIGHT)) != 0;
3438 118380750 : const bool traciChange = ((state | traciState) & LCA_TRACI) != 0;
3439 118380750 : if (nonSublaneChange && !traciChange) {
3440 19455110 : if ((latDist < NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) && (oldLatDist > 0)) {
3441 : #ifdef DEBUG_KEEP_LATGAP
3442 : if (gDebugFlag2) {
3443 : std::cout << " wanted changeToLeft oldLatDist=" << oldLatDist << ", blocked latGap changeToRight\n";
3444 : }
3445 : #endif
3446 1181110 : latDist = oldLatDist; // restore old request for usage in decideDirection()
3447 1181110 : blocked = LCA_OVERLAPPING | LCA_BLOCKED_LEFT;
3448 18274000 : } else if ((latDist > -NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) && (oldLatDist < 0)) {
3449 : #ifdef DEBUG_KEEP_LATGAP
3450 : if (gDebugFlag2) {
3451 : std::cout << " wanted changeToRight oldLatDist=" << oldLatDist << ", blocked latGap changeToLeft\n";
3452 : }
3453 : #endif
3454 741181 : latDist = oldLatDist; // restore old request for usage in decideDirection()
3455 741181 : blocked = LCA_OVERLAPPING | LCA_BLOCKED_RIGHT;
3456 : }
3457 : }
3458 : // if we move, even though we wish to stay, update the change reason (except for TraCI)
3459 118380750 : if (fabs(latDist) > NUMERICAL_EPS * myVehicle.getActionStepLengthSecs() && oldLatDist == 0) {
3460 1188911 : state &= (~(LCA_CHANGE_REASONS | LCA_STAY) | LCA_TRACI);
3461 : }
3462 : // update blocked status
3463 118380750 : if (fabs(latDist - oldLatDist) > NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) {
3464 : #ifdef DEBUG_KEEP_LATGAP
3465 : if (gDebugFlag2) {
3466 : std::cout << " latDistUpdated=" << latDist << " oldLatDist=" << oldLatDist << "\n";
3467 : }
3468 : #endif
3469 4314822 : blocked = checkBlocking(neighLane, latDist, maneuverDist, laneOffset, leaders, followers, blockers, neighLeaders, neighFollowers, neighBlockers, nullptr, nullptr, nonSublaneChange);
3470 : }
3471 118380750 : if (fabs(latDist) > NUMERICAL_EPS * myVehicle.getActionStepLengthSecs()) {
3472 15013103 : state = (state & ~LCA_STAY);
3473 15013103 : if ((state & LCA_CHANGE_REASONS) == 0) {
3474 1187919 : state |= LCA_SUBLANE;
3475 : }
3476 : } else {
3477 103367647 : if ((state & LCA_SUBLANE) != 0) {
3478 90574660 : state |= LCA_STAY;
3479 : }
3480 : // avoid setting blinker due to numerical issues
3481 103367647 : latDist = 0;
3482 : }
3483 : #if defined(DEBUG_KEEP_LATGAP) || defined(DEBUG_STATE)
3484 : if (gDebugFlag2) {
3485 : std::cout << " latDist2=" << latDist
3486 : << " state2=" << toString((LaneChangeAction)state)
3487 : << " lastGapLeft=" << myLastLateralGapLeft
3488 : << " lastGapRight=" << myLastLateralGapRight
3489 : << " blockedAfter=" << toString((LaneChangeAction)blocked)
3490 : << "\n";
3491 : }
3492 : #endif
3493 118380750 : return state;
3494 : }
3495 :
3496 :
3497 : void
3498 390973128 : MSLCM_SL2015::updateGaps(const MSLeaderDistanceInfo& others, double foeOffset, double oldCenter, double gapFactor,
3499 : double& surplusGapRight, double& surplusGapLeft,
3500 : bool saveMinGap, double netOverlap,
3501 : double latDist,
3502 : std::vector<CLeaderDist>* collectBlockers) {
3503 390973128 : if (others.hasVehicles()) {
3504 334979843 : const double halfWidth = getWidth() * 0.5 + NUMERICAL_EPS;
3505 334979843 : const double baseMinGap = myMinGapLat;
3506 1763055569 : for (int i = 0; i < others.numSublanes(); ++i) {
3507 2777509740 : if (others[i].first != 0 && others[i].second <= 0
3508 1459151417 : && myCFRelated.count(others[i].first) == 0
3509 1632450080 : && (netOverlap == 0 || others[i].second + others[i].first->getVehicleType().getMinGap() < netOverlap)) {
3510 : /// foe vehicle occupies full sublanes
3511 173484438 : const MSVehicle* foe = others[i].first;
3512 173484438 : const double res = MSGlobals::gLateralResolution > 0 ? MSGlobals::gLateralResolution : others[i].first->getLane()->getWidth();
3513 : double foeRight, foeLeft;
3514 173484438 : others.getSublaneBorders(i, foeOffset, foeRight, foeLeft);
3515 173484438 : const double foeCenter = foeRight + 0.5 * res;
3516 173484438 : const double gap = MIN2(fabs(foeRight - oldCenter), fabs(foeLeft - oldCenter)) - halfWidth;
3517 173484438 : const double deltaV = MIN2(LATGAP_SPEED_THRESHOLD, MAX3(LATGAP_SPEED_THRESHOLD2, myVehicle.getSpeed(), fabs(myVehicle.getSpeed() - foe->getSpeed())));
3518 173484438 : const double desiredMinGap = baseMinGap * deltaV / LATGAP_SPEED_THRESHOLD;
3519 173484438 : const double currentMinGap = desiredMinGap * gapFactor; // pushy vehicles may accept a lower lateral gap temporarily
3520 : /*
3521 : if (netOverlap != 0) {
3522 : // foe vehicle is follower with its front ahead of the ego midpoint
3523 : // scale gap requirements so it gets lower for foe which are further behind ego
3524 : //
3525 : // relOverlap approaches 0 as the foe gets closer to the midpoint and it equals 1 if the foe is driving head-to-head
3526 : const double relOverlap = 1 - (others[i].second + others[i].first->getVehicleType().getMinGap()) / netOverlap;
3527 : currentMinGap *= currOverlap * relOverlap;
3528 : }
3529 : */
3530 : #if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
3531 : if (debugVehicle()) {
3532 : std::cout << " updateGaps"
3533 : << " i=" << i
3534 : << " foe=" << foe->getID()
3535 : << " foeRight=" << foeRight
3536 : << " foeLeft=" << foeLeft
3537 : << " oldCenter=" << oldCenter
3538 : << " gap=" << others[i].second
3539 : << " latgap=" << gap
3540 : << " currentMinGap=" << currentMinGap
3541 : << " surplusGapRight=" << surplusGapRight
3542 : << " surplusGapLeft=" << surplusGapLeft
3543 : << "\n";
3544 : }
3545 : #endif
3546 :
3547 : // If foe is maneuvering towards ego, reserve some additional distance.
3548 : // But don't expect the foe to come closer than currentMinGap if it isn't already there.
3549 : // (XXX: How can the ego know the foe's maneuver dist?)
3550 173484438 : if (foeCenter < oldCenter) { // && foe->getLaneChangeModel().getSpeedLat() > 0) {
3551 86087598 : const double foeManeuverDist = MAX2(0., foe->getLaneChangeModel().getManeuverDist());
3552 247641516 : surplusGapRight = MIN3(surplusGapRight, gap - currentMinGap, MAX2(currentMinGap, gap - foeManeuverDist));
3553 : } else { //if (foeCenter > oldCenter && foe->getLaneChangeModel().getSpeedLat() < 0) {
3554 87396840 : const double foeManeuverDist = -MIN2(0., foe->getLaneChangeModel().getManeuverDist());
3555 252697197 : surplusGapLeft = MIN3(surplusGapLeft, gap - currentMinGap, MAX2(currentMinGap, gap - foeManeuverDist));
3556 : }
3557 173484438 : if (saveMinGap) {
3558 84122231 : if (foeCenter < oldCenter) {
3559 : #if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
3560 : if (gDebugFlag2 && gap < myLastLateralGapRight) {
3561 : std::cout << " new minimum rightGap=" << gap << "\n";
3562 : }
3563 : #endif
3564 76788756 : myLastLateralGapRight = MIN2(myLastLateralGapRight, gap);
3565 : } else {
3566 : #if defined(DEBUG_BLOCKING) || defined(DEBUG_KEEP_LATGAP)
3567 : if (gDebugFlag2 && gap < myLastLateralGapLeft) {
3568 : std::cout << " new minimum leftGap=" << gap << "\n";
3569 : }
3570 : #endif
3571 54010379 : myLastLateralGapLeft = MIN2(myLastLateralGapLeft, gap);
3572 : }
3573 : }
3574 173484438 : if (collectBlockers != nullptr) {
3575 : // check if the vehicle is blocking a desire lane change
3576 9755879 : if ((foeCenter < oldCenter && latDist < 0 && gap < (desiredMinGap - latDist))
3577 7156686 : || (foeCenter > oldCenter && latDist > 0 && gap < (desiredMinGap + latDist))) {
3578 8416766 : collectBlockers->push_back(others[i]);
3579 : }
3580 : }
3581 : }
3582 : }
3583 : }
3584 390973128 : }
3585 :
3586 :
3587 : double
3588 1093783211 : MSLCM_SL2015::getWidth() const {
3589 1093783211 : return myVehicle.getVehicleType().getWidth() + NUMERICAL_EPS;
3590 : }
3591 :
3592 :
3593 : double
3594 119173036 : MSLCM_SL2015::computeSpeedLat(double latDist, double& maneuverDist, bool urgent) const {
3595 119173036 : int currentDirection = mySpeedLat >= 0 ? 1 : -1;
3596 119173036 : int directionWish = latDist >= 0 ? 1 : -1;
3597 119173036 : double maxSpeedLat = myVehicle.getVehicleType().getMaxSpeedLat();
3598 119173036 : double accelLat = myAccelLat;
3599 119173036 : if (!urgent && (myLeftSpace > POSITION_EPS || myMaxSpeedLatFactor < 0)) {
3600 67213483 : const double speedBound = myMaxSpeedLatStanding + myMaxSpeedLatFactor * myVehicle.getSpeed();
3601 67213483 : if (myMaxSpeedLatFactor >= 0) {
3602 : // speedbound increases with speed and needs an upper bound
3603 : maxSpeedLat = MIN2(maxSpeedLat, speedBound);
3604 : } else {
3605 : // speedbound decreases with speed and needs a lower bound
3606 : // (only useful if myMaxSpeedLatStanding > maxSpeedLat)
3607 : maxSpeedLat = MAX2(maxSpeedLat, speedBound);
3608 : // increase (never decrease) lateral acceleration in proportion
3609 512 : accelLat *= MAX2(1.0, speedBound / myVehicle.getVehicleType().getMaxSpeedLat());
3610 : }
3611 : }
3612 119173036 : if (getWidth() < myVehicle.getCurrentEdge()->getWidth() && !isOpposite()) {
3613 119084578 : const double rightVehSide = myVehicle.getRightSideOnEdge();
3614 119084578 : const double edgeOverlap = MAX2(-rightVehSide, rightVehSide + myVehicle.getVehicleType().getWidth() - myVehicle.getCurrentEdge()->getWidth());
3615 : // if vehicle is outside edge bounds. Permit stronger lateral maneuvering
3616 119084578 : accelLat = MAX2(accelLat, 2 * edgeOverlap);
3617 : maxSpeedLat = MAX2(maxSpeedLat, edgeOverlap);
3618 : }
3619 :
3620 : #ifdef DEBUG_MANEUVER
3621 : if (debugVehicle()) {
3622 : std::cout << SIMTIME
3623 : << " veh=" << myVehicle.getID()
3624 : << " computeSpeedLat()"
3625 : << " latDist=" << latDist
3626 : << " maneuverDist=" << maneuverDist
3627 : << " urgent=" << urgent
3628 : << " speedLat=" << mySpeedLat
3629 : << " currentDirection=" << currentDirection
3630 : << " directionWish=" << directionWish
3631 : << " myLeftSpace=" << myLeftSpace
3632 : << " maxSpeedLat=" << maxSpeedLat
3633 : << std::endl;
3634 : }
3635 : #endif
3636 : // reduced lateral speed (in the desired direction). Don't change direction against desired.
3637 : double speedDecel;
3638 119173036 : if (directionWish == 1) {
3639 111454318 : speedDecel = MAX2(mySpeedLat - ACCEL2SPEED(accelLat), 0.);
3640 : } else {
3641 7718718 : speedDecel = MIN2(mySpeedLat + ACCEL2SPEED(accelLat), 0.);
3642 : }
3643 : // increased lateral speed (in the desired direction)
3644 119173036 : double speedAccel = MAX2(MIN2(mySpeedLat + directionWish * ACCEL2SPEED(accelLat), maxSpeedLat), -maxSpeedLat);
3645 :
3646 : // can we reach the target distance in a single step? (XXX: assumes "Euler" update)
3647 119173036 : double speedBound = DIST2SPEED(latDist);
3648 : // for lat-gap keeping maneuvres myOrigLatDist may be 0
3649 231556504 : const double fullLatDist = latDist > 0 ? MIN2(mySafeLatDistLeft, MAX2(maneuverDist, latDist)) : MAX2(-mySafeLatDistRight, MIN2(maneuverDist, latDist));
3650 :
3651 : // update maneuverDist, if safety constraints apply in its direction
3652 119173036 : if (maneuverDist * latDist > 0) {
3653 14900420 : maneuverDist = fullLatDist;
3654 : }
3655 :
3656 : #ifdef DEBUG_MANEUVER
3657 : if (debugVehicle()) {
3658 : std::cout << " mySafeLatDistRight=" << mySafeLatDistRight
3659 : << " mySafeLatDistLeft=" << mySafeLatDistLeft
3660 : << " fullLatDist=" << fullLatDist
3661 : << " speedAccel=" << speedAccel
3662 : << " speedDecel=" << speedDecel
3663 : << " speedBound=" << speedBound
3664 : << std::endl;
3665 : }
3666 : #endif
3667 119173036 : if (speedDecel * speedAccel <= 0 && (
3668 : // speedAccel and speedDecel bracket speed 0. This means we can end the maneuver
3669 110641503 : (latDist >= 0 && speedAccel >= speedBound && speedBound >= speedDecel)
3670 9007278 : || (latDist <= 0 && speedAccel <= speedBound && speedBound <= speedDecel))) {
3671 : // we can reach the desired value in this step
3672 : #ifdef DEBUG_MANEUVER
3673 : if (debugVehicle()) {
3674 : std::cout << " computeSpeedLat a)\n";
3675 : }
3676 : #endif
3677 : return speedBound;
3678 : }
3679 : // are we currently moving in the wrong direction?
3680 3950895 : if (latDist * mySpeedLat < 0) {
3681 : #ifdef DEBUG_MANEUVER
3682 : if (debugVehicle()) {
3683 : std::cout << " computeSpeedLat b)\n";
3684 : }
3685 : #endif
3686 440952 : return emergencySpeedLat(speedAccel);
3687 : }
3688 : // check if the remaining distance allows to accelerate laterally
3689 3509943 : double minDistAccel = SPEED2DIST(speedAccel) + currentDirection * MSCFModel::brakeGapEuler(fabs(speedAccel), accelLat, 0); // most we can move in the target direction
3690 3509943 : if ((fabs(minDistAccel) < fabs(fullLatDist)) || (fabs(minDistAccel - fullLatDist) < NUMERICAL_EPS)) {
3691 : #ifdef DEBUG_MANEUVER
3692 : if (debugVehicle()) {
3693 : std::cout << " computeSpeedLat c)\n";
3694 : }
3695 : #endif
3696 : return speedAccel;
3697 : } else {
3698 : #ifdef DEBUG_MANEUVER
3699 : if (debugVehicle()) {
3700 : std::cout << " minDistAccel=" << minDistAccel << "\n";
3701 : }
3702 : #endif
3703 : // check if the remaining distance allows to maintain current lateral speed
3704 652685 : double minDistCurrent = SPEED2DIST(mySpeedLat) + currentDirection * MSCFModel::brakeGapEuler(fabs(mySpeedLat), accelLat, 0);
3705 652685 : if ((fabs(minDistCurrent) < fabs(fullLatDist)) || (fabs(minDistCurrent - fullLatDist) < NUMERICAL_EPS)) {
3706 : #ifdef DEBUG_MANEUVER
3707 : if (debugVehicle()) {
3708 : std::cout << " computeSpeedLat d)\n";
3709 : }
3710 : #endif
3711 154404 : return mySpeedLat;
3712 : }
3713 : }
3714 : // reduce lateral speed
3715 : #ifdef DEBUG_MANEUVER
3716 : if (debugVehicle()) {
3717 : std::cout << " computeSpeedLat e)\n";
3718 : }
3719 : #endif
3720 498281 : return emergencySpeedLat(speedDecel);
3721 : }
3722 :
3723 :
3724 : double
3725 939233 : MSLCM_SL2015::emergencySpeedLat(double speedLat) const {
3726 : // reduce lateral speed for safety purposes
3727 939233 : if (speedLat < 0 && SPEED2DIST(-speedLat) > mySafeLatDistRight) {
3728 14220 : speedLat = -DIST2SPEED(mySafeLatDistRight);
3729 : #ifdef DEBUG_MANEUVER
3730 : if (debugVehicle()) {
3731 : std::cout << " rightDanger speedLat=" << speedLat << "\n";
3732 : }
3733 : #endif
3734 925013 : } else if (speedLat > 0 && SPEED2DIST(speedLat) > mySafeLatDistLeft) {
3735 23298 : speedLat = DIST2SPEED(mySafeLatDistLeft);
3736 : #ifdef DEBUG_MANEUVER
3737 : if (debugVehicle()) {
3738 : std::cout << " leftDanger speedLat=" << speedLat << "\n";
3739 : }
3740 : #endif
3741 : }
3742 939233 : return speedLat;
3743 : }
3744 :
3745 :
3746 : LatAlignmentDefinition
3747 99632692 : MSLCM_SL2015::getDesiredAlignment() const {
3748 : LatAlignmentDefinition align = MSAbstractLaneChangeModel::getDesiredAlignment();
3749 : // Check whether the vehicle should adapt its alignment to an upcoming turn
3750 99632692 : if (myTurnAlignmentDist > 0) {
3751 373095 : const std::pair<double, const MSLink*>& turnInfo = myVehicle.getNextTurn();
3752 373095 : const LinkDirection turnDir = turnInfo.second == nullptr ? LinkDirection::NODIR : turnInfo.second->getDirection();
3753 373095 : const bool indirect = turnInfo.second == nullptr ? false : turnInfo.second->isIndirect();
3754 373095 : if (turnInfo.first < myTurnAlignmentDist) {
3755 : // Vehicle is close enough to the link to change its default alignment
3756 157099 : switch (turnDir) {
3757 13659 : case LinkDirection::TURN:
3758 : case LinkDirection::LEFT:
3759 : case LinkDirection::PARTLEFT:
3760 13659 : if (myVehicle.getLane()->getBidiLane() == nullptr) {
3761 : // no left alignment on bidi lane to avoid blocking oncoming traffic
3762 13591 : align = MSGlobals::gLefthand != indirect ? LatAlignmentDefinition::RIGHT : LatAlignmentDefinition::LEFT;
3763 : }
3764 : break;
3765 12824 : case LinkDirection::TURN_LEFTHAND:
3766 : case LinkDirection::RIGHT:
3767 : case LinkDirection::PARTRIGHT:
3768 12824 : align = MSGlobals::gLefthand != indirect ? LatAlignmentDefinition::LEFT : LatAlignmentDefinition::RIGHT;
3769 : break;
3770 : case LinkDirection::STRAIGHT:
3771 : case LinkDirection::NODIR:
3772 : default:
3773 : break;
3774 : }
3775 : }
3776 : }
3777 99632692 : return align;
3778 : }
3779 :
3780 :
3781 : void
3782 1041576 : MSLCM_SL2015::commitManoeuvre(int blocked, int blockedFully,
3783 : const MSLeaderDistanceInfo& leaders,
3784 : const MSLeaderDistanceInfo& neighLeaders,
3785 : const MSLane& neighLane,
3786 : double maneuverDist) {
3787 1041576 : if (!blocked && !blockedFully && !myCanChangeFully) {
3788 : // round to full action steps
3789 : double secondsToLeaveLane;
3790 527057 : if (MSGlobals::gSemiImplicitEulerUpdate) {
3791 450216 : secondsToLeaveLane = ceil(fabs(maneuverDist) / myVehicle.getVehicleType().getMaxSpeedLat() / myVehicle.getActionStepLengthSecs()) * myVehicle.getActionStepLengthSecs();
3792 : // XXX myAccelLat must be taken into account (refs #3601, see ballistic case for solution)
3793 :
3794 : // XXX This also causes probs: if the difference between the current speed and the committed is higher than the maximal decel,
3795 : // the vehicle may pass myLeftSpace before completing the maneuver.
3796 900432 : myCommittedSpeed = MIN3(myLeftSpace / secondsToLeaveLane,
3797 450216 : myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle),
3798 450216 : myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle));
3799 : #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3800 : if (debugVehicle()) {
3801 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " myCommittedSpeed=" << myCommittedSpeed << " leftSpace=" << myLeftSpace << " secondsToLeave=" << secondsToLeaveLane << "\n";
3802 : }
3803 : #endif
3804 : } else {
3805 :
3806 : // Calculate seconds needed for leaving lane assuming start from lateral speed zero, and lat.accel == -lat.decel
3807 76841 : secondsToLeaveLane = MSCFModel::estimateArrivalTime(fabs(maneuverDist), 0., 0., myVehicle.getVehicleType().getMaxSpeedLat(), myAccelLat, myAccelLat);
3808 : // round to full action steps
3809 76841 : secondsToLeaveLane = ceil(secondsToLeaveLane / myVehicle.getActionStepLengthSecs()) * myVehicle.getActionStepLengthSecs();
3810 :
3811 : // committed speed will eventually be pushed into a drive item during the next planMove() step. This item
3812 : // will not be read before the next action step at current time + actionStepLength-TS, so we need to schedule the corresponding speed.
3813 76841 : const double timeTillActionStep = myVehicle.getActionStepLengthSecs() - TS;
3814 76841 : const double nextActionStepSpeed = MAX2(0., myVehicle.getSpeed() + timeTillActionStep * myVehicle.getAcceleration());
3815 : double nextLeftSpace;
3816 76554 : if (nextActionStepSpeed > 0.) {
3817 76549 : nextLeftSpace = myLeftSpace - timeTillActionStep * (myVehicle.getSpeed() + nextActionStepSpeed) * 0.5;
3818 292 : } else if (myVehicle.getAcceleration() == 0) {
3819 0 : nextLeftSpace = myLeftSpace;
3820 : } else {
3821 : assert(myVehicle.getAcceleration() < 0.);
3822 292 : nextLeftSpace = myLeftSpace + (myVehicle.getSpeed() * myVehicle.getSpeed() / myVehicle.getAcceleration()) * 0.5;
3823 : }
3824 76841 : const double avoidArrivalSpeed = nextActionStepSpeed + ACCEL2SPEED(MSCFModel::avoidArrivalAccel(
3825 : nextLeftSpace, secondsToLeaveLane - timeTillActionStep, nextActionStepSpeed, myVehicle.getCarFollowModel().getEmergencyDecel()));
3826 :
3827 153682 : myCommittedSpeed = MIN3(avoidArrivalSpeed,
3828 76841 : myVehicle.getSpeed() + myVehicle.getCarFollowModel().getMaxAccel() * myVehicle.getActionStepLengthSecs(),
3829 76841 : myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle));
3830 :
3831 : #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3832 : if (gDebugFlag2) {
3833 : std::cout << SIMTIME
3834 : << " veh=" << myVehicle.getID()
3835 : << " avoidArrivalSpeed=" << avoidArrivalSpeed
3836 : << " currentSpeed=" << myVehicle.getSpeed()
3837 : << " myLeftSpace=" << myLeftSpace
3838 : << "\n nextLeftSpace=" << nextLeftSpace
3839 : << " nextActionStepSpeed=" << nextActionStepSpeed
3840 : << " nextActionStepRemainingSeconds=" << secondsToLeaveLane - timeTillActionStep
3841 : << "\n";
3842 : }
3843 : #endif
3844 : }
3845 527057 : myCommittedSpeed = commitFollowSpeed(myCommittedSpeed, maneuverDist, secondsToLeaveLane, leaders, myVehicle.getLane()->getRightSideOnEdge());
3846 527057 : myCommittedSpeed = commitFollowSpeed(myCommittedSpeed, maneuverDist, secondsToLeaveLane, neighLeaders, neighLane.getRightSideOnEdge());
3847 527057 : if (myCommittedSpeed < myVehicle.getCarFollowModel().minNextSpeed(myVehicle.getSpeed(), &myVehicle)) {
3848 72399 : myCommittedSpeed = 0;
3849 : }
3850 : #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3851 : if (gDebugFlag2) {
3852 : std::cout << SIMTIME
3853 : << " veh=" << myVehicle.getID()
3854 : << " secondsToLeave=" << secondsToLeaveLane
3855 : << " maxNext=" << myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle)
3856 : << " committed=" << myCommittedSpeed
3857 : << "\n";
3858 : }
3859 : #endif
3860 : }
3861 1041576 : }
3862 :
3863 : double
3864 1054114 : MSLCM_SL2015::commitFollowSpeed(double speed, double latDist, double secondsToLeaveLane, const MSLeaderDistanceInfo& leaders, double foeOffset) const {
3865 1054114 : if (leaders.hasVehicles()) {
3866 : // we distinguish 3 cases
3867 : // - vehicles with lateral overlap at the end of the maneuver: try to follow safely
3868 : // - vehicles with overlap at the start of the maneuver: avoid collision within secondsToLeaveLane
3869 : // - vehicles without overlap: ignore
3870 :
3871 517503 : const double maxDecel = myVehicle.getCarFollowModel().getMaxDecel();
3872 : // temporarily use another decel value
3873 : MSCFModel& cfmodel = const_cast<MSCFModel&>(myVehicle.getCarFollowModel());
3874 517503 : cfmodel.setMaxDecel(maxDecel / getSafetyFactor());
3875 :
3876 517503 : const double vehWidth = getWidth();
3877 517503 : const double rightVehSide = myVehicle.getCenterOnEdge() - 0.5 * vehWidth;
3878 517503 : const double leftVehSide = rightVehSide + vehWidth;
3879 517503 : const double rightVehSideDest = rightVehSide + latDist;
3880 517503 : const double leftVehSideDest = leftVehSide + latDist;
3881 : #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3882 : if (gDebugFlag2) {
3883 : std::cout << " commitFollowSpeed"
3884 : << " latDist=" << latDist
3885 : << " foeOffset=" << foeOffset
3886 : << " vehRight=" << rightVehSide
3887 : << " vehLeft=" << leftVehSide
3888 : << " destRight=" << rightVehSideDest
3889 : << " destLeft=" << leftVehSideDest
3890 : << "\n";
3891 : }
3892 : #endif
3893 2881085 : for (int i = 0; i < leaders.numSublanes(); ++i) {
3894 2363582 : CLeaderDist vehDist = leaders[i];
3895 2363582 : if (vehDist.first != 0) {
3896 : const MSVehicle* leader = vehDist.first;
3897 : // only check the current stripe occuped by foe (transform into edge-coordinates)
3898 : double foeRight, foeLeft;
3899 1896760 : leaders.getSublaneBorders(i, foeOffset, foeRight, foeLeft);
3900 : #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3901 : if (gDebugFlag2) {
3902 : std::cout << " foe=" << vehDist.first->getID()
3903 : << " gap=" << vehDist.second
3904 : << " secGap=" << myVehicle.getCarFollowModel().getSecureGap(&myVehicle, leader, myVehicle.getSpeed(), leader->getSpeed(), leader->getCarFollowModel().getMaxDecel())
3905 : << " foeRight=" << foeRight
3906 : << " foeLeft=" << foeLeft
3907 : << " overlapBefore=" << overlap(rightVehSide, leftVehSide, foeRight, foeLeft)
3908 : << " overlapDest=" << overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft)
3909 : << "\n";
3910 : }
3911 : #endif
3912 1896760 : if (overlap(rightVehSideDest, leftVehSideDest, foeRight, foeLeft)) {
3913 : // case 1
3914 873603 : const double vSafe = myVehicle.getCarFollowModel().followSpeed(
3915 873603 : &myVehicle, speed, vehDist.second, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel());
3916 : speed = MIN2(speed, vSafe);
3917 : #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3918 : if (gDebugFlag2) {
3919 : std::cout << " case1 vsafe=" << vSafe << " speed=" << speed << "\n";
3920 : }
3921 : #endif
3922 1023157 : } else if (overlap(rightVehSide, leftVehSide, foeRight, foeLeft)) {
3923 : // case 2
3924 509096 : const double vSafe = myVehicle.getCarFollowModel().followSpeedTransient(
3925 : secondsToLeaveLane,
3926 509096 : &myVehicle, speed, vehDist.second, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel());
3927 : speed = MIN2(speed, vSafe);
3928 : #if defined(DEBUG_MANEUVER) || defined(DEBUG_COMMITTED_SPEED)
3929 : if (gDebugFlag2) {
3930 : std::cout << " case2 vsafe=" << vSafe << " speed=" << speed << "\n";
3931 : }
3932 : #endif
3933 : }
3934 : }
3935 : }
3936 : // restore original deceleration
3937 517503 : cfmodel.setMaxDecel(maxDecel);
3938 :
3939 : }
3940 1054114 : return speed;
3941 : }
3942 :
3943 : double
3944 764665060 : MSLCM_SL2015::getSafetyFactor() const {
3945 764665060 : return 1 / ((1 + 0.5 * myImpatience) * myAssertive);
3946 : }
3947 :
3948 : double
3949 1851605 : MSLCM_SL2015::getOppositeSafetyFactor() const {
3950 1851605 : return myOppositeParam <= 0 ? std::numeric_limits<double>::max() : 1 / myOppositeParam;
3951 : }
3952 :
3953 :
3954 : std::string
3955 152 : MSLCM_SL2015::getParameter(const std::string& key) const {
3956 152 : if (key == toString(SUMO_ATTR_LCA_STRATEGIC_PARAM)) {
3957 36 : return toString(myStrategicParam);
3958 116 : } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_PARAM)) {
3959 36 : return toString(myCooperativeParam);
3960 80 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_PARAM)) {
3961 36 : return toString(mySpeedGainParam);
3962 44 : } else if (key == toString(SUMO_ATTR_LCA_KEEPRIGHT_PARAM)) {
3963 0 : return toString(myKeepRightParam);
3964 44 : } else if (key == toString(SUMO_ATTR_LCA_OPPOSITE_PARAM)) {
3965 0 : return toString(myOppositeParam);
3966 44 : } else if (key == toString(SUMO_ATTR_LCA_SUBLANE_PARAM)) {
3967 0 : return toString(mySublaneParam);
3968 44 : } else if (key == toString(SUMO_ATTR_MINGAP_LAT)) {
3969 44 : return toString(myMinGapLat);
3970 0 : } else if (key == toString(SUMO_ATTR_LCA_PUSHY)) {
3971 0 : return toString(myPushy);
3972 0 : } else if (key == toString(SUMO_ATTR_LCA_PUSHYGAP)) {
3973 0 : return toString((myPushy - 1) * myMinGapLat);
3974 0 : } else if (key == toString(SUMO_ATTR_LCA_ASSERTIVE)) {
3975 0 : return toString(myAssertive);
3976 0 : } else if (key == toString(SUMO_ATTR_LCA_IMPATIENCE)) {
3977 0 : return toString(myImpatience);
3978 0 : } else if (key == toString(SUMO_ATTR_LCA_TIME_TO_IMPATIENCE)) {
3979 0 : return toString(myTimeToImpatience);
3980 0 : } else if (key == toString(SUMO_ATTR_LCA_ACCEL_LAT)) {
3981 0 : return toString(myAccelLat);
3982 0 : } else if (key == toString(SUMO_ATTR_LCA_LOOKAHEADLEFT)) {
3983 0 : return toString(myLookaheadLeft);
3984 0 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAINRIGHT)) {
3985 0 : return toString(mySpeedGainRight);
3986 0 : } else if (key == toString(SUMO_ATTR_LCA_LANE_DISCIPLINE)) {
3987 0 : return toString(myLaneDiscipline);
3988 0 : } else if (key == toString(SUMO_ATTR_LCA_SIGMA)) {
3989 0 : return toString(mySigma);
3990 0 : } else if (key == toString(SUMO_ATTR_LCA_KEEPRIGHT_ACCEPTANCE_TIME)) {
3991 0 : return toString(myKeepRightAcceptanceTime);
3992 0 : } else if (key == toString(SUMO_ATTR_LCA_OVERTAKE_DELTASPEED_FACTOR)) {
3993 0 : return toString(myOvertakeDeltaSpeedFactor);
3994 0 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_LOOKAHEAD)) {
3995 0 : return toString(mySpeedGainLookahead);
3996 0 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_REMAIN_TIME)) {
3997 0 : return toString(mySpeedGainRemainTime);
3998 0 : } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_ROUNDABOUT)) {
3999 0 : return toString(myRoundaboutBonus);
4000 0 : } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_SPEED)) {
4001 0 : return toString(myCooperativeSpeed);
4002 0 : } else if (key == toString(SUMO_ATTR_LCA_MAXSPEEDLATSTANDING)) {
4003 0 : return toString(myMaxSpeedLatStanding);
4004 0 : } else if (key == toString(SUMO_ATTR_LCA_MAXSPEEDLATFACTOR)) {
4005 0 : return toString(myMaxSpeedLatFactor);
4006 0 : } else if (key == toString(SUMO_ATTR_LCA_MAXDISTLATSTANDING)) {
4007 0 : return toString(myMaxDistLatStanding);
4008 : // access to internal state for debugging in sumo-gui (not documented since it may change at any time)
4009 0 : } else if (key == "speedGainProbabilityRight") {
4010 0 : return toString(mySpeedGainProbabilityRight);
4011 0 : } else if (key == "speedGainProbabilityLeft") {
4012 0 : return toString(mySpeedGainProbabilityLeft);
4013 0 : } else if (key == "keepRightProbability") {
4014 0 : return toString(myKeepRightProbability);
4015 0 : } else if (key == "lookAheadSpeed") {
4016 0 : return toString(myLookAheadSpeed);
4017 0 : } else if (key == "sigmaState") {
4018 0 : return toString(mySigmaState);
4019 : // motivation relative to threshold
4020 0 : } else if (key == "speedGainRP") {
4021 0 : return toString(mySpeedGainProbabilityRight / myChangeProbThresholdRight);
4022 0 : } else if (key == "speedGainLP") {
4023 0 : return toString(mySpeedGainProbabilityLeft / myChangeProbThresholdLeft);
4024 0 : } else if (key == "keepRightP") {
4025 0 : return toString(myKeepRightProbability * myKeepRightParam / myChangeProbThresholdRight);
4026 : }
4027 0 : throw InvalidArgument("Parameter '" + key + "' is not supported for laneChangeModel of type '" + toString(myModel) + "'");
4028 : }
4029 :
4030 : void
4031 277340 : MSLCM_SL2015::setParameter(const std::string& key, const std::string& value) {
4032 : double doubleValue;
4033 : try {
4034 277340 : doubleValue = StringUtils::toDouble(value);
4035 0 : } catch (NumberFormatException&) {
4036 0 : throw InvalidArgument("Setting parameter '" + key + "' requires a number for laneChangeModel of type '" + toString(myModel) + "'");
4037 0 : }
4038 277340 : if (key == toString(SUMO_ATTR_LCA_STRATEGIC_PARAM)) {
4039 71016 : myStrategicParam = doubleValue;
4040 206324 : } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_PARAM)) {
4041 36 : myCooperativeParam = doubleValue;
4042 206288 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_PARAM)) {
4043 0 : mySpeedGainParam = doubleValue;
4044 206288 : } else if (key == toString(SUMO_ATTR_LCA_KEEPRIGHT_PARAM)) {
4045 0 : myKeepRightParam = doubleValue;
4046 206288 : } else if (key == toString(SUMO_ATTR_LCA_OPPOSITE_PARAM)) {
4047 0 : myOppositeParam = doubleValue;
4048 206288 : } else if (key == toString(SUMO_ATTR_LCA_SUBLANE_PARAM)) {
4049 0 : mySublaneParam = doubleValue;
4050 206288 : } else if (key == toString(SUMO_ATTR_MINGAP_LAT)) {
4051 68768 : myMinGapLat = doubleValue;
4052 137520 : } else if (key == toString(SUMO_ATTR_LCA_PUSHY)) {
4053 0 : myPushy = doubleValue;
4054 137520 : } else if (key == toString(SUMO_ATTR_LCA_PUSHYGAP)) {
4055 0 : myPushy = 1 - doubleValue / myMinGapLat;
4056 137520 : } else if (key == toString(SUMO_ATTR_LCA_ASSERTIVE)) {
4057 0 : myAssertive = doubleValue;
4058 137520 : } else if (key == toString(SUMO_ATTR_LCA_IMPATIENCE)) {
4059 0 : myImpatience = doubleValue;
4060 0 : myMinImpatience = doubleValue;
4061 137520 : } else if (key == toString(SUMO_ATTR_LCA_TIME_TO_IMPATIENCE)) {
4062 0 : myTimeToImpatience = doubleValue;
4063 137520 : } else if (key == toString(SUMO_ATTR_LCA_ACCEL_LAT)) {
4064 0 : myAccelLat = doubleValue;
4065 137520 : } else if (key == toString(SUMO_ATTR_LCA_TURN_ALIGNMENT_DISTANCE)) {
4066 0 : myTurnAlignmentDist = doubleValue;
4067 137520 : } else if (key == toString(SUMO_ATTR_LCA_LOOKAHEADLEFT)) {
4068 0 : myLookaheadLeft = doubleValue;
4069 137520 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAINRIGHT)) {
4070 0 : mySpeedGainRight = doubleValue;
4071 137520 : } else if (key == toString(SUMO_ATTR_LCA_LANE_DISCIPLINE)) {
4072 0 : myLaneDiscipline = doubleValue;
4073 137520 : } else if (key == toString(SUMO_ATTR_LCA_SIGMA)) {
4074 0 : mySigma = doubleValue;
4075 137520 : } else if (key == toString(SUMO_ATTR_LCA_KEEPRIGHT_ACCEPTANCE_TIME)) {
4076 0 : myKeepRightAcceptanceTime = doubleValue;
4077 137520 : } else if (key == toString(SUMO_ATTR_LCA_OVERTAKE_DELTASPEED_FACTOR)) {
4078 0 : myOvertakeDeltaSpeedFactor = doubleValue;
4079 137520 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_LOOKAHEAD)) {
4080 68760 : mySpeedGainLookahead = doubleValue;
4081 68760 : } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAIN_REMAIN_TIME)) {
4082 68760 : mySpeedGainRemainTime = doubleValue;
4083 0 : } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_ROUNDABOUT)) {
4084 0 : myRoundaboutBonus = doubleValue;
4085 0 : } else if (key == toString(SUMO_ATTR_LCA_COOPERATIVE_SPEED)) {
4086 0 : myCooperativeSpeed = doubleValue;
4087 0 : } else if (key == toString(SUMO_ATTR_LCA_MAXSPEEDLATSTANDING)) {
4088 0 : myMaxSpeedLatStanding = doubleValue;
4089 0 : } else if (key == toString(SUMO_ATTR_LCA_MAXSPEEDLATFACTOR)) {
4090 0 : myMaxSpeedLatFactor = doubleValue;
4091 0 : } else if (key == toString(SUMO_ATTR_LCA_MAXDISTLATSTANDING)) {
4092 0 : myMaxDistLatStanding = doubleValue;
4093 : // access to internal state
4094 0 : } else if (key == "speedGainProbabilityRight") {
4095 0 : mySpeedGainProbabilityRight = doubleValue;
4096 0 : } else if (key == "speedGainProbabilityLeft") {
4097 0 : mySpeedGainProbabilityLeft = doubleValue;
4098 0 : } else if (key == "keepRightProbability") {
4099 0 : myKeepRightProbability = doubleValue;
4100 0 : } else if (key == "lookAheadSpeed") {
4101 0 : myLookAheadSpeed = doubleValue;
4102 0 : } else if (key == "sigmaState") {
4103 0 : mySigmaState = doubleValue;
4104 : } else {
4105 0 : throw InvalidArgument("Setting parameter '" + key + "' is not supported for laneChangeModel of type '" + toString(myModel) + "'");
4106 : }
4107 277340 : initDerivedParameters();
4108 277340 : }
4109 :
4110 :
4111 : int
4112 3259 : MSLCM_SL2015::wantsChange(
4113 : int laneOffset,
4114 : MSAbstractLaneChangeModel::MSLCMessager& /* msgPass */,
4115 : int blocked,
4116 : const std::pair<MSVehicle*, double>& leader,
4117 : const std::pair<MSVehicle*, double>& follower,
4118 : const std::pair<MSVehicle*, double>& neighLead,
4119 : const std::pair<MSVehicle*, double>& neighFollow,
4120 : const MSLane& neighLane,
4121 : const std::vector<MSVehicle::LaneQ>& preb,
4122 : MSVehicle** lastBlocked,
4123 : MSVehicle** firstBlocked) {
4124 :
4125 : const LaneChangeAction alternatives = LCA_NONE; // @todo pas this data
4126 :
4127 : #ifdef DEBUG_WANTSCHANGE
4128 : if (DEBUG_COND) {
4129 : std::cout << "\nWANTS_CHANGE\n" << SIMTIME
4130 : //<< std::setprecision(10)
4131 : << " veh=" << myVehicle.getID()
4132 : << " lane=" << myVehicle.getLane()->getID()
4133 : << " neigh=" << neighLane.getID()
4134 : << " pos=" << myVehicle.getPositionOnLane()
4135 : << " posLat=" << myVehicle.getLateralPositionOnLane()
4136 : << " speed=" << myVehicle.getSpeed()
4137 : << " considerChangeTo=" << (laneOffset == -1 ? "right" : "left")
4138 : << "\n";
4139 : }
4140 : #endif
4141 :
4142 3259 : double latDist = 0;
4143 3259 : const double laneWidth = myVehicle.getLane()->getWidth();
4144 3259 : MSLeaderDistanceInfo leaders(leader, laneWidth);
4145 3259 : MSLeaderDistanceInfo followers(follower, laneWidth);
4146 3259 : MSLeaderDistanceInfo blockers(std::make_pair((MSVehicle*)nullptr, -1), laneWidth);
4147 3259 : MSLeaderDistanceInfo neighLeaders(neighLead, laneWidth);
4148 3259 : MSLeaderDistanceInfo neighFollowers(neighFollow, laneWidth);
4149 3259 : MSLeaderDistanceInfo neighBlockers(std::make_pair((MSVehicle*)nullptr, -1), laneWidth);
4150 :
4151 : double maneuverDist;
4152 3259 : int result = _wantsChangeSublane(laneOffset,
4153 : alternatives,
4154 : leaders, followers, blockers,
4155 : neighLeaders, neighFollowers, neighBlockers,
4156 : neighLane, preb,
4157 : lastBlocked, firstBlocked, latDist, maneuverDist, blocked);
4158 :
4159 3259 : myCanChangeFully = true;
4160 : // ignore sublane motivation
4161 3259 : result &= ~LCA_SUBLANE;
4162 3259 : result |= getLCA(result, latDist);
4163 :
4164 : #if defined(DEBUG_WANTSCHANGE) || defined(DEBUG_STATE)
4165 : if (DEBUG_COND) {
4166 : if (result & LCA_WANTS_LANECHANGE) {
4167 : std::cout << SIMTIME
4168 : << " veh=" << myVehicle.getID()
4169 : << " wantsChangeTo=" << (laneOffset == -1 ? "right" : "left")
4170 : << ((result & LCA_URGENT) ? " (urgent)" : "")
4171 : << ((result & LCA_CHANGE_TO_HELP) ? " (toHelp)" : "")
4172 : << ((result & LCA_STRATEGIC) ? " (strat)" : "")
4173 : << ((result & LCA_COOPERATIVE) ? " (coop)" : "")
4174 : << ((result & LCA_SPEEDGAIN) ? " (speed)" : "")
4175 : << ((result & LCA_KEEPRIGHT) ? " (keepright)" : "")
4176 : << ((result & LCA_TRACI) ? " (traci)" : "")
4177 : << ((blocked & LCA_BLOCKED) ? " (blocked)" : "")
4178 : << ((blocked & LCA_OVERLAPPING) ? " (overlap)" : "")
4179 : << "\n\n\n";
4180 : }
4181 : }
4182 : #endif
4183 :
4184 3259 : return result;
4185 3259 : }
4186 :
4187 :
4188 : double
4189 138302490 : MSLCM_SL2015::getLeftBorder(bool checkOpposite) const {
4190 138302490 : return (myVehicle.getLane()->getEdge().getWidth()
4191 138302490 : + ((myVehicle.getLane()->getParallelOpposite() != nullptr && checkOpposite) ? myVehicle.getLane()->getParallelOpposite()->getEdge().getWidth() : 0));
4192 : }
4193 :
4194 : double
4195 139681590 : MSLCM_SL2015::getVehicleCenter() const {
4196 139681590 : if (isOpposite()) {
4197 152675 : return myVehicle.getEdge()->getWidth() + myVehicle.getLane()->getWidth() * 0.5 - myVehicle.getLateralPositionOnLane();
4198 : } else {
4199 139528915 : return myVehicle.getCenterOnEdge();
4200 : }
4201 : }
4202 :
4203 : double
4204 67198923 : MSLCM_SL2015::getNeighRight(const MSLane& neighLane) const {
4205 67198923 : if (isOpposite()) {
4206 127929 : return myVehicle.getLane()->getRightSideOnEdge() - neighLane.getWidth() + 2 * myVehicle.getLateralPositionOnLane();
4207 67070994 : } else if ((&myVehicle.getLane()->getEdge() != &neighLane.getEdge())) {
4208 113747 : return myVehicle.getLane()->getRightSideOnEdge() + myVehicle.getLane()->getWidth();
4209 : } else {
4210 : // the normal case
4211 66957247 : return neighLane.getRightSideOnEdge();
4212 : }
4213 : }
4214 :
4215 :
4216 : bool
4217 8526954 : MSLCM_SL2015::preventSliding(double maneuverDist) const {
4218 : // prevent wide maneuvers with unsufficient forward space
4219 8526954 : if (fabs(maneuverDist) > myMaxDistLatStanding) {
4220 : // emergency vehicles should not be restricted (TODO solve this with LCA_URGENT)
4221 8168496 : if (myVehicle.getVehicleType().getVehicleClass() == SVC_EMERGENCY) {
4222 : return false;
4223 : }
4224 8140714 : const double brakeGap = myVehicle.getCarFollowModel().brakeGap(myVehicle.getSpeed());
4225 8140714 : const bool isSlide = fabs(maneuverDist) > myMaxDistLatStanding + brakeGap * fabs(myMaxSpeedLatFactor);
4226 : #ifdef DEBUG_SLIDING
4227 : if (gDebugFlag2) {
4228 : std::cout << SIMTIME << " veh=" << myVehicle.getID() << " bgap=" << brakeGap << " maneuverDist=" << maneuverDist
4229 : << " mds=" << myMaxDistLatStanding << " isSlide=" << isSlide << "\n";
4230 : }
4231 : #endif
4232 8140714 : return isSlide;
4233 : }
4234 : return false;
4235 : }
4236 :
4237 : bool
4238 : MSLCM_SL2015::wantsKeepRight(double keepRightProb) const {
4239 9598486 : return keepRightProb * myKeepRightParam > MAX2(myChangeProbThresholdRight, mySpeedGainProbabilityLeft);
4240 : }
4241 :
4242 :
4243 : bool
4244 53760 : MSLCM_SL2015::saveBlockerLength(double length, double foeLeftSpace) {
4245 53760 : const bool canReserve = MSLCHelper::canSaveBlockerLength(myVehicle, length, myLeftSpace);
4246 53760 : if (!isOpposite() && (canReserve || myLeftSpace > foeLeftSpace)) {
4247 22617 : myLeadingBlockerLength = MAX2(length, myLeadingBlockerLength);
4248 22617 : if (myLeftSpace == 0 && foeLeftSpace < 0) {
4249 : // called from opposite overtaking, myLeftSpace must be initialized
4250 4238 : myLeftSpace = myVehicle.getBestLanes()[myVehicle.getLane()->getIndex()].length - myVehicle.getPositionOnLane();
4251 : }
4252 22617 : return true;
4253 : } else {
4254 : return false;
4255 : }
4256 : }
4257 :
4258 :
4259 : bool
4260 29571315 : MSLCM_SL2015::outsideEdge() const {
4261 29571315 : return myVehicle.getLeftSideOnEdge() < 0 || myVehicle.getRightSideOnEdge() > myVehicle.getLane()->getEdge().getWidth();
4262 : }
4263 : /****************************************************************************/
|
