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 MSLink.cpp
11 /// @author Daniel Krajzewicz
12 /// @author Jakob Erdmann
13 /// @author Michael Behrisch
14 /// @author Laura Bieker
15 /// @date Sept 2002
16 /// @version $Id$
17 ///
18 // A connnection between lanes
19 /****************************************************************************/
20
21 // ===========================================================================
22 // included modules
23 // ===========================================================================
24 #include <config.h>
25
26 #include <iostream>
27 #include <algorithm>
28 #include <limits>
29 #include <utils/iodevices/OutputDevice.h>
30 #include "MSNet.h"
31 #include "MSJunction.h"
32 #include "MSLink.h"
33 #include "MSLane.h"
34 #include <microsim/pedestrians/MSPerson.h>
35 #include "MSEdge.h"
36 #include "MSGlobals.h"
37 #include "MSVehicle.h"
38 #include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
39 #include <microsim/pedestrians/MSPModel.h>
40
41 //#define MSLink_DEBUG_CROSSING_POINTS
42 //#define MSLink_DEBUG_OPENED
43 //#define DEBUG_APPROACHING
44 //#define DEBUG_ZIPPER
45 //#define DEBUG_COND (myLane->getID()=="43[0]_0" && myLaneBefore->getID()==":33_0_0")
46 //#define DEBUG_COND (myLane->getID()=="end_0")
47 //#define DEBUG_COND (true)
48 //#define DEBUG_COND_ZIPPER (gDebugFlag1)
49 //#define DEBUG_COND_ZIPPER (true)
50 #define DEBUG_COND_ZIPPER (ego->isSelected())
51
52 // ===========================================================================
53 // static member variables
54 // ===========================================================================
55 const SUMOTime MSLink::myLookaheadTime = TIME2STEPS(1);
56 // additional caution is needed when approaching a zipper link
57 const SUMOTime MSLink::myLookaheadTimeZipper = TIME2STEPS(4);
58
59 const double MSLink::ZIPPER_ADAPT_DIST(100);
60
61 // time to link in seconds below which adaptation should take place
62 #define ZIPPER_ADAPT_TIME 10
63 // the default safety gap when passing before oncoming pedestrians
64 #define JM_CROSSING_GAP_DEFAULT 10
65
66 // minimim width between sibling lanes to qualify as non-overlapping
67 #define DIVERGENCE_MIN_WIDTH 2.5
68
69 // ===========================================================================
70 // member method definitions
71 // ===========================================================================
MSLink(MSLane * predLane,MSLane * succLane,MSLane * via,LinkDirection dir,LinkState state,double length,double foeVisibilityDistance,bool keepClear,MSTrafficLightLogic * logic,int tlIndex)72 MSLink::MSLink(MSLane* predLane, MSLane* succLane, MSLane* via, LinkDirection dir, LinkState state, double length, double foeVisibilityDistance, bool keepClear, MSTrafficLightLogic* logic, int tlIndex) :
73 myLane(succLane),
74 myLaneBefore(predLane),
75 myIndex(-1),
76 myTLIndex(tlIndex),
77 myLogic(logic),
78 myState(state),
79 myOffState(state),
80 myLastStateChange(SUMOTime_MIN / 2), // a large negative value, but avoid overflows when subtracting
81 myDirection(dir),
82 myLength(length),
83 myFoeVisibilityDistance(foeVisibilityDistance),
84 myHasFoes(false),
85 myAmCont(false),
86 myAmContOff(false),
87 myKeepClear(keepClear),
88 myInternalLane(via),
89 myInternalLaneBefore(nullptr),
90 myMesoTLSPenalty(0),
91 myGreenFraction(1),
92 myLateralShift(0),
93 myWalkingAreaFoe(nullptr),
94 myWalkingAreaFoeExit(nullptr),
95 myHavePedestrianCrossingFoe(false),
96 myParallelRight(nullptr),
97 myParallelLeft(nullptr),
98 myJunction(nullptr) {
99
100 if (MSGlobals::gLateralResolution > 0) {
101 // detect lateral shift from lane geometries
102 //std::cout << "DEBUG link=" << myLaneBefore->getID() << "->" << getViaLaneOrLane()->getID() << " hasInternal=" << MSNet::getInstance()->hasInternalLinks() << " shapeBefore=" << myLaneBefore->getShape().back() << " shapeFront=" << getViaLaneOrLane()->getShape().front() << "\n";
103 if ((myInternalLane != nullptr || predLane->isInternal())
104 && myLaneBefore->getShape().back() != getViaLaneOrLane()->getShape().front()) {
105 PositionVector from = myLaneBefore->getShape();
106 const PositionVector& to = getViaLaneOrLane()->getShape();
107 const double dist = from.back().distanceTo2D(to.front());
108 // figure out direction of shift
109 try {
110 from.move2side(dist);
111 } catch (InvalidArgument&) {
112 }
113 myLateralShift = (from.back().distanceTo2D(to.front()) < dist) ? dist : -dist;
114 //std::cout << " lateral shift link=" << myLaneBefore->getID() << "->" << getViaLaneOrLane()->getID() << " dist=" << dist << " shift=" << myLateralShift << "\n";
115 }
116 }
117 }
118
119
~MSLink()120 MSLink::~MSLink() {}
121
122
123 void
setRequestInformation(int index,bool hasFoes,bool isCont,const std::vector<MSLink * > & foeLinks,const std::vector<MSLane * > & foeLanes,MSLane * internalLaneBefore)124 MSLink::setRequestInformation(int index, bool hasFoes, bool isCont,
125 const std::vector<MSLink*>& foeLinks,
126 const std::vector<MSLane*>& foeLanes,
127 MSLane* internalLaneBefore) {
128 //#ifdef MSLink_DEBUG_CROSSING_POINTS
129 // std::cout << " setRequestInformation() for junction " << getViaLaneOrLane()->getEdge().getFromJunction()->getID()
130 // << "\nInternalLanes = " << toString(getViaLaneOrLane()->getEdge().getFromJunction()->getInternalLanes())
131 // << std::endl;
132 //#endif
133 myIndex = index;
134 myHasFoes = hasFoes;
135 myAmCont = isCont;
136 myFoeLinks = foeLinks;
137 for (std::vector<MSLane*>::const_iterator it_lane = foeLanes.begin(); it_lane != foeLanes.end(); ++it_lane) {
138 // cannot assign vector due to const-ness
139 myFoeLanes.push_back(*it_lane);
140 }
141 myJunction = const_cast<MSJunction*>(myLane->getEdge().getFromJunction()); // junctionGraph is initialized after the whole network is loaded
142 myAmContOff = isCont && myLogic != nullptr && internalLaneBefore == nullptr && checkContOff();
143 myInternalLaneBefore = internalLaneBefore;
144 MSLane* lane = nullptr;
145 if (internalLaneBefore != nullptr) {
146 // this is an exit link. compute crossing points with all foeLanes
147 lane = internalLaneBefore;
148 //} else if (myLane->getEdge().isCrossing()) {
149 // // this is the link to a pedestrian crossing. compute crossing points with all foeLanes
150 // // @note not currently used by pedestrians
151 // lane = myLane;
152 }
153 #ifdef MSLink_DEBUG_CROSSING_POINTS
154 std::cout << " link " << myIndex << " to " << getViaLaneOrLane()->getID() << " internalLaneBefore=" << (lane == 0 ? "NULL" : lane->getID()) << " has foes: " << toString(foeLanes) << "\n";
155 #endif
156 if (lane != nullptr) {
157 const bool beforeInternalJunction = lane->getLinkCont()[0]->getViaLaneOrLane()->getEdge().isInternal();
158 if (lane->getIncomingLanes().size() != 1) {
159 throw ProcessError("Internal lane '" + lane->getID() + "' has " + toString(lane->getIncomingLanes().size()) + " predecessors");
160 }
161 // compute crossing points
162 for (std::vector<const MSLane*>::const_iterator it_lane = myFoeLanes.begin(); it_lane != myFoeLanes.end(); ++it_lane) {
163 myHavePedestrianCrossingFoe = myHavePedestrianCrossingFoe || (*it_lane)->getEdge().isCrossing();
164 const bool sameTarget = myLane == (*it_lane)->getLinkCont()[0]->getLane();
165 if (sameTarget && !beforeInternalJunction && !contIntersect(lane, *it_lane)) {
166 //if (myLane == (*it_lane)->getLinkCont()[0]->getLane()) {
167 // this foeLane has the same target and merges at the end (lane exits the junction)
168 myLengthsBehindCrossing.push_back(std::make_pair(0, 0)); // dummy value, never used
169 #ifdef MSLink_DEBUG_CROSSING_POINTS
170 std::cout
171 << " " << lane->getID()
172 << " merges with " << (*it_lane)->getID()
173 << " nextLane " << lane->getLinkCont()[0]->getViaLaneOrLane()->getID()
174 << " dist1=" << myLengthsBehindCrossing.back().first
175 << " dist2=" << myLengthsBehindCrossing.back().second
176 << "\n";
177 #endif
178 } else {
179 std::vector<double> intersections1 = lane->getShape().intersectsAtLengths2D((*it_lane)->getShape());
180 #ifdef MSLink_DEBUG_CROSSING_POINTS
181 // std::cout << " intersections1=" << toString(intersections1) << "\n";
182 #endif
183 bool haveIntersection = true;
184 if (intersections1.size() == 0) {
185 intersections1.push_back(-10000.0); // disregard this foe (using maxdouble leads to nasty problems down the line)
186 haveIntersection = false;
187 } else if (intersections1.size() > 1) {
188 std::sort(intersections1.begin(), intersections1.end());
189 }
190 std::vector<double> intersections2 = (*it_lane)->getShape().intersectsAtLengths2D(lane->getShape());
191 #ifdef MSLink_DEBUG_CROSSING_POINTS
192 //std::cout << " intersections2=" << toString(intersections2) << "\n";
193 #endif
194 if (intersections2.size() == 0) {
195 intersections2.push_back(0);
196 } else if (intersections2.size() > 1) {
197 std::sort(intersections2.begin(), intersections2.end());
198 }
199 if (haveIntersection) {
200 // lane width affects the crossing point
201 intersections1.back() -= (*it_lane)->getWidth() / 2;
202 intersections2.back() -= lane->getWidth() / 2;
203 // ensure negative offset for weird geometries
204 intersections1.back() = MAX2(0.0, intersections1.back());
205 intersections2.back() = MAX2(0.0, intersections2.back());
206
207 // also length/geometry factor. (XXX: Why subtract width/2 *before* converting geometric position to lane pos? refs #3031)
208 intersections1.back() = lane->interpolateGeometryPosToLanePos(intersections1.back());
209 intersections2.back() = (*it_lane)->interpolateGeometryPosToLanePos(intersections2.back());
210
211 if (internalLaneBefore->getLogicalPredecessorLane()->getEdge().isInternal() && !(*it_lane)->getEdge().isCrossing()) {
212 // wait at the internal junction
213 // (except for foes that are crossings since there is no internal junction)
214 intersections1.back() = 0;
215 }
216 }
217
218 myLengthsBehindCrossing.push_back(std::make_pair(
219 lane->getLength() - intersections1.back(),
220 (*it_lane)->getLength() - intersections2.back()));
221
222 #ifdef MSLink_DEBUG_CROSSING_POINTS
223 std::cout
224 << " intersection of " << lane->getID()
225 << " totalLength=" << lane->getLength()
226 << " with " << (*it_lane)->getID()
227 << " totalLength=" << (*it_lane)->getLength()
228 << " dist1=" << myLengthsBehindCrossing.back().first
229 << " dist2=" << myLengthsBehindCrossing.back().second
230 << "\n";
231 #endif
232 }
233 }
234 // check for overlap with internal lanes from the same source lane
235 const MSLane* pred = lane->getLogicalPredecessorLane();
236 // to avoid overlap with vehicles that came from pred (especially when pred has endOffset > 0)
237 // we add all other internal lanes from pred as foeLanes
238 for (const MSLink* const it : pred->getLinkCont()) {
239 const MSLane* sibling = it->getViaLane();
240 if (sibling != lane && sibling != nullptr) {
241 const double minDist = MIN2(DIVERGENCE_MIN_WIDTH, 0.5 * (lane->getWidth() + sibling->getWidth()));
242 const PositionVector& l = lane->getShape();
243 const PositionVector& s = sibling->getShape();
244 if (l.front().distanceTo2D(s.front()) >= minDist) {
245 // account for lateral shift by the entry links
246 continue;
247 }
248 double lbcSibling = 0;
249 double lbcLane = 0;
250 if (l.back().distanceTo2D(s.back()) > minDist) {
251 // compute the final divergence point
252 // this position serves two purposes:
253 // 1) once the foe vehicle back (on sibling) has passed this point, we can safely ignore it
254 // 2) both vehicles are put into a cf-relationship while before the point.
255 // Since the actual crossing point is at the start of the junction,
256 // we want to make sure that both vehicles have the same distance to the crossing point and thus follow each other naturally
257 std::vector<double> distances = l.distances(s);
258 #ifdef MSLink_DEBUG_CROSSING_POINTS
259 std::cout << " distances=" << toString(distances) << "\n";
260 #endif
261 assert(distances.size() == l.size() + s.size());
262 if (distances.back() > minDist && distances[l.size() - 1] > minDist) {
263 // do a pairwise check between lane and sibling to make because we do not know which of them bends more
264 for (int j = (int)s.size() - 2; j >= 0; j--) {
265 const int i = j + (int)l.size();
266 const double segLength = s[j].distanceTo2D(s[j + 1]);
267 if (distances[i] > minDist) {
268 lbcSibling += segLength;
269 } else {
270 // assume no sharp bends and just interpolate the last segment
271 lbcSibling += segLength - (minDist - distances[i]) * segLength / (distances[i + 1] - distances[i]);
272 break;
273 }
274 }
275 for (int i = (int)l.size() - 2; i >= 0; i--) {
276 const double segLength = l[i].distanceTo2D(l[i + 1]);
277 if (distances[i] > minDist) {
278 lbcLane += segLength;
279 } else {
280 // assume no sharp bends and just interpolate the last segment
281 lbcLane += segLength - (minDist - distances[i]) * segLength / (distances[i + 1] - distances[i]);
282 break;
283 }
284 }
285 }
286 assert(lbcSibling >= -NUMERICAL_EPS);
287 assert(lbcLane >= -NUMERICAL_EPS);
288 }
289 const double distToDivergence1 = sibling->getLength() - lbcSibling;
290 const double distToDivergence2 = lane->getLength() - lbcLane;
291 const double distToDivergence = MIN3(
292 MAX2(distToDivergence1, distToDivergence2),
293 sibling->getLength(), lane->getLength());
294 lbcLane = MAX2(0.0, lane->getLength() - distToDivergence);
295 lbcLane = lane->interpolateGeometryPosToLanePos(lbcLane);
296 lbcSibling = MAX2(0.0, sibling->getLength() - distToDivergence);
297 lbcSibling = lane->interpolateGeometryPosToLanePos(lbcSibling);
298 myLengthsBehindCrossing.push_back(std::make_pair(lbcLane, lbcSibling));
299 myFoeLanes.push_back(sibling);
300 #ifdef MSLink_DEBUG_CROSSING_POINTS
301 std::cout << " distToDivergence=" << distToDivergence
302 << " distTD1=" << distToDivergence1
303 << " distTD2=" << distToDivergence2
304 << " length=" << lane->getLength()
305 << " sibLength=" << sibling->getLength()
306 << "\n";
307 std::cout << " adding same-origin foe" << sibling->getID()
308 << " dist1=" << myLengthsBehindCrossing.back().first
309 << " dist2=" << myLengthsBehindCrossing.back().second
310 << "\n";
311 #endif
312 }
313 }
314 }
315 if (MSGlobals::gLateralResolution > 0) {
316 // check for links with the same origin lane and the same destination edge
317 const MSEdge* myTarget = &myLane->getEdge();
318 // save foes for entry links
319 for (MSLink* const it : myLaneBefore->getLinkCont()) {
320 const MSEdge* target = &(it->getLane()->getEdge());
321 if (it == this) {
322 continue;
323 }
324 if (target == myTarget) {
325 mySublaneFoeLinks.push_back(it);
326 } else if (myDirection != LINKDIR_STRAIGHT && it->getDirection() == LINKDIR_STRAIGHT) {
327 // potential turn conflicht
328 mySublaneFoeLinks2.push_back(it);
329 }
330 }
331 // save foes for exit links
332 if (fromInternalLane()) {
333 //std::cout << " setRequestInformation link=" << getViaLaneOrLane()->getID() << " before=" << myLaneBefore->getID() << " before2=" << myLaneBefore->getIncomingLanes().front().lane->getID() << "\n";
334 const MSLinkCont& predLinks2 = myLaneBefore->getIncomingLanes().front().lane->getLinkCont();
335 for (MSLinkCont::const_iterator it = predLinks2.begin(); it != predLinks2.end(); ++it) {
336 const MSEdge* target = &((*it)->getLane()->getEdge());
337 if ((*it)->getViaLane() != myInternalLaneBefore && target == myTarget) {
338 //std::cout << " add sublaneFoe=" << (*it)->getViaLane()->getID() << "\n";
339 mySublaneFoeLanes.push_back((*it)->getViaLane());
340 }
341 }
342 }
343 }
344 }
345
346
347 bool
contIntersect(const MSLane * lane,const MSLane * foe)348 MSLink::contIntersect(const MSLane* lane, const MSLane* foe) {
349 if (foe->getLinkCont()[0]->getViaLane() != nullptr) {
350 std::vector<double> intersections = lane->getShape().intersectsAtLengths2D(foe->getShape());
351 return intersections.size() > 0;
352 }
353 return false;
354 }
355
356
357 void
setApproaching(const SUMOVehicle * approaching,const SUMOTime arrivalTime,const double arrivalSpeed,const double leaveSpeed,const bool setRequest,const SUMOTime arrivalTimeBraking,const double arrivalSpeedBraking,const SUMOTime waitingTime,double dist)358 MSLink::setApproaching(const SUMOVehicle* approaching, const SUMOTime arrivalTime, const double arrivalSpeed, const double leaveSpeed,
359 const bool setRequest, const SUMOTime arrivalTimeBraking, const double arrivalSpeedBraking, const SUMOTime waitingTime, double dist) {
360 const SUMOTime leaveTime = getLeaveTime(arrivalTime, arrivalSpeed, leaveSpeed, approaching->getVehicleType().getLength());
361 #ifdef DEBUG_APPROACHING
362 if (DEBUG_COND) {
363 std::cout << SIMTIME << " Link ''" << (myLaneBefore == 0 ? "NULL" : myLaneBefore->getID()) << "'->'" << (myLane == 0 ? "NULL" : myLane->getID()) << "' Adding approaching vehicle '" << approaching->getID() << "'\nCurrently registered vehicles:" << std::endl;
364 for (auto i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
365 std::cout << "'" << i->first->getID() << "'" << std::endl;
366 }
367 }
368 #endif
369 myApproachingVehicles.emplace(approaching,
370 ApproachingVehicleInformation(arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, setRequest,
371 arrivalTimeBraking, arrivalSpeedBraking, waitingTime, dist));
372 }
373
374
375 void
setApproaching(const SUMOVehicle * approaching,ApproachingVehicleInformation ai)376 MSLink::setApproaching(const SUMOVehicle* approaching, ApproachingVehicleInformation ai) {
377
378 #ifdef DEBUG_APPROACHING
379 if (DEBUG_COND) {
380 std::cout << SIMTIME << " Link ''" << (myLaneBefore == 0 ? "NULL" : myLaneBefore->getID()) << "'->'" << (myLane == 0 ? "NULL" : myLane->getID()) << "' Adding approaching vehicle '" << approaching->getID() << "'\nCurrently registered vehicles:" << std::endl;
381 for (auto i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
382 std::cout << "'" << i->first->getID() << "'" << std::endl;
383 }
384 }
385 #endif
386 myApproachingVehicles.emplace(approaching, ai);
387 }
388
389
390 void
addBlockedLink(MSLink * link)391 MSLink::addBlockedLink(MSLink* link) {
392 myBlockedFoeLinks.insert(link);
393 }
394
395
396
397 bool
willHaveBlockedFoe() const398 MSLink::willHaveBlockedFoe() const {
399 for (std::set<MSLink*>::const_iterator i = myBlockedFoeLinks.begin(); i != myBlockedFoeLinks.end(); ++i) {
400 if ((*i)->isBlockingAnyone()) {
401 return true;
402 }
403 }
404 return false;
405 }
406
407
408 void
removeApproaching(const SUMOVehicle * veh)409 MSLink::removeApproaching(const SUMOVehicle* veh) {
410
411 #ifdef DEBUG_APPROACHING
412 if (DEBUG_COND) {
413 std::cout << SIMTIME << " Link ''" << (myLaneBefore == 0 ? "NULL" : myLaneBefore->getID()) << "'->'" << (myLane == 0 ? "NULL" : myLane->getID()) << std::endl;
414 std::cout << "' Removing approaching vehicle '" << veh->getID() << "'\nCurrently registered vehicles:" << std::endl;
415 for (auto i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
416 std::cout << "'" << i->first->getID() << "'" << std::endl;
417 }
418 }
419 #endif
420 myApproachingVehicles.erase(veh);
421 }
422
423
424 MSLink::ApproachingVehicleInformation
getApproaching(const SUMOVehicle * veh) const425 MSLink::getApproaching(const SUMOVehicle* veh) const {
426 auto i = myApproachingVehicles.find(veh);
427 if (i != myApproachingVehicles.end()) {
428 return i->second;
429 } else {
430 return ApproachingVehicleInformation(-1000, -1000, 0, 0, false, -1000, 0, 0, 0);
431 }
432 }
433
434
435 SUMOTime
getLeaveTime(const SUMOTime arrivalTime,const double arrivalSpeed,const double leaveSpeed,const double vehicleLength) const436 MSLink::getLeaveTime(const SUMOTime arrivalTime, const double arrivalSpeed,
437 const double leaveSpeed, const double vehicleLength) const {
438 return arrivalTime + TIME2STEPS((getLength() + vehicleLength) / MAX2(0.5 * (arrivalSpeed + leaveSpeed), NUMERICAL_EPS));
439 }
440
441
442 bool
opened(SUMOTime arrivalTime,double arrivalSpeed,double leaveSpeed,double vehicleLength,double impatience,double decel,SUMOTime waitingTime,double posLat,std::vector<const SUMOVehicle * > * collectFoes,bool ignoreRed,const SUMOVehicle * ego) const443 MSLink::opened(SUMOTime arrivalTime, double arrivalSpeed, double leaveSpeed, double vehicleLength,
444 double impatience, double decel, SUMOTime waitingTime, double posLat,
445 std::vector<const SUMOVehicle*>* collectFoes, bool ignoreRed, const SUMOVehicle* ego) const {
446 if (haveRed() && !ignoreRed) {
447 return false;
448 }
449 if (isCont() && MSGlobals::gUsingInternalLanes) {
450 return true;
451 }
452 const SUMOTime leaveTime = getLeaveTime(arrivalTime, arrivalSpeed, leaveSpeed, vehicleLength);
453 if (MSGlobals::gLateralResolution > 0) {
454 // check for foes on the same lane with the same target edge
455 for (const MSLink* foeLink : mySublaneFoeLinks) {
456 assert(myLane != foeLink->getLane());
457 for (auto& it : foeLink->myApproachingVehicles) {
458 const SUMOVehicle* foe = it.first;
459 if (
460 // there only is a conflict if the paths cross
461 ((posLat < foe->getLateralPositionOnLane() && myLane->getIndex() > foeLink->myLane->getIndex())
462 || (posLat > foe->getLateralPositionOnLane() && myLane->getIndex() < foeLink->myLane->getIndex()))
463 // the vehicle that arrives later must yield
464 && (arrivalTime > it.second.arrivalTime
465 // if both vehicles arrive at the same time, the one
466 // to the left must yield
467 || (arrivalTime == it.second.arrivalTime && posLat > foe->getLateralPositionOnLane()))) {
468 if (blockedByFoe(foe, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, false,
469 impatience, decel, waitingTime, ego)) {
470 #ifdef MSLink_DEBUG_OPENED
471 if (gDebugFlag1) {
472 std::cout << SIMTIME << " blocked by " << foe->getID() << " arrival=" << arrivalTime << " foeArrival=" << it.second.arrivalTime << "\n";
473 }
474 #endif
475 if (collectFoes == nullptr) {
476 #ifdef MSLink_DEBUG_OPENED
477 if (gDebugFlag1) {
478 std::cout << " link=" << getViaLaneOrLane()->getID() << " blocked by sublaneFoe=" << foe->getID() << " foeLink=" << foeLink->getViaLaneOrLane()->getID() << " posLat=" << posLat << "\n";
479 }
480 #endif
481 return false;
482 } else {
483 collectFoes->push_back(foe);
484 }
485 }
486 }
487 }
488 }
489 // check for foes on the same lane with a different target edge
490 // (straight movers take precedence if the paths cross)
491 for (const MSLink* foeLink : mySublaneFoeLinks2) {
492 assert(myDirection != LINKDIR_STRAIGHT);
493 for (auto& it : foeLink->myApproachingVehicles) {
494 const SUMOVehicle* foe = it.first;
495 // there only is a conflict if the paths cross
496 if (((myDirection == LINKDIR_RIGHT || myDirection == LINKDIR_PARTRIGHT)
497 && (posLat > foe->getLateralPositionOnLane()))
498 || ((myDirection == LINKDIR_LEFT || myDirection == LINKDIR_PARTLEFT)
499 && (posLat < foe->getLateralPositionOnLane()))) {
500 if (blockedByFoe(foe, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, false,
501 impatience, decel, waitingTime, ego)) {
502 #ifdef MSLink_DEBUG_OPENED
503 if (gDebugFlag1) {
504 std::cout << SIMTIME << " blocked by sublane foe " << foe->getID() << " arrival=" << arrivalTime << " foeArrival=" << it.second.arrivalTime << "\n";
505 }
506 #endif
507 if (collectFoes == nullptr) {
508 return false;
509 } else {
510 collectFoes->push_back(foe);
511 }
512 }
513 }
514 }
515 }
516 }
517 if (havePriority() && myState != LINKSTATE_ZIPPER) {
518 // priority usually means the link is open but there are exceptions:
519 // zipper still needs to collect foes
520 // sublane model could have detected a conflict
521 return collectFoes == nullptr || collectFoes->size() == 0;
522 }
523 if ((myState == LINKSTATE_STOP || myState == LINKSTATE_ALLWAY_STOP) && waitingTime == 0) {
524 return false;
525 }
526
527 #ifdef MSLink_DEBUG_OPENED
528 if (gDebugFlag1) {
529 std::cout << SIMTIME << " opened link=" << getViaLaneOrLane()->getID() << " foeLinks=" << myFoeLinks.size() << "\n";
530 }
531 #endif
532
533 for (std::vector<MSLink*>::const_iterator i = myFoeLinks.begin(); i != myFoeLinks.end(); ++i) {
534 if (MSGlobals::gUseMesoSim) {
535 if ((*i)->haveRed()) {
536 continue;
537 }
538 }
539 #ifdef MSLink_DEBUG_OPENED
540 if (gDebugFlag1) {
541 std::cout << " foeLink=" << (*i)->getViaLaneOrLane()->getID() << " numApproaching=" << (*i)->getApproaching().size() << "\n";
542 }
543 #endif
544 if ((*i)->blockedAtTime(arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, myLane == (*i)->getLane(),
545 impatience, decel, waitingTime, collectFoes, ego)) {
546 return false;
547 }
548 }
549 if (collectFoes != nullptr && collectFoes->size() > 0) {
550 return false;
551 }
552 return true;
553 }
554
555
556 bool
blockedAtTime(SUMOTime arrivalTime,SUMOTime leaveTime,double arrivalSpeed,double leaveSpeed,bool sameTargetLane,double impatience,double decel,SUMOTime waitingTime,std::vector<const SUMOVehicle * > * collectFoes,const SUMOVehicle * ego) const557 MSLink::blockedAtTime(SUMOTime arrivalTime, SUMOTime leaveTime, double arrivalSpeed, double leaveSpeed,
558 bool sameTargetLane, double impatience, double decel, SUMOTime waitingTime,
559 std::vector<const SUMOVehicle*>* collectFoes, const SUMOVehicle* ego) const {
560 for (auto it : myApproachingVehicles) {
561 #ifdef MSLink_DEBUG_OPENED
562 if (gDebugFlag1) {
563 if (ego != 0
564 && ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_SPEED, 0) >= it.first->getSpeed()
565 && ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) > 0) {
566 std::cout << " foe link=" << getViaLaneOrLane()->getID()
567 << " foeVeh=" << it.first->getID() << " (below ignore speed)"
568 << " ignoreFoeProb=" << ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0)
569 << "\n";
570 }
571 }
572 #endif
573 if (it.first != ego
574 && (ego == nullptr
575 || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) == 0
576 || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_SPEED, 0) < it.first->getSpeed()
577 || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) < RandHelper::rand(ego->getRNG()))
578 && blockedByFoe(it.first, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, sameTargetLane,
579 impatience, decel, waitingTime, ego)) {
580 if (collectFoes == nullptr) {
581 return true;
582 } else {
583 collectFoes->push_back(it.first);
584 }
585 }
586 }
587 return false;
588 }
589
590
591 bool
blockedByFoe(const SUMOVehicle * veh,const ApproachingVehicleInformation & avi,SUMOTime arrivalTime,SUMOTime leaveTime,double arrivalSpeed,double leaveSpeed,bool sameTargetLane,double impatience,double decel,SUMOTime waitingTime,const SUMOVehicle * ego) const592 MSLink::blockedByFoe(const SUMOVehicle* veh, const ApproachingVehicleInformation& avi,
593 SUMOTime arrivalTime, SUMOTime leaveTime, double arrivalSpeed, double leaveSpeed,
594 bool sameTargetLane, double impatience, double decel, SUMOTime waitingTime,
595 const SUMOVehicle* ego) const {
596 #ifdef MSLink_DEBUG_OPENED
597 if (gDebugFlag1) {
598 std::cout << " foe link=" << getViaLaneOrLane()->getID()
599 << " foeVeh=" << veh->getID()
600 << " req=" << avi.willPass
601 << " aT=" << avi.arrivalTime
602 << " lT=" << avi.leavingTime
603 << "\n";
604 }
605 #endif
606 if (!avi.willPass) {
607 return false;
608 }
609 if (myState == LINKSTATE_ALLWAY_STOP) {
610 assert(waitingTime > 0);
611 if (waitingTime > avi.waitingTime) {
612 return false;
613 }
614 if (waitingTime == avi.waitingTime && arrivalTime < avi.arrivalTime) {
615 return false;
616 }
617 }
618 const SUMOTime foeArrivalTime = (SUMOTime)((1.0 - impatience) * avi.arrivalTime + impatience * avi.arrivalTimeBraking);
619 const SUMOTime lookAhead = (myState == LINKSTATE_ZIPPER
620 ? myLookaheadTimeZipper
621 : (ego == nullptr
622 ? myLookaheadTime
623 : TIME2STEPS(ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_TIMEGAP_MINOR, STEPS2TIME(myLookaheadTime)))));
624 //if (ego != 0) std::cout << SIMTIME << " ego=" << ego->getID() << " jmTimegapMinor=" << ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_TIMEGAP_MINOR, -1) << " lookAhead=" << lookAhead << "\n";
625 #ifdef MSLink_DEBUG_OPENED
626 if (gDebugFlag1) {
627 std::cout << " imp=" << impatience << " fATb=" << avi.arrivalTimeBraking << " fAT2=" << foeArrivalTime << " lA=" << lookAhead << " egoAT=" << arrivalTime << " egoLT=" << leaveTime << "\n";
628 }
629 #endif
630 if (avi.leavingTime < arrivalTime) {
631 // ego wants to be follower
632 if (sameTargetLane && (arrivalTime - avi.leavingTime < lookAhead
633 || unsafeMergeSpeeds(avi.leaveSpeed, arrivalSpeed,
634 veh->getVehicleType().getCarFollowModel().getMaxDecel(), decel))) {
635 #ifdef MSLink_DEBUG_OPENED
636 if (gDebugFlag1) {
637 std::cout << " blocked (cannot follow)\n";
638 }
639 #endif
640 return true;
641 }
642 } else if (foeArrivalTime > leaveTime + lookAhead) {
643 // ego wants to be leader.
644 if (sameTargetLane && unsafeMergeSpeeds(leaveSpeed, avi.arrivalSpeedBraking,
645 decel, veh->getVehicleType().getCarFollowModel().getMaxDecel())) {
646 #ifdef MSLink_DEBUG_OPENED
647 if (gDebugFlag1) {
648 std::cout << " blocked (cannot lead)\n";
649 }
650 #endif
651 return true;
652 }
653 } else {
654 // even without considering safeHeadwayTime there is already a conflict
655 #ifdef MSLink_DEBUG_OPENED
656 if (gDebugFlag1) {
657 std::cout << " blocked (hard conflict)\n";
658 }
659 #endif
660 return true;
661 }
662 return false;
663 }
664
665
666 bool
maybeOccupied(MSLane * lane)667 MSLink::maybeOccupied(MSLane* lane) {
668 MSVehicle* veh = lane->getLastAnyVehicle();
669 double distLeft = 0;
670 if (veh == nullptr) {
671 return false;
672 } else {
673 distLeft = lane->getLength() - veh->getBackPositionOnLane(lane);
674 assert(distLeft > 0);
675 // can we be sure that the vehicle leaves this lane in the next step?
676 bool result = distLeft > (veh->getSpeed() - veh->getCarFollowModel().getMaxDecel());
677 return result;
678 }
679 }
680
681
682 bool
hasApproachingFoe(SUMOTime arrivalTime,SUMOTime leaveTime,double speed,double decel) const683 MSLink::hasApproachingFoe(SUMOTime arrivalTime, SUMOTime leaveTime, double speed, double decel) const {
684 for (std::vector<MSLink*>::const_iterator i = myFoeLinks.begin(); i != myFoeLinks.end(); ++i) {
685 if ((*i)->blockedAtTime(arrivalTime, leaveTime, speed, speed, myLane == (*i)->getLane(), 0, decel, 0)) {
686 return true;
687 }
688 }
689 for (std::vector<const MSLane*>::const_iterator i = myFoeLanes.begin(); i != myFoeLanes.end(); ++i) {
690 if ((*i)->getVehicleNumberWithPartials() > 0) {
691 return true;
692 }
693 }
694 return false;
695 }
696
697
698 LinkDirection
getDirection() const699 MSLink::getDirection() const {
700 return myDirection;
701 }
702
703
704 void
setTLState(LinkState state,SUMOTime t)705 MSLink::setTLState(LinkState state, SUMOTime t) {
706 if (myState != state) {
707 myLastStateChange = t;
708 }
709 myState = state;
710 }
711
712
713 MSLane*
getLane() const714 MSLink::getLane() const {
715 return myLane;
716 }
717
718
719 bool
isCont() const720 MSLink::isCont() const {
721 // when a traffic light is switched off minor roads have their cont status revoked
722 return myState != LINKSTATE_TL_OFF_BLINKING ? myAmCont : myAmContOff;
723 }
724
725
726 bool
lastWasContMajor() const727 MSLink::lastWasContMajor() const {
728 if (myInternalLane == nullptr || myAmCont || myHavePedestrianCrossingFoe) {
729 return false;
730 } else {
731 MSLane* pred = myInternalLane->getLogicalPredecessorLane();
732 if (!pred->getEdge().isInternal()) {
733 return false;
734 } else {
735 MSLane* pred2 = pred->getLogicalPredecessorLane();
736 assert(pred2 != 0);
737 MSLink* predLink = MSLinkContHelper::getConnectingLink(*pred2, *pred);
738 assert(predLink != 0);
739 return predLink->havePriority() || predLink->haveYellow();
740 }
741 }
742 }
743
744
745 void
writeApproaching(OutputDevice & od,const std::string fromLaneID) const746 MSLink::writeApproaching(OutputDevice& od, const std::string fromLaneID) const {
747 if (myApproachingVehicles.size() > 0) {
748 od.openTag("link");
749 od.writeAttr(SUMO_ATTR_FROM, fromLaneID);
750 const std::string via = getViaLane() == nullptr ? "" : getViaLane()->getID();
751 od.writeAttr(SUMO_ATTR_VIA, via);
752 od.writeAttr(SUMO_ATTR_TO, getLane() == nullptr ? "" : getLane()->getID());
753 std::vector<std::pair<SUMOTime, const SUMOVehicle*> > toSort; // stabilize output
754 for (auto it : myApproachingVehicles) {
755 toSort.push_back(std::make_pair(it.second.arrivalTime, it.first));
756 }
757 std::sort(toSort.begin(), toSort.end());
758 for (std::vector<std::pair<SUMOTime, const SUMOVehicle*> >::const_iterator it = toSort.begin(); it != toSort.end(); ++it) {
759 od.openTag("approaching");
760 const ApproachingVehicleInformation& avi = myApproachingVehicles.find(it->second)->second;
761 od.writeAttr(SUMO_ATTR_ID, it->second->getID());
762 od.writeAttr(SUMO_ATTR_IMPATIENCE, it->second->getImpatience());
763 od.writeAttr("arrivalTime", time2string(avi.arrivalTime));
764 od.writeAttr("arrivalTimeBraking", time2string(avi.arrivalTimeBraking));
765 od.writeAttr("leaveTime", time2string(avi.leavingTime));
766 od.writeAttr("arrivalSpeed", toString(avi.arrivalSpeed));
767 od.writeAttr("arrivalSpeedBraking", toString(avi.arrivalSpeedBraking));
768 od.writeAttr("leaveSpeed", toString(avi.leaveSpeed));
769 od.writeAttr("willPass", toString(avi.willPass));
770 od.closeTag();
771 }
772 od.closeTag();
773 }
774 }
775
776
777 double
getInternalLengthsAfter() const778 MSLink::getInternalLengthsAfter() const {
779 double len = 0.;
780 MSLane* lane = myInternalLane;
781
782 while (lane != nullptr && lane->isInternal()) {
783 len += lane->getLength();
784 lane = lane->getLinkCont()[0]->getViaLane();
785 }
786 return len;
787 }
788
789 double
getInternalLengthsBefore() const790 MSLink::getInternalLengthsBefore() const {
791 double len = 0.;
792 const MSLane* lane = myInternalLane;
793
794 while (lane != nullptr && lane->isInternal()) {
795 len += lane->getLength();
796 if (lane->getIncomingLanes().size() == 1) {
797 lane = lane->getIncomingLanes()[0].lane;
798 } else {
799 break;
800 }
801 }
802 return len;
803 }
804
805
806 double
getLengthsBeforeCrossing(const MSLane * foeLane) const807 MSLink::getLengthsBeforeCrossing(const MSLane* foeLane) const {
808 MSLane* via = myInternalLane;
809 double totalDist = 0.;
810 bool foundCrossing = false;
811 while (via != nullptr) {
812 MSLink* link = via->getLinkCont()[0];
813 double dist = link->getLengthBeforeCrossing(foeLane);
814 if (dist != INVALID_DOUBLE) {
815 // found conflicting lane
816 totalDist += dist;
817 foundCrossing = true;
818 break;
819 } else {
820 totalDist += via->getLength();
821 via = link->getViaLane();
822 }
823 }
824 if (foundCrossing) {
825 return totalDist;
826 } else {
827 return INVALID_DOUBLE;
828 }
829 }
830
831
832 double
getLengthBeforeCrossing(const MSLane * foeLane) const833 MSLink::getLengthBeforeCrossing(const MSLane* foeLane) const {
834 int foe_ix;
835 for (foe_ix = 0; foe_ix != (int)myFoeLanes.size(); ++foe_ix) {
836 if (myFoeLanes[foe_ix] == foeLane) {
837 break;
838 }
839 }
840 if (foe_ix == (int)myFoeLanes.size()) {
841 // no conflict with the given lane, indicate by returning -1
842 #ifdef MSLink_DEBUG_CROSSING_POINTS
843 std::cout << "No crossing of lanes '" << foeLane->getID() << "' and '" << myInternalLaneBefore->getID() << "'" << std::endl;
844 #endif
845 return INVALID_DOUBLE;
846 } else {
847 // found conflicting lane index
848 double dist = myInternalLaneBefore->getLength() - myLengthsBehindCrossing[foe_ix].first;
849 if (dist == -10000.) {
850 // this is the value in myLengthsBehindCrossing, if the relation allows intersection but none is present for the actual geometry.
851 return INVALID_DOUBLE;
852 }
853 #ifdef MSLink_DEBUG_CROSSING_POINTS
854 std::cout << "Crossing of lanes '" << myInternalLaneBefore->getID() << "' and '" << foeLane->getID()
855 << "' at distance " << dist << " (approach along '"
856 << myInternalLaneBefore->getEntryLink()->getLaneBefore()->getID() << "')" << std::endl;
857 #endif
858 return dist;
859 }
860 }
861
862
863 MSLane*
getViaLane() const864 MSLink::getViaLane() const {
865 return myInternalLane;
866 }
867
868
869 bool
isEntryLink() const870 MSLink::isEntryLink() const {
871 if (MSGlobals::gUsingInternalLanes) {
872 return myInternalLane != nullptr && myInternalLaneBefore == nullptr;
873 } else {
874 return false;
875 }
876 }
877
878 bool
isConflictEntryLink() const879 MSLink::isConflictEntryLink() const {
880 // either a non-cont entry link or the link after a cont-link
881 return !myAmCont && (isEntryLink() || (myInternalLaneBefore != nullptr && myInternalLane != nullptr));
882 }
883
884 bool
isExitLink() const885 MSLink::isExitLink() const {
886 if (MSGlobals::gUsingInternalLanes) {
887 return myInternalLaneBefore != nullptr && myInternalLane == nullptr;
888 } else {
889 return false;
890 }
891 }
892
893 bool
isExitLinkAfterInternalJunction() const894 MSLink::isExitLinkAfterInternalJunction() const {
895 if (MSGlobals::gUsingInternalLanes) {
896 return (getInternalLaneBefore() != nullptr
897 && myInternalLaneBefore->getIncomingLanes().size() == 1
898 && myInternalLaneBefore->getIncomingLanes().front().viaLink->isInternalJunctionLink());
899 } else {
900 return false;
901 }
902 }
903
904
905 MSLink*
getCorrespondingExitLink() const906 MSLink::getCorrespondingExitLink() const {
907 MSLane* lane = myInternalLane;
908 MSLink* link = nullptr;
909 while (lane != nullptr) {
910 link = lane->getLinkCont()[0];
911 lane = link->getViaLane();
912 }
913 return link;
914 }
915
916
917 bool
isInternalJunctionLink() const918 MSLink::isInternalJunctionLink() const {
919 return getInternalLaneBefore() != nullptr && myInternalLane != nullptr;
920 }
921
922 bool
fromInternalLane() const923 MSLink::fromInternalLane() const {
924 return myInternalLaneBefore != nullptr;
925 }
926
927 MSLink::LinkLeaders
getLeaderInfo(const MSVehicle * ego,double dist,std::vector<const MSPerson * > * collectBlockers,bool isShadowLink) const928 MSLink::getLeaderInfo(const MSVehicle* ego, double dist, std::vector<const MSPerson*>* collectBlockers, bool isShadowLink) const {
929 LinkLeaders result;
930 if (ego != nullptr && ego->getLaneChangeModel().isOpposite()) {
931 // ignore link leaders
932 return result;
933 }
934 //gDebugFlag1 = true;
935 // this link needs to start at an internal lane (either an exit link or between two internal lanes)
936 // or it must be queried by the pedestrian model (ego == 0)
937 if (fromInternalLane() || ego == nullptr) {
938 if (gDebugFlag1) {
939 std::cout << SIMTIME << " getLeaderInfo link=" << getViaLaneOrLane()->getID() << " dist=" << dist << " isShadowLink=" << isShadowLink << "\n";
940 }
941 // this is an exit link
942 for (int i = 0; i < (int)myFoeLanes.size(); ++i) {
943 const MSLane* foeLane = myFoeLanes[i];
944 // distance from the querying vehicle to the crossing point with foeLane
945 double distToCrossing = dist - myLengthsBehindCrossing[i].first;
946 const bool sameTarget = (myLane == foeLane->getLinkCont()[0]->getLane()) && !isInternalJunctionLink();
947 const bool sameSource = (myInternalLaneBefore != nullptr && myInternalLaneBefore->getLogicalPredecessorLane() == foeLane->getLogicalPredecessorLane());
948 const double crossingWidth = (sameTarget || sameSource) ? 0 : foeLane->getWidth();
949 const double foeCrossingWidth = (sameTarget || sameSource) ? 0 : myInternalLaneBefore->getWidth();
950 if (gDebugFlag1) {
951 std::cout << " distToCrossing=" << distToCrossing << " foeLane=" << foeLane->getID() << " cWidth=" << crossingWidth
952 << " ijl=" << isInternalJunctionLink() << " sT=" << sameTarget << " sS=" << sameSource
953 << " lbc=" << myLengthsBehindCrossing[i].first
954 << " flbc=" << myLengthsBehindCrossing[i].second
955 << "\n";
956 }
957 // special treatment of contLane foe only applies if this lane is not a contLane or contLane follower itself
958 const bool contLane = (foeLane->getLinkCont()[0]->getViaLaneOrLane()->getEdge().isInternal() && !(
959 isInternalJunctionLink() || isExitLinkAfterInternalJunction()));
960 if (distToCrossing + crossingWidth < 0
961 && (ego == nullptr || !MSGlobals::gComputeLC || distToCrossing + crossingWidth + ego->getVehicleType().getLength() < 0)) {
962 continue; // vehicle is behind the crossing point, continue with next foe lane
963 }
964 const double foeDistToCrossing = foeLane->getLength() - myLengthsBehindCrossing[i].second;
965 // it is not sufficient to return the last vehicle on the foeLane because ego might be its leader
966 // therefore we return all vehicles on the lane
967 //
968 // special care must be taken for continuation lanes. (next lane is also internal)
969 // vehicles on these lanes should always block (gap = -1)
970 // vehicles on cont. lanes or on internal lanes with the same target as this link can never be ignored
971 MSLane::AnyVehicleIterator end = foeLane->anyVehiclesEnd();
972 for (MSLane::AnyVehicleIterator it_veh = foeLane->anyVehiclesBegin(); it_veh != end; ++it_veh) {
973 MSVehicle* leader = (MSVehicle*)*it_veh;
974 const double leaderBack = leader->getBackPositionOnLane(foeLane);
975 const double leaderBackDist = foeDistToCrossing - leaderBack;
976 const bool pastTheCrossingPoint = leaderBackDist + foeCrossingWidth < 0;
977 const bool ignoreIndirectBicycleTurn = (pastTheCrossingPoint
978 && leader->getVehicleType().getVehicleClass() == SVC_BICYCLE
979 && foeLane->getIncomingLanes().front().viaLink->getDirection() == LINKDIR_LEFT);
980 const bool cannotIgnore = ((contLane && !ignoreIndirectBicycleTurn) || sameTarget || sameSource) && ego != nullptr;
981 const bool inTheWay = !pastTheCrossingPoint && leaderBackDist < leader->getVehicleType().getLength();
982 const bool isOpposite = leader->getLaneChangeModel().isOpposite();
983 if (gDebugFlag1) {
984 std::cout << " candiate leader=" << leader->getID()
985 << " cannotIgnore=" << cannotIgnore
986 << " fdtc=" << foeDistToCrossing
987 << " lb=" << leaderBack
988 << " lbd=" << leaderBackDist
989 << " fcwidth=" << foeCrossingWidth
990 << " foePastCP=" << pastTheCrossingPoint
991 << " inTheWay=" << inTheWay
992 << " willPass=" << foeLane->getLinkCont()[0]->getApproaching(leader).willPass
993 << " isFrontOnLane=" << leader->isFrontOnLane(foeLane)
994 << " isOpposite=" << isOpposite << "\n";
995 }
996 if (leader == ego) {
997 continue;
998 }
999 // after entering the conflict area, ignore foe vehicles that are not in the way
1000 if (distToCrossing < -POSITION_EPS && !inTheWay
1001 && (ego == nullptr || !MSGlobals::gComputeLC || distToCrossing < -ego->getVehicleType().getLength())) {
1002 continue;
1003 }
1004 // ignore foe vehicles that will not pass
1005 if ((!cannotIgnore || leader->isStopped() || sameTarget)
1006 && !foeLane->getLinkCont()[0]->getApproaching(leader).willPass
1007 && leader->isFrontOnLane(foeLane)
1008 && !isOpposite
1009 && !inTheWay
1010 // willPass is false if the vehicle is already on the stopping edge
1011 && !leader->willStop()) {
1012 continue;
1013 }
1014 if (cannotIgnore || inTheWay || leader->getWaitingTime() < MSGlobals::gIgnoreJunctionBlocker) {
1015 // compute distance between vehicles on the the superimposition of both lanes
1016 // where the crossing point is the common point
1017 double gap;
1018 bool fromLeft = true;
1019 if (ego == nullptr) {
1020 // request from pedestrian model. return distance between leaderBack and crossing point
1021 //std::cout << " foeLane=" << foeLane->getID() << " leaderBack=" << leaderBack << " foeDistToCrossing=" << foeDistToCrossing << " foeLength=" << foeLane->getLength() << " foebehind=" << myLengthsBehindCrossing[i].second << " dist=" << dist << " behind=" << myLengthsBehindCrossing[i].first << "\n";
1022 gap = leaderBackDist;
1023 // distToCrossing should not take into account the with of the foe lane
1024 // (which was subtracted in setRequestInformation)
1025 // Instead, the width of the foe vehicle is used directly by the caller.
1026 distToCrossing += foeLane->getWidth() / 2;
1027 if (gap + foeCrossingWidth < 0) {
1028 // leader is completely past the crossing point
1029 // or there is no crossing point
1030 continue; // next vehicle
1031 }
1032 // we need to determine whether the vehicle passes the
1033 // crossing from the left or the right (heuristic)
1034 fromLeft = foeDistToCrossing > 0.5 * foeLane->getLength();
1035 } else if ((contLane && !sameSource && !ignoreIndirectBicycleTurn) || isOpposite) {
1036 gap = -1; // always break for vehicles which are on a continuation lane or for opposite-direction vehicles
1037 } else {
1038 if (gDebugFlag1) {
1039 std::cout << " distToCrossing=" << distToCrossing << " leader back=" << leaderBack << " backDist=" << leaderBackDist
1040 << " blockedStrategic=" << leader->getLaneChangeModel().isStrategicBlocked()
1041 //<< " stateRight=" << toString((LaneChangeAction)leader->getLaneChangeModel().getSavedState(-1).second)
1042 << "\n";
1043 }
1044 if (leaderBackDist + foeCrossingWidth < 0) {
1045 // leader is completely past the crossing point
1046 // or there is no crossing point
1047 continue; // next vehicle
1048 }
1049 gap = distToCrossing - ego->getVehicleType().getMinGap() - leaderBackDist - foeCrossingWidth;
1050 if (gap < leader->getVehicleType().getLength() && leader->getLaneChangeModel().isStrategicBlocked()) {
1051 // do not encroach on leader when it tries to change lanes
1052 gap = -1;
1053 }
1054 }
1055 // if the foe is already moving off the intersection, we may
1056 // advance up to the crossing point unless we have the same target or same source
1057 // (for sameSource, the crossing point indicates the point of divergence)
1058 const bool stopAsap = leader->isFrontOnLane(foeLane) ? cannotIgnore : (sameTarget || sameSource);
1059 if (gDebugFlag1) {
1060 std::cout << " leader=" << leader->getID() << " contLane=" << contLane << " cannotIgnore=" << cannotIgnore << " stopAsap=" << stopAsap << "\n";
1061 }
1062 result.push_back(LinkLeader(leader, gap, stopAsap ? -1 : distToCrossing, fromLeft));
1063 }
1064
1065 }
1066 if (ego != nullptr && MSNet::getInstance()->hasPersons()) {
1067 // check for crossing pedestrians (keep driving if already on top of the crossing
1068 const double distToPeds = distToCrossing - MSPModel::SAFETY_GAP;
1069 const double vehWidth = ego->getVehicleType().getWidth() + MSPModel::SAFETY_GAP; // + configurable safety gap
1070 /// @todo consider lateral position (depending on whether the crossing is encountered on the way in or out)
1071 // @check lefthand?!
1072 const bool wayIn = myLengthsBehindCrossing[i].first < myLaneBefore->getLength() * 0.5;
1073 const double vehSideOffset = (foeDistToCrossing + myLaneBefore->getWidth() * 0.5 - vehWidth * 0.5
1074 + ego->getLateralPositionOnLane() * (wayIn ? -1 : 1));
1075 if (distToPeds >= -MSPModel::SAFETY_GAP && MSPModel::getModel()->blockedAtDist(foeLane, vehSideOffset, vehWidth,
1076 ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_CROSSING_GAP, JM_CROSSING_GAP_DEFAULT),
1077 collectBlockers)) {
1078 result.push_back(LinkLeader((MSVehicle*)nullptr, -1, distToPeds));
1079 }
1080 }
1081 }
1082
1083 //std::cout << SIMTIME << " ego=" << Named::getIDSecure(ego) << " link=" << getViaLaneOrLane()->getID() << " myWalkingAreaFoe=" << Named::getIDSecure(myWalkingAreaFoe) << "\n";
1084 if (ego != nullptr) {
1085 checkWalkingAreaFoe(ego, myWalkingAreaFoe, collectBlockers, result);
1086 checkWalkingAreaFoe(ego, myWalkingAreaFoeExit, collectBlockers, result);
1087 }
1088
1089 if (MSGlobals::gLateralResolution > 0 && ego != nullptr && !isShadowLink) {
1090 // check for foes on the same lane
1091 for (std::vector<MSLane*>::const_iterator it = mySublaneFoeLanes.begin(); it != mySublaneFoeLanes.end(); ++it) {
1092 const MSLane* foeLane = *it;
1093 MSLane::AnyVehicleIterator end = foeLane->anyVehiclesEnd();
1094 for (MSLane::AnyVehicleIterator it_veh = foeLane->anyVehiclesBegin(); it_veh != end; ++it_veh) {
1095 MSVehicle* leader = (MSVehicle*)*it_veh;
1096 if (leader == ego) {
1097 continue;
1098 }
1099 const double maxLength = MAX2(myInternalLaneBefore->getLength(), foeLane->getLength());
1100 const double gap = dist - maxLength - ego->getVehicleType().getMinGap() + leader->getBackPositionOnLane(foeLane);
1101 if (gap < -(ego->getVehicleType().getMinGap() + leader->getLength())) {
1102 // ego is ahead of leader
1103 continue;
1104 }
1105
1106 const double posLat = ego->getLateralPositionOnLane();
1107 const double posLatLeader = leader->getLateralPositionOnLane() + leader->getLatOffset(foeLane);
1108 if (gDebugFlag1) {
1109 std::cout << " sublaneFoe lane=" << myInternalLaneBefore->getID()
1110 << " foeLane=" << foeLane->getID()
1111 << " leader=" << leader->getID()
1112 << " egoLane=" << ego->getLane()->getID()
1113 << " leaderLane=" << leader->getLane()->getID()
1114 << " egoLat=" << posLat
1115 << " leaderLat=" << posLatLeader
1116 << " leaderLatOffset=" << leader->getLatOffset(foeLane)
1117 << " egoIndex=" << myInternalLaneBefore->getIndex()
1118 << " foeIndex=" << foeLane->getIndex()
1119 << " dist=" << dist
1120 << " leaderBack=" << leader->getBackPositionOnLane(foeLane)
1121 << "\n";
1122 }
1123 // there only is a conflict if the paths cross
1124 if ((posLat < posLatLeader && myInternalLaneBefore->getIndex() > foeLane->getIndex())
1125 || (posLat > posLatLeader && myInternalLaneBefore->getIndex() < foeLane->getIndex())) {
1126 if (gDebugFlag1) {
1127 std::cout << SIMTIME << " blocked by " << leader->getID() << " (sublane split) foeLane=" << foeLane->getID() << "\n";
1128 }
1129 result.push_back(LinkLeader(leader, gap, -1));
1130 }
1131 }
1132 }
1133 }
1134 }
1135 return result;
1136 }
1137
1138
1139 void
checkWalkingAreaFoe(const MSVehicle * ego,const MSLane * foeLane,std::vector<const MSPerson * > * collectBlockers,LinkLeaders & result) const1140 MSLink::checkWalkingAreaFoe(const MSVehicle* ego, const MSLane* foeLane, std::vector<const MSPerson*>* collectBlockers, LinkLeaders& result) const {
1141 if (foeLane != nullptr && foeLane->getEdge().getPersons().size() > 0) {
1142 // pedestrians may be on an arbitrary path across this
1143 // walkingarea. make sure to keep enough distance.
1144 // This is a simple but conservative solution that could be improved
1145 // by ignoring pedestrians that are "obviously" not on a collision course
1146 double distToPeds = std::numeric_limits<double>::max();
1147 for (MSTransportable* t : foeLane->getEdge().getPersons()) {
1148 MSPerson* p = static_cast<MSPerson*>(t);
1149 const double dist = ego->getPosition().distanceTo2D(p->getPosition()) - p->getVehicleType().getLength();
1150 if (p->getSpeed() > 0 || dist < MSPModel::SAFETY_GAP / 2) {
1151 distToPeds = MIN2(distToPeds, dist - MSPModel::SAFETY_GAP);
1152 if (collectBlockers != nullptr) {
1153 collectBlockers->push_back(p);
1154 }
1155 }
1156 }
1157 result.push_back(LinkLeader((MSVehicle*)nullptr, -1, distToPeds));
1158 }
1159 }
1160
1161
1162 MSLane*
getViaLaneOrLane() const1163 MSLink::getViaLaneOrLane() const {
1164 if (myInternalLane != nullptr) {
1165 return myInternalLane;
1166 }
1167 return myLane;
1168 }
1169
1170
1171 const MSLane*
getLaneBefore() const1172 MSLink::getLaneBefore() const {
1173 if (myInternalLaneBefore != nullptr) {
1174 if (myLaneBefore != myInternalLaneBefore) {
1175 throw ProcessError("lane before mismatch!");
1176 }
1177 }
1178 return myLaneBefore;
1179 }
1180
1181
1182 MSLink*
getParallelLink(int direction) const1183 MSLink::getParallelLink(int direction) const {
1184 if (direction == -1) {
1185 return myParallelRight;
1186 } else if (direction == 1) {
1187 return myParallelLeft;
1188 } else {
1189 assert(false);
1190 return nullptr;
1191 }
1192 }
1193
1194
1195 MSLink*
computeParallelLink(int direction)1196 MSLink::computeParallelLink(int direction) {
1197 MSLane* before = getLaneBefore()->getParallelLane(direction);
1198 MSLane* after = getLane()->getParallelLane(direction);
1199 if (before != nullptr && after != nullptr) {
1200 return MSLinkContHelper::getConnectingLink(*before, *after);
1201 } else {
1202 return nullptr;
1203 }
1204 }
1205
1206
1207 const MSLane*
getInternalLaneBefore() const1208 MSLink::getInternalLaneBefore() const {
1209 return myInternalLaneBefore;
1210 }
1211
1212
1213 double
getZipperSpeed(const MSVehicle * ego,const double dist,double vSafe,SUMOTime arrivalTime,std::vector<const SUMOVehicle * > * collectFoes) const1214 MSLink::getZipperSpeed(const MSVehicle* ego, const double dist, double vSafe,
1215 SUMOTime arrivalTime,
1216 std::vector<const SUMOVehicle*>* collectFoes) const {
1217 if (myFoeLinks.size() == 0) {
1218 // link should have LINKSTATE_MAJOR in this case
1219 assert(false);
1220 return vSafe;
1221 } else if (myFoeLinks.size() > 1) {
1222 throw ProcessError("Zipper junctions with more than two conflicting lanes are not supported (at junction '"
1223 + myJunction->getID() + "')");
1224 }
1225 const SUMOTime now = MSNet::getInstance()->getCurrentTimeStep();
1226 const double secondsToArrival = STEPS2TIME(arrivalTime - now);
1227 if (secondsToArrival > ZIPPER_ADAPT_TIME && dist > ZIPPER_ADAPT_DIST) {
1228 #ifdef DEBUG_ZIPPER
1229 if (DEBUG_COND_ZIPPER) std::cout << SIMTIME << " getZipperSpeed ego=" << ego->getID()
1230 << " dist=" << dist << " ignoring foes (arrival in " << STEPS2TIME(arrivalTime - now) << ")\n";
1231 #endif
1232 return vSafe;
1233 }
1234 #ifdef DEBUG_ZIPPER
1235 if (DEBUG_COND_ZIPPER) std::cout << SIMTIME << " getZipperSpeed ego=" << ego->getID()
1236 << " egoAT=" << arrivalTime
1237 << " dist=" << dist
1238 << " vSafe=" << vSafe
1239 << " numFoes=" << collectFoes->size()
1240 << "\n";
1241 #endif
1242 MSLink* foeLink = myFoeLinks[0];
1243 for (std::vector<const SUMOVehicle*>::const_iterator i = collectFoes->begin(); i != collectFoes->end(); ++i) {
1244 const MSVehicle* foe = dynamic_cast<const MSVehicle*>(*i);
1245 assert(foe != 0);
1246 const ApproachingVehicleInformation& avi = foeLink->getApproaching(foe);
1247 if ( // ignore vehicles that arrive after us (unless they are ahead and we could easily brake for them)
1248 ((avi.arrivalTime > arrivalTime) && !couldBrakeForLeader(dist, avi.dist, ego, foe)) ||
1249 // also ignore vehicles that are behind us and are able to brake for us
1250 couldBrakeForLeader(avi.dist, dist, foe, ego) ||
1251 // resolve ties by lane index
1252 (avi.arrivalTime == arrivalTime && avi.dist == dist && ego->getLane()->getIndex() < foe->getLane()->getIndex())) {
1253 #ifdef DEBUG_ZIPPER
1254 if (DEBUG_COND_ZIPPER) std::cout
1255 << " ignoring foe=" << foe->getID()
1256 << " foeAT=" << avi.arrivalTime
1257 << " foeDist=" << avi.dist
1258 << " foeSpeed=" << foe->getSpeed()
1259 << " egoSpeed=" << ego->getSpeed()
1260 << " deltaDist=" << avi.dist - dist
1261 << " delteSpeed=" << foe->getSpeed() - foe->getCarFollowModel().getMaxDecel() - ego->getSpeed()
1262 << "\n";
1263 #endif
1264 continue;
1265 }
1266 // the idea behind speed adaption is three-fold:
1267 // 1) ego needs to be in a car-following relationship with foe eventually
1268 // thus, the ego speed should be equal to the follow speed once the foe enters
1269 // the zipper junction
1270 // 2) ego vehicle needs to put a certain distance beteen himself and foe (safeGap)
1271 // achieving this distance can be spread over time but computing
1272 // safeGap is subject to estimation errors of future speeds
1273 // 3) deceleration can be spread out over the time until true
1274 // car-following happens, at the start of speed adaptions, smaller
1275 // decelerations should be sufficient
1276
1277 // we cannot trust avi.arrivalSpeed if the foe has leader vehicles that are accelerating
1278 // lets try to extrapolate
1279 const double uMax = foe->getLane()->getVehicleMaxSpeed(foe);
1280 const double uAccel = foe->getCarFollowModel().estimateSpeedAfterDistance(avi.dist, foe->getSpeed(), foe->getCarFollowModel().getMaxAccel());
1281 const double uEnd = MIN2(uMax, uAccel);
1282 const double uAvg = (foe->getSpeed() + uEnd) / 2;
1283 const double tf0 = avi.dist / MAX2(NUMERICAL_EPS, uAvg);
1284 const double tf = MAX2(1.0, ceil((tf0) / TS) * TS);
1285
1286 const double vMax = ego->getLane()->getVehicleMaxSpeed(ego);
1287 const double vAccel = ego->getCarFollowModel().estimateSpeedAfterDistance(dist, ego->getSpeed(), ego->getCarFollowModel().getMaxAccel());
1288 const double vEnd = MIN3(vMax, vAccel, uEnd);
1289 const double vAvg = (ego->getSpeed() + vEnd) / 2;
1290 const double te0 = dist / MAX2(NUMERICAL_EPS, vAvg);
1291 const double te = MAX2(1.0, ceil((te0) / TS) * TS);
1292
1293 const double gap = dist - foe->getVehicleType().getLength() - ego->getVehicleType().getMinGap() - avi.dist;
1294 const double safeGap = ego->getCarFollowModel().getSecureGap(vEnd, uEnd, foe->getCarFollowModel().getMaxDecel());
1295 // round t to next step size
1296 // increase gap to safeGap by the time foe reaches link
1297 // gap + u*t - (t * v + a * t^2 / 2) = safeGap
1298 const double deltaGap = gap + tf * uAvg - safeGap - vAvg * tf;
1299 const double a = 2 * deltaGap / (tf * tf);
1300 const double vSafeGap = ego->getSpeed() + ACCEL2SPEED(a);
1301 const double vFollow = ego->getCarFollowModel().followSpeed(
1302 ego, ego->getSpeed(), gap, foe->getSpeed(), foe->getCarFollowModel().getMaxDecel(), foe);
1303
1304 // scale behavior based on ego time to link (te)
1305 const double w = MIN2(1.0, te / 10);
1306 const double maxDecel = w * ego->getCarFollowModel().getMaxDecel() + (1 - w) * ego->getCarFollowModel().getEmergencyDecel();
1307 const double vZipper = MAX3(vFollow, ego->getSpeed() - ACCEL2SPEED(maxDecel), w * vSafeGap + (1 - w) * vFollow);
1308
1309 vSafe = MIN2(vSafe, vZipper);
1310 #ifdef DEBUG_ZIPPER
1311 if (DEBUG_COND_ZIPPER) std::cout << " adapting to foe=" << foe->getID()
1312 << " foeDist=" << avi.dist
1313 << " foeSpeed=" << foe->getSpeed()
1314 << " foeAS=" << avi.arrivalSpeed
1315 << " egoSpeed=" << ego->getSpeed()
1316 << " uMax=" << uMax
1317 << " uAccel=" << uAccel
1318 << " uEnd=" << uEnd
1319 << " uAvg=" << uAvg
1320 << " gap=" << gap
1321 << " safeGap=" << safeGap
1322 << "\n "
1323 << " tf=" << tf
1324 << " te=" << te
1325 << " dg=" << deltaGap
1326 << " aSafeGap=" << a
1327 << " vMax=" << vMax
1328 << " vAccel=" << vAccel
1329 << " vEnd=" << vEnd
1330 << " vSafeGap=" << vSafeGap
1331 << " vFollow=" << vFollow
1332 << " w=" << w
1333 << " maxDecel=" << maxDecel
1334 << " vZipper=" << vZipper
1335 << " vSafe=" << vSafe
1336 << "\n";
1337 #endif
1338 }
1339 return vSafe;
1340 }
1341
1342
1343 bool
couldBrakeForLeader(double followDist,double leaderDist,const MSVehicle * follow,const MSVehicle * leader)1344 MSLink::couldBrakeForLeader(double followDist, double leaderDist, const MSVehicle* follow, const MSVehicle* leader) {
1345 return (// leader is ahead of follower
1346 followDist > leaderDist &&
1347 // and follower could brake for 1 s to stay behind leader
1348 followDist - leaderDist > follow->getSpeed() - follow->getCarFollowModel().getMaxDecel() - leader->getSpeed());
1349 }
1350
1351
1352 void
initParallelLinks()1353 MSLink::initParallelLinks() {
1354 myParallelRight = computeParallelLink(-1);
1355 myParallelLeft = computeParallelLink(1);
1356 }
1357
1358 bool
checkContOff() const1359 MSLink::checkContOff() const {
1360 // check whether this link gets to keep its cont status switching the tls off
1361 // @note: this could also be pre-computed in netconvert
1362 // we check whether there is any major link from this edge
1363 for (const MSLane* cand : myLaneBefore->getEdge().getLanes()) {
1364 for (const MSLink* link : cand->getLinkCont()) {
1365 if (link->getOffState() == LINKSTATE_TL_OFF_NOSIGNAL) {
1366 return true;
1367 }
1368 }
1369 }
1370 return false;
1371 }
1372
1373 /****************************************************************************/
1374
1375