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