Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-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 MSLane.cpp
15 : /// @author Christian Roessel
16 : /// @author Jakob Erdmann
17 : /// @author Daniel Krajzewicz
18 : /// @author Tino Morenz
19 : /// @author Axel Wegener
20 : /// @author Michael Behrisch
21 : /// @author Christoph Sommer
22 : /// @author Mario Krumnow
23 : /// @author Leonhard Luecken
24 : /// @author Mirko Barthauer
25 : /// @date Mon, 05 Mar 2001
26 : ///
27 : // Representation of a lane in the micro simulation
28 : /****************************************************************************/
29 : #include <config.h>
30 :
31 : #include <cmath>
32 : #include <bitset>
33 : #include <iostream>
34 : #include <cassert>
35 : #include <functional>
36 : #include <algorithm>
37 : #include <iterator>
38 : #include <exception>
39 : #include <climits>
40 : #include <set>
41 : #include <utils/common/UtilExceptions.h>
42 : #include <utils/common/StdDefs.h>
43 : #include <utils/common/MsgHandler.h>
44 : #include <utils/common/ToString.h>
45 : #ifdef HAVE_FOX
46 : #include <utils/common/ScopedLocker.h>
47 : #endif
48 : #include <utils/options/OptionsCont.h>
49 : #include <utils/emissions/HelpersHarmonoise.h>
50 : #include <utils/geom/GeomHelper.h>
51 : #include <libsumo/TraCIConstants.h>
52 : #include <microsim/transportables/MSPModel.h>
53 : #include <microsim/transportables/MSTransportableControl.h>
54 : #include <microsim/traffic_lights/MSRailSignal.h>
55 : #include <microsim/traffic_lights/MSRailSignalControl.h>
56 : #include <microsim/traffic_lights/MSDriveWay.h>
57 : #include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
58 : #include <mesosim/MELoop.h>
59 : #include "MSNet.h"
60 : #include "MSVehicleType.h"
61 : #include "MSEdge.h"
62 : #include "MSEdgeControl.h"
63 : #include "MSJunction.h"
64 : #include "MSLogicJunction.h"
65 : #include "MSLink.h"
66 : #include "MSLane.h"
67 : #include "MSVehicleTransfer.h"
68 : #include "MSGlobals.h"
69 : #include "MSVehicleControl.h"
70 : #include "MSInsertionControl.h"
71 : #include "MSVehicleControl.h"
72 : #include "MSLeaderInfo.h"
73 : #include "MSVehicle.h"
74 : #include "MSStop.h"
75 :
76 : //#define DEBUG_INSERTION
77 : //#define DEBUG_PLAN_MOVE
78 : //#define DEBUG_EXEC_MOVE
79 : //#define DEBUG_CONTEXT
80 : //#define DEBUG_PARTIALS
81 : //#define DEBUG_OPPOSITE
82 : //#define DEBUG_VEHICLE_CONTAINER
83 : //#define DEBUG_COLLISIONS
84 : //#define DEBUG_JUNCTION_COLLISIONS
85 : //#define DEBUG_PEDESTRIAN_COLLISIONS
86 : //#define DEBUG_LANE_SORTER
87 : //#define DEBUG_NO_CONNECTION
88 : //#define DEBUG_SURROUNDING
89 : //#define DEBUG_EXTRAPOLATE_DEPARTPOS
90 : //#define DEBUG_ITERATOR
91 :
92 : //#define DEBUG_COND (false)
93 : //#define DEBUG_COND (true)
94 : //#define DEBUG_COND (getID() == "undefined")
95 : #define DEBUG_COND (isSelected())
96 : //#define DEBUG_COND2(obj) ((obj != 0 && (obj)->getID() == "disabled"))
97 : #define DEBUG_COND2(obj) ((obj != 0 && (obj)->isSelected()))
98 : //#define DEBUG_COND (getID() == "ego")
99 : //#define DEBUG_COND2(obj) ((obj != 0 && (obj)->getID() == "ego"))
100 : //#define DEBUG_COND2(obj) (true)
101 :
102 :
103 : // ===========================================================================
104 : // static member definitions
105 : // ===========================================================================
106 : MSLane::DictType MSLane::myDict;
107 : MSLane::CollisionAction MSLane::myCollisionAction(MSLane::COLLISION_ACTION_TELEPORT);
108 : MSLane::CollisionAction MSLane::myIntermodalCollisionAction(MSLane::COLLISION_ACTION_WARN);
109 : bool MSLane::myCheckJunctionCollisions(false);
110 : double MSLane::myCheckJunctionCollisionMinGap(0);
111 : SUMOTime MSLane::myCollisionStopTime(0);
112 : SUMOTime MSLane::myIntermodalCollisionStopTime(0);
113 : double MSLane::myCollisionMinGapFactor(1.0);
114 : bool MSLane::myExtrapolateSubstepDepart(false);
115 : std::vector<SumoRNG> MSLane::myRNGs;
116 :
117 :
118 : // ===========================================================================
119 : // internal class method definitions
120 : // ===========================================================================
121 : void
122 1686868 : MSLane::StoringVisitor::add(const MSLane* const l) const {
123 1686868 : switch (myDomain) {
124 680877 : case libsumo::CMD_GET_VEHICLE_VARIABLE: {
125 1364622 : for (const MSVehicle* veh : l->getVehiclesSecure()) {
126 683745 : if (myShape.distance2D(veh->getPosition()) <= myRange) {
127 339460 : myObjects.insert(veh);
128 : }
129 : }
130 681052 : for (const MSBaseVehicle* veh : l->getParkingVehicles()) {
131 175 : if (myShape.distance2D(veh->getPosition()) <= myRange) {
132 35 : myObjects.insert(veh);
133 : }
134 : }
135 680877 : l->releaseVehicles();
136 : }
137 680877 : break;
138 673184 : case libsumo::CMD_GET_PERSON_VARIABLE: {
139 673184 : l->getVehiclesSecure();
140 673184 : std::vector<MSTransportable*> persons = l->getEdge().getSortedPersons(MSNet::getInstance()->getCurrentTimeStep(), true);
141 770411 : for (auto p : persons) {
142 97227 : if (myShape.distance2D(p->getPosition()) <= myRange) {
143 50810 : myObjects.insert(p);
144 : }
145 : }
146 673184 : l->releaseVehicles();
147 673184 : }
148 673184 : break;
149 267910 : case libsumo::CMD_GET_EDGE_VARIABLE: {
150 267910 : if (myShape.size() != 1 || l->getShape().distance2D(myShape[0]) <= myRange) {
151 251123 : myObjects.insert(&l->getEdge());
152 : }
153 : }
154 : break;
155 64897 : case libsumo::CMD_GET_LANE_VARIABLE: {
156 64897 : if (myShape.size() != 1 || l->getShape().distance2D(myShape[0]) <= myRange) {
157 64311 : myObjects.insert(l);
158 : }
159 : }
160 : break;
161 : default:
162 : break;
163 :
164 : }
165 1686868 : }
166 :
167 :
168 : MSLane::AnyVehicleIterator&
169 10927379359 : MSLane::AnyVehicleIterator::operator++() {
170 10927379359 : if (nextIsMyVehicles()) {
171 10625124213 : if (myI1 != myI1End) {
172 7852802681 : myI1 += myDirection;
173 2772321532 : } else if (myI3 != myI3End) {
174 2772321532 : myI3 += myDirection;
175 : }
176 : // else: already at end
177 : } else {
178 302255146 : myI2 += myDirection;
179 : }
180 : //if (DEBUG_COND2(myLane)) std::cout << SIMTIME << " AnyVehicleIterator::operator++ lane=" << myLane->getID() << " myI1=" << myI1 << " myI2=" << myI2 << "\n";
181 10927379359 : return *this;
182 : }
183 :
184 :
185 : const MSVehicle*
186 11527651489 : MSLane::AnyVehicleIterator::operator*() {
187 11527651489 : if (nextIsMyVehicles()) {
188 11219246834 : if (myI1 != myI1End) {
189 7987153536 : return myLane->myVehicles[myI1];
190 3232093298 : } else if (myI3 != myI3End) {
191 2845469013 : return myLane->myTmpVehicles[myI3];
192 : } else {
193 : assert(myI2 == myI2End);
194 : return nullptr;
195 : }
196 : } else {
197 308404655 : return myLane->myPartialVehicles[myI2];
198 : }
199 : }
200 :
201 :
202 : bool
203 22455030848 : MSLane::AnyVehicleIterator::nextIsMyVehicles() const {
204 : #ifdef DEBUG_ITERATOR
205 : if (DEBUG_COND2(myLane)) std::cout << SIMTIME << " AnyVehicleIterator::nextIsMyVehicles lane=" << myLane->getID()
206 : << " myI1=" << myI1
207 : << " myI1End=" << myI1End
208 : << " myI2=" << myI2
209 : << " myI2End=" << myI2End
210 : << " myI3=" << myI3
211 : << " myI3End=" << myI3End
212 : << "\n";
213 : #endif
214 22455030848 : if (myI1 == myI1End && myI3 == myI3End) {
215 595459537 : if (myI2 != myI2End) {
216 : return false;
217 : } else {
218 386624285 : return true; // @note. must be caught
219 : }
220 : } else {
221 21859571311 : if (myI2 == myI2End) {
222 : return true;
223 : } else {
224 5735286359 : MSVehicle* cand = myI1 == myI1End ? myLane->myTmpVehicles[myI3] : myLane->myVehicles[myI1];
225 : #ifdef DEBUG_ITERATOR
226 : if (DEBUG_COND2(myLane)) std::cout << " "
227 : << " veh1=" << cand->getID()
228 : << " isTmp=" << (myI1 == myI1End)
229 : << " veh2=" << myLane->myPartialVehicles[myI2]->getID()
230 : << " pos1=" << cand->getPositionOnLane(myLane)
231 : << " pos2=" << myLane->myPartialVehicles[myI2]->getPositionOnLane(myLane)
232 : << "\n";
233 : #endif
234 5735286359 : if (cand->getPositionOnLane() < myLane->myPartialVehicles[myI2]->getPositionOnLane(myLane)) {
235 5374546933 : return myDownstream;
236 : } else {
237 360739426 : return !myDownstream;
238 : }
239 : }
240 : }
241 : }
242 :
243 :
244 : // ===========================================================================
245 : // member method definitions
246 : // ===========================================================================
247 2160064 : MSLane::MSLane(const std::string& id, double maxSpeed, double friction, double length, MSEdge* const edge,
248 : int numericalID, const PositionVector& shape, double width,
249 : SVCPermissions permissions,
250 : SVCPermissions changeLeft, SVCPermissions changeRight,
251 : int index, bool isRampAccel,
252 : const std::string& type,
253 2160064 : const PositionVector& outlineShape) :
254 : Named(id),
255 4320128 : myNumericalID(numericalID), myShape(shape), myIndex(index),
256 2160064 : myVehicles(), myLength(length), myWidth(width),
257 2160064 : myEdge(edge), myMaxSpeed(maxSpeed),
258 2160064 : myFrictionCoefficient(friction),
259 2160064 : mySpeedByVSS(false),
260 2160064 : mySpeedByTraCI(false),
261 2160064 : myPermissions(permissions),
262 2160064 : myChangeLeft(changeLeft),
263 2160064 : myChangeRight(changeRight),
264 2160064 : myOriginalPermissions(permissions),
265 2160064 : myLogicalPredecessorLane(nullptr),
266 2160064 : myCanonicalPredecessorLane(nullptr),
267 2160064 : myCanonicalSuccessorLane(nullptr),
268 2160064 : myBruttoVehicleLengthSum(0), myNettoVehicleLengthSum(0),
269 2160064 : myBruttoVehicleLengthSumToRemove(0), myNettoVehicleLengthSumToRemove(0),
270 2160064 : myRecalculateBruttoSum(false),
271 2160064 : myLeaderInfo(width, nullptr, 0.),
272 2160064 : myFollowerInfo(width, nullptr, 0.),
273 2160064 : myLeaderInfoTime(SUMOTime_MIN),
274 2160064 : myFollowerInfoTime(SUMOTime_MIN),
275 2160064 : myLengthGeometryFactor(MAX2(POSITION_EPS, myShape.length()) / myLength), // factor should not be 0
276 2160064 : myIsRampAccel(isRampAccel),
277 2160064 : myLaneType(type),
278 2160064 : myRightSideOnEdge(0), // initialized in MSEdge::initialize
279 2160064 : myRightmostSublane(0),
280 2160064 : myNeedsCollisionCheck(false),
281 2160064 : myOpposite(nullptr),
282 2160064 : myBidiLane(nullptr),
283 : #ifdef HAVE_FOX
284 : mySimulationTask(*this, 0),
285 : #endif
286 4320128 : myStopWatch(3) {
287 : // initialized in MSEdge::initialize
288 2160064 : initRestrictions();// may be reloaded again from initialized in MSEdge::closeBuilding
289 : assert(myRNGs.size() > 0);
290 2160064 : myRNGIndex = numericalID % myRNGs.size();
291 2160064 : if (outlineShape.size() > 0) {
292 20320 : myOutlineShape = new PositionVector(outlineShape);
293 : }
294 2160064 : }
295 :
296 :
297 5622494 : MSLane::~MSLane() {
298 4557747 : for (MSLink* const l : myLinks) {
299 2565774 : delete l;
300 : }
301 1991973 : delete myOutlineShape;
302 11598413 : }
303 :
304 :
305 : void
306 2162728 : MSLane::initRestrictions() {
307 : // simplify unit testing without MSNet instance
308 2162728 : myRestrictions = MSGlobals::gUnitTests ? nullptr : MSNet::getInstance()->getRestrictions(myEdge->getEdgeType());
309 2162728 : }
310 :
311 :
312 : void
313 2156909 : MSLane::checkBufferType() {
314 2156909 : if (MSGlobals::gNumSimThreads <= 1) {
315 : myVehBuffer.unsetCondition();
316 : // } else {
317 : // this is an idea for better memory locality, lanes with nearby numerical ids get the same rng and thus the same thread
318 : // first tests show no visible effect though
319 : // myRNGIndex = myNumericalID * myRNGs.size() / dictSize();
320 : }
321 2156909 : }
322 :
323 :
324 : void
325 2759582 : MSLane::addLink(MSLink* link) {
326 2759582 : myLinks.push_back(link);
327 2759582 : }
328 :
329 :
330 : void
331 7808 : MSLane::setOpposite(MSLane* oppositeLane) {
332 7808 : myOpposite = oppositeLane;
333 7808 : if (myOpposite != nullptr && getLength() > myOpposite->getLength()) {
334 0 : WRITE_WARNINGF(TL("Unequal lengths of neigh lane '%' and lane '%' (% != %)."), getID(), myOpposite->getID(), getLength(), myOpposite->getLength());
335 : }
336 7808 : }
337 :
338 : void
339 24974 : MSLane::setBidiLane(MSLane* bidiLane) {
340 24974 : myBidiLane = bidiLane;
341 24974 : if (myBidiLane != nullptr && getLength() > myBidiLane->getLength()) {
342 66 : if (isNormal() || MSGlobals::gUsingInternalLanes) {
343 198 : WRITE_WARNINGF(TL("Unequal lengths of bidi lane '%' and lane '%' (% != %)."), getID(), myBidiLane->getID(), getLength(), myBidiLane->getLength());
344 : }
345 : }
346 24974 : }
347 :
348 :
349 :
350 : // ------ interaction with MSMoveReminder ------
351 : void
352 2180016 : MSLane::addMoveReminder(MSMoveReminder* rem) {
353 2180016 : myMoveReminders.push_back(rem);
354 2196162 : for (MSVehicle* const veh : myVehicles) {
355 16146 : veh->addReminder(rem);
356 : }
357 : // XXX: Here, the partial occupators are ignored!? Refs. #3255
358 2180016 : }
359 :
360 :
361 : void
362 0 : MSLane::removeMoveReminder(MSMoveReminder* rem) {
363 0 : auto it = std::find(myMoveReminders.begin(), myMoveReminders.end(), rem);
364 0 : if (it != myMoveReminders.end()) {
365 0 : myMoveReminders.erase(it);
366 0 : for (MSVehicle* const veh : myVehicles) {
367 0 : veh->removeReminder(rem);
368 : }
369 : }
370 0 : }
371 :
372 :
373 : double
374 14803813 : MSLane::setPartialOccupation(MSVehicle* v) {
375 : // multithreading: there are concurrent writes to myNeedsCollisionCheck but all of them should set it to true
376 14803813 : myNeedsCollisionCheck = true; // always check
377 : #ifdef DEBUG_PARTIALS
378 : if (DEBUG_COND2(v)) {
379 : std::cout << SIMTIME << " setPartialOccupation. lane=" << getID() << " veh=" << v->getID() << "\n";
380 : }
381 : #endif
382 : // XXX update occupancy here?
383 : #ifdef HAVE_FOX
384 14803813 : ScopedLocker<> lock(myPartialOccupatorMutex, MSGlobals::gNumSimThreads > 1);
385 : #endif
386 : //assert(std::find(myPartialVehicles.begin(), myPartialVehicles.end(), v) == myPartialVehicles.end());
387 14803813 : myPartialVehicles.push_back(v);
388 17090088 : return myLength;
389 : }
390 :
391 :
392 : void
393 14803842 : MSLane::resetPartialOccupation(MSVehicle* v) {
394 : #ifdef HAVE_FOX
395 14803842 : ScopedLocker<> lock(myPartialOccupatorMutex, MSGlobals::gNumSimThreads > 1);
396 : #endif
397 : #ifdef DEBUG_PARTIALS
398 : if (DEBUG_COND2(v)) {
399 : std::cout << SIMTIME << " resetPartialOccupation. lane=" << getID() << " veh=" << v->getID() << "\n";
400 : }
401 : #endif
402 15722767 : for (VehCont::iterator i = myPartialVehicles.begin(); i != myPartialVehicles.end(); ++i) {
403 15722718 : if (v == *i) {
404 14803793 : myPartialVehicles.erase(i);
405 : // XXX update occupancy here?
406 : //std::cout << " removed from myPartialVehicles\n";
407 : return;
408 : }
409 : }
410 : // bluelight eqipped vehicle can teleport onto the intersection without using a connection
411 : assert(false || MSGlobals::gClearState || v->getLaneChangeModel().hasBlueLight());
412 : }
413 :
414 :
415 : void
416 204754 : MSLane::setManeuverReservation(MSVehicle* v) {
417 : #ifdef DEBUG_CONTEXT
418 : if (DEBUG_COND2(v)) {
419 : std::cout << SIMTIME << " setManeuverReservation. lane=" << getID() << " veh=" << v->getID() << "\n";
420 : }
421 : #endif
422 204754 : myManeuverReservations.push_back(v);
423 204754 : }
424 :
425 :
426 : void
427 204754 : MSLane::resetManeuverReservation(MSVehicle* v) {
428 : #ifdef DEBUG_CONTEXT
429 : if (DEBUG_COND2(v)) {
430 : std::cout << SIMTIME << " resetManeuverReservation(): lane=" << getID() << " veh=" << v->getID() << "\n";
431 : }
432 : #endif
433 236541 : for (VehCont::iterator i = myManeuverReservations.begin(); i != myManeuverReservations.end(); ++i) {
434 236541 : if (v == *i) {
435 204754 : myManeuverReservations.erase(i);
436 : return;
437 : }
438 : }
439 : assert(false);
440 : }
441 :
442 :
443 : // ------ Vehicle emission ------
444 : void
445 2939047 : MSLane::incorporateVehicle(MSVehicle* veh, double pos, double speed, double posLat, const MSLane::VehCont::iterator& at, MSMoveReminder::Notification notification) {
446 2939047 : myNeedsCollisionCheck = true;
447 : assert(pos <= myLength);
448 : bool wasInactive = myVehicles.size() == 0;
449 2939047 : veh->enterLaneAtInsertion(this, pos, speed, posLat, notification);
450 2939047 : if (at == myVehicles.end()) {
451 : // vehicle will be the first on the lane
452 598033 : myVehicles.push_back(veh);
453 : } else {
454 2341014 : myVehicles.insert(at, veh);
455 : }
456 2939047 : myBruttoVehicleLengthSum += veh->getVehicleType().getLengthWithGap();
457 2939047 : myNettoVehicleLengthSum += veh->getVehicleType().getLength();
458 2939047 : myEdge->markDelayed();
459 2939047 : if (wasInactive) {
460 582907 : MSNet::getInstance()->getEdgeControl().gotActive(this);
461 : }
462 2939047 : if (getBidiLane() != nullptr && (!isRailway(veh->getVClass()) || (getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
463 : // railways don't need to "see" each other when moving in opposite directions on the same track (efficiency)
464 539 : getBidiLane()->setPartialOccupation(veh);
465 : }
466 2939047 : }
467 :
468 :
469 : bool
470 724350 : MSLane::lastInsertion(MSVehicle& veh, double mspeed, double posLat, bool patchSpeed) {
471 724350 : double pos = getLength() - POSITION_EPS;
472 724350 : MSVehicle* leader = getLastAnyVehicle();
473 : // back position of leader relative to this lane
474 : double leaderBack;
475 724350 : if (leader == nullptr) {
476 : /// look for a leaders on consecutive lanes
477 3842 : veh.setTentativeLaneAndPosition(this, pos, posLat);
478 3842 : veh.updateBestLanes(false, this);
479 3842 : std::pair<MSVehicle* const, double> leaderInfo = getLeader(&veh, pos, veh.getBestLanesContinuation(), veh.getCarFollowModel().brakeGap(mspeed));
480 3842 : leader = leaderInfo.first;
481 3842 : leaderBack = pos + leaderInfo.second + veh.getVehicleType().getMinGap();
482 : } else {
483 720508 : leaderBack = leader->getBackPositionOnLane(this);
484 : //std::cout << " leaderPos=" << leader->getPositionOnLane(this) << " leaderBack=" << leader->getBackPositionOnLane(this) << " leaderLane=" << leader->getLane()->getID() << "\n";
485 : }
486 3842 : if (leader == nullptr) {
487 : // insert at the end of this lane
488 2623 : return isInsertionSuccess(&veh, mspeed, pos, posLat, patchSpeed, MSMoveReminder::NOTIFICATION_DEPARTED);
489 : } else {
490 : // try to insert behind the leader
491 721727 : const double frontGapNeeded = veh.getCarFollowModel().getSecureGap(&veh, leader, mspeed, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel()) + veh.getVehicleType().getMinGap() + POSITION_EPS;
492 721727 : if (leaderBack >= frontGapNeeded) {
493 409348 : pos = MIN2(pos, leaderBack - frontGapNeeded);
494 409348 : bool result = isInsertionSuccess(&veh, mspeed, pos, posLat, patchSpeed, MSMoveReminder::NOTIFICATION_DEPARTED);
495 : //if (!result) std::cout << " insertLast failed for " << veh.getID() << " pos=" << pos << " leaderBack=" << leaderBack << " frontGapNeeded=" << frontGapNeeded << "\n";
496 409348 : return result;
497 : }
498 : //std::cout << " insertLast failed for " << veh.getID() << " pos=" << pos << " leaderBack=" << leaderBack << " frontGapNeeded=" << frontGapNeeded << "\n";
499 : }
500 : return false;
501 : }
502 :
503 :
504 : bool
505 544268 : MSLane::freeInsertion(MSVehicle& veh, double mspeed, double posLat,
506 : MSMoveReminder::Notification notification) {
507 : // try to insert teleporting vehicles fully on this lane
508 544268 : const double minPos = (notification == MSMoveReminder::NOTIFICATION_TELEPORT ?
509 279494 : MIN2(myLength, veh.getVehicleType().getLength()) : 0);
510 544268 : veh.setTentativeLaneAndPosition(this, minPos, 0);
511 544268 : if (myVehicles.size() == 0) {
512 : // ensure sufficient gap to followers on predecessor lanes
513 9604 : const double backOffset = minPos - veh.getVehicleType().getLength();
514 9604 : const double missingRearGap = getMissingRearGap(&veh, backOffset, mspeed);
515 9604 : if (missingRearGap > 0) {
516 2027 : if (minPos + missingRearGap <= myLength) {
517 : // @note. The rear gap is tailored to mspeed. If it changes due
518 : // to a leader vehicle (on subsequent lanes) insertion will
519 : // still fail. Under the right combination of acceleration and
520 : // deceleration values there might be another insertion
521 : // positions that would be successful be we do not look for it.
522 : //std::cout << SIMTIME << " freeInsertion lane=" << getID() << " veh=" << veh.getID() << " unclear @(340)\n";
523 993 : return isInsertionSuccess(&veh, mspeed, minPos + missingRearGap, posLat, true, notification);
524 : }
525 : return false;
526 : } else {
527 7577 : return isInsertionSuccess(&veh, mspeed, minPos, posLat, true, notification);
528 : }
529 :
530 : } else {
531 : // check whether the vehicle can be put behind the last one if there is such
532 534664 : const MSVehicle* const leader = myVehicles.back(); // @todo reproduction of bogus old behavior. see #1961
533 534664 : const double leaderPos = leader->getBackPositionOnLane(this);
534 534664 : const double speed = leader->getSpeed();
535 534664 : const double frontGapNeeded = veh.getCarFollowModel().getSecureGap(&veh, leader, speed, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel()) + veh.getVehicleType().getMinGap();
536 534664 : if (leaderPos >= frontGapNeeded) {
537 522117 : const double tspeed = MIN2(veh.getCarFollowModel().insertionFollowSpeed(&veh, mspeed, frontGapNeeded, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel(), leader), mspeed);
538 : // check whether we can insert our vehicle behind the last vehicle on the lane
539 522117 : if (isInsertionSuccess(&veh, tspeed, minPos, posLat, true, notification)) {
540 : //std::cout << SIMTIME << " freeInsertion lane=" << getID() << " veh=" << veh.getID() << " pos=" << minPos<< " speed=" << speed << " tspeed=" << tspeed << " frontGapNeeded=" << frontGapNeeded << " lead=" << leader->getID() << " lPos=" << leaderPos << "\n vehsOnLane=" << toString(myVehicles) << " @(358)\n";
541 : return true;
542 : }
543 : }
544 : }
545 : // go through the lane, look for free positions (starting after the last vehicle)
546 : MSLane::VehCont::iterator predIt = myVehicles.begin();
547 12519491 : while (predIt != myVehicles.end()) {
548 : // get leader (may be zero) and follower
549 : // @todo compute secure position in regard to sublane-model
550 12110176 : const MSVehicle* leader = predIt != myVehicles.end() - 1 ? *(predIt + 1) : nullptr;
551 12110176 : if (leader == nullptr && myPartialVehicles.size() > 0) {
552 64976 : leader = myPartialVehicles.front();
553 : }
554 12110176 : const MSVehicle* follower = *predIt;
555 :
556 : // patch speed if allowed
557 : double speed = mspeed;
558 12110176 : if (leader != nullptr) {
559 11762624 : speed = MIN2(leader->getSpeed(), mspeed);
560 : }
561 :
562 : // compute the space needed to not collide with leader
563 : double frontMax = getLength();
564 12110176 : if (leader != nullptr) {
565 11762624 : double leaderRearPos = leader->getBackPositionOnLane(this);
566 11762624 : double frontGapNeeded = veh.getCarFollowModel().getSecureGap(&veh, leader, speed, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel()) + veh.getVehicleType().getMinGap();
567 11762624 : frontMax = leaderRearPos - frontGapNeeded;
568 : }
569 : // compute the space needed to not let the follower collide
570 12110176 : const double followPos = follower->getPositionOnLane() + follower->getVehicleType().getMinGap();
571 12110176 : const double backGapNeeded = follower->getCarFollowModel().getSecureGap(follower, &veh, follower->getSpeed(), veh.getSpeed(), veh.getCarFollowModel().getMaxDecel());
572 12110176 : const double backMin = followPos + backGapNeeded + veh.getVehicleType().getLength();
573 :
574 : // check whether there is enough room (given some extra space for rounding errors)
575 12110176 : if (frontMax > minPos && backMin + POSITION_EPS < frontMax) {
576 : // try to insert vehicle (should be always ok)
577 23791 : if (isInsertionSuccess(&veh, speed, backMin + POSITION_EPS, posLat, true, notification)) {
578 : //std::cout << SIMTIME << " freeInsertion lane=" << getID() << " veh=" << veh.getID() << " @(393)\n";
579 : return true;
580 : }
581 : }
582 : ++predIt;
583 : }
584 : // first check at lane's begin
585 : //std::cout << SIMTIME << " freeInsertion lane=" << getID() << " veh=" << veh.getID() << " fail final\n";
586 : return false;
587 : }
588 :
589 :
590 : double
591 11199001 : MSLane::getDepartSpeed(const MSVehicle& veh, bool& patchSpeed) {
592 : double speed = 0;
593 11199001 : const SUMOVehicleParameter& pars = veh.getParameter();
594 11199001 : switch (pars.departSpeedProcedure) {
595 1309192 : case DepartSpeedDefinition::GIVEN:
596 1309192 : speed = pars.departSpeed;
597 1309192 : patchSpeed = false;
598 1309192 : break;
599 43761 : case DepartSpeedDefinition::RANDOM:
600 87522 : speed = roundDecimal(RandHelper::rand(getVehicleMaxSpeed(&veh)), gPrecisionRandom);
601 43761 : patchSpeed = true;
602 43761 : break;
603 2547558 : case DepartSpeedDefinition::MAX:
604 2547558 : speed = getVehicleMaxSpeed(&veh);
605 2547558 : patchSpeed = true;
606 2547558 : break;
607 351274 : case DepartSpeedDefinition::DESIRED:
608 351274 : speed = getVehicleMaxSpeed(&veh);
609 351274 : patchSpeed = false;
610 351274 : break;
611 82536 : case DepartSpeedDefinition::LIMIT:
612 82536 : speed = getVehicleMaxSpeed(&veh) / veh.getChosenSpeedFactor();
613 82536 : patchSpeed = false;
614 82536 : break;
615 8159 : case DepartSpeedDefinition::LAST: {
616 8159 : MSVehicle* last = getLastAnyVehicle();
617 8159 : speed = getVehicleMaxSpeed(&veh);
618 8159 : if (last != nullptr) {
619 7952 : speed = MIN2(speed, last->getSpeed());
620 7952 : patchSpeed = false;
621 : }
622 : break;
623 : }
624 1206724 : case DepartSpeedDefinition::AVG: {
625 1206724 : speed = MIN2(getVehicleMaxSpeed(&veh), getMeanSpeed());
626 1206724 : if (getLastAnyVehicle() != nullptr) {
627 1200606 : patchSpeed = false;
628 : }
629 : break;
630 : }
631 5649797 : case DepartSpeedDefinition::DEFAULT:
632 : default:
633 : // speed = 0 was set before
634 5649797 : patchSpeed = false; // @todo check
635 5649797 : break;
636 : }
637 11199001 : return speed;
638 : }
639 :
640 :
641 : double
642 11605445 : MSLane::getDepartPosLat(const MSVehicle& veh) {
643 11605445 : const SUMOVehicleParameter& pars = veh.getParameter();
644 11605445 : switch (pars.departPosLatProcedure) {
645 105220 : case DepartPosLatDefinition::GIVEN:
646 105220 : return pars.departPosLat;
647 : case DepartPosLatDefinition::RIGHT:
648 27847 : return -getWidth() * 0.5 + veh.getVehicleType().getWidth() * 0.5;
649 : case DepartPosLatDefinition::LEFT:
650 27663 : return getWidth() * 0.5 - veh.getVehicleType().getWidth() * 0.5;
651 : case DepartPosLatDefinition::RANDOM: {
652 216030 : const double raw = RandHelper::rand(getWidth() - veh.getVehicleType().getWidth()) - getWidth() * 0.5 + veh.getVehicleType().getWidth() * 0.5;
653 216030 : return roundDecimal(raw, gPrecisionRandom);
654 : }
655 : case DepartPosLatDefinition::CENTER:
656 : case DepartPosLatDefinition::DEFAULT:
657 : // @note:
658 : // case DepartPosLatDefinition::FREE
659 : // case DepartPosLatDefinition::RANDOM_FREE
660 : // are not handled here because they involve multiple insertion attempts
661 : default:
662 : return 0;
663 : }
664 : }
665 :
666 :
667 : bool
668 11198931 : MSLane::insertVehicle(MSVehicle& veh) {
669 : double pos = 0;
670 11198931 : bool patchSpeed = true; // whether the speed shall be adapted to infrastructure/traffic in front
671 11198931 : const SUMOVehicleParameter& pars = veh.getParameter();
672 11198931 : double speed = getDepartSpeed(veh, patchSpeed);
673 11198931 : double posLat = getDepartPosLat(veh);
674 :
675 : // determine the position
676 11198931 : switch (pars.departPosProcedure) {
677 909027 : case DepartPosDefinition::GIVEN:
678 909027 : pos = pars.departPos;
679 909027 : if (pos < 0.) {
680 158274 : pos += myLength;
681 : }
682 : break;
683 141735 : case DepartPosDefinition::RANDOM:
684 141735 : pos = roundDecimal(RandHelper::rand(getLength()), gPrecisionRandom);
685 : break;
686 : case DepartPosDefinition::RANDOM_FREE: {
687 444853 : for (int i = 0; i < 10; i++) {
688 : // we will try some random positions ...
689 : pos = RandHelper::rand(getLength());
690 406514 : posLat = getDepartPosLat(veh); // could be random as well
691 406514 : if (isInsertionSuccess(&veh, speed, pos, posLat, patchSpeed, MSMoveReminder::NOTIFICATION_DEPARTED)) {
692 5892 : MSNet::getInstance()->getInsertionControl().retractDescheduleDeparture(&veh);
693 5892 : return true;
694 : }
695 : }
696 : // ... and if that doesn't work, we put the vehicle to the free position
697 38339 : bool success = freeInsertion(veh, speed, posLat);
698 38339 : if (success) {
699 9193 : MSNet::getInstance()->getInsertionControl().retractDescheduleDeparture(&veh);
700 : }
701 : return success;
702 : }
703 226435 : case DepartPosDefinition::FREE:
704 226435 : return freeInsertion(veh, speed, posLat);
705 724350 : case DepartPosDefinition::LAST:
706 724350 : return lastInsertion(veh, speed, posLat, patchSpeed);
707 3555 : case DepartPosDefinition::STOP:
708 3555 : if (veh.hasStops() && veh.getNextStop().lane == this) {
709 : // getLastFreePos of stopping place could return negative position to avoid blocking the stop
710 3549 : pos = MAX2(0.0, veh.getNextStop().getEndPos(veh));
711 : break;
712 : }
713 : FALLTHROUGH;
714 : case DepartPosDefinition::BASE:
715 : case DepartPosDefinition::DEFAULT:
716 : case DepartPosDefinition::SPLIT_FRONT:
717 : default:
718 9149604 : if (pars.departProcedure == DepartDefinition::SPLIT) {
719 : pos = getLength();
720 : // find the vehicle from which we are splitting off (should only be a single lane to check)
721 : AnyVehicleIterator end = anyVehiclesEnd();
722 3 : for (AnyVehicleIterator it = anyVehiclesBegin(); it != end; ++it) {
723 24 : const MSVehicle* cand = *it;
724 24 : if (cand->isStopped() && cand->getNextStopParameter()->split == veh.getID()) {
725 21 : if (pars.departPosProcedure == DepartPosDefinition::SPLIT_FRONT) {
726 3 : pos = cand->getPositionOnLane() + cand->getVehicleType().getMinGap() + veh.getLength();
727 : } else {
728 18 : pos = cand->getBackPositionOnLane() - veh.getVehicleType().getMinGap();
729 : }
730 : break;
731 : }
732 : }
733 : } else {
734 9149583 : pos = veh.basePos(myEdge);
735 : }
736 : break;
737 : }
738 : // determine the lateral position for special cases
739 10203915 : if (MSGlobals::gLateralResolution > 0) {
740 1338322 : switch (pars.departPosLatProcedure) {
741 : case DepartPosLatDefinition::RANDOM_FREE: {
742 0 : for (int i = 0; i < 10; i++) {
743 : // we will try some random positions ...
744 0 : posLat = RandHelper::rand(getWidth()) - getWidth() * 0.5;
745 0 : if (isInsertionSuccess(&veh, speed, pos, posLat, patchSpeed, MSMoveReminder::NOTIFICATION_DEPARTED)) {
746 : return true;
747 : }
748 : }
749 : FALLTHROUGH;
750 : }
751 : // no break! continue with DepartPosLatDefinition::FREE
752 : case DepartPosLatDefinition::FREE: {
753 : // systematically test all positions until a free lateral position is found
754 5670 : double posLatMin = -getWidth() * 0.5 + veh.getVehicleType().getWidth() * 0.5;
755 5670 : double posLatMax = getWidth() * 0.5 - veh.getVehicleType().getWidth() * 0.5;
756 18495 : for (posLat = posLatMin; posLat < posLatMax; posLat += MSGlobals::gLateralResolution) {
757 16599 : if (isInsertionSuccess(&veh, speed, pos, posLat, patchSpeed, MSMoveReminder::NOTIFICATION_DEPARTED)) {
758 : return true;
759 : }
760 : }
761 : return false;
762 : }
763 : default:
764 : break;
765 : }
766 : }
767 : // try to insert
768 10198245 : const bool success = isInsertionSuccess(&veh, speed, pos, posLat, patchSpeed, MSMoveReminder::NOTIFICATION_DEPARTED);
769 : #ifdef DEBUG_EXTRAPOLATE_DEPARTPOS
770 : if (DEBUG_COND2(&veh)) {
771 : std::cout << SIMTIME << " veh=" << veh.getID() << " success=" << success << " extrapolate=" << myExtrapolateSubstepDepart << " delay=" << veh.getDepartDelay() << " speed=" << speed << "\n";
772 : }
773 : #endif
774 10198244 : if (success && myExtrapolateSubstepDepart && veh.getDepartDelay() > 0) {
775 179598 : SUMOTime relevantDelay = MIN2(DELTA_T, veh.getDepartDelay());
776 : // try to compensate sub-step depart delay by moving the vehicle forward
777 179598 : speed = veh.getSpeed(); // may have been adapted in isInsertionSuccess
778 179598 : double dist = speed * STEPS2TIME(relevantDelay);
779 179598 : std::pair<MSVehicle* const, double> leaderInfo = getLeader(&veh, pos, veh.getBestLanesContinuation());
780 179598 : if (leaderInfo.first != nullptr) {
781 : MSVehicle* leader = leaderInfo.first;
782 179354 : const double frontGapNeeded = veh.getCarFollowModel().getSecureGap(&veh, leader, speed, leader->getSpeed(),
783 : leader->getCarFollowModel().getMaxDecel());
784 179354 : dist = MIN2(dist, leaderInfo.second - frontGapNeeded);
785 : }
786 179598 : if (dist > 0) {
787 174007 : veh.executeFractionalMove(dist);
788 : }
789 : }
790 : return success;
791 : }
792 :
793 :
794 : bool
795 5982970 : MSLane::checkFailure(const MSVehicle* aVehicle, double& speed, double& dist, const double nspeed, const bool patchSpeed, const std::string errorMsg, InsertionCheck check) const {
796 5982970 : if (nspeed < speed) {
797 2306765 : if (patchSpeed) {
798 642279 : speed = MIN2(nspeed, speed);
799 642279 : dist = aVehicle->getCarFollowModel().brakeGap(speed) + aVehicle->getVehicleType().getMinGap();
800 1664486 : } else if (speed > 0) {
801 1664486 : if ((getInsertionChecks(aVehicle) & (int)check) == 0) {
802 : return false;
803 : }
804 1664471 : if (MSGlobals::gEmergencyInsert) {
805 : // Check whether vehicle can stop at the given distance when applying emergency braking
806 47 : double emergencyBrakeGap = 0.5 * speed * speed / aVehicle->getCarFollowModel().getEmergencyDecel();
807 47 : if (emergencyBrakeGap <= dist) {
808 : // Vehicle may stop in time with emergency deceleration
809 : // stil, emit a warning
810 141 : WRITE_WARNINGF(TL("Vehicle '%' is inserted in emergency situation."), aVehicle->getID());
811 47 : return false;
812 : }
813 : }
814 :
815 1664424 : if (errorMsg != "") {
816 87 : WRITE_ERRORF(TL("Vehicle '%' will not be able to depart using the given velocity (%)!"), aVehicle->getID(), errorMsg);
817 29 : MSNet::getInstance()->getInsertionControl().descheduleDeparture(aVehicle);
818 : }
819 1664424 : return true;
820 : }
821 : }
822 : return false;
823 : }
824 :
825 :
826 : bool
827 11671419 : MSLane::isInsertionSuccess(MSVehicle* aVehicle,
828 : double speed, double pos, double posLat, bool patchSpeed,
829 : MSMoveReminder::Notification notification) {
830 11671419 : int insertionChecks = getInsertionChecks(aVehicle);
831 11671419 : if (pos < 0 || pos > myLength) {
832 : // we may not start there
833 9015 : WRITE_WARNINGF(TL("Invalid departPos % given for vehicle '%'. Inserting at lane end instead."),
834 : pos, aVehicle->getID());
835 3005 : pos = myLength;
836 : }
837 :
838 : #ifdef DEBUG_INSERTION
839 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
840 : std::cout << "\nIS_INSERTION_SUCCESS\n"
841 : << SIMTIME << " lane=" << getID()
842 : << " veh '" << aVehicle->getID()
843 : << " bestLanes=" << toString(aVehicle->getBestLanesContinuation(this))
844 : << " pos=" << pos
845 : << " speed=" << speed
846 : << " patchSpeed=" << patchSpeed
847 : << "'\n";
848 : }
849 : #endif
850 :
851 11671419 : aVehicle->setTentativeLaneAndPosition(this, pos, posLat);
852 11671419 : aVehicle->updateBestLanes(false, this);
853 : const MSCFModel& cfModel = aVehicle->getCarFollowModel();
854 11671419 : const std::vector<MSLane*>& bestLaneConts = aVehicle->getBestLanesContinuation(this);
855 : std::vector<MSLane*>::const_iterator ri = bestLaneConts.begin();
856 11671419 : double seen = getLength() - pos; // == distance from insertion position until the end of the currentLane
857 11671419 : double dist = cfModel.brakeGap(speed) + aVehicle->getVehicleType().getMinGap();
858 11671419 : const bool isRail = isRailway(aVehicle->getVClass());
859 11671419 : if (isRail && insertionChecks != (int)InsertionCheck::NONE
860 11671419 : && aVehicle->getParameter().departProcedure != DepartDefinition::SPLIT) {
861 47870 : const MSDriveWay* dw = MSDriveWay::getDepartureDriveway(aVehicle);
862 : MSEdgeVector occupied;
863 47870 : if (dw->foeDriveWayOccupied(false, aVehicle, occupied)) {
864 : #ifdef DEBUG_INSERTION
865 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
866 : std::cout << " foe of driveway " + dw->getID() + " has occupied edges " + toString(occupied) << "\n";
867 : }
868 : #endif
869 : return false;
870 : }
871 47870 : }
872 : // do not insert if the bidirectional edge is occupied
873 11631142 : if (getBidiLane() != nullptr && isRail && getBidiLane()->getVehicleNumberWithPartials() > 0) {
874 0 : if ((insertionChecks & (int)InsertionCheck::BIDI) != 0) {
875 : #ifdef DEBUG_INSERTION
876 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
877 : std::cout << " bidi-lane occupied\n";
878 : }
879 : #endif
880 : return false;
881 : }
882 : }
883 : MSLink* firstRailSignal = nullptr;
884 : double firstRailSignalDist = -1;
885 :
886 : // before looping through the continuation lanes, check if a stop is scheduled on this lane
887 : // (the code is duplicated in the loop)
888 11631142 : if (aVehicle->hasStops()) {
889 261148 : const MSStop& nextStop = aVehicle->getNextStop();
890 261148 : if (nextStop.lane == this) {
891 111214 : std::stringstream msg;
892 : double distToStop, safeSpeed;
893 111214 : if (nextStop.pars.speed > 0) {
894 3746 : msg << "scheduled waypoint on lane '" << myID << "' too close";
895 3746 : distToStop = MAX2(0.0, nextStop.pars.startPos - pos);
896 3746 : safeSpeed = cfModel.freeSpeed(aVehicle, speed, distToStop, nextStop.pars.speed, true, MSCFModel::CalcReason::FUTURE);
897 : } else {
898 107468 : msg << "scheduled stop on lane '" << myID << "' too close";
899 107468 : distToStop = nextStop.pars.endPos - pos;
900 107468 : safeSpeed = cfModel.stopSpeed(aVehicle, speed, distToStop, MSCFModel::CalcReason::FUTURE);
901 : }
902 333629 : if (checkFailure(aVehicle, speed, dist, MAX2(0.0, safeSpeed), patchSpeed, msg.str(), InsertionCheck::STOP)) {
903 : // we may not drive with the given velocity - we cannot stop at the stop
904 : return false;
905 : }
906 111214 : }
907 : }
908 :
909 11631134 : const MSRoute& r = aVehicle->getRoute();
910 11631134 : MSRouteIterator ce = r.begin();
911 : int nRouteSuccs = 1;
912 : MSLane* currentLane = this;
913 : MSLane* nextLane = this;
914 17582710 : SUMOTime arrivalTime = MSNet::getInstance()->getCurrentTimeStep() + TIME2STEPS(seen / MAX2(speed, SUMO_const_haltingSpeed));
915 12154679 : while ((seen < dist || (isRail && firstRailSignal == nullptr)) && ri != bestLaneConts.end()) {
916 : // get the next link used...
917 660662 : std::vector<MSLink*>::const_iterator link = succLinkSec(*aVehicle, nRouteSuccs, *currentLane, bestLaneConts);
918 660662 : if (currentLane->isLinkEnd(link)) {
919 50656 : if (¤tLane->getEdge() == r.getLastEdge()) {
920 : // reached the end of the route
921 14029 : if (aVehicle->getParameter().arrivalSpeedProcedure == ArrivalSpeedDefinition::GIVEN) {
922 129 : const double remaining = seen + aVehicle->getArrivalPos() - currentLane->getLength();
923 129 : const double nspeed = cfModel.freeSpeed(aVehicle, speed, remaining, aVehicle->getParameter().arrivalSpeed, true, MSCFModel::CalcReason::FUTURE);
924 258 : if (checkFailure(aVehicle, speed, dist, nspeed,
925 : patchSpeed, "arrival speed too low", InsertionCheck::ARRIVAL_SPEED)) {
926 : // we may not drive with the given velocity - we cannot match the specified arrival speed
927 : return false;
928 : }
929 : }
930 : } else {
931 : // lane does not continue
932 36627 : if (checkFailure(aVehicle, speed, dist, cfModel.insertionStopSpeed(aVehicle, speed, seen),
933 73254 : patchSpeed, "junction '" + currentLane->getEdge().getToJunction()->getID() + "' too close", InsertionCheck::JUNCTION)) {
934 : // we may not drive with the given velocity - we cannot stop at the junction
935 : return false;
936 : }
937 : }
938 : break;
939 : }
940 610006 : if (isRail && firstRailSignal == nullptr) {
941 : std::string constraintInfo;
942 : bool isInsertionOrder;
943 14310 : if (MSRailSignal::hasInsertionConstraint(*link, aVehicle, constraintInfo, isInsertionOrder)) {
944 6216 : setParameter((isInsertionOrder ? "insertionOrder" : "insertionConstraint:")
945 9261 : + aVehicle->getID(), constraintInfo);
946 : #ifdef DEBUG_INSERTION
947 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
948 : std::cout << " insertion constraint at link " << (*link)->getDescription() << " not cleared \n";
949 : }
950 : #endif
951 : return false;
952 : }
953 : }
954 :
955 : // might also by a regular traffic_light instead of a rail_signal
956 606898 : if (firstRailSignal == nullptr && (*link)->getTLLogic() != nullptr) {
957 : firstRailSignal = *link;
958 : firstRailSignalDist = seen;
959 : }
960 606898 : if (!(*link)->opened(arrivalTime, speed, speed, aVehicle->getVehicleType().getLength(), aVehicle->getImpatience(),
961 : cfModel.getMaxDecel(), 0, posLat, nullptr, false, aVehicle)
962 606898 : || !(*link)->havePriority()) {
963 : // have to stop at junction
964 38695 : std::string errorMsg = "";
965 38695 : const LinkState state = (*link)->getState();
966 38695 : if (state == LINKSTATE_MINOR
967 38695 : || state == LINKSTATE_EQUAL
968 : || state == LINKSTATE_STOP
969 : || state == LINKSTATE_ALLWAY_STOP) {
970 : // no sense in trying later
971 : errorMsg = "unpriorised junction too close";
972 27996 : } else if ((*link)->getTLLogic() != nullptr && !(*link)->getTLLogic()->getsMajorGreen((*link)->getTLIndex())) {
973 : // traffic light never turns 'G'?
974 6092 : errorMsg = "tlLogic '" + (*link)->getTLLogic()->getID() + "' link " + toString((*link)->getTLIndex()) + " never switches to 'G'";
975 : }
976 38695 : const double laneStopOffset = MAX2(getVehicleStopOffset(aVehicle),
977 38695 : aVehicle->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPLINE_CROSSING_GAP, MSPModel::SAFETY_GAP) - (*link)->getDistToFoePedCrossing());
978 38695 : const double remaining = seen - laneStopOffset;
979 38695 : auto dsp = aVehicle->getParameter().departSpeedProcedure;
980 38695 : const bool patchSpeedSpecial = patchSpeed || dsp == DepartSpeedDefinition::DESIRED || dsp == DepartSpeedDefinition::LIMIT;
981 : // patchSpeed depends on the presence of vehicles for these procedures. We never want to abort them here
982 38695 : if (dsp == DepartSpeedDefinition::LAST || dsp == DepartSpeedDefinition::AVG) {
983 : errorMsg = "";
984 : }
985 77390 : if (checkFailure(aVehicle, speed, dist, cfModel.insertionStopSpeed(aVehicle, speed, remaining),
986 : patchSpeedSpecial, errorMsg, InsertionCheck::JUNCTION)) {
987 : // we may not drive with the given velocity - we cannot stop at the junction in time
988 : #ifdef DEBUG_INSERTION
989 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
990 : std::cout << SIMTIME << " isInsertionSuccess lane=" << getID()
991 : << " veh=" << aVehicle->getID()
992 : << " patchSpeed=" << patchSpeed
993 : << " speed=" << speed
994 : << " remaining=" << remaining
995 : << " leader=" << currentLane->getLastVehicleInformation(aVehicle, 0, 0).toString()
996 : << " last=" << Named::getIDSecure(getLastAnyVehicle())
997 : << " meanSpeed=" << getMeanSpeed()
998 : << " failed (@926)!\n";
999 : }
1000 : #endif
1001 : return false;
1002 : }
1003 : #ifdef DEBUG_INSERTION
1004 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1005 : std::cout << "trying insertion before minor link: "
1006 : << "insertion speed = " << speed << " dist=" << dist
1007 : << "\n";
1008 : }
1009 : #endif
1010 : break;
1011 568203 : } else if (nextLane->isInternal()) {
1012 215809 : double tmp = 0;
1013 215809 : bool dummyReq = true;
1014 : #ifdef DEBUG_INSERTION
1015 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1016 : std::cout << "checking linkLeader for lane '" << nextLane->getID() << "'\n";
1017 : gDebugFlag1 = true;
1018 : }
1019 : #endif
1020 215809 : aVehicle->checkLinkLeader(*link, nextLane, seen, nullptr, speed, tmp, tmp, dummyReq);
1021 : #ifdef DEBUG_INSERTION
1022 : gDebugFlag1 = false;
1023 : #endif
1024 : }
1025 : // get the next used lane (including internal)
1026 568203 : nextLane = (*link)->getViaLaneOrLane();
1027 : // check how next lane affects the journey
1028 259607 : if (nextLane != nullptr) {
1029 :
1030 : // do not insert if the bidirectional edge is occupied before a railSignal has been encountered
1031 568203 : if (firstRailSignal == nullptr && nextLane->getBidiLane() != nullptr && nextLane->getBidiLane()->getVehicleNumberWithPartials() > 0) {
1032 0 : if ((insertionChecks & (int)InsertionCheck::ONCOMING_TRAIN) != 0) {
1033 44657 : return false;
1034 : }
1035 : }
1036 :
1037 : // check if there are stops on the next lane that should be regarded
1038 : // (this block is duplicated before the loop to deal with the insertion lane)
1039 568203 : if (aVehicle->hasStops()) {
1040 6754 : const MSStop& nextStop = aVehicle->getNextStop();
1041 6754 : if (nextStop.lane == nextLane) {
1042 297 : std::stringstream msg;
1043 297 : msg << "scheduled stop on lane '" << nextStop.lane->getID() << "' too close";
1044 297 : const double distToStop = seen + nextStop.pars.endPos;
1045 297 : if (checkFailure(aVehicle, speed, dist, cfModel.insertionStopSpeed(aVehicle, speed, distToStop),
1046 297 : patchSpeed, msg.str(), InsertionCheck::STOP)) {
1047 : // we may not drive with the given velocity - we cannot stop at the stop
1048 : return false;
1049 : }
1050 297 : }
1051 : }
1052 :
1053 : // check leader on next lane
1054 568203 : MSLeaderInfo leaders = nextLane->getLastVehicleInformation(aVehicle, 0, 0);
1055 568203 : if (leaders.hasVehicles()) {
1056 243563 : const double nspeed = nextLane->safeInsertionSpeed(aVehicle, seen, leaders, speed);
1057 : #ifdef DEBUG_INSERTION
1058 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1059 : std::cout << SIMTIME << " leader on lane '" << nextLane->getID() << "': " << leaders.toString() << " nspeed=" << nspeed << "\n";
1060 : }
1061 : #endif
1062 468470 : if (nspeed == INVALID_SPEED || checkFailure(aVehicle, speed, dist, nspeed, patchSpeed, "", InsertionCheck::LEADER_GAP)) {
1063 : // we may not drive with the given velocity - we crash into the leader
1064 : #ifdef DEBUG_INSERTION
1065 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1066 : std::cout << " isInsertionSuccess lane=" << getID()
1067 : << " veh=" << aVehicle->getID()
1068 : << " pos=" << pos
1069 : << " posLat=" << posLat
1070 : << " patchSpeed=" << patchSpeed
1071 : << " speed=" << speed
1072 : << " nspeed=" << nspeed
1073 : << " nextLane=" << nextLane->getID()
1074 : << " lead=" << leaders.toString()
1075 : << " failed (@641)!\n";
1076 : }
1077 : #endif
1078 : return false;
1079 : }
1080 : }
1081 527951 : if (!nextLane->checkForPedestrians(aVehicle, speed, dist, -seen, patchSpeed)) {
1082 : return false;
1083 : }
1084 : // check next lane's maximum velocity
1085 527951 : const double nspeed = cfModel.freeSpeed(aVehicle, speed, seen, nextLane->getVehicleMaxSpeed(aVehicle), true, MSCFModel::CalcReason::FUTURE);
1086 527950 : if (nspeed < speed) {
1087 67353 : if (patchSpeed || aVehicle->getParameter().departSpeedProcedure != DepartSpeedDefinition::GIVEN) {
1088 66862 : speed = nspeed;
1089 66862 : dist = cfModel.brakeGap(speed) + aVehicle->getVehicleType().getMinGap();
1090 : } else {
1091 491 : if ((insertionChecks & (int)InsertionCheck::SPEED_LIMIT) != 0) {
1092 491 : if (!MSGlobals::gCheckRoutes) {
1093 15 : WRITE_WARNINGF(TL("Vehicle '%' is inserted too fast and will violate the speed limit on a lane '%'."),
1094 : aVehicle->getID(), nextLane->getID());
1095 : } else {
1096 : // we may not drive with the given velocity - we would be too fast on the next lane
1097 1458 : WRITE_ERRORF(TL("Vehicle '%' will not be able to depart using the given velocity (slow lane ahead)!"), aVehicle->getID());
1098 486 : MSNet::getInstance()->getInsertionControl().descheduleDeparture(aVehicle);
1099 : return false;
1100 : }
1101 : }
1102 : }
1103 : }
1104 : // check traffic on next junction
1105 : // we cannot use (*link)->opened because a vehicle without priority
1106 : // may already be comitted to blocking the link and unable to stop
1107 527464 : const SUMOTime leaveTime = (*link)->getLeaveTime(arrivalTime, speed, speed, aVehicle->getVehicleType().getLength());
1108 527464 : if ((*link)->hasApproachingFoe(arrivalTime, leaveTime, speed, cfModel.getMaxDecel())) {
1109 18250 : if (checkFailure(aVehicle, speed, dist, cfModel.insertionStopSpeed(aVehicle, speed, seen), patchSpeed, "", InsertionCheck::JUNCTION)) {
1110 : // we may not drive with the given velocity - we crash at the junction
1111 : return false;
1112 : }
1113 : }
1114 562526 : arrivalTime += TIME2STEPS(nextLane->getLength() / MAX2(speed, NUMERICAL_EPS));
1115 523545 : seen += nextLane->getLength();
1116 : currentLane = nextLane;
1117 523545 : if ((*link)->getViaLane() == nullptr) {
1118 257539 : nRouteSuccs++;
1119 : ++ce;
1120 : ++ri;
1121 : }
1122 568203 : }
1123 : }
1124 :
1125 : // get the pointer to the vehicle next in front of the given position
1126 11568158 : MSLeaderInfo leaders = getLastVehicleInformation(aVehicle, 0, pos);
1127 : //if (aVehicle->getID() == "disabled") std::cout << " leaders=" << leaders.toString() << "\n";
1128 11568158 : const double nspeed = safeInsertionSpeed(aVehicle, -pos, leaders, speed);
1129 17110060 : if (nspeed == INVALID_SPEED || checkFailure(aVehicle, speed, dist, nspeed, patchSpeed, "", InsertionCheck::LEADER_GAP)) {
1130 : // we may not drive with the given velocity - we crash into the leader
1131 : #ifdef DEBUG_INSERTION
1132 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1133 : std::cout << SIMTIME << " isInsertionSuccess lane=" << getID()
1134 : << " veh=" << aVehicle->getID()
1135 : << " pos=" << pos
1136 : << " posLat=" << posLat
1137 : << " patchSpeed=" << patchSpeed
1138 : << " speed=" << speed
1139 : << " nspeed=" << nspeed
1140 : << " nextLane=" << nextLane->getID()
1141 : << " leaders=" << leaders.toString()
1142 : << " failed (@700)!\n";
1143 : }
1144 : #endif
1145 : return false;
1146 : }
1147 : #ifdef DEBUG_INSERTION
1148 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1149 : std::cout << SIMTIME << " speed = " << speed << " nspeed = " << nspeed << std::endl;
1150 : }
1151 : #endif
1152 :
1153 3918425 : const MSLeaderDistanceInfo& followers = getFollowersOnConsecutive(aVehicle, aVehicle->getBackPositionOnLane(), false);
1154 8464292 : for (int i = 0; i < followers.numSublanes(); ++i) {
1155 5572980 : const MSVehicle* follower = followers[i].first;
1156 5572980 : if (follower != nullptr) {
1157 1351268 : const double backGapNeeded = follower->getCarFollowModel().getSecureGap(follower, aVehicle, follower->getSpeed(), speed, cfModel.getMaxDecel());
1158 1351268 : if (followers[i].second < backGapNeeded
1159 1351268 : && ((insertionChecks & (int)InsertionCheck::FOLLOWER_GAP) != 0
1160 8 : || (followers[i].second < 0 && (insertionChecks & (int)InsertionCheck::COLLISION) != 0))) {
1161 : // too close to the follower on this lane
1162 : #ifdef DEBUG_INSERTION
1163 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1164 : std::cout << SIMTIME << " isInsertionSuccess lane=" << getID()
1165 : << " veh=" << aVehicle->getID()
1166 : << " pos=" << pos
1167 : << " posLat=" << posLat
1168 : << " speed=" << speed
1169 : << " nspeed=" << nspeed
1170 : << " follower=" << follower->getID()
1171 : << " backGapNeeded=" << backGapNeeded
1172 : << " gap=" << followers[i].second
1173 : << " failure (@719)!\n";
1174 : }
1175 : #endif
1176 1027113 : return false;
1177 : }
1178 : }
1179 : }
1180 :
1181 2891312 : if (!checkForPedestrians(aVehicle, speed, dist, pos, patchSpeed)) {
1182 : return false;
1183 : }
1184 :
1185 2890762 : MSLane* shadowLane = aVehicle->getLaneChangeModel().getShadowLane(this);
1186 : #ifdef DEBUG_INSERTION
1187 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1188 : std::cout << " shadowLane=" << Named::getIDSecure(shadowLane) << "\n";
1189 : }
1190 : #endif
1191 2890762 : if (shadowLane != nullptr) {
1192 2797 : const MSLeaderDistanceInfo& shadowFollowers = shadowLane->getFollowersOnConsecutive(aVehicle, aVehicle->getBackPositionOnLane(), false);
1193 7652 : for (int i = 0; i < shadowFollowers.numSublanes(); ++i) {
1194 4872 : const MSVehicle* follower = shadowFollowers[i].first;
1195 4872 : if (follower != nullptr) {
1196 18 : const double backGapNeeded = follower->getCarFollowModel().getSecureGap(follower, aVehicle, follower->getSpeed(), speed, cfModel.getMaxDecel());
1197 18 : if (shadowFollowers[i].second < backGapNeeded
1198 18 : && ((insertionChecks & (int)InsertionCheck::FOLLOWER_GAP) != 0
1199 0 : || (shadowFollowers[i].second < 0 && (insertionChecks & (int)InsertionCheck::COLLISION) != 0))) {
1200 : // too close to the follower on this lane
1201 : #ifdef DEBUG_INSERTION
1202 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1203 : std::cout << SIMTIME
1204 : << " isInsertionSuccess shadowlane=" << shadowLane->getID()
1205 : << " veh=" << aVehicle->getID()
1206 : << " pos=" << pos
1207 : << " posLat=" << posLat
1208 : << " speed=" << speed
1209 : << " nspeed=" << nspeed
1210 : << " follower=" << follower->getID()
1211 : << " backGapNeeded=" << backGapNeeded
1212 : << " gap=" << shadowFollowers[i].second
1213 : << " failure (@812)!\n";
1214 : }
1215 : #endif
1216 17 : return false;
1217 : }
1218 : }
1219 : }
1220 2780 : const MSLeaderInfo& ahead = shadowLane->getLastVehicleInformation(nullptr, 0, aVehicle->getPositionOnLane(), false);
1221 6675 : for (int i = 0; i < ahead.numSublanes(); ++i) {
1222 3988 : const MSVehicle* veh = ahead[i];
1223 3988 : if (veh != nullptr) {
1224 339 : const double gap = veh->getBackPositionOnLane(shadowLane) - aVehicle->getPositionOnLane() - aVehicle->getVehicleType().getMinGap();
1225 339 : const double gapNeeded = aVehicle->getCarFollowModel().getSecureGap(aVehicle, veh, speed, veh->getSpeed(), veh->getCarFollowModel().getMaxDecel());
1226 339 : if (gap < gapNeeded
1227 93 : && ((insertionChecks & (int)InsertionCheck::LEADER_GAP) != 0
1228 0 : || (gap < 0 && (insertionChecks & (int)InsertionCheck::COLLISION) != 0))) {
1229 : // too close to the shadow leader
1230 : #ifdef DEBUG_INSERTION
1231 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1232 : std::cout << SIMTIME
1233 : << " isInsertionSuccess shadowlane=" << shadowLane->getID()
1234 : << " veh=" << aVehicle->getID()
1235 : << " pos=" << pos
1236 : << " posLat=" << posLat
1237 : << " speed=" << speed
1238 : << " nspeed=" << nspeed
1239 : << " leader=" << veh->getID()
1240 : << " gapNeeded=" << gapNeeded
1241 : << " gap=" << gap
1242 : << " failure (@842)!\n";
1243 : }
1244 : #endif
1245 : return false;
1246 : }
1247 : }
1248 : }
1249 2797 : }
1250 2890652 : if (followers.numFreeSublanes() > 0) {
1251 : // check approaching vehicles to prevent rear-end collisions
1252 2698849 : const double backOffset = pos - aVehicle->getVehicleType().getLength();
1253 2698849 : const double missingRearGap = getMissingRearGap(aVehicle, backOffset, speed);
1254 2698849 : if (missingRearGap > 0
1255 0 : && (insertionChecks & (int)InsertionCheck::LEADER_GAP) != 0) {
1256 : // too close to a follower
1257 : #ifdef DEBUG_INSERTION
1258 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1259 : std::cout << SIMTIME
1260 : << " isInsertionSuccess lane=" << getID()
1261 : << " veh=" << aVehicle->getID()
1262 : << " pos=" << pos
1263 : << " posLat=" << posLat
1264 : << " speed=" << speed
1265 : << " nspeed=" << nspeed
1266 : << " missingRearGap=" << missingRearGap
1267 : << " failure (@728)!\n";
1268 : }
1269 : #endif
1270 : return false;
1271 : }
1272 : }
1273 2890652 : if (insertionChecks == (int)InsertionCheck::NONE) {
1274 2190 : speed = MAX2(0.0, speed);
1275 : }
1276 : // may got negative while adaptation
1277 2890652 : if (speed < 0) {
1278 : #ifdef DEBUG_INSERTION
1279 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1280 : std::cout << SIMTIME
1281 : << " isInsertionSuccess lane=" << getID()
1282 : << " veh=" << aVehicle->getID()
1283 : << " pos=" << pos
1284 : << " posLat=" << posLat
1285 : << " speed=" << speed
1286 : << " nspeed=" << nspeed
1287 : << " failed (@733)!\n";
1288 : }
1289 : #endif
1290 : return false;
1291 : }
1292 2890644 : const int bestLaneOffset = aVehicle->getBestLaneOffset();
1293 2890644 : const double extraReservation = aVehicle->getLaneChangeModel().getExtraReservation(bestLaneOffset);
1294 2890644 : if (extraReservation > 0) {
1295 19538 : std::stringstream msg;
1296 19538 : msg << "too many lane changes required on lane '" << myID << "'";
1297 : // we need to take into acount one extra actionStep of delay due to #3665
1298 37630 : double distToStop = MAX2(0.0, aVehicle->getBestLaneDist() - pos - extraReservation - speed * aVehicle->getActionStepLengthSecs());
1299 : double stopSpeed = cfModel.stopSpeed(aVehicle, speed, distToStop, MSCFModel::CalcReason::FUTURE);
1300 : #ifdef DEBUG_INSERTION
1301 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1302 : std::cout << "\nIS_INSERTION_SUCCESS\n"
1303 : << SIMTIME << " veh=" << aVehicle->getID() << " bestLaneOffset=" << bestLaneOffset << " bestLaneDist=" << aVehicle->getBestLaneDist() << " extraReservation=" << extraReservation
1304 : << " distToStop=" << distToStop << " v=" << speed << " v2=" << stopSpeed << "\n";
1305 : }
1306 : #endif
1307 19538 : if (checkFailure(aVehicle, speed, distToStop, MAX2(0.0, stopSpeed),
1308 19538 : patchSpeed, msg.str(), InsertionCheck::LANECHANGE)) {
1309 : // we may not drive with the given velocity - we cannot reserve enough space for lane changing
1310 : return false;
1311 : }
1312 19538 : }
1313 : // enter
1314 2890644 : incorporateVehicle(aVehicle, pos, speed, posLat, find_if(myVehicles.begin(), myVehicles.end(), [&](MSVehicle * const v) {
1315 2411429 : return v->getPositionOnLane() >= pos;
1316 : }), notification);
1317 : #ifdef DEBUG_INSERTION
1318 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1319 : std::cout << SIMTIME
1320 : << " isInsertionSuccess lane=" << getID()
1321 : << " veh=" << aVehicle->getID()
1322 : << " pos=" << pos
1323 : << " posLat=" << posLat
1324 : << " speed=" << speed
1325 : << " nspeed=" << nspeed
1326 : << "\n myVehicles=" << toString(myVehicles)
1327 : << " myPartial=" << toString(myPartialVehicles)
1328 : << " myManeuverReservations=" << toString(myManeuverReservations)
1329 : << "\n leaders=" << leaders.toString()
1330 : << "\n success!\n";
1331 : }
1332 : #endif
1333 2890644 : if (isRail) {
1334 4335 : unsetParameter("insertionConstraint:" + aVehicle->getID());
1335 4335 : unsetParameter("insertionOrder:" + aVehicle->getID());
1336 4335 : unsetParameter("insertionFail:" + aVehicle->getID());
1337 : // rail_signal (not traffic_light) requires approach information for
1338 : // switching correctly at the start of the next simulation step
1339 4335 : if (firstRailSignal != nullptr && firstRailSignal->getJunction()->getType() == SumoXMLNodeType::RAIL_SIGNAL) {
1340 1440 : aVehicle->registerInsertionApproach(firstRailSignal, firstRailSignalDist);
1341 : }
1342 : }
1343 : return true;
1344 11568158 : }
1345 :
1346 :
1347 : void
1348 47003 : MSLane::forceVehicleInsertion(MSVehicle* veh, double pos, MSMoveReminder::Notification notification, double posLat) {
1349 47003 : veh->updateBestLanes(true, this);
1350 : bool dummy;
1351 47003 : const double speed = veh->hasDeparted() ? veh->getSpeed() : getDepartSpeed(*veh, dummy);
1352 47003 : incorporateVehicle(veh, pos, speed, posLat, find_if(myVehicles.begin(), myVehicles.end(), [&](MSVehicle * const v) {
1353 119872 : return v->getPositionOnLane() >= pos;
1354 : }), notification);
1355 47003 : }
1356 :
1357 : int
1358 19381153 : MSLane::getInsertionChecks(const MSVehicle* veh) {
1359 19381153 : if (veh->getParameter().wasSet(VEHPARS_INSERTION_CHECKS_SET)) {
1360 282008 : return veh->getParameter().insertionChecks;
1361 : } else {
1362 19099145 : return MSGlobals::gInsertionChecks;
1363 : }
1364 : }
1365 :
1366 : double
1367 11811721 : MSLane::safeInsertionSpeed(const MSVehicle* veh, double seen, const MSLeaderInfo& leaders, double speed) {
1368 : double nspeed = speed;
1369 : #ifdef DEBUG_INSERTION
1370 : if (DEBUG_COND2(veh)) {
1371 : std::cout << SIMTIME << " safeInsertionSpeed veh=" << veh->getID() << " speed=" << speed << "\n";
1372 : }
1373 : #endif
1374 21034387 : for (int i = 0; i < leaders.numSublanes(); ++i) {
1375 15267578 : const MSVehicle* leader = leaders[i];
1376 15267578 : if (leader != nullptr) {
1377 13176578 : double gap = leader->getBackPositionOnLane(this) + seen - veh->getVehicleType().getMinGap();
1378 13176578 : if (leader->getLane() == getBidiLane()) {
1379 : // use distance to front position and account for movement
1380 294 : gap -= (leader->getLength() + leader->getBrakeGap(true));
1381 : }
1382 13176578 : if (gap < 0) {
1383 : #ifdef DEBUG_INSERTION
1384 : if (DEBUG_COND2(veh)) {
1385 : std::cout << " leader=" << leader->getID() << " bPos=" << leader->getBackPositionOnLane(this) << " gap=" << gap << "\n";
1386 : }
1387 : #endif
1388 6044912 : if ((getInsertionChecks(veh) & (int)InsertionCheck::COLLISION) != 0) {
1389 : return INVALID_SPEED;
1390 : } else {
1391 0 : return 0;
1392 : }
1393 : }
1394 7131666 : nspeed = MIN2(nspeed,
1395 7131666 : veh->getCarFollowModel().insertionFollowSpeed(veh, speed, gap, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel(), leader));
1396 : #ifdef DEBUG_INSERTION
1397 : if (DEBUG_COND2(veh)) {
1398 : std::cout << " leader=" << leader->getID() << " bPos=" << leader->getBackPositionOnLane(this) << " gap=" << gap << " nspeed=" << nspeed << "\n";
1399 : }
1400 : #endif
1401 : }
1402 : }
1403 : return nspeed;
1404 : }
1405 :
1406 :
1407 : // ------ Handling vehicles lapping into lanes ------
1408 : const MSLeaderInfo
1409 659296949 : MSLane::getLastVehicleInformation(const MSVehicle* ego, double latOffset, double minPos, bool allowCached) const {
1410 659296949 : if (myLeaderInfoTime < MSNet::getInstance()->getCurrentTimeStep() || ego != nullptr || minPos > 0 || !allowCached) {
1411 227251361 : MSLeaderInfo leaderTmp(myWidth, ego, latOffset);
1412 : AnyVehicleIterator last = anyVehiclesBegin();
1413 : int freeSublanes = 1; // number of sublanes for which no leader was found
1414 : //if (ego->getID() == "disabled" && SIMTIME == 58) {
1415 : // std::cout << "DEBUG\n";
1416 : //}
1417 227251361 : const MSVehicle* veh = *last;
1418 1701541974 : while (freeSublanes > 0 && veh != nullptr) {
1419 : #ifdef DEBUG_PLAN_MOVE
1420 : if (DEBUG_COND2(ego) || DEBUG_COND) {
1421 : gDebugFlag1 = true;
1422 : std::cout << " getLastVehicleInformation lane=" << getID() << " minPos=" << minPos << " veh=" << veh->getID() << " pos=" << veh->getPositionOnLane(this) << "\n";
1423 : }
1424 : #endif
1425 2946397344 : if (veh != ego && MAX2(0.0, veh->getPositionOnLane(this)) >= minPos) {
1426 237403579 : const double vehLatOffset = veh->getLatOffset(this);
1427 237403579 : freeSublanes = leaderTmp.addLeader(veh, true, vehLatOffset);
1428 : #ifdef DEBUG_PLAN_MOVE
1429 : if (DEBUG_COND2(ego) || DEBUG_COND) {
1430 : std::cout << " latOffset=" << vehLatOffset << " newLeaders=" << leaderTmp.toString() << "\n";
1431 : }
1432 : #endif
1433 : }
1434 1474290613 : veh = *(++last);
1435 : }
1436 227251361 : if (ego == nullptr && minPos == 0) {
1437 : #ifdef HAVE_FOX
1438 98486276 : ScopedLocker<> lock(myLeaderInfoMutex, MSGlobals::gNumSimThreads > 1);
1439 : #endif
1440 : // update cached value
1441 : myLeaderInfo = leaderTmp;
1442 98486276 : myLeaderInfoTime = MSNet::getInstance()->getCurrentTimeStep();
1443 : }
1444 : #ifdef DEBUG_PLAN_MOVE
1445 : //if (DEBUG_COND2(ego)) std::cout << SIMTIME
1446 : // << " getLastVehicleInformation lane=" << getID()
1447 : // << " ego=" << Named::getIDSecure(ego)
1448 : // << "\n"
1449 : // << " vehicles=" << toString(myVehicles)
1450 : // << " partials=" << toString(myPartialVehicles)
1451 : // << "\n"
1452 : // << " result=" << leaderTmp.toString()
1453 : // << " cached=" << myLeaderInfo.toString()
1454 : // << " myLeaderInfoTime=" << myLeaderInfoTime
1455 : // << "\n";
1456 : gDebugFlag1 = false;
1457 : #endif
1458 : return leaderTmp;
1459 227251361 : }
1460 : return myLeaderInfo;
1461 : }
1462 :
1463 :
1464 : const MSLeaderInfo
1465 359540272 : MSLane::getFirstVehicleInformation(const MSVehicle* ego, double latOffset, bool onlyFrontOnLane, double maxPos, bool allowCached) const {
1466 : #ifdef HAVE_FOX
1467 359540272 : ScopedLocker<> lock(myFollowerInfoMutex, MSGlobals::gNumSimThreads > 1);
1468 : #endif
1469 359540272 : if (myFollowerInfoTime < MSNet::getInstance()->getCurrentTimeStep() || ego != nullptr || maxPos < myLength || !allowCached || onlyFrontOnLane) {
1470 : // XXX separate cache for onlyFrontOnLane = true
1471 359540272 : MSLeaderInfo followerTmp(myWidth, ego, latOffset);
1472 : AnyVehicleIterator first = anyVehiclesUpstreamBegin();
1473 : int freeSublanes = 1; // number of sublanes for which no leader was found
1474 359540272 : const MSVehicle* veh = *first;
1475 609801307 : while (freeSublanes > 0 && veh != nullptr) {
1476 : #ifdef DEBUG_PLAN_MOVE
1477 : if (DEBUG_COND2(ego)) {
1478 : std::cout << " veh=" << veh->getID() << " pos=" << veh->getPositionOnLane(this) << " maxPos=" << maxPos << "\n";
1479 : }
1480 : #endif
1481 250261035 : if (veh != ego && veh->getPositionOnLane(this) <= maxPos
1482 500522070 : && (!onlyFrontOnLane || veh->isFrontOnLane(this))) {
1483 : //const double vehLatOffset = veh->getLane()->getRightSideOnEdge() - getRightSideOnEdge();
1484 229733775 : const double vehLatOffset = veh->getLatOffset(this);
1485 : #ifdef DEBUG_PLAN_MOVE
1486 : if (DEBUG_COND2(ego)) {
1487 : std::cout << " veh=" << veh->getID() << " latOffset=" << vehLatOffset << "\n";
1488 : }
1489 : #endif
1490 229733775 : freeSublanes = followerTmp.addLeader(veh, true, vehLatOffset);
1491 : }
1492 250261035 : veh = *(++first);
1493 : }
1494 359540272 : if (ego == nullptr && maxPos == std::numeric_limits<double>::max()) {
1495 : // update cached value
1496 : myFollowerInfo = followerTmp;
1497 359540272 : myFollowerInfoTime = MSNet::getInstance()->getCurrentTimeStep();
1498 : }
1499 : #ifdef DEBUG_PLAN_MOVE
1500 : //if (DEBUG_COND2(ego)) std::cout << SIMTIME
1501 : // << " getFirstVehicleInformation lane=" << getID()
1502 : // << " ego=" << Named::getIDSecure(ego)
1503 : // << "\n"
1504 : // << " vehicles=" << toString(myVehicles)
1505 : // << " partials=" << toString(myPartialVehicles)
1506 : // << "\n"
1507 : // << " result=" << followerTmp.toString()
1508 : // //<< " cached=" << myFollowerInfo.toString()
1509 : // << " myLeaderInfoTime=" << myLeaderInfoTime
1510 : // << "\n";
1511 : #endif
1512 : return followerTmp;
1513 359540272 : }
1514 : return myFollowerInfo;
1515 : }
1516 :
1517 :
1518 : // ------ ------
1519 : void
1520 79518598 : MSLane::planMovements(SUMOTime t) {
1521 : assert(myVehicles.size() != 0);
1522 : double cumulatedVehLength = 0.;
1523 79518598 : MSLeaderInfo leaders(myWidth);
1524 :
1525 : // iterate over myVehicles, myPartialVehicles, and myManeuverReservations merge-sort style
1526 : VehCont::reverse_iterator veh = myVehicles.rbegin();
1527 : VehCont::reverse_iterator vehPart = myPartialVehicles.rbegin();
1528 : VehCont::reverse_iterator vehRes = myManeuverReservations.rbegin();
1529 : #ifdef DEBUG_PLAN_MOVE
1530 : if (DEBUG_COND) std::cout
1531 : << "\n"
1532 : << SIMTIME
1533 : << " planMovements() lane=" << getID()
1534 : << "\n vehicles=" << toString(myVehicles)
1535 : << "\n partials=" << toString(myPartialVehicles)
1536 : << "\n reservations=" << toString(myManeuverReservations)
1537 : << "\n";
1538 : #endif
1539 : assert(MSGlobals::gLateralResolution || myManeuverReservations.size() == 0);
1540 702762610 : for (; veh != myVehicles.rend(); ++veh) {
1541 : #ifdef DEBUG_PLAN_MOVE
1542 : if (DEBUG_COND2((*veh))) {
1543 : std::cout << " plan move for: " << (*veh)->getID();
1544 : }
1545 : #endif
1546 623244012 : updateLeaderInfo(*veh, vehPart, vehRes, leaders); // 36ns with 8 threads, 9ns with 1
1547 : #ifdef DEBUG_PLAN_MOVE
1548 : if (DEBUG_COND2((*veh))) {
1549 : std::cout << " leaders=" << leaders.toString() << "\n";
1550 : }
1551 : #endif
1552 623244012 : (*veh)->planMove(t, leaders, cumulatedVehLength); // 4800ns with 8 threads, 3100 with 1
1553 623244012 : cumulatedVehLength += (*veh)->getVehicleType().getLengthWithGap();
1554 623244012 : leaders.addLeader(*veh, false, 0);
1555 : }
1556 79518598 : }
1557 :
1558 :
1559 : void
1560 79518598 : MSLane::setJunctionApproaches(const SUMOTime t) const {
1561 702762610 : for (MSVehicle* const veh : myVehicles) {
1562 623244012 : veh->setApproachingForAllLinks(t);
1563 : }
1564 79518598 : }
1565 :
1566 :
1567 : void
1568 623244012 : MSLane::updateLeaderInfo(const MSVehicle* veh, VehCont::reverse_iterator& vehPart, VehCont::reverse_iterator& vehRes, MSLeaderInfo& ahead) const {
1569 : bool morePartialVehsAhead = vehPart != myPartialVehicles.rend();
1570 : bool moreReservationsAhead = vehRes != myManeuverReservations.rend();
1571 : bool nextToConsiderIsPartial;
1572 :
1573 : // Determine relevant leaders for veh
1574 631583031 : while (moreReservationsAhead || morePartialVehsAhead) {
1575 1075942 : if ((!moreReservationsAhead || (*vehRes)->getPositionOnLane(this) <= veh->getPositionOnLane())
1576 17343463 : && (!morePartialVehsAhead || (*vehPart)->getPositionOnLane(this) <= veh->getPositionOnLane())) {
1577 : // All relevant downstream vehicles have been collected.
1578 : break;
1579 : }
1580 :
1581 : // Check whether next farthest relevant vehicle downstream is a partial vehicle or a maneuver reservation
1582 8339019 : if (moreReservationsAhead && !morePartialVehsAhead) {
1583 : nextToConsiderIsPartial = false;
1584 8255127 : } else if (morePartialVehsAhead && !moreReservationsAhead) {
1585 : nextToConsiderIsPartial = true;
1586 : } else {
1587 : assert(morePartialVehsAhead && moreReservationsAhead);
1588 : // Add farthest downstream vehicle first
1589 124848 : nextToConsiderIsPartial = (*vehPart)->getPositionOnLane(this) > (*vehRes)->getPositionOnLane(this);
1590 : }
1591 : // Add appropriate leader information
1592 124848 : if (nextToConsiderIsPartial) {
1593 8216527 : const double latOffset = (*vehPart)->getLatOffset(this);
1594 : #ifdef DEBUG_PLAN_MOVE
1595 : if (DEBUG_COND) {
1596 : std::cout << " partial ahead: " << (*vehPart)->getID() << " latOffset=" << latOffset << "\n";
1597 : }
1598 : #endif
1599 8283528 : if (!(MSGlobals::gLaneChangeDuration > 0 && (*vehPart)->getLaneChangeModel().isOpposite()
1600 67001 : && !(*vehPart)->getLaneChangeModel().isChangingLanes())) {
1601 8166648 : ahead.addLeader(*vehPart, false, latOffset);
1602 : }
1603 : ++vehPart;
1604 : morePartialVehsAhead = vehPart != myPartialVehicles.rend();
1605 : } else {
1606 122492 : const double latOffset = (*vehRes)->getLatOffset(this);
1607 : #ifdef DEBUG_PLAN_MOVE
1608 : if (DEBUG_COND) {
1609 : std::cout << " reservation ahead: " << (*vehRes)->getID() << " latOffset=" << latOffset << "\n";
1610 : }
1611 : #endif
1612 122492 : ahead.addLeader(*vehRes, false, latOffset);
1613 : ++vehRes;
1614 : moreReservationsAhead = vehRes != myManeuverReservations.rend();
1615 : }
1616 : }
1617 623244012 : }
1618 :
1619 :
1620 : void
1621 86435226 : MSLane::detectCollisions(SUMOTime timestep, const std::string& stage) {
1622 86435226 : myNeedsCollisionCheck = false;
1623 : #ifdef DEBUG_COLLISIONS
1624 : if (DEBUG_COND) {
1625 : std::vector<const MSVehicle*> all;
1626 : for (AnyVehicleIterator last = anyVehiclesBegin(); last != anyVehiclesEnd(); ++last) {
1627 : all.push_back(*last);
1628 : }
1629 : std::cout << SIMTIME << " detectCollisions stage=" << stage << " lane=" << getID() << ":\n"
1630 : << " vehs=" << toString(myVehicles) << "\n"
1631 : << " part=" << toString(myPartialVehicles) << "\n"
1632 : << " all=" << toString(all) << "\n"
1633 : << "\n";
1634 : }
1635 : #endif
1636 :
1637 86435226 : if (myCollisionAction == COLLISION_ACTION_NONE) {
1638 605626 : return;
1639 : }
1640 :
1641 : std::set<const MSVehicle*, ComparatorNumericalIdLess> toRemove;
1642 : std::set<const MSVehicle*, ComparatorNumericalIdLess> toTeleport;
1643 86422312 : if (mustCheckJunctionCollisions()) {
1644 1212437 : myNeedsCollisionCheck = true; // always check
1645 : #ifdef DEBUG_JUNCTION_COLLISIONS
1646 : if (DEBUG_COND) {
1647 : std::cout << SIMTIME << " detect junction Collisions stage=" << stage << " lane=" << getID() << ":\n"
1648 : << " vehs=" << toString(myVehicles) << "\n"
1649 : << " part=" << toString(myPartialVehicles) << "\n"
1650 : << "\n";
1651 : }
1652 : #endif
1653 : assert(myLinks.size() == 1);
1654 1212437 : const std::vector<const MSLane*>& foeLanes = myLinks.front()->getFoeLanes();
1655 : // save the iterator, it might get modified, see #8842
1656 : MSLane::AnyVehicleIterator end = anyVehiclesEnd();
1657 2145615 : for (AnyVehicleIterator veh = anyVehiclesBegin(); veh != end; ++veh) {
1658 2145615 : const MSVehicle* const collider = *veh;
1659 : //std::cout << " collider " << collider->getID() << "\n";
1660 2145615 : PositionVector colliderBoundary = collider->getBoundingBox(myCheckJunctionCollisionMinGap);
1661 30326433 : for (const MSLane* const foeLane : foeLanes) {
1662 : #ifdef DEBUG_JUNCTION_COLLISIONS
1663 : if (DEBUG_COND) {
1664 : std::cout << " foeLane " << foeLane->getID()
1665 : << " foeVehs=" << toString(foeLane->myVehicles)
1666 : << " foePart=" << toString(foeLane->myPartialVehicles) << "\n";
1667 : }
1668 : #endif
1669 : MSLane::AnyVehicleIterator foeEnd = foeLane->anyVehiclesEnd();
1670 3625580 : for (MSLane::AnyVehicleIterator it_veh = foeLane->anyVehiclesBegin(); it_veh != foeEnd; ++it_veh) {
1671 3625580 : const MSVehicle* const victim = *it_veh;
1672 3625580 : if (victim == collider) {
1673 : // may happen if the vehicles lane and shadow lane are siblings
1674 1532 : continue;
1675 : }
1676 : #ifdef DEBUG_JUNCTION_COLLISIONS
1677 : if (DEBUG_COND && DEBUG_COND2(collider)) {
1678 : std::cout << SIMTIME << " foe=" << victim->getID()
1679 : << " bound=" << colliderBoundary << " foeBound=" << victim->getBoundingBox()
1680 : << " overlaps=" << colliderBoundary.overlapsWith(victim->getBoundingBox())
1681 : << " poly=" << collider->getBoundingPoly()
1682 : << " foePoly=" << victim->getBoundingPoly()
1683 : << " overlaps2=" << collider->getBoundingPoly().overlapsWith(victim->getBoundingPoly())
1684 : << "\n";
1685 : }
1686 : #endif
1687 3624048 : if (colliderBoundary.overlapsWith(victim->getBoundingBox())) {
1688 : // make a detailed check
1689 20629 : PositionVector boundingPoly = collider->getBoundingPoly();
1690 20629 : if (collider->getBoundingPoly(myCheckJunctionCollisionMinGap).overlapsWith(victim->getBoundingPoly())) {
1691 : // junction leader is the victim (collider must still be on junction)
1692 : assert(isInternal());
1693 12580 : if (victim->getLane()->isInternal() && victim->isLeader(myLinks.front(), collider, -1)) {
1694 5433 : foeLane->handleCollisionBetween(timestep, stage, victim, collider, -1, 0, toRemove, toTeleport);
1695 : } else {
1696 7147 : handleCollisionBetween(timestep, stage, collider, victim, -1, 0, toRemove, toTeleport);
1697 : }
1698 : }
1699 20629 : }
1700 : }
1701 28180818 : detectPedestrianJunctionCollision(collider, colliderBoundary, foeLane, timestep, stage, toRemove, toTeleport);
1702 : }
1703 2145615 : if (myLinks.front()->getWalkingAreaFoe() != nullptr) {
1704 47225 : detectPedestrianJunctionCollision(collider, colliderBoundary, myLinks.front()->getWalkingAreaFoe(), timestep, stage, toRemove, toTeleport);
1705 : }
1706 2145615 : if (myLinks.front()->getWalkingAreaFoeExit() != nullptr) {
1707 54925 : detectPedestrianJunctionCollision(collider, colliderBoundary, myLinks.front()->getWalkingAreaFoeExit(), timestep, stage, toRemove, toTeleport);
1708 : }
1709 2145615 : }
1710 : }
1711 :
1712 :
1713 86422312 : if (myIntermodalCollisionAction != COLLISION_ACTION_NONE && myEdge->getPersons().size() > 0 && hasPedestrians()) {
1714 : #ifdef DEBUG_PEDESTRIAN_COLLISIONS
1715 : if (DEBUG_COND) {
1716 : std::cout << SIMTIME << " detect pedestrian collisions stage=" << stage << " lane=" << getID() << "\n";
1717 : }
1718 : #endif
1719 : AnyVehicleIterator v_end = anyVehiclesEnd();
1720 142789 : for (AnyVehicleIterator it_v = anyVehiclesBegin(); it_v != v_end; ++it_v) {
1721 142789 : const MSVehicle* v = *it_v;
1722 142789 : double back = v->getBackPositionOnLane(this);
1723 142789 : const double length = v->getVehicleType().getLength();
1724 142789 : const double right = v->getRightSideOnEdge(this) - getRightSideOnEdge();
1725 142789 : if (v->getLane() == getBidiLane()) {
1726 : // use the front position for checking
1727 565 : back -= length;
1728 : }
1729 142789 : PersonDist leader = nextBlocking(back, right, right + v->getVehicleType().getWidth());
1730 : #ifdef DEBUG_PEDESTRIAN_COLLISIONS
1731 : if (DEBUG_COND && DEBUG_COND2(v)) {
1732 : std::cout << SIMTIME << " back=" << back << " right=" << right << " person=" << Named::getIDSecure(leader.first)
1733 : << " dist=" << leader.second << " jammed=" << leader.first->isJammed() << "\n";
1734 : }
1735 : #endif
1736 142789 : if (leader.first != 0 && leader.second < length && !leader.first->isJammed()) {
1737 84 : if (v->getVehicleType().getGuiShape() == SUMOVehicleShape::AIRCRAFT) {
1738 : // aircraft wings and body are above walking level
1739 : continue;
1740 : }
1741 84 : const double gap = leader.second - length;
1742 168 : handleIntermodalCollisionBetween(timestep, stage, v, leader.first, gap, "sharedLane", toRemove, toTeleport);
1743 : }
1744 : }
1745 : }
1746 :
1747 86422312 : if (myVehicles.size() == 0) {
1748 : return;
1749 : }
1750 85829600 : if (!MSGlobals::gSublane) {
1751 : // no sublanes
1752 : VehCont::reverse_iterator lastVeh = myVehicles.rend() - 1;
1753 560958530 : for (VehCont::reverse_iterator pred = myVehicles.rbegin(); pred != lastVeh; ++pred) {
1754 : VehCont::reverse_iterator veh = pred + 1;
1755 494026220 : detectCollisionBetween(timestep, stage, *veh, *pred, toRemove, toTeleport);
1756 : }
1757 66932310 : if (myPartialVehicles.size() > 0) {
1758 5315908 : detectCollisionBetween(timestep, stage, *lastVeh, myPartialVehicles.front(), toRemove, toTeleport);
1759 : }
1760 66932310 : if (getBidiLane() != nullptr) {
1761 : // bidirectional railway
1762 384442 : MSLane* bidiLane = getBidiLane();
1763 384442 : if (bidiLane->getVehicleNumberWithPartials() > 0) {
1764 323887 : for (auto veh = myVehicles.begin(); veh != myVehicles.end(); ++veh) {
1765 222151 : double high = (*veh)->getPositionOnLane(this);
1766 222151 : double low = (*veh)->getBackPositionOnLane(this);
1767 222151 : if (stage == MSNet::STAGE_MOVEMENTS) {
1768 : // use previous back position to catch trains that
1769 : // "jump" through each other
1770 198760 : low -= SPEED2DIST((*veh)->getSpeed());
1771 : }
1772 935817 : for (AnyVehicleIterator veh2 = bidiLane->anyVehiclesBegin(); veh2 != bidiLane->anyVehiclesEnd(); ++veh2) {
1773 : // self-collisions might legitemately occur when a long train loops back on itself
1774 935817 : if (*veh == *veh2 && !isRailway((*veh)->getVClass())) {
1775 220865 : continue;
1776 : }
1777 860862 : if ((*veh)->getLane() == (*veh2)->getLane() ||
1778 825870 : (*veh)->getLane() == (*veh2)->getBackLane() ||
1779 110918 : (*veh)->getBackLane() == (*veh2)->getLane()) {
1780 : // vehicles are not in a bidi relation
1781 604034 : continue;
1782 : }
1783 110918 : double low2 = myLength - (*veh2)->getPositionOnLane(bidiLane);
1784 110918 : double high2 = myLength - (*veh2)->getBackPositionOnLane(bidiLane);
1785 110918 : if (stage == MSNet::STAGE_MOVEMENTS) {
1786 : // use previous back position to catch trains that
1787 : // "jump" through each other
1788 95492 : high2 += SPEED2DIST((*veh2)->getSpeed());
1789 : }
1790 110918 : if (!(high < low2 || high2 < low)) {
1791 : #ifdef DEBUG_COLLISIONS
1792 : if (DEBUG_COND) {
1793 : std::cout << SIMTIME << " bidi-collision veh=" << (*veh)->getID() << " bidiVeh=" << (*veh2)->getID()
1794 : << " vehFurther=" << toString((*veh)->getFurtherLanes())
1795 : << " high=" << high << " low=" << low << " high2=" << high2 << " low2=" << low2 << "\n";
1796 : }
1797 : #endif
1798 : // the faster vehicle is at fault
1799 41 : MSVehicle* collider = const_cast<MSVehicle*>(*veh);
1800 41 : MSVehicle* victim = const_cast<MSVehicle*>(*veh2);
1801 41 : if (collider->getSpeed() < victim->getSpeed()) {
1802 : std::swap(victim, collider);
1803 : }
1804 41 : handleCollisionBetween(timestep, stage, collider, victim, -1, 0, toRemove, toTeleport);
1805 : }
1806 : }
1807 : }
1808 : }
1809 : }
1810 : } else {
1811 : // in the sublane-case it is insufficient to check the vehicles ordered
1812 : // by their front position as there might be more than 2 vehicles next to each
1813 : // other on the same lane
1814 : // instead, a moving-window approach is used where all vehicles that
1815 : // overlap in the longitudinal direction receive pairwise checks
1816 : // XXX for efficiency, all lanes of an edge should be checked together
1817 : // (lanechanger-style)
1818 :
1819 : // XXX quick hack: check each in myVehicles against all others
1820 132052114 : for (AnyVehicleIterator veh = anyVehiclesBegin(); veh != anyVehiclesEnd(); ++veh) {
1821 132052114 : MSVehicle* follow = (MSVehicle*)*veh;
1822 3471001131 : for (AnyVehicleIterator veh2 = anyVehiclesBegin(); veh2 != anyVehiclesEnd(); ++veh2) {
1823 3471057950 : MSVehicle* lead = (MSVehicle*)*veh2;
1824 3471057950 : if (lead == follow) {
1825 132041881 : continue;
1826 : }
1827 3339016069 : if (lead->getPositionOnLane(this) < follow->getPositionOnLane(this)) {
1828 1668728873 : continue;
1829 : }
1830 1670287196 : if (detectCollisionBetween(timestep, stage, follow, lead, toRemove, toTeleport)) {
1831 : // XXX what about collisions with multiple leaders at once?
1832 : break;
1833 : }
1834 : }
1835 : }
1836 : }
1837 :
1838 :
1839 85833616 : for (std::set<const MSVehicle*, ComparatorNumericalIdLess>::iterator it = toRemove.begin(); it != toRemove.end(); ++it) {
1840 4016 : MSVehicle* veh = const_cast<MSVehicle*>(*it);
1841 : MSLane* vehLane = veh->getMutableLane();
1842 4016 : vehLane->removeVehicle(veh, MSMoveReminder::NOTIFICATION_TELEPORT, false);
1843 : if (toTeleport.count(veh) > 0) {
1844 3631 : MSVehicleTransfer::getInstance()->add(timestep, veh);
1845 : } else {
1846 385 : veh->onRemovalFromNet(MSMoveReminder::NOTIFICATION_VAPORIZED_COLLISION);
1847 385 : MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(veh);
1848 : }
1849 : }
1850 : }
1851 :
1852 :
1853 : void
1854 28282968 : MSLane::detectPedestrianJunctionCollision(const MSVehicle* collider, const PositionVector& colliderBoundary, const MSLane* foeLane,
1855 : SUMOTime timestep, const std::string& stage,
1856 : std::set<const MSVehicle*, ComparatorNumericalIdLess>& toRemove,
1857 : std::set<const MSVehicle*, ComparatorNumericalIdLess>& toTeleport) {
1858 28282968 : if (myIntermodalCollisionAction != COLLISION_ACTION_NONE && foeLane->getEdge().getPersons().size() > 0 && foeLane->hasPedestrians()) {
1859 : #ifdef DEBUG_PEDESTRIAN_COLLISIONS
1860 : if (DEBUG_COND) {
1861 : std::cout << SIMTIME << " detect pedestrian junction collisions stage=" << stage << " lane=" << getID() << " foeLane=" << foeLane->getID() << "\n";
1862 : }
1863 : #endif
1864 100021 : const std::vector<MSTransportable*>& persons = foeLane->getEdge().getSortedPersons(timestep);
1865 1194955 : for (std::vector<MSTransportable*>::const_iterator it_p = persons.begin(); it_p != persons.end(); ++it_p) {
1866 : #ifdef DEBUG_PEDESTRIAN_COLLISIONS
1867 : if (DEBUG_COND) {
1868 : std::cout << " collider=" << collider->getID()
1869 : << " ped=" << (*it_p)->getID()
1870 : << " jammed=" << (*it_p)->isJammed()
1871 : << " colliderBoundary=" << colliderBoundary
1872 : << " pedBoundary=" << (*it_p)->getBoundingBox()
1873 : << "\n";
1874 : }
1875 : #endif
1876 1094934 : if ((*it_p)->isJammed()) {
1877 720 : continue;
1878 : }
1879 2188428 : if (colliderBoundary.overlapsWith((*it_p)->getBoundingBox())
1880 1094214 : && collider->getBoundingPoly().overlapsWith((*it_p)->getBoundingBox())) {
1881 3894 : std::string collisionType = "junctionPedestrian";
1882 3894 : if (foeLane->getEdge().isCrossing()) {
1883 : collisionType = "crossing";
1884 3686 : } else if (foeLane->getEdge().isWalkingArea()) {
1885 : collisionType = "walkingarea";
1886 : }
1887 3894 : handleIntermodalCollisionBetween(timestep, stage, collider, *it_p, 0, collisionType, toRemove, toTeleport);
1888 : }
1889 : }
1890 100021 : }
1891 28282968 : }
1892 :
1893 :
1894 : bool
1895 2169629324 : MSLane::detectCollisionBetween(SUMOTime timestep, const std::string& stage, MSVehicle* collider, MSVehicle* victim,
1896 : std::set<const MSVehicle*, ComparatorNumericalIdLess>& toRemove,
1897 : std::set<const MSVehicle*, ComparatorNumericalIdLess>& toTeleport) const {
1898 4325601257 : if (myCollisionAction == COLLISION_ACTION_TELEPORT && ((victim->hasInfluencer() && victim->getInfluencer().isRemoteAffected(timestep)) ||
1899 2156229318 : (collider->hasInfluencer() && collider->getInfluencer().isRemoteAffected(timestep)))) {
1900 17 : return false;
1901 : }
1902 :
1903 : // No self-collisions! (This is assumed to be ensured at caller side)
1904 2169629307 : if (collider == victim) {
1905 : return false;
1906 : }
1907 :
1908 2169629244 : const bool colliderOpposite = collider->getLaneChangeModel().isOpposite() || collider->isBidiOn(this);
1909 2169629244 : const bool victimOpposite = victim->getLaneChangeModel().isOpposite() || victim->isBidiOn(this);
1910 2169629244 : const bool bothOpposite = victimOpposite && colliderOpposite;
1911 2169629244 : if (bothOpposite) {
1912 : std::swap(victim, collider);
1913 : }
1914 2169629244 : const double colliderPos = colliderOpposite && !bothOpposite ? collider->getBackPositionOnLane(this) : collider->getPositionOnLane(this);
1915 2169629244 : const double minGapFactor = myCollisionMinGapFactor >= 0 ? myCollisionMinGapFactor : collider->getCarFollowModel().getCollisionMinGapFactor();
1916 2169629244 : double victimBack = victimOpposite && !bothOpposite ? victim->getPositionOnLane(this) : victim->getBackPositionOnLane(this);
1917 2169629244 : if (victim->getLateralOverlap() > 0 || collider->getLateralOverlap() > 0) {
1918 110836493 : if (&collider->getLane()->getEdge() == myEdge && collider->getLane()->getLength() > getLength()) {
1919 : // interpret victim position on the longer lane
1920 728 : victimBack *= collider->getLane()->getLength() / getLength();
1921 : }
1922 : }
1923 2169629244 : double gap = victimBack - colliderPos - minGapFactor * collider->getVehicleType().getMinGap();
1924 2169629244 : if (bothOpposite) {
1925 1304962 : gap = colliderPos - victimBack - minGapFactor * collider->getVehicleType().getMinGap();
1926 2168324282 : } else if (colliderOpposite) {
1927 : // vehicles are back to back so (frontal) minGap doesn't apply
1928 3610980 : gap += minGapFactor * collider->getVehicleType().getMinGap();
1929 : }
1930 : #ifdef DEBUG_COLLISIONS
1931 : if (DEBUG_COND && (DEBUG_COND2(collider) || DEBUG_COND2(victim))) {
1932 : std::cout << SIMTIME
1933 : << " thisLane=" << getID()
1934 : << " collider=" << collider->getID()
1935 : << " victim=" << victim->getID()
1936 : << " colOpposite=" << colliderOpposite
1937 : << " vicOpposite=" << victimOpposite
1938 : << " colLane=" << collider->getLane()->getID()
1939 : << " vicLane=" << victim->getLane()->getID()
1940 : << " colPos=" << colliderPos
1941 : << " vicBack=" << victimBack
1942 : << " colLat=" << collider->getCenterOnEdge(this)
1943 : << " vicLat=" << victim->getCenterOnEdge(this)
1944 : << " minGap=" << collider->getVehicleType().getMinGap()
1945 : << " minGapFactor=" << minGapFactor
1946 : << " gap=" << gap
1947 : << "\n";
1948 : }
1949 : #endif
1950 2169629244 : if (victimOpposite && gap < -(collider->getLength() + victim->getLength())) {
1951 : // already past each other
1952 : return false;
1953 : }
1954 2169608641 : if (gap < -NUMERICAL_EPS) {
1955 : double latGap = 0;
1956 10468930 : if (MSGlobals::gSublane) {
1957 10463291 : latGap = (fabs(victim->getCenterOnEdge(this) - collider->getCenterOnEdge(this))
1958 10463291 : - 0.5 * fabs(victim->getVehicleType().getWidth() + collider->getVehicleType().getWidth()));
1959 10463291 : if (latGap + NUMERICAL_EPS > 0) {
1960 : return false;
1961 : }
1962 : // account for ambiguous gap computation related to partial
1963 : // occupation of lanes with different lengths
1964 56821 : if (isInternal() && getEdge().getNumLanes() > 1 && victim->getLane() != collider->getLane()) {
1965 : double gapDelta = 0;
1966 1640 : const MSVehicle* otherLaneVeh = collider->getLane() == this ? victim : collider;
1967 1640 : if (otherLaneVeh->getLaneChangeModel().getShadowLane() == this) {
1968 459 : gapDelta = getLength() - otherLaneVeh->getLane()->getLength();
1969 : } else {
1970 1181 : for (const MSLane* cand : otherLaneVeh->getFurtherLanes()) {
1971 1181 : if (&cand->getEdge() == &getEdge()) {
1972 1181 : gapDelta = getLength() - cand->getLength();
1973 1181 : break;
1974 : }
1975 : }
1976 : }
1977 1640 : if (gap + gapDelta >= 0) {
1978 : return false;
1979 : }
1980 : }
1981 : }
1982 62458 : if (MSGlobals::gLaneChangeDuration > DELTA_T
1983 37 : && collider->getLaneChangeModel().isChangingLanes()
1984 31 : && victim->getLaneChangeModel().isChangingLanes()
1985 62458 : && victim->getLane() != this) {
1986 : // synchroneous lane change maneuver
1987 : return false;
1988 : }
1989 : #ifdef DEBUG_COLLISIONS
1990 : if (DEBUG_COND && (DEBUG_COND2(collider) || DEBUG_COND2(victim))) {
1991 : std::cout << SIMTIME << " detectedCollision gap=" << gap << " latGap=" << latGap << "\n";
1992 : }
1993 : #endif
1994 62458 : handleCollisionBetween(timestep, stage, collider, victim, gap, latGap, toRemove, toTeleport);
1995 62458 : return true;
1996 : }
1997 : return false;
1998 : }
1999 :
2000 :
2001 : void
2002 75079 : MSLane::handleCollisionBetween(SUMOTime timestep, const std::string& stage, const MSVehicle* collider, const MSVehicle* victim,
2003 : double gap, double latGap, std::set<const MSVehicle*, ComparatorNumericalIdLess>& toRemove,
2004 : std::set<const MSVehicle*, ComparatorNumericalIdLess>& toTeleport) const {
2005 75079 : if (collider->ignoreCollision() || victim->ignoreCollision()) {
2006 65105 : return;
2007 : }
2008 72829 : const std::string collisionType = ((collider->getLaneChangeModel().isOpposite() != victim->getLaneChangeModel().isOpposite()
2009 72806 : || (&collider->getLane()->getEdge() == victim->getLane()->getEdge().getBidiEdge()))
2010 202903 : ? "frontal" : (isInternal() ? "junction" : "collision"));
2011 32 : const std::string collisionText = collisionType == "frontal" ? TL("frontal collision") :
2012 72861 : (collisionType == "junction" ? TL("junction collision") : TL("collision"));
2013 :
2014 : // in frontal collisions the opposite vehicle is the collider
2015 72829 : if (victim->getLaneChangeModel().isOpposite() && !collider->getLaneChangeModel().isOpposite()) {
2016 : std::swap(collider, victim);
2017 : }
2018 291316 : std::string prefix = TLF("Vehicle '%'; % with vehicle '%", collider->getID(), collisionText, victim->getID());
2019 72829 : if (myCollisionStopTime > 0) {
2020 64692 : if (collider->collisionStopTime() >= 0 && victim->collisionStopTime() >= 0) {
2021 62855 : return;
2022 : }
2023 : std::string dummyError;
2024 1837 : SUMOVehicleParameter::Stop stop;
2025 1837 : stop.duration = myCollisionStopTime;
2026 1837 : stop.parametersSet |= STOP_DURATION_SET;
2027 1837 : const double collisionAngle = RAD2DEG(fabs(GeomHelper::angleDiff(victim->getAngle(), collider->getAngle())));
2028 : // determine new speeds from collision angle (@todo account for vehicle mass)
2029 1837 : double victimSpeed = victim->getSpeed();
2030 1837 : double colliderSpeed = collider->getSpeed();
2031 : // double victimOrigSpeed = victim->getSpeed();
2032 : // double colliderOrigSpeed = collider->getSpeed();
2033 1837 : if (collisionAngle < 45) {
2034 : // rear-end collisions
2035 : colliderSpeed = MIN2(colliderSpeed, victimSpeed);
2036 261 : } else if (collisionAngle < 135) {
2037 : // side collision
2038 257 : colliderSpeed /= 2;
2039 257 : victimSpeed /= 2;
2040 : } else {
2041 : // frontal collision
2042 : colliderSpeed = 0;
2043 : victimSpeed = 0;
2044 : }
2045 1837 : const double victimStopPos = MIN2(victim->getLane()->getLength(),
2046 1837 : victim->getPositionOnLane() + victim->getCarFollowModel().brakeGap(victimSpeed, victim->getCarFollowModel().getEmergencyDecel(), 0));
2047 1837 : if (victim->collisionStopTime() < 0) {
2048 1307 : stop.collision = true;
2049 1307 : stop.lane = victim->getLane()->getID();
2050 : // @todo: push victim forward?
2051 1307 : stop.startPos = victimStopPos;
2052 1307 : stop.endPos = stop.startPos;
2053 1307 : stop.parametersSet |= STOP_START_SET | STOP_END_SET;
2054 1307 : ((MSBaseVehicle*)victim)->addStop(stop, dummyError, 0);
2055 : }
2056 1837 : if (collider->collisionStopTime() < 0) {
2057 1418 : stop.collision = true;
2058 1418 : stop.lane = collider->getLane()->getID();
2059 1418 : stop.startPos = MIN2(collider->getPositionOnLane() + collider->getCarFollowModel().brakeGap(colliderSpeed, collider->getCarFollowModel().getEmergencyDecel(), 0),
2060 1418 : MAX3(0.0, victimStopPos - 0.75 * victim->getVehicleType().getLength(),
2061 1418 : collider->getPositionOnLane() - SPEED2DIST(collider->getSpeed())));
2062 1418 : stop.endPos = stop.startPos;
2063 1418 : stop.parametersSet |= STOP_START_SET | STOP_END_SET;
2064 1418 : ((MSBaseVehicle*)collider)->addStop(stop, dummyError, 0);
2065 : }
2066 : //std::cout << " collisionAngle=" << collisionAngle
2067 : // << "\n vPos=" << victim->getPositionOnLane() << " vStop=" << victimStopPos << " vSpeed=" << victimOrigSpeed << " vSpeed2=" << victimSpeed << " vSpeed3=" << victim->getSpeed()
2068 : // << "\n cPos=" << collider->getPositionOnLane() << " cStop=" << stop.startPos << " cSpeed=" << colliderOrigSpeed << " cSpeed2=" << colliderSpeed << " cSpeed3=" << collider->getSpeed()
2069 : // << "\n";
2070 1837 : } else {
2071 8137 : switch (myCollisionAction) {
2072 : case COLLISION_ACTION_WARN:
2073 : break;
2074 3618 : case COLLISION_ACTION_TELEPORT:
2075 7236 : prefix = TLF("Teleporting vehicle '%'; % with vehicle '%", collider->getID(), collisionText, victim->getID());
2076 : toRemove.insert(collider);
2077 : toTeleport.insert(collider);
2078 : break;
2079 210 : case COLLISION_ACTION_REMOVE: {
2080 420 : prefix = TLF("Removing % participants: vehicle '%', vehicle '%", collisionText, collider->getID(), victim->getID());
2081 : bool removeCollider = true;
2082 : bool removeVictim = true;
2083 210 : removeVictim = !(victim->hasInfluencer() && victim->getInfluencer()->isRemoteAffected(timestep));
2084 210 : removeCollider = !(collider->hasInfluencer() && collider->getInfluencer()->isRemoteAffected(timestep));
2085 210 : if (removeVictim) {
2086 : toRemove.insert(victim);
2087 : }
2088 210 : if (removeCollider) {
2089 : toRemove.insert(collider);
2090 : }
2091 210 : if (!removeVictim) {
2092 0 : if (!removeCollider) {
2093 0 : prefix = TLF("Keeping remote-controlled % participants: vehicle '%', vehicle '%", collisionText, collider->getID(), victim->getID());
2094 : } else {
2095 0 : prefix = TLF("Removing % participant: vehicle '%', keeping remote-controlled vehicle '%", collisionText, collider->getID(), victim->getID());
2096 : }
2097 210 : } else if (!removeCollider) {
2098 0 : prefix = TLF("Keeping remote-controlled % participant: vehicle '%', removing vehicle '%", collisionText, collider->getID(), victim->getID());
2099 : }
2100 : break;
2101 : }
2102 : default:
2103 : break;
2104 : }
2105 : }
2106 9974 : const bool newCollision = MSNet::getInstance()->registerCollision(collider, victim, collisionType, this, collider->getPositionOnLane(this));
2107 9974 : if (newCollision) {
2108 31046 : WRITE_WARNINGF(prefix + "', lane='%', gap=%%, time=%, stage=%.",
2109 : getID(), toString(gap), (MSGlobals::gSublane ? TL(", latGap=") + toString(latGap) : ""),
2110 : time2string(timestep), stage);
2111 6239 : MSNet::getInstance()->informVehicleStateListener(victim, MSNet::VehicleState::COLLISION);
2112 6239 : MSNet::getInstance()->informVehicleStateListener(collider, MSNet::VehicleState::COLLISION);
2113 6239 : MSNet::getInstance()->getVehicleControl().registerCollision(myCollisionAction == COLLISION_ACTION_TELEPORT);
2114 : }
2115 : #ifdef DEBUG_COLLISIONS
2116 : if (DEBUG_COND2(collider)) {
2117 : toRemove.erase(collider);
2118 : toTeleport.erase(collider);
2119 : }
2120 : if (DEBUG_COND2(victim)) {
2121 : toRemove.erase(victim);
2122 : toTeleport.erase(victim);
2123 : }
2124 : #endif
2125 : }
2126 :
2127 :
2128 : void
2129 3978 : MSLane::handleIntermodalCollisionBetween(SUMOTime timestep, const std::string& stage, const MSVehicle* collider, const MSTransportable* victim,
2130 : double gap, const std::string& collisionType,
2131 : std::set<const MSVehicle*, ComparatorNumericalIdLess>& toRemove,
2132 : std::set<const MSVehicle*, ComparatorNumericalIdLess>& toTeleport) const {
2133 3978 : if (collider->ignoreCollision()) {
2134 3304 : return;
2135 : }
2136 7956 : std::string prefix = TLF("Vehicle '%'", collider->getID());
2137 3978 : if (myIntermodalCollisionStopTime > 0) {
2138 3344 : if (collider->collisionStopTime() >= 0) {
2139 3304 : return;
2140 : }
2141 : std::string dummyError;
2142 40 : SUMOVehicleParameter::Stop stop;
2143 40 : stop.duration = myIntermodalCollisionStopTime;
2144 40 : stop.parametersSet |= STOP_DURATION_SET;
2145 : // determine new speeds from collision angle (@todo account for vehicle mass)
2146 40 : double colliderSpeed = collider->getSpeed();
2147 40 : const double victimStopPos = victim->getEdgePos();
2148 : // double victimOrigSpeed = victim->getSpeed();
2149 : // double colliderOrigSpeed = collider->getSpeed();
2150 40 : if (collider->collisionStopTime() < 0) {
2151 40 : stop.collision = true;
2152 40 : stop.lane = collider->getLane()->getID();
2153 40 : stop.startPos = MIN2(collider->getPositionOnLane() + collider->getCarFollowModel().brakeGap(colliderSpeed, collider->getCarFollowModel().getEmergencyDecel(), 0),
2154 40 : MAX3(0.0, victimStopPos - 0.75 * victim->getVehicleType().getLength(),
2155 40 : collider->getPositionOnLane() - SPEED2DIST(collider->getSpeed())));
2156 40 : stop.endPos = stop.startPos;
2157 40 : stop.parametersSet |= STOP_START_SET | STOP_END_SET;
2158 40 : ((MSBaseVehicle*)collider)->addStop(stop, dummyError, 0);
2159 : }
2160 40 : } else {
2161 634 : switch (myIntermodalCollisionAction) {
2162 : case COLLISION_ACTION_WARN:
2163 : break;
2164 15 : case COLLISION_ACTION_TELEPORT:
2165 30 : prefix = TLF("Teleporting vehicle '%' after", collider->getID());
2166 : toRemove.insert(collider);
2167 : toTeleport.insert(collider);
2168 : break;
2169 15 : case COLLISION_ACTION_REMOVE: {
2170 30 : prefix = TLF("Removing vehicle '%' after", collider->getID());
2171 : bool removeCollider = true;
2172 15 : removeCollider = !(collider->hasInfluencer() && collider->getInfluencer()->isRemoteAffected(timestep));
2173 : if (!removeCollider) {
2174 0 : prefix = TLF("Keeping remote-controlled vehicle '%' after", collider->getID());
2175 : } else {
2176 : toRemove.insert(collider);
2177 : }
2178 : break;
2179 : }
2180 : default:
2181 : break;
2182 : }
2183 : }
2184 674 : const bool newCollision = MSNet::getInstance()->registerCollision(collider, victim, collisionType, this, victim->getEdgePos());
2185 674 : if (newCollision) {
2186 270 : if (gap != 0) {
2187 420 : WRITE_WARNING(prefix + TLF(" collision with person '%', lane='%', gap=%, time=%, stage=%.",
2188 : victim->getID(), getID(), gap, time2string(timestep), stage));
2189 : } else {
2190 930 : WRITE_WARNING(prefix + TLF(" collision with person '%', lane='%', time=%, stage=%.",
2191 : victim->getID(), getID(), time2string(timestep), stage));
2192 : }
2193 270 : MSNet::getInstance()->informVehicleStateListener(collider, MSNet::VehicleState::COLLISION);
2194 270 : MSNet::getInstance()->getVehicleControl().registerCollision(myIntermodalCollisionAction == COLLISION_ACTION_TELEPORT);
2195 : }
2196 : #ifdef DEBUG_COLLISIONS
2197 : if (DEBUG_COND2(collider)) {
2198 : toRemove.erase(collider);
2199 : toTeleport.erase(collider);
2200 : }
2201 : #endif
2202 : }
2203 :
2204 :
2205 : void
2206 79518583 : MSLane::executeMovements(const SUMOTime t) {
2207 : // multithreading: there are concurrent writes to myNeedsCollisionCheck but all of them should set it to true
2208 79518583 : myNeedsCollisionCheck = true;
2209 79518583 : MSLane* bidi = getBidiLane();
2210 79518583 : if (bidi != nullptr && bidi->getVehicleNumber() == 0) {
2211 398916 : MSNet::getInstance()->getEdgeControl().checkCollisionForInactive(bidi);
2212 : }
2213 79518583 : MSVehicle* firstNotStopped = nullptr;
2214 : // iterate over vehicles in reverse so that move reminders will be called in the correct order
2215 702762573 : for (VehCont::reverse_iterator i = myVehicles.rbegin(); i != myVehicles.rend();) {
2216 623243997 : MSVehicle* veh = *i;
2217 : // length is needed later when the vehicle may not exist anymore
2218 623243997 : const double length = veh->getVehicleType().getLengthWithGap();
2219 623243997 : const double nettoLength = veh->getVehicleType().getLength();
2220 623243997 : const bool moved = veh->executeMove();
2221 : MSLane* const target = veh->getMutableLane();
2222 623243990 : if (veh->hasArrived()) {
2223 : // vehicle has reached its arrival position
2224 : #ifdef DEBUG_EXEC_MOVE
2225 : if DEBUG_COND2(veh) {
2226 : std::cout << SIMTIME << " veh " << veh->getID() << " has arrived." << std::endl;
2227 : }
2228 : #endif
2229 2736119 : veh->onRemovalFromNet(MSMoveReminder::NOTIFICATION_ARRIVED);
2230 2736119 : MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(veh);
2231 620507871 : } else if (target != nullptr && moved) {
2232 14365273 : if (target->getEdge().isVaporizing()) {
2233 : // vehicle has reached a vaporizing edge
2234 756 : veh->onRemovalFromNet(MSMoveReminder::NOTIFICATION_VAPORIZED_VAPORIZER);
2235 756 : MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(veh);
2236 : } else {
2237 : // vehicle has entered a new lane (leaveLane and workOnMoveReminders were already called in MSVehicle::executeMove)
2238 14364517 : target->myVehBuffer.push_back(veh);
2239 14364517 : MSNet::getInstance()->getEdgeControl().needsVehicleIntegration(target);
2240 14364517 : if (MSGlobals::gSublane && veh->getLaneChangeModel().getShadowLane() != nullptr) {
2241 : // trigger sorting of partial vehicles as their order may have changed (lane might not be active and only contain partial vehicles)
2242 61310 : MSNet::getInstance()->getEdgeControl().needsVehicleIntegration(veh->getLaneChangeModel().getShadowLane());
2243 : }
2244 : }
2245 606142598 : } else if (veh->isParking()) {
2246 : // vehicle started to park
2247 12477 : MSVehicleTransfer::getInstance()->add(t, veh);
2248 12477 : myParkingVehicles.insert(veh);
2249 606130121 : } else if (veh->brokeDown()) {
2250 8 : veh->resumeFromStopping();
2251 24 : WRITE_WARNINGF(TL("Removing vehicle '%' after breaking down, lane='%', time=%."),
2252 : veh->getID(), veh->getLane()->getID(), time2string(t));
2253 8 : veh->onRemovalFromNet(MSMoveReminder::NOTIFICATION_VAPORIZED_BREAKDOWN);
2254 8 : MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(veh);
2255 606130113 : } else if (veh->isJumping()) {
2256 : // vehicle jumps to next route edge
2257 423 : MSVehicleTransfer::getInstance()->add(t, veh);
2258 606129690 : } else if (veh->getPositionOnLane() > getLength()) {
2259 : // for any reasons the vehicle is beyond its lane...
2260 : // this should never happen because it is handled in MSVehicle::executeMove
2261 : assert(false);
2262 0 : WRITE_WARNINGF(TL("Teleporting vehicle '%'; beyond end of lane, target lane='%', time=%."),
2263 : veh->getID(), getID(), time2string(t));
2264 0 : MSNet::getInstance()->getVehicleControl().registerCollision(true);
2265 0 : MSVehicleTransfer::getInstance()->add(t, veh);
2266 :
2267 606129690 : } else if (veh->collisionStopTime() == 0) {
2268 2942 : veh->resumeFromStopping();
2269 2942 : if (getCollisionAction() == COLLISION_ACTION_REMOVE) {
2270 531 : WRITE_WARNINGF(TL("Removing vehicle '%' after earlier collision, lane='%', time=%."),
2271 : veh->getID(), veh->getLane()->getID(), time2string(t));
2272 177 : veh->onRemovalFromNet(MSMoveReminder::NOTIFICATION_VAPORIZED_COLLISION);
2273 177 : MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(veh);
2274 2765 : } else if (getCollisionAction() == COLLISION_ACTION_TELEPORT) {
2275 6156 : WRITE_WARNINGF(TL("Teleporting vehicle '%' after earlier collision, lane='%', time=%."),
2276 : veh->getID(), veh->getLane()->getID(), time2string(t));
2277 2052 : MSVehicleTransfer::getInstance()->add(t, veh);
2278 : } else {
2279 713 : if (firstNotStopped == nullptr && !(*i)->isStopped() && (*i)->getLane() == this) {
2280 450 : firstNotStopped = *i;
2281 : }
2282 : ++i;
2283 713 : continue;
2284 : }
2285 : } else {
2286 606126748 : if (firstNotStopped == nullptr && !(*i)->isStopped() && (*i)->getLane() == this) {
2287 69316731 : firstNotStopped = *i;
2288 : }
2289 : ++i;
2290 606126748 : continue;
2291 : }
2292 17116529 : myBruttoVehicleLengthSumToRemove += length;
2293 17116529 : myNettoVehicleLengthSumToRemove += nettoLength;
2294 : ++i;
2295 17116529 : i = VehCont::reverse_iterator(myVehicles.erase(i.base()));
2296 : }
2297 79518576 : if (firstNotStopped != nullptr) {
2298 69317181 : const SUMOTime ttt = firstNotStopped->getVehicleType().getParameter().getTimeToTeleport(MSGlobals::gTimeToGridlock);
2299 69317181 : const SUMOTime tttb = firstNotStopped->getVehicleType().getParameter().getTimeToTeleportBidi(MSGlobals::gTimeToTeleportBidi);
2300 69317181 : if (ttt > 0 || MSGlobals::gTimeToGridlockHighways > 0 || MSGlobals::gTimeToTeleportDisconnected >= 0 || tttb > 0 || MSGlobals::gTimeToTeleportRSDeadlock > 0) {
2301 66286538 : const bool wrongLane = !appropriate(firstNotStopped);
2302 66286538 : const bool disconnected = (MSGlobals::gTimeToTeleportDisconnected >= 0
2303 17978 : && firstNotStopped->succEdge(1) != nullptr
2304 66303562 : && firstNotStopped->getEdge()->allowedLanes(*firstNotStopped->succEdge(1), firstNotStopped->getVClass()) == nullptr);
2305 :
2306 66286538 : const bool r1 = ttt > 0 && firstNotStopped->getWaitingTime() > ttt && !disconnected;
2307 66283275 : const bool r2 = !r1 && MSGlobals::gTimeToGridlockHighways > 0
2308 1025 : && firstNotStopped->getWaitingTime() > MSGlobals::gTimeToGridlockHighways
2309 602 : && getSpeedLimit() > MSGlobals::gGridlockHighwaysSpeed && wrongLane
2310 602 : && !disconnected;
2311 66286526 : const bool r3 = disconnected && firstNotStopped->getWaitingTime() > MSGlobals::gTimeToTeleportDisconnected;
2312 66283263 : const bool r4 = !r1 && !r2 && !r3 && tttb > 0
2313 66286982 : && firstNotStopped->getWaitingTime() > tttb && getBidiLane() && !disconnected;
2314 301592 : const bool r5 = MSGlobals::gTimeToTeleportRSDeadlock > 0 && MSRailSignalControl::hasInstance() && !r1 && !r2 && !r3 && !r4
2315 66554824 : && firstNotStopped->getWaitingTime() > MSGlobals::gTimeToTeleportRSDeadlock && MSRailSignalControl::getInstance().haveDeadlock(firstNotStopped);
2316 66286538 : if (r1 || r2 || r3 || r4 || r5) {
2317 3365 : const std::vector<MSLink*>::const_iterator link = succLinkSec(*firstNotStopped, 1, *this, firstNotStopped->getBestLanesContinuation());
2318 3365 : const bool minorLink = !wrongLane && (link != myLinks.end()) && !((*link)->havePriority());
2319 4733 : std::string reason = (wrongLane ? " (wrong lane" : (minorLink ? " (yield" : " (jam"));
2320 3365 : myBruttoVehicleLengthSumToRemove += firstNotStopped->getVehicleType().getLengthWithGap();
2321 3365 : myNettoVehicleLengthSumToRemove += firstNotStopped->getVehicleType().getLength();
2322 3365 : if (firstNotStopped == myVehicles.back()) {
2323 : myVehicles.pop_back();
2324 : } else {
2325 250 : myVehicles.erase(std::find(myVehicles.begin(), myVehicles.end(), firstNotStopped));
2326 : reason = " (blocked";
2327 : }
2328 30183 : WRITE_WARNINGF("Teleporting vehicle '%'; waited too long" + reason
2329 : + (r2 ? ", highway" : "")
2330 : + (r3 ? ", disconnected" : "")
2331 : + (r4 ? ", bidi" : "")
2332 : + (r5 ? ", railSignal" : "")
2333 : + "), lane='%', time=%.", firstNotStopped->getID(), getID(), time2string(t));
2334 3365 : if (wrongLane) {
2335 626 : MSNet::getInstance()->getVehicleControl().registerTeleportWrongLane();
2336 2739 : } else if (minorLink) {
2337 1371 : MSNet::getInstance()->getVehicleControl().registerTeleportYield();
2338 : } else {
2339 1368 : MSNet::getInstance()->getVehicleControl().registerTeleportJam();
2340 : }
2341 3365 : if (MSGlobals::gRemoveGridlocked) {
2342 14 : firstNotStopped->onRemovalFromNet(MSMoveReminder::NOTIFICATION_TELEPORT_ARRIVED);
2343 14 : MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(firstNotStopped);
2344 : } else {
2345 3351 : MSVehicleTransfer::getInstance()->add(t, firstNotStopped);
2346 : }
2347 : }
2348 : }
2349 : }
2350 79518576 : if (MSGlobals::gSublane) {
2351 : // trigger sorting of vehicles as their order may have changed
2352 15349673 : MSNet::getInstance()->getEdgeControl().needsVehicleIntegration(this);
2353 : }
2354 79518576 : }
2355 :
2356 :
2357 : void
2358 234 : MSLane::markRecalculateBruttoSum() {
2359 234 : myRecalculateBruttoSum = true;
2360 234 : }
2361 :
2362 :
2363 : void
2364 79518578 : MSLane::updateLengthSum() {
2365 79518578 : myBruttoVehicleLengthSum -= myBruttoVehicleLengthSumToRemove;
2366 79518578 : myNettoVehicleLengthSum -= myNettoVehicleLengthSumToRemove;
2367 79518578 : myBruttoVehicleLengthSumToRemove = 0;
2368 79518578 : myNettoVehicleLengthSumToRemove = 0;
2369 79518578 : if (myVehicles.empty()) {
2370 : // avoid numerical instability
2371 6428610 : myBruttoVehicleLengthSum = 0;
2372 6428610 : myNettoVehicleLengthSum = 0;
2373 73089968 : } else if (myRecalculateBruttoSum) {
2374 177 : myBruttoVehicleLengthSum = 0;
2375 651 : for (VehCont::const_iterator i = myVehicles.begin(); i != myVehicles.end(); ++i) {
2376 474 : myBruttoVehicleLengthSum += (*i)->getVehicleType().getLengthWithGap();
2377 : }
2378 177 : myRecalculateBruttoSum = false;
2379 : }
2380 79518578 : }
2381 :
2382 :
2383 : void
2384 0 : MSLane::changeLanes(const SUMOTime t) {
2385 0 : myEdge->changeLanes(t);
2386 0 : }
2387 :
2388 :
2389 : const MSEdge*
2390 4164364 : MSLane::getNextNormal() const {
2391 4164364 : return myEdge->getNormalSuccessor();
2392 : }
2393 :
2394 :
2395 : const MSLane*
2396 11804 : MSLane::getFirstInternalInConnection(double& offset) const {
2397 11804 : if (!this->isInternal()) {
2398 : return nullptr;
2399 : }
2400 11804 : offset = 0.;
2401 : const MSLane* firstInternal = this;
2402 11804 : MSLane* pred = getCanonicalPredecessorLane();
2403 11812 : while (pred != nullptr && pred->isInternal()) {
2404 : firstInternal = pred;
2405 8 : offset += pred->getLength();
2406 8 : pred = firstInternal->getCanonicalPredecessorLane();
2407 : }
2408 : return firstInternal;
2409 : }
2410 :
2411 :
2412 : // ------ Static (sic!) container methods ------
2413 : bool
2414 2160062 : MSLane::dictionary(const std::string& id, MSLane* ptr) {
2415 : const DictType::iterator it = myDict.lower_bound(id);
2416 2160062 : if (it == myDict.end() || it->first != id) {
2417 : // id not in myDict
2418 2160054 : myDict.emplace_hint(it, id, ptr);
2419 2160054 : return true;
2420 : }
2421 : return false;
2422 : }
2423 :
2424 :
2425 : MSLane*
2426 8905251 : MSLane::dictionary(const std::string& id) {
2427 : const DictType::iterator it = myDict.find(id);
2428 8905251 : if (it == myDict.end()) {
2429 : // id not in myDict
2430 : return nullptr;
2431 : }
2432 8903670 : return it->second;
2433 : }
2434 :
2435 :
2436 : void
2437 40327 : MSLane::clear() {
2438 2032292 : for (DictType::iterator i = myDict.begin(); i != myDict.end(); ++i) {
2439 1991965 : delete (*i).second;
2440 : }
2441 : myDict.clear();
2442 40327 : }
2443 :
2444 :
2445 : void
2446 226 : MSLane::insertIDs(std::vector<std::string>& into) {
2447 7716 : for (DictType::iterator i = myDict.begin(); i != myDict.end(); ++i) {
2448 7490 : into.push_back((*i).first);
2449 : }
2450 226 : }
2451 :
2452 :
2453 : template<class RTREE> void
2454 530 : MSLane::fill(RTREE& into) {
2455 46196 : for (DictType::iterator i = myDict.begin(); i != myDict.end(); ++i) {
2456 22833 : MSLane* l = (*i).second;
2457 22833 : Boundary b = l->getShape().getBoxBoundary();
2458 22833 : b.grow(3.);
2459 22833 : const float cmin[2] = {(float) b.xmin(), (float) b.ymin()};
2460 22833 : const float cmax[2] = {(float) b.xmax(), (float) b.ymax()};
2461 22833 : into.Insert(cmin, cmax, l);
2462 : }
2463 530 : }
2464 :
2465 : template void MSLane::fill<NamedRTree>(NamedRTree& into);
2466 : template void MSLane::fill<LANE_RTREE_QUAL>(LANE_RTREE_QUAL& into);
2467 :
2468 : // ------ ------
2469 : bool
2470 66286538 : MSLane::appropriate(const MSVehicle* veh) const {
2471 66286538 : if (veh->getLaneChangeModel().isOpposite()) {
2472 : return false;
2473 : }
2474 66177100 : if (myEdge->isInternal()) {
2475 : return true;
2476 : }
2477 62184784 : if (veh->succEdge(1) == nullptr) {
2478 : assert((int)veh->getBestLanes().size() > veh->getLaneIndex());
2479 16641117 : if (veh->getBestLanes()[veh->getLaneIndex()].bestLaneOffset == 0) {
2480 : return true;
2481 : } else {
2482 : return false;
2483 : }
2484 : }
2485 45543667 : std::vector<MSLink*>::const_iterator link = succLinkSec(*veh, 1, *this, veh->getBestLanesContinuation());
2486 : return (link != myLinks.end());
2487 : }
2488 :
2489 :
2490 : void
2491 29775500 : MSLane::integrateNewVehicles() {
2492 29775500 : myNeedsCollisionCheck = true;
2493 : std::vector<MSVehicle*>& buffered = myVehBuffer.getContainer();
2494 29775500 : sort(buffered.begin(), buffered.end(), vehicle_position_sorter(this));
2495 44140017 : for (MSVehicle* const veh : buffered) {
2496 : assert(veh->getLane() == this);
2497 14364517 : myVehicles.insert(myVehicles.begin(), veh);
2498 14364517 : myBruttoVehicleLengthSum += veh->getVehicleType().getLengthWithGap();
2499 14364517 : myNettoVehicleLengthSum += veh->getVehicleType().getLength();
2500 : //if (true) std::cout << SIMTIME << " integrateNewVehicle lane=" << getID() << " veh=" << veh->getID() << " (on lane " << veh->getLane()->getID() << ") into lane=" << getID() << " myBrutto=" << myBruttoVehicleLengthSum << "\n";
2501 14364517 : myEdge->markDelayed();
2502 : }
2503 : buffered.clear();
2504 : myVehBuffer.unlock();
2505 : //std::cout << SIMTIME << " integrateNewVehicle lane=" << getID() << " myVehicles1=" << toString(myVehicles);
2506 29775500 : if (MSGlobals::gLateralResolution > 0 || myOpposite != nullptr) {
2507 17800234 : sort(myVehicles.begin(), myVehicles.end(), vehicle_natural_position_sorter(this));
2508 : }
2509 29775500 : sortPartialVehicles();
2510 : #ifdef DEBUG_VEHICLE_CONTAINER
2511 : if (DEBUG_COND) std::cout << SIMTIME << " integrateNewVehicle lane=" << getID()
2512 : << " vehicles=" << toString(myVehicles) << " partials=" << toString(myPartialVehicles) << "\n";
2513 : #endif
2514 29775500 : }
2515 :
2516 :
2517 : void
2518 100394194 : MSLane::sortPartialVehicles() {
2519 100394194 : if (myPartialVehicles.size() > 1) {
2520 1383897 : sort(myPartialVehicles.begin(), myPartialVehicles.end(), vehicle_natural_position_sorter(this));
2521 : }
2522 100394194 : }
2523 :
2524 :
2525 : void
2526 18044471 : MSLane::sortManeuverReservations() {
2527 18044471 : if (myManeuverReservations.size() > 1) {
2528 : #ifdef DEBUG_CONTEXT
2529 : if (DEBUG_COND) {
2530 : std::cout << "sortManeuverReservations on lane " << getID()
2531 : << "\nBefore sort: " << toString(myManeuverReservations) << std::endl;
2532 : }
2533 : #endif
2534 18059 : sort(myManeuverReservations.begin(), myManeuverReservations.end(), vehicle_natural_position_sorter(this));
2535 : #ifdef DEBUG_CONTEXT
2536 : if (DEBUG_COND) {
2537 : std::cout << "After sort: " << toString(myManeuverReservations) << std::endl;
2538 : }
2539 : #endif
2540 : }
2541 18044471 : }
2542 :
2543 :
2544 : bool
2545 3736164071 : MSLane::isInternal() const {
2546 3736164071 : return myEdge->isInternal();
2547 : }
2548 :
2549 :
2550 : bool
2551 80353847 : MSLane::isNormal() const {
2552 80353847 : return myEdge->isNormal();
2553 : }
2554 :
2555 :
2556 : bool
2557 5344847 : MSLane::isCrossing() const {
2558 5344847 : return myEdge->isCrossing();
2559 : }
2560 :
2561 : bool
2562 1721 : MSLane::isWalkingArea() const {
2563 1721 : return myEdge->isWalkingArea();
2564 : }
2565 :
2566 :
2567 : MSVehicle*
2568 137744092 : MSLane::getLastFullVehicle() const {
2569 137744092 : if (myVehicles.size() == 0) {
2570 : return nullptr;
2571 : }
2572 132766283 : return myVehicles.front();
2573 : }
2574 :
2575 :
2576 : MSVehicle*
2577 77343 : MSLane::getFirstFullVehicle() const {
2578 77343 : if (myVehicles.size() == 0) {
2579 : return nullptr;
2580 : }
2581 28969 : return myVehicles.back();
2582 : }
2583 :
2584 :
2585 : MSVehicle*
2586 370229367 : MSLane::getLastAnyVehicle() const {
2587 : // all vehicles in myVehicles should have positions smaller or equal to
2588 : // those in myPartialVehicles (unless we're on a bidi-lane)
2589 370229367 : if (myVehicles.size() > 0) {
2590 295858192 : if (myBidiLane != nullptr && myPartialVehicles.size() > 0) {
2591 27490 : if (myVehicles.front()->getPositionOnLane() > myPartialVehicles.front()->getPositionOnLane(this)) {
2592 10466 : return myPartialVehicles.front();
2593 : }
2594 : }
2595 295847726 : return myVehicles.front();
2596 : }
2597 74371175 : if (myPartialVehicles.size() > 0) {
2598 4290633 : return myPartialVehicles.front();
2599 : }
2600 : return nullptr;
2601 : }
2602 :
2603 :
2604 : MSVehicle*
2605 135347 : MSLane::getFirstAnyVehicle() const {
2606 : MSVehicle* result = nullptr;
2607 135347 : if (myVehicles.size() > 0) {
2608 135347 : result = myVehicles.back();
2609 : }
2610 : if (myPartialVehicles.size() > 0
2611 135347 : && (result == nullptr || result->getPositionOnLane(this) < myPartialVehicles.back()->getPositionOnLane(this))) {
2612 129 : result = myPartialVehicles.back();
2613 : }
2614 135347 : return result;
2615 : }
2616 :
2617 :
2618 : std::vector<MSLink*>::const_iterator
2619 1776490422 : MSLane::succLinkSec(const SUMOVehicle& veh, int nRouteSuccs,
2620 : const MSLane& succLinkSource, const std::vector<MSLane*>& conts) {
2621 1776490422 : const MSEdge* nRouteEdge = veh.succEdge(nRouteSuccs);
2622 : // check whether the vehicle tried to look beyond its route
2623 1776490422 : if (nRouteEdge == nullptr) {
2624 : // return end (no succeeding link) if so
2625 : return succLinkSource.myLinks.end();
2626 : }
2627 : // if we are on an internal lane there should only be one link and it must be allowed
2628 1264870686 : if (succLinkSource.isInternal()) {
2629 : assert(succLinkSource.myLinks.size() == 1);
2630 : // could have been disallowed dynamically with a rerouter or via TraCI
2631 : // assert(succLinkSource.myLinks[0]->getLane()->allowsVehicleClass(veh.getVehicleType().getVehicleClass()));
2632 : return succLinkSource.myLinks.begin();
2633 : }
2634 : // a link may be used if
2635 : // 1) there is a destination lane ((*link)->getLane()!=0)
2636 : // 2) the destination lane belongs to the next edge in route ((*link)->getLane()->myEdge == nRouteEdge)
2637 : // 3) the destination lane allows the vehicle's class ((*link)->getLane()->allowsVehicleClass(veh.getVehicleClass()))
2638 :
2639 : // there should be a link which leads to the next desired lane our route in "conts" (built in "getBestLanes")
2640 : // "conts" stores the best continuations of our current lane
2641 : // we should never return an arbitrary link since this may cause collisions
2642 :
2643 1002017083 : if (nRouteSuccs < (int)conts.size()) {
2644 : // we go through the links in our list and return the matching one
2645 1101970466 : for (std::vector<MSLink*>::const_iterator link = succLinkSource.myLinks.begin(); link != succLinkSource.myLinks.end(); ++link) {
2646 1101248865 : if ((*link)->getLane() != nullptr && (*link)->getLane()->myEdge == nRouteEdge && (*link)->getLane()->allowsVehicleClass(veh.getVehicleType().getVehicleClass())) {
2647 : // we should use the link if it connects us to the best lane
2648 995171853 : if ((*link)->getLane() == conts[nRouteSuccs]) {
2649 985208890 : return link;
2650 : }
2651 : }
2652 : }
2653 : } else {
2654 : // the source lane is a dead end (no continuations exist)
2655 : return succLinkSource.myLinks.end();
2656 : }
2657 : // the only case where this should happen is for a disconnected route (deliberately ignored)
2658 : #ifdef DEBUG_NO_CONNECTION
2659 : // the "'" around the ids are missing intentionally in the message below because it slows messaging down, resulting in test timeouts
2660 : WRITE_WARNING("Could not find connection between lane " + succLinkSource.getID() + " and lane " + conts[nRouteSuccs]->getID() +
2661 : " for vehicle " + veh.getID() + ", time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
2662 : #endif
2663 : return succLinkSource.myLinks.end();
2664 : }
2665 :
2666 :
2667 : const MSLink*
2668 455962144 : MSLane::getLinkTo(const MSLane* const target) const {
2669 455962144 : const bool internal = target->isInternal();
2670 564163343 : for (const MSLink* const l : myLinks) {
2671 563699837 : if ((internal && l->getViaLane() == target) || (!internal && l->getLane() == target)) {
2672 : return l;
2673 : }
2674 : }
2675 : return nullptr;
2676 : }
2677 :
2678 :
2679 : const MSLane*
2680 66919 : MSLane::getInternalFollowingLane(const MSLane* const target) const {
2681 93724 : for (const MSLink* const l : myLinks) {
2682 90643 : if (l->getLane() == target) {
2683 : return l->getViaLane();
2684 : }
2685 : }
2686 : return nullptr;
2687 : }
2688 :
2689 :
2690 : const MSLink*
2691 80292178 : MSLane::getEntryLink() const {
2692 80292178 : if (!isInternal()) {
2693 : return nullptr;
2694 : }
2695 : const MSLane* internal = this;
2696 80262241 : const MSLane* lane = this->getCanonicalPredecessorLane();
2697 : assert(lane != nullptr);
2698 81131231 : while (lane->isInternal()) {
2699 : internal = lane;
2700 868990 : lane = lane->getCanonicalPredecessorLane();
2701 : assert(lane != nullptr);
2702 : }
2703 80262241 : return lane->getLinkTo(internal);
2704 : }
2705 :
2706 :
2707 : void
2708 987 : MSLane::setMaxSpeed(double val, bool byVSS, bool byTraCI, double jamThreshold) {
2709 987 : myMaxSpeed = val;
2710 987 : mySpeedByVSS = byVSS;
2711 987 : mySpeedByTraCI = byTraCI;
2712 987 : myEdge->recalcCache();
2713 987 : if (MSGlobals::gUseMesoSim) {
2714 203 : MESegment* first = MSGlobals::gMesoNet->getSegmentForEdge(*myEdge);
2715 1174 : while (first != nullptr) {
2716 971 : first->setSpeed(val, SIMSTEP, jamThreshold, myIndex);
2717 : first = first->getNextSegment();
2718 : }
2719 : }
2720 987 : }
2721 :
2722 :
2723 : void
2724 83 : MSLane::setFrictionCoefficient(double val) {
2725 83 : myFrictionCoefficient = val;
2726 83 : myEdge->recalcCache();
2727 83 : }
2728 :
2729 :
2730 : void
2731 15 : MSLane::setLength(double val) {
2732 15 : myLength = val;
2733 15 : myEdge->recalcCache();
2734 15 : }
2735 :
2736 :
2737 : void
2738 69873227 : MSLane::swapAfterLaneChange(SUMOTime) {
2739 : //if (getID() == "disabled_lane") std::cout << SIMTIME << " swapAfterLaneChange lane=" << getID() << " myVehicles=" << toString(myVehicles) << " myTmpVehicles=" << toString(myTmpVehicles) << "\n";
2740 69873227 : myVehicles = myTmpVehicles;
2741 : myTmpVehicles.clear();
2742 : // this needs to be done after finishing lane-changing for all lanes on the
2743 : // current edge (MSLaneChanger::updateLanes())
2744 69873227 : sortPartialVehicles();
2745 69873227 : if (MSGlobals::gSublane && getOpposite() != nullptr) {
2746 420463 : getOpposite()->sortPartialVehicles();
2747 : }
2748 69873227 : if (myBidiLane != nullptr) {
2749 325004 : myBidiLane->sortPartialVehicles();
2750 : }
2751 69873227 : }
2752 :
2753 :
2754 : MSVehicle*
2755 25478 : MSLane::removeVehicle(MSVehicle* remVehicle, MSMoveReminder::Notification notification, bool notify) {
2756 : assert(remVehicle->getLane() == this);
2757 54484 : for (MSLane::VehCont::iterator it = myVehicles.begin(); it < myVehicles.end(); it++) {
2758 54468 : if (remVehicle == *it) {
2759 25462 : if (notify) {
2760 17220 : remVehicle->leaveLane(notification);
2761 : }
2762 25462 : myVehicles.erase(it);
2763 25462 : myBruttoVehicleLengthSum -= remVehicle->getVehicleType().getLengthWithGap();
2764 25462 : myNettoVehicleLengthSum -= remVehicle->getVehicleType().getLength();
2765 25462 : break;
2766 : }
2767 : }
2768 25478 : return remVehicle;
2769 : }
2770 :
2771 :
2772 : MSLane*
2773 64782661 : MSLane::getParallelLane(int offset, bool includeOpposite) const {
2774 64782661 : return myEdge->parallelLane(this, offset, includeOpposite);
2775 : }
2776 :
2777 :
2778 : void
2779 2759582 : MSLane::addIncomingLane(MSLane* lane, MSLink* viaLink) {
2780 : IncomingLaneInfo ili;
2781 2759582 : ili.lane = lane;
2782 2759582 : ili.viaLink = viaLink;
2783 2759582 : ili.length = lane->getLength();
2784 2759582 : myIncomingLanes.push_back(ili);
2785 2759582 : }
2786 :
2787 :
2788 : void
2789 2759582 : MSLane::addApproachingLane(MSLane* lane, bool warnMultiCon) {
2790 2759582 : MSEdge* approachingEdge = &lane->getEdge();
2791 2759582 : if (myApproachingLanes.find(approachingEdge) == myApproachingLanes.end()) {
2792 2735027 : myApproachingLanes[approachingEdge] = std::vector<MSLane*>();
2793 24555 : } else if (!approachingEdge->isInternal() && warnMultiCon) {
2794 : // whenever a normal edge connects twice, there is a corresponding
2795 : // internal edge wich connects twice, one warning is sufficient
2796 18 : WRITE_WARNINGF(TL("Lane '%' is approached multiple times from edge '%'. This may cause collisions."),
2797 : getID(), approachingEdge->getID());
2798 : }
2799 2759582 : myApproachingLanes[approachingEdge].push_back(lane);
2800 2759582 : }
2801 :
2802 :
2803 : bool
2804 96326876 : MSLane::isApproachedFrom(MSEdge* const edge, MSLane* const lane) {
2805 : std::map<MSEdge*, std::vector<MSLane*> >::const_iterator i = myApproachingLanes.find(edge);
2806 96326876 : if (i == myApproachingLanes.end()) {
2807 : return false;
2808 : }
2809 : const std::vector<MSLane*>& lanes = (*i).second;
2810 94555051 : return std::find(lanes.begin(), lanes.end(), lane) != lanes.end();
2811 : }
2812 :
2813 :
2814 2708453 : double MSLane::getMissingRearGap(const MSVehicle* leader, double backOffset, double leaderSpeed) const {
2815 : // this follows the same logic as getFollowerOnConsecutive. we do a tree
2816 : // search and check for the vehicle with the largest missing rear gap within
2817 : // relevant range
2818 : double result = 0;
2819 : const double leaderDecel = leader->getCarFollowModel().getMaxDecel();
2820 2708453 : CLeaderDist followerInfo = getFollowersOnConsecutive(leader, backOffset, false)[0];
2821 2708453 : const MSVehicle* v = followerInfo.first;
2822 2708453 : if (v != nullptr) {
2823 4997 : result = v->getCarFollowModel().getSecureGap(v, leader, v->getSpeed(), leaderSpeed, leaderDecel) - followerInfo.second;
2824 : }
2825 2708453 : return result;
2826 : }
2827 :
2828 :
2829 : double
2830 299384828 : MSLane::getMaximumBrakeDist() const {
2831 299384828 : const MSVehicleControl& vc = MSNet::getInstance()->getVehicleControl();
2832 299384828 : const double maxSpeed = getSpeedLimit() * vc.getMaxSpeedFactor();
2833 : // NOTE: For the euler update this is an upper bound on the actual braking distance (see ticket #860)
2834 : // impose a hard bound due to visibility / common sense to avoid unnecessary computation if there are strange vehicles in the fleet
2835 299384828 : const double minDecel = isRailway(myPermissions) ? vc.getMinDecelerationRail() : vc.getMinDeceleration();
2836 299384828 : return MIN2(maxSpeed * maxSpeed * 0.5 / minDecel,
2837 299384828 : myPermissions == SVC_SHIP ? 10000.0 : 1000.0);
2838 : }
2839 :
2840 :
2841 : std::pair<MSVehicle* const, double>
2842 44845264 : MSLane::getLeader(const MSVehicle* veh, const double vehPos, const std::vector<MSLane*>& bestLaneConts, double dist, bool checkTmpVehicles) const {
2843 : // get the leading vehicle for (shadow) veh
2844 : // XXX this only works as long as all lanes of an edge have equal length
2845 : #ifdef DEBUG_CONTEXT
2846 : if (DEBUG_COND2(veh)) {
2847 : std::cout << " getLeader lane=" << getID() << " ego=" << veh->getID() << " vehs=" << toString(myVehicles) << " tmpVehs=" << toString(myTmpVehicles) << "\n";
2848 : }
2849 : #endif
2850 44845264 : if (checkTmpVehicles) {
2851 291490708 : for (VehCont::const_iterator last = myTmpVehicles.begin(); last != myTmpVehicles.end(); ++last) {
2852 : // XXX refactor leaderInfo to use a const vehicle all the way through the call hierarchy
2853 288702850 : MSVehicle* pred = (MSVehicle*)*last;
2854 288702850 : if (pred == veh) {
2855 : continue;
2856 : }
2857 : #ifdef DEBUG_CONTEXT
2858 : if (DEBUG_COND2(veh)) {
2859 : std::cout << std::setprecision(gPrecision) << " getLeader lane=" << getID() << " ego=" << veh->getID() << " egoPos=" << vehPos << " pred=" << pred->getID() << " predPos=" << pred->getPositionOnLane() << "\n";
2860 : }
2861 : #endif
2862 248726844 : if (pred->getPositionOnLane() >= vehPos) {
2863 37253039 : return std::pair<MSVehicle* const, double>(pred, pred->getBackPositionOnLane(this) - veh->getVehicleType().getMinGap() - vehPos);
2864 : }
2865 : }
2866 : } else {
2867 47720245 : for (AnyVehicleIterator last = anyVehiclesBegin(); last != anyVehiclesEnd(); ++last) {
2868 : // XXX refactor leaderInfo to use a const vehicle all the way through the call hierarchy
2869 51636356 : MSVehicle* pred = (MSVehicle*)*last;
2870 51636356 : if (pred == veh) {
2871 3792042 : continue;
2872 : }
2873 : #ifdef DEBUG_CONTEXT
2874 : if (DEBUG_COND2(veh)) {
2875 : std::cout << " getLeader lane=" << getID() << " ego=" << veh->getID() << " egoPos=" << vehPos
2876 : << " pred=" << pred->getID() << " predPos=" << pred->getPositionOnLane(this) << " predBack=" << pred->getBackPositionOnLane(this) << "\n";
2877 : }
2878 : #endif
2879 47844314 : if (pred->getPositionOnLane(this) >= vehPos) {
2880 4034047 : if (MSGlobals::gLaneChangeDuration > 0
2881 328408 : && pred->getLaneChangeModel().isOpposite()
2882 89706 : && !pred->getLaneChangeModel().isChangingLanes()
2883 4046556 : && pred->getLaneChangeModel().getShadowLane() == this) {
2884 : // skip non-overlapping shadow
2885 58968 : continue;
2886 : }
2887 3916111 : return std::pair<MSVehicle* const, double>(pred, pred->getBackPositionOnLane(this) - veh->getVehicleType().getMinGap() - vehPos);
2888 : }
2889 : }
2890 : }
2891 : // XXX from here on the code mirrors MSLaneChanger::getRealLeader
2892 3676114 : if (bestLaneConts.size() > 0) {
2893 3352377 : double seen = getLength() - vehPos;
2894 3352377 : double speed = veh->getSpeed();
2895 3352377 : if (dist < 0) {
2896 31504 : dist = veh->getCarFollowModel().brakeGap(speed) + veh->getVehicleType().getMinGap();
2897 : }
2898 : #ifdef DEBUG_CONTEXT
2899 : if (DEBUG_COND2(veh)) {
2900 : std::cout << " getLeader lane=" << getID() << " seen=" << seen << " dist=" << dist << "\n";
2901 : }
2902 : #endif
2903 3352377 : if (seen > dist) {
2904 1388955 : return std::pair<MSVehicle* const, double>(static_cast<MSVehicle*>(nullptr), -1);
2905 : }
2906 1963422 : return getLeaderOnConsecutive(dist, seen, speed, *veh, bestLaneConts);
2907 : } else {
2908 323737 : return std::make_pair(static_cast<MSVehicle*>(nullptr), -1);
2909 : }
2910 : }
2911 :
2912 :
2913 : std::pair<MSVehicle* const, double>
2914 32205454 : MSLane::getLeaderOnConsecutive(double dist, double seen, double speed, const MSVehicle& veh,
2915 : const std::vector<MSLane*>& bestLaneConts) const {
2916 : #ifdef DEBUG_CONTEXT
2917 : if (DEBUG_COND2(&veh)) {
2918 : std::cout << " getLeaderOnConsecutive lane=" << getID() << " ego=" << veh.getID() << " seen=" << seen << " dist=" << dist << " conts=" << toString(bestLaneConts) << "\n";
2919 : }
2920 : #endif
2921 32205454 : if (seen > dist && !isInternal()) {
2922 112654 : return std::make_pair(static_cast<MSVehicle*>(nullptr), -1);
2923 : }
2924 : int view = 1;
2925 : // loop over following lanes
2926 32092800 : if (myPartialVehicles.size() > 0) {
2927 : // XXX
2928 1131372 : MSVehicle* pred = myPartialVehicles.front();
2929 1131372 : const double gap = seen - (getLength() - pred->getBackPositionOnLane(this)) - veh.getVehicleType().getMinGap();
2930 : #ifdef DEBUG_CONTEXT
2931 : if (DEBUG_COND2(&veh)) {
2932 : std::cout << " predGap=" << gap << " partials=" << toString(myPartialVehicles) << "\n";
2933 : }
2934 : #endif
2935 : // make sure pred is really a leader and not doing continous lane-changing behind ego
2936 1131372 : if (gap > 0) {
2937 763837 : return std::pair<MSVehicle* const, double>(pred, gap);
2938 : }
2939 : }
2940 : #ifdef DEBUG_CONTEXT
2941 : if (DEBUG_COND2(&veh)) {
2942 : gDebugFlag1 = true;
2943 : }
2944 : #endif
2945 : const MSLane* nextLane = this;
2946 : do {
2947 45623099 : nextLane->getVehiclesSecure(); // lock against running sim when called from GUI for time gap coloring
2948 : // get the next link used
2949 45623099 : std::vector<MSLink*>::const_iterator link = succLinkSec(veh, view, *nextLane, bestLaneConts);
2950 45623099 : if (nextLane->isLinkEnd(link) && view < veh.getRoute().size() - veh.getRoutePosition()) {
2951 3341034 : const MSEdge* nextEdge = *(veh.getCurrentRouteEdge() + view);
2952 3341034 : if (nextEdge->getNumLanes() == 1) {
2953 : // lanes are unambiguous on the next route edge, continue beyond bestLaneConts
2954 4431725 : for (link = nextLane->getLinkCont().begin(); link < nextLane->getLinkCont().end(); link++) {
2955 2839749 : if ((*link)->getLane() == nextEdge->getLanes().front()) {
2956 : break;
2957 : }
2958 : }
2959 : }
2960 : }
2961 45623099 : if (nextLane->isLinkEnd(link)) {
2962 : #ifdef DEBUG_CONTEXT
2963 : if (DEBUG_COND2(&veh)) {
2964 : std::cout << " cannot continue after nextLane=" << nextLane->getID() << "\n";
2965 : }
2966 : #endif
2967 11040197 : nextLane->releaseVehicles();
2968 11040197 : break;
2969 : }
2970 : // check for link leaders
2971 34582902 : const bool laneChanging = veh.getLane() != this;
2972 34582902 : const MSLink::LinkLeaders linkLeaders = (*link)->getLeaderInfo(&veh, seen);
2973 34582900 : nextLane->releaseVehicles();
2974 34582900 : if (linkLeaders.size() > 0) {
2975 : std::pair<MSVehicle*, double> result;
2976 : double shortestGap = std::numeric_limits<double>::max();
2977 2140089 : for (auto ll : linkLeaders) {
2978 : double gap = ll.vehAndGap.second;
2979 : MSVehicle* lVeh = ll.vehAndGap.first;
2980 1170371 : if (lVeh != nullptr) {
2981 : // leader is a vehicle, not a pedestrian
2982 1060597 : gap += lVeh->getCarFollowModel().brakeGap(lVeh->getSpeed(), lVeh->getCarFollowModel().getMaxDecel(), 0);
2983 : }
2984 : #ifdef DEBUG_CONTEXT
2985 : if (DEBUG_COND2(&veh)) {
2986 : std::cout << " linkLeader candidate " << Named::getIDSecure(lVeh)
2987 : << " isLeader=" << veh.isLeader(*link, lVeh, gap)
2988 : << " gap=" << ll.vehAndGap.second
2989 : << " gap+brakeing=" << gap
2990 : << "\n";
2991 : }
2992 : #endif
2993 : // in the context of lane-changing, all candidates are leaders
2994 1170371 : if (lVeh != nullptr && !laneChanging && !veh.isLeader(*link, lVeh, ll.vehAndGap.second)) {
2995 7162 : continue;
2996 : }
2997 1163209 : if (gap < shortestGap) {
2998 : shortestGap = gap;
2999 1001630 : if (ll.vehAndGap.second < 0 && !MSGlobals::gComputeLC) {
3000 : // can always continue up to the stop line or crossing point
3001 : // @todo: figure out whether this should also impact lane changing
3002 74434 : ll.vehAndGap.second = MAX2(seen - nextLane->getLength(), ll.distToCrossing);
3003 : }
3004 : result = ll.vehAndGap;
3005 : }
3006 : }
3007 969718 : if (shortestGap != std::numeric_limits<double>::max()) {
3008 : #ifdef DEBUG_CONTEXT
3009 : if (DEBUG_COND2(&veh)) {
3010 : std::cout << " found linkLeader after nextLane=" << nextLane->getID() << "\n";
3011 : gDebugFlag1 = false;
3012 : }
3013 : #endif
3014 963065 : return result;
3015 : }
3016 : }
3017 33619835 : bool nextInternal = (*link)->getViaLane() != nullptr;
3018 : nextLane = (*link)->getViaLaneOrLane();
3019 17058226 : if (nextLane == nullptr) {
3020 : break;
3021 : }
3022 33619835 : nextLane->getVehiclesSecure(); // lock against running sim when called from GUI for time gap coloring
3023 33619835 : MSVehicle* leader = nextLane->getLastAnyVehicle();
3024 33619835 : if (leader != nullptr) {
3025 : #ifdef DEBUG_CONTEXT
3026 : if (DEBUG_COND2(&veh)) {
3027 : std::cout << " found leader " << leader->getID() << " on nextLane=" << nextLane->getID() << "\n";
3028 : }
3029 : #endif
3030 13993211 : const double leaderDist = seen + leader->getBackPositionOnLane(nextLane) - veh.getVehicleType().getMinGap();
3031 13993211 : nextLane->releaseVehicles();
3032 13993211 : return std::make_pair(leader, leaderDist);
3033 : }
3034 19626624 : nextLane->releaseVehicles();
3035 19626624 : if (nextLane->getVehicleMaxSpeed(&veh) < speed) {
3036 2378580 : dist = veh.getCarFollowModel().brakeGap(nextLane->getVehicleMaxSpeed(&veh));
3037 : }
3038 19626624 : seen += nextLane->getLength();
3039 19626624 : if (!nextInternal) {
3040 6057267 : view++;
3041 : }
3042 48877036 : } while (seen <= dist || nextLane->isInternal());
3043 : #ifdef DEBUG_CONTEXT
3044 : gDebugFlag1 = false;
3045 : #endif
3046 16372685 : return std::make_pair(static_cast<MSVehicle*>(nullptr), -1);
3047 : }
3048 :
3049 :
3050 : std::pair<MSVehicle* const, double>
3051 84434 : MSLane::getCriticalLeader(double dist, double seen, double speed, const MSVehicle& veh) const {
3052 : #ifdef DEBUG_CONTEXT
3053 : if (DEBUG_COND2(&veh)) {
3054 : std::cout << SIMTIME << " getCriticalLeader. lane=" << getID() << " veh=" << veh.getID() << "\n";
3055 : }
3056 : #endif
3057 84434 : const std::vector<MSLane*>& bestLaneConts = veh.getBestLanesContinuation(this);
3058 : std::pair<MSVehicle*, double> result = std::make_pair(static_cast<MSVehicle*>(nullptr), -1);
3059 : double safeSpeed = std::numeric_limits<double>::max();
3060 : int view = 1;
3061 : // loop over following lanes
3062 : // @note: we don't check the partial occupator for this lane since it was
3063 : // already checked in MSLaneChanger::getRealLeader()
3064 : const MSLane* nextLane = this;
3065 168868 : SUMOTime arrivalTime = MSNet::getInstance()->getCurrentTimeStep() + TIME2STEPS(seen / MAX2(speed, NUMERICAL_EPS));
3066 : do {
3067 : // get the next link used
3068 155095 : std::vector<MSLink*>::const_iterator link = succLinkSec(veh, view, *nextLane, bestLaneConts);
3069 152815 : if (nextLane->isLinkEnd(link) || !(*link)->opened(arrivalTime, speed, speed, veh.getVehicleType().getLength(),
3070 305797 : veh.getImpatience(), veh.getCarFollowModel().getMaxDecel(), 0, veh.getLateralPositionOnLane(), nullptr, false, &veh) || (*link)->haveRed()) {
3071 4393 : return result;
3072 : }
3073 : // check for link leaders
3074 : #ifdef DEBUG_CONTEXT
3075 : if (DEBUG_COND2(&veh)) {
3076 : gDebugFlag1 = true; // See MSLink::getLeaderInfo
3077 : }
3078 : #endif
3079 150702 : const MSLink::LinkLeaders linkLeaders = (*link)->getLeaderInfo(&veh, seen);
3080 : #ifdef DEBUG_CONTEXT
3081 : if (DEBUG_COND2(&veh)) {
3082 : gDebugFlag1 = false; // See MSLink::getLeaderInfo
3083 : }
3084 : #endif
3085 152895 : for (MSLink::LinkLeaders::const_iterator it = linkLeaders.begin(); it != linkLeaders.end(); ++it) {
3086 2193 : const MSVehicle* leader = (*it).vehAndGap.first;
3087 2193 : if (leader != nullptr && leader != result.first) {
3088 : // XXX ignoring pedestrians here!
3089 : // XXX ignoring the fact that the link leader may alread by following us
3090 : // XXX ignoring the fact that we may drive up to the crossing point
3091 2168 : double tmpSpeed = safeSpeed;
3092 2168 : veh.adaptToJunctionLeader((*it).vehAndGap, seen, nullptr, nextLane, tmpSpeed, tmpSpeed, (*it).distToCrossing);
3093 : #ifdef DEBUG_CONTEXT
3094 : if (DEBUG_COND2(&veh)) {
3095 : std::cout << " linkLeader=" << leader->getID() << " gap=" << result.second << " tmpSpeed=" << tmpSpeed << " safeSpeed=" << safeSpeed << "\n";
3096 : }
3097 : #endif
3098 2168 : if (tmpSpeed < safeSpeed) {
3099 : safeSpeed = tmpSpeed;
3100 : result = (*it).vehAndGap;
3101 : }
3102 : }
3103 : }
3104 150702 : bool nextInternal = (*link)->getViaLane() != nullptr;
3105 : nextLane = (*link)->getViaLaneOrLane();
3106 83158 : if (nextLane == nullptr) {
3107 : break;
3108 : }
3109 150702 : MSVehicle* leader = nextLane->getLastAnyVehicle();
3110 150702 : if (leader != nullptr && leader != result.first) {
3111 84093 : const double gap = seen + leader->getBackPositionOnLane(nextLane) - veh.getVehicleType().getMinGap();
3112 84093 : const double tmpSpeed = veh.getCarFollowModel().insertionFollowSpeed(&veh, speed, gap, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel(), leader);
3113 84093 : if (tmpSpeed < safeSpeed) {
3114 : safeSpeed = tmpSpeed;
3115 : result = std::make_pair(leader, gap);
3116 : }
3117 : }
3118 150702 : if (nextLane->getVehicleMaxSpeed(&veh) < speed) {
3119 24114 : dist = veh.getCarFollowModel().brakeGap(nextLane->getVehicleMaxSpeed(&veh));
3120 : }
3121 150702 : seen += nextLane->getLength();
3122 150702 : if (seen <= dist) {
3123 : // delaying the update of arrivalTime and making it conditional to avoid possible integer overflows
3124 33957 : arrivalTime += TIME2STEPS(nextLane->getLength() / MAX2(speed, NUMERICAL_EPS));
3125 : }
3126 150702 : if (!nextInternal) {
3127 83158 : view++;
3128 : }
3129 221363 : } while (seen <= dist || nextLane->isInternal());
3130 80041 : return result;
3131 : }
3132 :
3133 :
3134 : MSLane*
3135 856640676 : MSLane::getLogicalPredecessorLane() const {
3136 856640676 : if (myLogicalPredecessorLane == nullptr) {
3137 4475898 : MSEdgeVector pred = myEdge->getPredecessors();
3138 : // get only those edges which connect to this lane
3139 8984111 : for (MSEdgeVector::iterator i = pred.begin(); i != pred.end();) {
3140 4508213 : std::vector<IncomingLaneInfo>::const_iterator j = find_if(myIncomingLanes.begin(), myIncomingLanes.end(), edge_finder(*i));
3141 4508213 : if (j == myIncomingLanes.end()) {
3142 : i = pred.erase(i);
3143 : } else {
3144 : ++i;
3145 : }
3146 : }
3147 : // get the lane with the "straightest" connection
3148 4475898 : if (pred.size() != 0) {
3149 1888992 : std::sort(pred.begin(), pred.end(), by_connections_to_sorter(&getEdge()));
3150 944496 : MSEdge* best = *pred.begin();
3151 944496 : std::vector<IncomingLaneInfo>::const_iterator j = find_if(myIncomingLanes.begin(), myIncomingLanes.end(), edge_finder(best));
3152 944496 : myLogicalPredecessorLane = j->lane;
3153 : }
3154 4475898 : }
3155 856640676 : return myLogicalPredecessorLane;
3156 : }
3157 :
3158 :
3159 : const MSLane*
3160 17667619 : MSLane::getNormalPredecessorLane() const {
3161 35570663 : if (isInternal()) {
3162 17903044 : return getLogicalPredecessorLane()->getNormalPredecessorLane();
3163 : } else {
3164 : return this;
3165 : }
3166 : }
3167 :
3168 :
3169 : const MSLane*
3170 446799 : MSLane::getNormalSuccessorLane() const {
3171 623417 : if (isInternal()) {
3172 176618 : return getCanonicalSuccessorLane()->getNormalSuccessorLane();
3173 : } else {
3174 : return this;
3175 : }
3176 : }
3177 :
3178 :
3179 : MSLane*
3180 73985 : MSLane::getLogicalPredecessorLane(const MSEdge& fromEdge) const {
3181 130131 : for (const IncomingLaneInfo& cand : myIncomingLanes) {
3182 86224 : if (&(cand.lane->getEdge()) == &fromEdge) {
3183 : return cand.lane;
3184 : }
3185 : }
3186 : return nullptr;
3187 : }
3188 :
3189 :
3190 : MSLane*
3191 82335719 : MSLane::getCanonicalPredecessorLane() const {
3192 82335719 : if (myCanonicalPredecessorLane != nullptr) {
3193 : return myCanonicalPredecessorLane;
3194 : }
3195 916535 : if (myIncomingLanes.empty()) {
3196 : return nullptr;
3197 : }
3198 : // myCanonicalPredecessorLane has not yet been determined and there exist incoming lanes
3199 : // get the lane with the priorized (or if this does not apply the "straightest") connection
3200 916396 : const auto bestLane = std::min_element(myIncomingLanes.begin(), myIncomingLanes.end(), incoming_lane_priority_sorter(this));
3201 : {
3202 : #ifdef HAVE_FOX
3203 916396 : ScopedLocker<> lock(myLeaderInfoMutex, MSGlobals::gNumSimThreads > 1);
3204 : #endif
3205 916396 : myCanonicalPredecessorLane = bestLane->lane;
3206 : }
3207 : #ifdef DEBUG_LANE_SORTER
3208 : std::cout << "\nBest predecessor lane for lane '" << myID << "': '" << myCanonicalPredecessorLane->getID() << "'" << std::endl;
3209 : #endif
3210 916396 : return myCanonicalPredecessorLane;
3211 : }
3212 :
3213 :
3214 : MSLane*
3215 2513433 : MSLane::getCanonicalSuccessorLane() const {
3216 2513433 : if (myCanonicalSuccessorLane != nullptr) {
3217 : return myCanonicalSuccessorLane;
3218 : }
3219 64409 : if (myLinks.empty()) {
3220 : return nullptr;
3221 : }
3222 : // myCanonicalSuccessorLane has not yet been determined and there exist outgoing links
3223 7283 : std::vector<MSLink*> candidateLinks = myLinks;
3224 : // get the lane with the priorized (or if this does not apply the "straightest") connection
3225 14566 : std::sort(candidateLinks.begin(), candidateLinks.end(), outgoing_lane_priority_sorter(this));
3226 7283 : MSLane* best = (*candidateLinks.begin())->getViaLaneOrLane();
3227 : #ifdef DEBUG_LANE_SORTER
3228 : std::cout << "\nBest successor lane for lane '" << myID << "': '" << best->getID() << "'" << std::endl;
3229 : #endif
3230 7283 : myCanonicalSuccessorLane = best;
3231 : return myCanonicalSuccessorLane;
3232 7283 : }
3233 :
3234 :
3235 : LinkState
3236 3551015 : MSLane::getIncomingLinkState() const {
3237 3551015 : const MSLane* const pred = getLogicalPredecessorLane();
3238 3551015 : if (pred == nullptr) {
3239 : return LINKSTATE_DEADEND;
3240 : } else {
3241 3551015 : return pred->getLinkTo(this)->getState();
3242 : }
3243 : }
3244 :
3245 :
3246 : const std::vector<std::pair<const MSLane*, const MSEdge*> >
3247 256472 : MSLane::getOutgoingViaLanes() const {
3248 : std::vector<std::pair<const MSLane*, const MSEdge*> > result;
3249 562257 : for (const MSLink* link : myLinks) {
3250 : assert(link->getLane() != nullptr);
3251 611570 : result.push_back(std::make_pair(link->getLane(), link->getViaLane() == nullptr ? nullptr : &link->getViaLane()->getEdge()));
3252 : }
3253 256472 : return result;
3254 0 : }
3255 :
3256 : std::vector<const MSLane*>
3257 76 : MSLane::getNormalIncomingLanes() const {
3258 76 : std::vector<const MSLane*> result = {};
3259 226 : for (std::map<MSEdge*, std::vector<MSLane*> >::const_iterator it = myApproachingLanes.begin(); it != myApproachingLanes.end(); ++it) {
3260 328 : for (std::vector<MSLane*>::const_iterator it_lane = (*it).second.begin(); it_lane != (*it).second.end(); ++it_lane) {
3261 178 : if (!((*it_lane)->isInternal())) {
3262 146 : result.push_back(*it_lane);
3263 : }
3264 : }
3265 : }
3266 76 : return result;
3267 0 : }
3268 :
3269 :
3270 : void
3271 1009067 : MSLane::leftByLaneChange(MSVehicle* v) {
3272 1009067 : myBruttoVehicleLengthSum -= v->getVehicleType().getLengthWithGap();
3273 1009067 : myNettoVehicleLengthSum -= v->getVehicleType().getLength();
3274 1009067 : }
3275 :
3276 :
3277 : void
3278 966201 : MSLane::enteredByLaneChange(MSVehicle* v) {
3279 966201 : myBruttoVehicleLengthSum += v->getVehicleType().getLengthWithGap();
3280 966201 : myNettoVehicleLengthSum += v->getVehicleType().getLength();
3281 966201 : }
3282 :
3283 :
3284 : int
3285 0 : MSLane::getCrossingIndex() const {
3286 0 : for (std::vector<MSLink*>::const_iterator i = myLinks.begin(); i != myLinks.end(); ++i) {
3287 0 : if ((*i)->getLane()->getEdge().isCrossing()) {
3288 0 : return (int)(i - myLinks.begin());
3289 : }
3290 : }
3291 : return -1;
3292 : }
3293 :
3294 : // ------------ Current state retrieval
3295 : double
3296 1143074559 : MSLane::getFractionalVehicleLength(bool brutto) const {
3297 : double sum = 0;
3298 1143074559 : if (myPartialVehicles.size() > 0) {
3299 226918243 : const MSLane* bidi = getBidiLane();
3300 470465888 : for (MSVehicle* cand : myPartialVehicles) {
3301 243547645 : if (MSGlobals::gSublane && cand->getLaneChangeModel().getShadowLane() == this) {
3302 24024514 : continue;
3303 : }
3304 219523131 : if (cand->getLane() == bidi) {
3305 195246 : sum += (brutto ? cand->getVehicleType().getLengthWithGap() : cand->getVehicleType().getLength());
3306 : } else {
3307 219425508 : sum += myLength - cand->getBackPositionOnLane(this);
3308 : }
3309 : }
3310 : }
3311 1143074559 : return sum;
3312 : }
3313 :
3314 : double
3315 1143030369 : MSLane::getBruttoOccupancy() const {
3316 1143030369 : getVehiclesSecure();
3317 1143030369 : double fractions = getFractionalVehicleLength(true);
3318 1143030369 : if (myVehicles.size() != 0) {
3319 1064656116 : MSVehicle* lastVeh = myVehicles.front();
3320 1064656116 : if (lastVeh->getPositionOnLane() < lastVeh->getVehicleType().getLength()) {
3321 32913821 : fractions -= (lastVeh->getVehicleType().getLength() - lastVeh->getPositionOnLane());
3322 : }
3323 : }
3324 1143030369 : releaseVehicles();
3325 1143030369 : return MIN2(1., (myBruttoVehicleLengthSum + fractions) / myLength);
3326 : }
3327 :
3328 :
3329 : double
3330 44190 : MSLane::getNettoOccupancy() const {
3331 44190 : getVehiclesSecure();
3332 44190 : double fractions = getFractionalVehicleLength(false);
3333 44190 : if (myVehicles.size() != 0) {
3334 380 : MSVehicle* lastVeh = myVehicles.front();
3335 380 : if (lastVeh->getPositionOnLane() < lastVeh->getVehicleType().getLength()) {
3336 0 : fractions -= (lastVeh->getVehicleType().getLength() - lastVeh->getPositionOnLane());
3337 : }
3338 : }
3339 44190 : releaseVehicles();
3340 44190 : return (myNettoVehicleLengthSum + fractions) / myLength;
3341 : }
3342 :
3343 :
3344 : double
3345 20 : MSLane::getWaitingSeconds() const {
3346 20 : if (myVehicles.size() == 0) {
3347 : return 0;
3348 : }
3349 : double wtime = 0;
3350 24 : for (VehCont::const_iterator i = myVehicles.begin(); i != myVehicles.end(); ++i) {
3351 12 : wtime += (*i)->getWaitingSeconds();
3352 : }
3353 : return wtime;
3354 : }
3355 :
3356 :
3357 : double
3358 147340867 : MSLane::getMeanSpeed() const {
3359 147340867 : if (myVehicles.size() == 0) {
3360 129546076 : return myMaxSpeed;
3361 : }
3362 : double v = 0;
3363 : int numVehs = 0;
3364 126119823 : for (const MSVehicle* const veh : getVehiclesSecure()) {
3365 108325032 : if (!veh->isStopped() || !myEdge->hasLaneChanger()) {
3366 108139531 : v += veh->getSpeed();
3367 108139531 : numVehs++;
3368 : }
3369 : }
3370 17794791 : releaseVehicles();
3371 17794791 : if (numVehs == 0) {
3372 109446 : return myMaxSpeed;
3373 : }
3374 17685345 : return v / numVehs;
3375 : }
3376 :
3377 :
3378 : double
3379 1676 : MSLane::getMeanSpeedBike() const {
3380 : // @note: redundant code with getMeanSpeed to avoid extra checks in a function that is called very often
3381 1676 : if (myVehicles.size() == 0) {
3382 1184 : return myMaxSpeed;
3383 : }
3384 : double v = 0;
3385 : int numBikes = 0;
3386 1808 : for (MSVehicle* veh : getVehiclesSecure()) {
3387 1316 : if (veh->getVClass() == SVC_BICYCLE) {
3388 920 : v += veh->getSpeed();
3389 920 : numBikes++;
3390 : }
3391 : }
3392 : double ret;
3393 492 : if (numBikes > 0) {
3394 268 : ret = v / (double) myVehicles.size();
3395 : } else {
3396 224 : ret = myMaxSpeed;
3397 : }
3398 492 : releaseVehicles();
3399 492 : return ret;
3400 : }
3401 :
3402 :
3403 : double
3404 44187 : MSLane::getHarmonoise_NoiseEmissions() const {
3405 : double ret = 0;
3406 44187 : const MSLane::VehCont& vehs = getVehiclesSecure();
3407 44187 : if (vehs.size() == 0) {
3408 43811 : releaseVehicles();
3409 43811 : return 0;
3410 : }
3411 968 : for (MSLane::VehCont::const_iterator i = vehs.begin(); i != vehs.end(); ++i) {
3412 592 : double sv = (*i)->getHarmonoise_NoiseEmissions();
3413 592 : ret += (double) pow(10., (sv / 10.));
3414 : }
3415 376 : releaseVehicles();
3416 376 : return HelpersHarmonoise::sum(ret);
3417 : }
3418 :
3419 :
3420 : int
3421 276911 : MSLane::vehicle_position_sorter::operator()(MSVehicle* v1, MSVehicle* v2) const {
3422 276911 : const double pos1 = v1->getBackPositionOnLane(myLane);
3423 276911 : const double pos2 = v2->getBackPositionOnLane(myLane);
3424 276911 : if (pos1 != pos2) {
3425 273195 : return pos1 > pos2;
3426 : } else {
3427 3716 : return v1->getNumericalID() > v2->getNumericalID();
3428 : }
3429 : }
3430 :
3431 :
3432 : int
3433 335338425 : MSLane::vehicle_natural_position_sorter::operator()(MSVehicle* v1, MSVehicle* v2) const {
3434 335338425 : const double pos1 = v1->getBackPositionOnLane(myLane);
3435 335338425 : const double pos2 = v2->getBackPositionOnLane(myLane);
3436 335338425 : if (pos1 != pos2) {
3437 334932857 : return pos1 < pos2;
3438 : } else {
3439 405568 : return v1->getLateralPositionOnLane() < v2->getLateralPositionOnLane();
3440 : }
3441 : }
3442 :
3443 :
3444 944496 : MSLane::by_connections_to_sorter::by_connections_to_sorter(const MSEdge* const e) :
3445 944496 : myEdge(e),
3446 944496 : myLaneDir(e->getLanes()[0]->getShape().angleAt2D(0)) {
3447 944496 : }
3448 :
3449 :
3450 : int
3451 16473 : MSLane::by_connections_to_sorter::operator()(const MSEdge* const e1, const MSEdge* const e2) const {
3452 : // std::cout << "\nby_connections_to_sorter()";
3453 :
3454 16473 : const std::vector<MSLane*>* ae1 = e1->allowedLanes(*myEdge);
3455 16473 : const std::vector<MSLane*>* ae2 = e2->allowedLanes(*myEdge);
3456 : double s1 = 0;
3457 16473 : if (ae1 != nullptr && ae1->size() != 0) {
3458 : // std::cout << "\nsize 1 = " << ae1->size()
3459 : // << " anglediff 1 = " << fabs(GeomHelper::angleDiff((*ae1)[0]->getShape().angleAt2D(0), myLaneDir)) / M_PI / 2.
3460 : // << "\nallowed lanes: ";
3461 : // for (std::vector<MSLane*>::const_iterator j = ae1->begin(); j != ae1->end(); ++j){
3462 : // std::cout << "\n" << (*j)->getID();
3463 : // }
3464 16473 : s1 = (double) ae1->size() + fabs(GeomHelper::angleDiff((*ae1)[0]->getShape().angleAt2D(0), myLaneDir)) / M_PI / 2.;
3465 : }
3466 : double s2 = 0;
3467 16473 : if (ae2 != nullptr && ae2->size() != 0) {
3468 : // std::cout << "\nsize 2 = " << ae2->size()
3469 : // << " anglediff 2 = " << fabs(GeomHelper::angleDiff((*ae2)[0]->getShape().angleAt2D(0), myLaneDir)) / M_PI / 2.
3470 : // << "\nallowed lanes: ";
3471 : // for (std::vector<MSLane*>::const_iterator j = ae2->begin(); j != ae2->end(); ++j){
3472 : // std::cout << "\n" << (*j)->getID();
3473 : // }
3474 16473 : s2 = (double) ae2->size() + fabs(GeomHelper::angleDiff((*ae2)[0]->getShape().angleAt2D(0), myLaneDir)) / M_PI / 2.;
3475 : }
3476 :
3477 : // std::cout << "\ne1 = " << e1->getID() << " e2 = " << e2->getID()
3478 : // << "\ns1 = " << s1 << " s2 = " << s2
3479 : // << std::endl;
3480 :
3481 16473 : return s1 < s2;
3482 : }
3483 :
3484 :
3485 916396 : MSLane::incoming_lane_priority_sorter::incoming_lane_priority_sorter(const MSLane* const targetLane) :
3486 916396 : myLane(targetLane),
3487 916396 : myLaneDir(targetLane->getShape().angleAt2D(0)) {}
3488 :
3489 : int
3490 743 : MSLane::incoming_lane_priority_sorter::operator()(const IncomingLaneInfo& laneInfo1, const IncomingLaneInfo& laneInfo2) const {
3491 743 : const MSLane* noninternal1 = laneInfo1.lane;
3492 1817 : while (noninternal1->isInternal()) {
3493 : assert(noninternal1->getIncomingLanes().size() == 1);
3494 1074 : noninternal1 = noninternal1->getIncomingLanes()[0].lane;
3495 : }
3496 743 : MSLane* noninternal2 = laneInfo2.lane;
3497 1649 : while (noninternal2->isInternal()) {
3498 : assert(noninternal2->getIncomingLanes().size() == 1);
3499 906 : noninternal2 = noninternal2->getIncomingLanes()[0].lane;
3500 : }
3501 :
3502 743 : const MSLink* link1 = noninternal1->getLinkTo(myLane);
3503 743 : const MSLink* link2 = noninternal2->getLinkTo(myLane);
3504 :
3505 : #ifdef DEBUG_LANE_SORTER
3506 : std::cout << "\nincoming_lane_priority sorter()\n"
3507 : << "noninternal predecessor for lane '" << laneInfo1.lane->getID()
3508 : << "': '" << noninternal1->getID() << "'\n"
3509 : << "noninternal predecessor for lane '" << laneInfo2.lane->getID()
3510 : << "': '" << noninternal2->getID() << "'\n";
3511 : #endif
3512 :
3513 : assert(laneInfo1.lane->isInternal() || link1 == laneInfo1.viaLink);
3514 : assert(link1 != 0);
3515 : assert(link2 != 0);
3516 :
3517 : // check priority between links
3518 : bool priorized1 = true;
3519 : bool priorized2 = true;
3520 :
3521 : #ifdef DEBUG_LANE_SORTER
3522 : std::cout << "FoeLinks of '" << noninternal1->getID() << "'" << std::endl;
3523 : #endif
3524 1541 : for (const MSLink* const foeLink : link1->getFoeLinks()) {
3525 : #ifdef DEBUG_LANE_SORTER
3526 : std::cout << foeLink->getLaneBefore()->getID() << std::endl;
3527 : #endif
3528 1361 : if (foeLink == link2) {
3529 : priorized1 = false;
3530 : break;
3531 : }
3532 : }
3533 :
3534 : #ifdef DEBUG_LANE_SORTER
3535 : std::cout << "FoeLinks of '" << noninternal2->getID() << "'" << std::endl;
3536 : #endif
3537 1994 : for (const MSLink* const foeLink : link2->getFoeLinks()) {
3538 : #ifdef DEBUG_LANE_SORTER
3539 : std::cout << foeLink->getLaneBefore()->getID() << std::endl;
3540 : #endif
3541 : // either link1 is priorized, or it should not appear in link2's foes
3542 1545 : if (foeLink == link1) {
3543 : priorized2 = false;
3544 : break;
3545 : }
3546 : }
3547 : // if one link is subordinate, the other must be priorized (except for
3548 : // traffic lights where mutual response is permitted to handle stuck-on-red
3549 : // situation)
3550 743 : if (priorized1 != priorized2) {
3551 629 : return priorized1;
3552 : }
3553 :
3554 : // both are priorized, compare angle difference
3555 114 : double d1 = fabs(GeomHelper::angleDiff(noninternal1->getShape().angleAt2D(0), myLaneDir));
3556 114 : double d2 = fabs(GeomHelper::angleDiff(noninternal2->getShape().angleAt2D(0), myLaneDir));
3557 :
3558 114 : return d2 > d1;
3559 : }
3560 :
3561 :
3562 :
3563 7283 : MSLane::outgoing_lane_priority_sorter::outgoing_lane_priority_sorter(const MSLane* const sourceLane) :
3564 7283 : myLaneDir(sourceLane->getShape().angleAt2D(0)) {}
3565 :
3566 : int
3567 13557 : MSLane::outgoing_lane_priority_sorter::operator()(const MSLink* link1, const MSLink* link2) const {
3568 : const MSLane* target1 = link1->getLane();
3569 : const MSLane* target2 = link2->getLane();
3570 13557 : if (target2 == nullptr) {
3571 : return true;
3572 : }
3573 13557 : if (target1 == nullptr) {
3574 : return false;
3575 : }
3576 :
3577 : #ifdef DEBUG_LANE_SORTER
3578 : std::cout << "\noutgoing_lane_priority sorter()\n"
3579 : << "noninternal successors for lane '" << myLane->getID()
3580 : << "': '" << target1->getID() << "' and "
3581 : << "'" << target2->getID() << "'\n";
3582 : #endif
3583 :
3584 : // priority of targets
3585 : int priority1 = target1->getEdge().getPriority();
3586 : int priority2 = target2->getEdge().getPriority();
3587 :
3588 13557 : if (priority1 != priority2) {
3589 127 : return priority1 > priority2;
3590 : }
3591 :
3592 : // if priority of targets coincides, use angle difference
3593 :
3594 : // both are priorized, compare angle difference
3595 13430 : double d1 = fabs(GeomHelper::angleDiff(target1->getShape().angleAt2D(0), myLaneDir));
3596 13430 : double d2 = fabs(GeomHelper::angleDiff(target2->getShape().angleAt2D(0), myLaneDir));
3597 :
3598 13430 : return d2 > d1;
3599 : }
3600 :
3601 : void
3602 1523 : MSLane::addParking(MSBaseVehicle* veh) {
3603 : myParkingVehicles.insert(veh);
3604 1523 : }
3605 :
3606 :
3607 : void
3608 10271 : MSLane::removeParking(MSBaseVehicle* veh) {
3609 : myParkingVehicles.erase(veh);
3610 10271 : }
3611 :
3612 : bool
3613 73 : MSLane::hasApproaching() const {
3614 146 : for (const MSLink* link : myLinks) {
3615 79 : if (link->getApproaching().size() > 0) {
3616 : return true;
3617 : }
3618 : }
3619 : return false;
3620 : }
3621 :
3622 : void
3623 8854 : MSLane::saveState(OutputDevice& out) {
3624 8854 : const bool toRailJunction = myLinks.size() > 0 && (
3625 8421 : myEdge->getToJunction()->getType() == SumoXMLNodeType::RAIL_SIGNAL
3626 8337 : || myEdge->getToJunction()->getType() == SumoXMLNodeType::RAIL_CROSSING);
3627 : const bool hasVehicles = myVehicles.size() > 0;
3628 8854 : if (hasVehicles || (toRailJunction && hasApproaching())) {
3629 386 : out.openTag(SUMO_TAG_LANE);
3630 : out.writeAttr(SUMO_ATTR_ID, getID());
3631 386 : if (hasVehicles) {
3632 380 : out.openTag(SUMO_TAG_VIEWSETTINGS_VEHICLES);
3633 380 : out.writeAttr(SUMO_ATTR_VALUE, myVehicles);
3634 760 : out.closeTag();
3635 : }
3636 386 : if (toRailJunction) {
3637 37 : for (const MSLink* link : myLinks) {
3638 20 : if (link->getApproaching().size() > 0) {
3639 17 : out.openTag(SUMO_TAG_LINK);
3640 : out.writeAttr(SUMO_ATTR_TO, link->getViaLaneOrLane()->getID());
3641 34 : for (auto item : link->getApproaching()) {
3642 17 : out.openTag(SUMO_TAG_APPROACHING);
3643 17 : out.writeAttr(SUMO_ATTR_ID, item.first->getID());
3644 : out.writeAttr(SUMO_ATTR_ARRIVALTIME, item.second.arrivalTime);
3645 : out.writeAttr(SUMO_ATTR_ARRIVALSPEED, item.second.arrivalSpeed);
3646 : out.writeAttr(SUMO_ATTR_DEPARTSPEED, item.second.leaveSpeed);
3647 : out.writeAttr(SUMO_ATTR_REQUEST, item.second.willPass);
3648 : out.writeAttr(SUMO_ATTR_ARRIVALSPEEDBRAKING, item.second.arrivalSpeedBraking);
3649 : out.writeAttr(SUMO_ATTR_WAITINGTIME, item.second.waitingTime);
3650 : out.writeAttr(SUMO_ATTR_DISTANCE, item.second.dist);
3651 17 : if (item.second.latOffset != 0) {
3652 : out.writeAttr(SUMO_ATTR_POSITION_LAT, item.second.latOffset);
3653 : }
3654 34 : out.closeTag();
3655 : }
3656 34 : out.closeTag();
3657 : }
3658 : }
3659 : }
3660 772 : out.closeTag();
3661 : }
3662 8854 : }
3663 :
3664 : void
3665 9609 : MSLane::clearState() {
3666 : myVehicles.clear();
3667 : myParkingVehicles.clear();
3668 : myPartialVehicles.clear();
3669 : myManeuverReservations.clear();
3670 9609 : myBruttoVehicleLengthSum = 0;
3671 9609 : myNettoVehicleLengthSum = 0;
3672 9609 : myBruttoVehicleLengthSumToRemove = 0;
3673 9609 : myNettoVehicleLengthSumToRemove = 0;
3674 9609 : myLeaderInfoTime = SUMOTime_MIN;
3675 9609 : myFollowerInfoTime = SUMOTime_MIN;
3676 22145 : for (MSLink* link : myLinks) {
3677 12536 : link->clearState();
3678 : }
3679 9609 : }
3680 :
3681 : void
3682 538 : MSLane::loadState(const std::vector<std::string>& vehIds, MSVehicleControl& vc) {
3683 1942 : for (const std::string& id : vehIds) {
3684 1404 : MSVehicle* v = dynamic_cast<MSVehicle*>(vc.getVehicle(id));
3685 : // vehicle could be removed due to options
3686 1400 : if (v != nullptr) {
3687 1400 : v->updateBestLanes(false, this);
3688 : // incorporateVehicle resets the lastActionTime (which has just been loaded from state) so we must restore it
3689 1400 : const SUMOTime lastActionTime = v->getLastActionTime();
3690 1400 : incorporateVehicle(v, v->getPositionOnLane(), v->getSpeed(), v->getLateralPositionOnLane(), myVehicles.end(),
3691 : MSMoveReminder::NOTIFICATION_LOAD_STATE);
3692 1400 : v->resetActionOffset(lastActionTime - MSNet::getInstance()->getCurrentTimeStep());
3693 1400 : v->processNextStop(v->getSpeed());
3694 : }
3695 : }
3696 538 : }
3697 :
3698 :
3699 : double
3700 2176038284 : MSLane::getVehicleStopOffset(const MSVehicle* veh) const {
3701 2176038284 : if (!myLaneStopOffset.isDefined()) {
3702 : return 0;
3703 : }
3704 88013 : if ((myLaneStopOffset.getPermissions() & veh->getVClass()) != 0) {
3705 28508 : return myLaneStopOffset.getOffset();
3706 : } else {
3707 : return 0;
3708 : }
3709 : }
3710 :
3711 :
3712 : const StopOffset&
3713 2980 : MSLane::getLaneStopOffsets() const {
3714 2980 : return myLaneStopOffset;
3715 : }
3716 :
3717 :
3718 : void
3719 2152 : MSLane::setLaneStopOffset(const StopOffset& stopOffset) {
3720 2152 : myLaneStopOffset = stopOffset;
3721 2152 : }
3722 :
3723 :
3724 : MSLeaderDistanceInfo
3725 271016856 : MSLane::getFollowersOnConsecutive(const MSVehicle* ego, double backOffset,
3726 : bool allSublanes, double searchDist, MinorLinkMode mLinkMode) const {
3727 : assert(ego != 0);
3728 : // get the follower vehicle on the lane to change to
3729 271016856 : const double egoPos = backOffset + ego->getVehicleType().getLength();
3730 271016856 : const double egoLatDist = ego->getLane()->getRightSideOnEdge() - getRightSideOnEdge();
3731 271977077 : const bool getOppositeLeaders = ((ego->getLaneChangeModel().isOpposite() && ego->getLane() == this)
3732 271146985 : || (!ego->getLaneChangeModel().isOpposite() && &ego->getLane()->getEdge() != &getEdge()));
3733 : #ifdef DEBUG_CONTEXT
3734 : if (DEBUG_COND2(ego)) {
3735 : std::cout << SIMTIME << " getFollowers lane=" << getID() << " ego=" << ego->getID()
3736 : << " backOffset=" << backOffset << " pos=" << egoPos
3737 : << " allSub=" << allSublanes << " searchDist=" << searchDist << " ignoreMinor=" << mLinkMode
3738 : << " egoLatDist=" << egoLatDist
3739 : << " getOppositeLeaders=" << getOppositeLeaders
3740 : << "\n";
3741 : }
3742 : #endif
3743 277646531 : MSCriticalFollowerDistanceInfo result(myWidth, allSublanes ? nullptr : ego, allSublanes ? 0 : egoLatDist, getOppositeLeaders);
3744 271016856 : if (MSGlobals::gLateralResolution > 0 && egoLatDist == 0) {
3745 : // check whether ego is outside lane bounds far enough so that another vehicle might
3746 : // be between itself and the first "actual" sublane
3747 : // shift the offset so that we "see" this vehicle
3748 179922258 : if (ego->getLeftSideOnLane() < -MSGlobals::gLateralResolution) {
3749 30281 : result.setSublaneOffset(int(-ego->getLeftSideOnLane() / MSGlobals::gLateralResolution));
3750 179891977 : } else if (ego->getRightSideOnLane() > getWidth() + MSGlobals::gLateralResolution) {
3751 107720 : result.setSublaneOffset(-int((ego->getRightSideOnLane() - getWidth()) / MSGlobals::gLateralResolution));
3752 : }
3753 : #ifdef DEBUG_CONTEXT
3754 : if (DEBUG_COND2(ego)) {
3755 : std::cout << SIMTIME << " getFollowers lane=" << getID() << " ego=" << ego->getID()
3756 : << " egoPosLat=" << ego->getLateralPositionOnLane()
3757 : << " egoLatDist=" << ego->getLane()->getRightSideOnEdge() - getRightSideOnEdge()
3758 : << " extraOffset=" << result.getSublaneOffset()
3759 : << "\n";
3760 : }
3761 : #endif
3762 : }
3763 : /// XXX iterate in reverse and abort when there are no more freeSublanes
3764 5482213762 : for (AnyVehicleIterator last = anyVehiclesBegin(); last != anyVehiclesEnd(); ++last) {
3765 5482213762 : const MSVehicle* veh = *last;
3766 : #ifdef DEBUG_CONTEXT
3767 : if (DEBUG_COND2(ego)) {
3768 : std::cout << " veh=" << veh->getID() << " lane=" << veh->getLane()->getID() << " pos=" << veh->getPositionOnLane(this) << "\n";
3769 : }
3770 : #endif
3771 5482213762 : if (veh != ego && veh->getPositionOnLane(this) < egoPos) {
3772 : //const double latOffset = veh->getLane()->getRightSideOnEdge() - getRightSideOnEdge();
3773 2438262928 : const double latOffset = veh->getLatOffset(this);
3774 2438262928 : double dist = backOffset - veh->getPositionOnLane(this) - veh->getVehicleType().getMinGap();
3775 2438262928 : if (veh->isBidiOn(this)) {
3776 98948 : dist -= veh->getLength();
3777 : }
3778 2438262928 : result.addFollower(veh, ego, dist, latOffset);
3779 : #ifdef DEBUG_CONTEXT
3780 : if (DEBUG_COND2(ego)) {
3781 : std::cout << " (1) added veh=" << veh->getID() << " latOffset=" << latOffset << " result=" << result.toString() << "\n";
3782 : }
3783 : #endif
3784 : }
3785 : }
3786 : #ifdef DEBUG_CONTEXT
3787 : if (DEBUG_COND2(ego)) {
3788 : std::cout << " result.numFreeSublanes=" << result.numFreeSublanes() << "\n";
3789 : }
3790 : #endif
3791 271016856 : if (result.numFreeSublanes() > 0) {
3792 : // do a tree search among all follower lanes and check for the most
3793 : // important vehicle (the one requiring the largest reargap)
3794 : // to get a safe bound on the necessary search depth, we need to consider the maximum speed and minimum
3795 : // deceleration of potential follower vehicles
3796 120378027 : if (searchDist == -1) {
3797 118952853 : searchDist = getMaximumBrakeDist() - backOffset;
3798 : #ifdef DEBUG_CONTEXT
3799 : if (DEBUG_COND2(ego)) {
3800 : std::cout << " computed searchDist=" << searchDist << "\n";
3801 : }
3802 : #endif
3803 : }
3804 : std::set<const MSEdge*> egoFurther;
3805 130463625 : for (MSLane* further : ego->getFurtherLanes()) {
3806 10085598 : egoFurther.insert(&further->getEdge());
3807 : }
3808 129719859 : if (ego->getPositionOnLane() < ego->getVehicleType().getLength() && egoFurther.size() == 0
3809 121002898 : && ego->getLane()->getLogicalPredecessorLane() != nullptr) {
3810 : // on insertion
3811 362216 : egoFurther.insert(&ego->getLane()->getLogicalPredecessorLane()->getEdge());
3812 : }
3813 :
3814 : // avoid loops
3815 120378027 : std::set<const MSLane*> visited(myEdge->getLanes().begin(), myEdge->getLanes().end());
3816 120378027 : if (myEdge->getBidiEdge() != nullptr) {
3817 : visited.insert(myEdge->getBidiEdge()->getLanes().begin(), myEdge->getBidiEdge()->getLanes().end());
3818 : }
3819 : std::vector<MSLane::IncomingLaneInfo> newFound;
3820 120378027 : std::vector<MSLane::IncomingLaneInfo> toExamine = myIncomingLanes;
3821 268797137 : while (toExamine.size() != 0) {
3822 328189246 : for (std::vector<MSLane::IncomingLaneInfo>::iterator it = toExamine.begin(); it != toExamine.end(); ++it) {
3823 179770136 : MSLane* next = (*it).lane;
3824 179770136 : searchDist = MAX2(searchDist, next->getMaximumBrakeDist() - backOffset);
3825 179770136 : MSLeaderInfo first = next->getFirstVehicleInformation(nullptr, 0, false, std::numeric_limits<double>::max(), false);
3826 179770136 : MSLeaderInfo firstFront = next->getFirstVehicleInformation(nullptr, 0, true);
3827 : #ifdef DEBUG_CONTEXT
3828 : if (DEBUG_COND2(ego)) {
3829 : std::cout << " next=" << next->getID() << " seen=" << (*it).length << " first=" << first.toString() << " firstFront=" << firstFront.toString() << " backOffset=" << backOffset << "\n";
3830 : gDebugFlag1 = true; // for calling getLeaderInfo
3831 : }
3832 : #endif
3833 179770136 : if (backOffset + (*it).length - next->getLength() < 0
3834 179770136 : && egoFurther.count(&next->getEdge()) != 0
3835 : ) {
3836 : // check for junction foes that would interfere with lane changing
3837 : // @note: we are passing the back of ego as its front position so
3838 : // we need to add this back to the returned gap
3839 10070221 : const MSLink::LinkLeaders linkLeaders = (*it).viaLink->getLeaderInfo(ego, -backOffset);
3840 10289995 : for (const auto& ll : linkLeaders) {
3841 219774 : if (ll.vehAndGap.first != nullptr) {
3842 219752 : const bool bidiFoe = (*it).viaLink->getLane() == ll.vehAndGap.first->getLane()->getNormalPredecessorLane()->getBidiLane();
3843 219752 : const bool egoIsLeader = !bidiFoe && ll.vehAndGap.first->isLeader((*it).viaLink, ego, ll.vehAndGap.second);
3844 : // if ego is leader the returned gap still assumes that ego follows the leader
3845 : // if the foe vehicle follows ego we need to deduce that gap
3846 : const double gap = (egoIsLeader
3847 219752 : ? -ll.vehAndGap.second - ll.vehAndGap.first->getVehicleType().getLengthWithGap() - ego->getVehicleType().getMinGap()
3848 311 : : ll.vehAndGap.second + ego->getVehicleType().getLength());
3849 219752 : result.addFollower(ll.vehAndGap.first, ego, gap);
3850 : #ifdef DEBUG_CONTEXT
3851 : if (DEBUG_COND2(ego)) {
3852 : std::cout << SIMTIME << " ego=" << ego->getID() << " link=" << (*it).viaLink->getViaLaneOrLane()->getID()
3853 : << " (3) added veh=" << Named::getIDSecure(ll.vehAndGap.first)
3854 : << " gap=" << ll.vehAndGap.second << " dtC=" << ll.distToCrossing
3855 : << " bidiFoe=" << bidiFoe
3856 : << " egoIsLeader=" << egoIsLeader << " gap2=" << gap
3857 : << "\n";
3858 : }
3859 : #endif
3860 : }
3861 : }
3862 10070221 : }
3863 : #ifdef DEBUG_CONTEXT
3864 : if (DEBUG_COND2(ego)) {
3865 : gDebugFlag1 = false;
3866 : }
3867 : #endif
3868 :
3869 708951384 : for (int i = 0; i < first.numSublanes(); ++i) {
3870 529181248 : const MSVehicle* v = first[i] == ego ? firstFront[i] : first[i];
3871 : double agap = 0;
3872 :
3873 529181248 : if (v != nullptr && v != ego) {
3874 187220436 : if (!v->isFrontOnLane(next)) {
3875 : // the front of v is already on divergent trajectory from the ego vehicle
3876 : // for which this method is called (in the context of MSLaneChanger).
3877 : // Therefore, technically v is not a follower but only an obstruction and
3878 : // the gap is not between the front of v and the back of ego
3879 : // but rather between the flank of v and the back of ego.
3880 26552620 : agap = (*it).length - next->getLength() + backOffset
3881 : /// XXX dubious term. here for backwards compatibility
3882 26552620 : - v->getVehicleType().getMinGap();
3883 : #ifdef DEBUG_CONTEXT
3884 : if (DEBUG_COND2(ego)) {
3885 : std::cout << " agap1=" << agap << "\n";
3886 : }
3887 : #endif
3888 26552620 : if (agap > 0 && &v->getLane()->getEdge() != &ego->getLane()->getEdge()) {
3889 : // Only if ego overlaps we treat v as if it were a real follower
3890 : // Otherwise we ignore it and look for another follower
3891 17481645 : if (!getOppositeLeaders) {
3892 : // even if the vehicle is not a real
3893 : // follower, it still forms a real
3894 : // obstruction in opposite direction driving
3895 17295631 : v = firstFront[i];
3896 17295631 : if (v != nullptr && v != ego) {
3897 13584031 : agap = (*it).length - v->getPositionOnLane() + backOffset - v->getVehicleType().getMinGap();
3898 : } else {
3899 : v = nullptr;
3900 : }
3901 : }
3902 : }
3903 : } else {
3904 160667816 : if (next->getBidiLane() != nullptr && v->isBidiOn(next)) {
3905 70943 : agap = v->getPositionOnLane() + backOffset - v->getVehicleType().getLengthWithGap();
3906 : } else {
3907 160596873 : agap = (*it).length - v->getPositionOnLane() + backOffset - v->getVehicleType().getMinGap();
3908 : }
3909 160667816 : if (!(*it).viaLink->havePriority() && !ego->onFurtherEdge(&(*it).lane->getEdge())
3910 10302292 : && ego->isOnRoad() // during insertion, this can lead to collisions because ego's further lanes are not set (see #3053)
3911 10202775 : && !ego->getLaneChangeModel().isOpposite()
3912 170868744 : && v->getSpeed() < SUMO_const_haltingSpeed
3913 : ) {
3914 : // if v is stopped on a minor side road it should not block lane changing
3915 : agap = MAX2(agap, 0.0);
3916 : }
3917 : }
3918 187220436 : result.addFollower(v, ego, agap, 0, i);
3919 : #ifdef DEBUG_CONTEXT
3920 : if (DEBUG_COND2(ego)) {
3921 : std::cout << " (2) added veh=" << Named::getIDSecure(v) << " agap=" << agap << " next=" << next->getID() << " result=" << result.toString() << "\n";
3922 : }
3923 : #endif
3924 : }
3925 : }
3926 179770136 : if ((*it).length < searchDist) {
3927 : const std::vector<MSLane::IncomingLaneInfo>& followers = next->getIncomingLanes();
3928 150340483 : for (std::vector<MSLane::IncomingLaneInfo>::const_iterator j = followers.begin(); j != followers.end(); ++j) {
3929 76107603 : if (visited.find((*j).lane) == visited.end() && (((*j).viaLink->havePriority() && !(*j).viaLink->isTurnaround())
3930 17325192 : || mLinkMode == MinorLinkMode::FOLLOW_ALWAYS
3931 1629704 : || (mLinkMode == MinorLinkMode::FOLLOW_ONCOMING && (*j).viaLink->getDirection() == LinkDirection::STRAIGHT))) {
3932 73094151 : visited.insert((*j).lane);
3933 : MSLane::IncomingLaneInfo ili;
3934 73094151 : ili.lane = (*j).lane;
3935 73094151 : ili.length = (*j).length + (*it).length;
3936 73094151 : ili.viaLink = (*j).viaLink;
3937 73094151 : newFound.push_back(ili);
3938 : }
3939 : }
3940 : }
3941 179770136 : }
3942 : toExamine.clear();
3943 : swap(newFound, toExamine);
3944 : }
3945 : //return result;
3946 :
3947 120378027 : }
3948 542033712 : return result;
3949 271016856 : }
3950 :
3951 :
3952 : void
3953 14974890 : MSLane::getLeadersOnConsecutive(double dist, double seen, double speed, const MSVehicle* ego,
3954 : const std::vector<MSLane*>& bestLaneConts, MSLeaderDistanceInfo& result,
3955 : bool oppositeDirection) const {
3956 14974890 : if (seen > dist && !(isInternal() && MSGlobals::gComputeLC)) {
3957 : return;
3958 : }
3959 : // check partial vehicles (they might be on a different route and thus not
3960 : // found when iterating along bestLaneConts)
3961 15689225 : for (VehCont::const_iterator it = myPartialVehicles.begin(); it != myPartialVehicles.end(); ++it) {
3962 2391592 : MSVehicle* veh = *it;
3963 2391592 : if (!veh->isFrontOnLane(this)) {
3964 714335 : result.addLeader(veh, seen, veh->getLatOffset(this));
3965 : } else {
3966 : break;
3967 : }
3968 : }
3969 : #ifdef DEBUG_CONTEXT
3970 : if (DEBUG_COND2(ego)) {
3971 : gDebugFlag1 = true;
3972 : }
3973 : #endif
3974 : const MSLane* nextLane = this;
3975 : int view = 1;
3976 : // loop over following lanes
3977 31774924 : while ((seen < dist || nextLane->isInternal()) && result.numFreeSublanes() > 0) {
3978 : // get the next link used
3979 : bool nextInternal = false;
3980 21545189 : if (oppositeDirection) {
3981 9262 : if (view >= (int)bestLaneConts.size()) {
3982 : break;
3983 : }
3984 4318 : nextLane = bestLaneConts[view];
3985 : } else {
3986 21535927 : std::vector<MSLink*>::const_iterator link = succLinkSec(*ego, view, *nextLane, bestLaneConts);
3987 21535927 : if (nextLane->isLinkEnd(link)) {
3988 : break;
3989 : }
3990 : // check for link leaders
3991 17643135 : const MSLink::LinkLeaders linkLeaders = (*link)->getLeaderInfo(ego, seen);
3992 17643135 : if (linkLeaders.size() > 0) {
3993 947424 : const MSLink::LinkLeader ll = linkLeaders[0];
3994 947424 : MSVehicle* veh = ll.vehAndGap.first;
3995 : // in the context of lane changing all junction leader candidates must be respected
3996 947424 : if (veh != 0 && (ego->isLeader(*link, veh, ll.vehAndGap.second)
3997 94248 : || (MSGlobals::gComputeLC
3998 1292767 : && veh->getPosition().distanceTo2D(ego->getPosition()) - veh->getVehicleType().getMinGap() - ego->getVehicleType().getLength()
3999 94248 : < veh->getCarFollowModel().brakeGap(veh->getSpeed())))) {
4000 : #ifdef DEBUG_CONTEXT
4001 : if (DEBUG_COND2(ego)) {
4002 : std::cout << " linkleader=" << veh->getID() << " gap=" << ll.vehAndGap.second << " leaderOffset=" << ll.latOffset << " flags=" << ll.llFlags << "\n";
4003 : }
4004 : #endif
4005 847419 : if (ll.sameTarget() || ll.sameSource()) {
4006 588202 : result.addLeader(veh, ll.vehAndGap.second, ll.latOffset);
4007 : } else {
4008 : // add link leader to all sublanes and return
4009 1745926 : for (int i = 0; i < result.numSublanes(); ++i) {
4010 1486709 : result.addLeader(veh, ll.vehAndGap.second, 0, i);
4011 : }
4012 : }
4013 : #ifdef DEBUG_CONTEXT
4014 : gDebugFlag1 = false;
4015 : #endif
4016 847419 : return;
4017 : } // XXX else, deal with pedestrians
4018 : }
4019 16795716 : nextInternal = (*link)->getViaLane() != nullptr;
4020 : nextLane = (*link)->getViaLaneOrLane();
4021 9967901 : if (nextLane == nullptr) {
4022 : break;
4023 : }
4024 17643135 : }
4025 :
4026 16800034 : MSLeaderInfo leaders = nextLane->getLastVehicleInformation(nullptr, 0, 0, false);
4027 : #ifdef DEBUG_CONTEXT
4028 : if (DEBUG_COND2(ego)) {
4029 : std::cout << SIMTIME << " getLeadersOnConsecutive lane=" << getID() << " nextLane=" << nextLane->getID() << " leaders=" << leaders.toString() << "\n";
4030 : }
4031 : #endif
4032 : // @todo check alignment issues if the lane width changes
4033 : const int iMax = MIN2(leaders.numSublanes(), result.numSublanes());
4034 89970441 : for (int i = 0; i < iMax; ++i) {
4035 73170407 : const MSVehicle* veh = leaders[i];
4036 73170407 : if (veh != nullptr) {
4037 : #ifdef DEBUG_CONTEXT
4038 : if (DEBUG_COND2(ego)) std::cout << " lead=" << veh->getID()
4039 : << " seen=" << seen
4040 : << " minGap=" << ego->getVehicleType().getMinGap()
4041 : << " backPos=" << veh->getBackPositionOnLane(nextLane)
4042 : << " gap=" << seen - ego->getVehicleType().getMinGap() + veh->getBackPositionOnLane(nextLane)
4043 : << "\n";
4044 : #endif
4045 29897500 : result.addLeader(veh, seen - ego->getVehicleType().getMinGap() + veh->getBackPositionOnLane(nextLane), 0, i);
4046 : }
4047 : }
4048 :
4049 16800034 : if (nextLane->getVehicleMaxSpeed(ego) < speed) {
4050 909713 : dist = ego->getCarFollowModel().brakeGap(nextLane->getVehicleMaxSpeed(ego));
4051 : }
4052 16800034 : seen += nextLane->getLength();
4053 16800034 : if (!nextInternal) {
4054 9972219 : view++;
4055 : }
4056 16800034 : }
4057 : #ifdef DEBUG_CONTEXT
4058 : gDebugFlag1 = false;
4059 : #endif
4060 : }
4061 :
4062 :
4063 : void
4064 112626185 : MSLane::addLeaders(const MSVehicle* vehicle, double vehPos, MSLeaderDistanceInfo& result, bool opposite) {
4065 : // if there are vehicles on the target lane with the same position as ego,
4066 : // they may not have been added to 'ahead' yet
4067 : #ifdef DEBUG_SURROUNDING
4068 : if (DEBUG_COND || DEBUG_COND2(vehicle)) {
4069 : std::cout << " addLeaders lane=" << getID() << " veh=" << vehicle->getID() << " vehPos=" << vehPos << " opposite=" << opposite << "\n";
4070 : }
4071 : #endif
4072 112626185 : const MSLeaderInfo& aheadSamePos = getLastVehicleInformation(nullptr, 0, vehPos, false);
4073 584270874 : for (int i = 0; i < aheadSamePos.numSublanes(); ++i) {
4074 471644689 : const MSVehicle* veh = aheadSamePos[i];
4075 471644689 : if (veh != nullptr && veh != vehicle) {
4076 126049696 : const double gap = veh->getBackPositionOnLane(this) - vehPos - vehicle->getVehicleType().getMinGap();
4077 : #ifdef DEBUG_SURROUNDING
4078 : if (DEBUG_COND || DEBUG_COND2(vehicle)) {
4079 : std::cout << " further lead=" << veh->getID() << " leadBack=" << veh->getBackPositionOnLane(this) << " gap=" << gap << "\n";
4080 : }
4081 : #endif
4082 126049696 : result.addLeader(veh, gap, 0, i);
4083 : }
4084 : }
4085 :
4086 112626185 : if (result.numFreeSublanes() > 0) {
4087 32954290 : double seen = vehicle->getLane()->getLength() - vehPos;
4088 32954290 : double speed = vehicle->getSpeed();
4089 : // leader vehicle could be link leader on the next junction
4090 32954290 : double dist = MAX2(vehicle->getCarFollowModel().brakeGap(speed), 10.0) + vehicle->getVehicleType().getMinGap();
4091 32954290 : if (getBidiLane() != nullptr) {
4092 48730 : dist = MAX2(dist, myMaxSpeed * 20);
4093 : }
4094 : // check for link leaders when on internal
4095 32954290 : if (seen > dist && !(isInternal() && MSGlobals::gComputeLC)) {
4096 : #ifdef DEBUG_SURROUNDING
4097 : if (DEBUG_COND || DEBUG_COND2(vehicle)) {
4098 : std::cout << " aborting forward search. dist=" << dist << " seen=" << seen << "\n";
4099 : }
4100 : #endif
4101 : return;
4102 : }
4103 : #ifdef DEBUG_SURROUNDING
4104 : if (DEBUG_COND || DEBUG_COND2(vehicle)) {
4105 : std::cout << " add consecutive before=" << result.toString() << " seen=" << seen << " dist=" << dist;
4106 : }
4107 : #endif
4108 14974890 : if (opposite) {
4109 7302 : const std::vector<MSLane*> bestLaneConts = vehicle->getUpstreamOppositeLanes();
4110 : #ifdef DEBUG_SURROUNDING
4111 : if (DEBUG_COND || DEBUG_COND2(vehicle)) {
4112 : std::cout << " upstreamOpposite=" << toString(bestLaneConts);
4113 : }
4114 : #endif
4115 7302 : getLeadersOnConsecutive(dist, seen, speed, vehicle, bestLaneConts, result, opposite);
4116 7302 : } else {
4117 14967588 : const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation(this);
4118 14967588 : getLeadersOnConsecutive(dist, seen, speed, vehicle, bestLaneConts, result);
4119 : }
4120 : #ifdef DEBUG_SURROUNDING
4121 : if (DEBUG_COND || DEBUG_COND2(vehicle)) {
4122 : std::cout << " after=" << result.toString() << "\n";
4123 : }
4124 : #endif
4125 : }
4126 112626185 : }
4127 :
4128 :
4129 : MSVehicle*
4130 381103178 : MSLane::getPartialBehind(const MSVehicle* ego) const {
4131 405716878 : for (VehCont::const_reverse_iterator i = myPartialVehicles.rbegin(); i != myPartialVehicles.rend(); ++i) {
4132 26290342 : MSVehicle* veh = *i;
4133 26290342 : if (veh->isFrontOnLane(this)
4134 3169665 : && veh != ego
4135 29460007 : && veh->getPositionOnLane() <= ego->getPositionOnLane()) {
4136 : #ifdef DEBUG_CONTEXT
4137 : if (DEBUG_COND2(ego)) {
4138 : std::cout << SIMTIME << " getPartialBehind lane=" << getID() << " ego=" << ego->getID() << " found=" << veh->getID() << "\n";
4139 : }
4140 : #endif
4141 : return veh;
4142 : }
4143 : }
4144 : #ifdef DEBUG_CONTEXT
4145 : if (DEBUG_COND2(ego)) {
4146 : std::cout << SIMTIME << " getPartialBehind lane=" << getID() << " ego=" << ego->getID() << " nothing found. partials=" << toString(myPartialVehicles) << "\n";
4147 : }
4148 : #endif
4149 : return nullptr;
4150 : }
4151 :
4152 : MSLeaderInfo
4153 18044471 : MSLane::getPartialBeyond() const {
4154 18044471 : MSLeaderInfo result(myWidth);
4155 19371459 : for (VehCont::const_iterator it = myPartialVehicles.begin(); it != myPartialVehicles.end(); ++it) {
4156 2695499 : MSVehicle* veh = *it;
4157 2695499 : if (!veh->isFrontOnLane(this)) {
4158 1326988 : result.addLeader(veh, false, veh->getLatOffset(this));
4159 : } else {
4160 : break;
4161 : }
4162 : }
4163 18044471 : return result;
4164 0 : }
4165 :
4166 :
4167 : std::set<MSVehicle*>
4168 11180 : MSLane::getSurroundingVehicles(double startPos, double downstreamDist, double upstreamDist, std::shared_ptr<LaneCoverageInfo> checkedLanes) const {
4169 : assert(checkedLanes != nullptr);
4170 11180 : if (checkedLanes->find(this) != checkedLanes->end()) {
4171 : #ifdef DEBUG_SURROUNDING
4172 : std::cout << "Skipping previously scanned lane: " << getID() << std::endl;
4173 : #endif
4174 2266 : return std::set<MSVehicle*>();
4175 : } else {
4176 : // Add this lane's coverage to the lane coverage info
4177 18841 : (*checkedLanes)[this] = std::make_pair(MAX2(0.0, startPos - upstreamDist), MIN2(startPos + downstreamDist, getLength()));
4178 : }
4179 : #ifdef DEBUG_SURROUNDING
4180 : std::cout << "Scanning on lane " << myID << "(downstr. " << downstreamDist << ", upstr. " << upstreamDist << ", startPos " << startPos << "): " << std::endl;
4181 : #endif
4182 14492 : std::set<MSVehicle*> foundVehicles = getVehiclesInRange(MAX2(0., startPos - upstreamDist), MIN2(myLength, startPos + downstreamDist));
4183 8914 : if (startPos < upstreamDist) {
4184 : // scan incoming lanes
4185 10886 : for (const IncomingLaneInfo& incomingInfo : getIncomingLanes()) {
4186 5412 : MSLane* incoming = incomingInfo.lane;
4187 : #ifdef DEBUG_SURROUNDING
4188 : std::cout << "Checking on incoming: " << incoming->getID() << std::endl;
4189 : if (checkedLanes->find(incoming) != checkedLanes->end()) {
4190 : std::cout << "Skipping previous: " << incoming->getID() << std::endl;
4191 : }
4192 : #endif
4193 10824 : std::set<MSVehicle*> newVehs = incoming->getSurroundingVehicles(incoming->getLength(), 0.0, upstreamDist - startPos, checkedLanes);
4194 5412 : foundVehicles.insert(newVehs.begin(), newVehs.end());
4195 : }
4196 : }
4197 :
4198 8914 : if (getLength() < startPos + downstreamDist) {
4199 : // scan successive lanes
4200 : const std::vector<MSLink*>& lc = getLinkCont();
4201 6130 : for (MSLink* l : lc) {
4202 : #ifdef DEBUG_SURROUNDING
4203 : std::cout << "Checking on outgoing: " << l->getViaLaneOrLane()->getID() << std::endl;
4204 : #endif
4205 5588 : std::set<MSVehicle*> newVehs = l->getViaLaneOrLane()->getSurroundingVehicles(0.0, downstreamDist - (myLength - startPos), upstreamDist, checkedLanes);
4206 2794 : foundVehicles.insert(newVehs.begin(), newVehs.end());
4207 : }
4208 : }
4209 : #ifdef DEBUG_SURROUNDING
4210 : std::cout << "On lane (2) " << myID << ": \nFound vehicles: " << std::endl;
4211 : for (MSVehicle* v : foundVehicles) {
4212 : std::cout << v->getID() << " pos = " << v->getPositionOnLane() << std::endl;
4213 : }
4214 : #endif
4215 : return foundVehicles;
4216 : }
4217 :
4218 :
4219 : std::set<MSVehicle*>
4220 11749 : MSLane::getVehiclesInRange(const double a, const double b) const {
4221 : std::set<MSVehicle*> res;
4222 11749 : const VehCont& vehs = getVehiclesSecure();
4223 :
4224 11749 : if (!vehs.empty()) {
4225 12545 : for (MSVehicle* const veh : vehs) {
4226 8356 : if (veh->getPositionOnLane() >= a) {
4227 7012 : if (veh->getBackPositionOnLane() > b) {
4228 : break;
4229 : }
4230 : res.insert(veh);
4231 : }
4232 : }
4233 : }
4234 11749 : releaseVehicles();
4235 11749 : return res;
4236 : }
4237 :
4238 :
4239 : std::vector<const MSJunction*>
4240 0 : MSLane::getUpcomingJunctions(double pos, double range, const std::vector<MSLane*>& contLanes) const {
4241 : // set of upcoming junctions and the corresponding conflict links
4242 : std::vector<const MSJunction*> junctions;
4243 0 : for (auto l : getUpcomingLinks(pos, range, contLanes)) {
4244 0 : junctions.insert(junctions.end(), l->getJunction());
4245 0 : }
4246 0 : return junctions;
4247 0 : }
4248 :
4249 :
4250 : std::vector<const MSLink*>
4251 761 : MSLane::getUpcomingLinks(double pos, double range, const std::vector<MSLane*>& contLanes) const {
4252 : #ifdef DEBUG_SURROUNDING
4253 : std::cout << "getUpcoming links on lane '" << getID() << "' with pos=" << pos
4254 : << " range=" << range << std::endl;
4255 : #endif
4256 : // set of upcoming junctions and the corresponding conflict links
4257 : std::vector<const MSLink*> links;
4258 :
4259 : // Currently scanned lane
4260 : const MSLane* lane = this;
4261 :
4262 : // continuation lanes for the vehicle
4263 : std::vector<MSLane*>::const_iterator contLanesIt = contLanes.begin();
4264 : // scanned distance so far
4265 : double dist = 0.0;
4266 : // link to be crossed by the vehicle
4267 761 : const MSLink* link = nullptr;
4268 761 : if (lane->isInternal()) {
4269 : assert(*contLanesIt == nullptr); // is called with vehicle's bestLane structure
4270 116 : link = lane->getEntryLink();
4271 116 : links.insert(links.end(), link);
4272 116 : dist += link->getInternalLengthsAfter();
4273 : // next non-internal lane behind junction
4274 : lane = link->getLane();
4275 : pos = 0.0;
4276 : assert(*(contLanesIt + 1) == lane);
4277 : }
4278 1266 : while (++contLanesIt != contLanes.end()) {
4279 : assert(!lane->isInternal());
4280 969 : dist += lane->getLength() - pos;
4281 : pos = 0.;
4282 : #ifdef DEBUG_SURROUNDING
4283 : std::cout << "Distance until end of lane '" << lane->getID() << "' is " << dist << "." << std::endl;
4284 : #endif
4285 969 : if (dist > range) {
4286 : break;
4287 : }
4288 505 : link = lane->getLinkTo(*contLanesIt);
4289 505 : if (link != nullptr) {
4290 493 : links.insert(links.end(), link);
4291 : }
4292 505 : lane = *contLanesIt;
4293 : }
4294 761 : return links;
4295 0 : }
4296 :
4297 :
4298 : MSLane*
4299 222122938 : MSLane::getOpposite() const {
4300 222122938 : return myOpposite;
4301 : }
4302 :
4303 :
4304 : MSLane*
4305 168885677 : MSLane::getParallelOpposite() const {
4306 168885677 : return myEdge->getLanes().back()->getOpposite();
4307 : }
4308 :
4309 :
4310 : double
4311 7909840 : MSLane::getOppositePos(double pos) const {
4312 7909840 : return MAX2(0., myLength - pos);
4313 : }
4314 :
4315 : std::pair<MSVehicle* const, double>
4316 9838660 : MSLane::getFollower(const MSVehicle* ego, double egoPos, double dist, MinorLinkMode mLinkMode) const {
4317 34649107 : for (AnyVehicleIterator first = anyVehiclesUpstreamBegin(); first != anyVehiclesUpstreamEnd(); ++first) {
4318 : // XXX refactor leaderInfo to use a const vehicle all the way through the call hierarchy
4319 42867502 : MSVehicle* pred = (MSVehicle*)*first;
4320 : #ifdef DEBUG_CONTEXT
4321 : if (DEBUG_COND2(ego)) {
4322 : std::cout << " getFollower lane=" << getID() << " egoPos=" << egoPos << " pred=" << pred->getID() << " predPos=" << pred->getPositionOnLane(this) << "\n";
4323 : }
4324 : #endif
4325 42867502 : if (pred != ego && pred->getPositionOnLane(this) < egoPos) {
4326 8218395 : return std::pair<MSVehicle* const, double>(pred, egoPos - pred->getPositionOnLane(this) - ego->getVehicleType().getLength() - pred->getVehicleType().getMinGap());
4327 : }
4328 : }
4329 1620265 : const double backOffset = egoPos - ego->getVehicleType().getLength();
4330 1620265 : if (dist > 0 && backOffset > dist) {
4331 247310 : return std::make_pair(nullptr, -1);
4332 : }
4333 1372955 : const MSLeaderDistanceInfo followers = getFollowersOnConsecutive(ego, backOffset, true, dist, mLinkMode);
4334 1372955 : CLeaderDist result = followers.getClosest();
4335 1372955 : return std::make_pair(const_cast<MSVehicle*>(result.first), result.second);
4336 1372955 : }
4337 :
4338 : std::pair<MSVehicle* const, double>
4339 5477869 : MSLane::getOppositeLeader(const MSVehicle* ego, double dist, bool oppositeDir, MinorLinkMode mLinkMode) const {
4340 : #ifdef DEBUG_OPPOSITE
4341 : if (DEBUG_COND2(ego)) std::cout << SIMTIME << " getOppositeLeader lane=" << getID()
4342 : << " ego=" << ego->getID()
4343 : << " pos=" << ego->getPositionOnLane()
4344 : << " posOnOpposite=" << getOppositePos(ego->getPositionOnLane())
4345 : << " dist=" << dist
4346 : << " oppositeDir=" << oppositeDir
4347 : << "\n";
4348 : #endif
4349 5477869 : if (!oppositeDir) {
4350 387238 : return getLeader(ego, getOppositePos(ego->getPositionOnLane()), ego->getBestLanesContinuation(this));
4351 : } else {
4352 5090631 : const double egoLength = ego->getVehicleType().getLength();
4353 5090631 : const double egoPos = ego->getLaneChangeModel().isOpposite() ? ego->getPositionOnLane() : getOppositePos(ego->getPositionOnLane());
4354 5090631 : std::pair<MSVehicle* const, double> result = getFollower(ego, egoPos + egoLength, dist, mLinkMode);
4355 5090631 : if (result.first != nullptr) {
4356 4445924 : result.second -= ego->getVehicleType().getMinGap();
4357 4445924 : if (result.first->getLaneChangeModel().isOpposite()) {
4358 1324341 : result.second -= result.first->getVehicleType().getLength();
4359 : }
4360 : }
4361 5090631 : return result;
4362 : }
4363 : }
4364 :
4365 :
4366 : std::pair<MSVehicle* const, double>
4367 701358 : MSLane::getOppositeFollower(const MSVehicle* ego) const {
4368 : #ifdef DEBUG_OPPOSITE
4369 : if (DEBUG_COND2(ego)) std::cout << SIMTIME << " getOppositeFollower lane=" << getID()
4370 : << " ego=" << ego->getID()
4371 : << " backPos=" << ego->getBackPositionOnLane()
4372 : << " posOnOpposite=" << getOppositePos(ego->getBackPositionOnLane())
4373 : << "\n";
4374 : #endif
4375 701358 : if (ego->getLaneChangeModel().isOpposite()) {
4376 417834 : std::pair<MSVehicle* const, double> result = getFollower(ego, getOppositePos(ego->getPositionOnLane()), -1, MinorLinkMode::FOLLOW_NEVER);
4377 417834 : return result;
4378 : } else {
4379 283524 : double vehPos = getOppositePos(ego->getPositionOnLane() - ego->getVehicleType().getLength());
4380 283524 : std::pair<MSVehicle*, double> result = getLeader(ego, vehPos, std::vector<MSLane*>());
4381 283524 : double dist = getMaximumBrakeDist() + getOppositePos(ego->getPositionOnLane() - getLength());
4382 : MSLane* next = const_cast<MSLane*>(this);
4383 510524 : while (result.first == nullptr && dist > 0) {
4384 : // cannot call getLeadersOnConsecutive because succLinkSec doesn't
4385 : // uses the vehicle's route and doesn't work on the opposite side
4386 284057 : vehPos -= next->getLength();
4387 284057 : next = next->getCanonicalSuccessorLane();
4388 284057 : if (next == nullptr) {
4389 : break;
4390 : }
4391 227000 : dist -= next->getLength();
4392 227000 : result = next->getLeader(ego, vehPos, std::vector<MSLane*>());
4393 : }
4394 283524 : if (result.first != nullptr) {
4395 221572 : if (result.first->getLaneChangeModel().isOpposite()) {
4396 82367 : result.second -= result.first->getVehicleType().getLength();
4397 : } else {
4398 139205 : if (result.second > POSITION_EPS) {
4399 : // follower can be safely ignored since it is going the other way
4400 131512 : return std::make_pair(static_cast<MSVehicle*>(nullptr), -1);
4401 : }
4402 : }
4403 : }
4404 152012 : return result;
4405 : }
4406 : }
4407 :
4408 : void
4409 85990 : MSLane::initCollisionAction(const OptionsCont& oc, const std::string& option, CollisionAction& myAction) {
4410 85990 : const std::string action = oc.getString(option);
4411 85990 : if (action == "none") {
4412 15 : myAction = COLLISION_ACTION_NONE;
4413 85975 : } else if (action == "warn") {
4414 43469 : myAction = COLLISION_ACTION_WARN;
4415 42506 : } else if (action == "teleport") {
4416 42471 : myAction = COLLISION_ACTION_TELEPORT;
4417 35 : } else if (action == "remove") {
4418 35 : myAction = COLLISION_ACTION_REMOVE;
4419 : } else {
4420 0 : WRITE_ERROR(TLF("Invalid % '%'.", option, action));
4421 : }
4422 85990 : }
4423 :
4424 : void
4425 42995 : MSLane::initCollisionOptions(const OptionsCont& oc) {
4426 42995 : initCollisionAction(oc, "collision.action", myCollisionAction);
4427 42995 : initCollisionAction(oc, "intermodal-collision.action", myIntermodalCollisionAction);
4428 42995 : myCheckJunctionCollisions = oc.getBool("collision.check-junctions");
4429 42995 : myCheckJunctionCollisionMinGap = oc.getFloat("collision.check-junctions.mingap");
4430 42995 : myCollisionStopTime = string2time(oc.getString("collision.stoptime"));
4431 42995 : myIntermodalCollisionStopTime = string2time(oc.getString("intermodal-collision.stoptime"));
4432 42995 : myCollisionMinGapFactor = oc.getFloat("collision.mingap-factor");
4433 42995 : myExtrapolateSubstepDepart = oc.getBool("extrapolate-departpos");
4434 42995 : }
4435 :
4436 :
4437 : void
4438 676 : MSLane::setPermissions(SVCPermissions permissions, long long transientID) {
4439 676 : if (transientID == CHANGE_PERMISSIONS_PERMANENT) {
4440 65 : myPermissions = permissions;
4441 65 : myOriginalPermissions = permissions;
4442 : } else {
4443 611 : myPermissionChanges[transientID] = permissions;
4444 611 : resetPermissions(CHANGE_PERMISSIONS_PERMANENT);
4445 : }
4446 676 : }
4447 :
4448 :
4449 : void
4450 828 : MSLane::resetPermissions(long long transientID) {
4451 : myPermissionChanges.erase(transientID);
4452 828 : if (myPermissionChanges.empty()) {
4453 203 : myPermissions = myOriginalPermissions;
4454 : } else {
4455 : // combine all permission changes
4456 625 : myPermissions = SVCAll;
4457 1264 : for (const auto& item : myPermissionChanges) {
4458 639 : myPermissions &= item.second;
4459 : }
4460 : }
4461 828 : }
4462 :
4463 :
4464 : bool
4465 11250437 : MSLane::hadPermissionChanges() const {
4466 11250437 : return !myPermissionChanges.empty();
4467 : }
4468 :
4469 :
4470 : void
4471 9 : MSLane::setChangeLeft(SVCPermissions permissions) {
4472 9 : myChangeLeft = permissions;
4473 9 : }
4474 :
4475 :
4476 : void
4477 9 : MSLane::setChangeRight(SVCPermissions permissions) {
4478 9 : myChangeRight = permissions;
4479 9 : }
4480 :
4481 :
4482 : bool
4483 42754683 : MSLane::hasPedestrians() const {
4484 42754683 : MSNet* const net = MSNet::getInstance();
4485 42754683 : return net->hasPersons() && net->getPersonControl().getMovementModel()->hasPedestrians(this);
4486 : }
4487 :
4488 :
4489 : PersonDist
4490 683320 : MSLane::nextBlocking(double minPos, double minRight, double maxLeft, double stopTime, bool bidi) const {
4491 683320 : return MSNet::getInstance()->getPersonControl().getMovementModel()->nextBlocking(this, minPos, minRight, maxLeft, stopTime, bidi);
4492 : }
4493 :
4494 :
4495 : bool
4496 3419263 : MSLane::checkForPedestrians(const MSVehicle* aVehicle, double& speed, double& dist, double pos, bool patchSpeed) const {
4497 3419263 : if (getEdge().getPersons().size() > 0 && hasPedestrians()) {
4498 : #ifdef DEBUG_INSERTION
4499 : if (DEBUG_COND2(aVehicle)) {
4500 : std::cout << SIMTIME << " check for pedestrians on lane=" << getID() << " pos=" << pos << "\n";
4501 : }
4502 : #endif
4503 3792 : PersonDist leader = nextBlocking(pos - aVehicle->getVehicleType().getLength(),
4504 1896 : aVehicle->getRightSideOnLane(), aVehicle->getRightSideOnLane() + aVehicle->getVehicleType().getWidth(), ceil(speed / aVehicle->getCarFollowModel().getMaxDecel()));
4505 1896 : if (leader.first != 0) {
4506 872 : const double gap = leader.second - aVehicle->getVehicleType().getLengthWithGap();
4507 872 : const double stopSpeed = aVehicle->getCarFollowModel().stopSpeed(aVehicle, speed, gap, MSCFModel::CalcReason::FUTURE);
4508 336 : if ((gap < 0 && (getInsertionChecks(aVehicle) & ((int)InsertionCheck::COLLISION | (int)InsertionCheck::PEDESTRIAN)) != 0)
4509 1744 : || checkFailure(aVehicle, speed, dist, stopSpeed, patchSpeed, "", InsertionCheck::PEDESTRIAN)) {
4510 : // we may not drive with the given velocity - we crash into the pedestrian
4511 : #ifdef DEBUG_INSERTION
4512 : if (DEBUG_COND2(aVehicle)) std::cout << SIMTIME
4513 : << " isInsertionSuccess lane=" << getID()
4514 : << " veh=" << aVehicle->getID()
4515 : << " pos=" << pos
4516 : << " posLat=" << aVehicle->getLateralPositionOnLane()
4517 : << " patchSpeed=" << patchSpeed
4518 : << " speed=" << speed
4519 : << " stopSpeed=" << stopSpeed
4520 : << " pedestrianLeader=" << leader.first->getID()
4521 : << " failed (@796)!\n";
4522 : #endif
4523 550 : return false;
4524 : }
4525 : }
4526 : }
4527 : return true;
4528 : }
4529 :
4530 :
4531 : void
4532 42997 : MSLane::initRNGs(const OptionsCont& oc) {
4533 : myRNGs.clear();
4534 42997 : const int numRNGs = oc.getInt("thread-rngs");
4535 42997 : const bool random = oc.getBool("random");
4536 42997 : int seed = oc.getInt("seed");
4537 42997 : myRNGs.reserve(numRNGs); // this is needed for stable pointers on debugging
4538 2794805 : for (int i = 0; i < numRNGs; i++) {
4539 5503616 : myRNGs.push_back(SumoRNG("lanes_" + toString(i)));
4540 2751808 : RandHelper::initRand(&myRNGs.back(), random, seed++);
4541 : }
4542 42997 : }
4543 :
4544 : void
4545 6 : MSLane::saveRNGStates(OutputDevice& out) {
4546 390 : for (int i = 0; i < getNumRNGs(); i++) {
4547 384 : out.openTag(SUMO_TAG_RNGLANE);
4548 : out.writeAttr(SUMO_ATTR_INDEX, i);
4549 384 : out.writeAttr(SUMO_ATTR_STATE, RandHelper::saveState(&myRNGs[i]));
4550 768 : out.closeTag();
4551 : }
4552 6 : }
4553 :
4554 : void
4555 384 : MSLane::loadRNGState(int index, const std::string& state) {
4556 384 : if (index >= getNumRNGs()) {
4557 0 : throw ProcessError(TLF("State was saved with more than % threads. Change the number of threads or do not load RNG state", toString(getNumRNGs())));
4558 : }
4559 384 : RandHelper::loadState(state, &myRNGs[index]);
4560 384 : }
4561 :
4562 :
4563 : MSLane*
4564 15261272041 : MSLane::getBidiLane() const {
4565 15261272041 : return myBidiLane;
4566 : }
4567 :
4568 :
4569 : bool
4570 103257688 : MSLane::mustCheckJunctionCollisions() const {
4571 103257688 : return myCheckJunctionCollisions && myEdge->isInternal() && (
4572 1279342 : myLinks.front()->getFoeLanes().size() > 0
4573 13036 : || myLinks.front()->getWalkingAreaFoe() != nullptr
4574 12128 : || myLinks.front()->getWalkingAreaFoeExit() != nullptr);
4575 : }
4576 :
4577 :
4578 : double
4579 548374586 : MSLane::getSpaceTillLastStanding(const MSVehicle* ego, bool& foundStopped) const {
4580 : /// @todo if ego isn't on this lane, we could use a cached value
4581 : double lengths = 0;
4582 4853756124 : for (const MSVehicle* last : myVehicles) {
4583 4350643202 : if (last->getSpeed() < SUMO_const_haltingSpeed && !last->getLane()->getEdge().isRoundabout()
4584 21893902 : && last != ego
4585 : // @todo recheck
4586 4349144216 : && last->isFrontOnLane(this)) {
4587 21881339 : foundStopped = true;
4588 21881339 : const double lastBrakeGap = last->getCarFollowModel().brakeGap(last->getSpeed());
4589 21881339 : const double ret = last->getBackPositionOnLane() + lastBrakeGap - lengths;
4590 : return ret;
4591 : }
4592 4305381538 : if (MSGlobals::gSublane && ego->getVehicleType().getWidth() + last->getVehicleType().getWidth() < getWidth()) {
4593 31081221 : lengths += last->getVehicleType().getLengthWithGap() * (last->getVehicleType().getWidth() + last->getVehicleType().getMinGapLat()) / getWidth();
4594 : } else {
4595 4274300317 : lengths += last->getVehicleType().getLengthWithGap();
4596 : }
4597 : }
4598 526493247 : return getLength() - lengths;
4599 : }
4600 :
4601 :
4602 : bool
4603 429 : MSLane::allowsVehicleClass(SUMOVehicleClass vclass, int routingMode) const {
4604 429 : return (((routingMode & libsumo::ROUTING_MODE_IGNORE_TRANSIENT_PERMISSIONS) ? myOriginalPermissions : myPermissions) & vclass) == vclass;
4605 : }
4606 :
4607 : /****************************************************************************/
|
