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