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 159726 : MSLaneChangerSublane::MSLaneChangerSublane(const std::vector<MSLane*>* lanes, bool allowChanging) :
55 159726 : MSLaneChanger(lanes, allowChanging) {
56 : // initialize siblings
57 159726 : if (myChanger.front().lane->isInternal()) {
58 178187 : for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
59 206716 : for (ChangerIt ce2 = myChanger.begin(); ce2 != myChanger.end(); ++ce2) {
60 113545 : 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 3452 : ce->siblings.push_back(ce2->lane->getIndex() - ce->lane->getIndex());
63 : }
64 : }
65 : }
66 : }
67 159726 : }
68 :
69 :
70 315662 : MSLaneChangerSublane::~MSLaneChangerSublane() {}
71 :
72 : void
73 11840922 : MSLaneChangerSublane::initChanger() {
74 11840922 : MSLaneChanger::initChanger();
75 : // Prepare myChanger with a safe state.
76 29885393 : for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
77 36088942 : 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 11840922 : }
84 :
85 :
86 :
87 : void
88 95002595 : MSLaneChangerSublane::updateChanger(bool vehHasChanged) {
89 95002595 : MSLaneChanger::updateChanger(vehHasChanged);
90 95002595 : if (!vehHasChanged) {
91 94659818 : 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 94659818 : if (lead->getLeftSideOnLane() < 0 || lead->getRightSideOnLane() > myCandi->lane->getWidth()) {
94 121008 : myCandi->outsideBounds.push_back(lead);
95 : } else {
96 94538810 : myCandi->ahead.addLeader(lead, false, 0);
97 : }
98 94659818 : MSLane* shadowLane = lead->getLaneChangeModel().getShadowLane();
99 94659818 : if (shadowLane != nullptr && &shadowLane->getEdge() == &lead->getLane()->getEdge()) {
100 : assert(shadowLane->getIndex() < (int)myChanger.size());
101 1967922 : const double latOffset = lead->getLane()->getRightSideOnEdge() - shadowLane->getRightSideOnEdge();
102 : //std::cout << SIMTIME << " updateChanger shadowLane=" << shadowLane->getID() << " lead=" << Named::getIDSecure(lead) << "\n";
103 1967922 : (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 95002595 : }
111 :
112 :
113 : bool
114 95002595 : MSLaneChangerSublane::change() {
115 : // variant of change() for the sublane case
116 95002595 : myCandi = findCandidate();
117 : MSVehicle* vehicle = veh(myCandi);
118 95002595 : 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 95002595 : if (/*!myAllowsChanging*/ vehicle->getLaneChangeModel().alreadyChanged() || vehicle->isStoppedOnLane()) {
127 518929 : registerUnchanged(vehicle);
128 518929 : if (vehicle->isStoppedOnLane()) {
129 517011 : myCandi->lastStopped = vehicle;
130 : }
131 518929 : return false;
132 : }
133 94483666 : 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 73571927 : vehicle->updateBestLanes(); // needed?
164 73571927 : const bool isOpposite = vehicle->getLaneChangeModel().isOpposite();
165 73571927 : if (!isOpposite) {
166 185736617 : for (int i = 0; i < (int) myChanger.size(); ++i) {
167 112253849 : vehicle->adaptBestLanesOccupation(i, myChanger[i].dens);
168 : }
169 : }
170 : // update leaders beyond the current edge for all lanes
171 185935572 : for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
172 112363645 : ce->aheadNext = getLeaders(ce, vehicle);
173 : }
174 : // update expected speeds
175 : int sublaneIndex = 0;
176 185935572 : for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
177 112363645 : vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(ce->aheadNext, sublaneIndex, ce->lane->getIndex());
178 113192313 : 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 828668 : vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(ceSib->aheadNext, sublaneIndex, ceSib->lane->getIndex());
182 : }
183 112363645 : 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 73571927 : const bool stopOpposite = hasOppositeStop(vehicle);
194 73571927 : const int traciState = vehicle->influenceChangeDecision(0);
195 73571927 : const bool traciRequestOpposite = !mayChange(1) && (traciState & LCA_LEFT) != 0;
196 :
197 75732922 : if (myChangeToOpposite && (
198 : // cannot overtake since there is only one usable lane (or emergency)
199 4321990 : ((!mayChange(-1) && !mayChange(1)) || vehicle->getVClass() == SVC_EMERGENCY)
200 : || traciRequestOpposite
201 597765 : || stopOpposite
202 : // can alway come back from the opposite side
203 597353 : || isOpposite)) {
204 1564199 : const MSLeaderDistanceInfo& leaders = myCandi->aheadNext;
205 1564199 : if (leaders.hasVehicles() || isOpposite || stopOpposite || traciRequestOpposite) {
206 1438906 : std::pair<MSVehicle*, double> leader = findClosestLeader(leaders, vehicle);
207 1438906 : myCheckedChangeOpposite = false;
208 120049 : if ((leader.first != nullptr || isOpposite || stopOpposite || traciRequestOpposite)
209 1471337 : && changeOpposite(vehicle, leader, myCandi->lastStopped)) {
210 168802 : return true;
211 1434796 : } else if (myCheckedChangeOpposite) {
212 164692 : registerUnchanged(vehicle);
213 164692 : return false;
214 : }
215 : // try sublane change within current lane otherwise
216 : }
217 : }
218 :
219 73403125 : LaneChangeAction alternatives = (LaneChangeAction)((mayChange(-1) ? LCA_RIGHT : LCA_NONE)
220 73403125 : | (mayChange(1) ? LCA_LEFT : LCA_NONE));
221 :
222 73403125 : StateAndDist right = checkChangeHelper(vehicle, -1, alternatives);
223 73403125 : StateAndDist left = checkChangeHelper(vehicle, 1, alternatives);
224 73403125 : StateAndDist current = checkChangeHelper(vehicle, 0, alternatives);
225 :
226 146806250 : StateAndDist decision = vehicle->getLaneChangeModel().decideDirection(current,
227 73403125 : 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 73403125 : vehicle->getLaneChangeModel().setOwnState(decision.state);
234 73403125 : 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 2828123 : const bool changed = startChangeSublane(vehicle, myCandi, decision.latDist, decision.maneuverDist);
242 2828123 : if (!changed) {
243 2495528 : registerUnchanged(vehicle);
244 : }
245 2828123 : return changed;
246 : }
247 : // @note this assumes vehicles can instantly abort any maneuvre in case of emergency
248 70575002 : abortLCManeuver(vehicle);
249 70575002 : 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 70575456 : 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 70575456 : const int priorReason = vehicle->getLaneChangeModel().getPrevState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
269 70575456 : const int reason = vehicle->getLaneChangeModel().getOwnState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
270 70575456 : if (priorReason != 0 && priorReason != reason && vehicle->getLaneChangeModel().getPreviousManeuverDist() != 0) {
271 : // original from cannot be reconstructed
272 72381 : 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 72381 : outputLCEnded(vehicle, myCandi, myCandi, priorDirection);
280 : }
281 70575456 : const double updatedSpeedLat = vehicle->getLaneChangeModel().getSpeedLat() != 0;
282 70575456 : vehicle->getLaneChangeModel().setSpeedLat(0);
283 70575456 : vehicle->getLaneChangeModel().setManeuverDist(0.);
284 70575456 : vehicle->getLaneChangeModel().updateTargetLane();
285 70575456 : if (updatedSpeedLat) {
286 : // update angle after having reset lateral speed
287 1200306 : vehicle->setAngle(vehicle->computeAngle());
288 : }
289 70575456 : }
290 :
291 :
292 : MSLaneChangerSublane::StateAndDist
293 220209375 : MSLaneChangerSublane::checkChangeHelper(MSVehicle* vehicle, int laneOffset, LaneChangeAction alternatives) {
294 : StateAndDist result = StateAndDist(0, 0, 0, 0);
295 220209375 : if (mayChange(laneOffset)) {
296 104697804 : if (laneOffset != 0 && vehicle->getLaneChangeModel().isOpposite()) {
297 42 : return result;
298 : }
299 104697762 : const std::vector<MSVehicle::LaneQ>& preb = (vehicle->getLaneChangeModel().isOpposite()
300 104697762 : ? getBestLanesOpposite(vehicle, nullptr, 1000)
301 104697762 : : vehicle->getBestLanes());
302 104697762 : result.state = checkChangeSublane(laneOffset, alternatives, preb, result.latDist, result.maneuverDist);
303 104697762 : result.dir = laneOffset;
304 104697762 : if ((result.state & LCA_WANTS_LANECHANGE) != 0 && (result.state & LCA_URGENT) != 0 && (result.state & LCA_BLOCKED) != 0) {
305 1328389 : (myCandi + laneOffset)->lastBlocked = vehicle;
306 1328389 : if ((myCandi + laneOffset)->firstBlocked == nullptr) {
307 495287 : (myCandi + laneOffset)->firstBlocked = vehicle;
308 : }
309 : }
310 104697762 : }
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 3607073 : MSLaneChangerSublane::startChangeSublane(MSVehicle* vehicle, ChangerIt& from, double latDist, double maneuverDist) {
341 3607073 : if (vehicle->isRemoteControlled()) {
342 : return false;
343 : }
344 3606574 : MSLane* source = from->lane;
345 : // Prevent continuation of LC beyond lane borders if change is not allowed
346 3606574 : double distToRightLaneBorder = vehicle->getLane()->getWidth() * 0.5 + vehicle->getLateralPositionOnLane() - vehicle->getWidth() * 0.5;
347 3606574 : double distToLeftLaneBorder = vehicle->getLane()->getWidth() * 0.5 - vehicle->getLateralPositionOnLane() - vehicle->getWidth() * 0.5;
348 3606574 : if (vehicle->getLaneChangeModel().isOpposite()) {
349 : std::swap(distToRightLaneBorder, distToLeftLaneBorder);
350 : }
351 : // determine direction of LC
352 : int direction = 0;
353 3606574 : if (latDist > 0 && latDist > distToLeftLaneBorder) {
354 : direction = 1;
355 3155899 : } else if (latDist < 0 && -latDist > distToRightLaneBorder) {
356 : direction = -1;
357 : }
358 3606574 : const int changerDirection = vehicle->getLaneChangeModel().isOpposite() ? -direction : direction;
359 3606574 : 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 3606574 : if (mayChange(changerDirection)) {
368 3588134 : 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 3606120 : vehicle->myState.myPosLat += latDist * (vehicle->getLaneChangeModel().isOpposite() ? -1 : 1);
392 3838357 : for (int i = 0; i < (int)vehicle->myFurtherLanesPosLat.size(); i++) {
393 464474 : vehicle->myFurtherLanesPosLat[i] += latDist * (vehicle->getLaneChangeModel().isOpposite() ? -1 : 1);
394 : }
395 3606120 : vehicle->myCachedPosition = Position::INVALID;
396 3606120 : 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 3606120 : const bool completedPriorManeuver = fabs(vehicle->getLaneChangeModel().getPreviousManeuverDist()) < NUMERICAL_EPS;
412 3606120 : const bool completedManeuver = fabs(maneuverDist - latDist) < NUMERICAL_EPS;
413 3606120 : vehicle->getLaneChangeModel().updateSafeLatDist(latDist);
414 :
415 : // current maneuver is aborted when direction or reason changes
416 3606120 : const int priorReason = vehicle->getLaneChangeModel().getPrevState() & LCA_CHANGE_REASONS & ~LCA_SUBLANE;
417 3606120 : 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 3606120 : if (!completedManeuver && !completedPriorManeuver && priorReason != 0 &&
432 1078086 : (vehicle->getLaneChangeModel().getPreviousManeuverDist() * latDist < 0
433 1068653 : || priorReason != reason)) {
434 124765 : 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 124765 : outputLCEnded(vehicle, from, from, priorDirection);
443 : }
444 :
445 3606120 : outputLCStarted(vehicle, from, to, direction, maneuverDist);
446 3606120 : vehicle->getLaneChangeModel().setManeuverDist(maneuverDist - latDist);
447 3606120 : const bool changedToNewLane = checkChangeToNewLane(vehicle, direction, from, to);
448 :
449 3606120 : MSLane* oldShadowLane = vehicle->getLaneChangeModel().getShadowLane();
450 3606120 : vehicle->getLaneChangeModel().updateShadowLane();
451 3606120 : MSLane* shadowLane = vehicle->getLaneChangeModel().getShadowLane();
452 3606120 : if (shadowLane != nullptr && shadowLane != oldShadowLane
453 3606120 : && &shadowLane->getEdge() == &source->getEdge()) {
454 : assert(oldShadowLane == 0 || vehicle->getLaneChangeModel().isOpposite() || to != from);
455 700306 : const double latOffset = vehicle->getLane()->getRightSideOnEdge() - shadowLane->getRightSideOnEdge();
456 700306 : (myChanger.begin() + shadowLane->getIndex())->ahead.addLeader(vehicle, false, latOffset);
457 : }
458 3606120 : if (completedManeuver) {
459 1560607 : outputLCEnded(vehicle, from, to, direction);
460 : }
461 :
462 : // Update maneuver reservations on target lanes
463 3606120 : MSLane* targetLane = vehicle->getLaneChangeModel().updateTargetLane();
464 3606120 : if (!changedToNewLane && targetLane != nullptr
465 189675 : && vehicle->getActionStepLength() > DELTA_T
466 3692305 : && &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 3606120 : double laneAngle = vehicle->computeAngle();
482 3606120 : 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 3606120 : 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 3606120 : 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 3606120 : from->lane->requireCollisionCheck();
517 3606120 : to->lane->requireCollisionCheck();
518 3606120 : return changedToNewLane;
519 : }
520 :
521 : bool
522 3606120 : MSLaneChangerSublane::checkChangeToNewLane(MSVehicle* vehicle, const int direction, ChangerIt from, ChangerIt to) {
523 3606120 : const int oppositeSign = vehicle->getLaneChangeModel().isOpposite() ? -1 : 1;
524 3606120 : const bool opposite = (&from->lane->getEdge() != &to->lane->getEdge());
525 : const bool changedToNewLane = (to->lane != from->lane
526 842287 : && fabs(vehicle->getLateralPositionOnLane()) > 0.5 * vehicle->getLane()->getWidth()
527 3948897 : && (mayChange(direction * oppositeSign) || opposite));
528 : if (changedToNewLane) {
529 342777 : vehicle->myState.myPosLat -= direction * 0.5 * (from->lane->getWidth() + to->lane->getWidth()) * oppositeSign;
530 342777 : if (!opposite) {
531 338768 : to->lane->myTmpVehicles.insert(to->lane->myTmpVehicles.begin(), vehicle);
532 338768 : to->dens += vehicle->getVehicleType().getLengthWithGap();
533 : }
534 342777 : 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 342777 : vehicle->getLaneChangeModel().startLaneChangeManeuver(from->lane, to->lane, direction);
546 342777 : if (!opposite) {
547 338768 : to->ahead.addLeader(vehicle, false, 0);
548 : }
549 : } else {
550 3263343 : from->ahead.addLeader(vehicle, false, 0);
551 : }
552 3606120 : return changedToNewLane;
553 : }
554 :
555 : void
556 3606120 : 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 3608166 : && ((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 3606120 : }
582 :
583 : void
584 1757753 : MSLaneChangerSublane::outputLCEnded(MSVehicle* vehicle, ChangerIt& from, ChangerIt& to, int direction) {
585 135214 : if (MSAbstractLaneChangeModel::haveLCOutput() && MSAbstractLaneChangeModel::outputLCEnded()
586 : // non-sublane change ended
587 1757871 : && ((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 1757753 : }
594 :
595 :
596 : MSLeaderDistanceInfo
597 112366541 : 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 112366541 : MSLeaderDistanceInfo result(target->lane->getWidth(), nullptr, 0);
605 : int sublaneShift = 0;
606 112366541 : if (target->lane == vehicle->getLane()) {
607 73573375 : if (vehicle->getLeftSideOnLane() < -MSGlobals::gLateralResolution) {
608 10730 : sublaneShift = int(-vehicle->getLeftSideOnLane() / MSGlobals::gLateralResolution);
609 73562645 : } else if (vehicle->getRightSideOnLane() > target->lane->getWidth() + MSGlobals::gLateralResolution) {
610 42335 : sublaneShift = -int((vehicle->getRightSideOnLane() - target->lane->getWidth()) / MSGlobals::gLateralResolution);
611 : }
612 73573375 : result.setSublaneOffset(sublaneShift);
613 : }
614 582967220 : for (int i = 0; i < target->ahead.numSublanes(); ++i) {
615 470600679 : const MSVehicle* veh = target->ahead[i];
616 470600679 : if (veh != nullptr) {
617 351687098 : 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 351687098 : if (i + sublaneShift >= 0 && i + sublaneShift < result.numSublanes()) {
624 351632314 : result.addLeader(veh, gap, 0, i + sublaneShift);
625 : }
626 : }
627 : }
628 112366541 : 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 112366541 : target->lane->addLeaders(vehicle, vehicle->getPositionOnLane(), result);
640 112366541 : return result;
641 0 : }
642 :
643 :
644 : int
645 104697762 : 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 104697762 : const MSLane& neighLane = *(target->lane);
655 104697762 : int blocked = 0;
656 :
657 104697762 : MSLeaderDistanceInfo neighLeaders = target->aheadNext;
658 104697762 : MSLeaderDistanceInfo neighFollowers = target->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
659 104697762 : MSLeaderDistanceInfo neighBlockers(neighLane.getWidth(), vehicle, vehicle->getLane()->getRightSideOnEdge() - neighLane.getRightSideOnEdge());
660 104697762 : MSLeaderDistanceInfo leaders = myCandi->aheadNext;
661 104697762 : addOutsideLeaders(vehicle, leaders);
662 104697762 : MSLeaderDistanceInfo followers = myCandi->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
663 104697762 : MSLeaderDistanceInfo blockers(vehicle->getLane()->getWidth(), vehicle, 0);
664 :
665 : // consider sibling lanes of the origin and target lane
666 105028023 : 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 330261 : MSLeaderDistanceInfo sibFollowers = ceSib->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
670 330261 : if (sibFollowers.hasVehicles()) {
671 297719 : followers.addLeaders(sibFollowers);
672 : }
673 330261 : if (ceSib->aheadNext.hasVehicles()) {
674 228855 : 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 330261 : }
682 105046211 : 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 348449 : MSLeaderDistanceInfo sibFollowers = ceSib->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
686 348449 : if (sibFollowers.hasVehicles()) {
687 314268 : neighFollowers.addLeaders(sibFollowers);
688 : }
689 348449 : if (ceSib->aheadNext.hasVehicles()) {
690 251696 : 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 348449 : }
698 :
699 : // break leader symmetry
700 104697762 : if (laneOffset == -1 && neighLeaders.hasVehicles()) {
701 11758479 : 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 104697762 : 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 104697762 : 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 104697762 : 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 104697762 : vehicle->getLaneChangeModel().getCanceledState(laneOffset) |= blocked;
741 104697762 : vehicle->getLaneChangeModel().saveLCState(laneOffset, oldstate, state);
742 104697762 : if (laneOffset != 0) {
743 31294637 : vehicle->getLaneChangeModel().saveNeighbors(laneOffset, neighFollowers, neighLeaders);
744 : }
745 104697762 : return state;
746 104697762 : }
747 :
748 :
749 : bool
750 168802 : MSLaneChangerSublane::checkChangeOpposite(
751 : MSVehicle* vehicle,
752 : int laneOffset,
753 : MSLane* targetLane,
754 : const std::pair<MSVehicle* const, double>& leader,
755 : const std::pair<MSVehicle* const, double>& neighLead,
756 : const std::pair<MSVehicle* const, double>& neighFollow,
757 : const std::vector<MSVehicle::LaneQ>& preb) {
758 168802 : myCheckedChangeOpposite = true;
759 :
760 : UNUSED_PARAMETER(leader);
761 : UNUSED_PARAMETER(neighLead);
762 : UNUSED_PARAMETER(neighFollow);
763 :
764 : const MSLane& neighLane = *targetLane;
765 168802 : MSLane* curLane = myCandi->lane;
766 :
767 168802 : MSLeaderDistanceInfo neighLeaders(targetLane->getWidth(), nullptr, 0);
768 168802 : MSLeaderDistanceInfo neighFollowers(targetLane->getWidth(), nullptr, 0);
769 168802 : MSLeaderDistanceInfo neighBlockers(targetLane->getWidth(), nullptr, 0);
770 168802 : MSLeaderDistanceInfo leaders(curLane->getWidth(), nullptr, 0);
771 168802 : MSLeaderDistanceInfo followers(curLane->getWidth(), nullptr, 0);
772 168802 : MSLeaderDistanceInfo blockers(curLane->getWidth(), nullptr, 0);
773 :
774 337604 : const double backPosOnTarget = vehicle->getLane()->getOppositePos(vehicle->getBackPositionOnLane());
775 168802 : if (vehicle->getLaneChangeModel().isOpposite()) {
776 88992 : leaders = curLane->getFollowersOnConsecutive(vehicle, vehicle->getPositionOnLane(), true, -1, MSLane::MinorLinkMode::FOLLOW_ONCOMING);
777 88992 : leaders.fixOppositeGaps(false);
778 88992 : curLane->addLeaders(vehicle, vehicle->getBackPositionOnLane(), followers);
779 88992 : followers.fixOppositeGaps(true);
780 88992 : neighFollowers = targetLane->getFollowersOnConsecutive(vehicle, backPosOnTarget, true);
781 88992 : neighFollowers.fixOppositeGaps(false);
782 : // artificially increase the position to ensure that ego is not added as a leader
783 88992 : const double posOnTarget = backPosOnTarget + vehicle->getVehicleType().getLength() + POSITION_EPS;
784 88992 : targetLane->addLeaders(vehicle, posOnTarget, neighLeaders);
785 88992 : neighLeaders.patchGaps(2 * POSITION_EPS);
786 : int sublaneIndex = 0;
787 109171 : for (int i = 0; i < targetLane->getIndex(); i++) {
788 20179 : sublaneIndex += MSLeaderInfo(targetLane->getEdge().getLanes()[i]->getWidth()).numSublanes();
789 : }
790 88992 : vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(neighLeaders, sublaneIndex, targetLane->getIndex());
791 : } else {
792 79810 : leaders = myCandi->aheadNext;
793 79810 : followers = myCandi->lane->getFollowersOnConsecutive(vehicle, vehicle->getBackPositionOnLane(), true);
794 79810 : const double posOnTarget = backPosOnTarget - vehicle->getVehicleType().getLength();
795 79810 : targetLane->addLeaders(vehicle, backPosOnTarget, neighFollowers, true);
796 79810 : neighFollowers.fixOppositeGaps(true);
797 79810 : neighLeaders = targetLane->getFollowersOnConsecutive(vehicle, posOnTarget, true);
798 79810 : neighLeaders.fixOppositeGaps(false);
799 : }
800 :
801 :
802 : #ifdef DEBUG_CHANGE_OPPOSITE
803 : if (DEBUG_COND) std::cout << SIMTIME
804 : << " checkChangeOppositeSublane: veh=" << vehicle->getID()
805 : << " isOpposite=" << vehicle->getLaneChangeModel().isOpposite()
806 : << " laneOffset=" << laneOffset
807 : << "\n leaders=" << leaders.toString()
808 : << "\n neighLeaders=" << neighLeaders.toString()
809 : << "\n followers=" << followers.toString()
810 : << "\n neighFollowers=" << neighFollowers.toString()
811 : << "\n";
812 : #endif
813 :
814 168802 : LaneChangeAction alternatives = (LaneChangeAction)((mayChange(-1) ? LCA_RIGHT : LCA_NONE)
815 168802 : | (mayChange(1) ? LCA_LEFT : LCA_NONE));
816 :
817 168802 : int blocked = 0;
818 168802 : double latDist = 0;
819 168802 : double maneuverDist = 0;
820 168802 : const int wish = vehicle->getLaneChangeModel().wantsChangeSublane(
821 : laneOffset, alternatives,
822 : leaders, followers, blockers,
823 : neighLeaders, neighFollowers, neighBlockers,
824 : neighLane, preb,
825 : &(myCandi->lastBlocked), &(myCandi->firstBlocked), latDist, maneuverDist, blocked);
826 168802 : int state = blocked | wish;
827 :
828 : const int oldstate = state;
829 168802 : state = vehicle->influenceChangeDecision(state);
830 : #ifdef DEBUG_CHANGE_OPPOSITE
831 : if (DEBUG_COND && state != oldstate) {
832 : std::cout << SIMTIME << " veh=" << vehicle->getID() << " stateAfterTraCI=" << toString((LaneChangeAction)state) << " original=" << toString((LaneChangeAction)oldstate) << "\n";
833 : }
834 : #endif
835 168802 : vehicle->getLaneChangeModel().saveLCState(laneOffset, oldstate, state);
836 168802 : if (laneOffset != 0) {
837 168802 : vehicle->getLaneChangeModel().saveNeighbors(laneOffset, neighFollowers, neighLeaders);
838 : }
839 :
840 168802 : if ((state & LCA_WANTS_LANECHANGE) != 0 && (state & LCA_BLOCKED) == 0) {
841 : // change if the vehicle wants to and is allowed to change
842 : #ifdef DEBUG_CHANGE_OPPOSITE
843 : if (DEBUG_COND) {
844 : std::cout << SIMTIME << " veh '" << vehicle->getID() << "' performing sublane change latDist=" << latDist << " maneuverDist=" << maneuverDist << "\n";
845 : }
846 : #endif
847 58378 : vehicle->getLaneChangeModel().setOwnState(state);
848 58378 : return startChangeSublane(vehicle, myCandi, latDist, maneuverDist);
849 : } else {
850 110424 : vehicle->getLaneChangeModel().setSpeedLat(0);
851 : return false;
852 : }
853 168802 : }
854 :
855 : std::pair<MSVehicle*, double>
856 1438906 : MSLaneChangerSublane::findClosestLeader(const MSLeaderDistanceInfo& leaders, const MSVehicle* vehicle) {
857 1438906 : const double egoWidth = vehicle->getVehicleType().getWidth() + vehicle->getVehicleType().getMinGapLat();
858 : std::pair<MSVehicle*, double> leader(nullptr, std::numeric_limits<double>::max());
859 7594600 : for (int i = 0; i < leaders.numSublanes(); ++i) {
860 6155694 : CLeaderDist cand = leaders[i];
861 6155694 : if (cand.first != nullptr) {
862 5159799 : const double rightSide = cand.first->getRightSideOnLane();
863 : if (cand.second < leader.second
864 1698680 : && rightSide < egoWidth
865 6576748 : && vehicle->getLane()->getWidth() - rightSide - cand.first->getVehicleType().getWidth() < egoWidth) {
866 : leader.first = const_cast<MSVehicle*>(cand.first);
867 : leader.second = cand.second;
868 : }
869 : }
870 : }
871 1438906 : return leader;
872 : }
873 :
874 :
875 : void
876 104697762 : MSLaneChangerSublane::addOutsideLeaders(const MSVehicle* vehicle, MSLeaderDistanceInfo& leaders) const {
877 104697762 : if (vehicle->getLaneChangeModel().getShadowLane() == nullptr) {
878 101221932 : const MSLane* lane = vehicle->getLane();
879 101221932 : const double rightOL = vehicle->getRightSideOnLane(lane);
880 101221932 : const double leftOL = vehicle->getLeftSideOnLane(lane);
881 : const bool outsideLeft = rightOL > lane->getWidth();
882 : #ifdef DEBUG_SURROUNDING
883 : if (DEBUG_COND) {
884 : std::cout << SIMTIME << " addOutsideLeaders veh=" << vehicle->getID() << " lane=" << lane->getID() << " rightOL=" << rightOL << " leftOL=" << leftOL << "\n";
885 : }
886 : #endif
887 101221932 : if (leftOL < 0 || outsideLeft) {
888 : int sublaneOffset = 0;
889 22378 : if (outsideLeft) {
890 20915 : sublaneOffset = MIN2(0, -(int)ceil((leftOL - lane->getWidth()) / MSGlobals::gLateralResolution) + 1);
891 : } else {
892 1463 : sublaneOffset = MAX2(0, (int)ceil(-rightOL / MSGlobals::gLateralResolution) - 1);
893 : }
894 22378 : if (sublaneOffset != 0) {
895 18078 : leaders.setSublaneOffset(sublaneOffset);
896 : #ifdef DEBUG_SURROUNDING
897 : if (DEBUG_COND) {
898 : std::cout << " sublaneOffset=" << sublaneOffset << " outsideLeft=" << outsideLeft << "\n";
899 : }
900 : #endif
901 120649 : for (const MSVehicle* cand : lane->myTmpVehicles) {
902 : #ifdef DEBUG_SURROUNDING
903 : if (DEBUG_COND) {
904 : std::cout << " cand=" << cand->getID() << " cLOE=" << cand->getLeftSideOnEdge() << " cROE=" << cand->getRightSideOnEdge() << "\n";
905 : }
906 : #endif
907 102571 : if (cand->getPositionOnLane() > vehicle->getPositionOnLane()
908 102571 : && ((!outsideLeft && cand->getLeftSideOnEdge() < 0)
909 102474 : || (outsideLeft && cand->getLeftSideOnEdge() > lane->getEdge().getWidth()))) {
910 74678 : const double gap = cand->getPositionOnLane() - vehicle->getPositionOnLane() - cand->getLength() - vehicle->getVehicleType().getMinGap();
911 74678 : leaders.addLeader(cand, gap);
912 : #ifdef DEBUG_SURROUNDING
913 : if (DEBUG_COND) {
914 : std::cout << " outsideLeader=" << cand->getID() << " ahead=" << leaders.toString() << "\n";
915 : }
916 : #endif
917 : }
918 : }
919 : }
920 : }
921 : }
922 104697762 : }
923 :
924 : /****************************************************************************/
|
