Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2024 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file MSLink.cpp
15 : /// @author Daniel Krajzewicz
16 : /// @author Jakob Erdmann
17 : /// @author Michael Behrisch
18 : /// @author Laura Bieker
19 : /// @date Sept 2002
20 : ///
21 : // A connection between lanes
22 : /****************************************************************************/
23 : #include <config.h>
24 :
25 : #include <iostream>
26 : #include <algorithm>
27 : #include <limits>
28 : #include <utils/iodevices/OutputDevice.h>
29 : #include <utils/common/RandHelper.h>
30 : #include <utils/common/StringTokenizer.h>
31 : #include "MSNet.h"
32 : #include "MSJunction.h"
33 : #include "MSJunctionLogic.h"
34 : #include "MSLink.h"
35 : #include "MSLane.h"
36 : #include <microsim/transportables/MSPerson.h>
37 : #include <microsim/transportables/MSTransportableControl.h>
38 : #include "MSEdge.h"
39 : #include "MSGlobals.h"
40 : #include "MSVehicle.h"
41 : #include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
42 : #include <microsim/transportables/MSPModel.h>
43 :
44 : //#define MSLink_DEBUG_CROSSING_POINTS
45 : //#define MSLink_DEBUG_CROSSING_POINTS_DETAILS
46 : //#define MSLink_DEBUG_OPENED
47 : //#define DEBUG_APPROACHING
48 : //#define DEBUG_ZIPPER
49 : //#define DEBUG_WALKINGAREA
50 : //#define DEBUG_COND (myLane->getID()=="43[0]_0" && myLaneBefore->getID()==":33_0_0")
51 : //#define DEBUG_COND (myLane->getID()=="end_0")
52 : //#define DEBUG_COND (true)
53 : #define DEBUG_COND2(obj) (obj->isSelected())
54 : //#define DEBUG_COND2(obj) (obj->getID() == "train2")
55 : //#define DEBUG_COND_ZIPPER (gDebugFlag1)
56 : //#define DEBUG_COND_ZIPPER (true)
57 : #define DEBUG_COND_ZIPPER (ego->isSelected())
58 :
59 : // ===========================================================================
60 : // static member variables
61 : // ===========================================================================
62 :
63 : #define INVALID_TIME -1000
64 :
65 : // the default safety gap when passing before oncoming pedestrians
66 : #define JM_CROSSING_GAP_DEFAULT 10
67 :
68 : // minimim width between sibling lanes to qualify as non-overlapping
69 : #define DIVERGENCE_MIN_WIDTH 2.5
70 :
71 : const SUMOTime MSLink::myLookaheadTime = TIME2STEPS(1);
72 : // additional caution is needed when approaching a zipper link
73 : const SUMOTime MSLink::myLookaheadTimeZipper = TIME2STEPS(4);
74 : std::set<std::pair<MSLink*, MSLink*> > MSLink::myRecheck;
75 : const double MSLink::NO_INTERSECTION(10000);
76 :
77 : // ===========================================================================
78 : // ConflictInfo member method definitions
79 : // ===========================================================================
80 :
81 : double
82 351592532 : MSLink::ConflictInfo::getFoeLengthBehindCrossing(const MSLink* foeExitLink) const {
83 351592532 : if (flag == CONFLICT_DUMMY_MERGE) {
84 : return 0;
85 344751593 : } else if (foeConflictIndex >= 0) {
86 328594470 : return foeExitLink->myConflicts[foeConflictIndex].lengthBehindCrossing;
87 : } else {
88 : return -NO_INTERSECTION;
89 : }
90 : }
91 :
92 : double
93 197596182 : MSLink::ConflictInfo::getFoeConflictSize(const MSLink* foeExitLink) const {
94 197596182 : if (foeConflictIndex >= 0) {
95 181439059 : return foeExitLink->myConflicts[foeConflictIndex].conflictSize;
96 : } else {
97 : return 0;
98 : }
99 : }
100 :
101 : double
102 351605510 : MSLink::ConflictInfo::getLengthBehindCrossing(const MSLink* exitLink) const {
103 351605510 : if (flag == CONFLICT_STOP_AT_INTERNAL_JUNCTION) {
104 14868855 : return exitLink->getInternalLaneBefore()->getLength();
105 : } else {
106 336736655 : return lengthBehindCrossing;
107 : }
108 : }
109 :
110 : // ===========================================================================
111 : // member method definitions
112 : // ===========================================================================
113 2751925 : MSLink::MSLink(MSLane* predLane, MSLane* succLane, MSLane* via, LinkDirection dir, LinkState state,
114 : double length, double foeVisibilityDistance, bool keepClear,
115 : MSTrafficLightLogic* logic, int tlIndex,
116 2751925 : bool indirect) :
117 2751925 : myLane(succLane),
118 2751925 : myLaneBefore(predLane),
119 2751925 : myApproachingPersons(nullptr),
120 2751925 : myIndex(-1),
121 2751925 : myTLIndex(tlIndex),
122 2751925 : myLogic(logic),
123 2751925 : myState(state),
124 2751925 : myLastGreenState(LINKSTATE_TL_GREEN_MINOR),
125 2751925 : myOffState(state),
126 2751925 : myLastStateChange(SUMOTime_MIN / 2), // a large negative value, but avoid overflows when subtracting
127 2751925 : myDirection(dir),
128 2751925 : myLength(length),
129 2751925 : myFoeVisibilityDistance(foeVisibilityDistance),
130 2751925 : myDistToFoePedCrossing(std::numeric_limits<double>::max()),
131 2751925 : myHasFoes(false),
132 2751925 : myAmCont(false),
133 2751925 : myAmContOff(false),
134 2751925 : myKeepClear(keepClear),
135 2751925 : myInternalLane(via),
136 2751925 : myInternalLaneBefore(nullptr),
137 2751925 : myMesoTLSPenalty(0),
138 2751925 : myGreenFraction(1),
139 2751925 : myLateralShift(0),
140 2751925 : myOffFoeLinks(nullptr),
141 2751925 : myWalkingAreaFoe(nullptr),
142 2751925 : myWalkingAreaFoeExit(nullptr),
143 2751925 : myHavePedestrianCrossingFoe(false),
144 2751925 : myParallelRight(nullptr),
145 2751925 : myParallelLeft(nullptr),
146 2751925 : myAmIndirect(indirect),
147 2751925 : myRadius(std::numeric_limits<double>::max()),
148 2751925 : myPermissions(myLaneBefore->getPermissions() & myLane->getPermissions() & (via == nullptr ? SVCAll : via->getPermissions())),
149 2751925 : myJunction(nullptr) {
150 :
151 2751925 : if (MSGlobals::gLateralResolution > 0) {
152 : // detect lateral shift from lane geometries
153 : //std::cout << "DEBUG link=" << myLaneBefore->getID() << "->" << getViaLaneOrLane()->getID() << " hasInternal=" << MSNet::getInstance()->hasInternalLinks() << " shapeBefore=" << myLaneBefore->getShape().back() << " shapeFront=" << getViaLaneOrLane()->getShape().front() << "\n";
154 184371 : if ((myInternalLane != nullptr || predLane->isInternal())
155 444905 : && myLaneBefore->getShape().back() != getViaLaneOrLane()->getShape().front()) {
156 : PositionVector from = myLaneBefore->getShape();
157 : const PositionVector& to = getViaLaneOrLane()->getShape();
158 : const double dist = from.back().distanceTo2D(to.front());
159 : // figure out direction of shift
160 : try {
161 634 : from.move2side(dist);
162 0 : } catch (InvalidArgument&) {
163 0 : }
164 634 : myLateralShift = (from.back().distanceTo2D(to.front()) < dist) ? dist : -dist;
165 634 : if (MSGlobals::gLefthand) {
166 90 : myLateralShift *= -1;
167 : }
168 : //std::cout << " lateral shift link=" << myLaneBefore->getID() << "->" << getViaLaneOrLane()->getID() << " dist=" << dist << " shift=" << myLateralShift << "\n";
169 634 : }
170 : }
171 2751925 : }
172 :
173 :
174 2558205 : MSLink::~MSLink() {
175 2558205 : delete myOffFoeLinks;
176 2559778 : delete myApproachingPersons;
177 2558205 : }
178 :
179 :
180 : void
181 4 : MSLink::addCustomConflict(const MSLane* from, const MSLane* to, double startPos, double endPos) {
182 4 : myCustomConflicts.push_back(CustomConflict(from, to, startPos, endPos));
183 4 : }
184 :
185 : const MSLink::CustomConflict*
186 3416817 : MSLink::getCustomConflict(const MSLane* foeLane) const {
187 3416817 : if (myCustomConflicts.size() > 0) {
188 16 : const MSLane* foeFrom = foeLane->getNormalPredecessorLane();
189 16 : const MSLane* foeTo = foeLane->getNormalSuccessorLane();
190 24 : for (const CustomConflict& cc : myCustomConflicts) {
191 16 : if (cc.from == foeFrom && cc.to == foeTo) {
192 : return &cc;
193 : }
194 : }
195 :
196 : }
197 : return nullptr;
198 : }
199 :
200 : void
201 2547026 : MSLink::setRequestInformation(int index, bool hasFoes, bool isCont,
202 : const std::vector<MSLink*>& foeLinks,
203 : const std::vector<MSLane*>& foeLanes,
204 : MSLane* internalLaneBefore) {
205 : //#ifdef MSLink_DEBUG_CROSSING_POINTS
206 : // std::cout << " setRequestInformation() for junction " << getViaLaneOrLane()->getEdge().getFromJunction()->getID()
207 : // << "\nInternalLanes = " << toString(getViaLaneOrLane()->getEdge().getFromJunction()->getInternalLanes())
208 : // << std::endl;
209 : //#endif
210 2547026 : myIndex = index;
211 2547026 : myHasFoes = hasFoes;
212 2547026 : myAmCont = isCont;
213 2547026 : myFoeLinks = foeLinks;
214 8791379 : for (MSLane* foeLane : foeLanes) {
215 : // cannot assign vector due to const-ness
216 6244353 : myFoeLanes.push_back(foeLane);
217 : }
218 2547026 : myJunction = const_cast<MSJunction*>(myLane->getEdge().getFromJunction()); // junctionGraph is initialized after the whole network is loaded
219 2547026 : myAmContOff = isCont && myLogic != nullptr && internalLaneBefore == nullptr && checkContOff();
220 2547026 : myInternalLaneBefore = internalLaneBefore;
221 : MSLane* lane = nullptr;
222 : if (internalLaneBefore != nullptr) {
223 : // this is an exit link. compute crossing points with all foeLanes
224 : lane = internalLaneBefore;
225 : //} else if (myLane->getEdge().isCrossing()) {
226 : // // this is the link to a pedestrian crossing. compute crossing points with all foeLanes
227 : // // @note not currently used by pedestrians
228 : // lane = myLane;
229 : }
230 2547026 : const MSLink* entryLink = getCorrespondingEntryLink();
231 2547026 : if (entryLink->getOffState() == LinkState::LINKSTATE_ALLWAY_STOP && entryLink->getTLLogic() != nullptr) {
232 : // TLS has "normal" right of way rules but all conflicting links are foes when switching TLS off
233 : // (unless it's an internal junction link which should ignore all foes and should be ignored by all foes
234 3149 : myOffFoeLinks = new std::vector<MSLink*>();
235 3149 : if (isEntryLink()) {
236 7172 : for (MSLane* foeLane : foeLanes) {
237 : assert(foeLane->isInternal() || foeLane->isCrossing());
238 5744 : MSLink* viaLink = foeLane->getIncomingLanes().front().viaLink;
239 5744 : if (viaLink->getLaneBefore()->isNormal()) {
240 3826 : myOffFoeLinks->push_back(viaLink);
241 : }
242 : }
243 : }
244 : }
245 : #ifdef MSLink_DEBUG_CROSSING_POINTS
246 : std::cout << "link " << myIndex << " to " << getViaLaneOrLane()->getID() << " internalLaneBefore=" << (lane == 0 ? "NULL" : lane->getID()) << " has foes: " << toString(foeLanes) << "\n";
247 : #endif
248 2547026 : if (lane != nullptr) {
249 928804 : const bool beforeInternalJunction = lane->getLinkCont()[0]->getViaLaneOrLane()->getEdge().isInternal();
250 928804 : if (lane->getIncomingLanes().size() != 1) {
251 0 : throw ProcessError("Internal lane '" + lane->getID() + "' has " + toString(lane->getIncomingLanes().size()) + " predecessors");
252 : }
253 928804 : const MSLink* junctionEntryLink = lane->getEntryLink();
254 928804 : const bool isSecondPart = isExitLinkAfterInternalJunction();
255 : // compute crossing points
256 4439990 : for (const MSLane* foeLane : myFoeLanes) {
257 3511186 : const CustomConflict* cc = junctionEntryLink != nullptr ? junctionEntryLink->getCustomConflict(foeLane) : nullptr;
258 3416809 : if (cc != nullptr) {
259 : // handle custom conflict definition
260 8 : double startPos = cc->startPos;
261 8 : const double conflictSize = cc->endPos - cc->startPos;
262 8 : if (isSecondPart) {
263 0 : startPos -= junctionEntryLink->getViaLane()->getLength();
264 : }
265 : // the foe connection may be split at an internal
266 : // junction, we need to figure out whether the current
267 : // foeLane is the intended target for the custom conflict
268 : // There are two possibilities:
269 : // a) We have no custom conflict for the reverse pair of connections
270 : // -> just check whether lane and foeLane intersect
271 : // b) We have a "reverse" custom conflict
272 : // -> check whether it covers the foeLane
273 8 : const CustomConflict* rcc = foeLane->getEntryLink()->getCustomConflict(lane);
274 : bool haveIntersection = false;
275 8 : if (rcc == nullptr) {
276 : // a)
277 8 : haveIntersection = lane->getShape().intersectsAtLengths2D(foeLane->getShape()).size() > 0;
278 : } else {
279 : // b)
280 0 : const bool foeIsSecondPart = foeLane->getLogicalPredecessorLane()->isInternal();
281 0 : double foeStartPos = rcc->startPos;
282 0 : const double foeConflictSize = rcc->endPos - rcc->startPos;
283 0 : if (foeIsSecondPart) {
284 0 : foeStartPos -= foeLane->getLogicalPredecessorLane()->getLength();
285 : }
286 0 : const double foeEndPos = foeStartPos + foeConflictSize;
287 0 : haveIntersection = ((foeStartPos > 0 && foeStartPos < foeLane->getLength())
288 0 : || (foeEndPos > 0 && foeEndPos < foeLane->getLength()));
289 : }
290 8 : if (haveIntersection) {
291 4 : myConflicts.push_back(ConflictInfo(lane->getLength() - startPos, conflictSize));
292 : } else {
293 4 : myConflicts.push_back(ConflictInfo(-NO_INTERSECTION, 0));
294 : }
295 : #ifdef MSLink_DEBUG_CROSSING_POINTS
296 : std::cout << " " << lane->getID() << " custom conflict with " << foeLane->getID() << " customReverse=" << (rcc != nullptr)
297 : << " haveIntersection=" << haveIntersection
298 : << " startPos=" << startPos << " conflictSize=" << conflictSize
299 : << " lbc=" << myConflicts.back().lengthBehindCrossing
300 : << "\n";
301 : #endif
302 8 : continue;
303 8 : }
304 3511178 : myHavePedestrianCrossingFoe = myHavePedestrianCrossingFoe || foeLane->getEdge().isCrossing();
305 3511178 : const bool sameTarget = myLane == foeLane->getLinkCont()[0]->getLane();
306 3511178 : if (sameTarget && !beforeInternalJunction && !contIntersect(lane, foeLane)) {
307 : //if (myLane == foeLane->getLinkCont()[0]->getLane()) {
308 : // this foeLane has the same target and merges at the end (lane exits the junction)
309 1251665 : const double minDist = MIN2(DIVERGENCE_MIN_WIDTH, 0.5 * (lane->getWidth() + foeLane->getWidth()));
310 1251665 : if (lane->getShape().back().distanceTo2D(foeLane->getShape().back()) >= minDist) {
311 : // account for lateral shift by the entry links
312 109296 : if (foeLane->getEntryLink()->isIndirect()) {
313 25 : myConflicts.push_back(ConflictInfo(-NO_INTERSECTION, 0)); // dummy value, never used
314 : #ifdef MSLink_DEBUG_CROSSING_POINTS
315 : std::cout << " " << lane->getID() << " dummy merge with indirect" << foeLane->getID() << "\n";
316 : #endif
317 : } else {
318 109271 : myConflicts.push_back(ConflictInfo(0, foeLane->getWidth(), CONFLICT_DUMMY_MERGE)); // dummy value, never used
319 : #ifdef MSLink_DEBUG_CROSSING_POINTS
320 : std::cout << " " << lane->getID() << " dummy merge with " << foeLane->getID() << "\n";
321 : #endif
322 : }
323 : } else {
324 1142369 : const double distAfterDivergence = computeDistToDivergence(lane, foeLane, minDist, false);
325 : const double lbcLane = lane->interpolateGeometryPosToLanePos(distAfterDivergence);
326 1142369 : myConflicts.push_back(ConflictInfo(lbcLane, foeLane->getWidth()));
327 : #ifdef MSLink_DEBUG_CROSSING_POINTS
328 : std::cout
329 : << " " << lane->getID()
330 : << " merges with " << foeLane->getID()
331 : << " nextLane " << lane->getLinkCont()[0]->getViaLaneOrLane()->getID()
332 : << " dist1=" << myConflicts.back().lengthBehindCrossing
333 : << "\n";
334 : #endif
335 : }
336 : } else {
337 2259513 : std::vector<double> intersections1 = lane->getShape().intersectsAtLengths2D(foeLane->getShape());
338 : #ifdef MSLink_DEBUG_CROSSING_POINTS_DETAILS
339 : std::cout << " intersections1=" << toString(intersections1) << "\n";
340 : #endif
341 : bool haveIntersection = true;
342 2259513 : if (intersections1.size() == 0) {
343 1139738 : intersections1.push_back(-NO_INTERSECTION); // disregard this foe (using maxdouble leads to nasty problems down the line)
344 : haveIntersection = false;
345 1119775 : } else if (intersections1.size() > 1) {
346 1277 : std::sort(intersections1.begin(), intersections1.end());
347 : }
348 2259513 : std::vector<double> intersections2 = foeLane->getShape().intersectsAtLengths2D(lane->getShape());
349 : #ifdef MSLink_DEBUG_CROSSING_POINTS_DETAILS
350 : std::cout << " intersections2=" << toString(intersections2) << "\n";
351 : #endif
352 2259513 : if (intersections2.size() == 0) {
353 1139738 : intersections2.push_back(0);
354 1119775 : } else if (intersections2.size() > 1) {
355 1277 : std::sort(intersections2.begin(), intersections2.end());
356 : }
357 : double conflictSize = foeLane->getWidth();
358 : ConflictFlag flag = CONFLICT_NO_INTERSECTION;
359 2259513 : if (haveIntersection) {
360 : flag = CONFLICT_DEFAULT;
361 1119775 : const double angle1 = GeomHelper::naviDegree(lane->getShape().rotationAtOffset(intersections1.back()));
362 1119775 : const double angle2 = GeomHelper::naviDegree(foeLane->getShape().rotationAtOffset(intersections2.back()));
363 1119775 : const double angleDiff = GeomHelper::getMinAngleDiff(angle1, angle2);
364 : //const double angleDiff = MIN2(GeomHelper::getMinAngleDiff(angle1, angle2),
365 : // GeomHelper::getMinAngleDiff(angle1, angle2 + 180));
366 1119775 : const double widthFactor = 1 / MAX2(sin(DEG2RAD(angleDiff)), 0.2) * 2 - 1;
367 : //std::cout << " intersection of " << lane->getID() << " with " << foeLane->getID() << " angle1=" << angle1 << " angle2=" << angle2 << " angleDiff=" << angleDiff << " widthFactor=" << widthFactor << "\n";
368 1119775 : conflictSize *= widthFactor;
369 : conflictSize = MIN2(conflictSize, lane->getLength());
370 : // lane width affects the crossing point
371 1119775 : intersections1.back() -= conflictSize / 2;
372 : // ensure non-negative offset for weird geometries
373 1119775 : intersections1.back() = MAX2(0.0, intersections1.back());
374 :
375 : // also length/geometry factor. (XXX: Why subtract width/2 *before* converting geometric position to lane pos? refs #3031)
376 1119775 : intersections1.back() = lane->interpolateGeometryPosToLanePos(intersections1.back());
377 :
378 1119775 : if (internalLaneBefore->getLogicalPredecessorLane()->getEdge().isInternal() && !foeLane->getEdge().isCrossing()) {
379 : flag = CONFLICT_STOP_AT_INTERNAL_JUNCTION;
380 : }
381 :
382 1119775 : if (foeLane->getEdge().isCrossing()) {
383 88360 : const_cast<MSLink*>(getCorrespondingEntryLink())->updateDistToFoePedCrossing(intersections1.back());
384 : };
385 : }
386 :
387 2259513 : myConflicts.push_back(ConflictInfo(
388 2259513 : lane->getLength() - intersections1.back(),
389 : conflictSize, flag));
390 :
391 : #ifdef MSLink_DEBUG_CROSSING_POINTS
392 : std::cout
393 : << " intersection of " << lane->getID()
394 : << " totalLength=" << lane->getLength()
395 : << " with " << foeLane->getID()
396 : << " totalLength=" << foeLane->getLength()
397 : << " dist1=" << myConflicts.back().lengthBehindCrossing
398 : << " widthFactor=" << myConflicts.back().conflictSize / foeLane->getWidth()
399 : << "\n";
400 : #endif
401 2259513 : }
402 : }
403 : // check for overlap with internal lanes from the same source lane
404 928804 : const MSLane* pred = lane->getLogicalPredecessorLane();
405 : // to avoid overlap with vehicles that came from pred (especially when pred has endOffset > 0)
406 : // we add all other internal lanes from pred as foeLanes
407 2823609 : for (const MSLink* const link : pred->getLinkCont()) {
408 1894805 : const MSLane* const sibling = link->getViaLane();
409 1894805 : if (sibling != lane && sibling != nullptr) {
410 944094 : const double minDist = MIN2(DIVERGENCE_MIN_WIDTH, 0.5 * (lane->getWidth() + sibling->getWidth()));
411 944094 : if (lane->getShape().front().distanceTo2D(sibling->getShape().front()) >= minDist) {
412 : // account for lateral shift by the entry links
413 616 : continue;
414 : }
415 943478 : const double distToDivergence = computeDistToDivergence(lane, sibling, minDist, true);
416 : double lbcLane;
417 943478 : if (lane->getLength() == sibling->getLength() && &lane->getEdge() == &sibling->getEdge()) {
418 : // for parallel lanes, avoid inconsistency in distance estimation (#10988)
419 : // between forward distance (getLeaderInfo)
420 : // and backward distance used in lane-changing (getFollowersOnConsecutive)
421 60590 : lbcLane = lane->getLength() - distToDivergence;
422 : } else {
423 882888 : lbcLane = MAX2(0.0, lane->getLength() - lane->interpolateGeometryPosToLanePos(distToDivergence));
424 : }
425 : ConflictInfo ci = ConflictInfo(lbcLane, sibling->getWidth());
426 943478 : auto it = std::find(myFoeLanes.begin(), myFoeLanes.end(), sibling);
427 943478 : if (it != myFoeLanes.end()) {
428 : // avoid duplicate foeLane
429 50 : const int replacedIndex = (int)(it - myFoeLanes.begin());
430 50 : myConflicts[replacedIndex] = ci;
431 : } else {
432 943428 : myConflicts.push_back(ci);
433 943428 : myFoeLanes.push_back(sibling);
434 : }
435 : #ifdef MSLink_DEBUG_CROSSING_POINTS
436 : std::cout << " adding same-origin foe" << sibling->getID()
437 : << " dist1=" << myConflicts.back().lengthBehindCrossing
438 : << "\n";
439 : #endif
440 : }
441 : }
442 : // init points for the symmetrical conflict
443 : // for each pair of conflicting lanes, the link that gets second, sets the pointers
444 5383418 : for (int i = 0; i < (int)myFoeLanes.size(); i++) {
445 4454614 : const MSLane* foeLane = myFoeLanes[i];
446 4454614 : MSLink* foeExitLink = foeLane->getLinkCont()[0];
447 : int foundIndex = -1;
448 16593977 : for (int i2 = 0; i2 < (int)foeExitLink->myFoeLanes.size(); i2++) {
449 14145660 : if (foeExitLink->myFoeLanes[i2] == lane) {
450 2006297 : myConflicts[i].foeConflictIndex = i2;
451 2006297 : foeExitLink->myConflicts[i2].foeConflictIndex = i;
452 2006297 : myRecheck.erase({foeExitLink, this});
453 : foundIndex = i2;
454 2006297 : break;
455 : }
456 : }
457 : #ifdef MSLink_DEBUG_CROSSING_POINTS
458 : std::cout << lane->getID() << " foeLane=" << foeLane->getID() << " index=" << i << " foundIndex=" << foundIndex << "\n";
459 : #endif
460 4454614 : if (foundIndex < 0) {
461 2448317 : if (myConflicts[i].flag != CONFLICT_NO_INTERSECTION) {
462 1714333 : myRecheck.insert({this, foeExitLink});
463 : }
464 : }
465 : }
466 : }
467 2547026 : if (MSGlobals::gLateralResolution > 0) {
468 : // check for links with the same origin lane and the same destination edge
469 261040 : const MSEdge* myTarget = &myLane->getEdge();
470 : // save foes for entry links
471 835030 : for (MSLink* const it : myLaneBefore->getLinkCont()) {
472 : const MSEdge* target = &(it->getLane()->getEdge());
473 573990 : if (it == this) {
474 261040 : continue;
475 : }
476 312950 : if (target == myTarget) {
477 6390 : mySublaneFoeLinks.push_back(it);
478 : #ifdef MSLink_DEBUG_CROSSING_POINTS
479 : std::cout << " sublaneFoeLink (same target): " << it->getViaLaneOrLane()->getID() << "\n";
480 : #endif
481 306560 : } else if (myDirection != LinkDirection::STRAIGHT && it->getDirection() == LinkDirection::STRAIGHT) {
482 : // potential turn conflict
483 74641 : mySublaneFoeLinks2.push_back(it);
484 : #ifdef MSLink_DEBUG_CROSSING_POINTS
485 : std::cout << " sublaneFoeLink2 (other target: " << it->getViaLaneOrLane()->getID() << "\n";
486 : #endif
487 : }
488 : }
489 : // save foes for exit links
490 261040 : if (fromInternalLane()) {
491 : //std::cout << " setRequestInformation link=" << getViaLaneOrLane()->getID() << " before=" << myLaneBefore->getID() << " before2=" << myLaneBefore->getIncomingLanes().front().lane->getID() << "\n";
492 312648 : for (const MSLink* const link : myLaneBefore->getIncomingLanes().front().lane->getLinkCont()) {
493 219726 : if (link->getViaLane() != myInternalLaneBefore && &link->getLane()->getEdge() == myTarget) {
494 : //std::cout << " add sublaneFoe=" << (*it)->getViaLane()->getID() << "\n";
495 3966 : mySublaneFoeLanes.push_back(link->getViaLane());
496 : }
497 : }
498 : }
499 : }
500 2547026 : if (myInternalLaneBefore != nullptr
501 928804 : && myDirection != LinkDirection::STRAIGHT
502 : // for right turns, the curvature helps rather than restricts the linkLeader check
503 407703 : && (
504 407703 : (!MSGlobals::gLefthand && myDirection != LinkDirection::RIGHT)
505 123906 : || (MSGlobals::gLefthand && myDirection != LinkDirection::LEFT))) {
506 568424 : const double angle = fabs(GeomHelper::angleDiff(
507 284212 : myLaneBefore->getNormalPredecessorLane()->getShape().angleAt2D(-2),
508 284212 : myLane->getShape().angleAt2D(0)));
509 284212 : if (angle > 0) {
510 284212 : double length = myInternalLaneBefore->getShape().length2D();
511 568424 : if (myInternalLaneBefore->getIncomingLanes().size() == 1 &&
512 284212 : myInternalLaneBefore->getIncomingLanes()[0].lane->isInternal()) {
513 76574 : length += myInternalLaneBefore->getIncomingLanes()[0].lane->getShape().length2D();
514 207638 : } else if (myInternalLane != nullptr) {
515 76574 : length += myInternalLane->getShape().length2D();
516 : }
517 284212 : myRadius = length / angle;
518 : //std::cout << getDescription() << " a=" << RAD2DEG(angle) << " l=" << length << " r=" << myRadius << "\n";
519 : }
520 : }
521 2547026 : }
522 :
523 :
524 : void
525 42320 : MSLink::recheckSetRequestInformation() {
526 70681 : for (auto item : myRecheck) {
527 : #ifdef MSLink_DEBUG_CROSSING_POINTS
528 : std::cout << " recheck l1=" << item.first->getDescription() << " l2=" << item.second->getDescription() << "\n";
529 : #endif
530 : MSLink* link = item.first;
531 : MSLink* foeExitLink = item.second;
532 : const MSLane* lane = link->getInternalLaneBefore();
533 : const MSLane* foeLane = foeExitLink->getInternalLaneBefore();
534 : int conflictIndex = -1;
535 185221 : for (int i = 0; i < (int)link->myFoeLanes.size(); i++) {
536 185221 : if (link->myFoeLanes[i] == foeLane) {
537 : conflictIndex = i;
538 : break;
539 : }
540 : }
541 28361 : if (conflictIndex == -1) {
542 0 : WRITE_WARNING("Could not recheck ConflictInfo for " + link->getDescription() + " and " + foeExitLink->getDescription() + "\n");
543 72 : continue;
544 : }
545 28361 : ConflictInfo& ci = link->myConflicts[conflictIndex];
546 28361 : std::vector<double> intersections1 = foeLane->getShape().intersectsAtLengths2D(lane->getShape());
547 28361 : if (intersections1.size() == 0) {
548 : #ifdef MSLink_DEBUG_CROSSING_POINTS
549 : std::cout << " no intersection\n";
550 : #endif
551 : continue;
552 : }
553 28289 : const double widthFactor = ci.conflictSize / foeLane->getWidth();
554 28289 : const double conflictSize2 = lane->getWidth() * widthFactor;
555 28289 : std::sort(intersections1.begin(), intersections1.end());
556 28289 : intersections1.back() -= conflictSize2 / 2;
557 28289 : intersections1.back() = MAX2(0.0, intersections1.back());
558 28289 : ci.foeConflictIndex = (int)foeExitLink->myConflicts.size();
559 28289 : foeExitLink->myConflicts.push_back(ConflictInfo(foeLane->getLength() - intersections1.back(), conflictSize2));
560 : #ifdef MSLink_DEBUG_CROSSING_POINTS
561 : std::cout << " ci=" << conflictIndex << " wf=" << widthFactor << " flag=" << ci.flag << " flbc=" << foeExitLink->myConflicts.back().lengthBehindCrossing << "\n";
562 : #endif
563 28361 : }
564 : myRecheck.clear();
565 42320 : }
566 :
567 : double
568 2085847 : MSLink::computeDistToDivergence(const MSLane* lane, const MSLane* sibling, double minDist, bool sameSource) const {
569 : double lbcSibling = 0;
570 : double lbcLane = 0;
571 :
572 : PositionVector l = lane->getShape();
573 : PositionVector s = sibling->getShape();
574 2085847 : double length = l.length2D();
575 2085847 : double sibLength = s.length2D();
576 2085847 : if (!sameSource) {
577 2284738 : l = l.reverse();
578 2284738 : s = s.reverse();
579 943478 : } else if (sibling->getEntryLink()->myAmIndirect) {
580 : // ignore final waiting position since it may be quite close to the lane
581 : // shape but the waiting position is perpendicular (so the minDist
582 : // requirement is not necessary
583 54 : lbcSibling += s[-1].distanceTo2D(s[-2]);
584 : s.pop_back();
585 943424 : } else if (lane->getEntryLink()->myAmIndirect) {
586 : // ignore final waiting position since it may be quite close to the lane
587 : // shape but the waiting position is perpendicular (so the minDist
588 : // requirement is not necessary
589 54 : lbcLane += l[-1].distanceTo2D(l[-2]);
590 : l.pop_back();
591 : }
592 :
593 : #ifdef MSLink_DEBUG_CROSSING_POINTS_DETAILS
594 : std::cout << " sameSource=" << sameSource << " minDist=" << minDist << " backDist=" << l.back().distanceTo2D(s.back()) << "\n";
595 : #endif
596 2085847 : if (l.back().distanceTo2D(s.back()) > minDist) {
597 : // compute the final divergence point
598 : // this position serves two purposes:
599 : // 1) once the foe vehicle back (on sibling) has passed this point, we can safely ignore it
600 : // 2) both vehicles are put into a cf-relationship while before the point.
601 : // Since the actual crossing point is at the start of the junction,
602 : // we want to make sure that both vehicles have the same distance to the crossing point and thus follow each other naturally
603 2041449 : std::vector<double> distances = l.distances(s);
604 : #ifdef MSLink_DEBUG_CROSSING_POINTS
605 : std::cout << " distances=" << toString(distances) << "\n";
606 : #endif
607 : assert(distances.size() == l.size() + s.size());
608 2041449 : if (distances.back() > minDist && distances[l.size() - 1] > minDist) {
609 : // do a pairwise check between lane and sibling to make because we do not know which of them bends more
610 2156821 : for (int j = (int)s.size() - 2; j >= 0; j--) {
611 2156821 : const int i = j + (int)l.size();
612 2156821 : const double segLength = s[j].distanceTo2D(s[j + 1]);
613 2156821 : if (distances[i] > minDist) {
614 957452 : lbcSibling += segLength;
615 : } else {
616 : // assume no sharp bends and just interpolate the last segment
617 1199369 : lbcSibling += segLength - (minDist - distances[i]) * segLength / (distances[i + 1] - distances[i]);
618 1199369 : break;
619 : }
620 : }
621 2157157 : for (int i = (int)l.size() - 2; i >= 0; i--) {
622 2157157 : const double segLength = l[i].distanceTo2D(l[i + 1]);
623 2157157 : if (distances[i] > minDist) {
624 957788 : lbcLane += segLength;
625 : } else {
626 : // assume no sharp bends and just interpolate the last segment
627 1199369 : lbcLane += segLength - (minDist - distances[i]) * segLength / (distances[i + 1] - distances[i]);
628 1199369 : break;
629 : }
630 : }
631 : }
632 : assert(lbcSibling >= -NUMERICAL_EPS);
633 : assert(lbcLane >= -NUMERICAL_EPS);
634 2041449 : }
635 2085847 : const double distToDivergence1 = sibling->getLength() - lbcSibling;
636 2085847 : const double distToDivergence2 = lane->getLength() - lbcLane;
637 : const double distToDivergence = MIN3(
638 : MAX2(distToDivergence1, distToDivergence2),
639 : sibLength, length);
640 : #ifdef MSLink_DEBUG_CROSSING_POINTS
641 : std::cout << " distToDivergence=" << distToDivergence
642 : << " distTD1=" << distToDivergence1
643 : << " distTD2=" << distToDivergence2
644 : << " length=" << length
645 : << " sibLength=" << sibLength
646 : << "\n";
647 : #endif
648 2085847 : return distToDivergence;
649 2085847 : }
650 :
651 :
652 : bool
653 1251825 : MSLink::contIntersect(const MSLane* lane, const MSLane* foe) {
654 1251825 : if (foe->getLinkCont()[0]->getViaLane() != nullptr) {
655 109721 : std::vector<double> intersections = lane->getShape().intersectsAtLengths2D(foe->getShape());
656 109721 : return intersections.size() > 0;
657 109721 : }
658 : return false;
659 : }
660 :
661 :
662 : void
663 771283125 : MSLink::setApproaching(const SUMOVehicle* approaching, const SUMOTime arrivalTime, const double arrivalSpeed, const double leaveSpeed,
664 : const bool setRequest, const double arrivalSpeedBraking, const SUMOTime waitingTime, double dist, double latOffset) {
665 771283125 : const SUMOTime leaveTime = getLeaveTime(arrivalTime, arrivalSpeed, leaveSpeed, approaching->getVehicleType().getLength());
666 : #ifdef DEBUG_APPROACHING
667 : if (DEBUG_COND2(approaching)) {
668 : std::cout << SIMTIME << " Link '" << (myLaneBefore == 0 ? "NULL" : myLaneBefore->getID()) << "'->'" << (myLane == 0 ? "NULL" : myLane->getID()) << "' Adding approaching vehicle '" << approaching->getID() << "'\nCurrently registered vehicles:" << std::endl;
669 : for (auto i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
670 : std::cout << "'" << i->first->getID() << "'" << std::endl;
671 : }
672 : }
673 : #endif
674 : myApproachingVehicles.emplace(approaching,
675 1542566250 : ApproachingVehicleInformation(arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, setRequest,
676 771283125 : arrivalSpeedBraking, waitingTime, dist, approaching->getSpeed(), latOffset));
677 771283125 : }
678 :
679 :
680 : void
681 1030657 : MSLink::setApproaching(const SUMOVehicle* approaching, ApproachingVehicleInformation ai) {
682 :
683 : #ifdef DEBUG_APPROACHING
684 : if (DEBUG_COND2(approaching)) {
685 : std::cout << SIMTIME << " Link '" << (myLaneBefore == 0 ? "NULL" : myLaneBefore->getID()) << "'->'" << (myLane == 0 ? "NULL" : myLane->getID()) << "' Adding approaching vehicle '" << approaching->getID() << "'\nCurrently registered vehicles:" << std::endl;
686 : for (auto i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
687 : std::cout << "'" << i->first->getID() << "'" << std::endl;
688 : }
689 : }
690 : #endif
691 : myApproachingVehicles.emplace(approaching, ai);
692 1030657 : }
693 :
694 : void
695 535883 : MSLink::setApproachingPerson(const MSPerson* approaching, const SUMOTime arrivalTime, const SUMOTime leaveTime) {
696 535883 : if (myApproachingPersons == nullptr) {
697 1573 : myApproachingPersons = new PersonApproachInfos();
698 : }
699 535883 : myApproachingPersons->emplace(approaching, ApproachingPersonInformation(arrivalTime, leaveTime));
700 535883 : }
701 :
702 : void
703 770830892 : MSLink::removeApproaching(const SUMOVehicle* veh) {
704 :
705 : #ifdef DEBUG_APPROACHING
706 : if (DEBUG_COND2(veh)) {
707 : std::cout << SIMTIME << " Link '" << (myLaneBefore == 0 ? "NULL" : myLaneBefore->getID()) << "'->'" << (myLane == 0 ? "NULL" : myLane->getID()) << std::endl;
708 : std::cout << "' Removing approaching vehicle '" << veh->getID() << "'\nCurrently registered vehicles:" << std::endl;
709 : for (auto i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
710 : std::cout << "'" << i->first->getID() << "'" << std::endl;
711 : }
712 : }
713 : #endif
714 : myApproachingVehicles.erase(veh);
715 770830892 : }
716 :
717 :
718 : void
719 30706 : MSLink::removeApproachingPerson(const MSPerson* person) {
720 30706 : if (myApproachingPersons == nullptr) {
721 16 : WRITE_WARNINGF("Person '%' entered crossing lane '%' without registering approach, time=%", person->getID(), myLane->getID(), time2string(SIMSTEP));
722 4 : return;
723 : }
724 : #ifdef DEBUG_APPROACHING
725 : if (DEBUG_COND2(person)) {
726 : std::cout << SIMTIME << " Link '" << (myLaneBefore == 0 ? "NULL" : myLaneBefore->getID()) << "'->'" << (myLane == 0 ? "NULL" : myLane->getID()) << std::endl;
727 : std::cout << "' Removing approaching person '" << person->getID() << "'\nCurrently registered persons:" << std::endl;
728 : for (auto i = myApproachingPersons->begin(); i != myApproachingPersons->end(); ++i) {
729 : std::cout << "'" << i->first->getID() << "'" << std::endl;
730 : }
731 : }
732 : #endif
733 : myApproachingPersons->erase(person);
734 : }
735 :
736 :
737 : MSLink::ApproachingVehicleInformation
738 25705363 : MSLink::getApproaching(const SUMOVehicle* veh) const {
739 : auto i = myApproachingVehicles.find(veh);
740 25705363 : if (i != myApproachingVehicles.end()) {
741 23039265 : return i->second;
742 : } else {
743 : return ApproachingVehicleInformation(INVALID_TIME, INVALID_TIME, 0, 0, false, 0, 0, 0, 0, 0);
744 : }
745 : }
746 :
747 :
748 : void
749 13088 : MSLink::clearState() {
750 : myApproachingVehicles.clear();
751 13088 : }
752 :
753 :
754 : SUMOTime
755 1372153258 : MSLink::getLeaveTime(const SUMOTime arrivalTime, const double arrivalSpeed,
756 : const double leaveSpeed, const double vehicleLength) const {
757 1403544745 : return arrivalTime == SUMOTime_MAX ? SUMOTime_MAX : arrivalTime + TIME2STEPS((getLength() + vehicleLength) / MAX2(0.5 * (arrivalSpeed + leaveSpeed), NUMERICAL_EPS));
758 : }
759 :
760 :
761 : bool
762 604037984 : MSLink::opened(SUMOTime arrivalTime, double arrivalSpeed, double leaveSpeed, double vehicleLength,
763 : double impatience, double decel, SUMOTime waitingTime, double posLat,
764 : BlockingFoes* collectFoes, bool ignoreRed, const SUMOTrafficObject* ego, double dist) const {
765 : #ifdef MSLink_DEBUG_OPENED
766 : if (gDebugFlag1) {
767 : std::cout << SIMTIME << " opened? link=" << getDescription() << " red=" << haveRed() << " cont=" << isCont() << " numFoeLinks=" << myFoeLinks.size() << " havePrio=" << havePriority() << " lastWasContMajorGreen=" << lastWasContState(LINKSTATE_TL_GREEN_MAJOR) << "\n";
768 : }
769 : #endif
770 604037984 : if (haveRed() && !ignoreRed) {
771 : return false;
772 : }
773 603151243 : if (isCont() && MSGlobals::gUsingInternalLanes) {
774 : return true;
775 : }
776 600131233 : const SUMOTime leaveTime = getLeaveTime(arrivalTime, arrivalSpeed, leaveSpeed, vehicleLength);
777 600131233 : if (MSGlobals::gLateralResolution > 0) {
778 : // check for foes on the same lane with the same target edge
779 127210605 : for (const MSLink* foeLink : mySublaneFoeLinks) {
780 : assert(myLane != foeLink->getLane());
781 9549726 : for (const auto& it : foeLink->myApproachingVehicles) {
782 6523589 : const SUMOVehicle* foe = it.first;
783 : if (
784 : // there only is a conflict if the paths cross
785 7020226 : ((posLat < foe->getLateralPositionOnLane() + it.second.latOffset && myLane->getIndex() > foeLink->myLane->getIndex())
786 6314989 : || (posLat > foe->getLateralPositionOnLane() + it.second.latOffset && myLane->getIndex() < foeLink->myLane->getIndex()))
787 : // the vehicle that arrives later must yield
788 7031991 : && (arrivalTime > it.second.arrivalTime
789 : // if both vehicles arrive at the same time, the one
790 : // to the left must yield
791 325366 : || (arrivalTime == it.second.arrivalTime && posLat > foe->getLateralPositionOnLane()))) {
792 183679 : if (blockedByFoe(foe, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, false,
793 : impatience, decel, waitingTime, ego)) {
794 : #ifdef MSLink_DEBUG_OPENED
795 : if (gDebugFlag1) {
796 : std::cout << SIMTIME << " blocked by " << foe->getID() << " arrival=" << arrivalTime << " foeArrival=" << it.second.arrivalTime << "\n";
797 : }
798 : #endif
799 39373 : if (collectFoes == nullptr) {
800 : #ifdef MSLink_DEBUG_OPENED
801 : if (gDebugFlag1) {
802 : std::cout << " link=" << getViaLaneOrLane()->getID() << " blocked by sublaneFoe=" << foe->getID() << " foeLink=" << foeLink->getViaLaneOrLane()->getID() << " posLat=" << posLat << "\n";
803 : }
804 : #endif
805 : return false;
806 : } else {
807 0 : collectFoes->push_back(it.first);
808 : }
809 : }
810 : }
811 : }
812 : }
813 : // check for foes on the same lane with a different target edge
814 : // (straight movers take precedence if the paths cross)
815 124145095 : const int lhSign = MSGlobals::gLefthand ? -1 : 1;
816 125326973 : for (const MSLink* foeLink : mySublaneFoeLinks2) {
817 : assert(myDirection != LinkDirection::STRAIGHT);
818 5425932 : for (const auto& it : foeLink->myApproachingVehicles) {
819 4244054 : const SUMOVehicle* foe = it.first;
820 : // there only is a conflict if the paths cross
821 : // and if the vehicles are not currently in a car-following relationship
822 4244054 : const double egoWidth = ego == nullptr ? 1.8 : ego->getVehicleType().getWidth();
823 4244054 : if (!lateralOverlap(posLat, egoWidth, foe->getLateralPositionOnLane() + it.second.latOffset, foe->getVehicleType().getWidth())
824 4244054 : && (((myDirection == LinkDirection::RIGHT || myDirection == LinkDirection::PARTRIGHT)
825 461931 : && (posLat * lhSign > (foe->getLateralPositionOnLane() + it.second.latOffset) * lhSign))
826 434339 : || ((myDirection == LinkDirection::LEFT || myDirection == LinkDirection::PARTLEFT)
827 41903 : && (posLat * lhSign < (foe->getLateralPositionOnLane() + it.second.latOffset) * lhSign)))) {
828 71979 : if (blockedByFoe(foe, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, false,
829 : impatience, decel, waitingTime, ego)) {
830 : #ifdef MSLink_DEBUG_OPENED
831 : if (gDebugFlag1) {
832 : std::cout << SIMTIME << " blocked by sublane foe " << foe->getID() << " arrival=" << arrivalTime << " foeArrival=" << it.second.arrivalTime << "\n";
833 : }
834 : #endif
835 8621 : if (collectFoes == nullptr) {
836 : #ifdef MSLink_DEBUG_OPENED
837 : if (gDebugFlag1) {
838 : std::cout << " link=" << getViaLaneOrLane()->getID() << " blocked by sublaneFoe2=" << foe->getID() << " foeLink=" << foeLink->getViaLaneOrLane()->getID() << " posLat=" << posLat << "\n";
839 : }
840 : #endif
841 : return false;
842 : } else {
843 0 : collectFoes->push_back(it.first);
844 : }
845 : }
846 : }
847 : }
848 : }
849 : }
850 600083239 : if ((havePriority() || lastWasContState(LINKSTATE_TL_GREEN_MAJOR)) && myState != LINKSTATE_ZIPPER) {
851 : // priority usually means the link is open but there are exceptions:
852 : // zipper still needs to collect foes
853 : // sublane model could have detected a conflict
854 573425278 : return collectFoes == nullptr || collectFoes->size() == 0;
855 : }
856 26657961 : if (myState == LINKSTATE_ALLWAY_STOP && waitingTime < TIME2STEPS(ego == nullptr ? TS : ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_ALLWAYSTOP_WAIT, TS))) {
857 : return false;
858 25618640 : } else if (myState == LINKSTATE_STOP && waitingTime < TIME2STEPS(ego == nullptr ? TS : ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPSIGN_WAIT, TS))) {
859 : return false;
860 : }
861 :
862 25588112 : const std::vector<MSLink*>& foeLinks = (myOffFoeLinks == nullptr || getCorrespondingEntryLink()->getState() != LINKSTATE_ALLWAY_STOP) ? myFoeLinks : *myOffFoeLinks;
863 : #ifdef MSLink_DEBUG_OPENED
864 : if (gDebugFlag1) {
865 : std::cout << SIMTIME << " opened link=" << getViaLaneOrLane()->getID() << " foeLinks=" << foeLinks.size() << "\n";
866 : }
867 : #endif
868 :
869 25588112 : if (MSGlobals::gUseMesoSim && impatience == 1) {
870 : return true;
871 : }
872 25587574 : const bool lastWasContRed = lastWasContState(LINKSTATE_TL_RED);
873 71236478 : for (const MSLink* const link : foeLinks) {
874 50033390 : if (MSGlobals::gUseMesoSim) {
875 1903360 : if (link->haveRed()) {
876 46444 : continue;
877 : }
878 : }
879 : #ifdef MSLink_DEBUG_OPENED
880 : if (gDebugFlag1) {
881 : std::cout << SIMTIME << " foeLink=" << link->getViaLaneOrLane()->getID() << " numApproaching=" << link->getApproaching().size() << "\n";
882 : if (link->getLane()->isCrossing()) {
883 : std::cout << SIMTIME << " approachingPersons=" << (link->myApproachingPersons == nullptr ? "NULL" : toString(link->myApproachingPersons->size())) << "\n";
884 : }
885 : }
886 : #endif
887 49986946 : if (link->blockedAtTime(arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, myLane == link->getLane(),
888 : impatience, decel, waitingTime, collectFoes, ego, lastWasContRed, dist)) {
889 : return false;
890 : }
891 : }
892 21203088 : if (collectFoes != nullptr && collectFoes->size() > 0) {
893 : return false;
894 : }
895 : return true;
896 : }
897 :
898 :
899 : bool
900 49999447 : MSLink::blockedAtTime(SUMOTime arrivalTime, SUMOTime leaveTime, double arrivalSpeed, double leaveSpeed,
901 : bool sameTargetLane, double impatience, double decel, SUMOTime waitingTime,
902 : BlockingFoes* collectFoes, const SUMOTrafficObject* ego, bool lastWasContRed, double dist) const {
903 185754428 : for (const auto& it : myApproachingVehicles) {
904 : #ifdef MSLink_DEBUG_OPENED
905 : if (gDebugFlag1) {
906 : if (ego != nullptr
907 : && ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_SPEED, 0) >= it.second.speed
908 : && ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) > 0) {
909 : std::stringstream stream; // to reduce output interleaving from different threads
910 : stream << SIMTIME << " " << myApproachingVehicles.size() << " foe link=" << getViaLaneOrLane()->getID()
911 : << " foeVeh=" << it.first->getID() << " (below ignore speed)"
912 : << " ignoreFoeProb=" << ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0)
913 : << "\n";
914 : std::cout << stream.str();
915 : }
916 : }
917 : #endif
918 140022580 : if (it.first != ego
919 140017546 : && (ego == nullptr
920 140006023 : || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) == 0
921 17880 : || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_SPEED, 0) < it.second.speed
922 1278 : || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) < RandHelper::rand(ego->getRNG()))
923 140016424 : && !ignoreFoe(ego, it.first)
924 140016182 : && (!lastWasContRed || it.first->getSpeed() > SUMO_const_haltingSpeed)
925 280019872 : && blockedByFoe(it.first, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, sameTargetLane,
926 : impatience, decel, waitingTime, ego)) {
927 6267042 : if (collectFoes == nullptr) {
928 : return true;
929 : } else {
930 1999443 : collectFoes->push_back(it.first);
931 : }
932 : }
933 : }
934 45731848 : if (myApproachingPersons != nullptr && !haveRed()) {
935 961694 : for (const auto& it : *myApproachingPersons) {
936 : if ((ego == nullptr
937 213907 : || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) == 0
938 600 : || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_SPEED, 0) < it.first->getSpeed()
939 600 : || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) < RandHelper::rand(ego->getRNG()))
940 213307 : && !ignoreFoe(ego, it.first)
941 427134 : && !((arrivalTime > it.second.leavingTime) || (leaveTime < it.second.arrivalTime))) {
942 : // check whether braking is feasible (ego might have started to accelerate already)
943 128667 : const auto& cfm = ego->getVehicleType().getCarFollowModel();
944 : #ifdef MSLink_DEBUG_OPENED
945 : if (gDebugFlag1) {
946 : std::cout << SIMTIME << ": " << ego->getID() << " conflict with person " << it.first->getID() << " aTime=" << arrivalTime << " foeATime=" << it.second.arrivalTime << " dist=" << dist << " bGap=" << cfm.brakeGap(ego->getSpeed(), cfm.getMaxDecel(), 0) << "\n";
947 : }
948 : #endif
949 128667 : if (dist > cfm.brakeGap(ego->getSpeed(), cfm.getMaxDecel(), 0)) {
950 : #ifdef MSLink_DEBUG_OPENED
951 : if (gDebugFlag1) {
952 : std::cout << SIMTIME << ": " << ego->getID() << " blocked by person " << it.first->getID() << "\n";
953 : }
954 : #endif
955 117967 : if (collectFoes == nullptr) {
956 : return true;
957 : } else {
958 0 : collectFoes->push_back(it.first);
959 : }
960 : }
961 : }
962 : }
963 : }
964 : return false;
965 : }
966 :
967 :
968 : bool
969 140252950 : MSLink::blockedByFoe(const SUMOVehicle* veh, const ApproachingVehicleInformation& avi,
970 : SUMOTime arrivalTime, SUMOTime leaveTime, double arrivalSpeed, double leaveSpeed,
971 : bool sameTargetLane, double impatience, double decel, SUMOTime waitingTime,
972 : const SUMOTrafficObject* ego) const {
973 : #ifdef MSLink_DEBUG_OPENED
974 : if (gDebugFlag1) {
975 : std::stringstream stream; // to reduce output interleaving from different threads
976 : stream << " link=" << getDescription()
977 : << " foeVeh=" << veh->getID()
978 : << " req=" << avi.willPass
979 : << " aT=" << avi.arrivalTime
980 : << " lT=" << avi.leavingTime
981 : << "\n";
982 : std::cout << stream.str();
983 : }
984 : #endif
985 140252950 : if (!avi.willPass) {
986 : return false;
987 : }
988 45318237 : if (myState == LINKSTATE_ALLWAY_STOP) {
989 : assert(waitingTime > 0);
990 : #ifdef MSLink_DEBUG_OPENED
991 : if (gDebugFlag1) {
992 : std::stringstream stream; // to reduce output interleaving from different threads
993 : stream << " foeDist=" << avi.dist
994 : << " foeBGap=" << veh->getBrakeGap(false)
995 : << " foeWait=" << avi.waitingTime
996 : << " wait=" << waitingTime
997 : << "\n";
998 : std::cout << stream.str();
999 : }
1000 : #endif
1001 : // when using actionSteps, the foe waiting time may be outdated
1002 1462377 : const SUMOTime actionDelta = SIMSTEP - veh->getLastActionTime();
1003 1462377 : if (waitingTime > avi.waitingTime + actionDelta) {
1004 : return false;
1005 : }
1006 232207 : if (waitingTime == (avi.waitingTime + actionDelta) && arrivalTime < avi.arrivalTime + actionDelta) {
1007 : return false;
1008 : }
1009 : }
1010 44055140 : SUMOTime foeArrivalTime = avi.arrivalTime;
1011 44055140 : double foeArrivalSpeedBraking = avi.arrivalSpeedBraking;
1012 44055140 : if (impatience > 0 && arrivalTime < avi.arrivalTime) {
1013 : #ifdef MSLink_DEBUG_OPENED
1014 : gDebugFlag6 = ((ego == nullptr || ego->isSelected()) && (veh == nullptr || veh->isSelected()));
1015 : #endif
1016 2393140 : const SUMOTime fatb = computeFoeArrivalTimeBraking(arrivalTime, veh, avi.arrivalTime, impatience, avi.dist, foeArrivalSpeedBraking);
1017 2393140 : foeArrivalTime = (SUMOTime)((1. - impatience) * (double)avi.arrivalTime + impatience * (double)fatb);
1018 : #ifdef MSLink_DEBUG_OPENED
1019 : if (gDebugFlag6) {
1020 : std::cout << SIMTIME << " link=" << getDescription() << " ego=" << ego->getID() << " foe=" << veh->getID()
1021 : << " at=" << STEPS2TIME(arrivalTime)
1022 : << " fat=" << STEPS2TIME(avi.arrivalTime)
1023 : << " fatb=" << STEPS2TIME(fatb)
1024 : << " fat2=" << STEPS2TIME(foeArrivalTime)
1025 : << "\n";
1026 : }
1027 : #endif
1028 : }
1029 :
1030 :
1031 44055140 : const SUMOTime lookAhead = (myState == LINKSTATE_ZIPPER
1032 44055140 : ? myLookaheadTimeZipper
1033 : : (ego == nullptr
1034 38375644 : ? myLookaheadTime
1035 38372935 : : TIME2STEPS(ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_TIMEGAP_MINOR, STEPS2TIME(myLookaheadTime)))));
1036 : //if (ego != 0) std::cout << SIMTIME << " ego=" << ego->getID() << " jmTimegapMinor=" << ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_TIMEGAP_MINOR, -1) << " lookAhead=" << lookAhead << "\n";
1037 : #ifdef MSLink_DEBUG_OPENED
1038 : if (gDebugFlag1 || gDebugFlag6) {
1039 : std::stringstream stream; // to reduce output interleaving from different threads
1040 : stream << " imp=" << impatience << " fAT2=" << foeArrivalTime << " fASb=" << foeArrivalSpeedBraking << " lA=" << lookAhead << " egoAT=" << arrivalTime << " egoLT=" << leaveTime << " egoLS=" << leaveSpeed << "\n";
1041 : std::cout << stream.str();
1042 : }
1043 : #endif
1044 44055140 : if (avi.leavingTime < arrivalTime) {
1045 : // ego wants to be follower
1046 33064889 : if (sameTargetLane && (arrivalTime - avi.leavingTime < lookAhead
1047 15859326 : || unsafeMergeSpeeds(avi.leaveSpeed, arrivalSpeed,
1048 15859326 : veh->getVehicleType().getCarFollowModel().getMaxDecel(), decel))) {
1049 : #ifdef MSLink_DEBUG_OPENED
1050 : if (gDebugFlag1 || gDebugFlag6) {
1051 : std::cout << " blocked (cannot follow)\n";
1052 : }
1053 : #endif
1054 2004651 : return true;
1055 : }
1056 10990251 : } else if (foeArrivalTime > leaveTime + lookAhead) {
1057 : // ego wants to be leader.
1058 11274127 : if (sameTargetLane && unsafeMergeSpeeds(leaveSpeed, foeArrivalSpeedBraking,
1059 4488854 : decel, veh->getVehicleType().getCarFollowModel().getMaxDecel())) {
1060 : #ifdef MSLink_DEBUG_OPENED
1061 : if (gDebugFlag1 || gDebugFlag6) {
1062 : std::cout << " blocked (cannot lead)\n";
1063 : }
1064 : #endif
1065 : return true;
1066 : }
1067 : } else {
1068 : // even without considering safeHeadwayTime there is already a conflict
1069 : #ifdef MSLink_DEBUG_OPENED
1070 : if (gDebugFlag1 || gDebugFlag6) {
1071 : std::cout << " blocked (hard conflict)\n";
1072 : }
1073 : #endif
1074 : return true;
1075 : }
1076 : return false;
1077 : }
1078 :
1079 :
1080 : SUMOTime
1081 2393140 : MSLink::computeFoeArrivalTimeBraking(SUMOTime arrivalTime, const SUMOVehicle* foe, SUMOTime foeArrivalTime, double impatience, double dist, double& fasb) {
1082 : // a: distance saved when foe brakes from arrivalTime to foeArrivalTime
1083 : // b: distance driven past foeArrivalTime
1084 : // m: permitted decceleration
1085 : // d: total deceleration until foeArrivalTime
1086 : // dist2: distance of foe at arrivalTime
1087 : // actual arrivalTime must fall on a simulation step
1088 2393140 : if (arrivalTime - arrivalTime % DELTA_T == foeArrivalTime - foeArrivalTime % DELTA_T) {
1089 : // foe enters the junction in the same step
1090 : return foeArrivalTime;
1091 : }
1092 2061468 : if (arrivalTime % DELTA_T > 0) {
1093 2034127 : arrivalTime = arrivalTime - (arrivalTime % DELTA_T) + DELTA_T;
1094 : }
1095 : //arrivalTime += DELTA_T - arrivalTime % DELTA_T;
1096 2061468 : const double m = foe->getVehicleType().getCarFollowModel().getMaxDecel() * impatience;
1097 2061468 : const double dt = STEPS2TIME(foeArrivalTime - arrivalTime);
1098 2061468 : const double d = dt * m;
1099 2061468 : const double a = dt * d / 2;
1100 2061468 : const double v = dist / STEPS2TIME(foeArrivalTime - SIMSTEP + DELTA_T);
1101 2061468 : const double dist2 = dist - v * STEPS2TIME(arrivalTime - SIMSTEP);
1102 2061468 : if (0.5 * v * v / m <= dist2) {
1103 1002108 : if (gDebugFlag6) {
1104 0 : std::cout << " dist=" << dist << " dist2=" << dist2 << " at=" << STEPS2TIME(arrivalTime) << " m=" << m << " d=" << d << " a=" << a << " canBrakeToStop\n";
1105 : }
1106 1002108 : fasb = 0;
1107 1002108 : return foeArrivalTime + TIME2STEPS(30);
1108 : }
1109 : // a = b (foe reaches the original distance to the stop line)
1110 : // x: time driven past foeArrivalTime
1111 : // v: foe speed without braking
1112 : // v2: average foe speed after foeArrivalTime (braking continues for time x)
1113 : // v2 = (v - d - x * m / 2)
1114 : // b = v2 * x
1115 : // solving for x gives:
1116 1059360 : const double x = (sqrt(4 * (v - d) * (v - d) - 8 * m * a) * -0.5 - d + v) / m;
1117 :
1118 : #ifdef MSLink_DEBUG_OPENED
1119 : const double x2 = (sqrt(4 * (v - d) * (v - d) - 8 * m * a) * 0.5 - d + v) / m;
1120 : if (gDebugFlag6 || std::isnan(x)) {
1121 : std::cout << SIMTIME << " dist=" << dist << " dist2=" << dist2 << " at=" << STEPS2TIME(arrivalTime) << " m=" << m << " d=" << d << " v=" << v << " a=" << a << " x=" << x << " x2=" << x2 << "\n";
1122 : }
1123 : #endif
1124 1059360 : fasb = v - (dt + x) * m;
1125 1059360 : return foeArrivalTime + TIME2STEPS(x);
1126 : }
1127 :
1128 :
1129 : bool
1130 738992 : MSLink::hasApproachingFoe(SUMOTime arrivalTime, SUMOTime leaveTime, double speed, double decel) const {
1131 750413 : for (const MSLink* const link : myFoeLinks) {
1132 12501 : if (link->blockedAtTime(arrivalTime, leaveTime, speed, speed, myLane == link->getLane(), 0, decel, 0)) {
1133 : return true;
1134 : }
1135 : }
1136 836056 : for (const MSLane* const lane : myFoeLanes) {
1137 107055 : if (lane->getVehicleNumberWithPartials() > 0) {
1138 : return true;
1139 : }
1140 : }
1141 : return false;
1142 : }
1143 :
1144 :
1145 : std::pair<const SUMOVehicle*, const MSLink*>
1146 5696 : MSLink::getFirstApproachingFoe(const MSLink* wrapAround) const {
1147 : double closetDist = std::numeric_limits<double>::max();
1148 : const SUMOVehicle* closest = nullptr;
1149 : const MSLink* foeLink = nullptr;
1150 18596 : for (MSLink* link : myFoeLinks) {
1151 19860 : for (const auto& it : link->myApproachingVehicles) {
1152 : //std::cout << " link=" << getDescription() << " foeLink_in=" << link->getLaneBefore()->getID() << " wrapAround=" << wrapAround->getDescription() << "\n";
1153 6960 : if (link->getLaneBefore() == wrapAround->getLaneBefore()) {
1154 488 : return std::make_pair(nullptr, wrapAround);
1155 6472 : } else if (it.second.dist < closetDist) {
1156 : closetDist = it.second.dist;
1157 3450 : if (it.second.willPass) {
1158 3184 : closest = it.first;
1159 : foeLink = link;
1160 : }
1161 : }
1162 : }
1163 : }
1164 : return std::make_pair(closest, foeLink);
1165 : }
1166 :
1167 :
1168 : void
1169 82399772 : MSLink::setTLState(LinkState state, SUMOTime t) {
1170 82399772 : if (myState != state) {
1171 14205051 : myLastStateChange = t;
1172 : }
1173 82399772 : myState = state;
1174 82399772 : if (haveGreen()) {
1175 63646069 : myLastGreenState = myState;
1176 : }
1177 82399772 : }
1178 :
1179 :
1180 : void
1181 195048 : MSLink::setTLLogic(const MSTrafficLightLogic* logic) {
1182 195048 : myLogic = logic;
1183 195048 : }
1184 :
1185 :
1186 : bool
1187 1134178052 : MSLink::isCont() const {
1188 : // when a traffic light is switched off minor roads have their cont status revoked
1189 1134178052 : return (myState == LINKSTATE_TL_OFF_BLINKING || myState == LINKSTATE_ALLWAY_STOP) ? myAmContOff : myAmCont;
1190 : }
1191 :
1192 :
1193 : bool
1194 55384643 : MSLink::lastWasContMajor() const {
1195 55384643 : if (myInternalLane == nullptr || myAmCont) {
1196 : return false;
1197 : } else {
1198 42075356 : MSLane* pred = myInternalLane->getLogicalPredecessorLane();
1199 42075356 : if (!pred->getEdge().isInternal()) {
1200 : return false;
1201 : } else {
1202 7886343 : const MSLane* const pred2 = pred->getLogicalPredecessorLane();
1203 : assert(pred2 != nullptr);
1204 7886343 : const MSLink* const predLink = pred2->getLinkTo(pred);
1205 : assert(predLink != nullptr);
1206 7886343 : if (predLink->havePriority()) {
1207 : return true;
1208 : }
1209 7430988 : if (myHavePedestrianCrossingFoe) {
1210 562026 : return predLink->getLastGreenState() == LINKSTATE_TL_GREEN_MAJOR;
1211 : } else {
1212 6868962 : return predLink->haveYellow();
1213 : }
1214 : }
1215 : }
1216 : }
1217 :
1218 :
1219 : bool
1220 51302715 : MSLink::lastWasContState(LinkState linkState) const {
1221 51302715 : if (myInternalLane == nullptr || myAmCont || myHavePedestrianCrossingFoe) {
1222 : return false;
1223 : } else {
1224 44413170 : MSLane* pred = myInternalLane->getLogicalPredecessorLane();
1225 44413170 : if (!pred->getEdge().isInternal()) {
1226 : return false;
1227 : } else {
1228 10790368 : const MSLane* const pred2 = pred->getLogicalPredecessorLane();
1229 : assert(pred2 != nullptr);
1230 10790368 : const MSLink* const predLink = pred2->getLinkTo(pred);
1231 : assert(predLink != nullptr);
1232 10790368 : return predLink->getState() == linkState;
1233 : }
1234 : }
1235 : }
1236 :
1237 :
1238 : void
1239 81324 : MSLink::writeApproaching(OutputDevice& od, const std::string fromLaneID) const {
1240 81324 : if (myApproachingVehicles.size() > 0) {
1241 18080 : od.openTag("link");
1242 : od.writeAttr(SUMO_ATTR_FROM, fromLaneID);
1243 11408 : const std::string via = getViaLane() == nullptr ? "" : getViaLane()->getID();
1244 : od.writeAttr(SUMO_ATTR_VIA, via);
1245 18080 : od.writeAttr(SUMO_ATTR_TO, getLane() == nullptr ? "" : getLane()->getID());
1246 : std::vector<std::pair<SUMOTime, const SUMOVehicle*> > toSort; // stabilize output
1247 19578 : for (auto it : myApproachingVehicles) {
1248 10538 : toSort.push_back(std::make_pair(it.second.arrivalTime, it.first));
1249 : }
1250 9040 : std::sort(toSort.begin(), toSort.end());
1251 19578 : for (std::vector<std::pair<SUMOTime, const SUMOVehicle*> >::const_iterator it = toSort.begin(); it != toSort.end(); ++it) {
1252 10538 : od.openTag("approaching");
1253 10538 : const ApproachingVehicleInformation& avi = myApproachingVehicles.find(it->second)->second;
1254 10538 : od.writeAttr(SUMO_ATTR_ID, it->second->getID());
1255 10538 : od.writeAttr(SUMO_ATTR_IMPATIENCE, it->second->getImpatience());
1256 21076 : od.writeAttr("arrivalTime", time2string(avi.arrivalTime));
1257 21076 : od.writeAttr("leaveTime", time2string(avi.leavingTime));
1258 21076 : od.writeAttr("arrivalSpeed", toString(avi.arrivalSpeed));
1259 21076 : od.writeAttr("arrivalSpeedBraking", toString(avi.arrivalSpeedBraking));
1260 21076 : od.writeAttr("leaveSpeed", toString(avi.leaveSpeed));
1261 21076 : od.writeAttr("willPass", toString(avi.willPass));
1262 21076 : od.closeTag();
1263 : }
1264 9040 : od.closeTag();
1265 9040 : }
1266 81324 : }
1267 :
1268 :
1269 : double
1270 886303 : MSLink::getInternalLengthsAfter() const {
1271 : double len = 0.;
1272 886303 : MSLane* lane = myInternalLane;
1273 :
1274 1693913 : while (lane != nullptr && lane->isInternal()) {
1275 807610 : len += lane->getLength();
1276 807610 : lane = lane->getLinkCont()[0]->getViaLane();
1277 : }
1278 886303 : return len;
1279 : }
1280 :
1281 : double
1282 0 : MSLink::getInternalLengthsBefore() const {
1283 : double len = 0.;
1284 0 : const MSLane* lane = myInternalLane;
1285 :
1286 0 : while (lane != nullptr && lane->isInternal()) {
1287 0 : len += lane->getLength();
1288 0 : if (lane->getIncomingLanes().size() == 1) {
1289 0 : lane = lane->getIncomingLanes()[0].lane;
1290 : } else {
1291 : break;
1292 : }
1293 : }
1294 0 : return len;
1295 : }
1296 :
1297 :
1298 : double
1299 13038 : MSLink::getLengthsBeforeCrossing(const MSLane* foeLane) const {
1300 13038 : MSLane* via = myInternalLane;
1301 : double totalDist = 0.;
1302 : bool foundCrossing = false;
1303 15506 : while (via != nullptr) {
1304 14272 : MSLink* link = via->getLinkCont()[0];
1305 14272 : double dist = link->getLengthBeforeCrossing(foeLane);
1306 14272 : if (dist != INVALID_DOUBLE) {
1307 : // found conflicting lane
1308 11804 : totalDist += dist;
1309 : foundCrossing = true;
1310 : break;
1311 : } else {
1312 2468 : totalDist += via->getLength();
1313 : via = link->getViaLane();
1314 : }
1315 : }
1316 : if (foundCrossing) {
1317 11804 : return totalDist;
1318 : } else {
1319 : return INVALID_DOUBLE;
1320 : }
1321 : }
1322 :
1323 :
1324 : double
1325 14272 : MSLink::getLengthBeforeCrossing(const MSLane* foeLane) const {
1326 : int foe_ix;
1327 80108 : for (foe_ix = 0; foe_ix != (int)myFoeLanes.size(); ++foe_ix) {
1328 78874 : if (myFoeLanes[foe_ix] == foeLane) {
1329 : break;
1330 : }
1331 : }
1332 14272 : if (foe_ix == (int)myFoeLanes.size()) {
1333 : // no conflict with the given lane, indicate by returning -1
1334 : #ifdef MSLink_DEBUG_CROSSING_POINTS
1335 : std::cout << "No crossing of lanes '" << foeLane->getID() << "' and '" << myInternalLaneBefore->getID() << "'" << std::endl;
1336 : #endif
1337 : return INVALID_DOUBLE;
1338 : } else {
1339 : // found conflicting lane index
1340 13038 : double dist = myInternalLaneBefore->getLength() - myConflicts[foe_ix].getLengthBehindCrossing(this);
1341 13038 : if (dist == -10000.) {
1342 : // this is the value in myConflicts, if the relation allows intersection but none is present for the actual geometry.
1343 : return INVALID_DOUBLE;
1344 : }
1345 : #ifdef MSLink_DEBUG_CROSSING_POINTS
1346 : std::cout << "Crossing of lanes '" << myInternalLaneBefore->getID() << "' and '" << foeLane->getID()
1347 : << "' at distance " << dist << " (approach along '"
1348 : << myInternalLaneBefore->getEntryLink()->getLaneBefore()->getID() << "')" << std::endl;
1349 : #endif
1350 : return dist;
1351 : }
1352 : }
1353 :
1354 :
1355 : bool
1356 33723639 : MSLink::isEntryLink() const {
1357 33723639 : if (MSGlobals::gUsingInternalLanes) {
1358 54619067 : return myInternalLane != nullptr && myInternalLaneBefore == nullptr;
1359 : } else {
1360 : return false;
1361 : }
1362 : }
1363 :
1364 : bool
1365 16815416 : MSLink::isConflictEntryLink() const {
1366 : // either a non-cont entry link or the link after a cont-link
1367 16815416 : return !myAmCont && (isEntryLink() || (myInternalLaneBefore != nullptr && myInternalLane != nullptr));
1368 : }
1369 :
1370 : bool
1371 86167049 : MSLink::isExitLink() const {
1372 86167049 : if (MSGlobals::gUsingInternalLanes) {
1373 148431322 : return myInternalLaneBefore != nullptr && myInternalLane == nullptr;
1374 : } else {
1375 : return false;
1376 : }
1377 : }
1378 :
1379 : bool
1380 42626487 : MSLink::isExitLinkAfterInternalJunction() const {
1381 42626487 : if (MSGlobals::gUsingInternalLanes) {
1382 : return (getInternalLaneBefore() != nullptr
1383 42626487 : && myInternalLaneBefore->getIncomingLanes().size() == 1
1384 85252974 : && myInternalLaneBefore->getIncomingLanes().front().viaLink->isInternalJunctionLink());
1385 : } else {
1386 : return false;
1387 : }
1388 : }
1389 :
1390 :
1391 : const MSLink*
1392 20300 : MSLink::getCorrespondingExitLink() const {
1393 20300 : MSLane* lane = myInternalLane;
1394 : const MSLink* link = this;
1395 41654 : while (lane != nullptr) {
1396 21354 : link = lane->getLinkCont()[0];
1397 : lane = link->getViaLane();
1398 : }
1399 20300 : return link;
1400 : }
1401 :
1402 :
1403 : const MSLink*
1404 719626301 : MSLink::getCorrespondingEntryLink() const {
1405 : const MSLink* link = this;
1406 957020606 : while (link->myLaneBefore->isInternal()) {
1407 : assert(myLaneBefore->getIncomingLanes().size() == 1);
1408 237394305 : link = link->myLaneBefore->getIncomingLanes().front().viaLink;
1409 : }
1410 719626301 : return link;
1411 : }
1412 :
1413 :
1414 : bool
1415 316511166 : MSLink::isInternalJunctionLink() const {
1416 316511166 : return getInternalLaneBefore() != nullptr && myInternalLane != nullptr;
1417 : }
1418 :
1419 :
1420 : const MSLink::LinkLeaders
1421 779915572 : MSLink::getLeaderInfo(const MSVehicle* ego, double dist, std::vector<const MSPerson*>* collectBlockers, bool isShadowLink) const {
1422 : LinkLeaders result;
1423 : // this link needs to start at an internal lane (either an exit link or between two internal lanes)
1424 : // or it must be queried by the pedestrian model (ego == 0)
1425 779915572 : if (ego != nullptr && (!fromInternalLane() || ego->getLaneChangeModel().isOpposite())) {
1426 : // ignore link leaders
1427 : return result;
1428 : }
1429 : //gDebugFlag1 = true;
1430 253835495 : if (gDebugFlag1) {
1431 0 : std::cout << SIMTIME << " getLeaderInfo link=" << getDescription() << " dist=" << dist << " isShadowLink=" << isShadowLink << "\n";
1432 : }
1433 253835495 : if (MSGlobals::gComputeLC && ego != nullptr && ego->getLane()->isNormal()) {
1434 21394053 : const MSLink* junctionEntry = getLaneBefore()->getEntryLink();
1435 4331361 : if (junctionEntry->haveRed() && !ego->ignoreRed(junctionEntry, true)
1436 : // check oncoming on bidiLane during laneChanging
1437 25721380 : && (!MSGlobals::gComputeLC || junctionEntry->getLaneBefore()->getBidiLane() == nullptr)) {
1438 4290397 : if (gDebugFlag1) {
1439 0 : std::cout << " ignore linkLeaders beyond red light\n";
1440 : }
1441 : return result;
1442 : }
1443 : }
1444 : // this is an exit link
1445 598190756 : for (int i = 0; i < (int)myFoeLanes.size(); ++i) {
1446 348645661 : const MSLane* foeLane = myFoeLanes[i];
1447 348645661 : const MSLink* foeExitLink = foeLane->getLinkCont()[0];
1448 : // distance from the querying vehicle to the crossing point with foeLane
1449 348645661 : double distToCrossing = dist - myConflicts[i].getLengthBehindCrossing(this);
1450 348645661 : const double foeDistToCrossing = foeLane->getLength() - myConflicts[i].getFoeLengthBehindCrossing(foeExitLink);
1451 348645661 : const bool sameTarget = (myLane == foeExitLink->getLane()) && !isInternalJunctionLink() && !foeExitLink->isInternalJunctionLink();
1452 348645661 : const bool sameSource = (myInternalLaneBefore != nullptr && myInternalLaneBefore->getLogicalPredecessorLane() == foeLane->getLogicalPredecessorLane());
1453 348645661 : const double crossingWidth = (sameTarget || sameSource) ? 0 : myConflicts[i].conflictSize;
1454 348645661 : const double foeCrossingWidth = (sameTarget || sameSource) ? 0 : myConflicts[i].getFoeConflictSize(foeExitLink);
1455 : // special treatment of contLane foe only applies if this lane is not a contLane or contLane follower itself
1456 439369659 : const bool contLane = (foeExitLink->getViaLaneOrLane()->getEdge().isInternal() && !(
1457 90723998 : isInternalJunctionLink() || isExitLinkAfterInternalJunction()));
1458 348645661 : if (gDebugFlag1) {
1459 : std::cout << " distToCrossing=" << distToCrossing << " foeLane=" << foeLane->getID() << " cWidth=" << crossingWidth
1460 0 : << " ijl=" << isInternalJunctionLink() << " sT=" << sameTarget << " sS=" << sameSource
1461 0 : << " lbc=" << myConflicts[i].getLengthBehindCrossing(this)
1462 0 : << " flbc=" << myConflicts[i].getFoeLengthBehindCrossing(foeExitLink)
1463 : << " cw=" << crossingWidth
1464 : << " fcw=" << foeCrossingWidth
1465 : << " contLane=" << contLane
1466 0 : << " state=" << toString(myState)
1467 0 : << " foeState=" << toString(foeExitLink->getState())
1468 0 : << "\n";
1469 : }
1470 84292844 : if (distToCrossing + crossingWidth < 0 && !sameTarget
1471 428123248 : && (ego == nullptr || !MSGlobals::gComputeLC || distToCrossing + crossingWidth + ego->getVehicleType().getLength() < 0)) {
1472 76373803 : continue; // vehicle is behind the crossing point, continue with next foe lane
1473 : }
1474 : bool ignoreGreenCont = false;
1475 : bool foeIndirect = false;
1476 272271858 : if (contLane) {
1477 26991546 : const MSLink* entry = getLaneBefore()->getEntryLink();
1478 26991546 : const MSLink* foeEntry = foeLane->getEntryLink();
1479 26991546 : foeIndirect = foeEntry->myAmIndirect;
1480 26977265 : if (entry != nullptr && entry->haveGreen()
1481 15093303 : && foeEntry != nullptr && foeEntry->haveGreen()
1482 34205338 : && entry->myLaneBefore != foeEntry->myLaneBefore) {
1483 : // ignore vehicles before an internaljunction as long as they are still in green minor mode
1484 : ignoreGreenCont = true;
1485 : }
1486 : }
1487 26991546 : if (foeIndirect && distToCrossing >= NO_INTERSECTION) {
1488 32543 : if (gDebugFlag1) {
1489 0 : std::cout << " ignore:noIntersection\n";
1490 : }
1491 32543 : continue;
1492 : }
1493 : // it is not sufficient to return the last vehicle on the foeLane because ego might be its leader
1494 : // therefore we return all vehicles on the lane
1495 : //
1496 : // special care must be taken for continuation lanes. (next lane is also internal)
1497 : // vehicles on cont. lanes or on internal lanes with the same target as this link can not be ignored
1498 : // and should block (gap = -1) unless they are part of an indirect turn
1499 : MSLane::AnyVehicleIterator end = foeLane->anyVehiclesEnd();
1500 24183790 : for (MSLane::AnyVehicleIterator it_veh = foeLane->anyVehiclesBegin(); it_veh != end; ++it_veh) {
1501 24183793 : MSVehicle* leader = (MSVehicle*)*it_veh;
1502 24183793 : const double leaderBack = leader->getBackPositionOnLane(foeLane);
1503 24183793 : const double leaderBackDist = foeDistToCrossing - leaderBack;
1504 24183793 : const double l2 = ego != nullptr ? ego->getLength() + 2 : 0; // add some slack to account for further meeting-angle effects
1505 24092965 : const double sagitta = ego != nullptr && myRadius != std::numeric_limits<double>::max() ? myRadius - sqrt(myRadius * myRadius - 0.25 * l2 * l2) : 0;
1506 24183793 : const bool pastTheCrossingPoint = leaderBackDist + foeCrossingWidth + sagitta < 0;
1507 24183793 : const bool enteredTheCrossingPoint = leaderBackDist < leader->getVehicleType().getLength();
1508 24183793 : const bool foeIsBicycleTurn = (leader->getVehicleType().getVehicleClass() == SVC_BICYCLE
1509 24183793 : && foeLane->getIncomingLanes().front().viaLink->getDirection() == LinkDirection::LEFT);
1510 24183793 : const bool ignoreIndirectBicycleTurn = pastTheCrossingPoint && foeIsBicycleTurn;
1511 24183793 : const bool cannotIgnore = ((contLane && !ignoreIndirectBicycleTurn) || sameTarget || sameSource) && ego != nullptr;
1512 24183793 : const bool inTheWay = ((((!pastTheCrossingPoint && distToCrossing > 0) || (sameTarget && distToCrossing > leaderBackDist - leader->getLength()))
1513 19661428 : && enteredTheCrossingPoint
1514 10726264 : && (!foeExitLink->isInternalJunctionLink() || foeIsBicycleTurn))
1515 38063447 : || foeExitLink->getLaneBefore()->getNormalPredecessorLane() == myLane->getBidiLane());
1516 24183793 : const bool isOpposite = leader->getLaneChangeModel().isOpposite();
1517 24183793 : const auto avi = foeExitLink->getApproaching(leader);
1518 : // if leader is not found, assume that it performed a lane change in the last step
1519 24183793 : const bool willPass = avi.willPass || (avi.arrivalTime == INVALID_TIME && sameTarget);
1520 24183793 : if (gDebugFlag1) {
1521 : std::cout << " candidate leader=" << leader->getID()
1522 : << " cannotIgnore=" << cannotIgnore
1523 : << " fdtc=" << foeDistToCrossing
1524 : << " lb=" << leaderBack
1525 : << " lbd=" << leaderBackDist
1526 : << " fcwidth=" << foeCrossingWidth
1527 0 : << " r=" << myRadius
1528 : << " sagitta=" << sagitta
1529 : << " foePastCP=" << pastTheCrossingPoint
1530 : << " foeEnteredCP=" << enteredTheCrossingPoint
1531 : << " inTheWay=" << inTheWay
1532 : << " willPass=" << willPass
1533 0 : << " isFrontOnLane=" << leader->isFrontOnLane(foeLane)
1534 : << " ignoreGreenCont=" << ignoreGreenCont
1535 : << " foeIndirect=" << foeIndirect
1536 : << " foeBikeTurn=" << foeIsBicycleTurn
1537 0 : << " isOpposite=" << isOpposite << "\n";
1538 : }
1539 24183793 : if (leader == ego) {
1540 6159220 : continue;
1541 : }
1542 : // ignore greenCont foe vehicles that are not in the way
1543 23601069 : if (!inTheWay && ignoreGreenCont) {
1544 8235 : if (gDebugFlag1) {
1545 0 : std::cout << " ignoreGreenCont\n";
1546 : }
1547 8235 : continue;
1548 : }
1549 : // after entering the conflict area, ignore foe vehicles that are not in the way
1550 2877184 : if ((!MSGlobals::gComputeLC || (ego != nullptr && ego->getLane() == foeLane) || MSGlobals::gSublane)
1551 22477596 : && distToCrossing < -POSITION_EPS && !inTheWay
1552 23878033 : && (ego == nullptr || !MSGlobals::gComputeLC || distToCrossing < -ego->getVehicleType().getLength())) {
1553 142386 : if (gDebugFlag1) {
1554 0 : std::cout << " ego entered conflict area\n";
1555 : }
1556 142386 : continue;
1557 : }
1558 23508025 : if (!MSGlobals::gComputeLC
1559 20619189 : && sameSource
1560 5192777 : && &ego->getLane()->getEdge() == &myInternalLaneBefore->getEdge()
1561 23742376 : && leaderBack + leader->getLength() < ego->getPositionOnLane() - ego->getLength()) {
1562 : // ego is already on the junction and clearly ahead of foe
1563 57577 : if (gDebugFlag1) {
1564 0 : std::cout << " ego ahead of same-source foe\n";
1565 : }
1566 57577 : continue;
1567 : }
1568 :
1569 : // ignore foe vehicles that will not pass
1570 15913635 : if ((!cannotIgnore || leader->isStopped() || sameTarget)
1571 16359687 : && !willPass
1572 1130730 : && leader->isFrontOnLane(foeLane)
1573 : && !isOpposite
1574 614642 : && !inTheWay
1575 : // willPass is false if the vehicle is already on the stopping edge
1576 23839142 : && !leader->willStop()) {
1577 445608 : if (gDebugFlag1) {
1578 0 : std::cout << " foe will not pass\n";
1579 : }
1580 445608 : continue;
1581 : }
1582 22947263 : if (leader->isBidiOn(foeLane)) {
1583 : // conflict resolved via forward lane of the foe
1584 45099 : continue;
1585 : }
1586 : // check whether foe is blocked and might need to change before leaving the junction
1587 22902164 : const bool foeStrategicBlocked = (leader->getLaneChangeModel().isStrategicBlocked() &&
1588 1143262 : leader->getCarFollowModel().brakeGap(leader->getSpeed()) <= foeLane->getLength() - leaderBack);
1589 22902164 : const bool sameInternalEdge = &myInternalLaneBefore->getEdge() == &foeExitLink->getInternalLaneBefore()->getEdge();
1590 :
1591 22902164 : const bool foeLaneIsBidi = myInternalLaneBefore->getBidiLane() == foeLane;
1592 22902164 : if (MSGlobals::gSublane && ego != nullptr && (sameSource || sameTarget || foeLaneIsBidi)
1593 6075162 : && (!foeStrategicBlocked || sameInternalEdge)) {
1594 5923747 : if (ego->getLane() == leader->getLane()) {
1595 125194 : continue;
1596 : }
1597 : // ignore vehicles if not in conflict sublane-wise
1598 5798553 : const double egoLatOffset = isShadowLink ? ego->getLatOffset(ego->getLaneChangeModel().getShadowLane()) : 0;
1599 5798553 : const double posLat = ego->getLateralPositionOnLane() + egoLatOffset;
1600 5798553 : double posLatLeader = leader->getLateralPositionOnLane() + leader->getLatOffset(foeLane);
1601 5798553 : if (foeLaneIsBidi) {
1602 : // leader is oncoming
1603 0 : posLatLeader = foeLane->getWidth() - posLatLeader;
1604 : }
1605 5798553 : const double latGap = (fabs(posLat - posLatLeader)
1606 5798553 : - 0.5 * (ego->getVehicleType().getWidth() + leader->getVehicleType().getWidth()));
1607 5798553 : const double maneuverDist = leader->getLaneChangeModel().getManeuverDist() * (posLat < posLatLeader ? -1 : 1);
1608 5798553 : if (gDebugFlag1) {
1609 0 : std::cout << " checkIgnore sublaneFoe lane=" << myInternalLaneBefore->getID()
1610 : << " sameSource=" << sameSource
1611 : << " sameTarget=" << sameTarget
1612 : << " foeLaneIsBidi=" << foeLaneIsBidi
1613 : << " foeLane=" << foeLane->getID()
1614 : << " leader=" << leader->getID()
1615 0 : << " egoLane=" << ego->getLane()->getID()
1616 0 : << " leaderLane=" << leader->getLane()->getID()
1617 : << " egoLat=" << posLat
1618 : << " egoLatOffset=" << egoLatOffset
1619 : << " leaderLat=" << posLatLeader
1620 0 : << " leaderLatOffset=" << leader->getLatOffset(foeLane)
1621 : << " latGap=" << latGap
1622 : << " maneuverDist=" << maneuverDist
1623 0 : << " computeLC=" << MSGlobals::gComputeLC
1624 0 : << " egoMaxSpeedLat=" << ego->getVehicleType().getMaxSpeedLat()
1625 0 : << "\n";
1626 : }
1627 1059079 : if (latGap > 0 && (latGap > maneuverDist || !sameTarget || !MSGlobals::gComputeLC)
1628 : // do not perform sublane changes that interfere with the leader vehicle
1629 6849399 : && (!MSGlobals::gComputeLC || latGap > ego->getVehicleType().getMaxSpeedLat())) {
1630 919065 : const MSLink* foeEntryLink = foeLane->getIncomingLanes().front().viaLink;
1631 919065 : if (sameSource) {
1632 : // for lanes from the same edge, higer index implies a
1633 : // connection further to the left
1634 608283 : const bool leaderFromRight = (myIndex > foeEntryLink->getIndex());
1635 608283 : if ((posLat > posLatLeader) == leaderFromRight) {
1636 : // ignore speed since lanes diverge
1637 334589 : if (gDebugFlag1) {
1638 0 : std::cout << " ignored (same source) leaderFromRight=" << leaderFromRight << "\n";
1639 : }
1640 334589 : continue;
1641 : }
1642 310782 : } else if (sameTarget) {
1643 : // for lanes from different edges we cannot rely on the
1644 : // index due to wrap-around issues
1645 310782 : if (myDirection != foeEntryLink->getDirection()) {
1646 298110 : bool leaderFromRight = foeEntryLink->getDirection() < myDirection;
1647 : // leader vehicle should not move towards ego
1648 298110 : if (MSGlobals::gLefthand) {
1649 0 : leaderFromRight = !leaderFromRight;
1650 : }
1651 409913 : if ((posLat > posLatLeader) == leaderFromRight
1652 : // leader should keep lateral position or move away from ego
1653 165280 : && (leader->getLaneChangeModel().getSpeedLat() == 0
1654 54119 : || leaderFromRight == (leader->getLaneChangeModel().getSpeedLat() < latGap))
1655 417522 : && (ego->getLaneChangeModel().getSpeedLat() == 0
1656 16025 : || leaderFromRight == (ego->getLaneChangeModel().getSpeedLat() > latGap))) {
1657 111803 : if (gDebugFlag1) {
1658 0 : std::cout << " ignored (different source) leaderFromRight=" << leaderFromRight << "\n";
1659 : }
1660 111803 : continue;
1661 : }
1662 : } else {
1663 : // XXX figure out relative direction somehow
1664 : }
1665 : } else {
1666 0 : if (gDebugFlag1) {
1667 0 : std::cout << " ignored oncoming bidi leader\n";
1668 : }
1669 0 : continue;
1670 : }
1671 : }
1672 : }
1673 22330578 : if (leader->getWaitingTime() < MSGlobals::gIgnoreJunctionBlocker) {
1674 : // compute distance between vehicles on the superimposition of both lanes
1675 : // where the crossing point is the common point
1676 : double gap;
1677 : bool fromLeft = true;
1678 21896730 : if (ego == nullptr) {
1679 : // request from pedestrian model. return distance between leaderBack and crossing point
1680 : //std::cout << " foeLane=" << foeLane->getID() << " leaderBack=" << leaderBack << " foeDistToCrossing=" << foeDistToCrossing << " foeLength=" << foeLane->getLength() << " foebehind=" << myConflicts[i].second << " dist=" << dist << " behind=" << myConflicts[i].first << "\n";
1681 90419 : gap = leaderBackDist;
1682 : // distToCrossing should not take into account the with of the foe lane
1683 : // (which was subtracted in setRequestInformation)
1684 : // Instead, the width of the foe vehicle is used directly by the caller.
1685 90419 : distToCrossing += myConflicts[i].conflictSize / 2;
1686 90419 : if (gap + foeCrossingWidth < 0) {
1687 : // leader is completely past the crossing point
1688 : // or there is no crossing point
1689 3723281 : continue; // next vehicle
1690 : }
1691 : // we need to determine whether the vehicle passes the
1692 : // crossing from the left or the right (heuristic)
1693 89196 : fromLeft = foeDistToCrossing > 0.5 * foeLane->getLength();
1694 21806311 : } else if ((contLane && !sameSource && !ignoreIndirectBicycleTurn) || isOpposite) {
1695 969952 : gap = -std::numeric_limits<double>::max(); // always break for vehicles which are on a continuation lane or for opposite-direction vehicles
1696 : } else {
1697 20836359 : if (pastTheCrossingPoint && !sameTarget) {
1698 : // leader is completely past the crossing point
1699 : // or there is no crossing point
1700 3721979 : if (gDebugFlag1) {
1701 0 : std::cout << " foePastCP ignored\n";
1702 : }
1703 3721979 : continue;
1704 : }
1705 : double leaderBackDist2 = leaderBackDist;
1706 17114380 : if (sameTarget && leaderBackDist2 < 0) {
1707 2946811 : const double mismatch = myConflicts[i].getFoeLengthBehindCrossing(foeExitLink) - myConflicts[i].getLengthBehindCrossing(this);
1708 2946811 : if (mismatch > 0) {
1709 1465912 : leaderBackDist2 += mismatch;
1710 : }
1711 : }
1712 17114380 : if (gDebugFlag1) {
1713 : std::cout << " distToCrossing=" << distToCrossing << " leaderBack=" << leaderBack
1714 : << " backDist=" << leaderBackDist
1715 : << " backDist2=" << leaderBackDist2
1716 0 : << " blockedStrategic=" << leader->getLaneChangeModel().isStrategicBlocked()
1717 0 : << "\n";
1718 : }
1719 17114380 : gap = distToCrossing - ego->getVehicleType().getMinGap() - leaderBackDist2 - foeCrossingWidth;
1720 : }
1721 : // if the foe is already moving off the intersection, we may
1722 : // advance up to the crossing point unless we have the same target or same source
1723 : // (for sameSource, the crossing point indicates the point of divergence)
1724 18173528 : const bool stopAsap = leader->isFrontOnLane(foeLane) ? cannotIgnore : (sameTarget || sameSource);
1725 18173528 : if (gDebugFlag1) {
1726 0 : std::cout << " leader=" << leader->getID() << " contLane=" << contLane << " cannotIgnore=" << cannotIgnore << " stopAsap=" << stopAsap << " gap=" << gap << "\n";
1727 : }
1728 18173528 : if (ignoreFoe(ego, leader)) {
1729 79 : continue;
1730 : }
1731 18173449 : const int llFlags = ((fromLeft ? LL_FROM_LEFT : 0) |
1732 18173449 : (inTheWay ? LL_IN_THE_WAY : 0) |
1733 18173449 : (sameSource ? LL_SAME_SOURCE : 0) |
1734 18173449 : (sameTarget ? LL_SAME_TARGET : 0));
1735 23696925 : result.emplace_back(leader, gap, stopAsap ? -1 : distToCrossing, llFlags, leader->getLatOffset(foeLane));
1736 : }
1737 :
1738 : }
1739 272239312 : if (ego != nullptr && MSNet::getInstance()->hasPersons()) {
1740 : // check for crossing pedestrians (keep driving if already on top of the crossing
1741 4458573 : const double distToPeds = distToCrossing - ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPLINE_CROSSING_GAP, MSPModel::SAFETY_GAP);
1742 4458573 : const double vehWidth = ego->getVehicleType().getWidth() + MSPModel::SAFETY_GAP; // + configurable safety gap
1743 : /// @todo consider lateral position (depending on whether the crossing is encountered on the way in or out)
1744 : // @check lefthand?!
1745 4458573 : const bool wayIn = myConflicts[i].lengthBehindCrossing < myLaneBefore->getLength() * 0.5;
1746 4458573 : const double vehSideOffset = (foeDistToCrossing + myLaneBefore->getWidth() * 0.5 - vehWidth * 0.5
1747 4458573 : + ego->getLateralPositionOnLane() * (wayIn ? -1 : 1));
1748 : // can access the movement model here since we already checked for existing persons above
1749 8558824 : if (distToPeds >= -MSPModel::SAFETY_GAP && MSNet::getInstance()->getPersonControl().getMovementModel()->blockedAtDist(ego, foeLane, vehSideOffset, vehWidth,
1750 4100251 : ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_CROSSING_GAP, JM_CROSSING_GAP_DEFAULT),
1751 : collectBlockers)) {
1752 593179 : result.emplace_back(nullptr, -1, distToPeds);
1753 3865394 : } else if (foeLane->isCrossing() && ego->getLane()->isInternal() && ego->getLane()->getEdge().getToJunction() == myJunction) {
1754 131469 : const MSLink* crossingLink = foeLane->getIncomingLanes()[0].viaLink;
1755 131469 : if (distToCrossing > 0 && crossingLink->havePriority() && crossingLink->myApproachingPersons != nullptr) {
1756 : // a person might step on the crossing at any moment, since ego
1757 : // is already on the junction, the opened() check is not done anymore
1758 24386 : const double timeToEnterCrossing = distToCrossing / MAX2(ego->getSpeed(), 1.0);
1759 29856 : for (const auto& item : (*crossingLink->myApproachingPersons)) {
1760 6289 : if (!ignoreFoe(ego, item.first) && timeToEnterCrossing > STEPS2TIME(item.second.arrivalTime - SIMSTEP)) {
1761 819 : if (gDebugFlag1) {
1762 0 : std::cout << SIMTIME << ": " << ego->getID() << " breaking for approaching person " << item.first->getID()
1763 : //<< " dtc=" << distToCrossing << " ttc=" << distToCrossing / MAX2(ego->getSpeed(), 1.0) << " foeAT=" << item.second.arrivalTime << " foeTTC=" << STEPS2TIME(item.second.arrivalTime - SIMSTEP)
1764 0 : << "\n";
1765 : }
1766 819 : result.emplace_back(nullptr, -1, distToPeds);
1767 819 : break;
1768 : //} else {
1769 : // if (gDebugFlag1) {
1770 : // std::cout << SIMTIME << ": " << ego->getID() << " notBreaking for approaching person " << item.first->getID()
1771 : // << " dtc=" << distToCrossing << " ttc=" << distToCrossing / MAX2(ego->getSpeed(), 1.0) << " foeAT=" << item.second.arrivalTime << " foeTTC=" << STEPS2TIME(item.second.arrivalTime - SIMSTEP)
1772 : // << "\n";
1773 : // }
1774 : }
1775 : }
1776 : }
1777 : }
1778 : }
1779 : }
1780 :
1781 : //std::cout << SIMTIME << " ego=" << Named::getIDSecure(ego) << " link=" << getViaLaneOrLane()->getID() << " myWalkingAreaFoe=" << Named::getIDSecure(myWalkingAreaFoe) << "\n";
1782 249545095 : if (ego != nullptr) {
1783 248668755 : checkWalkingAreaFoe(ego, myWalkingAreaFoe, collectBlockers, result);
1784 248668755 : checkWalkingAreaFoe(ego, myWalkingAreaFoeExit, collectBlockers, result);
1785 : }
1786 :
1787 249545095 : if (MSGlobals::gLateralResolution > 0 && ego != nullptr && !isShadowLink) {
1788 : // check for foes on the same edge
1789 62876783 : for (std::vector<MSLane*>::const_iterator it = mySublaneFoeLanes.begin(); it != mySublaneFoeLanes.end(); ++it) {
1790 5261287 : const MSLane* foeLane = *it;
1791 : MSLane::AnyVehicleIterator end = foeLane->anyVehiclesEnd();
1792 3569675 : for (MSLane::AnyVehicleIterator it_veh = foeLane->anyVehiclesBegin(); it_veh != end; ++it_veh) {
1793 3569675 : MSVehicle* leader = (MSVehicle*)*it_veh;
1794 3569675 : if (leader == ego) {
1795 2081897 : continue;
1796 : }
1797 3011779 : if (leader->getLane()->isNormal()) {
1798 : // leader is past the conflict point
1799 1170845 : continue;
1800 : }
1801 1840934 : const double maxLength = MAX2(myInternalLaneBefore->getLength(), foeLane->getLength());
1802 1840934 : const double gap = dist - maxLength - ego->getVehicleType().getMinGap() + leader->getBackPositionOnLane(foeLane);
1803 1840934 : if (gap < -(ego->getVehicleType().getMinGap() + leader->getLength())) {
1804 : // ego is ahead of leader
1805 353156 : continue;
1806 : }
1807 1487778 : const double posLat = ego->getLateralPositionOnLane();
1808 1487778 : const double posLatLeader = leader->getLateralPositionOnLane() + leader->getLatOffset(foeLane);
1809 1487778 : if (gDebugFlag1) {
1810 0 : std::cout << " sublaneFoe lane=" << myInternalLaneBefore->getID()
1811 : << " foeLane=" << foeLane->getID()
1812 : << " leader=" << leader->getID()
1813 0 : << " egoLane=" << ego->getLane()->getID()
1814 0 : << " leaderLane=" << leader->getLane()->getID()
1815 : << " gap=" << gap
1816 : << " egoLat=" << posLat
1817 : << " leaderLat=" << posLatLeader
1818 0 : << " leaderLatOffset=" << leader->getLatOffset(foeLane)
1819 0 : << " egoIndex=" << myInternalLaneBefore->getIndex()
1820 0 : << " foeIndex=" << foeLane->getIndex()
1821 0 : << " dist=" << dist
1822 0 : << " leaderBack=" << leader->getBackPositionOnLane(foeLane)
1823 0 : << "\n";
1824 : }
1825 : // there only is a conflict if the paths cross
1826 609607 : if ((posLat < posLatLeader && myInternalLaneBefore->getIndex() > foeLane->getIndex())
1827 1810509 : || (posLat > posLatLeader && myInternalLaneBefore->getIndex() < foeLane->getIndex())) {
1828 678097 : if (gDebugFlag1) {
1829 0 : std::cout << SIMTIME << " blocked by " << leader->getID() << " (sublane split) foeLane=" << foeLane->getID() << "\n";
1830 : }
1831 678097 : if (ignoreFoe(ego, leader)) {
1832 0 : continue;
1833 : }
1834 678097 : result.emplace_back(leader, gap, -1);
1835 : }
1836 : }
1837 : }
1838 : }
1839 : return result;
1840 3 : }
1841 :
1842 :
1843 : void
1844 497337510 : MSLink::checkWalkingAreaFoe(const MSVehicle* ego, const MSLane* foeLane, std::vector<const MSPerson*>* collectBlockers, LinkLeaders& result) const {
1845 497337510 : if (foeLane != nullptr && foeLane->getEdge().getPersons().size() > 0) {
1846 : // pedestrians may be on an arbitrary path across this
1847 : // walkingarea. make sure to keep enough distance.
1848 : // This is a simple but conservative solution that could be improved
1849 : // by ignoring pedestrians that are "obviously" not on a collision course
1850 85165 : double distToPeds = std::numeric_limits<double>::max();
1851 : assert(myInternalLaneBefore != nullptr);
1852 85165 : PositionVector egoPath = myInternalLaneBefore->getShape();
1853 85165 : if (ego->getLateralPositionOnLane() != 0) {
1854 33736 : egoPath.move2side((MSGlobals::gLefthand ? 1 : -1) * ego->getLateralPositionOnLane());
1855 : }
1856 861066 : for (MSTransportable* t : foeLane->getEdge().getPersons()) {
1857 775901 : MSPerson* p = static_cast<MSPerson*>(t);
1858 775901 : double dist = ego->getPosition().distanceTo2D(p->getPosition()) - p->getVehicleType().getLength();
1859 1458930 : const bool inFront = isInFront(ego, egoPath, p->getPosition()) || isInFront(ego, egoPath, getFuturePosition(p));
1860 775901 : if (inFront) {
1861 109249 : dist -= ego->getVehicleType().getMinGap();
1862 : }
1863 : #ifdef DEBUG_WALKINGAREA
1864 : if (ego->isSelected()) {
1865 : std::cout << SIMTIME << " veh=" << ego->getID() << " ped=" << p->getID()
1866 : << " pos=" << ego->getPosition() << " pedPos=" << p->getPosition()
1867 : << " futurePedPos=" << getFuturePosition(p)
1868 : << " rawDist=" << ego->getPosition().distanceTo2D(p->getPosition())
1869 : << " inFront=" << inFront
1870 : << " dist=" << dist << "\n";
1871 : }
1872 : #endif
1873 775901 : if (dist < ego->getVehicleType().getWidth() / 2 || inFront) {
1874 109540 : if (inFront) {
1875 109249 : const double oncomingFactor = isOnComingPed(ego, p);
1876 109249 : if (oncomingFactor > 0) {
1877 : // account for pedestrian movement while closing in
1878 53857 : const double timeToStop = sqrt(dist) / 2;
1879 53857 : const double pedDist = p->getMaxSpeed() * MAX2(timeToStop, TS) * oncomingFactor;
1880 53857 : dist = MAX2(0.0, dist - pedDist);
1881 : #ifdef DEBUG_WALKINGAREA
1882 : if (ego->isSelected()) {
1883 : std::cout << " timeToStop=" << timeToStop << " pedDist=" << pedDist << " factor=" << oncomingFactor << " dist2=" << dist << "\n";
1884 : }
1885 : #endif
1886 : }
1887 : }
1888 109540 : if (ignoreFoe(ego, p)) {
1889 25237 : continue;
1890 : }
1891 84303 : distToPeds = MIN2(distToPeds, dist);
1892 84303 : if (collectBlockers != nullptr) {
1893 0 : collectBlockers->push_back(p);
1894 : }
1895 : }
1896 : }
1897 85165 : if (distToPeds != std::numeric_limits<double>::max()) {
1898 : // leave extra space in front
1899 47565 : result.emplace_back(nullptr, -1, distToPeds);
1900 : }
1901 85165 : }
1902 497337510 : }
1903 :
1904 : bool
1905 1458930 : MSLink::isInFront(const MSVehicle* ego, const PositionVector& egoPath, const Position& pPos) const {
1906 1458930 : const double pedAngle = ego->getPosition().angleTo2D(pPos);
1907 1458930 : const double angleDiff = fabs(GeomHelper::angleDiff(ego->getAngle(), pedAngle));
1908 : #ifdef DEBUG_WALKINGAREA
1909 : if (ego->isSelected()) {
1910 : std::cout << " angleDiff=" << RAD2DEG(angleDiff) << "\n";
1911 : }
1912 : #endif
1913 1458930 : if (angleDiff < DEG2RAD(75)) {
1914 1019971 : return egoPath.distance2D(pPos) < ego->getVehicleType().getWidth() + MSPModel::SAFETY_GAP;
1915 : }
1916 : return false;
1917 : }
1918 :
1919 :
1920 : double
1921 109249 : MSLink::isOnComingPed(const MSVehicle* ego, const MSPerson* p) const {
1922 109249 : const double pedToEgoAngle = p->getPosition().angleTo2D(ego->getPosition());
1923 109249 : const double angleDiff = fabs(GeomHelper::angleDiff(p->getAngle(), pedToEgoAngle));
1924 : #ifdef DEBUG_WALKINGAREA
1925 : if (ego->isSelected()) {
1926 : std::cout << " ped-angleDiff=" << RAD2DEG(angleDiff) << " res=" << cos(angleDiff) << "\n";
1927 : }
1928 : #endif
1929 109249 : if (angleDiff <= DEG2RAD(90)) {
1930 : ;
1931 53857 : return cos(angleDiff);
1932 : } else {
1933 : return 0;
1934 : }
1935 : }
1936 :
1937 :
1938 : Position
1939 683029 : MSLink::getFuturePosition(const MSPerson* p, double timeHorizon) const {
1940 683029 : const double a = p->getAngle();
1941 683029 : const double dist = timeHorizon * p->getMaxSpeed();
1942 :
1943 683029 : const Position offset(cos(a) * dist, sin(a) * dist);
1944 683029 : return p->getPosition() + offset;
1945 : }
1946 :
1947 :
1948 : MSLink*
1949 8653681 : MSLink::getParallelLink(int direction) const {
1950 8653681 : if (direction == -1) {
1951 3430577 : return myParallelRight;
1952 5223104 : } else if (direction == 1) {
1953 5223104 : return myParallelLeft;
1954 : } else {
1955 : assert(false || myLane->getOpposite() != nullptr);
1956 : return nullptr;
1957 : }
1958 : }
1959 :
1960 : MSLink*
1961 172201 : MSLink::getOppositeDirectionLink() const {
1962 172201 : if (myLane->getOpposite() != nullptr && myLaneBefore->getOpposite() != nullptr) {
1963 42540 : for (MSLink* cand : myLane->getOpposite()->getLinkCont()) {
1964 40943 : if (cand->getLane() == myLaneBefore->getOpposite()) {
1965 : return cand;
1966 : }
1967 : }
1968 : }
1969 : return nullptr;
1970 : }
1971 :
1972 :
1973 : MSLink*
1974 5491162 : MSLink::computeParallelLink(int direction) {
1975 5491162 : const MSLane* const before = getLaneBefore()->getParallelLane(direction, false);
1976 5491162 : const MSLane* const after = getLane()->getParallelLane(direction, false);
1977 5491162 : if (before != nullptr && after != nullptr) {
1978 1003081 : for (MSLink* const link : before->getLinkCont()) {
1979 705516 : if (link->getLane() == after) {
1980 : return link;
1981 : }
1982 : }
1983 : }
1984 : return nullptr;
1985 : }
1986 :
1987 :
1988 : double
1989 817941 : MSLink::getZipperSpeed(const MSVehicle* ego, const double dist, double vSafe,
1990 : SUMOTime arrivalTime,
1991 : const BlockingFoes* foes) const {
1992 817941 : if (myFoeLinks.size() == 0) {
1993 : // link should have LINKSTATE_MAJOR in this case
1994 : assert(false);
1995 : return vSafe;
1996 817941 : } else if (myFoeLinks.size() > 1) {
1997 : throw ProcessError("Zipper junctions with more than two conflicting lanes are not supported (at junction '"
1998 10 : + myJunction->getID() + "')");
1999 : }
2000 817936 : const double brakeGap = ego->getCarFollowModel().brakeGap(ego->getSpeed(), ego->getCarFollowModel().getMaxDecel(), TS);
2001 914632 : if (dist > MAX2(myFoeVisibilityDistance, brakeGap)) {
2002 : #ifdef DEBUG_ZIPPER
2003 : const SUMOTime now = MSNet::getInstance()->getCurrentTimeStep();
2004 : DEBUGOUT(DEBUG_COND_ZIPPER, SIMTIME << " getZipperSpeed ego=" << ego->getID()
2005 : << " dist=" << dist << " bGap=" << brakeGap << " ignoring foes (arrival in " << STEPS2TIME(arrivalTime - now) << ")\n")
2006 : #endif
2007 : return vSafe;
2008 : }
2009 : #ifdef DEBUG_ZIPPER
2010 : DEBUGOUT(DEBUG_COND_ZIPPER, SIMTIME << " getZipperSpeed ego=" << ego->getID()
2011 : << " egoAT=" << arrivalTime
2012 : << " dist=" << dist
2013 : << " brakeGap=" << brakeGap
2014 : << " vSafe=" << vSafe
2015 : << " numFoes=" << foes->size()
2016 : << "\n")
2017 : #endif
2018 275105 : MSLink* foeLink = myFoeLinks[0];
2019 739188 : for (const auto& item : *foes) {
2020 464083 : if (!item->isVehicle()) {
2021 254025 : continue;
2022 : }
2023 464083 : const MSVehicle* foe = dynamic_cast<const MSVehicle*>(item);
2024 : assert(foe != 0);
2025 464083 : const ApproachingVehicleInformation& avi = foeLink->getApproaching(foe);
2026 464083 : const double foeDist = (foe->isActive() ? avi.dist : MAX2(0.0, avi.dist -
2027 32 : STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - foe->getLastActionTime()) * avi.speed));
2028 :
2029 254025 : if ( // ignore vehicles that arrive after us (unless they are ahead and we could easily brake for them)
2030 475005 : ((avi.arrivalTime > arrivalTime) && !couldBrakeForLeader(dist, foeDist, ego, foe)) ||
2031 : // also ignore vehicles that are behind us and are able to brake for us
2032 685067 : couldBrakeForLeader(foeDist, dist, foe, ego) ||
2033 : // resolve ties by lane index
2034 3045 : (avi.arrivalTime == arrivalTime && foeDist == dist && ego->getLane()->getIndex() < foe->getLane()->getIndex())) {
2035 : #ifdef DEBUG_ZIPPER
2036 : if (DEBUG_COND_ZIPPER) std::cout
2037 : << " ignoring foe=" << foe->getID()
2038 : << " foeAT=" << avi.arrivalTime
2039 : << " foeDist=" << avi.dist
2040 : << " foeDist2=" << foeDist
2041 : << " foeSpeed=" << avi.speed
2042 : << " egoSpeed=" << ego->getSpeed()
2043 : << " deltaDist=" << foeDist - dist
2044 : << " delteSpeed=" << avi.speed - foe->getCarFollowModel().getMaxDecel() - ego->getSpeed()
2045 : << " egoCouldBrake=" << couldBrakeForLeader(dist, foeDist, ego, foe)
2046 : << " foeCouldBrake=" << couldBrakeForLeader(foeDist, dist, foe, ego)
2047 : << "\n";
2048 : #endif
2049 254025 : continue;
2050 : }
2051 : // the idea behind speed adaption is three-fold:
2052 : // 1) ego needs to be in a car-following relationship with foe eventually
2053 : // thus, the ego speed should be equal to the follow speed once the foe enters
2054 : // the zipper junction
2055 : // 2) ego vehicle needs to put a certain distance beteen himself and foe (safeGap)
2056 : // achieving this distance can be spread over time but computing
2057 : // safeGap is subject to estimation errors of future speeds
2058 : // 3) deceleration can be spread out over the time until true
2059 : // car-following happens, at the start of speed adaptions, smaller
2060 : // decelerations should be sufficient
2061 :
2062 : // we cannot trust avi.arrivalSpeed if the foe has leader vehicles that are accelerating
2063 : // lets try to extrapolate
2064 210058 : const double uMax = foe->getLane()->getVehicleMaxSpeed(foe);
2065 210058 : const double uAccel = foe->getCarFollowModel().estimateSpeedAfterDistance(foeDist, avi.speed, foe->getCarFollowModel().getMaxAccel());
2066 : const double uEnd = MIN2(uMax, uAccel);
2067 210058 : const double uAvg = (avi.speed + uEnd) / 2;
2068 210058 : const double tf0 = foeDist / MAX2(NUMERICAL_EPS, uAvg);
2069 210058 : const double tf = MAX2(1.0, ceil((tf0) / TS) * TS);
2070 :
2071 210058 : const double vMax = ego->getLane()->getVehicleMaxSpeed(ego);
2072 210058 : const double vAccel = ego->getCarFollowModel().estimateSpeedAfterDistance(dist, ego->getSpeed(), ego->getCarFollowModel().getMaxAccel());
2073 210058 : const double vDecel = ego->getCarFollowModel().estimateSpeedAfterDistance(dist, ego->getSpeed(), ego->getCarFollowModel().getMaxDecel());
2074 : const double vEnd = MIN3(vMax, vAccel, MAX2(uEnd, vDecel));
2075 210058 : const double vAvg = (ego->getSpeed() + vEnd) / 2;
2076 210058 : const double te0 = dist / MAX2(NUMERICAL_EPS, vAvg);
2077 210058 : const double te = MAX2(1.0, ceil((te0) / TS) * TS);
2078 :
2079 210058 : const double gap = dist - foe->getVehicleType().getLength() - ego->getVehicleType().getMinGap() - foeDist;
2080 210058 : const double safeGap = ego->getCarFollowModel().getSecureGap(ego, foe, vEnd, uEnd, foe->getCarFollowModel().getMaxDecel());
2081 : // round t to next step size
2082 : // increase gap to safeGap by the time foe reaches link
2083 : // gap + u*t - (t * v + a * t^2 / 2) = safeGap
2084 210058 : const double deltaGap = gap + tf * uAvg - safeGap - vAvg * tf;
2085 210058 : const double a = 2 * deltaGap / (tf * tf);
2086 210058 : const double vSafeGap = ego->getSpeed() + ACCEL2SPEED(a);
2087 210058 : const double vFollow = ego->getCarFollowModel().followSpeed(
2088 210058 : ego, ego->getSpeed(), gap, avi.speed, foe->getCarFollowModel().getMaxDecel(), foe);
2089 :
2090 : // scale behavior based on ego time to link (te)
2091 210058 : const double w = MIN2(1.0, te / 10);
2092 210058 : const double maxDecel = w * ego->getCarFollowModel().getMaxDecel() + (1 - w) * ego->getCarFollowModel().getEmergencyDecel();
2093 210058 : const double vZipper = MAX3(vFollow, ego->getSpeed() - ACCEL2SPEED(maxDecel), w * vSafeGap + (1 - w) * vFollow);
2094 :
2095 : vSafe = MIN2(vSafe, vZipper);
2096 : #ifdef DEBUG_ZIPPER
2097 : if (DEBUG_COND_ZIPPER) std::cout << " adapting to foe=" << foe->getID()
2098 : << " foeDist=" << foeDist
2099 : << " foeSpeed=" << avi.speed
2100 : << " foeAS=" << avi.arrivalSpeed
2101 : << " egoSpeed=" << ego->getSpeed()
2102 : << " uMax=" << uMax
2103 : << " uAccel=" << uAccel
2104 : << " uEnd=" << uEnd
2105 : << " uAvg=" << uAvg
2106 : << " gap=" << gap
2107 : << " safeGap=" << safeGap
2108 : << "\n "
2109 : << " tf=" << tf
2110 : << " te=" << te
2111 : << " dg=" << deltaGap
2112 : << " aSafeGap=" << a
2113 : << " vMax=" << vMax
2114 : << " vAccel=" << vAccel
2115 : << " vDecel=" << vDecel
2116 : << " vEnd=" << vEnd
2117 : << " vSafeGap=" << vSafeGap
2118 : << " vFollow=" << vFollow
2119 : << " w=" << w
2120 : << " maxDecel=" << maxDecel
2121 : << " vZipper=" << vZipper
2122 : << " vSafe=" << vSafe
2123 : << "\n";
2124 : #endif
2125 : }
2126 : return vSafe;
2127 : }
2128 :
2129 :
2130 : bool
2131 475005 : MSLink::couldBrakeForLeader(double followDist, double leaderDist, const MSVehicle* follow, const MSVehicle* leader) {
2132 : return (// leader is ahead of follower
2133 475005 : followDist > leaderDist &&
2134 : // and follower could brake for 1 s to stay behind leader
2135 32755 : followDist - leaderDist > follow->getSpeed() - follow->getCarFollowModel().getMaxDecel() - leader->getSpeed());
2136 : }
2137 :
2138 :
2139 : void
2140 2745581 : MSLink::initParallelLinks() {
2141 2745581 : myParallelRight = computeParallelLink(-1);
2142 2745581 : myParallelLeft = computeParallelLink(1);
2143 2745581 : }
2144 :
2145 : bool
2146 60275 : MSLink::checkContOff() const {
2147 : // check whether this link gets to keep its cont status switching the tls off
2148 : // @note: this could also be pre-computed in netconvert
2149 : // we check whether there is any major link from this edge
2150 185310 : for (const MSLane* cand : myLaneBefore->getEdge().getLanes()) {
2151 368494 : for (const MSLink* link : cand->getLinkCont()) {
2152 243459 : if (link->getOffState() == LINKSTATE_TL_OFF_NOSIGNAL) {
2153 : return true;
2154 : }
2155 : }
2156 : }
2157 : return false;
2158 : }
2159 :
2160 : bool
2161 4244054 : MSLink::lateralOverlap(double posLat, double width, double posLat2, double width2) {
2162 4244054 : return fabs(posLat2 - posLat) < (width + width2) / 2;
2163 : }
2164 :
2165 : std::string
2166 0 : MSLink::getDescription() const {
2167 0 : return myLaneBefore->getID() + "->" + getViaLaneOrLane()->getID();
2168 : }
2169 :
2170 :
2171 : bool
2172 159790394 : MSLink::ignoreFoe(const SUMOTrafficObject* ego, const SUMOTrafficObject* foe) {
2173 159790394 : if (ego == nullptr || !ego->getParameter().wasSet(VEHPARS_JUNCTIONMODEL_PARAMS_SET)) {
2174 159757955 : return false;
2175 : }
2176 32439 : const SUMOVehicleParameter& param = ego->getParameter();
2177 71050 : for (const std::string& typeID : StringTokenizer(param.getParameter(toString(SUMO_ATTR_JM_IGNORE_TYPES), "")).getVector()) {
2178 31537 : if (typeID == foe->getVehicleType().getID()) {
2179 : return true;
2180 : }
2181 32439 : }
2182 16136 : for (const std::string& id : StringTokenizer(param.getParameter(toString(SUMO_ATTR_JM_IGNORE_IDS), "")).getVector()) {
2183 2291 : if (id == foe->getID()) {
2184 : return true;
2185 : }
2186 7074 : }
2187 6771 : return false;
2188 : }
2189 :
2190 :
2191 : void
2192 88360 : MSLink::updateDistToFoePedCrossing(double dist) {
2193 88360 : myDistToFoePedCrossing = MIN2(myDistToFoePedCrossing, dist);
2194 88360 : }
2195 :
2196 :
2197 : std::pair<const SUMOVehicle* const, const MSLink::ApproachingVehicleInformation>
2198 465039 : MSLink::getClosest() const {
2199 : assert(getApproaching().size() > 0);
2200 : double minDist = std::numeric_limits<double>::max();
2201 : auto closestIt = getApproaching().begin();
2202 930579 : for (auto apprIt = getApproaching().begin(); apprIt != getApproaching().end(); apprIt++) {
2203 465540 : if (apprIt->second.dist < minDist) {
2204 : minDist = apprIt->second.dist;
2205 : closestIt = apprIt;
2206 : }
2207 : }
2208 : // maybe a parallel link has a closer vehicle
2209 : /*
2210 : for (MSLink* link2 : link->getLaneBefore()->getLinkCont()) {
2211 : if (link2 != link) {
2212 : for (auto apprIt2 = link2->getApproaching().begin(); apprIt2 != link2->getApproaching().end(); apprIt2++) {
2213 : if (apprIt2->second.dist < minDist) {
2214 : minDist = apprIt2->second.dist;
2215 : closestIt = apprIt2;
2216 : }
2217 : }
2218 : }
2219 : }
2220 : */
2221 465039 : return *closestIt;
2222 : }
2223 :
2224 :
2225 : /****************************************************************************/
|
