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