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