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