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