Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2026 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 <microsim/devices/MSDevice_Taxi.h>
59 : #include <mesosim/MELoop.h>
60 : #include "MSNet.h"
61 : #include "MSVehicleType.h"
62 : #include "MSEdge.h"
63 : #include "MSEdgeControl.h"
64 : #include "MSJunction.h"
65 : #include "MSLogicJunction.h"
66 : #include "MSLink.h"
67 : #include "MSLane.h"
68 : #include "MSVehicleTransfer.h"
69 : #include "MSGlobals.h"
70 : #include "MSVehicleControl.h"
71 : #include "MSInsertionControl.h"
72 : #include "MSVehicleControl.h"
73 : #include "MSLeaderInfo.h"
74 : #include "MSVehicle.h"
75 : #include "MSStop.h"
76 :
77 : //#define DEBUG_INSERTION
78 : //#define DEBUG_PLAN_MOVE
79 : //#define DEBUG_EXEC_MOVE
80 : //#define DEBUG_CONTEXT
81 : //#define DEBUG_PARTIALS
82 : //#define DEBUG_OPPOSITE
83 : //#define DEBUG_VEHICLE_CONTAINER
84 : //#define DEBUG_COLLISIONS
85 : //#define DEBUG_JUNCTION_COLLISIONS
86 : //#define DEBUG_PEDESTRIAN_COLLISIONS
87 : //#define DEBUG_LANE_SORTER
88 : //#define DEBUG_NO_CONNECTION
89 : //#define DEBUG_SURROUNDING
90 : //#define DEBUG_EXTRAPOLATE_DEPARTPOS
91 : //#define DEBUG_ITERATOR
92 :
93 : //#define DEBUG_COND (false)
94 : //#define DEBUG_COND (true)
95 : #define DEBUG_COND (isSelected())
96 : #define DEBUG_COND2(obj) ((obj != nullptr && (obj)->isSelected()))
97 : //#define DEBUG_COND (getID() == "ego")
98 : //#define DEBUG_COND2(obj) ((obj != 0 && (obj)->getID() == "ego"))
99 : //#define DEBUG_COND2(obj) (true)
100 :
101 :
102 : // ===========================================================================
103 : // static member definitions
104 : // ===========================================================================
105 : MSLane::DictType MSLane::myDict;
106 : MSLane::CollisionAction MSLane::myCollisionAction(MSLane::COLLISION_ACTION_TELEPORT);
107 : MSLane::CollisionAction MSLane::myIntermodalCollisionAction(MSLane::COLLISION_ACTION_WARN);
108 : bool MSLane::myCheckJunctionCollisions(false);
109 : double MSLane::myCheckJunctionCollisionMinGap(0);
110 : SUMOTime MSLane::myCollisionStopTime(0);
111 : SUMOTime MSLane::myIntermodalCollisionStopTime(0);
112 : double MSLane::myCollisionMinGapFactor(1.0);
113 : bool MSLane::myExtrapolateSubstepDepart(false);
114 : std::vector<SumoRNG> MSLane::myRNGs;
115 : DepartSpeedDefinition MSLane::myDefaultDepartSpeedDefinition(DepartSpeedDefinition::DEFAULT);
116 : double MSLane::myDefaultDepartSpeed(0);
117 :
118 :
119 : // ===========================================================================
120 : // internal class method definitions
121 : // ===========================================================================
122 : void
123 170804121 : MSLane::StoringVisitor::add(const MSLane* const l) const {
124 170804121 : switch (myDomain) {
125 843591 : case libsumo::CMD_GET_VEHICLE_VARIABLE: {
126 1527294 : for (const MSVehicle* veh : l->getVehiclesSecure()) {
127 683703 : if (myShape.distance2D(veh->getPosition()) <= myRange) {
128 339434 : myObjects.insert(veh);
129 : }
130 : }
131 843766 : for (const MSBaseVehicle* veh : l->getParkingVehicles()) {
132 175 : if (myShape.distance2D(veh->getPosition()) <= myRange) {
133 36 : myObjects.insert(veh);
134 : }
135 : }
136 843591 : l->releaseVehicles();
137 : }
138 843591 : break;
139 834882 : case libsumo::CMD_GET_PERSON_VARIABLE: {
140 834882 : l->getVehiclesSecure();
141 834882 : std::vector<MSTransportable*> persons = l->getEdge().getSortedPersons(MSNet::getInstance()->getCurrentTimeStep(), true);
142 954089 : for (auto p : persons) {
143 119207 : if (myShape.distance2D(p->getPosition()) <= myRange) {
144 62310 : myObjects.insert(p);
145 : }
146 : }
147 834882 : l->releaseVehicles();
148 834882 : }
149 834882 : break;
150 169060197 : case libsumo::CMD_GET_EDGE_VARIABLE: {
151 169060197 : if (myShape.size() != 1 || l->getShape().distance2D(myShape[0]) <= myRange) {
152 167108552 : myObjects.insert(&l->getEdge());
153 : }
154 : }
155 : break;
156 65451 : case libsumo::CMD_GET_LANE_VARIABLE: {
157 65451 : if (myShape.size() != 1 || l->getShape().distance2D(myShape[0]) <= myRange) {
158 64563 : myObjects.insert(l);
159 : }
160 : }
161 : break;
162 : default:
163 : break;
164 :
165 : }
166 170804121 : }
167 :
168 :
169 : MSLane::AnyVehicleIterator&
170 12076833979 : MSLane::AnyVehicleIterator::operator++() {
171 12076833979 : if (nextIsMyVehicles()) {
172 11699135919 : if (myI1 != myI1End) {
173 8579970493 : myI1 += myDirection;
174 3119165426 : } else if (myI3 != myI3End) {
175 3119165426 : myI3 += myDirection;
176 : }
177 : // else: already at end
178 : } else {
179 377698060 : myI2 += myDirection;
180 : }
181 : //if (DEBUG_COND2(myLane)) std::cout << SIMTIME << " AnyVehicleIterator::operator++ lane=" << myLane->getID() << " myI1=" << myI1 << " myI2=" << myI2 << "\n";
182 12076833979 : return *this;
183 : }
184 :
185 :
186 : const MSVehicle*
187 12799447115 : MSLane::AnyVehicleIterator::operator*() {
188 12799447115 : if (nextIsMyVehicles()) {
189 12414504140 : if (myI1 != myI1End) {
190 8725424808 : return myLane->myVehicles[myI1];
191 3689079332 : } else if (myI3 != myI3End) {
192 3198639915 : return myLane->myTmpVehicles[myI3];
193 : } else {
194 : assert(myI2 == myI2End);
195 : return nullptr;
196 : }
197 : } else {
198 384942975 : return myLane->myPartialVehicles[myI2];
199 : }
200 : }
201 :
202 :
203 : bool
204 24876281094 : MSLane::AnyVehicleIterator::nextIsMyVehicles() const {
205 : #ifdef DEBUG_ITERATOR
206 : if (DEBUG_COND2(myLane)) std::cout << SIMTIME << " AnyVehicleIterator::nextIsMyVehicles lane=" << myLane->getID()
207 : << " myI1=" << myI1
208 : << " myI1End=" << myI1End
209 : << " myI2=" << myI2
210 : << " myI2End=" << myI2End
211 : << " myI3=" << myI3
212 : << " myI3End=" << myI3End
213 : << "\n";
214 : #endif
215 24876281094 : if (myI1 == myI1End && myI3 == myI3End) {
216 730487725 : if (myI2 != myI2End) {
217 : return false;
218 : } else {
219 490439417 : return true; // @note. must be caught
220 : }
221 : } else {
222 24145793369 : if (myI2 == myI2End) {
223 : return true;
224 : } else {
225 6860666374 : MSVehicle* cand = myI1 == myI1End ? myLane->myTmpVehicles[myI3] : myLane->myVehicles[myI1];
226 : #ifdef DEBUG_ITERATOR
227 : if (DEBUG_COND2(myLane)) std::cout << " "
228 : << " veh1=" << cand->getID()
229 : << " isTmp=" << (myI1 == myI1End)
230 : << " veh2=" << myLane->myPartialVehicles[myI2]->getID()
231 : << " pos1=" << cand->getPositionOnLane(myLane)
232 : << " pos2=" << myLane->myPartialVehicles[myI2]->getPositionOnLane(myLane)
233 : << "\n";
234 : #endif
235 6860666374 : if (cand->getPositionOnLane() < myLane->myPartialVehicles[myI2]->getPositionOnLane(myLane)) {
236 6381722110 : return myDownstream;
237 : } else {
238 478944264 : return !myDownstream;
239 : }
240 : }
241 : }
242 : }
243 :
244 :
245 : // ===========================================================================
246 : // member method definitions
247 : // ===========================================================================
248 : #ifdef _MSC_VER
249 : #pragma warning(push)
250 : #pragma warning(disable: 4355) // mask warning about "this" in initializers
251 : #endif
252 2103835 : MSLane::MSLane(const std::string& id, double maxSpeed, double friction, double length, MSEdge* const edge,
253 : int numericalID, const PositionVector& shape, double width,
254 : SVCPermissions permissions,
255 : SVCPermissions changeLeft, SVCPermissions changeRight,
256 : int index, bool isRampAccel,
257 : const std::string& type,
258 2103835 : const PositionVector& outlineShape) :
259 : Named(id),
260 4207670 : myNumericalID(numericalID), myShape(shape), myIndex(index),
261 2103835 : myVehicles(), myLength(length), myWidth(width),
262 2103835 : myEdge(edge), myMaxSpeed(maxSpeed),
263 2103835 : myFrictionCoefficient(friction),
264 2103835 : mySpeedModified(false),
265 2103835 : myPermissions(permissions),
266 2103835 : myChangeLeft(changeLeft),
267 2103835 : myChangeRight(changeRight),
268 2103835 : myOriginalPermissions(permissions),
269 2103835 : myLogicalPredecessorLane(nullptr),
270 2103835 : myCanonicalPredecessorLane(nullptr),
271 2103835 : myCanonicalSuccessorLane(nullptr),
272 2103835 : myBruttoVehicleLengthSum(0), myNettoVehicleLengthSum(0),
273 2103835 : myBruttoVehicleLengthSumToRemove(0), myNettoVehicleLengthSumToRemove(0),
274 2103835 : myRecalculateBruttoSum(false),
275 2103835 : myLeaderInfo(width, nullptr, 0.),
276 2103835 : myFollowerInfo(width, nullptr, 0.),
277 2103835 : myLeaderInfoTime(SUMOTime_MIN),
278 2103835 : myFollowerInfoTime(SUMOTime_MIN),
279 2103835 : myLengthGeometryFactor(MAX2(POSITION_EPS, myShape.length()) / myLength), // factor should not be 0
280 2103835 : myIsRampAccel(isRampAccel),
281 2103835 : myLaneType(type),
282 2103835 : myRightSideOnEdge(0), // initialized in MSEdge::initialize
283 2103835 : myRightmostSublane(0),
284 2103835 : myNeedsCollisionCheck(false),
285 2103835 : myOpposite(nullptr),
286 2103835 : myBidiLane(nullptr),
287 : #ifdef HAVE_FOX
288 : mySimulationTask(*this, 0),
289 : #endif
290 4207670 : myStopWatch(3) {
291 : // initialized in MSEdge::initialize
292 2103835 : initRestrictions();// may be reloaded again from initialized in MSEdge::closeBuilding
293 : assert(myRNGs.size() > 0);
294 2103835 : myRNGIndex = numericalID % myRNGs.size();
295 2103835 : if (outlineShape.size() > 0) {
296 23882 : myOutlineShape = new PositionVector(outlineShape);
297 : }
298 2103835 : }
299 : #ifdef _MSC_VER
300 : #pragma warning(pop)
301 : #endif
302 :
303 :
304 5828447 : MSLane::~MSLane() {
305 4954583 : for (MSLink* const l : myLinks) {
306 2869177 : delete l;
307 : }
308 2085406 : delete myOutlineShape;
309 12084665 : }
310 :
311 :
312 : void
313 2106499 : MSLane::initRestrictions() {
314 : // simplify unit testing without MSNet instance
315 2106499 : myRestrictions = MSGlobals::gUnitTests ? nullptr : MSNet::getInstance()->getRestrictions(myEdge->getEdgeType());
316 2106499 : }
317 :
318 :
319 : void
320 2100814 : MSLane::checkBufferType() {
321 2100814 : if (MSGlobals::gNumSimThreads <= 1) {
322 : myVehBuffer.unsetCondition();
323 : // } else {
324 : // this is an idea for better memory locality, lanes with nearby numerical ids get the same rng and thus the same thread
325 : // first tests show no visible effect though
326 : // myRNGIndex = myNumericalID * myRNGs.size() / dictSize();
327 : }
328 2100814 : }
329 :
330 :
331 : void
332 2895686 : MSLane::addLink(MSLink* link) {
333 2895686 : myLinks.push_back(link);
334 2895686 : }
335 :
336 :
337 : void
338 8846 : MSLane::setOpposite(MSLane* oppositeLane) {
339 8846 : myOpposite = oppositeLane;
340 8846 : if (myOpposite != nullptr && getLength() > myOpposite->getLength()) {
341 15 : WRITE_WARNINGF(TL("Unequal lengths of neigh lane '%' and lane '%' (% != %)."), getID(), myOpposite->getID(), getLength(), myOpposite->getLength());
342 : }
343 8846 : }
344 :
345 : void
346 31830 : MSLane::setBidiLane(MSLane* bidiLane) {
347 31830 : myBidiLane = bidiLane;
348 31830 : if (myBidiLane != nullptr && getLength() > myBidiLane->getLength()) {
349 66 : if (isNormal() || MSGlobals::gUsingInternalLanes) {
350 198 : WRITE_WARNINGF(TL("Unequal lengths of bidi lane '%' and lane '%' (% != %)."), getID(), myBidiLane->getID(), getLength(), myBidiLane->getLength());
351 : }
352 : }
353 31830 : }
354 :
355 :
356 :
357 : // ------ interaction with MSMoveReminder ------
358 : void
359 2610505 : MSLane::addMoveReminder(MSMoveReminder* rem, bool addToVehicles) {
360 2610505 : myMoveReminders.push_back(rem);
361 2610505 : if (addToVehicles) {
362 2617769 : for (MSVehicle* const veh : myVehicles) {
363 15980 : veh->addReminder(rem);
364 : }
365 : }
366 : // XXX: Here, the partial occupators are ignored!? Refs. #3255
367 2610505 : }
368 :
369 :
370 : void
371 0 : MSLane::removeMoveReminder(MSMoveReminder* rem) {
372 0 : auto it = std::find(myMoveReminders.begin(), myMoveReminders.end(), rem);
373 0 : if (it != myMoveReminders.end()) {
374 0 : myMoveReminders.erase(it);
375 0 : for (MSVehicle* const veh : myVehicles) {
376 0 : veh->removeReminder(rem);
377 : }
378 : }
379 0 : }
380 :
381 :
382 : double
383 17647386 : MSLane::setPartialOccupation(MSVehicle* v) {
384 : // multithreading: there are concurrent writes to myNeedsCollisionCheck but all of them should set it to true
385 17647386 : myNeedsCollisionCheck = true; // always check
386 : #ifdef DEBUG_PARTIALS
387 : if (DEBUG_COND2(v)) {
388 : std::cout << SIMTIME << " setPartialOccupation. lane=" << getID() << " veh=" << v->getID() << "\n";
389 : }
390 : #endif
391 : // XXX update occupancy here?
392 : #ifdef HAVE_FOX
393 17647386 : ScopedLocker<> lock(myPartialOccupatorMutex, MSGlobals::gNumSimThreads > 1);
394 : #endif
395 : //assert(std::find(myPartialVehicles.begin(), myPartialVehicles.end(), v) == myPartialVehicles.end());
396 17647386 : myPartialVehicles.push_back(v);
397 20130905 : return myLength;
398 : }
399 :
400 :
401 : void
402 17647436 : MSLane::resetPartialOccupation(MSVehicle* v) {
403 : #ifdef HAVE_FOX
404 17647436 : ScopedLocker<> lock(myPartialOccupatorMutex, MSGlobals::gNumSimThreads > 1);
405 : #endif
406 : #ifdef DEBUG_PARTIALS
407 : if (DEBUG_COND2(v)) {
408 : std::cout << SIMTIME << " resetPartialOccupation. lane=" << getID() << " veh=" << v->getID() << "\n";
409 : }
410 : #endif
411 18856411 : for (VehCont::iterator i = myPartialVehicles.begin(); i != myPartialVehicles.end(); ++i) {
412 18856349 : if (v == *i) {
413 17647374 : myPartialVehicles.erase(i);
414 : // XXX update occupancy here?
415 : //std::cout << " removed from myPartialVehicles\n";
416 : return;
417 : }
418 : }
419 : // bluelight eqipped vehicle can teleport onto the intersection without using a connection
420 : assert(false || MSGlobals::gClearState || v->getLaneChangeModel().hasBlueLight());
421 : }
422 :
423 :
424 : void
425 223450 : MSLane::setManeuverReservation(MSVehicle* v) {
426 : #ifdef DEBUG_CONTEXT
427 : if (DEBUG_COND2(v)) {
428 : std::cout << SIMTIME << " setManeuverReservation. lane=" << getID() << " veh=" << v->getID() << "\n";
429 : }
430 : #endif
431 223450 : myManeuverReservations.push_back(v);
432 223450 : }
433 :
434 :
435 : void
436 223450 : MSLane::resetManeuverReservation(MSVehicle* v) {
437 : #ifdef DEBUG_CONTEXT
438 : if (DEBUG_COND2(v)) {
439 : std::cout << SIMTIME << " resetManeuverReservation(): lane=" << getID() << " veh=" << v->getID() << "\n";
440 : }
441 : #endif
442 275455 : for (VehCont::iterator i = myManeuverReservations.begin(); i != myManeuverReservations.end(); ++i) {
443 275455 : if (v == *i) {
444 223450 : myManeuverReservations.erase(i);
445 : return;
446 : }
447 : }
448 : assert(false);
449 : }
450 :
451 :
452 : // ------ Vehicle emission ------
453 : void
454 3519217 : MSLane::incorporateVehicle(MSVehicle* veh, double pos, double speed, double posLat, const MSLane::VehCont::iterator& at, MSMoveReminder::Notification notification) {
455 3519217 : myNeedsCollisionCheck = true;
456 : assert(pos <= myLength);
457 : bool wasInactive = myVehicles.size() == 0;
458 3519217 : veh->enterLaneAtInsertion(this, pos, speed, posLat, notification);
459 3519215 : if (at == myVehicles.end()) {
460 : // vehicle will be the first on the lane
461 668997 : myVehicles.push_back(veh);
462 : } else {
463 2850218 : myVehicles.insert(at, veh);
464 : }
465 3519215 : myBruttoVehicleLengthSum += veh->getVehicleType().getLengthWithGap();
466 3519215 : myNettoVehicleLengthSum += veh->getVehicleType().getLength();
467 3519215 : myEdge->markDelayed();
468 3519215 : if (wasInactive) {
469 652938 : MSNet::getInstance()->getEdgeControl().gotActive(this);
470 : }
471 3519215 : if (getBidiLane() != nullptr && (!isRailway(veh->getVClass()) || (getPermissions() & ~SVC_RAIL_CLASSES) != 0)) {
472 : // railways don't need to "see" each other when moving in opposite directions on the same track (efficiency)
473 539 : getBidiLane()->setPartialOccupation(veh);
474 : }
475 3519215 : }
476 :
477 :
478 : bool
479 760802 : MSLane::lastInsertion(MSVehicle& veh, double mspeed, double posLat, bool patchSpeed) {
480 760802 : double pos = getLength() - POSITION_EPS;
481 760802 : MSVehicle* leader = getLastAnyVehicle();
482 : // back position of leader relative to this lane
483 : double leaderBack;
484 760802 : if (leader == nullptr) {
485 : /// look for a leaders on consecutive lanes
486 8674 : veh.setTentativeLaneAndPosition(this, pos, posLat);
487 8674 : veh.updateBestLanes(false, this);
488 8674 : std::pair<MSVehicle* const, double> leaderInfo = getLeader(&veh, pos, veh.getBestLanesContinuation(), veh.getCarFollowModel().brakeGap(mspeed));
489 8674 : leader = leaderInfo.first;
490 8674 : leaderBack = pos + leaderInfo.second + veh.getVehicleType().getMinGap();
491 : } else {
492 752128 : leaderBack = leader->getBackPositionOnLane(this);
493 : //std::cout << " leaderPos=" << leader->getPositionOnLane(this) << " leaderBack=" << leader->getBackPositionOnLane(this) << " leaderLane=" << leader->getLane()->getID() << "\n";
494 : }
495 8674 : if (leader == nullptr) {
496 : // insert at the end of this lane
497 2612 : return isInsertionSuccess(&veh, mspeed, pos, posLat, patchSpeed, MSMoveReminder::NOTIFICATION_DEPARTED);
498 : } else {
499 : // try to insert behind the leader
500 758190 : const double frontGapNeeded = veh.getCarFollowModel().getSecureGap(&veh, leader, mspeed, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel()) + veh.getVehicleType().getMinGap() + POSITION_EPS;
501 758190 : if (leaderBack >= frontGapNeeded) {
502 422298 : pos = MIN2(pos, leaderBack - frontGapNeeded);
503 422298 : bool result = isInsertionSuccess(&veh, mspeed, pos, posLat, patchSpeed, MSMoveReminder::NOTIFICATION_DEPARTED);
504 : //if (!result) std::cout << " insertLast failed for " << veh.getID() << " pos=" << pos << " leaderBack=" << leaderBack << " frontGapNeeded=" << frontGapNeeded << "\n";
505 422298 : return result;
506 : }
507 : //std::cout << " insertLast failed for " << veh.getID() << " pos=" << pos << " leaderBack=" << leaderBack << " frontGapNeeded=" << frontGapNeeded << "\n";
508 : }
509 : return false;
510 : }
511 :
512 :
513 : bool
514 590709 : MSLane::freeInsertion(MSVehicle& veh, double mspeed, double posLat,
515 : MSMoveReminder::Notification notification) {
516 : // try to insert teleporting vehicles fully on this lane
517 590709 : double maxPos = myLength;
518 590709 : if (veh.hasStops() && veh.getNextStop().edge == veh.getCurrentRouteEdge()) {
519 9389 : maxPos = MAX2(0.0, veh.getNextStop().getEndPos(veh));
520 : }
521 590709 : const double minPos = (notification == MSMoveReminder::NOTIFICATION_TELEPORT ?
522 288822 : MIN2(maxPos, veh.getVehicleType().getLength()) : 0);
523 590709 : veh.setTentativeLaneAndPosition(this, minPos, 0);
524 590709 : if (myVehicles.size() == 0) {
525 : // ensure sufficient gap to followers on predecessor lanes
526 13039 : const double backOffset = minPos - veh.getVehicleType().getLength();
527 13039 : const double missingRearGap = getMissingRearGap(&veh, backOffset, mspeed);
528 13039 : if (missingRearGap > 0) {
529 1296 : if (minPos + missingRearGap <= maxPos) {
530 : // @note. The rear gap is tailored to mspeed. If it changes due
531 : // to a leader vehicle (on subsequent lanes) insertion will
532 : // still fail. Under the right combination of acceleration and
533 : // deceleration values there might be another insertion
534 : // positions that would be successful be we do not look for it.
535 : //std::cout << SIMTIME << " freeInsertion lane=" << getID() << " veh=" << veh.getID() << " unclear @(340)\n";
536 763 : return isInsertionSuccess(&veh, mspeed, minPos + missingRearGap, posLat, true, notification);
537 : }
538 : return false;
539 : } else {
540 11743 : return isInsertionSuccess(&veh, mspeed, minPos, posLat, true, notification);
541 : }
542 :
543 : } else {
544 : // check whether the vehicle can be put behind the last one if there is such
545 577670 : const MSVehicle* const leader = myVehicles.back(); // @todo reproduction of bogus old behavior. see #1961
546 577670 : const double leaderPos = leader->getBackPositionOnLane(this);
547 577670 : const double speed = leader->getSpeed();
548 577670 : const double frontGapNeeded = veh.getCarFollowModel().getSecureGap(&veh, leader, speed, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel()) + veh.getVehicleType().getMinGap();
549 577670 : if (leaderPos >= frontGapNeeded) {
550 564040 : const double tspeed = MIN2(veh.getCarFollowModel().insertionFollowSpeed(&veh, mspeed, frontGapNeeded, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel(), leader), mspeed);
551 : // check whether we can insert our vehicle behind the last vehicle on the lane
552 564040 : if (isInsertionSuccess(&veh, tspeed, minPos, posLat, true, notification)) {
553 : //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";
554 : return true;
555 : }
556 : }
557 : }
558 : // go through the lane, look for free positions (starting after the last vehicle)
559 : MSLane::VehCont::iterator predIt = myVehicles.begin();
560 13434638 : while (predIt != myVehicles.end()) {
561 : // get leader (may be zero) and follower
562 : // @todo compute secure position in regard to sublane-model
563 12988747 : const MSVehicle* leader = predIt != myVehicles.end() - 1 ? *(predIt + 1) : nullptr;
564 12988747 : if (leader == nullptr && myPartialVehicles.size() > 0) {
565 73047 : leader = myPartialVehicles.front();
566 : }
567 12988747 : const MSVehicle* follower = *predIt;
568 :
569 : // patch speed if allowed
570 : double speed = mspeed;
571 12988747 : if (leader != nullptr) {
572 12612767 : speed = MIN2(leader->getSpeed(), mspeed);
573 : }
574 :
575 : // compute the space needed to not collide with leader
576 : double frontMax = maxPos;
577 : if (leader != nullptr) {
578 12612767 : double leaderRearPos = leader->getBackPositionOnLane(this);
579 12612767 : double frontGapNeeded = veh.getCarFollowModel().getSecureGap(&veh, leader, speed, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel()) + veh.getVehicleType().getMinGap();
580 12612767 : frontMax = MIN2(maxPos, leaderRearPos - frontGapNeeded);
581 : }
582 : // compute the space needed to not let the follower collide
583 12988747 : const double followPos = follower->getPositionOnLane() + follower->getVehicleType().getMinGap();
584 12988747 : const double backGapNeeded = follower->getCarFollowModel().getSecureGap(follower, &veh, follower->getSpeed(), veh.getSpeed(), veh.getCarFollowModel().getMaxDecel());
585 12988747 : const double backMin = followPos + backGapNeeded + veh.getVehicleType().getLength();
586 :
587 : // check whether there is enough room (given some extra space for rounding errors)
588 12988747 : if (frontMax > minPos && backMin + POSITION_EPS < frontMax) {
589 : // try to insert vehicle (should be always ok)
590 20664 : if (isInsertionSuccess(&veh, speed, backMin + POSITION_EPS, posLat, true, notification)) {
591 : //std::cout << SIMTIME << " freeInsertion lane=" << getID() << " veh=" << veh.getID() << " @(393)\n";
592 : return true;
593 : }
594 : }
595 : ++predIt;
596 : }
597 : // first check at lane's begin
598 : //std::cout << SIMTIME << " freeInsertion lane=" << getID() << " veh=" << veh.getID() << " fail final\n";
599 : return false;
600 : }
601 :
602 :
603 : double
604 13386942 : MSLane::getDepartSpeed(const MSVehicle& veh, bool& patchSpeed) {
605 : double speed = 0;
606 13386942 : const SUMOVehicleParameter& pars = veh.getParameter();
607 13386942 : DepartSpeedDefinition dsd = pars.departSpeedProcedure;
608 13386942 : if (dsd == DepartSpeedDefinition::DEFAULT) {
609 6976543 : dsd = myDefaultDepartSpeedDefinition;
610 6976543 : if (dsd == DepartSpeedDefinition::GIVEN) {
611 3777222 : speed = myDefaultDepartSpeed;
612 : }
613 6410399 : } else if (dsd == DepartSpeedDefinition::GIVEN) {
614 1375765 : speed = pars.departSpeed;;
615 : }
616 13386942 : switch (dsd) {
617 5152987 : case DepartSpeedDefinition::GIVEN:
618 5152987 : patchSpeed = false;
619 5152987 : break;
620 53183 : case DepartSpeedDefinition::RANDOM:
621 106366 : speed = roundDecimal(RandHelper::rand(getVehicleMaxSpeed(&veh)), gPrecisionRandom);
622 53183 : patchSpeed = true;
623 53183 : break;
624 2881479 : case DepartSpeedDefinition::MAX:
625 2881479 : speed = getVehicleMaxSpeed(&veh);
626 2881479 : patchSpeed = true;
627 2881479 : break;
628 376384 : case DepartSpeedDefinition::DESIRED:
629 376384 : speed = getVehicleMaxSpeed(&veh);
630 376384 : patchSpeed = false;
631 376384 : break;
632 137513 : case DepartSpeedDefinition::LIMIT:
633 137513 : speed = getVehicleMaxSpeed(&veh) / veh.getChosenSpeedFactor();
634 137513 : patchSpeed = false;
635 137513 : break;
636 8177 : case DepartSpeedDefinition::LAST: {
637 8177 : MSVehicle* last = getLastAnyVehicle();
638 8177 : speed = getVehicleMaxSpeed(&veh);
639 8177 : if (last != nullptr) {
640 7845 : speed = MIN2(speed, last->getSpeed());
641 7845 : patchSpeed = false;
642 : }
643 : break;
644 : }
645 4777219 : case DepartSpeedDefinition::AVG: {
646 4777219 : speed = MIN2(getVehicleMaxSpeed(&veh), getMeanSpeed());
647 4777219 : if (getLastAnyVehicle() != nullptr) {
648 4479051 : patchSpeed = false;
649 : }
650 : break;
651 : }
652 0 : case DepartSpeedDefinition::DEFAULT:
653 : default:
654 : // speed = 0 was set before
655 0 : patchSpeed = false; // @todo check
656 0 : break;
657 : }
658 13386942 : return speed;
659 : }
660 :
661 :
662 : double
663 13871667 : MSLane::getDepartPosLat(const MSVehicle& veh) {
664 13871667 : const SUMOVehicleParameter& pars = veh.getParameter();
665 13871667 : switch (pars.departPosLatProcedure) {
666 107669 : case DepartPosLatDefinition::GIVEN:
667 107669 : return pars.departPosLat;
668 : case DepartPosLatDefinition::RIGHT:
669 36054 : return -getWidth() * 0.5 + veh.getVehicleType().getWidth() * 0.5;
670 : case DepartPosLatDefinition::LEFT:
671 35843 : return getWidth() * 0.5 - veh.getVehicleType().getWidth() * 0.5;
672 : case DepartPosLatDefinition::RANDOM: {
673 243600 : const double raw = RandHelper::rand(getWidth() - veh.getVehicleType().getWidth()) - getWidth() * 0.5 + veh.getVehicleType().getWidth() * 0.5;
674 243600 : return roundDecimal(raw, gPrecisionRandom);
675 : }
676 : case DepartPosLatDefinition::CENTER:
677 : case DepartPosLatDefinition::DEFAULT:
678 : // @note:
679 : // case DepartPosLatDefinition::FREE
680 : // case DepartPosLatDefinition::RANDOM_FREE
681 : // are not handled here because they involve multiple insertion attempts
682 : default:
683 : return 0;
684 : }
685 : }
686 :
687 :
688 : bool
689 13386880 : MSLane::insertVehicle(MSVehicle& veh) {
690 : double pos = 0;
691 13386880 : bool patchSpeed = true; // whether the speed shall be adapted to infrastructure/traffic in front
692 13386880 : const SUMOVehicleParameter& pars = veh.getParameter();
693 13386880 : double speed = getDepartSpeed(veh, patchSpeed);
694 13386880 : double posLat = getDepartPosLat(veh);
695 :
696 : // determine the position
697 13386880 : switch (pars.departPosProcedure) {
698 972955 : case DepartPosDefinition::GIVEN:
699 972955 : pos = pars.departPos;
700 972955 : if (pos < 0.) {
701 144307 : pos += myLength;
702 : }
703 : break;
704 196248 : case DepartPosDefinition::RANDOM:
705 196248 : pos = roundDecimal(RandHelper::rand(getLength()), gPrecisionRandom);
706 : break;
707 : case DepartPosDefinition::RANDOM_FREE: {
708 532296 : for (int i = 0; i < 10; i++) {
709 : // we will try some random positions ...
710 : pos = RandHelper::rand(getLength());
711 484787 : posLat = getDepartPosLat(veh); // could be random as well
712 484787 : if (isInsertionSuccess(&veh, speed, pos, posLat, patchSpeed, MSMoveReminder::NOTIFICATION_DEPARTED)) {
713 2151 : MSNet::getInstance()->getInsertionControl().retractDescheduleDeparture(&veh);
714 2151 : return true;
715 : }
716 : }
717 : // ... and if that doesn't work, we put the vehicle to the free position
718 47509 : bool success = freeInsertion(veh, speed, posLat);
719 47509 : if (success) {
720 12922 : MSNet::getInstance()->getInsertionControl().retractDescheduleDeparture(&veh);
721 : }
722 : return success;
723 : }
724 254378 : case DepartPosDefinition::FREE:
725 254378 : return freeInsertion(veh, speed, posLat);
726 760802 : case DepartPosDefinition::LAST:
727 760802 : return lastInsertion(veh, speed, posLat, patchSpeed);
728 3791 : case DepartPosDefinition::STOP:
729 3791 : if (veh.hasStops() && veh.getNextStop().edge == veh.getCurrentRouteEdge()) {
730 : // getLastFreePos of stopping place could return negative position to avoid blocking the stop
731 3785 : pos = MAX2(0.0, veh.getNextStop().getEndPos(veh));
732 : break;
733 : }
734 : FALLTHROUGH;
735 : case DepartPosDefinition::BASE:
736 : case DepartPosDefinition::DEFAULT:
737 : case DepartPosDefinition::SPLIT_FRONT:
738 : default:
739 11149052 : if (pars.departProcedure == DepartDefinition::SPLIT) {
740 : pos = getLength();
741 : // find the vehicle from which we are splitting off (should only be a single lane to check)
742 : AnyVehicleIterator end = anyVehiclesEnd();
743 3 : for (AnyVehicleIterator it = anyVehiclesBegin(); it != end; ++it) {
744 24 : const MSVehicle* cand = *it;
745 24 : if (cand->isStopped() && cand->getNextStopParameter()->split == veh.getID()) {
746 21 : if (pars.departPosProcedure == DepartPosDefinition::SPLIT_FRONT) {
747 3 : pos = cand->getPositionOnLane() + cand->getVehicleType().getMinGap() + veh.getLength();
748 : } else {
749 18 : pos = cand->getBackPositionOnLane() - veh.getVehicleType().getMinGap();
750 : }
751 : break;
752 : }
753 : }
754 : } else {
755 11149031 : pos = veh.basePos(myEdge);
756 : }
757 : break;
758 : }
759 : // determine the lateral position for special cases
760 12322040 : if (MSGlobals::gLateralResolution > 0) {
761 1406328 : switch (pars.departPosLatProcedure) {
762 : case DepartPosLatDefinition::RANDOM_FREE: {
763 0 : for (int i = 0; i < 10; i++) {
764 : // we will try some random positions ...
765 0 : posLat = RandHelper::rand(getWidth()) - getWidth() * 0.5;
766 0 : if (isInsertionSuccess(&veh, speed, pos, posLat, patchSpeed, MSMoveReminder::NOTIFICATION_DEPARTED)) {
767 : return true;
768 : }
769 : }
770 : FALLTHROUGH;
771 : }
772 : // no break! continue with DepartPosLatDefinition::FREE
773 : case DepartPosLatDefinition::FREE: {
774 : // systematically test all positions until a free lateral position is found
775 5790 : double posLatMin = -getWidth() * 0.5 + veh.getVehicleType().getWidth() * 0.5;
776 5790 : double posLatMax = getWidth() * 0.5 - veh.getVehicleType().getWidth() * 0.5;
777 19291 : for (posLat = posLatMin; posLat < posLatMax; posLat += MSGlobals::gLateralResolution) {
778 17275 : if (isInsertionSuccess(&veh, speed, pos, posLat, patchSpeed, MSMoveReminder::NOTIFICATION_DEPARTED)) {
779 : return true;
780 : }
781 : }
782 : return false;
783 : }
784 : default:
785 : break;
786 : }
787 : }
788 : // try to insert
789 12316250 : const bool success = isInsertionSuccess(&veh, speed, pos, posLat, patchSpeed, MSMoveReminder::NOTIFICATION_DEPARTED);
790 : #ifdef DEBUG_EXTRAPOLATE_DEPARTPOS
791 : if (DEBUG_COND2(&veh)) {
792 : std::cout << SIMTIME << " veh=" << veh.getID() << " success=" << success << " extrapolate=" << myExtrapolateSubstepDepart << " delay=" << veh.getDepartDelay() << " speed=" << speed << "\n";
793 : }
794 : #endif
795 12316247 : if (success && myExtrapolateSubstepDepart && veh.getDepartDelay() > 0) {
796 221848 : SUMOTime relevantDelay = MIN2(DELTA_T, veh.getDepartDelay());
797 : // try to compensate sub-step depart delay by moving the vehicle forward
798 221848 : speed = veh.getSpeed(); // may have been adapted in isInsertionSuccess
799 221848 : double dist = speed * STEPS2TIME(relevantDelay);
800 221848 : std::pair<MSVehicle* const, double> leaderInfo = getLeader(&veh, pos, veh.getBestLanesContinuation());
801 221848 : if (leaderInfo.first != nullptr) {
802 : MSVehicle* leader = leaderInfo.first;
803 221592 : const double frontGapNeeded = veh.getCarFollowModel().getSecureGap(&veh, leader, speed, leader->getSpeed(),
804 : leader->getCarFollowModel().getMaxDecel());
805 221592 : dist = MIN2(dist, leaderInfo.second - frontGapNeeded);
806 : }
807 221848 : if (dist > 0) {
808 216257 : veh.executeFractionalMove(dist);
809 : }
810 : }
811 : return success;
812 : }
813 :
814 :
815 : bool
816 7751996 : MSLane::checkFailure(const MSVehicle* aVehicle, double& speed, double& dist, const double nspeed, const bool patchSpeed, const std::string errorMsg, InsertionCheck check) const {
817 7751996 : if (nspeed < speed) {
818 3327394 : if (patchSpeed) {
819 677535 : speed = MIN2(nspeed, speed);
820 677535 : dist = aVehicle->getCarFollowModel().brakeGap(speed) + aVehicle->getVehicleType().getMinGap();
821 2649859 : } else if (speed > 0) {
822 2649859 : if ((aVehicle->getInsertionChecks() & (int)check) == 0) {
823 : return false;
824 : }
825 2649819 : if (MSGlobals::gEmergencyInsert) {
826 : // Check whether vehicle can stop at the given distance when applying emergency braking
827 47 : double emergencyBrakeGap = 0.5 * speed * speed / aVehicle->getCarFollowModel().getEmergencyDecel();
828 47 : if (emergencyBrakeGap <= dist) {
829 : // Vehicle may stop in time with emergency deceleration
830 : // still, emit a warning
831 141 : WRITE_WARNINGF(TL("Vehicle '%' is inserted in an emergency situation, time=%."), aVehicle->getID(), time2string(SIMSTEP));
832 47 : return false;
833 : }
834 : }
835 :
836 2649772 : if (errorMsg != "") {
837 156 : WRITE_ERRORF(TL("Vehicle '%' will not be able to depart on lane '%' with speed % (%), time=%."),
838 : aVehicle->getID(), getID(), speed, errorMsg, time2string(SIMSTEP));
839 39 : MSNet::getInstance()->getInsertionControl().descheduleDeparture(aVehicle);
840 : }
841 2649772 : return true;
842 : }
843 : }
844 : return false;
845 : }
846 :
847 :
848 : bool
849 13972610 : MSLane::isInsertionSuccess(MSVehicle* aVehicle,
850 : double speed, double pos, double posLat, bool patchSpeed,
851 : MSMoveReminder::Notification notification) {
852 13972610 : int insertionChecks = aVehicle->getInsertionChecks();
853 13972610 : if (pos < 0 || pos > myLength) {
854 : // we may not start there
855 11139 : WRITE_WARNINGF(TL("Invalid departPos % given for vehicle '%', time=%. Inserting at lane end instead."),
856 : pos, aVehicle->getID(), time2string(SIMSTEP));
857 3713 : pos = myLength;
858 : }
859 :
860 : #ifdef DEBUG_INSERTION
861 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
862 : std::cout << "\nIS_INSERTION_SUCCESS\n"
863 : << SIMTIME << " lane=" << getID()
864 : << " veh '" << aVehicle->getID()
865 : << " bestLanes=" << toString(aVehicle->getBestLanesContinuation(this))
866 : << " pos=" << pos
867 : << " speed=" << speed
868 : << " patchSpeed=" << patchSpeed
869 : << "'\n";
870 : }
871 : #endif
872 :
873 13972610 : aVehicle->setTentativeLaneAndPosition(this, pos, posLat);
874 13972610 : aVehicle->updateBestLanes(false, this);
875 : const MSCFModel& cfModel = aVehicle->getCarFollowModel();
876 13972610 : const std::vector<MSLane*>& bestLaneConts = aVehicle->getBestLanesContinuation(this);
877 : std::vector<MSLane*>::const_iterator ri = bestLaneConts.begin();
878 13972610 : double seen = getLength() - pos; // == distance from insertion position until the end of the currentLane
879 13972610 : double dist = cfModel.brakeGap(speed) + aVehicle->getVehicleType().getMinGap();
880 13972610 : const bool isRail = aVehicle->isRail();
881 13972610 : if (isRail && insertionChecks != (int)InsertionCheck::NONE
882 62821 : && aVehicle->getParameter().departProcedure != DepartDefinition::SPLIT
883 62797 : && MSRailSignalControl::isSignalized(aVehicle->getVClass())
884 14035230 : && isRailwayOrShared(myPermissions)) {
885 62489 : const MSDriveWay* dw = MSDriveWay::getDepartureDriveway(aVehicle);
886 : MSEdgeVector occupied;
887 : #ifdef DEBUG_INSERTION
888 : gDebugFlag4 = DEBUG_COND2(aVehicle) || DEBUG_COND;
889 : #endif
890 62489 : if (dw->foeDriveWayOccupied(false, aVehicle, occupied)) {
891 53854 : setParameter("insertionBlocked:" + aVehicle->getID(), dw->getID());
892 : #ifdef DEBUG_INSERTION
893 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
894 : std::cout << " foe of driveway " + dw->getID() + " has occupied edges " + toString(occupied) << "\n";
895 : }
896 : gDebugFlag4 = false;
897 : #endif
898 : return false;
899 : }
900 : #ifdef DEBUG_INSERTION
901 : gDebugFlag4 = false;
902 : #endif
903 62489 : }
904 13918756 : if (getBidiLane() != nullptr && isRail) {
905 : // do not insert if the bidirectional edge is occupied
906 1437 : if (getBidiLane()->getVehicleNumberWithPartials() > 0 && (insertionChecks & (int)InsertionCheck::BIDI) != 0) {
907 : #ifdef DEBUG_INSERTION
908 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
909 : std::cout << " bidi-lane occupied\n";
910 : }
911 : #endif
912 : return false;
913 : }
914 : // do not insert the back of the train would be put onto an occupied bidi-lane
915 1431 : double backLength = aVehicle->getLength() - pos;
916 1431 : if (backLength > 0 && (insertionChecks & (int)InsertionCheck::BIDI) != 0) {
917 236 : MSLane* pred = getLogicalPredecessorLane();
918 236 : MSLane* bidi = pred == nullptr ? nullptr : pred->getBidiLane();
919 382 : while (backLength > 0 && bidi != nullptr) {
920 194 : if (bidi->getVehicleNumberWithPartials() > 0) {
921 : #ifdef DEBUG_INSERTION
922 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
923 : std::cout << " bidi-lane furtherLanes occupied\n";
924 : }
925 : #endif
926 : return false;
927 : }
928 146 : backLength -= bidi->getLength();
929 146 : pred = pred->getLogicalPredecessorLane();
930 146 : bidi = pred == nullptr ? nullptr : pred->getBidiLane();
931 : }
932 : }
933 : }
934 : MSLink* firstRailSignal = nullptr;
935 : double firstRailSignalDist = -1;
936 : // whether speed may be patched for unavoidable reasons (stops, speedLimits, ...)
937 13918702 : const bool patchSpeedSpecial = patchSpeed || aVehicle->getParameter().departSpeedProcedure != DepartSpeedDefinition::GIVEN;
938 :
939 : // before looping through the continuation lanes, check if a stop is scheduled on this lane
940 : // (the code is duplicated in the loop)
941 13918702 : if (aVehicle->hasStops()) {
942 497770 : const MSStop& nextStop = aVehicle->getNextStop();
943 497770 : if (nextStop.lane == this) {
944 161948 : std::stringstream msg;
945 : double distToStop, safeSpeed;
946 161948 : if (nextStop.pars.speed > 0) {
947 3090 : msg << "scheduled waypoint on lane '" << myID << "' too close";
948 3090 : distToStop = MAX2(0.0, nextStop.pars.startPos - pos);
949 3090 : safeSpeed = cfModel.freeSpeed(aVehicle, speed, distToStop, nextStop.pars.speed, true, MSCFModel::CalcReason::FUTURE);
950 : } else {
951 158858 : msg << "scheduled stop on lane '" << myID << "' too close";
952 158858 : distToStop = nextStop.pars.endPos - pos;
953 158858 : safeSpeed = cfModel.stopSpeed(aVehicle, speed, distToStop, MSCFModel::CalcReason::FUTURE);
954 : }
955 485827 : if (checkFailure(aVehicle, speed, dist, MAX2(0.0, safeSpeed), patchSpeedSpecial, msg.str(), InsertionCheck::STOP)) {
956 : // we may not drive with the given velocity - we cannot stop at the stop
957 : return false;
958 : }
959 161948 : }
960 : }
961 : // check leader vehicle first because it could have influenced the departSpeed (for departSpeed=avg)
962 : // get the pointer to the vehicle next in front of the given position
963 13918694 : const MSLeaderInfo leaders = getLastVehicleInformation(aVehicle, 0, pos);
964 : //if (aVehicle->getID() == "disabled") std::cout << " leaders=" << leaders.toString() << "\n";
965 13918694 : const double nspeed = safeInsertionSpeed(aVehicle, -pos, leaders, speed);
966 21029821 : if (nspeed == INVALID_SPEED || checkFailure(aVehicle, speed, dist, nspeed, patchSpeed, "", InsertionCheck::LEADER_GAP)) {
967 : // we may not drive with the given velocity - we crash into the leader
968 : #ifdef DEBUG_INSERTION
969 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
970 : std::cout << SIMTIME << " isInsertionSuccess lane=" << getID()
971 : << " veh=" << aVehicle->getID()
972 : << " pos=" << pos
973 : << " posLat=" << posLat
974 : << " patchSpeed=" << patchSpeed
975 : << " speed=" << speed
976 : << " nspeed=" << nspeed
977 : << " leaders=" << leaders.toString()
978 : << " failed (@700)!\n";
979 : }
980 : #endif
981 : return false;
982 : }
983 : #ifdef DEBUG_INSERTION
984 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
985 : std::cout << SIMTIME << " speed = " << speed << " nspeed = " << nspeed << " leaders=" << leaders.toString() << "\n";
986 : }
987 : #endif
988 :
989 4514527 : const MSRoute& r = aVehicle->getRoute();
990 4514527 : MSRouteIterator ce = r.begin();
991 : int nRouteSuccs = 1;
992 : MSLane* currentLane = this;
993 : MSLane* nextLane = this;
994 5995198 : SUMOTime arrivalTime = MSNet::getInstance()->getCurrentTimeStep() + TIME2STEPS(seen / MAX2(speed, SUMO_const_haltingSpeed));
995 4920770 : while ((seen < dist || (isRail && firstRailSignal == nullptr)) && ri != bestLaneConts.end()) {
996 : // get the next link used...
997 516537 : std::vector<MSLink*>::const_iterator link = succLinkSec(*aVehicle, nRouteSuccs, *currentLane, bestLaneConts);
998 : // get the next used lane (including internal)
999 516537 : if (currentLane->isLinkEnd(link)) {
1000 21800 : if (¤tLane->getEdge() == r.getLastEdge()) {
1001 : // reached the end of the route
1002 11200 : if (aVehicle->getParameter().arrivalSpeedProcedure == ArrivalSpeedDefinition::GIVEN) {
1003 98 : const double remaining = seen + aVehicle->getArrivalPos() - currentLane->getLength();
1004 98 : const double fspeed = cfModel.freeSpeed(aVehicle, speed, remaining, aVehicle->getParameter().arrivalSpeed, true, MSCFModel::CalcReason::FUTURE);
1005 196 : if (checkFailure(aVehicle, speed, dist, fspeed,
1006 : patchSpeedSpecial, "arrival speed too low", InsertionCheck::ARRIVAL_SPEED)) {
1007 : // we may not drive with the given velocity - we cannot match the specified arrival speed
1008 : return false;
1009 : }
1010 : }
1011 : } else {
1012 : // lane does not continue
1013 10600 : if (checkFailure(aVehicle, speed, dist, cfModel.insertionStopSpeed(aVehicle, speed, seen),
1014 21200 : patchSpeedSpecial, "junction '" + currentLane->getEdge().getToJunction()->getID() + "' too close", InsertionCheck::JUNCTION)) {
1015 : // we may not drive with the given velocity - we cannot stop at the junction
1016 : return false;
1017 : }
1018 : }
1019 : break;
1020 : }
1021 494737 : if (isRail && firstRailSignal == nullptr) {
1022 : std::string constraintInfo;
1023 : bool isInsertionOrder;
1024 17061 : if (MSRailSignal::hasInsertionConstraint(*link, aVehicle, constraintInfo, isInsertionOrder)) {
1025 6216 : setParameter((isInsertionOrder ? "insertionOrder" : "insertionConstraint:")
1026 9261 : + aVehicle->getID(), constraintInfo);
1027 : #ifdef DEBUG_INSERTION
1028 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1029 : std::cout << " insertion constraint at link " << (*link)->getDescription() << " not cleared \n";
1030 : }
1031 : #endif
1032 : return false;
1033 : }
1034 : }
1035 :
1036 : // might also by a regular traffic_light instead of a rail_signal
1037 491629 : if (firstRailSignal == nullptr && (*link)->getTLLogic() != nullptr) {
1038 : firstRailSignal = *link;
1039 : firstRailSignalDist = seen;
1040 : }
1041 491629 : nextLane = (*link)->getViaLaneOrLane();
1042 491629 : if (!(*link)->opened(arrivalTime, speed, speed, aVehicle->getVehicleType().getLength(), aVehicle->getImpatience(),
1043 : cfModel.getMaxDecel(), 0, posLat, nullptr, false, aVehicle)
1044 468609 : || (*link)->railSignalWasPassed()
1045 960235 : || !(*link)->havePriority()) {
1046 : // have to stop at junction
1047 40885 : std::string errorMsg = "";
1048 40885 : const LinkState state = (*link)->getState();
1049 40885 : if (state == LINKSTATE_MINOR
1050 40885 : || state == LINKSTATE_EQUAL
1051 : || state == LINKSTATE_STOP
1052 : || state == LINKSTATE_ALLWAY_STOP) {
1053 : // no sense in trying later
1054 : errorMsg = "unpriorised junction too close";
1055 24333 : } else if ((*link)->getTLLogic() != nullptr && !(*link)->getTLLogic()->getsMajorGreen((*link)->getTLIndex())) {
1056 : // traffic light never turns 'G'?
1057 19464 : errorMsg = "tlLogic '" + (*link)->getTLLogic()->getID() + "' link " + toString((*link)->getTLIndex()) + " never switches to 'G'";
1058 : }
1059 40885 : const double laneStopOffset = MAX2(getVehicleStopOffset(aVehicle),
1060 40885 : aVehicle->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPLINE_CROSSING_GAP, MSPModel::SAFETY_GAP) - (*link)->getDistToFoePedCrossing());
1061 40885 : const double remaining = seen - laneStopOffset;
1062 81770 : if (checkFailure(aVehicle, speed, dist, cfModel.insertionStopSpeed(aVehicle, speed, remaining),
1063 : patchSpeedSpecial, errorMsg, InsertionCheck::JUNCTION)) {
1064 : // we may not drive with the given velocity - we cannot stop at the junction in time
1065 : #ifdef DEBUG_INSERTION
1066 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1067 : std::cout << SIMTIME << " isInsertionSuccess lane=" << getID()
1068 : << " veh=" << aVehicle->getID()
1069 : << " patchSpeed=" << patchSpeed
1070 : << " speed=" << speed
1071 : << " remaining=" << remaining
1072 : << " leader=" << currentLane->getLastVehicleInformation(aVehicle, 0, 0).toString()
1073 : << " last=" << Named::getIDSecure(getLastAnyVehicle())
1074 : << " meanSpeed=" << getMeanSpeed()
1075 : << " failed (@926)!\n";
1076 : }
1077 : #endif
1078 : return false;
1079 : }
1080 : #ifdef DEBUG_INSERTION
1081 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1082 : std::cout << "trying insertion before minor link: "
1083 : << "insertion speed = " << speed << " dist=" << dist
1084 : << "\n";
1085 : }
1086 : #endif
1087 27876 : if (seen >= aVehicle->getVehicleType().getMinGap()) {
1088 : break;
1089 : }
1090 450744 : } else if (nextLane->isInternal()) {
1091 246169 : double tmp = 0;
1092 246169 : bool dummyReq = true;
1093 : #ifdef DEBUG_INSERTION
1094 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1095 : std::cout << "checking linkLeader for lane '" << nextLane->getID() << "'\n";
1096 : gDebugFlag1 = true;
1097 : }
1098 : #endif
1099 246169 : double nSpeed = speed;
1100 246169 : aVehicle->checkLinkLeader(nextLane->getLinkCont()[0], nextLane, seen + nextLane->getLength(), nullptr, nSpeed, tmp, tmp, dummyReq);
1101 : #ifdef DEBUG_INSERTION
1102 : gDebugFlag1 = false;
1103 : #endif
1104 492338 : if (checkFailure(aVehicle, speed, dist, nSpeed, patchSpeed, "", InsertionCheck::LEADER_GAP)) {
1105 : // we may not drive with the given velocity - there is a junction leader
1106 : #ifdef DEBUG_INSERTION
1107 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1108 : std::cout << " linkLeader nSpeed=" << nSpeed << " failed (@1058)!\n";
1109 : }
1110 : #endif
1111 2907 : return false;
1112 : }
1113 : }
1114 : // check how next lane affects the journey
1115 450742 : if (nextLane != nullptr) {
1116 :
1117 : // do not insert if the bidirectional edge is occupied before a railSignal has been encountered
1118 450742 : if (firstRailSignal == nullptr && nextLane->getBidiLane() != nullptr && nextLane->getBidiLane()->getVehicleNumberWithPartials() > 0) {
1119 0 : if ((insertionChecks & (int)InsertionCheck::ONCOMING_TRAIN) != 0) {
1120 : #ifdef DEBUG_INSERTION
1121 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1122 : std::cout << " nextLane=" << nextLane->getID() << " occupiedBidi\n";
1123 : }
1124 : #endif
1125 44498 : return false;
1126 : }
1127 : }
1128 :
1129 : // check if there are stops on the next lane that should be regarded
1130 : // (this block is duplicated before the loop to deal with the insertion lane)
1131 450742 : if (aVehicle->hasStops()) {
1132 8426 : const MSStop& nextStop = aVehicle->getNextStop();
1133 8426 : if (nextStop.lane == nextLane) {
1134 538 : std::stringstream msg;
1135 538 : msg << "scheduled stop on lane '" << nextStop.lane->getID() << "' too close";
1136 538 : const double distToStop = seen + nextStop.pars.endPos;
1137 538 : if (checkFailure(aVehicle, speed, dist, cfModel.insertionStopSpeed(aVehicle, speed, distToStop),
1138 538 : patchSpeedSpecial, msg.str(), InsertionCheck::STOP)) {
1139 : // we may not drive with the given velocity - we cannot stop at the stop
1140 : return false;
1141 : }
1142 538 : }
1143 : }
1144 :
1145 : // check leader on next lane
1146 450742 : const MSLeaderInfo nextLeaders = nextLane->getLastVehicleInformation(aVehicle, 0, 0);
1147 450742 : if (nextLeaders.hasVehicles()) {
1148 159651 : const double nextLaneSpeed = nextLane->safeInsertionSpeed(aVehicle, seen, nextLeaders, speed);
1149 : #ifdef DEBUG_INSERTION
1150 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1151 : std::cout << SIMTIME << " leader on lane '" << nextLane->getID() << "': " << nextLeaders.toString() << " nspeed=" << nextLaneSpeed << "\n";
1152 : }
1153 : #endif
1154 312249 : if (nextLaneSpeed == INVALID_SPEED || checkFailure(aVehicle, speed, dist, nextLaneSpeed, patchSpeed, "", InsertionCheck::LEADER_GAP)) {
1155 : // we may not drive with the given velocity - we crash into the leader
1156 : #ifdef DEBUG_INSERTION
1157 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1158 : std::cout << " isInsertionSuccess lane=" << getID()
1159 : << " veh=" << aVehicle->getID()
1160 : << " pos=" << pos
1161 : << " posLat=" << posLat
1162 : << " patchSpeed=" << patchSpeed
1163 : << " speed=" << speed
1164 : << " nspeed=" << nextLaneSpeed
1165 : << " nextLane=" << nextLane->getID()
1166 : << " lead=" << nextLeaders.toString()
1167 : << " failed (@641)!\n";
1168 : }
1169 : #endif
1170 : return false;
1171 : }
1172 : }
1173 408747 : if (!nextLane->checkForPedestrians(aVehicle, speed, dist, -seen, patchSpeed)) {
1174 : return false;
1175 : }
1176 : // check next lane's maximum velocity
1177 408723 : const double freeSpeed = cfModel.freeSpeed(aVehicle, speed, seen, nextLane->getVehicleMaxSpeed(aVehicle), true, MSCFModel::CalcReason::FUTURE);
1178 408722 : if (freeSpeed < speed) {
1179 60209 : if (patchSpeedSpecial) {
1180 59799 : speed = freeSpeed;
1181 59799 : dist = cfModel.brakeGap(speed) + aVehicle->getVehicleType().getMinGap();
1182 : } else {
1183 410 : if ((insertionChecks & (int)InsertionCheck::SPEED_LIMIT) != 0) {
1184 410 : if (!MSGlobals::gCheckRoutes) {
1185 15 : WRITE_WARNINGF(TL("Vehicle '%' is inserted too fast and will violate the speed limit on a lane '%', time=%."),
1186 : aVehicle->getID(), nextLane->getID(), time2string(SIMSTEP));
1187 : } else {
1188 : // we may not drive with the given velocity - we would be too fast on the next lane
1189 1215 : WRITE_ERRORF(TL("Vehicle '%' will not be able to depart using the given velocity (slow lane ahead), time=%."), aVehicle->getID(), time2string(SIMSTEP));
1190 405 : MSNet::getInstance()->getInsertionControl().descheduleDeparture(aVehicle);
1191 : return false;
1192 : }
1193 : }
1194 : }
1195 : }
1196 : // check traffic on next junction
1197 : // we cannot use (*link)->opened because a vehicle without priority
1198 : // may already be comitted to blocking the link and unable to stop
1199 408317 : const SUMOTime leaveTime = (*link)->getLeaveTime(arrivalTime, speed, speed, aVehicle->getVehicleType().getLength());
1200 408317 : if ((*link)->hasApproachingFoe(arrivalTime, leaveTime, speed, cfModel.getMaxDecel())) {
1201 10110 : if (checkFailure(aVehicle, speed, dist, cfModel.insertionStopSpeed(aVehicle, speed, seen), patchSpeed, "", InsertionCheck::JUNCTION)) {
1202 : // we may not drive with the given velocity - we crash at the junction
1203 : return false;
1204 : }
1205 : }
1206 433614 : arrivalTime += TIME2STEPS(nextLane->getLength() / MAX2(speed, NUMERICAL_EPS));
1207 406243 : seen += nextLane->getLength();
1208 : currentLane = nextLane;
1209 406243 : if ((*link)->getViaLane() == nullptr) {
1210 191571 : nRouteSuccs++;
1211 : ++ce;
1212 : ++ri;
1213 : }
1214 450742 : }
1215 : }
1216 :
1217 4450996 : const MSLeaderDistanceInfo& followers = getFollowersOnConsecutive(aVehicle, aVehicle->getBackPositionOnLane(), false);
1218 9666215 : for (int i = 0; i < followers.numSublanes(); ++i) {
1219 6195612 : const MSVehicle* follower = followers[i].first;
1220 6195612 : if (follower != nullptr) {
1221 1332544 : const double backGapNeeded = follower->getCarFollowModel().getSecureGap(follower, aVehicle, follower->getSpeed(), speed, cfModel.getMaxDecel());
1222 1332544 : if (followers[i].second < backGapNeeded
1223 1332544 : && ((insertionChecks & (int)InsertionCheck::FOLLOWER_GAP) != 0
1224 80 : || (followers[i].second < 0 && (insertionChecks & (int)InsertionCheck::COLLISION) != 0))) {
1225 : // too close to the follower on this lane
1226 : #ifdef DEBUG_INSERTION
1227 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1228 : std::cout << SIMTIME << " isInsertionSuccess lane=" << getID()
1229 : << " veh=" << aVehicle->getID()
1230 : << " pos=" << pos
1231 : << " posLat=" << posLat
1232 : << " speed=" << speed
1233 : << " nspeed=" << nspeed
1234 : << " follower=" << follower->getID()
1235 : << " backGapNeeded=" << backGapNeeded
1236 : << " gap=" << followers[i].second
1237 : << " failure (@719)!\n";
1238 : }
1239 : #endif
1240 980393 : return false;
1241 : }
1242 : }
1243 : }
1244 :
1245 3470603 : if (!checkForPedestrians(aVehicle, speed, dist, pos, patchSpeed)) {
1246 : return false;
1247 : }
1248 :
1249 3470097 : MSLane* shadowLane = aVehicle->getLaneChangeModel().getShadowLane(this);
1250 : #ifdef DEBUG_INSERTION
1251 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1252 : std::cout << " shadowLane=" << Named::getIDSecure(shadowLane) << "\n";
1253 : }
1254 : #endif
1255 3470097 : if (shadowLane != nullptr) {
1256 1270 : const MSLeaderDistanceInfo& shadowFollowers = shadowLane->getFollowersOnConsecutive(aVehicle, aVehicle->getBackPositionOnLane(), false);
1257 4228 : for (int i = 0; i < shadowFollowers.numSublanes(); ++i) {
1258 3369 : const MSVehicle* follower = shadowFollowers[i].first;
1259 3369 : if (follower != nullptr) {
1260 419 : const double backGapNeeded = follower->getCarFollowModel().getSecureGap(follower, aVehicle, follower->getSpeed(), speed, cfModel.getMaxDecel());
1261 419 : if (shadowFollowers[i].second < backGapNeeded
1262 419 : && ((insertionChecks & (int)InsertionCheck::FOLLOWER_GAP) != 0
1263 0 : || (shadowFollowers[i].second < 0 && (insertionChecks & (int)InsertionCheck::COLLISION) != 0))) {
1264 : // too close to the follower on this lane
1265 : #ifdef DEBUG_INSERTION
1266 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1267 : std::cout << SIMTIME
1268 : << " isInsertionSuccess shadowlane=" << shadowLane->getID()
1269 : << " veh=" << aVehicle->getID()
1270 : << " pos=" << pos
1271 : << " posLat=" << posLat
1272 : << " speed=" << speed
1273 : << " nspeed=" << nspeed
1274 : << " follower=" << follower->getID()
1275 : << " backGapNeeded=" << backGapNeeded
1276 : << " gap=" << shadowFollowers[i].second
1277 : << " failure (@812)!\n";
1278 : }
1279 : #endif
1280 411 : return false;
1281 : }
1282 : }
1283 : }
1284 859 : const MSLeaderInfo& ahead = shadowLane->getLastVehicleInformation(nullptr, 0, aVehicle->getPositionOnLane(), false);
1285 2849 : for (int i = 0; i < ahead.numSublanes(); ++i) {
1286 2085 : const MSVehicle* veh = ahead[i];
1287 2085 : if (veh != nullptr) {
1288 344 : const double gap = veh->getBackPositionOnLane(shadowLane) - aVehicle->getPositionOnLane() - aVehicle->getVehicleType().getMinGap();
1289 344 : const double gapNeeded = aVehicle->getCarFollowModel().getSecureGap(aVehicle, veh, speed, veh->getSpeed(), veh->getCarFollowModel().getMaxDecel());
1290 344 : if (gap < gapNeeded
1291 95 : && ((insertionChecks & (int)InsertionCheck::LEADER_GAP) != 0
1292 0 : || (gap < 0 && (insertionChecks & (int)InsertionCheck::COLLISION) != 0))) {
1293 : // too close to the shadow leader
1294 : #ifdef DEBUG_INSERTION
1295 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1296 : std::cout << SIMTIME
1297 : << " isInsertionSuccess shadowlane=" << shadowLane->getID()
1298 : << " veh=" << aVehicle->getID()
1299 : << " pos=" << pos
1300 : << " posLat=" << posLat
1301 : << " speed=" << speed
1302 : << " nspeed=" << nspeed
1303 : << " leader=" << veh->getID()
1304 : << " gapNeeded=" << gapNeeded
1305 : << " gap=" << gap
1306 : << " failure (@842)!\n";
1307 : }
1308 : #endif
1309 : return false;
1310 : }
1311 : }
1312 : }
1313 1270 : }
1314 3469591 : if (followers.numFreeSublanes() > 0) {
1315 : // check approaching vehicles to prevent rear-end collisions
1316 3255796 : const double backOffset = pos - aVehicle->getVehicleType().getLength();
1317 3255796 : const double missingRearGap = getMissingRearGap(aVehicle, backOffset, speed);
1318 3255796 : if (missingRearGap > 0
1319 0 : && (insertionChecks & (int)InsertionCheck::LEADER_GAP) != 0) {
1320 : // too close to a follower
1321 : #ifdef DEBUG_INSERTION
1322 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1323 : std::cout << SIMTIME
1324 : << " isInsertionSuccess lane=" << getID()
1325 : << " veh=" << aVehicle->getID()
1326 : << " pos=" << pos
1327 : << " posLat=" << posLat
1328 : << " speed=" << speed
1329 : << " nspeed=" << nspeed
1330 : << " missingRearGap=" << missingRearGap
1331 : << " failure (@728)!\n";
1332 : }
1333 : #endif
1334 : return false;
1335 : }
1336 : }
1337 3469591 : if (insertionChecks == (int)InsertionCheck::NONE) {
1338 2332 : speed = MAX2(0.0, speed);
1339 : }
1340 : // may got negative while adaptation
1341 3469591 : if (speed < 0) {
1342 : #ifdef DEBUG_INSERTION
1343 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1344 : std::cout << SIMTIME
1345 : << " isInsertionSuccess lane=" << getID()
1346 : << " veh=" << aVehicle->getID()
1347 : << " pos=" << pos
1348 : << " posLat=" << posLat
1349 : << " speed=" << speed
1350 : << " nspeed=" << nspeed
1351 : << " failed (@733)!\n";
1352 : }
1353 : #endif
1354 : return false;
1355 : }
1356 3469590 : const int bestLaneOffset = aVehicle->getBestLaneOffset();
1357 3469590 : const double extraReservation = aVehicle->getLaneChangeModel().getExtraReservation(bestLaneOffset);
1358 3469590 : if (extraReservation > 0) {
1359 23387 : std::stringstream msg;
1360 23387 : msg << "too many lane changes required on lane '" << myID << "'";
1361 : // we need to take into acount one extra actionStep of delay due to #3665
1362 23387 : double distToStop = aVehicle->getBestLaneDist() - pos - extraReservation - speed * aVehicle->getActionStepLengthSecs();
1363 23387 : if (distToStop >= 0) {
1364 : double stopSpeed = cfModel.stopSpeed(aVehicle, speed, distToStop, MSCFModel::CalcReason::FUTURE);
1365 : #ifdef DEBUG_INSERTION
1366 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1367 : std::cout << "\nIS_INSERTION_SUCCESS\n"
1368 : << SIMTIME << " veh=" << aVehicle->getID() << " bestLaneOffset=" << bestLaneOffset << " bestLaneDist=" << aVehicle->getBestLaneDist() << " extraReservation=" << extraReservation
1369 : << " distToStop=" << distToStop << " v=" << speed << " v2=" << stopSpeed << "\n";
1370 : }
1371 : #endif
1372 44977 : if (checkFailure(aVehicle, speed, distToStop, MAX2(0.0, stopSpeed),
1373 22493 : patchSpeedSpecial, msg.str(), InsertionCheck::LANECHANGE)) {
1374 : // we may not drive with the given velocity - we cannot reserve enough space for lane changing
1375 : return false;
1376 : }
1377 : }
1378 23387 : }
1379 : // enter
1380 3469580 : incorporateVehicle(aVehicle, pos, speed, posLat, find_if(myVehicles.begin(), myVehicles.end(), [&](MSVehicle * const v) {
1381 2919674 : return v->getPositionOnLane() >= pos;
1382 : }), notification);
1383 : #ifdef DEBUG_INSERTION
1384 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1385 : std::cout << SIMTIME
1386 : << " isInsertionSuccess lane=" << getID()
1387 : << " veh=" << aVehicle->getID()
1388 : << " pos=" << pos
1389 : << " posLat=" << posLat
1390 : << " speed=" << speed
1391 : << " nspeed=" << nspeed
1392 : << "\n myVehicles=" << toString(myVehicles)
1393 : << " myPartial=" << toString(myPartialVehicles)
1394 : << " myManeuverReservations=" << toString(myManeuverReservations)
1395 : << "\n success!\n";
1396 : }
1397 : #endif
1398 3469578 : if (isRail) {
1399 5488 : unsetParameter("insertionConstraint:" + aVehicle->getID());
1400 5488 : unsetParameter("insertionOrder:" + aVehicle->getID());
1401 5488 : unsetParameter("insertionBlocked:" + aVehicle->getID());
1402 : // rail_signal (not traffic_light) requires approach information for
1403 : // switching correctly at the start of the next simulation step
1404 5488 : if (firstRailSignal != nullptr && firstRailSignal->getJunction()->getType() == SumoXMLNodeType::RAIL_SIGNAL) {
1405 1842 : aVehicle->registerInsertionApproach(firstRailSignal, firstRailSignalDist);
1406 : }
1407 : }
1408 : return true;
1409 13918696 : }
1410 :
1411 :
1412 : void
1413 48268 : MSLane::forceVehicleInsertion(MSVehicle* veh, double pos, MSMoveReminder::Notification notification, double posLat) {
1414 48268 : veh->updateBestLanes(true, this);
1415 : bool dummy;
1416 48268 : const double speed = veh->hasDeparted() ? veh->getSpeed() : getDepartSpeed(*veh, dummy);
1417 48268 : incorporateVehicle(veh, pos, speed, posLat, find_if(myVehicles.begin(), myVehicles.end(), [&](MSVehicle * const v) {
1418 126103 : return v->getPositionOnLane() >= pos;
1419 : }), notification);
1420 48268 : }
1421 :
1422 :
1423 : double
1424 14078345 : MSLane::safeInsertionSpeed(const MSVehicle* veh, double seen, const MSLeaderInfo& leaders, double speed) {
1425 : double nspeed = speed;
1426 : #ifdef DEBUG_INSERTION
1427 : if (DEBUG_COND2(veh)) {
1428 : std::cout << SIMTIME << " safeInsertionSpeed veh=" << veh->getID() << " speed=" << speed << "\n";
1429 : }
1430 : #endif
1431 24784269 : for (int i = 0; i < leaders.numSublanes(); ++i) {
1432 17520544 : const MSVehicle* leader = leaders[i];
1433 17520544 : if (leader != nullptr) {
1434 15298789 : double gap = leader->getBackPositionOnLane(this) + seen - veh->getVehicleType().getMinGap();
1435 15298789 : if (leader->getLane() == getBidiLane()) {
1436 : // use distance to front position and account for movement
1437 270 : gap -= (leader->getLength() + leader->getBrakeGap(true));
1438 : }
1439 15298789 : if (gap < 0) {
1440 : #ifdef DEBUG_INSERTION
1441 : if (DEBUG_COND2(veh)) {
1442 : std::cout << " leader=" << leader->getID() << " bPos=" << leader->getBackPositionOnLane(this) << " gap=" << gap << "\n";
1443 : }
1444 : #endif
1445 6814620 : if ((veh->getInsertionChecks() & (int)InsertionCheck::COLLISION) != 0) {
1446 : return INVALID_SPEED;
1447 : } else {
1448 0 : return 0;
1449 : }
1450 : }
1451 8484169 : nspeed = MIN2(nspeed,
1452 8484169 : veh->getCarFollowModel().insertionFollowSpeed(veh, speed, gap, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel(), leader));
1453 : #ifdef DEBUG_INSERTION
1454 : if (DEBUG_COND2(veh)) {
1455 : std::cout << " leader=" << leader->getID() << " bPos=" << leader->getBackPositionOnLane(this) << " gap=" << gap << " nspeed=" << nspeed << "\n";
1456 : }
1457 : #endif
1458 : }
1459 : }
1460 : return nspeed;
1461 : }
1462 :
1463 :
1464 : // ------ Handling vehicles lapping into lanes ------
1465 : const MSLeaderInfo
1466 730438352 : MSLane::getLastVehicleInformation(const MSVehicle* ego, double latOffset, double minPos, bool allowCached) const {
1467 730438352 : if (myLeaderInfoTime < MSNet::getInstance()->getCurrentTimeStep() || ego != nullptr || minPos > 0 || !allowCached) {
1468 251878660 : MSLeaderInfo leaderTmp(myWidth, ego, latOffset);
1469 : AnyVehicleIterator last = anyVehiclesBegin();
1470 : int freeSublanes = 1; // number of sublanes for which no leader was found
1471 : //if (ego->getID() == "disabled" && SIMTIME == 58) {
1472 : // std::cout << "DEBUG\n";
1473 : //}
1474 251878660 : const MSVehicle* veh = *last;
1475 1894594183 : while (freeSublanes > 0 && veh != nullptr) {
1476 : #ifdef DEBUG_PLAN_MOVE
1477 : if (DEBUG_COND2(ego) || DEBUG_COND) {
1478 : gDebugFlag1 = true;
1479 : std::cout << " getLastVehicleInformation lane=" << getID() << " minPos=" << minPos << " veh=" << veh->getID() << " pos=" << veh->getPositionOnLane(this) << "\n";
1480 : }
1481 : #endif
1482 3282989515 : if (veh != ego && MAX2(0.0, veh->getPositionOnLane(this)) >= minPos) {
1483 265676081 : const double vehLatOffset = veh->getLatOffset(this);
1484 265676081 : freeSublanes = leaderTmp.addLeader(veh, true, vehLatOffset);
1485 : #ifdef DEBUG_PLAN_MOVE
1486 : if (DEBUG_COND2(ego) || DEBUG_COND) {
1487 : std::cout << " latOffset=" << vehLatOffset << " newLeaders=" << leaderTmp.toString() << "\n";
1488 : }
1489 : #endif
1490 : }
1491 1642715523 : veh = *(++last);
1492 : }
1493 251878660 : if (ego == nullptr && minPos == 0) {
1494 : #ifdef HAVE_FOX
1495 110254974 : ScopedLocker<> lock(myLeaderInfoMutex, MSGlobals::gNumSimThreads > 1);
1496 : #endif
1497 : // update cached value
1498 : myLeaderInfo = leaderTmp;
1499 110254974 : myLeaderInfoTime = MSNet::getInstance()->getCurrentTimeStep();
1500 : }
1501 : #ifdef DEBUG_PLAN_MOVE
1502 : //if (DEBUG_COND2(ego)) std::cout << SIMTIME
1503 : // << " getLastVehicleInformation lane=" << getID()
1504 : // << " ego=" << Named::getIDSecure(ego)
1505 : // << "\n"
1506 : // << " vehicles=" << toString(myVehicles)
1507 : // << " partials=" << toString(myPartialVehicles)
1508 : // << "\n"
1509 : // << " result=" << leaderTmp.toString()
1510 : // << " cached=" << myLeaderInfo.toString()
1511 : // << " myLeaderInfoTime=" << myLeaderInfoTime
1512 : // << "\n";
1513 : gDebugFlag1 = false;
1514 : #endif
1515 : return leaderTmp;
1516 251878660 : }
1517 : return myLeaderInfo;
1518 : }
1519 :
1520 :
1521 : const MSLeaderInfo
1522 457760038 : MSLane::getFirstVehicleInformation(const MSVehicle* ego, double latOffset, bool onlyFrontOnLane, double maxPos, bool allowCached) const {
1523 : #ifdef HAVE_FOX
1524 457760038 : ScopedLocker<> lock(myFollowerInfoMutex, MSGlobals::gNumSimThreads > 1);
1525 : #endif
1526 457760038 : if (myFollowerInfoTime < MSNet::getInstance()->getCurrentTimeStep() || ego != nullptr || maxPos < myLength || !allowCached || onlyFrontOnLane) {
1527 : // XXX separate cache for onlyFrontOnLane = true
1528 457760038 : MSLeaderInfo followerTmp(myWidth, ego, latOffset);
1529 : AnyVehicleIterator first = anyVehiclesUpstreamBegin();
1530 : int freeSublanes = 1; // number of sublanes for which no leader was found
1531 457760038 : const MSVehicle* veh = *first;
1532 729520209 : while (freeSublanes > 0 && veh != nullptr) {
1533 : #ifdef DEBUG_PLAN_MOVE
1534 : if (DEBUG_COND2(ego)) {
1535 : std::cout << " veh=" << veh->getID() << " pos=" << veh->getPositionOnLane(this) << " maxPos=" << maxPos << "\n";
1536 : }
1537 : #endif
1538 271760171 : if (veh != ego && veh->getPositionOnLane(this) <= maxPos
1539 543520342 : && (!onlyFrontOnLane || veh->isFrontOnLane(this))) {
1540 : //const double vehLatOffset = veh->getLane()->getRightSideOnEdge() - getRightSideOnEdge();
1541 248289988 : const double vehLatOffset = veh->getLatOffset(this);
1542 : #ifdef DEBUG_PLAN_MOVE
1543 : if (DEBUG_COND2(ego)) {
1544 : std::cout << " veh=" << veh->getID() << " latOffset=" << vehLatOffset << "\n";
1545 : }
1546 : #endif
1547 248289988 : freeSublanes = followerTmp.addLeader(veh, true, vehLatOffset);
1548 : }
1549 271760171 : veh = *(++first);
1550 : }
1551 457760038 : if (ego == nullptr && maxPos == std::numeric_limits<double>::max()) {
1552 : // update cached value
1553 : myFollowerInfo = followerTmp;
1554 457760038 : myFollowerInfoTime = MSNet::getInstance()->getCurrentTimeStep();
1555 : }
1556 : #ifdef DEBUG_PLAN_MOVE
1557 : //if (DEBUG_COND2(ego)) std::cout << SIMTIME
1558 : // << " getFirstVehicleInformation lane=" << getID()
1559 : // << " ego=" << Named::getIDSecure(ego)
1560 : // << "\n"
1561 : // << " vehicles=" << toString(myVehicles)
1562 : // << " partials=" << toString(myPartialVehicles)
1563 : // << "\n"
1564 : // << " result=" << followerTmp.toString()
1565 : // //<< " cached=" << myFollowerInfo.toString()
1566 : // << " myLeaderInfoTime=" << myLeaderInfoTime
1567 : // << "\n";
1568 : #endif
1569 : return followerTmp;
1570 457760038 : }
1571 : return myFollowerInfo;
1572 : }
1573 :
1574 :
1575 : // ------ ------
1576 : void
1577 94171225 : MSLane::planMovements(SUMOTime t) {
1578 : assert(myVehicles.size() != 0);
1579 : double cumulatedVehLength = 0.;
1580 94171225 : MSLeaderInfo leaders(myWidth);
1581 :
1582 : // iterate over myVehicles, myPartialVehicles, and myManeuverReservations merge-sort style
1583 : VehCont::reverse_iterator veh = myVehicles.rbegin();
1584 : VehCont::reverse_iterator vehPart = myPartialVehicles.rbegin();
1585 : VehCont::reverse_iterator vehRes = myManeuverReservations.rbegin();
1586 : #ifdef DEBUG_PLAN_MOVE
1587 : if (DEBUG_COND) std::cout
1588 : << "\n"
1589 : << SIMTIME
1590 : << " planMovements() lane=" << getID()
1591 : << "\n vehicles=" << toString(myVehicles)
1592 : << "\n partials=" << toString(myPartialVehicles)
1593 : << "\n reservations=" << toString(myManeuverReservations)
1594 : << "\n";
1595 : #endif
1596 : assert(MSGlobals::gLateralResolution || myManeuverReservations.size() == 0);
1597 795305918 : for (; veh != myVehicles.rend(); ++veh) {
1598 : #ifdef DEBUG_PLAN_MOVE
1599 : if (DEBUG_COND2((*veh))) {
1600 : std::cout << " plan move for: " << (*veh)->getID();
1601 : }
1602 : #endif
1603 701134693 : updateLeaderInfo(*veh, vehPart, vehRes, leaders); // 36ns with 8 threads, 9ns with 1
1604 : #ifdef DEBUG_PLAN_MOVE
1605 : if (DEBUG_COND2((*veh))) {
1606 : std::cout << " leaders=" << leaders.toString() << "\n";
1607 : }
1608 : #endif
1609 701134693 : (*veh)->planMove(t, leaders, cumulatedVehLength); // 4800ns with 8 threads, 3100 with 1
1610 701134693 : cumulatedVehLength += (*veh)->getVehicleType().getLengthWithGap();
1611 701134693 : leaders.addLeader(*veh, false, 0);
1612 : }
1613 94171225 : }
1614 :
1615 :
1616 : void
1617 94171225 : MSLane::setJunctionApproaches() const {
1618 795305918 : for (MSVehicle* const veh : myVehicles) {
1619 701134693 : veh->setApproachingForAllLinks();
1620 : }
1621 94171225 : }
1622 :
1623 :
1624 : void
1625 701134693 : MSLane::updateLeaderInfo(const MSVehicle* veh, VehCont::reverse_iterator& vehPart, VehCont::reverse_iterator& vehRes, MSLeaderInfo& ahead) const {
1626 : bool morePartialVehsAhead = vehPart != myPartialVehicles.rend();
1627 : bool moreReservationsAhead = vehRes != myManeuverReservations.rend();
1628 : bool nextToConsiderIsPartial;
1629 :
1630 : // Determine relevant leaders for veh
1631 711442894 : while (moreReservationsAhead || morePartialVehsAhead) {
1632 1251144 : if ((!moreReservationsAhead || (*vehRes)->getPositionOnLane(this) <= veh->getPositionOnLane())
1633 20508621 : && (!morePartialVehsAhead || (*vehPart)->getPositionOnLane(this) <= veh->getPositionOnLane())) {
1634 : // All relevant downstream vehicles have been collected.
1635 : break;
1636 : }
1637 :
1638 : // Check whether next farthest relevant vehicle downstream is a partial vehicle or a maneuver reservation
1639 10308201 : if (moreReservationsAhead && !morePartialVehsAhead) {
1640 : nextToConsiderIsPartial = false;
1641 10218121 : } else if (morePartialVehsAhead && !moreReservationsAhead) {
1642 : nextToConsiderIsPartial = true;
1643 : } else {
1644 : assert(morePartialVehsAhead && moreReservationsAhead);
1645 : // Add farthest downstream vehicle first
1646 166751 : nextToConsiderIsPartial = (*vehPart)->getPositionOnLane(this) > (*vehRes)->getPositionOnLane(this);
1647 : }
1648 : // Add appropriate leader information
1649 166751 : if (nextToConsiderIsPartial) {
1650 10169078 : const double latOffset = (*vehPart)->getLatOffset(this);
1651 : #ifdef DEBUG_PLAN_MOVE
1652 : if (DEBUG_COND) {
1653 : std::cout << " partial ahead: " << (*vehPart)->getID() << " latOffset=" << latOffset << "\n";
1654 : }
1655 : #endif
1656 10236173 : if (!(MSGlobals::gLaneChangeDuration > 0 && (*vehPart)->getLaneChangeModel().isOpposite()
1657 67095 : && !(*vehPart)->getLaneChangeModel().isChangingLanes())) {
1658 10119315 : ahead.addLeader(*vehPart, false, latOffset);
1659 : }
1660 : ++vehPart;
1661 : morePartialVehsAhead = vehPart != myPartialVehicles.rend();
1662 : } else {
1663 139123 : const double latOffset = (*vehRes)->getLatOffset(this);
1664 : #ifdef DEBUG_PLAN_MOVE
1665 : if (DEBUG_COND) {
1666 : std::cout << " reservation ahead: " << (*vehRes)->getID() << " latOffset=" << latOffset << "\n";
1667 : }
1668 : #endif
1669 139123 : ahead.addLeader(*vehRes, false, latOffset);
1670 : ++vehRes;
1671 : moreReservationsAhead = vehRes != myManeuverReservations.rend();
1672 : }
1673 : }
1674 701134693 : }
1675 :
1676 :
1677 : void
1678 101970155 : MSLane::detectCollisions(SUMOTime timestep, const std::string& stage) {
1679 101970155 : myNeedsCollisionCheck = false;
1680 : #ifdef DEBUG_COLLISIONS
1681 : if (DEBUG_COND) {
1682 : std::vector<const MSVehicle*> all;
1683 : for (AnyVehicleIterator last = anyVehiclesBegin(); last != anyVehiclesEnd(); ++last) {
1684 : all.push_back(*last);
1685 : }
1686 : std::cout << SIMTIME << " detectCollisions stage=" << stage << " lane=" << getID() << ":\n"
1687 : << " vehs=" << toString(myVehicles) << "\n"
1688 : << " part=" << toString(myPartialVehicles) << "\n"
1689 : << " all=" << toString(all) << "\n"
1690 : << "\n";
1691 : }
1692 : #endif
1693 :
1694 101970155 : if (myCollisionAction == COLLISION_ACTION_NONE) {
1695 730429 : return;
1696 : }
1697 :
1698 : std::set<const MSVehicle*, ComparatorNumericalIdLess> toRemove;
1699 : std::set<const MSVehicle*, ComparatorNumericalIdLess> toTeleport;
1700 101957233 : if (mustCheckJunctionCollisions()) {
1701 1349238 : myNeedsCollisionCheck = true; // always check
1702 : #ifdef DEBUG_JUNCTION_COLLISIONS
1703 : if (DEBUG_COND) {
1704 : std::cout << SIMTIME << " detect junction Collisions stage=" << stage << " lane=" << getID() << ":\n"
1705 : << " vehs=" << toString(myVehicles) << "\n"
1706 : << " part=" << toString(myPartialVehicles) << "\n"
1707 : << "\n";
1708 : }
1709 : #endif
1710 : assert(myLinks.size() == 1);
1711 1349238 : const std::vector<const MSLane*>& foeLanes = myLinks.front()->getFoeLanes();
1712 : // save the iterator, it might get modified, see #8842
1713 : MSLane::AnyVehicleIterator end = anyVehiclesEnd();
1714 2475684 : for (AnyVehicleIterator veh = anyVehiclesBegin(); veh != end; ++veh) {
1715 2475684 : const MSVehicle* const collider = *veh;
1716 : //std::cout << " collider " << collider->getID() << "\n";
1717 2475684 : PositionVector colliderBoundary = collider->getBoundingBox(myCheckJunctionCollisionMinGap);
1718 34777085 : for (const MSLane* const foeLane : foeLanes) {
1719 : #ifdef DEBUG_JUNCTION_COLLISIONS
1720 : if (DEBUG_COND) {
1721 : std::cout << " foeLane " << foeLane->getID()
1722 : << " foeVehs=" << toString(foeLane->myVehicles)
1723 : << " foePart=" << toString(foeLane->myPartialVehicles) << "\n";
1724 : }
1725 : #endif
1726 : MSLane::AnyVehicleIterator foeEnd = foeLane->anyVehiclesEnd();
1727 4257532 : for (MSLane::AnyVehicleIterator it_veh = foeLane->anyVehiclesBegin(); it_veh != foeEnd; ++it_veh) {
1728 4257532 : const MSVehicle* const victim = *it_veh;
1729 4257532 : if (victim == collider) {
1730 : // may happen if the vehicles lane and shadow lane are siblings
1731 1553 : continue;
1732 : }
1733 : #ifdef DEBUG_JUNCTION_COLLISIONS
1734 : if (DEBUG_COND && DEBUG_COND2(collider)) {
1735 : std::cout << SIMTIME << " foe=" << victim->getID()
1736 : << " bound=" << colliderBoundary << " foeBound=" << victim->getBoundingBox()
1737 : << " overlaps=" << colliderBoundary.overlapsWith(victim->getBoundingBox())
1738 : << " poly=" << collider->getBoundingPoly()
1739 : << " foePoly=" << victim->getBoundingPoly()
1740 : << " overlaps2=" << collider->getBoundingPoly().overlapsWith(victim->getBoundingPoly())
1741 : << "\n";
1742 : }
1743 : #endif
1744 4255979 : if (MSGlobals::gIgnoreJunctionBlocker < std::numeric_limits<SUMOTime>::max()) {
1745 7280 : if (collider->getWaitingTime() >= MSGlobals::gIgnoreJunctionBlocker
1746 7280 : || victim->getWaitingTime() >= MSGlobals::gIgnoreJunctionBlocker) {
1747 : // ignored vehicles should not tigger collision
1748 5904 : continue;
1749 : }
1750 : }
1751 :
1752 4250075 : if (colliderBoundary.overlapsWith(victim->getBoundingBox())) {
1753 : // make a detailed check
1754 22976 : PositionVector boundingPoly = collider->getBoundingPoly();
1755 22976 : if (collider->getBoundingPoly(myCheckJunctionCollisionMinGap).overlapsWith(victim->getBoundingPoly())) {
1756 : // junction leader is the victim (collider must still be on junction)
1757 : assert(isInternal());
1758 14458 : if (victim->getLane()->isInternal() && victim->isLeader(myLinks.front(), collider, -1)) {
1759 6427 : foeLane->handleCollisionBetween(timestep, stage, victim, collider, -1, 0, toRemove, toTeleport);
1760 : } else {
1761 8031 : handleCollisionBetween(timestep, stage, collider, victim, -1, 0, toRemove, toTeleport);
1762 : }
1763 : }
1764 22976 : }
1765 : }
1766 32301401 : detectPedestrianJunctionCollision(collider, colliderBoundary, foeLane, timestep, stage, toRemove, toTeleport);
1767 : }
1768 2475684 : if (myLinks.front()->getWalkingAreaFoe() != nullptr) {
1769 59508 : detectPedestrianJunctionCollision(collider, colliderBoundary, myLinks.front()->getWalkingAreaFoe(), timestep, stage, toRemove, toTeleport);
1770 : }
1771 2475684 : if (myLinks.front()->getWalkingAreaFoeExit() != nullptr) {
1772 55556 : detectPedestrianJunctionCollision(collider, colliderBoundary, myLinks.front()->getWalkingAreaFoeExit(), timestep, stage, toRemove, toTeleport);
1773 : }
1774 2475684 : }
1775 : }
1776 :
1777 :
1778 101957233 : if (myIntermodalCollisionAction != COLLISION_ACTION_NONE && myEdge->getPersons().size() > 0 && hasPedestrians()) {
1779 : #ifdef DEBUG_PEDESTRIAN_COLLISIONS
1780 : if (DEBUG_COND) {
1781 : std::cout << SIMTIME << " detect pedestrian collisions stage=" << stage << " lane=" << getID() << "\n";
1782 : }
1783 : #endif
1784 : AnyVehicleIterator v_end = anyVehiclesEnd();
1785 146796 : for (AnyVehicleIterator it_v = anyVehiclesBegin(); it_v != v_end; ++it_v) {
1786 146796 : const MSVehicle* v = *it_v;
1787 146796 : double back = v->getBackPositionOnLane(this);
1788 146796 : const double length = v->getVehicleType().getLength();
1789 146796 : const double right = v->getRightSideOnEdge(this) - getRightSideOnEdge();
1790 146796 : if (v->getLane() == getBidiLane()) {
1791 : // use the front position for checking
1792 611 : back -= length;
1793 : }
1794 146796 : PersonDist leader = nextBlocking(back, right, right + v->getVehicleType().getWidth());
1795 : #ifdef DEBUG_PEDESTRIAN_COLLISIONS
1796 : if (DEBUG_COND && DEBUG_COND2(v)) {
1797 : std::cout << SIMTIME << " back=" << back << " right=" << right << " person=" << Named::getIDSecure(leader.first)
1798 : << " dist=" << leader.second << " jammed=" << (leader.first == nullptr ? false : leader.first->isJammed()) << "\n";
1799 : }
1800 : #endif
1801 146796 : if (leader.first != 0 && leader.second < length && !leader.first->isJammed()) {
1802 96 : if (v->getVehicleType().getGuiShape() == SUMOVehicleShape::AIRCRAFT) {
1803 : // aircraft wings and body are above walking level
1804 : continue;
1805 : }
1806 96 : const double gap = leader.second - length;
1807 192 : handleIntermodalCollisionBetween(timestep, stage, v, leader.first, gap, "sharedLane", toRemove, toTeleport);
1808 : }
1809 : }
1810 : }
1811 :
1812 101957233 : if (myVehicles.size() == 0) {
1813 : return;
1814 : }
1815 101239726 : if (!MSGlobals::gSublane) {
1816 : // no sublanes
1817 : VehCont::reverse_iterator lastVeh = myVehicles.rend() - 1;
1818 638154540 : for (VehCont::reverse_iterator pred = myVehicles.rbegin(); pred != lastVeh; ++pred) {
1819 : VehCont::reverse_iterator veh = pred + 1;
1820 557363445 : detectCollisionBetween(timestep, stage, *veh, *pred, toRemove, toTeleport);
1821 : }
1822 80791095 : if (myPartialVehicles.size() > 0) {
1823 6989858 : detectCollisionBetween(timestep, stage, *lastVeh, myPartialVehicles.front(), toRemove, toTeleport);
1824 : }
1825 80791095 : if (getBidiLane() != nullptr) {
1826 : // bidirectional railway
1827 481295 : MSLane* bidiLane = getBidiLane();
1828 481295 : if (bidiLane->getVehicleNumberWithPartials() > 0) {
1829 331162 : for (auto veh = myVehicles.begin(); veh != myVehicles.end(); ++veh) {
1830 232547 : double high = (*veh)->getPositionOnLane(this);
1831 232547 : double low = (*veh)->getBackPositionOnLane(this);
1832 232547 : if (stage == MSNet::STAGE_MOVEMENTS) {
1833 : // use previous back position to catch trains that
1834 : // "jump" through each other
1835 210272 : low -= SPEED2DIST((*veh)->getSpeed());
1836 : }
1837 1047899 : for (AnyVehicleIterator veh2 = bidiLane->anyVehiclesBegin(); veh2 != bidiLane->anyVehiclesEnd(); ++veh2) {
1838 : // self-collisions might legitemately occur when a long train loops back on itself
1839 1047899 : if (*veh == *veh2 && !(*veh)->isRail()) {
1840 231693 : continue;
1841 : }
1842 933478 : if ((*veh)->getLane() == (*veh2)->getLane() ||
1843 902050 : (*veh)->getLane() == (*veh2)->getBackLane() ||
1844 85844 : (*veh)->getBackLane() == (*veh2)->getLane()) {
1845 : // vehicles are not in a bidi relation
1846 730362 : continue;
1847 : }
1848 85844 : double low2 = myLength - (*veh2)->getPositionOnLane(bidiLane);
1849 85844 : double high2 = myLength - (*veh2)->getBackPositionOnLane(bidiLane);
1850 85844 : if (stage == MSNet::STAGE_MOVEMENTS) {
1851 : // use previous back position to catch trains that
1852 : // "jump" through each other
1853 72593 : high2 += SPEED2DIST((*veh2)->getSpeed());
1854 : }
1855 85844 : if (!(high < low2 || high2 < low)) {
1856 : #ifdef DEBUG_COLLISIONS
1857 : if (DEBUG_COND) {
1858 : std::cout << SIMTIME << " bidi-collision veh=" << (*veh)->getID() << " bidiVeh=" << (*veh2)->getID()
1859 : << " vehFurther=" << toString((*veh)->getFurtherLanes())
1860 : << " high=" << high << " low=" << low << " high2=" << high2 << " low2=" << low2 << "\n";
1861 : }
1862 : #endif
1863 : // the faster vehicle is at fault
1864 41 : MSVehicle* collider = const_cast<MSVehicle*>(*veh);
1865 41 : MSVehicle* victim = const_cast<MSVehicle*>(*veh2);
1866 41 : if (collider->getSpeed() < victim->getSpeed()) {
1867 : std::swap(victim, collider);
1868 : }
1869 41 : handleCollisionBetween(timestep, stage, collider, victim, -1, 0, toRemove, toTeleport);
1870 : }
1871 : }
1872 : }
1873 : }
1874 : }
1875 : } else {
1876 : // in the sublane-case it is insufficient to check the vehicles ordered
1877 : // by their front position as there might be more than 2 vehicles next to each
1878 : // other on the same lane
1879 : // instead, a moving-window approach is used where all vehicles that
1880 : // overlap in the longitudinal direction receive pairwise checks
1881 : // XXX for efficiency, all lanes of an edge should be checked together
1882 : // (lanechanger-style)
1883 :
1884 : // XXX quick hack: check each in myVehicles against all others
1885 141231344 : for (AnyVehicleIterator veh = anyVehiclesBegin(); veh != anyVehiclesEnd(); ++veh) {
1886 141231344 : MSVehicle* follow = (MSVehicle*)*veh;
1887 3760049718 : for (AnyVehicleIterator veh2 = anyVehiclesBegin(); veh2 != anyVehiclesEnd(); ++veh2) {
1888 3760147941 : MSVehicle* lead = (MSVehicle*)*veh2;
1889 3760147941 : if (lead == follow) {
1890 141208814 : continue;
1891 : }
1892 3618939127 : if (lead->getPositionOnLane(this) < follow->getPositionOnLane(this)) {
1893 1808464011 : continue;
1894 : }
1895 1810475116 : if (detectCollisionBetween(timestep, stage, follow, lead, toRemove, toTeleport)) {
1896 : // XXX what about collisions with multiple leaders at once?
1897 : break;
1898 : }
1899 : }
1900 : }
1901 : }
1902 :
1903 :
1904 101243701 : for (std::set<const MSVehicle*, ComparatorNumericalIdLess>::iterator it = toRemove.begin(); it != toRemove.end(); ++it) {
1905 3975 : MSVehicle* veh = const_cast<MSVehicle*>(*it);
1906 : MSLane* vehLane = veh->getMutableLane();
1907 3975 : vehLane->removeVehicle(veh, MSMoveReminder::NOTIFICATION_TELEPORT, false);
1908 : if (toTeleport.count(veh) > 0) {
1909 3590 : MSVehicleTransfer::getInstance()->add(timestep, veh);
1910 : } else {
1911 385 : veh->onRemovalFromNet(MSMoveReminder::NOTIFICATION_VAPORIZED_COLLISION);
1912 385 : MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(veh);
1913 : }
1914 : }
1915 : }
1916 :
1917 :
1918 : void
1919 32416465 : MSLane::detectPedestrianJunctionCollision(const MSVehicle* collider, const PositionVector& colliderBoundary, const MSLane* foeLane,
1920 : SUMOTime timestep, const std::string& stage,
1921 : std::set<const MSVehicle*, ComparatorNumericalIdLess>& toRemove,
1922 : std::set<const MSVehicle*, ComparatorNumericalIdLess>& toTeleport) {
1923 32416465 : if (myIntermodalCollisionAction != COLLISION_ACTION_NONE && foeLane->getEdge().getPersons().size() > 0 && foeLane->hasPedestrians()) {
1924 : #ifdef DEBUG_PEDESTRIAN_COLLISIONS
1925 : if (DEBUG_COND) {
1926 : std::cout << SIMTIME << " detect pedestrian junction collisions stage=" << stage << " lane=" << getID() << " foeLane=" << foeLane->getID() << "\n";
1927 : }
1928 : #endif
1929 112324 : const std::vector<MSTransportable*>& persons = foeLane->getEdge().getSortedPersons(timestep);
1930 1267220 : for (std::vector<MSTransportable*>::const_iterator it_p = persons.begin(); it_p != persons.end(); ++it_p) {
1931 : #ifdef DEBUG_PEDESTRIAN_COLLISIONS
1932 : if (DEBUG_COND) {
1933 : std::cout << " collider=" << collider->getID()
1934 : << " ped=" << (*it_p)->getID()
1935 : << " jammed=" << (*it_p)->isJammed()
1936 : << " colliderBoundary=" << colliderBoundary
1937 : << " pedBoundary=" << (*it_p)->getBoundingBox()
1938 : << "\n";
1939 : }
1940 : #endif
1941 1154896 : if ((*it_p)->isJammed()) {
1942 876 : continue;
1943 : }
1944 2308040 : if (colliderBoundary.overlapsWith((*it_p)->getBoundingBox())
1945 1154020 : && collider->getBoundingPoly().overlapsWith((*it_p)->getBoundingBox())) {
1946 3870 : std::string collisionType = "junctionPedestrian";
1947 3870 : if (foeLane->isCrossing()) {
1948 : collisionType = "crossing";
1949 3654 : } else if (foeLane->isWalkingArea()) {
1950 : collisionType = "walkingarea";
1951 : }
1952 3870 : handleIntermodalCollisionBetween(timestep, stage, collider, *it_p, 0, collisionType, toRemove, toTeleport);
1953 : }
1954 : }
1955 112324 : }
1956 32416465 : }
1957 :
1958 :
1959 : bool
1960 2374828419 : MSLane::detectCollisionBetween(SUMOTime timestep, const std::string& stage, MSVehicle* collider, MSVehicle* victim,
1961 : std::set<const MSVehicle*, ComparatorNumericalIdLess>& toRemove,
1962 : std::set<const MSVehicle*, ComparatorNumericalIdLess>& toTeleport) const {
1963 4725120851 : if (myCollisionAction == COLLISION_ACTION_TELEPORT && ((victim->hasInfluencer() && victim->getInfluencer().isRemoteAffected(timestep)) ||
1964 2350553979 : (collider->hasInfluencer() && collider->getInfluencer().isRemoteAffected(timestep)))) {
1965 17 : return false;
1966 : }
1967 :
1968 : // No self-collisions! (This is assumed to be ensured at caller side)
1969 2374828402 : if (collider == victim) {
1970 : return false;
1971 : }
1972 :
1973 2374828339 : const bool colliderOpposite = collider->getLaneChangeModel().isOpposite() || collider->isBidiOn(this);
1974 2374828339 : const bool victimOpposite = victim->getLaneChangeModel().isOpposite() || victim->isBidiOn(this);
1975 2374828339 : const bool bothOpposite = victimOpposite && colliderOpposite;
1976 2374828339 : if (bothOpposite) {
1977 : std::swap(victim, collider);
1978 : }
1979 2374828339 : const double colliderPos = colliderOpposite && !bothOpposite ? collider->getBackPositionOnLane(this) : collider->getPositionOnLane(this);
1980 2374828339 : const double minGapFactor = myCollisionMinGapFactor >= 0 ? myCollisionMinGapFactor : collider->getCarFollowModel().getCollisionMinGapFactor();
1981 2374828339 : double victimBack = victimOpposite && !bothOpposite ? victim->getPositionOnLane(this) : victim->getBackPositionOnLane(this);
1982 2374828339 : if (victim->getLateralOverlap() > 0 || collider->getLateralOverlap() > 0) {
1983 144640822 : if (&collider->getLane()->getEdge() == myEdge && collider->getLane()->getLength() > getLength()) {
1984 : // interpret victim position on the longer lane
1985 836 : victimBack *= collider->getLane()->getLength() / getLength();
1986 : }
1987 : }
1988 2374828339 : double gap = victimBack - colliderPos - minGapFactor * collider->getVehicleType().getMinGap();
1989 2374828339 : if (bothOpposite) {
1990 1295057 : gap = colliderPos - victimBack - minGapFactor * collider->getVehicleType().getMinGap();
1991 2373533282 : } else if (colliderOpposite) {
1992 : // vehicles are back to back so (frontal) minGap doesn't apply
1993 3610140 : gap += minGapFactor * collider->getVehicleType().getMinGap();
1994 : }
1995 : #ifdef DEBUG_COLLISIONS
1996 : if (DEBUG_COND && (DEBUG_COND2(collider) || DEBUG_COND2(victim))) {
1997 : std::cout << SIMTIME
1998 : << " thisLane=" << getID()
1999 : << " collider=" << collider->getID()
2000 : << " victim=" << victim->getID()
2001 : << " colOpposite=" << colliderOpposite
2002 : << " vicOpposite=" << victimOpposite
2003 : << " colLane=" << collider->getLane()->getID()
2004 : << " vicLane=" << victim->getLane()->getID()
2005 : << " colPos=" << colliderPos
2006 : << " vicBack=" << victimBack
2007 : << " colLat=" << collider->getCenterOnEdge(this)
2008 : << " vicLat=" << victim->getCenterOnEdge(this)
2009 : << " minGap=" << collider->getVehicleType().getMinGap()
2010 : << " minGapFactor=" << minGapFactor
2011 : << " gap=" << gap
2012 : << "\n";
2013 : }
2014 : #endif
2015 2374828339 : if (victimOpposite && gap < -(collider->getLength() + victim->getLength())) {
2016 : // already past each other
2017 : return false;
2018 : }
2019 2374811885 : if (gap < -NUMERICAL_EPS) {
2020 : double latGap = 0;
2021 12090228 : if (MSGlobals::gSublane) {
2022 12083843 : latGap = (fabs(victim->getCenterOnEdge(this) - collider->getCenterOnEdge(this))
2023 12083843 : - 0.5 * fabs(victim->getVehicleType().getWidth() + collider->getVehicleType().getWidth()));
2024 12083843 : if (latGap + NUMERICAL_EPS > 0) {
2025 : return false;
2026 : }
2027 : // account for ambiguous gap computation related to partial
2028 : // occupation of lanes with different lengths
2029 98223 : if (isInternal() && getEdge().getNumLanes() > 1 && victim->getLane() != collider->getLane()) {
2030 : double gapDelta = 0;
2031 1739 : const MSVehicle* otherLaneVeh = collider->getLane() == this ? victim : collider;
2032 1739 : if (otherLaneVeh->getLaneChangeModel().getShadowLane() == this) {
2033 653 : gapDelta = getLength() - otherLaneVeh->getLane()->getLength();
2034 : } else {
2035 1086 : for (const MSLane* cand : otherLaneVeh->getFurtherLanes()) {
2036 1086 : if (&cand->getEdge() == &getEdge()) {
2037 1086 : gapDelta = getLength() - cand->getLength();
2038 1086 : break;
2039 : }
2040 : }
2041 : }
2042 1739 : if (gap + gapDelta >= 0) {
2043 : return false;
2044 : }
2045 : }
2046 : }
2047 104608 : if (MSGlobals::gLaneChangeDuration > DELTA_T
2048 37 : && collider->getLaneChangeModel().isChangingLanes()
2049 31 : && victim->getLaneChangeModel().isChangingLanes()
2050 104608 : && victim->getLane() != this) {
2051 : // synchroneous lane change maneuver
2052 : return false;
2053 : }
2054 : #ifdef DEBUG_COLLISIONS
2055 : if (DEBUG_COND && (DEBUG_COND2(collider) || DEBUG_COND2(victim))) {
2056 : std::cout << SIMTIME << " detectedCollision gap=" << gap << " latGap=" << latGap << "\n";
2057 : }
2058 : #endif
2059 104608 : handleCollisionBetween(timestep, stage, collider, victim, gap, latGap, toRemove, toTeleport);
2060 104608 : return true;
2061 : }
2062 : return false;
2063 : }
2064 :
2065 :
2066 : void
2067 119107 : MSLane::handleCollisionBetween(SUMOTime timestep, const std::string& stage, const MSVehicle* collider, const MSVehicle* victim,
2068 : double gap, double latGap, std::set<const MSVehicle*, ComparatorNumericalIdLess>& toRemove,
2069 : std::set<const MSVehicle*, ComparatorNumericalIdLess>& toTeleport) const {
2070 119107 : if (collider->ignoreCollision() || victim->ignoreCollision()) {
2071 108905 : return;
2072 : }
2073 : std::string collisionType;
2074 : std::string collisionText;
2075 116189 : if (isFrontalCollision(collider, victim)) {
2076 : collisionType = "frontal";
2077 59 : collisionText = TL("frontal collision");
2078 116130 : } else if (stage == MSNet::STAGE_LANECHANGE) {
2079 : collisionType = "side";
2080 13177 : collisionText = TL("side collision");
2081 102953 : } else if (isInternal()) {
2082 : collisionType = "junction";
2083 14168 : collisionText = TL("junction collision");
2084 : } else {
2085 : collisionType = "collision";
2086 88785 : collisionText = TL("collision");
2087 : }
2088 :
2089 : // in frontal collisions the opposite vehicle is the collider
2090 116189 : if (victim->getLaneChangeModel().isOpposite() && !collider->getLaneChangeModel().isOpposite()) {
2091 : std::swap(collider, victim);
2092 : }
2093 464756 : std::string prefix = TLF("Vehicle '%'; % with vehicle '%", collider->getID(), collisionText, victim->getID());
2094 116189 : if (myCollisionStopTime > 0) {
2095 108099 : if (collider->collisionStopTime() >= 0 && victim->collisionStopTime() >= 0) {
2096 105987 : return;
2097 : }
2098 : std::string dummyError;
2099 2112 : SUMOVehicleParameter::Stop stop;
2100 2112 : stop.duration = myCollisionStopTime;
2101 2112 : stop.parametersSet |= STOP_DURATION_SET;
2102 2112 : const double collisionAngle = RAD2DEG(fabs(GeomHelper::angleDiff(victim->getAngle(), collider->getAngle())));
2103 : // determine new speeds from collision angle (@todo account for vehicle mass)
2104 2112 : double victimSpeed = victim->getSpeed();
2105 2112 : double colliderSpeed = collider->getSpeed();
2106 : // double victimOrigSpeed = victim->getSpeed();
2107 : // double colliderOrigSpeed = collider->getSpeed();
2108 2112 : if (collisionAngle < 45) {
2109 : // rear-end collisions
2110 : colliderSpeed = MIN2(colliderSpeed, victimSpeed);
2111 337 : } else if (collisionAngle < 135) {
2112 : // side collision
2113 326 : colliderSpeed /= 2;
2114 326 : victimSpeed /= 2;
2115 : } else {
2116 : // frontal collision
2117 : colliderSpeed = 0;
2118 : victimSpeed = 0;
2119 : }
2120 2112 : const double victimStopPos = MIN2(victim->getLane()->getLength(),
2121 2112 : victim->getPositionOnLane() + victim->getCarFollowModel().brakeGap(victimSpeed, victim->getCarFollowModel().getEmergencyDecel(), 0));
2122 2112 : if (victim->collisionStopTime() < 0) {
2123 1428 : stop.collision = true;
2124 1428 : stop.lane = victim->getLane()->getID();
2125 : // @todo: push victim forward?
2126 1428 : stop.startPos = victimStopPos;
2127 1428 : stop.endPos = stop.startPos;
2128 1428 : stop.parametersSet |= STOP_START_SET | STOP_END_SET;
2129 1428 : ((MSBaseVehicle*)victim)->addStop(stop, dummyError, 0);
2130 : }
2131 2112 : if (collider->collisionStopTime() < 0) {
2132 1654 : stop.collision = true;
2133 1654 : stop.lane = collider->getLane()->getID();
2134 1654 : stop.startPos = MIN2(collider->getPositionOnLane() + collider->getCarFollowModel().brakeGap(colliderSpeed, collider->getCarFollowModel().getEmergencyDecel(), 0),
2135 1654 : MAX3(0.0, victimStopPos - 0.75 * victim->getVehicleType().getLength(),
2136 1654 : collider->getPositionOnLane() - SPEED2DIST(collider->getSpeed())));
2137 1654 : stop.endPos = stop.startPos;
2138 1654 : stop.parametersSet |= STOP_START_SET | STOP_END_SET;
2139 1654 : ((MSBaseVehicle*)collider)->addStop(stop, dummyError, 0);
2140 : }
2141 : //std::cout << " collisionAngle=" << collisionAngle
2142 : // << "\n vPos=" << victim->getPositionOnLane() << " vStop=" << victimStopPos << " vSpeed=" << victimOrigSpeed << " vSpeed2=" << victimSpeed << " vSpeed3=" << victim->getSpeed()
2143 : // << "\n cPos=" << collider->getPositionOnLane() << " cStop=" << stop.startPos << " cSpeed=" << colliderOrigSpeed << " cSpeed2=" << colliderSpeed << " cSpeed3=" << collider->getSpeed()
2144 : // << "\n";
2145 2112 : } else {
2146 8090 : switch (myCollisionAction) {
2147 : case COLLISION_ACTION_WARN:
2148 : break;
2149 3579 : case COLLISION_ACTION_TELEPORT:
2150 7158 : prefix = TLF("Teleporting vehicle '%'; % with vehicle '%", collider->getID(), collisionText, victim->getID());
2151 : toRemove.insert(collider);
2152 : toTeleport.insert(collider);
2153 : break;
2154 210 : case COLLISION_ACTION_REMOVE: {
2155 420 : prefix = TLF("Removing % participants: vehicle '%', vehicle '%", collisionText, collider->getID(), victim->getID());
2156 : bool removeCollider = true;
2157 : bool removeVictim = true;
2158 210 : removeVictim = !(victim->hasInfluencer() && victim->getInfluencer()->isRemoteAffected(timestep));
2159 210 : removeCollider = !(collider->hasInfluencer() && collider->getInfluencer()->isRemoteAffected(timestep));
2160 210 : if (removeVictim) {
2161 : toRemove.insert(victim);
2162 : }
2163 210 : if (removeCollider) {
2164 : toRemove.insert(collider);
2165 : }
2166 210 : if (!removeVictim) {
2167 0 : if (!removeCollider) {
2168 0 : prefix = TLF("Keeping remote-controlled % participants: vehicle '%', vehicle '%", collisionText, collider->getID(), victim->getID());
2169 : } else {
2170 0 : prefix = TLF("Removing % participant: vehicle '%', keeping remote-controlled vehicle '%", collisionText, collider->getID(), victim->getID());
2171 : }
2172 210 : } else if (!removeCollider) {
2173 0 : prefix = TLF("Keeping remote-controlled % participant: vehicle '%', removing vehicle '%", collisionText, collider->getID(), victim->getID());
2174 : }
2175 : break;
2176 : }
2177 : default:
2178 : break;
2179 : }
2180 : }
2181 10202 : const bool newCollision = MSNet::getInstance()->registerCollision(collider, victim, collisionType, this, collider->getPositionOnLane(this));
2182 10202 : if (newCollision) {
2183 30174 : WRITE_WARNINGF(prefix + "', lane='%', gap=%%, time=%, stage=%.",
2184 : getID(), toString(gap), (MSGlobals::gSublane ? TL(", latGap=") + toString(latGap) : ""),
2185 : time2string(timestep), stage);
2186 6113 : MSNet::getInstance()->informVehicleStateListener(victim, MSNet::VehicleState::COLLISION);
2187 6113 : MSNet::getInstance()->informVehicleStateListener(collider, MSNet::VehicleState::COLLISION);
2188 6113 : MSNet::getInstance()->getVehicleControl().countCollision(myCollisionAction == COLLISION_ACTION_TELEPORT);
2189 : }
2190 : #ifdef DEBUG_COLLISIONS
2191 : if (DEBUG_COND2(collider)) {
2192 : toRemove.erase(collider);
2193 : toTeleport.erase(collider);
2194 : }
2195 : if (DEBUG_COND2(victim)) {
2196 : toRemove.erase(victim);
2197 : toTeleport.erase(victim);
2198 : }
2199 : #endif
2200 : }
2201 :
2202 :
2203 : void
2204 3966 : MSLane::handleIntermodalCollisionBetween(SUMOTime timestep, const std::string& stage, const MSVehicle* collider, const MSTransportable* victim,
2205 : double gap, const std::string& collisionType,
2206 : std::set<const MSVehicle*, ComparatorNumericalIdLess>& toRemove,
2207 : std::set<const MSVehicle*, ComparatorNumericalIdLess>& toTeleport) const {
2208 3966 : if (collider->ignoreCollision()) {
2209 3304 : return;
2210 : }
2211 7932 : std::string prefix = TLF("Vehicle '%'", collider->getID());
2212 3966 : if (myIntermodalCollisionStopTime > 0) {
2213 3344 : if (collider->collisionStopTime() >= 0) {
2214 3304 : return;
2215 : }
2216 : std::string dummyError;
2217 40 : SUMOVehicleParameter::Stop stop;
2218 40 : stop.duration = myIntermodalCollisionStopTime;
2219 40 : stop.parametersSet |= STOP_DURATION_SET;
2220 : // determine new speeds from collision angle (@todo account for vehicle mass)
2221 40 : double colliderSpeed = collider->getSpeed();
2222 40 : const double victimStopPos = victim->getEdgePos();
2223 : // double victimOrigSpeed = victim->getSpeed();
2224 : // double colliderOrigSpeed = collider->getSpeed();
2225 40 : if (collider->collisionStopTime() < 0) {
2226 40 : stop.collision = true;
2227 40 : stop.lane = collider->getLane()->getID();
2228 40 : stop.startPos = MIN2(collider->getPositionOnLane() + collider->getCarFollowModel().brakeGap(colliderSpeed, collider->getCarFollowModel().getEmergencyDecel(), 0),
2229 40 : MAX3(0.0, victimStopPos - 0.75 * victim->getVehicleType().getLength(),
2230 40 : collider->getPositionOnLane() - SPEED2DIST(collider->getSpeed())));
2231 40 : stop.endPos = stop.startPos;
2232 40 : stop.parametersSet |= STOP_START_SET | STOP_END_SET;
2233 40 : ((MSBaseVehicle*)collider)->addStop(stop, dummyError, 0);
2234 : }
2235 40 : } else {
2236 622 : switch (myIntermodalCollisionAction) {
2237 : case COLLISION_ACTION_WARN:
2238 : break;
2239 15 : case COLLISION_ACTION_TELEPORT:
2240 30 : prefix = TLF("Teleporting vehicle '%' after", collider->getID());
2241 : toRemove.insert(collider);
2242 : toTeleport.insert(collider);
2243 : break;
2244 15 : case COLLISION_ACTION_REMOVE: {
2245 30 : prefix = TLF("Removing vehicle '%' after", collider->getID());
2246 : bool removeCollider = true;
2247 15 : removeCollider = !(collider->hasInfluencer() && collider->getInfluencer()->isRemoteAffected(timestep));
2248 : if (!removeCollider) {
2249 0 : prefix = TLF("Keeping remote-controlled vehicle '%' after", collider->getID());
2250 : } else {
2251 : toRemove.insert(collider);
2252 : }
2253 : break;
2254 : }
2255 : default:
2256 : break;
2257 : }
2258 : }
2259 662 : const bool newCollision = MSNet::getInstance()->registerCollision(collider, victim, collisionType, this, victim->getEdgePos());
2260 662 : if (newCollision) {
2261 273 : if (gap != 0) {
2262 460 : WRITE_WARNING(prefix + TLF(" collision with person '%', lane='%', gap=%, time=%, stage=%.",
2263 : victim->getID(), getID(), gap, time2string(timestep), stage));
2264 : } else {
2265 905 : WRITE_WARNING(prefix + TLF(" collision with person '%', lane='%', time=%, stage=%.",
2266 : victim->getID(), getID(), time2string(timestep), stage));
2267 : }
2268 273 : MSNet::getInstance()->informVehicleStateListener(collider, MSNet::VehicleState::COLLISION);
2269 273 : MSNet::getInstance()->getVehicleControl().countCollision(myIntermodalCollisionAction == COLLISION_ACTION_TELEPORT);
2270 : }
2271 : #ifdef DEBUG_COLLISIONS
2272 : if (DEBUG_COND2(collider)) {
2273 : toRemove.erase(collider);
2274 : toTeleport.erase(collider);
2275 : }
2276 : #endif
2277 : }
2278 :
2279 :
2280 : bool
2281 116189 : MSLane::isFrontalCollision(const MSVehicle* collider, const MSVehicle* victim) {
2282 116189 : if (collider->getLaneChangeModel().isOpposite() != victim->getLaneChangeModel().isOpposite()) {
2283 : return true;
2284 : } else {
2285 116171 : const MSEdge* victimBidi = victim->getLane()->getEdge().getBidiEdge();
2286 116171 : if (&collider->getLane()->getEdge() == victimBidi) {
2287 : return true;
2288 : } else {
2289 154038 : for (MSLane* further : collider->getFurtherLanes()) {
2290 37908 : if (&further->getEdge() == victimBidi) {
2291 : return true;
2292 : }
2293 : }
2294 : }
2295 : }
2296 : return false;
2297 : }
2298 :
2299 : void
2300 94171225 : MSLane::executeMovements(const SUMOTime t) {
2301 : // multithreading: there are concurrent writes to myNeedsCollisionCheck but all of them should set it to true
2302 94171225 : myNeedsCollisionCheck = true;
2303 94171225 : MSLane* bidi = getBidiLane();
2304 94171225 : if (bidi != nullptr && bidi->getVehicleNumber() == 0) {
2305 514875 : MSNet::getInstance()->getEdgeControl().checkCollisionForInactive(bidi);
2306 : }
2307 94171225 : MSVehicle* firstNotStopped = nullptr;
2308 : // iterate over vehicles in reverse so that move reminders will be called in the correct order
2309 793164755 : for (VehCont::reverse_iterator i = myVehicles.rbegin(); i != myVehicles.rend();) {
2310 701134693 : MSVehicle* veh = *i;
2311 : // length is needed later when the vehicle may not exist anymore
2312 701134693 : const double length = veh->getVehicleType().getLengthWithGap();
2313 701134693 : const double nettoLength = veh->getVehicleType().getLength();
2314 701134693 : const bool moved = veh->executeMove();
2315 : MSLane* const target = veh->getMutableLane();
2316 698993530 : if (veh->hasArrived()) {
2317 : // vehicle has reached its arrival position
2318 : #ifdef DEBUG_EXEC_MOVE
2319 : if DEBUG_COND2(veh) {
2320 : std::cout << SIMTIME << " veh " << veh->getID() << " has arrived." << std::endl;
2321 : }
2322 : #endif
2323 3297180 : veh->onRemovalFromNet(MSMoveReminder::NOTIFICATION_ARRIVED);
2324 3297180 : MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(veh);
2325 695696350 : } else if (target != nullptr && moved) {
2326 16714367 : if (target->getEdge().isVaporizing()) {
2327 : // vehicle has reached a vaporizing edge
2328 756 : veh->onRemovalFromNet(MSMoveReminder::NOTIFICATION_VAPORIZED_VAPORIZER);
2329 756 : MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(veh);
2330 : } else {
2331 : // vehicle has entered a new lane (leaveLane and workOnMoveReminders were already called in MSVehicle::executeMove)
2332 16713611 : target->myVehBuffer.push_back(veh);
2333 16713611 : MSNet::getInstance()->getEdgeControl().needsVehicleIntegration(target);
2334 16713611 : if (MSGlobals::gSublane && veh->getLaneChangeModel().getShadowLane() != nullptr) {
2335 : // trigger sorting of partial vehicles as their order may have changed (lane might not be active and only contain partial vehicles)
2336 56798 : MSNet::getInstance()->getEdgeControl().needsVehicleIntegration(veh->getLaneChangeModel().getShadowLane());
2337 : }
2338 : }
2339 678981983 : } else if (veh->isParking()) {
2340 : // vehicle started to park
2341 16153 : MSVehicleTransfer::getInstance()->add(t, veh);
2342 16153 : myParkingVehicles.insert(veh);
2343 678965830 : } else if (veh->brokeDown()) {
2344 12 : veh->resumeFromStopping();
2345 36 : WRITE_WARNINGF(TL("Removing vehicle '%' after breaking down, lane='%', time=%."),
2346 : veh->getID(), veh->getLane()->getID(), time2string(t));
2347 12 : veh->onRemovalFromNet(MSMoveReminder::NOTIFICATION_VAPORIZED_BREAKDOWN);
2348 12 : MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(veh);
2349 678965818 : } else if (veh->isJumping()) {
2350 : // vehicle jumps to next route edge
2351 953 : MSVehicleTransfer::getInstance()->add(t, veh);
2352 678964865 : } else if (veh->getPositionOnLane() > getLength()) {
2353 : // for any reasons the vehicle is beyond its lane...
2354 : // this should never happen because it is handled in MSVehicle::executeMove
2355 : assert(false);
2356 0 : WRITE_WARNINGF(TL("Teleporting vehicle '%'; beyond end of lane, target lane='%', time=%."),
2357 : veh->getID(), getID(), time2string(t));
2358 0 : MSNet::getInstance()->getVehicleControl().countCollision(true);
2359 0 : MSVehicleTransfer::getInstance()->add(t, veh);
2360 :
2361 678964865 : } else if (veh->collisionStopTime() == 0) {
2362 3303 : veh->resumeFromStopping();
2363 3303 : if (getCollisionAction() == COLLISION_ACTION_REMOVE) {
2364 531 : WRITE_WARNINGF(TL("Removing vehicle '%' after earlier collision, lane='%', time=%."),
2365 : veh->getID(), veh->getLane()->getID(), time2string(t));
2366 177 : veh->onRemovalFromNet(MSMoveReminder::NOTIFICATION_VAPORIZED_COLLISION);
2367 177 : MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(veh);
2368 3126 : } else if (getCollisionAction() == COLLISION_ACTION_TELEPORT) {
2369 6879 : WRITE_WARNINGF(TL("Teleporting vehicle '%' after earlier collision, lane='%', time=%."),
2370 : veh->getID(), veh->getLane()->getID(), time2string(t));
2371 2293 : MSVehicleTransfer::getInstance()->add(t, veh);
2372 : } else {
2373 833 : if (firstNotStopped == nullptr && !(*i)->isStopped() && (*i)->getLane() == this) {
2374 557 : firstNotStopped = *i;
2375 : }
2376 : ++i;
2377 833 : continue;
2378 : }
2379 : } else {
2380 678961562 : if (firstNotStopped == nullptr && !(*i)->isStopped() && (*i)->getLane() == this) {
2381 80352707 : firstNotStopped = *i;
2382 : }
2383 : ++i;
2384 678961562 : continue;
2385 : }
2386 20031135 : myBruttoVehicleLengthSumToRemove += length;
2387 20031135 : myNettoVehicleLengthSumToRemove += nettoLength;
2388 : ++i;
2389 20031135 : i = VehCont::reverse_iterator(myVehicles.erase(i.base()));
2390 : }
2391 92030062 : if (firstNotStopped != nullptr) {
2392 80353264 : const SUMOTime ttt = firstNotStopped->getVehicleType().getParameter().getTimeToTeleport(MSGlobals::gTimeToGridlock);
2393 80353264 : const SUMOTime tttb = firstNotStopped->getVehicleType().getParameter().getTimeToTeleportBidi(MSGlobals::gTimeToTeleportBidi);
2394 80353264 : if (ttt > 0 || MSGlobals::gTimeToGridlockHighways > 0 || MSGlobals::gTimeToTeleportDisconnected >= 0 || tttb > 0 || MSGlobals::gTimeToTeleportRSDeadlock > 0) {
2395 77315136 : const bool wrongLane = !appropriate(firstNotStopped);
2396 77315136 : const bool disconnected = (MSGlobals::gTimeToTeleportDisconnected >= 0
2397 40802 : && firstNotStopped->succEdge(1) != nullptr
2398 77352098 : && firstNotStopped->getEdge()->allowedLanes(*firstNotStopped->succEdge(1), firstNotStopped->getVClass()) == nullptr);
2399 :
2400 77306046 : const bool r1 = ttt > 0 && firstNotStopped->getWaitingTime() > ttt && !disconnected
2401 : // never teleport a taxi on the last edge of it's route (where it would exit the simulation)
2402 77323033 : && (firstNotStopped->getDevice(typeid(MSDevice_Taxi)) == nullptr || firstNotStopped->getRoutePosition() < (firstNotStopped->getRoute().size() - 1));
2403 77307623 : const bool r2 = !r1 && MSGlobals::gTimeToGridlockHighways > 0
2404 1025 : && firstNotStopped->getWaitingTime() > MSGlobals::gTimeToGridlockHighways
2405 602 : && getSpeedLimit() > MSGlobals::gGridlockHighwaysSpeed && wrongLane
2406 602 : && !disconnected;
2407 77315124 : const bool r3 = disconnected && firstNotStopped->getWaitingTime() > MSGlobals::gTimeToTeleportDisconnected;
2408 77307611 : const bool r4 = !r1 && !r2 && !r3 && tttb > 0
2409 77315580 : && firstNotStopped->getWaitingTime() > tttb && getBidiLane() && !disconnected;
2410 505959 : const bool r5 = MSGlobals::gTimeToTeleportRSDeadlock > 0 && MSRailSignalControl::hasInstance() && !r1 && !r2 && !r3 && !r4
2411 77773613 : && firstNotStopped->getWaitingTime() > MSGlobals::gTimeToTeleportRSDeadlock && MSRailSignalControl::getInstance().haveDeadlock(firstNotStopped);
2412 77315136 : if (r1 || r2 || r3 || r4 || r5) {
2413 7633 : const std::vector<MSLink*>::const_iterator link = succLinkSec(*firstNotStopped, 1, *this, firstNotStopped->getBestLanesContinuation());
2414 7633 : const bool minorLink = !wrongLane && (link != myLinks.end()) && !((*link)->havePriority());
2415 9495 : std::string reason = (wrongLane ? " (wrong lane" : (minorLink ? " (yield" : " (jam"));
2416 7633 : myBruttoVehicleLengthSumToRemove += firstNotStopped->getVehicleType().getLengthWithGap();
2417 7633 : myNettoVehicleLengthSumToRemove += firstNotStopped->getVehicleType().getLength();
2418 7633 : if (firstNotStopped == myVehicles.back()) {
2419 : myVehicles.pop_back();
2420 : } else {
2421 307 : myVehicles.erase(std::find(myVehicles.begin(), myVehicles.end(), firstNotStopped));
2422 : reason = " (blocked";
2423 : }
2424 68577 : WRITE_WARNINGF("Teleporting vehicle '%'; waited too long" + reason
2425 : + (r2 ? ", highway" : "")
2426 : + (r3 ? ", disconnected" : "")
2427 : + (r4 ? ", bidi" : "")
2428 : + (r5 ? ", railSignal" : "")
2429 : + "), lane='%', time=%.", firstNotStopped->getID(), getID(), time2string(t));
2430 7633 : if (wrongLane) {
2431 998 : MSNet::getInstance()->getVehicleControl().registerTeleportWrongLane();
2432 6635 : } else if (minorLink) {
2433 4773 : MSNet::getInstance()->getVehicleControl().registerTeleportYield();
2434 : } else {
2435 1862 : MSNet::getInstance()->getVehicleControl().registerTeleportJam();
2436 : }
2437 7633 : if (MSGlobals::gRemoveGridlocked) {
2438 14 : firstNotStopped->onRemovalFromNet(MSMoveReminder::NOTIFICATION_TELEPORT_ARRIVED);
2439 14 : MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(firstNotStopped);
2440 : } else {
2441 7619 : MSVehicleTransfer::getInstance()->add(t, firstNotStopped);
2442 : }
2443 : }
2444 : }
2445 : }
2446 92030062 : if (MSGlobals::gSublane) {
2447 : // trigger sorting of vehicles as their order may have changed
2448 16696968 : MSNet::getInstance()->getEdgeControl().needsVehicleIntegration(this);
2449 : }
2450 92030062 : }
2451 :
2452 :
2453 : void
2454 236 : MSLane::markRecalculateBruttoSum() {
2455 236 : myRecalculateBruttoSum = true;
2456 236 : }
2457 :
2458 :
2459 : void
2460 94171222 : MSLane::updateLengthSum() {
2461 94171222 : myBruttoVehicleLengthSum -= myBruttoVehicleLengthSumToRemove;
2462 94171222 : myNettoVehicleLengthSum -= myNettoVehicleLengthSumToRemove;
2463 94171222 : myBruttoVehicleLengthSumToRemove = 0;
2464 94171222 : myNettoVehicleLengthSumToRemove = 0;
2465 94171222 : if (myVehicles.empty()) {
2466 : // avoid numerical instability
2467 7740479 : myBruttoVehicleLengthSum = 0;
2468 7740479 : myNettoVehicleLengthSum = 0;
2469 86430743 : } else if (myRecalculateBruttoSum) {
2470 175 : myBruttoVehicleLengthSum = 0;
2471 646 : for (VehCont::const_iterator i = myVehicles.begin(); i != myVehicles.end(); ++i) {
2472 471 : myBruttoVehicleLengthSum += (*i)->getVehicleType().getLengthWithGap();
2473 : }
2474 175 : myRecalculateBruttoSum = false;
2475 : }
2476 94171222 : }
2477 :
2478 :
2479 : void
2480 0 : MSLane::changeLanes(const SUMOTime t) {
2481 0 : myEdge->changeLanes(t);
2482 0 : }
2483 :
2484 :
2485 : const MSEdge*
2486 5342895 : MSLane::getNextNormal() const {
2487 5342895 : return myEdge->getNormalSuccessor();
2488 : }
2489 :
2490 :
2491 : const MSLane*
2492 127300 : MSLane::getFirstInternalInConnection(double& offset) const {
2493 127300 : if (!this->isInternal()) {
2494 : return nullptr;
2495 : }
2496 127300 : offset = 0.;
2497 : const MSLane* firstInternal = this;
2498 127300 : MSLane* pred = getCanonicalPredecessorLane();
2499 127348 : while (pred != nullptr && pred->isInternal()) {
2500 : firstInternal = pred;
2501 48 : offset += pred->getLength();
2502 48 : pred = firstInternal->getCanonicalPredecessorLane();
2503 : }
2504 : return firstInternal;
2505 : }
2506 :
2507 :
2508 : // ------ Static (sic!) container methods ------
2509 : bool
2510 2103833 : MSLane::dictionary(const std::string& id, MSLane* ptr) {
2511 : const DictType::iterator it = myDict.lower_bound(id);
2512 2103833 : if (it == myDict.end() || it->first != id) {
2513 : // id not in myDict
2514 2103825 : myDict.emplace_hint(it, id, ptr);
2515 2103825 : return true;
2516 : }
2517 : return false;
2518 : }
2519 :
2520 :
2521 : MSLane*
2522 9241596 : MSLane::dictionary(const std::string& id) {
2523 : const DictType::iterator it = myDict.find(id);
2524 9241596 : if (it == myDict.end()) {
2525 : // id not in myDict
2526 : return nullptr;
2527 : }
2528 9241150 : return it->second;
2529 : }
2530 :
2531 :
2532 : void
2533 41625 : MSLane::clear() {
2534 2127023 : for (DictType::iterator i = myDict.begin(); i != myDict.end(); ++i) {
2535 2085398 : delete (*i).second;
2536 : }
2537 : myDict.clear();
2538 41625 : }
2539 :
2540 :
2541 : void
2542 242 : MSLane::insertIDs(std::vector<std::string>& into) {
2543 8196 : for (DictType::iterator i = myDict.begin(); i != myDict.end(); ++i) {
2544 7954 : into.push_back((*i).first);
2545 : }
2546 242 : }
2547 :
2548 :
2549 : template<class RTREE> void
2550 615 : MSLane::fill(RTREE& into) {
2551 26128 : for (DictType::iterator i = myDict.begin(); i != myDict.end(); ++i) {
2552 25513 : MSLane* l = (*i).second;
2553 25513 : Boundary b = l->getShape().getBoxBoundary();
2554 25513 : b.grow(3.);
2555 25513 : const float cmin[2] = {(float) b.xmin(), (float) b.ymin()};
2556 25513 : const float cmax[2] = {(float) b.xmax(), (float) b.ymax()};
2557 25513 : into.Insert(cmin, cmax, l);
2558 : }
2559 615 : }
2560 :
2561 : template void MSLane::fill<NamedRTree>(NamedRTree& into);
2562 : template void MSLane::fill<LANE_RTREE_QUAL>(LANE_RTREE_QUAL& into);
2563 :
2564 : // ------ ------
2565 : bool
2566 77315136 : MSLane::appropriate(const MSVehicle* veh) const {
2567 77315136 : if (veh->getLaneChangeModel().isOpposite()) {
2568 : return false;
2569 : }
2570 77205239 : if (myEdge->isInternal()) {
2571 : return true;
2572 : }
2573 72032146 : if (veh->succEdge(1) == nullptr) {
2574 : assert((int)veh->getBestLanes().size() > veh->getLaneIndex());
2575 19512908 : if (veh->getBestLanes()[veh->getLaneIndex()].bestLaneOffset == 0) {
2576 : return true;
2577 : } else {
2578 : return false;
2579 : }
2580 : }
2581 52519238 : std::vector<MSLink*>::const_iterator link = succLinkSec(*veh, 1, *this, veh->getBestLanesContinuation());
2582 : return (link != myLinks.end());
2583 : }
2584 :
2585 :
2586 : void
2587 33467377 : MSLane::integrateNewVehicles() {
2588 33467377 : myNeedsCollisionCheck = true;
2589 : std::vector<MSVehicle*>& buffered = myVehBuffer.getContainer();
2590 33467377 : sort(buffered.begin(), buffered.end(), vehicle_position_sorter(this));
2591 50180988 : for (MSVehicle* const veh : buffered) {
2592 : assert(veh->getLane() == this);
2593 16713611 : myVehicles.insert(myVehicles.begin(), veh);
2594 16713611 : myBruttoVehicleLengthSum += veh->getVehicleType().getLengthWithGap();
2595 16713611 : myNettoVehicleLengthSum += veh->getVehicleType().getLength();
2596 : //if (true) std::cout << SIMTIME << " integrateNewVehicle lane=" << getID() << " veh=" << veh->getID() << " (on lane " << veh->getLane()->getID() << ") into lane=" << getID() << " myBrutto=" << myBruttoVehicleLengthSum << "\n";
2597 16713611 : myEdge->markDelayed();
2598 : }
2599 : buffered.clear();
2600 : myVehBuffer.unlock();
2601 : //std::cout << SIMTIME << " integrateNewVehicle lane=" << getID() << " myVehicles1=" << toString(myVehicles);
2602 33467377 : if (MSGlobals::gLateralResolution > 0 || myOpposite != nullptr) {
2603 19301605 : sort(myVehicles.begin(), myVehicles.end(), vehicle_natural_position_sorter(this));
2604 : }
2605 33467377 : sortPartialVehicles();
2606 : #ifdef DEBUG_VEHICLE_CONTAINER
2607 : if (DEBUG_COND) std::cout << SIMTIME << " integrateNewVehicle lane=" << getID()
2608 : << " vehicles=" << toString(myVehicles) << " partials=" << toString(myPartialVehicles) << "\n";
2609 : #endif
2610 33467377 : }
2611 :
2612 :
2613 : void
2614 117762801 : MSLane::sortPartialVehicles() {
2615 117762801 : if (myPartialVehicles.size() > 1) {
2616 1567030 : sort(myPartialVehicles.begin(), myPartialVehicles.end(), vehicle_natural_position_sorter(this));
2617 : }
2618 117762801 : }
2619 :
2620 :
2621 : void
2622 19985800 : MSLane::sortManeuverReservations() {
2623 19985800 : if (myManeuverReservations.size() > 1) {
2624 : #ifdef DEBUG_CONTEXT
2625 : if (DEBUG_COND) {
2626 : std::cout << "sortManeuverReservations on lane " << getID()
2627 : << "\nBefore sort: " << toString(myManeuverReservations) << std::endl;
2628 : }
2629 : #endif
2630 23215 : sort(myManeuverReservations.begin(), myManeuverReservations.end(), vehicle_natural_position_sorter(this));
2631 : #ifdef DEBUG_CONTEXT
2632 : if (DEBUG_COND) {
2633 : std::cout << "After sort: " << toString(myManeuverReservations) << std::endl;
2634 : }
2635 : #endif
2636 : }
2637 19985800 : }
2638 :
2639 :
2640 : bool
2641 6935701847 : MSLane::isInternal() const {
2642 6935701847 : return myEdge->isInternal();
2643 : }
2644 :
2645 :
2646 : bool
2647 52238111 : MSLane::isNormal() const {
2648 52238111 : return myEdge->isNormal();
2649 : }
2650 :
2651 :
2652 : bool
2653 100765288 : MSLane::isCrossing() const {
2654 100765288 : return myEdge->isCrossing();
2655 : }
2656 :
2657 :
2658 : bool
2659 846147 : MSLane::isPriorityCrossing() const {
2660 846147 : return isCrossing() && getIncomingLanes()[0].viaLink->getOffState() == LINKSTATE_MAJOR;
2661 : }
2662 :
2663 :
2664 : bool
2665 383937084 : MSLane::isWalkingArea() const {
2666 383937084 : return myEdge->isWalkingArea();
2667 : }
2668 :
2669 :
2670 : MSVehicle*
2671 1087661632 : MSLane::getLastFullVehicle() const {
2672 1087661632 : if (myVehicles.size() == 0) {
2673 : return nullptr;
2674 : }
2675 1069768135 : return myVehicles.front();
2676 : }
2677 :
2678 :
2679 : MSVehicle*
2680 76249 : MSLane::getFirstFullVehicle() const {
2681 76249 : if (myVehicles.size() == 0) {
2682 : return nullptr;
2683 : }
2684 24164 : return myVehicles.back();
2685 : }
2686 :
2687 :
2688 : MSVehicle*
2689 407231538 : MSLane::getLastAnyVehicle() const {
2690 : // all vehicles in myVehicles should have positions smaller or equal to
2691 : // those in myPartialVehicles (unless we're on a bidi-lane)
2692 407231538 : if (myVehicles.size() > 0) {
2693 322984776 : if (myBidiLane != nullptr && myPartialVehicles.size() > 0) {
2694 24080 : if (myVehicles.front()->getPositionOnLane() > myPartialVehicles.front()->getPositionOnLane(this)) {
2695 7238 : return myPartialVehicles.front();
2696 : }
2697 : }
2698 322977538 : return myVehicles.front();
2699 : }
2700 84246762 : if (myPartialVehicles.size() > 0) {
2701 4852185 : return myPartialVehicles.front();
2702 : }
2703 : return nullptr;
2704 : }
2705 :
2706 :
2707 : MSVehicle*
2708 135677 : MSLane::getFirstAnyVehicle() const {
2709 : MSVehicle* result = nullptr;
2710 135677 : if (myVehicles.size() > 0) {
2711 135677 : result = myVehicles.back();
2712 : }
2713 : if (myPartialVehicles.size() > 0
2714 135677 : && (result == nullptr || result->getPositionOnLane(this) < myPartialVehicles.back()->getPositionOnLane(this))) {
2715 135 : result = myPartialVehicles.back();
2716 : }
2717 135677 : return result;
2718 : }
2719 :
2720 :
2721 : std::vector<MSLink*>::const_iterator
2722 1999291856 : MSLane::succLinkSec(const SUMOVehicle& veh, int nRouteSuccs,
2723 : const MSLane& succLinkSource, const std::vector<MSLane*>& conts) {
2724 1999291856 : const MSEdge* nRouteEdge = veh.succEdge(nRouteSuccs);
2725 : // check whether the vehicle tried to look beyond its route
2726 1999291856 : if (nRouteEdge == nullptr) {
2727 : // return end (no succeeding link) if so
2728 : return succLinkSource.myLinks.end();
2729 : }
2730 : // if we are on an internal lane there should only be one link and it must be allowed
2731 1400883884 : if (succLinkSource.isInternal()) {
2732 : assert(succLinkSource.myLinks.size() == 1);
2733 : // could have been disallowed dynamically with a rerouter or via TraCI
2734 : // assert(succLinkSource.myLinks[0]->getLane()->allowsVehicleClass(veh.getVehicleType().getVehicleClass()));
2735 : return succLinkSource.myLinks.begin();
2736 : }
2737 : // a link may be used if
2738 : // 1) there is a destination lane ((*link)->getLane()!=0)
2739 : // 2) the destination lane belongs to the next edge in route ((*link)->getLane()->myEdge == nRouteEdge)
2740 : // 3) the destination lane allows the vehicle's class ((*link)->getLane()->allowsVehicleClass(veh.getVehicleClass()))
2741 :
2742 : // there should be a link which leads to the next desired lane our route in "conts" (built in "getBestLanes")
2743 : // "conts" stores the best continuations of our current lane
2744 : // we should never return an arbitrary link since this may cause collisions
2745 :
2746 1102250943 : if (nRouteSuccs < (int)conts.size()) {
2747 : // we go through the links in our list and return the matching one
2748 1223852526 : for (std::vector<MSLink*>::const_iterator link = succLinkSource.myLinks.begin(); link != succLinkSource.myLinks.end(); ++link) {
2749 1223121023 : if ((*link)->getLane() != nullptr && (*link)->getLane()->myEdge == nRouteEdge
2750 1092454960 : && (*link)->getLane()->allowsVehicleClass(veh.getVClass())
2751 2315143383 : && ((*link)->getViaLane() == nullptr || (*link)->getViaLane()->allowsVehicleClass(veh.getVClass()))) {
2752 : // we should use the link if it connects us to the best lane
2753 1092018283 : if ((*link)->getLane() == conts[nRouteSuccs]) {
2754 1077626057 : return link;
2755 : }
2756 : }
2757 : }
2758 : } else {
2759 : // the source lane is a dead end (no continuations exist)
2760 : return succLinkSource.myLinks.end();
2761 : }
2762 : // the only case where this should happen is for a disconnected route (deliberately ignored)
2763 : #ifdef DEBUG_NO_CONNECTION
2764 : // the "'" around the ids are missing intentionally in the message below because it slows messaging down, resulting in test timeouts
2765 : WRITE_WARNING("Could not find connection between lane " + succLinkSource.getID() + " and lane " + conts[nRouteSuccs]->getID() +
2766 : " for vehicle " + veh.getID() + ", time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
2767 : #endif
2768 : return succLinkSource.myLinks.end();
2769 : }
2770 :
2771 :
2772 : const MSLink*
2773 616571117 : MSLane::getLinkTo(const MSLane* const target) const {
2774 616571117 : const bool internal = target->isInternal();
2775 822068921 : for (const MSLink* const l : myLinks) {
2776 789032287 : if ((internal && l->getViaLane() == target) || (!internal && l->getLane() == target)) {
2777 : return l;
2778 : }
2779 : }
2780 : return nullptr;
2781 : }
2782 :
2783 :
2784 : const MSLane*
2785 77229 : MSLane::getInternalFollowingLane(const MSLane* const target) const {
2786 126696 : for (const MSLink* const l : myLinks) {
2787 117713 : if (l->getLane() == target) {
2788 : return l->getViaLane();
2789 : }
2790 : }
2791 : return nullptr;
2792 : }
2793 :
2794 :
2795 : const MSLink*
2796 113248933 : MSLane::getEntryLink() const {
2797 113248933 : if (!isInternal()) {
2798 : return nullptr;
2799 : }
2800 : const MSLane* internal = this;
2801 112949707 : const MSLane* lane = this->getCanonicalPredecessorLane();
2802 : assert(lane != nullptr);
2803 114341920 : while (lane->isInternal()) {
2804 : internal = lane;
2805 1392213 : lane = lane->getCanonicalPredecessorLane();
2806 : assert(lane != nullptr);
2807 : }
2808 112949707 : return lane->getLinkTo(internal);
2809 : }
2810 :
2811 :
2812 : void
2813 1024 : MSLane::setMaxSpeed(const double val, const bool modified, const double jamThreshold) {
2814 1024 : myMaxSpeed = val;
2815 1024 : mySpeedModified = modified;
2816 1024 : myEdge->recalcCache();
2817 1024 : if (MSGlobals::gUseMesoSim) {
2818 222 : MESegment* first = MSGlobals::gMesoNet->getSegmentForEdge(*myEdge);
2819 1266 : while (first != nullptr) {
2820 1044 : first->setSpeed(val, SIMSTEP, jamThreshold, myIndex);
2821 : first = first->getNextSegment();
2822 : }
2823 : }
2824 1024 : }
2825 :
2826 :
2827 : void
2828 83 : MSLane::setFrictionCoefficient(double val) {
2829 83 : myFrictionCoefficient = val;
2830 83 : myEdge->recalcCache();
2831 83 : }
2832 :
2833 :
2834 : void
2835 15 : MSLane::setLength(double val) {
2836 15 : myLength = val;
2837 15 : myEdge->recalcCache();
2838 15 : }
2839 :
2840 :
2841 : void
2842 83529119 : MSLane::swapAfterLaneChange(SUMOTime) {
2843 : //if (getID() == "disabled_lane") std::cout << SIMTIME << " swapAfterLaneChange lane=" << getID() << " myVehicles=" << toString(myVehicles) << " myTmpVehicles=" << toString(myTmpVehicles) << "\n";
2844 83529119 : myVehicles = myTmpVehicles;
2845 : myTmpVehicles.clear();
2846 : // this needs to be done after finishing lane-changing for all lanes on the
2847 : // current edge (MSLaneChanger::updateLanes())
2848 83529119 : sortPartialVehicles();
2849 83529119 : if (MSGlobals::gSublane && getOpposite() != nullptr) {
2850 425328 : getOpposite()->sortPartialVehicles();
2851 : }
2852 83529119 : if (myBidiLane != nullptr) {
2853 340977 : myBidiLane->sortPartialVehicles();
2854 : }
2855 83529119 : }
2856 :
2857 :
2858 : MSVehicle*
2859 25756 : MSLane::removeVehicle(MSVehicle* remVehicle, MSMoveReminder::Notification notification, bool notify) {
2860 : assert(remVehicle->getLane() == this);
2861 54879 : for (MSLane::VehCont::iterator it = myVehicles.begin(); it < myVehicles.end(); it++) {
2862 54863 : if (remVehicle == *it) {
2863 25740 : if (notify) {
2864 17565 : remVehicle->leaveLane(notification);
2865 : }
2866 25740 : myVehicles.erase(it);
2867 25740 : myBruttoVehicleLengthSum -= remVehicle->getVehicleType().getLengthWithGap();
2868 25740 : myNettoVehicleLengthSum -= remVehicle->getVehicleType().getLength();
2869 25740 : break;
2870 : }
2871 : }
2872 25756 : return remVehicle;
2873 : }
2874 :
2875 :
2876 : MSLane*
2877 82659517 : MSLane::getParallelLane(int offset, bool includeOpposite) const {
2878 82659517 : return myEdge->parallelLane(this, offset, includeOpposite);
2879 : }
2880 :
2881 :
2882 : void
2883 2895686 : MSLane::addIncomingLane(MSLane* lane, MSLink* viaLink) {
2884 : IncomingLaneInfo ili;
2885 2895686 : ili.lane = lane;
2886 2895686 : ili.viaLink = viaLink;
2887 2895686 : ili.length = lane->getLength();
2888 2895686 : myIncomingLanes.push_back(ili);
2889 2895686 : }
2890 :
2891 :
2892 : void
2893 2895686 : MSLane::addApproachingLane(MSLane* lane, bool warnMultiCon) {
2894 2895686 : MSEdge* approachingEdge = &lane->getEdge();
2895 2895686 : if (myApproachingLanes.find(approachingEdge) == myApproachingLanes.end()) {
2896 2864071 : myApproachingLanes[approachingEdge] = std::vector<MSLane*>();
2897 31615 : } else if (!approachingEdge->isInternal() && warnMultiCon) {
2898 : // whenever a normal edge connects twice, there is a corresponding
2899 : // internal edge wich connects twice, one warning is sufficient
2900 18 : WRITE_WARNINGF(TL("Lane '%' is approached multiple times from edge '%'. This may cause collisions."),
2901 : getID(), approachingEdge->getID());
2902 : }
2903 2895686 : myApproachingLanes[approachingEdge].push_back(lane);
2904 2895686 : }
2905 :
2906 :
2907 : bool
2908 101302020 : MSLane::isApproachedFrom(MSEdge* const edge, MSLane* const lane) {
2909 : std::map<MSEdge*, std::vector<MSLane*> >::const_iterator i = myApproachingLanes.find(edge);
2910 101302020 : if (i == myApproachingLanes.end()) {
2911 : return false;
2912 : }
2913 : const std::vector<MSLane*>& lanes = (*i).second;
2914 99001182 : return std::find(lanes.begin(), lanes.end(), lane) != lanes.end();
2915 : }
2916 :
2917 :
2918 3268835 : double MSLane::getMissingRearGap(const MSVehicle* leader, double backOffset, double leaderSpeed) const {
2919 : // this follows the same logic as getFollowerOnConsecutive. we do a tree
2920 : // search and check for the vehicle with the largest missing rear gap within
2921 : // relevant range
2922 : double result = 0;
2923 : const double leaderDecel = leader->getCarFollowModel().getMaxDecel();
2924 3268835 : CLeaderDist followerInfo = getFollowersOnConsecutive(leader, backOffset, false)[0];
2925 3268835 : const MSVehicle* v = followerInfo.first;
2926 3268835 : if (v != nullptr) {
2927 5520 : result = v->getCarFollowModel().getSecureGap(v, leader, v->getSpeed(), leaderSpeed, leaderDecel) - followerInfo.second;
2928 : }
2929 3268835 : return result;
2930 : }
2931 :
2932 :
2933 : double
2934 367642721 : MSLane::getMaximumBrakeDist() const {
2935 367642721 : const MSVehicleControl& vc = MSNet::getInstance()->getVehicleControl();
2936 367642721 : const double maxSpeed = getSpeedLimit() * vc.getMaxSpeedFactor();
2937 : // NOTE: For the euler update this is an upper bound on the actual braking distance (see ticket #860)
2938 : // impose a hard bound due to visibility / common sense to avoid unnecessary computation if there are strange vehicles in the fleet
2939 367642721 : const double minDecel = isRailway(myPermissions) ? vc.getMinDecelerationRail() : vc.getMinDeceleration();
2940 367642721 : return MIN2(maxSpeed * maxSpeed * 0.5 / minDecel,
2941 367642721 : myPermissions == SVC_SHIP ? 10000.0 : 1000.0);
2942 : }
2943 :
2944 :
2945 : std::pair<MSVehicle* const, double>
2946 39822211 : MSLane::getLeader(const MSVehicle* veh, const double vehPos, const std::vector<MSLane*>& bestLaneConts, double dist, bool checkTmpVehicles) const {
2947 : // get the leading vehicle for (shadow) veh
2948 : // XXX this only works as long as all lanes of an edge have equal length
2949 : #ifdef DEBUG_CONTEXT
2950 : if (DEBUG_COND2(veh)) {
2951 : std::cout << " getLeader lane=" << getID() << " ego=" << veh->getID() << " vehs=" << toString(myVehicles) << " tmpVehs=" << toString(myTmpVehicles) << "\n";
2952 : }
2953 : #endif
2954 39822211 : if (checkTmpVehicles) {
2955 241770528 : for (VehCont::const_iterator last = myTmpVehicles.begin(); last != myTmpVehicles.end(); ++last) {
2956 : // XXX refactor leaderInfo to use a const vehicle all the way through the call hierarchy
2957 239405029 : MSVehicle* pred = (MSVehicle*)*last;
2958 239405029 : if (pred == veh) {
2959 : continue;
2960 : }
2961 : #ifdef DEBUG_CONTEXT
2962 : if (DEBUG_COND2(veh)) {
2963 : std::cout << std::setprecision(gPrecision) << " getLeader lane=" << getID() << " ego=" << veh->getID() << " egoPos=" << vehPos << " pred=" << pred->getID() << " predPos=" << pred->getPositionOnLane() << "\n";
2964 : }
2965 : #endif
2966 204498038 : if (pred->getPositionOnLane() >= vehPos) {
2967 32609148 : return std::pair<MSVehicle* const, double>(pred, pred->getBackPositionOnLane(this) - veh->getVehicleType().getMinGap() - vehPos);
2968 : }
2969 : }
2970 : } else {
2971 46885587 : for (AnyVehicleIterator last = anyVehiclesBegin(); last != anyVehiclesEnd(); ++last) {
2972 : // XXX refactor leaderInfo to use a const vehicle all the way through the call hierarchy
2973 50848265 : MSVehicle* pred = (MSVehicle*)*last;
2974 50848265 : if (pred == veh) {
2975 3827573 : continue;
2976 : }
2977 : #ifdef DEBUG_CONTEXT
2978 : if (DEBUG_COND2(veh)) {
2979 : std::cout << " getLeader lane=" << getID() << " ego=" << veh->getID() << " egoPos=" << vehPos
2980 : << " pred=" << pred->getID() << " predPos=" << pred->getPositionOnLane(this) << " predBack=" << pred->getBackPositionOnLane(this) << "\n";
2981 : }
2982 : #endif
2983 47020692 : if (pred->getPositionOnLane(this) >= vehPos) {
2984 4079474 : if (MSGlobals::gLaneChangeDuration > 0
2985 332837 : && pred->getLaneChangeModel().isOpposite()
2986 89310 : && !pred->getLaneChangeModel().isChangingLanes()
2987 4092291 : && pred->getLaneChangeModel().getShadowLane() == this) {
2988 : // skip non-overlapping shadow
2989 58398 : continue;
2990 : }
2991 3962678 : return std::pair<MSVehicle* const, double>(pred, pred->getBackPositionOnLane(this) - veh->getVehicleType().getMinGap() - vehPos);
2992 : }
2993 : }
2994 : }
2995 : // XXX from here on the code mirrors MSLaneChanger::getRealLeader
2996 3250385 : if (bestLaneConts.size() > 0) {
2997 2925430 : double seen = getLength() - vehPos;
2998 2925430 : double speed = veh->getSpeed();
2999 2925430 : if (dist < 0) {
3000 31700 : dist = veh->getCarFollowModel().brakeGap(speed) + veh->getVehicleType().getMinGap();
3001 : }
3002 : #ifdef DEBUG_CONTEXT
3003 : if (DEBUG_COND2(veh)) {
3004 : std::cout << " getLeader lane=" << getID() << " seen=" << seen << " dist=" << dist << "\n";
3005 : }
3006 : #endif
3007 2925430 : if (seen > dist) {
3008 1108772 : return std::pair<MSVehicle* const, double>(static_cast<MSVehicle*>(nullptr), -1);
3009 : }
3010 1816658 : return getLeaderOnConsecutive(dist, seen, speed, *veh, bestLaneConts);
3011 : } else {
3012 324955 : return std::make_pair(static_cast<MSVehicle*>(nullptr), -1);
3013 : }
3014 : }
3015 :
3016 :
3017 : std::pair<MSVehicle* const, double>
3018 39112414 : MSLane::getLeaderOnConsecutive(double dist, double seen, double speed, const MSVehicle& veh,
3019 : const std::vector<MSLane*>& bestLaneConts, bool considerCrossingFoes) const {
3020 : #ifdef DEBUG_CONTEXT
3021 : if (DEBUG_COND2(&veh)) {
3022 : std::cout << " getLeaderOnConsecutive lane=" << getID() << " ego=" << veh.getID() << " seen=" << seen << " dist=" << dist << " conts=" << toString(bestLaneConts) << "\n";
3023 : }
3024 : #endif
3025 39112414 : if (seen > dist && !isInternal()) {
3026 113658 : return std::make_pair(static_cast<MSVehicle*>(nullptr), -1);
3027 : }
3028 : int view = 1;
3029 : // loop over following lanes
3030 38998756 : if (myPartialVehicles.size() > 0) {
3031 : // XXX
3032 1130536 : MSVehicle* pred = myPartialVehicles.front();
3033 1130536 : const double gap = seen - (getLength() - pred->getBackPositionOnLane(this)) - veh.getVehicleType().getMinGap();
3034 : #ifdef DEBUG_CONTEXT
3035 : if (DEBUG_COND2(&veh)) {
3036 : std::cout << " predGap=" << gap << " partials=" << toString(myPartialVehicles) << "\n";
3037 : }
3038 : #endif
3039 : // make sure pred is really a leader and not doing continous lane-changing behind ego
3040 1130536 : if (gap > 0) {
3041 770004 : return std::pair<MSVehicle* const, double>(pred, gap);
3042 : }
3043 : }
3044 : #ifdef DEBUG_CONTEXT
3045 : if (DEBUG_COND2(&veh)) {
3046 : gDebugFlag1 = true;
3047 : }
3048 : #endif
3049 : const MSLane* nextLane = this;
3050 : do {
3051 54735695 : nextLane->getVehiclesSecure(); // lock against running sim when called from GUI for time gap coloring
3052 : // get the next link used
3053 54735695 : std::vector<MSLink*>::const_iterator link = succLinkSec(veh, view, *nextLane, bestLaneConts);
3054 54735695 : if (nextLane->isLinkEnd(link) && view < veh.getRoute().size() - veh.getRoutePosition()) {
3055 5161324 : const MSEdge* nextEdge = *(veh.getCurrentRouteEdge() + view);
3056 5161324 : if (nextEdge->getNumLanes() == 1) {
3057 : // lanes are unambiguous on the next route edge, continue beyond bestLaneConts
3058 5624611 : for (link = nextLane->getLinkCont().begin(); link < nextLane->getLinkCont().end(); link++) {
3059 3534084 : if ((*link)->getLane() == nextEdge->getLanes().front()) {
3060 : break;
3061 : }
3062 : }
3063 : }
3064 : }
3065 54735695 : if (nextLane->isLinkEnd(link)) {
3066 : #ifdef DEBUG_CONTEXT
3067 : if (DEBUG_COND2(&veh)) {
3068 : std::cout << " cannot continue after nextLane=" << nextLane->getID() << "\n";
3069 : }
3070 : #endif
3071 14665157 : nextLane->releaseVehicles();
3072 14665157 : break;
3073 : }
3074 : // check for link leaders
3075 40070538 : const bool laneChanging = veh.getLane() != this;
3076 40070538 : const MSLink::LinkLeaders linkLeaders = (*link)->getLeaderInfo(&veh, seen);
3077 40070524 : nextLane->releaseVehicles();
3078 40070524 : if (linkLeaders.size() > 0) {
3079 : std::pair<MSVehicle*, double> result;
3080 : double shortestGap = std::numeric_limits<double>::max();
3081 2280212 : for (auto ll : linkLeaders) {
3082 : double gap = ll.vehAndGap.second;
3083 : MSVehicle* lVeh = ll.vehAndGap.first;
3084 1261619 : if (lVeh != nullptr) {
3085 : // leader is a vehicle, not a pedestrian
3086 1165879 : gap += lVeh->getCarFollowModel().brakeGap(lVeh->getSpeed(), lVeh->getCarFollowModel().getMaxDecel(), 0);
3087 : }
3088 : #ifdef DEBUG_CONTEXT
3089 : if (DEBUG_COND2(&veh)) {
3090 : std::cout << " linkLeader candidate " << Named::getIDSecure(lVeh)
3091 : << " isLeader=" << veh.isLeader(*link, lVeh, ll.vehAndGap.second)
3092 : << " gap=" << ll.vehAndGap.second
3093 : << " gap+brakeing=" << gap
3094 : << "\n";
3095 : }
3096 : #endif
3097 : // skip vehicles which do not share the outgoing edge (to get only real leader vehicles in TraCI #13842)
3098 1261619 : if (!considerCrossingFoes && !ll.sameTarget()) {
3099 35 : continue;
3100 : }
3101 : // in the context of lane-changing, all candidates are leaders
3102 1261584 : if (lVeh != nullptr && !laneChanging && !veh.isLeader(*link, lVeh, ll.vehAndGap.second)) {
3103 9032 : continue;
3104 : }
3105 1252552 : if (gap < shortestGap) {
3106 : shortestGap = gap;
3107 1061834 : if (ll.vehAndGap.second < 0 && !MSGlobals::gComputeLC) {
3108 : // can always continue up to the stop line or crossing point
3109 : // @todo: figure out whether this should also impact lane changing
3110 74088 : ll.vehAndGap.second = MAX2(seen - nextLane->getLength(), ll.distToCrossing);
3111 : }
3112 : result = ll.vehAndGap;
3113 : }
3114 : }
3115 1018593 : if (shortestGap != std::numeric_limits<double>::max()) {
3116 : #ifdef DEBUG_CONTEXT
3117 : if (DEBUG_COND2(&veh)) {
3118 : std::cout << " found linkLeader after nextLane=" << nextLane->getID() << "\n";
3119 : gDebugFlag1 = false;
3120 : }
3121 : #endif
3122 1011241 : return result;
3123 : }
3124 : }
3125 39059283 : bool nextInternal = (*link)->getViaLane() != nullptr;
3126 : nextLane = (*link)->getViaLaneOrLane();
3127 19384243 : if (nextLane == nullptr) {
3128 : break;
3129 : }
3130 39059283 : nextLane->getVehiclesSecure(); // lock against running sim when called from GUI for time gap coloring
3131 39059283 : MSVehicle* leader = nextLane->getLastAnyVehicle();
3132 39059283 : if (leader != nullptr) {
3133 : #ifdef DEBUG_CONTEXT
3134 : if (DEBUG_COND2(&veh)) {
3135 : std::cout << " found leader " << leader->getID() << " on nextLane=" << nextLane->getID() << "\n";
3136 : }
3137 : #endif
3138 15304946 : const double leaderDist = seen + leader->getBackPositionOnLane(nextLane) - veh.getVehicleType().getMinGap();
3139 15304946 : nextLane->releaseVehicles();
3140 15304946 : return std::make_pair(leader, leaderDist);
3141 : }
3142 23754337 : nextLane->releaseVehicles();
3143 23754337 : if (nextLane->getVehicleMaxSpeed(&veh) < speed) {
3144 2355279 : dist = veh.getCarFollowModel().brakeGap(nextLane->getVehicleMaxSpeed(&veh));
3145 : }
3146 23754337 : seen += nextLane->getLength();
3147 23754337 : if (!nextInternal) {
3148 8005108 : view++;
3149 : }
3150 56577467 : } while (seen <= dist || nextLane->isInternal());
3151 : #ifdef DEBUG_CONTEXT
3152 : gDebugFlag1 = false;
3153 : #endif
3154 21912551 : return std::make_pair(static_cast<MSVehicle*>(nullptr), -1);
3155 : }
3156 :
3157 :
3158 : std::pair<MSVehicle* const, double>
3159 104721 : MSLane::getCriticalLeader(double dist, double seen, double speed, const MSVehicle& veh) const {
3160 : #ifdef DEBUG_CONTEXT
3161 : if (DEBUG_COND2(&veh)) {
3162 : std::cout << SIMTIME << " getCriticalLeader. lane=" << getID() << " veh=" << veh.getID() << "\n";
3163 : }
3164 : #endif
3165 104721 : const std::vector<MSLane*>& bestLaneConts = veh.getBestLanesContinuation(this);
3166 : std::pair<MSVehicle*, double> result = std::make_pair(static_cast<MSVehicle*>(nullptr), -1);
3167 : double safeSpeed = std::numeric_limits<double>::max();
3168 : int view = 1;
3169 : // loop over following lanes
3170 : // @note: we don't check the partial occupator for this lane since it was
3171 : // already checked in MSLaneChanger::getRealLeader()
3172 : const MSLane* nextLane = this;
3173 209442 : SUMOTime arrivalTime = MSNet::getInstance()->getCurrentTimeStep() + TIME2STEPS(seen / MAX2(speed, NUMERICAL_EPS));
3174 : do {
3175 : // get the next link used
3176 186897 : std::vector<MSLink*>::const_iterator link = succLinkSec(veh, view, *nextLane, bestLaneConts);
3177 184095 : if (nextLane->isLinkEnd(link) || !(*link)->opened(arrivalTime, speed, speed, veh.getVehicleType().getLength(),
3178 366048 : veh.getImpatience(), veh.getCarFollowModel().getMaxDecel(), 0, veh.getLateralPositionOnLane(), nullptr, false, &veh) || (*link)->haveRed()) {
3179 7746 : return result;
3180 : }
3181 : // check for link leaders
3182 : #ifdef DEBUG_CONTEXT
3183 : if (DEBUG_COND2(&veh)) {
3184 : gDebugFlag1 = true; // See MSLink::getLeaderInfo
3185 : }
3186 : #endif
3187 179151 : const MSLink::LinkLeaders linkLeaders = (*link)->getLeaderInfo(&veh, seen);
3188 : #ifdef DEBUG_CONTEXT
3189 : if (DEBUG_COND2(&veh)) {
3190 : gDebugFlag1 = false; // See MSLink::getLeaderInfo
3191 : }
3192 : #endif
3193 210088 : for (MSLink::LinkLeaders::const_iterator it = linkLeaders.begin(); it != linkLeaders.end(); ++it) {
3194 30937 : const MSVehicle* leader = (*it).vehAndGap.first;
3195 30937 : if (leader != nullptr && leader != result.first) {
3196 : // XXX ignoring pedestrians here!
3197 : // XXX ignoring the fact that the link leader may alread by following us
3198 : // XXX ignoring the fact that we may drive up to the crossing point
3199 30846 : double tmpSpeed = safeSpeed;
3200 30846 : veh.adaptToJunctionLeader((*it).vehAndGap, seen, nullptr, nextLane, tmpSpeed, tmpSpeed, (*it).distToCrossing);
3201 : #ifdef DEBUG_CONTEXT
3202 : if (DEBUG_COND2(&veh)) {
3203 : std::cout << " linkLeader=" << leader->getID() << " gap=" << result.second << " tmpSpeed=" << tmpSpeed << " safeSpeed=" << safeSpeed << "\n";
3204 : }
3205 : #endif
3206 30846 : if (tmpSpeed < safeSpeed) {
3207 : safeSpeed = tmpSpeed;
3208 : result = (*it).vehAndGap;
3209 : }
3210 : }
3211 : }
3212 179151 : bool nextInternal = (*link)->getViaLane() != nullptr;
3213 : nextLane = (*link)->getViaLaneOrLane();
3214 100800 : if (nextLane == nullptr) {
3215 : break;
3216 : }
3217 179151 : MSVehicle* leader = nextLane->getLastAnyVehicle();
3218 179151 : if (leader != nullptr && leader != result.first) {
3219 101722 : const double gap = seen + leader->getBackPositionOnLane(nextLane) - veh.getVehicleType().getMinGap();
3220 101722 : const double tmpSpeed = veh.getCarFollowModel().insertionFollowSpeed(&veh, speed, gap, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel(), leader);
3221 101722 : if (tmpSpeed < safeSpeed) {
3222 : safeSpeed = tmpSpeed;
3223 : result = std::make_pair(leader, gap);
3224 : }
3225 : }
3226 179151 : if (nextLane->getVehicleMaxSpeed(&veh) < speed) {
3227 27559 : dist = veh.getCarFollowModel().brakeGap(nextLane->getVehicleMaxSpeed(&veh));
3228 : }
3229 179151 : seen += nextLane->getLength();
3230 179151 : if (seen <= dist) {
3231 : // delaying the update of arrivalTime and making it conditional to avoid possible integer overflows
3232 37139 : arrivalTime += TIME2STEPS(nextLane->getLength() / MAX2(speed, NUMERICAL_EPS));
3233 : }
3234 179151 : if (!nextInternal) {
3235 100800 : view++;
3236 : }
3237 261327 : } while (seen <= dist || nextLane->isInternal());
3238 96975 : return result;
3239 : }
3240 :
3241 :
3242 : MSLane*
3243 1331798608 : MSLane::getLogicalPredecessorLane() const {
3244 1331798608 : if (myLogicalPredecessorLane == nullptr) {
3245 5462208 : MSEdgeVector pred = myEdge->getPredecessors();
3246 : // get only those edges which connect to this lane
3247 11315835 : for (MSEdgeVector::iterator i = pred.begin(); i != pred.end();) {
3248 5853627 : std::vector<IncomingLaneInfo>::const_iterator j = find_if(myIncomingLanes.begin(), myIncomingLanes.end(), edge_finder(*i));
3249 5853627 : if (j == myIncomingLanes.end()) {
3250 : i = pred.erase(i);
3251 : } else {
3252 : ++i;
3253 : }
3254 : }
3255 : // get the lane with the "straightest" connection
3256 5462208 : if (pred.size() != 0) {
3257 2081508 : std::sort(pred.begin(), pred.end(), by_connections_to_sorter(&getEdge()));
3258 1040754 : MSEdge* best = *pred.begin();
3259 1040754 : std::vector<IncomingLaneInfo>::const_iterator j = find_if(myIncomingLanes.begin(), myIncomingLanes.end(), edge_finder(best));
3260 1040754 : myLogicalPredecessorLane = j->lane;
3261 : }
3262 5462208 : }
3263 1331798608 : return myLogicalPredecessorLane;
3264 : }
3265 :
3266 :
3267 : const MSLane*
3268 925186464 : MSLane::getNormalPredecessorLane() const {
3269 2002177958 : if (isInternal()) {
3270 1076991494 : return getLogicalPredecessorLane()->getNormalPredecessorLane();
3271 : } else {
3272 : return this;
3273 : }
3274 : }
3275 :
3276 :
3277 : const MSLane*
3278 454262 : MSLane::getNormalSuccessorLane() const {
3279 621222 : if (isInternal()) {
3280 166960 : return getCanonicalSuccessorLane()->getNormalSuccessorLane();
3281 : } else {
3282 : return this;
3283 : }
3284 : }
3285 :
3286 :
3287 : MSLane*
3288 80304 : MSLane::getLogicalPredecessorLane(const MSEdge& fromEdge) const {
3289 141585 : for (const IncomingLaneInfo& cand : myIncomingLanes) {
3290 95219 : if (&(cand.lane->getEdge()) == &fromEdge) {
3291 : return cand.lane;
3292 : }
3293 : }
3294 : return nullptr;
3295 : }
3296 :
3297 :
3298 : MSLane*
3299 116116846 : MSLane::getCanonicalPredecessorLane() const {
3300 116116846 : if (myCanonicalPredecessorLane != nullptr) {
3301 : return myCanonicalPredecessorLane;
3302 : }
3303 1006891 : if (myIncomingLanes.empty()) {
3304 : return nullptr;
3305 : }
3306 : // myCanonicalPredecessorLane has not yet been determined and there exist incoming lanes
3307 : // get the lane with the priorized (or if this does not apply the "straightest") connection
3308 1006563 : const auto bestLane = std::min_element(myIncomingLanes.begin(), myIncomingLanes.end(), incoming_lane_priority_sorter(this));
3309 : {
3310 : #ifdef HAVE_FOX
3311 1006563 : ScopedLocker<> lock(myLeaderInfoMutex, MSGlobals::gNumSimThreads > 1);
3312 : #endif
3313 1006563 : myCanonicalPredecessorLane = bestLane->lane;
3314 : }
3315 : #ifdef DEBUG_LANE_SORTER
3316 : std::cout << "\nBest predecessor lane for lane '" << myID << "': '" << myCanonicalPredecessorLane->getID() << "'" << std::endl;
3317 : #endif
3318 1006563 : return myCanonicalPredecessorLane;
3319 : }
3320 :
3321 :
3322 : MSLane*
3323 2505069 : MSLane::getCanonicalSuccessorLane() const {
3324 2505069 : if (myCanonicalSuccessorLane != nullptr) {
3325 : return myCanonicalSuccessorLane;
3326 : }
3327 68170 : if (myLinks.empty()) {
3328 : return nullptr;
3329 : }
3330 : // myCanonicalSuccessorLane has not yet been determined and there exist outgoing links
3331 7770 : std::vector<MSLink*> candidateLinks = myLinks;
3332 : // get the lane with the priorized (or if this does not apply the "straightest") connection
3333 15540 : std::sort(candidateLinks.begin(), candidateLinks.end(), outgoing_lane_priority_sorter(this));
3334 7770 : MSLane* best = (*candidateLinks.begin())->getViaLaneOrLane();
3335 : #ifdef DEBUG_LANE_SORTER
3336 : std::cout << "\nBest successor lane for lane '" << myID << "': '" << best->getID() << "'" << std::endl;
3337 : #endif
3338 7770 : myCanonicalSuccessorLane = best;
3339 : return myCanonicalSuccessorLane;
3340 7770 : }
3341 :
3342 :
3343 : LinkState
3344 4726433 : MSLane::getIncomingLinkState() const {
3345 4726433 : const MSLane* const pred = getLogicalPredecessorLane();
3346 4726433 : if (pred == nullptr) {
3347 : return LINKSTATE_DEADEND;
3348 : } else {
3349 4726433 : return pred->getLinkTo(this)->getState();
3350 : }
3351 : }
3352 :
3353 :
3354 : const std::vector<std::pair<const MSLane*, const MSEdge*> >
3355 287620 : MSLane::getOutgoingViaLanes() const {
3356 : std::vector<std::pair<const MSLane*, const MSEdge*> > result;
3357 648250 : for (const MSLink* link : myLinks) {
3358 : assert(link->getLane() != nullptr);
3359 721260 : result.push_back(std::make_pair(link->getLane(), link->getViaLane() == nullptr ? nullptr : &link->getViaLane()->getEdge()));
3360 : }
3361 287620 : return result;
3362 0 : }
3363 :
3364 : std::vector<const MSLane*>
3365 76 : MSLane::getNormalIncomingLanes() const {
3366 76 : std::vector<const MSLane*> result = {};
3367 226 : for (std::map<MSEdge*, std::vector<MSLane*> >::const_iterator it = myApproachingLanes.begin(); it != myApproachingLanes.end(); ++it) {
3368 328 : for (std::vector<MSLane*>::const_iterator it_lane = (*it).second.begin(); it_lane != (*it).second.end(); ++it_lane) {
3369 178 : if (!((*it_lane)->isInternal())) {
3370 146 : result.push_back(*it_lane);
3371 : }
3372 : }
3373 : }
3374 76 : return result;
3375 0 : }
3376 :
3377 :
3378 : void
3379 1118555 : MSLane::leftByLaneChange(MSVehicle* v) {
3380 1118555 : myBruttoVehicleLengthSum -= v->getVehicleType().getLengthWithGap();
3381 1118555 : myNettoVehicleLengthSum -= v->getVehicleType().getLength();
3382 1118555 : }
3383 :
3384 :
3385 : void
3386 1074397 : MSLane::enteredByLaneChange(MSVehicle* v) {
3387 1074397 : myBruttoVehicleLengthSum += v->getVehicleType().getLengthWithGap();
3388 1074397 : myNettoVehicleLengthSum += v->getVehicleType().getLength();
3389 1074397 : }
3390 :
3391 :
3392 : int
3393 0 : MSLane::getCrossingIndex() const {
3394 0 : for (std::vector<MSLink*>::const_iterator i = myLinks.begin(); i != myLinks.end(); ++i) {
3395 0 : if ((*i)->getLane()->isCrossing()) {
3396 0 : return (int)(i - myLinks.begin());
3397 : }
3398 : }
3399 : return -1;
3400 : }
3401 :
3402 : // ------------ Current state retrieval
3403 : double
3404 2108410973 : MSLane::getFractionalVehicleLength(bool brutto) const {
3405 : double sum = 0;
3406 2108410973 : if (myPartialVehicles.size() > 0) {
3407 414949952 : const MSLane* bidi = getBidiLane();
3408 847343733 : for (MSVehicle* cand : myPartialVehicles) {
3409 432393781 : if (MSGlobals::gSublane && cand->getLaneChangeModel().getShadowLane() == this) {
3410 25223347 : continue;
3411 : }
3412 407170434 : if (cand->getLane() == bidi) {
3413 203622 : sum += (brutto ? cand->getVehicleType().getLengthWithGap() : cand->getVehicleType().getLength());
3414 : } else {
3415 407068623 : sum += myLength - cand->getBackPositionOnLane(this);
3416 : }
3417 : }
3418 : }
3419 2108410973 : return sum;
3420 : }
3421 :
3422 : double
3423 2108354253 : MSLane::getBruttoOccupancy() const {
3424 2108354253 : getVehiclesSecure();
3425 2108354253 : double fractions = getFractionalVehicleLength(true);
3426 2108354253 : if (myVehicles.size() != 0) {
3427 1566060703 : MSVehicle* lastVeh = myVehicles.front();
3428 1566060703 : if (lastVeh->getPositionOnLane() < lastVeh->getVehicleType().getLength()) {
3429 49123076 : fractions -= (lastVeh->getVehicleType().getLength() - lastVeh->getPositionOnLane());
3430 : }
3431 : }
3432 2108354253 : releaseVehicles();
3433 2108354253 : return MIN2(1., (myBruttoVehicleLengthSum + fractions) / myLength);
3434 : }
3435 :
3436 :
3437 : double
3438 56720 : MSLane::getNettoOccupancy() const {
3439 56720 : getVehiclesSecure();
3440 56720 : double fractions = getFractionalVehicleLength(false);
3441 56720 : if (myVehicles.size() != 0) {
3442 504 : MSVehicle* lastVeh = myVehicles.front();
3443 504 : if (lastVeh->getPositionOnLane() < lastVeh->getVehicleType().getLength()) {
3444 4 : fractions -= (lastVeh->getVehicleType().getLength() - lastVeh->getPositionOnLane());
3445 : }
3446 : }
3447 56720 : releaseVehicles();
3448 56720 : return (myNettoVehicleLengthSum + fractions) / myLength;
3449 : }
3450 :
3451 :
3452 : double
3453 46 : MSLane::getWaitingSeconds() const {
3454 46 : if (myVehicles.size() == 0) {
3455 : return 0;
3456 : }
3457 : double wtime = 0;
3458 48 : for (VehCont::const_iterator i = myVehicles.begin(); i != myVehicles.end(); ++i) {
3459 24 : wtime += (*i)->getWaitingSeconds();
3460 : }
3461 : return wtime;
3462 : }
3463 :
3464 :
3465 : double
3466 163870997 : MSLane::getMeanSpeed() const {
3467 163870997 : if (myVehicles.size() == 0) {
3468 136947047 : return myMaxSpeed;
3469 : }
3470 : double v = 0;
3471 : int numVehs = 0;
3472 170891164 : for (const MSVehicle* const veh : getVehiclesSecure()) {
3473 143967214 : if (!veh->isStopped() || !myEdge->hasLaneChanger()) {
3474 143461556 : v += veh->getSpeed();
3475 143461556 : numVehs++;
3476 : }
3477 : }
3478 26923950 : releaseVehicles();
3479 26923950 : if (numVehs == 0) {
3480 240813 : return myMaxSpeed;
3481 : }
3482 26683137 : return v / numVehs;
3483 : }
3484 :
3485 :
3486 : double
3487 2095 : MSLane::getMeanSpeedBike() const {
3488 : // @note: redundant code with getMeanSpeed to avoid extra checks in a function that is called very often
3489 2095 : if (myVehicles.size() == 0) {
3490 1480 : return myMaxSpeed;
3491 : }
3492 : double v = 0;
3493 : int numBikes = 0;
3494 2260 : for (MSVehicle* veh : getVehiclesSecure()) {
3495 1645 : if (veh->getVClass() == SVC_BICYCLE) {
3496 1150 : v += veh->getSpeed();
3497 1150 : numBikes++;
3498 : }
3499 : }
3500 : double ret;
3501 615 : if (numBikes > 0) {
3502 335 : ret = v / (double) myVehicles.size();
3503 : } else {
3504 280 : ret = myMaxSpeed;
3505 : }
3506 615 : releaseVehicles();
3507 615 : return ret;
3508 : }
3509 :
3510 :
3511 : double
3512 56723 : MSLane::getHarmonoise_NoiseEmissions() const {
3513 : double ret = 0;
3514 56723 : const MSLane::VehCont& vehs = getVehiclesSecure();
3515 56723 : if (vehs.size() == 0) {
3516 56223 : releaseVehicles();
3517 56223 : return 0;
3518 : }
3519 1216 : for (MSLane::VehCont::const_iterator i = vehs.begin(); i != vehs.end(); ++i) {
3520 716 : double sv = (*i)->getHarmonoise_NoiseEmissions();
3521 716 : ret += (double) pow(10., (sv / 10.));
3522 : }
3523 500 : releaseVehicles();
3524 500 : return HelpersHarmonoise::sum(ret);
3525 : }
3526 :
3527 :
3528 : int
3529 275649 : MSLane::vehicle_position_sorter::operator()(MSVehicle* v1, MSVehicle* v2) const {
3530 275649 : const double pos1 = v1->getBackPositionOnLane(myLane);
3531 275649 : const double pos2 = v2->getBackPositionOnLane(myLane);
3532 275649 : if (pos1 != pos2) {
3533 271692 : return pos1 > pos2;
3534 : } else {
3535 3957 : return v1->getNumericalID() > v2->getNumericalID();
3536 : }
3537 : }
3538 :
3539 :
3540 : int
3541 356106431 : MSLane::vehicle_natural_position_sorter::operator()(MSVehicle* v1, MSVehicle* v2) const {
3542 356106431 : const double pos1 = v1->getBackPositionOnLane(myLane);
3543 356106431 : const double pos2 = v2->getBackPositionOnLane(myLane);
3544 356106431 : if (pos1 != pos2) {
3545 355541350 : return pos1 < pos2;
3546 : } else {
3547 565081 : return v1->getLateralPositionOnLane() < v2->getLateralPositionOnLane();
3548 : }
3549 : }
3550 :
3551 :
3552 1040754 : MSLane::by_connections_to_sorter::by_connections_to_sorter(const MSEdge* const e) :
3553 1040754 : myEdge(e),
3554 1040754 : myLaneDir(e->getLanes()[0]->getShape().angleAt2D(0)) {
3555 1040754 : }
3556 :
3557 :
3558 : int
3559 20288 : MSLane::by_connections_to_sorter::operator()(const MSEdge* const e1, const MSEdge* const e2) const {
3560 : // std::cout << "\nby_connections_to_sorter()";
3561 :
3562 20288 : const std::vector<MSLane*>* ae1 = e1->allowedLanes(*myEdge);
3563 20288 : const std::vector<MSLane*>* ae2 = e2->allowedLanes(*myEdge);
3564 : double s1 = 0;
3565 20288 : if (ae1 != nullptr && ae1->size() != 0) {
3566 : // std::cout << "\nsize 1 = " << ae1->size()
3567 : // << " anglediff 1 = " << fabs(GeomHelper::angleDiff((*ae1)[0]->getShape().angleAt2D(0), myLaneDir)) / M_PI / 2.
3568 : // << "\nallowed lanes: ";
3569 : // for (std::vector<MSLane*>::const_iterator j = ae1->begin(); j != ae1->end(); ++j){
3570 : // std::cout << "\n" << (*j)->getID();
3571 : // }
3572 20288 : s1 = (double) ae1->size() + fabs(GeomHelper::angleDiff((*ae1)[0]->getShape().angleAt2D(0), myLaneDir)) / M_PI / 2.;
3573 : }
3574 : double s2 = 0;
3575 20288 : if (ae2 != nullptr && ae2->size() != 0) {
3576 : // std::cout << "\nsize 2 = " << ae2->size()
3577 : // << " anglediff 2 = " << fabs(GeomHelper::angleDiff((*ae2)[0]->getShape().angleAt2D(0), myLaneDir)) / M_PI / 2.
3578 : // << "\nallowed lanes: ";
3579 : // for (std::vector<MSLane*>::const_iterator j = ae2->begin(); j != ae2->end(); ++j){
3580 : // std::cout << "\n" << (*j)->getID();
3581 : // }
3582 20288 : s2 = (double) ae2->size() + fabs(GeomHelper::angleDiff((*ae2)[0]->getShape().angleAt2D(0), myLaneDir)) / M_PI / 2.;
3583 : }
3584 :
3585 : // std::cout << "\ne1 = " << e1->getID() << " e2 = " << e2->getID()
3586 : // << "\ns1 = " << s1 << " s2 = " << s2
3587 : // << std::endl;
3588 :
3589 20288 : return s1 < s2;
3590 : }
3591 :
3592 :
3593 1006563 : MSLane::incoming_lane_priority_sorter::incoming_lane_priority_sorter(const MSLane* const targetLane) :
3594 1006563 : myLane(targetLane),
3595 1006563 : myLaneDir(targetLane->getShape().angleAt2D(0)) {}
3596 :
3597 : int
3598 1362 : MSLane::incoming_lane_priority_sorter::operator()(const IncomingLaneInfo& laneInfo1, const IncomingLaneInfo& laneInfo2) const {
3599 1362 : const MSLane* noninternal1 = laneInfo1.lane;
3600 3349 : while (noninternal1->isInternal()) {
3601 : assert(noninternal1->getIncomingLanes().size() == 1);
3602 1987 : noninternal1 = noninternal1->getIncomingLanes()[0].lane;
3603 : }
3604 1362 : MSLane* noninternal2 = laneInfo2.lane;
3605 3025 : while (noninternal2->isInternal()) {
3606 : assert(noninternal2->getIncomingLanes().size() == 1);
3607 1663 : noninternal2 = noninternal2->getIncomingLanes()[0].lane;
3608 : }
3609 :
3610 1362 : const MSLink* link1 = noninternal1->getLinkTo(myLane);
3611 1362 : const MSLink* link2 = noninternal2->getLinkTo(myLane);
3612 :
3613 : #ifdef DEBUG_LANE_SORTER
3614 : std::cout << "\nincoming_lane_priority sorter()\n"
3615 : << "noninternal predecessor for lane '" << laneInfo1.lane->getID()
3616 : << "': '" << noninternal1->getID() << "'\n"
3617 : << "noninternal predecessor for lane '" << laneInfo2.lane->getID()
3618 : << "': '" << noninternal2->getID() << "'\n";
3619 : #endif
3620 :
3621 : assert(laneInfo1.lane->isInternal() || link1 == laneInfo1.viaLink);
3622 : assert(link1 != 0);
3623 : assert(link2 != 0);
3624 :
3625 : // check priority between links
3626 : bool priorized1 = true;
3627 : bool priorized2 = true;
3628 :
3629 : #ifdef DEBUG_LANE_SORTER
3630 : std::cout << "FoeLinks of '" << noninternal1->getID() << "'" << std::endl;
3631 : #endif
3632 2678 : for (const MSLink* const foeLink : link1->getFoeLinks()) {
3633 : #ifdef DEBUG_LANE_SORTER
3634 : std::cout << foeLink->getLaneBefore()->getID() << std::endl;
3635 : #endif
3636 2385 : if (foeLink == link2) {
3637 : priorized1 = false;
3638 : break;
3639 : }
3640 : }
3641 :
3642 : #ifdef DEBUG_LANE_SORTER
3643 : std::cout << "FoeLinks of '" << noninternal2->getID() << "'" << std::endl;
3644 : #endif
3645 3691 : for (const MSLink* const foeLink : link2->getFoeLinks()) {
3646 : #ifdef DEBUG_LANE_SORTER
3647 : std::cout << foeLink->getLaneBefore()->getID() << std::endl;
3648 : #endif
3649 : // either link1 is priorized, or it should not appear in link2's foes
3650 2839 : if (foeLink == link1) {
3651 : priorized2 = false;
3652 : break;
3653 : }
3654 : }
3655 : // if one link is subordinate, the other must be priorized (except for
3656 : // traffic lights where mutual response is permitted to handle stuck-on-red
3657 : // situation)
3658 1362 : if (priorized1 != priorized2) {
3659 1137 : return priorized1;
3660 : }
3661 :
3662 : // both are priorized, compare angle difference
3663 225 : double d1 = fabs(GeomHelper::angleDiff(noninternal1->getShape().angleAt2D(0), myLaneDir));
3664 225 : double d2 = fabs(GeomHelper::angleDiff(noninternal2->getShape().angleAt2D(0), myLaneDir));
3665 :
3666 225 : return d2 > d1;
3667 : }
3668 :
3669 :
3670 :
3671 7770 : MSLane::outgoing_lane_priority_sorter::outgoing_lane_priority_sorter(const MSLane* const sourceLane) :
3672 7770 : myLaneDir(sourceLane->getShape().angleAt2D(0)) {}
3673 :
3674 : int
3675 13670 : MSLane::outgoing_lane_priority_sorter::operator()(const MSLink* link1, const MSLink* link2) const {
3676 : const MSLane* target1 = link1->getLane();
3677 : const MSLane* target2 = link2->getLane();
3678 13670 : if (target2 == nullptr) {
3679 : return true;
3680 : }
3681 13670 : if (target1 == nullptr) {
3682 : return false;
3683 : }
3684 :
3685 : #ifdef DEBUG_LANE_SORTER
3686 : std::cout << "\noutgoing_lane_priority sorter()\n"
3687 : << "noninternal successors for lane '" << myLane->getID()
3688 : << "': '" << target1->getID() << "' and "
3689 : << "'" << target2->getID() << "'\n";
3690 : #endif
3691 :
3692 : // priority of targets
3693 : int priority1 = target1->getEdge().getPriority();
3694 : int priority2 = target2->getEdge().getPriority();
3695 :
3696 13670 : if (priority1 != priority2) {
3697 127 : return priority1 > priority2;
3698 : }
3699 :
3700 : // if priority of targets coincides, use angle difference
3701 :
3702 : // both are priorized, compare angle difference
3703 13543 : double d1 = fabs(GeomHelper::angleDiff(target1->getShape().angleAt2D(0), myLaneDir));
3704 13543 : double d2 = fabs(GeomHelper::angleDiff(target2->getShape().angleAt2D(0), myLaneDir));
3705 :
3706 13543 : return d2 > d1;
3707 : }
3708 :
3709 : void
3710 6259 : MSLane::addParking(MSBaseVehicle* veh) {
3711 : myParkingVehicles.insert(veh);
3712 6259 : }
3713 :
3714 :
3715 : void
3716 18134 : MSLane::removeParking(MSBaseVehicle* veh) {
3717 : myParkingVehicles.erase(veh);
3718 18134 : }
3719 :
3720 : bool
3721 73 : MSLane::hasApproaching() const {
3722 146 : for (const MSLink* link : myLinks) {
3723 79 : if (link->getApproaching().size() > 0) {
3724 : return true;
3725 : }
3726 : }
3727 : return false;
3728 : }
3729 :
3730 : void
3731 11044 : MSLane::saveState(OutputDevice& out) {
3732 11044 : const bool toRailJunction = myLinks.size() > 0 && (
3733 10615 : myEdge->getToJunction()->getType() == SumoXMLNodeType::RAIL_SIGNAL
3734 10531 : || myEdge->getToJunction()->getType() == SumoXMLNodeType::RAIL_CROSSING);
3735 : const bool hasVehicles = myVehicles.size() > 0;
3736 11044 : if (hasVehicles || (toRailJunction && hasApproaching())) {
3737 486 : out.openTag(SUMO_TAG_LANE);
3738 486 : out.writeAttr(SUMO_ATTR_ID, getID());
3739 486 : if (hasVehicles) {
3740 480 : out.openTag(SUMO_TAG_VIEWSETTINGS_VEHICLES);
3741 480 : out.writeAttr(SUMO_ATTR_VALUE, myVehicles);
3742 960 : out.closeTag();
3743 : }
3744 486 : if (toRailJunction) {
3745 37 : for (const MSLink* link : myLinks) {
3746 20 : if (link->getApproaching().size() > 0) {
3747 17 : out.openTag(SUMO_TAG_LINK);
3748 17 : out.writeAttr(SUMO_ATTR_TO, link->getViaLaneOrLane()->getID());
3749 34 : for (auto item : link->getApproaching()) {
3750 17 : out.openTag(SUMO_TAG_APPROACHING);
3751 17 : out.writeAttr(SUMO_ATTR_ID, item.first->getID());
3752 17 : out.writeAttr(SUMO_ATTR_ARRIVALTIME, item.second.arrivalTime);
3753 17 : out.writeAttr(SUMO_ATTR_ARRIVALSPEED, item.second.arrivalSpeed);
3754 17 : out.writeAttr(SUMO_ATTR_DEPARTSPEED, item.second.leaveSpeed);
3755 17 : out.writeAttr(SUMO_ATTR_REQUEST, item.second.willPass);
3756 17 : out.writeAttr(SUMO_ATTR_ARRIVALSPEEDBRAKING, item.second.arrivalSpeedBraking);
3757 17 : out.writeAttr(SUMO_ATTR_WAITINGTIME, item.second.waitingTime);
3758 17 : out.writeAttr(SUMO_ATTR_DISTANCE, item.second.dist);
3759 17 : if (item.second.latOffset != 0) {
3760 0 : out.writeAttr(SUMO_ATTR_POSITION_LAT, item.second.latOffset);
3761 : }
3762 34 : out.closeTag();
3763 : }
3764 34 : out.closeTag();
3765 : }
3766 : }
3767 : }
3768 972 : out.closeTag();
3769 : }
3770 11044 : }
3771 :
3772 : void
3773 8544 : MSLane::clearState() {
3774 : myVehicles.clear();
3775 : myParkingVehicles.clear();
3776 : myPartialVehicles.clear();
3777 : myManeuverReservations.clear();
3778 8544 : myBruttoVehicleLengthSum = 0;
3779 8544 : myNettoVehicleLengthSum = 0;
3780 8544 : myBruttoVehicleLengthSumToRemove = 0;
3781 8544 : myNettoVehicleLengthSumToRemove = 0;
3782 8544 : myLeaderInfoTime = SUMOTime_MIN;
3783 8544 : myFollowerInfoTime = SUMOTime_MIN;
3784 19840 : for (MSLink* link : myLinks) {
3785 11296 : link->clearState();
3786 : }
3787 8544 : }
3788 :
3789 : void
3790 572 : MSLane::loadState(const std::vector<SUMOVehicle*>& vehs) {
3791 1941 : for (SUMOVehicle* veh : vehs) {
3792 1369 : MSVehicle* v = dynamic_cast<MSVehicle*>(veh);
3793 1369 : v->updateBestLanes(false, this);
3794 : // incorporateVehicle resets the lastActionTime (which has just been loaded from state) so we must restore it
3795 1369 : const SUMOTime lastActionTime = v->getLastActionTime();
3796 1369 : incorporateVehicle(v, v->getPositionOnLane(), v->getSpeed(), v->getLateralPositionOnLane(), myVehicles.end(),
3797 : MSMoveReminder::NOTIFICATION_LOAD_STATE);
3798 1369 : v->resetActionOffset(lastActionTime - MSNet::getInstance()->getCurrentTimeStep());
3799 1369 : v->processNextStop(v->getSpeed());
3800 : }
3801 572 : }
3802 :
3803 :
3804 : double
3805 2405516170 : MSLane::getVehicleStopOffset(const MSVehicle* veh) const {
3806 2405516170 : if (!myLaneStopOffset.isDefined()) {
3807 : return 0;
3808 : }
3809 93531 : if ((myLaneStopOffset.getPermissions() & veh->getVClass()) != 0) {
3810 31232 : return myLaneStopOffset.getOffset();
3811 : } else {
3812 : return 0;
3813 : }
3814 : }
3815 :
3816 :
3817 : const StopOffset&
3818 2980 : MSLane::getLaneStopOffsets() const {
3819 2980 : return myLaneStopOffset;
3820 : }
3821 :
3822 :
3823 : void
3824 2152 : MSLane::setLaneStopOffset(const StopOffset& stopOffset) {
3825 2152 : myLaneStopOffset = stopOffset;
3826 2152 : }
3827 :
3828 :
3829 : MSLeaderDistanceInfo
3830 303803341 : MSLane::getFollowersOnConsecutive(const MSVehicle* ego, double backOffset,
3831 : bool allSublanes, double searchDist, MinorLinkMode mLinkMode) const {
3832 : assert(ego != 0);
3833 : // get the follower vehicle on the lane to change to
3834 303803341 : const double egoPos = backOffset + ego->getVehicleType().getLength();
3835 303803341 : const double egoLatDist = ego->getLane()->getRightSideOnEdge() - getRightSideOnEdge();
3836 304746680 : const bool getOppositeLeaders = ((ego->getLaneChangeModel().isOpposite() && ego->getLane() == this)
3837 303932418 : || (!ego->getLaneChangeModel().isOpposite() && &ego->getLane()->getEdge() != &getEdge()));
3838 : #ifdef DEBUG_CONTEXT
3839 : if (DEBUG_COND2(ego)) {
3840 : std::cout << SIMTIME << " getFollowers lane=" << getID() << " ego=" << ego->getID()
3841 : << " backOffset=" << backOffset << " pos=" << egoPos
3842 : << " allSub=" << allSublanes << " searchDist=" << searchDist << " ignoreMinor=" << mLinkMode
3843 : << " egoLatDist=" << egoLatDist
3844 : << " getOppositeLeaders=" << getOppositeLeaders
3845 : << "\n";
3846 : }
3847 : #endif
3848 311524442 : MSCriticalFollowerDistanceInfo result(myWidth, allSublanes ? nullptr : ego, allSublanes ? 0 : egoLatDist, getOppositeLeaders);
3849 303803341 : if (MSGlobals::gLateralResolution > 0 && egoLatDist == 0) {
3850 : // check whether ego is outside lane bounds far enough so that another vehicle might
3851 : // be between itself and the first "actual" sublane
3852 : // shift the offset so that we "see" this vehicle
3853 196036531 : if (ego->getLeftSideOnLane() < -MSGlobals::gLateralResolution) {
3854 82546 : result.setSublaneOffset(int(-ego->getLeftSideOnLane() / MSGlobals::gLateralResolution));
3855 195953985 : } else if (ego->getRightSideOnLane() > getWidth() + MSGlobals::gLateralResolution) {
3856 103321 : result.setSublaneOffset(-int((ego->getRightSideOnLane() - getWidth()) / MSGlobals::gLateralResolution));
3857 : }
3858 : #ifdef DEBUG_CONTEXT
3859 : if (DEBUG_COND2(ego)) {
3860 : std::cout << SIMTIME << " getFollowers lane=" << getID() << " ego=" << ego->getID()
3861 : << " egoPosLat=" << ego->getLateralPositionOnLane()
3862 : << " egoLatDist=" << ego->getLane()->getRightSideOnEdge() - getRightSideOnEdge()
3863 : << " extraOffset=" << result.getSublaneOffset()
3864 : << "\n";
3865 : }
3866 : #endif
3867 : }
3868 : /// XXX iterate in reverse and abort when there are no more freeSublanes
3869 6139108197 : for (AnyVehicleIterator last = anyVehiclesBegin(); last != anyVehiclesEnd(); ++last) {
3870 6139108197 : const MSVehicle* veh = *last;
3871 : #ifdef DEBUG_CONTEXT
3872 : if (DEBUG_COND2(ego)) {
3873 : std::cout << " veh=" << veh->getID() << " lane=" << veh->getLane()->getID() << " pos=" << veh->getPositionOnLane(this) << "\n";
3874 : }
3875 : #endif
3876 6139108197 : if (veh != ego && veh->getPositionOnLane(this) < egoPos) {
3877 : //const double latOffset = veh->getLane()->getRightSideOnEdge() - getRightSideOnEdge();
3878 2707074452 : const double latOffset = veh->getLatOffset(this);
3879 2707074452 : double dist = backOffset - veh->getPositionOnLane(this) - veh->getVehicleType().getMinGap();
3880 2707074452 : if (veh->isBidiOn(this)) {
3881 82860 : dist -= veh->getLength();
3882 : }
3883 2707074452 : result.addFollower(veh, ego, dist, latOffset);
3884 : #ifdef DEBUG_CONTEXT
3885 : if (DEBUG_COND2(ego)) {
3886 : std::cout << " (1) added veh=" << veh->getID() << " latOffset=" << latOffset << " result=" << result.toString() << "\n";
3887 : }
3888 : #endif
3889 : }
3890 : }
3891 : #ifdef DEBUG_CONTEXT
3892 : if (DEBUG_COND2(ego)) {
3893 : std::cout << " result.numFreeSublanes=" << result.numFreeSublanes() << "\n";
3894 : }
3895 : #endif
3896 303803341 : if (result.numFreeSublanes() > 0) {
3897 : // do a tree search among all follower lanes and check for the most
3898 : // important vehicle (the one requiring the largest reargap)
3899 : // to get a safe bound on the necessary search depth, we need to consider the maximum speed and minimum
3900 : // deceleration of potential follower vehicles
3901 139530001 : if (searchDist == -1) {
3902 138113823 : searchDist = getMaximumBrakeDist() - backOffset;
3903 : #ifdef DEBUG_CONTEXT
3904 : if (DEBUG_COND2(ego)) {
3905 : std::cout << " computed searchDist=" << searchDist << "\n";
3906 : }
3907 : #endif
3908 : }
3909 : std::set<const MSEdge*> egoFurther;
3910 151115701 : for (MSLane* further : ego->getFurtherLanes()) {
3911 11585700 : egoFurther.insert(&further->getEdge());
3912 : }
3913 150516722 : if (ego->getPositionOnLane() < ego->getVehicleType().getLength() && egoFurther.size() == 0
3914 140588250 : && ego->getLane()->getLogicalPredecessorLane() != nullptr) {
3915 : // on insertion
3916 690584 : egoFurther.insert(&ego->getLane()->getLogicalPredecessorLane()->getEdge());
3917 : }
3918 :
3919 : // avoid loops
3920 139530001 : std::set<const MSLane*> visited(myEdge->getLanes().begin(), myEdge->getLanes().end());
3921 139530001 : if (myEdge->getBidiEdge() != nullptr) {
3922 : visited.insert(myEdge->getBidiEdge()->getLanes().begin(), myEdge->getBidiEdge()->getLanes().end());
3923 : }
3924 : std::vector<MSLane::IncomingLaneInfo> newFound;
3925 139530001 : std::vector<MSLane::IncomingLaneInfo> toExamine = myIncomingLanes;
3926 314982321 : while (toExamine.size() != 0) {
3927 404332339 : for (std::vector<MSLane::IncomingLaneInfo>::iterator it = toExamine.begin(); it != toExamine.end(); ++it) {
3928 228880019 : MSLane* next = (*it).lane;
3929 228880019 : searchDist = MAX2(searchDist, next->getMaximumBrakeDist() - backOffset);
3930 228880019 : MSLeaderInfo first = next->getFirstVehicleInformation(nullptr, 0, false, std::numeric_limits<double>::max(), false);
3931 228880019 : MSLeaderInfo firstFront = next->getFirstVehicleInformation(nullptr, 0, true);
3932 : #ifdef DEBUG_CONTEXT
3933 : if (DEBUG_COND2(ego)) {
3934 : std::cout << " next=" << next->getID() << " seen=" << (*it).length << " first=" << first.toString() << " firstFront=" << firstFront.toString() << " backOffset=" << backOffset << "\n";
3935 : gDebugFlag1 = true; // for calling getLeaderInfo
3936 : }
3937 : #endif
3938 228880019 : if (backOffset + (*it).length - next->getLength() < 0
3939 228880019 : && egoFurther.count(&next->getEdge()) != 0
3940 : ) {
3941 : // check for junction foes that would interfere with lane changing
3942 : // @note: we are passing the back of ego as its front position so
3943 : // we need to add this back to the returned gap
3944 11845496 : const MSLink::LinkLeaders linkLeaders = (*it).viaLink->getLeaderInfo(ego, -backOffset);
3945 12138924 : for (const auto& ll : linkLeaders) {
3946 293428 : if (ll.vehAndGap.first != nullptr) {
3947 293416 : const bool bidiFoe = (*it).viaLink->getLane() == ll.vehAndGap.first->getLane()->getNormalPredecessorLane()->getBidiLane();
3948 293416 : const bool egoIsLeader = !bidiFoe && ll.vehAndGap.first->isLeader((*it).viaLink, ego, ll.vehAndGap.second);
3949 : // if ego is leader the returned gap still assumes that ego follows the leader
3950 : // if the foe vehicle follows ego we need to deduce that gap
3951 : const double gap = (egoIsLeader
3952 293416 : ? -ll.vehAndGap.second - ll.vehAndGap.first->getVehicleType().getLengthWithGap() - ego->getVehicleType().getMinGap()
3953 403 : : ll.vehAndGap.second + ego->getVehicleType().getLength());
3954 293416 : result.addFollower(ll.vehAndGap.first, ego, gap);
3955 : #ifdef DEBUG_CONTEXT
3956 : if (DEBUG_COND2(ego)) {
3957 : std::cout << SIMTIME << " ego=" << ego->getID() << " link=" << (*it).viaLink->getViaLaneOrLane()->getID()
3958 : << " (3) added veh=" << Named::getIDSecure(ll.vehAndGap.first)
3959 : << " gap=" << ll.vehAndGap.second << " dtC=" << ll.distToCrossing
3960 : << " bidiFoe=" << bidiFoe
3961 : << " egoIsLeader=" << egoIsLeader << " gap2=" << gap
3962 : << "\n";
3963 : }
3964 : #endif
3965 : }
3966 : }
3967 11845496 : }
3968 : #ifdef DEBUG_CONTEXT
3969 : if (DEBUG_COND2(ego)) {
3970 : gDebugFlag1 = false;
3971 : }
3972 : #endif
3973 :
3974 976092278 : for (int i = 0; i < first.numSublanes(); ++i) {
3975 747212259 : const MSVehicle* v = first[i] == ego ? firstFront[i] : first[i];
3976 : double agap = 0;
3977 :
3978 747212259 : if (v != nullptr && v != ego) {
3979 201145324 : if (!v->isFrontOnLane(next)) {
3980 : // the front of v is already on divergent trajectory from the ego vehicle
3981 : // for which this method is called (in the context of MSLaneChanger).
3982 : // Therefore, technically v is not a follower but only an obstruction and
3983 : // the gap is not between the front of v and the back of ego
3984 : // but rather between the flank of v and the back of ego.
3985 30789890 : agap = (*it).length - next->getLength() + backOffset;
3986 30789890 : if (MSGlobals::gUsingInternalLanes) {
3987 : // ego should have left the intersection still occupied by v
3988 30780768 : agap -= v->getVehicleType().getMinGap();
3989 : }
3990 : #ifdef DEBUG_CONTEXT
3991 : if (DEBUG_COND2(ego)) {
3992 : std::cout << " agap1=" << agap << "\n";
3993 : }
3994 : #endif
3995 30789890 : const bool differentEdge = &v->getLane()->getEdge() != &ego->getLane()->getEdge();
3996 30789890 : if (agap > 0 && differentEdge) {
3997 : // Only if ego overlaps we treat v as if it were a real follower
3998 : // Otherwise we ignore it and look for another follower
3999 19428191 : if (!getOppositeLeaders) {
4000 : // even if the vehicle is not a real
4001 : // follower, it still forms a real
4002 : // obstruction in opposite direction driving
4003 19237941 : v = firstFront[i];
4004 19237941 : if (v != nullptr && v != ego) {
4005 15181789 : agap = (*it).length - v->getPositionOnLane() + backOffset - v->getVehicleType().getMinGap();
4006 : } else {
4007 : v = nullptr;
4008 : }
4009 : }
4010 11361699 : } else if (differentEdge && result.hasVehicle(v)) {
4011 : // ignore this vehicle as it was already seen on another lane
4012 : agap = 0;
4013 : }
4014 : } else {
4015 170355434 : if (next->getBidiLane() != nullptr && v->isBidiOn(next)) {
4016 76282 : agap = v->getPositionOnLane() + backOffset - v->getVehicleType().getLengthWithGap();
4017 : } else {
4018 170279152 : agap = (*it).length - v->getPositionOnLane() + backOffset - v->getVehicleType().getMinGap();
4019 : }
4020 170355434 : if (!(*it).viaLink->havePriority() && egoFurther.count(&(*it).lane->getEdge()) == 0
4021 11604852 : && ego->isOnRoad() // during insertion, this can lead to collisions because ego's further lanes are not set (see #3053)
4022 11510073 : && !ego->getLaneChangeModel().isOpposite()
4023 181864445 : && v->getSpeed() < SUMO_const_haltingSpeed
4024 : ) {
4025 : // if v is stopped on a minor side road it should not block lane changing
4026 : agap = MAX2(agap, 0.0);
4027 : }
4028 : }
4029 201145324 : result.addFollower(v, ego, agap, 0, i);
4030 : #ifdef DEBUG_CONTEXT
4031 : if (DEBUG_COND2(ego)) {
4032 : std::cout << " (2) added veh=" << Named::getIDSecure(v) << " agap=" << agap << " next=" << next->getID() << " result=" << result.toString() << "\n";
4033 : }
4034 : #endif
4035 : }
4036 : }
4037 228880019 : if ((*it).length < searchDist) {
4038 : const std::vector<MSLane::IncomingLaneInfo>& followers = next->getIncomingLanes();
4039 212358741 : for (std::vector<MSLane::IncomingLaneInfo>::const_iterator j = followers.begin(); j != followers.end(); ++j) {
4040 109339335 : if (visited.find((*j).lane) == visited.end() && (((*j).viaLink->havePriority() && !(*j).viaLink->isTurnaround())
4041 31041815 : || mLinkMode == MinorLinkMode::FOLLOW_ALWAYS
4042 1593010 : || (mLinkMode == MinorLinkMode::FOLLOW_ONCOMING && (*j).viaLink->getDirection() == LinkDirection::STRAIGHT))) {
4043 102198962 : visited.insert((*j).lane);
4044 : MSLane::IncomingLaneInfo ili;
4045 102198962 : ili.lane = (*j).lane;
4046 102198962 : ili.length = (*j).length + (*it).length;
4047 102198962 : ili.viaLink = (*j).viaLink;
4048 102198962 : newFound.push_back(ili);
4049 : }
4050 : }
4051 : }
4052 228880019 : }
4053 : toExamine.clear();
4054 : swap(newFound, toExamine);
4055 : }
4056 : //return result;
4057 :
4058 139530001 : }
4059 607606682 : return result;
4060 303803341 : }
4061 :
4062 :
4063 : void
4064 16124921 : MSLane::getLeadersOnConsecutive(double dist, double seen, double speed, const MSVehicle* ego,
4065 : const std::vector<MSLane*>& bestLaneConts, MSLeaderDistanceInfo& result,
4066 : bool oppositeDirection) const {
4067 16124921 : if (seen > dist && !(isInternal() && MSGlobals::gComputeLC)) {
4068 : return;
4069 : }
4070 : // check partial vehicles (they might be on a different route and thus not
4071 : // found when iterating along bestLaneConts)
4072 16920757 : for (VehCont::const_iterator it = myPartialVehicles.begin(); it != myPartialVehicles.end(); ++it) {
4073 2691074 : MSVehicle* veh = *it;
4074 2691074 : if (!veh->isFrontOnLane(this)) {
4075 795836 : result.addLeader(veh, seen, veh->getLatOffset(this));
4076 : } else {
4077 : break;
4078 : }
4079 : }
4080 : #ifdef DEBUG_CONTEXT
4081 : if (DEBUG_COND2(ego)) {
4082 : gDebugFlag1 = true;
4083 : }
4084 : #endif
4085 : const MSLane* nextLane = this;
4086 : int view = 1;
4087 : // loop over following lanes
4088 34269440 : while ((seen < dist || nextLane->isInternal()) && result.numFreeSublanes() > 0) {
4089 : // get the next link used
4090 : bool nextInternal = false;
4091 23280827 : if (oppositeDirection) {
4092 7720 : if (view >= (int)bestLaneConts.size()) {
4093 : break;
4094 : }
4095 2731 : nextLane = bestLaneConts[view];
4096 : } else {
4097 23273107 : std::vector<MSLink*>::const_iterator link = succLinkSec(*ego, view, *nextLane, bestLaneConts);
4098 23273107 : if (nextLane->isLinkEnd(link)) {
4099 : break;
4100 : }
4101 : // check for link leaders
4102 19005782 : const MSLink::LinkLeaders linkLeaders = (*link)->getLeaderInfo(ego, seen);
4103 19005782 : if (linkLeaders.size() > 0) {
4104 977405 : const MSLink::LinkLeader ll = linkLeaders[0];
4105 977405 : MSVehicle* veh = ll.vehAndGap.first;
4106 : // in the context of lane changing all junction leader candidates must be respected
4107 977405 : if (veh != 0 && (ego->isLeader(*link, veh, ll.vehAndGap.second)
4108 100546 : || (MSGlobals::gComputeLC
4109 1077951 : && veh->getPosition().distanceTo2D(ego->getPosition()) - veh->getVehicleType().getMinGap() - ego->getVehicleType().getLength()
4110 100546 : < veh->getCarFollowModel().brakeGap(veh->getSpeed())))) {
4111 : #ifdef DEBUG_CONTEXT
4112 : if (DEBUG_COND2(ego)) {
4113 : std::cout << " linkleader=" << veh->getID() << " gap=" << ll.vehAndGap.second << " leaderOffset=" << ll.latOffset << " flags=" << ll.llFlags << "\n";
4114 : }
4115 : #endif
4116 863994 : if (ll.sameTarget() || ll.sameSource()) {
4117 587493 : result.addLeader(veh, ll.vehAndGap.second, ll.latOffset);
4118 : } else {
4119 : // add link leader to all sublanes and return
4120 1899871 : for (int i = 0; i < result.numSublanes(); ++i) {
4121 1623370 : result.addLeader(veh, ll.vehAndGap.second, 0, i);
4122 : }
4123 : }
4124 : #ifdef DEBUG_CONTEXT
4125 : gDebugFlag1 = false;
4126 : #endif
4127 863994 : return;
4128 : } // XXX else, deal with pedestrians
4129 : }
4130 18141788 : nextInternal = (*link)->getViaLane() != nullptr;
4131 : nextLane = (*link)->getViaLaneOrLane();
4132 10693500 : if (nextLane == nullptr) {
4133 : break;
4134 : }
4135 19005782 : }
4136 :
4137 18144519 : MSLeaderInfo leaders = nextLane->getLastVehicleInformation(nullptr, 0, 0, false);
4138 : #ifdef DEBUG_CONTEXT
4139 : if (DEBUG_COND2(ego)) {
4140 : std::cout << SIMTIME << " getLeadersOnConsecutive lane=" << getID() << " nextLane=" << nextLane->getID() << " leaders=" << leaders.toString() << "\n";
4141 : }
4142 : #endif
4143 : // @todo check alignment issues if the lane width changes
4144 : const int iMax = MIN2(leaders.numSublanes(), result.numSublanes());
4145 96885352 : for (int i = 0; i < iMax; ++i) {
4146 78740833 : const MSVehicle* veh = leaders[i];
4147 78740833 : if (veh != nullptr) {
4148 : #ifdef DEBUG_CONTEXT
4149 : if (DEBUG_COND2(ego)) std::cout << " lead=" << veh->getID()
4150 : << " seen=" << seen
4151 : << " minGap=" << ego->getVehicleType().getMinGap()
4152 : << " backPos=" << veh->getBackPositionOnLane(nextLane)
4153 : << " gap=" << seen - ego->getVehicleType().getMinGap() + veh->getBackPositionOnLane(nextLane)
4154 : << "\n";
4155 : #endif
4156 31443082 : result.addLeader(veh, seen - ego->getVehicleType().getMinGap() + veh->getBackPositionOnLane(nextLane), 0, i);
4157 : }
4158 : }
4159 :
4160 18144519 : if (nextLane->getVehicleMaxSpeed(ego) < speed) {
4161 935065 : dist = ego->getCarFollowModel().brakeGap(nextLane->getVehicleMaxSpeed(ego));
4162 : }
4163 18144519 : seen += nextLane->getLength();
4164 18144519 : if (!nextInternal) {
4165 10696231 : view++;
4166 : }
4167 18144519 : }
4168 : #ifdef DEBUG_CONTEXT
4169 : gDebugFlag1 = false;
4170 : #endif
4171 : }
4172 :
4173 :
4174 : void
4175 122849761 : MSLane::addLeaders(const MSVehicle* vehicle, double vehPos, MSLeaderDistanceInfo& result, bool opposite) {
4176 : // if there are vehicles on the target lane with the same position as ego,
4177 : // they may not have been added to 'ahead' yet
4178 : #ifdef DEBUG_SURROUNDING
4179 : if (DEBUG_COND || DEBUG_COND2(vehicle)) {
4180 : std::cout << " addLeaders lane=" << getID() << " veh=" << vehicle->getID() << " vehPos=" << vehPos << " opposite=" << opposite << "\n";
4181 : }
4182 : #endif
4183 122849761 : const MSLeaderInfo& aheadSamePos = getLastVehicleInformation(nullptr, 0, vehPos, false);
4184 637992901 : for (int i = 0; i < aheadSamePos.numSublanes(); ++i) {
4185 515143140 : const MSVehicle* veh = aheadSamePos[i];
4186 515143140 : if (veh != nullptr && veh != vehicle) {
4187 138531976 : const double gap = veh->getBackPositionOnLane(this) - vehPos - vehicle->getVehicleType().getMinGap();
4188 : #ifdef DEBUG_SURROUNDING
4189 : if (DEBUG_COND || DEBUG_COND2(vehicle)) {
4190 : std::cout << " further lead=" << veh->getID() << " leadBack=" << veh->getBackPositionOnLane(this) << " gap=" << gap << "\n";
4191 : }
4192 : #endif
4193 138531976 : result.addLeader(veh, gap, 0, i);
4194 : }
4195 : }
4196 :
4197 122849761 : if (result.numFreeSublanes() > 0) {
4198 36193163 : double seen = vehicle->getLane()->getLength() - vehPos;
4199 36193163 : double speed = vehicle->getSpeed();
4200 : // leader vehicle could be link leader on the next junction
4201 36193163 : double dist = MAX2(vehicle->getCarFollowModel().brakeGap(speed), 10.0) + vehicle->getVehicleType().getMinGap();
4202 36193163 : if (getBidiLane() != nullptr) {
4203 56603 : dist = MAX2(dist, myMaxSpeed * 20);
4204 : }
4205 : // check for link leaders when on internal
4206 36193163 : if (seen > dist && !(isInternal() && MSGlobals::gComputeLC)) {
4207 : #ifdef DEBUG_SURROUNDING
4208 : if (DEBUG_COND || DEBUG_COND2(vehicle)) {
4209 : std::cout << " aborting forward search. dist=" << dist << " seen=" << seen << "\n";
4210 : }
4211 : #endif
4212 : return;
4213 : }
4214 : #ifdef DEBUG_SURROUNDING
4215 : if (DEBUG_COND || DEBUG_COND2(vehicle)) {
4216 : std::cout << " add consecutive before=" << result.toString() << " seen=" << seen << " dist=" << dist;
4217 : }
4218 : #endif
4219 16124921 : if (opposite) {
4220 6355 : const std::vector<MSLane*> bestLaneConts = vehicle->getUpstreamOppositeLanes();
4221 : #ifdef DEBUG_SURROUNDING
4222 : if (DEBUG_COND || DEBUG_COND2(vehicle)) {
4223 : std::cout << " upstreamOpposite=" << toString(bestLaneConts);
4224 : }
4225 : #endif
4226 6355 : getLeadersOnConsecutive(dist, seen, speed, vehicle, bestLaneConts, result, opposite);
4227 6355 : } else {
4228 16118566 : const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation(this);
4229 16118566 : getLeadersOnConsecutive(dist, seen, speed, vehicle, bestLaneConts, result);
4230 : }
4231 : #ifdef DEBUG_SURROUNDING
4232 : if (DEBUG_COND || DEBUG_COND2(vehicle)) {
4233 : std::cout << " after=" << result.toString() << "\n";
4234 : }
4235 : #endif
4236 : }
4237 122849761 : }
4238 :
4239 :
4240 : MSVehicle*
4241 485719214 : MSLane::getPartialBehind(const MSVehicle* ego) const {
4242 512022676 : for (VehCont::const_reverse_iterator i = myPartialVehicles.rbegin(); i != myPartialVehicles.rend(); ++i) {
4243 27976184 : MSVehicle* veh = *i;
4244 27976184 : if (veh->isFrontOnLane(this)
4245 3262538 : && veh != ego
4246 31238722 : && veh->getPositionOnLane() <= ego->getPositionOnLane()) {
4247 : #ifdef DEBUG_CONTEXT
4248 : if (DEBUG_COND2(ego)) {
4249 : std::cout << SIMTIME << " getPartialBehind lane=" << getID() << " ego=" << ego->getID() << " found=" << veh->getID() << "\n";
4250 : }
4251 : #endif
4252 : return veh;
4253 : }
4254 : }
4255 : #ifdef DEBUG_CONTEXT
4256 : if (DEBUG_COND2(ego)) {
4257 : std::cout << SIMTIME << " getPartialBehind lane=" << getID() << " ego=" << ego->getID() << " nothing found. partials=" << toString(myPartialVehicles) << "\n";
4258 : }
4259 : #endif
4260 : return nullptr;
4261 : }
4262 :
4263 : MSLeaderInfo
4264 19985800 : MSLane::getPartialBeyond() const {
4265 19985800 : MSLeaderInfo result(myWidth);
4266 21410338 : for (VehCont::const_iterator it = myPartialVehicles.begin(); it != myPartialVehicles.end(); ++it) {
4267 2938758 : MSVehicle* veh = *it;
4268 2938758 : if (!veh->isFrontOnLane(this)) {
4269 1424538 : result.addLeader(veh, false, veh->getLatOffset(this));
4270 : } else {
4271 : break;
4272 : }
4273 : }
4274 19985800 : return result;
4275 0 : }
4276 :
4277 :
4278 : std::set<MSVehicle*>
4279 11180 : MSLane::getSurroundingVehicles(double startPos, double downstreamDist, double upstreamDist, std::shared_ptr<LaneCoverageInfo> checkedLanes) const {
4280 : assert(checkedLanes != nullptr);
4281 11180 : if (checkedLanes->find(this) != checkedLanes->end()) {
4282 : #ifdef DEBUG_SURROUNDING
4283 : std::cout << "Skipping previously scanned lane: " << getID() << std::endl;
4284 : #endif
4285 2266 : return std::set<MSVehicle*>();
4286 : } else {
4287 : // Add this lane's coverage to the lane coverage info
4288 18841 : (*checkedLanes)[this] = std::make_pair(MAX2(0.0, startPos - upstreamDist), MIN2(startPos + downstreamDist, getLength()));
4289 : }
4290 : #ifdef DEBUG_SURROUNDING
4291 : std::cout << "Scanning on lane " << myID << "(downstr. " << downstreamDist << ", upstr. " << upstreamDist << ", startPos " << startPos << "): " << std::endl;
4292 : #endif
4293 14492 : std::set<MSVehicle*> foundVehicles = getVehiclesInRange(MAX2(0., startPos - upstreamDist), MIN2(myLength, startPos + downstreamDist));
4294 8914 : if (startPos < upstreamDist) {
4295 : // scan incoming lanes
4296 10886 : for (const IncomingLaneInfo& incomingInfo : getIncomingLanes()) {
4297 5412 : MSLane* incoming = incomingInfo.lane;
4298 : #ifdef DEBUG_SURROUNDING
4299 : std::cout << "Checking on incoming: " << incoming->getID() << std::endl;
4300 : if (checkedLanes->find(incoming) != checkedLanes->end()) {
4301 : std::cout << "Skipping previous: " << incoming->getID() << std::endl;
4302 : }
4303 : #endif
4304 10824 : std::set<MSVehicle*> newVehs = incoming->getSurroundingVehicles(incoming->getLength(), 0.0, upstreamDist - startPos, checkedLanes);
4305 5412 : foundVehicles.insert(newVehs.begin(), newVehs.end());
4306 : }
4307 : }
4308 :
4309 8914 : if (getLength() < startPos + downstreamDist) {
4310 : // scan successive lanes
4311 : const std::vector<MSLink*>& lc = getLinkCont();
4312 6130 : for (MSLink* l : lc) {
4313 : #ifdef DEBUG_SURROUNDING
4314 : std::cout << "Checking on outgoing: " << l->getViaLaneOrLane()->getID() << std::endl;
4315 : #endif
4316 5588 : std::set<MSVehicle*> newVehs = l->getViaLaneOrLane()->getSurroundingVehicles(0.0, downstreamDist - (myLength - startPos), upstreamDist, checkedLanes);
4317 2794 : foundVehicles.insert(newVehs.begin(), newVehs.end());
4318 : }
4319 : }
4320 : #ifdef DEBUG_SURROUNDING
4321 : std::cout << "On lane (2) " << myID << ": \nFound vehicles: " << std::endl;
4322 : for (MSVehicle* v : foundVehicles) {
4323 : std::cout << v->getID() << " pos = " << v->getPositionOnLane() << std::endl;
4324 : }
4325 : #endif
4326 : return foundVehicles;
4327 : }
4328 :
4329 :
4330 : std::set<MSVehicle*>
4331 11749 : MSLane::getVehiclesInRange(const double a, const double b) const {
4332 : std::set<MSVehicle*> res;
4333 11749 : const VehCont& vehs = getVehiclesSecure();
4334 :
4335 11749 : if (!vehs.empty()) {
4336 12545 : for (MSVehicle* const veh : vehs) {
4337 8356 : if (veh->getPositionOnLane() >= a) {
4338 7012 : if (veh->getBackPositionOnLane() > b) {
4339 : break;
4340 : }
4341 : res.insert(veh);
4342 : }
4343 : }
4344 : }
4345 11749 : releaseVehicles();
4346 11749 : return res;
4347 : }
4348 :
4349 :
4350 : std::vector<const MSJunction*>
4351 0 : MSLane::getUpcomingJunctions(double pos, double range, const std::vector<MSLane*>& contLanes) const {
4352 : // set of upcoming junctions and the corresponding conflict links
4353 : std::vector<const MSJunction*> junctions;
4354 0 : for (auto l : getUpcomingLinks(pos, range, contLanes)) {
4355 0 : junctions.insert(junctions.end(), l->getJunction());
4356 0 : }
4357 0 : return junctions;
4358 0 : }
4359 :
4360 :
4361 : std::vector<const MSLink*>
4362 734 : MSLane::getUpcomingLinks(double pos, double range, const std::vector<MSLane*>& contLanes) const {
4363 : #ifdef DEBUG_SURROUNDING
4364 : std::cout << "getUpcoming links on lane '" << getID() << "' with pos=" << pos
4365 : << " range=" << range << std::endl;
4366 : #endif
4367 : // set of upcoming junctions and the corresponding conflict links
4368 : std::vector<const MSLink*> links;
4369 :
4370 : // Currently scanned lane
4371 : const MSLane* lane = this;
4372 :
4373 : // continuation lanes for the vehicle
4374 : std::vector<MSLane*>::const_iterator contLanesIt = contLanes.begin();
4375 : // scanned distance so far
4376 : double dist = 0.0;
4377 : // link to be crossed by the vehicle
4378 734 : const MSLink* link = nullptr;
4379 734 : if (lane->isInternal()) {
4380 : assert(*contLanesIt == nullptr); // is called with vehicle's bestLane structure
4381 113 : link = lane->getEntryLink();
4382 113 : links.insert(links.end(), link);
4383 113 : dist += link->getInternalLengthsAfter();
4384 : // next non-internal lane behind junction
4385 : lane = link->getLane();
4386 : pos = 0.0;
4387 : assert(*(contLanesIt + 1) == lane);
4388 : }
4389 1224 : while (++contLanesIt != contLanes.end()) {
4390 : assert(!lane->isInternal());
4391 934 : dist += lane->getLength() - pos;
4392 : pos = 0.;
4393 : #ifdef DEBUG_SURROUNDING
4394 : std::cout << "Distance until end of lane '" << lane->getID() << "' is " << dist << "." << std::endl;
4395 : #endif
4396 934 : if (dist > range) {
4397 : break;
4398 : }
4399 490 : link = lane->getLinkTo(*contLanesIt);
4400 490 : if (link != nullptr) {
4401 478 : links.insert(links.end(), link);
4402 : }
4403 490 : lane = *contLanesIt;
4404 : }
4405 734 : return links;
4406 0 : }
4407 :
4408 :
4409 : MSLane*
4410 245659316 : MSLane::getOpposite() const {
4411 245659316 : return myOpposite;
4412 : }
4413 :
4414 :
4415 : MSLane*
4416 186864175 : MSLane::getParallelOpposite() const {
4417 186864175 : return myEdge->getLanes().back()->getOpposite();
4418 : }
4419 :
4420 :
4421 : double
4422 7562868 : MSLane::getOppositePos(double pos) const {
4423 7562868 : return MAX2(0., myLength - pos);
4424 : }
4425 :
4426 : std::pair<MSVehicle* const, double>
4427 9339847 : MSLane::getFollower(const MSVehicle* ego, double egoPos, double dist, MinorLinkMode mLinkMode) const {
4428 33703727 : for (AnyVehicleIterator first = anyVehiclesUpstreamBegin(); first != anyVehiclesUpstreamEnd(); ++first) {
4429 : // XXX refactor leaderInfo to use a const vehicle all the way through the call hierarchy
4430 41353585 : MSVehicle* pred = (MSVehicle*)*first;
4431 : #ifdef DEBUG_CONTEXT
4432 : if (DEBUG_COND2(ego)) {
4433 : std::cout << " getFollower lane=" << getID() << " egoPos=" << egoPos << " pred=" << pred->getID() << " predPos=" << pred->getPositionOnLane(this) << "\n";
4434 : }
4435 : #endif
4436 41353585 : if (pred != ego && pred->getPositionOnLane(this) < egoPos) {
4437 7649858 : return std::pair<MSVehicle* const, double>(pred, egoPos - pred->getPositionOnLane(this) - ego->getVehicleType().getLength() - pred->getVehicleType().getMinGap());
4438 : }
4439 : }
4440 1689989 : const double backOffset = egoPos - ego->getVehicleType().getLength();
4441 1689989 : if (dist > 0 && backOffset > dist) {
4442 322102 : return std::make_pair(nullptr, -1);
4443 : }
4444 1367887 : const MSLeaderDistanceInfo followers = getFollowersOnConsecutive(ego, backOffset, true, dist, mLinkMode);
4445 1367887 : CLeaderDist result = followers.getClosest();
4446 1367887 : return std::make_pair(const_cast<MSVehicle*>(result.first), result.second);
4447 1367887 : }
4448 :
4449 : std::pair<MSVehicle* const, double>
4450 5138376 : MSLane::getOppositeLeader(const MSVehicle* ego, double dist, bool oppositeDir, MinorLinkMode mLinkMode) const {
4451 : #ifdef DEBUG_OPPOSITE
4452 : if (DEBUG_COND2(ego)) std::cout << SIMTIME << " getOppositeLeader lane=" << getID()
4453 : << " ego=" << ego->getID()
4454 : << " pos=" << ego->getPositionOnLane()
4455 : << " posOnOpposite=" << getOppositePos(ego->getPositionOnLane())
4456 : << " dist=" << dist
4457 : << " oppositeDir=" << oppositeDir
4458 : << "\n";
4459 : #endif
4460 5138376 : if (!oppositeDir) {
4461 384420 : return getLeader(ego, getOppositePos(ego->getPositionOnLane()), ego->getBestLanesContinuation(this));
4462 : } else {
4463 4753956 : const double egoLength = ego->getVehicleType().getLength();
4464 4753956 : const double egoPos = ego->getLaneChangeModel().isOpposite() ? ego->getPositionOnLane() : getOppositePos(ego->getPositionOnLane());
4465 4753956 : std::pair<MSVehicle* const, double> result = getFollower(ego, egoPos + egoLength, dist, mLinkMode);
4466 4753956 : if (result.first != nullptr) {
4467 4108545 : result.second -= ego->getVehicleType().getMinGap();
4468 4108545 : if (result.first->getLaneChangeModel().isOpposite()) {
4469 1309412 : result.second -= result.first->getVehicleType().getLength();
4470 : }
4471 : }
4472 4753956 : return result;
4473 : }
4474 : }
4475 :
4476 :
4477 : std::pair<MSVehicle* const, double>
4478 700848 : MSLane::getOppositeFollower(const MSVehicle* ego) const {
4479 : #ifdef DEBUG_OPPOSITE
4480 : if (DEBUG_COND2(ego)) std::cout << SIMTIME << " getOppositeFollower lane=" << getID()
4481 : << " ego=" << ego->getID()
4482 : << " backPos=" << ego->getBackPositionOnLane()
4483 : << " posOnOpposite=" << getOppositePos(ego->getBackPositionOnLane())
4484 : << "\n";
4485 : #endif
4486 700848 : if (ego->getLaneChangeModel().isOpposite()) {
4487 415120 : std::pair<MSVehicle* const, double> result = getFollower(ego, getOppositePos(ego->getPositionOnLane()), -1, MinorLinkMode::FOLLOW_NEVER);
4488 415120 : return result;
4489 : } else {
4490 285728 : double vehPos = getOppositePos(ego->getPositionOnLane() - ego->getVehicleType().getLength());
4491 285728 : std::pair<MSVehicle*, double> result = getLeader(ego, vehPos, std::vector<MSLane*>());
4492 285728 : double dist = getMaximumBrakeDist() + getOppositePos(ego->getPositionOnLane() - getLength());
4493 : MSLane* next = const_cast<MSLane*>(this);
4494 510482 : while (result.first == nullptr && dist > 0) {
4495 : // cannot call getLeadersOnConsecutive because succLinkSec doesn't
4496 : // uses the vehicle's route and doesn't work on the opposite side
4497 285091 : vehPos -= next->getLength();
4498 285091 : next = next->getCanonicalSuccessorLane();
4499 285091 : if (next == nullptr) {
4500 : break;
4501 : }
4502 224754 : dist -= next->getLength();
4503 224754 : result = next->getLeader(ego, vehPos, std::vector<MSLane*>());
4504 : }
4505 285728 : if (result.first != nullptr) {
4506 220591 : if (result.first->getLaneChangeModel().isOpposite()) {
4507 81718 : result.second -= result.first->getVehicleType().getLength();
4508 : } else {
4509 138873 : if (result.second > POSITION_EPS) {
4510 : // follower can be safely ignored since it is going the other way
4511 130797 : return std::make_pair(static_cast<MSVehicle*>(nullptr), -1);
4512 : }
4513 : }
4514 : }
4515 154931 : return result;
4516 : }
4517 : }
4518 :
4519 : void
4520 83580 : MSLane::initCollisionAction(const OptionsCont& oc, const std::string& option, CollisionAction& myAction) {
4521 83580 : const std::string action = oc.getString(option);
4522 83580 : if (action == "none") {
4523 15 : myAction = COLLISION_ACTION_NONE;
4524 83565 : } else if (action == "warn") {
4525 42432 : myAction = COLLISION_ACTION_WARN;
4526 41133 : } else if (action == "teleport") {
4527 41098 : myAction = COLLISION_ACTION_TELEPORT;
4528 35 : } else if (action == "remove") {
4529 35 : myAction = COLLISION_ACTION_REMOVE;
4530 : } else {
4531 0 : WRITE_ERROR(TLF("Invalid % '%'.", option, action));
4532 : }
4533 83580 : }
4534 :
4535 : void
4536 41790 : MSLane::initCollisionOptions(const OptionsCont& oc) {
4537 41790 : initCollisionAction(oc, "collision.action", myCollisionAction);
4538 41790 : initCollisionAction(oc, "intermodal-collision.action", myIntermodalCollisionAction);
4539 41790 : myCheckJunctionCollisions = oc.getBool("collision.check-junctions");
4540 41790 : myCheckJunctionCollisionMinGap = oc.getFloat("collision.check-junctions.mingap");
4541 41790 : myCollisionStopTime = string2time(oc.getString("collision.stoptime"));
4542 41790 : myIntermodalCollisionStopTime = string2time(oc.getString("intermodal-collision.stoptime"));
4543 41790 : myCollisionMinGapFactor = oc.getFloat("collision.mingap-factor");
4544 41790 : myExtrapolateSubstepDepart = oc.getBool("extrapolate-departpos");
4545 41790 : }
4546 :
4547 :
4548 : void
4549 1263 : MSLane::setPermissions(SVCPermissions permissions, long long transientID) {
4550 1263 : if (transientID == CHANGE_PERMISSIONS_PERMANENT) {
4551 83 : myPermissions = permissions;
4552 83 : myOriginalPermissions = permissions;
4553 : } else {
4554 1180 : myPermissionChanges[transientID] = permissions;
4555 1180 : resetPermissions(CHANGE_PERMISSIONS_PERMANENT);
4556 : }
4557 1263 : }
4558 :
4559 :
4560 : void
4561 1442 : MSLane::resetPermissions(long long transientID) {
4562 : myPermissionChanges.erase(transientID);
4563 1442 : if (myPermissionChanges.empty()) {
4564 248 : myPermissions = myOriginalPermissions;
4565 : } else {
4566 : // combine all permission changes
4567 1194 : myPermissions = SVCAll;
4568 2402 : for (const auto& item : myPermissionChanges) {
4569 1208 : myPermissions &= item.second;
4570 : }
4571 : }
4572 1442 : }
4573 :
4574 :
4575 : bool
4576 11962784 : MSLane::hadPermissionChanges() const {
4577 11962784 : return !myPermissionChanges.empty();
4578 : }
4579 :
4580 :
4581 : void
4582 9 : MSLane::setChangeLeft(SVCPermissions permissions) {
4583 9 : myChangeLeft = permissions;
4584 9 : }
4585 :
4586 :
4587 : void
4588 9 : MSLane::setChangeRight(SVCPermissions permissions) {
4589 9 : myChangeRight = permissions;
4590 9 : }
4591 :
4592 :
4593 : bool
4594 51951689 : MSLane::hasPedestrians() const {
4595 51951689 : MSNet* const net = MSNet::getInstance();
4596 51951689 : return net->hasPersons() && net->getPersonControl().getMovementModel()->hasPedestrians(this);
4597 : }
4598 :
4599 :
4600 : PersonDist
4601 698234 : MSLane::nextBlocking(double minPos, double minRight, double maxLeft, double stopTime, bool bidi) const {
4602 698234 : return MSNet::getInstance()->getPersonControl().getMovementModel()->nextBlocking(this, minPos, minRight, maxLeft, stopTime, bidi);
4603 : }
4604 :
4605 :
4606 : bool
4607 3879350 : MSLane::checkForPedestrians(const MSVehicle* aVehicle, double& speed, double& dist, double pos, bool patchSpeed) const {
4608 3879350 : if (getEdge().getPersons().size() > 0 && hasPedestrians()) {
4609 : #ifdef DEBUG_INSERTION
4610 : if (DEBUG_COND2(aVehicle)) {
4611 : std::cout << SIMTIME << " check for pedestrians on lane=" << getID() << " pos=" << pos << "\n";
4612 : }
4613 : #endif
4614 3800 : PersonDist leader = nextBlocking(pos - aVehicle->getVehicleType().getLength(),
4615 1900 : aVehicle->getRightSideOnLane(), aVehicle->getRightSideOnLane() + aVehicle->getVehicleType().getWidth(), ceil(speed / aVehicle->getCarFollowModel().getMaxDecel()));
4616 1900 : if (leader.first != 0) {
4617 769 : const double gap = leader.second - aVehicle->getVehicleType().getLengthWithGap();
4618 769 : const double stopSpeed = aVehicle->getCarFollowModel().stopSpeed(aVehicle, speed, gap, MSCFModel::CalcReason::FUTURE);
4619 284 : if ((gap < 0 && (aVehicle->getInsertionChecks() & ((int)InsertionCheck::COLLISION | (int)InsertionCheck::PEDESTRIAN)) != 0)
4620 1538 : || checkFailure(aVehicle, speed, dist, stopSpeed, patchSpeed, "", InsertionCheck::PEDESTRIAN)) {
4621 : // we may not drive with the given velocity - we crash into the pedestrian
4622 : #ifdef DEBUG_INSERTION
4623 : if (DEBUG_COND2(aVehicle)) std::cout << SIMTIME
4624 : << " isInsertionSuccess lane=" << getID()
4625 : << " veh=" << aVehicle->getID()
4626 : << " pos=" << pos
4627 : << " posLat=" << aVehicle->getLateralPositionOnLane()
4628 : << " patchSpeed=" << patchSpeed
4629 : << " speed=" << speed
4630 : << " stopSpeed=" << stopSpeed
4631 : << " pedestrianLeader=" << leader.first->getID()
4632 : << " failed (@796)!\n";
4633 : #endif
4634 498 : return false;
4635 : }
4636 : }
4637 : }
4638 3878852 : double backLength = aVehicle->getLength() - pos;
4639 3878852 : if (backLength > 0 && MSNet::getInstance()->hasPersons()) {
4640 : // look upstream for pedestrian crossings
4641 6318 : const MSLane* prev = getLogicalPredecessorLane();
4642 : const MSLane* cur = this;
4643 96467 : while (backLength > 0 && prev != nullptr) {
4644 90181 : const MSLink* link = prev->getLinkTo(cur);
4645 90181 : if (link->hasFoeCrossing()) {
4646 1288 : for (const MSLane* foe : link->getFoeLanes()) {
4647 1031 : if (foe->isCrossing() && (foe->hasPedestrians() ||
4648 372 : (foe->getIncomingLanes()[0].viaLink->getApproachingPersons() != nullptr
4649 53 : && foe->getIncomingLanes()[0].viaLink->getApproachingPersons()->size() > 0))) {
4650 : #ifdef DEBUG_INSERTION
4651 : if (DEBUG_COND2(aVehicle)) std::cout << SIMTIME
4652 : << " isInsertionSuccess lane=" << getID()
4653 : << " veh=" << aVehicle->getID()
4654 : << " pos=" << pos
4655 : << " backCrossing=" << foe->getID()
4656 : << " peds=" << joinNamedToString(foe->getEdge().getPersons(), " ")
4657 : << " approaching=" << foe->getIncomingLanes()[0].viaLink->getApproachingPersons()->size()
4658 : << " failed (@4550)!\n";
4659 : #endif
4660 : return false;
4661 : }
4662 : }
4663 : }
4664 90149 : backLength -= prev->getLength();
4665 : cur = prev;
4666 90149 : prev = prev->getLogicalPredecessorLane();
4667 : }
4668 : }
4669 : return true;
4670 : }
4671 :
4672 :
4673 : void
4674 41792 : MSLane::initRNGs(const OptionsCont& oc) {
4675 : myRNGs.clear();
4676 41792 : const int numRNGs = oc.getInt("thread-rngs");
4677 41792 : const bool random = oc.getBool("random");
4678 41792 : int seed = oc.getInt("seed");
4679 41792 : myRNGs.reserve(numRNGs); // this is needed for stable pointers on debugging
4680 2716480 : for (int i = 0; i < numRNGs; i++) {
4681 5349376 : myRNGs.push_back(SumoRNG("lanes_" + toString(i)));
4682 2674688 : RandHelper::initRand(&myRNGs.back(), random, seed++);
4683 : }
4684 41792 : }
4685 :
4686 : void
4687 23 : MSLane::saveRNGStates(OutputDevice& out) {
4688 1495 : for (int i = 0; i < getNumRNGs(); i++) {
4689 1472 : out.openTag(SUMO_TAG_RNGLANE);
4690 1472 : out.writeAttr(SUMO_ATTR_INDEX, i);
4691 1472 : out.writeAttr(SUMO_ATTR_STATE, RandHelper::saveState(&myRNGs[i]));
4692 2944 : out.closeTag();
4693 : }
4694 23 : }
4695 :
4696 : void
4697 1472 : MSLane::loadRNGState(int index, const std::string& state) {
4698 1472 : if (index >= getNumRNGs()) {
4699 0 : throw ProcessError(TLF("State was saved with more than % threads. Change the number of threads or do not load RNG state", toString(getNumRNGs())));
4700 : }
4701 1472 : RandHelper::loadState(state, &myRNGs[index]);
4702 1472 : }
4703 :
4704 :
4705 : MSLane*
4706 17415876992 : MSLane::getBidiLane() const {
4707 17415876992 : return myBidiLane;
4708 : }
4709 :
4710 :
4711 : bool
4712 121354527 : MSLane::mustCheckJunctionCollisions() const {
4713 121354527 : return myCheckJunctionCollisions && myEdge->isInternal() && (
4714 1418632 : myLinks.front()->getFoeLanes().size() > 0
4715 8878 : || myLinks.front()->getWalkingAreaFoe() != nullptr
4716 7970 : || myLinks.front()->getWalkingAreaFoeExit() != nullptr);
4717 : }
4718 :
4719 :
4720 : double
4721 607744282 : MSLane::getSpaceTillLastStanding(const MSVehicle* ego, bool& foundStopped) const {
4722 : /// @todo if ego isn't on this lane, we could use a cached value
4723 : double lengths = 0;
4724 4972764588 : for (const MSVehicle* last : myVehicles) {
4725 4419446849 : if (last->getSpeed() < SUMO_const_haltingSpeed && !last->getLane()->getEdge().isRoundabout()
4726 26618435 : && last != ego
4727 : // @todo recheck
4728 4418231562 : && last->isFrontOnLane(this)) {
4729 26605628 : foundStopped = true;
4730 26605628 : const double lastBrakeGap = last->getCarFollowModel().brakeGap(last->getSpeed());
4731 26605628 : const double ret = last->getBackPositionOnLane() + lastBrakeGap - lengths;
4732 : return ret;
4733 : }
4734 4365020306 : if (MSGlobals::gSublane && ego->getVehicleType().getWidth() + last->getVehicleType().getWidth() < getWidth()) {
4735 32016664 : lengths += last->getVehicleType().getLengthWithGap() * (last->getVehicleType().getWidth() + last->getVehicleType().getMinGapLat()) / getWidth();
4736 : } else {
4737 4333003642 : lengths += last->getVehicleType().getLengthWithGap();
4738 : }
4739 : }
4740 581138654 : return getLength() - lengths;
4741 : }
4742 :
4743 :
4744 : bool
4745 495050398 : MSLane::allowsVehicleClass(SUMOVehicleClass vclass, int routingMode) const {
4746 495050398 : return (((routingMode & libsumo::ROUTING_MODE_IGNORE_TRANSIENT_PERMISSIONS) ? myOriginalPermissions : myPermissions) & vclass) == vclass;
4747 : }
4748 :
4749 :
4750 : const MSJunction*
4751 297576 : MSLane::getFromJunction() const {
4752 297576 : return myEdge->getFromJunction();
4753 : }
4754 :
4755 :
4756 : const MSJunction*
4757 396452 : MSLane::getToJunction() const {
4758 396452 : return myEdge->getToJunction();
4759 : }
4760 :
4761 : /****************************************************************************/
|
