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 "MSVehicle.h"
26 : #include "MSVehicleType.h"
27 : #include "MSVehicleTransfer.h"
28 : #include "MSGlobals.h"
29 : #include <cassert>
30 : #include <iterator>
31 : #include <cstdlib>
32 : #include <cmath>
33 : #include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
34 : #include <utils/common/MsgHandler.h>
35 : #include <utils/geom/GeomHelper.h>
36 :
37 :
38 : // ===========================================================================
39 : // DEBUG constants
40 : // ===========================================================================
41 : #define DEBUG_COND (vehicle->getLaneChangeModel().debugVehicle())
42 : //#define DEBUG_COND (vehicle->getID() == "disabled")
43 : //#define DEBUG_COND true
44 : //#define DEBUG_DECISION
45 : //#define DEBUG_ACTIONSTEPS
46 : //#define DEBUG_STATE
47 : //#define DEBUG_MANEUVER
48 : //#define DEBUG_SURROUNDING
49 : //#define DEBUG_CHANGE_OPPOSITE
50 :
51 : // ===========================================================================
52 : // member method definitions
53 : // ===========================================================================
54 175425 : MSLaneChangerSublane::MSLaneChangerSublane(const std::vector<MSLane*>* lanes, bool allowChanging) :
55 175425 : MSLaneChanger(lanes, allowChanging) {
56 : // initialize siblings
57 175425 : if (myChanger.front().lane->isInternal()) {
58 200256 : for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
59 231150 : for (ChangerIt ce2 = myChanger.begin(); ce2 != myChanger.end(); ++ce2) {
60 126587 : if (ce != ce2 && ce->lane->getIncomingLanes().front().lane == ce2->lane->getIncomingLanes().front().lane) {
61 : //std::cout << "addSibling lane=" << ce->lane->getID() << " offset=" << ce2->lane->getIndex() - ce->lane->getIndex() << "\n";
62 3672 : ce->siblings.push_back(ce2->lane->getIndex() - ce->lane->getIndex());
63 : }
64 : }
65 : }
66 : }
67 175425 : }
68 :
69 :
70 347060 : MSLaneChangerSublane::~MSLaneChangerSublane() {}
71 :
72 : void
73 13254311 : MSLaneChangerSublane::initChanger() {
74 13254311 : MSLaneChanger::initChanger();
75 : // Prepare myChanger with a safe state.
76 34280082 : for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
77 42051542 : ce->ahead = ce->lane->getPartialBeyond();
78 : ce->outsideBounds.clear();
79 : // std::cout << SIMTIME << " initChanger lane=" << ce->lane->getID() << " vehicles=" << toString(ce->lane->myVehicles) << "\n";
80 : // std::cout << SIMTIME << " initChanger lane=" << ce->lane->getID() << " partial vehicles=" << toString(ce->lane->myPartialVehicles) << "\n";
81 : // std::cout << SIMTIME << " initChanger lane=" << ce->lane->getID() << " partial vehicles beyond=" << toString(ce->ahead.toString()) << "\n";
82 : }
83 13254311 : }
84 :
85 :
86 :
87 : void
88 100344911 : MSLaneChangerSublane::updateChanger(bool vehHasChanged) {
89 100344911 : MSLaneChanger::updateChanger(vehHasChanged);
90 100344911 : if (!vehHasChanged) {
91 99965891 : MSVehicle* lead = myCandi->lead;
92 : //std::cout << SIMTIME << " updateChanger lane=" << myCandi->lane->getID() << " lead=" << Named::getIDSecure(lead) << " lsol=" << lead->getLeftSideOnLane() << " rsol=" << lead->getRightSideOnLane() << "\n";
93 99965891 : if (lead->getLeftSideOnLane() < 0 || lead->getRightSideOnLane() > myCandi->lane->getWidth()) {
94 134423 : myCandi->outsideBounds.push_back(lead);
95 : } else {
96 99831468 : myCandi->ahead.addLeader(lead, false, 0);
97 : }
98 99965891 : MSLane* shadowLane = lead->getLaneChangeModel().getShadowLane();
99 99965891 : if (shadowLane != nullptr && &shadowLane->getEdge() == &lead->getLane()->getEdge()) {
100 : assert(shadowLane->getIndex() < (int)myChanger.size());
101 2164940 : const double latOffset = lead->getLane()->getRightSideOnEdge() - shadowLane->getRightSideOnEdge();
102 : //std::cout << SIMTIME << " updateChanger shadowLane=" << shadowLane->getID() << " lead=" << Named::getIDSecure(lead) << "\n";
103 2164940 : (myChanger.begin() + shadowLane->getIndex())->ahead.addLeader(lead, false, latOffset);
104 : }
105 : }
106 : //std::cout << SIMTIME << " updateChanger: lane=" << myCandi->lane->getID() << " lead=" << Named::getIDSecure(myCandi->lead) << " ahead=" << myCandi->ahead.toString() << " vehHasChanged=" << vehHasChanged << "\n";
107 : //for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
108 : // std::cout << " lane=" << ce->lane->getID() << " vehicles=" << toString(ce->lane->myVehicles) << "\n";
109 : //}
110 100344911 : }
111 :
112 :
113 : bool
114 100344911 : MSLaneChangerSublane::change() {
115 : // variant of change() for the sublane case
116 100344911 : myCandi = findCandidate();
117 : MSVehicle* vehicle = veh(myCandi);
118 100344911 : vehicle->getLaneChangeModel().clearNeighbors();
119 : #ifdef DEBUG_ACTIONSTEPS
120 : if (DEBUG_COND) {
121 : std::cout << "\n" << SIMTIME << " CHANGE veh=" << vehicle->getID() << " lane=" << vehicle->getLane()->getID() << "\n";
122 : }
123 : #endif
124 : assert(vehicle->getLane() == (*myCandi).lane);
125 : assert(!vehicle->getLaneChangeModel().isChangingLanes());
126 100344911 : if (/*!myAllowsChanging*/ vehicle->getLaneChangeModel().alreadyChanged() || vehicle->isStoppedOnLane()) {
127 666205 : registerUnchanged(vehicle);
128 666205 : if (vehicle->isStoppedOnLane()) {
129 664266 : myCandi->lastStopped = vehicle;
130 : }
131 666205 : return false;
132 : }
133 99678706 : if (!vehicle->isActive()) {
134 : #ifdef DEBUG_ACTIONSTEPS
135 : if (DEBUG_COND) {
136 : std::cout << SIMTIME << " veh '" << vehicle->getID() << "' skips regular change checks." << std::endl;
137 : }
138 : #endif
139 :
140 : bool changed;
141 : // let TraCI influence the wish to change lanes during non-actionsteps
142 20409291 : checkTraCICommands(vehicle);
143 :
144 : // Resume change
145 20409291 : changed = continueChangeSublane(vehicle, myCandi);
146 : #ifdef DEBUG_ACTIONSTEPS
147 : if (DEBUG_COND) {
148 : std::cout << SIMTIME << " veh '" << vehicle->getID() << "' lcm->maneuverDist=" << vehicle->getLaneChangeModel().getManeuverDist()
149 : << " lcm->speedLat=" << vehicle->getLaneChangeModel().getSpeedLat() << std::endl;
150 : }
151 : #endif
152 20409291 : if (!changed) {
153 20401915 : registerUnchanged(vehicle);
154 : }
155 20409291 : return changed;
156 : }
157 :
158 : #ifdef DEBUG_ACTIONSTEPS
159 : if (DEBUG_COND) {
160 : std::cout << "\n" << SIMTIME << " veh '" << vehicle->getID() << "' plans lanechange maneuver." << std::endl;
161 : }
162 : #endif
163 79269415 : vehicle->updateBestLanes(); // needed?
164 79269415 : const bool isOpposite = vehicle->getLaneChangeModel().isOpposite();
165 79269415 : if (!isOpposite) {
166 201217633 : for (int i = 0; i < (int) myChanger.size(); ++i) {
167 122037290 : vehicle->adaptBestLanesOccupation(i, myChanger[i].dens);
168 : }
169 : }
170 : // update leaders beyond the current edge for all lanes
171 201415648 : for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
172 122146233 : ce->aheadNext = getLeaders(ce, vehicle);
173 : }
174 : // update expected speeds
175 : int sublaneIndex = 0;
176 201415648 : for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
177 122146233 : vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(ce->aheadNext, sublaneIndex, ce->lane->getIndex());
178 122967585 : for (int offset : ce->siblings) {
179 : // treat sibling lanes (internal lanes with the same origin lane) as if they have the same geometry
180 : ChangerIt ceSib = ce + offset;
181 821352 : vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(ceSib->aheadNext, sublaneIndex, ceSib->lane->getIndex());
182 : }
183 122146233 : sublaneIndex += ce->ahead.numSublanes();
184 : }
185 :
186 : // Check for changes to the opposite lane if vehicle is active
187 : #ifdef DEBUG_ACTIONSTEPS
188 : if (DEBUG_COND) {
189 : std::cout << " myChangeToOpposite=" << myChangeToOpposite << " isOpposite=" << isOpposite << " mayChangeRight=" << mayChange(-1) << " mayChangeLeft=" << mayChange(1) << "\n";
190 : }
191 : #endif
192 :
193 79269415 : const bool stopOpposite = hasOppositeStop(vehicle);
194 79269415 : const int traciState = vehicle->influenceChangeDecision(0);
195 79269415 : const bool traciRequestOpposite = !mayChange(1) && (traciState & LCA_LEFT) != 0;
196 :
197 81468341 : if (myChangeToOpposite && (
198 : // cannot overtake since there is only one usable lane (or emergency)
199 4397852 : ((!mayChange(-1) && !mayChange(1)) || vehicle->getVClass() == SVC_EMERGENCY)
200 : || traciRequestOpposite
201 634262 : || stopOpposite
202 : // can alway come back from the opposite side
203 633850 : || isOpposite)) {
204 1565633 : const MSLeaderDistanceInfo& leaders = myCandi->aheadNext;
205 1565633 : if (leaders.hasVehicles() || isOpposite || stopOpposite || traciRequestOpposite) {
206 1439192 : std::pair<MSVehicle*, double> leader = findClosestLeader(leaders, vehicle);
207 1439192 : myCheckedChangeOpposite = false;
208 130582 : if ((leader.first != nullptr || isOpposite || stopOpposite || traciRequestOpposite)
209 1470680 : && changeOpposite(vehicle, leader, myCandi->lastStopped)) {
210 165236 : return true;
211 1435059 : } else if (myCheckedChangeOpposite) {
212 161103 : registerUnchanged(vehicle);
213 161103 : return false;
214 : }
215 : // try sublane change within current lane otherwise
216 : }
217 : }
218 :
219 79104179 : LaneChangeAction alternatives = (LaneChangeAction)((mayChange(-1) ? LCA_RIGHT : LCA_NONE)
220 79104179 : | (mayChange(1) ? LCA_LEFT : LCA_NONE));
221 :
222 79104179 : StateAndDist right = checkChangeHelper(vehicle, -1, alternatives);
223 79104179 : StateAndDist left = checkChangeHelper(vehicle, 1, alternatives);
224 79104179 : StateAndDist current = checkChangeHelper(vehicle, 0, alternatives);
225 :
226 158208358 : StateAndDist decision = vehicle->getLaneChangeModel().decideDirection(current,
227 79104179 : vehicle->getLaneChangeModel().decideDirection(right, left));
228 : #ifdef DEBUG_DECISION
229 : if (vehicle->getLaneChangeModel().debugVehicle()) {
230 : std::cout << "\n" << SIMTIME << " decision=" << toString((LaneChangeAction)decision.state) << " dir=" << decision.dir << " latDist=" << decision.latDist << " maneuverDist=" << decision.maneuverDist << "\n";
231 : }
232 : #endif
233 79104179 : vehicle->getLaneChangeModel().setOwnState(decision.state);
234 79104179 : if ((decision.state & LCA_WANTS_LANECHANGE) != 0 && (decision.state & LCA_BLOCKED) == 0) {
235 : // change if the vehicle wants to and is allowed to change
236 : #ifdef DEBUG_MANEUVER
237 : if (DEBUG_COND) {
238 : std::cout << SIMTIME << " veh '" << vehicle->getID() << "' performing sublane change..." << std::endl;
239 : }
240 : #endif
241 3084579 : const bool changed = startChangeSublane(vehicle, myCandi, decision.latDist, decision.maneuverDist);
242 3084579 : if (!changed) {
243 2717068 : registerUnchanged(vehicle);
244 : }
245 3084579 : return changed;
246 : }
247 : // @note this assumes vehicles can instantly abort any maneuvre in case of emergency
248 76019600 : abortLCManeuver(vehicle);
249 76019600 : registerUnchanged(vehicle);
250 :
251 : if ((right.state & (LCA_URGENT)) != 0 && (left.state & (LCA_URGENT)) != 0) {
252 : // ... wants to go to the left AND to the right
253 : // just let them go to the right lane...
254 : left.state = 0;
255 : }
256 : return false;
257 : }
258 :
259 :
260 : void
261 76020052 : MSLaneChangerSublane::abortLCManeuver(MSVehicle* vehicle) {
262 : #ifdef DEBUG_MANEUVER
263 : if (DEBUG_COND) {
264 : std::cout << SIMTIME << " veh '" << vehicle->getID() << "' aborts LC-continuation."
265 : << std::endl;
266 : }
267 : #endif
268 76020052 : const int priorReason = vehicle->getLaneChangeModel().getPrevState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
269 76020052 : const int reason = vehicle->getLaneChangeModel().getOwnState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
270 76020052 : if (priorReason != 0 && priorReason != reason && vehicle->getLaneChangeModel().getPreviousManeuverDist() != 0) {
271 : // original from cannot be reconstructed
272 72306 : const int priorDirection = vehicle->getLaneChangeModel().getPreviousManeuverDist() > 0 ? 1 : -1;
273 : #ifdef DEBUG_MANEUVER
274 : if (DEBUG_COND) {
275 : std::cout << SIMTIME << " abortLCManeuver priorReason=" << toString((LaneChangeAction)priorReason)
276 : << " reason=" << toString((LaneChangeAction)reason) << " veh=" << vehicle->getID() << "\n";
277 : }
278 : #endif
279 72306 : outputLCEnded(vehicle, myCandi, myCandi, priorDirection);
280 : }
281 76020052 : const double updatedSpeedLat = vehicle->getLaneChangeModel().getSpeedLat() != 0;
282 76020052 : vehicle->getLaneChangeModel().setSpeedLat(0);
283 76020052 : vehicle->getLaneChangeModel().setManeuverDist(0.);
284 76020052 : vehicle->getLaneChangeModel().updateTargetLane();
285 76020052 : if (updatedSpeedLat) {
286 : // update angle after having reset lateral speed
287 1270526 : vehicle->setAngle(vehicle->computeAngle());
288 : }
289 76020052 : }
290 :
291 :
292 : MSLaneChangerSublane::StateAndDist
293 237312537 : MSLaneChangerSublane::checkChangeHelper(MSVehicle* vehicle, int laneOffset, LaneChangeAction alternatives) {
294 : StateAndDist result = StateAndDist(0, 0, 0, 0);
295 237312537 : if (mayChange(laneOffset)) {
296 114203542 : if (laneOffset != 0 && vehicle->getLaneChangeModel().isOpposite()) {
297 42 : return result;
298 : }
299 114203500 : const std::vector<MSVehicle::LaneQ>& preb = (vehicle->getLaneChangeModel().isOpposite()
300 114203500 : ? getBestLanesOpposite(vehicle, nullptr, 1000)
301 114203500 : : vehicle->getBestLanes());
302 114203500 : result.state = checkChangeSublane(laneOffset, alternatives, preb, result.latDist, result.maneuverDist);
303 114203500 : result.dir = laneOffset;
304 114203500 : if ((result.state & LCA_WANTS_LANECHANGE) != 0 && (result.state & LCA_URGENT) != 0 && (result.state & LCA_BLOCKED) != 0) {
305 1556800 : (myCandi + laneOffset)->lastBlocked = vehicle;
306 1556800 : if ((myCandi + laneOffset)->firstBlocked == nullptr) {
307 534426 : (myCandi + laneOffset)->firstBlocked = vehicle;
308 : }
309 : }
310 114203500 : }
311 : return result;
312 : }
313 :
314 :
315 : /// @brief Continue a sublane-lane change maneuver and return whether the midpoint was passed in this step
316 : // (used to continue sublane changing in non-action steps).
317 : bool
318 20409291 : MSLaneChangerSublane::continueChangeSublane(MSVehicle* vehicle, ChangerIt& from) {
319 : // lateral distance to complete maneuver
320 20409291 : double remLatDist = vehicle->getLaneChangeModel().getManeuverDist();
321 20409291 : if (remLatDist == 0) {
322 : return false;
323 : }
324 811218 : const bool urgent = (vehicle->getLaneChangeModel().getOwnState() & LCA_URGENT) != 0;
325 811218 : const double nextLatDist = SPEED2DIST(vehicle->getLaneChangeModel().computeSpeedLat(remLatDist, remLatDist, urgent));
326 : #ifdef DEBUG_MANEUVER
327 : if (DEBUG_COND) {
328 : std::cout << SIMTIME << " vehicle '" << vehicle->getID() << "' continueChangeSublane()"
329 : << " remLatDist=" << remLatDist << " nextLatDist=" << nextLatDist
330 : << std::endl;
331 : }
332 : #endif
333 :
334 811218 : const bool changed = startChangeSublane(vehicle, from, nextLatDist, remLatDist);
335 : return changed;
336 : }
337 :
338 :
339 : bool
340 3952747 : MSLaneChangerSublane::startChangeSublane(MSVehicle* vehicle, ChangerIt& from, double latDist, double maneuverDist) {
341 3952747 : if (vehicle->isRemoteControlled()) {
342 : return false;
343 : }
344 3952248 : MSLane* source = from->lane;
345 : // Prevent continuation of LC beyond lane borders if change is not allowed
346 3952248 : double distToRightLaneBorder = vehicle->getLane()->getWidth() * 0.5 + vehicle->getLateralPositionOnLane() - vehicle->getWidth() * 0.5;
347 3952248 : double distToLeftLaneBorder = vehicle->getLane()->getWidth() * 0.5 - vehicle->getLateralPositionOnLane() - vehicle->getWidth() * 0.5;
348 3952248 : if (vehicle->getLaneChangeModel().isOpposite()) {
349 : std::swap(distToRightLaneBorder, distToLeftLaneBorder);
350 : }
351 : // determine direction of LC
352 : int direction = 0;
353 3952248 : if (latDist > 0 && latDist > distToLeftLaneBorder) {
354 : direction = 1;
355 3459533 : } else if (latDist < 0 && -latDist > distToRightLaneBorder) {
356 : direction = -1;
357 : }
358 3952248 : const int changerDirection = vehicle->getLaneChangeModel().isOpposite() ? -direction : direction;
359 3952248 : ChangerIt to = from;
360 : #ifdef DEBUG_MANEUVER
361 : if (DEBUG_COND) {
362 : std::cout << SIMTIME << " vehicle '" << vehicle->getID() << "' latDist=" << latDist << " maneuverDist=" << maneuverDist
363 : << " distRight=" << distToRightLaneBorder << " distLeft=" << distToLeftLaneBorder
364 : << " dir=" << direction << " cDir=" << changerDirection << " mayChange=" << mayChange(changerDirection) << "\n";
365 : }
366 : #endif
367 3952248 : if (mayChange(changerDirection)) {
368 3933584 : to = from + changerDirection;
369 18664 : } else if (changerDirection == 1 && source->getOpposite() != nullptr) {
370 : // change to the opposite direction lane
371 18212 : to = (source->getOpposite()->getEdge().myLaneChanger->getChanger().end() - 1);
372 : } else {
373 : // This may occur during maneuver continuation in non-actionsteps.
374 : // TODO: Understand better why and test later if additional sublane actionstep debugging resolves this
375 : // (XXX: perhaps one should try to extrapolate check for this case before to avoid maneuver initialization
376 : // similar as for continuous LC in MSLaneChanger::checkChange())
377 : //assert(false);
378 452 : abortLCManeuver(vehicle);
379 452 : return false;
380 : }
381 :
382 : // The following does:
383 : // 1) update vehicles lateral position according to latDist and target lane
384 : // 2) distinguish several cases
385 : // a) vehicle moves completely within the same lane
386 : // b) vehicle intersects another lane
387 : // - vehicle must be moved to the lane where its midpoint is (either old or new)
388 : // - shadow vehicle must be created/moved to the other lane if the vehicle intersects it
389 : // 3) updated dens of all lanes that hold the vehicle or its shadow
390 :
391 3951796 : vehicle->myState.myPosLat += latDist * (vehicle->getLaneChangeModel().isOpposite() ? -1 : 1);
392 4191726 : for (int i = 0; i < (int)vehicle->myFurtherLanesPosLat.size(); i++) {
393 479860 : vehicle->myFurtherLanesPosLat[i] += latDist * (vehicle->getLaneChangeModel().isOpposite() ? -1 : 1);
394 : }
395 3951796 : vehicle->myCachedPosition = Position::INVALID;
396 3951796 : vehicle->getLaneChangeModel().setSpeedLat(DIST2SPEED(latDist));
397 : #ifdef DEBUG_MANEUVER
398 : if (DEBUG_COND) {
399 : std::cout << SIMTIME << " vehicle '" << vehicle->getID() << "' with maneuverDist=" << maneuverDist
400 : << " and committedSpeed=" << vehicle->getLaneChangeModel().getCommittedSpeed()
401 : << " increments lateral position by latDist=" << latDist << std::endl;
402 : }
403 : #endif
404 : #ifdef DEBUG_SURROUNDING
405 : if (DEBUG_COND) {
406 : std::cout << SIMTIME << " vehicle '" << vehicle->getID() << "'\n to->ahead=" << to->ahead.toString()
407 : << "'\n to->aheadNext=" << to->aheadNext.toString()
408 : << std::endl;
409 : }
410 : #endif
411 3951796 : const bool completedPriorManeuver = fabs(vehicle->getLaneChangeModel().getPreviousManeuverDist()) < NUMERICAL_EPS;
412 3951796 : const bool completedManeuver = fabs(maneuverDist - latDist) < NUMERICAL_EPS;
413 3951796 : vehicle->getLaneChangeModel().updateSafeLatDist(latDist);
414 :
415 : // current maneuver is aborted when direction or reason changes
416 3951796 : const int priorReason = vehicle->getLaneChangeModel().getPrevState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
417 3951796 : const int reason = vehicle->getLaneChangeModel().getOwnState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
418 : #ifdef DEBUG_MANEUVER
419 : if (DEBUG_COND) {
420 : std::cout << SIMTIME << " vehicle '" << vehicle->getID()
421 : << "' completedPriorManeuver=" << completedPriorManeuver
422 : << " completedManeuver=" << completedManeuver
423 : << " priorReason=" << toString((LaneChangeAction)priorReason)
424 : << " reason=" << toString((LaneChangeAction)reason)
425 : << " priorManeuverDist=" << vehicle->getLaneChangeModel().getPreviousManeuverDist()
426 : << " maneuverDist=" << maneuverDist
427 : << " latDist=" << latDist
428 : << std::endl;
429 : }
430 : #endif
431 3951796 : if (!completedManeuver && !completedPriorManeuver && priorReason != 0 &&
432 1221477 : (vehicle->getLaneChangeModel().getPreviousManeuverDist() * latDist < 0
433 1206431 : || priorReason != reason)) {
434 144286 : const int priorDirection = vehicle->getLaneChangeModel().getPreviousManeuverDist() > 0 ? 1 : -1;
435 : // original from cannot be reconstructed
436 : #ifdef DEBUG_MANEUVER
437 : if (DEBUG_COND) {
438 : std::cout << SIMTIME << " startChangeSublane abort priorReason=" << toString((LaneChangeAction)priorReason)
439 : << " reason=" << toString((LaneChangeAction)reason) << " veh=" << vehicle->getID() << "\n";
440 : }
441 : #endif
442 144286 : outputLCEnded(vehicle, from, from, priorDirection);
443 : }
444 :
445 3951796 : outputLCStarted(vehicle, from, to, direction, maneuverDist);
446 3951796 : vehicle->getLaneChangeModel().setManeuverDist(maneuverDist - latDist);
447 3951796 : const bool changedToNewLane = checkChangeToNewLane(vehicle, direction, from, to);
448 :
449 3951796 : MSLane* oldShadowLane = vehicle->getLaneChangeModel().getShadowLane();
450 3951796 : vehicle->getLaneChangeModel().updateShadowLane();
451 3951796 : MSLane* shadowLane = vehicle->getLaneChangeModel().getShadowLane();
452 3951796 : if (shadowLane != nullptr && shadowLane != oldShadowLane
453 3951796 : && &shadowLane->getEdge() == &source->getEdge()) {
454 : assert(oldShadowLane == 0 || vehicle->getLaneChangeModel().isOpposite() || to != from);
455 770850 : const double latOffset = vehicle->getLane()->getRightSideOnEdge() - shadowLane->getRightSideOnEdge();
456 770850 : (myChanger.begin() + shadowLane->getIndex())->ahead.addLeader(vehicle, false, latOffset);
457 : }
458 3951796 : if (completedManeuver) {
459 1686450 : outputLCEnded(vehicle, from, to, direction);
460 : }
461 :
462 : // Update maneuver reservations on target lanes
463 3951796 : MSLane* targetLane = vehicle->getLaneChangeModel().updateTargetLane();
464 3951796 : if (!changedToNewLane && targetLane != nullptr
465 210423 : && vehicle->getActionStepLength() > DELTA_T
466 4050637 : && &targetLane->getEdge() == &source->getEdge()
467 : ) {
468 98816 : const int dir = (vehicle->getLaneChangeModel().getManeuverDist() > 0 ? 1 : -1);
469 : ChangerIt target = from + dir;
470 98816 : const double actionStepDist = dir * vehicle->getVehicleType().getMaxSpeedLat() * vehicle->getActionStepLengthSecs();
471 98816 : const double latOffset = vehicle->getLatOffset(targetLane) + actionStepDist;
472 98816 : target->ahead.addLeader(vehicle, false, latOffset);
473 : //std::cout << SIMTIME << " veh=" << vehicle->getID() << " target=" << targetLane->getID()
474 : // << " actionStepDist=" << actionStepDist << " latOffset=" << latOffset
475 : // << " targetAhead=" << target->ahead.toString() << "\n";
476 : }
477 :
478 : // compute new angle of the vehicle from the x- and y-distances travelled within last time step
479 : // (should happen last because primaryLaneChanged() also triggers angle computation)
480 : // this part of the angle comes from the orientation of our current lane
481 3951796 : double laneAngle = vehicle->computeAngle();
482 3951796 : if (vehicle->getLaneChangeModel().isOpposite()) {
483 : // reverse lane angle
484 35385 : laneAngle += M_PI;
485 : }
486 : #ifdef DEBUG_MANEUVER
487 : if (DEBUG_COND) {
488 : std::cout << SIMTIME << " startChangeSublane"
489 : << " oldLane=" << from->lane->getID()
490 : << " newLane=" << to->lane->getID()
491 : << " shadowLane=" << (shadowLane != nullptr ? shadowLane->getID() : "NULL")
492 : << " targetLane=" << (targetLane != nullptr ? targetLane->getID() : "NULL")
493 : << " maneuverDist=" << vehicle->getLaneChangeModel().getManeuverDist()
494 : << " latDist=" << latDist
495 : << " oldShadowLane=" << Named::getIDSecure(oldShadowLane)
496 : << " newShadowLane=" << Named::getIDSecure(vehicle->getLaneChangeModel().getShadowLane())
497 : << " laneA=" << RAD2DEG(laneAngle)
498 : << " oldA=" << RAD2DEG(vehicle->getAngle())
499 : << " oppposite=" << vehicle->getLaneChangeModel().isOpposite()
500 : << " changedToNewLane=" << changedToNewLane
501 : << "\n";
502 : }
503 : #endif
504 3951796 : vehicle->setAngle(laneAngle, completedManeuver);
505 :
506 : // check if a traci maneuver must continue
507 : // getOwnState is reset to 0 when changing lanes so we use the stored reason
508 3951796 : if ((reason & LCA_TRACI) != 0) {
509 : #ifdef DEBUG_MANEUVER
510 : if (DEBUG_COND) {
511 : std::cout << SIMTIME << " continue TraCI-maneuver remainingLatDist=" << vehicle->getLaneChangeModel().getManeuverDist() << "\n";
512 : }
513 : #endif
514 1151 : vehicle->getInfluencer().setSublaneChange(vehicle->getLaneChangeModel().getManeuverDist());
515 : }
516 3951796 : from->lane->requireCollisionCheck();
517 3951796 : to->lane->requireCollisionCheck();
518 3951796 : return changedToNewLane;
519 : }
520 :
521 : bool
522 3951796 : MSLaneChangerSublane::checkChangeToNewLane(MSVehicle* vehicle, const int direction, ChangerIt from, ChangerIt to) {
523 3951796 : const int oppositeSign = vehicle->getLaneChangeModel().isOpposite() ? -1 : 1;
524 3951796 : const bool opposite = (&from->lane->getEdge() != &to->lane->getEdge());
525 : const bool changedToNewLane = (to->lane != from->lane
526 934984 : && fabs(vehicle->getLateralPositionOnLane()) > 0.5 * vehicle->getLane()->getWidth()
527 4330816 : && (mayChange(direction * oppositeSign) || opposite));
528 : if (changedToNewLane) {
529 379020 : vehicle->myState.myPosLat -= direction * 0.5 * (from->lane->getWidth() + to->lane->getWidth()) * oppositeSign;
530 379020 : if (!opposite) {
531 374988 : to->lane->myTmpVehicles.insert(to->lane->myTmpVehicles.begin(), vehicle);
532 374988 : to->dens += vehicle->getVehicleType().getLengthWithGap();
533 : }
534 379020 : if (MSAbstractLaneChangeModel::haveLCOutput()) {
535 7366 : if (!vehicle->isActive()) {
536 : // update leaders beyond the current edge for all lanes
537 : // @note to->aheadNext and from->aheadNext are only needed for output in non-action steps.
538 1928 : to->aheadNext = getLeaders(to, vehicle);
539 1928 : from->aheadNext = getLeaders(from, vehicle);
540 : }
541 7366 : vehicle->getLaneChangeModel().setLeaderGaps(to->aheadNext);
542 7366 : vehicle->getLaneChangeModel().setFollowerGaps(to->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true));
543 7366 : vehicle->getLaneChangeModel().setOrigLeaderGaps(from->aheadNext);
544 : }
545 379020 : vehicle->getLaneChangeModel().startLaneChangeManeuver(from->lane, to->lane, direction);
546 379020 : if (!opposite) {
547 374988 : to->ahead.addLeader(vehicle, false, 0);
548 : }
549 : } else {
550 3572776 : from->ahead.addLeader(vehicle, false, 0);
551 : }
552 3951796 : return changedToNewLane;
553 : }
554 :
555 : void
556 3951796 : MSLaneChangerSublane::outputLCStarted(MSVehicle* vehicle, ChangerIt& from, ChangerIt& to, int direction, double maneuverDist) {
557 435595 : if (MSAbstractLaneChangeModel::haveLCOutput() && MSAbstractLaneChangeModel::outputLCStarted()
558 : // non-sublane change started
559 1209 : && ((vehicle->getLaneChangeModel().getOwnState() & (LCA_CHANGE_REASONS & ~LCA_SUBLANE)) != 0)
560 1019 : && ((vehicle->getLaneChangeModel().getOwnState() & LCA_STAY) == 0)
561 : // no changing for the same reason in previous step (either not wanted or blocked)
562 3953834 : && ((vehicle->getLaneChangeModel().getPrevState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE) !=
563 1019 : (vehicle->getLaneChangeModel().getOwnState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE)
564 835 : || ((vehicle->getLaneChangeModel().getPrevState() & LCA_STAY) != 0)
565 835 : || ((vehicle->getLaneChangeModel().getPrevState() & LCA_BLOCKED) != 0))
566 : ) {
567 : #ifdef DEBUG_STATE
568 : if (DEBUG_COND) {
569 : std::cout << SIMTIME << " veh=" << vehicle->getID() << " laneChangeStarted state=" << toString((LaneChangeAction)vehicle->getLaneChangeModel().getOwnState())
570 : << " prevState=" << toString((LaneChangeAction)vehicle->getLaneChangeModel().getPrevState())
571 : << " filter=" << toString((LaneChangeAction)(LCA_CHANGE_REASONS & ~LCA_SUBLANE))
572 : << " filtered=" << toString((LaneChangeAction)(vehicle->getLaneChangeModel().getOwnState() & (LCA_CHANGE_REASONS & ~LCA_SUBLANE)))
573 : << "\n";
574 : }
575 : #endif
576 204 : vehicle->getLaneChangeModel().setLeaderGaps(to->aheadNext);
577 204 : vehicle->getLaneChangeModel().setFollowerGaps(to->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true));
578 204 : vehicle->getLaneChangeModel().setOrigLeaderGaps(from->aheadNext);
579 408 : vehicle->getLaneChangeModel().laneChangeOutput("changeStarted", from->lane, to->lane, direction, maneuverDist);
580 : }
581 3951796 : }
582 :
583 : void
584 1903042 : MSLaneChangerSublane::outputLCEnded(MSVehicle* vehicle, ChangerIt& from, ChangerIt& to, int direction) {
585 147413 : if (MSAbstractLaneChangeModel::haveLCOutput() && MSAbstractLaneChangeModel::outputLCEnded()
586 : // non-sublane change ended
587 1903164 : && ((vehicle->getLaneChangeModel().getOwnState() & (LCA_CHANGE_REASONS & ~LCA_SUBLANE)) != 0)) {
588 80 : vehicle->getLaneChangeModel().setLeaderGaps(to->aheadNext);
589 80 : vehicle->getLaneChangeModel().setFollowerGaps(to->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true));
590 80 : vehicle->getLaneChangeModel().setOrigLeaderGaps(from->aheadNext);
591 160 : vehicle->getLaneChangeModel().laneChangeOutput("changeEnded", from->lane, to->lane, direction);
592 : }
593 1903042 : }
594 :
595 :
596 : MSLeaderDistanceInfo
597 122150089 : MSLaneChangerSublane::getLeaders(const ChangerIt& target, const MSVehicle* vehicle) const {
598 : // get the leading vehicle on the lane to change to
599 : #ifdef DEBUG_SURROUNDING
600 : if (DEBUG_COND) {
601 : std::cout << SIMTIME << " getLeaders lane=" << target->lane->getID() << " ego=" << vehicle->getID() << " ahead=" << target->ahead.toString() << "\n";
602 : }
603 : #endif
604 122150089 : MSLeaderDistanceInfo result(target->lane->getWidth(), nullptr, 0);
605 : int sublaneShift = 0;
606 122150089 : if (target->lane == vehicle->getLane()) {
607 79271343 : if (vehicle->getLeftSideOnLane() < -MSGlobals::gLateralResolution) {
608 16206 : sublaneShift = int(-vehicle->getLeftSideOnLane() / MSGlobals::gLateralResolution);
609 79255137 : } else if (vehicle->getRightSideOnLane() > target->lane->getWidth() + MSGlobals::gLateralResolution) {
610 61328 : sublaneShift = -int((vehicle->getRightSideOnLane() - target->lane->getWidth()) / MSGlobals::gLateralResolution);
611 : }
612 79271343 : result.setSublaneOffset(sublaneShift);
613 : }
614 632832656 : for (int i = 0; i < target->ahead.numSublanes(); ++i) {
615 510682567 : const MSVehicle* veh = target->ahead[i];
616 510682567 : if (veh != nullptr) {
617 377744398 : const double gap = veh->getBackPositionOnLane(target->lane) - vehicle->getPositionOnLane() - vehicle->getVehicleType().getMinGap();
618 : #ifdef DEBUG_SURROUNDING
619 : if (DEBUG_COND) {
620 : std::cout << " ahead lead=" << veh->getID() << " leadBack=" << veh->getBackPositionOnLane() << " gap=" << gap << " sublaneShift=" << sublaneShift << "\n";
621 : }
622 : #endif
623 377744398 : if (i + sublaneShift >= 0 && i + sublaneShift < result.numSublanes()) {
624 377663694 : result.addLeader(veh, gap, 0, i + sublaneShift);
625 : }
626 : }
627 : }
628 122150089 : if (sublaneShift != 0) {
629 166034 : for (MSVehicle* cand : target->outsideBounds) {
630 88500 : const double gap = cand->getBackPositionOnLane() - vehicle->getPositionOnLane() - vehicle->getVehicleType().getMinGap();
631 88500 : result.addLeader(cand, gap);
632 : }
633 : }
634 : #ifdef DEBUG_SURROUNDING
635 : if (DEBUG_COND) {
636 : std::cout << " outsideBounds=" << toString(target->outsideBounds) << " result=" << result.toString() << "\n";
637 : }
638 : #endif
639 122150089 : target->lane->addLeaders(vehicle, vehicle->getPositionOnLane(), result);
640 122150089 : return result;
641 0 : }
642 :
643 :
644 : int
645 114203500 : MSLaneChangerSublane::checkChangeSublane(
646 : int laneOffset,
647 : LaneChangeAction alternatives,
648 : const std::vector<MSVehicle::LaneQ>& preb,
649 : double& latDist,
650 : double& maneuverDist) const {
651 :
652 : ChangerIt target = myCandi + laneOffset;
653 : MSVehicle* vehicle = veh(myCandi);
654 114203500 : const MSLane& neighLane = *(target->lane);
655 114203500 : int blocked = 0;
656 :
657 114203500 : MSLeaderDistanceInfo neighLeaders = target->aheadNext;
658 114203500 : MSLeaderDistanceInfo neighFollowers = target->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
659 114203500 : MSLeaderDistanceInfo neighBlockers(neighLane.getWidth(), vehicle, vehicle->getLane()->getRightSideOnEdge() - neighLane.getRightSideOnEdge());
660 114203500 : MSLeaderDistanceInfo leaders = myCandi->aheadNext;
661 114203500 : addOutsideLeaders(vehicle, leaders);
662 114203500 : MSLeaderDistanceInfo followers = myCandi->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
663 114203500 : MSLeaderDistanceInfo blockers(vehicle->getLane()->getWidth(), vehicle, 0);
664 :
665 : // consider sibling lanes of the origin and target lane
666 114537674 : for (int offset : myCandi->siblings) {
667 : // treat sibling lanes (internal lanes with the same origin lane) as if they have the same geometry
668 : ChangerIt ceSib = myCandi + offset;
669 334174 : MSLeaderDistanceInfo sibFollowers = ceSib->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
670 334174 : if (sibFollowers.hasVehicles()) {
671 303638 : followers.addLeaders(sibFollowers);
672 : }
673 334174 : if (ceSib->aheadNext.hasVehicles()) {
674 223318 : leaders.addLeaders(ceSib->aheadNext);
675 : }
676 : #ifdef DEBUG_SURROUNDING
677 : if (DEBUG_COND) {
678 : std::cout << SIMTIME << " ego=" << vehicle->getID() << " ahead=" << myCandi->aheadNext.toString() << " sib=" << ceSib->lane->getID() << " sibAhead=" << ceSib->aheadNext.toString() << " leaders=" << leaders.toString() << "\n";
679 : }
680 : #endif
681 334174 : }
682 114565499 : for (int offset : target->siblings) {
683 : // treat sibling lanes (internal lanes with the same origin lane) as if they have the same geometry
684 : ChangerIt ceSib = target + offset;
685 361999 : MSLeaderDistanceInfo sibFollowers = ceSib->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
686 361999 : if (sibFollowers.hasVehicles()) {
687 330456 : neighFollowers.addLeaders(sibFollowers);
688 : }
689 361999 : if (ceSib->aheadNext.hasVehicles()) {
690 253388 : neighLeaders.addLeaders(ceSib->aheadNext);
691 : }
692 : #ifdef DEBUG_SURROUNDING
693 : if (DEBUG_COND) {
694 : std::cout << SIMTIME << " ego=" << vehicle->getID() << " neighAhead=" << target->aheadNext.toString() << " sib=" << ceSib->lane->getID() << " sibAhead=" << ceSib->aheadNext.toString() << " neighLeaders=" << neighLeaders.toString() << "\n";
695 : }
696 : #endif
697 361999 : }
698 :
699 : // break leader symmetry
700 114203500 : if (laneOffset == -1 && neighLeaders.hasVehicles()) {
701 12774987 : neighLeaders.moveSamePosTo(vehicle, neighFollowers);
702 : }
703 :
704 : #ifdef DEBUG_SURROUNDING
705 : if (DEBUG_COND) std::cout << SIMTIME
706 : << " checkChangeSublane: veh=" << vehicle->getID()
707 : << " laneOffset=" << laneOffset
708 : << "\n leaders=" << leaders.toString()
709 : << "\n neighLeaders=" << neighLeaders.toString()
710 : << "\n followers=" << followers.toString()
711 : << "\n neighFollowers=" << neighFollowers.toString()
712 : << "\n";
713 : #endif
714 :
715 :
716 114203500 : const int wish = vehicle->getLaneChangeModel().wantsChangeSublane(
717 : laneOffset, alternatives,
718 : leaders, followers, blockers,
719 : neighLeaders, neighFollowers, neighBlockers,
720 : neighLane, preb,
721 : &(myCandi->lastBlocked), &(myCandi->firstBlocked), latDist, maneuverDist, blocked);
722 114203500 : int state = blocked | wish;
723 :
724 : // XXX
725 : // do are more careful (but expensive) check to ensure that a
726 : // safety-critical leader is not being overlooked
727 :
728 : // XXX
729 : // ensure that a continuous lane change manoeuvre can be completed
730 : // before the next turning movement
731 :
732 : // let TraCI influence the wish to change lanes and the security to take
733 : const int oldstate = state;
734 114203500 : state = vehicle->influenceChangeDecision(state);
735 : #ifdef DEBUG_STATE
736 : if (DEBUG_COND && state != oldstate) {
737 : std::cout << SIMTIME << " veh=" << vehicle->getID() << " stateAfterTraCI=" << toString((LaneChangeAction)state) << " original=" << toString((LaneChangeAction)oldstate) << "\n";
738 : }
739 : #endif
740 114203500 : vehicle->getLaneChangeModel().getCanceledState(laneOffset) |= blocked;
741 114203500 : vehicle->getLaneChangeModel().saveLCState(laneOffset, oldstate, state);
742 114203500 : if (laneOffset != 0) {
743 35099321 : vehicle->getLaneChangeModel().saveNeighbors(laneOffset, neighFollowers, neighLeaders);
744 : }
745 114203500 : return state;
746 114203500 : }
747 :
748 :
749 : bool
750 165236 : MSLaneChangerSublane::checkChangeOpposite(
751 : MSVehicle* vehicle,
752 : int laneOffset,
753 : MSLane* targetLane,
754 : const std::pair<MSVehicle* const, double>& leader,
755 : const std::pair<MSVehicle* const, double>& neighLead,
756 : const std::pair<MSVehicle* const, double>& neighFollow,
757 : const std::vector<MSVehicle::LaneQ>& preb) {
758 165236 : myCheckedChangeOpposite = true;
759 :
760 : UNUSED_PARAMETER(leader);
761 : UNUSED_PARAMETER(neighLead);
762 : UNUSED_PARAMETER(neighFollow);
763 :
764 : const MSLane& neighLane = *targetLane;
765 165236 : MSLane* curLane = myCandi->lane;
766 :
767 165236 : MSLeaderDistanceInfo neighLeaders(targetLane->getWidth(), nullptr, 0);
768 165236 : MSLeaderDistanceInfo neighFollowers(targetLane->getWidth(), nullptr, 0);
769 165236 : MSLeaderDistanceInfo neighBlockers(targetLane->getWidth(), nullptr, 0);
770 165236 : MSLeaderDistanceInfo leaders(curLane->getWidth(), nullptr, 0);
771 165236 : MSLeaderDistanceInfo followers(curLane->getWidth(), nullptr, 0);
772 165236 : MSLeaderDistanceInfo blockers(curLane->getWidth(), nullptr, 0);
773 :
774 330472 : const double backPosOnTarget = vehicle->getLane()->getOppositePos(vehicle->getBackPositionOnLane());
775 165236 : if (vehicle->getLaneChangeModel().isOpposite()) {
776 88905 : leaders = curLane->getFollowersOnConsecutive(vehicle, vehicle->getPositionOnLane(), true, -1, MSLane::MinorLinkMode::FOLLOW_ONCOMING);
777 88905 : leaders.fixOppositeGaps(false);
778 88905 : curLane->addLeaders(vehicle, vehicle->getBackPositionOnLane(), followers);
779 88905 : followers.fixOppositeGaps(true);
780 88905 : neighFollowers = targetLane->getFollowersOnConsecutive(vehicle, backPosOnTarget, true);
781 88905 : neighFollowers.fixOppositeGaps(false);
782 : // artificially increase the position to ensure that ego is not added as a leader
783 88905 : const double posOnTarget = backPosOnTarget + vehicle->getVehicleType().getLength() + POSITION_EPS;
784 88905 : targetLane->addLeaders(vehicle, posOnTarget, neighLeaders);
785 88905 : neighLeaders.patchGaps(2 * POSITION_EPS);
786 : int sublaneIndex = 0;
787 108318 : for (int i = 0; i < targetLane->getIndex(); i++) {
788 19413 : sublaneIndex += MSLeaderInfo(targetLane->getEdge().getLanes()[i]->getWidth()).numSublanes();
789 : }
790 88905 : vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(neighLeaders, sublaneIndex, targetLane->getIndex());
791 : } else {
792 76331 : leaders = myCandi->aheadNext;
793 76331 : followers = myCandi->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
794 76331 : const double posOnTarget = backPosOnTarget - vehicle->getVehicleType().getLength();
795 76331 : targetLane->addLeaders(vehicle, backPosOnTarget, neighFollowers, true);
796 76331 : neighFollowers.fixOppositeGaps(true);
797 76331 : neighLeaders = targetLane->getFollowersOnConsecutive(vehicle, posOnTarget, true);
798 76331 : neighLeaders.fixOppositeGaps(false);
799 : }
800 :
801 :
802 : #ifdef DEBUG_CHANGE_OPPOSITE
803 : if (DEBUG_COND) std::cout << SIMTIME
804 : << " checkChangeOppositeSublane: veh=" << vehicle->getID()
805 : << " isOpposite=" << vehicle->getLaneChangeModel().isOpposite()
806 : << " laneOffset=" << laneOffset
807 : << "\n leaders=" << leaders.toString()
808 : << "\n neighLeaders=" << neighLeaders.toString()
809 : << "\n followers=" << followers.toString()
810 : << "\n neighFollowers=" << neighFollowers.toString()
811 : << "\n";
812 : #endif
813 :
814 165236 : LaneChangeAction alternatives = (LaneChangeAction)((mayChange(-1) ? LCA_RIGHT : LCA_NONE)
815 165236 : | (mayChange(1) ? LCA_LEFT : LCA_NONE));
816 :
817 165236 : int blocked = 0;
818 165236 : double latDist = 0;
819 165236 : double maneuverDist = 0;
820 165236 : const int wish = vehicle->getLaneChangeModel().wantsChangeSublane(
821 : laneOffset, alternatives,
822 : leaders, followers, blockers,
823 : neighLeaders, neighFollowers, neighBlockers,
824 : neighLane, preb,
825 : &(myCandi->lastBlocked), &(myCandi->firstBlocked), latDist, maneuverDist, blocked);
826 165236 : int state = blocked | wish;
827 :
828 : const int oldstate = state;
829 165236 : state = vehicle->influenceChangeDecision(state);
830 : #ifdef DEBUG_CHANGE_OPPOSITE
831 : if (DEBUG_COND && state != oldstate) {
832 : std::cout << SIMTIME << " veh=" << vehicle->getID() << " stateAfterTraCI=" << toString((LaneChangeAction)state) << " original=" << toString((LaneChangeAction)oldstate) << "\n";
833 : }
834 : #endif
835 165236 : vehicle->getLaneChangeModel().saveLCState(laneOffset, oldstate, state);
836 165236 : if (laneOffset != 0) {
837 165236 : vehicle->getLaneChangeModel().saveNeighbors(laneOffset, neighFollowers, neighLeaders);
838 : }
839 :
840 165236 : if ((state & LCA_WANTS_LANECHANGE) != 0 && (state & LCA_BLOCKED) == 0) {
841 : // change if the vehicle wants to and is allowed to change
842 : #ifdef DEBUG_CHANGE_OPPOSITE
843 : if (DEBUG_COND) {
844 : std::cout << SIMTIME << " veh '" << vehicle->getID() << "' performing sublane change latDist=" << latDist << " maneuverDist=" << maneuverDist << "\n";
845 : }
846 : #endif
847 56950 : vehicle->getLaneChangeModel().setOwnState(state);
848 56950 : return startChangeSublane(vehicle, myCandi, latDist, maneuverDist);
849 : } else {
850 108286 : vehicle->getLaneChangeModel().setSpeedLat(0);
851 : return false;
852 : }
853 165236 : }
854 :
855 : std::pair<MSVehicle*, double>
856 1439192 : MSLaneChangerSublane::findClosestLeader(const MSLeaderDistanceInfo& leaders, const MSVehicle* vehicle) {
857 1439192 : const double egoWidth = vehicle->getVehicleType().getWidth() + vehicle->getVehicleType().getMinGapLat();
858 : std::pair<MSVehicle*, double> leader(nullptr, std::numeric_limits<double>::max());
859 7603123 : for (int i = 0; i < leaders.numSublanes(); ++i) {
860 6163931 : CLeaderDist cand = leaders[i];
861 6163931 : if (cand.first != nullptr) {
862 5175255 : double rightSide = cand.first->getRightSideOnLane();
863 5175255 : if (cand.first->getLane() != vehicle->getLane()) {
864 : // the candidate may be a parial (sideways) occupier so getRightSideOnLane() cannot be used
865 191530 : rightSide += (cand.first->getCenterOnEdge(cand.first->getLane())
866 191530 : - vehicle->getCenterOnEdge(vehicle->getLane()));
867 : }
868 : #ifdef DEBUG_CHANGE_OPPOSITE
869 : if (vehicle->isSelected()) {
870 : std::cout << SIMTIME << " cand=" << cand.first->getID() << " rightSide=" << rightSide << "\n";
871 : }
872 : #endif
873 : if (cand.second < leader.second
874 1720268 : && rightSide < egoWidth
875 6603973 : && vehicle->getLane()->getWidth() - rightSide - cand.first->getVehicleType().getWidth() < egoWidth) {
876 : leader.first = const_cast<MSVehicle*>(cand.first);
877 : leader.second = cand.second;
878 : }
879 : }
880 : }
881 1439192 : return leader;
882 : }
883 :
884 :
885 : void
886 114203500 : MSLaneChangerSublane::addOutsideLeaders(const MSVehicle* vehicle, MSLeaderDistanceInfo& leaders) const {
887 114203500 : if (vehicle->getLaneChangeModel().getShadowLane() == nullptr) {
888 110235425 : const MSLane* lane = vehicle->getLane();
889 110235425 : const double rightOL = vehicle->getRightSideOnLane(lane);
890 110235425 : const double leftOL = vehicle->getLeftSideOnLane(lane);
891 : const bool outsideLeft = rightOL > lane->getWidth();
892 : #ifdef DEBUG_SURROUNDING
893 : if (DEBUG_COND) {
894 : std::cout << SIMTIME << " addOutsideLeaders veh=" << vehicle->getID() << " lane=" << lane->getID() << " rightOL=" << rightOL << " leftOL=" << leftOL << "\n";
895 : }
896 : #endif
897 110235425 : if (leftOL < 0 || outsideLeft) {
898 : int sublaneOffset = 0;
899 22658 : if (outsideLeft) {
900 21304 : sublaneOffset = MIN2(0, -(int)ceil((leftOL - lane->getWidth()) / MSGlobals::gLateralResolution) + 1);
901 : } else {
902 1354 : sublaneOffset = MAX2(0, (int)ceil(-rightOL / MSGlobals::gLateralResolution) - 1);
903 : }
904 22658 : if (sublaneOffset != 0) {
905 17772 : leaders.setSublaneOffset(sublaneOffset);
906 : #ifdef DEBUG_SURROUNDING
907 : if (DEBUG_COND) {
908 : std::cout << " sublaneOffset=" << sublaneOffset << " outsideLeft=" << outsideLeft << "\n";
909 : }
910 : #endif
911 118928 : for (const MSVehicle* cand : lane->myTmpVehicles) {
912 : #ifdef DEBUG_SURROUNDING
913 : if (DEBUG_COND) {
914 : std::cout << " cand=" << cand->getID() << " cLOE=" << cand->getLeftSideOnEdge() << " cROE=" << cand->getRightSideOnEdge() << "\n";
915 : }
916 : #endif
917 101156 : if (cand->getPositionOnLane() > vehicle->getPositionOnLane()
918 101156 : && ((!outsideLeft && cand->getLeftSideOnEdge() < 0)
919 101082 : || (outsideLeft && cand->getLeftSideOnEdge() > lane->getEdge().getWidth()))) {
920 73796 : const double gap = cand->getPositionOnLane() - vehicle->getPositionOnLane() - cand->getLength() - vehicle->getVehicleType().getMinGap();
921 73796 : leaders.addLeader(cand, gap);
922 : #ifdef DEBUG_SURROUNDING
923 : if (DEBUG_COND) {
924 : std::cout << " outsideLeader=" << cand->getID() << " ahead=" << leaders.toString() << "\n";
925 : }
926 : #endif
927 : }
928 : }
929 : }
930 : }
931 : }
932 114203500 : }
933 :
934 : /****************************************************************************/
|
