Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2002-2024 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file 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 159095 : MSLaneChangerSublane::MSLaneChangerSublane(const std::vector<MSLane*>* lanes, bool allowChanging) :
55 159095 : MSLaneChanger(lanes, allowChanging) {
56 : // initialize siblings
57 159095 : if (myChanger.front().lane->isInternal()) {
58 177607 : for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
59 205780 : for (ChangerIt ce2 = myChanger.begin(); ce2 != myChanger.end(); ++ce2) {
60 112945 : 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 3416 : ce->siblings.push_back(ce2->lane->getIndex() - ce->lane->getIndex());
63 : }
64 : }
65 : }
66 : }
67 159095 : }
68 :
69 :
70 314432 : MSLaneChangerSublane::~MSLaneChangerSublane() {}
71 :
72 : void
73 11831693 : MSLaneChangerSublane::initChanger() {
74 11831693 : MSLaneChanger::initChanger();
75 : // Prepare myChanger with a safe state.
76 29854346 : for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
77 36045306 : 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 11831693 : }
84 :
85 :
86 :
87 : void
88 94686903 : MSLaneChangerSublane::updateChanger(bool vehHasChanged) {
89 94686903 : MSLaneChanger::updateChanger(vehHasChanged);
90 94686903 : if (!vehHasChanged) {
91 94344785 : 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 94344785 : if (lead->getLeftSideOnLane() < 0 || lead->getRightSideOnLane() > myCandi->lane->getWidth()) {
94 121008 : myCandi->outsideBounds.push_back(lead);
95 : } else {
96 94223777 : myCandi->ahead.addLeader(lead, false, 0);
97 : }
98 94344785 : MSLane* shadowLane = lead->getLaneChangeModel().getShadowLane();
99 94344785 : if (shadowLane != nullptr && &shadowLane->getEdge() == &lead->getLane()->getEdge()) {
100 : assert(shadowLane->getIndex() < (int)myChanger.size());
101 1968575 : const double latOffset = lead->getLane()->getRightSideOnEdge() - shadowLane->getRightSideOnEdge();
102 : //std::cout << SIMTIME << " updateChanger shadowLane=" << shadowLane->getID() << " lead=" << Named::getIDSecure(lead) << "\n";
103 1968575 : (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 94686903 : }
111 :
112 :
113 : bool
114 94686903 : MSLaneChangerSublane::change() {
115 : // variant of change() for the sublane case
116 94686903 : myCandi = findCandidate();
117 : MSVehicle* vehicle = veh(myCandi);
118 94686903 : 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 94686903 : if (/*!myAllowsChanging*/ vehicle->getLaneChangeModel().alreadyChanged() || vehicle->isStoppedOnLane()) {
127 518806 : registerUnchanged(vehicle);
128 518806 : if (vehicle->isStoppedOnLane()) {
129 516888 : myCandi->lastStopped = vehicle;
130 : }
131 518806 : return false;
132 : }
133 94168097 : 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 20911739 : checkTraCICommands(vehicle);
143 :
144 : // Resume change
145 20911739 : 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 20911739 : if (!changed) {
153 20905667 : registerUnchanged(vehicle);
154 : }
155 20911739 : 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 73256358 : vehicle->updateBestLanes(); // needed?
164 73256358 : const bool isOpposite = vehicle->getLaneChangeModel().isOpposite();
165 73256358 : if (!isOpposite) {
166 184944471 : for (int i = 0; i < (int) myChanger.size(); ++i) {
167 111777272 : vehicle->adaptBestLanesOccupation(i, myChanger[i].dens);
168 : }
169 : }
170 : // update leaders beyond the current edge for all lanes
171 185143426 : for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
172 111887068 : ce->aheadNext = getLeaders(ce, vehicle);
173 : }
174 : // update expected speeds
175 : int sublaneIndex = 0;
176 185143426 : for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
177 111887068 : vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(ce->aheadNext, sublaneIndex, ce->lane->getIndex());
178 112715924 : 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 828856 : vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(ceSib->aheadNext, sublaneIndex, ceSib->lane->getIndex());
182 : }
183 111887068 : 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 73256358 : const bool stopOpposite = hasOppositeStop(vehicle);
194 73256358 : const int traciState = vehicle->influenceChangeDecision(0);
195 73256358 : const bool traciRequestOpposite = !mayChange(1) && (traciState & LCA_LEFT) != 0;
196 :
197 75453151 : if (myChangeToOpposite && (
198 : // cannot overtake since there is only one usable lane (or emergency)
199 4393586 : ((!mayChange(-1) && !mayChange(1)) || vehicle->getVClass() == SVC_EMERGENCY)
200 : || traciRequestOpposite
201 632812 : || stopOpposite
202 : // can alway come back from the opposite side
203 632400 : || isOpposite)) {
204 1564950 : const MSLeaderDistanceInfo& leaders = myCandi->aheadNext;
205 1564950 : if (leaders.hasVehicles() || isOpposite || stopOpposite || traciRequestOpposite) {
206 1439388 : std::pair<MSVehicle*, double> leader = findClosestLeader(leaders, vehicle);
207 1439388 : myCheckedChangeOpposite = false;
208 120051 : if ((leader.first != nullptr || isOpposite || stopOpposite || traciRequestOpposite)
209 1471819 : && changeOpposite(vehicle, leader, myCandi->lastStopped)) {
210 168802 : return true;
211 1435278 : } else if (myCheckedChangeOpposite) {
212 164692 : registerUnchanged(vehicle);
213 164692 : return false;
214 : }
215 : // try sublane change within current lane otherwise
216 : }
217 : }
218 :
219 73087556 : LaneChangeAction alternatives = (LaneChangeAction)((mayChange(-1) ? LCA_RIGHT : LCA_NONE)
220 73087556 : | (mayChange(1) ? LCA_LEFT : LCA_NONE));
221 :
222 73087556 : StateAndDist right = checkChangeHelper(vehicle, -1, alternatives);
223 73087556 : StateAndDist left = checkChangeHelper(vehicle, 1, alternatives);
224 73087556 : StateAndDist current = checkChangeHelper(vehicle, 0, alternatives);
225 :
226 146175112 : StateAndDist decision = vehicle->getLaneChangeModel().decideDirection(current,
227 73087556 : 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 73087556 : vehicle->getLaneChangeModel().setOwnState(decision.state);
234 73087556 : 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 2826229 : const bool changed = startChangeSublane(vehicle, myCandi, decision.latDist, decision.maneuverDist);
242 2826229 : if (!changed) {
243 2494293 : registerUnchanged(vehicle);
244 : }
245 2826229 : return changed;
246 : }
247 : // @note this assumes vehicles can instantly abort any maneuvre in case of emergency
248 70261327 : abortLCManeuver(vehicle);
249 70261327 : 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 70261781 : 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 70261781 : const int priorReason = vehicle->getLaneChangeModel().getPrevState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
269 70261781 : const int reason = vehicle->getLaneChangeModel().getOwnState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
270 70261781 : if (priorReason != 0 && priorReason != reason && vehicle->getLaneChangeModel().getPreviousManeuverDist() != 0) {
271 : // original from cannot be reconstructed
272 72457 : 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 72457 : outputLCEnded(vehicle, myCandi, myCandi, priorDirection);
280 : }
281 70261781 : const double updatedSpeedLat = vehicle->getLaneChangeModel().getSpeedLat() != 0;
282 70261781 : vehicle->getLaneChangeModel().setSpeedLat(0);
283 70261781 : vehicle->getLaneChangeModel().setManeuverDist(0.);
284 70261781 : vehicle->getLaneChangeModel().updateTargetLane();
285 70261781 : if (updatedSpeedLat) {
286 : // update angle after having reset lateral speed
287 1198964 : vehicle->setAngle(vehicle->computeAngle());
288 : }
289 70261781 : }
290 :
291 :
292 : MSLaneChangerSublane::StateAndDist
293 219262668 : MSLaneChangerSublane::checkChangeHelper(MSVehicle* vehicle, int laneOffset, LaneChangeAction alternatives) {
294 : StateAndDist result = StateAndDist(0, 0, 0, 0);
295 219262668 : if (mayChange(laneOffset)) {
296 104272466 : if (laneOffset != 0 && vehicle->getLaneChangeModel().isOpposite()) {
297 42 : return result;
298 : }
299 104272424 : const std::vector<MSVehicle::LaneQ>& preb = (vehicle->getLaneChangeModel().isOpposite()
300 104272424 : ? getBestLanesOpposite(vehicle, nullptr, 1000)
301 104272424 : : vehicle->getBestLanes());
302 104272424 : result.state = checkChangeSublane(laneOffset, alternatives, preb, result.latDist, result.maneuverDist);
303 104272424 : result.dir = laneOffset;
304 104272424 : if ((result.state & LCA_WANTS_LANECHANGE) != 0 && (result.state & LCA_URGENT) != 0 && (result.state & LCA_BLOCKED) != 0) {
305 1320458 : (myCandi + laneOffset)->lastBlocked = vehicle;
306 1320458 : if ((myCandi + laneOffset)->firstBlocked == nullptr) {
307 495393 : (myCandi + laneOffset)->firstBlocked = vehicle;
308 : }
309 : }
310 104272424 : }
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 20911739 : MSLaneChangerSublane::continueChangeSublane(MSVehicle* vehicle, ChangerIt& from) {
319 : // lateral distance to complete maneuver
320 20911739 : double remLatDist = vehicle->getLaneChangeModel().getManeuverDist();
321 20911739 : if (remLatDist == 0) {
322 : return false;
323 : }
324 720572 : const bool urgent = (vehicle->getLaneChangeModel().getOwnState() & LCA_URGENT) != 0;
325 720572 : 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 720572 : const bool changed = startChangeSublane(vehicle, from, nextLatDist, remLatDist);
335 : return changed;
336 : }
337 :
338 :
339 : bool
340 3605179 : MSLaneChangerSublane::startChangeSublane(MSVehicle* vehicle, ChangerIt& from, double latDist, double maneuverDist) {
341 3605179 : if (vehicle->isRemoteControlled()) {
342 : return false;
343 : }
344 3604680 : MSLane* source = from->lane;
345 : // Prevent continuation of LC beyond lane borders if change is not allowed
346 3604680 : double distToRightLaneBorder = vehicle->getLane()->getWidth() * 0.5 + vehicle->getLateralPositionOnLane() - vehicle->getWidth() * 0.5;
347 3604680 : double distToLeftLaneBorder = vehicle->getLane()->getWidth() * 0.5 - vehicle->getLateralPositionOnLane() - vehicle->getWidth() * 0.5;
348 3604680 : if (vehicle->getLaneChangeModel().isOpposite()) {
349 : std::swap(distToRightLaneBorder, distToLeftLaneBorder);
350 : }
351 : // determine direction of LC
352 : int direction = 0;
353 3604680 : if (latDist > 0 && latDist > distToLeftLaneBorder) {
354 : direction = 1;
355 3154984 : } else if (latDist < 0 && -latDist > distToRightLaneBorder) {
356 : direction = -1;
357 : }
358 3604680 : const int changerDirection = vehicle->getLaneChangeModel().isOpposite() ? -direction : direction;
359 3604680 : 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 3604680 : if (mayChange(changerDirection)) {
368 3586240 : to = from + changerDirection;
369 18440 : } else if (changerDirection == 1 && source->getOpposite() != nullptr) {
370 : // change to the opposite direction lane
371 17986 : 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 454 : abortLCManeuver(vehicle);
379 454 : 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 3604226 : vehicle->myState.myPosLat += latDist * (vehicle->getLaneChangeModel().isOpposite() ? -1 : 1);
392 3836419 : for (int i = 0; i < (int)vehicle->myFurtherLanesPosLat.size(); i++) {
393 464386 : vehicle->myFurtherLanesPosLat[i] += latDist * (vehicle->getLaneChangeModel().isOpposite() ? -1 : 1);
394 : }
395 3604226 : vehicle->myCachedPosition = Position::INVALID;
396 3604226 : 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 3604226 : const bool completedPriorManeuver = fabs(vehicle->getLaneChangeModel().getPreviousManeuverDist()) < NUMERICAL_EPS;
412 3604226 : const bool completedManeuver = fabs(maneuverDist - latDist) < NUMERICAL_EPS;
413 3604226 : vehicle->getLaneChangeModel().updateSafeLatDist(latDist);
414 :
415 : // current maneuver is aborted when direction or reason changes
416 3604226 : const int priorReason = vehicle->getLaneChangeModel().getPrevState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
417 3604226 : 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 3604226 : if (!completedManeuver && !completedPriorManeuver && priorReason != 0 &&
432 1078014 : (vehicle->getLaneChangeModel().getPreviousManeuverDist() * latDist < 0
433 1068583 : || priorReason != reason)) {
434 124746 : 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 124746 : outputLCEnded(vehicle, from, from, priorDirection);
443 : }
444 :
445 3604226 : outputLCStarted(vehicle, from, to, direction, maneuverDist);
446 3604226 : vehicle->getLaneChangeModel().setManeuverDist(maneuverDist - latDist);
447 3604226 : const bool changedToNewLane = checkChangeToNewLane(vehicle, direction, from, to);
448 :
449 3604226 : MSLane* oldShadowLane = vehicle->getLaneChangeModel().getShadowLane();
450 3604226 : vehicle->getLaneChangeModel().updateShadowLane();
451 3604226 : MSLane* shadowLane = vehicle->getLaneChangeModel().getShadowLane();
452 3604226 : if (shadowLane != nullptr && shadowLane != oldShadowLane
453 3604226 : && &shadowLane->getEdge() == &source->getEdge()) {
454 : assert(oldShadowLane == 0 || vehicle->getLaneChangeModel().isOpposite() || to != from);
455 698881 : const double latOffset = vehicle->getLane()->getRightSideOnEdge() - shadowLane->getRightSideOnEdge();
456 698881 : (myChanger.begin() + shadowLane->getIndex())->ahead.addLeader(vehicle, false, latOffset);
457 : }
458 3604226 : if (completedManeuver) {
459 1558972 : outputLCEnded(vehicle, from, to, direction);
460 : }
461 :
462 : // Update maneuver reservations on target lanes
463 3604226 : MSLane* targetLane = vehicle->getLaneChangeModel().updateTargetLane();
464 3604226 : if (!changedToNewLane && targetLane != nullptr
465 189987 : && vehicle->getActionStepLength() > DELTA_T
466 3690411 : && &targetLane->getEdge() == &source->getEdge()
467 : ) {
468 86160 : const int dir = (vehicle->getLaneChangeModel().getManeuverDist() > 0 ? 1 : -1);
469 : ChangerIt target = from + dir;
470 86160 : const double actionStepDist = dir * vehicle->getVehicleType().getMaxSpeedLat() * vehicle->getActionStepLengthSecs();
471 86160 : const double latOffset = vehicle->getLatOffset(targetLane) + actionStepDist;
472 86160 : 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 3604226 : double laneAngle = vehicle->computeAngle();
482 3604226 : if (vehicle->getLaneChangeModel().isOpposite()) {
483 : // reverse lane angle
484 36402 : 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 3604226 : 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 3604226 : 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 3604226 : from->lane->requireCollisionCheck();
517 3604226 : to->lane->requireCollisionCheck();
518 3604226 : return changedToNewLane;
519 : }
520 :
521 : bool
522 3604226 : MSLaneChangerSublane::checkChangeToNewLane(MSVehicle* vehicle, const int direction, ChangerIt from, ChangerIt to) {
523 3604226 : const int oppositeSign = vehicle->getLaneChangeModel().isOpposite() ? -1 : 1;
524 3604226 : const bool opposite = (&from->lane->getEdge() != &to->lane->getEdge());
525 : const bool changedToNewLane = (to->lane != from->lane
526 841173 : && fabs(vehicle->getLateralPositionOnLane()) > 0.5 * vehicle->getLane()->getWidth()
527 3946344 : && (mayChange(direction * oppositeSign) || opposite));
528 : if (changedToNewLane) {
529 342118 : vehicle->myState.myPosLat -= direction * 0.5 * (from->lane->getWidth() + to->lane->getWidth()) * oppositeSign;
530 342118 : if (!opposite) {
531 338109 : to->lane->myTmpVehicles.insert(to->lane->myTmpVehicles.begin(), vehicle);
532 338109 : to->dens += vehicle->getVehicleType().getLengthWithGap();
533 : }
534 342118 : if (MSAbstractLaneChangeModel::haveLCOutput()) {
535 6249 : 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 1448 : to->aheadNext = getLeaders(to, vehicle);
539 1448 : from->aheadNext = getLeaders(from, vehicle);
540 : }
541 6249 : vehicle->getLaneChangeModel().setLeaderGaps(to->aheadNext);
542 6249 : vehicle->getLaneChangeModel().setFollowerGaps(to->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true));
543 6249 : vehicle->getLaneChangeModel().setOrigLeaderGaps(from->aheadNext);
544 : }
545 342118 : vehicle->getLaneChangeModel().startLaneChangeManeuver(from->lane, to->lane, direction);
546 342118 : if (!opposite) {
547 338109 : to->ahead.addLeader(vehicle, false, 0);
548 : }
549 : } else {
550 3262108 : from->ahead.addLeader(vehicle, false, 0);
551 : }
552 3604226 : return changedToNewLane;
553 : }
554 :
555 : void
556 3604226 : MSLaneChangerSublane::outputLCStarted(MSVehicle* vehicle, ChangerIt& from, ChangerIt& to, int direction, double maneuverDist) {
557 394818 : if (MSAbstractLaneChangeModel::haveLCOutput() && MSAbstractLaneChangeModel::outputLCStarted()
558 : // non-sublane change started
559 1209 : && ((vehicle->getLaneChangeModel().getOwnState() & (LCA_CHANGE_REASONS & ~LCA_SUBLANE)) != 0)
560 1023 : && ((vehicle->getLaneChangeModel().getOwnState() & LCA_STAY) == 0)
561 : // no changing for the same reason in previous step (either not wanted or blocked)
562 3606272 : && ((vehicle->getLaneChangeModel().getPrevState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE) !=
563 1023 : (vehicle->getLaneChangeModel().getOwnState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE)
564 839 : || ((vehicle->getLaneChangeModel().getPrevState() & LCA_STAY) != 0)
565 839 : || ((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 3604226 : }
582 :
583 : void
584 1756175 : MSLaneChangerSublane::outputLCEnded(MSVehicle* vehicle, ChangerIt& from, ChangerIt& to, int direction) {
585 135214 : if (MSAbstractLaneChangeModel::haveLCOutput() && MSAbstractLaneChangeModel::outputLCEnded()
586 : // non-sublane change ended
587 1756293 : && ((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 1756175 : }
594 :
595 :
596 : MSLeaderDistanceInfo
597 111889964 : 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 111889964 : MSLeaderDistanceInfo result(target->lane->getWidth(), nullptr, 0);
605 : int sublaneShift = 0;
606 111889964 : if (target->lane == vehicle->getLane()) {
607 73257806 : if (vehicle->getLeftSideOnLane() < -MSGlobals::gLateralResolution) {
608 10730 : sublaneShift = int(-vehicle->getLeftSideOnLane() / MSGlobals::gLateralResolution);
609 73247076 : } else if (vehicle->getRightSideOnLane() > target->lane->getWidth() + MSGlobals::gLateralResolution) {
610 42335 : sublaneShift = -int((vehicle->getRightSideOnLane() - target->lane->getWidth()) / MSGlobals::gLateralResolution);
611 : }
612 73257806 : result.setSublaneOffset(sublaneShift);
613 : }
614 580744225 : for (int i = 0; i < target->ahead.numSublanes(); ++i) {
615 468854261 : const MSVehicle* veh = target->ahead[i];
616 468854261 : if (veh != nullptr) {
617 350048945 : 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 350048945 : if (i + sublaneShift >= 0 && i + sublaneShift < result.numSublanes()) {
624 349994161 : result.addLeader(veh, gap, 0, i + sublaneShift);
625 : }
626 : }
627 : }
628 111889964 : if (sublaneShift != 0) {
629 140815 : for (MSVehicle* cand : target->outsideBounds) {
630 87750 : const double gap = cand->getBackPositionOnLane() - vehicle->getPositionOnLane() - vehicle->getVehicleType().getMinGap();
631 87750 : 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 111889964 : target->lane->addLeaders(vehicle, vehicle->getPositionOnLane(), result);
640 111889964 : return result;
641 0 : }
642 :
643 :
644 : int
645 104272424 : 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 104272424 : const MSLane& neighLane = *(target->lane);
655 104272424 : int blocked = 0;
656 :
657 104272424 : MSLeaderDistanceInfo neighLeaders = target->aheadNext;
658 104272424 : MSLeaderDistanceInfo neighFollowers = target->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
659 104272424 : MSLeaderDistanceInfo neighBlockers(neighLane.getWidth(), vehicle, vehicle->getLane()->getRightSideOnEdge() - neighLane.getRightSideOnEdge());
660 104272424 : MSLeaderDistanceInfo leaders = myCandi->aheadNext;
661 104272424 : addOutsideLeaders(vehicle, leaders);
662 104272424 : MSLeaderDistanceInfo followers = myCandi->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
663 104272424 : MSLeaderDistanceInfo blockers(vehicle->getLane()->getWidth(), vehicle, 0);
664 :
665 : // consider sibling lanes of the origin and target lane
666 104602779 : 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 330355 : MSLeaderDistanceInfo sibFollowers = ceSib->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
670 330355 : if (sibFollowers.hasVehicles()) {
671 297815 : followers.addLeaders(sibFollowers);
672 : }
673 330355 : if (ceSib->aheadNext.hasVehicles()) {
674 228925 : 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 330355 : }
682 104620967 : 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 348543 : MSLeaderDistanceInfo sibFollowers = ceSib->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
686 348543 : if (sibFollowers.hasVehicles()) {
687 314364 : neighFollowers.addLeaders(sibFollowers);
688 : }
689 348543 : if (ceSib->aheadNext.hasVehicles()) {
690 251765 : 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 348543 : }
698 :
699 : // break leader symmetry
700 104272424 : if (laneOffset == -1 && neighLeaders.hasVehicles()) {
701 11713359 : 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 104272424 : 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 104272424 : 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 104272424 : 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 104272424 : vehicle->getLaneChangeModel().saveLCState(laneOffset, oldstate, state);
741 104272424 : if (laneOffset != 0) {
742 31184868 : vehicle->getLaneChangeModel().saveNeighbors(laneOffset, neighFollowers, neighLeaders);
743 : }
744 104272424 : return state;
745 104272424 : }
746 :
747 :
748 : bool
749 168802 : MSLaneChangerSublane::checkChangeOpposite(
750 : MSVehicle* vehicle,
751 : int laneOffset,
752 : MSLane* targetLane,
753 : const std::pair<MSVehicle* const, double>& leader,
754 : const std::pair<MSVehicle* const, double>& neighLead,
755 : const std::pair<MSVehicle* const, double>& neighFollow,
756 : const std::vector<MSVehicle::LaneQ>& preb) {
757 168802 : myCheckedChangeOpposite = true;
758 :
759 : UNUSED_PARAMETER(leader);
760 : UNUSED_PARAMETER(neighLead);
761 : UNUSED_PARAMETER(neighFollow);
762 :
763 : const MSLane& neighLane = *targetLane;
764 168802 : MSLane* curLane = myCandi->lane;
765 :
766 168802 : MSLeaderDistanceInfo neighLeaders(targetLane->getWidth(), nullptr, 0);
767 168802 : MSLeaderDistanceInfo neighFollowers(targetLane->getWidth(), nullptr, 0);
768 168802 : MSLeaderDistanceInfo neighBlockers(targetLane->getWidth(), nullptr, 0);
769 168802 : MSLeaderDistanceInfo leaders(curLane->getWidth(), nullptr, 0);
770 168802 : MSLeaderDistanceInfo followers(curLane->getWidth(), nullptr, 0);
771 168802 : MSLeaderDistanceInfo blockers(curLane->getWidth(), nullptr, 0);
772 :
773 337604 : const double backPosOnTarget = vehicle->getLane()->getOppositePos(vehicle->getBackPositionOnLane());
774 168802 : if (vehicle->getLaneChangeModel().isOpposite()) {
775 88992 : leaders = curLane->getFollowersOnConsecutive(vehicle, vehicle->getPositionOnLane(), true, -1, MSLane::MinorLinkMode::FOLLOW_ONCOMING);
776 88992 : leaders.fixOppositeGaps(false);
777 88992 : curLane->addLeaders(vehicle, vehicle->getBackPositionOnLane(), followers);
778 88992 : followers.fixOppositeGaps(true);
779 88992 : neighFollowers = targetLane->getFollowersOnConsecutive(vehicle, backPosOnTarget, true);
780 88992 : neighFollowers.fixOppositeGaps(false);
781 : // artificially increase the position to ensure that ego is not added as a leader
782 88992 : const double posOnTarget = backPosOnTarget + vehicle->getVehicleType().getLength() + POSITION_EPS;
783 88992 : targetLane->addLeaders(vehicle, posOnTarget, neighLeaders);
784 88992 : neighLeaders.patchGaps(2 * POSITION_EPS);
785 : int sublaneIndex = 0;
786 109171 : for (int i = 0; i < targetLane->getIndex(); i++) {
787 20179 : sublaneIndex += MSLeaderInfo(targetLane->getEdge().getLanes()[i]->getWidth()).numSublanes();
788 : }
789 88992 : vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(neighLeaders, sublaneIndex, targetLane->getIndex());
790 : } else {
791 79810 : leaders = myCandi->aheadNext;
792 79810 : followers = myCandi->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
793 79810 : const double posOnTarget = backPosOnTarget - vehicle->getVehicleType().getLength();
794 79810 : targetLane->addLeaders(vehicle, backPosOnTarget, neighFollowers, true);
795 79810 : neighFollowers.fixOppositeGaps(true);
796 79810 : neighLeaders = targetLane->getFollowersOnConsecutive(vehicle, posOnTarget, true);
797 79810 : neighLeaders.fixOppositeGaps(false);
798 : }
799 :
800 :
801 : #ifdef DEBUG_CHANGE_OPPOSITE
802 : if (DEBUG_COND) std::cout << SIMTIME
803 : << " checkChangeOppositeSublane: veh=" << vehicle->getID()
804 : << " isOpposite=" << vehicle->getLaneChangeModel().isOpposite()
805 : << " laneOffset=" << laneOffset
806 : << "\n leaders=" << leaders.toString()
807 : << "\n neighLeaders=" << neighLeaders.toString()
808 : << "\n followers=" << followers.toString()
809 : << "\n neighFollowers=" << neighFollowers.toString()
810 : << "\n";
811 : #endif
812 :
813 168802 : LaneChangeAction alternatives = (LaneChangeAction)((mayChange(-1) ? LCA_RIGHT : LCA_NONE)
814 168802 : | (mayChange(1) ? LCA_LEFT : LCA_NONE));
815 :
816 168802 : int blocked = 0;
817 168802 : double latDist = 0;
818 168802 : double maneuverDist = 0;
819 168802 : const int wish = vehicle->getLaneChangeModel().wantsChangeSublane(
820 : laneOffset, alternatives,
821 : leaders, followers, blockers,
822 : neighLeaders, neighFollowers, neighBlockers,
823 : neighLane, preb,
824 : &(myCandi->lastBlocked), &(myCandi->firstBlocked), latDist, maneuverDist, blocked);
825 168802 : int state = blocked | wish;
826 :
827 : const int oldstate = state;
828 168802 : state = vehicle->influenceChangeDecision(state);
829 : #ifdef DEBUG_CHANGE_OPPOSITE
830 : if (DEBUG_COND && state != oldstate) {
831 : std::cout << SIMTIME << " veh=" << vehicle->getID() << " stateAfterTraCI=" << toString((LaneChangeAction)state) << " original=" << toString((LaneChangeAction)oldstate) << "\n";
832 : }
833 : #endif
834 168802 : vehicle->getLaneChangeModel().saveLCState(laneOffset, oldstate, state);
835 168802 : if (laneOffset != 0) {
836 168802 : vehicle->getLaneChangeModel().saveNeighbors(laneOffset, neighFollowers, neighLeaders);
837 : }
838 :
839 168802 : if ((state & LCA_WANTS_LANECHANGE) != 0 && (state & LCA_BLOCKED) == 0) {
840 : // change if the vehicle wants to and is allowed to change
841 : #ifdef DEBUG_CHANGE_OPPOSITE
842 : if (DEBUG_COND) {
843 : std::cout << SIMTIME << " veh '" << vehicle->getID() << "' performing sublane change latDist=" << latDist << " maneuverDist=" << maneuverDist << "\n";
844 : }
845 : #endif
846 58378 : vehicle->getLaneChangeModel().setOwnState(state);
847 58378 : return startChangeSublane(vehicle, myCandi, latDist, maneuverDist);
848 : } else {
849 110424 : vehicle->getLaneChangeModel().setSpeedLat(0);
850 : return false;
851 : }
852 168802 : }
853 :
854 : std::pair<MSVehicle*, double>
855 1439388 : MSLaneChangerSublane::findClosestLeader(const MSLeaderDistanceInfo& leaders, const MSVehicle* vehicle) {
856 1439388 : const double egoWidth = vehicle->getVehicleType().getWidth() + vehicle->getVehicleType().getMinGapLat();
857 : std::pair<MSVehicle*, double> leader(nullptr, std::numeric_limits<double>::max());
858 7597974 : for (int i = 0; i < leaders.numSublanes(); ++i) {
859 6158586 : CLeaderDist cand = leaders[i];
860 6158586 : if (cand.first != nullptr) {
861 5162375 : const double rightSide = cand.first->getRightSideOnLane();
862 : if (cand.second < leader.second
863 1699454 : && rightSide < egoWidth
864 6580095 : && vehicle->getLane()->getWidth() - rightSide - cand.first->getVehicleType().getWidth() < egoWidth) {
865 : leader.first = const_cast<MSVehicle*>(cand.first);
866 : leader.second = cand.second;
867 : }
868 : }
869 : }
870 1439388 : return leader;
871 : }
872 :
873 :
874 : void
875 104272424 : MSLaneChangerSublane::addOutsideLeaders(const MSVehicle* vehicle, MSLeaderDistanceInfo& leaders) const {
876 104272424 : if (vehicle->getLaneChangeModel().getShadowLane() == nullptr) {
877 100797422 : const MSLane* lane = vehicle->getLane();
878 100797422 : const double rightOL = vehicle->getRightSideOnLane(lane);
879 100797422 : const double leftOL = vehicle->getLeftSideOnLane(lane);
880 : const bool outsideLeft = rightOL > lane->getWidth();
881 : #ifdef DEBUG_SURROUNDING
882 : if (DEBUG_COND) {
883 : std::cout << SIMTIME << " addOutsideLeaders veh=" << vehicle->getID() << " lane=" << lane->getID() << " rightOL=" << rightOL << " leftOL=" << leftOL << "\n";
884 : }
885 : #endif
886 100797422 : if (leftOL < 0 || outsideLeft) {
887 : int sublaneOffset = 0;
888 22378 : if (outsideLeft) {
889 20915 : sublaneOffset = MIN2(0, -(int)ceil((leftOL - lane->getWidth()) / MSGlobals::gLateralResolution) + 1);
890 : } else {
891 1463 : sublaneOffset = MAX2(0, (int)ceil(-rightOL / MSGlobals::gLateralResolution) - 1);
892 : }
893 22378 : if (sublaneOffset != 0) {
894 18078 : leaders.setSublaneOffset(sublaneOffset);
895 : #ifdef DEBUG_SURROUNDING
896 : if (DEBUG_COND) {
897 : std::cout << " sublaneOffset=" << sublaneOffset << " outsideLeft=" << outsideLeft << "\n";
898 : }
899 : #endif
900 120649 : for (const MSVehicle* cand : lane->myTmpVehicles) {
901 : #ifdef DEBUG_SURROUNDING
902 : if (DEBUG_COND) {
903 : std::cout << " cand=" << cand->getID() << " cLOE=" << cand->getLeftSideOnEdge() << " cROE=" << cand->getRightSideOnEdge() << "\n";
904 : }
905 : #endif
906 102571 : if (cand->getPositionOnLane() > vehicle->getPositionOnLane()
907 102571 : && ((!outsideLeft && cand->getLeftSideOnEdge() < 0)
908 102474 : || (outsideLeft && cand->getLeftSideOnEdge() > lane->getEdge().getWidth()))) {
909 74678 : const double gap = cand->getPositionOnLane() - vehicle->getPositionOnLane() - cand->getLength() - vehicle->getVehicleType().getMinGap();
910 74678 : leaders.addLeader(cand, gap);
911 : #ifdef DEBUG_SURROUNDING
912 : if (DEBUG_COND) {
913 : std::cout << " outsideLeader=" << cand->getID() << " ahead=" << leaders.toString() << "\n";
914 : }
915 : #endif
916 : }
917 : }
918 : }
919 : }
920 : }
921 104272424 : }
922 :
923 : /****************************************************************************/
|
