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