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 176162 : MSLaneChangerSublane::MSLaneChangerSublane(const std::vector<MSLane*>* lanes, bool allowChanging) :
55 176162 : MSLaneChanger(lanes, allowChanging) {
56 : // initialize siblings
57 176162 : if (myChanger.front().lane->isInternal()) {
58 200613 : for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
59 231820 : for (ChangerIt ce2 = myChanger.begin(); ce2 != myChanger.end(); ++ce2) {
60 127034 : 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 3726 : ce->siblings.push_back(ce2->lane->getIndex() - ce->lane->getIndex());
63 : }
64 : }
65 : }
66 : }
67 176162 : }
68 :
69 :
70 348534 : MSLaneChangerSublane::~MSLaneChangerSublane() {}
71 :
72 : void
73 13668030 : MSLaneChangerSublane::initChanger() {
74 13668030 : MSLaneChanger::initChanger();
75 : // Prepare myChanger with a safe state.
76 35857414 : for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
77 44378768 : 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 13668030 : }
84 :
85 :
86 :
87 : void
88 101264299 : MSLaneChangerSublane::updateChanger(bool vehHasChanged) {
89 101264299 : MSLaneChanger::updateChanger(vehHasChanged);
90 101264299 : if (!vehHasChanged) {
91 100883593 : 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 100883593 : if (lead->getLeftSideOnLane() < 0 || lead->getRightSideOnLane() > myCandi->lane->getWidth()) {
94 101647 : myCandi->outsideBounds.push_back(lead);
95 : } else {
96 100781946 : myCandi->ahead.addLeader(lead, false, 0);
97 : }
98 100883593 : MSLane* shadowLane = lead->getLaneChangeModel().getShadowLane();
99 100883593 : if (shadowLane != nullptr && &shadowLane->getEdge() == &lead->getLane()->getEdge()) {
100 : assert(shadowLane->getIndex() < (int)myChanger.size());
101 2136874 : const double latOffset = lead->getLane()->getRightSideOnEdge() - shadowLane->getRightSideOnEdge();
102 : //std::cout << SIMTIME << " updateChanger shadowLane=" << shadowLane->getID() << " lead=" << Named::getIDSecure(lead) << "\n";
103 2136874 : (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 101264299 : }
111 :
112 :
113 : bool
114 101264299 : MSLaneChangerSublane::change() {
115 : // variant of change() for the sublane case
116 101264299 : myCandi = findCandidate();
117 : MSVehicle* vehicle = veh(myCandi);
118 101264299 : 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 101264299 : if (/*!myAllowsChanging*/ vehicle->getLaneChangeModel().alreadyChanged() || vehicle->isStoppedOnLane()) {
127 671572 : registerUnchanged(vehicle);
128 671572 : if (vehicle->isStoppedOnLane()) {
129 669616 : myCandi->lastStopped = vehicle;
130 : }
131 671572 : return false;
132 : }
133 100592727 : 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 20410007 : checkTraCICommands(vehicle);
143 :
144 : // Resume change
145 20410007 : 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 20410007 : if (!changed) {
153 20402631 : registerUnchanged(vehicle);
154 : }
155 20410007 : 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 80182720 : vehicle->updateBestLanes(); // needed?
164 80182720 : const bool isOpposite = vehicle->getLaneChangeModel().isOpposite();
165 80182720 : if (!isOpposite) {
166 204272440 : for (int i = 0; i < (int) myChanger.size(); ++i) {
167 124178918 : vehicle->adaptBestLanesOccupation(i, myChanger[i].dens);
168 : }
169 : }
170 : // update leaders beyond the current edge for all lanes
171 204470707 : for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
172 124287987 : ce->aheadNext = getLeaders(ce, vehicle);
173 : }
174 : // update expected speeds
175 : int sublaneIndex = 0;
176 204470707 : for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
177 124287987 : vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(ce->aheadNext, sublaneIndex, ce->lane->getIndex());
178 125185695 : 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 897708 : if (ceSib->lane->allowsVehicleClass(vehicle->getVClass())) {
182 896302 : vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(ceSib->aheadNext, sublaneIndex, ceSib->lane->getIndex());
183 : }
184 : }
185 124287987 : sublaneIndex += ce->ahead.numSublanes();
186 : }
187 :
188 : // Check for changes to the opposite lane if vehicle is active
189 : #ifdef DEBUG_ACTIONSTEPS
190 : if (DEBUG_COND) {
191 : std::cout << " myChangeToOpposite=" << myChangeToOpposite << " isOpposite=" << isOpposite << " mayChangeRight=" << mayChange(-1) << " mayChangeLeft=" << mayChange(1) << "\n";
192 : }
193 : #endif
194 :
195 80182720 : const bool stopOpposite = hasOppositeStop(vehicle);
196 80182720 : const int traciState = vehicle->influenceChangeDecision(0);
197 80182720 : const bool traciRequestOpposite = !mayChange(1) && (traciState & LCA_LEFT) != 0;
198 :
199 82386986 : if (myChangeToOpposite && (
200 : // cannot overtake since there is only one usable lane (or emergency)
201 4408532 : ((!mayChange(-1) && !mayChange(1)) || vehicle->getVClass() == SVC_EMERGENCY)
202 : || traciRequestOpposite
203 634458 : || stopOpposite
204 : // can alway come back from the opposite side
205 634046 : || isOpposite)) {
206 1570777 : const MSLeaderDistanceInfo& leaders = myCandi->aheadNext;
207 1570777 : if (leaders.hasVehicles() || isOpposite || stopOpposite || traciRequestOpposite) {
208 1443154 : std::pair<MSVehicle*, double> leader = findClosestLeader(leaders, vehicle);
209 1443154 : myCheckedChangeOpposite = false;
210 130595 : if ((leader.first != nullptr || isOpposite || stopOpposite || traciRequestOpposite)
211 1474635 : && changeOpposite(vehicle, leader, myCandi->lastStopped)) {
212 165422 : return true;
213 1438986 : } else if (myCheckedChangeOpposite) {
214 161254 : registerUnchanged(vehicle);
215 161254 : return false;
216 : }
217 : // try sublane change within current lane otherwise
218 : }
219 : }
220 :
221 80017298 : LaneChangeAction alternatives = (LaneChangeAction)((mayChange(-1) ? LCA_RIGHT : LCA_NONE)
222 80017298 : | (mayChange(1) ? LCA_LEFT : LCA_NONE));
223 :
224 80017298 : StateAndDist right = checkChangeHelper(vehicle, -1, alternatives);
225 80017298 : StateAndDist left = checkChangeHelper(vehicle, 1, alternatives);
226 80017298 : StateAndDist current = checkChangeHelper(vehicle, 0, alternatives);
227 :
228 160034596 : StateAndDist decision = vehicle->getLaneChangeModel().decideDirection(current,
229 80017298 : vehicle->getLaneChangeModel().decideDirection(right, left));
230 : #ifdef DEBUG_DECISION
231 : if (vehicle->getLaneChangeModel().debugVehicle()) {
232 : std::cout << "\n" << SIMTIME << " decision=" << toString((LaneChangeAction)decision.state) << " dir=" << decision.dir << " latDist=" << decision.latDist << " maneuverDist=" << decision.maneuverDist << "\n";
233 : }
234 : #endif
235 80017298 : vehicle->getLaneChangeModel().setOwnState(decision.state);
236 80017298 : if ((decision.state & LCA_WANTS_LANECHANGE) != 0 && (decision.state & LCA_BLOCKED) == 0) {
237 : // change if the vehicle wants to and is allowed to change
238 : #ifdef DEBUG_MANEUVER
239 : if (DEBUG_COND) {
240 : std::cout << SIMTIME << " veh '" << vehicle->getID() << "' performing sublane change..." << std::endl;
241 : }
242 : #endif
243 3086382 : const bool changed = startChangeSublane(vehicle, myCandi, decision.latDist, decision.maneuverDist);
244 3086382 : if (!changed) {
245 2717220 : registerUnchanged(vehicle);
246 : }
247 3086382 : return changed;
248 : }
249 : // @note this assumes vehicles can instantly abort any maneuvre in case of emergency
250 76930916 : abortLCManeuver(vehicle);
251 76930916 : registerUnchanged(vehicle);
252 :
253 : if ((right.state & (LCA_URGENT)) != 0 && (left.state & (LCA_URGENT)) != 0) {
254 : // ... wants to go to the left AND to the right
255 : // just let them go to the right lane...
256 : left.state = 0;
257 : }
258 : return false;
259 : }
260 :
261 :
262 : void
263 76931367 : MSLaneChangerSublane::abortLCManeuver(MSVehicle* vehicle) {
264 : #ifdef DEBUG_MANEUVER
265 : if (DEBUG_COND) {
266 : std::cout << SIMTIME << " veh '" << vehicle->getID() << "' aborts LC-continuation."
267 : << std::endl;
268 : }
269 : #endif
270 76931367 : const int priorReason = vehicle->getLaneChangeModel().getPrevState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
271 76931367 : const int reason = vehicle->getLaneChangeModel().getOwnState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
272 76931367 : if (priorReason != 0 && priorReason != reason && vehicle->getLaneChangeModel().getPreviousManeuverDist() != 0) {
273 : // original from cannot be reconstructed
274 71718 : const int priorDirection = vehicle->getLaneChangeModel().getPreviousManeuverDist() > 0 ? 1 : -1;
275 : #ifdef DEBUG_MANEUVER
276 : if (DEBUG_COND) {
277 : std::cout << SIMTIME << " abortLCManeuver priorReason=" << toString((LaneChangeAction)priorReason)
278 : << " reason=" << toString((LaneChangeAction)reason) << " veh=" << vehicle->getID() << "\n";
279 : }
280 : #endif
281 71718 : outputLCEnded(vehicle, myCandi, myCandi, priorDirection);
282 : }
283 76931367 : const double updatedSpeedLat = vehicle->getLaneChangeModel().getSpeedLat() != 0;
284 76931367 : vehicle->getLaneChangeModel().setSpeedLat(0);
285 76931367 : vehicle->getLaneChangeModel().setManeuverDist(0.);
286 76931367 : vehicle->getLaneChangeModel().updateTargetLane();
287 76931367 : if (updatedSpeedLat) {
288 : // update angle after having reset lateral speed
289 1267892 : vehicle->setAngle(vehicle->computeAngle());
290 : }
291 76931367 : }
292 :
293 :
294 : MSLaneChangerSublane::StateAndDist
295 240051894 : MSLaneChangerSublane::checkChangeHelper(MSVehicle* vehicle, int laneOffset, LaneChangeAction alternatives) {
296 : StateAndDist result = StateAndDist(0, 0, 0, 0);
297 240051894 : if (mayChange(laneOffset)) {
298 115397330 : if (laneOffset != 0 && vehicle->getLaneChangeModel().isOpposite()) {
299 42 : return result;
300 : }
301 115397288 : const std::vector<MSVehicle::LaneQ>& preb = (vehicle->getLaneChangeModel().isOpposite()
302 115397288 : ? getBestLanesOpposite(vehicle, nullptr, 1000)
303 115397288 : : vehicle->getBestLanes());
304 115397288 : result.state = checkChangeSublane(laneOffset, alternatives, preb, result.latDist, result.maneuverDist);
305 115397288 : result.dir = laneOffset;
306 115397288 : if ((result.state & LCA_WANTS_LANECHANGE) != 0 && (result.state & LCA_URGENT) != 0 && (result.state & LCA_BLOCKED) != 0) {
307 1609228 : (myCandi + laneOffset)->lastBlocked = vehicle;
308 1609228 : if ((myCandi + laneOffset)->firstBlocked == nullptr) {
309 550266 : (myCandi + laneOffset)->firstBlocked = vehicle;
310 : }
311 : }
312 115397288 : }
313 : return result;
314 : }
315 :
316 :
317 : /// @brief Continue a sublane-lane change maneuver and return whether the midpoint was passed in this step
318 : // (used to continue sublane changing in non-action steps).
319 : bool
320 20410007 : MSLaneChangerSublane::continueChangeSublane(MSVehicle* vehicle, ChangerIt& from) {
321 : // lateral distance to complete maneuver
322 20410007 : double remLatDist = vehicle->getLaneChangeModel().getManeuverDist();
323 20410007 : if (remLatDist == 0) {
324 : return false;
325 : }
326 810768 : const bool urgent = (vehicle->getLaneChangeModel().getOwnState() & LCA_URGENT) != 0;
327 810768 : const double nextLatDist = SPEED2DIST(vehicle->getLaneChangeModel().computeSpeedLat(remLatDist, remLatDist, urgent));
328 : #ifdef DEBUG_MANEUVER
329 : if (DEBUG_COND) {
330 : std::cout << SIMTIME << " vehicle '" << vehicle->getID() << "' continueChangeSublane()"
331 : << " remLatDist=" << remLatDist << " nextLatDist=" << nextLatDist
332 : << std::endl;
333 : }
334 : #endif
335 :
336 810768 : const bool changed = startChangeSublane(vehicle, from, nextLatDist, remLatDist);
337 : return changed;
338 : }
339 :
340 :
341 : bool
342 3954161 : MSLaneChangerSublane::startChangeSublane(MSVehicle* vehicle, ChangerIt& from, double latDist, double maneuverDist) {
343 3954161 : if (vehicle->isRemoteControlled()) {
344 : return false;
345 : }
346 3953662 : MSLane* source = from->lane;
347 : // Prevent continuation of LC beyond lane borders if change is not allowed
348 3953662 : double distToRightLaneBorder = vehicle->getLane()->getWidth() * 0.5 + vehicle->getLateralPositionOnLane() - vehicle->getWidth() * 0.5;
349 3953662 : double distToLeftLaneBorder = vehicle->getLane()->getWidth() * 0.5 - vehicle->getLateralPositionOnLane() - vehicle->getWidth() * 0.5;
350 3953662 : if (vehicle->getLaneChangeModel().isOpposite()) {
351 : std::swap(distToRightLaneBorder, distToLeftLaneBorder);
352 : }
353 : // determine direction of LC
354 : int direction = 0;
355 3953662 : if (latDist > 0 && latDist > distToLeftLaneBorder) {
356 : direction = 1;
357 3458139 : } else if (latDist < 0 && -latDist > distToRightLaneBorder) {
358 : direction = -1;
359 : }
360 3953662 : const int changerDirection = vehicle->getLaneChangeModel().isOpposite() ? -direction : direction;
361 3953662 : ChangerIt to = from;
362 : #ifdef DEBUG_MANEUVER
363 : if (DEBUG_COND) {
364 : std::cout << SIMTIME << " vehicle '" << vehicle->getID() << "' latDist=" << latDist << " maneuverDist=" << maneuverDist
365 : << " distRight=" << distToRightLaneBorder << " distLeft=" << distToLeftLaneBorder
366 : << " dir=" << direction << " cDir=" << changerDirection << " mayChange=" << mayChange(changerDirection) << "\n";
367 : }
368 : #endif
369 3953662 : if (mayChange(changerDirection)) {
370 3934938 : to = from + changerDirection;
371 18724 : } else if (changerDirection == 1 && source->getOpposite() != nullptr) {
372 : // change to the opposite direction lane
373 18273 : to = (source->getOpposite()->getEdge().myLaneChanger->getChanger().end() - 1);
374 : } else {
375 : // This may occur during maneuver continuation in non-actionsteps.
376 : // TODO: Understand better why and test later if additional sublane actionstep debugging resolves this
377 : // (XXX: perhaps one should try to extrapolate check for this case before to avoid maneuver initialization
378 : // similar as for continuous LC in MSLaneChanger::checkChange())
379 : //assert(false);
380 451 : abortLCManeuver(vehicle);
381 451 : return false;
382 : }
383 :
384 : // The following does:
385 : // 1) update vehicles lateral position according to latDist and target lane
386 : // 2) distinguish several cases
387 : // a) vehicle moves completely within the same lane
388 : // b) vehicle intersects another lane
389 : // - vehicle must be moved to the lane where its midpoint is (either old or new)
390 : // - shadow vehicle must be created/moved to the other lane if the vehicle intersects it
391 : // 3) updated dens of all lanes that hold the vehicle or its shadow
392 :
393 3953211 : vehicle->myState.myPosLat += latDist * (vehicle->getLaneChangeModel().isOpposite() ? -1 : 1);
394 4195948 : for (int i = 0; i < (int)vehicle->myFurtherLanesPosLat.size(); i++) {
395 485474 : vehicle->myFurtherLanesPosLat[i] += latDist * (vehicle->getLaneChangeModel().isOpposite() ? -1 : 1);
396 : }
397 3953211 : vehicle->myCachedPosition = Position::INVALID;
398 3953211 : vehicle->getLaneChangeModel().setSpeedLat(DIST2SPEED(latDist));
399 : #ifdef DEBUG_MANEUVER
400 : if (DEBUG_COND) {
401 : std::cout << SIMTIME << " vehicle '" << vehicle->getID() << "' with maneuverDist=" << maneuverDist
402 : << " and committedSpeed=" << vehicle->getLaneChangeModel().getCommittedSpeed()
403 : << " increments lateral position by latDist=" << latDist << std::endl;
404 : }
405 : #endif
406 : #ifdef DEBUG_SURROUNDING
407 : if (DEBUG_COND) {
408 : std::cout << SIMTIME << " vehicle '" << vehicle->getID() << "'\n to->ahead=" << to->ahead.toString()
409 : << "'\n to->aheadNext=" << to->aheadNext.toString()
410 : << std::endl;
411 : }
412 : #endif
413 3953211 : const bool completedPriorManeuver = fabs(vehicle->getLaneChangeModel().getPreviousManeuverDist()) < NUMERICAL_EPS;
414 3953211 : const bool completedManeuver = fabs(maneuverDist - latDist) < NUMERICAL_EPS;
415 3953211 : vehicle->getLaneChangeModel().updateSafeLatDist(latDist);
416 :
417 : // current maneuver is aborted when direction or reason changes
418 3953211 : const int priorReason = vehicle->getLaneChangeModel().getPrevState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
419 3953211 : const int reason = vehicle->getLaneChangeModel().getOwnState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
420 : #ifdef DEBUG_MANEUVER
421 : if (DEBUG_COND) {
422 : std::cout << SIMTIME << " vehicle '" << vehicle->getID()
423 : << "' completedPriorManeuver=" << completedPriorManeuver
424 : << " completedManeuver=" << completedManeuver
425 : << " priorReason=" << toString((LaneChangeAction)priorReason)
426 : << " reason=" << toString((LaneChangeAction)reason)
427 : << " priorManeuverDist=" << vehicle->getLaneChangeModel().getPreviousManeuverDist()
428 : << " maneuverDist=" << maneuverDist
429 : << " latDist=" << latDist
430 : << std::endl;
431 : }
432 : #endif
433 3953211 : if (!completedManeuver && !completedPriorManeuver && priorReason != 0 &&
434 1230663 : (vehicle->getLaneChangeModel().getPreviousManeuverDist() * latDist < 0
435 1215110 : || priorReason != reason)) {
436 145419 : const int priorDirection = vehicle->getLaneChangeModel().getPreviousManeuverDist() > 0 ? 1 : -1;
437 : // original from cannot be reconstructed
438 : #ifdef DEBUG_MANEUVER
439 : if (DEBUG_COND) {
440 : std::cout << SIMTIME << " startChangeSublane abort priorReason=" << toString((LaneChangeAction)priorReason)
441 : << " reason=" << toString((LaneChangeAction)reason) << " veh=" << vehicle->getID() << "\n";
442 : }
443 : #endif
444 145419 : outputLCEnded(vehicle, from, from, priorDirection);
445 : }
446 :
447 3953211 : outputLCStarted(vehicle, from, to, direction, maneuverDist);
448 3953211 : vehicle->getLaneChangeModel().setManeuverDist(maneuverDist - latDist);
449 3953211 : const bool changedToNewLane = checkChangeToNewLane(vehicle, direction, from, to);
450 :
451 3953211 : MSLane* oldShadowLane = vehicle->getLaneChangeModel().getShadowLane();
452 3953211 : vehicle->getLaneChangeModel().updateShadowLane();
453 3953211 : MSLane* shadowLane = vehicle->getLaneChangeModel().getShadowLane();
454 3953211 : if (shadowLane != nullptr && shadowLane != oldShadowLane
455 3953211 : && &shadowLane->getEdge() == &source->getEdge()) {
456 : assert(oldShadowLane == 0 || vehicle->getLaneChangeModel().isOpposite() || to != from);
457 775847 : const double latOffset = vehicle->getLane()->getRightSideOnEdge() - shadowLane->getRightSideOnEdge();
458 775847 : (myChanger.begin() + shadowLane->getIndex())->ahead.addLeader(vehicle, false, latOffset);
459 : }
460 3953211 : if (completedManeuver) {
461 1685944 : outputLCEnded(vehicle, from, to, direction);
462 : }
463 :
464 : // Update maneuver reservations on target lanes
465 3953211 : MSLane* targetLane = vehicle->getLaneChangeModel().updateTargetLane();
466 3953211 : if (!changedToNewLane && targetLane != nullptr
467 211590 : && vehicle->getActionStepLength() > DELTA_T
468 4052019 : && &targetLane->getEdge() == &source->getEdge()
469 : ) {
470 98783 : const int dir = (vehicle->getLaneChangeModel().getManeuverDist() > 0 ? 1 : -1);
471 : ChangerIt target = from + dir;
472 98783 : const double actionStepDist = dir * vehicle->getVehicleType().getMaxSpeedLat() * vehicle->getActionStepLengthSecs();
473 98783 : const double latOffset = vehicle->getLatOffset(targetLane) + actionStepDist;
474 98783 : target->ahead.addLeader(vehicle, false, latOffset);
475 : //std::cout << SIMTIME << " veh=" << vehicle->getID() << " target=" << targetLane->getID()
476 : // << " actionStepDist=" << actionStepDist << " latOffset=" << latOffset
477 : // << " targetAhead=" << target->ahead.toString() << "\n";
478 : }
479 :
480 : // compute new angle of the vehicle from the x- and y-distances travelled within last time step
481 : // (should happen last because primaryLaneChanged() also triggers angle computation)
482 : // this part of the angle comes from the orientation of our current lane
483 3953211 : double laneAngle = vehicle->computeAngle();
484 3953211 : if (vehicle->getLaneChangeModel().isOpposite()) {
485 : // reverse lane angle
486 35402 : laneAngle += M_PI;
487 : }
488 : #ifdef DEBUG_MANEUVER
489 : if (DEBUG_COND) {
490 : std::cout << SIMTIME << " startChangeSublane"
491 : << " oldLane=" << from->lane->getID()
492 : << " newLane=" << to->lane->getID()
493 : << " shadowLane=" << (shadowLane != nullptr ? shadowLane->getID() : "NULL")
494 : << " targetLane=" << (targetLane != nullptr ? targetLane->getID() : "NULL")
495 : << " maneuverDist=" << vehicle->getLaneChangeModel().getManeuverDist()
496 : << " latDist=" << latDist
497 : << " oldShadowLane=" << Named::getIDSecure(oldShadowLane)
498 : << " newShadowLane=" << Named::getIDSecure(vehicle->getLaneChangeModel().getShadowLane())
499 : << " laneA=" << RAD2DEG(laneAngle)
500 : << " oldA=" << RAD2DEG(vehicle->getAngle())
501 : << " oppposite=" << vehicle->getLaneChangeModel().isOpposite()
502 : << " changedToNewLane=" << changedToNewLane
503 : << "\n";
504 : }
505 : #endif
506 3953211 : vehicle->setAngle(laneAngle, completedManeuver);
507 :
508 : // check if a traci maneuver must continue
509 : // getOwnState is reset to 0 when changing lanes so we use the stored reason
510 3953211 : if ((reason & LCA_TRACI) != 0) {
511 : #ifdef DEBUG_MANEUVER
512 : if (DEBUG_COND) {
513 : std::cout << SIMTIME << " continue TraCI-maneuver remainingLatDist=" << vehicle->getLaneChangeModel().getManeuverDist() << "\n";
514 : }
515 : #endif
516 1151 : vehicle->getInfluencer().setSublaneChange(vehicle->getLaneChangeModel().getManeuverDist());
517 : }
518 3953211 : from->lane->requireCollisionCheck();
519 3953211 : to->lane->requireCollisionCheck();
520 3953211 : return changedToNewLane;
521 : }
522 :
523 : bool
524 3953211 : MSLaneChangerSublane::checkChangeToNewLane(MSVehicle* vehicle, const int direction, ChangerIt from, ChangerIt to) {
525 3953211 : const int oppositeSign = vehicle->getLaneChangeModel().isOpposite() ? -1 : 1;
526 3953211 : const bool opposite = (&from->lane->getEdge() != &to->lane->getEdge());
527 : const bool changedToNewLane = (to->lane != from->lane
528 940350 : && fabs(vehicle->getLateralPositionOnLane()) > 0.5 * vehicle->getLane()->getWidth()
529 4333917 : && (mayChange(direction * oppositeSign) || opposite));
530 : if (changedToNewLane) {
531 380706 : vehicle->myState.myPosLat -= direction * 0.5 * (from->lane->getWidth() + to->lane->getWidth()) * oppositeSign;
532 380706 : if (!opposite) {
533 376639 : to->lane->myTmpVehicles.insert(to->lane->myTmpVehicles.begin(), vehicle);
534 376639 : to->dens += vehicle->getVehicleType().getLengthWithGap();
535 : }
536 380706 : if (MSAbstractLaneChangeModel::haveLCOutput()) {
537 7254 : if (!vehicle->isActive()) {
538 : // update leaders beyond the current edge for all lanes
539 : // @note to->aheadNext and from->aheadNext are only needed for output in non-action steps.
540 1928 : to->aheadNext = getLeaders(to, vehicle);
541 1928 : from->aheadNext = getLeaders(from, vehicle);
542 : }
543 7254 : vehicle->getLaneChangeModel().setLeaderGaps(to->aheadNext);
544 7254 : vehicle->getLaneChangeModel().setFollowerGaps(to->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true));
545 7254 : vehicle->getLaneChangeModel().setOrigLeaderGaps(from->aheadNext);
546 : }
547 380706 : vehicle->getLaneChangeModel().startLaneChangeManeuver(from->lane, to->lane, direction);
548 380706 : if (!opposite) {
549 376639 : to->ahead.addLeader(vehicle, false, 0);
550 : }
551 : } else {
552 3572505 : from->ahead.addLeader(vehicle, false, 0);
553 : }
554 3953211 : return changedToNewLane;
555 : }
556 :
557 : void
558 3953211 : MSLaneChangerSublane::outputLCStarted(MSVehicle* vehicle, ChangerIt& from, ChangerIt& to, int direction, double maneuverDist) {
559 434947 : if (MSAbstractLaneChangeModel::haveLCOutput() && MSAbstractLaneChangeModel::outputLCStarted()
560 : // non-sublane change started
561 1209 : && ((vehicle->getLaneChangeModel().getOwnState() & (LCA_CHANGE_REASONS & ~LCA_SUBLANE)) != 0)
562 1019 : && ((vehicle->getLaneChangeModel().getOwnState() & LCA_STAY) == 0)
563 : // no changing for the same reason in previous step (either not wanted or blocked)
564 3955249 : && ((vehicle->getLaneChangeModel().getPrevState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE) !=
565 1019 : (vehicle->getLaneChangeModel().getOwnState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE)
566 835 : || ((vehicle->getLaneChangeModel().getPrevState() & LCA_STAY) != 0)
567 835 : || ((vehicle->getLaneChangeModel().getPrevState() & LCA_BLOCKED) != 0))
568 : ) {
569 : #ifdef DEBUG_STATE
570 : if (DEBUG_COND) {
571 : std::cout << SIMTIME << " veh=" << vehicle->getID() << " laneChangeStarted state=" << toString((LaneChangeAction)vehicle->getLaneChangeModel().getOwnState())
572 : << " prevState=" << toString((LaneChangeAction)vehicle->getLaneChangeModel().getPrevState())
573 : << " filter=" << toString((LaneChangeAction)(LCA_CHANGE_REASONS & ~LCA_SUBLANE))
574 : << " filtered=" << toString((LaneChangeAction)(vehicle->getLaneChangeModel().getOwnState() & (LCA_CHANGE_REASONS & ~LCA_SUBLANE)))
575 : << "\n";
576 : }
577 : #endif
578 204 : vehicle->getLaneChangeModel().setLeaderGaps(to->aheadNext);
579 204 : vehicle->getLaneChangeModel().setFollowerGaps(to->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true));
580 204 : vehicle->getLaneChangeModel().setOrigLeaderGaps(from->aheadNext);
581 408 : vehicle->getLaneChangeModel().laneChangeOutput("changeStarted", from->lane, to->lane, direction, maneuverDist);
582 : }
583 3953211 : }
584 :
585 : void
586 1903081 : MSLaneChangerSublane::outputLCEnded(MSVehicle* vehicle, ChangerIt& from, ChangerIt& to, int direction) {
587 146869 : if (MSAbstractLaneChangeModel::haveLCOutput() && MSAbstractLaneChangeModel::outputLCEnded()
588 : // non-sublane change ended
589 1903203 : && ((vehicle->getLaneChangeModel().getOwnState() & (LCA_CHANGE_REASONS & ~LCA_SUBLANE)) != 0)) {
590 80 : vehicle->getLaneChangeModel().setLeaderGaps(to->aheadNext);
591 80 : vehicle->getLaneChangeModel().setFollowerGaps(to->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true));
592 80 : vehicle->getLaneChangeModel().setOrigLeaderGaps(from->aheadNext);
593 160 : vehicle->getLaneChangeModel().laneChangeOutput("changeEnded", from->lane, to->lane, direction);
594 : }
595 1903081 : }
596 :
597 :
598 : MSLeaderDistanceInfo
599 124291843 : MSLaneChangerSublane::getLeaders(const ChangerIt& target, const MSVehicle* vehicle) const {
600 : // get the leading vehicle on the lane to change to
601 : #ifdef DEBUG_SURROUNDING
602 : if (DEBUG_COND) {
603 : std::cout << SIMTIME << " getLeaders lane=" << target->lane->getID() << " ego=" << vehicle->getID() << " ahead=" << target->ahead.toString() << "\n";
604 : }
605 : #endif
606 124291843 : MSLeaderDistanceInfo result(target->lane->getWidth(), nullptr, 0);
607 : int sublaneShift = 0;
608 124291843 : if (target->lane == vehicle->getLane()) {
609 80184648 : if (vehicle->getLeftSideOnLane() < -MSGlobals::gLateralResolution) {
610 13833 : sublaneShift = int(-vehicle->getLeftSideOnLane() / MSGlobals::gLateralResolution);
611 80170815 : } else if (vehicle->getRightSideOnLane() > target->lane->getWidth() + MSGlobals::gLateralResolution) {
612 40224 : sublaneShift = -int((vehicle->getRightSideOnLane() - target->lane->getWidth()) / MSGlobals::gLateralResolution);
613 : }
614 80184648 : result.setSublaneOffset(sublaneShift);
615 : }
616 645469680 : for (int i = 0; i < target->ahead.numSublanes(); ++i) {
617 521177837 : const MSVehicle* veh = target->ahead[i];
618 521177837 : if (veh != nullptr) {
619 381387705 : const double gap = veh->getBackPositionOnLane(target->lane) - vehicle->getPositionOnLane() - vehicle->getVehicleType().getMinGap();
620 : #ifdef DEBUG_SURROUNDING
621 : if (DEBUG_COND) {
622 : std::cout << " ahead lead=" << veh->getID() << " leadBack=" << veh->getBackPositionOnLane() << " gap=" << gap << " sublaneShift=" << sublaneShift << "\n";
623 : }
624 : #endif
625 381387705 : if (i + sublaneShift >= 0 && i + sublaneShift < result.numSublanes()) {
626 381315279 : result.addLeader(veh, gap, 0, i + sublaneShift);
627 : }
628 : }
629 : }
630 124291843 : if (sublaneShift != 0) {
631 102134 : for (MSVehicle* cand : target->outsideBounds) {
632 48077 : const double gap = cand->getBackPositionOnLane() - vehicle->getPositionOnLane() - vehicle->getVehicleType().getMinGap();
633 48077 : result.addLeader(cand, gap);
634 : }
635 : }
636 : #ifdef DEBUG_SURROUNDING
637 : if (DEBUG_COND) {
638 : std::cout << " outsideBounds=" << toString(target->outsideBounds) << " result=" << result.toString() << "\n";
639 : }
640 : #endif
641 : // @note use the exact same vehiclePos as in getLastVehicleInformation() to avoid missing vehicles
642 124291843 : target->lane->addLeaders(vehicle, vehicle->getPositionOnLane(vehicle->getLane()), result);
643 124291843 : return result;
644 0 : }
645 :
646 :
647 : int
648 115397288 : MSLaneChangerSublane::checkChangeSublane(
649 : int laneOffset,
650 : LaneChangeAction alternatives,
651 : const std::vector<MSVehicle::LaneQ>& preb,
652 : double& latDist,
653 : double& maneuverDist) const {
654 :
655 : ChangerIt target = myCandi + laneOffset;
656 : MSVehicle* vehicle = veh(myCandi);
657 115397288 : const MSLane& neighLane = *(target->lane);
658 115397288 : int blocked = 0;
659 :
660 115397288 : MSLeaderDistanceInfo neighLeaders = target->aheadNext;
661 115397288 : MSLeaderDistanceInfo neighFollowers = target->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
662 115397288 : MSLeaderDistanceInfo neighBlockers(neighLane.getWidth(), vehicle, vehicle->getLane()->getRightSideOnEdge() - neighLane.getRightSideOnEdge());
663 115397288 : MSLeaderDistanceInfo leaders = myCandi->aheadNext;
664 115397288 : addOutsideLeaders(vehicle, leaders);
665 115397288 : MSLeaderDistanceInfo followers = myCandi->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
666 115397288 : MSLeaderDistanceInfo blockers(vehicle->getLane()->getWidth(), vehicle, 0);
667 :
668 : // consider sibling lanes of the origin and target lane
669 115760549 : for (int offset : myCandi->siblings) {
670 : // treat sibling lanes (internal lanes with the same origin lane) as if they have the same geometry
671 : ChangerIt ceSib = myCandi + offset;
672 363261 : MSLeaderDistanceInfo sibFollowers = ceSib->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
673 363261 : if (sibFollowers.hasVehicles()) {
674 331885 : followers.addLeaders(sibFollowers);
675 : }
676 363261 : if (ceSib->aheadNext.hasVehicles()) {
677 250582 : leaders.addLeaders(ceSib->aheadNext);
678 : }
679 : #ifdef DEBUG_SURROUNDING
680 : if (DEBUG_COND) {
681 : std::cout << SIMTIME << " ego=" << vehicle->getID() << " ahead=" << myCandi->aheadNext.toString() << " sib=" << ceSib->lane->getID() << " sibAhead=" << ceSib->aheadNext.toString() << " leaders=" << leaders.toString() << "\n";
682 : }
683 : #endif
684 363261 : }
685 115788610 : for (int offset : target->siblings) {
686 : // treat sibling lanes (internal lanes with the same origin lane) as if they have the same geometry
687 : ChangerIt ceSib = target + offset;
688 391322 : MSLeaderDistanceInfo sibFollowers = ceSib->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
689 391322 : if (sibFollowers.hasVehicles()) {
690 358886 : neighFollowers.addLeaders(sibFollowers);
691 : }
692 391322 : if (ceSib->aheadNext.hasVehicles()) {
693 280647 : neighLeaders.addLeaders(ceSib->aheadNext);
694 : }
695 : #ifdef DEBUG_SURROUNDING
696 : if (DEBUG_COND) {
697 : std::cout << SIMTIME << " ego=" << vehicle->getID() << " neighAhead=" << target->aheadNext.toString() << " sib=" << ceSib->lane->getID() << " sibAhead=" << ceSib->aheadNext.toString() << " neighLeaders=" << neighLeaders.toString() << "\n";
698 : }
699 : #endif
700 391322 : }
701 :
702 : // break leader symmetry
703 115397288 : if (laneOffset == -1 && neighLeaders.hasVehicles()) {
704 12880570 : neighLeaders.moveSamePosTo(vehicle, neighFollowers);
705 : }
706 :
707 : #ifdef DEBUG_SURROUNDING
708 : if (DEBUG_COND) std::cout << SIMTIME
709 : << " checkChangeSublane: veh=" << vehicle->getID()
710 : << " laneOffset=" << laneOffset
711 : << "\n leaders=" << leaders.toString()
712 : << "\n neighLeaders=" << neighLeaders.toString()
713 : << "\n followers=" << followers.toString()
714 : << "\n neighFollowers=" << neighFollowers.toString()
715 : << "\n";
716 : #endif
717 :
718 :
719 115397288 : const int wish = vehicle->getLaneChangeModel().wantsChangeSublane(
720 : laneOffset, alternatives,
721 : leaders, followers, blockers,
722 : neighLeaders, neighFollowers, neighBlockers,
723 : neighLane, preb,
724 : &(myCandi->lastBlocked), &(myCandi->firstBlocked), latDist, maneuverDist, blocked);
725 115397288 : int state = blocked | wish;
726 :
727 : // XXX
728 : // do are more careful (but expensive) check to ensure that a
729 : // safety-critical leader is not being overlooked
730 :
731 : // XXX
732 : // ensure that a continuous lane change manoeuvre can be completed
733 : // before the next turning movement
734 :
735 : // let TraCI influence the wish to change lanes and the security to take
736 : const int oldstate = state;
737 115397288 : state = vehicle->influenceChangeDecision(state);
738 : #ifdef DEBUG_STATE
739 : if (DEBUG_COND && state != oldstate) {
740 : std::cout << SIMTIME << " veh=" << vehicle->getID() << " stateAfterTraCI=" << toString((LaneChangeAction)state) << " original=" << toString((LaneChangeAction)oldstate) << "\n";
741 : }
742 : #endif
743 115397288 : vehicle->getLaneChangeModel().getCanceledState(laneOffset) |= blocked;
744 115397288 : vehicle->getLaneChangeModel().saveLCState(laneOffset, oldstate, state);
745 115397288 : if (laneOffset != 0) {
746 35379990 : vehicle->getLaneChangeModel().saveNeighbors(laneOffset, neighFollowers, neighLeaders);
747 : }
748 115397288 : return state;
749 115397288 : }
750 :
751 :
752 : bool
753 165422 : MSLaneChangerSublane::checkChangeOpposite(
754 : MSVehicle* vehicle,
755 : int laneOffset,
756 : MSLane* targetLane,
757 : const std::pair<MSVehicle* const, double>& leader,
758 : const std::pair<MSVehicle* const, double>& neighLead,
759 : const std::pair<MSVehicle* const, double>& neighFollow,
760 : const std::vector<MSVehicle::LaneQ>& preb) {
761 165422 : myCheckedChangeOpposite = true;
762 :
763 : UNUSED_PARAMETER(leader);
764 : UNUSED_PARAMETER(neighLead);
765 : UNUSED_PARAMETER(neighFollow);
766 :
767 : const MSLane& neighLane = *targetLane;
768 165422 : MSLane* curLane = myCandi->lane;
769 :
770 165422 : MSLeaderDistanceInfo neighLeaders(targetLane->getWidth(), nullptr, 0);
771 165422 : MSLeaderDistanceInfo neighFollowers(targetLane->getWidth(), nullptr, 0);
772 165422 : MSLeaderDistanceInfo neighBlockers(targetLane->getWidth(), nullptr, 0);
773 165422 : MSLeaderDistanceInfo leaders(curLane->getWidth(), nullptr, 0);
774 165422 : MSLeaderDistanceInfo followers(curLane->getWidth(), nullptr, 0);
775 165422 : MSLeaderDistanceInfo blockers(curLane->getWidth(), nullptr, 0);
776 :
777 330844 : const double backPosOnTarget = vehicle->getLane()->getOppositePos(vehicle->getBackPositionOnLane());
778 165422 : if (vehicle->getLaneChangeModel().isOpposite()) {
779 89031 : leaders = curLane->getFollowersOnConsecutive(vehicle, vehicle->getPositionOnLane(), true, -1, MSLane::MinorLinkMode::FOLLOW_ONCOMING);
780 89031 : leaders.fixOppositeGaps(false);
781 89031 : curLane->addLeaders(vehicle, vehicle->getBackPositionOnLane(), followers);
782 89031 : followers.fixOppositeGaps(true);
783 89031 : neighFollowers = targetLane->getFollowersOnConsecutive(vehicle, backPosOnTarget, true);
784 89031 : neighFollowers.fixOppositeGaps(false);
785 : // artificially increase the position to ensure that ego is not added as a leader
786 89031 : const double posOnTarget = backPosOnTarget + vehicle->getVehicleType().getLength() + POSITION_EPS;
787 89031 : targetLane->addLeaders(vehicle, posOnTarget, neighLeaders);
788 89031 : neighLeaders.patchGaps(2 * POSITION_EPS);
789 : int sublaneIndex = 0;
790 108444 : for (int i = 0; i < targetLane->getIndex(); i++) {
791 19413 : sublaneIndex += MSLeaderInfo(targetLane->getEdge().getLanes()[i]->getWidth()).numSublanes();
792 : }
793 89031 : vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(neighLeaders, sublaneIndex, targetLane->getIndex());
794 : } else {
795 76391 : leaders = myCandi->aheadNext;
796 76391 : followers = myCandi->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
797 76391 : const double posOnTarget = backPosOnTarget - vehicle->getVehicleType().getLength();
798 76391 : targetLane->addLeaders(vehicle, backPosOnTarget, neighFollowers, true);
799 76391 : neighFollowers.fixOppositeGaps(true);
800 76391 : neighLeaders = targetLane->getFollowersOnConsecutive(vehicle, posOnTarget, true);
801 76391 : neighLeaders.fixOppositeGaps(false);
802 : }
803 :
804 :
805 : #ifdef DEBUG_CHANGE_OPPOSITE
806 : if (DEBUG_COND) std::cout << SIMTIME
807 : << " checkChangeOppositeSublane: veh=" << vehicle->getID()
808 : << " isOpposite=" << vehicle->getLaneChangeModel().isOpposite()
809 : << " laneOffset=" << laneOffset
810 : << "\n leaders=" << leaders.toString()
811 : << "\n neighLeaders=" << neighLeaders.toString()
812 : << "\n followers=" << followers.toString()
813 : << "\n neighFollowers=" << neighFollowers.toString()
814 : << "\n";
815 : #endif
816 :
817 165422 : LaneChangeAction alternatives = (LaneChangeAction)((mayChange(-1) ? LCA_RIGHT : LCA_NONE)
818 165422 : | (mayChange(1) ? LCA_LEFT : LCA_NONE));
819 :
820 165422 : int blocked = 0;
821 165422 : double latDist = 0;
822 165422 : double maneuverDist = 0;
823 165422 : const int wish = vehicle->getLaneChangeModel().wantsChangeSublane(
824 : laneOffset, alternatives,
825 : leaders, followers, blockers,
826 : neighLeaders, neighFollowers, neighBlockers,
827 : neighLane, preb,
828 : &(myCandi->lastBlocked), &(myCandi->firstBlocked), latDist, maneuverDist, blocked);
829 165422 : int state = blocked | wish;
830 :
831 : const int oldstate = state;
832 165422 : state = vehicle->influenceChangeDecision(state);
833 : #ifdef DEBUG_CHANGE_OPPOSITE
834 : if (DEBUG_COND && state != oldstate) {
835 : std::cout << SIMTIME << " veh=" << vehicle->getID() << " stateAfterTraCI=" << toString((LaneChangeAction)state) << " original=" << toString((LaneChangeAction)oldstate) << "\n";
836 : }
837 : #endif
838 165422 : vehicle->getLaneChangeModel().saveLCState(laneOffset, oldstate, state);
839 165422 : if (laneOffset != 0) {
840 165422 : vehicle->getLaneChangeModel().saveNeighbors(laneOffset, neighFollowers, neighLeaders);
841 : }
842 :
843 165422 : if ((state & LCA_WANTS_LANECHANGE) != 0 && (state & LCA_BLOCKED) == 0) {
844 : // change if the vehicle wants to and is allowed to change
845 : #ifdef DEBUG_CHANGE_OPPOSITE
846 : if (DEBUG_COND) {
847 : std::cout << SIMTIME << " veh '" << vehicle->getID() << "' performing sublane change latDist=" << latDist << " maneuverDist=" << maneuverDist << "\n";
848 : }
849 : #endif
850 57011 : vehicle->getLaneChangeModel().setOwnState(state);
851 57011 : return startChangeSublane(vehicle, myCandi, latDist, maneuverDist);
852 : } else {
853 108411 : vehicle->getLaneChangeModel().setSpeedLat(0);
854 : return false;
855 : }
856 165422 : }
857 :
858 : std::pair<MSVehicle*, double>
859 1443154 : MSLaneChangerSublane::findClosestLeader(const MSLeaderDistanceInfo& leaders, const MSVehicle* vehicle) {
860 1443154 : const double egoWidth = vehicle->getVehicleType().getWidth() + vehicle->getVehicleType().getMinGapLat();
861 : std::pair<MSVehicle*, double> leader(nullptr, std::numeric_limits<double>::max());
862 7626779 : for (int i = 0; i < leaders.numSublanes(); ++i) {
863 6183625 : CLeaderDist cand = leaders[i];
864 6183625 : if (cand.first != nullptr) {
865 5192032 : double rightSide = cand.first->getRightSideOnLane();
866 5192032 : if (cand.first->getLane() != vehicle->getLane()) {
867 : // the candidate may be a parial (sideways) occupier so getRightSideOnLane() cannot be used
868 193811 : rightSide += (cand.first->getCenterOnEdge(cand.first->getLane())
869 193811 : - vehicle->getCenterOnEdge(vehicle->getLane()));
870 : }
871 : #ifdef DEBUG_CHANGE_OPPOSITE
872 : if (vehicle->isSelected()) {
873 : std::cout << SIMTIME << " cand=" << cand.first->getID() << " rightSide=" << rightSide << "\n";
874 : }
875 : #endif
876 : if (cand.second < leader.second
877 1726059 : && rightSide < egoWidth
878 6626612 : && vehicle->getLane()->getWidth() - rightSide - cand.first->getVehicleType().getWidth() < egoWidth) {
879 : leader.first = const_cast<MSVehicle*>(cand.first);
880 : leader.second = cand.second;
881 : }
882 : }
883 : }
884 1443154 : return leader;
885 : }
886 :
887 :
888 : void
889 115397288 : MSLaneChangerSublane::addOutsideLeaders(const MSVehicle* vehicle, MSLeaderDistanceInfo& leaders) const {
890 115397288 : if (vehicle->getLaneChangeModel().getShadowLane() == nullptr) {
891 111514000 : const MSLane* lane = vehicle->getLane();
892 111514000 : const double rightOL = vehicle->getRightSideOnLane(lane);
893 111514000 : const double leftOL = vehicle->getLeftSideOnLane(lane);
894 : const bool outsideLeft = rightOL > lane->getWidth();
895 : #ifdef DEBUG_SURROUNDING
896 : if (DEBUG_COND) {
897 : std::cout << SIMTIME << " addOutsideLeaders veh=" << vehicle->getID() << " lane=" << lane->getID() << " rightOL=" << rightOL << " leftOL=" << leftOL << "\n";
898 : }
899 : #endif
900 111514000 : if (leftOL < 0 || outsideLeft) {
901 : int sublaneOffset = 0;
902 437 : if (outsideLeft) {
903 147 : sublaneOffset = MIN2(0, -(int)ceil((leftOL - lane->getWidth()) / MSGlobals::gLateralResolution) + 1);
904 : } else {
905 290 : sublaneOffset = MAX2(0, (int)ceil(-rightOL / MSGlobals::gLateralResolution) - 1);
906 : }
907 437 : if (sublaneOffset != 0) {
908 367 : leaders.setSublaneOffset(sublaneOffset);
909 : #ifdef DEBUG_SURROUNDING
910 : if (DEBUG_COND) {
911 : std::cout << " sublaneOffset=" << sublaneOffset << " outsideLeft=" << outsideLeft << "\n";
912 : }
913 : #endif
914 713 : for (const MSVehicle* cand : lane->myTmpVehicles) {
915 : #ifdef DEBUG_SURROUNDING
916 : if (DEBUG_COND) {
917 : std::cout << " cand=" << cand->getID() << " cLOE=" << cand->getLeftSideOnEdge() << " cROE=" << cand->getRightSideOnEdge() << "\n";
918 : }
919 : #endif
920 346 : if (cand->getPositionOnLane() > vehicle->getPositionOnLane()
921 346 : && ((!outsideLeft && cand->getLeftSideOnEdge() < 0)
922 320 : || (outsideLeft && cand->getLeftSideOnEdge() > lane->getEdge().getWidth()))) {
923 35 : const double gap = cand->getPositionOnLane() - vehicle->getPositionOnLane() - cand->getLength() - vehicle->getVehicleType().getMinGap();
924 35 : leaders.addLeader(cand, gap);
925 : #ifdef DEBUG_SURROUNDING
926 : if (DEBUG_COND) {
927 : std::cout << " outsideLeader=" << cand->getID() << " ahead=" << leaders.toString() << "\n";
928 : }
929 : #endif
930 : }
931 : }
932 : }
933 : }
934 : }
935 115397288 : }
936 :
937 : /****************************************************************************/
|
