Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2025 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file MSLane.cpp
15 : /// @author Christian Roessel
16 : /// @author Jakob Erdmann
17 : /// @author Daniel Krajzewicz
18 : /// @author Tino Morenz
19 : /// @author Axel Wegener
20 : /// @author Michael Behrisch
21 : /// @author Christoph Sommer
22 : /// @author Mario Krumnow
23 : /// @author Leonhard Luecken
24 : /// @author Mirko Barthauer
25 : /// @date Mon, 05 Mar 2001
26 : ///
27 : // Representation of a lane in the micro simulation
28 : /****************************************************************************/
29 : #include <config.h>
30 :
31 : #include <cmath>
32 : #include <bitset>
33 : #include <iostream>
34 : #include <cassert>
35 : #include <functional>
36 : #include <algorithm>
37 : #include <iterator>
38 : #include <exception>
39 : #include <climits>
40 : #include <set>
41 : #include <utils/common/UtilExceptions.h>
42 : #include <utils/common/StdDefs.h>
43 : #include <utils/common/MsgHandler.h>
44 : #include <utils/common/ToString.h>
45 : #ifdef HAVE_FOX
46 : #include <utils/common/ScopedLocker.h>
47 : #endif
48 : #include <utils/options/OptionsCont.h>
49 : #include <utils/emissions/HelpersHarmonoise.h>
50 : #include <utils/geom/GeomHelper.h>
51 : #include <libsumo/TraCIConstants.h>
52 : #include <microsim/transportables/MSPModel.h>
53 : #include <microsim/transportables/MSTransportableControl.h>
54 : #include <microsim/traffic_lights/MSRailSignal.h>
55 : #include <microsim/traffic_lights/MSRailSignalControl.h>
56 : #include <microsim/traffic_lights/MSDriveWay.h>
57 : #include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
58 : #include <microsim/devices/MSDevice_Taxi.h>
59 : #include <mesosim/MELoop.h>
60 : #include "MSNet.h"
61 : #include "MSVehicleType.h"
62 : #include "MSEdge.h"
63 : #include "MSEdgeControl.h"
64 : #include "MSJunction.h"
65 : #include "MSLogicJunction.h"
66 : #include "MSLink.h"
67 : #include "MSLane.h"
68 : #include "MSVehicleTransfer.h"
69 : #include "MSGlobals.h"
70 : #include "MSVehicleControl.h"
71 : #include "MSInsertionControl.h"
72 : #include "MSVehicleControl.h"
73 : #include "MSLeaderInfo.h"
74 : #include "MSVehicle.h"
75 : #include "MSStop.h"
76 :
77 : //#define DEBUG_INSERTION
78 : //#define DEBUG_PLAN_MOVE
79 : //#define DEBUG_EXEC_MOVE
80 : //#define DEBUG_CONTEXT
81 : //#define DEBUG_PARTIALS
82 : //#define DEBUG_OPPOSITE
83 : //#define DEBUG_VEHICLE_CONTAINER
84 : //#define DEBUG_COLLISIONS
85 : //#define DEBUG_JUNCTION_COLLISIONS
86 : //#define DEBUG_PEDESTRIAN_COLLISIONS
87 : //#define DEBUG_LANE_SORTER
88 : //#define DEBUG_NO_CONNECTION
89 : //#define DEBUG_SURROUNDING
90 : //#define DEBUG_EXTRAPOLATE_DEPARTPOS
91 : //#define DEBUG_ITERATOR
92 :
93 : //#define DEBUG_COND (false)
94 : //#define DEBUG_COND (true)
95 : #define DEBUG_COND (isSelected())
96 : #define DEBUG_COND2(obj) ((obj != nullptr && (obj)->isSelected()))
97 : //#define DEBUG_COND (getID() == "ego")
98 : //#define DEBUG_COND2(obj) ((obj != 0 && (obj)->getID() == "ego"))
99 : //#define DEBUG_COND2(obj) (true)
100 :
101 :
102 : // ===========================================================================
103 : // static member definitions
104 : // ===========================================================================
105 : MSLane::DictType MSLane::myDict;
106 : MSLane::CollisionAction MSLane::myCollisionAction(MSLane::COLLISION_ACTION_TELEPORT);
107 : MSLane::CollisionAction MSLane::myIntermodalCollisionAction(MSLane::COLLISION_ACTION_WARN);
108 : bool MSLane::myCheckJunctionCollisions(false);
109 : double MSLane::myCheckJunctionCollisionMinGap(0);
110 : SUMOTime MSLane::myCollisionStopTime(0);
111 : SUMOTime MSLane::myIntermodalCollisionStopTime(0);
112 : double MSLane::myCollisionMinGapFactor(1.0);
113 : bool MSLane::myExtrapolateSubstepDepart(false);
114 : std::vector<SumoRNG> MSLane::myRNGs;
115 : DepartSpeedDefinition MSLane::myDefaultDepartSpeedDefinition(DepartSpeedDefinition::DEFAULT);
116 : double MSLane::myDefaultDepartSpeed(0);
117 :
118 :
119 : // ===========================================================================
120 : // internal class method definitions
121 : // ===========================================================================
122 : void
123 1691853 : MSLane::StoringVisitor::add(const MSLane* const l) const {
124 1691853 : switch (myDomain) {
125 681834 : case libsumo::CMD_GET_VEHICLE_VARIABLE: {
126 1365906 : for (const MSVehicle* veh : l->getVehiclesSecure()) {
127 684072 : if (myShape.distance2D(veh->getPosition()) <= myRange) {
128 339526 : myObjects.insert(veh);
129 : }
130 : }
131 682009 : for (const MSBaseVehicle* veh : l->getParkingVehicles()) {
132 175 : if (myShape.distance2D(veh->getPosition()) <= myRange) {
133 35 : myObjects.insert(veh);
134 : }
135 : }
136 681834 : l->releaseVehicles();
137 : }
138 681834 : break;
139 674026 : case libsumo::CMD_GET_PERSON_VARIABLE: {
140 674026 : l->getVehiclesSecure();
141 674026 : std::vector<MSTransportable*> persons = l->getEdge().getSortedPersons(MSNet::getInstance()->getCurrentTimeStep(), true);
142 771441 : for (auto p : persons) {
143 97415 : if (myShape.distance2D(p->getPosition()) <= myRange) {
144 50854 : myObjects.insert(p);
145 : }
146 : }
147 674026 : l->releaseVehicles();
148 674026 : }
149 674026 : break;
150 268998 : case libsumo::CMD_GET_EDGE_VARIABLE: {
151 268998 : if (myShape.size() != 1 || l->getShape().distance2D(myShape[0]) <= myRange) {
152 252210 : myObjects.insert(&l->getEdge());
153 : }
154 : }
155 : break;
156 66995 : case libsumo::CMD_GET_LANE_VARIABLE: {
157 66995 : if (myShape.size() != 1 || l->getShape().distance2D(myShape[0]) <= myRange) {
158 66405 : myObjects.insert(l);
159 : }
160 : }
161 : break;
162 : default:
163 : break;
164 :
165 : }
166 1691853 : }
167 :
168 :
169 : MSLane::AnyVehicleIterator&
170 12078342773 : MSLane::AnyVehicleIterator::operator++() {
171 12078342773 : if (nextIsMyVehicles()) {
172 11702889531 : if (myI1 != myI1End) {
173 8588488175 : myI1 += myDirection;
174 3114401356 : } else if (myI3 != myI3End) {
175 3114401356 : myI3 += myDirection;
176 : }
177 : // else: already at end
178 : } else {
179 375453242 : myI2 += myDirection;
180 : }
181 : //if (DEBUG_COND2(myLane)) std::cout << SIMTIME << " AnyVehicleIterator::operator++ lane=" << myLane->getID() << " myI1=" << myI1 << " myI2=" << myI2 << "\n";
182 12078342773 : return *this;
183 : }
184 :
185 :
186 : const MSVehicle*
187 12790397057 : MSLane::AnyVehicleIterator::operator*() {
188 12790397057 : if (nextIsMyVehicles()) {
189 12407567295 : if (myI1 != myI1End) {
190 8731279423 : return myLane->myVehicles[myI1];
191 3676287872 : } else if (myI3 != myI3End) {
192 3193420654 : return myLane->myTmpVehicles[myI3];
193 : } else {
194 : assert(myI2 == myI2End);
195 : return nullptr;
196 : }
197 : } else {
198 382829762 : return myLane->myPartialVehicles[myI2];
199 : }
200 : }
201 :
202 :
203 : bool
204 24868739830 : MSLane::AnyVehicleIterator::nextIsMyVehicles() const {
205 : #ifdef DEBUG_ITERATOR
206 : if (DEBUG_COND2(myLane)) std::cout << SIMTIME << " AnyVehicleIterator::nextIsMyVehicles lane=" << myLane->getID()
207 : << " myI1=" << myI1
208 : << " myI1End=" << myI1End
209 : << " myI2=" << myI2
210 : << " myI2End=" << myI2End
211 : << " myI3=" << myI3
212 : << " myI3End=" << myI3End
213 : << "\n";
214 : #endif
215 24868739830 : if (myI1 == myI1End && myI3 == myI3End) {
216 720614820 : if (myI2 != myI2End) {
217 : return false;
218 : } else {
219 482867218 : return true; // @note. must be caught
220 : }
221 : } else {
222 24148125010 : if (myI2 == myI2End) {
223 : return true;
224 : } else {
225 6875586707 : MSVehicle* cand = myI1 == myI1End ? myLane->myTmpVehicles[myI3] : myLane->myVehicles[myI1];
226 : #ifdef DEBUG_ITERATOR
227 : if (DEBUG_COND2(myLane)) std::cout << " "
228 : << " veh1=" << cand->getID()
229 : << " isTmp=" << (myI1 == myI1End)
230 : << " veh2=" << myLane->myPartialVehicles[myI2]->getID()
231 : << " pos1=" << cand->getPositionOnLane(myLane)
232 : << " pos2=" << myLane->myPartialVehicles[myI2]->getPositionOnLane(myLane)
233 : << "\n";
234 : #endif
235 6875586707 : if (cand->getPositionOnLane() < myLane->myPartialVehicles[myI2]->getPositionOnLane(myLane)) {
236 6397202894 : return myDownstream;
237 : } else {
238 478383813 : return !myDownstream;
239 : }
240 : }
241 : }
242 : }
243 :
244 :
245 : // ===========================================================================
246 : // member method definitions
247 : // ===========================================================================
248 : #ifdef _MSC_VER
249 : #pragma warning(push)
250 : #pragma warning(disable: 4355) // mask warning about "this" in initializers
251 : #endif
252 2005842 : MSLane::MSLane(const std::string& id, double maxSpeed, double friction, double length, MSEdge* const edge,
253 : int numericalID, const PositionVector& shape, double width,
254 : SVCPermissions permissions,
255 : SVCPermissions changeLeft, SVCPermissions changeRight,
256 : int index, bool isRampAccel,
257 : const std::string& type,
258 2005842 : const PositionVector& outlineShape) :
259 : Named(id),
260 4011684 : myNumericalID(numericalID), myShape(shape), myIndex(index),
261 2005842 : myVehicles(), myLength(length), myWidth(width),
262 2005842 : myEdge(edge), myMaxSpeed(maxSpeed),
263 2005842 : myFrictionCoefficient(friction),
264 2005842 : mySpeedByVSS(false),
265 2005842 : mySpeedByTraCI(false),
266 2005842 : myPermissions(permissions),
267 2005842 : myChangeLeft(changeLeft),
268 2005842 : myChangeRight(changeRight),
269 2005842 : myOriginalPermissions(permissions),
270 2005842 : myLogicalPredecessorLane(nullptr),
271 2005842 : myCanonicalPredecessorLane(nullptr),
272 2005842 : myCanonicalSuccessorLane(nullptr),
273 2005842 : myBruttoVehicleLengthSum(0), myNettoVehicleLengthSum(0),
274 2005842 : myBruttoVehicleLengthSumToRemove(0), myNettoVehicleLengthSumToRemove(0),
275 2005842 : myRecalculateBruttoSum(false),
276 2005842 : myLeaderInfo(width, nullptr, 0.),
277 2005842 : myFollowerInfo(width, nullptr, 0.),
278 2005842 : myLeaderInfoTime(SUMOTime_MIN),
279 2005842 : myFollowerInfoTime(SUMOTime_MIN),
280 2005842 : myLengthGeometryFactor(MAX2(POSITION_EPS, myShape.length()) / myLength), // factor should not be 0
281 2005842 : myIsRampAccel(isRampAccel),
282 2005842 : myLaneType(type),
283 2005842 : myRightSideOnEdge(0), // initialized in MSEdge::initialize
284 2005842 : myRightmostSublane(0),
285 2005842 : myNeedsCollisionCheck(false),
286 2005842 : myOpposite(nullptr),
287 2005842 : myBidiLane(nullptr),
288 : #ifdef HAVE_FOX
289 : mySimulationTask(*this, 0),
290 : #endif
291 4011684 : myStopWatch(3) {
292 : // initialized in MSEdge::initialize
293 2005842 : initRestrictions();// may be reloaded again from initialized in MSEdge::closeBuilding
294 : assert(myRNGs.size() > 0);
295 2005842 : myRNGIndex = numericalID % myRNGs.size();
296 2005842 : if (outlineShape.size() > 0) {
297 23284 : myOutlineShape = new PositionVector(outlineShape);
298 : }
299 2005842 : }
300 : #ifdef _MSC_VER
301 : #pragma warning(pop)
302 : #endif
303 :
304 :
305 5569261 : MSLane::~MSLane() {
306 4539696 : for (MSLink* const l : myLinks) {
307 2552905 : delete l;
308 : }
309 1986791 : delete myOutlineShape;
310 11529634 : }
311 :
312 :
313 : void
314 2008506 : MSLane::initRestrictions() {
315 : // simplify unit testing without MSNet instance
316 2008506 : myRestrictions = MSGlobals::gUnitTests ? nullptr : MSNet::getInstance()->getRestrictions(myEdge->getEdgeType());
317 2008506 : }
318 :
319 :
320 : void
321 2002852 : MSLane::checkBufferType() {
322 2002852 : 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 2002852 : }
330 :
331 :
332 : void
333 2578038 : MSLane::addLink(MSLink* link) {
334 2578038 : myLinks.push_back(link);
335 2578038 : }
336 :
337 :
338 : void
339 8004 : MSLane::setOpposite(MSLane* oppositeLane) {
340 8004 : myOpposite = oppositeLane;
341 8004 : 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 8004 : }
345 :
346 : void
347 28960 : MSLane::setBidiLane(MSLane* bidiLane) {
348 28960 : myBidiLane = bidiLane;
349 28960 : 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 28960 : }
355 :
356 :
357 :
358 : // ------ interaction with MSMoveReminder ------
359 : void
360 2600541 : MSLane::addMoveReminder(MSMoveReminder* rem, bool addToVehicles) {
361 2600541 : myMoveReminders.push_back(rem);
362 2600541 : if (addToVehicles) {
363 2608837 : for (MSVehicle* const veh : myVehicles) {
364 15980 : veh->addReminder(rem);
365 : }
366 : }
367 : // XXX: Here, the partial occupators are ignored!? Refs. #3255
368 2600541 : }
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 17558757 : MSLane::setPartialOccupation(MSVehicle* v) {
385 : // multithreading: there are concurrent writes to myNeedsCollisionCheck but all of them should set it to true
386 17558757 : 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 17558757 : ScopedLocker<> lock(myPartialOccupatorMutex, MSGlobals::gNumSimThreads > 1);
395 : #endif
396 : //assert(std::find(myPartialVehicles.begin(), myPartialVehicles.end(), v) == myPartialVehicles.end());
397 17558757 : myPartialVehicles.push_back(v);
398 20021830 : return myLength;
399 : }
400 :
401 :
402 : void
403 17558807 : MSLane::resetPartialOccupation(MSVehicle* v) {
404 : #ifdef HAVE_FOX
405 17558807 : 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 18766665 : for (VehCont::iterator i = myPartialVehicles.begin(); i != myPartialVehicles.end(); ++i) {
413 18766595 : if (v == *i) {
414 17558737 : 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 221655 : 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 221655 : myManeuverReservations.push_back(v);
433 221655 : }
434 :
435 :
436 : void
437 221655 : 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 273789 : for (VehCont::iterator i = myManeuverReservations.begin(); i != myManeuverReservations.end(); ++i) {
444 273789 : if (v == *i) {
445 221655 : myManeuverReservations.erase(i);
446 : return;
447 : }
448 : }
449 : assert(false);
450 : }
451 :
452 :
453 : // ------ Vehicle emission ------
454 : void
455 3618849 : MSLane::incorporateVehicle(MSVehicle* veh, double pos, double speed, double posLat, const MSLane::VehCont::iterator& at, MSMoveReminder::Notification notification) {
456 3618849 : myNeedsCollisionCheck = true;
457 : assert(pos <= myLength);
458 : bool wasInactive = myVehicles.size() == 0;
459 3618849 : veh->enterLaneAtInsertion(this, pos, speed, posLat, notification);
460 3618847 : if (at == myVehicles.end()) {
461 : // vehicle will be the first on the lane
462 649058 : myVehicles.push_back(veh);
463 : } else {
464 2969789 : myVehicles.insert(at, veh);
465 : }
466 3618847 : myBruttoVehicleLengthSum += veh->getVehicleType().getLengthWithGap();
467 3618847 : myNettoVehicleLengthSum += veh->getVehicleType().getLength();
468 3618847 : myEdge->markDelayed();
469 3618847 : if (wasInactive) {
470 631479 : MSNet::getInstance()->getEdgeControl().gotActive(this);
471 : }
472 3618847 : 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 539 : getBidiLane()->setPartialOccupation(veh);
475 : }
476 3618847 : }
477 :
478 :
479 : bool
480 765630 : MSLane::lastInsertion(MSVehicle& veh, double mspeed, double posLat, bool patchSpeed) {
481 765630 : double pos = getLength() - POSITION_EPS;
482 765630 : MSVehicle* leader = getLastAnyVehicle();
483 : // back position of leader relative to this lane
484 : double leaderBack;
485 765630 : if (leader == nullptr) {
486 : /// look for a leaders on consecutive lanes
487 8714 : veh.setTentativeLaneAndPosition(this, pos, posLat);
488 8714 : veh.updateBestLanes(false, this);
489 8714 : std::pair<MSVehicle* const, double> leaderInfo = getLeader(&veh, pos, veh.getBestLanesContinuation(), veh.getCarFollowModel().brakeGap(mspeed));
490 8714 : leader = leaderInfo.first;
491 8714 : leaderBack = pos + leaderInfo.second + veh.getVehicleType().getMinGap();
492 : } else {
493 756916 : leaderBack = leader->getBackPositionOnLane(this);
494 : //std::cout << " leaderPos=" << leader->getPositionOnLane(this) << " leaderBack=" << leader->getBackPositionOnLane(this) << " leaderLane=" << leader->getLane()->getID() << "\n";
495 : }
496 8714 : if (leader == nullptr) {
497 : // insert at the end of this lane
498 2657 : return isInsertionSuccess(&veh, mspeed, pos, posLat, patchSpeed, MSMoveReminder::NOTIFICATION_DEPARTED);
499 : } else {
500 : // try to insert behind the leader
501 762973 : const double frontGapNeeded = veh.getCarFollowModel().getSecureGap(&veh, leader, mspeed, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel()) + veh.getVehicleType().getMinGap() + POSITION_EPS;
502 762973 : if (leaderBack >= frontGapNeeded) {
503 422848 : pos = MIN2(pos, leaderBack - frontGapNeeded);
504 422848 : 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 422848 : 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 565822 : MSLane::freeInsertion(MSVehicle& veh, double mspeed, double posLat,
516 : MSMoveReminder::Notification notification) {
517 : // try to insert teleporting vehicles fully on this lane
518 565822 : double maxPos = myLength;
519 565822 : if (veh.hasStops() && veh.getNextStop().edge == veh.getCurrentRouteEdge()) {
520 7461 : maxPos = MAX2(0.0, veh.getNextStop().getEndPos(veh));
521 : }
522 565822 : const double minPos = (notification == MSMoveReminder::NOTIFICATION_TELEPORT ?
523 285547 : MIN2(maxPos, veh.getVehicleType().getLength()) : 0);
524 565822 : veh.setTentativeLaneAndPosition(this, minPos, 0);
525 565822 : if (myVehicles.size() == 0) {
526 : // ensure sufficient gap to followers on predecessor lanes
527 12871 : const double backOffset = minPos - veh.getVehicleType().getLength();
528 12871 : const double missingRearGap = getMissingRearGap(&veh, backOffset, mspeed);
529 12871 : if (missingRearGap > 0) {
530 1349 : 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 764 : return isInsertionSuccess(&veh, mspeed, minPos + missingRearGap, posLat, true, notification);
538 : }
539 : return false;
540 : } else {
541 11522 : 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 552951 : const MSVehicle* const leader = myVehicles.back(); // @todo reproduction of bogus old behavior. see #1961
547 552951 : const double leaderPos = leader->getBackPositionOnLane(this);
548 552951 : const double speed = leader->getSpeed();
549 552951 : const double frontGapNeeded = veh.getCarFollowModel().getSecureGap(&veh, leader, speed, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel()) + veh.getVehicleType().getMinGap();
550 552951 : if (leaderPos >= frontGapNeeded) {
551 539318 : 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 539318 : 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 12647410 : while (predIt != myVehicles.end()) {
562 : // get leader (may be zero) and follower
563 : // @todo compute secure position in regard to sublane-model
564 12224320 : const MSVehicle* leader = predIt != myVehicles.end() - 1 ? *(predIt + 1) : nullptr;
565 12224320 : if (leader == nullptr && myPartialVehicles.size() > 0) {
566 69563 : leader = myPartialVehicles.front();
567 : }
568 12224320 : const MSVehicle* follower = *predIt;
569 :
570 : // patch speed if allowed
571 : double speed = mspeed;
572 12224320 : if (leader != nullptr) {
573 11867511 : 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 11867511 : double leaderRearPos = leader->getBackPositionOnLane(this);
580 11867511 : double frontGapNeeded = veh.getCarFollowModel().getSecureGap(&veh, leader, speed, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel()) + veh.getVehicleType().getMinGap();
581 11867511 : frontMax = MIN2(maxPos, leaderRearPos - frontGapNeeded);
582 : }
583 : // compute the space needed to not let the follower collide
584 12224320 : const double followPos = follower->getPositionOnLane() + follower->getVehicleType().getMinGap();
585 12224320 : const double backGapNeeded = follower->getCarFollowModel().getSecureGap(follower, &veh, follower->getSpeed(), veh.getSpeed(), veh.getCarFollowModel().getMaxDecel());
586 12224320 : const double backMin = followPos + backGapNeeded + veh.getVehicleType().getLength();
587 :
588 : // check whether there is enough room (given some extra space for rounding errors)
589 12224320 : if (frontMax > minPos && backMin + POSITION_EPS < frontMax) {
590 : // try to insert vehicle (should be always ok)
591 24498 : 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 14086604 : MSLane::getDepartSpeed(const MSVehicle& veh, bool& patchSpeed) {
606 : double speed = 0;
607 14086604 : const SUMOVehicleParameter& pars = veh.getParameter();
608 14086604 : DepartSpeedDefinition dsd = pars.departSpeedProcedure;
609 14086604 : if (dsd == DepartSpeedDefinition::DEFAULT) {
610 7656866 : dsd = myDefaultDepartSpeedDefinition;
611 7656866 : if (dsd == DepartSpeedDefinition::GIVEN) {
612 7655966 : speed = myDefaultDepartSpeed;
613 : }
614 6429738 : } else if (dsd == DepartSpeedDefinition::GIVEN) {
615 1418888 : speed = pars.departSpeed;;
616 : }
617 14086604 : switch (dsd) {
618 9074854 : case DepartSpeedDefinition::GIVEN:
619 9074854 : patchSpeed = false;
620 9074854 : break;
621 53031 : case DepartSpeedDefinition::RANDOM:
622 106062 : speed = roundDecimal(RandHelper::rand(getVehicleMaxSpeed(&veh)), gPrecisionRandom);
623 53031 : patchSpeed = true;
624 53031 : break;
625 2860580 : case DepartSpeedDefinition::MAX:
626 2860580 : speed = getVehicleMaxSpeed(&veh);
627 2860580 : patchSpeed = true;
628 2860580 : break;
629 360571 : case DepartSpeedDefinition::DESIRED:
630 360571 : speed = getVehicleMaxSpeed(&veh);
631 360571 : patchSpeed = false;
632 360571 : break;
633 179292 : case DepartSpeedDefinition::LIMIT:
634 179292 : speed = getVehicleMaxSpeed(&veh) / veh.getChosenSpeedFactor();
635 179292 : patchSpeed = false;
636 179292 : break;
637 8167 : case DepartSpeedDefinition::LAST: {
638 8167 : MSVehicle* last = getLastAnyVehicle();
639 8167 : speed = getVehicleMaxSpeed(&veh);
640 8167 : if (last != nullptr) {
641 7840 : speed = MIN2(speed, last->getSpeed());
642 7840 : patchSpeed = false;
643 : }
644 : break;
645 : }
646 1550109 : case DepartSpeedDefinition::AVG: {
647 1550109 : speed = MIN2(getVehicleMaxSpeed(&veh), getMeanSpeed());
648 1550109 : if (getLastAnyVehicle() != nullptr) {
649 1540522 : 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 14086604 : return speed;
660 : }
661 :
662 :
663 : double
664 14583893 : MSLane::getDepartPosLat(const MSVehicle& veh) {
665 14583893 : const SUMOVehicleParameter& pars = veh.getParameter();
666 14583893 : switch (pars.departPosLatProcedure) {
667 220173 : case DepartPosLatDefinition::GIVEN:
668 220173 : return pars.departPosLat;
669 : case DepartPosLatDefinition::RIGHT:
670 39911 : return -getWidth() * 0.5 + veh.getVehicleType().getWidth() * 0.5;
671 : case DepartPosLatDefinition::LEFT:
672 39727 : return getWidth() * 0.5 - veh.getVehicleType().getWidth() * 0.5;
673 : case DepartPosLatDefinition::RANDOM: {
674 247134 : const double raw = RandHelper::rand(getWidth() - veh.getVehicleType().getWidth()) - getWidth() * 0.5 + veh.getVehicleType().getWidth() * 0.5;
675 247134 : 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 14086534 : MSLane::insertVehicle(MSVehicle& veh) {
691 : double pos = 0;
692 14086534 : bool patchSpeed = true; // whether the speed shall be adapted to infrastructure/traffic in front
693 14086534 : const SUMOVehicleParameter& pars = veh.getParameter();
694 14086534 : double speed = getDepartSpeed(veh, patchSpeed);
695 14086534 : double posLat = getDepartPosLat(veh);
696 :
697 : // determine the position
698 14086534 : switch (pars.departPosProcedure) {
699 1091040 : case DepartPosDefinition::GIVEN:
700 1091040 : pos = pars.departPos;
701 1091040 : if (pos < 0.) {
702 204482 : pos += myLength;
703 : }
704 : break;
705 237823 : case DepartPosDefinition::RANDOM:
706 237823 : pos = roundDecimal(RandHelper::rand(getLength()), gPrecisionRandom);
707 : break;
708 : case DepartPosDefinition::RANDOM_FREE: {
709 543059 : for (int i = 0; i < 10; i++) {
710 : // we will try some random positions ...
711 : pos = RandHelper::rand(getLength());
712 497359 : posLat = getDepartPosLat(veh); // could be random as well
713 497359 : if (isInsertionSuccess(&veh, speed, pos, posLat, patchSpeed, MSMoveReminder::NOTIFICATION_DEPARTED)) {
714 13038 : MSNet::getInstance()->getInsertionControl().retractDescheduleDeparture(&veh);
715 13038 : return true;
716 : }
717 : }
718 : // ... and if that doesn't work, we put the vehicle to the free position
719 45700 : bool success = freeInsertion(veh, speed, posLat);
720 45700 : if (success) {
721 10971 : MSNet::getInstance()->getInsertionControl().retractDescheduleDeparture(&veh);
722 : }
723 : return success;
724 : }
725 234575 : case DepartPosDefinition::FREE:
726 234575 : return freeInsertion(veh, speed, posLat);
727 765630 : case DepartPosDefinition::LAST:
728 765630 : return lastInsertion(veh, speed, posLat, patchSpeed);
729 3731 : case DepartPosDefinition::STOP:
730 3731 : if (veh.hasStops() && veh.getNextStop().edge == veh.getCurrentRouteEdge()) {
731 : // getLastFreePos of stopping place could return negative position to avoid blocking the stop
732 3725 : 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 11695003 : 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 11694982 : pos = veh.basePos(myEdge);
757 : }
758 : break;
759 : }
760 : // determine the lateral position for special cases
761 13027591 : if (MSGlobals::gLateralResolution > 0) {
762 1374345 : 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 13021801 : 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 13021798 : if (success && myExtrapolateSubstepDepart && veh.getDepartDelay() > 0) {
797 219548 : SUMOTime relevantDelay = MIN2(DELTA_T, veh.getDepartDelay());
798 : // try to compensate sub-step depart delay by moving the vehicle forward
799 219548 : speed = veh.getSpeed(); // may have been adapted in isInsertionSuccess
800 219548 : double dist = speed * STEPS2TIME(relevantDelay);
801 219548 : std::pair<MSVehicle* const, double> leaderInfo = getLeader(&veh, pos, veh.getBestLanesContinuation());
802 219548 : if (leaderInfo.first != nullptr) {
803 : MSVehicle* leader = leaderInfo.first;
804 219299 : const double frontGapNeeded = veh.getCarFollowModel().getSecureGap(&veh, leader, speed, leader->getSpeed(),
805 : leader->getCarFollowModel().getMaxDecel());
806 219299 : dist = MIN2(dist, leaderInfo.second - frontGapNeeded);
807 : }
808 219548 : if (dist > 0) {
809 213956 : veh.executeFractionalMove(dist);
810 : }
811 : }
812 : return success;
813 : }
814 :
815 :
816 : bool
817 7212078 : MSLane::checkFailure(const MSVehicle* aVehicle, double& speed, double& dist, const double nspeed, const bool patchSpeed, const std::string errorMsg, InsertionCheck check) const {
818 7212078 : if (nspeed < speed) {
819 2575189 : if (patchSpeed) {
820 627297 : speed = MIN2(nspeed, speed);
821 627297 : dist = aVehicle->getCarFollowModel().brakeGap(speed) + aVehicle->getVehicleType().getMinGap();
822 1947892 : } else if (speed > 0) {
823 1947892 : if ((aVehicle->getInsertionChecks() & (int)check) == 0) {
824 : return false;
825 : }
826 1947852 : 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 1947805 : if (errorMsg != "") {
838 188 : WRITE_ERRORF(TL("Vehicle '%' will not be able to depart on lane '%' with speed % (%), time=%."),
839 : aVehicle->getID(), getID(), speed, errorMsg, time2string(SIMSTEP));
840 47 : MSNet::getInstance()->getInsertionControl().descheduleDeparture(aVehicle);
841 : }
842 1947805 : return true;
843 : }
844 : }
845 : return false;
846 : }
847 :
848 :
849 : bool
850 14656332 : MSLane::isInsertionSuccess(MSVehicle* aVehicle,
851 : double speed, double pos, double posLat, bool patchSpeed,
852 : MSMoveReminder::Notification notification) {
853 14656332 : int insertionChecks = aVehicle->getInsertionChecks();
854 14656332 : if (pos < 0 || pos > myLength) {
855 : // we may not start there
856 9081 : WRITE_WARNINGF(TL("Invalid departPos % given for vehicle '%', time=%. Inserting at lane end instead."),
857 : pos, aVehicle->getID(), time2string(SIMSTEP));
858 3027 : 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 14656332 : aVehicle->setTentativeLaneAndPosition(this, pos, posLat);
875 14656332 : aVehicle->updateBestLanes(false, this);
876 : const MSCFModel& cfModel = aVehicle->getCarFollowModel();
877 14656332 : const std::vector<MSLane*>& bestLaneConts = aVehicle->getBestLanesContinuation(this);
878 : std::vector<MSLane*>::const_iterator ri = bestLaneConts.begin();
879 14656332 : double seen = getLength() - pos; // == distance from insertion position until the end of the currentLane
880 14656332 : double dist = cfModel.brakeGap(speed) + aVehicle->getVehicleType().getMinGap();
881 14656332 : const bool isRail = aVehicle->isRail();
882 14656332 : if (isRail && insertionChecks != (int)InsertionCheck::NONE
883 51004 : && aVehicle->getParameter().departProcedure != DepartDefinition::SPLIT
884 50980 : && MSRailSignalControl::isSignalized(aVehicle->getVClass())
885 14707079 : && isRailwayOrShared(myPermissions)) {
886 50639 : const MSDriveWay* dw = MSDriveWay::getDepartureDriveway(aVehicle);
887 : MSEdgeVector occupied;
888 50639 : if (dw->foeDriveWayOccupied(false, aVehicle, occupied)) {
889 42510 : setParameter("insertionBlocked:" + aVehicle->getID(), dw->getID());
890 : #ifdef DEBUG_INSERTION
891 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
892 : std::cout << " foe of driveway " + dw->getID() + " has occupied edges " + toString(occupied) << "\n";
893 : }
894 : #endif
895 : return false;
896 : }
897 50639 : }
898 : // do not insert if the bidirectional edge is occupied
899 14613822 : if (getBidiLane() != nullptr && isRail && getBidiLane()->getVehicleNumberWithPartials() > 0) {
900 0 : if ((insertionChecks & (int)InsertionCheck::BIDI) != 0) {
901 : #ifdef DEBUG_INSERTION
902 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
903 : std::cout << " bidi-lane occupied\n";
904 : }
905 : #endif
906 : return false;
907 : }
908 : }
909 : MSLink* firstRailSignal = nullptr;
910 : double firstRailSignalDist = -1;
911 :
912 : // before looping through the continuation lanes, check if a stop is scheduled on this lane
913 : // (the code is duplicated in the loop)
914 14613822 : if (aVehicle->hasStops()) {
915 359975 : const MSStop& nextStop = aVehicle->getNextStop();
916 359975 : if (nextStop.lane == this) {
917 146321 : std::stringstream msg;
918 : double distToStop, safeSpeed;
919 146321 : if (nextStop.pars.speed > 0) {
920 2653 : msg << "scheduled waypoint on lane '" << myID << "' too close";
921 2653 : distToStop = MAX2(0.0, nextStop.pars.startPos - pos);
922 2653 : safeSpeed = cfModel.freeSpeed(aVehicle, speed, distToStop, nextStop.pars.speed, true, MSCFModel::CalcReason::FUTURE);
923 : } else {
924 143668 : msg << "scheduled stop on lane '" << myID << "' too close";
925 143668 : distToStop = nextStop.pars.endPos - pos;
926 143668 : safeSpeed = cfModel.stopSpeed(aVehicle, speed, distToStop, MSCFModel::CalcReason::FUTURE);
927 : }
928 438946 : if (checkFailure(aVehicle, speed, dist, MAX2(0.0, safeSpeed), patchSpeed, msg.str(), InsertionCheck::STOP)) {
929 : // we may not drive with the given velocity - we cannot stop at the stop
930 : return false;
931 : }
932 146321 : }
933 : }
934 : // check leader vehicle first because it could have influenced the departSpeed (for departSpeed=avg)
935 : // get the pointer to the vehicle next in front of the given position
936 14613814 : const MSLeaderInfo leaders = getLastVehicleInformation(aVehicle, 0, pos);
937 : //if (aVehicle->getID() == "disabled") std::cout << " leaders=" << leaders.toString() << "\n";
938 14613814 : const double nspeed = safeInsertionSpeed(aVehicle, -pos, leaders, speed);
939 21246237 : if (nspeed == INVALID_SPEED || checkFailure(aVehicle, speed, dist, nspeed, patchSpeed, "", InsertionCheck::LEADER_GAP)) {
940 : // we may not drive with the given velocity - we crash into the leader
941 : #ifdef DEBUG_INSERTION
942 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
943 : std::cout << SIMTIME << " isInsertionSuccess lane=" << getID()
944 : << " veh=" << aVehicle->getID()
945 : << " pos=" << pos
946 : << " posLat=" << posLat
947 : << " patchSpeed=" << patchSpeed
948 : << " speed=" << speed
949 : << " nspeed=" << nspeed
950 : << " leaders=" << leaders.toString()
951 : << " failed (@700)!\n";
952 : }
953 : #endif
954 : return false;
955 : }
956 : #ifdef DEBUG_INSERTION
957 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
958 : std::cout << SIMTIME << " speed = " << speed << " nspeed = " << nspeed << " leaders=" << leaders.toString() << "\n";
959 : }
960 : #endif
961 :
962 4724918 : const MSRoute& r = aVehicle->getRoute();
963 4724918 : MSRouteIterator ce = r.begin();
964 : int nRouteSuccs = 1;
965 : MSLane* currentLane = this;
966 : MSLane* nextLane = this;
967 7552717 : SUMOTime arrivalTime = MSNet::getInstance()->getCurrentTimeStep() + TIME2STEPS(seen / MAX2(speed, SUMO_const_haltingSpeed));
968 5119953 : while ((seen < dist || (isRail && firstRailSignal == nullptr)) && ri != bestLaneConts.end()) {
969 : // get the next link used...
970 474052 : std::vector<MSLink*>::const_iterator link = succLinkSec(*aVehicle, nRouteSuccs, *currentLane, bestLaneConts);
971 : // get the next used lane (including internal)
972 474052 : if (currentLane->isLinkEnd(link)) {
973 19602 : if (¤tLane->getEdge() == r.getLastEdge()) {
974 : // reached the end of the route
975 9986 : if (aVehicle->getParameter().arrivalSpeedProcedure == ArrivalSpeedDefinition::GIVEN) {
976 100 : const double remaining = seen + aVehicle->getArrivalPos() - currentLane->getLength();
977 100 : const double fspeed = cfModel.freeSpeed(aVehicle, speed, remaining, aVehicle->getParameter().arrivalSpeed, true, MSCFModel::CalcReason::FUTURE);
978 200 : if (checkFailure(aVehicle, speed, dist, fspeed,
979 : patchSpeed, "arrival speed too low", InsertionCheck::ARRIVAL_SPEED)) {
980 : // we may not drive with the given velocity - we cannot match the specified arrival speed
981 : return false;
982 : }
983 : }
984 : } else {
985 : // lane does not continue
986 9616 : if (checkFailure(aVehicle, speed, dist, cfModel.insertionStopSpeed(aVehicle, speed, seen),
987 19232 : patchSpeed, "junction '" + currentLane->getEdge().getToJunction()->getID() + "' too close", InsertionCheck::JUNCTION)) {
988 : // we may not drive with the given velocity - we cannot stop at the junction
989 : return false;
990 : }
991 : }
992 : break;
993 : }
994 454450 : if (isRail && firstRailSignal == nullptr) {
995 : std::string constraintInfo;
996 : bool isInsertionOrder;
997 16308 : if (MSRailSignal::hasInsertionConstraint(*link, aVehicle, constraintInfo, isInsertionOrder)) {
998 6216 : setParameter((isInsertionOrder ? "insertionOrder" : "insertionConstraint:")
999 9261 : + aVehicle->getID(), constraintInfo);
1000 : #ifdef DEBUG_INSERTION
1001 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1002 : std::cout << " insertion constraint at link " << (*link)->getDescription() << " not cleared \n";
1003 : }
1004 : #endif
1005 : return false;
1006 : }
1007 : }
1008 :
1009 : // might also by a regular traffic_light instead of a rail_signal
1010 451342 : if (firstRailSignal == nullptr && (*link)->getTLLogic() != nullptr) {
1011 : firstRailSignal = *link;
1012 : firstRailSignalDist = seen;
1013 : }
1014 451342 : nextLane = (*link)->getViaLaneOrLane();
1015 451342 : if (!(*link)->opened(arrivalTime, speed, speed, aVehicle->getVehicleType().getLength(), aVehicle->getImpatience(),
1016 : cfModel.getMaxDecel(), 0, posLat, nullptr, false, aVehicle)
1017 430541 : || (*link)->railSignalWasPassed()
1018 881880 : || !(*link)->havePriority()) {
1019 : // have to stop at junction
1020 24669 : std::string errorMsg = "";
1021 24669 : const LinkState state = (*link)->getState();
1022 24669 : if (state == LINKSTATE_MINOR
1023 24669 : || state == LINKSTATE_EQUAL
1024 : || state == LINKSTATE_STOP
1025 : || state == LINKSTATE_ALLWAY_STOP) {
1026 : // no sense in trying later
1027 : errorMsg = "unpriorised junction too close";
1028 22102 : } else if ((*link)->getTLLogic() != nullptr && !(*link)->getTLLogic()->getsMajorGreen((*link)->getTLIndex())) {
1029 : // traffic light never turns 'G'?
1030 18952 : errorMsg = "tlLogic '" + (*link)->getTLLogic()->getID() + "' link " + toString((*link)->getTLIndex()) + " never switches to 'G'";
1031 : }
1032 24669 : const double laneStopOffset = MAX2(getVehicleStopOffset(aVehicle),
1033 24669 : aVehicle->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPLINE_CROSSING_GAP, MSPModel::SAFETY_GAP) - (*link)->getDistToFoePedCrossing());
1034 24669 : const double remaining = seen - laneStopOffset;
1035 24669 : auto dsp = aVehicle->getParameter().departSpeedProcedure;
1036 24669 : const bool patchSpeedSpecial = patchSpeed || dsp == DepartSpeedDefinition::DESIRED || dsp == DepartSpeedDefinition::LIMIT;
1037 : // patchSpeed depends on the presence of vehicles for these procedures. We never want to abort them here
1038 24669 : if (dsp == DepartSpeedDefinition::LAST || dsp == DepartSpeedDefinition::AVG) {
1039 : errorMsg = "";
1040 : }
1041 49338 : if (checkFailure(aVehicle, speed, dist, cfModel.insertionStopSpeed(aVehicle, speed, remaining),
1042 : patchSpeedSpecial, errorMsg, InsertionCheck::JUNCTION)) {
1043 : // we may not drive with the given velocity - we cannot stop at the junction in time
1044 : #ifdef DEBUG_INSERTION
1045 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1046 : std::cout << SIMTIME << " isInsertionSuccess lane=" << getID()
1047 : << " veh=" << aVehicle->getID()
1048 : << " patchSpeed=" << patchSpeed
1049 : << " speed=" << speed
1050 : << " remaining=" << remaining
1051 : << " leader=" << currentLane->getLastVehicleInformation(aVehicle, 0, 0).toString()
1052 : << " last=" << Named::getIDSecure(getLastAnyVehicle())
1053 : << " meanSpeed=" << getMeanSpeed()
1054 : << " failed (@926)!\n";
1055 : }
1056 : #endif
1057 : return false;
1058 : }
1059 : #ifdef DEBUG_INSERTION
1060 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1061 : std::cout << "trying insertion before minor link: "
1062 : << "insertion speed = " << speed << " dist=" << dist
1063 : << "\n";
1064 : }
1065 : #endif
1066 11607 : if (seen >= aVehicle->getVehicleType().getMinGap()) {
1067 : break;
1068 : }
1069 426673 : } else if (nextLane->isInternal()) {
1070 235570 : double tmp = 0;
1071 235570 : bool dummyReq = true;
1072 : #ifdef DEBUG_INSERTION
1073 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1074 : std::cout << "checking linkLeader for lane '" << nextLane->getID() << "'\n";
1075 : gDebugFlag1 = true;
1076 : }
1077 : #endif
1078 235570 : double nSpeed = speed;
1079 235570 : aVehicle->checkLinkLeader(nextLane->getLinkCont()[0], nextLane, seen + nextLane->getLength(), nullptr, nSpeed, tmp, tmp, dummyReq);
1080 : #ifdef DEBUG_INSERTION
1081 : gDebugFlag1 = false;
1082 : #endif
1083 471140 : if (checkFailure(aVehicle, speed, dist, nSpeed, patchSpeed, "", InsertionCheck::LEADER_GAP)) {
1084 : // we may not drive with the given velocity - there is a junction leader
1085 : #ifdef DEBUG_INSERTION
1086 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1087 : std::cout << " linkLeader nSpeed=" << nSpeed << " failed (@1058)!\n";
1088 : }
1089 : #endif
1090 2907 : return false;
1091 : }
1092 : }
1093 : // check how next lane affects the journey
1094 426623 : if (nextLane != nullptr) {
1095 :
1096 : // do not insert if the bidirectional edge is occupied before a railSignal has been encountered
1097 426623 : if (firstRailSignal == nullptr && nextLane->getBidiLane() != nullptr && nextLane->getBidiLane()->getVehicleNumberWithPartials() > 0) {
1098 0 : if ((insertionChecks & (int)InsertionCheck::ONCOMING_TRAIN) != 0) {
1099 : #ifdef DEBUG_INSERTION
1100 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1101 : std::cout << " nextLane=" << nextLane->getID() << " occupiedBidi\n";
1102 : }
1103 : #endif
1104 31587 : return false;
1105 : }
1106 : }
1107 :
1108 : // check if there are stops on the next lane that should be regarded
1109 : // (this block is duplicated before the loop to deal with the insertion lane)
1110 426623 : if (aVehicle->hasStops()) {
1111 8305 : const MSStop& nextStop = aVehicle->getNextStop();
1112 8305 : if (nextStop.lane == nextLane) {
1113 531 : std::stringstream msg;
1114 531 : msg << "scheduled stop on lane '" << nextStop.lane->getID() << "' too close";
1115 531 : const double distToStop = seen + nextStop.pars.endPos;
1116 531 : if (checkFailure(aVehicle, speed, dist, cfModel.insertionStopSpeed(aVehicle, speed, distToStop),
1117 531 : patchSpeed, msg.str(), InsertionCheck::STOP)) {
1118 : // we may not drive with the given velocity - we cannot stop at the stop
1119 : return false;
1120 : }
1121 531 : }
1122 : }
1123 :
1124 : // check leader on next lane
1125 426623 : const MSLeaderInfo nextLeaders = nextLane->getLastVehicleInformation(aVehicle, 0, 0);
1126 426623 : if (nextLeaders.hasVehicles()) {
1127 145778 : const double nextLaneSpeed = nextLane->safeInsertionSpeed(aVehicle, seen, nextLeaders, speed);
1128 : #ifdef DEBUG_INSERTION
1129 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1130 : std::cout << SIMTIME << " leader on lane '" << nextLane->getID() << "': " << nextLeaders.toString() << " nspeed=" << nextLaneSpeed << "\n";
1131 : }
1132 : #endif
1133 284481 : if (nextLaneSpeed == INVALID_SPEED || checkFailure(aVehicle, speed, dist, nextLaneSpeed, patchSpeed, "", InsertionCheck::LEADER_GAP)) {
1134 : // we may not drive with the given velocity - we crash into the leader
1135 : #ifdef DEBUG_INSERTION
1136 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1137 : std::cout << " isInsertionSuccess lane=" << getID()
1138 : << " veh=" << aVehicle->getID()
1139 : << " pos=" << pos
1140 : << " posLat=" << posLat
1141 : << " patchSpeed=" << patchSpeed
1142 : << " speed=" << speed
1143 : << " nspeed=" << nextLaneSpeed
1144 : << " nextLane=" << nextLane->getID()
1145 : << " lead=" << nextLeaders.toString()
1146 : << " failed (@641)!\n";
1147 : }
1148 : #endif
1149 : return false;
1150 : }
1151 : }
1152 397539 : if (!nextLane->checkForPedestrians(aVehicle, speed, dist, -seen, patchSpeed)) {
1153 : return false;
1154 : }
1155 : // check next lane's maximum velocity
1156 397515 : const double freeSpeed = cfModel.freeSpeed(aVehicle, speed, seen, nextLane->getVehicleMaxSpeed(aVehicle), true, MSCFModel::CalcReason::FUTURE);
1157 397514 : if (freeSpeed < speed) {
1158 47842 : if (patchSpeed || aVehicle->getParameter().departSpeedProcedure != DepartSpeedDefinition::GIVEN) {
1159 47432 : speed = freeSpeed;
1160 47432 : dist = cfModel.brakeGap(speed) + aVehicle->getVehicleType().getMinGap();
1161 : } else {
1162 410 : if ((insertionChecks & (int)InsertionCheck::SPEED_LIMIT) != 0) {
1163 410 : if (!MSGlobals::gCheckRoutes) {
1164 15 : WRITE_WARNINGF(TL("Vehicle '%' is inserted too fast and will violate the speed limit on a lane '%', time=%."),
1165 : aVehicle->getID(), nextLane->getID(), time2string(SIMSTEP));
1166 : } else {
1167 : // we may not drive with the given velocity - we would be too fast on the next lane
1168 1215 : WRITE_ERRORF(TL("Vehicle '%' will not be able to depart using the given velocity (slow lane ahead), time=%."), aVehicle->getID(), time2string(SIMSTEP));
1169 405 : MSNet::getInstance()->getInsertionControl().descheduleDeparture(aVehicle);
1170 : return false;
1171 : }
1172 : }
1173 : }
1174 : }
1175 : // check traffic on next junction
1176 : // we cannot use (*link)->opened because a vehicle without priority
1177 : // may already be comitted to blocking the link and unable to stop
1178 397109 : const SUMOTime leaveTime = (*link)->getLeaveTime(arrivalTime, speed, speed, aVehicle->getVehicleType().getLength());
1179 397109 : if ((*link)->hasApproachingFoe(arrivalTime, leaveTime, speed, cfModel.getMaxDecel())) {
1180 9994 : if (checkFailure(aVehicle, speed, dist, cfModel.insertionStopSpeed(aVehicle, speed, seen), patchSpeed, "", InsertionCheck::JUNCTION)) {
1181 : // we may not drive with the given velocity - we crash at the junction
1182 : return false;
1183 : }
1184 : }
1185 431032 : arrivalTime += TIME2STEPS(nextLane->getLength() / MAX2(speed, NUMERICAL_EPS));
1186 395035 : seen += nextLane->getLength();
1187 : currentLane = nextLane;
1188 395035 : if ((*link)->getViaLane() == nullptr) {
1189 188448 : nRouteSuccs++;
1190 : ++ce;
1191 : ++ri;
1192 : }
1193 426623 : }
1194 : }
1195 :
1196 4674239 : const MSLeaderDistanceInfo& followers = getFollowersOnConsecutive(aVehicle, aVehicle->getBackPositionOnLane(), false);
1197 9962934 : for (int i = 0; i < followers.numSublanes(); ++i) {
1198 6393437 : const MSVehicle* follower = followers[i].first;
1199 6393437 : if (follower != nullptr) {
1200 1457449 : const double backGapNeeded = follower->getCarFollowModel().getSecureGap(follower, aVehicle, follower->getSpeed(), speed, cfModel.getMaxDecel());
1201 1457449 : if (followers[i].second < backGapNeeded
1202 1457449 : && ((insertionChecks & (int)InsertionCheck::FOLLOWER_GAP) != 0
1203 80 : || (followers[i].second < 0 && (insertionChecks & (int)InsertionCheck::COLLISION) != 0))) {
1204 : // too close to the follower on this lane
1205 : #ifdef DEBUG_INSERTION
1206 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1207 : std::cout << SIMTIME << " isInsertionSuccess lane=" << getID()
1208 : << " veh=" << aVehicle->getID()
1209 : << " pos=" << pos
1210 : << " posLat=" << posLat
1211 : << " speed=" << speed
1212 : << " nspeed=" << nspeed
1213 : << " follower=" << follower->getID()
1214 : << " backGapNeeded=" << backGapNeeded
1215 : << " gap=" << followers[i].second
1216 : << " failure (@719)!\n";
1217 : }
1218 : #endif
1219 1104742 : return false;
1220 : }
1221 : }
1222 : }
1223 :
1224 3569497 : if (!checkForPedestrians(aVehicle, speed, dist, pos, patchSpeed)) {
1225 : return false;
1226 : }
1227 :
1228 3568991 : MSLane* shadowLane = aVehicle->getLaneChangeModel().getShadowLane(this);
1229 : #ifdef DEBUG_INSERTION
1230 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1231 : std::cout << " shadowLane=" << Named::getIDSecure(shadowLane) << "\n";
1232 : }
1233 : #endif
1234 3568991 : if (shadowLane != nullptr) {
1235 9809 : const MSLeaderDistanceInfo& shadowFollowers = shadowLane->getFollowersOnConsecutive(aVehicle, aVehicle->getBackPositionOnLane(), false);
1236 21700 : for (int i = 0; i < shadowFollowers.numSublanes(); ++i) {
1237 11908 : const MSVehicle* follower = shadowFollowers[i].first;
1238 11908 : if (follower != nullptr) {
1239 24 : const double backGapNeeded = follower->getCarFollowModel().getSecureGap(follower, aVehicle, follower->getSpeed(), speed, cfModel.getMaxDecel());
1240 24 : if (shadowFollowers[i].second < backGapNeeded
1241 24 : && ((insertionChecks & (int)InsertionCheck::FOLLOWER_GAP) != 0
1242 0 : || (shadowFollowers[i].second < 0 && (insertionChecks & (int)InsertionCheck::COLLISION) != 0))) {
1243 : // too close to the follower on this lane
1244 : #ifdef DEBUG_INSERTION
1245 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1246 : std::cout << SIMTIME
1247 : << " isInsertionSuccess shadowlane=" << shadowLane->getID()
1248 : << " veh=" << aVehicle->getID()
1249 : << " pos=" << pos
1250 : << " posLat=" << posLat
1251 : << " speed=" << speed
1252 : << " nspeed=" << nspeed
1253 : << " follower=" << follower->getID()
1254 : << " backGapNeeded=" << backGapNeeded
1255 : << " gap=" << shadowFollowers[i].second
1256 : << " failure (@812)!\n";
1257 : }
1258 : #endif
1259 17 : return false;
1260 : }
1261 : }
1262 : }
1263 9792 : const MSLeaderInfo& ahead = shadowLane->getLastVehicleInformation(nullptr, 0, aVehicle->getPositionOnLane(), false);
1264 20719 : for (int i = 0; i < ahead.numSublanes(); ++i) {
1265 11021 : const MSVehicle* veh = ahead[i];
1266 11021 : if (veh != nullptr) {
1267 347 : const double gap = veh->getBackPositionOnLane(shadowLane) - aVehicle->getPositionOnLane() - aVehicle->getVehicleType().getMinGap();
1268 347 : const double gapNeeded = aVehicle->getCarFollowModel().getSecureGap(aVehicle, veh, speed, veh->getSpeed(), veh->getCarFollowModel().getMaxDecel());
1269 347 : if (gap < gapNeeded
1270 94 : && ((insertionChecks & (int)InsertionCheck::LEADER_GAP) != 0
1271 0 : || (gap < 0 && (insertionChecks & (int)InsertionCheck::COLLISION) != 0))) {
1272 : // too close to the shadow leader
1273 : #ifdef DEBUG_INSERTION
1274 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1275 : std::cout << SIMTIME
1276 : << " isInsertionSuccess shadowlane=" << shadowLane->getID()
1277 : << " veh=" << aVehicle->getID()
1278 : << " pos=" << pos
1279 : << " posLat=" << posLat
1280 : << " speed=" << speed
1281 : << " nspeed=" << nspeed
1282 : << " leader=" << veh->getID()
1283 : << " gapNeeded=" << gapNeeded
1284 : << " gap=" << gap
1285 : << " failure (@842)!\n";
1286 : }
1287 : #endif
1288 : return false;
1289 : }
1290 : }
1291 : }
1292 9809 : }
1293 3568880 : if (followers.numFreeSublanes() > 0) {
1294 : // check approaching vehicles to prevent rear-end collisions
1295 3352103 : const double backOffset = pos - aVehicle->getVehicleType().getLength();
1296 3352103 : const double missingRearGap = getMissingRearGap(aVehicle, backOffset, speed);
1297 3352103 : if (missingRearGap > 0
1298 0 : && (insertionChecks & (int)InsertionCheck::LEADER_GAP) != 0) {
1299 : // too close to a follower
1300 : #ifdef DEBUG_INSERTION
1301 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1302 : std::cout << SIMTIME
1303 : << " isInsertionSuccess lane=" << getID()
1304 : << " veh=" << aVehicle->getID()
1305 : << " pos=" << pos
1306 : << " posLat=" << posLat
1307 : << " speed=" << speed
1308 : << " nspeed=" << nspeed
1309 : << " missingRearGap=" << missingRearGap
1310 : << " failure (@728)!\n";
1311 : }
1312 : #endif
1313 : return false;
1314 : }
1315 : }
1316 3568880 : if (insertionChecks == (int)InsertionCheck::NONE) {
1317 2332 : speed = MAX2(0.0, speed);
1318 : }
1319 : // may got negative while adaptation
1320 3568880 : if (speed < 0) {
1321 : #ifdef DEBUG_INSERTION
1322 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1323 : std::cout << SIMTIME
1324 : << " isInsertionSuccess lane=" << getID()
1325 : << " veh=" << aVehicle->getID()
1326 : << " pos=" << pos
1327 : << " posLat=" << posLat
1328 : << " speed=" << speed
1329 : << " nspeed=" << nspeed
1330 : << " failed (@733)!\n";
1331 : }
1332 : #endif
1333 : return false;
1334 : }
1335 3568879 : const int bestLaneOffset = aVehicle->getBestLaneOffset();
1336 3568879 : const double extraReservation = aVehicle->getLaneChangeModel().getExtraReservation(bestLaneOffset);
1337 3568879 : if (extraReservation > 0) {
1338 20137 : std::stringstream msg;
1339 20137 : msg << "too many lane changes required on lane '" << myID << "'";
1340 : // we need to take into acount one extra actionStep of delay due to #3665
1341 20137 : double distToStop = aVehicle->getBestLaneDist() - pos - extraReservation - speed * aVehicle->getActionStepLengthSecs();
1342 20137 : if (distToStop >= 0) {
1343 : double stopSpeed = cfModel.stopSpeed(aVehicle, speed, distToStop, MSCFModel::CalcReason::FUTURE);
1344 : #ifdef DEBUG_INSERTION
1345 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1346 : std::cout << "\nIS_INSERTION_SUCCESS\n"
1347 : << SIMTIME << " veh=" << aVehicle->getID() << " bestLaneOffset=" << bestLaneOffset << " bestLaneDist=" << aVehicle->getBestLaneDist() << " extraReservation=" << extraReservation
1348 : << " distToStop=" << distToStop << " v=" << speed << " v2=" << stopSpeed << "\n";
1349 : }
1350 : #endif
1351 18660 : if (checkFailure(aVehicle, speed, distToStop, MAX2(0.0, stopSpeed),
1352 18660 : patchSpeed, msg.str(), InsertionCheck::LANECHANGE)) {
1353 : // we may not drive with the given velocity - we cannot reserve enough space for lane changing
1354 : return false;
1355 : }
1356 : }
1357 20137 : }
1358 : // enter
1359 3568867 : incorporateVehicle(aVehicle, pos, speed, posLat, find_if(myVehicles.begin(), myVehicles.end(), [&](MSVehicle * const v) {
1360 3053186 : return v->getPositionOnLane() >= pos;
1361 : }), notification);
1362 : #ifdef DEBUG_INSERTION
1363 : if (DEBUG_COND2(aVehicle) || DEBUG_COND) {
1364 : std::cout << SIMTIME
1365 : << " isInsertionSuccess lane=" << getID()
1366 : << " veh=" << aVehicle->getID()
1367 : << " pos=" << pos
1368 : << " posLat=" << posLat
1369 : << " speed=" << speed
1370 : << " nspeed=" << nspeed
1371 : << "\n myVehicles=" << toString(myVehicles)
1372 : << " myPartial=" << toString(myPartialVehicles)
1373 : << " myManeuverReservations=" << toString(myManeuverReservations)
1374 : << "\n success!\n";
1375 : }
1376 : #endif
1377 3568865 : if (isRail) {
1378 5102 : unsetParameter("insertionConstraint:" + aVehicle->getID());
1379 5102 : unsetParameter("insertionOrder:" + aVehicle->getID());
1380 5102 : unsetParameter("insertionBlocked:" + aVehicle->getID());
1381 : // rail_signal (not traffic_light) requires approach information for
1382 : // switching correctly at the start of the next simulation step
1383 5102 : if (firstRailSignal != nullptr && firstRailSignal->getJunction()->getType() == SumoXMLNodeType::RAIL_SIGNAL) {
1384 1675 : aVehicle->registerInsertionApproach(firstRailSignal, firstRailSignalDist);
1385 : }
1386 : }
1387 : return true;
1388 14613816 : }
1389 :
1390 :
1391 : void
1392 48406 : MSLane::forceVehicleInsertion(MSVehicle* veh, double pos, MSMoveReminder::Notification notification, double posLat) {
1393 48406 : veh->updateBestLanes(true, this);
1394 : bool dummy;
1395 48406 : const double speed = veh->hasDeparted() ? veh->getSpeed() : getDepartSpeed(*veh, dummy);
1396 48406 : incorporateVehicle(veh, pos, speed, posLat, find_if(myVehicles.begin(), myVehicles.end(), [&](MSVehicle * const v) {
1397 126188 : return v->getPositionOnLane() >= pos;
1398 : }), notification);
1399 48406 : }
1400 :
1401 :
1402 : double
1403 14759592 : MSLane::safeInsertionSpeed(const MSVehicle* veh, double seen, const MSLeaderInfo& leaders, double speed) {
1404 : double nspeed = speed;
1405 : #ifdef DEBUG_INSERTION
1406 : if (DEBUG_COND2(veh)) {
1407 : std::cout << SIMTIME << " safeInsertionSpeed veh=" << veh->getID() << " speed=" << speed << "\n";
1408 : }
1409 : #endif
1410 24870661 : for (int i = 0; i < leaders.numSublanes(); ++i) {
1411 18099535 : const MSVehicle* leader = leaders[i];
1412 18099535 : if (leader != nullptr) {
1413 15938083 : double gap = leader->getBackPositionOnLane(this) + seen - veh->getVehicleType().getMinGap();
1414 15938083 : if (leader->getLane() == getBidiLane()) {
1415 : // use distance to front position and account for movement
1416 270 : gap -= (leader->getLength() + leader->getBrakeGap(true));
1417 : }
1418 15938083 : if (gap < 0) {
1419 : #ifdef DEBUG_INSERTION
1420 : if (DEBUG_COND2(veh)) {
1421 : std::cout << " leader=" << leader->getID() << " bPos=" << leader->getBackPositionOnLane(this) << " gap=" << gap << "\n";
1422 : }
1423 : #endif
1424 7988466 : if ((veh->getInsertionChecks() & (int)InsertionCheck::COLLISION) != 0) {
1425 : return INVALID_SPEED;
1426 : } else {
1427 0 : return 0;
1428 : }
1429 : }
1430 7949617 : nspeed = MIN2(nspeed,
1431 7949617 : veh->getCarFollowModel().insertionFollowSpeed(veh, speed, gap, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel(), leader));
1432 : #ifdef DEBUG_INSERTION
1433 : if (DEBUG_COND2(veh)) {
1434 : std::cout << " leader=" << leader->getID() << " bPos=" << leader->getBackPositionOnLane(this) << " gap=" << gap << " nspeed=" << nspeed << "\n";
1435 : }
1436 : #endif
1437 : }
1438 : }
1439 : return nspeed;
1440 : }
1441 :
1442 :
1443 : // ------ Handling vehicles lapping into lanes ------
1444 : const MSLeaderInfo
1445 719721439 : MSLane::getLastVehicleInformation(const MSVehicle* ego, double latOffset, double minPos, bool allowCached) const {
1446 719721439 : if (myLeaderInfoTime < MSNet::getInstance()->getCurrentTimeStep() || ego != nullptr || minPos > 0 || !allowCached) {
1447 253627936 : MSLeaderInfo leaderTmp(myWidth, ego, latOffset);
1448 : AnyVehicleIterator last = anyVehiclesBegin();
1449 : int freeSublanes = 1; // number of sublanes for which no leader was found
1450 : //if (ego->getID() == "disabled" && SIMTIME == 58) {
1451 : // std::cout << "DEBUG\n";
1452 : //}
1453 253627936 : const MSVehicle* veh = *last;
1454 1900661824 : while (freeSublanes > 0 && veh != nullptr) {
1455 : #ifdef DEBUG_PLAN_MOVE
1456 : if (DEBUG_COND2(ego) || DEBUG_COND) {
1457 : gDebugFlag1 = true;
1458 : std::cout << " getLastVehicleInformation lane=" << getID() << " minPos=" << minPos << " veh=" << veh->getID() << " pos=" << veh->getPositionOnLane(this) << "\n";
1459 : }
1460 : #endif
1461 3291634997 : if (veh != ego && MAX2(0.0, veh->getPositionOnLane(this)) >= minPos) {
1462 266482381 : const double vehLatOffset = veh->getLatOffset(this);
1463 266482381 : freeSublanes = leaderTmp.addLeader(veh, true, vehLatOffset);
1464 : #ifdef DEBUG_PLAN_MOVE
1465 : if (DEBUG_COND2(ego) || DEBUG_COND) {
1466 : std::cout << " latOffset=" << vehLatOffset << " newLeaders=" << leaderTmp.toString() << "\n";
1467 : }
1468 : #endif
1469 : }
1470 1647033888 : veh = *(++last);
1471 : }
1472 253627936 : if (ego == nullptr && minPos == 0) {
1473 : #ifdef HAVE_FOX
1474 112118332 : ScopedLocker<> lock(myLeaderInfoMutex, MSGlobals::gNumSimThreads > 1);
1475 : #endif
1476 : // update cached value
1477 : myLeaderInfo = leaderTmp;
1478 112118332 : myLeaderInfoTime = MSNet::getInstance()->getCurrentTimeStep();
1479 : }
1480 : #ifdef DEBUG_PLAN_MOVE
1481 : //if (DEBUG_COND2(ego)) std::cout << SIMTIME
1482 : // << " getLastVehicleInformation lane=" << getID()
1483 : // << " ego=" << Named::getIDSecure(ego)
1484 : // << "\n"
1485 : // << " vehicles=" << toString(myVehicles)
1486 : // << " partials=" << toString(myPartialVehicles)
1487 : // << "\n"
1488 : // << " result=" << leaderTmp.toString()
1489 : // << " cached=" << myLeaderInfo.toString()
1490 : // << " myLeaderInfoTime=" << myLeaderInfoTime
1491 : // << "\n";
1492 : gDebugFlag1 = false;
1493 : #endif
1494 : return leaderTmp;
1495 253627936 : }
1496 : return myLeaderInfo;
1497 : }
1498 :
1499 :
1500 : const MSLeaderInfo
1501 445630380 : MSLane::getFirstVehicleInformation(const MSVehicle* ego, double latOffset, bool onlyFrontOnLane, double maxPos, bool allowCached) const {
1502 : #ifdef HAVE_FOX
1503 445630380 : ScopedLocker<> lock(myFollowerInfoMutex, MSGlobals::gNumSimThreads > 1);
1504 : #endif
1505 445630380 : if (myFollowerInfoTime < MSNet::getInstance()->getCurrentTimeStep() || ego != nullptr || maxPos < myLength || !allowCached || onlyFrontOnLane) {
1506 : // XXX separate cache for onlyFrontOnLane = true
1507 445630380 : MSLeaderInfo followerTmp(myWidth, ego, latOffset);
1508 : AnyVehicleIterator first = anyVehiclesUpstreamBegin();
1509 : int freeSublanes = 1; // number of sublanes for which no leader was found
1510 445630380 : const MSVehicle* veh = *first;
1511 713001165 : while (freeSublanes > 0 && veh != nullptr) {
1512 : #ifdef DEBUG_PLAN_MOVE
1513 : if (DEBUG_COND2(ego)) {
1514 : std::cout << " veh=" << veh->getID() << " pos=" << veh->getPositionOnLane(this) << " maxPos=" << maxPos << "\n";
1515 : }
1516 : #endif
1517 267370785 : if (veh != ego && veh->getPositionOnLane(this) <= maxPos
1518 534741570 : && (!onlyFrontOnLane || veh->isFrontOnLane(this))) {
1519 : //const double vehLatOffset = veh->getLane()->getRightSideOnEdge() - getRightSideOnEdge();
1520 244525686 : const double vehLatOffset = veh->getLatOffset(this);
1521 : #ifdef DEBUG_PLAN_MOVE
1522 : if (DEBUG_COND2(ego)) {
1523 : std::cout << " veh=" << veh->getID() << " latOffset=" << vehLatOffset << "\n";
1524 : }
1525 : #endif
1526 244525686 : freeSublanes = followerTmp.addLeader(veh, true, vehLatOffset);
1527 : }
1528 267370785 : veh = *(++first);
1529 : }
1530 445630380 : if (ego == nullptr && maxPos == std::numeric_limits<double>::max()) {
1531 : // update cached value
1532 : myFollowerInfo = followerTmp;
1533 445630380 : myFollowerInfoTime = MSNet::getInstance()->getCurrentTimeStep();
1534 : }
1535 : #ifdef DEBUG_PLAN_MOVE
1536 : //if (DEBUG_COND2(ego)) std::cout << SIMTIME
1537 : // << " getFirstVehicleInformation lane=" << getID()
1538 : // << " ego=" << Named::getIDSecure(ego)
1539 : // << "\n"
1540 : // << " vehicles=" << toString(myVehicles)
1541 : // << " partials=" << toString(myPartialVehicles)
1542 : // << "\n"
1543 : // << " result=" << followerTmp.toString()
1544 : // //<< " cached=" << myFollowerInfo.toString()
1545 : // << " myLeaderInfoTime=" << myLeaderInfoTime
1546 : // << "\n";
1547 : #endif
1548 : return followerTmp;
1549 445630380 : }
1550 : return myFollowerInfo;
1551 : }
1552 :
1553 :
1554 : // ------ ------
1555 : void
1556 96903732 : MSLane::planMovements(SUMOTime t) {
1557 : assert(myVehicles.size() != 0);
1558 : double cumulatedVehLength = 0.;
1559 96903732 : MSLeaderInfo leaders(myWidth);
1560 :
1561 : // iterate over myVehicles, myPartialVehicles, and myManeuverReservations merge-sort style
1562 : VehCont::reverse_iterator veh = myVehicles.rbegin();
1563 : VehCont::reverse_iterator vehPart = myPartialVehicles.rbegin();
1564 : VehCont::reverse_iterator vehRes = myManeuverReservations.rbegin();
1565 : #ifdef DEBUG_PLAN_MOVE
1566 : if (DEBUG_COND) std::cout
1567 : << "\n"
1568 : << SIMTIME
1569 : << " planMovements() lane=" << getID()
1570 : << "\n vehicles=" << toString(myVehicles)
1571 : << "\n partials=" << toString(myPartialVehicles)
1572 : << "\n reservations=" << toString(myManeuverReservations)
1573 : << "\n";
1574 : #endif
1575 : assert(MSGlobals::gLateralResolution || myManeuverReservations.size() == 0);
1576 787914489 : for (; veh != myVehicles.rend(); ++veh) {
1577 : #ifdef DEBUG_PLAN_MOVE
1578 : if (DEBUG_COND2((*veh))) {
1579 : std::cout << " plan move for: " << (*veh)->getID();
1580 : }
1581 : #endif
1582 691010757 : updateLeaderInfo(*veh, vehPart, vehRes, leaders); // 36ns with 8 threads, 9ns with 1
1583 : #ifdef DEBUG_PLAN_MOVE
1584 : if (DEBUG_COND2((*veh))) {
1585 : std::cout << " leaders=" << leaders.toString() << "\n";
1586 : }
1587 : #endif
1588 691010757 : (*veh)->planMove(t, leaders, cumulatedVehLength); // 4800ns with 8 threads, 3100 with 1
1589 691010757 : cumulatedVehLength += (*veh)->getVehicleType().getLengthWithGap();
1590 691010757 : leaders.addLeader(*veh, false, 0);
1591 : }
1592 96903732 : }
1593 :
1594 :
1595 : void
1596 96903732 : MSLane::setJunctionApproaches() const {
1597 787914489 : for (MSVehicle* const veh : myVehicles) {
1598 691010757 : veh->setApproachingForAllLinks();
1599 : }
1600 96903732 : }
1601 :
1602 :
1603 : void
1604 691010757 : MSLane::updateLeaderInfo(const MSVehicle* veh, VehCont::reverse_iterator& vehPart, VehCont::reverse_iterator& vehRes, MSLeaderInfo& ahead) const {
1605 : bool morePartialVehsAhead = vehPart != myPartialVehicles.rend();
1606 : bool moreReservationsAhead = vehRes != myManeuverReservations.rend();
1607 : bool nextToConsiderIsPartial;
1608 :
1609 : // Determine relevant leaders for veh
1610 700991630 : while (moreReservationsAhead || morePartialVehsAhead) {
1611 1246043 : if ((!moreReservationsAhead || (*vehRes)->getPositionOnLane(this) <= veh->getPositionOnLane())
1612 20157475 : && (!morePartialVehsAhead || (*vehPart)->getPositionOnLane(this) <= veh->getPositionOnLane())) {
1613 : // All relevant downstream vehicles have been collected.
1614 : break;
1615 : }
1616 :
1617 : // Check whether next farthest relevant vehicle downstream is a partial vehicle or a maneuver reservation
1618 9980873 : if (moreReservationsAhead && !morePartialVehsAhead) {
1619 : nextToConsiderIsPartial = false;
1620 9891829 : } else if (morePartialVehsAhead && !moreReservationsAhead) {
1621 : nextToConsiderIsPartial = true;
1622 : } else {
1623 : assert(morePartialVehsAhead && moreReservationsAhead);
1624 : // Add farthest downstream vehicle first
1625 166287 : nextToConsiderIsPartial = (*vehPart)->getPositionOnLane(this) > (*vehRes)->getPositionOnLane(this);
1626 : }
1627 : // Add appropriate leader information
1628 166287 : if (nextToConsiderIsPartial) {
1629 9842857 : const double latOffset = (*vehPart)->getLatOffset(this);
1630 : #ifdef DEBUG_PLAN_MOVE
1631 : if (DEBUG_COND) {
1632 : std::cout << " partial ahead: " << (*vehPart)->getID() << " latOffset=" << latOffset << "\n";
1633 : }
1634 : #endif
1635 9909858 : if (!(MSGlobals::gLaneChangeDuration > 0 && (*vehPart)->getLaneChangeModel().isOpposite()
1636 67001 : && !(*vehPart)->getLaneChangeModel().isChangingLanes())) {
1637 9792978 : ahead.addLeader(*vehPart, false, latOffset);
1638 : }
1639 : ++vehPart;
1640 : morePartialVehsAhead = vehPart != myPartialVehicles.rend();
1641 : } else {
1642 138016 : const double latOffset = (*vehRes)->getLatOffset(this);
1643 : #ifdef DEBUG_PLAN_MOVE
1644 : if (DEBUG_COND) {
1645 : std::cout << " reservation ahead: " << (*vehRes)->getID() << " latOffset=" << latOffset << "\n";
1646 : }
1647 : #endif
1648 138016 : ahead.addLeader(*vehRes, false, latOffset);
1649 : ++vehRes;
1650 : moreReservationsAhead = vehRes != myManeuverReservations.rend();
1651 : }
1652 : }
1653 691010757 : }
1654 :
1655 :
1656 : void
1657 104776186 : MSLane::detectCollisions(SUMOTime timestep, const std::string& stage) {
1658 104776186 : myNeedsCollisionCheck = false;
1659 : #ifdef DEBUG_COLLISIONS
1660 : if (DEBUG_COND) {
1661 : std::vector<const MSVehicle*> all;
1662 : for (AnyVehicleIterator last = anyVehiclesBegin(); last != anyVehiclesEnd(); ++last) {
1663 : all.push_back(*last);
1664 : }
1665 : std::cout << SIMTIME << " detectCollisions stage=" << stage << " lane=" << getID() << ":\n"
1666 : << " vehs=" << toString(myVehicles) << "\n"
1667 : << " part=" << toString(myPartialVehicles) << "\n"
1668 : << " all=" << toString(all) << "\n"
1669 : << "\n";
1670 : }
1671 : #endif
1672 :
1673 104776186 : if (myCollisionAction == COLLISION_ACTION_NONE) {
1674 704138 : return;
1675 : }
1676 :
1677 : std::set<const MSVehicle*, ComparatorNumericalIdLess> toRemove;
1678 : std::set<const MSVehicle*, ComparatorNumericalIdLess> toTeleport;
1679 104763264 : if (mustCheckJunctionCollisions()) {
1680 1343543 : myNeedsCollisionCheck = true; // always check
1681 : #ifdef DEBUG_JUNCTION_COLLISIONS
1682 : if (DEBUG_COND) {
1683 : std::cout << SIMTIME << " detect junction Collisions stage=" << stage << " lane=" << getID() << ":\n"
1684 : << " vehs=" << toString(myVehicles) << "\n"
1685 : << " part=" << toString(myPartialVehicles) << "\n"
1686 : << "\n";
1687 : }
1688 : #endif
1689 : assert(myLinks.size() == 1);
1690 1343543 : const std::vector<const MSLane*>& foeLanes = myLinks.front()->getFoeLanes();
1691 : // save the iterator, it might get modified, see #8842
1692 : MSLane::AnyVehicleIterator end = anyVehiclesEnd();
1693 2470009 : for (AnyVehicleIterator veh = anyVehiclesBegin(); veh != end; ++veh) {
1694 2470009 : const MSVehicle* const collider = *veh;
1695 : //std::cout << " collider " << collider->getID() << "\n";
1696 2470009 : PositionVector colliderBoundary = collider->getBoundingBox(myCheckJunctionCollisionMinGap);
1697 34724907 : for (const MSLane* const foeLane : foeLanes) {
1698 : #ifdef DEBUG_JUNCTION_COLLISIONS
1699 : if (DEBUG_COND) {
1700 : std::cout << " foeLane " << foeLane->getID()
1701 : << " foeVehs=" << toString(foeLane->myVehicles)
1702 : << " foePart=" << toString(foeLane->myPartialVehicles) << "\n";
1703 : }
1704 : #endif
1705 : MSLane::AnyVehicleIterator foeEnd = foeLane->anyVehiclesEnd();
1706 4256217 : for (MSLane::AnyVehicleIterator it_veh = foeLane->anyVehiclesBegin(); it_veh != foeEnd; ++it_veh) {
1707 4256217 : const MSVehicle* const victim = *it_veh;
1708 4256217 : if (victim == collider) {
1709 : // may happen if the vehicles lane and shadow lane are siblings
1710 1553 : continue;
1711 : }
1712 : #ifdef DEBUG_JUNCTION_COLLISIONS
1713 : if (DEBUG_COND && DEBUG_COND2(collider)) {
1714 : std::cout << SIMTIME << " foe=" << victim->getID()
1715 : << " bound=" << colliderBoundary << " foeBound=" << victim->getBoundingBox()
1716 : << " overlaps=" << colliderBoundary.overlapsWith(victim->getBoundingBox())
1717 : << " poly=" << collider->getBoundingPoly()
1718 : << " foePoly=" << victim->getBoundingPoly()
1719 : << " overlaps2=" << collider->getBoundingPoly().overlapsWith(victim->getBoundingPoly())
1720 : << "\n";
1721 : }
1722 : #endif
1723 4254664 : if (MSGlobals::gIgnoreJunctionBlocker < std::numeric_limits<SUMOTime>::max()) {
1724 7280 : if (collider->getWaitingTime() >= MSGlobals::gIgnoreJunctionBlocker
1725 7280 : || victim->getWaitingTime() >= MSGlobals::gIgnoreJunctionBlocker) {
1726 : // ignored vehicles should not tigger collision
1727 5904 : continue;
1728 : }
1729 : }
1730 :
1731 4248760 : if (colliderBoundary.overlapsWith(victim->getBoundingBox())) {
1732 : // make a detailed check
1733 22976 : PositionVector boundingPoly = collider->getBoundingPoly();
1734 22976 : if (collider->getBoundingPoly(myCheckJunctionCollisionMinGap).overlapsWith(victim->getBoundingPoly())) {
1735 : // junction leader is the victim (collider must still be on junction)
1736 : assert(isInternal());
1737 14458 : if (victim->getLane()->isInternal() && victim->isLeader(myLinks.front(), collider, -1)) {
1738 6427 : foeLane->handleCollisionBetween(timestep, stage, victim, collider, -1, 0, toRemove, toTeleport);
1739 : } else {
1740 8031 : handleCollisionBetween(timestep, stage, collider, victim, -1, 0, toRemove, toTeleport);
1741 : }
1742 : }
1743 22976 : }
1744 : }
1745 32254898 : detectPedestrianJunctionCollision(collider, colliderBoundary, foeLane, timestep, stage, toRemove, toTeleport);
1746 : }
1747 2470009 : if (myLinks.front()->getWalkingAreaFoe() != nullptr) {
1748 59508 : detectPedestrianJunctionCollision(collider, colliderBoundary, myLinks.front()->getWalkingAreaFoe(), timestep, stage, toRemove, toTeleport);
1749 : }
1750 2470009 : if (myLinks.front()->getWalkingAreaFoeExit() != nullptr) {
1751 55556 : detectPedestrianJunctionCollision(collider, colliderBoundary, myLinks.front()->getWalkingAreaFoeExit(), timestep, stage, toRemove, toTeleport);
1752 : }
1753 2470009 : }
1754 : }
1755 :
1756 :
1757 104763264 : if (myIntermodalCollisionAction != COLLISION_ACTION_NONE && myEdge->getPersons().size() > 0 && hasPedestrians()) {
1758 : #ifdef DEBUG_PEDESTRIAN_COLLISIONS
1759 : if (DEBUG_COND) {
1760 : std::cout << SIMTIME << " detect pedestrian collisions stage=" << stage << " lane=" << getID() << "\n";
1761 : }
1762 : #endif
1763 : AnyVehicleIterator v_end = anyVehiclesEnd();
1764 147951 : for (AnyVehicleIterator it_v = anyVehiclesBegin(); it_v != v_end; ++it_v) {
1765 147951 : const MSVehicle* v = *it_v;
1766 147951 : double back = v->getBackPositionOnLane(this);
1767 147951 : const double length = v->getVehicleType().getLength();
1768 147951 : const double right = v->getRightSideOnEdge(this) - getRightSideOnEdge();
1769 147951 : if (v->getLane() == getBidiLane()) {
1770 : // use the front position for checking
1771 611 : back -= length;
1772 : }
1773 147951 : PersonDist leader = nextBlocking(back, right, right + v->getVehicleType().getWidth());
1774 : #ifdef DEBUG_PEDESTRIAN_COLLISIONS
1775 : if (DEBUG_COND && DEBUG_COND2(v)) {
1776 : std::cout << SIMTIME << " back=" << back << " right=" << right << " person=" << Named::getIDSecure(leader.first)
1777 : << " dist=" << leader.second << " jammed=" << (leader.first == nullptr ? false : leader.first->isJammed()) << "\n";
1778 : }
1779 : #endif
1780 147951 : if (leader.first != 0 && leader.second < length && !leader.first->isJammed()) {
1781 97 : if (v->getVehicleType().getGuiShape() == SUMOVehicleShape::AIRCRAFT) {
1782 : // aircraft wings and body are above walking level
1783 : continue;
1784 : }
1785 97 : const double gap = leader.second - length;
1786 194 : handleIntermodalCollisionBetween(timestep, stage, v, leader.first, gap, "sharedLane", toRemove, toTeleport);
1787 : }
1788 : }
1789 : }
1790 :
1791 104763264 : if (myVehicles.size() == 0) {
1792 : return;
1793 : }
1794 104072048 : if (!MSGlobals::gSublane) {
1795 : // no sublanes
1796 : VehCont::reverse_iterator lastVeh = myVehicles.rend() - 1;
1797 627437779 : for (VehCont::reverse_iterator pred = myVehicles.rbegin(); pred != lastVeh; ++pred) {
1798 : VehCont::reverse_iterator veh = pred + 1;
1799 543596270 : detectCollisionBetween(timestep, stage, *veh, *pred, toRemove, toTeleport);
1800 : }
1801 83841509 : if (myPartialVehicles.size() > 0) {
1802 6725165 : detectCollisionBetween(timestep, stage, *lastVeh, myPartialVehicles.front(), toRemove, toTeleport);
1803 : }
1804 83841509 : if (getBidiLane() != nullptr) {
1805 : // bidirectional railway
1806 455078 : MSLane* bidiLane = getBidiLane();
1807 455078 : if (bidiLane->getVehicleNumberWithPartials() > 0) {
1808 331168 : for (auto veh = myVehicles.begin(); veh != myVehicles.end(); ++veh) {
1809 232550 : double high = (*veh)->getPositionOnLane(this);
1810 232550 : double low = (*veh)->getBackPositionOnLane(this);
1811 232550 : if (stage == MSNet::STAGE_MOVEMENTS) {
1812 : // use previous back position to catch trains that
1813 : // "jump" through each other
1814 210275 : low -= SPEED2DIST((*veh)->getSpeed());
1815 : }
1816 1047902 : for (AnyVehicleIterator veh2 = bidiLane->anyVehiclesBegin(); veh2 != bidiLane->anyVehiclesEnd(); ++veh2) {
1817 : // self-collisions might legitemately occur when a long train loops back on itself
1818 1047902 : if (*veh == *veh2 && !(*veh)->isRail()) {
1819 231696 : continue;
1820 : }
1821 933478 : if ((*veh)->getLane() == (*veh2)->getLane() ||
1822 902050 : (*veh)->getLane() == (*veh2)->getBackLane() ||
1823 85844 : (*veh)->getBackLane() == (*veh2)->getLane()) {
1824 : // vehicles are not in a bidi relation
1825 730362 : continue;
1826 : }
1827 85844 : double low2 = myLength - (*veh2)->getPositionOnLane(bidiLane);
1828 85844 : double high2 = myLength - (*veh2)->getBackPositionOnLane(bidiLane);
1829 85844 : if (stage == MSNet::STAGE_MOVEMENTS) {
1830 : // use previous back position to catch trains that
1831 : // "jump" through each other
1832 72593 : high2 += SPEED2DIST((*veh2)->getSpeed());
1833 : }
1834 85844 : if (!(high < low2 || high2 < low)) {
1835 : #ifdef DEBUG_COLLISIONS
1836 : if (DEBUG_COND) {
1837 : std::cout << SIMTIME << " bidi-collision veh=" << (*veh)->getID() << " bidiVeh=" << (*veh2)->getID()
1838 : << " vehFurther=" << toString((*veh)->getFurtherLanes())
1839 : << " high=" << high << " low=" << low << " high2=" << high2 << " low2=" << low2 << "\n";
1840 : }
1841 : #endif
1842 : // the faster vehicle is at fault
1843 41 : MSVehicle* collider = const_cast<MSVehicle*>(*veh);
1844 41 : MSVehicle* victim = const_cast<MSVehicle*>(*veh2);
1845 41 : if (collider->getSpeed() < victim->getSpeed()) {
1846 : std::swap(victim, collider);
1847 : }
1848 41 : handleCollisionBetween(timestep, stage, collider, victim, -1, 0, toRemove, toTeleport);
1849 : }
1850 : }
1851 : }
1852 : }
1853 : }
1854 : } else {
1855 : // in the sublane-case it is insufficient to check the vehicles ordered
1856 : // by their front position as there might be more than 2 vehicles next to each
1857 : // other on the same lane
1858 : // instead, a moving-window approach is used where all vehicles that
1859 : // overlap in the longitudinal direction receive pairwise checks
1860 : // XXX for efficiency, all lanes of an edge should be checked together
1861 : // (lanechanger-style)
1862 :
1863 : // XXX quick hack: check each in myVehicles against all others
1864 140478030 : for (AnyVehicleIterator veh = anyVehiclesBegin(); veh != anyVehiclesEnd(); ++veh) {
1865 140478030 : MSVehicle* follow = (MSVehicle*)*veh;
1866 3766662903 : for (AnyVehicleIterator veh2 = anyVehiclesBegin(); veh2 != anyVehiclesEnd(); ++veh2) {
1867 3766761142 : MSVehicle* lead = (MSVehicle*)*veh2;
1868 3766761142 : if (lead == follow) {
1869 140455497 : continue;
1870 : }
1871 3626305645 : if (lead->getPositionOnLane(this) < follow->getPositionOnLane(this)) {
1872 1812147879 : continue;
1873 : }
1874 1814157766 : if (detectCollisionBetween(timestep, stage, follow, lead, toRemove, toTeleport)) {
1875 : // XXX what about collisions with multiple leaders at once?
1876 : break;
1877 : }
1878 : }
1879 : }
1880 : }
1881 :
1882 :
1883 104076011 : for (std::set<const MSVehicle*, ComparatorNumericalIdLess>::iterator it = toRemove.begin(); it != toRemove.end(); ++it) {
1884 3963 : MSVehicle* veh = const_cast<MSVehicle*>(*it);
1885 : MSLane* vehLane = veh->getMutableLane();
1886 3963 : vehLane->removeVehicle(veh, MSMoveReminder::NOTIFICATION_TELEPORT, false);
1887 : if (toTeleport.count(veh) > 0) {
1888 3578 : MSVehicleTransfer::getInstance()->add(timestep, veh);
1889 : } else {
1890 385 : veh->onRemovalFromNet(MSMoveReminder::NOTIFICATION_VAPORIZED_COLLISION);
1891 385 : MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(veh);
1892 : }
1893 : }
1894 : }
1895 :
1896 :
1897 : void
1898 32369962 : MSLane::detectPedestrianJunctionCollision(const MSVehicle* collider, const PositionVector& colliderBoundary, const MSLane* foeLane,
1899 : SUMOTime timestep, const std::string& stage,
1900 : std::set<const MSVehicle*, ComparatorNumericalIdLess>& toRemove,
1901 : std::set<const MSVehicle*, ComparatorNumericalIdLess>& toTeleport) {
1902 32369962 : if (myIntermodalCollisionAction != COLLISION_ACTION_NONE && foeLane->getEdge().getPersons().size() > 0 && foeLane->hasPedestrians()) {
1903 : #ifdef DEBUG_PEDESTRIAN_COLLISIONS
1904 : if (DEBUG_COND) {
1905 : std::cout << SIMTIME << " detect pedestrian junction collisions stage=" << stage << " lane=" << getID() << " foeLane=" << foeLane->getID() << "\n";
1906 : }
1907 : #endif
1908 111524 : const std::vector<MSTransportable*>& persons = foeLane->getEdge().getSortedPersons(timestep);
1909 1262890 : for (std::vector<MSTransportable*>::const_iterator it_p = persons.begin(); it_p != persons.end(); ++it_p) {
1910 : #ifdef DEBUG_PEDESTRIAN_COLLISIONS
1911 : if (DEBUG_COND) {
1912 : std::cout << " collider=" << collider->getID()
1913 : << " ped=" << (*it_p)->getID()
1914 : << " jammed=" << (*it_p)->isJammed()
1915 : << " colliderBoundary=" << colliderBoundary
1916 : << " pedBoundary=" << (*it_p)->getBoundingBox()
1917 : << "\n";
1918 : }
1919 : #endif
1920 1151366 : if ((*it_p)->isJammed()) {
1921 876 : continue;
1922 : }
1923 2300980 : if (colliderBoundary.overlapsWith((*it_p)->getBoundingBox())
1924 1150490 : && collider->getBoundingPoly().overlapsWith((*it_p)->getBoundingBox())) {
1925 3874 : std::string collisionType = "junctionPedestrian";
1926 3874 : if (foeLane->isCrossing()) {
1927 : collisionType = "crossing";
1928 3654 : } else if (foeLane->isWalkingArea()) {
1929 : collisionType = "walkingarea";
1930 : }
1931 3874 : handleIntermodalCollisionBetween(timestep, stage, collider, *it_p, 0, collisionType, toRemove, toTeleport);
1932 : }
1933 : }
1934 111524 : }
1935 32369962 : }
1936 :
1937 :
1938 : bool
1939 2364479201 : MSLane::detectCollisionBetween(SUMOTime timestep, const std::string& stage, MSVehicle* collider, MSVehicle* victim,
1940 : std::set<const MSVehicle*, ComparatorNumericalIdLess>& toRemove,
1941 : std::set<const MSVehicle*, ComparatorNumericalIdLess>& toTeleport) const {
1942 4704389212 : if (myCollisionAction == COLLISION_ACTION_TELEPORT && ((victim->hasInfluencer() && victim->getInfluencer().isRemoteAffected(timestep)) ||
1943 2340171721 : (collider->hasInfluencer() && collider->getInfluencer().isRemoteAffected(timestep)))) {
1944 17 : return false;
1945 : }
1946 :
1947 : // No self-collisions! (This is assumed to be ensured at caller side)
1948 2364479184 : if (collider == victim) {
1949 : return false;
1950 : }
1951 :
1952 2364479121 : const bool colliderOpposite = collider->getLaneChangeModel().isOpposite() || collider->isBidiOn(this);
1953 2364479121 : const bool victimOpposite = victim->getLaneChangeModel().isOpposite() || victim->isBidiOn(this);
1954 2364479121 : const bool bothOpposite = victimOpposite && colliderOpposite;
1955 2364479121 : if (bothOpposite) {
1956 : std::swap(victim, collider);
1957 : }
1958 2364479121 : const double colliderPos = colliderOpposite && !bothOpposite ? collider->getBackPositionOnLane(this) : collider->getPositionOnLane(this);
1959 2364479121 : const double minGapFactor = myCollisionMinGapFactor >= 0 ? myCollisionMinGapFactor : collider->getCarFollowModel().getCollisionMinGapFactor();
1960 2364479121 : double victimBack = victimOpposite && !bothOpposite ? victim->getPositionOnLane(this) : victim->getBackPositionOnLane(this);
1961 2364479121 : if (victim->getLateralOverlap() > 0 || collider->getLateralOverlap() > 0) {
1962 144626264 : if (&collider->getLane()->getEdge() == myEdge && collider->getLane()->getLength() > getLength()) {
1963 : // interpret victim position on the longer lane
1964 836 : victimBack *= collider->getLane()->getLength() / getLength();
1965 : }
1966 : }
1967 2364479121 : double gap = victimBack - colliderPos - minGapFactor * collider->getVehicleType().getMinGap();
1968 2364479121 : if (bothOpposite) {
1969 1297474 : gap = colliderPos - victimBack - minGapFactor * collider->getVehicleType().getMinGap();
1970 2363181647 : } else if (colliderOpposite) {
1971 : // vehicles are back to back so (frontal) minGap doesn't apply
1972 3602983 : gap += minGapFactor * collider->getVehicleType().getMinGap();
1973 : }
1974 : #ifdef DEBUG_COLLISIONS
1975 : if (DEBUG_COND && (DEBUG_COND2(collider) || DEBUG_COND2(victim))) {
1976 : std::cout << SIMTIME
1977 : << " thisLane=" << getID()
1978 : << " collider=" << collider->getID()
1979 : << " victim=" << victim->getID()
1980 : << " colOpposite=" << colliderOpposite
1981 : << " vicOpposite=" << victimOpposite
1982 : << " colLane=" << collider->getLane()->getID()
1983 : << " vicLane=" << victim->getLane()->getID()
1984 : << " colPos=" << colliderPos
1985 : << " vicBack=" << victimBack
1986 : << " colLat=" << collider->getCenterOnEdge(this)
1987 : << " vicLat=" << victim->getCenterOnEdge(this)
1988 : << " minGap=" << collider->getVehicleType().getMinGap()
1989 : << " minGapFactor=" << minGapFactor
1990 : << " gap=" << gap
1991 : << "\n";
1992 : }
1993 : #endif
1994 2364479121 : if (victimOpposite && gap < -(collider->getLength() + victim->getLength())) {
1995 : // already past each other
1996 : return false;
1997 : }
1998 2364462655 : if (gap < -NUMERICAL_EPS) {
1999 : double latGap = 0;
2000 12097725 : if (MSGlobals::gSublane) {
2001 12091378 : latGap = (fabs(victim->getCenterOnEdge(this) - collider->getCenterOnEdge(this))
2002 12091378 : - 0.5 * fabs(victim->getVehicleType().getWidth() + collider->getVehicleType().getWidth()));
2003 12091378 : if (latGap + NUMERICAL_EPS > 0) {
2004 : return false;
2005 : }
2006 : // account for ambiguous gap computation related to partial
2007 : // occupation of lanes with different lengths
2008 98239 : if (isInternal() && getEdge().getNumLanes() > 1 && victim->getLane() != collider->getLane()) {
2009 : double gapDelta = 0;
2010 1739 : const MSVehicle* otherLaneVeh = collider->getLane() == this ? victim : collider;
2011 1739 : if (otherLaneVeh->getLaneChangeModel().getShadowLane() == this) {
2012 653 : gapDelta = getLength() - otherLaneVeh->getLane()->getLength();
2013 : } else {
2014 1086 : for (const MSLane* cand : otherLaneVeh->getFurtherLanes()) {
2015 1086 : if (&cand->getEdge() == &getEdge()) {
2016 1086 : gapDelta = getLength() - cand->getLength();
2017 1086 : break;
2018 : }
2019 : }
2020 : }
2021 1739 : if (gap + gapDelta >= 0) {
2022 : return false;
2023 : }
2024 : }
2025 : }
2026 104586 : if (MSGlobals::gLaneChangeDuration > DELTA_T
2027 37 : && collider->getLaneChangeModel().isChangingLanes()
2028 31 : && victim->getLaneChangeModel().isChangingLanes()
2029 104586 : && victim->getLane() != this) {
2030 : // synchroneous lane change maneuver
2031 : return false;
2032 : }
2033 : #ifdef DEBUG_COLLISIONS
2034 : if (DEBUG_COND && (DEBUG_COND2(collider) || DEBUG_COND2(victim))) {
2035 : std::cout << SIMTIME << " detectedCollision gap=" << gap << " latGap=" << latGap << "\n";
2036 : }
2037 : #endif
2038 104586 : handleCollisionBetween(timestep, stage, collider, victim, gap, latGap, toRemove, toTeleport);
2039 104586 : return true;
2040 : }
2041 : return false;
2042 : }
2043 :
2044 :
2045 : void
2046 119085 : MSLane::handleCollisionBetween(SUMOTime timestep, const std::string& stage, const MSVehicle* collider, const MSVehicle* victim,
2047 : double gap, double latGap, std::set<const MSVehicle*, ComparatorNumericalIdLess>& toRemove,
2048 : std::set<const MSVehicle*, ComparatorNumericalIdLess>& toTeleport) const {
2049 119085 : if (collider->ignoreCollision() || victim->ignoreCollision()) {
2050 108905 : return;
2051 : }
2052 : std::string collisionType;
2053 : std::string collisionText;
2054 116167 : if (isFrontalCollision(collider, victim)) {
2055 : collisionType = "frontal";
2056 59 : collisionText = TL("frontal collision");
2057 116108 : } else if (stage == MSNet::STAGE_LANECHANGE) {
2058 : collisionType = "side";
2059 13188 : collisionText = TL("side collision");
2060 102920 : } else if (isInternal()) {
2061 : collisionType = "junction";
2062 14168 : collisionText = TL("junction collision");
2063 : } else {
2064 : collisionType = "collision";
2065 88752 : collisionText = TL("collision");
2066 : }
2067 :
2068 : // in frontal collisions the opposite vehicle is the collider
2069 116167 : if (victim->getLaneChangeModel().isOpposite() && !collider->getLaneChangeModel().isOpposite()) {
2070 : std::swap(collider, victim);
2071 : }
2072 464668 : std::string prefix = TLF("Vehicle '%'; % with vehicle '%", collider->getID(), collisionText, victim->getID());
2073 116167 : if (myCollisionStopTime > 0) {
2074 108099 : if (collider->collisionStopTime() >= 0 && victim->collisionStopTime() >= 0) {
2075 105987 : return;
2076 : }
2077 : std::string dummyError;
2078 2112 : SUMOVehicleParameter::Stop stop;
2079 2112 : stop.duration = myCollisionStopTime;
2080 2112 : stop.parametersSet |= STOP_DURATION_SET;
2081 2112 : const double collisionAngle = RAD2DEG(fabs(GeomHelper::angleDiff(victim->getAngle(), collider->getAngle())));
2082 : // determine new speeds from collision angle (@todo account for vehicle mass)
2083 2112 : double victimSpeed = victim->getSpeed();
2084 2112 : double colliderSpeed = collider->getSpeed();
2085 : // double victimOrigSpeed = victim->getSpeed();
2086 : // double colliderOrigSpeed = collider->getSpeed();
2087 2112 : if (collisionAngle < 45) {
2088 : // rear-end collisions
2089 : colliderSpeed = MIN2(colliderSpeed, victimSpeed);
2090 337 : } else if (collisionAngle < 135) {
2091 : // side collision
2092 326 : colliderSpeed /= 2;
2093 326 : victimSpeed /= 2;
2094 : } else {
2095 : // frontal collision
2096 : colliderSpeed = 0;
2097 : victimSpeed = 0;
2098 : }
2099 2112 : const double victimStopPos = MIN2(victim->getLane()->getLength(),
2100 2112 : victim->getPositionOnLane() + victim->getCarFollowModel().brakeGap(victimSpeed, victim->getCarFollowModel().getEmergencyDecel(), 0));
2101 2112 : if (victim->collisionStopTime() < 0) {
2102 1428 : stop.collision = true;
2103 1428 : stop.lane = victim->getLane()->getID();
2104 : // @todo: push victim forward?
2105 1428 : stop.startPos = victimStopPos;
2106 1428 : stop.endPos = stop.startPos;
2107 1428 : stop.parametersSet |= STOP_START_SET | STOP_END_SET;
2108 1428 : ((MSBaseVehicle*)victim)->addStop(stop, dummyError, 0);
2109 : }
2110 2112 : if (collider->collisionStopTime() < 0) {
2111 1654 : stop.collision = true;
2112 1654 : stop.lane = collider->getLane()->getID();
2113 1654 : stop.startPos = MIN2(collider->getPositionOnLane() + collider->getCarFollowModel().brakeGap(colliderSpeed, collider->getCarFollowModel().getEmergencyDecel(), 0),
2114 1654 : MAX3(0.0, victimStopPos - 0.75 * victim->getVehicleType().getLength(),
2115 1654 : collider->getPositionOnLane() - SPEED2DIST(collider->getSpeed())));
2116 1654 : stop.endPos = stop.startPos;
2117 1654 : stop.parametersSet |= STOP_START_SET | STOP_END_SET;
2118 1654 : ((MSBaseVehicle*)collider)->addStop(stop, dummyError, 0);
2119 : }
2120 : //std::cout << " collisionAngle=" << collisionAngle
2121 : // << "\n vPos=" << victim->getPositionOnLane() << " vStop=" << victimStopPos << " vSpeed=" << victimOrigSpeed << " vSpeed2=" << victimSpeed << " vSpeed3=" << victim->getSpeed()
2122 : // << "\n cPos=" << collider->getPositionOnLane() << " cStop=" << stop.startPos << " cSpeed=" << colliderOrigSpeed << " cSpeed2=" << colliderSpeed << " cSpeed3=" << collider->getSpeed()
2123 : // << "\n";
2124 2112 : } else {
2125 8068 : switch (myCollisionAction) {
2126 : case COLLISION_ACTION_WARN:
2127 : break;
2128 3567 : case COLLISION_ACTION_TELEPORT:
2129 7134 : prefix = TLF("Teleporting vehicle '%'; % with vehicle '%", collider->getID(), collisionText, victim->getID());
2130 : toRemove.insert(collider);
2131 : toTeleport.insert(collider);
2132 : break;
2133 210 : case COLLISION_ACTION_REMOVE: {
2134 420 : prefix = TLF("Removing % participants: vehicle '%', vehicle '%", collisionText, collider->getID(), victim->getID());
2135 : bool removeCollider = true;
2136 : bool removeVictim = true;
2137 210 : removeVictim = !(victim->hasInfluencer() && victim->getInfluencer()->isRemoteAffected(timestep));
2138 210 : removeCollider = !(collider->hasInfluencer() && collider->getInfluencer()->isRemoteAffected(timestep));
2139 210 : if (removeVictim) {
2140 : toRemove.insert(victim);
2141 : }
2142 210 : if (removeCollider) {
2143 : toRemove.insert(collider);
2144 : }
2145 210 : if (!removeVictim) {
2146 0 : if (!removeCollider) {
2147 0 : prefix = TLF("Keeping remote-controlled % participants: vehicle '%', vehicle '%", collisionText, collider->getID(), victim->getID());
2148 : } else {
2149 0 : prefix = TLF("Removing % participant: vehicle '%', keeping remote-controlled vehicle '%", collisionText, collider->getID(), victim->getID());
2150 : }
2151 210 : } else if (!removeCollider) {
2152 0 : prefix = TLF("Keeping remote-controlled % participant: vehicle '%', removing vehicle '%", collisionText, collider->getID(), victim->getID());
2153 : }
2154 : break;
2155 : }
2156 : default:
2157 : break;
2158 : }
2159 : }
2160 10180 : const bool newCollision = MSNet::getInstance()->registerCollision(collider, victim, collisionType, this, collider->getPositionOnLane(this));
2161 10180 : if (newCollision) {
2162 30156 : WRITE_WARNINGF(prefix + "', lane='%', gap=%%, time=%, stage=%.",
2163 : getID(), toString(gap), (MSGlobals::gSublane ? TL(", latGap=") + toString(latGap) : ""),
2164 : time2string(timestep), stage);
2165 6105 : MSNet::getInstance()->informVehicleStateListener(victim, MSNet::VehicleState::COLLISION);
2166 6105 : MSNet::getInstance()->informVehicleStateListener(collider, MSNet::VehicleState::COLLISION);
2167 6105 : MSNet::getInstance()->getVehicleControl().countCollision(myCollisionAction == COLLISION_ACTION_TELEPORT);
2168 : }
2169 : #ifdef DEBUG_COLLISIONS
2170 : if (DEBUG_COND2(collider)) {
2171 : toRemove.erase(collider);
2172 : toTeleport.erase(collider);
2173 : }
2174 : if (DEBUG_COND2(victim)) {
2175 : toRemove.erase(victim);
2176 : toTeleport.erase(victim);
2177 : }
2178 : #endif
2179 : }
2180 :
2181 :
2182 : void
2183 3971 : MSLane::handleIntermodalCollisionBetween(SUMOTime timestep, const std::string& stage, const MSVehicle* collider, const MSTransportable* victim,
2184 : double gap, const std::string& collisionType,
2185 : std::set<const MSVehicle*, ComparatorNumericalIdLess>& toRemove,
2186 : std::set<const MSVehicle*, ComparatorNumericalIdLess>& toTeleport) const {
2187 3971 : if (collider->ignoreCollision()) {
2188 3304 : return;
2189 : }
2190 7942 : std::string prefix = TLF("Vehicle '%'", collider->getID());
2191 3971 : if (myIntermodalCollisionStopTime > 0) {
2192 3344 : if (collider->collisionStopTime() >= 0) {
2193 3304 : return;
2194 : }
2195 : std::string dummyError;
2196 40 : SUMOVehicleParameter::Stop stop;
2197 40 : stop.duration = myIntermodalCollisionStopTime;
2198 40 : stop.parametersSet |= STOP_DURATION_SET;
2199 : // determine new speeds from collision angle (@todo account for vehicle mass)
2200 40 : double colliderSpeed = collider->getSpeed();
2201 40 : const double victimStopPos = victim->getEdgePos();
2202 : // double victimOrigSpeed = victim->getSpeed();
2203 : // double colliderOrigSpeed = collider->getSpeed();
2204 40 : if (collider->collisionStopTime() < 0) {
2205 40 : stop.collision = true;
2206 40 : stop.lane = collider->getLane()->getID();
2207 40 : stop.startPos = MIN2(collider->getPositionOnLane() + collider->getCarFollowModel().brakeGap(colliderSpeed, collider->getCarFollowModel().getEmergencyDecel(), 0),
2208 40 : MAX3(0.0, victimStopPos - 0.75 * victim->getVehicleType().getLength(),
2209 40 : collider->getPositionOnLane() - SPEED2DIST(collider->getSpeed())));
2210 40 : stop.endPos = stop.startPos;
2211 40 : stop.parametersSet |= STOP_START_SET | STOP_END_SET;
2212 40 : ((MSBaseVehicle*)collider)->addStop(stop, dummyError, 0);
2213 : }
2214 40 : } else {
2215 627 : switch (myIntermodalCollisionAction) {
2216 : case COLLISION_ACTION_WARN:
2217 : break;
2218 15 : case COLLISION_ACTION_TELEPORT:
2219 30 : prefix = TLF("Teleporting vehicle '%' after", collider->getID());
2220 : toRemove.insert(collider);
2221 : toTeleport.insert(collider);
2222 : break;
2223 15 : case COLLISION_ACTION_REMOVE: {
2224 30 : prefix = TLF("Removing vehicle '%' after", collider->getID());
2225 : bool removeCollider = true;
2226 15 : removeCollider = !(collider->hasInfluencer() && collider->getInfluencer()->isRemoteAffected(timestep));
2227 : if (!removeCollider) {
2228 0 : prefix = TLF("Keeping remote-controlled vehicle '%' after", collider->getID());
2229 : } else {
2230 : toRemove.insert(collider);
2231 : }
2232 : break;
2233 : }
2234 : default:
2235 : break;
2236 : }
2237 : }
2238 667 : const bool newCollision = MSNet::getInstance()->registerCollision(collider, victim, collisionType, this, victim->getEdgePos());
2239 667 : if (newCollision) {
2240 275 : if (gap != 0) {
2241 465 : WRITE_WARNING(prefix + TLF(" collision with person '%', lane='%', gap=%, time=%, stage=%.",
2242 : victim->getID(), getID(), gap, time2string(timestep), stage));
2243 : } else {
2244 910 : WRITE_WARNING(prefix + TLF(" collision with person '%', lane='%', time=%, stage=%.",
2245 : victim->getID(), getID(), time2string(timestep), stage));
2246 : }
2247 275 : MSNet::getInstance()->informVehicleStateListener(collider, MSNet::VehicleState::COLLISION);
2248 275 : MSNet::getInstance()->getVehicleControl().countCollision(myIntermodalCollisionAction == COLLISION_ACTION_TELEPORT);
2249 : }
2250 : #ifdef DEBUG_COLLISIONS
2251 : if (DEBUG_COND2(collider)) {
2252 : toRemove.erase(collider);
2253 : toTeleport.erase(collider);
2254 : }
2255 : #endif
2256 : }
2257 :
2258 :
2259 : bool
2260 116167 : MSLane::isFrontalCollision(const MSVehicle* collider, const MSVehicle* victim) {
2261 116167 : if (collider->getLaneChangeModel().isOpposite() != victim->getLaneChangeModel().isOpposite()) {
2262 : return true;
2263 : } else {
2264 116149 : const MSEdge* victimBidi = victim->getLane()->getEdge().getBidiEdge();
2265 116149 : if (&collider->getLane()->getEdge() == victimBidi) {
2266 : return true;
2267 : } else {
2268 153997 : for (MSLane* further : collider->getFurtherLanes()) {
2269 37889 : if (&further->getEdge() == victimBidi) {
2270 : return true;
2271 : }
2272 : }
2273 : }
2274 : }
2275 : return false;
2276 : }
2277 :
2278 : void
2279 96903732 : MSLane::executeMovements(const SUMOTime t) {
2280 : // multithreading: there are concurrent writes to myNeedsCollisionCheck but all of them should set it to true
2281 96903732 : myNeedsCollisionCheck = true;
2282 96903732 : MSLane* bidi = getBidiLane();
2283 96903732 : if (bidi != nullptr && bidi->getVehicleNumber() == 0) {
2284 488337 : MSNet::getInstance()->getEdgeControl().checkCollisionForInactive(bidi);
2285 : }
2286 96903732 : MSVehicle* firstNotStopped = nullptr;
2287 : // iterate over vehicles in reverse so that move reminders will be called in the correct order
2288 783889705 : for (VehCont::reverse_iterator i = myVehicles.rbegin(); i != myVehicles.rend();) {
2289 691010757 : MSVehicle* veh = *i;
2290 : // length is needed later when the vehicle may not exist anymore
2291 691010757 : const double length = veh->getVehicleType().getLengthWithGap();
2292 691010757 : const double nettoLength = veh->getVehicleType().getLength();
2293 691010757 : const bool moved = veh->executeMove();
2294 : MSLane* const target = veh->getMutableLane();
2295 686985973 : if (veh->hasArrived()) {
2296 : // vehicle has reached its arrival position
2297 : #ifdef DEBUG_EXEC_MOVE
2298 : if DEBUG_COND2(veh) {
2299 : std::cout << SIMTIME << " veh " << veh->getID() << " has arrived." << std::endl;
2300 : }
2301 : #endif
2302 3406778 : veh->onRemovalFromNet(MSMoveReminder::NOTIFICATION_ARRIVED);
2303 3406778 : MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(veh);
2304 683579195 : } else if (target != nullptr && moved) {
2305 17130022 : if (target->getEdge().isVaporizing()) {
2306 : // vehicle has reached a vaporizing edge
2307 756 : veh->onRemovalFromNet(MSMoveReminder::NOTIFICATION_VAPORIZED_VAPORIZER);
2308 756 : MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(veh);
2309 : } else {
2310 : // vehicle has entered a new lane (leaveLane and workOnMoveReminders were already called in MSVehicle::executeMove)
2311 17129266 : target->myVehBuffer.push_back(veh);
2312 17129266 : MSNet::getInstance()->getEdgeControl().needsVehicleIntegration(target);
2313 17129266 : if (MSGlobals::gSublane && veh->getLaneChangeModel().getShadowLane() != nullptr) {
2314 : // trigger sorting of partial vehicles as their order may have changed (lane might not be active and only contain partial vehicles)
2315 56787 : MSNet::getInstance()->getEdgeControl().needsVehicleIntegration(veh->getLaneChangeModel().getShadowLane());
2316 : }
2317 : }
2318 666449173 : } else if (veh->isParking()) {
2319 : // vehicle started to park
2320 15517 : MSVehicleTransfer::getInstance()->add(t, veh);
2321 15517 : myParkingVehicles.insert(veh);
2322 666433656 : } else if (veh->brokeDown()) {
2323 12 : veh->resumeFromStopping();
2324 36 : WRITE_WARNINGF(TL("Removing vehicle '%' after breaking down, lane='%', time=%."),
2325 : veh->getID(), veh->getLane()->getID(), time2string(t));
2326 12 : veh->onRemovalFromNet(MSMoveReminder::NOTIFICATION_VAPORIZED_BREAKDOWN);
2327 12 : MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(veh);
2328 666433644 : } else if (veh->isJumping()) {
2329 : // vehicle jumps to next route edge
2330 953 : MSVehicleTransfer::getInstance()->add(t, veh);
2331 666432691 : } else if (veh->getPositionOnLane() > getLength()) {
2332 : // for any reasons the vehicle is beyond its lane...
2333 : // this should never happen because it is handled in MSVehicle::executeMove
2334 : assert(false);
2335 0 : WRITE_WARNINGF(TL("Teleporting vehicle '%'; beyond end of lane, target lane='%', time=%."),
2336 : veh->getID(), getID(), time2string(t));
2337 0 : MSNet::getInstance()->getVehicleControl().countCollision(true);
2338 0 : MSVehicleTransfer::getInstance()->add(t, veh);
2339 :
2340 666432691 : } else if (veh->collisionStopTime() == 0) {
2341 3303 : veh->resumeFromStopping();
2342 3303 : if (getCollisionAction() == COLLISION_ACTION_REMOVE) {
2343 531 : WRITE_WARNINGF(TL("Removing vehicle '%' after earlier collision, lane='%', time=%."),
2344 : veh->getID(), veh->getLane()->getID(), time2string(t));
2345 177 : veh->onRemovalFromNet(MSMoveReminder::NOTIFICATION_VAPORIZED_COLLISION);
2346 177 : MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(veh);
2347 3126 : } else if (getCollisionAction() == COLLISION_ACTION_TELEPORT) {
2348 6879 : WRITE_WARNINGF(TL("Teleporting vehicle '%' after earlier collision, lane='%', time=%."),
2349 : veh->getID(), veh->getLane()->getID(), time2string(t));
2350 2293 : MSVehicleTransfer::getInstance()->add(t, veh);
2351 : } else {
2352 833 : if (firstNotStopped == nullptr && !(*i)->isStopped() && (*i)->getLane() == this) {
2353 557 : firstNotStopped = *i;
2354 : }
2355 : ++i;
2356 833 : continue;
2357 : }
2358 : } else {
2359 666429388 : if (firstNotStopped == nullptr && !(*i)->isStopped() && (*i)->getLane() == this) {
2360 80975199 : firstNotStopped = *i;
2361 : }
2362 : ++i;
2363 666429388 : continue;
2364 : }
2365 20555752 : myBruttoVehicleLengthSumToRemove += length;
2366 20555752 : myNettoVehicleLengthSumToRemove += nettoLength;
2367 : ++i;
2368 20555752 : i = VehCont::reverse_iterator(myVehicles.erase(i.base()));
2369 : }
2370 92878948 : if (firstNotStopped != nullptr) {
2371 80975756 : const SUMOTime ttt = firstNotStopped->getVehicleType().getParameter().getTimeToTeleport(MSGlobals::gTimeToGridlock);
2372 80975756 : const SUMOTime tttb = firstNotStopped->getVehicleType().getParameter().getTimeToTeleportBidi(MSGlobals::gTimeToTeleportBidi);
2373 80975756 : if (ttt > 0 || MSGlobals::gTimeToGridlockHighways > 0 || MSGlobals::gTimeToTeleportDisconnected >= 0 || tttb > 0 || MSGlobals::gTimeToTeleportRSDeadlock > 0) {
2374 77930666 : const bool wrongLane = !appropriate(firstNotStopped);
2375 77930666 : const bool disconnected = (MSGlobals::gTimeToTeleportDisconnected >= 0
2376 40691 : && firstNotStopped->succEdge(1) != nullptr
2377 77967486 : && firstNotStopped->getEdge()->allowedLanes(*firstNotStopped->succEdge(1), firstNotStopped->getVClass()) == nullptr);
2378 :
2379 77930666 : const bool r1 = ttt > 0 && firstNotStopped->getWaitingTime() > ttt && !disconnected
2380 : // never teleport a taxi on the last edge of it's route (where it would exit the simulation)
2381 77938291 : && (firstNotStopped->getDevice(typeid(MSDevice_Taxi)) == nullptr || firstNotStopped->getRoutePosition() < (firstNotStopped->getRoute().size() - 1));
2382 77923425 : const bool r2 = !r1 && MSGlobals::gTimeToGridlockHighways > 0
2383 1025 : && firstNotStopped->getWaitingTime() > MSGlobals::gTimeToGridlockHighways
2384 602 : && getSpeedLimit() > MSGlobals::gGridlockHighwaysSpeed && wrongLane
2385 602 : && !disconnected;
2386 77930654 : const bool r3 = disconnected && firstNotStopped->getWaitingTime() > MSGlobals::gTimeToTeleportDisconnected;
2387 77923413 : const bool r4 = !r1 && !r2 && !r3 && tttb > 0
2388 77931110 : && firstNotStopped->getWaitingTime() > tttb && getBidiLane() && !disconnected;
2389 475977 : const bool r5 = MSGlobals::gTimeToTeleportRSDeadlock > 0 && MSRailSignalControl::hasInstance() && !r1 && !r2 && !r3 && !r4
2390 78359944 : && firstNotStopped->getWaitingTime() > MSGlobals::gTimeToTeleportRSDeadlock && MSRailSignalControl::getInstance().haveDeadlock(firstNotStopped);
2391 77930666 : if (r1 || r2 || r3 || r4 || r5) {
2392 7352 : const std::vector<MSLink*>::const_iterator link = succLinkSec(*firstNotStopped, 1, *this, firstNotStopped->getBestLanesContinuation());
2393 7352 : const bool minorLink = !wrongLane && (link != myLinks.end()) && !((*link)->havePriority());
2394 9135 : std::string reason = (wrongLane ? " (wrong lane" : (minorLink ? " (yield" : " (jam"));
2395 7352 : myBruttoVehicleLengthSumToRemove += firstNotStopped->getVehicleType().getLengthWithGap();
2396 7352 : myNettoVehicleLengthSumToRemove += firstNotStopped->getVehicleType().getLength();
2397 7352 : if (firstNotStopped == myVehicles.back()) {
2398 : myVehicles.pop_back();
2399 : } else {
2400 305 : myVehicles.erase(std::find(myVehicles.begin(), myVehicles.end(), firstNotStopped));
2401 : reason = " (blocked";
2402 : }
2403 66057 : WRITE_WARNINGF("Teleporting vehicle '%'; waited too long" + reason
2404 : + (r2 ? ", highway" : "")
2405 : + (r3 ? ", disconnected" : "")
2406 : + (r4 ? ", bidi" : "")
2407 : + (r5 ? ", railSignal" : "")
2408 : + "), lane='%', time=%.", firstNotStopped->getID(), getID(), time2string(t));
2409 7352 : if (wrongLane) {
2410 945 : MSNet::getInstance()->getVehicleControl().registerTeleportWrongLane();
2411 6407 : } else if (minorLink) {
2412 4624 : MSNet::getInstance()->getVehicleControl().registerTeleportYield();
2413 : } else {
2414 1783 : MSNet::getInstance()->getVehicleControl().registerTeleportJam();
2415 : }
2416 7352 : if (MSGlobals::gRemoveGridlocked) {
2417 14 : firstNotStopped->onRemovalFromNet(MSMoveReminder::NOTIFICATION_TELEPORT_ARRIVED);
2418 14 : MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(firstNotStopped);
2419 : } else {
2420 7338 : MSVehicleTransfer::getInstance()->add(t, firstNotStopped);
2421 : }
2422 : }
2423 : }
2424 : }
2425 92878948 : if (MSGlobals::gSublane) {
2426 : // trigger sorting of vehicles as their order may have changed
2427 16495596 : MSNet::getInstance()->getEdgeControl().needsVehicleIntegration(this);
2428 : }
2429 92878948 : }
2430 :
2431 :
2432 : void
2433 261 : MSLane::markRecalculateBruttoSum() {
2434 261 : myRecalculateBruttoSum = true;
2435 261 : }
2436 :
2437 :
2438 : void
2439 96903729 : MSLane::updateLengthSum() {
2440 96903729 : myBruttoVehicleLengthSum -= myBruttoVehicleLengthSumToRemove;
2441 96903729 : myNettoVehicleLengthSum -= myNettoVehicleLengthSumToRemove;
2442 96903729 : myBruttoVehicleLengthSumToRemove = 0;
2443 96903729 : myNettoVehicleLengthSumToRemove = 0;
2444 96903729 : if (myVehicles.empty()) {
2445 : // avoid numerical instability
2446 8063655 : myBruttoVehicleLengthSum = 0;
2447 8063655 : myNettoVehicleLengthSum = 0;
2448 88840074 : } else if (myRecalculateBruttoSum) {
2449 193 : myBruttoVehicleLengthSum = 0;
2450 715 : for (VehCont::const_iterator i = myVehicles.begin(); i != myVehicles.end(); ++i) {
2451 522 : myBruttoVehicleLengthSum += (*i)->getVehicleType().getLengthWithGap();
2452 : }
2453 193 : myRecalculateBruttoSum = false;
2454 : }
2455 96903729 : }
2456 :
2457 :
2458 : void
2459 0 : MSLane::changeLanes(const SUMOTime t) {
2460 0 : myEdge->changeLanes(t);
2461 0 : }
2462 :
2463 :
2464 : const MSEdge*
2465 5579333 : MSLane::getNextNormal() const {
2466 5579333 : return myEdge->getNormalSuccessor();
2467 : }
2468 :
2469 :
2470 : const MSLane*
2471 127082 : MSLane::getFirstInternalInConnection(double& offset) const {
2472 127082 : if (!this->isInternal()) {
2473 : return nullptr;
2474 : }
2475 127082 : offset = 0.;
2476 : const MSLane* firstInternal = this;
2477 127082 : MSLane* pred = getCanonicalPredecessorLane();
2478 127130 : while (pred != nullptr && pred->isInternal()) {
2479 : firstInternal = pred;
2480 48 : offset += pred->getLength();
2481 48 : pred = firstInternal->getCanonicalPredecessorLane();
2482 : }
2483 : return firstInternal;
2484 : }
2485 :
2486 :
2487 : // ------ Static (sic!) container methods ------
2488 : bool
2489 2005840 : MSLane::dictionary(const std::string& id, MSLane* ptr) {
2490 : const DictType::iterator it = myDict.lower_bound(id);
2491 2005840 : if (it == myDict.end() || it->first != id) {
2492 : // id not in myDict
2493 2005832 : myDict.emplace_hint(it, id, ptr);
2494 2005832 : return true;
2495 : }
2496 : return false;
2497 : }
2498 :
2499 :
2500 : MSLane*
2501 8542204 : MSLane::dictionary(const std::string& id) {
2502 : const DictType::iterator it = myDict.find(id);
2503 8542204 : if (it == myDict.end()) {
2504 : // id not in myDict
2505 : return nullptr;
2506 : }
2507 8540508 : return it->second;
2508 : }
2509 :
2510 :
2511 : void
2512 40224 : MSLane::clear() {
2513 2027007 : for (DictType::iterator i = myDict.begin(); i != myDict.end(); ++i) {
2514 1986783 : delete (*i).second;
2515 : }
2516 : myDict.clear();
2517 40224 : }
2518 :
2519 :
2520 : void
2521 266 : MSLane::insertIDs(std::vector<std::string>& into) {
2522 9036 : for (DictType::iterator i = myDict.begin(); i != myDict.end(); ++i) {
2523 8770 : into.push_back((*i).first);
2524 : }
2525 266 : }
2526 :
2527 :
2528 : template<class RTREE> void
2529 555 : MSLane::fill(RTREE& into) {
2530 24886 : for (DictType::iterator i = myDict.begin(); i != myDict.end(); ++i) {
2531 24331 : MSLane* l = (*i).second;
2532 24331 : Boundary b = l->getShape().getBoxBoundary();
2533 24331 : b.grow(3.);
2534 24331 : const float cmin[2] = {(float) b.xmin(), (float) b.ymin()};
2535 24331 : const float cmax[2] = {(float) b.xmax(), (float) b.ymax()};
2536 24331 : into.Insert(cmin, cmax, l);
2537 : }
2538 555 : }
2539 :
2540 : template void MSLane::fill<NamedRTree>(NamedRTree& into);
2541 : template void MSLane::fill<LANE_RTREE_QUAL>(LANE_RTREE_QUAL& into);
2542 :
2543 : // ------ ------
2544 : bool
2545 77930666 : MSLane::appropriate(const MSVehicle* veh) const {
2546 77930666 : if (veh->getLaneChangeModel().isOpposite()) {
2547 : return false;
2548 : }
2549 77820414 : if (myEdge->isInternal()) {
2550 : return true;
2551 : }
2552 72880842 : if (veh->succEdge(1) == nullptr) {
2553 : assert((int)veh->getBestLanes().size() > veh->getLaneIndex());
2554 19869085 : if (veh->getBestLanes()[veh->getLaneIndex()].bestLaneOffset == 0) {
2555 : return true;
2556 : } else {
2557 : return false;
2558 : }
2559 : }
2560 53011757 : std::vector<MSLink*>::const_iterator link = succLinkSec(*veh, 1, *this, veh->getBestLanesContinuation());
2561 : return (link != myLinks.end());
2562 : }
2563 :
2564 :
2565 : void
2566 33681649 : MSLane::integrateNewVehicles() {
2567 33681649 : myNeedsCollisionCheck = true;
2568 : std::vector<MSVehicle*>& buffered = myVehBuffer.getContainer();
2569 33681649 : sort(buffered.begin(), buffered.end(), vehicle_position_sorter(this));
2570 50810915 : for (MSVehicle* const veh : buffered) {
2571 : assert(veh->getLane() == this);
2572 17129266 : myVehicles.insert(myVehicles.begin(), veh);
2573 17129266 : myBruttoVehicleLengthSum += veh->getVehicleType().getLengthWithGap();
2574 17129266 : myNettoVehicleLengthSum += veh->getVehicleType().getLength();
2575 : //if (true) std::cout << SIMTIME << " integrateNewVehicle lane=" << getID() << " veh=" << veh->getID() << " (on lane " << veh->getLane()->getID() << ") into lane=" << getID() << " myBrutto=" << myBruttoVehicleLengthSum << "\n";
2576 17129266 : myEdge->markDelayed();
2577 : }
2578 : buffered.clear();
2579 : myVehBuffer.unlock();
2580 : //std::cout << SIMTIME << " integrateNewVehicle lane=" << getID() << " myVehicles1=" << toString(myVehicles);
2581 33681649 : if (MSGlobals::gLateralResolution > 0 || myOpposite != nullptr) {
2582 19077977 : sort(myVehicles.begin(), myVehicles.end(), vehicle_natural_position_sorter(this));
2583 : }
2584 33681649 : sortPartialVehicles();
2585 : #ifdef DEBUG_VEHICLE_CONTAINER
2586 : if (DEBUG_COND) std::cout << SIMTIME << " integrateNewVehicle lane=" << getID()
2587 : << " vehicles=" << toString(myVehicles) << " partials=" << toString(myPartialVehicles) << "\n";
2588 : #endif
2589 33681649 : }
2590 :
2591 :
2592 : void
2593 122703585 : MSLane::sortPartialVehicles() {
2594 122703585 : if (myPartialVehicles.size() > 1) {
2595 1563479 : sort(myPartialVehicles.begin(), myPartialVehicles.end(), vehicle_natural_position_sorter(this));
2596 : }
2597 122703585 : }
2598 :
2599 :
2600 : void
2601 19751683 : MSLane::sortManeuverReservations() {
2602 19751683 : if (myManeuverReservations.size() > 1) {
2603 : #ifdef DEBUG_CONTEXT
2604 : if (DEBUG_COND) {
2605 : std::cout << "sortManeuverReservations on lane " << getID()
2606 : << "\nBefore sort: " << toString(myManeuverReservations) << std::endl;
2607 : }
2608 : #endif
2609 23199 : sort(myManeuverReservations.begin(), myManeuverReservations.end(), vehicle_natural_position_sorter(this));
2610 : #ifdef DEBUG_CONTEXT
2611 : if (DEBUG_COND) {
2612 : std::cout << "After sort: " << toString(myManeuverReservations) << std::endl;
2613 : }
2614 : #endif
2615 : }
2616 19751683 : }
2617 :
2618 :
2619 : bool
2620 6537877987 : MSLane::isInternal() const {
2621 6537877987 : return myEdge->isInternal();
2622 : }
2623 :
2624 :
2625 : bool
2626 51598221 : MSLane::isNormal() const {
2627 51598221 : return myEdge->isNormal();
2628 : }
2629 :
2630 :
2631 : bool
2632 117254084 : MSLane::isCrossing() const {
2633 117254084 : return myEdge->isCrossing();
2634 : }
2635 :
2636 :
2637 : bool
2638 862934 : MSLane::isPriorityCrossing() const {
2639 862934 : return isCrossing() && getIncomingLanes()[0].viaLink->getOffState() == LINKSTATE_MAJOR;
2640 : }
2641 :
2642 :
2643 : bool
2644 121978151 : MSLane::isWalkingArea() const {
2645 121978151 : return myEdge->isWalkingArea();
2646 : }
2647 :
2648 :
2649 : MSVehicle*
2650 599363650 : MSLane::getLastFullVehicle() const {
2651 599363650 : if (myVehicles.size() == 0) {
2652 : return nullptr;
2653 : }
2654 536644796 : return myVehicles.front();
2655 : }
2656 :
2657 :
2658 : MSVehicle*
2659 76039 : MSLane::getFirstFullVehicle() const {
2660 76039 : if (myVehicles.size() == 0) {
2661 : return nullptr;
2662 : }
2663 24164 : return myVehicles.back();
2664 : }
2665 :
2666 :
2667 : MSVehicle*
2668 400456084 : MSLane::getLastAnyVehicle() const {
2669 : // all vehicles in myVehicles should have positions smaller or equal to
2670 : // those in myPartialVehicles (unless we're on a bidi-lane)
2671 400456084 : if (myVehicles.size() > 0) {
2672 317384368 : if (myBidiLane != nullptr && myPartialVehicles.size() > 0) {
2673 24076 : if (myVehicles.front()->getPositionOnLane() > myPartialVehicles.front()->getPositionOnLane(this)) {
2674 7238 : return myPartialVehicles.front();
2675 : }
2676 : }
2677 317377130 : return myVehicles.front();
2678 : }
2679 83071716 : if (myPartialVehicles.size() > 0) {
2680 5269354 : return myPartialVehicles.front();
2681 : }
2682 : return nullptr;
2683 : }
2684 :
2685 :
2686 : MSVehicle*
2687 136524 : MSLane::getFirstAnyVehicle() const {
2688 : MSVehicle* result = nullptr;
2689 136524 : if (myVehicles.size() > 0) {
2690 136524 : result = myVehicles.back();
2691 : }
2692 : if (myPartialVehicles.size() > 0
2693 136524 : && (result == nullptr || result->getPositionOnLane(this) < myPartialVehicles.back()->getPositionOnLane(this))) {
2694 147 : result = myPartialVehicles.back();
2695 : }
2696 136524 : return result;
2697 : }
2698 :
2699 :
2700 : std::vector<MSLink*>::const_iterator
2701 1967994708 : MSLane::succLinkSec(const SUMOVehicle& veh, int nRouteSuccs,
2702 : const MSLane& succLinkSource, const std::vector<MSLane*>& conts) {
2703 1967994708 : const MSEdge* nRouteEdge = veh.succEdge(nRouteSuccs);
2704 : // check whether the vehicle tried to look beyond its route
2705 1967994708 : if (nRouteEdge == nullptr) {
2706 : // return end (no succeeding link) if so
2707 : return succLinkSource.myLinks.end();
2708 : }
2709 : // if we are on an internal lane there should only be one link and it must be allowed
2710 1376960584 : if (succLinkSource.isInternal()) {
2711 : assert(succLinkSource.myLinks.size() == 1);
2712 : // could have been disallowed dynamically with a rerouter or via TraCI
2713 : // assert(succLinkSource.myLinks[0]->getLane()->allowsVehicleClass(veh.getVehicleType().getVehicleClass()));
2714 : return succLinkSource.myLinks.begin();
2715 : }
2716 : // a link may be used if
2717 : // 1) there is a destination lane ((*link)->getLane()!=0)
2718 : // 2) the destination lane belongs to the next edge in route ((*link)->getLane()->myEdge == nRouteEdge)
2719 : // 3) the destination lane allows the vehicle's class ((*link)->getLane()->allowsVehicleClass(veh.getVehicleClass()))
2720 :
2721 : // there should be a link which leads to the next desired lane our route in "conts" (built in "getBestLanes")
2722 : // "conts" stores the best continuations of our current lane
2723 : // we should never return an arbitrary link since this may cause collisions
2724 :
2725 1083893401 : if (nRouteSuccs < (int)conts.size()) {
2726 : // we go through the links in our list and return the matching one
2727 1189199041 : for (std::vector<MSLink*>::const_iterator link = succLinkSource.myLinks.begin(); link != succLinkSource.myLinks.end(); ++link) {
2728 1188465887 : if ((*link)->getLane() != nullptr && (*link)->getLane()->myEdge == nRouteEdge
2729 1070169464 : && (*link)->getLane()->allowsVehicleClass(veh.getVClass())
2730 2258202685 : && ((*link)->getViaLane() == nullptr || (*link)->getViaLane()->allowsVehicleClass(veh.getVClass()))) {
2731 : // we should use the link if it connects us to the best lane
2732 1069732721 : if ((*link)->getLane() == conts[nRouteSuccs]) {
2733 1058820225 : return link;
2734 : }
2735 : }
2736 : }
2737 : } else {
2738 : // the source lane is a dead end (no continuations exist)
2739 : return succLinkSource.myLinks.end();
2740 : }
2741 : // the only case where this should happen is for a disconnected route (deliberately ignored)
2742 : #ifdef DEBUG_NO_CONNECTION
2743 : // the "'" around the ids are missing intentionally in the message below because it slows messaging down, resulting in test timeouts
2744 : WRITE_WARNING("Could not find connection between lane " + succLinkSource.getID() + " and lane " + conts[nRouteSuccs]->getID() +
2745 : " for vehicle " + veh.getID() + ", time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
2746 : #endif
2747 : return succLinkSource.myLinks.end();
2748 : }
2749 :
2750 :
2751 : const MSLink*
2752 588195792 : MSLane::getLinkTo(const MSLane* const target) const {
2753 588195792 : const bool internal = target->isInternal();
2754 758029098 : for (const MSLink* const l : myLinks) {
2755 725134142 : if ((internal && l->getViaLane() == target) || (!internal && l->getLane() == target)) {
2756 : return l;
2757 : }
2758 : }
2759 : return nullptr;
2760 : }
2761 :
2762 :
2763 : const MSLane*
2764 69984 : MSLane::getInternalFollowingLane(const MSLane* const target) const {
2765 119411 : for (const MSLink* const l : myLinks) {
2766 110433 : if (l->getLane() == target) {
2767 : return l->getViaLane();
2768 : }
2769 : }
2770 : return nullptr;
2771 : }
2772 :
2773 :
2774 : const MSLink*
2775 92400989 : MSLane::getEntryLink() const {
2776 92400989 : if (!isInternal()) {
2777 : return nullptr;
2778 : }
2779 : const MSLane* internal = this;
2780 92128861 : const MSLane* lane = this->getCanonicalPredecessorLane();
2781 : assert(lane != nullptr);
2782 93322169 : while (lane->isInternal()) {
2783 : internal = lane;
2784 1193308 : lane = lane->getCanonicalPredecessorLane();
2785 : assert(lane != nullptr);
2786 : }
2787 92128861 : return lane->getLinkTo(internal);
2788 : }
2789 :
2790 :
2791 : void
2792 1018 : MSLane::setMaxSpeed(double val, bool byVSS, bool byTraCI, double jamThreshold) {
2793 1018 : myMaxSpeed = val;
2794 1018 : mySpeedByVSS = byVSS;
2795 1018 : mySpeedByTraCI = byTraCI;
2796 1018 : myEdge->recalcCache();
2797 1018 : if (MSGlobals::gUseMesoSim) {
2798 207 : MESegment* first = MSGlobals::gMesoNet->getSegmentForEdge(*myEdge);
2799 1198 : while (first != nullptr) {
2800 991 : first->setSpeed(val, SIMSTEP, jamThreshold, myIndex);
2801 : first = first->getNextSegment();
2802 : }
2803 : }
2804 1018 : }
2805 :
2806 :
2807 : void
2808 85 : MSLane::setFrictionCoefficient(double val) {
2809 85 : myFrictionCoefficient = val;
2810 85 : myEdge->recalcCache();
2811 85 : }
2812 :
2813 :
2814 : void
2815 15 : MSLane::setLength(double val) {
2816 15 : myLength = val;
2817 15 : myEdge->recalcCache();
2818 15 : }
2819 :
2820 :
2821 : void
2822 88257639 : MSLane::swapAfterLaneChange(SUMOTime) {
2823 : //if (getID() == "disabled_lane") std::cout << SIMTIME << " swapAfterLaneChange lane=" << getID() << " myVehicles=" << toString(myVehicles) << " myTmpVehicles=" << toString(myTmpVehicles) << "\n";
2824 88257639 : myVehicles = myTmpVehicles;
2825 : myTmpVehicles.clear();
2826 : // this needs to be done after finishing lane-changing for all lanes on the
2827 : // current edge (MSLaneChanger::updateLanes())
2828 88257639 : sortPartialVehicles();
2829 88257639 : if (MSGlobals::gSublane && getOpposite() != nullptr) {
2830 423562 : getOpposite()->sortPartialVehicles();
2831 : }
2832 88257639 : if (myBidiLane != nullptr) {
2833 340735 : myBidiLane->sortPartialVehicles();
2834 : }
2835 88257639 : }
2836 :
2837 :
2838 : MSVehicle*
2839 25771 : MSLane::removeVehicle(MSVehicle* remVehicle, MSMoveReminder::Notification notification, bool notify) {
2840 : assert(remVehicle->getLane() == this);
2841 54894 : for (MSLane::VehCont::iterator it = myVehicles.begin(); it < myVehicles.end(); it++) {
2842 54878 : if (remVehicle == *it) {
2843 25755 : if (notify) {
2844 17565 : remVehicle->leaveLane(notification);
2845 : }
2846 25755 : myVehicles.erase(it);
2847 25755 : myBruttoVehicleLengthSum -= remVehicle->getVehicleType().getLengthWithGap();
2848 25755 : myNettoVehicleLengthSum -= remVehicle->getVehicleType().getLength();
2849 25755 : break;
2850 : }
2851 : }
2852 25771 : return remVehicle;
2853 : }
2854 :
2855 :
2856 : MSLane*
2857 86097697 : MSLane::getParallelLane(int offset, bool includeOpposite) const {
2858 86097697 : return myEdge->parallelLane(this, offset, includeOpposite);
2859 : }
2860 :
2861 :
2862 : void
2863 2578038 : MSLane::addIncomingLane(MSLane* lane, MSLink* viaLink) {
2864 : IncomingLaneInfo ili;
2865 2578038 : ili.lane = lane;
2866 2578038 : ili.viaLink = viaLink;
2867 2578038 : ili.length = lane->getLength();
2868 2578038 : myIncomingLanes.push_back(ili);
2869 2578038 : }
2870 :
2871 :
2872 : void
2873 2578038 : MSLane::addApproachingLane(MSLane* lane, bool warnMultiCon) {
2874 2578038 : MSEdge* approachingEdge = &lane->getEdge();
2875 2578038 : if (myApproachingLanes.find(approachingEdge) == myApproachingLanes.end()) {
2876 2546939 : myApproachingLanes[approachingEdge] = std::vector<MSLane*>();
2877 31099 : } else if (!approachingEdge->isInternal() && warnMultiCon) {
2878 : // whenever a normal edge connects twice, there is a corresponding
2879 : // internal edge wich connects twice, one warning is sufficient
2880 18 : WRITE_WARNINGF(TL("Lane '%' is approached multiple times from edge '%'. This may cause collisions."),
2881 : getID(), approachingEdge->getID());
2882 : }
2883 2578038 : myApproachingLanes[approachingEdge].push_back(lane);
2884 2578038 : }
2885 :
2886 :
2887 : bool
2888 100046228 : MSLane::isApproachedFrom(MSEdge* const edge, MSLane* const lane) {
2889 : std::map<MSEdge*, std::vector<MSLane*> >::const_iterator i = myApproachingLanes.find(edge);
2890 100046228 : if (i == myApproachingLanes.end()) {
2891 : return false;
2892 : }
2893 : const std::vector<MSLane*>& lanes = (*i).second;
2894 97793192 : return std::find(lanes.begin(), lanes.end(), lane) != lanes.end();
2895 : }
2896 :
2897 :
2898 3364974 : double MSLane::getMissingRearGap(const MSVehicle* leader, double backOffset, double leaderSpeed) const {
2899 : // this follows the same logic as getFollowerOnConsecutive. we do a tree
2900 : // search and check for the vehicle with the largest missing rear gap within
2901 : // relevant range
2902 : double result = 0;
2903 : const double leaderDecel = leader->getCarFollowModel().getMaxDecel();
2904 3364974 : CLeaderDist followerInfo = getFollowersOnConsecutive(leader, backOffset, false)[0];
2905 3364974 : const MSVehicle* v = followerInfo.first;
2906 3364974 : if (v != nullptr) {
2907 5452 : result = v->getCarFollowModel().getSecureGap(v, leader, v->getSpeed(), leaderSpeed, leaderDecel) - followerInfo.second;
2908 : }
2909 3364974 : return result;
2910 : }
2911 :
2912 :
2913 : double
2914 356639687 : MSLane::getMaximumBrakeDist() const {
2915 356639687 : const MSVehicleControl& vc = MSNet::getInstance()->getVehicleControl();
2916 356639687 : const double maxSpeed = getSpeedLimit() * vc.getMaxSpeedFactor();
2917 : // NOTE: For the euler update this is an upper bound on the actual braking distance (see ticket #860)
2918 : // impose a hard bound due to visibility / common sense to avoid unnecessary computation if there are strange vehicles in the fleet
2919 356639687 : const double minDecel = isRailway(myPermissions) ? vc.getMinDecelerationRail() : vc.getMinDeceleration();
2920 356639687 : return MIN2(maxSpeed * maxSpeed * 0.5 / minDecel,
2921 356639687 : myPermissions == SVC_SHIP ? 10000.0 : 1000.0);
2922 : }
2923 :
2924 :
2925 : std::pair<MSVehicle* const, double>
2926 39821819 : MSLane::getLeader(const MSVehicle* veh, const double vehPos, const std::vector<MSLane*>& bestLaneConts, double dist, bool checkTmpVehicles) const {
2927 : // get the leading vehicle for (shadow) veh
2928 : // XXX this only works as long as all lanes of an edge have equal length
2929 : #ifdef DEBUG_CONTEXT
2930 : if (DEBUG_COND2(veh)) {
2931 : std::cout << " getLeader lane=" << getID() << " ego=" << veh->getID() << " vehs=" << toString(myVehicles) << " tmpVehs=" << toString(myTmpVehicles) << "\n";
2932 : }
2933 : #endif
2934 39821819 : if (checkTmpVehicles) {
2935 241733744 : for (VehCont::const_iterator last = myTmpVehicles.begin(); last != myTmpVehicles.end(); ++last) {
2936 : // XXX refactor leaderInfo to use a const vehicle all the way through the call hierarchy
2937 239369814 : MSVehicle* pred = (MSVehicle*)*last;
2938 239369814 : if (pred == veh) {
2939 : continue;
2940 : }
2941 : #ifdef DEBUG_CONTEXT
2942 : if (DEBUG_COND2(veh)) {
2943 : std::cout << std::setprecision(gPrecision) << " getLeader lane=" << getID() << " ego=" << veh->getID() << " egoPos=" << vehPos << " pred=" << pred->getID() << " predPos=" << pred->getPositionOnLane() << "\n";
2944 : }
2945 : #endif
2946 204471276 : if (pred->getPositionOnLane() >= vehPos) {
2947 32602274 : return std::pair<MSVehicle* const, double>(pred, pred->getBackPositionOnLane(this) - veh->getVehicleType().getMinGap() - vehPos);
2948 : }
2949 : }
2950 : } else {
2951 46926294 : for (AnyVehicleIterator last = anyVehiclesBegin(); last != anyVehiclesEnd(); ++last) {
2952 : // XXX refactor leaderInfo to use a const vehicle all the way through the call hierarchy
2953 50895960 : MSVehicle* pred = (MSVehicle*)*last;
2954 50895960 : if (pred == veh) {
2955 3833114 : continue;
2956 : }
2957 : #ifdef DEBUG_CONTEXT
2958 : if (DEBUG_COND2(veh)) {
2959 : std::cout << " getLeader lane=" << getID() << " ego=" << veh->getID() << " egoPos=" << vehPos
2960 : << " pred=" << pred->getID() << " predPos=" << pred->getPositionOnLane(this) << " predBack=" << pred->getBackPositionOnLane(this) << "\n";
2961 : }
2962 : #endif
2963 47062846 : if (pred->getPositionOnLane(this) >= vehPos) {
2964 4087602 : if (MSGlobals::gLaneChangeDuration > 0
2965 333962 : && pred->getLaneChangeModel().isOpposite()
2966 89706 : && !pred->getLaneChangeModel().isChangingLanes()
2967 4100111 : && pred->getLaneChangeModel().getShadowLane() == this) {
2968 : // skip non-overlapping shadow
2969 58968 : continue;
2970 : }
2971 3969666 : return std::pair<MSVehicle* const, double>(pred, pred->getBackPositionOnLane(this) - veh->getVehicleType().getMinGap() - vehPos);
2972 : }
2973 : }
2974 : }
2975 : // XXX from here on the code mirrors MSLaneChanger::getRealLeader
2976 3249879 : if (bestLaneConts.size() > 0) {
2977 2923057 : double seen = getLength() - vehPos;
2978 2923057 : double speed = veh->getSpeed();
2979 2923057 : if (dist < 0) {
2980 31983 : dist = veh->getCarFollowModel().brakeGap(speed) + veh->getVehicleType().getMinGap();
2981 : }
2982 : #ifdef DEBUG_CONTEXT
2983 : if (DEBUG_COND2(veh)) {
2984 : std::cout << " getLeader lane=" << getID() << " seen=" << seen << " dist=" << dist << "\n";
2985 : }
2986 : #endif
2987 2923057 : if (seen > dist) {
2988 1107689 : return std::pair<MSVehicle* const, double>(static_cast<MSVehicle*>(nullptr), -1);
2989 : }
2990 1815368 : return getLeaderOnConsecutive(dist, seen, speed, *veh, bestLaneConts);
2991 : } else {
2992 326822 : return std::make_pair(static_cast<MSVehicle*>(nullptr), -1);
2993 : }
2994 : }
2995 :
2996 :
2997 : std::pair<MSVehicle* const, double>
2998 39718254 : MSLane::getLeaderOnConsecutive(double dist, double seen, double speed, const MSVehicle& veh,
2999 : const std::vector<MSLane*>& bestLaneConts, bool considerCrossingFoes) const {
3000 : #ifdef DEBUG_CONTEXT
3001 : if (DEBUG_COND2(&veh)) {
3002 : std::cout << " getLeaderOnConsecutive lane=" << getID() << " ego=" << veh.getID() << " seen=" << seen << " dist=" << dist << " conts=" << toString(bestLaneConts) << "\n";
3003 : }
3004 : #endif
3005 39718254 : if (seen > dist && !isInternal()) {
3006 114499 : return std::make_pair(static_cast<MSVehicle*>(nullptr), -1);
3007 : }
3008 : int view = 1;
3009 : // loop over following lanes
3010 39603755 : if (myPartialVehicles.size() > 0) {
3011 : // XXX
3012 1131147 : MSVehicle* pred = myPartialVehicles.front();
3013 1131147 : const double gap = seen - (getLength() - pred->getBackPositionOnLane(this)) - veh.getVehicleType().getMinGap();
3014 : #ifdef DEBUG_CONTEXT
3015 : if (DEBUG_COND2(&veh)) {
3016 : std::cout << " predGap=" << gap << " partials=" << toString(myPartialVehicles) << "\n";
3017 : }
3018 : #endif
3019 : // make sure pred is really a leader and not doing continous lane-changing behind ego
3020 1131147 : if (gap > 0) {
3021 769846 : return std::pair<MSVehicle* const, double>(pred, gap);
3022 : }
3023 : }
3024 : #ifdef DEBUG_CONTEXT
3025 : if (DEBUG_COND2(&veh)) {
3026 : gDebugFlag1 = true;
3027 : }
3028 : #endif
3029 : const MSLane* nextLane = this;
3030 : do {
3031 55640683 : nextLane->getVehiclesSecure(); // lock against running sim when called from GUI for time gap coloring
3032 : // get the next link used
3033 55640683 : std::vector<MSLink*>::const_iterator link = succLinkSec(veh, view, *nextLane, bestLaneConts);
3034 55640683 : if (nextLane->isLinkEnd(link) && view < veh.getRoute().size() - veh.getRoutePosition()) {
3035 4796166 : const MSEdge* nextEdge = *(veh.getCurrentRouteEdge() + view);
3036 4796166 : if (nextEdge->getNumLanes() == 1) {
3037 : // lanes are unambiguous on the next route edge, continue beyond bestLaneConts
3038 5018323 : for (link = nextLane->getLinkCont().begin(); link < nextLane->getLinkCont().end(); link++) {
3039 3194621 : if ((*link)->getLane() == nextEdge->getLanes().front()) {
3040 : break;
3041 : }
3042 : }
3043 : }
3044 : }
3045 55640683 : if (nextLane->isLinkEnd(link)) {
3046 : #ifdef DEBUG_CONTEXT
3047 : if (DEBUG_COND2(&veh)) {
3048 : std::cout << " cannot continue after nextLane=" << nextLane->getID() << "\n";
3049 : }
3050 : #endif
3051 14578232 : nextLane->releaseVehicles();
3052 14578232 : break;
3053 : }
3054 : // check for link leaders
3055 41062451 : const bool laneChanging = veh.getLane() != this;
3056 41062451 : const MSLink::LinkLeaders linkLeaders = (*link)->getLeaderInfo(&veh, seen);
3057 41062451 : nextLane->releaseVehicles();
3058 41062451 : if (linkLeaders.size() > 0) {
3059 : std::pair<MSVehicle*, double> result;
3060 : double shortestGap = std::numeric_limits<double>::max();
3061 2260277 : for (auto ll : linkLeaders) {
3062 : double gap = ll.vehAndGap.second;
3063 : MSVehicle* lVeh = ll.vehAndGap.first;
3064 1251190 : if (lVeh != nullptr) {
3065 : // leader is a vehicle, not a pedestrian
3066 1155488 : gap += lVeh->getCarFollowModel().brakeGap(lVeh->getSpeed(), lVeh->getCarFollowModel().getMaxDecel(), 0);
3067 : }
3068 : #ifdef DEBUG_CONTEXT
3069 : if (DEBUG_COND2(&veh)) {
3070 : std::cout << " linkLeader candidate " << Named::getIDSecure(lVeh)
3071 : << " isLeader=" << veh.isLeader(*link, lVeh, ll.vehAndGap.second)
3072 : << " gap=" << ll.vehAndGap.second
3073 : << " gap+brakeing=" << gap
3074 : << "\n";
3075 : }
3076 : #endif
3077 : // skip vehicles which do not share the outgoing edge (to get only real leader vehicles in TraCI #13842)
3078 1251190 : if (!considerCrossingFoes && !ll.sameTarget()) {
3079 40 : continue;
3080 : }
3081 : // in the context of lane-changing, all candidates are leaders
3082 1251150 : if (lVeh != nullptr && !laneChanging && !veh.isLeader(*link, lVeh, ll.vehAndGap.second)) {
3083 9081 : continue;
3084 : }
3085 1242069 : if (gap < shortestGap) {
3086 : shortestGap = gap;
3087 1051933 : if (ll.vehAndGap.second < 0 && !MSGlobals::gComputeLC) {
3088 : // can always continue up to the stop line or crossing point
3089 : // @todo: figure out whether this should also impact lane changing
3090 74261 : ll.vehAndGap.second = MAX2(seen - nextLane->getLength(), ll.distToCrossing);
3091 : }
3092 : result = ll.vehAndGap;
3093 : }
3094 : }
3095 1009087 : if (shortestGap != std::numeric_limits<double>::max()) {
3096 : #ifdef DEBUG_CONTEXT
3097 : if (DEBUG_COND2(&veh)) {
3098 : std::cout << " found linkLeader after nextLane=" << nextLane->getID() << "\n";
3099 : gDebugFlag1 = false;
3100 : }
3101 : #endif
3102 1001692 : return result;
3103 : }
3104 : }
3105 40060759 : bool nextInternal = (*link)->getViaLane() != nullptr;
3106 : nextLane = (*link)->getViaLaneOrLane();
3107 19584066 : if (nextLane == nullptr) {
3108 : break;
3109 : }
3110 40060759 : nextLane->getVehiclesSecure(); // lock against running sim when called from GUI for time gap coloring
3111 40060759 : MSVehicle* leader = nextLane->getLastAnyVehicle();
3112 40060759 : if (leader != nullptr) {
3113 : #ifdef DEBUG_CONTEXT
3114 : if (DEBUG_COND2(&veh)) {
3115 : std::cout << " found leader " << leader->getID() << " on nextLane=" << nextLane->getID() << "\n";
3116 : }
3117 : #endif
3118 16420895 : const double leaderDist = seen + leader->getBackPositionOnLane(nextLane) - veh.getVehicleType().getMinGap();
3119 16420895 : nextLane->releaseVehicles();
3120 16420895 : return std::make_pair(leader, leaderDist);
3121 : }
3122 23639864 : nextLane->releaseVehicles();
3123 23639864 : if (nextLane->getVehicleMaxSpeed(&veh) < speed) {
3124 3061881 : dist = veh.getCarFollowModel().brakeGap(nextLane->getVehicleMaxSpeed(&veh));
3125 : }
3126 23639864 : seen += nextLane->getLength();
3127 23639864 : if (!nextInternal) {
3128 7583179 : view++;
3129 : }
3130 57869225 : } while (seen <= dist || nextLane->isInternal());
3131 : #ifdef DEBUG_CONTEXT
3132 : gDebugFlag1 = false;
3133 : #endif
3134 21411322 : return std::make_pair(static_cast<MSVehicle*>(nullptr), -1);
3135 : }
3136 :
3137 :
3138 : std::pair<MSVehicle* const, double>
3139 104505 : MSLane::getCriticalLeader(double dist, double seen, double speed, const MSVehicle& veh) const {
3140 : #ifdef DEBUG_CONTEXT
3141 : if (DEBUG_COND2(&veh)) {
3142 : std::cout << SIMTIME << " getCriticalLeader. lane=" << getID() << " veh=" << veh.getID() << "\n";
3143 : }
3144 : #endif
3145 104505 : const std::vector<MSLane*>& bestLaneConts = veh.getBestLanesContinuation(this);
3146 : std::pair<MSVehicle*, double> result = std::make_pair(static_cast<MSVehicle*>(nullptr), -1);
3147 : double safeSpeed = std::numeric_limits<double>::max();
3148 : int view = 1;
3149 : // loop over following lanes
3150 : // @note: we don't check the partial occupator for this lane since it was
3151 : // already checked in MSLaneChanger::getRealLeader()
3152 : const MSLane* nextLane = this;
3153 209010 : SUMOTime arrivalTime = MSNet::getInstance()->getCurrentTimeStep() + TIME2STEPS(seen / MAX2(speed, NUMERICAL_EPS));
3154 : do {
3155 : // get the next link used
3156 186003 : std::vector<MSLink*>::const_iterator link = succLinkSec(veh, view, *nextLane, bestLaneConts);
3157 183284 : if (nextLane->isLinkEnd(link) || !(*link)->opened(arrivalTime, speed, speed, veh.getVehicleType().getLength(),
3158 364362 : veh.getImpatience(), veh.getCarFollowModel().getMaxDecel(), 0, veh.getLateralPositionOnLane(), nullptr, false, &veh) || (*link)->haveRed()) {
3159 7644 : return result;
3160 : }
3161 : // check for link leaders
3162 : #ifdef DEBUG_CONTEXT
3163 : if (DEBUG_COND2(&veh)) {
3164 : gDebugFlag1 = true; // See MSLink::getLeaderInfo
3165 : }
3166 : #endif
3167 178359 : const MSLink::LinkLeaders linkLeaders = (*link)->getLeaderInfo(&veh, seen);
3168 : #ifdef DEBUG_CONTEXT
3169 : if (DEBUG_COND2(&veh)) {
3170 : gDebugFlag1 = false; // See MSLink::getLeaderInfo
3171 : }
3172 : #endif
3173 209313 : for (MSLink::LinkLeaders::const_iterator it = linkLeaders.begin(); it != linkLeaders.end(); ++it) {
3174 30954 : const MSVehicle* leader = (*it).vehAndGap.first;
3175 30954 : if (leader != nullptr && leader != result.first) {
3176 : // XXX ignoring pedestrians here!
3177 : // XXX ignoring the fact that the link leader may alread by following us
3178 : // XXX ignoring the fact that we may drive up to the crossing point
3179 30866 : double tmpSpeed = safeSpeed;
3180 30866 : veh.adaptToJunctionLeader((*it).vehAndGap, seen, nullptr, nextLane, tmpSpeed, tmpSpeed, (*it).distToCrossing);
3181 : #ifdef DEBUG_CONTEXT
3182 : if (DEBUG_COND2(&veh)) {
3183 : std::cout << " linkLeader=" << leader->getID() << " gap=" << result.second << " tmpSpeed=" << tmpSpeed << " safeSpeed=" << safeSpeed << "\n";
3184 : }
3185 : #endif
3186 30866 : if (tmpSpeed < safeSpeed) {
3187 : safeSpeed = tmpSpeed;
3188 : result = (*it).vehAndGap;
3189 : }
3190 : }
3191 : }
3192 178359 : bool nextInternal = (*link)->getViaLane() != nullptr;
3193 : nextLane = (*link)->getViaLaneOrLane();
3194 100598 : if (nextLane == nullptr) {
3195 : break;
3196 : }
3197 178359 : MSVehicle* leader = nextLane->getLastAnyVehicle();
3198 178359 : if (leader != nullptr && leader != result.first) {
3199 101616 : const double gap = seen + leader->getBackPositionOnLane(nextLane) - veh.getVehicleType().getMinGap();
3200 101616 : const double tmpSpeed = veh.getCarFollowModel().insertionFollowSpeed(&veh, speed, gap, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel(), leader);
3201 101616 : if (tmpSpeed < safeSpeed) {
3202 : safeSpeed = tmpSpeed;
3203 : result = std::make_pair(leader, gap);
3204 : }
3205 : }
3206 178359 : if (nextLane->getVehicleMaxSpeed(&veh) < speed) {
3207 27485 : dist = veh.getCarFollowModel().brakeGap(nextLane->getVehicleMaxSpeed(&veh));
3208 : }
3209 178359 : seen += nextLane->getLength();
3210 178359 : if (seen <= dist) {
3211 : // delaying the update of arrivalTime and making it conditional to avoid possible integer overflows
3212 36616 : arrivalTime += TIME2STEPS(nextLane->getLength() / MAX2(speed, NUMERICAL_EPS));
3213 : }
3214 178359 : if (!nextInternal) {
3215 100598 : view++;
3216 : }
3217 259857 : } while (seen <= dist || nextLane->isInternal());
3218 96861 : return result;
3219 : }
3220 :
3221 :
3222 : MSLane*
3223 1103197536 : MSLane::getLogicalPredecessorLane() const {
3224 1103197536 : if (myLogicalPredecessorLane == nullptr) {
3225 5258813 : MSEdgeVector pred = myEdge->getPredecessors();
3226 : // get only those edges which connect to this lane
3227 10876617 : for (MSEdgeVector::iterator i = pred.begin(); i != pred.end();) {
3228 5617804 : std::vector<IncomingLaneInfo>::const_iterator j = find_if(myIncomingLanes.begin(), myIncomingLanes.end(), edge_finder(*i));
3229 5617804 : if (j == myIncomingLanes.end()) {
3230 : i = pred.erase(i);
3231 : } else {
3232 : ++i;
3233 : }
3234 : }
3235 : // get the lane with the "straightest" connection
3236 5258813 : if (pred.size() != 0) {
3237 1622548 : std::sort(pred.begin(), pred.end(), by_connections_to_sorter(&getEdge()));
3238 811274 : MSEdge* best = *pred.begin();
3239 811274 : std::vector<IncomingLaneInfo>::const_iterator j = find_if(myIncomingLanes.begin(), myIncomingLanes.end(), edge_finder(best));
3240 811274 : myLogicalPredecessorLane = j->lane;
3241 : }
3242 5258813 : }
3243 1103197536 : return myLogicalPredecessorLane;
3244 : }
3245 :
3246 :
3247 : const MSLane*
3248 815733995 : MSLane::getNormalPredecessorLane() const {
3249 1758533584 : if (isInternal()) {
3250 942799589 : return getLogicalPredecessorLane()->getNormalPredecessorLane();
3251 : } else {
3252 : return this;
3253 : }
3254 : }
3255 :
3256 :
3257 : const MSLane*
3258 454468 : MSLane::getNormalSuccessorLane() const {
3259 621640 : if (isInternal()) {
3260 167172 : return getCanonicalSuccessorLane()->getNormalSuccessorLane();
3261 : } else {
3262 : return this;
3263 : }
3264 : }
3265 :
3266 :
3267 : MSLane*
3268 80147 : MSLane::getLogicalPredecessorLane(const MSEdge& fromEdge) const {
3269 141802 : for (const IncomingLaneInfo& cand : myIncomingLanes) {
3270 95223 : if (&(cand.lane->getEdge()) == &fromEdge) {
3271 : return cand.lane;
3272 : }
3273 : }
3274 : return nullptr;
3275 : }
3276 :
3277 :
3278 : MSLane*
3279 95082203 : MSLane::getCanonicalPredecessorLane() const {
3280 95082203 : if (myCanonicalPredecessorLane != nullptr) {
3281 : return myCanonicalPredecessorLane;
3282 : }
3283 780489 : if (myIncomingLanes.empty()) {
3284 : return nullptr;
3285 : }
3286 : // myCanonicalPredecessorLane has not yet been determined and there exist incoming lanes
3287 : // get the lane with the priorized (or if this does not apply the "straightest") connection
3288 780185 : const auto bestLane = std::min_element(myIncomingLanes.begin(), myIncomingLanes.end(), incoming_lane_priority_sorter(this));
3289 : {
3290 : #ifdef HAVE_FOX
3291 780185 : ScopedLocker<> lock(myLeaderInfoMutex, MSGlobals::gNumSimThreads > 1);
3292 : #endif
3293 780185 : myCanonicalPredecessorLane = bestLane->lane;
3294 : }
3295 : #ifdef DEBUG_LANE_SORTER
3296 : std::cout << "\nBest predecessor lane for lane '" << myID << "': '" << myCanonicalPredecessorLane->getID() << "'" << std::endl;
3297 : #endif
3298 780185 : return myCanonicalPredecessorLane;
3299 : }
3300 :
3301 :
3302 : MSLane*
3303 2506509 : MSLane::getCanonicalSuccessorLane() const {
3304 2506509 : if (myCanonicalSuccessorLane != nullptr) {
3305 : return myCanonicalSuccessorLane;
3306 : }
3307 67955 : if (myLinks.empty()) {
3308 : return nullptr;
3309 : }
3310 : // myCanonicalSuccessorLane has not yet been determined and there exist outgoing links
3311 7528 : std::vector<MSLink*> candidateLinks = myLinks;
3312 : // get the lane with the priorized (or if this does not apply the "straightest") connection
3313 15056 : std::sort(candidateLinks.begin(), candidateLinks.end(), outgoing_lane_priority_sorter(this));
3314 7528 : MSLane* best = (*candidateLinks.begin())->getViaLaneOrLane();
3315 : #ifdef DEBUG_LANE_SORTER
3316 : std::cout << "\nBest successor lane for lane '" << myID << "': '" << best->getID() << "'" << std::endl;
3317 : #endif
3318 7528 : myCanonicalSuccessorLane = best;
3319 : return myCanonicalSuccessorLane;
3320 7528 : }
3321 :
3322 :
3323 : LinkState
3324 4625367 : MSLane::getIncomingLinkState() const {
3325 4625367 : const MSLane* const pred = getLogicalPredecessorLane();
3326 4625367 : if (pred == nullptr) {
3327 : return LINKSTATE_DEADEND;
3328 : } else {
3329 4625367 : return pred->getLinkTo(this)->getState();
3330 : }
3331 : }
3332 :
3333 :
3334 : const std::vector<std::pair<const MSLane*, const MSEdge*> >
3335 273385 : MSLane::getOutgoingViaLanes() const {
3336 : std::vector<std::pair<const MSLane*, const MSEdge*> > result;
3337 598075 : for (const MSLink* link : myLinks) {
3338 : assert(link->getLane() != nullptr);
3339 649380 : result.push_back(std::make_pair(link->getLane(), link->getViaLane() == nullptr ? nullptr : &link->getViaLane()->getEdge()));
3340 : }
3341 273385 : return result;
3342 0 : }
3343 :
3344 : std::vector<const MSLane*>
3345 76 : MSLane::getNormalIncomingLanes() const {
3346 76 : std::vector<const MSLane*> result = {};
3347 226 : for (std::map<MSEdge*, std::vector<MSLane*> >::const_iterator it = myApproachingLanes.begin(); it != myApproachingLanes.end(); ++it) {
3348 328 : for (std::vector<MSLane*>::const_iterator it_lane = (*it).second.begin(); it_lane != (*it).second.end(); ++it_lane) {
3349 178 : if (!((*it_lane)->isInternal())) {
3350 146 : result.push_back(*it_lane);
3351 : }
3352 : }
3353 : }
3354 76 : return result;
3355 0 : }
3356 :
3357 :
3358 : void
3359 1108371 : MSLane::leftByLaneChange(MSVehicle* v) {
3360 1108371 : myBruttoVehicleLengthSum -= v->getVehicleType().getLengthWithGap();
3361 1108371 : myNettoVehicleLengthSum -= v->getVehicleType().getLength();
3362 1108371 : }
3363 :
3364 :
3365 : void
3366 1064108 : MSLane::enteredByLaneChange(MSVehicle* v) {
3367 1064108 : myBruttoVehicleLengthSum += v->getVehicleType().getLengthWithGap();
3368 1064108 : myNettoVehicleLengthSum += v->getVehicleType().getLength();
3369 1064108 : }
3370 :
3371 :
3372 : int
3373 0 : MSLane::getCrossingIndex() const {
3374 0 : for (std::vector<MSLink*>::const_iterator i = myLinks.begin(); i != myLinks.end(); ++i) {
3375 0 : if ((*i)->getLane()->isCrossing()) {
3376 0 : return (int)(i - myLinks.begin());
3377 : }
3378 : }
3379 : return -1;
3380 : }
3381 :
3382 : // ------------ Current state retrieval
3383 : double
3384 3297004260 : MSLane::getFractionalVehicleLength(bool brutto) const {
3385 : double sum = 0;
3386 3297004260 : if (myPartialVehicles.size() > 0) {
3387 717051375 : const MSLane* bidi = getBidiLane();
3388 1451668582 : for (MSVehicle* cand : myPartialVehicles) {
3389 734617207 : if (MSGlobals::gSublane && cand->getLaneChangeModel().getShadowLane() == this) {
3390 25348696 : continue;
3391 : }
3392 709268511 : if (cand->getLane() == bidi) {
3393 203622 : sum += (brutto ? cand->getVehicleType().getLengthWithGap() : cand->getVehicleType().getLength());
3394 : } else {
3395 709166700 : sum += myLength - cand->getBackPositionOnLane(this);
3396 : }
3397 : }
3398 : }
3399 3297004260 : return sum;
3400 : }
3401 :
3402 : double
3403 3296957410 : MSLane::getBruttoOccupancy() const {
3404 3296957410 : getVehiclesSecure();
3405 3296957410 : double fractions = getFractionalVehicleLength(true);
3406 3296957410 : if (myVehicles.size() != 0) {
3407 3040147772 : MSVehicle* lastVeh = myVehicles.front();
3408 3040147772 : if (lastVeh->getPositionOnLane() < lastVeh->getVehicleType().getLength()) {
3409 118873991 : fractions -= (lastVeh->getVehicleType().getLength() - lastVeh->getPositionOnLane());
3410 : }
3411 : }
3412 3296957410 : releaseVehicles();
3413 3296957410 : return MIN2(1., (myBruttoVehicleLengthSum + fractions) / myLength);
3414 : }
3415 :
3416 :
3417 : double
3418 46850 : MSLane::getNettoOccupancy() const {
3419 46850 : getVehiclesSecure();
3420 46850 : double fractions = getFractionalVehicleLength(false);
3421 46850 : if (myVehicles.size() != 0) {
3422 508 : MSVehicle* lastVeh = myVehicles.front();
3423 508 : if (lastVeh->getPositionOnLane() < lastVeh->getVehicleType().getLength()) {
3424 4 : fractions -= (lastVeh->getVehicleType().getLength() - lastVeh->getPositionOnLane());
3425 : }
3426 : }
3427 46850 : releaseVehicles();
3428 46850 : return (myNettoVehicleLengthSum + fractions) / myLength;
3429 : }
3430 :
3431 :
3432 : double
3433 52 : MSLane::getWaitingSeconds() const {
3434 52 : if (myVehicles.size() == 0) {
3435 : return 0;
3436 : }
3437 : double wtime = 0;
3438 56 : for (VehCont::const_iterator i = myVehicles.begin(); i != myVehicles.end(); ++i) {
3439 28 : wtime += (*i)->getWaitingSeconds();
3440 : }
3441 : return wtime;
3442 : }
3443 :
3444 :
3445 : double
3446 161521765 : MSLane::getMeanSpeed() const {
3447 161521765 : if (myVehicles.size() == 0) {
3448 134055039 : return myMaxSpeed;
3449 : }
3450 : double v = 0;
3451 : int numVehs = 0;
3452 161715843 : for (const MSVehicle* const veh : getVehiclesSecure()) {
3453 134249117 : if (!veh->isStopped() || !myEdge->hasLaneChanger()) {
3454 134003191 : v += veh->getSpeed();
3455 134003191 : numVehs++;
3456 : }
3457 : }
3458 27466726 : releaseVehicles();
3459 27466726 : if (numVehs == 0) {
3460 159580 : return myMaxSpeed;
3461 : }
3462 27307146 : return v / numVehs;
3463 : }
3464 :
3465 :
3466 : double
3467 2095 : MSLane::getMeanSpeedBike() const {
3468 : // @note: redundant code with getMeanSpeed to avoid extra checks in a function that is called very often
3469 2095 : if (myVehicles.size() == 0) {
3470 1480 : return myMaxSpeed;
3471 : }
3472 : double v = 0;
3473 : int numBikes = 0;
3474 2260 : for (MSVehicle* veh : getVehiclesSecure()) {
3475 1645 : if (veh->getVClass() == SVC_BICYCLE) {
3476 1150 : v += veh->getSpeed();
3477 1150 : numBikes++;
3478 : }
3479 : }
3480 : double ret;
3481 615 : if (numBikes > 0) {
3482 335 : ret = v / (double) myVehicles.size();
3483 : } else {
3484 280 : ret = myMaxSpeed;
3485 : }
3486 615 : releaseVehicles();
3487 615 : return ret;
3488 : }
3489 :
3490 :
3491 : double
3492 46847 : MSLane::getHarmonoise_NoiseEmissions() const {
3493 : double ret = 0;
3494 46847 : const MSLane::VehCont& vehs = getVehiclesSecure();
3495 46847 : if (vehs.size() == 0) {
3496 46343 : releaseVehicles();
3497 46343 : return 0;
3498 : }
3499 1224 : for (MSLane::VehCont::const_iterator i = vehs.begin(); i != vehs.end(); ++i) {
3500 720 : double sv = (*i)->getHarmonoise_NoiseEmissions();
3501 720 : ret += (double) pow(10., (sv / 10.));
3502 : }
3503 504 : releaseVehicles();
3504 504 : return HelpersHarmonoise::sum(ret);
3505 : }
3506 :
3507 :
3508 : int
3509 275589 : MSLane::vehicle_position_sorter::operator()(MSVehicle* v1, MSVehicle* v2) const {
3510 275589 : const double pos1 = v1->getBackPositionOnLane(myLane);
3511 275589 : const double pos2 = v2->getBackPositionOnLane(myLane);
3512 275589 : if (pos1 != pos2) {
3513 271635 : return pos1 > pos2;
3514 : } else {
3515 3954 : return v1->getNumericalID() > v2->getNumericalID();
3516 : }
3517 : }
3518 :
3519 :
3520 : int
3521 355313264 : MSLane::vehicle_natural_position_sorter::operator()(MSVehicle* v1, MSVehicle* v2) const {
3522 355313264 : const double pos1 = v1->getBackPositionOnLane(myLane);
3523 355313264 : const double pos2 = v2->getBackPositionOnLane(myLane);
3524 355313264 : if (pos1 != pos2) {
3525 354748565 : return pos1 < pos2;
3526 : } else {
3527 564699 : return v1->getLateralPositionOnLane() < v2->getLateralPositionOnLane();
3528 : }
3529 : }
3530 :
3531 :
3532 811274 : MSLane::by_connections_to_sorter::by_connections_to_sorter(const MSEdge* const e) :
3533 811274 : myEdge(e),
3534 811274 : myLaneDir(e->getLanes()[0]->getShape().angleAt2D(0)) {
3535 811274 : }
3536 :
3537 :
3538 : int
3539 18555 : MSLane::by_connections_to_sorter::operator()(const MSEdge* const e1, const MSEdge* const e2) const {
3540 : // std::cout << "\nby_connections_to_sorter()";
3541 :
3542 18555 : const std::vector<MSLane*>* ae1 = e1->allowedLanes(*myEdge);
3543 18555 : const std::vector<MSLane*>* ae2 = e2->allowedLanes(*myEdge);
3544 : double s1 = 0;
3545 18555 : if (ae1 != nullptr && ae1->size() != 0) {
3546 : // std::cout << "\nsize 1 = " << ae1->size()
3547 : // << " anglediff 1 = " << fabs(GeomHelper::angleDiff((*ae1)[0]->getShape().angleAt2D(0), myLaneDir)) / M_PI / 2.
3548 : // << "\nallowed lanes: ";
3549 : // for (std::vector<MSLane*>::const_iterator j = ae1->begin(); j != ae1->end(); ++j){
3550 : // std::cout << "\n" << (*j)->getID();
3551 : // }
3552 18555 : s1 = (double) ae1->size() + fabs(GeomHelper::angleDiff((*ae1)[0]->getShape().angleAt2D(0), myLaneDir)) / M_PI / 2.;
3553 : }
3554 : double s2 = 0;
3555 18555 : if (ae2 != nullptr && ae2->size() != 0) {
3556 : // std::cout << "\nsize 2 = " << ae2->size()
3557 : // << " anglediff 2 = " << fabs(GeomHelper::angleDiff((*ae2)[0]->getShape().angleAt2D(0), myLaneDir)) / M_PI / 2.
3558 : // << "\nallowed lanes: ";
3559 : // for (std::vector<MSLane*>::const_iterator j = ae2->begin(); j != ae2->end(); ++j){
3560 : // std::cout << "\n" << (*j)->getID();
3561 : // }
3562 18555 : s2 = (double) ae2->size() + fabs(GeomHelper::angleDiff((*ae2)[0]->getShape().angleAt2D(0), myLaneDir)) / M_PI / 2.;
3563 : }
3564 :
3565 : // std::cout << "\ne1 = " << e1->getID() << " e2 = " << e2->getID()
3566 : // << "\ns1 = " << s1 << " s2 = " << s2
3567 : // << std::endl;
3568 :
3569 18555 : return s1 < s2;
3570 : }
3571 :
3572 :
3573 780185 : MSLane::incoming_lane_priority_sorter::incoming_lane_priority_sorter(const MSLane* const targetLane) :
3574 780185 : myLane(targetLane),
3575 780185 : myLaneDir(targetLane->getShape().angleAt2D(0)) {}
3576 :
3577 : int
3578 963 : MSLane::incoming_lane_priority_sorter::operator()(const IncomingLaneInfo& laneInfo1, const IncomingLaneInfo& laneInfo2) const {
3579 963 : const MSLane* noninternal1 = laneInfo1.lane;
3580 2339 : while (noninternal1->isInternal()) {
3581 : assert(noninternal1->getIncomingLanes().size() == 1);
3582 1376 : noninternal1 = noninternal1->getIncomingLanes()[0].lane;
3583 : }
3584 963 : MSLane* noninternal2 = laneInfo2.lane;
3585 2119 : while (noninternal2->isInternal()) {
3586 : assert(noninternal2->getIncomingLanes().size() == 1);
3587 1156 : noninternal2 = noninternal2->getIncomingLanes()[0].lane;
3588 : }
3589 :
3590 963 : const MSLink* link1 = noninternal1->getLinkTo(myLane);
3591 963 : const MSLink* link2 = noninternal2->getLinkTo(myLane);
3592 :
3593 : #ifdef DEBUG_LANE_SORTER
3594 : std::cout << "\nincoming_lane_priority sorter()\n"
3595 : << "noninternal predecessor for lane '" << laneInfo1.lane->getID()
3596 : << "': '" << noninternal1->getID() << "'\n"
3597 : << "noninternal predecessor for lane '" << laneInfo2.lane->getID()
3598 : << "': '" << noninternal2->getID() << "'\n";
3599 : #endif
3600 :
3601 : assert(laneInfo1.lane->isInternal() || link1 == laneInfo1.viaLink);
3602 : assert(link1 != 0);
3603 : assert(link2 != 0);
3604 :
3605 : // check priority between links
3606 : bool priorized1 = true;
3607 : bool priorized2 = true;
3608 :
3609 : #ifdef DEBUG_LANE_SORTER
3610 : std::cout << "FoeLinks of '" << noninternal1->getID() << "'" << std::endl;
3611 : #endif
3612 2117 : for (const MSLink* const foeLink : link1->getFoeLinks()) {
3613 : #ifdef DEBUG_LANE_SORTER
3614 : std::cout << foeLink->getLaneBefore()->getID() << std::endl;
3615 : #endif
3616 1878 : if (foeLink == link2) {
3617 : priorized1 = false;
3618 : break;
3619 : }
3620 : }
3621 :
3622 : #ifdef DEBUG_LANE_SORTER
3623 : std::cout << "FoeLinks of '" << noninternal2->getID() << "'" << std::endl;
3624 : #endif
3625 2428 : for (const MSLink* const foeLink : link2->getFoeLinks()) {
3626 : #ifdef DEBUG_LANE_SORTER
3627 : std::cout << foeLink->getLaneBefore()->getID() << std::endl;
3628 : #endif
3629 : // either link1 is priorized, or it should not appear in link2's foes
3630 1867 : if (foeLink == link1) {
3631 : priorized2 = false;
3632 : break;
3633 : }
3634 : }
3635 : // if one link is subordinate, the other must be priorized (except for
3636 : // traffic lights where mutual response is permitted to handle stuck-on-red
3637 : // situation)
3638 963 : if (priorized1 != priorized2) {
3639 792 : return priorized1;
3640 : }
3641 :
3642 : // both are priorized, compare angle difference
3643 171 : double d1 = fabs(GeomHelper::angleDiff(noninternal1->getShape().angleAt2D(0), myLaneDir));
3644 171 : double d2 = fabs(GeomHelper::angleDiff(noninternal2->getShape().angleAt2D(0), myLaneDir));
3645 :
3646 171 : return d2 > d1;
3647 : }
3648 :
3649 :
3650 :
3651 7528 : MSLane::outgoing_lane_priority_sorter::outgoing_lane_priority_sorter(const MSLane* const sourceLane) :
3652 7528 : myLaneDir(sourceLane->getShape().angleAt2D(0)) {}
3653 :
3654 : int
3655 13605 : MSLane::outgoing_lane_priority_sorter::operator()(const MSLink* link1, const MSLink* link2) const {
3656 : const MSLane* target1 = link1->getLane();
3657 : const MSLane* target2 = link2->getLane();
3658 13605 : if (target2 == nullptr) {
3659 : return true;
3660 : }
3661 13605 : if (target1 == nullptr) {
3662 : return false;
3663 : }
3664 :
3665 : #ifdef DEBUG_LANE_SORTER
3666 : std::cout << "\noutgoing_lane_priority sorter()\n"
3667 : << "noninternal successors for lane '" << myLane->getID()
3668 : << "': '" << target1->getID() << "' and "
3669 : << "'" << target2->getID() << "'\n";
3670 : #endif
3671 :
3672 : // priority of targets
3673 : int priority1 = target1->getEdge().getPriority();
3674 : int priority2 = target2->getEdge().getPriority();
3675 :
3676 13605 : if (priority1 != priority2) {
3677 127 : return priority1 > priority2;
3678 : }
3679 :
3680 : // if priority of targets coincides, use angle difference
3681 :
3682 : // both are priorized, compare angle difference
3683 13478 : double d1 = fabs(GeomHelper::angleDiff(target1->getShape().angleAt2D(0), myLaneDir));
3684 13478 : double d2 = fabs(GeomHelper::angleDiff(target2->getShape().angleAt2D(0), myLaneDir));
3685 :
3686 13478 : return d2 > d1;
3687 : }
3688 :
3689 : void
3690 5709 : MSLane::addParking(MSBaseVehicle* veh) {
3691 : myParkingVehicles.insert(veh);
3692 5709 : }
3693 :
3694 :
3695 : void
3696 17392 : MSLane::removeParking(MSBaseVehicle* veh) {
3697 : myParkingVehicles.erase(veh);
3698 17392 : }
3699 :
3700 : bool
3701 73 : MSLane::hasApproaching() const {
3702 146 : for (const MSLink* link : myLinks) {
3703 79 : if (link->getApproaching().size() > 0) {
3704 : return true;
3705 : }
3706 : }
3707 : return false;
3708 : }
3709 :
3710 : void
3711 9724 : MSLane::saveState(OutputDevice& out) {
3712 9724 : const bool toRailJunction = myLinks.size() > 0 && (
3713 9265 : myEdge->getToJunction()->getType() == SumoXMLNodeType::RAIL_SIGNAL
3714 9181 : || myEdge->getToJunction()->getType() == SumoXMLNodeType::RAIL_CROSSING);
3715 : const bool hasVehicles = myVehicles.size() > 0;
3716 9724 : if (hasVehicles || (toRailJunction && hasApproaching())) {
3717 489 : out.openTag(SUMO_TAG_LANE);
3718 489 : out.writeAttr(SUMO_ATTR_ID, getID());
3719 489 : if (hasVehicles) {
3720 483 : out.openTag(SUMO_TAG_VIEWSETTINGS_VEHICLES);
3721 483 : out.writeAttr(SUMO_ATTR_VALUE, myVehicles);
3722 966 : out.closeTag();
3723 : }
3724 489 : if (toRailJunction) {
3725 37 : for (const MSLink* link : myLinks) {
3726 20 : if (link->getApproaching().size() > 0) {
3727 17 : out.openTag(SUMO_TAG_LINK);
3728 17 : out.writeAttr(SUMO_ATTR_TO, link->getViaLaneOrLane()->getID());
3729 34 : for (auto item : link->getApproaching()) {
3730 17 : out.openTag(SUMO_TAG_APPROACHING);
3731 17 : out.writeAttr(SUMO_ATTR_ID, item.first->getID());
3732 17 : out.writeAttr(SUMO_ATTR_ARRIVALTIME, item.second.arrivalTime);
3733 17 : out.writeAttr(SUMO_ATTR_ARRIVALSPEED, item.second.arrivalSpeed);
3734 17 : out.writeAttr(SUMO_ATTR_DEPARTSPEED, item.second.leaveSpeed);
3735 17 : out.writeAttr(SUMO_ATTR_REQUEST, item.second.willPass);
3736 17 : out.writeAttr(SUMO_ATTR_ARRIVALSPEEDBRAKING, item.second.arrivalSpeedBraking);
3737 17 : out.writeAttr(SUMO_ATTR_WAITINGTIME, item.second.waitingTime);
3738 17 : out.writeAttr(SUMO_ATTR_DISTANCE, item.second.dist);
3739 17 : if (item.second.latOffset != 0) {
3740 0 : out.writeAttr(SUMO_ATTR_POSITION_LAT, item.second.latOffset);
3741 : }
3742 34 : out.closeTag();
3743 : }
3744 34 : out.closeTag();
3745 : }
3746 : }
3747 : }
3748 978 : out.closeTag();
3749 : }
3750 9724 : }
3751 :
3752 : void
3753 10089 : MSLane::clearState() {
3754 : myVehicles.clear();
3755 : myParkingVehicles.clear();
3756 : myPartialVehicles.clear();
3757 : myManeuverReservations.clear();
3758 10089 : myBruttoVehicleLengthSum = 0;
3759 10089 : myNettoVehicleLengthSum = 0;
3760 10089 : myBruttoVehicleLengthSumToRemove = 0;
3761 10089 : myNettoVehicleLengthSumToRemove = 0;
3762 10089 : myLeaderInfoTime = SUMOTime_MIN;
3763 10089 : myFollowerInfoTime = SUMOTime_MIN;
3764 23185 : for (MSLink* link : myLinks) {
3765 13096 : link->clearState();
3766 : }
3767 10089 : }
3768 :
3769 : void
3770 634 : MSLane::loadState(const std::vector<SUMOVehicle*>& vehs) {
3771 2210 : for (SUMOVehicle* veh : vehs) {
3772 1576 : MSVehicle* v = dynamic_cast<MSVehicle*>(veh);
3773 1576 : v->updateBestLanes(false, this);
3774 : // incorporateVehicle resets the lastActionTime (which has just been loaded from state) so we must restore it
3775 1576 : const SUMOTime lastActionTime = v->getLastActionTime();
3776 1576 : incorporateVehicle(v, v->getPositionOnLane(), v->getSpeed(), v->getLateralPositionOnLane(), myVehicles.end(),
3777 : MSMoveReminder::NOTIFICATION_LOAD_STATE);
3778 1576 : v->resetActionOffset(lastActionTime - MSNet::getInstance()->getCurrentTimeStep());
3779 1576 : v->processNextStop(v->getSpeed());
3780 : }
3781 634 : }
3782 :
3783 :
3784 : double
3785 2364395394 : MSLane::getVehicleStopOffset(const MSVehicle* veh) const {
3786 2364395394 : if (!myLaneStopOffset.isDefined()) {
3787 : return 0;
3788 : }
3789 93431 : if ((myLaneStopOffset.getPermissions() & veh->getVClass()) != 0) {
3790 31192 : return myLaneStopOffset.getOffset();
3791 : } else {
3792 : return 0;
3793 : }
3794 : }
3795 :
3796 :
3797 : const StopOffset&
3798 2957 : MSLane::getLaneStopOffsets() const {
3799 2957 : return myLaneStopOffset;
3800 : }
3801 :
3802 :
3803 : void
3804 2133 : MSLane::setLaneStopOffset(const StopOffset& stopOffset) {
3805 2133 : myLaneStopOffset = stopOffset;
3806 2133 : }
3807 :
3808 :
3809 : MSLeaderDistanceInfo
3810 297968886 : MSLane::getFollowersOnConsecutive(const MSVehicle* ego, double backOffset,
3811 : bool allSublanes, double searchDist, MinorLinkMode mLinkMode) const {
3812 : assert(ego != 0);
3813 : // get the follower vehicle on the lane to change to
3814 297968886 : const double egoPos = backOffset + ego->getVehicleType().getLength();
3815 297968886 : const double egoLatDist = ego->getLane()->getRightSideOnEdge() - getRightSideOnEdge();
3816 298912867 : const bool getOppositeLeaders = ((ego->getLaneChangeModel().isOpposite() && ego->getLane() == this)
3817 298098168 : || (!ego->getLaneChangeModel().isOpposite() && &ego->getLane()->getEdge() != &getEdge()));
3818 : #ifdef DEBUG_CONTEXT
3819 : if (DEBUG_COND2(ego)) {
3820 : std::cout << SIMTIME << " getFollowers lane=" << getID() << " ego=" << ego->getID()
3821 : << " backOffset=" << backOffset << " pos=" << egoPos
3822 : << " allSub=" << allSublanes << " searchDist=" << searchDist << " ignoreMinor=" << mLinkMode
3823 : << " egoLatDist=" << egoLatDist
3824 : << " getOppositeLeaders=" << getOppositeLeaders
3825 : << "\n";
3826 : }
3827 : #endif
3828 306017908 : MSCriticalFollowerDistanceInfo result(myWidth, allSublanes ? nullptr : ego, allSublanes ? 0 : egoLatDist, getOppositeLeaders);
3829 297968886 : if (MSGlobals::gLateralResolution > 0 && egoLatDist == 0) {
3830 : // check whether ego is outside lane bounds far enough so that another vehicle might
3831 : // be between itself and the first "actual" sublane
3832 : // shift the offset so that we "see" this vehicle
3833 194682269 : if (ego->getLeftSideOnLane() < -MSGlobals::gLateralResolution) {
3834 82546 : result.setSublaneOffset(int(-ego->getLeftSideOnLane() / MSGlobals::gLateralResolution));
3835 194599723 : } else if (ego->getRightSideOnLane() > getWidth() + MSGlobals::gLateralResolution) {
3836 103321 : result.setSublaneOffset(-int((ego->getRightSideOnLane() - getWidth()) / MSGlobals::gLateralResolution));
3837 : }
3838 : #ifdef DEBUG_CONTEXT
3839 : if (DEBUG_COND2(ego)) {
3840 : std::cout << SIMTIME << " getFollowers lane=" << getID() << " ego=" << ego->getID()
3841 : << " egoPosLat=" << ego->getLateralPositionOnLane()
3842 : << " egoLatDist=" << ego->getLane()->getRightSideOnEdge() - getRightSideOnEdge()
3843 : << " extraOffset=" << result.getSublaneOffset()
3844 : << "\n";
3845 : }
3846 : #endif
3847 : }
3848 : /// XXX iterate in reverse and abort when there are no more freeSublanes
3849 6136411269 : for (AnyVehicleIterator last = anyVehiclesBegin(); last != anyVehiclesEnd(); ++last) {
3850 6136411269 : const MSVehicle* veh = *last;
3851 : #ifdef DEBUG_CONTEXT
3852 : if (DEBUG_COND2(ego)) {
3853 : std::cout << " veh=" << veh->getID() << " lane=" << veh->getLane()->getID() << " pos=" << veh->getPositionOnLane(this) << "\n";
3854 : }
3855 : #endif
3856 6136411269 : if (veh != ego && veh->getPositionOnLane(this) < egoPos) {
3857 : //const double latOffset = veh->getLane()->getRightSideOnEdge() - getRightSideOnEdge();
3858 2714719947 : const double latOffset = veh->getLatOffset(this);
3859 2714719947 : double dist = backOffset - veh->getPositionOnLane(this) - veh->getVehicleType().getMinGap();
3860 2714719947 : if (veh->isBidiOn(this)) {
3861 82860 : dist -= veh->getLength();
3862 : }
3863 2714719947 : result.addFollower(veh, ego, dist, latOffset);
3864 : #ifdef DEBUG_CONTEXT
3865 : if (DEBUG_COND2(ego)) {
3866 : std::cout << " (1) added veh=" << veh->getID() << " latOffset=" << latOffset << " result=" << result.toString() << "\n";
3867 : }
3868 : #endif
3869 : }
3870 : }
3871 : #ifdef DEBUG_CONTEXT
3872 : if (DEBUG_COND2(ego)) {
3873 : std::cout << " result.numFreeSublanes=" << result.numFreeSublanes() << "\n";
3874 : }
3875 : #endif
3876 297968886 : if (result.numFreeSublanes() > 0) {
3877 : // do a tree search among all follower lanes and check for the most
3878 : // important vehicle (the one requiring the largest reargap)
3879 : // to get a safe bound on the necessary search depth, we need to consider the maximum speed and minimum
3880 : // deceleration of potential follower vehicles
3881 134586451 : if (searchDist == -1) {
3882 133175392 : searchDist = getMaximumBrakeDist() - backOffset;
3883 : #ifdef DEBUG_CONTEXT
3884 : if (DEBUG_COND2(ego)) {
3885 : std::cout << " computed searchDist=" << searchDist << "\n";
3886 : }
3887 : #endif
3888 : }
3889 : std::set<const MSEdge*> egoFurther;
3890 145793792 : for (MSLane* further : ego->getFurtherLanes()) {
3891 11207341 : egoFurther.insert(&further->getEdge());
3892 : }
3893 145246038 : if (ego->getPositionOnLane() < ego->getVehicleType().getLength() && egoFurther.size() == 0
3894 135647817 : && ego->getLane()->getLogicalPredecessorLane() != nullptr) {
3895 : // on insertion
3896 695161 : egoFurther.insert(&ego->getLane()->getLogicalPredecessorLane()->getEdge());
3897 : }
3898 :
3899 : // avoid loops
3900 134586451 : std::set<const MSLane*> visited(myEdge->getLanes().begin(), myEdge->getLanes().end());
3901 134586451 : if (myEdge->getBidiEdge() != nullptr) {
3902 : visited.insert(myEdge->getBidiEdge()->getLanes().begin(), myEdge->getBidiEdge()->getLanes().end());
3903 : }
3904 : std::vector<MSLane::IncomingLaneInfo> newFound;
3905 134586451 : std::vector<MSLane::IncomingLaneInfo> toExamine = myIncomingLanes;
3906 304610817 : while (toExamine.size() != 0) {
3907 392839556 : for (std::vector<MSLane::IncomingLaneInfo>::iterator it = toExamine.begin(); it != toExamine.end(); ++it) {
3908 222815190 : MSLane* next = (*it).lane;
3909 222815190 : searchDist = MAX2(searchDist, next->getMaximumBrakeDist() - backOffset);
3910 222815190 : MSLeaderInfo first = next->getFirstVehicleInformation(nullptr, 0, false, std::numeric_limits<double>::max(), false);
3911 222815190 : MSLeaderInfo firstFront = next->getFirstVehicleInformation(nullptr, 0, true);
3912 : #ifdef DEBUG_CONTEXT
3913 : if (DEBUG_COND2(ego)) {
3914 : std::cout << " next=" << next->getID() << " seen=" << (*it).length << " first=" << first.toString() << " firstFront=" << firstFront.toString() << " backOffset=" << backOffset << "\n";
3915 : gDebugFlag1 = true; // for calling getLeaderInfo
3916 : }
3917 : #endif
3918 222815190 : if (backOffset + (*it).length - next->getLength() < 0
3919 222815190 : && egoFurther.count(&next->getEdge()) != 0
3920 : ) {
3921 : // check for junction foes that would interfere with lane changing
3922 : // @note: we are passing the back of ego as its front position so
3923 : // we need to add this back to the returned gap
3924 11487627 : const MSLink::LinkLeaders linkLeaders = (*it).viaLink->getLeaderInfo(ego, -backOffset);
3925 11766624 : for (const auto& ll : linkLeaders) {
3926 278997 : if (ll.vehAndGap.first != nullptr) {
3927 278985 : const bool bidiFoe = (*it).viaLink->getLane() == ll.vehAndGap.first->getLane()->getNormalPredecessorLane()->getBidiLane();
3928 278985 : const bool egoIsLeader = !bidiFoe && ll.vehAndGap.first->isLeader((*it).viaLink, ego, ll.vehAndGap.second);
3929 : // if ego is leader the returned gap still assumes that ego follows the leader
3930 : // if the foe vehicle follows ego we need to deduce that gap
3931 : const double gap = (egoIsLeader
3932 278985 : ? -ll.vehAndGap.second - ll.vehAndGap.first->getVehicleType().getLengthWithGap() - ego->getVehicleType().getMinGap()
3933 407 : : ll.vehAndGap.second + ego->getVehicleType().getLength());
3934 278985 : result.addFollower(ll.vehAndGap.first, ego, gap);
3935 : #ifdef DEBUG_CONTEXT
3936 : if (DEBUG_COND2(ego)) {
3937 : std::cout << SIMTIME << " ego=" << ego->getID() << " link=" << (*it).viaLink->getViaLaneOrLane()->getID()
3938 : << " (3) added veh=" << Named::getIDSecure(ll.vehAndGap.first)
3939 : << " gap=" << ll.vehAndGap.second << " dtC=" << ll.distToCrossing
3940 : << " bidiFoe=" << bidiFoe
3941 : << " egoIsLeader=" << egoIsLeader << " gap2=" << gap
3942 : << "\n";
3943 : }
3944 : #endif
3945 : }
3946 : }
3947 11487627 : }
3948 : #ifdef DEBUG_CONTEXT
3949 : if (DEBUG_COND2(ego)) {
3950 : gDebugFlag1 = false;
3951 : }
3952 : #endif
3953 :
3954 961866136 : for (int i = 0; i < first.numSublanes(); ++i) {
3955 739050946 : const MSVehicle* v = first[i] == ego ? firstFront[i] : first[i];
3956 : double agap = 0;
3957 :
3958 739050946 : if (v != nullptr && v != ego) {
3959 198544842 : if (!v->isFrontOnLane(next)) {
3960 : // the front of v is already on divergent trajectory from the ego vehicle
3961 : // for which this method is called (in the context of MSLaneChanger).
3962 : // Therefore, technically v is not a follower but only an obstruction and
3963 : // the gap is not between the front of v and the back of ego
3964 : // but rather between the flank of v and the back of ego.
3965 30204494 : agap = (*it).length - next->getLength() + backOffset;
3966 30204494 : if (MSGlobals::gUsingInternalLanes) {
3967 : // ego should have left the intersection still occupied by v
3968 30195372 : agap -= v->getVehicleType().getMinGap();
3969 : }
3970 : #ifdef DEBUG_CONTEXT
3971 : if (DEBUG_COND2(ego)) {
3972 : std::cout << " agap1=" << agap << "\n";
3973 : }
3974 : #endif
3975 30204494 : const bool differentEdge = &v->getLane()->getEdge() != &ego->getLane()->getEdge();
3976 30204494 : if (agap > 0 && differentEdge) {
3977 : // Only if ego overlaps we treat v as if it were a real follower
3978 : // Otherwise we ignore it and look for another follower
3979 18959329 : if (!getOppositeLeaders) {
3980 : // even if the vehicle is not a real
3981 : // follower, it still forms a real
3982 : // obstruction in opposite direction driving
3983 18771260 : v = firstFront[i];
3984 18771260 : if (v != nullptr && v != ego) {
3985 14811349 : agap = (*it).length - v->getPositionOnLane() + backOffset - v->getVehicleType().getMinGap();
3986 : } else {
3987 : v = nullptr;
3988 : }
3989 : }
3990 11245165 : } else if (differentEdge && result.hasVehicle(v)) {
3991 : // ignore this vehicle as it was already seen on another lane
3992 : agap = 0;
3993 : }
3994 : } else {
3995 168340348 : if (next->getBidiLane() != nullptr && v->isBidiOn(next)) {
3996 76280 : agap = v->getPositionOnLane() + backOffset - v->getVehicleType().getLengthWithGap();
3997 : } else {
3998 168264068 : agap = (*it).length - v->getPositionOnLane() + backOffset - v->getVehicleType().getMinGap();
3999 : }
4000 168340348 : if (!(*it).viaLink->havePriority() && egoFurther.count(&(*it).lane->getEdge()) == 0
4001 11538717 : && ego->isOnRoad() // during insertion, this can lead to collisions because ego's further lanes are not set (see #3053)
4002 11441522 : && !ego->getLaneChangeModel().isOpposite()
4003 179780808 : && v->getSpeed() < SUMO_const_haltingSpeed
4004 : ) {
4005 : // if v is stopped on a minor side road it should not block lane changing
4006 : agap = MAX2(agap, 0.0);
4007 : }
4008 : }
4009 198544842 : result.addFollower(v, ego, agap, 0, i);
4010 : #ifdef DEBUG_CONTEXT
4011 : if (DEBUG_COND2(ego)) {
4012 : std::cout << " (2) added veh=" << Named::getIDSecure(v) << " agap=" << agap << " next=" << next->getID() << " result=" << result.toString() << "\n";
4013 : }
4014 : #endif
4015 : }
4016 : }
4017 222815190 : if ((*it).length < searchDist) {
4018 : const std::vector<MSLane::IncomingLaneInfo>& followers = next->getIncomingLanes();
4019 209604530 : for (std::vector<MSLane::IncomingLaneInfo>::const_iterator j = followers.begin(); j != followers.end(); ++j) {
4020 107935413 : if (visited.find((*j).lane) == visited.end() && (((*j).viaLink->havePriority() && !(*j).viaLink->isTurnaround())
4021 31049428 : || mLinkMode == MinorLinkMode::FOLLOW_ALWAYS
4022 1577588 : || (mLinkMode == MinorLinkMode::FOLLOW_ONCOMING && (*j).viaLink->getDirection() == LinkDirection::STRAIGHT))) {
4023 100721940 : visited.insert((*j).lane);
4024 : MSLane::IncomingLaneInfo ili;
4025 100721940 : ili.lane = (*j).lane;
4026 100721940 : ili.length = (*j).length + (*it).length;
4027 100721940 : ili.viaLink = (*j).viaLink;
4028 100721940 : newFound.push_back(ili);
4029 : }
4030 : }
4031 : }
4032 222815190 : }
4033 : toExamine.clear();
4034 : swap(newFound, toExamine);
4035 : }
4036 : //return result;
4037 :
4038 134586451 : }
4039 595937772 : return result;
4040 297968886 : }
4041 :
4042 :
4043 : void
4044 15963787 : MSLane::getLeadersOnConsecutive(double dist, double seen, double speed, const MSVehicle* ego,
4045 : const std::vector<MSLane*>& bestLaneConts, MSLeaderDistanceInfo& result,
4046 : bool oppositeDirection) const {
4047 15963787 : if (seen > dist && !(isInternal() && MSGlobals::gComputeLC)) {
4048 : return;
4049 : }
4050 : // check partial vehicles (they might be on a different route and thus not
4051 : // found when iterating along bestLaneConts)
4052 16755202 : for (VehCont::const_iterator it = myPartialVehicles.begin(); it != myPartialVehicles.end(); ++it) {
4053 2671031 : MSVehicle* veh = *it;
4054 2671031 : if (!veh->isFrontOnLane(this)) {
4055 791415 : result.addLeader(veh, seen, veh->getLatOffset(this));
4056 : } else {
4057 : break;
4058 : }
4059 : }
4060 : #ifdef DEBUG_CONTEXT
4061 : if (DEBUG_COND2(ego)) {
4062 : gDebugFlag1 = true;
4063 : }
4064 : #endif
4065 : const MSLane* nextLane = this;
4066 : int view = 1;
4067 : // loop over following lanes
4068 33942604 : while ((seen < dist || nextLane->isInternal()) && result.numFreeSublanes() > 0) {
4069 : // get the next link used
4070 : bool nextInternal = false;
4071 23050097 : if (oppositeDirection) {
4072 7720 : if (view >= (int)bestLaneConts.size()) {
4073 : break;
4074 : }
4075 2731 : nextLane = bestLaneConts[view];
4076 : } else {
4077 23042377 : std::vector<MSLink*>::const_iterator link = succLinkSec(*ego, view, *nextLane, bestLaneConts);
4078 23042377 : if (nextLane->isLinkEnd(link)) {
4079 : break;
4080 : }
4081 : // check for link leaders
4082 18836661 : const MSLink::LinkLeaders linkLeaders = (*link)->getLeaderInfo(ego, seen);
4083 18836661 : if (linkLeaders.size() > 0) {
4084 974052 : const MSLink::LinkLeader ll = linkLeaders[0];
4085 974052 : MSVehicle* veh = ll.vehAndGap.first;
4086 : // in the context of lane changing all junction leader candidates must be respected
4087 974052 : if (veh != 0 && (ego->isLeader(*link, veh, ll.vehAndGap.second)
4088 100534 : || (MSGlobals::gComputeLC
4089 1074586 : && veh->getPosition().distanceTo2D(ego->getPosition()) - veh->getVehicleType().getMinGap() - ego->getVehicleType().getLength()
4090 100534 : < veh->getCarFollowModel().brakeGap(veh->getSpeed())))) {
4091 : #ifdef DEBUG_CONTEXT
4092 : if (DEBUG_COND2(ego)) {
4093 : std::cout << " linkleader=" << veh->getID() << " gap=" << ll.vehAndGap.second << " leaderOffset=" << ll.latOffset << " flags=" << ll.llFlags << "\n";
4094 : }
4095 : #endif
4096 860575 : if (ll.sameTarget() || ll.sameSource()) {
4097 584771 : result.addLeader(veh, ll.vehAndGap.second, ll.latOffset);
4098 : } else {
4099 : // add link leader to all sublanes and return
4100 1895733 : for (int i = 0; i < result.numSublanes(); ++i) {
4101 1619929 : result.addLeader(veh, ll.vehAndGap.second, 0, i);
4102 : }
4103 : }
4104 : #ifdef DEBUG_CONTEXT
4105 : gDebugFlag1 = false;
4106 : #endif
4107 860575 : return;
4108 : } // XXX else, deal with pedestrians
4109 : }
4110 17976086 : nextInternal = (*link)->getViaLane() != nullptr;
4111 : nextLane = (*link)->getViaLaneOrLane();
4112 10607821 : if (nextLane == nullptr) {
4113 : break;
4114 : }
4115 18836661 : }
4116 :
4117 17978817 : MSLeaderInfo leaders = nextLane->getLastVehicleInformation(nullptr, 0, 0, false);
4118 : #ifdef DEBUG_CONTEXT
4119 : if (DEBUG_COND2(ego)) {
4120 : std::cout << SIMTIME << " getLeadersOnConsecutive lane=" << getID() << " nextLane=" << nextLane->getID() << " leaders=" << leaders.toString() << "\n";
4121 : }
4122 : #endif
4123 : // @todo check alignment issues if the lane width changes
4124 : const int iMax = MIN2(leaders.numSublanes(), result.numSublanes());
4125 96050524 : for (int i = 0; i < iMax; ++i) {
4126 78071707 : const MSVehicle* veh = leaders[i];
4127 78071707 : if (veh != nullptr) {
4128 : #ifdef DEBUG_CONTEXT
4129 : if (DEBUG_COND2(ego)) std::cout << " lead=" << veh->getID()
4130 : << " seen=" << seen
4131 : << " minGap=" << ego->getVehicleType().getMinGap()
4132 : << " backPos=" << veh->getBackPositionOnLane(nextLane)
4133 : << " gap=" << seen - ego->getVehicleType().getMinGap() + veh->getBackPositionOnLane(nextLane)
4134 : << "\n";
4135 : #endif
4136 31125574 : result.addLeader(veh, seen - ego->getVehicleType().getMinGap() + veh->getBackPositionOnLane(nextLane), 0, i);
4137 : }
4138 : }
4139 :
4140 17978817 : if (nextLane->getVehicleMaxSpeed(ego) < speed) {
4141 928617 : dist = ego->getCarFollowModel().brakeGap(nextLane->getVehicleMaxSpeed(ego));
4142 : }
4143 17978817 : seen += nextLane->getLength();
4144 17978817 : if (!nextInternal) {
4145 10610552 : view++;
4146 : }
4147 17978817 : }
4148 : #ifdef DEBUG_CONTEXT
4149 : gDebugFlag1 = false;
4150 : #endif
4151 : }
4152 :
4153 :
4154 : void
4155 122063916 : MSLane::addLeaders(const MSVehicle* vehicle, double vehPos, MSLeaderDistanceInfo& result, bool opposite) {
4156 : // if there are vehicles on the target lane with the same position as ego,
4157 : // they may not have been added to 'ahead' yet
4158 : #ifdef DEBUG_SURROUNDING
4159 : if (DEBUG_COND || DEBUG_COND2(vehicle)) {
4160 : std::cout << " addLeaders lane=" << getID() << " veh=" << vehicle->getID() << " vehPos=" << vehPos << " opposite=" << opposite << "\n";
4161 : }
4162 : #endif
4163 122063916 : const MSLeaderInfo& aheadSamePos = getLastVehicleInformation(nullptr, 0, vehPos, false);
4164 634077387 : for (int i = 0; i < aheadSamePos.numSublanes(); ++i) {
4165 512013471 : const MSVehicle* veh = aheadSamePos[i];
4166 512013471 : if (veh != nullptr && veh != vehicle) {
4167 137916114 : const double gap = veh->getBackPositionOnLane(this) - vehPos - vehicle->getVehicleType().getMinGap();
4168 : #ifdef DEBUG_SURROUNDING
4169 : if (DEBUG_COND || DEBUG_COND2(vehicle)) {
4170 : std::cout << " further lead=" << veh->getID() << " leadBack=" << veh->getBackPositionOnLane(this) << " gap=" << gap << "\n";
4171 : }
4172 : #endif
4173 137916114 : result.addLeader(veh, gap, 0, i);
4174 : }
4175 : }
4176 :
4177 122063916 : if (result.numFreeSublanes() > 0) {
4178 35906191 : double seen = vehicle->getLane()->getLength() - vehPos;
4179 35906191 : double speed = vehicle->getSpeed();
4180 : // leader vehicle could be link leader on the next junction
4181 35906191 : double dist = MAX2(vehicle->getCarFollowModel().brakeGap(speed), 10.0) + vehicle->getVehicleType().getMinGap();
4182 35906191 : if (getBidiLane() != nullptr) {
4183 56361 : dist = MAX2(dist, myMaxSpeed * 20);
4184 : }
4185 : // check for link leaders when on internal
4186 35906191 : if (seen > dist && !(isInternal() && MSGlobals::gComputeLC)) {
4187 : #ifdef DEBUG_SURROUNDING
4188 : if (DEBUG_COND || DEBUG_COND2(vehicle)) {
4189 : std::cout << " aborting forward search. dist=" << dist << " seen=" << seen << "\n";
4190 : }
4191 : #endif
4192 : return;
4193 : }
4194 : #ifdef DEBUG_SURROUNDING
4195 : if (DEBUG_COND || DEBUG_COND2(vehicle)) {
4196 : std::cout << " add consecutive before=" << result.toString() << " seen=" << seen << " dist=" << dist;
4197 : }
4198 : #endif
4199 15963787 : if (opposite) {
4200 6355 : const std::vector<MSLane*> bestLaneConts = vehicle->getUpstreamOppositeLanes();
4201 : #ifdef DEBUG_SURROUNDING
4202 : if (DEBUG_COND || DEBUG_COND2(vehicle)) {
4203 : std::cout << " upstreamOpposite=" << toString(bestLaneConts);
4204 : }
4205 : #endif
4206 6355 : getLeadersOnConsecutive(dist, seen, speed, vehicle, bestLaneConts, result, opposite);
4207 6355 : } else {
4208 15957432 : const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation(this);
4209 15957432 : getLeadersOnConsecutive(dist, seen, speed, vehicle, bestLaneConts, result);
4210 : }
4211 : #ifdef DEBUG_SURROUNDING
4212 : if (DEBUG_COND || DEBUG_COND2(vehicle)) {
4213 : std::cout << " after=" << result.toString() << "\n";
4214 : }
4215 : #endif
4216 : }
4217 122063916 : }
4218 :
4219 :
4220 : MSVehicle*
4221 469898996 : MSLane::getPartialBehind(const MSVehicle* ego) const {
4222 495634238 : for (VehCont::const_reverse_iterator i = myPartialVehicles.rbegin(); i != myPartialVehicles.rend(); ++i) {
4223 27399390 : MSVehicle* veh = *i;
4224 27399390 : if (veh->isFrontOnLane(this)
4225 3258109 : && veh != ego
4226 30657499 : && veh->getPositionOnLane() <= ego->getPositionOnLane()) {
4227 : #ifdef DEBUG_CONTEXT
4228 : if (DEBUG_COND2(ego)) {
4229 : std::cout << SIMTIME << " getPartialBehind lane=" << getID() << " ego=" << ego->getID() << " found=" << veh->getID() << "\n";
4230 : }
4231 : #endif
4232 : return veh;
4233 : }
4234 : }
4235 : #ifdef DEBUG_CONTEXT
4236 : if (DEBUG_COND2(ego)) {
4237 : std::cout << SIMTIME << " getPartialBehind lane=" << getID() << " ego=" << ego->getID() << " nothing found. partials=" << toString(myPartialVehicles) << "\n";
4238 : }
4239 : #endif
4240 : return nullptr;
4241 : }
4242 :
4243 : MSLeaderInfo
4244 19751683 : MSLane::getPartialBeyond() const {
4245 19751683 : MSLeaderInfo result(myWidth);
4246 21158729 : for (VehCont::const_iterator it = myPartialVehicles.begin(); it != myPartialVehicles.end(); ++it) {
4247 2914311 : MSVehicle* veh = *it;
4248 2914311 : if (!veh->isFrontOnLane(this)) {
4249 1407046 : result.addLeader(veh, false, veh->getLatOffset(this));
4250 : } else {
4251 : break;
4252 : }
4253 : }
4254 19751683 : return result;
4255 0 : }
4256 :
4257 :
4258 : std::set<MSVehicle*>
4259 11180 : MSLane::getSurroundingVehicles(double startPos, double downstreamDist, double upstreamDist, std::shared_ptr<LaneCoverageInfo> checkedLanes) const {
4260 : assert(checkedLanes != nullptr);
4261 11180 : if (checkedLanes->find(this) != checkedLanes->end()) {
4262 : #ifdef DEBUG_SURROUNDING
4263 : std::cout << "Skipping previously scanned lane: " << getID() << std::endl;
4264 : #endif
4265 2266 : return std::set<MSVehicle*>();
4266 : } else {
4267 : // Add this lane's coverage to the lane coverage info
4268 18841 : (*checkedLanes)[this] = std::make_pair(MAX2(0.0, startPos - upstreamDist), MIN2(startPos + downstreamDist, getLength()));
4269 : }
4270 : #ifdef DEBUG_SURROUNDING
4271 : std::cout << "Scanning on lane " << myID << "(downstr. " << downstreamDist << ", upstr. " << upstreamDist << ", startPos " << startPos << "): " << std::endl;
4272 : #endif
4273 14492 : std::set<MSVehicle*> foundVehicles = getVehiclesInRange(MAX2(0., startPos - upstreamDist), MIN2(myLength, startPos + downstreamDist));
4274 8914 : if (startPos < upstreamDist) {
4275 : // scan incoming lanes
4276 10886 : for (const IncomingLaneInfo& incomingInfo : getIncomingLanes()) {
4277 5412 : MSLane* incoming = incomingInfo.lane;
4278 : #ifdef DEBUG_SURROUNDING
4279 : std::cout << "Checking on incoming: " << incoming->getID() << std::endl;
4280 : if (checkedLanes->find(incoming) != checkedLanes->end()) {
4281 : std::cout << "Skipping previous: " << incoming->getID() << std::endl;
4282 : }
4283 : #endif
4284 10824 : std::set<MSVehicle*> newVehs = incoming->getSurroundingVehicles(incoming->getLength(), 0.0, upstreamDist - startPos, checkedLanes);
4285 5412 : foundVehicles.insert(newVehs.begin(), newVehs.end());
4286 : }
4287 : }
4288 :
4289 8914 : if (getLength() < startPos + downstreamDist) {
4290 : // scan successive lanes
4291 : const std::vector<MSLink*>& lc = getLinkCont();
4292 6130 : for (MSLink* l : lc) {
4293 : #ifdef DEBUG_SURROUNDING
4294 : std::cout << "Checking on outgoing: " << l->getViaLaneOrLane()->getID() << std::endl;
4295 : #endif
4296 5588 : std::set<MSVehicle*> newVehs = l->getViaLaneOrLane()->getSurroundingVehicles(0.0, downstreamDist - (myLength - startPos), upstreamDist, checkedLanes);
4297 2794 : foundVehicles.insert(newVehs.begin(), newVehs.end());
4298 : }
4299 : }
4300 : #ifdef DEBUG_SURROUNDING
4301 : std::cout << "On lane (2) " << myID << ": \nFound vehicles: " << std::endl;
4302 : for (MSVehicle* v : foundVehicles) {
4303 : std::cout << v->getID() << " pos = " << v->getPositionOnLane() << std::endl;
4304 : }
4305 : #endif
4306 : return foundVehicles;
4307 : }
4308 :
4309 :
4310 : std::set<MSVehicle*>
4311 11749 : MSLane::getVehiclesInRange(const double a, const double b) const {
4312 : std::set<MSVehicle*> res;
4313 11749 : const VehCont& vehs = getVehiclesSecure();
4314 :
4315 11749 : if (!vehs.empty()) {
4316 12545 : for (MSVehicle* const veh : vehs) {
4317 8356 : if (veh->getPositionOnLane() >= a) {
4318 7012 : if (veh->getBackPositionOnLane() > b) {
4319 : break;
4320 : }
4321 : res.insert(veh);
4322 : }
4323 : }
4324 : }
4325 11749 : releaseVehicles();
4326 11749 : return res;
4327 : }
4328 :
4329 :
4330 : std::vector<const MSJunction*>
4331 0 : MSLane::getUpcomingJunctions(double pos, double range, const std::vector<MSLane*>& contLanes) const {
4332 : // set of upcoming junctions and the corresponding conflict links
4333 : std::vector<const MSJunction*> junctions;
4334 0 : for (auto l : getUpcomingLinks(pos, range, contLanes)) {
4335 0 : junctions.insert(junctions.end(), l->getJunction());
4336 0 : }
4337 0 : return junctions;
4338 0 : }
4339 :
4340 :
4341 : std::vector<const MSLink*>
4342 734 : MSLane::getUpcomingLinks(double pos, double range, const std::vector<MSLane*>& contLanes) const {
4343 : #ifdef DEBUG_SURROUNDING
4344 : std::cout << "getUpcoming links on lane '" << getID() << "' with pos=" << pos
4345 : << " range=" << range << std::endl;
4346 : #endif
4347 : // set of upcoming junctions and the corresponding conflict links
4348 : std::vector<const MSLink*> links;
4349 :
4350 : // Currently scanned lane
4351 : const MSLane* lane = this;
4352 :
4353 : // continuation lanes for the vehicle
4354 : std::vector<MSLane*>::const_iterator contLanesIt = contLanes.begin();
4355 : // scanned distance so far
4356 : double dist = 0.0;
4357 : // link to be crossed by the vehicle
4358 734 : const MSLink* link = nullptr;
4359 734 : if (lane->isInternal()) {
4360 : assert(*contLanesIt == nullptr); // is called with vehicle's bestLane structure
4361 113 : link = lane->getEntryLink();
4362 113 : links.insert(links.end(), link);
4363 113 : dist += link->getInternalLengthsAfter();
4364 : // next non-internal lane behind junction
4365 : lane = link->getLane();
4366 : pos = 0.0;
4367 : assert(*(contLanesIt + 1) == lane);
4368 : }
4369 1224 : while (++contLanesIt != contLanes.end()) {
4370 : assert(!lane->isInternal());
4371 934 : dist += lane->getLength() - pos;
4372 : pos = 0.;
4373 : #ifdef DEBUG_SURROUNDING
4374 : std::cout << "Distance until end of lane '" << lane->getID() << "' is " << dist << "." << std::endl;
4375 : #endif
4376 934 : if (dist > range) {
4377 : break;
4378 : }
4379 490 : link = lane->getLinkTo(*contLanesIt);
4380 490 : if (link != nullptr) {
4381 478 : links.insert(links.end(), link);
4382 : }
4383 490 : lane = *contLanesIt;
4384 : }
4385 734 : return links;
4386 0 : }
4387 :
4388 :
4389 : MSLane*
4390 243336760 : MSLane::getOpposite() const {
4391 243336760 : return myOpposite;
4392 : }
4393 :
4394 :
4395 : MSLane*
4396 185097721 : MSLane::getParallelOpposite() const {
4397 185097721 : return myEdge->getLanes().back()->getOpposite();
4398 : }
4399 :
4400 :
4401 : double
4402 7562826 : MSLane::getOppositePos(double pos) const {
4403 7562826 : return MAX2(0., myLength - pos);
4404 : }
4405 :
4406 : std::pair<MSVehicle* const, double>
4407 9072746 : MSLane::getFollower(const MSVehicle* ego, double egoPos, double dist, MinorLinkMode mLinkMode) const {
4408 33072461 : for (AnyVehicleIterator first = anyVehiclesUpstreamBegin(); first != anyVehiclesUpstreamEnd(); ++first) {
4409 : // XXX refactor leaderInfo to use a const vehicle all the way through the call hierarchy
4410 40536859 : MSVehicle* pred = (MSVehicle*)*first;
4411 : #ifdef DEBUG_CONTEXT
4412 : if (DEBUG_COND2(ego)) {
4413 : std::cout << " getFollower lane=" << getID() << " egoPos=" << egoPos << " pred=" << pred->getID() << " predPos=" << pred->getPositionOnLane(this) << "\n";
4414 : }
4415 : #endif
4416 40536859 : if (pred != ego && pred->getPositionOnLane(this) < egoPos) {
4417 7464398 : return std::pair<MSVehicle* const, double>(pred, egoPos - pred->getPositionOnLane(this) - ego->getVehicleType().getLength() - pred->getVehicleType().getMinGap());
4418 : }
4419 : }
4420 1608348 : const double backOffset = egoPos - ego->getVehicleType().getLength();
4421 1608348 : if (dist > 0 && backOffset > dist) {
4422 245479 : return std::make_pair(nullptr, -1);
4423 : }
4424 1362869 : const MSLeaderDistanceInfo followers = getFollowersOnConsecutive(ego, backOffset, true, dist, mLinkMode);
4425 1362869 : CLeaderDist result = followers.getClosest();
4426 1362869 : return std::make_pair(const_cast<MSVehicle*>(result.first), result.second);
4427 1362869 : }
4428 :
4429 : std::pair<MSVehicle* const, double>
4430 5136006 : MSLane::getOppositeLeader(const MSVehicle* ego, double dist, bool oppositeDir, MinorLinkMode mLinkMode) const {
4431 : #ifdef DEBUG_OPPOSITE
4432 : if (DEBUG_COND2(ego)) std::cout << SIMTIME << " getOppositeLeader lane=" << getID()
4433 : << " ego=" << ego->getID()
4434 : << " pos=" << ego->getPositionOnLane()
4435 : << " posOnOpposite=" << getOppositePos(ego->getPositionOnLane())
4436 : << " dist=" << dist
4437 : << " oppositeDir=" << oppositeDir
4438 : << "\n";
4439 : #endif
4440 5136006 : if (!oppositeDir) {
4441 384958 : return getLeader(ego, getOppositePos(ego->getPositionOnLane()), ego->getBestLanesContinuation(this));
4442 : } else {
4443 4751048 : const double egoLength = ego->getVehicleType().getLength();
4444 4751048 : const double egoPos = ego->getLaneChangeModel().isOpposite() ? ego->getPositionOnLane() : getOppositePos(ego->getPositionOnLane());
4445 4751048 : std::pair<MSVehicle* const, double> result = getFollower(ego, egoPos + egoLength, dist, mLinkMode);
4446 4751048 : if (result.first != nullptr) {
4447 4105770 : result.second -= ego->getVehicleType().getMinGap();
4448 4105770 : if (result.first->getLaneChangeModel().isOpposite()) {
4449 1310004 : result.second -= result.first->getVehicleType().getLength();
4450 : }
4451 : }
4452 4751048 : return result;
4453 : }
4454 : }
4455 :
4456 :
4457 : std::pair<MSVehicle* const, double>
4458 701658 : MSLane::getOppositeFollower(const MSVehicle* ego) const {
4459 : #ifdef DEBUG_OPPOSITE
4460 : if (DEBUG_COND2(ego)) std::cout << SIMTIME << " getOppositeFollower lane=" << getID()
4461 : << " ego=" << ego->getID()
4462 : << " backPos=" << ego->getBackPositionOnLane()
4463 : << " posOnOpposite=" << getOppositePos(ego->getBackPositionOnLane())
4464 : << "\n";
4465 : #endif
4466 701658 : if (ego->getLaneChangeModel().isOpposite()) {
4467 415704 : std::pair<MSVehicle* const, double> result = getFollower(ego, getOppositePos(ego->getPositionOnLane()), -1, MinorLinkMode::FOLLOW_NEVER);
4468 415704 : return result;
4469 : } else {
4470 285954 : double vehPos = getOppositePos(ego->getPositionOnLane() - ego->getVehicleType().getLength());
4471 285954 : std::pair<MSVehicle*, double> result = getLeader(ego, vehPos, std::vector<MSLane*>());
4472 285954 : double dist = getMaximumBrakeDist() + getOppositePos(ego->getPositionOnLane() - getLength());
4473 : MSLane* next = const_cast<MSLane*>(this);
4474 512127 : while (result.first == nullptr && dist > 0) {
4475 : // cannot call getLeadersOnConsecutive because succLinkSec doesn't
4476 : // uses the vehicle's route and doesn't work on the opposite side
4477 286535 : vehPos -= next->getLength();
4478 286535 : next = next->getCanonicalSuccessorLane();
4479 286535 : if (next == nullptr) {
4480 : break;
4481 : }
4482 226173 : dist -= next->getLength();
4483 226173 : result = next->getLeader(ego, vehPos, std::vector<MSLane*>());
4484 : }
4485 285954 : if (result.first != nullptr) {
4486 220612 : if (result.first->getLaneChangeModel().isOpposite()) {
4487 81772 : result.second -= result.first->getVehicleType().getLength();
4488 : } else {
4489 138840 : if (result.second > POSITION_EPS) {
4490 : // follower can be safely ignored since it is going the other way
4491 130763 : return std::make_pair(static_cast<MSVehicle*>(nullptr), -1);
4492 : }
4493 : }
4494 : }
4495 155191 : return result;
4496 : }
4497 : }
4498 :
4499 : void
4500 80824 : MSLane::initCollisionAction(const OptionsCont& oc, const std::string& option, CollisionAction& myAction) {
4501 80824 : const std::string action = oc.getString(option);
4502 80824 : if (action == "none") {
4503 15 : myAction = COLLISION_ACTION_NONE;
4504 80809 : } else if (action == "warn") {
4505 41008 : myAction = COLLISION_ACTION_WARN;
4506 39801 : } else if (action == "teleport") {
4507 39766 : myAction = COLLISION_ACTION_TELEPORT;
4508 35 : } else if (action == "remove") {
4509 35 : myAction = COLLISION_ACTION_REMOVE;
4510 : } else {
4511 0 : WRITE_ERROR(TLF("Invalid % '%'.", option, action));
4512 : }
4513 80824 : }
4514 :
4515 : void
4516 40412 : MSLane::initCollisionOptions(const OptionsCont& oc) {
4517 40412 : initCollisionAction(oc, "collision.action", myCollisionAction);
4518 40412 : initCollisionAction(oc, "intermodal-collision.action", myIntermodalCollisionAction);
4519 40412 : myCheckJunctionCollisions = oc.getBool("collision.check-junctions");
4520 40412 : myCheckJunctionCollisionMinGap = oc.getFloat("collision.check-junctions.mingap");
4521 40412 : myCollisionStopTime = string2time(oc.getString("collision.stoptime"));
4522 40412 : myIntermodalCollisionStopTime = string2time(oc.getString("intermodal-collision.stoptime"));
4523 40412 : myCollisionMinGapFactor = oc.getFloat("collision.mingap-factor");
4524 40412 : myExtrapolateSubstepDepart = oc.getBool("extrapolate-departpos");
4525 40412 : }
4526 :
4527 :
4528 : void
4529 1183 : MSLane::setPermissions(SVCPermissions permissions, long long transientID) {
4530 1183 : if (transientID == CHANGE_PERMISSIONS_PERMANENT) {
4531 85 : myPermissions = permissions;
4532 85 : myOriginalPermissions = permissions;
4533 : } else {
4534 1098 : myPermissionChanges[transientID] = permissions;
4535 1098 : resetPermissions(CHANGE_PERMISSIONS_PERMANENT);
4536 : }
4537 1183 : }
4538 :
4539 :
4540 : void
4541 1387 : MSLane::resetPermissions(long long transientID) {
4542 : myPermissionChanges.erase(transientID);
4543 1387 : if (myPermissionChanges.empty()) {
4544 271 : myPermissions = myOriginalPermissions;
4545 : } else {
4546 : // combine all permission changes
4547 1116 : myPermissions = SVCAll;
4548 2248 : for (const auto& item : myPermissionChanges) {
4549 1132 : myPermissions &= item.second;
4550 : }
4551 : }
4552 1387 : }
4553 :
4554 :
4555 : bool
4556 12549168 : MSLane::hadPermissionChanges() const {
4557 12549168 : return !myPermissionChanges.empty();
4558 : }
4559 :
4560 :
4561 : void
4562 9 : MSLane::setChangeLeft(SVCPermissions permissions) {
4563 9 : myChangeLeft = permissions;
4564 9 : }
4565 :
4566 :
4567 : void
4568 9 : MSLane::setChangeRight(SVCPermissions permissions) {
4569 9 : myChangeRight = permissions;
4570 9 : }
4571 :
4572 :
4573 : bool
4574 48183973 : MSLane::hasPedestrians() const {
4575 48183973 : MSNet* const net = MSNet::getInstance();
4576 48183973 : return net->hasPersons() && net->getPersonControl().getMovementModel()->hasPedestrians(this);
4577 : }
4578 :
4579 :
4580 : PersonDist
4581 701654 : MSLane::nextBlocking(double minPos, double minRight, double maxLeft, double stopTime, bool bidi) const {
4582 701654 : return MSNet::getInstance()->getPersonControl().getMovementModel()->nextBlocking(this, minPos, minRight, maxLeft, stopTime, bidi);
4583 : }
4584 :
4585 :
4586 : bool
4587 3967036 : MSLane::checkForPedestrians(const MSVehicle* aVehicle, double& speed, double& dist, double pos, bool patchSpeed) const {
4588 3967036 : if (getEdge().getPersons().size() > 0 && hasPedestrians()) {
4589 : #ifdef DEBUG_INSERTION
4590 : if (DEBUG_COND2(aVehicle)) {
4591 : std::cout << SIMTIME << " check for pedestrians on lane=" << getID() << " pos=" << pos << "\n";
4592 : }
4593 : #endif
4594 3814 : PersonDist leader = nextBlocking(pos - aVehicle->getVehicleType().getLength(),
4595 1907 : aVehicle->getRightSideOnLane(), aVehicle->getRightSideOnLane() + aVehicle->getVehicleType().getWidth(), ceil(speed / aVehicle->getCarFollowModel().getMaxDecel()));
4596 1907 : if (leader.first != 0) {
4597 772 : const double gap = leader.second - aVehicle->getVehicleType().getLengthWithGap();
4598 772 : const double stopSpeed = aVehicle->getCarFollowModel().stopSpeed(aVehicle, speed, gap, MSCFModel::CalcReason::FUTURE);
4599 284 : if ((gap < 0 && (aVehicle->getInsertionChecks() & ((int)InsertionCheck::COLLISION | (int)InsertionCheck::PEDESTRIAN)) != 0)
4600 1544 : || checkFailure(aVehicle, speed, dist, stopSpeed, patchSpeed, "", InsertionCheck::PEDESTRIAN)) {
4601 : // we may not drive with the given velocity - we crash into the pedestrian
4602 : #ifdef DEBUG_INSERTION
4603 : if (DEBUG_COND2(aVehicle)) std::cout << SIMTIME
4604 : << " isInsertionSuccess lane=" << getID()
4605 : << " veh=" << aVehicle->getID()
4606 : << " pos=" << pos
4607 : << " posLat=" << aVehicle->getLateralPositionOnLane()
4608 : << " patchSpeed=" << patchSpeed
4609 : << " speed=" << speed
4610 : << " stopSpeed=" << stopSpeed
4611 : << " pedestrianLeader=" << leader.first->getID()
4612 : << " failed (@796)!\n";
4613 : #endif
4614 498 : return false;
4615 : }
4616 : }
4617 : }
4618 3966538 : double backLength = aVehicle->getLength() - pos;
4619 3966538 : if (backLength > 0 && MSNet::getInstance()->hasPersons()) {
4620 : // look upstream for pedestrian crossings
4621 5550 : const MSLane* prev = getLogicalPredecessorLane();
4622 : const MSLane* cur = this;
4623 93683 : while (backLength > 0 && prev != nullptr) {
4624 88165 : const MSLink* link = prev->getLinkTo(cur);
4625 88165 : if (link->hasFoeCrossing()) {
4626 162 : for (const MSLane* foe : link->getFoeLanes()) {
4627 117 : if (foe->isCrossing() && (foe->hasPedestrians() ||
4628 83 : (foe->getIncomingLanes()[0].viaLink->getApproachingPersons() != nullptr
4629 47 : && foe->getIncomingLanes()[0].viaLink->getApproachingPersons()->size() > 0))) {
4630 : #ifdef DEBUG_INSERTION
4631 : if (DEBUG_COND2(aVehicle)) std::cout << SIMTIME
4632 : << " isInsertionSuccess lane=" << getID()
4633 : << " veh=" << aVehicle->getID()
4634 : << " pos=" << pos
4635 : << " backCrossing=" << foe->getID()
4636 : << " peds=" << joinNamedToString(foe->getEdge().getPersons(), " ")
4637 : << " approaching=" << foe->getIncomingLanes()[0].viaLink->getApproachingPersons()->size()
4638 : << " failed (@4550)!\n";
4639 : #endif
4640 : return false;
4641 : }
4642 : }
4643 : }
4644 88133 : backLength -= prev->getLength();
4645 : cur = prev;
4646 88133 : prev = prev->getLogicalPredecessorLane();
4647 : }
4648 : }
4649 : return true;
4650 : }
4651 :
4652 :
4653 : void
4654 40414 : MSLane::initRNGs(const OptionsCont& oc) {
4655 : myRNGs.clear();
4656 40414 : const int numRNGs = oc.getInt("thread-rngs");
4657 40414 : const bool random = oc.getBool("random");
4658 40414 : int seed = oc.getInt("seed");
4659 40414 : myRNGs.reserve(numRNGs); // this is needed for stable pointers on debugging
4660 2626910 : for (int i = 0; i < numRNGs; i++) {
4661 5172992 : myRNGs.push_back(SumoRNG("lanes_" + toString(i)));
4662 2586496 : RandHelper::initRand(&myRNGs.back(), random, seed++);
4663 : }
4664 40414 : }
4665 :
4666 : void
4667 11 : MSLane::saveRNGStates(OutputDevice& out) {
4668 715 : for (int i = 0; i < getNumRNGs(); i++) {
4669 704 : out.openTag(SUMO_TAG_RNGLANE);
4670 704 : out.writeAttr(SUMO_ATTR_INDEX, i);
4671 704 : out.writeAttr(SUMO_ATTR_STATE, RandHelper::saveState(&myRNGs[i]));
4672 1408 : out.closeTag();
4673 : }
4674 11 : }
4675 :
4676 : void
4677 704 : MSLane::loadRNGState(int index, const std::string& state) {
4678 704 : if (index >= getNumRNGs()) {
4679 0 : throw ProcessError(TLF("State was saved with more than % threads. Change the number of threads or do not load RNG state", toString(getNumRNGs())));
4680 : }
4681 704 : RandHelper::loadState(state, &myRNGs[index]);
4682 704 : }
4683 :
4684 :
4685 : MSLane*
4686 17936574797 : MSLane::getBidiLane() const {
4687 17936574797 : return myBidiLane;
4688 : }
4689 :
4690 :
4691 : bool
4692 124518836 : MSLane::mustCheckJunctionCollisions() const {
4693 124518836 : return myCheckJunctionCollisions && myEdge->isInternal() && (
4694 1412273 : myLinks.front()->getFoeLanes().size() > 0
4695 8862 : || myLinks.front()->getWalkingAreaFoe() != nullptr
4696 7954 : || myLinks.front()->getWalkingAreaFoeExit() != nullptr);
4697 : }
4698 :
4699 :
4700 : double
4701 603431813 : MSLane::getSpaceTillLastStanding(const MSVehicle* ego, bool& foundStopped) const {
4702 : /// @todo if ego isn't on this lane, we could use a cached value
4703 : double lengths = 0;
4704 5078709423 : for (const MSVehicle* last : myVehicles) {
4705 4526328794 : if (last->getSpeed() < SUMO_const_haltingSpeed && !last->getLane()->getEdge().isRoundabout()
4706 24931294 : && last != ego
4707 : // @todo recheck
4708 4525115068 : && last->isFrontOnLane(this)) {
4709 24918729 : foundStopped = true;
4710 24918729 : const double lastBrakeGap = last->getCarFollowModel().brakeGap(last->getSpeed());
4711 24918729 : const double ret = last->getBackPositionOnLane() + lastBrakeGap - lengths;
4712 : return ret;
4713 : }
4714 4475277610 : if (MSGlobals::gSublane && ego->getVehicleType().getWidth() + last->getVehicleType().getWidth() < getWidth()) {
4715 32020498 : lengths += last->getVehicleType().getLengthWithGap() * (last->getVehicleType().getWidth() + last->getVehicleType().getMinGapLat()) / getWidth();
4716 : } else {
4717 4443257112 : lengths += last->getVehicleType().getLengthWithGap();
4718 : }
4719 : }
4720 578513084 : return getLength() - lengths;
4721 : }
4722 :
4723 :
4724 : bool
4725 11416783782 : MSLane::allowsVehicleClass(SUMOVehicleClass vclass, int routingMode) const {
4726 11416783782 : return (((routingMode & libsumo::ROUTING_MODE_IGNORE_TRANSIENT_PERMISSIONS) ? myOriginalPermissions : myPermissions) & vclass) == vclass;
4727 : }
4728 :
4729 :
4730 : const MSJunction*
4731 290846 : MSLane::getFromJunction() const {
4732 290846 : return myEdge->getFromJunction();
4733 : }
4734 :
4735 :
4736 : const MSJunction*
4737 290846 : MSLane::getToJunction() const {
4738 290846 : return myEdge->getToJunction();
4739 : }
4740 :
4741 : /****************************************************************************/
|
