1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2001-2019 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials
5 // are made available under the terms of the Eclipse Public License v2.0
6 // which accompanies this distribution, and is available at
7 // http://www.eclipse.org/legal/epl-v20.html
8 // SPDX-License-Identifier: EPL-2.0
9 /****************************************************************************/
10 /// @file MSLane.cpp
11 /// @author Christian Roessel
12 /// @author Jakob Erdmann
13 /// @author Daniel Krajzewicz
14 /// @author Tino Morenz
15 /// @author Axel Wegener
16 /// @author Michael Behrisch
17 /// @author Christoph Sommer
18 /// @author Mario Krumnow
19 /// @author Leonhard Luecken
20 /// @date Mon, 05 Mar 2001
21 /// @version $Id$
22 ///
23 // Representation of a lane in the micro simulation
24 /****************************************************************************/
25
26
27 // ===========================================================================
28 // included modules
29 // ===========================================================================
30 #include <config.h>
31
32 #include <cmath>
33 #include <bitset>
34 #include <iostream>
35 #include <cassert>
36 #include <functional>
37 #include <algorithm>
38 #include <iterator>
39 #include <exception>
40 #include <climits>
41 #include <set>
42 #include <utils/common/UtilExceptions.h>
43 #include <utils/common/StdDefs.h>
44 #include <utils/common/MsgHandler.h>
45 #include <utils/common/ToString.h>
46 #ifdef HAVE_FOX
47 #include <utils/foxtools/FXConditionalLock.h>
48 #endif
49 #include <utils/options/OptionsCont.h>
50 #include <utils/emissions/HelpersHarmonoise.h>
51 #include <utils/geom/GeomHelper.h>
52 #include <microsim/pedestrians/MSPModel.h>
53 #include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
54 #include "MSNet.h"
55 #include "MSVehicleType.h"
56 #include "MSEdge.h"
57 #include "MSEdgeControl.h"
58 #include "MSJunction.h"
59 #include "MSLogicJunction.h"
60 #include "MSLink.h"
61 #include "MSLane.h"
62 #include "MSVehicleTransfer.h"
63 #include "MSGlobals.h"
64 #include "MSVehicleControl.h"
65 #include "MSInsertionControl.h"
66 #include "MSVehicleControl.h"
67 #include "MSLeaderInfo.h"
68 #include "MSVehicle.h"
69 #include "cfmodels/MSCFModel_CC.h"
70
71 //#define DEBUG_INSERTION
72 //#define DEBUG_PLAN_MOVE
73 //#define DEBUG_EXEC_MOVE
74 //#define DEBUG_CONTEXT
75 //#define DEBUG_OPPOSITE
76 //#define DEBUG_VEHICLE_CONTAINER
77 //#define DEBUG_COLLISIONS
78 //#define DEBUG_JUNCTION_COLLISIONS
79 //#define DEBUG_PEDESTRIAN_COLLISIONS
80 //#define DEBUG_LANE_SORTER
81 //#define DEBUG_NO_CONNECTION
82 //#define DEBUG_SURROUNDING
83
84 //#define DEBUG_COND (false)
85 //#define DEBUG_COND (true)
86 //#define DEBUG_COND (getID() == "undefined")
87 //#define DEBUG_COND2(obj) ((obj != 0 && (obj)->getID() == "disabled"))
88 //#define DEBUG_COND2(obj) ((obj != 0 && (obj)->isSelected()))
89 //#define DEBUG_COND (getID() == "ego")
90 //#define DEBUG_COND2(obj) ((obj != 0 && (obj)->getID() == "ego"))
91 //#define DEBUG_COND2(obj) (true)
92
93
94 // ===========================================================================
95 // static member definitions
96 // ===========================================================================
97 MSLane::DictType MSLane::myDict;
98 MSLane::CollisionAction MSLane::myCollisionAction(MSLane::COLLISION_ACTION_TELEPORT);
99 bool MSLane::myCheckJunctionCollisions(false);
100 SUMOTime MSLane::myCollisionStopTime(0);
101 double MSLane::myCollisionMinGapFactor(1.0);
102 std::vector<std::mt19937> MSLane::myRNGs;
103
104
105 // ===========================================================================
106 // internal class method definitions
107 // ===========================================================================
108 MSLane::AnyVehicleIterator&
operator ++()109 MSLane::AnyVehicleIterator::operator++() {
110 if (nextIsMyVehicles()) {
111 if (myI1 != myI1End) {
112 myI1 += myDirection;
113 } else if (myI3 != myI3End) {
114 myI3 += myDirection;
115 }
116 // else: already at end
117 } else {
118 myI2 += myDirection;
119 }
120 //if (DEBUG_COND2(myLane)) std::cout << SIMTIME << " AnyVehicleIterator::operator++ lane=" << myLane->getID() << " myI1=" << myI1 << " myI2=" << myI2 << "\n";
121 return *this;
122 }
123
124
125 const MSVehicle*
operator *()126 MSLane::AnyVehicleIterator::operator*() {
127 if (nextIsMyVehicles()) {
128 if (myI1 != myI1End) {
129 return myLane->myVehicles[myI1];
130 } else if (myI3 != myI3End) {
131 return myLane->myTmpVehicles[myI3];
132 } else {
133 return nullptr;
134 }
135 } else {
136 return myLane->myPartialVehicles[myI2];
137 }
138 }
139
140
141 bool
nextIsMyVehicles() const142 MSLane::AnyVehicleIterator::nextIsMyVehicles() const {
143 //if (DEBUG_COND2(myLane)) std::cout << SIMTIME << " AnyVehicleIterator::nextIsMyVehicles lane=" << myLane->getID()
144 // << " myI1=" << myI1
145 // << " myI2=" << myI2
146 // << "\n";
147 if (myI1 == myI1End && myI3 == myI3End) {
148 if (myI2 != myI2End) {
149 return false;
150 } else {
151 return true; // @note. must be caught
152 }
153 } else {
154 if (myI2 == myI2End) {
155 return true;
156 } else {
157 MSVehicle* cand = myI1 == myI1End ? myLane->myTmpVehicles[myI3] : myLane->myVehicles[myI1];
158 //if (DEBUG_COND2(myLane)) std::cout << " "
159 // << " veh1=" << candFull->getID()
160 // << " isTmp=" << (myI1 == myI1End)
161 // << " veh2=" << myLane->myPartialVehicles[myI2]->getID()
162 // << " pos1=" << cand->getPositionOnLane(myLane)
163 // << " pos2=" << myLane->myPartialVehicles[myI2]->getPositionOnLane(myLane)
164 // << "\n";
165 if (cand->getPositionOnLane() < myLane->myPartialVehicles[myI2]->getPositionOnLane(myLane)) {
166 return myDownstream;
167 } else {
168 return !myDownstream;
169 }
170 }
171 }
172 }
173
174
175 // ===========================================================================
176 // member method definitions
177 // ===========================================================================
MSLane(const std::string & id,double maxSpeed,double length,MSEdge * const edge,int numericalID,const PositionVector & shape,double width,SVCPermissions permissions,int index,bool isRampAccel)178 MSLane::MSLane(const std::string& id, double maxSpeed, double length, MSEdge* const edge,
179 int numericalID, const PositionVector& shape, double width,
180 SVCPermissions permissions, int index, bool isRampAccel) :
181 Named(id),
182 myNumericalID(numericalID), myShape(shape), myIndex(index),
183 myVehicles(), myLength(length), myWidth(width), myStopOffsets(),
184 myEdge(edge), myMaxSpeed(maxSpeed),
185 myPermissions(permissions),
186 myOriginalPermissions(permissions),
187 myLogicalPredecessorLane(nullptr),
188 myCanonicalPredecessorLane(nullptr),
189 myCanonicalSuccessorLane(nullptr),
190 myBruttoVehicleLengthSum(0), myNettoVehicleLengthSum(0),
191 myBruttoVehicleLengthSumToRemove(0), myNettoVehicleLengthSumToRemove(0),
192 myLeaderInfo(this, nullptr, 0),
193 myFollowerInfo(this, nullptr, 0),
194 myLeaderInfoTime(SUMOTime_MIN),
195 myFollowerInfoTime(SUMOTime_MIN),
196 myLengthGeometryFactor(MAX2(POSITION_EPS, myShape.length()) / myLength), // factor should not be 0
197 myIsRampAccel(isRampAccel),
198 myRightSideOnEdge(0), // initialized in MSEdge::initialize
199 myRightmostSublane(0),
200 myNeedsCollisionCheck(false)
201 #ifdef HAVE_FOX
202 , mySimulationTask(*this, 0)
203 #endif
204
205 {
206 // initialized in MSEdge::initialize
207 initRestrictions();// may be reloaded again from initialized in MSEdge::closeBuilding
208 assert(myRNGs.size() > 0);
209 myRNGIndex = numericalID % myRNGs.size();
210 }
211
212
~MSLane()213 MSLane::~MSLane() {
214 for (MSLinkCont::iterator i = myLinks.begin(); i != myLinks.end(); ++i) {
215 delete *i;
216 }
217 }
218
219
220 void
initRestrictions()221 MSLane::initRestrictions() {
222 // simplify unit testing without MSNet instance
223 myRestrictions = MSGlobals::gUnitTests ? nullptr : MSNet::getInstance()->getRestrictions(myEdge->getEdgeType());
224
225 }
226
227
228 void
checkBufferType()229 MSLane::checkBufferType() {
230 if (MSGlobals::gNumSimThreads <= 1 || myIncomingLanes.size() <= 1) {
231 myVehBuffer.unsetCondition();
232 }
233 }
234
235
236 void
addLink(MSLink * link)237 MSLane::addLink(MSLink* link) {
238 myLinks.push_back(link);
239 }
240
241
242 void
addNeigh(const std::string & id)243 MSLane::addNeigh(const std::string& id) {
244 myNeighs.push_back(id);
245 // warn about lengths after loading the second lane of the pair
246 if (getOpposite() != nullptr && getLength() != getOpposite()->getLength()) {
247 WRITE_WARNING("Unequal lengths of neigh lane '" + getID() + "' and lane '" + id + "' (" + toString(getLength())
248 + ", " + toString(getOpposite()->getLength()) + ")");
249 }
250 }
251
252
253 // ------ interaction with MSMoveReminder ------
254 void
addMoveReminder(MSMoveReminder * rem)255 MSLane::addMoveReminder(MSMoveReminder* rem) {
256 myMoveReminders.push_back(rem);
257 for (VehCont::iterator veh = myVehicles.begin(); veh != myVehicles.end(); ++veh) {
258 (*veh)->addReminder(rem);
259 }
260 // XXX: Here, the partial occupators are ignored!? Refs. #3255
261 }
262
263
264 double
setPartialOccupation(MSVehicle * v)265 MSLane::setPartialOccupation(MSVehicle* v) {
266 myNeedsCollisionCheck = true; // always check
267 #ifdef DEBUG_CONTEXT
268 if (DEBUG_COND2(v)) {
269 std::cout << SIMTIME << " setPartialOccupation. lane=" << getID() << " veh=" << v->getID() << "\n";
270 }
271 #endif
272 // XXX update occupancy here?
273 myPartialVehicles.push_back(v);
274 return myLength;
275 }
276
277
278 void
resetPartialOccupation(MSVehicle * v)279 MSLane::resetPartialOccupation(MSVehicle* v) {
280 #ifdef DEBUG_CONTEXT
281 if (DEBUG_COND2(v)) {
282 std::cout << SIMTIME << " resetPartialOccupation. lane=" << getID() << " veh=" << v->getID() << "\n";
283 }
284 #endif
285 for (VehCont::iterator i = myPartialVehicles.begin(); i != myPartialVehicles.end(); ++i) {
286 if (v == *i) {
287 myPartialVehicles.erase(i);
288 // XXX update occupancy here?
289 //std::cout << " removed from myPartialVehicles\n";
290 return;
291 }
292 }
293 assert(false);
294 }
295
296
297 void
setManeuverReservation(MSVehicle * v)298 MSLane::setManeuverReservation(MSVehicle* v) {
299 #ifdef DEBUG_CONTEXT
300 if (DEBUG_COND2(v)) {
301 std::cout << SIMTIME << " setManeuverReservation. lane=" << getID() << " veh=" << v->getID() << "\n";
302 }
303 #endif
304 myManeuverReservations.push_back(v);
305 }
306
307
308 void
resetManeuverReservation(MSVehicle * v)309 MSLane::resetManeuverReservation(MSVehicle* v) {
310 #ifdef DEBUG_CONTEXT
311 if (DEBUG_COND2(v)) {
312 std::cout << SIMTIME << " resetManeuverReservation(): lane=" << getID() << " veh=" << v->getID() << "\n";
313 }
314 #endif
315 for (VehCont::iterator i = myManeuverReservations.begin(); i != myManeuverReservations.end(); ++i) {
316 if (v == *i) {
317 myManeuverReservations.erase(i);
318 return;
319 }
320 }
321 assert(false);
322 }
323
324
325 // ------ Vehicle emission ------
326 void
incorporateVehicle(MSVehicle * veh,double pos,double speed,double posLat,const MSLane::VehCont::iterator & at,MSMoveReminder::Notification notification)327 MSLane::incorporateVehicle(MSVehicle* veh, double pos, double speed, double posLat, const MSLane::VehCont::iterator& at, MSMoveReminder::Notification notification) {
328 myNeedsCollisionCheck = true;
329 assert(pos <= myLength);
330 bool wasInactive = myVehicles.size() == 0;
331 veh->enterLaneAtInsertion(this, pos, speed, posLat, notification);
332 if (at == myVehicles.end()) {
333 // vehicle will be the first on the lane
334 myVehicles.push_back(veh);
335 } else {
336 myVehicles.insert(at, veh);
337 }
338 myBruttoVehicleLengthSum += veh->getVehicleType().getLengthWithGap();
339 myNettoVehicleLengthSum += veh->getVehicleType().getLength();
340 myEdge->markDelayed();
341 if (wasInactive) {
342 MSNet::getInstance()->getEdgeControl().gotActive(this);
343 }
344 }
345
346
347 bool
lastInsertion(MSVehicle & veh,double mspeed,double posLat,bool patchSpeed)348 MSLane::lastInsertion(MSVehicle& veh, double mspeed, double posLat, bool patchSpeed) {
349 double pos = getLength() - POSITION_EPS;
350 MSVehicle* leader = getLastAnyVehicle();
351 // back position of leader relative to this lane
352 double leaderBack;
353 if (leader == nullptr) {
354 /// look for a leaders on consecutive lanes
355 veh.setTentativeLaneAndPosition(this, pos, posLat);
356 veh.updateBestLanes(false, this);
357 std::pair<MSVehicle* const, double> leaderInfo = getLeader(&veh, pos, veh.getBestLanesContinuation(), veh.getCarFollowModel().brakeGap(mspeed));
358 leader = leaderInfo.first;
359 leaderBack = pos + leaderInfo.second + veh.getVehicleType().getMinGap();
360 } else {
361 leaderBack = leader->getBackPositionOnLane(this);
362 //std::cout << " leaderPos=" << leader->getPositionOnLane(this) << " leaderBack=" << leader->getBackPositionOnLane(this) << " leaderLane=" << leader->getLane()->getID() << "\n";
363 }
364 if (leader == nullptr) {
365 // insert at the end of this lane
366 return isInsertionSuccess(&veh, mspeed, pos, posLat, patchSpeed, MSMoveReminder::NOTIFICATION_DEPARTED);
367 } else {
368 // try to insert behind the leader
369 const double frontGapNeeded = veh.getCarFollowModel().getSecureGap(mspeed, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel()) + veh.getVehicleType().getMinGap() + POSITION_EPS;
370 if (leaderBack >= frontGapNeeded) {
371 pos = MIN2(pos, leaderBack - frontGapNeeded);
372 bool result = isInsertionSuccess(&veh, mspeed, pos, posLat, patchSpeed, MSMoveReminder::NOTIFICATION_DEPARTED);
373 //if (!result) std::cout << " insertLast failed for " << veh.getID() << " pos=" << pos << " leaderBack=" << leaderBack << " frontGapNeeded=" << frontGapNeeded << "\n";
374 return result;
375 }
376 //std::cout << " insertLast failed for " << veh.getID() << " pos=" << pos << " leaderBack=" << leaderBack << " frontGapNeeded=" << frontGapNeeded << "\n";
377 }
378 return false;
379 }
380
381
382 bool
freeInsertion(MSVehicle & veh,double mspeed,double posLat,MSMoveReminder::Notification notification)383 MSLane::freeInsertion(MSVehicle& veh, double mspeed, double posLat,
384 MSMoveReminder::Notification notification) {
385 bool adaptableSpeed = true;
386 // try to insert teleporting vehicles fully on this lane
387 const double minPos = (notification == MSMoveReminder::NOTIFICATION_TELEPORT ?
388 MIN2(myLength, veh.getVehicleType().getLength()) : 0);
389 veh.setTentativeLaneAndPosition(this, minPos, 0);
390 if (myVehicles.size() == 0) {
391 // ensure sufficient gap to followers on predecessor lanes
392 const double backOffset = minPos - veh.getVehicleType().getLength();
393 const double missingRearGap = getMissingRearGap(&veh, backOffset, mspeed);
394 if (missingRearGap > 0) {
395 if (minPos + missingRearGap <= myLength) {
396 // @note. The rear gap is tailored to mspeed. If it changes due
397 // to a leader vehicle (on subsequent lanes) insertion will
398 // still fail. Under the right combination of acceleration and
399 // deceleration values there might be another insertion
400 // positions that would be successful be we do not look for it.
401 //std::cout << SIMTIME << " freeInsertion lane=" << getID() << " veh=" << veh.getID() << " unclear @(340)\n";
402 return isInsertionSuccess(&veh, mspeed, minPos + missingRearGap, posLat, adaptableSpeed, notification);
403 } else {
404 return false;
405 }
406 } else {
407 return isInsertionSuccess(&veh, mspeed, minPos, posLat, adaptableSpeed, notification);
408 }
409
410 } else {
411 // check whether the vehicle can be put behind the last one if there is such
412 MSVehicle* leader = getFirstFullVehicle(); // @todo reproduction of bogus old behavior. see #1961
413 const double leaderPos = leader->getBackPositionOnLane(this);
414 const double speed = adaptableSpeed ? leader->getSpeed() : mspeed;
415 const double frontGapNeeded = veh.getCarFollowModel().getSecureGap(speed, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel()) + veh.getVehicleType().getMinGap();
416 if (leaderPos >= frontGapNeeded) {
417 const double tspeed = MIN2(veh.getCarFollowModel().insertionFollowSpeed(&veh, mspeed, frontGapNeeded, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel()), mspeed);
418 // check whether we can insert our vehicle behind the last vehicle on the lane
419 if (isInsertionSuccess(&veh, tspeed, minPos, posLat, adaptableSpeed, notification)) {
420 //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";
421 return true;
422 }
423 }
424 }
425 // go through the lane, look for free positions (starting after the last vehicle)
426 MSLane::VehCont::iterator predIt = myVehicles.begin();
427 while (predIt != myVehicles.end()) {
428 // get leader (may be zero) and follower
429 // @todo compute secure position in regard to sublane-model
430 const MSVehicle* leader = predIt != myVehicles.end() - 1 ? *(predIt + 1) : nullptr;
431 if (leader == nullptr && myPartialVehicles.size() > 0) {
432 leader = myPartialVehicles.front();
433 }
434 const MSVehicle* follower = *predIt;
435
436 // patch speed if allowed
437 double speed = mspeed;
438 if (adaptableSpeed && leader != nullptr) {
439 speed = MIN2(leader->getSpeed(), mspeed);
440 }
441
442 // compute the space needed to not collide with leader
443 double frontMax = getLength();
444 if (leader != nullptr) {
445 double leaderRearPos = leader->getBackPositionOnLane(this);
446 double frontGapNeeded = veh.getCarFollowModel().getSecureGap(speed, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel()) + veh.getVehicleType().getMinGap();
447 frontMax = leaderRearPos - frontGapNeeded;
448 }
449 // compute the space needed to not let the follower collide
450 const double followPos = follower->getPositionOnLane() + follower->getVehicleType().getMinGap();
451 const double backGapNeeded = follower->getCarFollowModel().getSecureGap(follower->getSpeed(), veh.getSpeed(), veh.getCarFollowModel().getMaxDecel());
452 const double backMin = followPos + backGapNeeded + veh.getVehicleType().getLength();
453
454 // check whether there is enough room (given some extra space for rounding errors)
455 if (frontMax > minPos && backMin + POSITION_EPS < frontMax) {
456 // try to insert vehicle (should be always ok)
457 if (isInsertionSuccess(&veh, speed, backMin + POSITION_EPS, posLat, adaptableSpeed, notification)) {
458 //std::cout << SIMTIME << " freeInsertion lane=" << getID() << " veh=" << veh.getID() << " @(393)\n";
459 return true;
460 }
461 }
462 ++predIt;
463 }
464 // first check at lane's begin
465 //std::cout << SIMTIME << " freeInsertion lane=" << getID() << " veh=" << veh.getID() << " fail final\n";
466 return false;
467 }
468
469
470 double
getDepartSpeed(const MSVehicle & veh,bool & patchSpeed)471 MSLane::getDepartSpeed(const MSVehicle& veh, bool& patchSpeed) {
472 double speed = 0;
473 const SUMOVehicleParameter& pars = veh.getParameter();
474 switch (pars.departSpeedProcedure) {
475 case DEPART_SPEED_GIVEN:
476 speed = pars.departSpeed;
477 patchSpeed = false;
478 break;
479 case DEPART_SPEED_RANDOM:
480 speed = RandHelper::rand(getVehicleMaxSpeed(&veh));
481 patchSpeed = true; // @todo check
482 break;
483 case DEPART_SPEED_MAX:
484 speed = getVehicleMaxSpeed(&veh);
485 patchSpeed = true; // @todo check
486 break;
487 case DEPART_SPEED_DEFAULT:
488 default:
489 // speed = 0 was set before
490 patchSpeed = false; // @todo check
491 break;
492 }
493 return speed;
494 }
495
496
497 double
getDepartPosLat(const MSVehicle & veh)498 MSLane::getDepartPosLat(const MSVehicle& veh) {
499 const SUMOVehicleParameter& pars = veh.getParameter();
500 switch (pars.departPosLatProcedure) {
501 case DEPART_POSLAT_GIVEN:
502 return pars.departPosLat;
503 case DEPART_POSLAT_RIGHT:
504 return -getWidth() * 0.5 + veh.getVehicleType().getWidth() * 0.5;
505 case DEPART_POSLAT_LEFT:
506 return getWidth() * 0.5 - veh.getVehicleType().getWidth() * 0.5;
507 case DEPART_POSLAT_RANDOM:
508 return RandHelper::rand(getWidth() - veh.getVehicleType().getWidth()) - getWidth() * 0.5 + veh.getVehicleType().getWidth() * 0.5;
509 case DEPART_POSLAT_CENTER:
510 case DEPART_POSLAT_DEFAULT:
511 // @note:
512 // case DEPART_POSLAT_FREE
513 // case DEPART_POSLAT_RANDOM_FREE
514 // are not handled here because they involve multiple insertion attempts
515 default:
516 return 0;
517 }
518 }
519
520
521 bool
insertVehicle(MSVehicle & veh)522 MSLane::insertVehicle(MSVehicle& veh) {
523 double pos = 0;
524 bool patchSpeed = true; // whether the speed shall be adapted to infrastructure/traffic in front
525 const SUMOVehicleParameter& pars = veh.getParameter();
526 double speed = getDepartSpeed(veh, patchSpeed);
527 double posLat = getDepartPosLat(veh);
528
529 // determine the position
530 switch (pars.departPosProcedure) {
531 case DEPART_POS_GIVEN:
532 pos = pars.departPos;
533 if (pos < 0.) {
534 pos += myLength;
535 }
536 break;
537 case DEPART_POS_RANDOM:
538 pos = RandHelper::rand(getLength());
539 break;
540 case DEPART_POS_RANDOM_FREE: {
541 for (int i = 0; i < 10; i++) {
542 // we will try some random positions ...
543 pos = RandHelper::rand(getLength());
544 posLat = getDepartPosLat(veh); // could be random as well
545 if (isInsertionSuccess(&veh, speed, pos, posLat, patchSpeed, MSMoveReminder::NOTIFICATION_DEPARTED)) {
546 return true;
547 }
548 }
549 // ... and if that doesn't work, we put the vehicle to the free position
550 return freeInsertion(veh, speed, posLat);
551 }
552 break;
553 case DEPART_POS_FREE:
554 return freeInsertion(veh, speed, posLat);
555 case DEPART_POS_LAST:
556 return lastInsertion(veh, speed, posLat, patchSpeed);
557 case DEPART_POS_BASE:
558 case DEPART_POS_DEFAULT:
559 default:
560 pos = veh.basePos(myEdge);
561 break;
562 }
563 // determine the lateral position for special cases
564 if (MSGlobals::gLateralResolution > 0) {
565 switch (pars.departPosLatProcedure) {
566 case DEPART_POSLAT_RANDOM_FREE: {
567 for (int i = 0; i < 10; i++) {
568 // we will try some random positions ...
569 posLat = RandHelper::rand(getWidth()) - getWidth() * 0.5;
570 if (isInsertionSuccess(&veh, speed, pos, posLat, patchSpeed, MSMoveReminder::NOTIFICATION_DEPARTED)) {
571 return true;
572 }
573 }
574 FALLTHROUGH;
575 }
576 // no break! continue with DEPART_POSLAT_FREE
577 case DEPART_POSLAT_FREE: {
578 // systematically test all positions until a free lateral position is found
579 double posLatMin = -getWidth() * 0.5 + veh.getVehicleType().getWidth() * 0.5;
580 double posLatMax = getWidth() * 0.5 - veh.getVehicleType().getWidth() * 0.5;
581 for (double posLat = posLatMin; posLat < posLatMax; posLat += MSGlobals::gLateralResolution) {
582 if (isInsertionSuccess(&veh, speed, pos, posLat, patchSpeed, MSMoveReminder::NOTIFICATION_DEPARTED)) {
583 return true;
584 }
585 }
586 return false;
587 }
588 default:
589 break;
590 }
591 }
592 // try to insert
593 return isInsertionSuccess(&veh, speed, pos, posLat, patchSpeed, MSMoveReminder::NOTIFICATION_DEPARTED);
594 }
595
596
597 bool
checkFailure(const MSVehicle * aVehicle,double & speed,double & dist,const double nspeed,const bool patchSpeed,const std::string errorMsg) const598 MSLane::checkFailure(const MSVehicle* aVehicle, double& speed, double& dist, const double nspeed, const bool patchSpeed, const std::string errorMsg) const {
599 if (nspeed < speed) {
600 if (patchSpeed) {
601 speed = MIN2(nspeed, speed);
602 dist = aVehicle->getCarFollowModel().brakeGap(speed) + aVehicle->getVehicleType().getMinGap();
603 } else if (speed > 0) {
604 if (!MSGlobals::gCheckRoutes) {
605 // Check whether vehicle can stop at the given distance when applying emergency braking
606 double emergencyBrakeGap = 0.5 * speed * speed / aVehicle->getCarFollowModel().getEmergencyDecel();
607 if (emergencyBrakeGap <= dist) {
608 // Vehicle may stop in time with emergency deceleration
609 // stil, emit a warning
610 WRITE_WARNING("Vehicle '" + aVehicle->getID() + "' is inserted in emergency situation.");
611 return false;
612 }
613 }
614
615 if (errorMsg != "") {
616 WRITE_ERROR("Vehicle '" + aVehicle->getID() + "' will not be able to depart using the given velocity (" + errorMsg + ")!");
617 MSNet::getInstance()->getInsertionControl().descheduleDeparture(aVehicle);
618 }
619 return true;
620 }
621 }
622 return false;
623 }
624
625
626 bool
isInsertionSuccess(MSVehicle * aVehicle,double speed,double pos,double posLat,bool patchSpeed,MSMoveReminder::Notification notification)627 MSLane::isInsertionSuccess(MSVehicle* aVehicle,
628 double speed, double pos, double posLat, bool patchSpeed,
629 MSMoveReminder::Notification notification) {
630 if (pos < 0 || pos > myLength) {
631 // we may not start there
632 WRITE_WARNING("Invalid departPos " + toString(pos) + " given for vehicle '" +
633 aVehicle->getID() + "'. Inserting at lane end instead.");
634 pos = myLength;
635 }
636
637 #ifdef DEBUG_INSERTION
638 if (DEBUG_COND2(aVehicle)) {
639 std::cout << "\nIS_INSERTION_SUCCESS\n"
640 << SIMTIME << " lane=" << getID()
641 << " veh '" << aVehicle->getID() << "'\n";
642 }
643 #endif
644
645 aVehicle->setTentativeLaneAndPosition(this, pos, posLat);
646 aVehicle->updateBestLanes(false, this);
647 const MSCFModel& cfModel = aVehicle->getCarFollowModel();
648 const std::vector<MSLane*>& bestLaneConts = aVehicle->getBestLanesContinuation(this);
649 std::vector<MSLane*>::const_iterator ri = bestLaneConts.begin();
650 double seen = getLength() - pos; // == distance from insertion position until the end of the currentLane
651 double dist = cfModel.brakeGap(speed) + aVehicle->getVehicleType().getMinGap();
652 // do not insert if the bidirectional edge is occupied
653 if (myEdge->getBidiEdge() != nullptr && getBidiLane()->getVehicleNumberWithPartials() > 0) {
654 return false;
655 }
656 bool hadRailSignal = false;
657
658 // before looping through the continuation lanes, check if a stop is scheduled on this lane
659 // (the code is duplicated in the loop)
660 if (aVehicle->hasStops()) {
661 const MSVehicle::Stop& nextStop = aVehicle->getNextStop();
662 if (nextStop.lane == this) {
663 std::stringstream msg;
664 msg << "scheduled stop on lane '" << myID << "' too close";
665 const double distToStop = nextStop.pars.endPos - pos;
666 if (checkFailure(aVehicle, speed, dist, cfModel.stopSpeed(aVehicle, speed, distToStop),
667 patchSpeed, msg.str())) {
668 // we may not drive with the given velocity - we cannot stop at the stop
669 return false;
670 }
671 }
672 }
673
674 const MSRoute& r = aVehicle->getRoute();
675 MSRouteIterator ce = r.begin();
676 int nRouteSuccs = 1;
677 MSLane* currentLane = this;
678 MSLane* nextLane = this;
679 SUMOTime arrivalTime = MSNet::getInstance()->getCurrentTimeStep() + TIME2STEPS(seen / MAX2(speed, SUMO_const_haltingSpeed));
680 while (seen < dist && ri != bestLaneConts.end()) {
681 // get the next link used...
682 MSLinkCont::const_iterator link = succLinkSec(*aVehicle, nRouteSuccs, *currentLane, bestLaneConts);
683 if (currentLane->isLinkEnd(link)) {
684 if (¤tLane->getEdge() == r.getLastEdge()) {
685 // reached the end of the route
686 if (aVehicle->getParameter().arrivalSpeedProcedure == ARRIVAL_SPEED_GIVEN) {
687 const double remaining = seen + aVehicle->getArrivalPos() - currentLane->getLength();
688 const double nspeed = cfModel.freeSpeed(aVehicle, speed, remaining, aVehicle->getParameter().arrivalSpeed, true);
689 if (checkFailure(aVehicle, speed, dist, nspeed,
690 patchSpeed, "arrival speed too low")) {
691 // we may not drive with the given velocity - we cannot match the specified arrival speed
692 return false;
693 }
694 }
695 } else {
696 // lane does not continue
697 if (checkFailure(aVehicle, speed, dist, cfModel.insertionStopSpeed(aVehicle, speed, seen),
698 patchSpeed, "junction too close")) {
699 // we may not drive with the given velocity - we cannot stop at the junction
700 return false;
701 }
702 }
703 break;
704 }
705 hadRailSignal |= (*link)->getTLLogic() != nullptr;
706
707 if (!(*link)->opened(arrivalTime, speed, speed, aVehicle->getVehicleType().getLength(), aVehicle->getImpatience(), cfModel.getMaxDecel(), 0, posLat)
708 || !(*link)->havePriority()) {
709 // have to stop at junction
710 std::string errorMsg = "";
711 const LinkState state = (*link)->getState();
712 if (state == LINKSTATE_MINOR
713 || state == LINKSTATE_EQUAL
714 || state == LINKSTATE_STOP
715 || state == LINKSTATE_ALLWAY_STOP) {
716 // no sense in trying later
717 errorMsg = "unpriorised junction too close";
718 }
719 if (checkFailure(aVehicle, speed, dist, cfModel.insertionStopSpeed(aVehicle, speed, seen),
720 patchSpeed, errorMsg)) {
721 // we may not drive with the given velocity - we cannot stop at the junction in time
722 return false;
723 }
724 #ifdef DEBUG_INSERTION
725 if DEBUG_COND2(aVehicle) {
726 std::cout << "trying insertion before minor link: "
727 << "insertion speed = " << speed << " dist=" << dist
728 << "\n";
729 }
730 #endif
731 if (currentLane == this
732 && currentLane->getEdge().getBidiEdge() != nullptr
733 && currentLane->getEdge().getToJunction()->getType() == NODETYPE_RAIL_SIGNAL
734 && (*link)->getTLLogic() != nullptr
735 && getLength() < aVehicle->getVehicleType().getLength()) {
736 // allow guarding bidirectional tracks at the network border with railSignal
737 return false;
738 }
739 break;
740 }
741 // get the next used lane (including internal)
742 nextLane = (*link)->getViaLaneOrLane();
743 // check how next lane affects the journey
744 if (nextLane != nullptr) {
745
746 // do not insert if the bidirectional edge is occupied before a railSignal has been encountered
747 if (!hadRailSignal && nextLane->getEdge().getBidiEdge() != nullptr && nextLane->getBidiLane()->getVehicleNumberWithPartials() > 0) {
748 return false;
749 }
750
751 // check if there are stops on the next lane that should be regarded
752 // (this block is duplicated before the loop to deal with the insertion lane)
753 if (aVehicle->hasStops()) {
754 const MSVehicle::Stop& nextStop = aVehicle->getNextStop();
755 if (nextStop.lane == nextLane) {
756 std::stringstream msg;
757 msg << "scheduled stop on lane '" << nextStop.lane->getID() << "' too close";
758 const double distToStop = seen + nextStop.pars.endPos;
759 if (checkFailure(aVehicle, speed, dist, cfModel.insertionStopSpeed(aVehicle, speed, distToStop),
760 patchSpeed, msg.str())) {
761 // we may not drive with the given velocity - we cannot stop at the stop
762 return false;
763 }
764 }
765 }
766
767 // check leader on next lane
768 MSLeaderInfo leaders = nextLane->getLastVehicleInformation(aVehicle, 0, 0);
769 if (leaders.hasVehicles()) {
770 const double nspeed = nextLane->safeInsertionSpeed(aVehicle, seen, leaders, speed);
771 #ifdef DEBUG_INSERTION
772 if (DEBUG_COND2(aVehicle)) std::cout << SIMTIME
773 << " leader on lane '" << nextLane->getID() << "': " << leaders.toString() << " nspeed=" << nspeed << "\n";
774 #endif
775 if (nspeed == INVALID_SPEED || checkFailure(aVehicle, speed, dist, nspeed, patchSpeed, "")) {
776 // we may not drive with the given velocity - we crash into the leader
777 #ifdef DEBUG_INSERTION
778 if (DEBUG_COND2(aVehicle)) std::cout << SIMTIME
779 << " isInsertionSuccess lane=" << getID()
780 << " veh=" << aVehicle->getID()
781 << " pos=" << pos
782 << " posLat=" << posLat
783 << " patchSpeed=" << patchSpeed
784 << " speed=" << speed
785 << " nspeed=" << nspeed
786 << " nextLane=" << nextLane->getID()
787 << " lead=" << leaders.toString()
788 // << " gap=" << gap
789 << " failed (@641)!\n";
790 #endif
791 return false;
792 }
793 }
794 if (!nextLane->checkForPedestrians(aVehicle, speed, dist, -seen, patchSpeed)) {
795 return false;
796 }
797 // check next lane's maximum velocity
798 const double nspeed = cfModel.freeSpeed(aVehicle, speed, seen, nextLane->getVehicleMaxSpeed(aVehicle), true);
799 if (nspeed < speed) {
800 if (patchSpeed) {
801 speed = nspeed;
802 dist = cfModel.brakeGap(speed) + aVehicle->getVehicleType().getMinGap();
803 } else {
804 // we may not drive with the given velocity - we would be too fast on the next lane
805 WRITE_ERROR("Vehicle '" + aVehicle->getID() + "' will not be able to depart using the given velocity (slow lane ahead)!");
806 MSNet::getInstance()->getInsertionControl().descheduleDeparture(aVehicle);
807 return false;
808 }
809 }
810 // check traffic on next junction
811 // we cannot use (*link)->opened because a vehicle without priority
812 // may already be comitted to blocking the link and unable to stop
813 const SUMOTime leaveTime = (*link)->getLeaveTime(arrivalTime, speed, speed, aVehicle->getVehicleType().getLength());
814 if ((*link)->hasApproachingFoe(arrivalTime, leaveTime, speed, cfModel.getMaxDecel())) {
815 if (checkFailure(aVehicle, speed, dist, cfModel.insertionStopSpeed(aVehicle, speed, seen), patchSpeed, "")) {
816 // we may not drive with the given velocity - we crash at the junction
817 return false;
818 }
819 }
820 arrivalTime += TIME2STEPS(nextLane->getLength() / MAX2(speed, NUMERICAL_EPS));
821 seen += nextLane->getLength();
822 currentLane = nextLane;
823 if ((*link)->getViaLane() == nullptr) {
824 nRouteSuccs++;
825 ++ce;
826 ++ri;
827 }
828 }
829 }
830
831 // get the pointer to the vehicle next in front of the given position
832 MSLeaderInfo leaders = getLastVehicleInformation(aVehicle, 0, pos);
833 //if (aVehicle->getID() == "disabled") std::cout << " leaders=" << leaders.toString() << "\n";
834 const double nspeed = safeInsertionSpeed(aVehicle, -pos, leaders, speed);
835 if (nspeed == INVALID_SPEED || checkFailure(aVehicle, speed, dist, nspeed, patchSpeed, "")) {
836 // we may not drive with the given velocity - we crash into the leader
837 #ifdef DEBUG_INSERTION
838 if (DEBUG_COND2(aVehicle)) std::cout << SIMTIME
839 << " isInsertionSuccess lane=" << getID()
840 << " veh=" << aVehicle->getID()
841 << " pos=" << pos
842 << " posLat=" << posLat
843 << " patchSpeed=" << patchSpeed
844 << " speed=" << speed
845 << " nspeed=" << nspeed
846 << " nextLane=" << nextLane->getID()
847 << " leaders=" << leaders.toString()
848 << " failed (@700)!\n";
849 #endif
850 return false;
851 }
852 #ifdef DEBUG_INSERTION
853 if (DEBUG_COND2(aVehicle)) std::cout << SIMTIME
854 << " speed = " << speed
855 << " nspeed = " << nspeed
856 << std::endl;
857 #endif
858
859 MSLeaderDistanceInfo followers = getFollowersOnConsecutive(aVehicle, aVehicle->getBackPositionOnLane(), false);
860 for (int i = 0; i < followers.numSublanes(); ++i) {
861 const MSVehicle* follower = followers[i].first;
862 if (follower != nullptr) {
863 const double backGapNeeded = follower->getCarFollowModel().getSecureGap(follower->getSpeed(), speed, cfModel.getMaxDecel());
864 if (followers[i].second < backGapNeeded) {
865 // too close to the follower on this lane
866 #ifdef DEBUG_INSERTION
867 if (DEBUG_COND2(aVehicle)) std::cout << SIMTIME
868 << " isInsertionSuccess lane=" << getID()
869 << " veh=" << aVehicle->getID()
870 << " pos=" << pos
871 << " posLat=" << posLat
872 << " patchSpeed=" << patchSpeed
873 << " speed=" << speed
874 << " nspeed=" << nspeed
875 << " follower=" << follower->getID()
876 << " backGapNeeded=" << backGapNeeded
877 << " gap=" << followers[i].second
878 << " failure (@719)!\n";
879 #endif
880 return false;
881 }
882 }
883 }
884
885 if (!checkForPedestrians(aVehicle, speed, dist, pos, patchSpeed)) {
886 return false;
887 }
888
889 MSLane* shadowLane = aVehicle->getLaneChangeModel().getShadowLane(this);
890 #ifdef DEBUG_INSERTION
891 if (DEBUG_COND2(aVehicle)) {
892 std::cout << " shadowLane=" << Named::getIDSecure(shadowLane) << "\n";
893 }
894 #endif
895 if (shadowLane != nullptr) {
896 MSLeaderDistanceInfo followers = shadowLane->getFollowersOnConsecutive(aVehicle, aVehicle->getBackPositionOnLane(), false);
897 for (int i = 0; i < followers.numSublanes(); ++i) {
898 const MSVehicle* follower = followers[i].first;
899 if (follower != nullptr) {
900 const double backGapNeeded = follower->getCarFollowModel().getSecureGap(follower->getSpeed(), speed, cfModel.getMaxDecel());
901 if (followers[i].second < backGapNeeded) {
902 // too close to the follower on this lane
903 #ifdef DEBUG_INSERTION
904 if (DEBUG_COND2(aVehicle)) std::cout << SIMTIME
905 << " isInsertionSuccess shadowlane=" << shadowLane->getID()
906 << " veh=" << aVehicle->getID()
907 << " pos=" << pos
908 << " posLat=" << posLat
909 << " patchSpeed=" << patchSpeed
910 << " speed=" << speed
911 << " nspeed=" << nspeed
912 << " follower=" << follower->getID()
913 << " backGapNeeded=" << backGapNeeded
914 << " gap=" << followers[i].second
915 << " failure (@812)!\n";
916 #endif
917 return false;
918 }
919 }
920 }
921 const MSLeaderInfo& ahead = shadowLane->getLastVehicleInformation(nullptr, 0, aVehicle->getPositionOnLane(), false);
922 for (int i = 0; i < ahead.numSublanes(); ++i) {
923 const MSVehicle* veh = ahead[i];
924 if (veh != nullptr) {
925 const double gap = veh->getBackPositionOnLane(shadowLane) - aVehicle->getPositionOnLane() - aVehicle->getVehicleType().getMinGap();
926 const double gapNeeded = aVehicle->getCarFollowModel().getSecureGap(speed, veh->getSpeed(), veh->getCarFollowModel().getMaxDecel());
927 if (gap < gapNeeded) {
928 // too close to the shadow leader
929 #ifdef DEBUG_INSERTION
930 if (DEBUG_COND2(aVehicle)) std::cout << SIMTIME
931 << " isInsertionSuccess shadowlane=" << shadowLane->getID()
932 << " veh=" << aVehicle->getID()
933 << " pos=" << pos
934 << " posLat=" << posLat
935 << " patchSpeed=" << patchSpeed
936 << " speed=" << speed
937 << " nspeed=" << nspeed
938 << " leader=" << veh->getID()
939 << " gapNeeded=" << gapNeeded
940 << " gap=" << gap
941 << " failure (@842)!\n";
942 #endif
943 return false;
944 }
945 }
946 }
947 }
948 if (followers.numFreeSublanes() > 0) {
949 // check approaching vehicles to prevent rear-end collisions
950 const double backOffset = pos - aVehicle->getVehicleType().getLength();
951 const double missingRearGap = getMissingRearGap(aVehicle, backOffset, speed);
952 if (missingRearGap > 0) {
953 // too close to a follower
954 #ifdef DEBUG_INSERTION
955 if (DEBUG_COND2(aVehicle)) std::cout << SIMTIME
956 << " isInsertionSuccess lane=" << getID()
957 << " veh=" << aVehicle->getID()
958 << " pos=" << pos
959 << " posLat=" << posLat
960 << " patchSpeed=" << patchSpeed
961 << " speed=" << speed
962 << " nspeed=" << nspeed
963 << " missingRearGap=" << missingRearGap
964 << " failure (@728)!\n";
965 #endif
966 return false;
967 }
968 }
969 // may got negative while adaptation
970 if (speed < 0) {
971 #ifdef DEBUG_INSERTION
972 if (DEBUG_COND2(aVehicle)) std::cout << SIMTIME
973 << " isInsertionSuccess lane=" << getID()
974 << " veh=" << aVehicle->getID()
975 << " pos=" << pos
976 << " posLat=" << posLat
977 << " patchSpeed=" << patchSpeed
978 << " speed=" << speed
979 << " nspeed=" << nspeed
980 << " failed (@733)!\n";
981 #endif
982 return false;
983 }
984 // enter
985 incorporateVehicle(aVehicle, pos, speed, posLat, find_if(myVehicles.begin(), myVehicles.end(), bind2nd(VehPosition(), pos)), notification);
986 #ifdef DEBUG_INSERTION
987 if (DEBUG_COND2(aVehicle)) std::cout << SIMTIME
988 << " isInsertionSuccess lane=" << getID()
989 << " veh=" << aVehicle->getID()
990 << " pos=" << pos
991 << " posLat=" << posLat
992 << " patchSpeed=" << patchSpeed
993 << " speed=" << speed
994 << " nspeed=" << nspeed
995 << "\n myVehicles=" << toString(myVehicles)
996 << " myPartial=" << toString(myPartialVehicles)
997 << " myManeuverReservations=" << toString(myManeuverReservations)
998 << "\n leaders=" << leaders.toString()
999 << "\n success!\n";
1000 #endif
1001 return true;
1002 }
1003
1004
1005 void
forceVehicleInsertion(MSVehicle * veh,double pos,MSMoveReminder::Notification notification,double posLat)1006 MSLane::forceVehicleInsertion(MSVehicle* veh, double pos, MSMoveReminder::Notification notification, double posLat) {
1007 veh->updateBestLanes(true, this);
1008 bool dummy;
1009 const double speed = veh->hasDeparted() ? veh->getSpeed() : getDepartSpeed(*veh, dummy);
1010 incorporateVehicle(veh, pos, speed, posLat, find_if(myVehicles.begin(), myVehicles.end(), bind2nd(VehPosition(), pos)), notification);
1011 }
1012
1013
1014 double
safeInsertionSpeed(const MSVehicle * veh,double seen,const MSLeaderInfo & leaders,double speed)1015 MSLane::safeInsertionSpeed(const MSVehicle* veh, double seen, const MSLeaderInfo& leaders, double speed) {
1016 double nspeed = speed;
1017 #ifdef DEBUG_INSERTION
1018 if (DEBUG_COND2(veh)) {
1019 std::cout << SIMTIME << " safeInsertionSpeed veh=" << veh->getID() << " speed=" << speed << "\n";
1020 }
1021 #endif
1022 for (int i = 0; i < leaders.numSublanes(); ++i) {
1023 const MSVehicle* leader = leaders[i];
1024 if (leader != nullptr) {
1025 const double gap = leader->getBackPositionOnLane(this) + seen - veh->getVehicleType().getMinGap();
1026 if (gap < 0) {
1027 return INVALID_SPEED;
1028 }
1029 nspeed = MIN2(nspeed,
1030 veh->getCarFollowModel().insertionFollowSpeed(veh, speed, gap, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel()));
1031 #ifdef DEBUG_INSERTION
1032 if (DEBUG_COND2(veh)) {
1033 std::cout << " leader=" << leader->getID() << " nspeed=" << nspeed << "\n";
1034 }
1035 #endif
1036 }
1037 }
1038 return nspeed;
1039 }
1040
1041
1042 // ------ Handling vehicles lapping into lanes ------
1043 const MSLeaderInfo
getLastVehicleInformation(const MSVehicle * ego,double latOffset,double minPos,bool allowCached) const1044 MSLane::getLastVehicleInformation(const MSVehicle* ego, double latOffset, double minPos, bool allowCached) const {
1045 #ifdef HAVE_FOX
1046 FXConditionalLock(myLeaderInfoMutex, MSGlobals::gNumSimThreads > 1);
1047 #endif
1048 if (myLeaderInfoTime < MSNet::getInstance()->getCurrentTimeStep() || ego != nullptr || minPos > 0 || !allowCached) {
1049 MSLeaderInfo leaderTmp(this, ego, latOffset);
1050 AnyVehicleIterator last = anyVehiclesBegin();
1051 int freeSublanes = 1; // number of sublanes for which no leader was found
1052 //if (ego->getID() == "disabled" && SIMTIME == 58) {
1053 // std::cout << "DEBUG\n";
1054 //}
1055 const MSVehicle* veh = *last;
1056 while (freeSublanes > 0 && veh != nullptr) {
1057 #ifdef DEBUG_PLAN_MOVE
1058 if (DEBUG_COND2(ego)) {
1059 gDebugFlag1 = true;
1060 std::cout << " getLastVehicleInformation lane=" << getID() << " minPos=" << minPos << " veh=" << veh->getID() << " pos=" << veh->getPositionOnLane(this) << "\n";
1061 }
1062 #endif
1063 if (veh != ego && veh->getPositionOnLane(this) >= minPos) {
1064 const double latOffset = veh->getLatOffset(this);
1065 freeSublanes = leaderTmp.addLeader(veh, true, latOffset);
1066 #ifdef DEBUG_PLAN_MOVE
1067 if (DEBUG_COND2(ego)) {
1068 std::cout << " latOffset=" << latOffset << " newLeaders=" << leaderTmp.toString() << "\n";
1069 }
1070 #endif
1071 }
1072 veh = *(++last);
1073 }
1074 if (ego == nullptr && minPos == 0) {
1075 // update cached value
1076 myLeaderInfo = leaderTmp;
1077 myLeaderInfoTime = MSNet::getInstance()->getCurrentTimeStep();
1078 }
1079 #ifdef DEBUG_PLAN_MOVE
1080 //if (DEBUG_COND2(ego)) std::cout << SIMTIME
1081 // << " getLastVehicleInformation lane=" << getID()
1082 // << " ego=" << Named::getIDSecure(ego)
1083 // << "\n"
1084 // << " vehicles=" << toString(myVehicles)
1085 // << " partials=" << toString(myPartialVehicles)
1086 // << "\n"
1087 // << " result=" << leaderTmp.toString()
1088 // << " cached=" << myLeaderInfo.toString()
1089 // << " myLeaderInfoTime=" << myLeaderInfoTime
1090 // << "\n";
1091 gDebugFlag1 = false;
1092 #endif
1093 return leaderTmp;
1094 }
1095 return myLeaderInfo;
1096 }
1097
1098
1099 const MSLeaderInfo
getFirstVehicleInformation(const MSVehicle * ego,double latOffset,bool onlyFrontOnLane,double maxPos,bool allowCached) const1100 MSLane::getFirstVehicleInformation(const MSVehicle* ego, double latOffset, bool onlyFrontOnLane, double maxPos, bool allowCached) const {
1101 #ifdef HAVE_FOX
1102 FXConditionalLock(myFollowerInfoMutex, MSGlobals::gNumSimThreads > 1);
1103 #endif
1104 if (myFollowerInfoTime < MSNet::getInstance()->getCurrentTimeStep() || ego != nullptr || maxPos < myLength || !allowCached || onlyFrontOnLane) {
1105 // XXX separate cache for onlyFrontOnLane = true
1106 MSLeaderInfo followerTmp(this, ego, latOffset);
1107 AnyVehicleIterator first = anyVehiclesUpstreamBegin();
1108 int freeSublanes = 1; // number of sublanes for which no leader was found
1109 const MSVehicle* veh = *first;
1110 while (freeSublanes > 0 && veh != nullptr) {
1111 #ifdef DEBUG_PLAN_MOVE
1112 if (DEBUG_COND2(ego)) {
1113 std::cout << " veh=" << veh->getID() << " pos=" << veh->getPositionOnLane(this) << " maxPos=" << maxPos << "\n";
1114 }
1115 #endif
1116 if (veh != ego && veh->getPositionOnLane(this) <= maxPos
1117 && (!onlyFrontOnLane || veh->isFrontOnLane(this))) {
1118 //const double latOffset = veh->getLane()->getRightSideOnEdge() - getRightSideOnEdge();
1119 const double latOffset = veh->getLatOffset(this);
1120 #ifdef DEBUG_PLAN_MOVE
1121 if (DEBUG_COND2(ego)) {
1122 std::cout << " veh=" << veh->getID() << " latOffset=" << latOffset << "\n";
1123 }
1124 #endif
1125 freeSublanes = followerTmp.addLeader(veh, true, latOffset);
1126 }
1127 veh = *(++first);
1128 }
1129 if (ego == nullptr && maxPos == std::numeric_limits<double>::max()) {
1130 // update cached value
1131 myFollowerInfo = followerTmp;
1132 myFollowerInfoTime = MSNet::getInstance()->getCurrentTimeStep();
1133 }
1134 #ifdef DEBUG_PLAN_MOVE
1135 //if (DEBUG_COND2(ego)) std::cout << SIMTIME
1136 // << " getFirstVehicleInformation lane=" << getID()
1137 // << " ego=" << Named::getIDSecure(ego)
1138 // << "\n"
1139 // << " vehicles=" << toString(myVehicles)
1140 // << " partials=" << toString(myPartialVehicles)
1141 // << "\n"
1142 // << " result=" << followerTmp.toString()
1143 // //<< " cached=" << myFollowerInfo.toString()
1144 // << " myLeaderInfoTime=" << myLeaderInfoTime
1145 // << "\n";
1146 #endif
1147 return followerTmp;
1148 }
1149 return myFollowerInfo;
1150 }
1151
1152
1153 // ------ ------
1154 void
planMovements(SUMOTime t)1155 MSLane::planMovements(SUMOTime t) {
1156 assert(myVehicles.size() != 0);
1157 double cumulatedVehLength = 0.;
1158 MSLeaderInfo leaders(this);
1159
1160 // iterate over myVehicles, myPartialVehicles, and myManeuverReservations merge-sort style
1161 VehCont::reverse_iterator veh = myVehicles.rbegin();
1162 VehCont::reverse_iterator vehPart = myPartialVehicles.rbegin();
1163 VehCont::reverse_iterator vehRes = myManeuverReservations.rbegin();
1164 #ifdef DEBUG_PLAN_MOVE
1165 if (DEBUG_COND) std::cout
1166 << "\n"
1167 << SIMTIME
1168 << " planMovements() lane=" << getID()
1169 << "\n vehicles=" << toString(myVehicles)
1170 << "\n partials=" << toString(myPartialVehicles)
1171 << "\n reservations=" << toString(myManeuverReservations)
1172 << "\n";
1173 #endif
1174 assert(MSGlobals::gLateralResolution || myManeuverReservations.size() == 0);
1175 for (; veh != myVehicles.rend(); ++veh) {
1176 #ifdef DEBUG_PLAN_MOVE
1177 if (DEBUG_COND2((*veh))) {
1178 std::cout << " plan move for: " << (*veh)->getID();
1179 }
1180 #endif
1181 updateLeaderInfo(*veh, vehPart, vehRes, leaders);
1182 #ifdef DEBUG_PLAN_MOVE
1183 if (DEBUG_COND2((*veh))) {
1184 std::cout << " leaders=" << leaders.toString() << "\n";
1185 }
1186 #endif
1187 (*veh)->planMove(t, leaders, cumulatedVehLength);
1188 cumulatedVehLength += (*veh)->getVehicleType().getLengthWithGap();
1189 leaders.addLeader(*veh, false, 0);
1190 }
1191 }
1192
1193
1194 void
setJunctionApproaches(const SUMOTime t) const1195 MSLane::setJunctionApproaches(const SUMOTime t) const {
1196 for (MSVehicle* const veh : myVehicles) {
1197 veh->setApproachingForAllLinks(t);
1198 }
1199 }
1200
1201
1202 void
updateLeaderInfo(const MSVehicle * veh,VehCont::reverse_iterator & vehPart,VehCont::reverse_iterator & vehRes,MSLeaderInfo & ahead) const1203 MSLane::updateLeaderInfo(const MSVehicle* veh, VehCont::reverse_iterator& vehPart, VehCont::reverse_iterator& vehRes, MSLeaderInfo& ahead) const {
1204 bool morePartialVehsAhead = vehPart != myPartialVehicles.rend();
1205 bool moreReservationsAhead = vehRes != myManeuverReservations.rend();
1206 bool nextToConsiderIsPartial;
1207
1208 // Determine relevant leaders for veh
1209 while (moreReservationsAhead || morePartialVehsAhead) {
1210 if ((!moreReservationsAhead || (*vehRes)->getPositionOnLane(this) <= veh->getPositionOnLane())
1211 && (!morePartialVehsAhead || (*vehPart)->getPositionOnLane(this) <= veh->getPositionOnLane())) {
1212 // All relevant downstream vehicles have been collected.
1213 break;
1214 }
1215
1216 // Check whether next farthest relevant vehicle downstream is a partial vehicle or a maneuver reservation
1217 if (moreReservationsAhead && !morePartialVehsAhead) {
1218 nextToConsiderIsPartial = false;
1219 } else if (morePartialVehsAhead && !moreReservationsAhead) {
1220 nextToConsiderIsPartial = true;
1221 } else {
1222 assert(morePartialVehsAhead && moreReservationsAhead);
1223 // Add farthest downstream vehicle first
1224 nextToConsiderIsPartial = (*vehPart)->getPositionOnLane(this) > (*vehRes)->getPositionOnLane(this);
1225 }
1226 // Add appropriate leader information
1227 if (nextToConsiderIsPartial) {
1228 const double latOffset = (*vehPart)->getLatOffset(this);
1229 #ifdef DEBUG_PLAN_MOVE
1230 if (DEBUG_COND) {
1231 std::cout << " partial ahead: " << (*vehPart)->getID() << " latOffset=" << latOffset << "\n";
1232 }
1233 #endif
1234 ahead.addLeader(*vehPart, false, latOffset);
1235 ++vehPart;
1236 morePartialVehsAhead = vehPart != myPartialVehicles.rend();
1237 } else {
1238 const double latOffset = (*vehRes)->getLatOffset(this);
1239 #ifdef DEBUG_PLAN_MOVE
1240 if (DEBUG_COND) {
1241 std::cout << " reservation ahead: " << (*vehRes)->getID() << " latOffset=" << latOffset << "\n";
1242 }
1243 #endif
1244 ahead.addLeader(*vehRes, false, latOffset);
1245 ++vehRes;
1246 moreReservationsAhead = vehRes != myManeuverReservations.rend();
1247 }
1248 }
1249 }
1250
1251
1252 void
detectCollisions(SUMOTime timestep,const std::string & stage)1253 MSLane::detectCollisions(SUMOTime timestep, const std::string& stage) {
1254 myNeedsCollisionCheck = false;
1255 #ifdef DEBUG_COLLISIONS
1256 if (DEBUG_COND) {
1257 std::vector<const MSVehicle*> all;
1258 for (AnyVehicleIterator last = anyVehiclesBegin(); last != anyVehiclesEnd(); ++last) {
1259 all.push_back(*last);
1260 }
1261 std::cout << SIMTIME << " detectCollisions stage=" << stage << " lane=" << getID() << ":\n"
1262 << " vehs=" << toString(myVehicles) << "\n"
1263 << " part=" << toString(myPartialVehicles) << "\n"
1264 << " all=" << toString(all) << "\n"
1265 << "\n";
1266 }
1267 #endif
1268
1269 if (myCollisionAction == COLLISION_ACTION_NONE) {
1270 return;
1271 }
1272
1273 std::set<const MSVehicle*, ComparatorNumericalIdLess> toRemove;
1274 std::set<const MSVehicle*> toTeleport;
1275 if (mustCheckJunctionCollisions()) {
1276 myNeedsCollisionCheck = true; // always check
1277 #ifdef DEBUG_JUNCTION_COLLISIONS
1278 if (DEBUG_COND) {
1279 std::cout << SIMTIME << " detect junction Collisions stage=" << stage << " lane=" << getID() << ":\n"
1280 << " vehs=" << toString(myVehicles) << "\n"
1281 << " part=" << toString(myPartialVehicles) << "\n"
1282 << "\n";
1283 }
1284 #endif
1285 assert(myLinks.size() == 1);
1286 //std::cout << SIMTIME << " checkJunctionCollisions " << getID() << "\n";
1287 const std::vector<const MSLane*>& foeLanes = myLinks.front()->getFoeLanes();
1288 for (AnyVehicleIterator veh = anyVehiclesBegin(); veh != anyVehiclesEnd(); ++veh) {
1289 MSVehicle* collider = const_cast<MSVehicle*>(*veh);
1290 //std::cout << " collider " << collider->getID() << "\n";
1291 PositionVector colliderBoundary = collider->getBoundingBox();
1292 for (std::vector<const MSLane*>::const_iterator it = foeLanes.begin(); it != foeLanes.end(); ++it) {
1293 const MSLane* foeLane = *it;
1294 //std::cout << " foeLane " << foeLane->getID() << "\n";
1295 MSLane::AnyVehicleIterator end = foeLane->anyVehiclesEnd();
1296 for (MSLane::AnyVehicleIterator it_veh = foeLane->anyVehiclesBegin(); it_veh != end; ++it_veh) {
1297 MSVehicle* victim = (MSVehicle*)*it_veh;
1298 if (victim == collider) {
1299 // may happen if the vehicles lane and shadow lane are siblings
1300 continue;
1301 }
1302 //std::cout << " victim " << victim->getID() << "\n";
1303 #ifdef DEBUG_JUNCTION_COLLISIONS
1304 if (DEBUG_COND && DEBUG_COND2(collider)) {
1305 std::cout << SIMTIME << " foe=" << victim->getID() << " bound=" << colliderBoundary << " foeBound=" << victim->getBoundingBox() << "\n";
1306 }
1307 #endif
1308 if (colliderBoundary.overlapsWith(victim->getBoundingBox())) {
1309 // make a detailed check
1310 if (collider->getBoundingPoly().overlapsWith(victim->getBoundingPoly())) {
1311 handleCollisionBetween(timestep, stage, collider, victim, -1, 0, toRemove, toTeleport);
1312 }
1313 }
1314 }
1315 detectPedestrianJunctionCollision(collider, colliderBoundary, foeLane, timestep, stage);
1316 }
1317 if (myLinks.front()->getWalkingAreaFoe() != nullptr) {
1318 detectPedestrianJunctionCollision(collider, colliderBoundary, myLinks.front()->getWalkingAreaFoe(), timestep, stage);
1319 }
1320 if (myLinks.front()->getWalkingAreaFoeExit() != nullptr) {
1321 detectPedestrianJunctionCollision(collider, colliderBoundary, myLinks.front()->getWalkingAreaFoeExit(), timestep, stage);
1322 }
1323 }
1324 }
1325
1326 if (myVehicles.size() == 0) {
1327 return;
1328 }
1329 if (!MSAbstractLaneChangeModel::haveLateralDynamics()) {
1330 // no sublanes
1331 VehCont::reverse_iterator lastVeh = myVehicles.rend() - 1;
1332 for (VehCont::reverse_iterator pred = myVehicles.rbegin(); pred != lastVeh; ++pred) {
1333 VehCont::reverse_iterator veh = pred + 1;
1334 detectCollisionBetween(timestep, stage, *veh, *pred, toRemove, toTeleport);
1335 }
1336 if (myPartialVehicles.size() > 0) {
1337 detectCollisionBetween(timestep, stage, *lastVeh, myPartialVehicles.front(), toRemove, toTeleport);
1338 }
1339 if (myEdge->getBidiEdge() != nullptr) {
1340 // bidirectional railway
1341 MSLane* bidiLane = getBidiLane();
1342 if (bidiLane->getVehicleNumberWithPartials() > 0) {
1343 for (AnyVehicleIterator veh = anyVehiclesBegin(); veh != anyVehiclesEnd(); ++veh) {
1344 double high = (*veh)->getPositionOnLane(this);
1345 double low = (*veh)->getBackPositionOnLane(this);
1346 for (AnyVehicleIterator veh2 = bidiLane->anyVehiclesBegin(); veh2 != bidiLane->anyVehiclesEnd(); ++veh2) {
1347 // self-collisions might ligitemately occur when a long train loops back on itself
1348 //if (*veh == *veh2) {
1349 // // no self-collision (when performing a turn-around)
1350 // continue;
1351 //}
1352 double low2 = myLength - (*veh2)->getPositionOnLane(bidiLane);
1353 double high2 = myLength - (*veh2)->getBackPositionOnLane(bidiLane);
1354 if (!(high < low2 || high2 < low)) {
1355 // the faster vehicle is at fault
1356 MSVehicle* collider = const_cast<MSVehicle*>(*veh);
1357 MSVehicle* victim = const_cast<MSVehicle*>(*veh2);
1358 if (collider->getSpeed() < victim->getSpeed()) {
1359 std::swap(victim, collider);
1360 }
1361 handleCollisionBetween(timestep, stage, collider, victim, -1, 0, toRemove, toTeleport);
1362 }
1363 }
1364 }
1365 }
1366 }
1367 } else {
1368 // in the sublane-case it is insufficient to check the vehicles ordered
1369 // by their front position as there might be more than 2 vehicles next to each
1370 // other on the same lane
1371 // instead, a moving-window approach is used where all vehicles that
1372 // overlap in the longitudinal direction receive pairwise checks
1373 // XXX for efficiency, all lanes of an edge should be checked together
1374 // (lanechanger-style)
1375
1376 // XXX quick hack: check each in myVehicles against all others
1377 for (AnyVehicleIterator veh = anyVehiclesBegin(); veh != anyVehiclesEnd(); ++veh) {
1378 MSVehicle* follow = (MSVehicle*)*veh;
1379 for (AnyVehicleIterator veh2 = anyVehiclesBegin(); veh2 != anyVehiclesEnd(); ++veh2) {
1380 MSVehicle* lead = (MSVehicle*)*veh2;
1381 if (lead == follow) {
1382 continue;
1383 }
1384 if (lead->getPositionOnLane(this) < follow->getPositionOnLane(this)) {
1385 continue;
1386 }
1387 if (detectCollisionBetween(timestep, stage, follow, lead, toRemove, toTeleport)) {
1388 // XXX what about collisions with multiple leaders at once?
1389 break;
1390 }
1391 }
1392 if (follow->getLaneChangeModel().getShadowLane() != nullptr && follow->getLane() == this) {
1393 // check whether follow collides on the shadow lane
1394 const MSLane* shadowLane = follow->getLaneChangeModel().getShadowLane();
1395 const MSLeaderInfo& ahead = shadowLane->getLastVehicleInformation(follow,
1396 getRightSideOnEdge() - shadowLane->getRightSideOnEdge(),
1397 follow->getPositionOnLane());
1398 for (int i = 0; i < ahead.numSublanes(); ++i) {
1399 MSVehicle* lead = const_cast<MSVehicle*>(ahead[i]);
1400 if (lead != nullptr && lead != follow && shadowLane->detectCollisionBetween(timestep, stage, follow, lead, toRemove, toTeleport)) {
1401 break;
1402 }
1403 }
1404 }
1405 }
1406 }
1407
1408
1409 if (myEdge->getPersons().size() > 0 && MSPModel::getModel()->hasPedestrians(this)) {
1410 #ifdef DEBUG_PEDESTRIAN_COLLISIONS
1411 if (DEBUG_COND) {
1412 std::cout << SIMTIME << " detect pedestrian collisions stage=" << stage << " lane=" << getID() << "\n";
1413 }
1414 #endif
1415 AnyVehicleIterator v_end = anyVehiclesEnd();
1416 for (AnyVehicleIterator it_v = anyVehiclesBegin(); it_v != v_end; ++it_v) {
1417 const MSVehicle* v = *it_v;
1418 const double back = v->getBackPositionOnLane(this);
1419 const double length = v->getVehicleType().getLength();
1420 const double right = v->getRightSideOnEdge(this) - getRightSideOnEdge();
1421 PersonDist leader = MSPModel::getModel()->nextBlocking(this, back, right, right + v->getVehicleType().getWidth());
1422 #ifdef DEBUG_PEDESTRIAN_COLLISIONS
1423 if (DEBUG_COND && DEBUG_COND2(v)) {
1424 std::cout << SIMTIME << " back=" << back << " right=" << right << " person=" << Named::getIDSecure(leader.first) << " dist=" << leader.second << "\n";
1425 }
1426 #endif
1427 if (leader.first != 0 && leader.second < length) {
1428 WRITE_WARNING(
1429 "Vehicle '" + v->getID()
1430 + "' collision with person '" + leader.first->getID()
1431 + "', lane='" + getID()
1432 + "', gap=" + toString(leader.second - length)
1433 + ", time=" + time2string(MSNet::getInstance()->getCurrentTimeStep())
1434 + " stage=" + stage + ".");
1435 MSNet::getInstance()->getVehicleControl().registerCollision();
1436 }
1437 }
1438 }
1439
1440
1441 for (std::set<const MSVehicle*, ComparatorNumericalIdLess>::iterator it = toRemove.begin(); it != toRemove.end(); ++it) {
1442 MSVehicle* veh = const_cast<MSVehicle*>(*it);
1443 MSLane* vehLane = veh->getLane();
1444 vehLane->removeVehicle(veh, MSMoveReminder::NOTIFICATION_TELEPORT, false);
1445 if (toTeleport.count(veh) > 0) {
1446 MSVehicleTransfer::getInstance()->add(timestep, veh);
1447 } else {
1448 veh->onRemovalFromNet(MSMoveReminder::NOTIFICATION_VAPORIZED);
1449 MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(veh);
1450 }
1451 }
1452 }
1453
1454
1455 void
detectPedestrianJunctionCollision(const MSVehicle * collider,const PositionVector & colliderBoundary,const MSLane * foeLane,SUMOTime timestep,const std::string & stage)1456 MSLane::detectPedestrianJunctionCollision(const MSVehicle* collider, const PositionVector& colliderBoundary, const MSLane* foeLane,
1457 SUMOTime timestep, const std::string& stage) {
1458 if (foeLane->getEdge().getPersons().size() > 0 && MSPModel::getModel()->hasPedestrians(foeLane)) {
1459 #ifdef DEBUG_PEDESTRIAN_COLLISIONS
1460 if (DEBUG_COND) {
1461 std::cout << SIMTIME << " detect pedestrian junction collisions stage=" << stage << " lane=" << getID() << " foeLane=" << foeLane->getID() << "\n";
1462 }
1463 #endif
1464 const std::vector<MSTransportable*>& persons = foeLane->getEdge().getSortedPersons(timestep);
1465 for (std::vector<MSTransportable*>::const_iterator it_p = persons.begin(); it_p != persons.end(); ++it_p) {
1466 #ifdef DEBUG_PEDESTRIAN_COLLISIONS
1467 if (DEBUG_COND) {
1468 std::cout << " collider=" << collider->getID()
1469 << " ped=" << (*it_p)->getID()
1470 << " colliderBoundary=" << colliderBoundary
1471 << " pedBoundary=" << (*it_p)->getBoundingBox()
1472 << "\n";
1473 }
1474 #endif
1475 if (colliderBoundary.overlapsWith((*it_p)->getBoundingBox())) {
1476 WRITE_WARNING(
1477 "Vehicle '" + collider->getID()
1478 + "' collision with person '" + (*it_p)->getID()
1479 + "', lane='" + getID()
1480 + ", time=" + time2string(MSNet::getInstance()->getCurrentTimeStep())
1481 + " stage=" + stage + ".");
1482 MSNet::getInstance()->getVehicleControl().registerCollision();
1483 }
1484 }
1485 }
1486 }
1487
1488
1489 bool
detectCollisionBetween(SUMOTime timestep,const std::string & stage,MSVehicle * collider,MSVehicle * victim,std::set<const MSVehicle *,ComparatorNumericalIdLess> & toRemove,std::set<const MSVehicle * > & toTeleport) const1490 MSLane::detectCollisionBetween(SUMOTime timestep, const std::string& stage, MSVehicle* collider, MSVehicle* victim,
1491 std::set<const MSVehicle*, ComparatorNumericalIdLess>& toRemove,
1492 std::set<const MSVehicle*>& toTeleport) const {
1493 if (myCollisionAction == COLLISION_ACTION_TELEPORT && ((victim->hasInfluencer() && victim->getInfluencer().isRemoteAffected(timestep)) ||
1494 (collider->hasInfluencer() && collider->getInfluencer().isRemoteAffected(timestep)))) {
1495 return false;
1496 }
1497
1498 // No self-collisions! (This is assumed to be ensured at caller side)
1499 if (collider == victim) {
1500 return false;
1501 }
1502
1503 const bool colliderOpposite = collider->getLaneChangeModel().isOpposite();
1504 const bool bothOpposite = victim->getLaneChangeModel().isOpposite() && colliderOpposite;
1505 if (bothOpposite) {
1506 std::swap(victim, collider);
1507 }
1508 const double colliderPos = colliderOpposite ? collider->getBackPositionOnLane(this) : collider->getPositionOnLane(this);
1509 double minGapFactor = myCollisionMinGapFactor >= 0 ? myCollisionMinGapFactor : collider->getCarFollowModel().getCollisionMinGapFactor();
1510 double gap = victim->getBackPositionOnLane(this) - colliderPos - minGapFactor * collider->getVehicleType().getMinGap();
1511 if (bothOpposite) {
1512 gap = -gap - 2 * myCollisionMinGapFactor * collider->getVehicleType().getMinGap();
1513 }
1514 #ifdef DEBUG_COLLISIONS
1515 if (DEBUG_COND && (DEBUG_COND2(collider) || DEBUG_COND2(victim))) {
1516 std::cout << SIMTIME
1517 << " thisLane=" << getID()
1518 << " collider=" << collider->getID()
1519 << " victim=" << victim->getID()
1520 << " colliderLane=" << collider->getLane()->getID()
1521 << " victimLane=" << victim->getLane()->getID()
1522 << " colliderPos=" << colliderPos
1523 << " victimBackPos=" << victim->getBackPositionOnLane(this)
1524 << " colliderLat=" << collider->getCenterOnEdge(this)
1525 << " victimLat=" << victim->getCenterOnEdge(this)
1526 << " minGap=" << collider->getVehicleType().getMinGap()
1527 << " minGapFactor=" << minGapFactor
1528 << " gap=" << gap
1529 << "\n";
1530 }
1531 #endif
1532 if (gap < -NUMERICAL_EPS) {
1533 double latGap = 0;
1534 if (MSAbstractLaneChangeModel::haveLateralDynamics()) {
1535 latGap = (fabs(victim->getCenterOnEdge(this) - collider->getCenterOnEdge(this))
1536 - 0.5 * fabs(victim->getVehicleType().getWidth() + collider->getVehicleType().getWidth()));
1537 if (latGap + NUMERICAL_EPS > 0) {
1538 return false;
1539 }
1540 }
1541 if (MSGlobals::gLaneChangeDuration > DELTA_T
1542 && collider->getLaneChangeModel().isChangingLanes()
1543 && victim->getLaneChangeModel().isChangingLanes()
1544 && victim->getLane() != this) {
1545 // synchroneous lane change maneuver
1546 return false;
1547 }
1548 #ifdef DEBUG_COLLISIONS
1549 if (DEBUG_COND && (DEBUG_COND2(collider) || DEBUG_COND2(victim))) {
1550 std::cout << SIMTIME << " detectedCollision gap=" << gap << " latGap=" << latGap << "\n";
1551 }
1552 #endif
1553 handleCollisionBetween(timestep, stage, collider, victim, gap, latGap, toRemove, toTeleport);
1554 return true;
1555 }
1556 return false;
1557 }
1558
1559
1560 void
handleCollisionBetween(SUMOTime timestep,const std::string & stage,MSVehicle * collider,MSVehicle * victim,double gap,double latGap,std::set<const MSVehicle *,ComparatorNumericalIdLess> & toRemove,std::set<const MSVehicle * > & toTeleport) const1561 MSLane::handleCollisionBetween(SUMOTime timestep, const std::string& stage, MSVehicle* collider, MSVehicle* victim,
1562 double gap, double latGap, std::set<const MSVehicle*, ComparatorNumericalIdLess>& toRemove,
1563 std::set<const MSVehicle*>& toTeleport) const {
1564 std::string collisionType = ((collider->getLaneChangeModel().isOpposite() != victim->getLaneChangeModel().isOpposite()
1565 || (&collider->getLane()->getEdge() == victim->getLane()->getEdge().getBidiEdge()))
1566 ? "frontal collision" : "collision");
1567 // in frontal collisions the opposite vehicle is the collider
1568 if (victim->getLaneChangeModel().isOpposite() && !collider->getLaneChangeModel().isOpposite()) {
1569 std::swap(collider, victim);
1570 }
1571 std::string prefix = "Vehicle '" + collider->getID() + "'; " + collisionType + " with vehicle '" + victim->getID() ;
1572 const MSCFModel_CC* model = dynamic_cast<const MSCFModel_CC*>(&collider->getCarFollowModel());
1573 if (model) {
1574 model->setCrashed(collider, true);
1575 }
1576 model = dynamic_cast<const MSCFModel_CC*>(&victim->getCarFollowModel());
1577 if (model) {
1578 model->setCrashed(victim, true, true);
1579 }
1580 if (myCollisionStopTime > 0) {
1581 if (collider->collisionStopTime() >= 0 && victim->collisionStopTime() >= 0) {
1582 return;
1583 }
1584 std::string dummyError;
1585 SUMOVehicleParameter::Stop stop;
1586 stop.duration = myCollisionStopTime;
1587 stop.busstop = "";
1588 stop.containerstop = "";
1589 stop.chargingStation = "";
1590 stop.parkingarea = "";
1591 stop.until = 0;
1592 stop.triggered = false;
1593 stop.containerTriggered = false;
1594 stop.parking = false;
1595 stop.index = 0;
1596 const double collisionAngle = RAD2DEG(fabs(GeomHelper::angleDiff(victim->getAngle(), collider->getAngle())));
1597 // determine new speeds from collision angle (@todo account for vehicle mass)
1598 double victimSpeed = victim->getSpeed();
1599 double colliderSpeed = collider->getSpeed();
1600 // double victimOrigSpeed = victim->getSpeed();
1601 // double colliderOrigSpeed = collider->getSpeed();
1602 if (collisionAngle < 45) {
1603 // rear-end collisions
1604 colliderSpeed = MIN2(colliderSpeed, victimSpeed);
1605 } else if (collisionAngle < 135) {
1606 // side collision
1607 colliderSpeed /= 2;
1608 victimSpeed /= 2;
1609 } else {
1610 // frontal collision
1611 colliderSpeed = 0;
1612 victimSpeed = 0;
1613 }
1614 const double victimStopPos = MIN2(victim->getLane()->getLength(),
1615 victim->getPositionOnLane() + victim->getCarFollowModel().brakeGap(victimSpeed, victim->getCarFollowModel().getEmergencyDecel(), 0));
1616 if (victim->collisionStopTime() < 0) {
1617 stop.lane = victim->getLane()->getID();
1618 // @todo: push victim forward?
1619 stop.startPos = victimStopPos;
1620 stop.endPos = stop.startPos;
1621 stop.duration = myCollisionStopTime;
1622 victim->addStop(stop, dummyError, 0, true);
1623 }
1624 if (collider->collisionStopTime() < 0) {
1625 stop.lane = collider->getLane()->getID();
1626 stop.startPos = MIN2(collider->getPositionOnLane() + collider->getCarFollowModel().brakeGap(colliderSpeed, collider->getCarFollowModel().getEmergencyDecel(), 0),
1627 MAX2(0.0, victimStopPos - 0.75 * victim->getVehicleType().getLength()));
1628 stop.endPos = stop.startPos;
1629 collider->addStop(stop, dummyError, 0, true);
1630 }
1631 //std::cout << " collisionAngle=" << collisionAngle
1632 // << "\n vPos=" << victim->getPositionOnLane() << " vStop=" << victimStopPos << " vSpeed=" << victimOrigSpeed << " vSpeed2=" << victimSpeed << " vSpeed3=" << victim->getSpeed()
1633 // << "\n cPos=" << collider->getPositionOnLane() << " cStop=" << stop.startPos << " cSpeed=" << colliderOrigSpeed << " cSpeed2=" << colliderSpeed << " cSpeed3=" << collider->getSpeed()
1634 // << "\n";
1635 } else {
1636 switch (myCollisionAction) {
1637 case COLLISION_ACTION_WARN:
1638 break;
1639 case COLLISION_ACTION_TELEPORT:
1640 prefix = "Teleporting vehicle '" + collider->getID() + "'; " + collisionType + " with vehicle '" + victim->getID() ;
1641 toRemove.insert(collider);
1642 toTeleport.insert(collider);
1643 break;
1644 case COLLISION_ACTION_REMOVE: {
1645 prefix = "Removing " + collisionType + " participants: vehicle '" + collider->getID() + "', vehicle '" + victim->getID();
1646 bool removeCollider = true;
1647 bool removeVictim = true;
1648 removeVictim = !(victim->hasInfluencer() && victim->getInfluencer().isRemoteAffected(timestep));
1649 removeCollider = !(collider->hasInfluencer() && collider->getInfluencer().isRemoteAffected(timestep));
1650 if (removeVictim) {
1651 toRemove.insert(victim);
1652 }
1653 if (removeCollider) {
1654 toRemove.insert(collider);
1655 }
1656 if (!removeVictim) {
1657 if (!removeCollider) {
1658 prefix = "Keeping remote-controlled " + collisionType + " participants: vehicle '" + collider->getID() + "', vehicle '" + victim->getID();
1659 } else {
1660 prefix = "Removing " + collisionType + " participant: vehicle '" + collider->getID() + "', keeping remote-controlled vehicle '" + victim->getID();
1661 }
1662 } else if (!removeCollider) {
1663 prefix = "Keeping remote-controlled " + collisionType + " participant: vehicle '" + collider->getID() + "', removing vehicle '" + victim->getID();
1664 }
1665 break;
1666 }
1667 default:
1668 break;
1669 }
1670 }
1671 WRITE_WARNING(prefix
1672 + "', lane='" + getID()
1673 + "', gap=" + toString(gap)
1674 + (MSAbstractLaneChangeModel::haveLateralDynamics() ? "', latGap=" + toString(latGap) : "")
1675 + ", time=" + time2string(MSNet::getInstance()->getCurrentTimeStep())
1676 + " stage=" + stage + ".");
1677 #ifdef DEBUG_COLLISIONS
1678 if (DEBUG_COND2(collider)) {
1679 toRemove.erase(collider);
1680 toTeleport.erase(collider);
1681 }
1682 if (DEBUG_COND2(victim)) {
1683 toRemove.erase(victim);
1684 toTeleport.erase(victim);
1685 }
1686 #endif
1687 MSNet::getInstance()->informVehicleStateListener(victim, MSNet::VEHICLE_STATE_COLLISION);
1688 MSNet::getInstance()->informVehicleStateListener(collider, MSNet::VEHICLE_STATE_COLLISION);
1689 MSNet::getInstance()->getVehicleControl().registerCollision();
1690 }
1691
1692
1693 void
executeMovements(const SUMOTime t)1694 MSLane::executeMovements(const SUMOTime t) {
1695 myNeedsCollisionCheck = true;
1696 // iterate over vehicles in reverse so that move reminders will be called in the correct order
1697 for (VehCont::reverse_iterator i = myVehicles.rbegin(); i != myVehicles.rend();) {
1698 MSVehicle* veh = *i;
1699 // length is needed later when the vehicle may not exist anymore
1700 const double length = veh->getVehicleType().getLengthWithGap();
1701 const double nettoLength = veh->getVehicleType().getLength();
1702 const bool moved = veh->executeMove();
1703 MSLane* const target = veh->getLane();
1704 if (veh->hasArrived()) {
1705 // vehicle has reached its arrival position
1706 #ifdef DEBUG_EXEC_MOVE
1707 if DEBUG_COND2(veh) {
1708 std::cout << SIMTIME << " veh " << veh->getID() << " has arrived." << std::endl;
1709 }
1710 #endif
1711 veh->onRemovalFromNet(MSMoveReminder::NOTIFICATION_ARRIVED);
1712 MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(veh);
1713 } else if (target != nullptr && moved) {
1714 if (target->getEdge().isVaporizing()) {
1715 // vehicle has reached a vaporizing edge
1716 veh->onRemovalFromNet(MSMoveReminder::NOTIFICATION_VAPORIZED);
1717 MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(veh);
1718 } else {
1719 // vehicle has entered a new lane (leaveLane and workOnMoveReminders were already called in MSVehicle::executeMove)
1720 target->myVehBuffer.push_back(veh);
1721 MSNet::getInstance()->getEdgeControl().needsVehicleIntegration(target);
1722 }
1723 } else if (veh->isParking()) {
1724 // vehicle started to park
1725 MSVehicleTransfer::getInstance()->add(t, veh);
1726 myParkingVehicles.insert(veh);
1727 } else if (veh->getPositionOnLane() > getLength()) {
1728 // for any reasons the vehicle is beyond its lane...
1729 // this should never happen because it is handled in MSVehicle::executeMove
1730 assert(false);
1731 WRITE_WARNING("Teleporting vehicle '" + veh->getID() + "'; beyond end of lane, target lane='" + getID() + "', time=" +
1732 time2string(t) + ".");
1733 MSNet::getInstance()->getVehicleControl().registerCollision();
1734 MSVehicleTransfer::getInstance()->add(t, veh);
1735 } else if (veh->collisionStopTime() == 0) {
1736 veh->resumeFromStopping();
1737 if (getCollisionAction() == COLLISION_ACTION_REMOVE) {
1738 WRITE_WARNING("Removing vehicle '" + veh->getID() + "' after earlier collision, lane='" + veh->getLane()->getID() + ", time=" +
1739 time2string(t) + ".");
1740 veh->onRemovalFromNet(MSMoveReminder::NOTIFICATION_VAPORIZED);
1741 MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(veh);
1742 } else if (getCollisionAction() == COLLISION_ACTION_TELEPORT) {
1743 WRITE_WARNING("Teleporting vehicle '" + veh->getID() + "' after earlier collision, lane='" + veh->getLane()->getID() + ", time=" +
1744 time2string(t) + ".");
1745 MSVehicleTransfer::getInstance()->add(t, veh);
1746 } else {
1747 ++i;
1748 continue;
1749 }
1750 } else {
1751 ++i;
1752 continue;
1753 }
1754 myBruttoVehicleLengthSumToRemove += length;
1755 myNettoVehicleLengthSumToRemove += nettoLength;
1756 ++i;
1757 i = VehCont::reverse_iterator(myVehicles.erase(i.base()));
1758 }
1759 if (myVehicles.size() > 0) {
1760 if (MSGlobals::gTimeToGridlock > 0 || MSGlobals::gTimeToGridlockHighways > 0) {
1761 MSVehicle* veh = myVehicles.back(); // the vehice at the front of the queue
1762 if (!veh->isStopped() && veh->getLane() == this) {
1763 const bool wrongLane = !veh->getLane()->appropriate(veh);
1764 const bool r1 = MSGlobals::gTimeToGridlock > 0 && veh->getWaitingTime() > MSGlobals::gTimeToGridlock;
1765 const bool r2 = MSGlobals::gTimeToGridlockHighways > 0 && veh->getWaitingTime() > MSGlobals::gTimeToGridlockHighways && veh->getLane()->getSpeedLimit() > 69. / 3.6 && wrongLane;
1766 if (r1 || r2) {
1767 const MSLinkCont::const_iterator link = succLinkSec(*veh, 1, *this, veh->getBestLanesContinuation());
1768 const bool minorLink = !wrongLane && (link != myLinks.end()) && !((*link)->havePriority());
1769 const std::string reason = (wrongLane ? " (wrong lane)" : (minorLink ? " (yield)" : " (jam)"));
1770 MSVehicle* veh = *(myVehicles.end() - 1);
1771 myBruttoVehicleLengthSumToRemove += veh->getVehicleType().getLengthWithGap();
1772 myNettoVehicleLengthSumToRemove += veh->getVehicleType().getLength();
1773 myVehicles.erase(myVehicles.end() - 1);
1774 WRITE_WARNING("Teleporting vehicle '" + veh->getID() + "'; waited too long"
1775 + reason
1776 + (r2 ? " (highway)" : "")
1777 + ", lane='" + getID() + "', time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
1778 if (wrongLane) {
1779 MSNet::getInstance()->getVehicleControl().registerTeleportWrongLane();
1780 } else if (minorLink) {
1781 MSNet::getInstance()->getVehicleControl().registerTeleportYield();
1782 } else {
1783 MSNet::getInstance()->getVehicleControl().registerTeleportJam();
1784 }
1785 MSVehicleTransfer::getInstance()->add(t, veh);
1786 }
1787 } // else look for a (waiting) vehicle that isn't stopped?
1788 }
1789 }
1790 if (MSGlobals::gLateralResolution > 0) {
1791 // trigger sorting of vehicles as their order may have changed
1792 MSNet::getInstance()->getEdgeControl().needsVehicleIntegration(this);
1793 }
1794 }
1795
1796
1797 void
updateLengthSum()1798 MSLane::updateLengthSum() {
1799 myBruttoVehicleLengthSum -= myBruttoVehicleLengthSumToRemove;
1800 myNettoVehicleLengthSum -= myNettoVehicleLengthSumToRemove;
1801 myBruttoVehicleLengthSumToRemove = 0;
1802 myNettoVehicleLengthSumToRemove = 0;
1803 if (myVehicles.empty()) {
1804 // avoid numerical instability
1805 myBruttoVehicleLengthSum = 0;
1806 myNettoVehicleLengthSum = 0;
1807 }
1808 }
1809
1810
1811 void
changeLanes(const SUMOTime t)1812 MSLane::changeLanes(const SUMOTime t) {
1813 myEdge->changeLanes(t);
1814 }
1815
1816
1817 const MSEdge*
getNextNormal() const1818 MSLane::getNextNormal() const {
1819 const MSEdge* e = myEdge;
1820 while (e->isInternal()) {
1821 e = e->getSuccessors()[0];
1822 }
1823 return e;
1824 }
1825
1826
1827 const MSLane*
getFirstInternalInConnection(double & offset) const1828 MSLane::getFirstInternalInConnection(double& offset) const {
1829 if (!this->isInternal()) {
1830 return nullptr;
1831 }
1832 offset = 0.;
1833 const MSLane* firstInternal = this;
1834 MSLane* pred = getCanonicalPredecessorLane();
1835 while (pred != nullptr && pred->isInternal()) {
1836 firstInternal = pred;
1837 offset += pred->getLength();
1838 pred = firstInternal->getCanonicalPredecessorLane();
1839 }
1840 return firstInternal;
1841 }
1842
1843
1844 // ------ Static (sic!) container methods ------
1845 bool
dictionary(const std::string & id,MSLane * ptr)1846 MSLane::dictionary(const std::string& id, MSLane* ptr) {
1847 DictType::iterator it = myDict.find(id);
1848 if (it == myDict.end()) {
1849 // id not in myDict.
1850 myDict.insert(DictType::value_type(id, ptr));
1851 return true;
1852 }
1853 return false;
1854 }
1855
1856
1857 MSLane*
dictionary(const std::string & id)1858 MSLane::dictionary(const std::string& id) {
1859 DictType::iterator it = myDict.find(id);
1860 if (it == myDict.end()) {
1861 // id not in myDict.
1862 return nullptr;
1863 }
1864 return it->second;
1865 }
1866
1867
1868 void
clear()1869 MSLane::clear() {
1870 for (DictType::iterator i = myDict.begin(); i != myDict.end(); ++i) {
1871 delete (*i).second;
1872 }
1873 myDict.clear();
1874 }
1875
1876
1877 void
insertIDs(std::vector<std::string> & into)1878 MSLane::insertIDs(std::vector<std::string>& into) {
1879 for (DictType::iterator i = myDict.begin(); i != myDict.end(); ++i) {
1880 into.push_back((*i).first);
1881 }
1882 }
1883
1884
1885 template<class RTREE> void
fill(RTREE & into)1886 MSLane::fill(RTREE& into) {
1887 for (DictType::iterator i = myDict.begin(); i != myDict.end(); ++i) {
1888 MSLane* l = (*i).second;
1889 Boundary b = l->getShape().getBoxBoundary();
1890 b.grow(3.);
1891 const float cmin[2] = {(float) b.xmin(), (float) b.ymin()};
1892 const float cmax[2] = {(float) b.xmax(), (float) b.ymax()};
1893 into.Insert(cmin, cmax, l);
1894 }
1895 }
1896
1897 template void MSLane::fill<NamedRTree>(NamedRTree& into);
1898 template void MSLane::fill<LANE_RTREE_QUAL>(LANE_RTREE_QUAL& into);
1899
1900 // ------ ------
1901 bool
appropriate(const MSVehicle * veh)1902 MSLane::appropriate(const MSVehicle* veh) {
1903 if (myEdge->isInternal()) {
1904 return true;
1905 }
1906 if (veh->succEdge(1) == nullptr) {
1907 assert((int)veh->getBestLanes().size() > veh->getLaneIndex());
1908 if (veh->getBestLanes()[veh->getLaneIndex()].bestLaneOffset == 0) {
1909 return true;
1910 } else {
1911 return false;
1912 }
1913 }
1914 MSLinkCont::const_iterator link = succLinkSec(*veh, 1, *this, veh->getBestLanesContinuation());
1915 return (link != myLinks.end());
1916 }
1917
1918
1919 void
integrateNewVehicles()1920 MSLane::integrateNewVehicles() {
1921 myNeedsCollisionCheck = true;
1922 std::vector<MSVehicle*>& buffered = myVehBuffer.getContainer();
1923 sort(buffered.begin(), buffered.end(), vehicle_position_sorter(this));
1924 for (MSVehicle* const veh : buffered) {
1925 assert(veh->getLane() == this);
1926 myVehicles.insert(myVehicles.begin(), veh);
1927 myBruttoVehicleLengthSum += veh->getVehicleType().getLengthWithGap();
1928 myNettoVehicleLengthSum += veh->getVehicleType().getLength();
1929 //if (true) std::cout << SIMTIME << " integrateNewVehicle lane=" << getID() << " veh=" << veh->getID() << " (on lane " << veh->getLane()->getID() << ") into lane=" << getID() << " myBrutto=" << myBruttoVehicleLengthSum << "\n";
1930 myEdge->markDelayed();
1931 }
1932 buffered.clear();
1933 myVehBuffer.unlock();
1934 //std::cout << SIMTIME << " integrateNewVehicle lane=" << getID() << " myVehicles1=" << toString(myVehicles);
1935 if (MSGlobals::gLateralResolution > 0 || myNeighs.size() > 0) {
1936 sort(myVehicles.begin(), myVehicles.end(), vehicle_natural_position_sorter(this));
1937 }
1938 sortPartialVehicles();
1939 #ifdef DEBUG_VEHICLE_CONTAINER
1940 if (DEBUG_COND) std::cout << SIMTIME << " integrateNewVehicle lane=" << getID()
1941 << " vehicles=" << toString(myVehicles) << " partials=" << toString(myPartialVehicles) << "\n";
1942 #endif
1943 }
1944
1945
1946 void
sortPartialVehicles()1947 MSLane::sortPartialVehicles() {
1948 if (myPartialVehicles.size() > 1) {
1949 sort(myPartialVehicles.begin(), myPartialVehicles.end(), vehicle_natural_position_sorter(this));
1950 }
1951 }
1952
1953
1954 void
sortManeuverReservations()1955 MSLane::sortManeuverReservations() {
1956 if (myManeuverReservations.size() > 1) {
1957 #ifdef DEBUG_CONTEXT
1958 if (DEBUG_COND) {
1959 std::cout << "sortManeuverReservations on lane " << getID()
1960 << "\nBefore sort: " << toString(myManeuverReservations) << std::endl;
1961 }
1962 #endif
1963 sort(myManeuverReservations.begin(), myManeuverReservations.end(), vehicle_natural_position_sorter(this));
1964 #ifdef DEBUG_CONTEXT
1965 if (DEBUG_COND) {
1966 std::cout << "After sort: " << toString(myManeuverReservations) << std::endl;
1967 }
1968 #endif
1969 }
1970 }
1971
1972
1973 bool
isLinkEnd(MSLinkCont::const_iterator & i) const1974 MSLane::isLinkEnd(MSLinkCont::const_iterator& i) const {
1975 return i == myLinks.end();
1976 }
1977
1978
1979 bool
isLinkEnd(MSLinkCont::iterator & i)1980 MSLane::isLinkEnd(MSLinkCont::iterator& i) {
1981 return i == myLinks.end();
1982 }
1983
1984 bool
isEmpty() const1985 MSLane::isEmpty() const {
1986 return myVehicles.empty() && myPartialVehicles.empty();
1987 }
1988
1989 bool
isInternal() const1990 MSLane::isInternal() const {
1991 return myEdge->isInternal();
1992 }
1993
1994 MSVehicle*
getLastFullVehicle() const1995 MSLane::getLastFullVehicle() const {
1996 if (myVehicles.size() == 0) {
1997 return nullptr;
1998 }
1999 return myVehicles.front();
2000 }
2001
2002
2003 MSVehicle*
getFirstFullVehicle() const2004 MSLane::getFirstFullVehicle() const {
2005 if (myVehicles.size() == 0) {
2006 return nullptr;
2007 }
2008 return myVehicles.back();
2009 }
2010
2011
2012 MSVehicle*
getLastAnyVehicle() const2013 MSLane::getLastAnyVehicle() const {
2014 // all vehicles in myVehicles should have positions smaller or equal to
2015 // those in myPartialVehicles
2016 if (myVehicles.size() > 0) {
2017 return myVehicles.front();
2018 }
2019 if (myPartialVehicles.size() > 0) {
2020 return myPartialVehicles.front();
2021 }
2022 return nullptr;
2023 }
2024
2025
2026 MSVehicle*
getFirstAnyVehicle() const2027 MSLane::getFirstAnyVehicle() const {
2028 MSVehicle* result = nullptr;
2029 if (myVehicles.size() > 0) {
2030 result = myVehicles.back();
2031 }
2032 if (myPartialVehicles.size() > 0
2033 && (result == nullptr || result->getPositionOnLane(this) < myPartialVehicles.back()->getPositionOnLane(this))) {
2034 result = myPartialVehicles.back();
2035 }
2036 return result;
2037 }
2038
2039
2040 MSLinkCont::const_iterator
succLinkSec(const SUMOVehicle & veh,int nRouteSuccs,const MSLane & succLinkSource,const std::vector<MSLane * > & conts)2041 MSLane::succLinkSec(const SUMOVehicle& veh, int nRouteSuccs,
2042 const MSLane& succLinkSource, const std::vector<MSLane*>& conts) {
2043 const MSEdge* nRouteEdge = veh.succEdge(nRouteSuccs);
2044 // check whether the vehicle tried to look beyond its route
2045 if (nRouteEdge == nullptr) {
2046 // return end (no succeeding link) if so
2047 return succLinkSource.myLinks.end();
2048 }
2049 // if we are on an internal lane there should only be one link and it must be allowed
2050 if (succLinkSource.isInternal()) {
2051 assert(succLinkSource.myLinks.size() == 1);
2052 // could have been disallowed dynamically with a rerouter or via TraCI
2053 // assert(succLinkSource.myLinks[0]->getLane()->allowsVehicleClass(veh.getVehicleType().getVehicleClass()));
2054 return succLinkSource.myLinks.begin();
2055 }
2056 // a link may be used if
2057 // 1) there is a destination lane ((*link)->getLane()!=0)
2058 // 2) the destination lane belongs to the next edge in route ((*link)->getLane()->myEdge == nRouteEdge)
2059 // 3) the destination lane allows the vehicle's class ((*link)->getLane()->allowsVehicleClass(veh.getVehicleClass()))
2060
2061 // there should be a link which leads to the next desired lane our route in "conts" (built in "getBestLanes")
2062 // "conts" stores the best continuations of our current lane
2063 // we should never return an arbitrary link since this may cause collisions
2064
2065 MSLinkCont::const_iterator link;
2066 if (nRouteSuccs < (int)conts.size()) {
2067 // we go through the links in our list and return the matching one
2068 for (link = succLinkSource.myLinks.begin(); link != succLinkSource.myLinks.end(); ++link) {
2069 if ((*link)->getLane() != nullptr && (*link)->getLane()->myEdge == nRouteEdge && (*link)->getLane()->allowsVehicleClass(veh.getVehicleType().getVehicleClass())) {
2070 // we should use the link if it connects us to the best lane
2071 if ((*link)->getLane() == conts[nRouteSuccs]) {
2072 return link;
2073 }
2074 }
2075 }
2076 } else {
2077 // the source lane is a dead end (no continuations exist)
2078 return succLinkSource.myLinks.end();
2079 }
2080 // the only case where this should happen is for a disconnected route (deliberately ignored)
2081 #ifdef DEBUG_NO_CONNECTION
2082 // the "'" around the ids are missing intentionally in the message below because it slows messaging down, resulting in test timeouts
2083 WRITE_WARNING("Could not find connection between lane " + succLinkSource.getID() + " and lane " + conts[nRouteSuccs]->getID() +
2084 " for vehicle " + veh.getID() + ", time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
2085 #endif
2086 return succLinkSource.myLinks.end();
2087 }
2088
2089 const MSLinkCont&
getLinkCont() const2090 MSLane::getLinkCont() const {
2091 return myLinks;
2092 }
2093
2094
2095 /// returns the link to the given lane or 0, if it is not connected
2096 MSLink*
getLinkTo(const MSLane * target) const2097 MSLane::getLinkTo(const MSLane* target) const {
2098 MSLinkCont::const_iterator l = myLinks.begin();
2099 if (target->isInternal()) {
2100 while (l != myLinks.end()) {
2101 if ((*l)->getViaLane()->getID() == target->getID()) {
2102 return *l;
2103 }
2104 ++l;
2105 }
2106 } else {
2107 while (l != myLinks.end()) {
2108 if ((*l)->getLane()->getID() == target->getID()) {
2109 return *l;
2110 }
2111 ++l;
2112 }
2113 }
2114 return nullptr;
2115 }
2116
2117 MSLink*
getEntryLink() const2118 MSLane::getEntryLink() const {
2119 if (!isInternal()) {
2120 return nullptr;
2121 }
2122 const MSLane* internal = this;
2123 const MSLane* lane = this->getCanonicalPredecessorLane();
2124 assert(lane != 0);
2125 while (lane->isInternal()) {
2126 internal = lane;
2127 lane = lane->getCanonicalPredecessorLane();
2128 assert(lane != 0);
2129 }
2130 return lane->getLinkTo(internal);
2131 }
2132
2133
2134 void
setMaxSpeed(double val)2135 MSLane::setMaxSpeed(double val) {
2136 myMaxSpeed = val;
2137 myEdge->recalcCache();
2138 }
2139
2140
2141 void
setLength(double val)2142 MSLane::setLength(double val) {
2143 myLength = val;
2144 myEdge->recalcCache();
2145 }
2146
2147
2148 void
swapAfterLaneChange(SUMOTime)2149 MSLane::swapAfterLaneChange(SUMOTime) {
2150 //if (getID() == "disabled_lane") std::cout << SIMTIME << " swapAfterLaneChange lane=" << getID() << " myVehicles=" << toString(myVehicles) << " myTmpVehicles=" << toString(myTmpVehicles) << "\n";
2151 myVehicles = myTmpVehicles;
2152 myTmpVehicles.clear();
2153 // this needs to be done after finishing lane-changing for all lanes on the
2154 // current edge (MSLaneChanger::updateLanes())
2155 sortPartialVehicles();
2156 }
2157
2158
2159 MSVehicle*
removeVehicle(MSVehicle * remVehicle,MSMoveReminder::Notification notification,bool notify)2160 MSLane::removeVehicle(MSVehicle* remVehicle, MSMoveReminder::Notification notification, bool notify) {
2161 assert(remVehicle->getLane() == this);
2162 for (MSLane::VehCont::iterator it = myVehicles.begin(); it < myVehicles.end(); it++) {
2163 if (remVehicle == *it) {
2164 if (notify) {
2165 remVehicle->leaveLane(notification);
2166 }
2167 myVehicles.erase(it);
2168 myBruttoVehicleLengthSum -= remVehicle->getVehicleType().getLengthWithGap();
2169 myNettoVehicleLengthSum -= remVehicle->getVehicleType().getLength();
2170 break;
2171 }
2172 }
2173 return remVehicle;
2174 }
2175
2176
2177 MSLane*
getParallelLane(int offset) const2178 MSLane::getParallelLane(int offset) const {
2179 return myEdge->parallelLane(this, offset);
2180 }
2181
2182
2183 void
addIncomingLane(MSLane * lane,MSLink * viaLink)2184 MSLane::addIncomingLane(MSLane* lane, MSLink* viaLink) {
2185 IncomingLaneInfo ili;
2186 ili.lane = lane;
2187 ili.viaLink = viaLink;
2188 ili.length = lane->getLength();
2189 myIncomingLanes.push_back(ili);
2190 }
2191
2192
2193 void
addApproachingLane(MSLane * lane,bool warnMultiCon)2194 MSLane::addApproachingLane(MSLane* lane, bool warnMultiCon) {
2195 MSEdge* approachingEdge = &lane->getEdge();
2196 if (myApproachingLanes.find(approachingEdge) == myApproachingLanes.end()) {
2197 myApproachingLanes[approachingEdge] = std::vector<MSLane*>();
2198 } else if (!approachingEdge->isInternal() && warnMultiCon) {
2199 // whenever a normal edge connects twice, there is a corresponding
2200 // internal edge wich connects twice, one warning is sufficient
2201 WRITE_WARNING("Lane '" + getID() + "' is approached multiple times from edge '" + approachingEdge->getID() + "'. This may cause collisions.");
2202 }
2203 myApproachingLanes[approachingEdge].push_back(lane);
2204 }
2205
2206
2207 bool
isApproachedFrom(MSEdge * const edge)2208 MSLane::isApproachedFrom(MSEdge* const edge) {
2209 return myApproachingLanes.find(edge) != myApproachingLanes.end();
2210 }
2211
2212
2213 bool
isApproachedFrom(MSEdge * const edge,MSLane * const lane)2214 MSLane::isApproachedFrom(MSEdge* const edge, MSLane* const lane) {
2215 std::map<MSEdge*, std::vector<MSLane*> >::const_iterator i = myApproachingLanes.find(edge);
2216 if (i == myApproachingLanes.end()) {
2217 return false;
2218 }
2219 const std::vector<MSLane*>& lanes = (*i).second;
2220 return std::find(lanes.begin(), lanes.end(), lane) != lanes.end();
2221 }
2222
2223
2224 class by_second_sorter {
2225 public:
operator ()(const std::pair<const MSVehicle *,double> & p1,const std::pair<const MSVehicle *,double> & p2) const2226 inline int operator()(const std::pair<const MSVehicle*, double>& p1, const std::pair<const MSVehicle*, double>& p2) const {
2227 return p1.second < p2.second;
2228 }
2229 };
2230
2231
getMissingRearGap(const MSVehicle * leader,double backOffset,double leaderSpeed) const2232 double MSLane::getMissingRearGap(const MSVehicle* leader, double backOffset, double leaderSpeed) const {
2233 // this follows the same logic as getFollowerOnConsecutive. we do a tree
2234 // search and check for the vehicle with the largest missing rear gap within
2235 // relevant range
2236 double result = 0;
2237 const double leaderDecel = leader->getCarFollowModel().getMaxDecel();
2238 CLeaderDist followerInfo = getFollowersOnConsecutive(leader, backOffset, false)[0];
2239 const MSVehicle* v = followerInfo.first;
2240 if (v != nullptr) {
2241 result = v->getCarFollowModel().getSecureGap(v->getSpeed(), leaderSpeed, leaderDecel) - followerInfo.second;
2242 }
2243 return result;
2244 }
2245
2246
2247 double
getMaximumBrakeDist() const2248 MSLane::getMaximumBrakeDist() const {
2249 const MSVehicleControl& vc = MSNet::getInstance()->getVehicleControl();
2250 const double maxSpeed = getSpeedLimit() * vc.getMaxSpeedFactor();
2251 // NOTE: For the euler update this is an upper bound on the actual braking distance (see ticket #860)
2252 // impose a hard bound due to visibiilty / common sense to avoid unnecessary computation if there are strange vehicles in the fleet
2253 return MIN2(maxSpeed * maxSpeed * 0.5 / vc.getMinDeceleration(),
2254 myPermissions == SVC_SHIP ? 10000.0 : 1000.0);
2255 }
2256
2257
2258 std::pair<MSVehicle* const, double>
getLeader(const MSVehicle * veh,const double vehPos,const std::vector<MSLane * > & bestLaneConts,double dist,bool checkTmpVehicles) const2259 MSLane::getLeader(const MSVehicle* veh, const double vehPos, const std::vector<MSLane*>& bestLaneConts, double dist, bool checkTmpVehicles) const {
2260 // get the leading vehicle for (shadow) veh
2261 // XXX this only works as long as all lanes of an edge have equal length
2262 #ifdef DEBUG_CONTEXT
2263 if (DEBUG_COND2(veh)) {
2264 std::cout << " getLeader lane=" << getID() << " ego=" << veh->getID() << " vehs=" << toString(myVehicles) << " tmpVehs=" << toString(myTmpVehicles) << "\n";
2265 }
2266 #endif
2267 if (checkTmpVehicles) {
2268 for (VehCont::const_iterator last = myTmpVehicles.begin(); last != myTmpVehicles.end(); ++last) {
2269 // XXX refactor leaderInfo to use a const vehicle all the way through the call hierarchy
2270 MSVehicle* pred = (MSVehicle*)*last;
2271 if (pred == veh) {
2272 continue;
2273 }
2274 #ifdef DEBUG_CONTEXT
2275 if (DEBUG_COND2(veh)) {
2276 std::cout << std::setprecision(gPrecision) << " getLeader lane=" << getID() << " ego=" << veh->getID() << " egoPos=" << vehPos << " pred=" << pred->getID() << " predPos=" << pred->getPositionOnLane() << "\n";
2277 }
2278 #endif
2279 if (pred->getPositionOnLane() >= vehPos) {
2280 return std::pair<MSVehicle* const, double>(pred, pred->getBackPositionOnLane(this) - veh->getVehicleType().getMinGap() - vehPos);
2281 }
2282 }
2283 } else {
2284 for (AnyVehicleIterator last = anyVehiclesBegin(); last != anyVehiclesEnd(); ++last) {
2285 // XXX refactor leaderInfo to use a const vehicle all the way through the call hierarchy
2286 MSVehicle* pred = (MSVehicle*)*last;
2287 if (pred == veh) {
2288 continue;
2289 }
2290 #ifdef DEBUG_CONTEXT
2291 if (DEBUG_COND2(veh)) {
2292 std::cout << " getLeader lane=" << getID() << " ego=" << veh->getID() << " egoPos=" << vehPos
2293 << " pred=" << pred->getID() << " predPos=" << pred->getPositionOnLane(this) << " predBack=" << pred->getBackPositionOnLane(this) << "\n";
2294 }
2295 #endif
2296 if (pred->getPositionOnLane(this) >= vehPos) {
2297 return std::pair<MSVehicle* const, double>(pred, pred->getBackPositionOnLane(this) - veh->getVehicleType().getMinGap() - vehPos);
2298 }
2299 }
2300 }
2301 // XXX from here on the code mirrors MSLaneChanger::getRealLeader
2302 if (bestLaneConts.size() > 0) {
2303 double seen = getLength() - vehPos;
2304 double speed = veh->getSpeed();
2305 if (dist < 0) {
2306 dist = veh->getCarFollowModel().brakeGap(speed) + veh->getVehicleType().getMinGap();
2307 }
2308 #ifdef DEBUG_CONTEXT
2309 if (DEBUG_COND2(veh)) {
2310 std::cout << " getLeader lane=" << getID() << " seen=" << seen << " dist=" << dist << "\n";
2311 }
2312 #endif
2313 if (seen > dist) {
2314 return std::pair<MSVehicle* const, double>(static_cast<MSVehicle*>(nullptr), -1);
2315 }
2316 return getLeaderOnConsecutive(dist, seen, speed, *veh, bestLaneConts);
2317 } else {
2318 return std::make_pair(static_cast<MSVehicle*>(nullptr), -1);
2319 }
2320 }
2321
2322
2323 std::pair<MSVehicle* const, double>
getLeaderOnConsecutive(double dist,double seen,double speed,const MSVehicle & veh,const std::vector<MSLane * > & bestLaneConts) const2324 MSLane::getLeaderOnConsecutive(double dist, double seen, double speed, const MSVehicle& veh,
2325 const std::vector<MSLane*>& bestLaneConts) const {
2326 #ifdef DEBUG_CONTEXT
2327 if (DEBUG_COND2(&veh)) {
2328 std::cout << " getLeaderOnConsecutive lane=" << getID() << " ego=" << veh.getID() << " seen=" << seen << " dist=" << dist << " conts=" << toString(bestLaneConts) << "\n";
2329 }
2330 #endif
2331 if (seen > dist && !isInternal()) {
2332 return std::make_pair(static_cast<MSVehicle*>(nullptr), -1);
2333 }
2334 int view = 1;
2335 // loop over following lanes
2336 if (myPartialVehicles.size() > 0) {
2337 // XXX
2338 MSVehicle* pred = myPartialVehicles.front();
2339 const double gap = seen - (getLength() - pred->getBackPositionOnLane(this)) - veh.getVehicleType().getMinGap();
2340 #ifdef DEBUG_CONTEXT
2341 if (DEBUG_COND2(&veh)) {
2342 std::cout << " predGap=" << gap << " partials=" << toString(myPartialVehicles) << "\n";
2343 }
2344 #endif
2345 // make sure pred is really a leader and not doing continous lane-changing behind ego
2346 if (gap > 0) {
2347 return std::pair<MSVehicle* const, double>(pred, gap);
2348 }
2349 }
2350 const MSLane* nextLane = this;
2351 SUMOTime arrivalTime = MSNet::getInstance()->getCurrentTimeStep() + TIME2STEPS(seen / MAX2(speed, NUMERICAL_EPS));
2352 do {
2353 nextLane->getVehiclesSecure(); // lock against running sim when called from GUI for time gap coloring
2354 // get the next link used
2355 MSLinkCont::const_iterator link = succLinkSec(veh, view, *nextLane, bestLaneConts);
2356 if (nextLane->isLinkEnd(link) || !(*link)->opened(arrivalTime, speed, speed, veh.getVehicleType().getLength(),
2357 veh.getImpatience(), veh.getCarFollowModel().getMaxDecel(), 0, veh.getLateralPositionOnLane()) || (*link)->haveRed()) {
2358 #ifdef DEBUG_CONTEXT
2359 if (DEBUG_COND2(&veh)) {
2360 std::cout << " cannot continue after nextLane=" << nextLane->getID() << "\n";
2361 }
2362 #endif
2363 nextLane->releaseVehicles();
2364 break;
2365 }
2366 // check for link leaders
2367 #ifdef DEBUG_CONTEXT
2368 if (DEBUG_COND2(&veh)) {
2369 gDebugFlag1 = true;
2370 }
2371 #endif
2372 const bool laneChanging = veh.getLane() != this;
2373 const MSLink::LinkLeaders linkLeaders = (*link)->getLeaderInfo(&veh, seen);
2374 #ifdef DEBUG_CONTEXT
2375 gDebugFlag1 = false;
2376 #endif
2377 nextLane->releaseVehicles();
2378 if (linkLeaders.size() > 0) {
2379 std::pair<MSVehicle*, double> result;
2380 double shortestGap = std::numeric_limits<double>::max();
2381 for (auto ll : linkLeaders) {
2382 double gap = ll.vehAndGap.second;
2383 MSVehicle* lVeh = ll.vehAndGap.first;
2384 if (lVeh != nullptr) {
2385 // leader is a vehicle, not a pedestrian
2386 gap += lVeh->getCarFollowModel().brakeGap(lVeh->getSpeed(), lVeh->getCarFollowModel().getMaxDecel(), 0);
2387 }
2388 #ifdef DEBUG_CONTEXT
2389 if (DEBUG_COND2(&veh)) {
2390 std::cout << " linkLeader candidate " << Named::getIDSecure(lVeh)
2391 << " isLeader=" << veh.isLeader(*link, lVeh)
2392 << " gap=" << ll.vehAndGap.second
2393 << " gap+brakeing=" << gap
2394 << "\n";
2395 }
2396 #endif
2397 // in the context of lane-changing, all candidates are leaders
2398 if (lVeh != nullptr && !laneChanging && !veh.isLeader(*link, lVeh)) {
2399 continue;
2400 }
2401 if (gap < shortestGap) {
2402 shortestGap = gap;
2403 result = ll.vehAndGap;
2404 }
2405 }
2406 if (shortestGap != std::numeric_limits<double>::max()) {
2407 #ifdef DEBUG_CONTEXT
2408 if (DEBUG_COND2(&veh)) {
2409 std::cout << " found linkLeader after nextLane=" << nextLane->getID() << "\n";
2410 }
2411 #endif
2412 return result;
2413 }
2414 }
2415 bool nextInternal = (*link)->getViaLane() != nullptr;
2416 nextLane = (*link)->getViaLaneOrLane();
2417 if (nextLane == nullptr) {
2418 break;
2419 }
2420 nextLane->getVehiclesSecure(); // lock against running sim when called from GUI for time gap coloring
2421 MSVehicle* leader = nextLane->getLastAnyVehicle();
2422 if (leader != nullptr) {
2423 #ifdef DEBUG_CONTEXT
2424 if (DEBUG_COND2(&veh)) {
2425 std::cout << " found leader " << leader->getID() << " on nextLane=" << nextLane->getID() << "\n";
2426 }
2427 #endif
2428 const double dist = seen + leader->getBackPositionOnLane(nextLane) - veh.getVehicleType().getMinGap();
2429 nextLane->releaseVehicles();
2430 return std::make_pair(leader, dist);
2431 }
2432 nextLane->releaseVehicles();
2433 if (nextLane->getVehicleMaxSpeed(&veh) < speed) {
2434 dist = veh.getCarFollowModel().brakeGap(nextLane->getVehicleMaxSpeed(&veh));
2435 }
2436 seen += nextLane->getLength();
2437 if (seen <= dist) {
2438 // delaying the update of arrivalTime and making it conditional to avoid possible integer overflows
2439 arrivalTime += TIME2STEPS(nextLane->getLength() / MAX2(speed, NUMERICAL_EPS));
2440 }
2441 if (!nextInternal) {
2442 view++;
2443 }
2444 } while (seen <= dist || nextLane->isInternal());
2445 return std::make_pair(static_cast<MSVehicle*>(nullptr), -1);
2446 }
2447
2448
2449 std::pair<MSVehicle* const, double>
getCriticalLeader(double dist,double seen,double speed,const MSVehicle & veh) const2450 MSLane::getCriticalLeader(double dist, double seen, double speed, const MSVehicle& veh) const {
2451 #ifdef DEBUG_CONTEXT
2452 if (DEBUG_COND2(&veh)) {
2453 std::cout << SIMTIME << " getCriticalLeader. lane=" << getID() << " veh=" << veh.getID() << "\n";
2454 }
2455 #endif
2456 const std::vector<MSLane*>& bestLaneConts = veh.getBestLanesContinuation(this);
2457 std::pair<MSVehicle*, double> result = std::make_pair(static_cast<MSVehicle*>(nullptr), -1);
2458 double safeSpeed = std::numeric_limits<double>::max();
2459 int view = 1;
2460 // loop over following lanes
2461 // @note: we don't check the partial occupator for this lane since it was
2462 // already checked in MSLaneChanger::getRealLeader()
2463 const MSLane* nextLane = this;
2464 SUMOTime arrivalTime = MSNet::getInstance()->getCurrentTimeStep() + TIME2STEPS(seen / MAX2(speed, NUMERICAL_EPS));
2465 do {
2466 // get the next link used
2467 MSLinkCont::const_iterator link = succLinkSec(veh, view, *nextLane, bestLaneConts);
2468 if (nextLane->isLinkEnd(link) || !(*link)->opened(arrivalTime, speed, speed, veh.getVehicleType().getLength(),
2469 veh.getImpatience(), veh.getCarFollowModel().getMaxDecel(), 0, veh.getLateralPositionOnLane()) || (*link)->haveRed()) {
2470 return result;
2471 }
2472 // check for link leaders
2473 const MSLink::LinkLeaders linkLeaders = (*link)->getLeaderInfo(&veh, seen);
2474 for (MSLink::LinkLeaders::const_iterator it = linkLeaders.begin(); it != linkLeaders.end(); ++it) {
2475 const MSVehicle* leader = (*it).vehAndGap.first;
2476 if (leader != nullptr && leader != result.first) {
2477 // XXX ignoring pedestrians here!
2478 // XXX ignoring the fact that the link leader may alread by following us
2479 // XXX ignoring the fact that we may drive up to the crossing point
2480 const double tmpSpeed = veh.getSafeFollowSpeed((*it).vehAndGap, seen, nextLane, (*it).distToCrossing);
2481 #ifdef DEBUG_CONTEXT
2482 if (DEBUG_COND2(&veh)) {
2483 std::cout << " linkLeader=" << leader->getID() << " gap=" << result.second << " tmpSpeed=" << tmpSpeed << " safeSpeed=" << safeSpeed << "\n";
2484 }
2485 #endif
2486 if (tmpSpeed < safeSpeed) {
2487 safeSpeed = tmpSpeed;
2488 result = (*it).vehAndGap;
2489 }
2490 }
2491 }
2492 bool nextInternal = (*link)->getViaLane() != nullptr;
2493 nextLane = (*link)->getViaLaneOrLane();
2494 if (nextLane == nullptr) {
2495 break;
2496 }
2497 MSVehicle* leader = nextLane->getLastAnyVehicle();
2498 if (leader != nullptr && leader != result.first) {
2499 const double gap = seen + leader->getBackPositionOnLane(nextLane) - veh.getVehicleType().getMinGap();
2500 const double tmpSpeed = veh.getCarFollowModel().insertionFollowSpeed(&veh, speed, gap, leader->getSpeed(), leader->getCarFollowModel().getMaxDecel());
2501 if (tmpSpeed < safeSpeed) {
2502 safeSpeed = tmpSpeed;
2503 result = std::make_pair(leader, gap);
2504 }
2505 }
2506 if (nextLane->getVehicleMaxSpeed(&veh) < speed) {
2507 dist = veh.getCarFollowModel().brakeGap(nextLane->getVehicleMaxSpeed(&veh));
2508 }
2509 seen += nextLane->getLength();
2510 if (seen <= dist) {
2511 // delaying the update of arrivalTime and making it conditional to avoid possible integer overflows
2512 arrivalTime += TIME2STEPS(nextLane->getLength() / MAX2(speed, NUMERICAL_EPS));
2513 }
2514 if (!nextInternal) {
2515 view++;
2516 }
2517 } while (seen <= dist || nextLane->isInternal());
2518 return result;
2519 }
2520
2521
2522 MSLane*
getLogicalPredecessorLane() const2523 MSLane::getLogicalPredecessorLane() const {
2524 if (myLogicalPredecessorLane == nullptr) {
2525 MSEdgeVector pred = myEdge->getPredecessors();
2526 // get only those edges which connect to this lane
2527 for (MSEdgeVector::iterator i = pred.begin(); i != pred.end();) {
2528 std::vector<IncomingLaneInfo>::const_iterator j = find_if(myIncomingLanes.begin(), myIncomingLanes.end(), edge_finder(*i));
2529 if (j == myIncomingLanes.end()) {
2530 i = pred.erase(i);
2531 } else {
2532 ++i;
2533 }
2534 }
2535 // get the lane with the "straightest" connection
2536 if (pred.size() != 0) {
2537 std::sort(pred.begin(), pred.end(), by_connections_to_sorter(&getEdge()));
2538 MSEdge* best = *pred.begin();
2539 std::vector<IncomingLaneInfo>::const_iterator j = find_if(myIncomingLanes.begin(), myIncomingLanes.end(), edge_finder(best));
2540 myLogicalPredecessorLane = j->lane;
2541 }
2542 }
2543 return myLogicalPredecessorLane;
2544 }
2545
2546
2547 const MSLane*
getNormalPredecessorLane() const2548 MSLane::getNormalPredecessorLane() const {
2549 if (isInternal()) {
2550 return getLogicalPredecessorLane()->getNormalPredecessorLane();
2551 } else {
2552 return this;
2553 }
2554 }
2555
2556
2557 MSLane*
getLogicalPredecessorLane(const MSEdge & fromEdge) const2558 MSLane::getLogicalPredecessorLane(const MSEdge& fromEdge) const {
2559 for (std::vector<IncomingLaneInfo>::const_iterator i = myIncomingLanes.begin(); i != myIncomingLanes.end(); ++i) {
2560 MSLane* cand = (*i).lane;
2561 if (&(cand->getEdge()) == &fromEdge) {
2562 return (*i).lane;
2563 }
2564 }
2565 return nullptr;
2566 }
2567
2568 MSLane*
getCanonicalPredecessorLane() const2569 MSLane::getCanonicalPredecessorLane() const {
2570 if (myCanonicalPredecessorLane != nullptr) {
2571 return myCanonicalPredecessorLane;
2572 }
2573 if (myIncomingLanes.size() == 0) {
2574 return nullptr;
2575 }
2576 // myCanonicalPredecessorLane has not yet been determined and there exist incoming lanes
2577 std::vector<IncomingLaneInfo> candidateLanes = myIncomingLanes;
2578 // get the lane with the priorized (or if this does not apply the "straightest") connection
2579 std::sort(candidateLanes.begin(), candidateLanes.end(), incoming_lane_priority_sorter(this));
2580 IncomingLaneInfo best = *(candidateLanes.begin());
2581 #ifdef DEBUG_LANE_SORTER
2582 std::cout << "\nBest predecessor lane for lane '" << myID << "': '" << best.lane->getID() << "'" << std::endl;
2583 #endif
2584 myCanonicalPredecessorLane = best.lane;
2585 return myCanonicalPredecessorLane;
2586 }
2587
2588 MSLane*
getCanonicalSuccessorLane() const2589 MSLane::getCanonicalSuccessorLane() const {
2590 if (myCanonicalSuccessorLane != nullptr) {
2591 return myCanonicalSuccessorLane;
2592 }
2593 if (myLinks.size() == 0) {
2594 return nullptr;
2595 }
2596 // myCanonicalSuccessorLane has not yet been determined and there exist outgoing links
2597 std::vector<MSLink*> candidateLinks = myLinks;
2598 // get the lane with the priorized (or if this does not apply the "straightest") connection
2599 std::sort(candidateLinks.begin(), candidateLinks.end(), outgoing_lane_priority_sorter(this));
2600 MSLane* best = (*candidateLinks.begin())->getViaLaneOrLane();
2601 #ifdef DEBUG_LANE_SORTER
2602 std::cout << "\nBest successor lane for lane '" << myID << "': '" << best->getID() << "'" << std::endl;
2603 #endif
2604 myCanonicalSuccessorLane = best;
2605 return myCanonicalSuccessorLane;
2606 }
2607
2608
2609 LinkState
getIncomingLinkState() const2610 MSLane::getIncomingLinkState() const {
2611 MSLane* pred = getLogicalPredecessorLane();
2612 if (pred == nullptr) {
2613 return LINKSTATE_DEADEND;
2614 } else {
2615 return MSLinkContHelper::getConnectingLink(*pred, *this)->getState();
2616 }
2617 }
2618
2619
2620 const std::vector<std::pair<const MSLane*, const MSEdge*> >
getOutgoingViaLanes() const2621 MSLane::getOutgoingViaLanes() const {
2622 std::vector<std::pair<const MSLane*, const MSEdge*> > result;
2623 for (const MSLink* link : myLinks) {
2624 assert(link->getLane() != nullptr);
2625 result.push_back(std::make_pair(link->getLane(), link->getViaLane() == nullptr ? nullptr : &link->getViaLane()->getEdge()));
2626 }
2627 return result;
2628 }
2629
2630
2631 void
leftByLaneChange(MSVehicle * v)2632 MSLane::leftByLaneChange(MSVehicle* v) {
2633 myBruttoVehicleLengthSum -= v->getVehicleType().getLengthWithGap();
2634 myNettoVehicleLengthSum -= v->getVehicleType().getLength();
2635 }
2636
2637
2638 void
enteredByLaneChange(MSVehicle * v)2639 MSLane::enteredByLaneChange(MSVehicle* v) {
2640 myBruttoVehicleLengthSum += v->getVehicleType().getLengthWithGap();
2641 myNettoVehicleLengthSum += v->getVehicleType().getLength();
2642 }
2643
2644
2645 int
getCrossingIndex() const2646 MSLane::getCrossingIndex() const {
2647 for (MSLinkCont::const_iterator i = myLinks.begin(); i != myLinks.end(); ++i) {
2648 if ((*i)->getLane()->getEdge().isCrossing()) {
2649 return (int)(i - myLinks.begin());
2650 }
2651 }
2652 return -1;
2653 }
2654
2655 // ------------ Current state retrieval
2656 double
getBruttoOccupancy() const2657 MSLane::getBruttoOccupancy() const {
2658 double fractions = myPartialVehicles.size() > 0 ? MIN2(myLength, myLength - myPartialVehicles.front()->getBackPositionOnLane(this)) : 0;
2659 getVehiclesSecure();
2660 if (myVehicles.size() != 0) {
2661 MSVehicle* lastVeh = myVehicles.front();
2662 if (lastVeh->getPositionOnLane() < lastVeh->getVehicleType().getLength()) {
2663 fractions -= (lastVeh->getVehicleType().getLength() - lastVeh->getPositionOnLane());
2664 }
2665 }
2666 releaseVehicles();
2667 return MIN2(1., (myBruttoVehicleLengthSum + fractions) / myLength);
2668 }
2669
2670
2671 double
getNettoOccupancy() const2672 MSLane::getNettoOccupancy() const {
2673 double fractions = myPartialVehicles.size() > 0 ? MIN2(myLength, myLength - myPartialVehicles.front()->getBackPositionOnLane(this)) : 0;
2674 getVehiclesSecure();
2675 if (myVehicles.size() != 0) {
2676 MSVehicle* lastVeh = myVehicles.front();
2677 if (lastVeh->getPositionOnLane() < lastVeh->getVehicleType().getLength()) {
2678 fractions -= (lastVeh->getVehicleType().getLength() - lastVeh->getPositionOnLane());
2679 }
2680 }
2681 releaseVehicles();
2682 return (myNettoVehicleLengthSum + fractions) / myLength;
2683 }
2684
2685
2686 double
getWaitingSeconds() const2687 MSLane::getWaitingSeconds() const {
2688 if (myVehicles.size() == 0) {
2689 return 0;
2690 }
2691 double wtime = 0;
2692 for (VehCont::const_iterator i = myVehicles.begin(); i != myVehicles.end(); ++i) {
2693 wtime += (*i)->getWaitingSeconds();
2694 }
2695 return wtime;
2696 }
2697
2698
2699 double
getMeanSpeed() const2700 MSLane::getMeanSpeed() const {
2701 if (myVehicles.size() == 0) {
2702 return myMaxSpeed;
2703 }
2704 double v = 0;
2705 const MSLane::VehCont& vehs = getVehiclesSecure();
2706 for (VehCont::const_iterator i = vehs.begin(); i != vehs.end(); ++i) {
2707 v += (*i)->getSpeed();
2708 }
2709 double ret = v / (double) myVehicles.size();
2710 releaseVehicles();
2711 return ret;
2712 }
2713
2714
2715 double
getCO2Emissions() const2716 MSLane::getCO2Emissions() const {
2717 double ret = 0;
2718 const MSLane::VehCont& vehs = getVehiclesSecure();
2719 for (MSLane::VehCont::const_iterator i = vehs.begin(); i != vehs.end(); ++i) {
2720 ret += (*i)->getCO2Emissions();
2721 }
2722 releaseVehicles();
2723 return ret;
2724 }
2725
2726
2727 double
getCOEmissions() const2728 MSLane::getCOEmissions() const {
2729 double ret = 0;
2730 const MSLane::VehCont& vehs = getVehiclesSecure();
2731 for (MSLane::VehCont::const_iterator i = vehs.begin(); i != vehs.end(); ++i) {
2732 ret += (*i)->getCOEmissions();
2733 }
2734 releaseVehicles();
2735 return ret;
2736 }
2737
2738
2739 double
getPMxEmissions() const2740 MSLane::getPMxEmissions() const {
2741 double ret = 0;
2742 const MSLane::VehCont& vehs = getVehiclesSecure();
2743 for (MSLane::VehCont::const_iterator i = vehs.begin(); i != vehs.end(); ++i) {
2744 ret += (*i)->getPMxEmissions();
2745 }
2746 releaseVehicles();
2747 return ret;
2748 }
2749
2750
2751 double
getNOxEmissions() const2752 MSLane::getNOxEmissions() const {
2753 double ret = 0;
2754 const MSLane::VehCont& vehs = getVehiclesSecure();
2755 for (MSLane::VehCont::const_iterator i = vehs.begin(); i != vehs.end(); ++i) {
2756 ret += (*i)->getNOxEmissions();
2757 }
2758 releaseVehicles();
2759 return ret;
2760 }
2761
2762
2763 double
getHCEmissions() const2764 MSLane::getHCEmissions() const {
2765 double ret = 0;
2766 const MSLane::VehCont& vehs = getVehiclesSecure();
2767 for (MSLane::VehCont::const_iterator i = vehs.begin(); i != vehs.end(); ++i) {
2768 ret += (*i)->getHCEmissions();
2769 }
2770 releaseVehicles();
2771 return ret;
2772 }
2773
2774
2775 double
getFuelConsumption() const2776 MSLane::getFuelConsumption() const {
2777 double ret = 0;
2778 const MSLane::VehCont& vehs = getVehiclesSecure();
2779 for (MSLane::VehCont::const_iterator i = vehs.begin(); i != vehs.end(); ++i) {
2780 ret += (*i)->getFuelConsumption();
2781 }
2782 releaseVehicles();
2783 return ret;
2784 }
2785
2786
2787 double
getElectricityConsumption() const2788 MSLane::getElectricityConsumption() const {
2789 double ret = 0;
2790 const MSLane::VehCont& vehs = getVehiclesSecure();
2791 for (MSLane::VehCont::const_iterator i = vehs.begin(); i != vehs.end(); ++i) {
2792 ret += (*i)->getElectricityConsumption();
2793 }
2794 releaseVehicles();
2795 return ret;
2796 }
2797
2798
2799 double
getHarmonoise_NoiseEmissions() const2800 MSLane::getHarmonoise_NoiseEmissions() const {
2801 double ret = 0;
2802 const MSLane::VehCont& vehs = getVehiclesSecure();
2803 if (vehs.size() == 0) {
2804 releaseVehicles();
2805 return 0;
2806 }
2807 for (MSLane::VehCont::const_iterator i = vehs.begin(); i != vehs.end(); ++i) {
2808 double sv = (*i)->getHarmonoise_NoiseEmissions();
2809 ret += (double) pow(10., (sv / 10.));
2810 }
2811 releaseVehicles();
2812 return HelpersHarmonoise::sum(ret);
2813 }
2814
2815
2816 bool
operator ()(const MSVehicle * cmp,double pos) const2817 MSLane::VehPosition::operator()(const MSVehicle* cmp, double pos) const {
2818 return cmp->getPositionOnLane() >= pos;
2819 }
2820
2821
2822 int
operator ()(MSVehicle * v1,MSVehicle * v2) const2823 MSLane::vehicle_position_sorter::operator()(MSVehicle* v1, MSVehicle* v2) const {
2824 return v1->getBackPositionOnLane(myLane) > v2->getBackPositionOnLane(myLane);
2825 }
2826
2827 int
operator ()(MSVehicle * v1,MSVehicle * v2) const2828 MSLane::vehicle_natural_position_sorter::operator()(MSVehicle* v1, MSVehicle* v2) const {
2829 const double pos1 = v1->getBackPositionOnLane(myLane);
2830 const double pos2 = v2->getBackPositionOnLane(myLane);
2831 if (pos1 != pos2) {
2832 return pos1 < pos2;
2833 } else {
2834 return v1->getLateralPositionOnLane() < v2->getLateralPositionOnLane();
2835 }
2836 }
2837
by_connections_to_sorter(const MSEdge * const e)2838 MSLane::by_connections_to_sorter::by_connections_to_sorter(const MSEdge* const e) :
2839 myEdge(e),
2840 myLaneDir(e->getLanes()[0]->getShape().angleAt2D(0)) {
2841 }
2842
2843
2844 int
operator ()(const MSEdge * const e1,const MSEdge * const e2) const2845 MSLane::by_connections_to_sorter::operator()(const MSEdge* const e1, const MSEdge* const e2) const {
2846 // std::cout << "\nby_connections_to_sorter()";
2847
2848 const std::vector<MSLane*>* ae1 = e1->allowedLanes(*myEdge);
2849 const std::vector<MSLane*>* ae2 = e2->allowedLanes(*myEdge);
2850 double s1 = 0;
2851 if (ae1 != nullptr && ae1->size() != 0) {
2852 // std::cout << "\nsize 1 = " << ae1->size()
2853 // << " anglediff 1 = " << fabs(GeomHelper::angleDiff((*ae1)[0]->getShape().angleAt2D(0), myLaneDir)) / M_PI / 2.
2854 // << "\nallowed lanes: ";
2855 // for (std::vector<MSLane*>::const_iterator j = ae1->begin(); j != ae1->end(); ++j){
2856 // std::cout << "\n" << (*j)->getID();
2857 // }
2858 s1 = (double) ae1->size() + fabs(GeomHelper::angleDiff((*ae1)[0]->getShape().angleAt2D(0), myLaneDir)) / M_PI / 2.;
2859 }
2860 double s2 = 0;
2861 if (ae2 != nullptr && ae2->size() != 0) {
2862 // std::cout << "\nsize 2 = " << ae2->size()
2863 // << " anglediff 2 = " << fabs(GeomHelper::angleDiff((*ae2)[0]->getShape().angleAt2D(0), myLaneDir)) / M_PI / 2.
2864 // << "\nallowed lanes: ";
2865 // for (std::vector<MSLane*>::const_iterator j = ae2->begin(); j != ae2->end(); ++j){
2866 // std::cout << "\n" << (*j)->getID();
2867 // }
2868 s2 = (double) ae2->size() + fabs(GeomHelper::angleDiff((*ae2)[0]->getShape().angleAt2D(0), myLaneDir)) / M_PI / 2.;
2869 }
2870
2871 // std::cout << "\ne1 = " << e1->getID() << " e2 = " << e2->getID()
2872 // << "\ns1 = " << s1 << " s2 = " << s2
2873 // << std::endl;
2874
2875 return s1 < s2;
2876 }
2877
2878
incoming_lane_priority_sorter(const MSLane * const targetLane)2879 MSLane::incoming_lane_priority_sorter::incoming_lane_priority_sorter(const MSLane* const targetLane) :
2880 myLane(targetLane),
2881 myLaneDir(targetLane->getShape().angleAt2D(0)) {}
2882
2883 int
operator ()(const IncomingLaneInfo & laneInfo1,const IncomingLaneInfo & laneInfo2) const2884 MSLane::incoming_lane_priority_sorter::operator()(const IncomingLaneInfo& laneInfo1, const IncomingLaneInfo& laneInfo2) const {
2885 const MSLane* noninternal1 = laneInfo1.lane;
2886 while (noninternal1->isInternal()) {
2887 assert(noninternal1->getIncomingLanes().size() == 1);
2888 noninternal1 = noninternal1->getIncomingLanes()[0].lane;
2889 }
2890 MSLane* noninternal2 = laneInfo2.lane;
2891 while (noninternal2->isInternal()) {
2892 assert(noninternal2->getIncomingLanes().size() == 1);
2893 noninternal2 = noninternal2->getIncomingLanes()[0].lane;
2894 }
2895
2896 MSLink* link1 = noninternal1->getLinkTo(myLane);
2897 MSLink* link2 = noninternal2->getLinkTo(myLane);
2898
2899 #ifdef DEBUG_LANE_SORTER
2900 std::cout << "\nincoming_lane_priority sorter()\n"
2901 << "noninternal predecessor for lane '" << laneInfo1.lane->getID()
2902 << "': '" << noninternal1->getID() << "'\n"
2903 << "noninternal predecessor for lane '" << laneInfo2.lane->getID()
2904 << "': '" << noninternal2->getID() << "'\n";
2905 #endif
2906
2907 assert(laneInfo1.lane->isInternal() || link1 == laneInfo1.viaLink);
2908 assert(link1 != 0);
2909 assert(link2 != 0);
2910
2911 // check priority between links
2912 bool priorized1 = true;
2913 bool priorized2 = true;
2914
2915 std::vector<MSLink*>::const_iterator j;
2916 #ifdef DEBUG_LANE_SORTER
2917 std::cout << "FoeLinks of '" << noninternal1->getID() << "'" << std::endl;
2918 #endif
2919 for (j = link1->getFoeLinks().begin(); j != link1->getFoeLinks().end(); ++j) {
2920 #ifdef DEBUG_LANE_SORTER
2921 std::cout << (*j)->getLaneBefore()->getID() << std::endl;
2922 #endif
2923 if (*j == link2) {
2924 priorized1 = false;
2925 break;
2926 }
2927 }
2928
2929 #ifdef DEBUG_LANE_SORTER
2930 std::cout << "FoeLinks of '" << noninternal2->getID() << "'" << std::endl;
2931 #endif
2932 for (j = link2->getFoeLinks().begin(); j != link2->getFoeLinks().end(); ++j) {
2933 #ifdef DEBUG_LANE_SORTER
2934 std::cout << (*j)->getLaneBefore()->getID() << std::endl;
2935 #endif
2936 // either link1 is priorized, or it should not appear in link2's foes
2937 if (*j == link2) {
2938 priorized2 = false;
2939 break;
2940 }
2941 }
2942 // if one link is subordinate, the other must be priorized
2943 assert(priorized1 || priorized2);
2944 if (priorized1 != priorized2) {
2945 return priorized1;
2946 }
2947
2948 // both are priorized, compare angle difference
2949 double d1 = fabs(GeomHelper::angleDiff(noninternal1->getShape().angleAt2D(0), myLaneDir));
2950 double d2 = fabs(GeomHelper::angleDiff(noninternal2->getShape().angleAt2D(0), myLaneDir));
2951
2952 return d2 > d1;
2953 }
2954
2955
2956
outgoing_lane_priority_sorter(const MSLane * const sourceLane)2957 MSLane::outgoing_lane_priority_sorter::outgoing_lane_priority_sorter(const MSLane* const sourceLane) :
2958 myLane(sourceLane),
2959 myLaneDir(sourceLane->getShape().angleAt2D(0)) {}
2960
2961 int
operator ()(const MSLink * link1,const MSLink * link2) const2962 MSLane::outgoing_lane_priority_sorter::operator()(const MSLink* link1, const MSLink* link2) const {
2963 const MSLane* target1 = link1->getLane();
2964 const MSLane* target2 = link2->getLane();
2965 if (target2 == nullptr) {
2966 return true;
2967 }
2968 if (target1 == nullptr) {
2969 return false;
2970 }
2971
2972 #ifdef DEBUG_LANE_SORTER
2973 std::cout << "\noutgoing_lane_priority sorter()\n"
2974 << "noninternal successors for lane '" << myLane->getID()
2975 << "': '" << target1->getID() << "' and "
2976 << "'" << target2->getID() << "'\n";
2977 #endif
2978
2979 // priority of targets
2980 int priority1 = target1->getEdge().getPriority();
2981 int priority2 = target2->getEdge().getPriority();
2982
2983 if (priority1 != priority2) {
2984 return priority1 > priority2;
2985 }
2986
2987 // if priority of targets coincides, use angle difference
2988
2989 // both are priorized, compare angle difference
2990 double d1 = fabs(GeomHelper::angleDiff(target1->getShape().angleAt2D(0), myLaneDir));
2991 double d2 = fabs(GeomHelper::angleDiff(target2->getShape().angleAt2D(0), myLaneDir));
2992
2993 return d2 > d1;
2994 }
2995
2996 void
addParking(MSVehicle * veh)2997 MSLane::addParking(MSVehicle* veh) {
2998 myParkingVehicles.insert(veh);
2999 }
3000
3001
3002 void
removeParking(MSVehicle * veh)3003 MSLane::removeParking(MSVehicle* veh) {
3004 myParkingVehicles.erase(veh);
3005 }
3006
3007 void
saveState(OutputDevice & out)3008 MSLane::saveState(OutputDevice& out) {
3009 out.openTag(SUMO_TAG_LANE);
3010 out.writeAttr("id", getID()); // using "id" instead of SUMO_ATTR_ID makes the value only show up in xml state
3011 out.openTag(SUMO_TAG_VIEWSETTINGS_VEHICLES);
3012 out.writeAttr(SUMO_ATTR_VALUE, myVehicles);
3013 out.closeTag();
3014 out.closeTag();
3015 }
3016
3017
3018 void
loadState(const std::vector<std::string> & vehIds,MSVehicleControl & vc)3019 MSLane::loadState(const std::vector<std::string>& vehIds, MSVehicleControl& vc) {
3020 for (const std::string& id : vehIds) {
3021 MSVehicle* v = dynamic_cast<MSVehicle*>(vc.getVehicle(id));
3022 // vehicle could be removed due to options
3023 if (v != nullptr) {
3024 v->updateBestLanes(false, this);
3025 incorporateVehicle(v, v->getPositionOnLane(), v->getSpeed(), v->getLateralPositionOnLane(), myVehicles.end(),
3026 MSMoveReminder::NOTIFICATION_JUNCTION);
3027 v->processNextStop(v->getSpeed());
3028 }
3029 }
3030 }
3031
3032
3033 double
getStopOffset(const MSVehicle * veh) const3034 MSLane::getStopOffset(const MSVehicle* veh) const {
3035 if (myStopOffsets.size() == 0) {
3036 return 0.;
3037 }
3038 if ((myStopOffsets.begin()->first & veh->getVClass()) != 0) {
3039 return myStopOffsets.begin()->second;
3040 } else {
3041 return 0.;
3042 }
3043 }
3044
3045
3046
3047 MSLeaderDistanceInfo
getFollowersOnConsecutive(const MSVehicle * ego,double backOffset,bool allSublanes,double searchDist,bool ignoreMinorLinks) const3048 MSLane::getFollowersOnConsecutive(const MSVehicle* ego, double backOffset,
3049 bool allSublanes, double searchDist, bool ignoreMinorLinks) const {
3050 // get the follower vehicle on the lane to change to
3051 const double egoPos = backOffset + ego->getVehicleType().getLength();
3052 #ifdef DEBUG_CONTEXT
3053 if (DEBUG_COND2(ego)) {
3054 std::cout << SIMTIME << " getFollowers lane=" << getID() << " ego=" << ego->getID()
3055 << " backOffset=" << backOffset << " pos=" << egoPos
3056 << " allSub=" << allSublanes << " searchDist=" << searchDist << " ignoreMinor=" << ignoreMinorLinks
3057 << "\n";
3058 }
3059 #endif
3060 assert(ego != 0);
3061 const double egoLatDist = ego->getLane()->getRightSideOnEdge() - getRightSideOnEdge();
3062 MSCriticalFollowerDistanceInfo result(this, allSublanes ? nullptr : ego, allSublanes ? 0 : egoLatDist);
3063 /// XXX iterate in reverse and abort when there are no more freeSublanes
3064 for (AnyVehicleIterator last = anyVehiclesBegin(); last != anyVehiclesEnd(); ++last) {
3065 const MSVehicle* veh = *last;
3066 #ifdef DEBUG_CONTEXT
3067 if (DEBUG_COND2(ego)) {
3068 std::cout << " veh=" << veh->getID() << " lane=" << veh->getLane()->getID() << " pos=" << veh->getPositionOnLane(this) << "\n";
3069 }
3070 #endif
3071 if (veh != ego && veh->getPositionOnLane(this) < egoPos) {
3072 //const double latOffset = veh->getLane()->getRightSideOnEdge() - getRightSideOnEdge();
3073 const double latOffset = veh->getLatOffset(this);
3074 const double dist = backOffset - veh->getPositionOnLane(this) - veh->getVehicleType().getMinGap();
3075 result.addFollower(veh, ego, dist, latOffset);
3076 #ifdef DEBUG_CONTEXT
3077 if (DEBUG_COND2(ego)) {
3078 std::cout << " (1) added veh=" << veh->getID() << " latOffset=" << latOffset << " result=" << result.toString() << "\n";
3079 }
3080 #endif
3081 }
3082 }
3083 #ifdef DEBUG_CONTEXT
3084 if (DEBUG_COND2(ego)) {
3085 std::cout << " result.numFreeSublanes=" << result.numFreeSublanes() << "\n";
3086 }
3087 #endif
3088 if (result.numFreeSublanes() > 0) {
3089 // do a tree search among all follower lanes and check for the most
3090 // important vehicle (the one requiring the largest reargap)
3091 // to get a safe bound on the necessary search depth, we need to consider the maximum speed and minimum
3092 // deceleration of potential follower vehicles
3093 if (searchDist == -1) {
3094 searchDist = getMaximumBrakeDist() - backOffset;
3095 #ifdef DEBUG_CONTEXT
3096 if (DEBUG_COND2(ego)) {
3097 std::cout << " computed searchDist=" << searchDist << "\n";
3098 }
3099 #endif
3100 }
3101 std::set<const MSEdge*> egoFurther;
3102 for (MSLane* further : ego->getFurtherLanes()) {
3103 egoFurther.insert(&further->getEdge());
3104 }
3105 if (ego->getPositionOnLane() < ego->getVehicleType().getLength() && egoFurther.size() == 0
3106 && ego->getLane()->getLogicalPredecessorLane() != nullptr) {
3107 // on insertion
3108 egoFurther.insert(&ego->getLane()->getLogicalPredecessorLane()->getEdge());
3109 }
3110
3111 // avoid loops
3112 std::set<const MSLane*> visited(myEdge->getLanes().begin(), myEdge->getLanes().end());
3113 std::vector<MSLane::IncomingLaneInfo> newFound;
3114 std::vector<MSLane::IncomingLaneInfo> toExamine = myIncomingLanes;
3115 while (toExamine.size() != 0) {
3116 for (std::vector<MSLane::IncomingLaneInfo>::iterator it = toExamine.begin(); it != toExamine.end(); ++it) {
3117 MSLane* next = (*it).lane;
3118 searchDist = MAX2(searchDist, next->getMaximumBrakeDist() - backOffset);
3119 MSLeaderInfo first = next->getFirstVehicleInformation(nullptr, 0, false, std::numeric_limits<double>::max(), false);
3120 MSLeaderInfo firstFront = next->getFirstVehicleInformation(nullptr, 0, true);
3121 #ifdef DEBUG_CONTEXT
3122 if (DEBUG_COND2(ego)) {
3123 std::cout << " next=" << next->getID() << " seen=" << (*it).length << " first=" << first.toString() << " firstFront=" << firstFront.toString() << "\n";
3124 gDebugFlag1 = true; // for calling getLeaderInfo
3125 }
3126 #endif
3127 if (backOffset + (*it).length - next->getLength() < 0
3128 && egoFurther.count(&next->getEdge()) != 0
3129 ) {
3130 // check for junction foes that would interfere with lane changing
3131 const MSLink::LinkLeaders linkLeaders = (*it).viaLink->getLeaderInfo(ego, -backOffset);
3132 for (const auto& ll : linkLeaders) {
3133 if (ll.vehAndGap.first != nullptr) {
3134 const bool egoIsLeader = ll.vehAndGap.first->isLeader((*it).viaLink, ego);
3135 const double gap = egoIsLeader ? NUMERICAL_EPS : ll.vehAndGap.second;
3136 result.addFollower(ll.vehAndGap.first, ego, gap);
3137 #ifdef DEBUG_CONTEXT
3138 if (DEBUG_COND2(ego)) {
3139 std::cout << SIMTIME << " ego=" << ego->getID() << " link=" << (*it).viaLink->getViaLaneOrLane()->getID()
3140 << " (3) added veh=" << Named::getIDSecure(ll.vehAndGap.first)
3141 << " gap=" << ll.vehAndGap.second << " dtC=" << ll.distToCrossing
3142 << " egoIsLeader=" << egoIsLeader << " gap2=" << gap
3143 << "\n";
3144 }
3145 #endif
3146 }
3147 }
3148 }
3149 #ifdef DEBUG_CONTEXT
3150 if (DEBUG_COND2(ego)) {
3151 gDebugFlag1 = false;
3152 }
3153 #endif
3154
3155 for (int i = 0; i < first.numSublanes(); ++i) {
3156 // NOTE: I added this because getFirstVehicleInformation() returns the ego as first if it partially laps into next.
3157 // EDIT: Disabled the previous changes (see commented code in next line and fourth upcoming) as I realized that this
3158 // did not completely fix the issue (to conserve test results). Refs #4727 (Leo)
3159 const MSVehicle* v = /* first[i] == ego ? firstFront[i] :*/ first[i];
3160 double agap = 0;
3161
3162 if (v != nullptr && v != ego) {
3163 if (!v->isFrontOnLane(next)) {
3164 // the front of v is already on divergent trajectory from the ego vehicle
3165 // for which this method is called (in the context of MSLaneChanger).
3166 // Therefore, technically v is not a follower but only an obstruction and
3167 // the gap is not between the front of v and the back of ego
3168 // but rather between the flank of v and the back of ego.
3169 agap = (*it).length - next->getLength() + backOffset
3170 /// XXX dubious term. here for backwards compatibility
3171 - v->getVehicleType().getMinGap();
3172 #ifdef DEBUG_CONTEXT
3173 if (DEBUG_COND2(ego)) {
3174 std::cout << " agap1=" << agap << "\n";
3175 }
3176 #endif
3177 if (agap > 0 && &v->getLane()->getEdge() != &ego->getLane()->getEdge()) {
3178 // Only if ego overlaps we treat v as if it were a real follower
3179 // Otherwise we ignore it and look for another follower
3180 v = firstFront[i];
3181 if (v != nullptr && v != ego) {
3182 agap = (*it).length - v->getPositionOnLane() + backOffset - v->getVehicleType().getMinGap();
3183 } else {
3184 v = nullptr;
3185 }
3186 }
3187 } else {
3188 agap = (*it).length - v->getPositionOnLane() + backOffset - v->getVehicleType().getMinGap();
3189 if (!(*it).viaLink->havePriority() && !ego->onFurtherEdge(&(*it).lane->getEdge())
3190 && ego->isOnRoad() // during insertion, this can lead to collisions because ego's further lanes are not set (see #3053)
3191 ) {
3192 // if v comes from a minor side road it should not block lane changing
3193 agap = MAX2(agap, 0.0);
3194 }
3195 }
3196 result.addFollower(v, ego, agap, 0, i);
3197 #ifdef DEBUG_CONTEXT
3198 if (DEBUG_COND2(ego)) {
3199 std::cout << " (2) added veh=" << Named::getIDSecure(v) << " agap=" << agap << " next=" << next->getID() << " result=" << result.toString() << "\n";
3200 }
3201 #endif
3202 }
3203 }
3204 if ((*it).length < searchDist) {
3205 const std::vector<MSLane::IncomingLaneInfo>& followers = next->getIncomingLanes();
3206 for (std::vector<MSLane::IncomingLaneInfo>::const_iterator j = followers.begin(); j != followers.end(); ++j) {
3207 if (visited.find((*j).lane) == visited.end() && ((*j).viaLink->havePriority() || !ignoreMinorLinks)) {
3208 visited.insert((*j).lane);
3209 MSLane::IncomingLaneInfo ili;
3210 ili.lane = (*j).lane;
3211 ili.length = (*j).length + (*it).length;
3212 ili.viaLink = (*j).viaLink;
3213 newFound.push_back(ili);
3214 }
3215 }
3216 }
3217 }
3218 toExamine.clear();
3219 swap(newFound, toExamine);
3220 }
3221 //return result;
3222
3223 }
3224 return result;
3225 }
3226
3227
3228 void
getLeadersOnConsecutive(double dist,double seen,double speed,const MSVehicle * ego,const std::vector<MSLane * > & bestLaneConts,MSLeaderDistanceInfo & result) const3229 MSLane::getLeadersOnConsecutive(double dist, double seen, double speed, const MSVehicle* ego,
3230 const std::vector<MSLane*>& bestLaneConts, MSLeaderDistanceInfo& result) const {
3231 if (seen > dist) {
3232 return;
3233 }
3234 // check partial vehicles (they might be on a different route and thus not
3235 // found when iterating along bestLaneConts)
3236 for (VehCont::const_iterator it = myPartialVehicles.begin(); it != myPartialVehicles.end(); ++it) {
3237 MSVehicle* veh = *it;
3238 if (!veh->isFrontOnLane(this)) {
3239 result.addLeader(veh, seen, veh->getLatOffset(this));
3240 } else {
3241 break;
3242 }
3243 }
3244 const MSLane* nextLane = this;
3245 int view = 1;
3246 SUMOTime arrivalTime = MSNet::getInstance()->getCurrentTimeStep() + TIME2STEPS(seen / MAX2(speed, NUMERICAL_EPS));
3247 // loop over following lanes
3248 while (seen < dist && result.numFreeSublanes() > 0) {
3249 // get the next link used
3250 MSLinkCont::const_iterator link = succLinkSec(*ego, view, *nextLane, bestLaneConts);
3251 if (nextLane->isLinkEnd(link) || !(*link)->opened(arrivalTime, speed, speed, ego->getVehicleType().getLength(),
3252 ego->getImpatience(), ego->getCarFollowModel().getMaxDecel(), 0, ego->getLateralPositionOnLane()) || (*link)->haveRed()) {
3253 break;
3254 }
3255 // check for link leaders
3256 const MSLink::LinkLeaders linkLeaders = (*link)->getLeaderInfo(ego, seen);
3257 if (linkLeaders.size() > 0) {
3258 const MSLink::LinkLeader ll = linkLeaders[0];
3259 if (ll.vehAndGap.first != 0 && ego->isLeader(*link, ll.vehAndGap.first)) {
3260 // add link leader to all sublanes and return
3261 for (int i = 0; i < result.numSublanes(); ++i) {
3262 MSVehicle* veh = ll.vehAndGap.first;
3263 #ifdef DEBUG_CONTEXT
3264 if (DEBUG_COND2(ego)) {
3265 std::cout << " linkleader=" << veh->getID() << " gap=" << ll.vehAndGap.second << "\n";
3266 }
3267 #endif
3268 result.addLeader(veh, ll.vehAndGap.second, 0);
3269 }
3270 return; ;
3271 } // XXX else, deal with pedestrians
3272 }
3273 bool nextInternal = (*link)->getViaLane() != nullptr;
3274 nextLane = (*link)->getViaLaneOrLane();
3275 if (nextLane == nullptr) {
3276 break;
3277 }
3278
3279 MSLeaderInfo leaders = nextLane->getLastVehicleInformation(nullptr, 0, 0, false);
3280 #ifdef DEBUG_CONTEXT
3281 if (DEBUG_COND2(ego)) {
3282 std::cout << SIMTIME << " getLeadersOnConsecutive lane=" << getID() << " nextLane=" << nextLane->getID() << " leaders=" << leaders.toString() << "\n";
3283 }
3284 #endif
3285 // @todo check alignment issues if the lane width changes
3286 const int iMax = MIN2(leaders.numSublanes(), result.numSublanes());
3287 for (int i = 0; i < iMax; ++i) {
3288 const MSVehicle* veh = leaders[i];
3289 if (veh != nullptr) {
3290 #ifdef DEBUG_CONTEXT
3291 if (DEBUG_COND2(ego)) std::cout << " lead=" << veh->getID()
3292 << " seen=" << seen
3293 << " minGap=" << ego->getVehicleType().getMinGap()
3294 << " backPos=" << veh->getBackPositionOnLane(nextLane)
3295 << " gap=" << seen - ego->getVehicleType().getMinGap() + veh->getBackPositionOnLane(nextLane)
3296 << "\n";
3297 #endif
3298 result.addLeader(veh, seen - ego->getVehicleType().getMinGap() + veh->getBackPositionOnLane(nextLane), 0, i);
3299 }
3300 }
3301
3302 if (nextLane->getVehicleMaxSpeed(ego) < speed) {
3303 dist = ego->getCarFollowModel().brakeGap(nextLane->getVehicleMaxSpeed(ego));
3304 }
3305 seen += nextLane->getLength();
3306 if (seen <= dist) {
3307 // delaying the update of arrivalTime and making it conditional to avoid possible integer overflows
3308 arrivalTime += TIME2STEPS(nextLane->getLength() / MAX2(speed, NUMERICAL_EPS));
3309 }
3310 if (!nextInternal) {
3311 view++;
3312 }
3313 }
3314 }
3315
3316
3317
3318 MSVehicle*
getPartialBehind(const MSVehicle * ego) const3319 MSLane::getPartialBehind(const MSVehicle* ego) const {
3320 for (VehCont::const_reverse_iterator i = myPartialVehicles.rbegin(); i != myPartialVehicles.rend(); ++i) {
3321 MSVehicle* veh = *i;
3322 if (veh->isFrontOnLane(this)
3323 && veh != ego
3324 && veh->getPositionOnLane() <= ego->getPositionOnLane()) {
3325 #ifdef DEBUG_CONTEXT
3326 if (DEBUG_COND2(ego)) {
3327 std::cout << SIMTIME << " getPartialBehind lane=" << getID() << " ego=" << ego->getID() << " found=" << veh->getID() << "\n";
3328 }
3329 #endif
3330 return veh;
3331 }
3332 }
3333 #ifdef DEBUG_CONTEXT
3334 if (DEBUG_COND2(ego)) {
3335 std::cout << SIMTIME << " getPartialBehind lane=" << getID() << " ego=" << ego->getID() << " nothing found. partials=" << toString(myPartialVehicles) << "\n";
3336 }
3337 #endif
3338 return nullptr;
3339 }
3340
3341 MSLeaderInfo
getPartialBeyond() const3342 MSLane::getPartialBeyond() const {
3343 MSLeaderInfo result(this);
3344 for (VehCont::const_iterator it = myPartialVehicles.begin(); it != myPartialVehicles.end(); ++it) {
3345 MSVehicle* veh = *it;
3346 if (!veh->isFrontOnLane(this)) {
3347 result.addLeader(veh, false, veh->getLatOffset(this));
3348 } else {
3349 break;
3350 }
3351 }
3352 return result;
3353 }
3354
3355
3356 std::set<MSVehicle*>
getSurroundingVehicles(double startPos,double downstreamDist,double upstreamDist,std::shared_ptr<LaneCoverageInfo> checkedLanes) const3357 MSLane::getSurroundingVehicles(double startPos, double downstreamDist, double upstreamDist, std::shared_ptr<LaneCoverageInfo> checkedLanes) const {
3358 assert(checkedLanes != nullptr);
3359 if (checkedLanes->find(this) != checkedLanes->end()) {
3360 #ifdef DEBUG_SURROUNDING
3361 std::cout << "Skipping previously scanned lane: " << getID() << std::endl;
3362 #endif
3363 return std::set<MSVehicle*>();
3364 } else {
3365 // Add this lane's coverage to the lane coverage info
3366 (*checkedLanes)[this] = std::make_pair(MAX2(0.0, startPos - upstreamDist), MIN2(startPos + downstreamDist, getLength()));
3367 }
3368 #ifdef DEBUG_SURROUNDING
3369 std::cout << "Scanning on lane " << myID << "(downstr. " << downstreamDist << ", upstr. " << upstreamDist << ", startPos " << startPos << "): " << std::endl;
3370 #endif
3371 std::set<MSVehicle*> foundVehicles = getVehiclesInRange(MAX2(0., startPos - upstreamDist), MIN2(myLength, startPos + downstreamDist));
3372 if (startPos < upstreamDist) {
3373 // scan incoming lanes
3374 for (const IncomingLaneInfo& incomingInfo : getIncomingLanes()) {
3375 MSLane* incoming = incomingInfo.lane;
3376 #ifdef DEBUG_SURROUNDING
3377 std::cout << "Checking on incoming: " << incoming->getID() << std::endl;
3378 if (checkedLanes->find(incoming) != checkedLanes->end()) {
3379 std::cout << "Skipping previous: " << incoming->getID() << std::endl;
3380 }
3381 #endif
3382 std::set<MSVehicle*> newVehs = incoming->getSurroundingVehicles(incoming->getLength(), 0.0, upstreamDist - startPos, checkedLanes);
3383 foundVehicles.insert(newVehs.begin(), newVehs.end());
3384 }
3385 }
3386
3387 if (getLength() < startPos + downstreamDist) {
3388 // scan successive lanes
3389 const MSLinkCont& lc = getLinkCont();
3390 for (MSLink* l : lc) {
3391 #ifdef DEBUG_SURROUNDING
3392 std::cout << "Checking on outgoing: " << l->getViaLaneOrLane()->getID() << std::endl;
3393 #endif
3394 std::set<MSVehicle*> newVehs = l->getViaLaneOrLane()->getSurroundingVehicles(0.0, downstreamDist - (myLength - startPos), upstreamDist, checkedLanes);
3395 foundVehicles.insert(newVehs.begin(), newVehs.end());
3396 }
3397 }
3398 #ifdef DEBUG_SURROUNDING
3399 std::cout << "On lane (2) " << myID << ": \nFound vehicles: " << std::endl;
3400 for (MSVehicle* v : foundVehicles) {
3401 std::cout << v->getID() << " pos = " << v->getPositionOnLane() << std::endl;
3402 }
3403 #endif
3404 return foundVehicles;
3405 }
3406
3407
3408 std::set<MSVehicle*>
getVehiclesInRange(const double a,const double b) const3409 MSLane::getVehiclesInRange(const double a, const double b) const {
3410 std::set<MSVehicle*> res;
3411 const VehCont& vehs = getVehiclesSecure();
3412
3413 if (!vehs.empty()) {
3414 for (MSVehicle* const veh : vehs) {
3415 if (veh->getPositionOnLane() >= a) {
3416 if (veh->getBackPositionOnLane() > b) {
3417 break;
3418 }
3419 res.insert(veh);
3420 }
3421 }
3422 }
3423 releaseVehicles();
3424 return res;
3425 }
3426
3427
3428 std::vector<const MSJunction*>
getUpcomingJunctions(double pos,double range,const std::vector<MSLane * > & contLanes) const3429 MSLane::getUpcomingJunctions(double pos, double range, const std::vector<MSLane*>& contLanes) const {
3430 // set of upcoming junctions and the corresponding conflict links
3431 std::vector<const MSJunction*> junctions;
3432 for (auto l : getUpcomingLinks(pos, range, contLanes)) {
3433 junctions.insert(junctions.end(), l->getJunction());
3434 }
3435 return junctions;
3436 }
3437
3438
3439 std::vector<const MSLink*>
getUpcomingLinks(double pos,double range,const std::vector<MSLane * > & contLanes) const3440 MSLane::getUpcomingLinks(double pos, double range, const std::vector<MSLane*>& contLanes) const {
3441 #ifdef DEBUG_SURROUNDING
3442 std::cout << "getUpcoming links on lane '" << getID() << "' with pos=" << pos
3443 << " range=" << range << std::endl;
3444 #endif
3445 // set of upcoming junctions and the corresponding conflict links
3446 std::vector<const MSLink*> links;
3447
3448 // Currently scanned lane
3449 const MSLane* lane = this;
3450
3451 // continuation lanes for the vehicle
3452 std::vector<MSLane*>::const_iterator contLanesIt = contLanes.begin();
3453 // scanned distance so far
3454 double dist = 0.0;
3455 // link to be crossed by the vehicle
3456 MSLink* link = nullptr;
3457 if (lane->isInternal()) {
3458 assert(*contLanesIt == nullptr); // is called with vehicle's bestLane structure
3459 link = lane->getEntryLink();
3460 links.insert(links.end(), link);
3461 dist += link->getInternalLengthsAfter();
3462 // next non-internal lane behind junction
3463 lane = link->getLane();
3464 pos = 0.0;
3465 assert(*(contLanesIt + 1) == lane);
3466 }
3467 while (++contLanesIt != contLanes.end()) {
3468 assert(!lane->isInternal());
3469 dist += lane->getLength() - pos;
3470 pos = 0.;
3471 #ifdef DEBUG_SURROUNDING
3472 std::cout << "Distance until end of lane '" << lane->getID() << "' is " << dist << "." << std::endl;
3473 #endif
3474 if (dist > range) {
3475 break;
3476 }
3477 link = lane->getLinkTo(*contLanesIt);
3478 if (link != nullptr) {
3479 links.insert(links.end(), link);
3480 }
3481 lane = *contLanesIt;
3482 }
3483 return links;
3484 }
3485
3486
3487 MSLane*
getOpposite() const3488 MSLane::getOpposite() const {
3489 if (myNeighs.size() == 1) {
3490 return dictionary(myNeighs[0]);
3491 }
3492 return nullptr;
3493 }
3494
3495
3496 double
getOppositePos(double pos) const3497 MSLane::getOppositePos(double pos) const {
3498 MSLane* opposite = getOpposite();
3499 if (opposite == nullptr) {
3500 assert(false);
3501 throw ProcessError("Lane '" + getID() + "' cannot compute oppositePos as there is no opposite lane.");
3502 }
3503 // XXX transformations for curved geometries
3504 return MAX2(0., opposite->getLength() - pos);
3505
3506 }
3507
3508 std::pair<MSVehicle* const, double>
getFollower(const MSVehicle * ego,double egoPos,double dist,bool ignoreMinorLinks) const3509 MSLane::getFollower(const MSVehicle* ego, double egoPos, double dist, bool ignoreMinorLinks) const {
3510 for (AnyVehicleIterator first = anyVehiclesUpstreamBegin(); first != anyVehiclesUpstreamEnd(); ++first) {
3511 // XXX refactor leaderInfo to use a const vehicle all the way through the call hierarchy
3512 MSVehicle* pred = (MSVehicle*)*first;
3513 #ifdef DEBUG_CONTEXT
3514 if (DEBUG_COND2(ego)) {
3515 std::cout << " getFollower lane=" << getID() << " egoPos=" << egoPos << " pred=" << pred->getID() << " predPos=" << pred->getPositionOnLane(this) << "\n";
3516 }
3517 #endif
3518 if (pred->getPositionOnLane(this) < egoPos && pred != ego) {
3519 return std::pair<MSVehicle* const, double>(pred, egoPos - pred->getPositionOnLane(this) - ego->getVehicleType().getLength() - pred->getVehicleType().getMinGap());
3520 }
3521 }
3522 const double backOffset = egoPos - ego->getVehicleType().getLength();
3523 CLeaderDist result = getFollowersOnConsecutive(ego, backOffset, true, dist, ignoreMinorLinks)[0];
3524 return std::make_pair(const_cast<MSVehicle*>(result.first), result.second);
3525 }
3526
3527 std::pair<MSVehicle* const, double>
getOppositeLeader(const MSVehicle * ego,double dist,bool oppositeDir) const3528 MSLane::getOppositeLeader(const MSVehicle* ego, double dist, bool oppositeDir) const {
3529 #ifdef DEBUG_OPPOSITE
3530 if (DEBUG_COND2(ego)) std::cout << SIMTIME << " getOppositeLeader lane=" << getID()
3531 << " ego=" << ego->getID()
3532 << " pos=" << ego->getPositionOnLane()
3533 << " posOnOpposite=" << getOppositePos(ego->getPositionOnLane())
3534 << " dist=" << dist
3535 << " oppositeDir=" << oppositeDir
3536 << "\n";
3537 #endif
3538 if (!oppositeDir) {
3539 return getLeader(ego, getOppositePos(ego->getPositionOnLane()), ego->getBestLanesContinuation(this));
3540 } else {
3541 const double egoLength = ego->getVehicleType().getLength();
3542 const double egoPos = ego->getLaneChangeModel().isOpposite() ? ego->getPositionOnLane() : getOppositePos(ego->getPositionOnLane());
3543 std::pair<MSVehicle* const, double> result = getFollower(ego, egoPos + egoLength, dist, true);
3544 result.second -= ego->getVehicleType().getMinGap();
3545 return result;
3546 }
3547 }
3548
3549
3550 std::pair<MSVehicle* const, double>
getOppositeFollower(const MSVehicle * ego) const3551 MSLane::getOppositeFollower(const MSVehicle* ego) const {
3552 #ifdef DEBUG_OPPOSITE
3553 if (DEBUG_COND2(ego)) std::cout << SIMTIME << " getOppositeFollower lane=" << getID()
3554 << " ego=" << ego->getID()
3555 << " backPos=" << ego->getBackPositionOnLane()
3556 << " posOnOpposite=" << getOppositePos(ego->getBackPositionOnLane())
3557 << "\n";
3558 #endif
3559 if (ego->getLaneChangeModel().isOpposite()) {
3560 std::pair<MSVehicle* const, double> result = getFollower(ego, getOppositePos(ego->getPositionOnLane()), -1, true);
3561 return result;
3562 } else {
3563 std::pair<MSVehicle* const, double> result = getLeader(ego, getOppositePos(ego->getPositionOnLane() - ego->getVehicleType().getLength()), std::vector<MSLane*>());
3564 if (result.first != nullptr) {
3565 if (result.first->getLaneChangeModel().isOpposite()) {
3566 result.second -= result.first->getVehicleType().getLength();
3567 } else {
3568 if (result.second > POSITION_EPS) {
3569 // follower can be safely ignored since it is going the other way
3570 return std::make_pair(static_cast<MSVehicle*>(nullptr), -1);
3571 }
3572 }
3573 }
3574 return result;
3575 }
3576 }
3577
3578
3579 void
initCollisionOptions(const OptionsCont & oc)3580 MSLane::initCollisionOptions(const OptionsCont& oc) {
3581 const std::string action = oc.getString("collision.action");
3582 if (action == "none") {
3583 myCollisionAction = COLLISION_ACTION_NONE;
3584 } else if (action == "warn") {
3585 myCollisionAction = COLLISION_ACTION_WARN;
3586 } else if (action == "teleport") {
3587 myCollisionAction = COLLISION_ACTION_TELEPORT;
3588 } else if (action == "remove") {
3589 myCollisionAction = COLLISION_ACTION_REMOVE;
3590 } else {
3591 WRITE_ERROR("Invalid collision.action '" + action + "'.");
3592 }
3593 myCheckJunctionCollisions = oc.getBool("collision.check-junctions");
3594 myCollisionStopTime = string2time(oc.getString("collision.stoptime"));
3595 myCollisionMinGapFactor = oc.getFloat("collision.mingap-factor");
3596 }
3597
3598
3599 void
setPermissions(SVCPermissions permissions,long long transientID)3600 MSLane::setPermissions(SVCPermissions permissions, long long transientID) {
3601 if (transientID == CHANGE_PERMISSIONS_PERMANENT) {
3602 myPermissions = permissions;
3603 myOriginalPermissions = permissions;
3604 } else {
3605 myPermissionChanges[transientID] = permissions;
3606 resetPermissions(CHANGE_PERMISSIONS_PERMANENT);
3607 }
3608 }
3609
3610
3611 void
resetPermissions(long long transientID)3612 MSLane::resetPermissions(long long transientID) {
3613 myPermissionChanges.erase(transientID);
3614 if (myPermissionChanges.empty()) {
3615 myPermissions = myOriginalPermissions;
3616 } else {
3617 // combine all permission changes
3618 myPermissions = SVCAll;
3619 for (const auto& item : myPermissionChanges) {
3620 myPermissions &= item.second;
3621 }
3622 }
3623 }
3624
3625
3626 bool
checkForPedestrians(const MSVehicle * aVehicle,double & speed,double & dist,double pos,bool patchSpeed) const3627 MSLane::checkForPedestrians(const MSVehicle* aVehicle, double& speed, double& dist, double pos, bool patchSpeed) const {
3628 if (getEdge().getPersons().size() > 0 && MSPModel::getModel()->hasPedestrians(this)) {
3629 #ifdef DEBUG_INSERTION
3630 if (DEBUG_COND2(aVehicle)) {
3631 std::cout << SIMTIME << " check for pedestrians on lane=" << getID() << " pos=" << pos << "\n";
3632 }
3633 #endif
3634 PersonDist leader = MSPModel::getModel()->nextBlocking(this, pos - aVehicle->getVehicleType().getLength(),
3635 aVehicle->getRightSideOnLane(), aVehicle->getRightSideOnLane() + aVehicle->getVehicleType().getWidth(), ceil(speed / aVehicle->getCarFollowModel().getMaxDecel()));
3636 if (leader.first != 0) {
3637 const double gap = leader.second - aVehicle->getVehicleType().getLengthWithGap();
3638 const double stopSpeed = aVehicle->getCarFollowModel().stopSpeed(aVehicle, speed, gap);
3639 if (gap < 0 || checkFailure(aVehicle, speed, dist, stopSpeed, patchSpeed, "")) {
3640 // we may not drive with the given velocity - we crash into the pedestrian
3641 #ifdef DEBUG_INSERTION
3642 if (DEBUG_COND2(aVehicle)) std::cout << SIMTIME
3643 << " isInsertionSuccess lane=" << getID()
3644 << " veh=" << aVehicle->getID()
3645 << " pos=" << pos
3646 << " posLat=" << aVehicle->getLateralPositionOnLane()
3647 << " patchSpeed=" << patchSpeed
3648 << " speed=" << speed
3649 << " stopSpeed=" << stopSpeed
3650 << " pedestrianLeader=" << leader.first->getID()
3651 << " failed (@796)!\n";
3652 #endif
3653 return false;
3654 }
3655 }
3656 }
3657 return true;
3658 }
3659
3660 void
initRNGs(const OptionsCont & oc)3661 MSLane::initRNGs(const OptionsCont& oc) {
3662 myRNGs.clear();
3663 const int numRNGs = oc.getInt("thread-rngs");
3664 const bool random = oc.getBool("random");
3665 int seed = oc.getInt("seed");
3666 for (int i = 0; i < numRNGs; i++) {
3667 myRNGs.push_back(std::mt19937());
3668 RandHelper::initRand(&myRNGs.back(), random, seed++);
3669 }
3670 }
3671
3672 MSLane*
getBidiLane() const3673 MSLane::getBidiLane() const {
3674 const MSEdge* bidiEdge = myEdge->getBidiEdge();
3675 if (bidiEdge == nullptr) {
3676 return nullptr;
3677 } else {
3678 /// XXX multi-lane edges are not considered
3679 assert(bidiEdge->getLanes().size() == 1);
3680 return bidiEdge->getLanes()[0];
3681 }
3682 }
3683
3684 bool
mustCheckJunctionCollisions() const3685 MSLane::mustCheckJunctionCollisions() const {
3686 return myCheckJunctionCollisions && myEdge->isInternal() && myLinks.front()->getFoeLanes().size() > 0;
3687 }
3688 /****************************************************************************/
3689
3690