Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2002-2025 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 MSLaneChanger.cpp
15 : /// @author Christian Roessel
16 : /// @author Daniel Krajzewicz
17 : /// @author Laura Bieker
18 : /// @author Michael Behrisch
19 : /// @author Friedemann Wesner
20 : /// @author Jakob Erdmann
21 : /// @date Fri, 01 Feb 2002
22 : ///
23 : // Performs lane changing of vehicles
24 : /****************************************************************************/
25 : #include <config.h>
26 :
27 : #include "MSLaneChanger.h"
28 : #include "MSNet.h"
29 : #include "MSLink.h"
30 : #include "MSVehicle.h"
31 : #include "MSVehicleType.h"
32 : #include "MSVehicleTransfer.h"
33 : #include "MSStop.h"
34 : #include "MSGlobals.h"
35 : #include <cassert>
36 : #include <iterator>
37 : #include <cstdlib>
38 : #include <cmath>
39 : #include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
40 : #include <microsim/transportables/MSTransportableControl.h>
41 : #include <microsim/transportables/MSPModel.h>
42 : #include <utils/common/MsgHandler.h>
43 :
44 : #define OPPOSITE_OVERTAKING_SAFE_TIMEGAP 0.0
45 : #define OPPOSITE_OVERTAKING_SAFETYGAP_HEADWAY_FACTOR 0.0
46 : #define OPPOSITE_OVERTAKING_SAFETY_FACTOR 1.2
47 : // XXX maxLookAhead should be higher if all leaders are stopped and lower when they are jammed/queued
48 : #define OPPOSITE_OVERTAKING_MAX_LOOKAHEAD 150.0 // just a guess
49 : #define OPPOSITE_OVERTAKING_MAX_LOOKAHEAD_EMERGENCY 1000.0 // just a guess
50 : // this is used for finding oncoming vehicles while driving in the opposite direction
51 : #define OPPOSITE_OVERTAKING_ONCOMING_LOOKAHEAD 1000.0 // just a guess
52 : // do not attempt overtaking maneuvers that would exceed this distance
53 : #define OPPOSITE_OVERTAKING_MAX_SPACE_TO_OVERTAKE 1000.0 // just a guess
54 : #define OPPOSITE_OVERTAKING_HILLTOP_THRESHOHOLD 5 // (m)
55 :
56 : // halting time to signal yielding in deadlock
57 : #define OPPOSITE_OVERTAKING_DEADLOCK_WAIT 1 // (s)
58 :
59 : // ===========================================================================
60 : // debug defines
61 : // ===========================================================================
62 :
63 : //#define DEBUG_CONTINUE_CHANGE
64 : //#define DEBUG_CHECK_CHANGE
65 : //#define DEBUG_SURROUNDING_VEHICLES // debug getRealFollower() and getRealLeader()
66 : //#define DEBUG_CHANGE_OPPOSITE
67 : //#define DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
68 : //#define DEBUG_CHANGE_OPPOSITE_DEADLOCK
69 : //#define DEBUG_ACTIONSTEPS
70 : //#define DEBUG_STATE
71 : //#define DEBUG_CANDIDATE
72 : //#define DEBUG_COND (vehicle->getLaneChangeModel().debugVehicle())
73 : #define DEBUG_COND (vehicle->isSelected())
74 : //#define DEBUG_COND (true)
75 :
76 :
77 :
78 : // ===========================================================================
79 : // ChangeElem member method definitions
80 : // ===========================================================================
81 726177 : MSLaneChanger::ChangeElem::ChangeElem(MSLane* _lane) :
82 726177 : lead(nullptr),
83 726177 : lane(_lane),
84 726177 : hoppedVeh(nullptr),
85 726177 : lastBlocked(nullptr),
86 726177 : firstBlocked(nullptr),
87 726177 : lastStopped(nullptr),
88 726177 : ahead(_lane->getWidth()),
89 726177 : aheadNext(_lane->getWidth(), nullptr, 0.),
90 726177 : zipperDist(0),
91 726177 : lastBlockedBackPos(-1),
92 726177 : lastBlockedWaitingTime(-1) {
93 726177 : if (lane->isInternal()) {
94 443282 : for (auto ili : lane->getIncomingLanes()) {
95 214278 : if (ili.viaLink->getState() == LINKSTATE_ZIPPER) {
96 308 : zipperDist = lane->getLength();
97 308 : break;
98 : }
99 : }
100 : } else {
101 1164706 : for (const MSLink* link : lane->getLinkCont()) {
102 667841 : if (link->getState() == LINKSTATE_ZIPPER) {
103 920 : zipperDist = MAX2(zipperDist, link->getFoeVisibilityDistance());
104 : // @note: if this lane is shorter than zipperDist it would be better to extend this to any upstream edges within the
105 : // visibility distance of the zipper link
106 : }
107 : }
108 : }
109 726177 : }
110 :
111 : void
112 1236531 : MSLaneChanger::ChangeElem::registerHop(MSVehicle* vehicle) {
113 : //std::cout << SIMTIME << " registerHop lane=" << lane->getID() << " veh=" << vehicle->getID() << "\n";
114 1236531 : lane->myTmpVehicles.insert(lane->myTmpVehicles.begin(), vehicle);
115 1236531 : dens += vehicle->getVehicleType().getLengthWithGap();
116 1236531 : hoppedVeh = vehicle;
117 1236531 : }
118 :
119 :
120 : // ===========================================================================
121 : // member method definitions
122 : // ===========================================================================
123 403687 : MSLaneChanger::MSLaneChanger(const std::vector<MSLane*>* lanes, bool allowChanging) :
124 403687 : myAllowsChanging(allowChanging),
125 403687 : myChangeToOpposite(lanes->front()->getEdge().canChangeToOpposite()) {
126 :
127 : // Fill the changer with the lane-data.
128 403687 : myChanger.reserve(lanes->size());
129 1129864 : for (std::vector<MSLane*>::const_iterator lane = lanes->begin(); lane != lanes->end(); ++lane) {
130 1452354 : myChanger.push_back(ChangeElem(*lane));
131 726177 : myChanger.back().mayChangeRight = lane != lanes->begin();
132 726177 : myChanger.back().mayChangeLeft = (lane + 1) != lanes->end();
133 726177 : if ((*lane)->isInternal()) {
134 : // avoid changing on internal sibling lane
135 229312 : if (myChanger.back().mayChangeRight && (*lane)->getLogicalPredecessorLane() == (*(lane - 1))->getLogicalPredecessorLane()) {
136 26869 : myChanger.back().mayChangeRight = false;
137 : }
138 229312 : if (myChanger.back().mayChangeLeft && (*lane)->getLogicalPredecessorLane() == (*(lane + 1))->getLogicalPredecessorLane()) {
139 26869 : myChanger.back().mayChangeLeft = false;
140 : }
141 : // avoid changing if lanes have different lengths
142 229312 : if (myChanger.back().mayChangeRight && (*lane)->getLength() != (*(lane - 1))->getLength()) {
143 : //std::cout << " cannot change right from lane=" << (*lane)->getID() << " len=" << (*lane)->getLength() << " to=" << (*(lane - 1))->getID() << " len2=" << (*(lane - 1))->getLength() << "\n";
144 14785 : myChanger.back().mayChangeRight = false;
145 : }
146 229312 : if (myChanger.back().mayChangeLeft && (*lane)->getLength() != (*(lane + 1))->getLength()) {
147 : //std::cout << " cannot change left from lane=" << (*lane)->getID() << " len=" << (*lane)->getLength() << " to=" << (*(lane + 1))->getID() << " len2=" << (*(lane + 1))->getLength() << "\n";
148 14785 : myChanger.back().mayChangeLeft = false;
149 : }
150 : }
151 : }
152 403687 : }
153 :
154 :
155 624889 : MSLaneChanger::~MSLaneChanger() {
156 624889 : }
157 :
158 :
159 : void
160 40721162 : MSLaneChanger::laneChange(SUMOTime t) {
161 : // This is what happens in one timestep. After initialization of the
162 : // changer, each vehicle will try to change. After that the changer
163 : // needs an update to prevent multiple changes of one vehicle.
164 : // Finally, the change-result has to be given back to the lanes.
165 40721162 : initChanger();
166 : try {
167 437337508 : while (vehInChanger()) {
168 355895184 : const bool haveChanged = change();
169 355895184 : updateChanger(haveChanged);
170 : }
171 40721162 : updateLanes(t);
172 0 : } catch (const ProcessError&) {
173 : // clean up locks or the gui may hang
174 0 : for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
175 0 : ce->lane->releaseVehicles();
176 : }
177 0 : throw;
178 0 : }
179 40721162 : }
180 :
181 :
182 : void
183 40721162 : MSLaneChanger::initChanger() {
184 : // Prepare myChanger with a safe state.
185 122050999 : for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
186 81329837 : ce->lead = nullptr;
187 81329837 : ce->hoppedVeh = nullptr;
188 81329837 : ce->lastBlocked = nullptr;
189 81329837 : ce->firstBlocked = nullptr;
190 81329837 : ce->lastStopped = nullptr;
191 81329837 : ce->dens = 0;
192 81329837 : ce->lastBlockedBackPos = -1;
193 81329837 : ce->lastBlockedWaitingTime = -1;
194 81329837 : ce->lane->getVehiclesSecure();
195 :
196 : //std::cout << SIMTIME << " initChanger lane=" << ce->lane->getID() << " vehicles=" << toString(ce->lane->myVehicles) << "\n";
197 : }
198 40721162 : }
199 :
200 :
201 : void
202 355895184 : MSLaneChanger::updateChanger(bool vehHasChanged) {
203 : assert(veh(myCandi) != 0);
204 :
205 : // "Push" the vehicles to the back, i.e. follower becomes vehicle,
206 : // vehicle becomes leader, and leader becomes predecessor of vehicle,
207 : // if it exists.
208 355895184 : if (!vehHasChanged) {
209 : //std::cout << SIMTIME << " updateChanger: lane=" << myCandi->lane->getID() << " has new lead=" << veh(myCandi)->getID() << "\n";
210 354815778 : myCandi->lead = veh(myCandi);
211 : }
212 :
213 355895184 : MSLane::VehCont& vehicles = myCandi->lane->myVehicles;
214 : vehicles.pop_back();
215 : //std::cout << SIMTIME << " updateChanger lane=" << myCandi->lane->getID() << " vehicles=" << toString(myCandi->lane->myVehicles) << "\n";
216 355895184 : }
217 :
218 :
219 : void
220 40721162 : MSLaneChanger::updateLanes(SUMOTime t) {
221 :
222 : // Update the lane's vehicle-container.
223 : // First: it is bad style to change other classes members, but for
224 : // this release, other attempts were too time-consuming. In a next
225 : // release we will change from this lane-centered design to a vehicle-
226 : // centered. This will solve many problems.
227 : // Second: this swap would be faster if vehicle-containers would have
228 : // been pointers, but then I had to change too much of the MSLane code.
229 122050999 : for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
230 : //std::cout << SIMTIME << " updateLanes lane=" << ce->lane->getID() << " myVehicles=" << toString(ce->lane->myVehicles) << " myTmpVehicles=" << toString(ce->lane->myTmpVehicles) << "\n";
231 81329837 : ce->lane->swapAfterLaneChange(t);
232 81329837 : ce->lane->releaseVehicles();
233 81329837 : if (ce->lastBlocked != nullptr) {
234 1523382 : ce->lastBlockedBackPos = ce->lastBlocked->getBackPositionOnLane();
235 1523382 : ce->lastBlockedWaitingTime = ce->lastBlocked->getWaitingTime();
236 : }
237 : }
238 40721162 : }
239 :
240 :
241 : MSLaneChanger::ChangerIt
242 355895184 : MSLaneChanger::findCandidate() {
243 : // Find the vehicle in myChanger with the largest position. If there
244 : // is no vehicle in myChanger (shouldn't happen) , return myChanger.end().
245 : ChangerIt max = myChanger.end();
246 : #ifdef DEBUG_CANDIDATE
247 : std::cout << SIMTIME << " findCandidate() on edge " << myChanger.begin()->lane->getEdge().getID() << std::endl;
248 : #endif
249 :
250 1111150357 : for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
251 664003236 : if (veh(ce) == nullptr) {
252 91251937 : continue;
253 : }
254 : #ifdef DEBUG_CANDIDATE
255 : std::cout << " lane = " << ce->lane->getID() << "\n";
256 : std::cout << " check vehicle=" << veh(ce)->getID() << " pos=" << veh(ce)->getPositionOnLane() << " lane=" << ce->lane->getID() << " isFrontOnLane=" << veh(ce)->isFrontOnLane(ce->lane) << "\n";
257 : #endif
258 664003236 : if (max == myChanger.end()) {
259 : #ifdef DEBUG_CANDIDATE
260 : std::cout << " new max vehicle=" << veh(ce)->getID() << " pos=" << veh(ce)->getPositionOnLane() << " lane=" << ce->lane->getID() << " isFrontOnLane=" << veh(ce)->isFrontOnLane(ce->lane) << "\n";
261 : #endif
262 : max = ce;
263 355895184 : continue;
264 : }
265 : assert(veh(ce) != 0);
266 : assert(veh(max) != 0);
267 616216104 : if (veh(max)->getPositionOnLane() < veh(ce)->getPositionOnLane()) {
268 : #ifdef DEBUG_CANDIDATE
269 : std::cout << " new max vehicle=" << veh(ce)->getID() << " pos=" << veh(ce)->getPositionOnLane() << " lane=" << ce->lane->getID() << " isFrontOnLane=" << veh(ce)->isFrontOnLane(ce->lane) << " oldMaxPos=" << veh(max)->getPositionOnLane() << "\n";
270 : #endif
271 : max = ce;
272 : }
273 : }
274 : assert(max != myChanger.end());
275 : assert(veh(max) != 0);
276 355895184 : return max;
277 : }
278 :
279 :
280 : bool
281 1240200333 : MSLaneChanger::mayChange(int direction) const {
282 1240200333 : if (direction == 0) {
283 : return true;
284 : }
285 1157170174 : if (!myAllowsChanging) {
286 : return false;
287 : }
288 1155294766 : SUMOVehicleClass svc = veh(myCandi)->getVClass();
289 1155294766 : if (direction == -1) {
290 594288109 : return myCandi->mayChangeRight && (myCandi - 1)->lane->allowsVehicleClass(svc) && myCandi->lane->allowsChangingRight(svc);
291 561006657 : } else if (direction == 1) {
292 561006657 : return myCandi->mayChangeLeft && (myCandi + 1)->lane->allowsVehicleClass(svc) && myCandi->lane->allowsChangingLeft(svc);
293 : } else {
294 : return false;
295 : }
296 : }
297 :
298 :
299 : bool
300 254630885 : MSLaneChanger::change() {
301 : // Find change-candidate. If it is on an allowed lane, try to change
302 : // to the right (there is a rule in Germany that you have to change
303 : // to the right, unless you are overtaking). If change to the right
304 : // isn't possible, check if there is a possibility to overtake (on the
305 : // left.
306 : // If candidate isn't on an allowed lane, changing to an allowed has
307 : // priority.
308 :
309 : #ifdef DEBUG_ACTIONSTEPS
310 : // std::cout<< "\nCHANGE" << std::endl;
311 : #endif
312 :
313 :
314 254630885 : myCandi = findCandidate();
315 : MSVehicle* vehicle = veh(myCandi);
316 254630885 : vehicle->getLaneChangeModel().clearNeighbors();
317 :
318 254630885 : if (vehicle->getLaneChangeModel().isChangingLanes() && !vehicle->getLaneChangeModel().alreadyChanged()) {
319 542341 : return continueChange(vehicle, myCandi);
320 : }
321 254088544 : if (!myAllowsChanging || vehicle->getLaneChangeModel().alreadyChanged() || vehicle->isStoppedOnLane()) {
322 1518658 : registerUnchanged(vehicle);
323 1518658 : if (vehicle->isStoppedOnLane()) {
324 1312028 : myCandi->lastStopped = vehicle;
325 : }
326 1518658 : return false;
327 : }
328 :
329 252569886 : if (!vehicle->isActive()) {
330 : #ifdef DEBUG_ACTIONSTEPS
331 : if (DEBUG_COND) {
332 : std::cout << SIMTIME << " veh '" << vehicle->getID() << "' skips regular change checks." << std::endl;
333 : }
334 : #endif
335 : bool changed = false;
336 20905741 : const int oldstate = vehicle->getLaneChangeModel().getOwnState();
337 : // let TraCI influence the wish to change lanes during non-actionsteps
338 20905741 : checkTraCICommands(vehicle);
339 20905741 : if (oldstate != vehicle->getLaneChangeModel().getOwnState()) {
340 5 : changed = applyTraCICommands(vehicle);
341 : }
342 5 : if (!changed) {
343 20905736 : registerUnchanged(vehicle);
344 : }
345 20905741 : return changed;
346 : }
347 :
348 : // Check for changes to the opposite lane if vehicle is active
349 231664145 : std::pair<MSVehicle* const, double> leader = getRealLeader(myCandi);
350 231664145 : if (myChanger.size() == 1 || vehicle->getLaneChangeModel().isOpposite() || (!mayChange(-1) && !mayChange(1))) {
351 24839089 : if (changeOpposite(vehicle, leader, myCandi->lastStopped)) {
352 : return true;
353 : }
354 24800180 : registerUnchanged(vehicle);
355 24800180 : return false;
356 : }
357 :
358 206825056 : vehicle->updateBestLanes(); // needed?
359 711885026 : for (int i = 0; i < (int) myChanger.size(); ++i) {
360 505059970 : vehicle->adaptBestLanesOccupation(i, myChanger[i].dens);
361 : }
362 :
363 206825056 : const std::vector<MSVehicle::LaneQ>& preb = vehicle->getBestLanes();
364 : // check whether the vehicle wants and is able to change to right lane
365 : int stateRight = 0;
366 206825056 : if (mayChange(-1)) {
367 112591161 : stateRight = checkChangeWithinEdge(-1, leader, preb);
368 : // change if the vehicle wants to and is allowed to change
369 112591161 : if ((stateRight & LCA_RIGHT) != 0 && (stateRight & LCA_BLOCKED) == 0) {
370 304798 : vehicle->getLaneChangeModel().setOwnState(stateRight);
371 304798 : return startChange(vehicle, myCandi, -1);
372 : }
373 112286363 : if ((stateRight & LCA_RIGHT) != 0 && (stateRight & LCA_URGENT) != 0) {
374 2379185 : (myCandi - 1)->lastBlocked = vehicle;
375 2379185 : if ((myCandi - 1)->firstBlocked == nullptr) {
376 520628 : (myCandi - 1)->firstBlocked = vehicle;
377 : }
378 : }
379 : }
380 :
381 : // check whether the vehicle wants and is able to change to left lane
382 : int stateLeft = 0;
383 206520258 : if (mayChange(1)) {
384 122566240 : stateLeft = checkChangeWithinEdge(1, leader, preb);
385 : // change if the vehicle wants to and is allowed to change
386 122566240 : if ((stateLeft & LCA_LEFT) != 0 && (stateLeft & LCA_BLOCKED) == 0) {
387 389393 : vehicle->getLaneChangeModel().setOwnState(stateLeft);
388 389393 : return startChange(vehicle, myCandi, 1);
389 : }
390 122176847 : if ((stateLeft & LCA_LEFT) != 0 && (stateLeft & LCA_URGENT) != 0) {
391 1652369 : (myCandi + 1)->lastBlocked = vehicle;
392 1652369 : if ((myCandi + 1)->firstBlocked == nullptr) {
393 452488 : (myCandi + 1)->firstBlocked = vehicle;
394 : }
395 : }
396 : }
397 :
398 206130865 : if ((stateRight & LCA_URGENT) != 0 && (stateLeft & LCA_URGENT) != 0) {
399 : // ... wants to go to the left AND to the right
400 : // just let them go to the right lane...
401 : stateLeft = 0;
402 : }
403 206130865 : vehicle->getLaneChangeModel().setOwnState(stateRight | stateLeft);
404 :
405 : // only emergency vehicles should change to the opposite side on a
406 : // multi-lane road (or vehicles that need to stop on the opposite side)
407 206130865 : if ((vehicle->getVClass() == SVC_EMERGENCY
408 206126203 : || hasOppositeStop(vehicle))
409 206130920 : && changeOpposite(vehicle, leader, myCandi->lastStopped)) {
410 40 : return true;
411 : }
412 :
413 206130825 : registerUnchanged(vehicle);
414 : return false;
415 : }
416 :
417 :
418 : void
419 354239039 : MSLaneChanger::registerUnchanged(MSVehicle* vehicle) {
420 : //std::cout << SIMTIME << " registerUnchanged lane=" << myCandi->lane->getID() << " veh=" << vehicle->getID() << "\n";
421 708478078 : myCandi->lane->myTmpVehicles.insert(myCandi->lane->myTmpVehicles.begin(), veh(myCandi));
422 354239039 : myCandi->dens += vehicle->getVehicleType().getLengthWithGap();
423 354239039 : vehicle->getLaneChangeModel().unchanged();
424 354239039 : }
425 :
426 :
427 :
428 : void
429 41315748 : MSLaneChanger::checkTraCICommands(MSVehicle* vehicle) {
430 : #ifdef DEBUG_STATE
431 : const int oldstate = vehicle->getLaneChangeModel().getOwnState();
432 : #endif
433 41315748 : vehicle->getLaneChangeModel().checkTraCICommands();
434 : #ifdef DEBUG_STATE
435 : if (DEBUG_COND) {
436 : const int newstate = vehicle->getLaneChangeModel().getOwnState();
437 : std::cout << SIMTIME
438 : << " veh=" << vehicle->getID()
439 : << " oldState=" << toString((LaneChangeAction) oldstate)
440 : << " newState=" << toString((LaneChangeAction) newstate)
441 : << ((newstate & LCA_BLOCKED) != 0 ? " (blocked)" : "")
442 : << ((newstate & LCA_OVERLAPPING) != 0 ? " (overlap)" : "")
443 : << "\n";
444 : }
445 : #endif
446 41315748 : }
447 :
448 :
449 : bool
450 5 : MSLaneChanger::applyTraCICommands(MSVehicle* vehicle) {
451 : // Execute request if not blocked
452 : bool changed = false;
453 5 : const int state = vehicle->getLaneChangeModel().getOwnState();
454 5 : const int dir = (state & LCA_RIGHT) != 0 ? -1 : ((state & LCA_LEFT) != 0 ? 1 : 0);
455 5 : const bool execute = dir != 0 && ((state & LCA_BLOCKED) == 0);
456 : if (execute) {
457 : ChangerIt to = myCandi + dir;
458 5 : bool continuous = vehicle->getLaneChangeModel().startLaneChangeManeuver(myCandi->lane, to->lane, dir);
459 5 : if (continuous) {
460 0 : changed = continueChange(vehicle, myCandi);
461 : } else {
462 : // insert vehicle into target lane
463 5 : to->registerHop(vehicle);
464 : changed = true;
465 : }
466 : }
467 5 : return changed;
468 : }
469 :
470 :
471 : bool
472 694191 : MSLaneChanger::startChange(MSVehicle* vehicle, ChangerIt& from, int direction) {
473 694191 : if (vehicle->isRemoteControlled()) {
474 47 : registerUnchanged(vehicle);
475 47 : return false;
476 : }
477 : ChangerIt to = from + direction;
478 : // @todo delay entering the target lane until the vehicle intersects it
479 : // physically (considering lane width and vehicle width)
480 : //if (to->lane->getID() == "beg_1") std::cout << SIMTIME << " startChange to lane=" << to->lane->getID() << " myTmpVehiclesBefore=" << toString(to->lane->myTmpVehicles) << "\n";
481 694144 : const bool continuous = vehicle->getLaneChangeModel().startLaneChangeManeuver(from->lane, to->lane, direction);
482 694144 : if (continuous) {
483 34423 : return continueChange(vehicle, myCandi);
484 : } else {
485 659721 : to->registerHop(vehicle);
486 659721 : to->lane->requireCollisionCheck();
487 659721 : return true;
488 : }
489 : }
490 :
491 : bool
492 579396 : MSLaneChanger::continueChange(MSVehicle* vehicle, ChangerIt& from) {
493 579396 : MSAbstractLaneChangeModel& lcm = vehicle->getLaneChangeModel();
494 579396 : const int direction = lcm.isOpposite() ? -lcm.getLaneChangeDirection() : lcm.getLaneChangeDirection();
495 579396 : const bool pastMidpoint = lcm.updateCompletion(); // computes lcm.mySpeedLat as a side effect
496 579396 : const double speedLat = lcm.isOpposite() ? -lcm.getSpeedLat() : lcm.getSpeedLat();
497 579396 : vehicle->myState.myPosLat += SPEED2DIST(speedLat);
498 579396 : vehicle->myCachedPosition = Position::INVALID;
499 : //std::cout << SIMTIME << " veh=" << vehicle->getID() << " dir=" << direction << " pm=" << pastMidpoint << " speedLat=" << speedLat << " posLat=" << vehicle->myState.myPosLat << "\n";
500 579396 : if (pastMidpoint) {
501 36777 : MSLane* source = myCandi->lane;
502 36777 : MSLane* target = source->getParallelLane(direction);
503 36777 : vehicle->myState.myPosLat -= direction * 0.5 * (source->getWidth() + target->getWidth());
504 36777 : lcm.primaryLaneChanged(source, target, direction);
505 36777 : if (&source->getEdge() == &target->getEdge()) {
506 : ChangerIt to = from + direction;
507 34186 : to->registerHop(vehicle);
508 : }
509 : target->requireCollisionCheck();
510 : } else {
511 542619 : from->registerHop(vehicle);
512 542619 : from->lane->requireCollisionCheck();
513 : }
514 579396 : if (!lcm.isChangingLanes()) {
515 33621 : vehicle->myState.myPosLat = 0;
516 33621 : lcm.endLaneChangeManeuver();
517 : }
518 579396 : lcm.updateShadowLane();
519 579396 : if (lcm.getShadowLane() != nullptr && &lcm.getShadowLane()->getEdge() == &vehicle->getLane()->getEdge()) {
520 : // set as hoppedVeh on the shadow lane so it is found as leader on both lanes
521 412029 : myChanger[lcm.getShadowLane()->getIndex()].hoppedVeh = vehicle;
522 : lcm.getShadowLane()->requireCollisionCheck();
523 : }
524 579396 : vehicle->myAngle = vehicle->computeAngle();
525 579396 : if (lcm.isOpposite()) {
526 19242 : vehicle->myAngle += M_PI;
527 : }
528 :
529 : #ifdef DEBUG_CONTINUE_CHANGE
530 : if (DEBUG_COND) {
531 : std::cout << SIMTIME
532 : << " continueChange veh=" << vehicle->getID()
533 : << " from=" << Named::getIDSecure(from->lane)
534 : << " dir=" << direction
535 : << " speedLat=" << speedLat
536 : << " pastMidpoint=" << pastMidpoint
537 : << " posLat=" << vehicle->getLateralPositionOnLane()
538 : << " completion=" << lcm.getLaneChangeCompletion()
539 : << " shadowLane=" << Named::getIDSecure(lcm.getShadowLane())
540 : //<< " shadowHopped=" << Named::getIDSecure(shadow->lane)
541 : << "\n";
542 : }
543 : #endif
544 579396 : return pastMidpoint && lcm.getShadowLane() == nullptr;
545 : }
546 :
547 :
548 : std::pair<MSVehicle* const, double>
549 466821546 : MSLaneChanger::getRealLeader(const ChangerIt& target) const {
550 : assert(veh(myCandi) != 0);
551 : MSVehicle* vehicle = veh(myCandi);
552 : #ifdef DEBUG_SURROUNDING_VEHICLES
553 : if (DEBUG_COND) {
554 : std::cout << SIMTIME << " veh '" << vehicle->getID() << "' looks for leader on lc-target lane '" << target->lane->getID() << "'." << std::endl;
555 : }
556 : #endif
557 : // get the leading vehicle on the lane to change to
558 466821546 : MSVehicle* neighLead = target->lead;
559 :
560 : #ifdef DEBUG_SURROUNDING_VEHICLES
561 : if (DEBUG_COND) {
562 : if (neighLead != 0) {
563 : std::cout << "Considering '" << neighLead->getID() << "' at position " << neighLead->getPositionOnLane() << std::endl;
564 : }
565 : }
566 : #endif
567 :
568 : // check whether the hopped vehicle became the leader
569 466821546 : if (target->hoppedVeh != nullptr) {
570 7157441 : double hoppedPos = target->hoppedVeh->getPositionOnLane();
571 : #ifdef DEBUG_SURROUNDING_VEHICLES
572 : if (DEBUG_COND) {
573 : std::cout << "Considering hopped vehicle '" << target->hoppedVeh->getID() << "' at position " << hoppedPos << std::endl;
574 : }
575 : #endif
576 7157441 : if (hoppedPos > vehicle->getPositionOnLane() && (neighLead == nullptr || neighLead->getPositionOnLane() > hoppedPos)) {
577 1032176 : neighLead = target->hoppedVeh;
578 : //if (vehicle->getID() == "flow.21") std::cout << SIMTIME << " neighLead=" << Named::getIDSecure(neighLead) << " (422)\n";
579 : }
580 : }
581 : // extra check for shared lane
582 466821546 : const bool checkBidi = target->lane->getBidiLane() != nullptr && target->lane->getBidiLane()->getVehicleNumberWithPartials() > 0;
583 466821546 : if (neighLead == nullptr || checkBidi) {
584 : #ifdef DEBUG_SURROUNDING_VEHICLES
585 : if (DEBUG_COND) {
586 : std::cout << "Looking for leader on consecutive lanes." << std::endl;
587 : }
588 : #endif
589 : // There's no leader on the target lane. Look for leaders on consecutive lanes.
590 : // (there might also be partial leaders due to continuous lane changing or bidiLane)
591 66862862 : MSLane* targetLane = target->lane;
592 : const double egoBack = vehicle->getBackPositionOnLane();
593 : double leaderBack = targetLane->getLength();
594 66862862 : if (neighLead != nullptr) {
595 240903 : leaderBack = neighLead->getBackPositionOnLane(targetLane);
596 : }
597 71643700 : for (MSVehicle* pl : targetLane->myPartialVehicles) {
598 4780838 : double plBack = pl->getBackPositionOnLane(targetLane);
599 4780838 : if (pl->isBidiOn(targetLane)) {
600 384303 : plBack -= pl->getVehicleType().getLengthWithGap();
601 : }
602 4780838 : const double plPos = plBack + pl->getVehicleType().getLength();
603 : #ifdef DEBUG_SURROUNDING_VEHICLES
604 : if (DEBUG_COND) {
605 : std::cout << " partial=" << pl->getID() << " plBack=" << plBack << " plPos=" << plPos << " leaderBack=" << leaderBack << " egoBack=" << egoBack << "\n";
606 : }
607 : #endif
608 4780838 : if (plBack < leaderBack && plPos + pl->getVehicleType().getMinGap() >= egoBack) {
609 : neighLead = pl;
610 : leaderBack = plBack;
611 : }
612 : }
613 66862862 : if (neighLead != nullptr) {
614 : #ifdef DEBUG_SURROUNDING_VEHICLES
615 : if (DEBUG_COND) {
616 : std::cout << " found leader=" << neighLead->getID() << " (partial)\n";
617 : }
618 : #endif
619 4095617 : const double gap = leaderBack - vehicle->getPositionOnLane() - vehicle->getVehicleType().getMinGap();
620 4095617 : return std::pair<MSVehicle*, double>(neighLead, gap);
621 : }
622 62767245 : double seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
623 62767245 : double speed = vehicle->getSpeed();
624 62767245 : double dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap();
625 62767245 : if (target->lane->getBidiLane() != nullptr) {
626 359949 : dist += target->lane->getBidiLane()->getMaximumBrakeDist();
627 : }
628 : // always check for link leaders while on an internal lane
629 62767245 : if (seen > dist && !myCandi->lane->isInternal()) {
630 : #ifdef DEBUG_SURROUNDING_VEHICLES
631 : if (DEBUG_COND) {
632 : std::cout << " found no leader within dist=" << dist << "\n";
633 : }
634 : #endif
635 31536120 : return std::pair<MSVehicle* const, double>(static_cast<MSVehicle*>(nullptr), -1);
636 : }
637 31231125 : const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation(targetLane);
638 :
639 31231125 : std::pair<MSVehicle* const, double> result = target->lane->getLeaderOnConsecutive(dist, seen, speed, *vehicle, bestLaneConts);
640 : #ifdef DEBUG_SURROUNDING_VEHICLES
641 : if (DEBUG_COND) {
642 : std::cout << " found consecutiveLeader=" << Named::getIDSecure(result.first) << "\n";
643 : }
644 : #endif
645 31231125 : return result;
646 : } else {
647 : #ifdef DEBUG_SURROUNDING_VEHICLES
648 : if (DEBUG_COND) {
649 : std::cout << " found leader=" << neighLead->getID() << "\n";
650 : }
651 : #endif
652 399958684 : return std::pair<MSVehicle* const, double>(neighLead, neighLead->getBackPositionOnLane(target->lane) - vehicle->getPositionOnLane() - vehicle->getVehicleType().getMinGap());
653 : }
654 : }
655 :
656 :
657 : std::pair<MSVehicle* const, double>
658 470314802 : MSLaneChanger::getRealFollower(const ChangerIt& target) const {
659 : assert(veh(myCandi) != 0);
660 : MSVehicle* vehicle = veh(myCandi);
661 : #ifdef DEBUG_SURROUNDING_VEHICLES
662 : if (DEBUG_COND) {
663 : std::cout << SIMTIME << " veh '" << vehicle->getID() << "' looks for follower on lc-target lane '" << target->lane->getID() << "'." << std::endl;
664 : }
665 : #endif
666 470314802 : const double candiPos = vehicle->getPositionOnLane();
667 : MSVehicle* neighFollow = nullptr;
668 470314802 : if (target != myCandi) {
669 : neighFollow = veh(target);
670 : } else {
671 : // veh(target) would return the ego vehicle so we use its predecessor instead
672 235157401 : if (target->lane->myVehicles.size() > 1) {
673 210988450 : neighFollow = target->lane->myVehicles[target->lane->myVehicles.size() - 2];
674 : }
675 : }
676 :
677 : #ifdef DEBUG_SURROUNDING_VEHICLES
678 : if (DEBUG_COND) {
679 : if (neighFollow != 0) {
680 : std::cout << "veh(target) returns '" << neighFollow->getID() << "' at position " << neighFollow->getPositionOnLane() << std::endl;
681 : } else {
682 : std::cout << "veh(target) returns none." << std::endl;
683 : }
684 : }
685 : #endif
686 :
687 :
688 : #ifdef DEBUG_SURROUNDING_VEHICLES
689 : if (DEBUG_COND) {
690 : if (getCloserFollower(candiPos, neighFollow, target->hoppedVeh) != neighFollow) {
691 : std::cout << "Hopped vehicle '" << target->hoppedVeh->getID() << "' at position " << target->hoppedVeh->getPositionOnLane() << " is closer." << std::endl;
692 : }
693 : }
694 : #endif
695 :
696 : // check whether the hopped vehicle became the follower
697 470314802 : neighFollow = getCloserFollower(candiPos, neighFollow, target->hoppedVeh);
698 :
699 :
700 : #ifdef DEBUG_SURROUNDING_VEHICLES
701 : if (DEBUG_COND) {
702 : MSVehicle* partialBehind = getCloserFollower(candiPos, neighFollow, target->lane->getPartialBehind(vehicle));
703 : if (partialBehind != 0 && partialBehind != neighFollow) {
704 : std::cout << "'Partial behind'-vehicle '" << target->lane->getPartialBehind(vehicle)->getID() << "' at position " << partialBehind->getPositionOnLane() << " is closer." << std::endl;
705 : }
706 : }
707 : #endif
708 : // or a follower which is partially lapping into the target lane
709 470314802 : neighFollow = getCloserFollower(candiPos, neighFollow, target->lane->getPartialBehind(vehicle));
710 :
711 470314802 : if (neighFollow == nullptr) {
712 59564229 : CLeaderDist consecutiveFollower = target->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true)[0];
713 : #ifdef DEBUG_SURROUNDING_VEHICLES
714 : if (DEBUG_COND) {
715 : if (consecutiveFollower.first == 0) {
716 : std::cout << "no follower found." << std::endl;
717 : } else {
718 : std::cout << "found follower '" << consecutiveFollower.first->getID() << "' on consecutive lanes." << std::endl;
719 : }
720 : }
721 : #endif
722 59564229 : return std::make_pair(const_cast<MSVehicle*>(consecutiveFollower.first), consecutiveFollower.second);
723 : } else {
724 : #ifdef DEBUG_SURROUNDING_VEHICLES
725 : if (DEBUG_COND) {
726 : std::cout << "found follower '" << neighFollow->getID() << "'." << std::endl;
727 : }
728 : #endif
729 : return std::pair<MSVehicle* const, double>(neighFollow,
730 410750573 : vehicle->getPositionOnLane() - vehicle->getVehicleType().getLength() - neighFollow->getPositionOnLane() - neighFollow->getVehicleType().getMinGap());
731 : }
732 : }
733 :
734 :
735 : MSVehicle*
736 940629604 : MSLaneChanger::getCloserFollower(const double maxPos, MSVehicle* follow1, MSVehicle* follow2) {
737 940629604 : if (follow1 == nullptr || follow1->getPositionOnLane() > maxPos) {
738 119431789 : return follow2;
739 821197815 : } else if (follow2 == nullptr || follow2->getPositionOnLane() > maxPos) {
740 819685822 : return follow1;
741 : } else {
742 1511993 : if (follow1->getPositionOnLane() > follow2->getPositionOnLane()) {
743 : return follow1;
744 : } else {
745 : return follow2;
746 : }
747 : }
748 : }
749 :
750 : int
751 235157401 : MSLaneChanger::checkChangeWithinEdge(
752 : int laneOffset,
753 : const std::pair<MSVehicle* const, double>& leader,
754 : const std::vector<MSVehicle::LaneQ>& preb) const {
755 :
756 235157401 : std::pair<MSVehicle*, double> follower = getRealFollower(myCandi);
757 235157401 : std::pair<MSVehicle* const, double> neighLead = getRealLeader(myCandi + laneOffset);
758 235157401 : std::pair<MSVehicle*, double> neighFollow = getRealFollower(myCandi + laneOffset);
759 235157401 : if (neighLead.first != nullptr && neighLead.first == neighFollow.first) {
760 : // vehicles should not be leader and follower at the same time to avoid
761 : // contradictory behavior
762 : neighFollow.first = 0;
763 : }
764 : ChangerIt target = myCandi + laneOffset;
765 235157401 : return checkChange(laneOffset, target->lane, leader, follower, neighLead, neighFollow, preb);
766 : }
767 :
768 : int
769 235614004 : MSLaneChanger::checkChange(
770 : int laneOffset,
771 : const MSLane* targetLane,
772 : const std::pair<MSVehicle* const, double>& leader,
773 : const std::pair<MSVehicle* const, double>& follower,
774 : const std::pair<MSVehicle* const, double>& neighLead,
775 : const std::pair<MSVehicle* const, double>& neighFollow,
776 : const std::vector<MSVehicle::LaneQ>& preb) const {
777 :
778 : MSVehicle* vehicle = veh(myCandi);
779 :
780 : #ifdef DEBUG_CHECK_CHANGE
781 : if (DEBUG_COND) {
782 : std::cout
783 : << "\n" << SIMTIME << " checkChange() for vehicle '" << vehicle->getID() << "'"
784 : << std::endl;
785 : }
786 : #endif
787 :
788 : int blocked = 0;
789 235614004 : int blockedByLeader = (laneOffset == -1 ? LCA_BLOCKED_BY_RIGHT_LEADER : LCA_BLOCKED_BY_LEFT_LEADER);
790 : int blockedByFollower = (laneOffset == -1 ? LCA_BLOCKED_BY_RIGHT_FOLLOWER : LCA_BLOCKED_BY_LEFT_FOLLOWER);
791 : // overlap
792 235614004 : if (neighFollow.first != nullptr && neighFollow.second < 0) {
793 67598151 : blocked |= (blockedByFollower | LCA_OVERLAPPING);
794 :
795 : // Debug (Leo)
796 : #ifdef DEBUG_CHECK_CHANGE
797 : if (DEBUG_COND) {
798 : std::cout << SIMTIME
799 : << " overlapping with follower..."
800 : << std::endl;
801 : }
802 : #endif
803 :
804 : }
805 235614004 : if (neighLead.first != nullptr && neighLead.second < 0) {
806 68487375 : blocked |= (blockedByLeader | LCA_OVERLAPPING);
807 :
808 : #ifdef DEBUG_CHECK_CHANGE
809 : if (DEBUG_COND) {
810 : std::cout << SIMTIME
811 : << " overlapping with leader..."
812 : << std::endl;
813 : }
814 : #endif
815 :
816 : }
817 235614004 : double secureFrontGap = MSAbstractLaneChangeModel::NO_NEIGHBOR;
818 : double secureBackGap = MSAbstractLaneChangeModel::NO_NEIGHBOR;
819 : double secureOrigFrontGap = MSAbstractLaneChangeModel::NO_NEIGHBOR;
820 :
821 235614004 : const double tauRemainder = vehicle->getActionStepLength() == DELTA_T ? 0 : MAX2(vehicle->getCarFollowModel().getHeadwayTime() - TS, 0.);
822 : // safe back gap
823 235614004 : if ((blocked & blockedByFollower) == 0 && neighFollow.first != nullptr) {
824 : // Calculate secure gap conservatively with vNextFollower / vNextLeader as
825 : // extrapolated speeds after the driver's expected reaction time (tau).
826 : // NOTE: there exists a possible source for collisions if the follower and the leader
827 : // have desynchronized action steps as the extrapolated speeds can be exceeded in this case
828 :
829 : // Expected reaction time (tau) for the follower-vehicle.
830 : // (subtracted TS since at this point the vehicles' states are already updated)
831 139009728 : const double vNextFollower = neighFollow.first->getSpeed() + MAX2(0., tauRemainder * neighFollow.first->getAcceleration());
832 139009728 : const double vNextLeader = vehicle->getSpeed() + MIN2(0., tauRemainder * vehicle->getAcceleration());
833 : // !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren
834 139009728 : secureBackGap = neighFollow.first->getCarFollowModel().getSecureGap(neighFollow.first, vehicle, vNextFollower,
835 : vNextLeader, vehicle->getCarFollowModel().getMaxDecel());
836 : #ifdef DEBUG_CHECK_CHANGE
837 : if (DEBUG_COND) {
838 : std::cout << SIMTIME
839 : << " follower=" << neighFollow.first->getID()
840 : << " backGap=" << neighFollow.second
841 : << " vNextFollower=" << vNextFollower
842 : << " vNextEgo=" << vNextLeader
843 : << " secureGap=" << secureBackGap
844 : << " safetyFactor=" << vehicle->getLaneChangeModel().getSafetyFactor()
845 : << " blocked=" << (neighFollow.second < secureBackGap * vehicle->getLaneChangeModel().getSafetyFactor())
846 : << "\n";
847 : }
848 : #endif
849 139009728 : if (neighFollow.second < secureBackGap * vehicle->getLaneChangeModel().getSafetyFactor()) {
850 85557658 : if (vehicle->getLaneChangeModel().isOpposite()
851 85557658 : && neighFollow.first->getLaneChangeModel().getLastLaneChangeOffset() == laneOffset) {
852 : // during opposite direction driving, the vehicle are handled in
853 : // downstream rather than upstream order, the neighFollower may have
854 : // been the actual follower in this simulation step and should not
855 : // block changing in this case
856 : #ifdef DEBUG_CHECK_CHANGE
857 : if (DEBUG_COND) {
858 : std::cout << " ignoring opposite follower who changed in this step\n";
859 : }
860 : #endif
861 : } else {
862 85557566 : blocked |= blockedByFollower;
863 : }
864 : }
865 : }
866 :
867 : // safe front gap
868 235614004 : if ((blocked & blockedByLeader) == 0 && neighLead.first != nullptr) {
869 : // Calculate secure gap conservatively with vNextFollower / vNextLeader as
870 : // extrapolated speeds after the driver's expected reaction time (tau).
871 : // NOTE: there exists a possible source for collisions if the follower and the leader
872 : // have desynchronized action steps as the extrapolated speeds can be exceeded in this case
873 :
874 : // Expected reaction time (tau) for the follower-vehicle.
875 : // (subtracted TS since at this point the vehicles' states are already updated)
876 137895202 : const double vNextFollower = vehicle->getSpeed() + MAX2(0., tauRemainder * vehicle->getAcceleration());
877 274024944 : const double vNextLeader = neighLead.first->getSpeed() + MIN2(0., tauRemainder * neighLead.first->getAcceleration());
878 : // !!! eigentlich: vsafe braucht die Max. Geschwindigkeit beider Spuren
879 137895202 : secureFrontGap = vehicle->getCarFollowModel().getSecureGap(vehicle, neighLead.first, vNextFollower,
880 137895202 : vNextLeader, neighLead.first->getCarFollowModel().getMaxDecel());
881 : #ifdef DEBUG_CHECK_CHANGE
882 : if (DEBUG_COND) {
883 : std::cout << SIMTIME
884 : << " leader=" << neighLead.first->getID()
885 : << " frontGap=" << neighLead.second
886 : << " vNextEgo=" << vNextFollower
887 : << " vNextLeader=" << vNextLeader
888 : << " secureGap=" << secureFrontGap
889 : << " safetyFactor=" << vehicle->getLaneChangeModel().getSafetyFactor()
890 : << " blocked=" << (neighLead.second < secureFrontGap * vehicle->getLaneChangeModel().getSafetyFactor())
891 : << "\n";
892 : }
893 : #endif
894 137895202 : if (neighLead.second < secureFrontGap * vehicle->getLaneChangeModel().getSafetyFactor()) {
895 77839516 : blocked |= blockedByLeader;
896 : }
897 : }
898 167126629 : if (blocked == 0 && targetLane->hasPedestrians()) {
899 100300 : PersonDist nextLeader = targetLane->nextBlocking(vehicle->getBackPositionOnLane(),
900 50150 : vehicle->getRightSideOnLane(targetLane), vehicle->getRightSideOnLane(targetLane) + vehicle->getVehicleType().getWidth(),
901 50150 : ceil(vehicle->getSpeed() / vehicle->getCarFollowModel().getMaxDecel()));
902 50150 : if (nextLeader.first != 0) {
903 2727 : const double brakeGap = vehicle->getCarFollowModel().brakeGap(vehicle->getSpeed());
904 : // returned gap value is relative to backPosition
905 2727 : const double gap = nextLeader.second - vehicle->getVehicleType().getLengthWithGap();
906 : #ifdef DEBUG_CHECK_CHANGE
907 : if (DEBUG_COND) {
908 : std::cout << SIMTIME << " pedestrian on road " + leader.first->getID() << " gap=" << gap << " brakeGap=" << brakeGap << "\n";
909 : }
910 : #endif
911 2727 : if (brakeGap > gap) {
912 2233 : blocked |= blockedByLeader;
913 : #ifdef DEBUG_CHECK_CHANGE
914 : if (DEBUG_COND) {
915 : std::cout << SIMTIME << " blocked by pedestrian " + leader.first->getID() << "\n";
916 : }
917 : #endif
918 : }
919 : }
920 : }
921 :
922 235614004 : if (leader.first != nullptr) {
923 219379954 : secureOrigFrontGap = vehicle->getCarFollowModel().getSecureGap(vehicle, leader.first, vehicle->getSpeed(), leader.first->getSpeed(), leader.first->getCarFollowModel().getMaxDecel());
924 : }
925 :
926 235614004 : MSAbstractLaneChangeModel::MSLCMessager msg(leader.first, neighLead.first, neighFollow.first);
927 235614004 : int state = blocked | vehicle->getLaneChangeModel().wantsChange(
928 235614004 : laneOffset, msg, blocked, leader, follower, neighLead, neighFollow, *targetLane, preb, &(myCandi->lastBlocked), &(myCandi->firstBlocked));
929 :
930 235614004 : if (blocked == 0 && (state & LCA_WANTS_LANECHANGE) != 0 && neighLead.first != nullptr) {
931 : // do a more careful (but expensive) check to ensure that a
932 : // safety-critical leader is not being overlooked
933 : // while changing on an intersection, it is not sufficient to abort the
934 : // search with a leader on the current lane because all linkLeaders must
935 : // be considered as well
936 572835 : const double seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
937 572835 : const double speed = vehicle->getSpeed();
938 572835 : const double dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap();
939 572835 : if (seen < dist || myCandi->lane->isInternal()) {
940 102242 : std::pair<MSVehicle* const, double> neighLead2 = targetLane->getCriticalLeader(dist, seen, speed, *vehicle);
941 102242 : if (neighLead2.first != nullptr && neighLead2.first != neighLead.first) {
942 17039 : const double secureGap = vehicle->getCarFollowModel().getSecureGap(vehicle, neighLead2.first, vehicle->getSpeed(),
943 17039 : neighLead2.first->getSpeed(), neighLead2.first->getCarFollowModel().getMaxDecel());
944 17039 : const double secureGap2 = secureGap * vehicle->getLaneChangeModel().getSafetyFactor();
945 : #ifdef DEBUG_SURROUNDING_VEHICLES
946 : if (DEBUG_COND) {
947 : std::cout << SIMTIME << " found critical leader=" << neighLead2.first->getID()
948 : << " gap=" << neighLead2.second << " secGap=" << secureGap << " secGap2=" << secureGap2 << "\n";
949 : }
950 : #endif
951 17039 : if (neighLead2.second < secureGap2) {
952 4994 : state |= blockedByLeader;
953 : }
954 : }
955 : }
956 : }
957 44224315 : if (blocked == 0 && (state & LCA_WANTS_LANECHANGE)) {
958 : // ensure that merging is safe for any upcoming zipper links after changing
959 : double targetZipperDist = 0;
960 800276 : if (laneOffset == 0) {
961 0 : targetZipperDist = myCandi->zipperDist;
962 800276 : } else if (laneOffset == 1) {
963 434464 : if ((myCandi + 1) != myChanger.end()) {
964 414952 : targetZipperDist = (myCandi + 1)->zipperDist;
965 : }
966 365812 : } else if (laneOffset == -1) {
967 365812 : if (myCandi > myChanger.begin()) {
968 346532 : targetZipperDist = (myCandi - 1)->zipperDist;
969 : }
970 : }
971 800276 : if (vehicle->unsafeLinkAhead(targetLane, targetZipperDist)) {
972 24192 : state |= blockedByLeader;
973 : }
974 : }
975 :
976 235614004 : if ((state & LCA_BLOCKED) == 0 && (state & LCA_WANTS_LANECHANGE) != 0 && MSGlobals::gLaneChangeDuration > DELTA_T) {
977 : // Ensure that a continuous lane change manoeuvre can be completed before the next turning movement.
978 : // Assume lateral position == 0. (If this should change in the future add + laneOffset*vehicle->getLateralPositionOnLane() to distToNeighLane)
979 74905 : const double distToNeighLane = 0.5 * (vehicle->getLane()->getWidth() + targetLane->getWidth());
980 : // Extrapolate the LC duration if operating with speed dependent lateral speed.
981 74905 : const MSAbstractLaneChangeModel& lcm = vehicle->getLaneChangeModel();
982 74905 : const double assumedDecel = lcm.getAssumedDecelForLaneChangeDuration();
983 74905 : const double estimatedLCDuration = lcm.estimateLCDuration(vehicle->getSpeed(), distToNeighLane, assumedDecel, (state & LCA_URGENT) != 0);
984 74905 : if (estimatedLCDuration == -1) {
985 : // Can't guarantee that LC will succeed if vehicle is braking -> assert(lcm.myMaxSpeedLatStanding==0)
986 : #ifdef DEBUG_CHECK_CHANGE
987 : if (DEBUG_COND) {
988 : std::cout << SIMTIME << " checkChange() too slow to guarantee completion of continuous lane change."
989 : << "\nestimatedLCDuration=" << estimatedLCDuration
990 : << "\ndistToNeighLane=" << distToNeighLane
991 : << std::endl;
992 : }
993 : #endif
994 25635 : state |= LCA_INSUFFICIENT_SPEED;
995 : } else {
996 : // Compute covered distance, when braking for the whole lc duration
997 49270 : const double decel = vehicle->getCarFollowModel().getMaxDecel() * estimatedLCDuration;
998 : const double avgSpeed = 0.5 * (
999 49270 : MAX2(0., vehicle->getSpeed() - ACCEL2SPEED(vehicle->getCarFollowModel().getMaxDecel())) +
1000 49270 : MAX2(0., vehicle->getSpeed() - decel));
1001 : // Distance required for lane change.
1002 49270 : const double space2change = avgSpeed * estimatedLCDuration;
1003 : // Available distance for LC maneuver (distance till next turn)
1004 49270 : double seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
1005 : #ifdef DEBUG_CHECK_CHANGE
1006 : if (DEBUG_COND) {
1007 : std::cout << SIMTIME << " checkChange() checking continuous lane change..."
1008 : << "\ndistToNeighLane=" << distToNeighLane
1009 : << " estimatedLCDuration=" << estimatedLCDuration
1010 : << " space2change=" << space2change
1011 : << " avgSpeed=" << avgSpeed
1012 : << std::endl;
1013 : }
1014 : #endif
1015 :
1016 : // for finding turns it doesn't matter whether we look along the current lane or the target lane
1017 49270 : const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation();
1018 : int view = 1;
1019 49270 : const MSLane* nextLane = vehicle->getLane();
1020 49270 : std::vector<MSLink*>::const_iterator link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
1021 53165 : while (!nextLane->isLinkEnd(link) && seen <= space2change) {
1022 3911 : if ((*link)->getDirection() == LinkDirection::LEFT || (*link)->getDirection() == LinkDirection::RIGHT
1023 : // the lanes after an internal junction are on different
1024 : // edges and do not allow lane-changing
1025 7822 : || (nextLane->getEdge().isInternal() && (*link)->getViaLaneOrLane()->getEdge().isInternal())
1026 : ) {
1027 16 : state |= LCA_INSUFFICIENT_SPACE;
1028 16 : break;
1029 : }
1030 3895 : if ((*link)->getViaLane() == nullptr) {
1031 3879 : view++;
1032 : }
1033 : nextLane = (*link)->getViaLaneOrLane();
1034 3895 : seen += nextLane->getLength();
1035 : // get the next link used
1036 3895 : link = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
1037 : }
1038 : #ifdef DEBUG_CHECK_CHANGE
1039 : if (DEBUG_COND) {
1040 : std::cout << " available distance=" << seen << std::endl;
1041 : }
1042 : #endif
1043 49270 : if (nextLane->isLinkEnd(link) && seen < space2change) {
1044 : #ifdef DEBUG_CHECK_CHANGE
1045 : if (DEBUG_COND) {
1046 : std::cout << SIMTIME << " checkChange insufficientSpace: seen=" << seen << " space2change=" << space2change << "\n";
1047 : }
1048 : #endif
1049 12134 : state |= LCA_INSUFFICIENT_SPACE;
1050 : }
1051 :
1052 49270 : if ((state & LCA_BLOCKED) == 0) {
1053 : // check for dangerous leaders in case the target lane changes laterally between
1054 : // now and the lane-changing midpoint
1055 37120 : const double speed = vehicle->getSpeed();
1056 37120 : seen = myCandi->lane->getLength() - vehicle->getPositionOnLane();
1057 37120 : nextLane = vehicle->getLane();
1058 : view = 1;
1059 37120 : const double dist = vehicle->getCarFollowModel().brakeGap(speed) + vehicle->getVehicleType().getMinGap();
1060 37120 : std::vector<MSLink*>::const_iterator nextLink = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
1061 40927 : while (!nextLane->isLinkEnd(nextLink) && seen <= space2change && seen <= dist) {
1062 3872 : nextLane = (*nextLink)->getViaLaneOrLane();
1063 3872 : const MSLane* const parallelLane = nextLane->getParallelLane(laneOffset);
1064 3872 : if (parallelLane == nullptr) {
1065 0 : state |= LCA_INSUFFICIENT_SPACE;
1066 0 : break;
1067 : } else {
1068 3872 : std::pair<MSVehicle* const, double> neighLead2 = parallelLane->getLeader(vehicle, -seen, std::vector<MSLane*>());
1069 2187 : if (neighLead2.first != nullptr && neighLead2.first != neighLead.first
1070 4698 : && (neighLead2.second < vehicle->getCarFollowModel().getSecureGap(vehicle, neighLead2.first,
1071 413 : vehicle->getSpeed(), neighLead2.first->getSpeed(), neighLead2.first->getCarFollowModel().getMaxDecel()))) {
1072 65 : state |= blockedByLeader;
1073 65 : break;
1074 : }
1075 : }
1076 3807 : if ((*nextLink)->getViaLane() == nullptr) {
1077 3799 : view++;
1078 : }
1079 3807 : seen += nextLane->getLength();
1080 : // get the next link used
1081 3807 : nextLink = MSLane::succLinkSec(*vehicle, view, *nextLane, bestLaneConts);
1082 : }
1083 : }
1084 : }
1085 : }
1086 : const int oldstate = state;
1087 : // let TraCI influence the wish to change lanes and the security to take
1088 235614004 : state = vehicle->influenceChangeDecision(state);
1089 235614004 : blocked = state & LCA_BLOCKED;
1090 : #ifdef DEBUG_CHECK_CHANGE
1091 : if (DEBUG_COND) {
1092 : std::cout << SIMTIME
1093 : << " veh=" << vehicle->getID()
1094 : << " oldState=" << toString((LaneChangeAction)oldstate)
1095 : << " newState=" << toString((LaneChangeAction)state)
1096 : << ((blocked & LCA_BLOCKED) ? " (blocked)" : "")
1097 : << ((blocked & LCA_OVERLAPPING) ? " (overlap)" : "")
1098 : << "\n";
1099 : }
1100 : #endif
1101 :
1102 235614004 : if (state & LCA_WANTS_LANECHANGE && blocked == 0) {
1103 733170 : blocked = vehicle->getLaneChangeModel().checkChangeBeforeCommitting(vehicle, state);
1104 733170 : state |= blocked;
1105 : }
1106 235614004 : vehicle->getLaneChangeModel().saveLCState(laneOffset, oldstate, state);
1107 235614004 : if (blocked == 0 && (state & LCA_WANTS_LANECHANGE)) {
1108 : // this lane change will be executed, save gaps
1109 733170 : vehicle->getLaneChangeModel().setFollowerGaps(neighFollow, secureBackGap);
1110 733170 : vehicle->getLaneChangeModel().setLeaderGaps(neighLead, secureFrontGap);
1111 733170 : vehicle->getLaneChangeModel().setOrigLeaderGaps(leader, secureOrigFrontGap);
1112 : }
1113 235614004 : if (laneOffset != 0) {
1114 235614004 : vehicle->getLaneChangeModel().saveNeighbors(laneOffset, neighFollow, neighLead);
1115 : }
1116 235614004 : return state;
1117 : }
1118 :
1119 : bool
1120 294069876 : MSLaneChanger::hasOppositeStop(MSVehicle* vehicle) {
1121 294069876 : if (vehicle->hasStops()) {
1122 6083246 : const MSStop& stop = vehicle->getNextStop();
1123 6083246 : if (stop.isOpposite && vehicle->nextStopDist() < OPPOSITE_OVERTAKING_MAX_LOOKAHEAD) {
1124 : return true;
1125 : }
1126 : }
1127 : return false;
1128 : }
1129 :
1130 :
1131 : bool
1132 1425 : MSLaneChanger::checkOppositeStop(MSVehicle* vehicle, const MSLane* oncomingLane, const MSLane* opposite, std::pair<MSVehicle*, double> leader) {
1133 1425 : const bool isOpposite = vehicle->getLaneChangeModel().isOpposite();
1134 1425 : double vMax = vehicle->getLane()->getVehicleMaxSpeed(vehicle);
1135 : std::pair<MSVehicle*, double> neighLead(nullptr, -1);
1136 1425 : std::pair<MSVehicle*, double> oncoming(nullptr, -1);
1137 1425 : const std::vector<MSVehicle::LaneQ> preb = getBestLanesOpposite(vehicle, vehicle->getNextStop().lane, -1);
1138 1425 : const int laneIndex = vehicle->getLaneChangeModel().getNormalizedLaneIndex();
1139 1425 : const int bestOffset = preb[laneIndex].bestLaneOffset;
1140 : //std::cout << SIMTIME << " veh=" << vehicle->getID() << " laneIndex=" << laneIndex << " bestOffset=" << bestOffset << " target=" << target->getID() << "\n";
1141 :
1142 : // compute safety constraints (assume vehicle is safe once stop is reached)
1143 : const double spaceToStop = vehicle->nextStopDist();
1144 1425 : const double timeToStopForward = spaceToStop / MAX2(vehicle->getSpeed(), vehicle->getCarFollowModel().getMaxAccel());
1145 1425 : const double timeToStopLateral = (MSGlobals::gLaneChangeDuration > 0
1146 1425 : ? STEPS2TIME(MSGlobals::gLaneChangeDuration) * bestOffset
1147 1256 : : (MSGlobals::gLateralResolution > 0
1148 1256 : ? bestOffset * SUMO_const_laneWidth / vehicle->getVehicleType().getMaxSpeedLat()
1149 : : 0.));
1150 : const double timeToStop = MAX2(timeToStopForward, timeToStopLateral);
1151 1425 : if (!isOpposite) {
1152 : // we keep neighLead distinct from oncoming because it determines blocking on the neigh lane
1153 : // but also look for an oncoming leader to compute safety constraint
1154 636 : const double searchDist = timeToStop * oncomingLane->getSpeedLimit() * 2 + spaceToStop;
1155 636 : neighLead = oncomingLane->getOppositeLeader(vehicle, searchDist, true);
1156 636 : oncoming = getOncomingVehicle(oncomingLane, neighLead, searchDist, vMax);
1157 : } else {
1158 : double searchDist = OPPOSITE_OVERTAKING_ONCOMING_LOOKAHEAD;
1159 789 : oncoming = oncomingLane->getOppositeLeader(vehicle, searchDist, true);
1160 789 : oncoming = getOncomingVehicle(oncomingLane, oncoming, searchDist, vMax);
1161 : }
1162 : double oncomingSpeed;
1163 1425 : const double surplusGap = computeSurplusGap(vehicle, opposite, oncoming, timeToStop, spaceToStop, oncomingSpeed);
1164 1425 : if (!isOpposite && surplusGap < 0) {
1165 : #ifdef DEBUG_CHANGE_OPPOSITE
1166 : if (DEBUG_COND) {
1167 : std::cout << " cannot changeOppositeStop due to dangerous oncoming spaceToStop=" << spaceToStop
1168 : << " timeToStopForward=" << timeToStopForward << " timeToStopLateral=" << timeToStopLateral << " surplusGap=" << surplusGap << "\n";
1169 : }
1170 : #endif
1171 : return false;
1172 : }
1173 :
1174 1071 : if (bestOffset > 0) {
1175 532 : MSLane* const target = preb[laneIndex + 1].lane;
1176 532 : neighLead = target->getOppositeLeader(vehicle, OPPOSITE_OVERTAKING_MAX_LOOKAHEAD, true);
1177 532 : std::pair<MSVehicle* const, double> neighFollow = target->getOppositeFollower(vehicle);
1178 532 : return checkChangeOpposite(vehicle, 1, target, leader, neighLead, neighFollow, preb);
1179 : } else {
1180 : // return prematurely (due to foe?)
1181 : //return checkChangeOpposite(vehicle, -1, target, leader, neighLead, neighFollow, preb);
1182 : return false;
1183 : }
1184 1425 : }
1185 :
1186 :
1187 : std::vector<MSVehicle::LaneQ>
1188 623085 : MSLaneChanger::getBestLanesOpposite(MSVehicle* vehicle, const MSLane* stopLane, double oppositeLength) {
1189 623085 : const bool isOpposite = vehicle->getLaneChangeModel().isOpposite();
1190 623085 : const MSEdge* forward = isOpposite ? vehicle->getLane()->getEdge().getOppositeEdge()->getNormalSuccessor() : vehicle->getLane()->getEdge().getNormalSuccessor();
1191 623085 : const MSEdge* opposite = forward->getOppositeEdge();
1192 : const int numForward = forward->getNumLanes();
1193 : const int numOpposite = opposite->getNumLanes();
1194 : const std::vector<MSLane*>& oLanes = opposite->getLanes();
1195 623085 : std::vector<MSVehicle::LaneQ> preb = vehicle->getBestLanes();
1196 1293590 : for (int i = 0; i < numOpposite; i++) {
1197 670505 : preb.push_back(preb.back());
1198 670505 : preb.back().lane = oLanes[numOpposite - 1 - i];
1199 670505 : preb.back().length = oppositeLength;
1200 670505 : if (isOpposite) {
1201 415377 : preb.back().bestLaneOffset = -1 - i;
1202 : //std::cout << " oi=" << i << " bestOffset =" << preb.back().bestLaneOffset << "\n";
1203 : }
1204 : }
1205 623085 : if (stopLane != nullptr) {
1206 1425 : const int stopIndex = numForward + numOpposite - stopLane->getIndex() - 1;
1207 6058 : for (int i = 0; i < (int)preb.size(); i++) {
1208 4633 : preb[i].bestLaneOffset = stopIndex - i;
1209 4633 : preb[i].length = vehicle->getLaneChangeModel().getForwardPos() + vehicle->nextStopDist();
1210 : //std::cout << " oi2=" << i << " stopIndex=" << stopIndex << " bestOffset =" << preb[i].bestLaneOffset << " stopDist=" << vehicle->nextStopDist() << " length=" << preb[i].length << "\n";
1211 : }
1212 : }
1213 : #ifdef DEBUG_CHANGE_OPPOSITE
1214 : if (DEBUG_COND) {
1215 : std::cout << SIMTIME << " getBestLanesOpposite " << vehicle->getID() << " isOpposite=" << isOpposite << "\n";
1216 : for (int i = 0; i < (int)preb.size(); i++) {
1217 : std::cout << " i=" << i << " lane=" << preb[i].lane->getID() << " bestOffset=" << preb[i].bestLaneOffset << " length=" << preb[i].length << "\n";
1218 : }
1219 : }
1220 : #endif
1221 623085 : return preb;
1222 0 : }
1223 :
1224 :
1225 : bool
1226 26187846 : MSLaneChanger::changeOpposite(MSVehicle* vehicle, std::pair<MSVehicle*, double> leader, MSVehicle* lastStopped) {
1227 : // Evaluate lane-changing between opposite direction lanes
1228 26187846 : if (!myChangeToOpposite) {
1229 : return false;
1230 : }
1231 7806569 : const bool isOpposite = vehicle->getLaneChangeModel().isOpposite();
1232 : MSLane* source = vehicle->getMutableLane();
1233 7806569 : MSLane* opposite = isOpposite ? source->getParallelLane(1) : source->getOpposite();
1234 :
1235 : #ifdef DEBUG_CHANGE_OPPOSITE
1236 : gDebugFlag5 = DEBUG_COND;
1237 : if (DEBUG_COND) {
1238 : std::cout << SIMTIME << " veh=" << vehicle->getID() << " considerChangeOpposite source=" << source->getID()
1239 : << " opposite=" << Named::getIDSecure(opposite) << " lead=" << Named::getIDSecure(leader.first) << " isOpposite=" << isOpposite << "\n";
1240 : }
1241 : #endif
1242 : //There is no lane for opposite driving
1243 7806569 : if (opposite == nullptr) {
1244 : return false;
1245 : }
1246 7763588 : if (vehicle->isStopped()) {
1247 : // stopped vehicles obviously should not change lanes. Usually this is
1248 : // prevent by appropriate bestLane distances
1249 : return false;
1250 : }
1251 : int ret = 0;
1252 7763588 : ret = vehicle->influenceChangeDecision(ret);
1253 : bool oppositeChangeByTraci = false;
1254 : // Check whether a lane change to the opposite direction was requested via TraCI
1255 7763588 : if ((ret & (LCA_TRACI)) != 0) {
1256 121 : if (isOpposite && (ret & LCA_LEFT) != 0) {
1257 : // stay on the opposite side
1258 : return false;
1259 : }
1260 : oppositeChangeByTraci = true;
1261 : }
1262 7763538 : if (!isOpposite && !oppositeChangeByTraci && !source->allowsChangingLeft(vehicle->getVClass())) {
1263 : // lane changing explicitly forbidden from this lane
1264 : #ifdef DEBUG_CHANGE_OPPOSITE
1265 : if (DEBUG_COND) {
1266 : std::cout << " not overtaking due to changeLeft restriction\n";
1267 : }
1268 : #endif
1269 : return false;
1270 : }
1271 :
1272 : //lane for opposite driving is not permitted
1273 7762281 : if (!opposite->allowsVehicleClass(vehicle->getVClass())) {
1274 : return false;
1275 : }
1276 :
1277 7761024 : const MSLane* oncomingLane = isOpposite ? source : opposite;
1278 : //const MSLane* forwardLane = isOpposite ? opposite : source;
1279 : // changing into the opposite direction is always to the left (XXX except for left-hand networkds)
1280 : int direction = isOpposite ? -1 : 1;
1281 : std::pair<MSVehicle*, double> neighLead(nullptr, -1);
1282 :
1283 : // distance that can safely be driven on the opposite side
1284 : double surplusGap = std::numeric_limits<double>::max();
1285 :
1286 : // we need to find two vehicles:
1287 : // 1) the leader that shall be overtaken (not necessarily the current leader but one of its leaders that has enough space in front)
1288 : // 2) the oncoming vehicle (we need to look past vehicles that are currently overtaking through the opposite direction themselves)
1289 : //
1290 : // if the vehicle is driving normally, then the search for 1) starts on the current lane and 2) on the opposite lane
1291 : // if the vehicle is driving on the opposite side then 1) is found on the neighboring lane and 2) on the current lane
1292 :
1293 : std::pair<MSVehicle*, double> overtaken(nullptr, -1);
1294 : // oncoming vehicle that is driving in the "correct" direction
1295 : std::pair<MSVehicle*, double> oncoming(nullptr, -1);
1296 : // oncoming vehicle that is driving against the flow
1297 : std::pair<MSVehicle*, double> oncomingOpposite(nullptr, -1);
1298 : // the maximum speed while overtaking (may be lowered if slow vehicles are
1299 : // currently overtaking ahead of vehicle)
1300 7761024 : double vMax = vehicle->getLane()->getVehicleMaxSpeed(vehicle);
1301 7761024 : double oncomingSpeed = oncomingLane->getSpeedLimit();
1302 7761024 : const bool isEmergency = vehicle->getVClass() == SVC_EMERGENCY;
1303 :
1304 : // check for opposite direction stop
1305 7761024 : if (!oppositeChangeByTraci && hasOppositeStop(vehicle)) {
1306 1425 : return checkOppositeStop(vehicle, oncomingLane, opposite, leader);
1307 : }
1308 :
1309 7759599 : if (!isOpposite && leader.first == nullptr && !oppositeChangeByTraci) {
1310 : // no reason to change unless there is a leader
1311 : // or we are changing back to the propper direction
1312 : // XXX also check whether the leader is so far away as to be irrelevant
1313 : return false;
1314 : }
1315 7398907 : if (!isOpposite && !oppositeChangeByTraci
1316 7009330 : && !isEmergency
1317 6980729 : && leader.first != nullptr) {
1318 12892221 : if (leader.first->signalSet(MSGlobals::gLefthand
1319 : ? MSVehicle::VEH_SIGNAL_BLINKER_RIGHT : MSVehicle::VEH_SIGNAL_BLINKER_LEFT)) {
1320 : // do not try to overtake a vehicle that is about to turn left or wants
1321 : // to change left itself
1322 : #ifdef DEBUG_CHANGE_OPPOSITE
1323 : if (DEBUG_COND) {
1324 : std::cout << " not overtaking leader " << leader.first->getID() << " that has blinker set\n";
1325 : }
1326 : #endif
1327 200898 : if (lastStopped != nullptr && vehicle->getWaitingSeconds() >= OPPOSITE_OVERTAKING_DEADLOCK_WAIT) {
1328 1484 : neighLead = oncomingLane->getOppositeLeader(vehicle, OPPOSITE_OVERTAKING_ONCOMING_LOOKAHEAD, true, MSLane::MinorLinkMode::FOLLOW_ONCOMING);
1329 1484 : const double lastStoppedGap = lastStopped->getBackPositionOnLane() - vehicle->getPositionOnLane() - vehicle->getVehicleType().getMinGap();
1330 1484 : resolveDeadlock(vehicle, leader, neighLead, std::make_pair(lastStopped, lastStoppedGap));
1331 : }
1332 185100 : return false;
1333 6795629 : } else if (leader.second < 0) {
1334 : // leaders is either a junction leader (that cannot be overtaken) or something else is wrong
1335 : #ifdef DEBUG_CHANGE_OPPOSITE
1336 : if (DEBUG_COND) {
1337 : std::cout << " not overtaking leader " << leader.first->getID() << " with gap " << leader.second << "\n";
1338 : }
1339 : #endif
1340 : return false;
1341 : }
1342 : }
1343 :
1344 : // checks for overtaking space
1345 6915032 : double timeToOvertake = std::numeric_limits<double>::max();
1346 6915032 : double spaceToOvertake = std::numeric_limits<double>::max();
1347 6915032 : double maxSpaceToOvertake = 0;
1348 :
1349 6915032 : if (oppositeChangeByTraci) {
1350 71 : timeToOvertake = STEPS2TIME(vehicle->getInfluencer().getLaneTimeLineDuration());//todo discuss concept
1351 71 : spaceToOvertake = timeToOvertake * vehicle->getLane()->getVehicleMaxSpeed(vehicle);
1352 : } else {
1353 6914961 : if (isOpposite) {
1354 : // -1 will use getMaximumBrakeDist() as look-ahead distance
1355 389506 : neighLead = opposite->getOppositeLeader(vehicle, -1, false);
1356 : // make sure that overlapping vehicles on the neighboring lane are found by starting search at the back position
1357 779012 : overtaken = opposite->getLeader(vehicle, opposite->getOppositePos(vehicle->getBackPositionOnLane()), vehicle->getBestLanesContinuation(opposite));
1358 389506 : overtaken.second -= vehicle->getVehicleType().getLength();
1359 389506 : if (overtaken.first == nullptr && neighLead.first != nullptr) {
1360 : overtaken = neighLead;
1361 : }
1362 389506 : if (overtaken.first != nullptr) {
1363 383394 : overtaken = getColumnleader(maxSpaceToOvertake, vehicle, overtaken);
1364 : }
1365 : #ifdef DEBUG_CHANGE_OPPOSITE
1366 : if (DEBUG_COND) {
1367 : std::cout << " leaderOnSource=" << Named::getIDSecure(oncoming.first) << " gap=" << oncoming.second << "\n";
1368 : std::cout << " leaderOnTarget=" << Named::getIDSecure(neighLead.first) << " gap=" << neighLead.second << "\n";
1369 : std::cout << " overtaken=" << Named::getIDSecure(overtaken.first) << " gap=" << overtaken.second << "\n";
1370 : }
1371 : #endif
1372 : } else {
1373 6525455 : overtaken = getColumnleader(maxSpaceToOvertake, vehicle, leader);
1374 : }
1375 :
1376 6914961 : if (overtaken.first == 0) {
1377 2674409 : if (!isOpposite) {
1378 : // no reason to change to the opposite side
1379 : #ifdef DEBUG_CHANGE_OPPOSITE
1380 : if (DEBUG_COND) {
1381 : std::cout << " no leader found\n";
1382 : }
1383 : #endif
1384 2632112 : if (vehicle->getWaitingSeconds() >= OPPOSITE_OVERTAKING_DEADLOCK_WAIT) {
1385 823247 : neighLead = oncomingLane->getOppositeLeader(vehicle, OPPOSITE_OVERTAKING_ONCOMING_LOOKAHEAD, true, MSLane::MinorLinkMode::FOLLOW_ONCOMING);
1386 823247 : resolveDeadlock(vehicle, leader, neighLead, std::make_pair(nullptr, leader.second));
1387 : }
1388 2632112 : return false;
1389 : }
1390 : } else {
1391 : #ifdef DEBUG_CHANGE_OPPOSITE
1392 : if (DEBUG_COND) {
1393 : std::cout << " compute time/space to overtake for columnLeader=" << overtaken.first->getID() << " egoGap=" << overtaken.second << "\n";
1394 : }
1395 : #endif
1396 : // if we have limited space to overtake, we may have to limit our maximum maneuver speed
1397 4240552 : vMax = MIN2(vMax, getMaxOvertakingSpeed(vehicle, maxSpaceToOvertake));
1398 : // there might be leader vehicles on the opposite side that also drive
1399 : // against the flow which are slower than ego (must be factored into
1400 : // overtaking time)
1401 4240552 : computeOvertakingTime(vehicle, vMax, overtaken.first, overtaken.second, timeToOvertake, spaceToOvertake);
1402 : #ifdef DEBUG_CHANGE_OPPOSITE
1403 : if (DEBUG_COND) {
1404 : std::cout << SIMTIME
1405 : << " veh=" << vehicle->getID()
1406 : << " changeOpposite opposite=" << opposite->getID()
1407 : << " lead=" << Named::getIDSecure(leader.first)
1408 : << " maxSpaceToOvertake=" << maxSpaceToOvertake
1409 : << " vMax=" << vMax
1410 : << " timeToOvertake=" << timeToOvertake
1411 : << " spaceToOvertake=" << spaceToOvertake
1412 : << "\n";
1413 : }
1414 : #endif
1415 :
1416 4240552 : if (!isOpposite && spaceToOvertake > OPPOSITE_OVERTAKING_MAX_SPACE_TO_OVERTAKE) {
1417 : #ifdef DEBUG_CHANGE_OPPOSITE
1418 : if (DEBUG_COND) {
1419 : std::cout << " cannot changeOpposite (cannot overtake fast leader " << Named::getIDSecure(overtaken.first) << " v=" << overtaken.first->getSpeed() << ")\n";
1420 : }
1421 : #endif
1422 1551586 : neighLead = oncomingLane->getOppositeLeader(vehicle, OPPOSITE_OVERTAKING_ONCOMING_LOOKAHEAD, true, MSLane::MinorLinkMode::FOLLOW_ONCOMING);
1423 : bool wait = false;
1424 1551586 : if (vehicle->getWaitingSeconds() >= OPPOSITE_OVERTAKING_DEADLOCK_WAIT) {
1425 79352 : wait = resolveDeadlock(vehicle, leader, neighLead, overtaken);
1426 : }
1427 1551586 : if (!wait && lastStopped != nullptr) {
1428 157522 : const double lastStoppedGap = lastStopped->getBackPositionOnLane() - vehicle->getPositionOnLane() - vehicle->getVehicleType().getMinGap();
1429 : #ifdef DEBUG_CHANGE_OPPOSITE
1430 : if (DEBUG_COND) {
1431 : std::cout << " lastStopped=" << Named::getIDSecure(lastStopped) << " gap=" << lastStoppedGap << "\n";
1432 : }
1433 : #endif
1434 157522 : avoidDeadlock(vehicle, neighLead, std::make_pair(lastStopped, lastStoppedGap), leader);
1435 : }
1436 1551586 : return false;
1437 : }
1438 : }
1439 :
1440 : // if we have a leader vehicle that is driving in the opposite
1441 : // direction, it may slow us down (update vMax)
1442 : if (!isOpposite) {
1443 : assert(timeToOvertake != std::numeric_limits<double>::max());
1444 : assert(spaceToOvertake != std::numeric_limits<double>::max());
1445 : // we keep neighLead distinct from oncoming because it determines blocking on the neigh lane
1446 : // but also look for an oncoming leader to compute safety constraint
1447 2341757 : double searchDist = timeToOvertake * oncomingLane->getSpeedLimit() * 2 + spaceToOvertake;
1448 2341757 : neighLead = oncomingLane->getOppositeLeader(vehicle, searchDist, true, MSLane::MinorLinkMode::FOLLOW_ONCOMING);
1449 2341757 : oncoming = getOncomingVehicle(oncomingLane, neighLead, searchDist, vMax, overtaken.first, MSLane::MinorLinkMode::FOLLOW_ONCOMING);
1450 2341757 : oncomingOpposite = getOncomingOppositeVehicle(vehicle, overtaken, searchDist);
1451 : } else {
1452 : double searchDist = OPPOSITE_OVERTAKING_ONCOMING_LOOKAHEAD;
1453 389506 : oncoming = oncomingLane->getOppositeLeader(vehicle, searchDist, true);
1454 389506 : oncoming = getOncomingVehicle(oncomingLane, oncoming, searchDist, vMax, overtaken.first);
1455 389506 : oncomingOpposite = getOncomingOppositeVehicle(vehicle, overtaken, searchDist);
1456 : }
1457 2731263 : if (oncoming.first != nullptr && (oncoming.first->isStopped()
1458 1537831 : || oncoming.first->getWaitingSeconds() >= OPPOSITE_OVERTAKING_DEADLOCK_WAIT)) {
1459 : // finish overtaking within the available space
1460 982517 : const double oncomingGap = oncoming.second - oncoming.first->getVehicleType().getMinGap();
1461 982517 : if (oncomingGap > 0) {
1462 852110 : vMax = MIN2(vMax, getMaxOvertakingSpeed(vehicle, oncomingGap));
1463 : }
1464 : #ifdef DEBUG_CHANGE_OPPOSITE
1465 : if (DEBUG_COND) {
1466 : std::cout << " oncoming=" << oncoming.first->getID() << " stopped=" << oncoming.first->isStopped()
1467 : << " halting=" << oncoming.first->getWaitingSeconds()
1468 : << " oncomingGap=" << oncomingGap
1469 : << " vMaxGap=" << getMaxOvertakingSpeed(vehicle, oncomingGap)
1470 : << " vMax=" << vMax << "\n";
1471 : }
1472 : #endif
1473 : }
1474 :
1475 2731263 : if (overtaken.first != nullptr && vMax != vehicle->getLane()->getVehicleMaxSpeed(vehicle)) {
1476 : // recompute overtaking time with slow opposite leader
1477 1064652 : computeOvertakingTime(vehicle, vMax, overtaken.first, overtaken.second, timeToOvertake, spaceToOvertake);
1478 : #ifdef DEBUG_CHANGE_OPPOSITE
1479 : if (DEBUG_COND) {
1480 : std::cout << " recomputed overtaking time with vMax=" << vMax
1481 : << " timeToOvertake=" << timeToOvertake
1482 : << " spaceToOvertake=" << spaceToOvertake
1483 : << "\n";
1484 : }
1485 : #endif
1486 : }
1487 2731263 : if (!isOpposite) {
1488 2341757 : if (spaceToOvertake > OPPOSITE_OVERTAKING_MAX_SPACE_TO_OVERTAKE) {
1489 : #ifdef DEBUG_CHANGE_OPPOSITE
1490 : if (DEBUG_COND) {
1491 : std::cout << " cannot changeOpposite (check2: cannot overtake fast leader " << Named::getIDSecure(overtaken.first) << " v=" << overtaken.first->getSpeed() << ")\n";
1492 : }
1493 : #endif
1494 330409 : resolveDeadlock(vehicle, leader, neighLead, overtaken);
1495 330409 : return false;
1496 : }
1497 : // check for upcoming stops
1498 2011348 : if (vehicle->nextStopDist() < spaceToOvertake) {
1499 : #ifdef DEBUG_CHANGE_OPPOSITE
1500 : if (DEBUG_COND) {
1501 : std::cout << " cannot changeOpposite due to upcoming stop (dist=" << vehicle->nextStopDist() << " spaceToOvertake=" << spaceToOvertake << ")\n";
1502 : }
1503 : #endif
1504 : return false;
1505 : }
1506 : assert(timeToOvertake != std::numeric_limits<double>::max());
1507 : assert(spaceToOvertake != std::numeric_limits<double>::max());
1508 : }
1509 :
1510 : // check for dangerous oncoming leader
1511 2334865 : surplusGap = computeSurplusGap(vehicle, opposite, oncoming, timeToOvertake, spaceToOvertake, oncomingSpeed);
1512 2334865 : if (oncomingOpposite.first != nullptr) {
1513 : double oncomingSpeed2;
1514 424769 : const double conservativeTime = ceil(timeToOvertake / TS) * TS;
1515 424769 : const double conservativeSpace = conservativeTime * vehicle->getLane()->getVehicleMaxSpeed(vehicle);
1516 424769 : const double surplusGap2 = computeSurplusGap(vehicle, opposite, oncomingOpposite, conservativeTime, conservativeSpace, oncomingSpeed2, true);
1517 : #ifdef DEBUG_CHANGE_OPPOSITE
1518 : if (DEBUG_COND) {
1519 : std::cout << " oncomingOpposite=" << oncomingOpposite.first->getID() << " speed=" << oncomingSpeed2 << " gap=" << oncomingOpposite.second << " surplusGap2=" << surplusGap2 << "\n";
1520 : }
1521 : #endif
1522 : surplusGap = MIN2(surplusGap, surplusGap2);
1523 424769 : oncomingSpeed = MAX2(oncomingSpeed, oncomingSpeed2);
1524 357693 : if (!isOpposite && surplusGap >= 0 && oncoming.first != nullptr && oncoming.first->isStopped()
1525 425482 : && oncomingOpposite.second > oncoming.second) {
1526 : // even if ego can change back and forth successfully, we have to
1527 : // make sure that the oncoming vehicle can also finish its lane
1528 : // change in time
1529 686 : const double oSpeed = MAX2(oncomingOpposite.first->getSpeed(), NUMERICAL_EPS);
1530 : // conservative estimate
1531 686 : const double closingSpeed = (vehicle->getLane()->getVehicleMaxSpeed(vehicle)
1532 686 : + oncomingOpposite.first->getLane()->getVehicleMaxSpeed(oncomingOpposite.first));
1533 686 : const double ooSTO = oncomingOpposite.second - oncoming.second + oncomingOpposite.first->getVehicleType().getLengthWithGap();
1534 686 : double ooTTO = ooSTO / oSpeed;
1535 : // round to multiples of step length (TS)
1536 686 : ooTTO = ceil(ooTTO / TS) * TS;
1537 686 : const double surplusGap3 = oncomingOpposite.second - ooTTO * closingSpeed;
1538 : #ifdef DEBUG_CHANGE_OPPOSITE
1539 : if (DEBUG_COND) {
1540 : std::cout << " oSpeed=" << oSpeed << " ooSTO=" << ooSTO << " ooTTO=" << ooTTO << " surplusGap3=" << surplusGap3 << "\n";
1541 : }
1542 : #endif
1543 : surplusGap = MIN2(surplusGap, surplusGap3);
1544 : }
1545 : }
1546 2334865 : if (!isOpposite && surplusGap < 0) {
1547 : #ifdef DEBUG_CHANGE_OPPOSITE
1548 : if (DEBUG_COND) {
1549 : std::cout << " cannot changeOpposite due to dangerous oncoming (surplusGap=" << surplusGap << ")\n";
1550 : }
1551 : #endif
1552 :
1553 : #ifdef DEBUG_CHANGE_OPPOSITE
1554 : if (DEBUG_COND) {
1555 : if (oncoming.first->getLaneChangeModel().isOpposite()) {
1556 : std::cout << SIMTIME << " ego=" << vehicle->getID() << " does not changeOpposite due to dangerous oncoming " << oncoming.first->getID() << " (but the leader is also opposite)\n";
1557 : }
1558 : }
1559 : #endif
1560 1392454 : avoidDeadlock(vehicle, neighLead, overtaken, leader);
1561 1392454 : return false;
1562 : }
1563 : }
1564 : // compute remaining space on the opposite side
1565 : // 1. the part that remains on the current lane
1566 942482 : double usableDist = isOpposite ? vehicle->getPositionOnLane() : source->getLength() - vehicle->getPositionOnLane();
1567 :
1568 942482 : if (usableDist < spaceToOvertake) {
1569 : // look forward along the next lanes
1570 357763 : const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation();
1571 : assert(bestLaneConts.size() >= 1);
1572 : std::vector<MSLane*>::const_iterator it = bestLaneConts.begin() + 1;
1573 470334 : while (usableDist < spaceToOvertake && it != bestLaneConts.end()) {
1574 : #ifdef DEBUG_CHANGE_OPPOSITE
1575 : if (DEBUG_COND) {
1576 : std::cout << " usableDist=" << usableDist << " opposite=" << Named::getIDSecure((*it)->getOpposite()) << "\n";
1577 : }
1578 : #endif
1579 244079 : const MSLane* oppLane = (*it)->getOpposite();
1580 244079 : if ((oppLane == nullptr && !isEmergency) || (oppLane != nullptr && !oppLane->allowsVehicleClass(vehicle->getVClass()))) {
1581 : // opposite lane ends
1582 : break;
1583 : }
1584 : // do not overtake past a minor link or turn
1585 133286 : const MSLane* const prev = *(it - 1);
1586 133286 : if (prev != nullptr) {
1587 130183 : const MSLink* link = prev->getLinkTo(*it);
1588 130183 : if (link == nullptr || link->getState() == LINKSTATE_ZIPPER
1589 130183 : || (link->getDirection() != LinkDirection::STRAIGHT && !isEmergency)
1590 241327 : || (!link->havePriority()
1591 : // consider traci-influence
1592 9915 : && (!vehicle->hasInfluencer() || vehicle->getInfluencer().getRespectJunctionPriority())
1593 : // consider junction model parameters
1594 787 : && ((!link->haveRed() && !link->haveYellow()) || !vehicle->ignoreRed(link, true)))) {
1595 : #ifdef DEBUG_CHANGE_OPPOSITE
1596 : if (DEBUG_COND) {
1597 : std::cout << " stop lookahead at link=" << (link == 0 ? "NULL" : link->getViaLaneOrLane()->getID()) << " state=" << (link == 0 ? "?" : toString(link->getState())) << " ignoreRed=" << vehicle->ignoreRed(link, true) << "\n";
1598 : }
1599 : #endif
1600 : break;
1601 : }
1602 : }
1603 112571 : usableDist += (*it)->getLength();
1604 : ++it;
1605 : }
1606 : }
1607 942482 : if (!isOpposite && usableDist < spaceToOvertake) {
1608 : #ifdef DEBUG_CHANGE_OPPOSITE
1609 : if (DEBUG_COND) {
1610 : std::cout << " cannot changeOpposite due to insufficient space (seen=" << usableDist << " spaceToOvertake=" << spaceToOvertake << ")\n";
1611 : }
1612 : #endif
1613 : return false;
1614 : }
1615 380852 : if (!isOpposite && MSNet::getInstance()->hasElevation() && !overtaken.first->isStopped()) {
1616 : // do not overtake before the top of a hill
1617 127046 : double searchDist = timeToOvertake * oncomingLane->getSpeedLimit() * 1.5 * vehicle->getLaneChangeModel().getOppositeSafetyFactor() + spaceToOvertake;
1618 127046 : int view = vehicle->getLane()->isInternal() ? 1 : 0;
1619 127046 : bool foundHill = vehicle->getSlope() > 0;
1620 127046 : if (foundHilltop(vehicle, foundHill, searchDist, vehicle->getBestLanesContinuation(), view, vehicle->getPositionOnLane(), vehicle->getPosition().z(), OPPOSITE_OVERTAKING_HILLTOP_THRESHOHOLD)) {
1621 : return false;
1622 : }
1623 : }
1624 : #ifdef DEBUG_CHANGE_OPPOSITE
1625 : if (DEBUG_COND) {
1626 : std::cout << " usableDist=" << usableDist << " spaceToOvertake=" << spaceToOvertake << " timeToOvertake=" << timeToOvertake << "\n";
1627 : }
1628 : #endif
1629 : // compute wish to change
1630 670447 : std::pair<MSVehicle* const, double> neighFollow = opposite->getOppositeFollower(vehicle);
1631 670447 : double oppositeLength = vehicle->getBestLanes().back().length;
1632 670447 : if (isOpposite) {
1633 389526 : const bool canOvertake = spaceToOvertake <= OPPOSITE_OVERTAKING_MAX_SPACE_TO_OVERTAKE;
1634 389526 : oppositeLength = computeSafeOppositeLength(vehicle, oppositeLength, source, usableDist, oncoming, vMax, oncomingSpeed, neighLead, overtaken, neighFollow, surplusGap, opposite, canOvertake);
1635 : leader.first = nullptr;
1636 389526 : if (neighLead.first != nullptr && neighLead.first->getLaneChangeModel().isOpposite()) {
1637 : // ignore oncoming vehicle on the target lane (it might even change back in this step)
1638 : neighLead.first = nullptr;
1639 : }
1640 : } else {
1641 519276 : if (leader.first != nullptr && neighLead.first != nullptr && leader.first->getWaitingSeconds() >= OPPOSITE_OVERTAKING_DEADLOCK_WAIT && !isEmergency) {
1642 : #ifdef DEBUG_CHANGE_OPPOSITE
1643 : if (DEBUG_COND) {
1644 : std::cout << " not changing to avoid deadlock\n";
1645 : }
1646 : #endif
1647 : return false;
1648 : }
1649 240442 : if (neighLead.first != nullptr && neighLead.first->isStopped()) {
1650 : // do not start overtaking if the opposite side has been waitin for longer
1651 8809 : if (yieldToOppositeWaiting(vehicle, neighLead.first, 10)) {
1652 : return false;
1653 : }
1654 : }
1655 234118 : if (oncoming.first != nullptr && oncoming.first != neighLead.first && oncoming.first->isStopped()) {
1656 : // only abort the current column of overtaking vehicles if the opposite side has been waiting long enough
1657 15150 : if (yieldToOppositeWaiting(vehicle, oncoming.first, 10, TIME2STEPS(60))) {
1658 : return false;
1659 : }
1660 : }
1661 : }
1662 621493 : std::vector<MSVehicle::LaneQ> preb = getBestLanesOpposite(vehicle, nullptr, oppositeLength);
1663 621493 : return checkChangeOpposite(vehicle, direction, opposite, leader, neighLead, neighFollow, preb);
1664 621493 : }
1665 :
1666 :
1667 : bool
1668 1549976 : MSLaneChanger::avoidDeadlock(MSVehicle* vehicle,
1669 : std::pair<MSVehicle*, double> neighLead,
1670 : std::pair<MSVehicle*, double> overtaken,
1671 : std::pair<MSVehicle*, double> leader) {
1672 : assert(!vehicle->getLaneChangeModel().isOpposite());
1673 : #ifdef DEBUG_CHANGE_OPPOSITE_DEADLOCK
1674 : if (DEBUG_COND) {
1675 : std::cout << SIMTIME << " veh=" << vehicle->getID() << " avoidDeadlock"
1676 : << " neighLead=" << Named::getIDSecure(neighLead.first)
1677 : << " overtaken=" << Named::getIDSecure(overtaken.first)
1678 : << " leader=" << Named::getIDSecure(leader.first)
1679 : << "\n";
1680 : }
1681 : #endif
1682 1549976 : if (leader.first == nullptr || neighLead.first == nullptr || overtaken.first == nullptr) {
1683 : return false;
1684 1549532 : } else if (!neighLead.first->isStopped()
1685 2668573 : && vehicle->getWaitingSeconds() >= OPPOSITE_OVERTAKING_DEADLOCK_WAIT) {
1686 : // possibly there is an oncoming vehicle before the stoppled leader that
1687 : // could drive due to our yielding
1688 406534 : auto neighLeadFollow = neighLead.first->getFollower(overtaken.second);
1689 406534 : neighLead.second += neighLead.first->getVehicleType().getLengthWithGap() + neighLeadFollow.second;
1690 406534 : neighLead.first = const_cast<MSVehicle*>(neighLeadFollow.first);
1691 : #ifdef DEBUG_CHANGE_OPPOSITE_DEADLOCK
1692 : if (DEBUG_COND) {
1693 : std::cout << " neighLead follower=" << Named::getIDSecure(neighLeadFollow.first) << "\n";
1694 : }
1695 : #endif
1696 406534 : if (neighLead.first == nullptr) {
1697 5924 : return false;
1698 : }
1699 : }
1700 :
1701 1543608 : const bool yield = (yieldToDeadlockOncoming(vehicle, neighLead.first, overtaken.second)
1702 2414884 : || leader.first->getWaitingSeconds() >= OPPOSITE_OVERTAKING_DEADLOCK_WAIT);
1703 1543608 : if (neighLead.first->isStopped()
1704 1543608 : && (overtaken.first->isStopped()
1705 43469 : || leader.first->getLaneChangeModel().isOpposite()
1706 42518 : || yield)) {
1707 :
1708 : // estimate required gap
1709 462059 : double requiredGap = MAX2(vehicle->getVehicleType().getLengthWithGap(), neighLead.first->getVehicleType().getLengthWithGap());
1710 462059 : requiredGap = MAX2(requiredGap, overtaken.first->getVehicleType().getLengthWithGap());
1711 462059 : requiredGap = MAX2(requiredGap, leader.first->getVehicleType().getLengthWithGap());
1712 462059 : requiredGap += 1;
1713 462059 : const double distToStop = neighLead.second - requiredGap;
1714 :
1715 : // find the next non-stopped vehicle behind neighLead
1716 462059 : double neighStoppedBack = vehicle->getVehicleType().getMinGap();
1717 1210140 : while (neighLead.first != nullptr && neighLead.first->isStopped()) {
1718 804771 : const double nextGap = neighLead.second + neighLead.first->getVehicleType().getLengthWithGap();
1719 804771 : if (neighStoppedBack + nextGap > overtaken.second) {
1720 : break;
1721 : }
1722 : neighStoppedBack += nextGap;
1723 748081 : auto neighLeadFollow = neighLead.first->getFollower();
1724 748081 : neighLead.second = neighLeadFollow.second;
1725 748081 : neighLead.first = const_cast<MSVehicle*>(neighLeadFollow.first);
1726 : #ifdef DEBUG_CHANGE_OPPOSITE_DEADLOCK
1727 : if (DEBUG_COND) {
1728 : std::cout << " neighLeadFollower=" << Named::getIDSecure(neighLead.first) << "\n";
1729 : }
1730 : #endif
1731 : if (neighStoppedBack > overtaken.second) {
1732 : break;
1733 : }
1734 : }
1735 :
1736 462059 : const double leaderBGap = leader.first->getBrakeGap();
1737 462059 : const double leaderFGap = leader.first->getLane()->getLeader(leader.first, leader.first->getPositionOnLane(), vehicle->getBestLanesContinuation(), overtaken.second, true).second;
1738 : const double extraGap = MAX2(leaderBGap, leaderFGap);
1739 462059 : const double gapWithEgo = leader.second + extraGap - neighStoppedBack - vehicle->getVehicleType().getLengthWithGap();
1740 : #ifdef DEBUG_CHANGE_OPPOSITE_DEADLOCK
1741 : if (DEBUG_COND) {
1742 : std::cout << SIMTIME << " veh=" << vehicle->getID() << " avoidDeadlock"
1743 : << " neighLeadGap=" << neighLead.second
1744 : << " leaderGap=" << leader.second
1745 : << " bGap=" << leaderBGap
1746 : << " fGap=" << leaderFGap
1747 : << " eGap=" << extraGap
1748 : << " neighStoppedBack=" << neighStoppedBack
1749 : << " neighStoppedBackPos=" << vehicle->getPositionOnLane() + neighStoppedBack
1750 : << " requiredGap=" << requiredGap
1751 : << " gapWithEgo=" << gapWithEgo
1752 : << " yield=" << yield
1753 : << "\n";
1754 : }
1755 : #endif
1756 : // vehicle must fit behind leader and still leave required gap
1757 462059 : if (leader.first->getLaneChangeModel().isOpposite() || yield || gapWithEgo < requiredGap) {
1758 439468 : const std::vector<MSVehicle::LaneQ>& preb = vehicle->getBestLanes();
1759 439468 : const double currentDist = preb[vehicle->getLane()->getIndex()].length;
1760 439468 : const double stopPos = vehicle->getPositionOnLane() + distToStop;
1761 : #ifdef DEBUG_CHANGE_OPPOSITE_DEADLOCK
1762 : if (DEBUG_COND) {
1763 : std::cout << " currentDist=" << currentDist << " stopPos=" << stopPos << " lGap+eGap=" << leader.second + extraGap << " distToStop=" << distToStop << "\n";
1764 : }
1765 : #endif
1766 439468 : if (leader.second + leaderBGap + leader.first->getLength() > distToStop) {
1767 169867 : const double blockerLength = currentDist - stopPos;
1768 169867 : const bool reserved = vehicle->getLaneChangeModel().saveBlockerLength(blockerLength, -1);
1769 : #ifdef DEBUG_CHANGE_OPPOSITE_DEADLOCK
1770 : if (DEBUG_COND) {
1771 : std::cout << SIMTIME << " veh=" << vehicle->getID() << " avoidDeadlock"
1772 : << " blockerLength=" << blockerLength
1773 : << " reserved=" << reserved
1774 : << "\n";
1775 : }
1776 : #endif
1777 169867 : return reserved;
1778 : }
1779 : }
1780 : }
1781 : return false;
1782 : }
1783 :
1784 :
1785 : bool
1786 1544298 : MSLaneChanger::yieldToDeadlockOncoming(const MSVehicle* vehicle, const MSVehicle* stoppedNeigh, double dist) {
1787 1544298 : if (vehicle->getWaitingSeconds() >= OPPOSITE_OVERTAKING_DEADLOCK_WAIT && stoppedNeigh != nullptr) {
1788 : // is there a vehicle waiting behind stoppedNeigh that is encouraged by
1789 : // halting ego? Due to edge-ordering it might change-opposite this step
1790 : // and not be visible as leader
1791 775135 : std::pair<const MSVehicle*, double> follower = stoppedNeigh->getFollower(dist);
1792 775135 : double followerGap = stoppedNeigh->getVehicleType().getLengthWithGap();
1793 1173254 : while (follower.first != nullptr && followerGap < dist && follower.first->isStopped()) {
1794 398119 : followerGap += follower.second + follower.first->getVehicleType().getLengthWithGap();
1795 398119 : follower = follower.first->getFollower(dist);
1796 : };
1797 775135 : if (follower.first != nullptr) {
1798 755243 : followerGap += follower.second;
1799 : }
1800 : #ifdef DEBUG_CHANGE_OPPOSITE_DEADLOCK
1801 : if (DEBUG_COND) {
1802 : std::cout << SIMTIME << " veh=" << vehicle->getID() << " yieldToDeadlockOncoming"
1803 : << " dist=" << dist << " follower=" << Named::getIDSecure(follower.first) << " fGap=" << followerGap
1804 : << "\n";
1805 : }
1806 : #endif
1807 775135 : return follower.first != nullptr && followerGap < dist && !follower.first->isStopped();
1808 : }
1809 : return false;
1810 : }
1811 :
1812 :
1813 : bool
1814 32386 : MSLaneChanger::yieldToOppositeWaiting(const MSVehicle* vehicle, const MSVehicle* stoppedNeigh, double dist, SUMOTime deltaWait) {
1815 32386 : std::pair<const MSVehicle*, double> follower = stoppedNeigh->getFollower(dist);
1816 86308 : while (follower.first != nullptr && follower.second < dist && follower.first->isStopped()) {
1817 53922 : follower = follower.first->getFollower(dist);
1818 : };
1819 : #ifdef DEBUG_CHANGE_OPPOSITE_DEADLOCK
1820 : if (DEBUG_COND && follower.first != nullptr) {
1821 : std::cout << SIMTIME << " yieldToOppositeWaiting veh=" << vehicle->getID() << " stoppedNeigh=" << stoppedNeigh->getID()
1822 : << " oncoming=" << follower.first->getID()
1823 : << " wait=" << follower.first->getWaitingSeconds()
1824 : << " vehWait=" << vehicle->getWaitingSeconds()
1825 : << " deltaWait=" << STEPS2TIME(deltaWait)
1826 : << "\n";
1827 : }
1828 : #endif
1829 32386 : if (follower.first != nullptr && follower.second < dist && follower.first->getWaitingTime() > vehicle->getWaitingTime() + deltaWait) {
1830 : return true;
1831 : }
1832 : return false;
1833 : }
1834 :
1835 :
1836 : bool
1837 1234492 : MSLaneChanger::resolveDeadlock(MSVehicle* vehicle,
1838 : std::pair<MSVehicle* const, double> leader,
1839 : std::pair<MSVehicle*, double> neighLead,
1840 : std::pair<MSVehicle*, double> overtaken) {
1841 1234492 : const double deadLockZone = overtaken.second;
1842 : #ifdef DEBUG_CHANGE_OPPOSITE_DEADLOCK
1843 : if (DEBUG_COND) {
1844 : std::cout << SIMTIME << " veh=" << vehicle->getID() << " resolveDeadlock waiting=" << vehicle->getWaitingSeconds()
1845 : << " leader=" << Named::getIDSecure(leader.first)
1846 : << " gap=" << leader.second
1847 : << "\n";
1848 : }
1849 : #endif
1850 : if (vehicle->getWaitingSeconds() >= OPPOSITE_OVERTAKING_DEADLOCK_WAIT
1851 1234492 : && leader.first != nullptr && leader.second > vehicle->getVehicleType().getLengthWithGap()) {
1852 : // assume vehicle is halting to avoid deadlock (since there is enough
1853 : // space to drive further)
1854 : // keep halting as long as there is an oncoming vehicle
1855 5740 : std::pair<MSVehicle* const, double> oncomingOpposite = getOncomingOppositeVehicle(vehicle, std::make_pair(nullptr, -1), leader.second);
1856 : #ifdef DEBUG_CHANGE_OPPOSITE_DEADLOCK
1857 : if (DEBUG_COND) {
1858 : std::cout << SIMTIME << " veh=" << vehicle->getID() << " resolveDeadlock"
1859 : << " leader=" << leader.first->getID()
1860 : << " leaderGap=" << leader.second
1861 : << " neighLead=" << Named::getIDSecure(neighLead.first)
1862 : << " deadLockZone=" << deadLockZone
1863 : << "\n";
1864 : }
1865 : #endif
1866 5740 : if (neighLead.first != nullptr && !neighLead.first->isStopped()) {
1867 : // possibly there is an oncoming vehicle before the stoppled leader that
1868 : // could drive due to our yielding
1869 4702 : auto neighLeadFollow = neighLead.first->getFollower(deadLockZone);
1870 4702 : neighLead.second += neighLead.first->getVehicleType().getLengthWithGap() + neighLeadFollow.second;
1871 4702 : neighLead.first = const_cast<MSVehicle*>(neighLeadFollow.first);
1872 : #ifdef DEBUG_CHANGE_OPPOSITE_DEADLOCK
1873 : if (DEBUG_COND) {
1874 : std::cout << " neighLead follower=" << Named::getIDSecure(neighLeadFollow.first) << "\n";
1875 : }
1876 : #endif
1877 : }
1878 :
1879 5740 : if (oncomingOpposite.first != nullptr ||
1880 3908 : (neighLead.first != nullptr && neighLead.first->isStopped()
1881 690 : && yieldToDeadlockOncoming(vehicle, neighLead.first, deadLockZone))) {
1882 1335 : const std::vector<MSVehicle::LaneQ>& preb = vehicle->getBestLanes();
1883 1335 : const double currentDist = preb[vehicle->getLane()->getIndex()].length;
1884 : // mirror code in patchSpeed
1885 1335 : const double blockerLength = currentDist - vehicle->getPositionOnLane() - 1 - vehicle->getVehicleType().getMinGap() - NUMERICAL_EPS;
1886 1335 : const bool reserved = vehicle->getLaneChangeModel().saveBlockerLength(blockerLength, -1);
1887 : #ifdef DEBUG_CHANGE_OPPOSITE_DEADLOCK
1888 : if (DEBUG_COND) {
1889 : std::cout << SIMTIME << " veh=" << vehicle->getID() << " resolveDeadlock"
1890 : << " leader=" << leader.first->getID()
1891 : << " leaderGap=" << leader.second
1892 : << " oncoming=" << Named::getIDSecure(oncomingOpposite.first)
1893 : << " currentDist=" << currentDist
1894 : << " blockerLength=" << blockerLength
1895 : << " reserved=" << reserved
1896 : << "\n";
1897 : }
1898 : #else
1899 : UNUSED_PARAMETER(reserved);
1900 : #endif
1901 1335 : return true;
1902 : }
1903 : }
1904 : return false;
1905 : }
1906 :
1907 :
1908 : double
1909 389526 : MSLaneChanger::computeSafeOppositeLength(MSVehicle* vehicle, double oppositeLength, const MSLane* source, double usableDist,
1910 : std::pair<MSVehicle*, double> oncoming, double vMax, double oncomingSpeed,
1911 : std::pair<MSVehicle*, double> neighLead,
1912 : std::pair<MSVehicle*, double> overtaken,
1913 : std::pair<MSVehicle*, double> neighFollow,
1914 : double surplusGap, const MSLane* opposite,
1915 : bool canOvertake) {
1916 : // compute the remaining distance that can be driven on the opposite side
1917 : // this value will put into oppositeLength of the opposite lanes
1918 : // @note: length counts from the start of the current lane
1919 : // @note: see MSLaneChangeModel::LC2013::_wantsChange @1092 (isOpposite()
1920 : // position on the target lane
1921 389526 : const double forwardPos = source->getOppositePos(vehicle->getPositionOnLane());
1922 :
1923 : // consider usableDist (due to minor links or end of opposite lanes)
1924 389526 : oppositeLength = MIN2(oppositeLength, usableDist + forwardPos);
1925 : // consider upcoming stops
1926 389526 : oppositeLength = MIN2(oppositeLength, vehicle->nextStopDist() + forwardPos);
1927 : #ifdef DEBUG_CHANGE_OPPOSITE
1928 : if (DEBUG_COND) {
1929 : std::cout << " laneQLength=" << oppositeLength << " usableDist=" << usableDist << " forwardPos=" << forwardPos << " stopDist=" << vehicle->nextStopDist() << "\n";
1930 : }
1931 : #endif
1932 : // consider oncoming leaders
1933 389526 : const MSVehicle* oncomingVeh = oncoming.first;
1934 389526 : if (oncomingVeh != 0) {
1935 277964 : if (!oncomingVeh->getLaneChangeModel().isOpposite() && oncomingVeh->getLaneChangeModel().getShadowLane() != source) {
1936 : double egoSpeedFraction = 0.5;
1937 274998 : if (oncomingSpeed > 0) {
1938 182715 : egoSpeedFraction = MIN2(egoSpeedFraction, vMax / (vMax + oncomingSpeed));
1939 : }
1940 274998 : oppositeLength = MIN2(oppositeLength, forwardPos + oncoming.second * egoSpeedFraction);
1941 : #ifdef DEBUG_CHANGE_OPPOSITE
1942 : if (DEBUG_COND) {
1943 : std::cout << SIMTIME << " found oncoming leader=" << oncomingVeh->getID() << " gap=" << oncoming.second
1944 : << " egoSpeedFraction=" << egoSpeedFraction << " newDist=" << oppositeLength << "\n";
1945 : }
1946 : #endif
1947 : } else {
1948 : #ifdef DEBUG_CHANGE_OPPOSITE
1949 : if (DEBUG_COND) {
1950 : std::cout << SIMTIME << " opposite leader=" << oncomingVeh->getID() << " gap=" << oncoming.second << " is driving against the flow\n";
1951 : }
1952 : #endif
1953 : }
1954 277964 : if (neighLead.first != nullptr) {
1955 267579 : if (overtaken.first == nullptr) {
1956 : #ifdef DEBUG_CHANGE_OPPOSITE
1957 : if (DEBUG_COND) {
1958 : std::cout << SIMTIME << " ego=" << vehicle->getID() << " did not find columnleader to overtake\n";
1959 : }
1960 : #endif
1961 239684 : } else if (oncomingVeh != nullptr && oncomingVeh->isStopped()
1962 77140 : && neighLead.second > 0
1963 37704 : && neighFollow.second > 0
1964 8427 : && yieldToOppositeWaiting(vehicle, oncomingVeh, 10, TIME2STEPS(60))) {
1965 : // merge back into the forward lane
1966 213 : oppositeLength = forwardPos + neighLead.second;
1967 : } else {
1968 239471 : if (surplusGap > 0) {
1969 : // exaggerate remaining dist so that the vehicle continues
1970 : // overtaking (otherwise the lane change model might abort prematurely)
1971 136828 : oppositeLength += 1000;
1972 : } else {
1973 : // return from the opposite side ahead of the unpassable column (unless overlapping)
1974 102643 : if (overtaken.second > 0) {
1975 100101 : oppositeLength = MIN2(oppositeLength, forwardPos + overtaken.second);
1976 : }
1977 : // (don't set the distance so low as to imply emergency braking)
1978 205286 : oppositeLength = MAX2(oppositeLength, forwardPos + vehicle->getCarFollowModel().brakeGap(vehicle->getSpeed()));
1979 : }
1980 : #ifdef DEBUG_CHANGE_OPPOSITE
1981 : if (DEBUG_COND) {
1982 : std::cout << SIMTIME << " ego=" << vehicle->getID() << " is overtaking " << overtaken.first->getID()
1983 : << " surplusGap=" << surplusGap
1984 : << " final laneQLength=" << oppositeLength
1985 : << "\n";
1986 : }
1987 : #endif
1988 : }
1989 : }
1990 : } else {
1991 111562 : if (overtaken.first == nullptr || !canOvertake) {
1992 : // there is no reason to stay on the opposite side
1993 30458 : std::pair<MSVehicle* const, double> oppFollow = opposite->getOppositeFollower(vehicle);
1994 30458 : if (oppFollow.first == nullptr) {
1995 : oppositeLength = forwardPos;
1996 : } else {
1997 57458 : const double secureGap = oppFollow.first->getCarFollowModel().getSecureGap(
1998 28729 : oppFollow.first, vehicle, oppFollow.first->getSpeed(), vehicle->getSpeed(), vehicle->getCarFollowModel().getMaxDecel());
1999 : #ifdef DEBUG_CHANGE_OPPOSITE
2000 : if (DEBUG_COND) {
2001 : std::cout << SIMTIME << " ego=" << vehicle->getID() << " neighFollow=" << oppFollow.first->getID() << " gap=" << oppFollow.second << " secureGap=" << secureGap << "\n";
2002 : }
2003 : #endif
2004 28729 : if (oppFollow.second > secureGap) {
2005 : // back gap is safe for immidiate return
2006 : oppositeLength = forwardPos;
2007 : }
2008 : }
2009 : }
2010 : }
2011 : #ifdef DEBUG_CHANGE_OPPOSITE
2012 : if (DEBUG_COND) {
2013 : std::cout << SIMTIME << " veh=" << vehicle->getID() << " remaining dist=" << oppositeLength - forwardPos << " forwardPos=" << forwardPos << " oppositeLength=" << oppositeLength << "\n";
2014 : }
2015 : #endif
2016 389526 : return oppositeLength;
2017 : }
2018 :
2019 :
2020 : std::pair<MSVehicle* const, double>
2021 2732688 : MSLaneChanger::getOncomingVehicle(const MSLane* opposite, std::pair<MSVehicle*, double> oncoming,
2022 : double searchDist, double& vMax, const MSVehicle* overtaken, MSLane::MinorLinkMode mLinkMode) {
2023 : double gap = oncoming.second;
2024 4657478 : while (oncoming.first != nullptr && (oncoming.first->getLaneChangeModel().isOpposite() || oncoming.first->getLaneChangeModel().getShadowLane() == opposite)) {
2025 1984920 : searchDist -= (oncoming.first->getVehicleType().getLengthWithGap() + MAX2(0.0, oncoming.second));
2026 : // leader is itself overtaking through the opposite side. find real oncoming vehicle
2027 1984920 : gap += oncoming.first->getVehicleType().getLengthWithGap();
2028 1984920 : if (oncoming.first != overtaken) {
2029 3001844 : vMax = MIN2(vMax, oncoming.first->getSpeed());
2030 : } // else: might be the case if we are overtaking a vehicle that is stopped on the opposite side
2031 : #ifdef DEBUG_CHANGE_OPPOSITE
2032 : if (gDebugFlag5) {
2033 : std::cout << SIMTIME << " oncoming=" << oncoming.first->getID() << " isOpposite gap=" << oncoming.second
2034 : << " totalGap=" << gap << " searchDist=" << searchDist << " vMax=" << vMax << "\n";
2035 : }
2036 : #endif
2037 1984920 : if (searchDist < 0) {
2038 : break;
2039 : }
2040 : // getOppositeLeader resets the search postion by ego length and may thus create cycles
2041 1924790 : if (oncoming.first->getLaneChangeModel().getShadowLane() != opposite) {
2042 1849020 : opposite = oncoming.first->getLane();
2043 : }
2044 1924790 : oncoming = opposite->getFollower(oncoming.first, oncoming.first->getPositionOnLane(opposite), searchDist, mLinkMode);
2045 1924790 : if (oncoming.first != nullptr) {
2046 1690172 : gap += oncoming.second + oncoming.first->getVehicleType().getLength();
2047 : #ifdef DEBUG_CHANGE_OPPOSITE
2048 : if (gDebugFlag5) {
2049 : std::cout << SIMTIME << " oncoming=" << oncoming.first->getID() << " gap=" << oncoming.second << " totalGap=" << gap << "\n";
2050 : }
2051 : #endif
2052 : }
2053 : }
2054 : oncoming.second = gap;
2055 2732688 : return oncoming;
2056 : }
2057 :
2058 :
2059 : std::pair<MSVehicle* const, double>
2060 2737003 : MSLaneChanger::getOncomingOppositeVehicle(const MSVehicle* vehicle, std::pair<MSVehicle*, double> overtaken, double searchDist) {
2061 : double gap = 0;
2062 : const MSVehicle* front = nullptr;
2063 2737003 : if (overtaken.first != nullptr) {
2064 2688966 : gap += overtaken.second + overtaken.first->getVehicleType().getLengthWithGap();
2065 : front = overtaken.first;
2066 : } else {
2067 : // only for special situations (i.e. traci-triggered)
2068 : front = vehicle;
2069 : }
2070 : // we only need to look for the next leader: If it's driving in the forward
2071 : // direction, it "protects" from oncoming opposite vehicles.
2072 : // all leader vehicles on the current laneChanger edge are already moved into MSLane::myTmpVehicles
2073 2737003 : const bool checkTmpVehicles = front->getLane() == vehicle->getLane();
2074 2737003 : std::vector<MSLane*> conts = vehicle->getBestLanesContinuation();
2075 3000874 : while (conts.size() > 0 && conts.front() != front->getLane()) {
2076 : conts.erase(conts.begin());
2077 : }
2078 2737003 : std::pair<MSVehicle* const, double> oncoming = front->getLane()->getLeader(front, front->getPositionOnLane(), conts, searchDist, checkTmpVehicles);
2079 2737003 : if (oncoming.first != nullptr) {
2080 2019579 : const bool isOpposite = oncoming.first->getLaneChangeModel().isOpposite();
2081 2019579 : const MSLane* shadowLane = oncoming.first->getLaneChangeModel().getShadowLane();
2082 : #ifdef DEBUG_CHANGE_OPPOSITE
2083 : if (gDebugFlag5) {
2084 : std::cout << SIMTIME
2085 : << " front=" << front->getID() << " searchDist=" << searchDist
2086 : << " oncomingOpposite=" << oncoming.first->getID()
2087 : << " gap=" << oncoming.second
2088 : << " isOpposite=" << isOpposite
2089 : << " shadowLane=" << Named::getIDSecure(shadowLane)
2090 : << "\n";
2091 : }
2092 : #endif
2093 2019579 : if (isOpposite && shadowLane != front->getLane()) {
2094 : // distance was to back position (but the vehicle is oncoming)
2095 493890 : oncoming.second -= oncoming.first->getVehicleType().getLength();
2096 493890 : oncoming.second += gap;
2097 493890 : return oncoming;
2098 : }
2099 : }
2100 2243113 : return std::make_pair(nullptr, -1);
2101 2737003 : }
2102 :
2103 :
2104 : double
2105 2761059 : MSLaneChanger::computeSurplusGap(const MSVehicle* vehicle, const MSLane* opposite, std::pair<MSVehicle*, double> oncoming,
2106 : double timeToOvertake, double spaceToOvertake, double& oncomingSpeed, bool oncomingOpposite) {
2107 : double surplusGap = std::numeric_limits<double>::max();
2108 2761059 : const MSVehicle* oncomingVeh = oncoming.first;
2109 2761059 : if (oncomingVeh != 0 && (oncomingOpposite
2110 1927399 : || (!oncomingVeh->getLaneChangeModel().isOpposite()
2111 1894259 : && oncomingVeh->getLaneChangeModel().getShadowLane() != opposite))) {
2112 : // conservative: assume that the oncoming vehicle accelerates to its maximum speed
2113 : // unless it has been standing (then assume it is trying to let us pass
2114 : // to avoid deadlock)
2115 4075104 : oncomingSpeed = (oncomingVeh->isStopped() || oncomingVeh->getWaitingSeconds() >= OPPOSITE_OVERTAKING_DEADLOCK_WAIT
2116 3533434 : ? 0 : oncomingVeh->getLane()->getVehicleMaxSpeed(oncomingVeh));
2117 2317479 : const double safetyGap = ((oncomingSpeed + vehicle->getLane()->getVehicleMaxSpeed(vehicle))
2118 2317479 : * vehicle->getCarFollowModel().getHeadwayTime()
2119 2317479 : * OPPOSITE_OVERTAKING_SAFETYGAP_HEADWAY_FACTOR);
2120 2317479 : surplusGap = oncoming.second - spaceToOvertake - timeToOvertake * oncomingSpeed - safetyGap;
2121 : #ifdef DEBUG_CHANGE_OPPOSITE
2122 : if (DEBUG_COND) {
2123 : std::cout << SIMTIME
2124 : << " oncoming=" << oncomingVeh->getID()
2125 : << " oGap=" << oncoming.second
2126 : << " oSpeed=" << oncomingSpeed
2127 : << " sto=" << spaceToOvertake
2128 : << " tto=" << timeToOvertake
2129 : << " safetyGap=" << safetyGap
2130 : << " surplusGap=" << surplusGap
2131 : << "\n";
2132 : }
2133 : #endif
2134 : }
2135 2761059 : return surplusGap;
2136 : }
2137 :
2138 : bool
2139 149475 : MSLaneChanger::foundHilltop(MSVehicle* vehicle, bool foundHill, double searchDist, const std::vector<MSLane*>& bestLanes, int view, double pos, double lastMax, double hilltopThreshold) {
2140 149475 : if (view >= (int)bestLanes.size()) {
2141 : return false;
2142 : }
2143 146463 : MSLane* lane = bestLanes[view];
2144 : double laneDist = 0;
2145 : const PositionVector& shape = lane->getShape();
2146 : double lastZ = lastMax;
2147 3953012 : for (int i = 1; i < (int)shape.size(); i++) {
2148 3930583 : const double dist = lane->interpolateGeometryPosToLanePos(shape[i - 1].distanceTo(shape[i]));
2149 3930583 : laneDist += dist;
2150 3930583 : if (laneDist > pos) {
2151 2118615 : const double z = shape[i].z();
2152 2118615 : if (z > lastMax) {
2153 : lastMax = z;
2154 : }
2155 2118615 : if (z > lastZ) {
2156 : foundHill = true;
2157 : }
2158 : lastZ = z;
2159 : #ifdef DEBUG_CHANGE_OPPOSITE
2160 : if (DEBUG_COND) {
2161 : std::cout << SIMTIME << " foundHill=" << foundHill << " searchDist=" << searchDist << " lastMax=" << lastMax << " lane=" << lane->getID() << " laneDist=" << laneDist << " z=" << z << "\n";
2162 : }
2163 : #endif
2164 2118615 : if (foundHill && z < lastMax) {
2165 1647938 : const double drop = lastMax - z;
2166 : //std::cout << SIMTIME << " searchDist=" << searchDist << " hillDrop=" << drop << " lastMax=" << lastMax << " lane=" << lane->getID() << " laneDist=" << laneDist << " z=" << z << "\n";
2167 1647938 : if (drop > hilltopThreshold) {
2168 : #ifdef DEBUG_CHANGE_OPPOSITE
2169 : if (DEBUG_COND) {
2170 : std::cout << " cannot changeOpposite before the top of a hill searchDist=" << searchDist << " hillDrop=" << drop
2171 : << " lastMax=" << lastMax << " lane=" << lane->getID() << " laneDist=" << laneDist << " z=" << z << "\n";
2172 : }
2173 : #endif
2174 : return true;
2175 : }
2176 : }
2177 2018684 : if (pos == 0) {
2178 1891640 : searchDist -= dist;
2179 : } else {
2180 127044 : searchDist -= laneDist - pos;
2181 : pos = 0;
2182 : }
2183 2018684 : if (searchDist <= 0) {
2184 : return false;
2185 : }
2186 : }
2187 : }
2188 22429 : return foundHilltop(vehicle, foundHill, searchDist, bestLanes, view + 1, 0, lastMax, hilltopThreshold);
2189 : }
2190 :
2191 :
2192 : bool
2193 456603 : MSLaneChanger::checkChangeOpposite(
2194 : MSVehicle* vehicle,
2195 : int laneOffset,
2196 : MSLane* targetLane,
2197 : const std::pair<MSVehicle* const, double>& leader,
2198 : const std::pair<MSVehicle* const, double>& neighLead,
2199 : const std::pair<MSVehicle* const, double>& neighFollow,
2200 : const std::vector<MSVehicle::LaneQ>& preb) {
2201 456603 : const bool isOpposite = vehicle->getLaneChangeModel().isOpposite();
2202 : MSLane* source = vehicle->getMutableLane();
2203 456603 : const std::pair<MSVehicle* const, double> follower(nullptr, -1);
2204 456603 : int state = checkChange(laneOffset, targetLane, leader, follower, neighLead, neighFollow, preb);
2205 456603 : vehicle->getLaneChangeModel().setOwnState(state);
2206 456603 : bool changingAllowed = (state & LCA_BLOCKED) == 0;
2207 : // change if the vehicle wants to and is allowed to change
2208 456603 : if ((state & LCA_WANTS_LANECHANGE) != 0 && changingAllowed
2209 : // do not change to the opposite direction for cooperative reasons
2210 38949 : && (isOpposite || (state & LCA_COOPERATIVE) == 0)) {
2211 38949 : const bool continuous = vehicle->getLaneChangeModel().startLaneChangeManeuver(source, targetLane, laneOffset);
2212 : #ifdef DEBUG_CHANGE_OPPOSITE
2213 : if (DEBUG_COND) {
2214 : std::cout << SIMTIME << " changing to opposite veh=" << vehicle->getID() << " dir=" << laneOffset << " opposite=" << Named::getIDSecure(targetLane)
2215 : << " state=" << toString((LaneChangeAction)state) << "\n";
2216 : }
2217 : #endif
2218 38949 : if (continuous) {
2219 2632 : continueChange(vehicle, myCandi);
2220 : }
2221 38949 : return true;
2222 : }
2223 : #ifdef DEBUG_CHANGE_OPPOSITE
2224 : if (DEBUG_COND) {
2225 : std::cout << SIMTIME << " not changing to opposite veh=" << vehicle->getID() << " dir=" << laneOffset
2226 : << " opposite=" << Named::getIDSecure(targetLane) << " state=" << toString((LaneChangeAction)state) << "\n";
2227 : }
2228 : #endif
2229 : return false;
2230 : }
2231 :
2232 :
2233 : void
2234 5305204 : MSLaneChanger::computeOvertakingTime(const MSVehicle* vehicle, double vMax, const MSVehicle* leader, double gap, double& timeToOvertake, double& spaceToOvertake) {
2235 : // Assumptions:
2236 : // - leader maintains the current speed
2237 : // - vehicle merges with maxSpeed ahead of leader
2238 : // XXX affected by ticket #860 (the formula is invalid for the current position update rule)
2239 :
2240 : // first compute these values for the case where vehicle is accelerating
2241 : // without upper bound on speed
2242 5305204 : const double v = vehicle->getSpeed();
2243 5305204 : const double u = leader->getAcceleration() > 0 ? leader->getLane()->getVehicleMaxSpeed(leader) : leader->getSpeed();
2244 : const double a = vehicle->getCarFollowModel().getMaxAccel();
2245 : const double d = vehicle->getCarFollowModel().getMaxDecel();
2246 5305204 : const double g = MAX2(0.0, (
2247 : // drive up to the rear of leader
2248 5305204 : gap + vehicle->getVehicleType().getMinGap()
2249 : // drive head-to-head with the leader
2250 5305204 : + leader->getVehicleType().getLengthWithGap()
2251 : // drive past the leader
2252 5305204 : + vehicle->getVehicleType().getLength()
2253 : // allow for safe gap between leader and vehicle
2254 5305204 : + leader->getCarFollowModel().getSecureGap(leader, vehicle, u, vMax, d))
2255 : // time to move between lanes
2256 5305204 : + (MSGlobals::gSublane ? vMax * vehicle->getLane()->getWidth() / vehicle->getVehicleType().getMaxSpeedLat() : 0));
2257 : const double sign = -1; // XXX recheck
2258 : // v*t + t*t*a*0.5 = g + u*t
2259 : // solve t
2260 : // t = ((u - v - (((((2.0*(u - v))**2.0) + (8.0*a*g))**(1.0/2.0))*sign/2.0))/a)
2261 5305204 : double t = (u - v - sqrt(4 * (u - v) * (u - v) + 8 * a * g) * sign * 0.5) / a;
2262 : #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
2263 : if (DEBUG_COND) {
2264 : std::cout << " computeOvertakingTime v=" << v << " vMax=" << vMax << " u=" << u << " a=" << a << " d=" << d << " gap=" << gap << " g=" << g << " t=" << t
2265 : << " distEgo=" << v* t + t* t* a * 0.5 << " distLead=" << g + u* t
2266 : << "\n";
2267 : }
2268 : #endif
2269 : assert(t >= 0);
2270 5305204 : if (vMax <= u) {
2271 : // do not try to overtake faster leader
2272 1370869 : timeToOvertake = std::numeric_limits<double>::max();
2273 1370869 : spaceToOvertake = std::numeric_limits<double>::max();
2274 1370869 : return;
2275 : }
2276 :
2277 : // allow for a safety time gap
2278 3934335 : t += OPPOSITE_OVERTAKING_SAFE_TIMEGAP;
2279 : // round to multiples of step length (TS)
2280 3934335 : if (u > 0) {
2281 2292836 : t = ceil(t / TS) * TS;
2282 : }
2283 :
2284 : /// XXX ignore speed limit when overtaking through the opposite lane?
2285 3934335 : const double timeToMaxSpeed = (vMax - v) / a;
2286 :
2287 : #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
2288 : if (DEBUG_COND) {
2289 : std::cout << " t=" << t << " tvMax=" << timeToMaxSpeed << "\n";
2290 : }
2291 : #endif
2292 3934335 : if (t <= timeToMaxSpeed) {
2293 185652 : timeToOvertake = t;
2294 185652 : spaceToOvertake = v * t + t * t * a * 0.5;
2295 : #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
2296 : if (DEBUG_COND) {
2297 : std::cout << " sto=" << spaceToOvertake << "\n";
2298 : }
2299 : #endif
2300 : } else {
2301 : // space until max speed is reached
2302 3748683 : const double s = v * timeToMaxSpeed + timeToMaxSpeed * timeToMaxSpeed * a * 0.5;
2303 : const double m = timeToMaxSpeed;
2304 : // s + (t-m) * vMax = g + u*t
2305 : // solve t
2306 3748683 : t = (g - s + m * vMax) / (vMax - u);
2307 3748683 : if (t < 0) {
2308 : // cannot overtake in time
2309 : #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
2310 : if (DEBUG_COND) {
2311 : std::cout << " t2=" << t << "\n";
2312 : }
2313 : #endif
2314 0 : timeToOvertake = std::numeric_limits<double>::max();
2315 0 : spaceToOvertake = std::numeric_limits<double>::max();
2316 0 : return;
2317 : } else {
2318 : // allow for a safety time gap
2319 3748683 : t += OPPOSITE_OVERTAKING_SAFE_TIMEGAP;
2320 : // round to multiples of step length (TS)
2321 3748683 : if (u > 0) {
2322 2268887 : t = ceil(t / TS) * TS;
2323 : }
2324 3748683 : timeToOvertake = t;
2325 3748683 : spaceToOvertake = s + (t - m) * vMax;
2326 : #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
2327 : if (DEBUG_COND) {
2328 : std::cout << " t2=" << t << " s=" << s << " sto=" << spaceToOvertake << " m=" << m << "\n";
2329 : }
2330 : #endif
2331 : }
2332 : }
2333 3934335 : const double safetyFactor = OPPOSITE_OVERTAKING_SAFETY_FACTOR * vehicle->getLaneChangeModel().getOppositeSafetyFactor();
2334 3934335 : timeToOvertake *= safetyFactor;
2335 3934335 : if (STEPS2TIME(leader->getStopDuration()) < timeToOvertake) {
2336 2493970 : spaceToOvertake *= safetyFactor;
2337 : }
2338 3934335 : double frac = fmod(timeToOvertake, TS);
2339 3934335 : if (frac > 0) {
2340 : // round to full sim step
2341 3618552 : timeToOvertake += TS - frac;
2342 : }
2343 : #ifdef DEBUG_CHANGE_OPPOSITE_OVERTAKINGTIME
2344 : if (DEBUG_COND) {
2345 : if (safetyFactor != 1) {
2346 : std::cout << " applying safetyFactor=" << safetyFactor
2347 : << " leaderStopTime=" << STEPS2TIME(leader->getStopDuration())
2348 : << " tto=" << timeToOvertake << " sto=" << spaceToOvertake << "\n";
2349 : }
2350 : }
2351 : #endif
2352 : }
2353 :
2354 :
2355 :
2356 : std::pair<MSVehicle*, double>
2357 6913925 : MSLaneChanger::getColumnleader(double& maxSpace, MSVehicle* vehicle, std::pair<MSVehicle*, double> leader, double maxLookAhead) {
2358 : assert(leader.first != 0);
2359 6913925 : const bool isEmergency = vehicle->getVClass() == SVC_EMERGENCY;
2360 6913925 : const MSLane* source = vehicle->getLane();
2361 : // find a leader vehicle with sufficient space ahead for merging back
2362 6913925 : const double overtakingSpeed = source->getVehicleMaxSpeed(vehicle); // just a guess
2363 : const double mergeBrakeGap = vehicle->getCarFollowModel().brakeGap(overtakingSpeed);
2364 : std::pair<MSVehicle*, double> columnLeader = leader;
2365 : double egoGap = leader.second;
2366 : bool foundSpaceAhead = false;
2367 6913925 : double seen = leader.second + leader.first->getVehicleType().getLengthWithGap();
2368 6913925 : std::vector<MSLane*> conts = vehicle->getBestLanesContinuation();
2369 6913925 : if (maxLookAhead == std::numeric_limits<double>::max()) {
2370 : maxLookAhead = (isEmergency
2371 6908849 : ? OPPOSITE_OVERTAKING_MAX_LOOKAHEAD_EMERGENCY
2372 : : OPPOSITE_OVERTAKING_MAX_LOOKAHEAD);
2373 6908849 : maxLookAhead = MAX2(maxLookAhead, mergeBrakeGap + 10
2374 6908849 : + vehicle->getVehicleType().getLengthWithGap()
2375 6908849 : + leader.first->getVehicleType().getLengthWithGap());
2376 : }
2377 : #ifdef DEBUG_CHANGE_OPPOSITE
2378 : if (DEBUG_COND) {
2379 : std::cout << " getColumnleader vehicle=" << vehicle->getID() << " leader=" << leader.first->getID() << " gap=" << leader.second << " maxLookAhead=" << maxLookAhead << "\n";
2380 : }
2381 : #endif
2382 6913925 : const double safetyFactor = OPPOSITE_OVERTAKING_SAFETY_FACTOR * vehicle->getLaneChangeModel().getOppositeSafetyFactor();
2383 44125267 : while (!foundSpaceAhead) {
2384 39884715 : const double requiredSpaceAfterLeader = (columnLeader.first->getCarFollowModel().getSecureGap(
2385 : columnLeader.first, vehicle,
2386 39884715 : columnLeader.first->getSpeed(), overtakingSpeed, vehicle->getCarFollowModel().getMaxDecel())
2387 39884715 : + columnLeader.first->getVehicleType().getMinGap()
2388 39884715 : + vehicle->getVehicleType().getLengthWithGap());
2389 :
2390 :
2391 : // all leader vehicles on the current laneChanger edge are already moved into MSLane::myTmpVehicles
2392 39884715 : const bool checkTmpVehicles = (&columnLeader.first->getLane()->getEdge() == &source->getEdge());
2393 39884715 : double searchStart = columnLeader.first->getPositionOnLane();
2394 39884715 : std::pair<MSVehicle*, double> leadLead = columnLeader.first->getLane()->getLeader(
2395 : columnLeader.first, searchStart, conts, requiredSpaceAfterLeader + mergeBrakeGap,
2396 : checkTmpVehicles);
2397 : std::set<MSVehicle*> seenLeaders;
2398 40683145 : while (leadLead.first != nullptr && leadLead.first->getLaneChangeModel().isOpposite()) {
2399 : #ifdef DEBUG_CHANGE_OPPOSITE
2400 : if (DEBUG_COND) {
2401 : std::cout << " skipping opposite leadLead=" << leadLead.first->getID() << " gap=" << leadLead.second << "\n";
2402 : }
2403 : #endif
2404 1034105 : if (leadLead.second + seen > maxLookAhead || seenLeaders.count(leadLead.first) > 0) {
2405 235675 : leadLead.first = nullptr;
2406 235675 : break;
2407 : }
2408 : seenLeaders.insert(leadLead.first);
2409 : // found via shadow lane, skip it
2410 798430 : const double searchStart2 = searchStart + MAX2(0.0, leadLead.second) + leadLead.first->getVehicleType().getLengthWithGap();
2411 798430 : leadLead = columnLeader.first->getLane()->getLeader(
2412 : columnLeader.first, searchStart2, conts, requiredSpaceAfterLeader + mergeBrakeGap,
2413 : checkTmpVehicles);
2414 798430 : leadLead.second += (searchStart2 - searchStart);
2415 : }
2416 39884715 : if (leadLead.first == nullptr) {
2417 1797418 : double availableSpace = columnLeader.first->getLane()->getLength() - columnLeader.first->getPositionOnLane();
2418 1797418 : double requiredSpace = safetyFactor * requiredSpaceAfterLeader;
2419 1797418 : if (!columnLeader.first->isStopped()) {
2420 : // if the leader is stopped we can trade space for time
2421 835804 : requiredSpace += safetyFactor * mergeBrakeGap;
2422 : }
2423 : #ifdef DEBUG_CHANGE_OPPOSITE
2424 : if (DEBUG_COND) {
2425 : std::cout << " no direct leader found after columnLeader " << columnLeader.first->getID()
2426 : << " availableSpace=" << availableSpace
2427 : << " reqAfterLeader=" << requiredSpaceAfterLeader
2428 : << " ovSpeed=" << overtakingSpeed
2429 : << " reqBGap=" << mergeBrakeGap
2430 : << " reqMin=" << requiredSpace / safetyFactor
2431 : << " req=" << requiredSpace
2432 : << "\n";
2433 : }
2434 : #endif
2435 1797418 : if (availableSpace > requiredSpace) {
2436 : foundSpaceAhead = true;
2437 : } else {
2438 : // maybe the columnleader is stopped before a junction or takes a different turn.
2439 : // try to find another columnleader on successive lanes
2440 394203 : bool contsEnd = false;
2441 394203 : const MSLane* next = getLaneAfter(columnLeader.first->getLane(), conts, isEmergency, contsEnd);
2442 : #ifdef DEBUG_CHANGE_OPPOSITE
2443 : if (DEBUG_COND) {
2444 : std::cout << " look for another leader on lane " << Named::getIDSecure(next) << "\n";
2445 : }
2446 : #endif
2447 427549 : while (next != nullptr && seen < maxLookAhead) {
2448 102465 : seen += next->getLength();
2449 102465 : MSVehicle* cand = next->getLastAnyVehicle();
2450 102465 : if (cand == nullptr) {
2451 81248 : availableSpace += next->getLength();
2452 81248 : if (availableSpace > requiredSpace) {
2453 : foundSpaceAhead = true;
2454 : break;
2455 : }
2456 33346 : next = getLaneAfter(next, conts, isEmergency, contsEnd);
2457 : } else {
2458 21217 : availableSpace += cand->getBackPositionOnLane();
2459 21217 : if (availableSpace > requiredSpace) {
2460 : foundSpaceAhead = true;
2461 : break;
2462 : } else {
2463 5076 : return getColumnleader(maxSpace, vehicle, std::make_pair(cand, availableSpace + cand->getPositionOnLane()), maxLookAhead - seen);
2464 : }
2465 : }
2466 : }
2467 : #ifdef DEBUG_CHANGE_OPPOSITE
2468 : if (DEBUG_COND) {
2469 : std::cout << " foundSpaceAhead=" << foundSpaceAhead << " availableSpace=" << availableSpace << " next=" << Named::getIDSecure(next) << " contsEnd=" << contsEnd << " conts=" << toString(conts) << "\n";
2470 : }
2471 : #endif
2472 389127 : if (!foundSpaceAhead && contsEnd) {
2473 : foundSpaceAhead = true;
2474 : availableSpace = requiredSpace;
2475 : }
2476 389127 : if (!foundSpaceAhead) {
2477 89920 : return std::make_pair(nullptr, -1);
2478 : }
2479 : }
2480 1702422 : maxSpace = egoGap + columnLeader.first->getVehicleType().getLength() + availableSpace;
2481 : } else {
2482 38087297 : const double sGap = vehicle->getCarFollowModel().getSecureGap(vehicle, leadLead.first,
2483 38087297 : overtakingSpeed, leadLead.first->getSpeed(), leadLead.first->getCarFollowModel().getMaxDecel());
2484 38087297 : double requiredSpace = safetyFactor * requiredSpaceAfterLeader;
2485 38087297 : if (!columnLeader.first->isStopped()) {
2486 : // if the leader is stopped we can trade space for time
2487 34950418 : requiredSpace += safetyFactor * sGap;
2488 : }
2489 : #ifdef DEBUG_CHANGE_OPPOSITE
2490 : if (DEBUG_COND) {
2491 : std::cout << " leader's leader " << leadLead.first->getID() << " space=" << leadLead.second
2492 : << " reqAfterLeader=" << requiredSpaceAfterLeader
2493 : << " ovSpeed=" << overtakingSpeed
2494 : << " reqSGap=" << sGap
2495 : << " reqMin=" << requiredSpace / safetyFactor
2496 : << " req=" << requiredSpace
2497 : << "\n";
2498 : }
2499 : #endif
2500 38087297 : if (leadLead.second > requiredSpace) {
2501 : foundSpaceAhead = true;
2502 2538130 : maxSpace = egoGap + columnLeader.first->getVehicleType().getLength() + leadLead.second;
2503 : } else {
2504 :
2505 35549167 : if (leadLead.second < 0) {
2506 : // must be a junction leader or some other dangerous situation
2507 : #ifdef DEBUG_CHANGE_OPPOSITE
2508 : if (DEBUG_COND) {
2509 : std::cout << " leader's leader " << leadLead.first->getID() << " gap=" << leadLead.second << " is junction leader (aborting)\n";
2510 : }
2511 : #endif
2512 162419 : return std::make_pair(nullptr, -1);
2513 : }
2514 :
2515 : #ifdef DEBUG_CHANGE_OPPOSITE
2516 : if (DEBUG_COND) {
2517 : std::cout << " not enough space after columnLeader=" << columnLeader.first->getID() << " required=" << requiredSpace << "\n";
2518 : }
2519 : #endif
2520 35386748 : seen += MAX2(0., leadLead.second) + leadLead.first->getVehicleType().getLengthWithGap();
2521 35386748 : if (seen > maxLookAhead) {
2522 : #ifdef DEBUG_CHANGE_OPPOSITE
2523 : if (DEBUG_COND) {
2524 : std::cout << " cannot changeOpposite due to insufficient free space after columnLeader (seen=" << seen << " columnLeader=" << columnLeader.first->getID() << ")\n";
2525 : }
2526 : #endif
2527 2415958 : return std::make_pair(nullptr, -1);
2528 : }
2529 : // see if merging after leadLead is possible
2530 32970790 : egoGap += columnLeader.first->getVehicleType().getLengthWithGap() + leadLead.second;
2531 : columnLeader = leadLead;
2532 : #ifdef DEBUG_CHANGE_OPPOSITE
2533 : if (DEBUG_COND) {
2534 : std::cout << " new columnLeader=" << columnLeader.first->getID() << "\n";
2535 : }
2536 : #endif
2537 : }
2538 : }
2539 : }
2540 : columnLeader.second = egoGap;
2541 4240552 : return columnLeader;
2542 6913925 : }
2543 :
2544 :
2545 : const MSLane*
2546 427549 : MSLaneChanger::getLaneAfter(const MSLane* lane, const std::vector<MSLane*>& conts, bool allowMinor, bool& contsEnd) {
2547 558028 : for (auto it = conts.begin(); it != conts.end(); ++it) {
2548 537491 : if (*it == lane) {
2549 407012 : if (it + 1 != conts.end()) {
2550 : // abort on minor link
2551 171848 : const MSLane* next = *(it + 1);
2552 171848 : const MSLink* link = lane->getLinkTo(next);
2553 171848 : if (link == nullptr || (!allowMinor && !link->havePriority())) {
2554 : return nullptr;
2555 : }
2556 : return next;
2557 : } else {
2558 235164 : contsEnd = true;
2559 235164 : return nullptr;
2560 : }
2561 : }
2562 : }
2563 : return nullptr;
2564 : }
2565 :
2566 : double
2567 4826049 : MSLaneChanger::getMaxOvertakingSpeed(const MSVehicle* vehicle, double maxSpaceToOvertake) {
2568 : // v*v/2*a + v*v/2*d = maxSpaceToOvertake
2569 : const double a = vehicle->getCarFollowModel().getMaxAccel();
2570 : const double d = vehicle->getCarFollowModel().getMaxDecel();
2571 4826049 : const double v = sqrt(2 * maxSpaceToOvertake * a * d / (a + d));
2572 4826049 : return v;
2573 : }
2574 :
2575 :
2576 : std::pair<double, SUMOTime>
2577 563727533 : MSLaneChanger::getLastBlocked(int index) const {
2578 : assert(index >= 0 && index < (int)myChanger.size());
2579 : return std::make_pair(
2580 : myChanger[index].lastBlockedBackPos,
2581 563727533 : myChanger[index].lastBlockedWaitingTime);
2582 : }
2583 :
2584 : /****************************************************************************/
|
