Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2002-2025 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file MSLaneChangerSublane.cpp
15 : /// @author Jakob Erdmann
16 : /// @author Leonhard Luecken
17 : /// @date Oct 2015
18 : ///
19 : // Performs sub-lane changing of vehicles
20 : /****************************************************************************/
21 : #include <config.h>
22 :
23 : #include "MSLaneChangerSublane.h"
24 : #include "MSNet.h"
25 : #include "MSLink.h"
26 : #include "MSVehicle.h"
27 : #include "MSVehicleType.h"
28 : #include "MSVehicleTransfer.h"
29 : #include "MSGlobals.h"
30 : #include <cassert>
31 : #include <iterator>
32 : #include <cstdlib>
33 : #include <cmath>
34 : #include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
35 : #include <utils/common/MsgHandler.h>
36 : #include <utils/geom/GeomHelper.h>
37 :
38 :
39 : // ===========================================================================
40 : // DEBUG constants
41 : // ===========================================================================
42 : #define DEBUG_COND (vehicle->getLaneChangeModel().debugVehicle())
43 : //#define DEBUG_COND (vehicle->getID() == "disabled")
44 : //#define DEBUG_COND true
45 : //#define DEBUG_DECISION
46 : //#define DEBUG_ACTIONSTEPS
47 : //#define DEBUG_STATE
48 : //#define DEBUG_MANEUVER
49 : //#define DEBUG_SURROUNDING
50 : //#define DEBUG_CHANGE_OPPOSITE
51 :
52 : // ===========================================================================
53 : // member method definitions
54 : // ===========================================================================
55 176315 : MSLaneChangerSublane::MSLaneChangerSublane(const std::vector<MSLane*>* lanes, bool allowChanging) :
56 176315 : MSLaneChanger(lanes, allowChanging) {
57 : // initialize siblings
58 176315 : if (myChanger.front().lane->isInternal()) {
59 200777 : for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
60 232016 : for (ChangerIt ce2 = myChanger.begin(); ce2 != myChanger.end(); ++ce2) {
61 127143 : if (ce != ce2 && ce->lane->getIncomingLanes().front().lane == ce2->lane->getIncomingLanes().front().lane) {
62 : //std::cout << "addSibling lane=" << ce->lane->getID() << " offset=" << ce2->lane->getIndex() - ce->lane->getIndex() << "\n";
63 3726 : ce->siblings.push_back(ce2->lane->getIndex() - ce->lane->getIndex());
64 : }
65 : }
66 : }
67 : }
68 176315 : }
69 :
70 :
71 348840 : MSLaneChangerSublane::~MSLaneChangerSublane() {}
72 :
73 : void
74 12853286 : MSLaneChangerSublane::initChanger() {
75 12853286 : MSLaneChanger::initChanger();
76 : // Prepare myChanger with a safe state.
77 32604969 : for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
78 39503366 : ce->ahead = ce->lane->getPartialBeyond();
79 : ce->outsideBounds.clear();
80 : // std::cout << SIMTIME << " initChanger lane=" << ce->lane->getID() << " vehicles=" << toString(ce->lane->myVehicles) << "\n";
81 : // std::cout << SIMTIME << " initChanger lane=" << ce->lane->getID() << " partial vehicles=" << toString(ce->lane->myPartialVehicles) << "\n";
82 : // std::cout << SIMTIME << " initChanger lane=" << ce->lane->getID() << " partial vehicles beyond=" << toString(ce->ahead.toString()) << "\n";
83 : }
84 12853286 : }
85 :
86 :
87 :
88 : void
89 100482708 : MSLaneChangerSublane::updateChanger(bool vehHasChanged) {
90 100482708 : MSLaneChanger::updateChanger(vehHasChanged);
91 100482708 : if (!vehHasChanged) {
92 100104903 : MSVehicle* lead = myCandi->lead;
93 : //std::cout << SIMTIME << " updateChanger lane=" << myCandi->lane->getID() << " lead=" << Named::getIDSecure(lead) << " lsol=" << lead->getLeftSideOnLane() << " rsol=" << lead->getRightSideOnLane() << "\n";
94 100104903 : if (lead->getLeftSideOnLane() < 0 || lead->getRightSideOnLane() > myCandi->lane->getWidth()) {
95 139824 : myCandi->outsideBounds.push_back(lead);
96 : } else {
97 99965079 : myCandi->ahead.addLeader(lead, false, 0);
98 : }
99 100104903 : MSLane* shadowLane = lead->getLaneChangeModel().getShadowLane();
100 100104903 : if (shadowLane != nullptr && &shadowLane->getEdge() == &lead->getLane()->getEdge()) {
101 : assert(shadowLane->getIndex() < (int)myChanger.size());
102 2339075 : const double latOffset = lead->getLane()->getRightSideOnEdge() - shadowLane->getRightSideOnEdge();
103 : //std::cout << SIMTIME << " updateChanger shadowLane=" << shadowLane->getID() << " lead=" << Named::getIDSecure(lead) << "\n";
104 2339075 : (myChanger.begin() + shadowLane->getIndex())->ahead.addLeader(lead, false, latOffset);
105 : }
106 : }
107 : //std::cout << SIMTIME << " updateChanger: lane=" << myCandi->lane->getID() << " lead=" << Named::getIDSecure(myCandi->lead) << " ahead=" << myCandi->ahead.toString() << " vehHasChanged=" << vehHasChanged << "\n";
108 : //for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
109 : // std::cout << " lane=" << ce->lane->getID() << " vehicles=" << toString(ce->lane->myVehicles) << "\n";
110 : //}
111 100482708 : }
112 :
113 :
114 : bool
115 100482708 : MSLaneChangerSublane::change() {
116 : // variant of change() for the sublane case
117 100482708 : myCandi = findCandidate();
118 : MSVehicle* vehicle = veh(myCandi);
119 100482708 : vehicle->getLaneChangeModel().clearNeighbors();
120 : #ifdef DEBUG_ACTIONSTEPS
121 : if (DEBUG_COND) {
122 : std::cout << "\n" << SIMTIME << " CHANGE veh=" << vehicle->getID() << " lane=" << vehicle->getLane()->getID() << "\n";
123 : }
124 : #endif
125 : assert(vehicle->getLane() == (*myCandi).lane);
126 : assert(!vehicle->getLaneChangeModel().isChangingLanes());
127 100482708 : if (/*!myAllowsChanging*/ vehicle->getLaneChangeModel().alreadyChanged() || vehicle->isStoppedOnLane()) {
128 669828 : registerUnchanged(vehicle);
129 669828 : if (vehicle->isStoppedOnLane()) {
130 667865 : myCandi->lastStopped = vehicle;
131 : }
132 669828 : return false;
133 : }
134 99812880 : if (!vehicle->isActive()) {
135 : #ifdef DEBUG_ACTIONSTEPS
136 : if (DEBUG_COND) {
137 : std::cout << SIMTIME << " veh '" << vehicle->getID() << "' skips regular change checks." << std::endl;
138 : }
139 : #endif
140 :
141 : bool changed;
142 : // let TraCI influence the wish to change lanes during non-actionsteps
143 20529528 : checkTraCICommands(vehicle);
144 :
145 : // Resume change
146 20529528 : changed = continueChangeSublane(vehicle, myCandi);
147 : #ifdef DEBUG_ACTIONSTEPS
148 : if (DEBUG_COND) {
149 : std::cout << SIMTIME << " veh '" << vehicle->getID() << "' lcm->maneuverDist=" << vehicle->getLaneChangeModel().getManeuverDist()
150 : << " lcm->speedLat=" << vehicle->getLaneChangeModel().getSpeedLat() << std::endl;
151 : }
152 : #endif
153 20529528 : if (!changed) {
154 20522327 : registerUnchanged(vehicle);
155 : }
156 20529528 : return changed;
157 : }
158 :
159 : #ifdef DEBUG_ACTIONSTEPS
160 : if (DEBUG_COND) {
161 : std::cout << "\n" << SIMTIME << " veh '" << vehicle->getID() << "' plans lanechange maneuver." << std::endl;
162 : }
163 : #endif
164 79283352 : vehicle->updateBestLanes(); // needed?
165 79283352 : const bool isOpposite = vehicle->getLaneChangeModel().isOpposite();
166 79283352 : if (!isOpposite) {
167 200891373 : for (int i = 0; i < (int) myChanger.size(); ++i) {
168 121696574 : vehicle->adaptBestLanesOccupation(i, myChanger[i].dens);
169 : }
170 : }
171 : // update leaders beyond the current edge for all lanes
172 201088350 : for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
173 121804998 : ce->aheadNext = getLeaders(ce, vehicle);
174 : }
175 : // update expected speeds
176 : int sublaneIndex = 0;
177 201088350 : for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
178 121804998 : vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(ce->aheadNext, sublaneIndex, ce->lane->getIndex());
179 122708342 : for (int offset : ce->siblings) {
180 : // treat sibling lanes (internal lanes with the same origin lane) as if they have the same geometry
181 : ChangerIt ceSib = ce + offset;
182 903344 : if (ceSib->lane->allowsVehicleClass(vehicle->getVClass())) {
183 901927 : vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(ceSib->aheadNext, sublaneIndex, ceSib->lane->getIndex());
184 : }
185 : }
186 121804998 : sublaneIndex += ce->ahead.numSublanes();
187 : }
188 :
189 : // Check for changes to the opposite lane if vehicle is active
190 : #ifdef DEBUG_ACTIONSTEPS
191 : if (DEBUG_COND) {
192 : std::cout << " myChangeToOpposite=" << myChangeToOpposite << " isOpposite=" << isOpposite << " mayChangeRight=" << mayChange(-1) << " mayChangeLeft=" << mayChange(1) << "\n";
193 : }
194 : #endif
195 :
196 79283352 : const bool stopOpposite = hasOppositeStop(vehicle);
197 79283352 : const int traciState = vehicle->influenceChangeDecision(0);
198 79283352 : const bool traciRequestOpposite = !mayChange(1) && (traciState & LCA_LEFT) != 0;
199 :
200 81481026 : if (myChangeToOpposite && (
201 : // cannot overtake since there is only one usable lane (or emergency)
202 4395348 : ((!mayChange(-1) && !mayChange(1)) || vehicle->getVClass() == SVC_EMERGENCY)
203 : || traciRequestOpposite
204 623087 : || stopOpposite
205 : // can alway come back from the opposite side
206 622675 : || isOpposite)) {
207 1575556 : const MSLeaderDistanceInfo& leaders = myCandi->aheadNext;
208 1575556 : if (leaders.hasVehicles() || isOpposite || stopOpposite || traciRequestOpposite) {
209 1447943 : std::pair<MSVehicle*, double> leader = findClosestLeader(leaders, vehicle);
210 1447943 : myCheckedChangeOpposite = false;
211 130685 : if ((leader.first != nullptr || isOpposite || stopOpposite || traciRequestOpposite)
212 1479518 : && changeOpposite(vehicle, leader, myCandi->lastStopped)) {
213 164708 : return true;
214 1443761 : } else if (myCheckedChangeOpposite) {
215 160526 : registerUnchanged(vehicle);
216 160526 : return false;
217 : }
218 : // try sublane change within current lane otherwise
219 : }
220 : }
221 :
222 79118644 : LaneChangeAction alternatives = (LaneChangeAction)((mayChange(-1) ? LCA_RIGHT : LCA_NONE)
223 79118644 : | (mayChange(1) ? LCA_LEFT : LCA_NONE));
224 :
225 79118644 : StateAndDist right = checkChangeHelper(vehicle, -1, alternatives);
226 79118644 : StateAndDist left = checkChangeHelper(vehicle, 1, alternatives);
227 79118644 : StateAndDist current = checkChangeHelper(vehicle, 0, alternatives);
228 :
229 158237288 : StateAndDist decision = vehicle->getLaneChangeModel().decideDirection(current,
230 79118644 : vehicle->getLaneChangeModel().decideDirection(right, left));
231 : #ifdef DEBUG_DECISION
232 : if (vehicle->getLaneChangeModel().debugVehicle()) {
233 : std::cout << "\n" << SIMTIME << " decision=" << toString((LaneChangeAction)decision.state) << " dir=" << decision.dir << " latDist=" << decision.latDist << " maneuverDist=" << decision.maneuverDist << "\n";
234 : }
235 : #endif
236 79118644 : vehicle->getLaneChangeModel().setOwnState(decision.state);
237 79118644 : if ((decision.state & LCA_WANTS_LANECHANGE) != 0 && (decision.state & LCA_BLOCKED) == 0) {
238 : // change if the vehicle wants to and is allowed to change
239 : #ifdef DEBUG_MANEUVER
240 : if (DEBUG_COND) {
241 : std::cout << SIMTIME << " veh '" << vehicle->getID() << "' performing sublane change..." << std::endl;
242 : }
243 : #endif
244 3050595 : const bool changed = startChangeSublane(vehicle, myCandi, decision.latDist, decision.maneuverDist);
245 3050595 : if (!changed) {
246 2684173 : registerUnchanged(vehicle);
247 : }
248 3050595 : return changed;
249 : }
250 : // @note this assumes vehicles can instantly abort any maneuvre in case of emergency
251 76068049 : abortLCManeuver(vehicle);
252 76068049 : registerUnchanged(vehicle);
253 :
254 : if ((right.state & (LCA_URGENT)) != 0 && (left.state & (LCA_URGENT)) != 0) {
255 : // ... wants to go to the left AND to the right
256 : // just let them go to the right lane...
257 : left.state = 0;
258 : }
259 : return false;
260 : }
261 :
262 :
263 : void
264 76068527 : MSLaneChangerSublane::abortLCManeuver(MSVehicle* vehicle) {
265 : #ifdef DEBUG_MANEUVER
266 : if (DEBUG_COND) {
267 : std::cout << SIMTIME << " veh '" << vehicle->getID() << "' aborts LC-continuation."
268 : << std::endl;
269 : }
270 : #endif
271 76068527 : const int priorReason = vehicle->getLaneChangeModel().getPrevState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
272 76068527 : const int reason = vehicle->getLaneChangeModel().getOwnState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
273 76068527 : if (priorReason != 0 && priorReason != reason && vehicle->getLaneChangeModel().getPreviousManeuverDist() != 0) {
274 : // original from cannot be reconstructed
275 71243 : const int priorDirection = vehicle->getLaneChangeModel().getPreviousManeuverDist() > 0 ? 1 : -1;
276 : #ifdef DEBUG_MANEUVER
277 : if (DEBUG_COND) {
278 : std::cout << SIMTIME << " abortLCManeuver priorReason=" << toString((LaneChangeAction)priorReason)
279 : << " reason=" << toString((LaneChangeAction)reason) << " veh=" << vehicle->getID() << "\n";
280 : }
281 : #endif
282 71243 : outputLCEnded(vehicle, myCandi, myCandi, priorDirection);
283 : }
284 76068527 : const double updatedSpeedLat = vehicle->getLaneChangeModel().getSpeedLat() != 0;
285 76068527 : vehicle->getLaneChangeModel().setSpeedLat(0);
286 76068527 : vehicle->getLaneChangeModel().setManeuverDist(0.);
287 76068527 : vehicle->getLaneChangeModel().updateTargetLane();
288 76068527 : if (updatedSpeedLat) {
289 : // update angle after having reset lateral speed
290 1260018 : vehicle->setAngle(vehicle->computeAngle());
291 : }
292 76068527 : }
293 :
294 :
295 : MSLaneChangerSublane::StateAndDist
296 237355932 : MSLaneChangerSublane::checkChangeHelper(MSVehicle* vehicle, int laneOffset, LaneChangeAction alternatives) {
297 : StateAndDist result = StateAndDist(0, 0, 0, 0);
298 237355932 : if (mayChange(laneOffset)) {
299 113731087 : if (laneOffset != 0 && vehicle->getLaneChangeModel().isOpposite()) {
300 42 : return result;
301 : }
302 113731045 : const std::vector<MSVehicle::LaneQ>& preb = (vehicle->getLaneChangeModel().isOpposite()
303 113731045 : ? getBestLanesOpposite(vehicle, nullptr, 1000)
304 113731045 : : vehicle->getBestLanes());
305 113731045 : result.state = checkChangeSublane(laneOffset, alternatives, preb, result.latDist, result.maneuverDist);
306 113731045 : result.dir = laneOffset;
307 113731045 : if ((result.state & LCA_WANTS_LANECHANGE) != 0 && (result.state & LCA_URGENT) != 0 && (result.state & LCA_BLOCKED) != 0) {
308 1649024 : (myCandi + laneOffset)->lastBlocked = vehicle;
309 1649024 : if ((myCandi + laneOffset)->firstBlocked == nullptr) {
310 580080 : (myCandi + laneOffset)->firstBlocked = vehicle;
311 : }
312 : }
313 113731045 : }
314 : return result;
315 : }
316 :
317 :
318 : /// @brief Continue a sublane-lane change maneuver and return whether the midpoint was passed in this step
319 : // (used to continue sublane changing in non-action steps).
320 : bool
321 20529528 : MSLaneChangerSublane::continueChangeSublane(MSVehicle* vehicle, ChangerIt& from) {
322 : // lateral distance to complete maneuver
323 20529528 : double remLatDist = vehicle->getLaneChangeModel().getManeuverDist();
324 20529528 : if (remLatDist == 0) {
325 : return false;
326 : }
327 794287 : const bool urgent = (vehicle->getLaneChangeModel().getOwnState() & LCA_URGENT) != 0;
328 794287 : const double nextLatDist = SPEED2DIST(vehicle->getLaneChangeModel().computeSpeedLat(remLatDist, remLatDist, urgent));
329 : #ifdef DEBUG_MANEUVER
330 : if (DEBUG_COND) {
331 : std::cout << SIMTIME << " vehicle '" << vehicle->getID() << "' continueChangeSublane()"
332 : << " remLatDist=" << remLatDist << " nextLatDist=" << nextLatDist
333 : << std::endl;
334 : }
335 : #endif
336 :
337 794287 : const bool changed = startChangeSublane(vehicle, from, nextLatDist, remLatDist);
338 : return changed;
339 : }
340 :
341 :
342 : bool
343 3902042 : MSLaneChangerSublane::startChangeSublane(MSVehicle* vehicle, ChangerIt& from, double latDist, double maneuverDist) {
344 3902042 : if (vehicle->isRemoteControlled()) {
345 : return false;
346 : }
347 3901543 : MSLane* source = from->lane;
348 : // Prevent continuation of LC beyond lane borders if change is not allowed
349 3901543 : double distToRightLaneBorder = vehicle->getLane()->getWidth() * 0.5 + vehicle->getLateralPositionOnLane() - vehicle->getWidth() * 0.5;
350 3901543 : double distToLeftLaneBorder = vehicle->getLane()->getWidth() * 0.5 - vehicle->getLateralPositionOnLane() - vehicle->getWidth() * 0.5;
351 3901543 : if (vehicle->getLaneChangeModel().isOpposite()) {
352 : std::swap(distToRightLaneBorder, distToLeftLaneBorder);
353 : }
354 : // determine direction of LC
355 : int direction = 0;
356 3901543 : if (latDist > 0 && latDist > distToLeftLaneBorder) {
357 : direction = 1;
358 3408383 : } else if (latDist < 0 && -latDist > distToRightLaneBorder) {
359 : direction = -1;
360 : }
361 3901543 : const int changerDirection = vehicle->getLaneChangeModel().isOpposite() ? -direction : direction;
362 3901543 : ChangerIt to = from;
363 : #ifdef DEBUG_MANEUVER
364 : if (DEBUG_COND) {
365 : std::cout << SIMTIME << " vehicle '" << vehicle->getID() << "' latDist=" << latDist << " maneuverDist=" << maneuverDist
366 : << " distRight=" << distToRightLaneBorder << " distLeft=" << distToLeftLaneBorder
367 : << " dir=" << direction << " cDir=" << changerDirection << " mayChange=" << mayChange(changerDirection) << "\n";
368 : }
369 : #endif
370 3901543 : if (mayChange(changerDirection)) {
371 3882756 : to = from + changerDirection;
372 18787 : } else if (changerDirection == 1 && source->getOpposite() != nullptr) {
373 : // change to the opposite direction lane
374 18309 : to = (source->getOpposite()->getEdge().myLaneChanger->getChanger().end() - 1);
375 : } else {
376 : // This may occur during maneuver continuation in non-actionsteps.
377 : // TODO: Understand better why and test later if additional sublane actionstep debugging resolves this
378 : // (XXX: perhaps one should try to extrapolate check for this case before to avoid maneuver initialization
379 : // similar as for continuous LC in MSLaneChanger::checkChange())
380 : //assert(false);
381 478 : abortLCManeuver(vehicle);
382 478 : return false;
383 : }
384 :
385 : // The following does:
386 : // 1) update vehicles lateral position according to latDist and target lane
387 : // 2) distinguish several cases
388 : // a) vehicle moves completely within the same lane
389 : // b) vehicle intersects another lane
390 : // - vehicle must be moved to the lane where its midpoint is (either old or new)
391 : // - shadow vehicle must be created/moved to the other lane if the vehicle intersects it
392 : // 3) updated dens of all lanes that hold the vehicle or its shadow
393 :
394 3901065 : vehicle->myState.myPosLat += latDist * (vehicle->getLaneChangeModel().isOpposite() ? -1 : 1);
395 4140731 : for (int i = 0; i < (int)vehicle->myFurtherLanesPosLat.size(); i++) {
396 479332 : vehicle->myFurtherLanesPosLat[i] += latDist * (vehicle->getLaneChangeModel().isOpposite() ? -1 : 1);
397 : }
398 3901065 : vehicle->myCachedPosition = Position::INVALID;
399 3901065 : vehicle->getLaneChangeModel().setSpeedLat(DIST2SPEED(latDist));
400 : #ifdef DEBUG_MANEUVER
401 : if (DEBUG_COND) {
402 : std::cout << SIMTIME << " vehicle '" << vehicle->getID() << "' with maneuverDist=" << maneuverDist
403 : << " and committedSpeed=" << vehicle->getLaneChangeModel().getCommittedSpeed()
404 : << " increments lateral position by latDist=" << latDist << std::endl;
405 : }
406 : #endif
407 : #ifdef DEBUG_SURROUNDING
408 : if (DEBUG_COND) {
409 : std::cout << SIMTIME << " vehicle '" << vehicle->getID() << "'\n to->ahead=" << to->ahead.toString()
410 : << "'\n to->aheadNext=" << to->aheadNext.toString()
411 : << std::endl;
412 : }
413 : #endif
414 3901065 : const bool completedPriorManeuver = fabs(vehicle->getLaneChangeModel().getPreviousManeuverDist()) < NUMERICAL_EPS;
415 3901065 : const bool completedManeuver = fabs(maneuverDist - latDist) < NUMERICAL_EPS;
416 3901065 : vehicle->getLaneChangeModel().updateSafeLatDist(latDist);
417 :
418 : // current maneuver is aborted when direction or reason changes
419 3901065 : const int priorReason = vehicle->getLaneChangeModel().getPrevState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
420 3901065 : const int reason = vehicle->getLaneChangeModel().getOwnState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
421 : #ifdef DEBUG_MANEUVER
422 : if (DEBUG_COND) {
423 : std::cout << SIMTIME << " vehicle '" << vehicle->getID()
424 : << "' completedPriorManeuver=" << completedPriorManeuver
425 : << " completedManeuver=" << completedManeuver
426 : << " priorReason=" << toString((LaneChangeAction)priorReason)
427 : << " reason=" << toString((LaneChangeAction)reason)
428 : << " priorManeuverDist=" << vehicle->getLaneChangeModel().getPreviousManeuverDist()
429 : << " maneuverDist=" << maneuverDist
430 : << " latDist=" << latDist
431 : << std::endl;
432 : }
433 : #endif
434 3901065 : if (!completedManeuver && !completedPriorManeuver && priorReason != 0 &&
435 1211175 : (vehicle->getLaneChangeModel().getPreviousManeuverDist() * latDist < 0
436 1194464 : || priorReason != reason)) {
437 142247 : const int priorDirection = vehicle->getLaneChangeModel().getPreviousManeuverDist() > 0 ? 1 : -1;
438 : // original from cannot be reconstructed
439 : #ifdef DEBUG_MANEUVER
440 : if (DEBUG_COND) {
441 : std::cout << SIMTIME << " startChangeSublane abort priorReason=" << toString((LaneChangeAction)priorReason)
442 : << " reason=" << toString((LaneChangeAction)reason) << " veh=" << vehicle->getID() << "\n";
443 : }
444 : #endif
445 142247 : outputLCEnded(vehicle, from, from, priorDirection);
446 : }
447 :
448 3901065 : outputLCStarted(vehicle, from, to, direction, maneuverDist);
449 3901065 : vehicle->getLaneChangeModel().setManeuverDist(maneuverDist - latDist);
450 3901065 : const bool changedToNewLane = checkChangeToNewLane(vehicle, direction, from, to);
451 :
452 3901065 : MSLane* oldShadowLane = vehicle->getLaneChangeModel().getShadowLane();
453 3901065 : vehicle->getLaneChangeModel().updateShadowLane();
454 3901065 : MSLane* shadowLane = vehicle->getLaneChangeModel().getShadowLane();
455 3901065 : if (shadowLane != nullptr && shadowLane != oldShadowLane
456 3901065 : && &shadowLane->getEdge() == &source->getEdge()) {
457 : assert(oldShadowLane == 0 || vehicle->getLaneChangeModel().isOpposite() || to != from);
458 767945 : const double latOffset = vehicle->getLane()->getRightSideOnEdge() - shadowLane->getRightSideOnEdge();
459 767945 : (myChanger.begin() + shadowLane->getIndex())->ahead.addLeader(vehicle, false, latOffset);
460 : }
461 3901065 : if (completedManeuver) {
462 1662949 : outputLCEnded(vehicle, from, to, direction);
463 : }
464 :
465 : // Update maneuver reservations on target lanes
466 3901065 : MSLane* targetLane = vehicle->getLaneChangeModel().updateTargetLane();
467 3901065 : if (!changedToNewLane && targetLane != nullptr
468 208195 : && vehicle->getActionStepLength() > DELTA_T
469 3999672 : && &targetLane->getEdge() == &source->getEdge()
470 : ) {
471 98582 : const int dir = (vehicle->getLaneChangeModel().getManeuverDist() > 0 ? 1 : -1);
472 : ChangerIt target = from + dir;
473 98582 : const double actionStepDist = dir * vehicle->getVehicleType().getMaxSpeedLat() * vehicle->getActionStepLengthSecs();
474 98582 : const double latOffset = vehicle->getLatOffset(targetLane) + actionStepDist;
475 98582 : target->ahead.addLeader(vehicle, false, latOffset);
476 : //std::cout << SIMTIME << " veh=" << vehicle->getID() << " target=" << targetLane->getID()
477 : // << " actionStepDist=" << actionStepDist << " latOffset=" << latOffset
478 : // << " targetAhead=" << target->ahead.toString() << "\n";
479 : }
480 :
481 : // compute new angle of the vehicle from the x- and y-distances travelled within last time step
482 : // (should happen last because primaryLaneChanged() also triggers angle computation)
483 : // this part of the angle comes from the orientation of our current lane
484 3901065 : double laneAngle = vehicle->computeAngle();
485 3901065 : if (vehicle->getLaneChangeModel().isOpposite()) {
486 : // reverse lane angle
487 35429 : laneAngle += M_PI;
488 : }
489 : #ifdef DEBUG_MANEUVER
490 : if (DEBUG_COND) {
491 : std::cout << SIMTIME << " startChangeSublane"
492 : << " oldLane=" << from->lane->getID()
493 : << " newLane=" << to->lane->getID()
494 : << " shadowLane=" << (shadowLane != nullptr ? shadowLane->getID() : "NULL")
495 : << " targetLane=" << (targetLane != nullptr ? targetLane->getID() : "NULL")
496 : << " maneuverDist=" << vehicle->getLaneChangeModel().getManeuverDist()
497 : << " latDist=" << latDist
498 : << " oldShadowLane=" << Named::getIDSecure(oldShadowLane)
499 : << " newShadowLane=" << Named::getIDSecure(vehicle->getLaneChangeModel().getShadowLane())
500 : << " laneA=" << RAD2DEG(laneAngle)
501 : << " oldA=" << RAD2DEG(vehicle->getAngle())
502 : << " oppposite=" << vehicle->getLaneChangeModel().isOpposite()
503 : << " changedToNewLane=" << changedToNewLane
504 : << "\n";
505 : }
506 : #endif
507 3901065 : vehicle->setAngle(laneAngle, completedManeuver);
508 :
509 : // check if a traci maneuver must continue
510 : // getOwnState is reset to 0 when changing lanes so we use the stored reason
511 3901065 : if ((reason & LCA_TRACI) != 0) {
512 : #ifdef DEBUG_MANEUVER
513 : if (DEBUG_COND) {
514 : std::cout << SIMTIME << " continue TraCI-maneuver remainingLatDist=" << vehicle->getLaneChangeModel().getManeuverDist() << "\n";
515 : }
516 : #endif
517 1151 : vehicle->getInfluencer().setSublaneChange(vehicle->getLaneChangeModel().getManeuverDist());
518 : }
519 3901065 : from->lane->requireCollisionCheck();
520 3901065 : to->lane->requireCollisionCheck();
521 3901065 : return changedToNewLane;
522 : }
523 :
524 : bool
525 3901065 : MSLaneChangerSublane::checkChangeToNewLane(MSVehicle* vehicle, const int direction, ChangerIt from, ChangerIt to) {
526 3901065 : const int oppositeSign = vehicle->getLaneChangeModel().isOpposite() ? -1 : 1;
527 3901065 : const bool opposite = (&from->lane->getEdge() != &to->lane->getEdge());
528 : const bool changedToNewLane = (to->lane != from->lane
529 932923 : && fabs(vehicle->getLateralPositionOnLane()) > 0.5 * vehicle->getLane()->getWidth()
530 4278870 : && (mayChange(direction * oppositeSign) || opposite));
531 : if (changedToNewLane) {
532 377805 : vehicle->myState.myPosLat -= direction * 0.5 * (from->lane->getWidth() + to->lane->getWidth()) * oppositeSign;
533 377805 : if (!opposite) {
534 373724 : to->lane->myTmpVehicles.insert(to->lane->myTmpVehicles.begin(), vehicle);
535 373724 : to->dens += vehicle->getVehicleType().getLengthWithGap();
536 : }
537 377805 : if (MSAbstractLaneChangeModel::haveLCOutput()) {
538 7396 : if (!vehicle->isActive()) {
539 : // update leaders beyond the current edge for all lanes
540 : // @note to->aheadNext and from->aheadNext are only needed for output in non-action steps.
541 1987 : to->aheadNext = getLeaders(to, vehicle);
542 1987 : from->aheadNext = getLeaders(from, vehicle);
543 : }
544 7396 : vehicle->getLaneChangeModel().setLeaderGaps(to->aheadNext);
545 7396 : vehicle->getLaneChangeModel().setFollowerGaps(to->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true));
546 7396 : vehicle->getLaneChangeModel().setOrigLeaderGaps(from->aheadNext);
547 : }
548 377805 : vehicle->getLaneChangeModel().startLaneChangeManeuver(from->lane, to->lane, direction);
549 377805 : if (!opposite) {
550 373724 : to->ahead.addLeader(vehicle, false, 0);
551 : }
552 : } else {
553 3523260 : from->ahead.addLeader(vehicle, false, 0);
554 : }
555 3901065 : return changedToNewLane;
556 : }
557 :
558 : void
559 3901065 : MSLaneChangerSublane::outputLCStarted(MSVehicle* vehicle, ChangerIt& from, ChangerIt& to, int direction, double maneuverDist) {
560 440402 : if (MSAbstractLaneChangeModel::haveLCOutput() && MSAbstractLaneChangeModel::outputLCStarted()
561 : // non-sublane change started
562 1209 : && ((vehicle->getLaneChangeModel().getOwnState() & (LCA_CHANGE_REASONS & ~LCA_SUBLANE)) != 0)
563 1019 : && ((vehicle->getLaneChangeModel().getOwnState() & LCA_STAY) == 0)
564 : // no changing for the same reason in previous step (either not wanted or blocked)
565 3903103 : && ((vehicle->getLaneChangeModel().getPrevState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE) !=
566 1019 : (vehicle->getLaneChangeModel().getOwnState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE)
567 835 : || ((vehicle->getLaneChangeModel().getPrevState() & LCA_STAY) != 0)
568 835 : || ((vehicle->getLaneChangeModel().getPrevState() & LCA_BLOCKED) != 0))
569 : ) {
570 : #ifdef DEBUG_STATE
571 : if (DEBUG_COND) {
572 : std::cout << SIMTIME << " veh=" << vehicle->getID() << " laneChangeStarted state=" << toString((LaneChangeAction)vehicle->getLaneChangeModel().getOwnState())
573 : << " prevState=" << toString((LaneChangeAction)vehicle->getLaneChangeModel().getPrevState())
574 : << " filter=" << toString((LaneChangeAction)(LCA_CHANGE_REASONS & ~LCA_SUBLANE))
575 : << " filtered=" << toString((LaneChangeAction)(vehicle->getLaneChangeModel().getOwnState() & (LCA_CHANGE_REASONS & ~LCA_SUBLANE)))
576 : << "\n";
577 : }
578 : #endif
579 204 : vehicle->getLaneChangeModel().setLeaderGaps(to->aheadNext);
580 204 : vehicle->getLaneChangeModel().setFollowerGaps(to->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true));
581 204 : vehicle->getLaneChangeModel().setOrigLeaderGaps(from->aheadNext);
582 408 : vehicle->getLaneChangeModel().laneChangeOutput("changeStarted", from->lane, to->lane, direction, maneuverDist);
583 : }
584 3901065 : }
585 :
586 : void
587 1876439 : MSLaneChangerSublane::outputLCEnded(MSVehicle* vehicle, ChangerIt& from, ChangerIt& to, int direction) {
588 148771 : if (MSAbstractLaneChangeModel::haveLCOutput() && MSAbstractLaneChangeModel::outputLCEnded()
589 : // non-sublane change ended
590 1876561 : && ((vehicle->getLaneChangeModel().getOwnState() & (LCA_CHANGE_REASONS & ~LCA_SUBLANE)) != 0)) {
591 80 : vehicle->getLaneChangeModel().setLeaderGaps(to->aheadNext);
592 80 : vehicle->getLaneChangeModel().setFollowerGaps(to->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true));
593 80 : vehicle->getLaneChangeModel().setOrigLeaderGaps(from->aheadNext);
594 160 : vehicle->getLaneChangeModel().laneChangeOutput("changeEnded", from->lane, to->lane, direction);
595 : }
596 1876439 : }
597 :
598 :
599 : MSLeaderDistanceInfo
600 121808972 : MSLaneChangerSublane::getLeaders(const ChangerIt& target, const MSVehicle* vehicle) const {
601 : // get the leading vehicle on the lane to change to
602 : #ifdef DEBUG_SURROUNDING
603 : if (DEBUG_COND) {
604 : std::cout << SIMTIME << " getLeaders lane=" << target->lane->getID() << " ego=" << vehicle->getID() << " ahead=" << target->ahead.toString() << "\n";
605 : }
606 : #endif
607 121808972 : MSLeaderDistanceInfo result(target->lane->getWidth(), nullptr, 0);
608 : int sublaneShift = 0;
609 121808972 : if (target->lane == vehicle->getLane()) {
610 79285339 : if (vehicle->getLeftSideOnLane() < -MSGlobals::gLateralResolution) {
611 27996 : sublaneShift = int(-vehicle->getLeftSideOnLane() / MSGlobals::gLateralResolution);
612 79257343 : } else if (vehicle->getRightSideOnLane() > target->lane->getWidth() + MSGlobals::gLateralResolution) {
613 31411 : sublaneShift = -int((vehicle->getRightSideOnLane() - target->lane->getWidth()) / MSGlobals::gLateralResolution);
614 : }
615 79285339 : result.setSublaneOffset(sublaneShift);
616 : }
617 632794724 : for (int i = 0; i < target->ahead.numSublanes(); ++i) {
618 510985752 : const MSVehicle* veh = target->ahead[i];
619 510985752 : if (veh != nullptr) {
620 381189953 : const double gap = veh->getBackPositionOnLane(target->lane) - vehicle->getPositionOnLane() - vehicle->getVehicleType().getMinGap();
621 : #ifdef DEBUG_SURROUNDING
622 : if (DEBUG_COND) {
623 : std::cout << " ahead lead=" << veh->getID() << " leadBack=" << veh->getBackPositionOnLane() << " gap=" << gap << " sublaneShift=" << sublaneShift << "\n";
624 : }
625 : #endif
626 381189953 : if (i + sublaneShift >= 0 && i + sublaneShift < result.numSublanes()) {
627 381121865 : result.addLeader(veh, gap, 0, i + sublaneShift);
628 : }
629 : }
630 : }
631 121808972 : if (sublaneShift != 0) {
632 89840 : for (MSVehicle* cand : target->outsideBounds) {
633 30433 : const double gap = cand->getBackPositionOnLane() - vehicle->getPositionOnLane() - vehicle->getVehicleType().getMinGap();
634 30433 : result.addLeader(cand, gap);
635 : }
636 : }
637 : #ifdef DEBUG_SURROUNDING
638 : if (DEBUG_COND) {
639 : std::cout << " outsideBounds=" << toString(target->outsideBounds) << " result=" << result.toString() << "\n";
640 : }
641 : #endif
642 : // @note use the exact same vehiclePos as in getLastVehicleInformation() to avoid missing vehicles
643 121808972 : target->lane->addLeaders(vehicle, vehicle->getPositionOnLane(vehicle->getLane()), result);
644 121808972 : return result;
645 0 : }
646 :
647 :
648 : int
649 113731045 : MSLaneChangerSublane::checkChangeSublane(
650 : int laneOffset,
651 : LaneChangeAction alternatives,
652 : const std::vector<MSVehicle::LaneQ>& preb,
653 : double& latDist,
654 : double& maneuverDist) const {
655 :
656 : ChangerIt target = myCandi + laneOffset;
657 : MSVehicle* vehicle = veh(myCandi);
658 113731045 : const MSLane& neighLane = *(target->lane);
659 113731045 : int blocked = 0;
660 :
661 113731045 : MSLeaderDistanceInfo neighLeaders = target->aheadNext;
662 113731045 : MSLeaderDistanceInfo neighFollowers = target->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
663 113731045 : MSLeaderDistanceInfo neighBlockers(neighLane.getWidth(), vehicle, vehicle->getLane()->getRightSideOnEdge() - neighLane.getRightSideOnEdge());
664 113731045 : MSLeaderDistanceInfo leaders = myCandi->aheadNext;
665 113731045 : addOutsideLeaders(vehicle, leaders);
666 113731045 : MSLeaderDistanceInfo followers = myCandi->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
667 113731045 : MSLeaderDistanceInfo blockers(vehicle->getLane()->getWidth(), vehicle, 0);
668 :
669 : // consider sibling lanes of the origin and target lane
670 114098416 : for (int offset : myCandi->siblings) {
671 : // treat sibling lanes (internal lanes with the same origin lane) as if they have the same geometry
672 : ChangerIt ceSib = myCandi + offset;
673 367371 : MSLeaderDistanceInfo sibFollowers = ceSib->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
674 367371 : if (sibFollowers.hasVehicles()) {
675 333087 : followers.addLeaders(sibFollowers);
676 : }
677 367371 : if (ceSib->aheadNext.hasVehicles()) {
678 260188 : leaders.addLeaders(ceSib->aheadNext);
679 : }
680 : #ifdef DEBUG_SURROUNDING
681 : if (DEBUG_COND) {
682 : std::cout << SIMTIME << " ego=" << vehicle->getID() << " ahead=" << myCandi->aheadNext.toString() << " sib=" << ceSib->lane->getID() << " sibAhead=" << ceSib->aheadNext.toString() << " leaders=" << leaders.toString() << "\n";
683 : }
684 : #endif
685 367371 : }
686 114123920 : for (int offset : target->siblings) {
687 : // treat sibling lanes (internal lanes with the same origin lane) as if they have the same geometry
688 : ChangerIt ceSib = target + offset;
689 392875 : MSLeaderDistanceInfo sibFollowers = ceSib->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
690 392875 : if (sibFollowers.hasVehicles()) {
691 357547 : neighFollowers.addLeaders(sibFollowers);
692 : }
693 392875 : if (ceSib->aheadNext.hasVehicles()) {
694 288605 : neighLeaders.addLeaders(ceSib->aheadNext);
695 : }
696 : #ifdef DEBUG_SURROUNDING
697 : if (DEBUG_COND) {
698 : std::cout << SIMTIME << " ego=" << vehicle->getID() << " neighAhead=" << target->aheadNext.toString() << " sib=" << ceSib->lane->getID() << " sibAhead=" << ceSib->aheadNext.toString() << " neighLeaders=" << neighLeaders.toString() << "\n";
699 : }
700 : #endif
701 392875 : }
702 :
703 : // break leader symmetry
704 113731045 : if (laneOffset == -1 && neighLeaders.hasVehicles()) {
705 12907123 : neighLeaders.moveSamePosTo(vehicle, neighFollowers);
706 : }
707 :
708 : #ifdef DEBUG_SURROUNDING
709 : if (DEBUG_COND) std::cout << SIMTIME
710 : << " checkChangeSublane: veh=" << vehicle->getID()
711 : << " laneOffset=" << laneOffset
712 : << "\n leaders=" << leaders.toString()
713 : << "\n neighLeaders=" << neighLeaders.toString()
714 : << "\n followers=" << followers.toString()
715 : << "\n neighFollowers=" << neighFollowers.toString()
716 : << "\n";
717 : #endif
718 :
719 :
720 113731045 : const int wish = vehicle->getLaneChangeModel().wantsChangeSublane(
721 : laneOffset, alternatives,
722 : leaders, followers, blockers,
723 : neighLeaders, neighFollowers, neighBlockers,
724 : neighLane, preb,
725 : &(myCandi->lastBlocked), &(myCandi->firstBlocked), latDist, maneuverDist, blocked);
726 :
727 :
728 9309908 : if (checkOpened && (blocked & LCA_BLOCKED) == 0 && (wish & LCA_WANTS_LANECHANGE) != 0
729 1033391 : && vehicle->getLane()->isNormal()
730 114764436 : && vehicle->getBestLanesContinuation().size() > 1) {
731 1002174 : const MSLink* link = vehicle->getLane()->getLinkTo(vehicle->getBestLanesContinuation()[1]);
732 1002174 : if (link != nullptr && link->isEntryLink()) {
733 1002174 : const MSLink* link2 = link->getParallelLink(laneOffset);
734 1002174 : if (link2 != nullptr) {
735 327010 : auto api = link->getApproachingPtr(vehicle);
736 327010 : if (api != nullptr) {
737 327010 : if (!link2->opened(api->arrivalTime, api->arrivalSpeed, api->leaveSpeed, vehicle->getLength(),
738 327010 : vehicle->getImpatience(), vehicle->getCarFollowModel().getMaxDecel(), vehicle->getWaitingTime(), vehicle->getLateralPositionOnLane(),
739 327010 : nullptr, false, vehicle, api->dist)) {
740 : //std::cout << SIMTIME << " unsafeLC " << vehicle->getID() << "\n";
741 53061 : blocked |= LCA_BLOCKED;
742 : }
743 : }
744 : }
745 : }
746 : }
747 :
748 113731045 : int state = blocked | wish;
749 :
750 : // XXX
751 : // do are more careful (but expensive) check to ensure that a
752 : // safety-critical leader is not being overlooked
753 :
754 : // let TraCI influence the wish to change lanes and the security to take
755 : const int oldstate = state;
756 113731045 : state = vehicle->influenceChangeDecision(state);
757 : #ifdef DEBUG_STATE
758 : if (DEBUG_COND && state != oldstate) {
759 : std::cout << SIMTIME << " veh=" << vehicle->getID() << " stateAfterTraCI=" << toString((LaneChangeAction)state) << " original=" << toString((LaneChangeAction)oldstate) << "\n";
760 : }
761 : #endif
762 113731045 : vehicle->getLaneChangeModel().getCanceledState(laneOffset) |= blocked;
763 113731045 : vehicle->getLaneChangeModel().saveLCState(laneOffset, oldstate, state);
764 113731045 : if (laneOffset != 0) {
765 34612401 : vehicle->getLaneChangeModel().saveNeighbors(laneOffset, neighFollowers, neighLeaders);
766 : }
767 113731045 : return state;
768 113731045 : }
769 :
770 :
771 : bool
772 164708 : MSLaneChangerSublane::checkChangeOpposite(
773 : MSVehicle* vehicle,
774 : int laneOffset,
775 : MSLane* targetLane,
776 : const std::pair<MSVehicle* const, double>& leader,
777 : const std::pair<MSVehicle* const, double>& neighLead,
778 : const std::pair<MSVehicle* const, double>& neighFollow,
779 : const std::vector<MSVehicle::LaneQ>& preb) {
780 164708 : myCheckedChangeOpposite = true;
781 :
782 : UNUSED_PARAMETER(leader);
783 : UNUSED_PARAMETER(neighLead);
784 : UNUSED_PARAMETER(neighFollow);
785 :
786 : const MSLane& neighLane = *targetLane;
787 164708 : MSLane* curLane = myCandi->lane;
788 :
789 164708 : MSLeaderDistanceInfo neighLeaders(targetLane->getWidth(), nullptr, 0);
790 164708 : MSLeaderDistanceInfo neighFollowers(targetLane->getWidth(), nullptr, 0);
791 164708 : MSLeaderDistanceInfo neighBlockers(targetLane->getWidth(), nullptr, 0);
792 164708 : MSLeaderDistanceInfo leaders(curLane->getWidth(), nullptr, 0);
793 164708 : MSLeaderDistanceInfo followers(curLane->getWidth(), nullptr, 0);
794 164708 : MSLeaderDistanceInfo blockers(curLane->getWidth(), nullptr, 0);
795 :
796 329416 : const double backPosOnTarget = vehicle->getLane()->getOppositePos(vehicle->getBackPositionOnLane());
797 164708 : if (vehicle->getLaneChangeModel().isOpposite()) {
798 88386 : leaders = curLane->getFollowersOnConsecutive(vehicle, vehicle->getPositionOnLane(), true, -1, MSLane::MinorLinkMode::FOLLOW_ONCOMING);
799 88386 : leaders.fixOppositeGaps(false);
800 88386 : curLane->addLeaders(vehicle, vehicle->getBackPositionOnLane(), followers);
801 88386 : followers.fixOppositeGaps(true);
802 88386 : neighFollowers = targetLane->getFollowersOnConsecutive(vehicle, backPosOnTarget, true);
803 88386 : neighFollowers.fixOppositeGaps(false);
804 : // artificially increase the position to ensure that ego is not added as a leader
805 88386 : const double posOnTarget = backPosOnTarget + vehicle->getVehicleType().getLength() + POSITION_EPS;
806 88386 : targetLane->addLeaders(vehicle, posOnTarget, neighLeaders);
807 88386 : neighLeaders.patchGaps(2 * POSITION_EPS);
808 : int sublaneIndex = 0;
809 107799 : for (int i = 0; i < targetLane->getIndex(); i++) {
810 19413 : sublaneIndex += MSLeaderInfo(targetLane->getEdge().getLanes()[i]->getWidth()).numSublanes();
811 : }
812 88386 : vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(neighLeaders, sublaneIndex, targetLane->getIndex());
813 : } else {
814 76322 : leaders = myCandi->aheadNext;
815 76322 : followers = myCandi->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
816 76322 : const double posOnTarget = backPosOnTarget - vehicle->getVehicleType().getLength();
817 76322 : targetLane->addLeaders(vehicle, backPosOnTarget, neighFollowers, true);
818 76322 : neighFollowers.fixOppositeGaps(true);
819 76322 : neighLeaders = targetLane->getFollowersOnConsecutive(vehicle, posOnTarget, true);
820 76322 : neighLeaders.fixOppositeGaps(false);
821 : }
822 :
823 :
824 : #ifdef DEBUG_CHANGE_OPPOSITE
825 : if (DEBUG_COND) std::cout << SIMTIME
826 : << " checkChangeOppositeSublane: veh=" << vehicle->getID()
827 : << " isOpposite=" << vehicle->getLaneChangeModel().isOpposite()
828 : << " laneOffset=" << laneOffset
829 : << "\n leaders=" << leaders.toString()
830 : << "\n neighLeaders=" << neighLeaders.toString()
831 : << "\n followers=" << followers.toString()
832 : << "\n neighFollowers=" << neighFollowers.toString()
833 : << "\n";
834 : #endif
835 :
836 164708 : LaneChangeAction alternatives = (LaneChangeAction)((mayChange(-1) ? LCA_RIGHT : LCA_NONE)
837 164708 : | (mayChange(1) ? LCA_LEFT : LCA_NONE));
838 :
839 164708 : int blocked = 0;
840 164708 : double latDist = 0;
841 164708 : double maneuverDist = 0;
842 164708 : const int wish = vehicle->getLaneChangeModel().wantsChangeSublane(
843 : laneOffset, alternatives,
844 : leaders, followers, blockers,
845 : neighLeaders, neighFollowers, neighBlockers,
846 : neighLane, preb,
847 : &(myCandi->lastBlocked), &(myCandi->firstBlocked), latDist, maneuverDist, blocked);
848 164708 : int state = blocked | wish;
849 :
850 : const int oldstate = state;
851 164708 : state = vehicle->influenceChangeDecision(state);
852 : #ifdef DEBUG_CHANGE_OPPOSITE
853 : if (DEBUG_COND && state != oldstate) {
854 : std::cout << SIMTIME << " veh=" << vehicle->getID() << " stateAfterTraCI=" << toString((LaneChangeAction)state) << " original=" << toString((LaneChangeAction)oldstate) << "\n";
855 : }
856 : #endif
857 164708 : vehicle->getLaneChangeModel().saveLCState(laneOffset, oldstate, state);
858 164708 : if (laneOffset != 0) {
859 164708 : vehicle->getLaneChangeModel().saveNeighbors(laneOffset, neighFollowers, neighLeaders);
860 : }
861 :
862 164708 : if ((state & LCA_WANTS_LANECHANGE) != 0 && (state & LCA_BLOCKED) == 0) {
863 : // change if the vehicle wants to and is allowed to change
864 : #ifdef DEBUG_CHANGE_OPPOSITE
865 : if (DEBUG_COND) {
866 : std::cout << SIMTIME << " veh '" << vehicle->getID() << "' performing sublane change latDist=" << latDist << " maneuverDist=" << maneuverDist << "\n";
867 : }
868 : #endif
869 57160 : vehicle->getLaneChangeModel().setOwnState(state);
870 57160 : return startChangeSublane(vehicle, myCandi, latDist, maneuverDist);
871 : } else {
872 107548 : vehicle->getLaneChangeModel().setSpeedLat(0);
873 : return false;
874 : }
875 164708 : }
876 :
877 : std::pair<MSVehicle*, double>
878 1447943 : MSLaneChangerSublane::findClosestLeader(const MSLeaderDistanceInfo& leaders, const MSVehicle* vehicle) {
879 1447943 : const double egoWidth = vehicle->getVehicleType().getWidth() + vehicle->getVehicleType().getMinGapLat();
880 : std::pair<MSVehicle*, double> leader(nullptr, std::numeric_limits<double>::max());
881 7650616 : for (int i = 0; i < leaders.numSublanes(); ++i) {
882 6202673 : CLeaderDist cand = leaders[i];
883 6202673 : if (cand.first != nullptr) {
884 5210919 : double rightSide = cand.first->getRightSideOnLane();
885 5210919 : if (cand.first->getLane() != vehicle->getLane()) {
886 : // the candidate may be a parial (sideways) occupier so getRightSideOnLane() cannot be used
887 193704 : rightSide += (cand.first->getCenterOnEdge(cand.first->getLane())
888 193704 : - vehicle->getCenterOnEdge(vehicle->getLane()));
889 : }
890 : #ifdef DEBUG_CHANGE_OPPOSITE
891 : if (vehicle->isSelected()) {
892 : std::cout << SIMTIME << " cand=" << cand.first->getID() << " rightSide=" << rightSide << "\n";
893 : }
894 : #endif
895 : if (cand.second < leader.second
896 1730520 : && rightSide < egoWidth
897 6649999 : && vehicle->getLane()->getWidth() - rightSide - cand.first->getVehicleType().getWidth() < egoWidth) {
898 : leader.first = const_cast<MSVehicle*>(cand.first);
899 : leader.second = cand.second;
900 : }
901 : }
902 : }
903 1447943 : return leader;
904 : }
905 :
906 :
907 : void
908 113731045 : MSLaneChangerSublane::addOutsideLeaders(const MSVehicle* vehicle, MSLeaderDistanceInfo& leaders) const {
909 113731045 : if (vehicle->getLaneChangeModel().getShadowLane() == nullptr) {
910 109690124 : const MSLane* lane = vehicle->getLane();
911 109690124 : const double rightOL = vehicle->getRightSideOnLane(lane);
912 109690124 : const double leftOL = vehicle->getLeftSideOnLane(lane);
913 : const bool outsideLeft = rightOL > lane->getWidth();
914 : #ifdef DEBUG_SURROUNDING
915 : if (DEBUG_COND) {
916 : std::cout << SIMTIME << " addOutsideLeaders veh=" << vehicle->getID() << " lane=" << lane->getID() << " rightOL=" << rightOL << " leftOL=" << leftOL << "\n";
917 : }
918 : #endif
919 109690124 : if (leftOL < 0 || outsideLeft) {
920 : int sublaneOffset = 0;
921 423 : if (outsideLeft) {
922 142 : sublaneOffset = MIN2(0, -(int)ceil((leftOL - lane->getWidth()) / MSGlobals::gLateralResolution) + 1);
923 : } else {
924 281 : sublaneOffset = MAX2(0, (int)ceil(-rightOL / MSGlobals::gLateralResolution) - 1);
925 : }
926 423 : if (sublaneOffset != 0) {
927 353 : leaders.setSublaneOffset(sublaneOffset);
928 : #ifdef DEBUG_SURROUNDING
929 : if (DEBUG_COND) {
930 : std::cout << " sublaneOffset=" << sublaneOffset << " outsideLeft=" << outsideLeft << "\n";
931 : }
932 : #endif
933 686 : for (const MSVehicle* cand : lane->myTmpVehicles) {
934 : #ifdef DEBUG_SURROUNDING
935 : if (DEBUG_COND) {
936 : std::cout << " cand=" << cand->getID() << " cLOE=" << cand->getLeftSideOnEdge() << " cROE=" << cand->getRightSideOnEdge() << "\n";
937 : }
938 : #endif
939 333 : if (cand->getPositionOnLane() > vehicle->getPositionOnLane()
940 333 : && ((!outsideLeft && cand->getLeftSideOnEdge() < 0)
941 318 : || (outsideLeft && cand->getLeftSideOnEdge() > lane->getEdge().getWidth()))) {
942 36 : const double gap = cand->getPositionOnLane() - vehicle->getPositionOnLane() - cand->getLength() - vehicle->getVehicleType().getMinGap();
943 36 : leaders.addLeader(cand, gap);
944 : #ifdef DEBUG_SURROUNDING
945 : if (DEBUG_COND) {
946 : std::cout << " outsideLeader=" << cand->getID() << " ahead=" << leaders.toString() << "\n";
947 : }
948 : #endif
949 : }
950 : }
951 : }
952 : }
953 : }
954 113731045 : }
955 :
956 : /****************************************************************************/
|
