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