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