Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2026 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file 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_COND2(obj) (true)
56 : //#define DEBUG_COND_ZIPPER (gDebugFlag1)
57 : //#define DEBUG_COND_ZIPPER (true)
58 : #define DEBUG_COND_ZIPPER (ego->isSelected())
59 :
60 : // ===========================================================================
61 : // static member variables
62 : // ===========================================================================
63 :
64 : #define INVALID_TIME -1000
65 :
66 : // the default safety gap when passing before oncoming pedestrians
67 : #define JM_CROSSING_GAP_DEFAULT 10
68 :
69 : // minimim width between sibling lanes to qualify as non-overlapping
70 : #define DIVERGENCE_MIN_WIDTH 2.5
71 :
72 : const SUMOTime MSLink::myLookaheadTime = TIME2STEPS(1);
73 : // additional caution is needed when approaching a zipper link
74 : const SUMOTime MSLink::myLookaheadTimeZipper = TIME2STEPS(16);
75 : std::set<std::pair<MSLink*, MSLink*> > MSLink::myRecheck;
76 : const double MSLink::NO_INTERSECTION(10000);
77 :
78 : // ===========================================================================
79 : // ConflictInfo member method definitions
80 : // ===========================================================================
81 :
82 : double
83 489375959 : MSLink::ConflictInfo::getFoeLengthBehindCrossing(const MSLink* foeExitLink) const {
84 489375959 : if (flag == CONFLICT_DUMMY_MERGE) {
85 : return 0;
86 479354866 : } else if (foeConflictIndex >= 0) {
87 457319697 : return foeExitLink->myConflicts[foeConflictIndex].lengthBehindCrossing;
88 : } else {
89 : return -NO_INTERSECTION;
90 : }
91 : }
92 :
93 : double
94 252670039 : MSLink::ConflictInfo::getFoeConflictSize(const MSLink* foeExitLink) const {
95 252670039 : if (foeConflictIndex >= 0) {
96 230703588 : return foeExitLink->myConflicts[foeConflictIndex].conflictSize;
97 : } else {
98 : return 0;
99 : }
100 : }
101 :
102 : double
103 489508037 : MSLink::ConflictInfo::getLengthBehindCrossing(const MSLink* exitLink) const {
104 489508037 : if (flag == CONFLICT_STOP_AT_INTERNAL_JUNCTION) {
105 20503901 : return exitLink->getInternalLaneBefore()->getLength();
106 : } else {
107 469004136 : return lengthBehindCrossing;
108 : }
109 : }
110 :
111 : // ===========================================================================
112 : // member method definitions
113 : // ===========================================================================
114 2967056 : MSLink::MSLink(MSLane* predLane, MSLane* succLane, MSLane* via, LinkDirection dir, LinkState state,
115 : double length, double foeVisibilityDistance, bool keepClear,
116 : MSTrafficLightLogic* logic, int tlIndex,
117 2967056 : bool indirect) :
118 2967056 : myLane(succLane),
119 2967056 : myLaneBefore(predLane),
120 2967056 : myApproachingPersons(nullptr),
121 2967056 : myIndex(-1),
122 2967056 : myTLIndex(tlIndex),
123 2967056 : myLogic(logic),
124 2967056 : myState(state),
125 2967056 : myLastGreenState(LINKSTATE_TL_GREEN_MINOR),
126 2967056 : myOffState(state),
127 2967056 : myLastStateChange(SUMOTime_MIN / 2), // a large negative value, but avoid overflows when subtracting
128 2967056 : myDirection(dir),
129 2967056 : myLength(length),
130 2967056 : myFoeVisibilityDistance(foeVisibilityDistance),
131 2967056 : myDistToFoePedCrossing(std::numeric_limits<double>::max()),
132 2967056 : myHasFoes(false),
133 2967056 : myAmCont(false),
134 2967056 : myAmContOff(false),
135 2967056 : myKeepClear(keepClear),
136 2967056 : myInternalLane(via),
137 2967056 : myInternalLaneBefore(nullptr),
138 2967056 : myMesoTLSPenalty(0),
139 2967056 : myGreenFraction(1),
140 2967056 : myLateralShift(0),
141 2967056 : myOffFoeLinks(nullptr),
142 2967056 : myWalkingAreaFoe(nullptr),
143 2967056 : myWalkingAreaFoeExit(nullptr),
144 2967056 : myHavePedestrianCrossingFoe(false),
145 2967056 : myParallelRight(nullptr),
146 2967056 : myParallelLeft(nullptr),
147 2967056 : myAmIndirect(indirect),
148 2967056 : myRadius(std::numeric_limits<double>::max()),
149 2967056 : myPermissions(0),
150 2967056 : myJunction(nullptr)
151 : {
152 2967056 : updatePermissions();
153 2967056 : if (MSGlobals::gLateralResolution > 0) {
154 : // detect lateral shift from lane geometries
155 : //std::cout << "DEBUG link=" << myLaneBefore->getID() << "->" << getViaLaneOrLane()->getID() << " hasInternal=" << MSNet::getInstance()->hasInternalLinks() << " shapeBefore=" << myLaneBefore->getShape().back() << " shapeFront=" << getViaLaneOrLane()->getShape().front() << "\n";
156 207124 : if ((myInternalLane != nullptr || predLane->isInternal())
157 516998 : && myLaneBefore->getShape().back() != getViaLaneOrLane()->getShape().front()) {
158 : PositionVector from = myLaneBefore->getShape();
159 : const PositionVector& to = getViaLaneOrLane()->getShape();
160 : const double dist = from.back().distanceTo2D(to.front());
161 : // figure out direction of shift
162 : try {
163 652 : from.move2side(dist);
164 0 : } catch (InvalidArgument&) {
165 0 : }
166 652 : myLateralShift = (from.back().distanceTo2D(to.front()) < dist) ? dist : -dist;
167 652 : if (MSGlobals::gLefthand) {
168 90 : myLateralShift *= -1;
169 : }
170 : //std::cout << " lateral shift link=" << myLaneBefore->getID() << "->" << getViaLaneOrLane()->getID() << " dist=" << dist << " shift=" << myLateralShift << "\n";
171 652 : }
172 : }
173 2967056 : }
174 :
175 :
176 2940243 : MSLink::~MSLink() {
177 2940243 : delete myOffFoeLinks;
178 2941574 : delete myApproachingPersons;
179 2940243 : }
180 :
181 :
182 : void
183 2970823 : MSLink::updatePermissions() {
184 : // we only ever increase permission because transient permission reductions lead to invalid bestLanes assignment otherwise
185 2970823 : myPermissions |= myLaneBefore->getPermissions() & myLane->getPermissions() & (myInternalLane == nullptr ? SVCAll : myInternalLane->getPermissions());
186 2970823 : }
187 :
188 :
189 : void
190 6 : MSLink::addCustomConflict(const MSLane* from, const MSLane* to, double startPos, double endPos) {
191 6 : myCustomConflicts.push_back(CustomConflict(from, to, startPos, endPos));
192 6 : }
193 :
194 : const MSLink::CustomConflict*
195 4153856 : MSLink::getCustomConflict(const MSLane* foeLane) const {
196 4153856 : if (myCustomConflicts.size() > 0) {
197 24 : const MSLane* foeFrom = foeLane->getNormalPredecessorLane();
198 24 : const MSLane* foeTo = foeLane->getNormalSuccessorLane();
199 36 : for (const CustomConflict& cc : myCustomConflicts) {
200 24 : if (cc.from == foeFrom && cc.to == foeTo) {
201 : return &cc;
202 : }
203 : }
204 :
205 : }
206 : return nullptr;
207 : }
208 :
209 : void
210 2685681 : MSLink::setRequestInformation(int index, bool hasFoes, bool isCont,
211 : const std::vector<MSLink*>& foeLinks,
212 : const std::vector<MSLane*>& foeLanes,
213 : MSLane* internalLaneBefore) {
214 : //#ifdef MSLink_DEBUG_CROSSING_POINTS
215 : // std::cout << " setRequestInformation() for junction " << getViaLaneOrLane()->getEdge().getFromJunction()->getID()
216 : // << "\nInternalLanes = " << toString(getViaLaneOrLane()->getEdge().getFromJunction()->getInternalLanes())
217 : // << std::endl;
218 : //#endif
219 2685681 : myIndex = index;
220 2685681 : myHasFoes = hasFoes;
221 2685681 : myAmCont = isCont && MSGlobals::gUsingInternalLanes;
222 2685681 : myFoeLinks = foeLinks;
223 10319917 : for (MSLane* foeLane : foeLanes) {
224 : // cannot assign vector due to const-ness
225 7634236 : myFoeLanes.push_back(foeLane);
226 : }
227 2685681 : myJunction = const_cast<MSJunction*>(myLane->getEdge().getFromJunction()); // junctionGraph is initialized after the whole network is loaded
228 2685681 : myAmContOff = isCont && myLogic != nullptr && internalLaneBefore == nullptr && checkContOff();
229 2685681 : myInternalLaneBefore = internalLaneBefore;
230 : MSLane* lane = nullptr;
231 : if (internalLaneBefore != nullptr) {
232 : // this is an exit link. compute crossing points with all foeLanes
233 : lane = internalLaneBefore;
234 : //} else if (myLane->isCrossing()) {
235 : // // this is the link to a pedestrian crossing. compute crossing points with all foeLanes
236 : // // @note not currently used by pedestrians
237 : // lane = myLane;
238 : }
239 2685681 : const MSLink* entryLink = getCorrespondingEntryLink();
240 2685681 : if (entryLink->getOffState() == LinkState::LINKSTATE_ALLWAY_STOP && entryLink->getTLLogic() != nullptr) {
241 : // TLS has "normal" right of way rules but all conflicting links are foes when switching TLS off
242 : // (unless it's an internal junction link which should ignore all foes and should be ignored by all foes
243 5762 : myOffFoeLinks = new std::vector<MSLink*>();
244 5762 : if (isEntryLink()) {
245 15648 : for (MSLane* foeLane : foeLanes) {
246 : assert(foeLane->isInternal() || foeLane->isCrossing());
247 13140 : MSLink* viaLink = foeLane->getIncomingLanes().front().viaLink;
248 13140 : if (viaLink->getLaneBefore()->isNormal()) {
249 7960 : myOffFoeLinks->push_back(viaLink);
250 : }
251 : }
252 : }
253 : }
254 : #ifdef MSLink_DEBUG_CROSSING_POINTS
255 : std::cout << "link " << myIndex << " to " << getViaLaneOrLane()->getID() << " internalLaneBefore=" << (lane == 0 ? "NULL" : lane->getID()) << " has foes: " << toString(foeLanes) << "\n";
256 : #endif
257 2685681 : if (lane != nullptr) {
258 1049641 : const bool beforeInternalJunction = lane->getLinkCont()[0]->getViaLaneOrLane()->getEdge().isInternal();
259 1049641 : if (lane->getIncomingLanes().size() != 1) {
260 0 : throw ProcessError(TLF("Internal lane '%' has % predecessors", lane->getID(), toString(lane->getIncomingLanes().size())));
261 : }
262 1049641 : const MSLink* junctionEntryLink = lane->getEntryLink();
263 1049641 : const bool isSecondPart = isExitLinkAfterInternalJunction();
264 : // compute crossing points
265 5364806 : for (const MSLane* foeLane : myFoeLanes) {
266 4315165 : const CustomConflict* cc = junctionEntryLink != nullptr ? junctionEntryLink->getCustomConflict(foeLane) : nullptr;
267 4153844 : if (cc != nullptr) {
268 : // handle custom conflict definition
269 12 : double startPos = cc->startPos;
270 12 : const double conflictSize = cc->endPos - cc->startPos;
271 12 : if (isSecondPart) {
272 0 : startPos -= junctionEntryLink->getViaLane()->getLength();
273 : }
274 : // the foe connection may be split at an internal
275 : // junction, we need to figure out whether the current
276 : // foeLane is the intended target for the custom conflict
277 : // There are two possibilities:
278 : // a) We have no custom conflict for the reverse pair of connections
279 : // -> just check whether lane and foeLane intersect
280 : // b) We have a "reverse" custom conflict
281 : // -> check whether it covers the foeLane
282 12 : const CustomConflict* rcc = foeLane->getEntryLink()->getCustomConflict(lane);
283 : bool haveIntersection = false;
284 12 : if (rcc == nullptr) {
285 : // a)
286 12 : haveIntersection = lane->getShape().intersectsAtLengths2D(foeLane->getShape()).size() > 0;
287 : } else {
288 : // b)
289 0 : const bool foeIsSecondPart = foeLane->getLogicalPredecessorLane()->isInternal();
290 0 : double foeStartPos = rcc->startPos;
291 0 : const double foeConflictSize = rcc->endPos - rcc->startPos;
292 0 : if (foeIsSecondPart) {
293 0 : foeStartPos -= foeLane->getLogicalPredecessorLane()->getLength();
294 : }
295 0 : const double foeEndPos = foeStartPos + foeConflictSize;
296 0 : haveIntersection = ((foeStartPos > 0 && foeStartPos < foeLane->getLength())
297 0 : || (foeEndPos > 0 && foeEndPos < foeLane->getLength()));
298 : }
299 12 : if (haveIntersection) {
300 6 : myConflicts.push_back(ConflictInfo(lane->getLength() - startPos, conflictSize));
301 : } else {
302 6 : myConflicts.push_back(ConflictInfo(-NO_INTERSECTION, 0));
303 : }
304 : #ifdef MSLink_DEBUG_CROSSING_POINTS
305 : std::cout << " " << lane->getID() << " custom conflict with " << foeLane->getID() << " customReverse=" << (rcc != nullptr)
306 : << " haveIntersection=" << haveIntersection
307 : << " startPos=" << startPos << " conflictSize=" << conflictSize
308 : << " lbc=" << myConflicts.back().lengthBehindCrossing
309 : << "\n";
310 : #endif
311 12 : continue;
312 12 : }
313 4315153 : myHavePedestrianCrossingFoe = myHavePedestrianCrossingFoe || foeLane->isCrossing();
314 4315153 : const bool sameTarget = myLane == foeLane->getLinkCont()[0]->getLane();
315 4315153 : if (sameTarget && !beforeInternalJunction && !contIntersect(lane, foeLane)) {
316 : //if (myLane == foeLane->getLinkCont()[0]->getLane()) {
317 : // this foeLane has the same target and merges at the end (lane exits the junction)
318 1450393 : const double minDist = MIN2(DIVERGENCE_MIN_WIDTH, 0.5 * (lane->getWidth() + foeLane->getWidth()));
319 1450393 : if (lane->getShape().back().distanceTo2D(foeLane->getShape().back()) >= minDist) {
320 : // account for lateral shift by the entry links
321 131237 : if (foeLane->getEntryLink()->isIndirect()) {
322 41 : myConflicts.push_back(ConflictInfo(-NO_INTERSECTION, 0)); // dummy value, never used
323 : #ifdef MSLink_DEBUG_CROSSING_POINTS
324 : std::cout << " " << lane->getID() << " dummy merge with indirect" << foeLane->getID() << "\n";
325 : #endif
326 : } else {
327 131196 : myConflicts.push_back(ConflictInfo(0, foeLane->getWidth(), CONFLICT_DUMMY_MERGE)); // dummy value, never used
328 : #ifdef MSLink_DEBUG_CROSSING_POINTS
329 : std::cout << " " << lane->getID() << " dummy merge with " << foeLane->getID() << "\n";
330 : #endif
331 : }
332 : } else {
333 1319156 : const double distAfterDivergence = computeDistToDivergence(lane, foeLane, minDist, false);
334 : const double lbcLane = lane->interpolateGeometryPosToLanePos(distAfterDivergence);
335 1319156 : myConflicts.push_back(ConflictInfo(lbcLane, foeLane->getWidth()));
336 : #ifdef MSLink_DEBUG_CROSSING_POINTS
337 : std::cout
338 : << " " << lane->getID()
339 : << " merges with " << foeLane->getID()
340 : << " nextLane " << lane->getLinkCont()[0]->getViaLaneOrLane()->getID()
341 : << " dist1=" << myConflicts.back().lengthBehindCrossing
342 : << "\n";
343 : #endif
344 : }
345 : } else {
346 2864760 : std::vector<double> intersections1 = lane->getShape().intersectsAtLengths2D(foeLane->getShape());
347 : #ifdef MSLink_DEBUG_CROSSING_POINTS_DETAILS
348 : std::cout << " intersections1=" << toString(intersections1) << "\n";
349 : #endif
350 : bool haveIntersection = true;
351 2864760 : if (intersections1.size() == 0) {
352 1487251 : intersections1.push_back(-NO_INTERSECTION); // disregard this foe (using maxdouble leads to nasty problems down the line)
353 : haveIntersection = false;
354 1377509 : } else if (intersections1.size() > 1) {
355 1306 : std::sort(intersections1.begin(), intersections1.end());
356 : }
357 2864760 : std::vector<double> intersections2 = foeLane->getShape().intersectsAtLengths2D(lane->getShape());
358 : #ifdef MSLink_DEBUG_CROSSING_POINTS_DETAILS
359 : std::cout << " intersections2=" << toString(intersections2) << "\n";
360 : #endif
361 2864760 : if (intersections2.size() == 0) {
362 1487251 : intersections2.push_back(0);
363 1377509 : } else if (intersections2.size() > 1) {
364 1306 : std::sort(intersections2.begin(), intersections2.end());
365 : }
366 :
367 : // check for near-intersection (internal junctions for a side road which are only relevant when they have stranded vehicles))
368 2864760 : if (!haveIntersection && foeLane->getLinkCont()[0]->getViaLane() != nullptr) {
369 266031 : const Position waitPos = foeLane->getShape().back();
370 266031 : const double dist = lane->getShape().distance2D(waitPos, true);
371 266031 : if (dist != GeomHelper::INVALID_OFFSET && dist < lane->getWidth() / 2) {
372 : // risk of collision
373 : intersections1.clear();
374 : intersections2.clear();
375 31012 : intersections1.push_back(lane->getShape().nearest_offset_to_point2D(waitPos));
376 31012 : intersections2.push_back(foeLane->getShape().length());
377 : haveIntersection = true;
378 : #ifdef MSLink_DEBUG_CROSSING_POINTS_DETAILS
379 : std::cout << " link=" << myIndex << " " << getDescription() << " almostIntersection with foeLane " << foeLane->getID() << " offset=" << intersections1.back() << "\n";
380 : #endif
381 : }
382 : }
383 :
384 : double conflictSize = foeLane->getWidth();
385 : ConflictFlag flag = CONFLICT_NO_INTERSECTION;
386 2833748 : if (haveIntersection) {
387 : flag = CONFLICT_DEFAULT;
388 1408521 : const double angle1 = GeomHelper::naviDegree(lane->getShape().rotationAtOffset(intersections1.back()));
389 1408521 : const double angle2 = GeomHelper::naviDegree(foeLane->getShape().rotationAtOffset(intersections2.back()));
390 1408521 : const double angleDiff = GeomHelper::getMinAngleDiff(angle1, angle2);
391 : //const double angleDiff = MIN2(GeomHelper::getMinAngleDiff(angle1, angle2),
392 : // GeomHelper::getMinAngleDiff(angle1, angle2 + 180));
393 1408521 : const double widthFactor = 1 / MAX2(sin(DEG2RAD(angleDiff)), 0.2) * 2 - 1;
394 : //std::cout << " intersection of " << lane->getID() << " with " << foeLane->getID() << " angle1=" << angle1 << " angle2=" << angle2 << " angleDiff=" << angleDiff << " widthFactor=" << widthFactor << "\n";
395 1408521 : conflictSize *= widthFactor;
396 : conflictSize = MIN2(conflictSize, lane->getLength());
397 : // lane width affects the crossing point
398 1408521 : intersections1.back() -= conflictSize / 2;
399 : // ensure non-negative offset for weird geometries
400 1408521 : intersections1.back() = MAX2(0.0, intersections1.back());
401 :
402 : // also length/geometry factor. (XXX: Why subtract width/2 *before* converting geometric position to lane pos? refs #3031)
403 1408521 : intersections1.back() = lane->interpolateGeometryPosToLanePos(intersections1.back());
404 :
405 1408521 : if (internalLaneBefore->getLogicalPredecessorLane()->getEdge().isInternal() && !foeLane->isCrossing()) {
406 : flag = CONFLICT_STOP_AT_INTERNAL_JUNCTION;
407 : }
408 :
409 1408521 : if (foeLane->isCrossing()) {
410 112873 : const MSLink* before = myInternalLaneBefore->getCanonicalPredecessorLane()->getLinkTo(myInternalLaneBefore);
411 112873 : const_cast<MSLink*>(before)->updateDistToFoePedCrossing(intersections1.back());
412 : };
413 : }
414 :
415 2864760 : myConflicts.push_back(ConflictInfo(
416 2864760 : lane->getLength() - intersections1.back(),
417 : conflictSize, flag));
418 :
419 : #ifdef MSLink_DEBUG_CROSSING_POINTS
420 : std::cout
421 : << " intersection of " << lane->getID()
422 : << " totalLength=" << lane->getLength()
423 : << " with " << foeLane->getID()
424 : << " totalLength=" << foeLane->getLength()
425 : << " dist1=" << myConflicts.back().lengthBehindCrossing
426 : << " widthFactor=" << myConflicts.back().conflictSize / foeLane->getWidth()
427 : << "\n";
428 : #endif
429 2864760 : }
430 : }
431 : // check for overlap with internal lanes from the same source lane
432 1049641 : const MSLane* pred = lane->getLogicalPredecessorLane();
433 : // to avoid overlap with vehicles that came from pred (especially when pred has endOffset > 0)
434 : // we add all other internal lanes from pred as foeLanes
435 3436649 : for (const MSLink* const link : pred->getLinkCont()) {
436 2387008 : const MSLane* const sibling = link->getViaLane();
437 2387008 : if (sibling != lane && sibling != nullptr) {
438 1312416 : const double minDist = MIN2(DIVERGENCE_MIN_WIDTH, 0.5 * (lane->getWidth() + sibling->getWidth()));
439 1312416 : if (lane->getShape().front().distanceTo2D(sibling->getShape().front()) >= minDist) {
440 : // account for lateral shift by the entry links
441 630 : continue;
442 : }
443 1311786 : const double distToDivergence = computeDistToDivergence(lane, sibling, minDist, true);
444 : double lbcLane;
445 1311786 : if (lane->getLength() == sibling->getLength() && &lane->getEdge() == &sibling->getEdge()) {
446 : // for parallel lanes, avoid inconsistency in distance estimation (#10988)
447 : // between forward distance (getLeaderInfo)
448 : // and backward distance used in lane-changing (getFollowersOnConsecutive)
449 18196 : lbcLane = lane->getLength() - distToDivergence;
450 : } else {
451 1293590 : lbcLane = MAX2(0.0, lane->getLength() - lane->interpolateGeometryPosToLanePos(distToDivergence));
452 : }
453 : ConflictInfo ci = ConflictInfo(lbcLane, sibling->getWidth());
454 1311786 : auto it = std::find(myFoeLanes.begin(), myFoeLanes.end(), sibling);
455 1311786 : if (it != myFoeLanes.end()) {
456 : // avoid duplicate foeLane
457 90 : const int replacedIndex = (int)(it - myFoeLanes.begin());
458 90 : myConflicts[replacedIndex] = ci;
459 : } else {
460 1311696 : myConflicts.push_back(ci);
461 1311696 : myFoeLanes.push_back(sibling);
462 : }
463 : #ifdef MSLink_DEBUG_CROSSING_POINTS
464 : std::cout << " adding same-origin foe" << sibling->getID()
465 : << " dist1=" << myConflicts.back().lengthBehindCrossing
466 : << "\n";
467 : #endif
468 1311786 : const MSLane* const siblingCont = sibling->getLinkCont().front()->getViaLaneOrLane();
469 1311786 : if (siblingCont->isInternal() && lane->getShape().distance2D(siblingCont->getShape().front()) < minDist) {
470 : // there may still be overlap with siblingCont (when considering vehicle widths)
471 298569 : const double maxCommonLength = MIN2(lane->getLength(), sibling->getLength() + siblingCont->getLength());
472 298569 : const double lengthBehindDivergence = MAX2(0.0, lane->getLength() - maxCommonLength);
473 : ConflictInfo ci2 = ConflictInfo(lengthBehindDivergence, siblingCont->getWidth(), CONFLICT_SIBLING_CONTINUATION);
474 298569 : myConflicts.push_back(ci2);
475 298569 : myFoeLanes.push_back(siblingCont);
476 298569 : myRecheck.insert({this, siblingCont->getLinkCont().front()});
477 :
478 : #ifdef MSLink_DEBUG_CROSSING_POINTS
479 : std::cout << " adding same-origin foeContinuation" << siblingCont->getID()
480 : << " dist1=" << myConflicts.back().lengthBehindCrossing
481 : << "\n";
482 : #endif
483 : }
484 : }
485 : }
486 : // init points for the symmetrical conflict
487 : // for each pair of conflicting lanes, the link that gets second, sets the pointers
488 6975071 : for (int i = 0; i < (int)myFoeLanes.size(); i++) {
489 5925430 : const MSLane* foeLane = myFoeLanes[i];
490 5925430 : MSLink* foeExitLink = foeLane->getLinkCont()[0];
491 : int foundIndex = -1;
492 22241712 : for (int i2 = 0; i2 < (int)foeExitLink->myFoeLanes.size(); i2++) {
493 18865568 : if (foeExitLink->myFoeLanes[i2] == lane) {
494 2549286 : myConflicts[i].foeConflictIndex = i2;
495 2549286 : foeExitLink->myConflicts[i2].foeConflictIndex = i;
496 2549286 : myRecheck.erase({foeExitLink, this});
497 : foundIndex = i2;
498 2549286 : break;
499 : }
500 : }
501 : #ifdef MSLink_DEBUG_CROSSING_POINTS
502 : std::cout << lane->getID() << " foeLane=" << foeLane->getID() << " index=" << i << " foundIndex=" << foundIndex << "\n";
503 : #endif
504 5925430 : if (foundIndex < 0) {
505 3376144 : if (myConflicts[i].flag != CONFLICT_NO_INTERSECTION) {
506 2456213 : myRecheck.insert({this, foeExitLink});
507 : }
508 : }
509 : }
510 : }
511 2685681 : if (MSGlobals::gLateralResolution > 0) {
512 : // check for links with the same origin lane and the same destination edge
513 297897 : const MSEdge* myTarget = &myLane->getEdge();
514 : // save foes for entry links
515 941174 : for (MSLink* const it : myLaneBefore->getLinkCont()) {
516 : const MSEdge* target = &(it->getLane()->getEdge());
517 643277 : if (it == this) {
518 297897 : continue;
519 : }
520 345380 : if (target == myTarget) {
521 6804 : mySublaneFoeLinks.push_back(it);
522 : #ifdef MSLink_DEBUG_CROSSING_POINTS
523 : std::cout << " sublaneFoeLink (same target): " << it->getViaLaneOrLane()->getID() << "\n";
524 : #endif
525 338576 : } else if (myDirection != LinkDirection::STRAIGHT && it->getDirection() == LinkDirection::STRAIGHT) {
526 : // potential turn conflict
527 82850 : mySublaneFoeLinks2.push_back(it);
528 : #ifdef MSLink_DEBUG_CROSSING_POINTS
529 : std::cout << " sublaneFoeLink2 (other target: " << it->getViaLaneOrLane()->getID() << "\n";
530 : #endif
531 : }
532 : }
533 : // save foes for exit links
534 297897 : if (fromInternalLane()) {
535 : //std::cout << " setRequestInformation link=" << getViaLaneOrLane()->getID() << " before=" << myLaneBefore->getID() << " before2=" << myLaneBefore->getIncomingLanes().front().lane->getID() << "\n";
536 371556 : for (const MSLink* const link : myLaneBefore->getIncomingLanes().front().lane->getLinkCont()) {
537 261070 : if (link->getViaLane() != myInternalLaneBefore && &link->getLane()->getEdge() == myTarget) {
538 : //std::cout << " add sublaneFoe=" << (*it)->getViaLane()->getID() << "\n";
539 4360 : mySublaneFoeLanes.push_back(link->getViaLane());
540 : }
541 : }
542 : }
543 : }
544 2685681 : if (myInternalLaneBefore != nullptr
545 1049641 : && myDirection != LinkDirection::STRAIGHT
546 : // for right turns, the curvature helps rather than restricts the linkLeader check
547 616005 : && (
548 616005 : (!MSGlobals::gLefthand && myDirection != LinkDirection::RIGHT)
549 180097 : || (MSGlobals::gLefthand && myDirection != LinkDirection::LEFT))) {
550 872726 : const double angle = fabs(GeomHelper::angleDiff(
551 436363 : myLaneBefore->getNormalPredecessorLane()->getShape().angleAt2D(-2),
552 436363 : myLane->getShape().angleAt2D(0)));
553 436363 : if (angle > 0) {
554 436363 : double length = myInternalLaneBefore->getShape().length2D();
555 872726 : if (myInternalLaneBefore->getIncomingLanes().size() == 1 &&
556 436363 : myInternalLaneBefore->getIncomingLanes()[0].lane->isInternal()) {
557 125293 : length += myInternalLaneBefore->getIncomingLanes()[0].lane->getShape().length2D();
558 311070 : } else if (myInternalLane != nullptr) {
559 125293 : length += myInternalLane->getShape().length2D();
560 : }
561 436363 : myRadius = length / angle;
562 : //std::cout << getDescription() << " a=" << RAD2DEG(angle) << " l=" << length << " r=" << myRadius << "\n";
563 : }
564 : }
565 2685681 : }
566 :
567 :
568 : void
569 42584 : MSLink::recheckSetRequestInformation() {
570 362648 : for (auto item : myRecheck) {
571 : #ifdef MSLink_DEBUG_CROSSING_POINTS
572 : std::cout << " recheck l1=" << item.first->getDescription() << " l2=" << item.second->getDescription() << "\n";
573 : #endif
574 : MSLink* const link = item.first;
575 : MSLink* const foeExitLink = item.second;
576 : const MSLane* const lane = link->getInternalLaneBefore();
577 : const MSLane* const foeLane = foeExitLink->getInternalLaneBefore();
578 : int conflictIndex = -1;
579 2790275 : for (int i = 0; i < (int)link->myFoeLanes.size(); i++) {
580 2790275 : if (link->myFoeLanes[i] == foeLane) {
581 : conflictIndex = i;
582 : break;
583 : }
584 : }
585 320064 : if (conflictIndex == -1) {
586 0 : WRITE_WARNING("Could not recheck ConflictInfo for " + link->getDescription() + " and " + foeExitLink->getDescription() + "\n");
587 298813 : continue;
588 : }
589 320064 : ConflictInfo& ci = link->myConflicts[conflictIndex];
590 320064 : if (ci.flag & CONFLICT_SIBLING_CONTINUATION) {
591 298563 : const MSLane* const intLane = link->getInternalLaneBefore();
592 : const MSLane* const siblingCont = foeExitLink->getInternalLaneBefore();
593 298563 : const MSLane* const sibling = siblingCont->getLogicalPredecessorLane();
594 : // this is an approximation because intLane and sibling+siblingCont are still close to each other but may have different curvature
595 298563 : const double distToDivergence = intLane->getLength() - ci.lengthBehindCrossing;
596 298563 : double lbcSibCont = MIN2(siblingCont->getLength(), MAX2(0.0, sibling->getLength() + siblingCont->getLength() - distToDivergence));
597 : #ifdef MSLink_DEBUG_CROSSING_POINTS
598 : std::cout << " siblingContinuation: distToDivergence=" << distToDivergence << " lbcSibCont=" << lbcSibCont << "\n";
599 : std::cout << " conflictIndex=" << conflictIndex << " foeLane=" << foeLane->getID() << " foeExitLink=" << foeExitLink->getDescription() << " intLane=" << intLane->getID() << "\n";
600 : #endif
601 : ConflictInfo ci2 = ConflictInfo(lbcSibCont, intLane->getWidth());
602 298563 : ci2.foeConflictIndex = conflictIndex;
603 298563 : ci.foeConflictIndex = (int)foeExitLink->myConflicts.size();
604 298563 : foeExitLink->myFoeLanes.push_back(intLane);
605 298563 : foeExitLink->myConflicts.push_back(ci2);
606 : continue;
607 298563 : }
608 :
609 21501 : std::vector<double> intersections1 = foeLane->getShape().intersectsAtLengths2D(lane->getShape());
610 21501 : if (intersections1.size() == 0) {
611 : #ifdef MSLink_DEBUG_CROSSING_POINTS
612 : std::cout << " no intersection\n";
613 : #endif
614 : continue;
615 : }
616 21251 : const double widthFactor = ci.conflictSize / foeLane->getWidth();
617 21251 : const double conflictSize2 = lane->getWidth() * widthFactor;
618 21251 : std::sort(intersections1.begin(), intersections1.end());
619 21251 : intersections1.back() -= conflictSize2 / 2;
620 21251 : intersections1.back() = MAX2(0.0, intersections1.back());
621 21251 : ci.foeConflictIndex = (int)foeExitLink->myConflicts.size();
622 21251 : foeExitLink->myConflicts.push_back(ConflictInfo(foeLane->getLength() - intersections1.back(), conflictSize2));
623 : #ifdef MSLink_DEBUG_CROSSING_POINTS
624 : std::cout << " ci=" << conflictIndex << " wf=" << widthFactor << " flag=" << ci.flag << " flbc=" << foeExitLink->myConflicts.back().lengthBehindCrossing << "\n";
625 : #endif
626 21501 : }
627 : myRecheck.clear();
628 42584 : }
629 :
630 : double
631 2630942 : MSLink::computeDistToDivergence(const MSLane* lane, const MSLane* sibling, double minDist, bool sameSource, double siblingPredLength) const {
632 : double lbcSibling = 0;
633 : double lbcLane = 0;
634 :
635 : PositionVector l = lane->getShape();
636 : PositionVector s = sibling->getShape();
637 2630942 : double length = l.length2D();
638 2630942 : double sibLength = s.length2D();
639 2630942 : if (!sameSource) {
640 2638312 : l = l.reverse();
641 2638312 : s = s.reverse();
642 1311786 : } else if (sibling->getEntryLink()->myAmIndirect) {
643 : // ignore final waiting position since it may be quite close to the lane
644 : // shape but the waiting position is perpendicular (so the minDist
645 : // requirement is not necessary
646 94 : lbcSibling += s[-1].distanceTo2D(s[-2]);
647 : s.pop_back();
648 1311692 : } else if (lane->getEntryLink()->myAmIndirect) {
649 : // ignore final waiting position since it may be quite close to the lane
650 : // shape but the waiting position is perpendicular (so the minDist
651 : // requirement is not necessary
652 94 : lbcLane += l[-1].distanceTo2D(l[-2]);
653 : l.pop_back();
654 : }
655 :
656 : #ifdef MSLink_DEBUG_CROSSING_POINTS_DETAILS
657 : std::cout << " sameSource=" << sameSource << " lane=" << lane->getID() << " sib=" << sibling->getID() << " minDist=" << minDist << " backDist=" << l.back().distanceTo2D(s.back()) << "\n";
658 : #endif
659 2630942 : if (l.back().distanceTo2D(s.back()) > minDist) {
660 : // compute the final divergence point
661 : // this position serves two purposes:
662 : // 1) once the foe vehicle back (on sibling) has passed this point, we can safely ignore it
663 : // 2) both vehicles are put into a cf-relationship while before the point.
664 : // Since the actual crossing point is at the start of the junction,
665 : // we want to make sure that both vehicles have the same distance to the crossing point and thus follow each other naturally
666 2573152 : std::vector<double> distances = l.distances(s);
667 : #ifdef MSLink_DEBUG_CROSSING_POINTS
668 : std::cout << " distances=" << toString(distances) << "\n";
669 : #endif
670 : assert(distances.size() == l.size() + s.size());
671 2573152 : if (distances.back() > minDist && distances[l.size() - 1] > minDist) {
672 : // do a pairwise check between lane and sibling to make because we do not know which of them bends more
673 2446529 : for (int j = (int)s.size() - 2; j >= 0; j--) {
674 2446529 : const int i = j + (int)l.size();
675 2446529 : const double segLength = s[j].distanceTo2D(s[j + 1]);
676 2446529 : if (distances[i] > minDist) {
677 948361 : lbcSibling += segLength;
678 : } else {
679 : // assume no sharp bends and just interpolate the last segment
680 1498168 : lbcSibling += segLength - (minDist - distances[i]) * segLength / (distances[i + 1] - distances[i]);
681 1498168 : break;
682 : }
683 : }
684 2447097 : for (int i = (int)l.size() - 2; i >= 0; i--) {
685 2447097 : const double segLength = l[i].distanceTo2D(l[i + 1]);
686 2447097 : if (distances[i] > minDist) {
687 948929 : lbcLane += segLength;
688 : } else {
689 : // assume no sharp bends and just interpolate the last segment
690 1498168 : lbcLane += segLength - (minDist - distances[i]) * segLength / (distances[i + 1] - distances[i]);
691 1498168 : break;
692 : }
693 : }
694 : }
695 : assert(lbcSibling >= -NUMERICAL_EPS);
696 : assert(lbcLane >= -NUMERICAL_EPS);
697 2573152 : }
698 2630942 : const double distToDivergence1 = sibling->getLength() + siblingPredLength - lbcSibling;
699 2630942 : const double distToDivergence2 = lane->getLength() - lbcLane;
700 : const double distToDivergence = MIN3(
701 : MAX2(distToDivergence1, distToDivergence2),
702 : sibLength, length);
703 : #ifdef MSLink_DEBUG_CROSSING_POINTS
704 : std::cout << " distToDivergence=" << distToDivergence
705 : << " distTD1=" << distToDivergence1
706 : << " distTD2=" << distToDivergence2
707 : << " length=" << length
708 : << " sibLength=" << sibLength
709 : << "\n";
710 : #endif
711 2630942 : return distToDivergence;
712 2630942 : }
713 :
714 :
715 : bool
716 1450624 : MSLink::contIntersect(const MSLane* lane, const MSLane* foe) {
717 1450624 : if (foe->getLinkCont()[0]->getViaLane() != nullptr) {
718 131782 : std::vector<double> intersections = lane->getShape().intersectsAtLengths2D(foe->getShape());
719 131782 : return intersections.size() > 0;
720 131782 : }
721 : return false;
722 : }
723 :
724 :
725 : void
726 897979161 : MSLink::setApproaching(const SUMOVehicle* approaching, const SUMOTime arrivalTime, const double arrivalSpeed, const double leaveSpeed,
727 : const bool setRequest, const double arrivalSpeedBraking, const SUMOTime waitingTime, double dist, double latOffset) {
728 897979161 : const SUMOTime leaveTime = getLeaveTime(arrivalTime, arrivalSpeed, leaveSpeed, approaching->getVehicleType().getLength());
729 : #ifdef DEBUG_APPROACHING
730 : if (DEBUG_COND2(approaching)) {
731 : std::cout << SIMTIME << " link=" << getDescription() << " setApproaching veh=" << approaching->getID();
732 : if (myApproachingVehicles.size() > 0) {
733 : std::cout << " curApproaching=";
734 : for (auto i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
735 : std::cout << i->first->getID() << " ";
736 : }
737 : }
738 : std::cout << "\n";
739 : }
740 : #endif
741 897979161 : if (MSGlobals::gUseMesoSim) {
742 : // - in meso, setApproaching may be called multiple times without intermediate removeApproaching (whenever a vehicle is blocked in MELoop::checkCar)
743 : // explicit erasure is necessary because emplace does nothing if the key already exists
744 : // - in micro, double registration only happens on looped routes. Here, we only wish to keep the first arrival and thus emplace has the correct behavior
745 : // (on meso, only the next upcoming link is registered so nothing gets overwritten on looped routes)
746 : myApproachingVehicles.erase(approaching);
747 : }
748 897979161 : myApproachingVehicles.emplace(approaching,
749 1795958322 : ApproachingVehicleInformation(arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, setRequest,
750 897979161 : arrivalSpeedBraking, waitingTime, dist, approaching->getSpeed(), latOffset));
751 897979161 : }
752 :
753 :
754 : void
755 1121904 : MSLink::setApproaching(const SUMOVehicle* approaching, ApproachingVehicleInformation ai) {
756 : #ifdef DEBUG_APPROACHING
757 : if (DEBUG_COND2(approaching)) {
758 : std::cout << SIMTIME << " link=" << getDescription() << " setApproaching veh=" << approaching->getID();
759 : if (myApproachingVehicles.size() > 0) {
760 : std::cout << " curApproaching=";
761 : for (auto i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
762 : std::cout << i->first->getID() << " ";
763 : }
764 : }
765 : std::cout << "\n";
766 : }
767 : #endif
768 1121904 : myApproachingVehicles.emplace(approaching, ai);
769 1121904 : }
770 :
771 : void
772 487667 : MSLink::setApproachingPerson(const MSPerson* approaching, const SUMOTime arrivalTime, const SUMOTime leaveTime) {
773 487667 : if (myApproachingPersons == nullptr) {
774 1331 : myApproachingPersons = new PersonApproachInfos();
775 : }
776 487667 : myApproachingPersons->emplace(approaching, ApproachingPersonInformation(arrivalTime, leaveTime));
777 487667 : }
778 :
779 : void
780 886909747 : MSLink::removeApproaching(const SUMOVehicle* veh) {
781 : #ifdef DEBUG_APPROACHING
782 : if (DEBUG_COND2(veh)) {
783 : std::cout << SIMTIME << " link=" << getDescription() << " removeApproaching veh=" << veh->getID();
784 : if (myApproachingVehicles.size() > 0) {
785 : std::cout << " curApproaching=";
786 : for (auto i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
787 : std::cout << i->first->getID() << " ";
788 : }
789 : }
790 : std::cout << "\n";
791 : }
792 : #endif
793 : myApproachingVehicles.erase(veh);
794 886909747 : }
795 :
796 :
797 : void
798 31239 : MSLink::removeApproachingPerson(const MSPerson* person) {
799 31239 : if (myApproachingPersons == nullptr) {
800 12 : WRITE_WARNINGF("Person '%' entered crossing lane '%' without registering approach, time=%", person->getID(), myLane->getID(), time2string(SIMSTEP));
801 3 : return;
802 : }
803 : #ifdef DEBUG_APPROACHING
804 : if (DEBUG_COND2(person)) {
805 : std::cout << SIMTIME << " Link '" << (myLaneBefore == 0 ? "NULL" : myLaneBefore->getID()) << "'->'" << (myLane == 0 ? "NULL" : myLane->getID()) << std::endl;
806 : std::cout << "' Removing approaching person '" << person->getID() << "'\nCurrently registered persons:" << std::endl;
807 : for (auto i = myApproachingPersons->begin(); i != myApproachingPersons->end(); ++i) {
808 : std::cout << "'" << i->first->getID() << "'" << std::endl;
809 : }
810 : }
811 : #endif
812 : myApproachingPersons->erase(person);
813 : }
814 :
815 :
816 : MSLink::ApproachingVehicleInformation
817 30884543 : MSLink::getApproaching(const SUMOVehicle* veh) const {
818 : auto i = myApproachingVehicles.find(veh);
819 30884543 : if (i != myApproachingVehicles.end()) {
820 27389453 : return i->second;
821 : } else {
822 : return ApproachingVehicleInformation(INVALID_TIME, INVALID_TIME, 0, 0, false, 0, 0, 0, 0, 0);
823 : }
824 : }
825 :
826 :
827 : const MSLink::ApproachingVehicleInformation*
828 2882530 : MSLink::getApproachingPtr(const SUMOVehicle* veh) const {
829 : auto i = myApproachingVehicles.find(veh);
830 2882530 : if (i != myApproachingVehicles.end()) {
831 2520293 : return &i->second;
832 : } else {
833 : return nullptr;
834 : }
835 : }
836 :
837 :
838 : void
839 11184 : MSLink::clearState() {
840 : myApproachingVehicles.clear();
841 11184 : }
842 :
843 :
844 : SUMOTime
845 1576652684 : MSLink::getLeaveTime(const SUMOTime arrivalTime, const double arrivalSpeed,
846 : const double leaveSpeed, const double vehicleLength) const {
847 1610437431 : return arrivalTime == SUMOTime_MAX ? SUMOTime_MAX : arrivalTime + TIME2STEPS((getLength() + vehicleLength) / MAX2(0.5 * (arrivalSpeed + leaveSpeed), NUMERICAL_EPS));
848 : }
849 :
850 :
851 : bool
852 693662205 : MSLink::opened(SUMOTime arrivalTime, double arrivalSpeed, double leaveSpeed, double vehicleLength,
853 : double impatience, double decel, SUMOTime waitingTime, double posLat,
854 : BlockingFoes* collectFoes, bool ignoreRed, const SUMOTrafficObject* ego, double dist) const {
855 : #ifdef MSLink_DEBUG_OPENED
856 : if (gDebugFlag1) {
857 : std::cout << SIMTIME << " opened? link=" << getDescription() << " red=" << haveRed() << " cont=" << isCont() << " numFoeLinks=" << myFoeLinks.size() << " havePrio=" << havePriority() << " lastWasContMajorGreen=" << lastWasContState(LINKSTATE_TL_GREEN_MAJOR) << "\n";
858 : }
859 : #endif
860 693662205 : if (haveRed() && !ignoreRed) {
861 : return false;
862 : }
863 682735194 : if (isCont() && MSGlobals::gUsingInternalLanes) {
864 : return true;
865 : }
866 678236469 : const SUMOTime leaveTime = getLeaveTime(arrivalTime, arrivalSpeed, leaveSpeed, vehicleLength);
867 678236469 : if (MSGlobals::gLateralResolution > 0) {
868 : // check for foes on the same lane with the same target edge
869 139704701 : for (const MSLink* foeLink : mySublaneFoeLinks) {
870 : assert(myLane != foeLink->getLane());
871 10080349 : for (const auto& it : foeLink->myApproachingVehicles) {
872 6867932 : const SUMOVehicle* foe = it.first;
873 : if (
874 : // there only is a conflict if the paths cross
875 7474406 : ((posLat < foe->getLateralPositionOnLane() + it.second.latOffset && myLane->getIndex() > foeLink->myLane->getIndex())
876 6620219 : || (posLat > foe->getLateralPositionOnLane() + it.second.latOffset && myLane->getIndex() < foeLink->myLane->getIndex()))
877 : // the vehicle that arrives later must yield
878 7458026 : && (arrivalTime > it.second.arrivalTime
879 : // if both vehicles arrive at the same time, the one
880 : // to the left must yield
881 384324 : || (arrivalTime == it.second.arrivalTime && posLat > foe->getLateralPositionOnLane()))) {
882 206527 : if (blockedByFoe(foe, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, false,
883 : impatience, decel, waitingTime, ego)) {
884 : #ifdef MSLink_DEBUG_OPENED
885 : if (gDebugFlag1) {
886 : std::cout << SIMTIME << " blocked by " << foe->getID() << " arrival=" << arrivalTime << " foeArrival=" << it.second.arrivalTime << "\n";
887 : }
888 : #endif
889 41338 : if (collectFoes == nullptr) {
890 : #ifdef MSLink_DEBUG_OPENED
891 : if (gDebugFlag1) {
892 : std::cout << " link=" << getViaLaneOrLane()->getID() << " blocked by sublaneFoe=" << foe->getID() << " foeLink=" << foeLink->getViaLaneOrLane()->getID() << " posLat=" << posLat << "\n";
893 : }
894 : #endif
895 : return false;
896 : } else {
897 0 : collectFoes->push_back(it.first);
898 : }
899 : }
900 : }
901 : }
902 : }
903 : // check for foes on the same lane with a different target edge
904 : // (straight movers take precedence if the paths cross)
905 136450946 : const int lhSign = MSGlobals::gLefthand ? -1 : 1;
906 137797147 : for (const MSLink* foeLink : mySublaneFoeLinks2) {
907 : assert(myDirection != LinkDirection::STRAIGHT);
908 5711093 : for (const auto& it : foeLink->myApproachingVehicles) {
909 4364892 : const SUMOVehicle* foe = it.first;
910 : // there only is a conflict if the paths cross
911 : // and if the vehicles are not currently in a car-following relationship
912 4364892 : const double egoWidth = ego == nullptr ? 1.8 : ego->getVehicleType().getWidth();
913 4364892 : if (!lateralOverlap(posLat, egoWidth, foe->getLateralPositionOnLane() + it.second.latOffset, foe->getVehicleType().getWidth())
914 4364892 : && (((myDirection == LinkDirection::RIGHT || myDirection == LinkDirection::PARTRIGHT)
915 488504 : && (posLat * lhSign > (foe->getLateralPositionOnLane() + it.second.latOffset) * lhSign))
916 435155 : || ((myDirection == LinkDirection::LEFT || myDirection == LinkDirection::PARTLEFT)
917 32104 : && (posLat * lhSign < (foe->getLateralPositionOnLane() + it.second.latOffset) * lhSign)))) {
918 86912 : if (blockedByFoe(foe, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, false,
919 : impatience, decel, waitingTime, ego)) {
920 : #ifdef MSLink_DEBUG_OPENED
921 : if (gDebugFlag1) {
922 : std::cout << SIMTIME << " blocked by sublane foe " << foe->getID() << " arrival=" << arrivalTime << " foeArrival=" << it.second.arrivalTime << "\n";
923 : }
924 : #endif
925 9782 : if (collectFoes == nullptr) {
926 : #ifdef MSLink_DEBUG_OPENED
927 : if (gDebugFlag1) {
928 : std::cout << " link=" << getViaLaneOrLane()->getID() << " blocked by sublaneFoe2=" << foe->getID() << " foeLink=" << foeLink->getViaLaneOrLane()->getID() << " posLat=" << posLat << "\n";
929 : }
930 : #endif
931 : return false;
932 : } else {
933 0 : collectFoes->push_back(it.first);
934 : }
935 : }
936 : }
937 : }
938 : }
939 : }
940 : #ifdef MSLink_DEBUG_OPENED
941 : /*
942 : if (gDebugFlag1) {
943 : std::cout << SIMTIME << " isExitLinkAfterInternalJunction=" << isExitLinkAfterInternalJunction()
944 : << " entryLink=" << getCorrespondingEntryLink()->getDescription()
945 : << " entryState=" << getCorrespondingEntryLink()->getState()
946 : << "\n";
947 : }
948 : */
949 : #endif
950 : if ((havePriority()
951 29924192 : || lastWasContState(LINKSTATE_TL_GREEN_MAJOR)
952 29487800 : || (isExitLinkAfterInternalJunction() && getCorrespondingEntryLink()->getState() == LINKSTATE_TL_GREEN_MAJOR))
953 678692929 : && myState != LINKSTATE_ZIPPER) {
954 : // priority usually means the link is open but there are exceptions:
955 : // zipper still needs to collect foes
956 : // sublane model could have detected a conflict
957 647056255 : return collectFoes == nullptr || collectFoes->size() == 0;
958 : }
959 31129094 : if (myState == LINKSTATE_ALLWAY_STOP && waitingTime < TIME2STEPS(ego == nullptr ? TS : ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_ALLWAYSTOP_WAIT, TS))) {
960 : return false;
961 30151897 : } else if (myState == LINKSTATE_STOP && waitingTime < TIME2STEPS(ego == nullptr ? TS : ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPSIGN_WAIT, TS))) {
962 : return false;
963 : }
964 :
965 30087944 : const std::vector<MSLink*>& foeLinks = (myOffFoeLinks == nullptr || getCorrespondingEntryLink()->getState() != LINKSTATE_ALLWAY_STOP) ? myFoeLinks : *myOffFoeLinks;
966 :
967 30087944 : if (MSGlobals::gUseMesoSim && impatience == 1 && !myLane->getEdge().isRoundabout()) {
968 : return true;
969 : }
970 30087456 : if (myLane->getBidiLane() != nullptr) {
971 119521 : MSLane* bidi = myLane->getBidiLane();
972 119521 : if (bidi->getVehicleNumber() > 0) {
973 9531 : if (ego == nullptr) {
974 : return false;
975 : }
976 : double maxOncomingWidth = 0;
977 9531 : const MSLane::VehCont& vehs = bidi->getVehiclesSecure();
978 46564 : for (MSVehicle* foe : vehs) {
979 37033 : maxOncomingWidth = MAX2(maxOncomingWidth, foe->getVehicleType().getWidth());
980 : }
981 9531 : bidi->releaseVehicles();
982 9531 : if (MSGlobals::gLateralResolution > 0) {
983 6343 : if (ego->getVehicleType().getWidth() + maxOncomingWidth + MSGlobals::gLateralResolution < myLane->getWidth()) {
984 : return false;
985 : }
986 3188 : } else if (maxOncomingWidth > 0) {
987 : // do not enter a lane that has any oncoming vehicles
988 : return false;
989 : }
990 : }
991 250031 : for (auto ili : bidi->getIncomingLanes()) {
992 134287 : if (ili.lane->getEdge().getPriority() > myLaneBefore->getEdge().getPriority()
993 134287 : || (ili.lane->getEdge().getPriority() == myLaneBefore->getEdge().getPriority()
994 103290 : && ili.lane->getID() > myLaneBefore->getID())) {
995 : BlockingFoes bidiApproachFoes;
996 : double maxOncomingWidth = 0;
997 18235 : if (ili.viaLink->blockedAtTime(arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, false, 0, decel, 0,
998 36470 : MSGlobals::gLateralResolution ? &bidiApproachFoes : nullptr, ego) || bidiApproachFoes.size() > 0) {
999 530 : if (MSGlobals::gLateralResolution > 0) {
1000 477 : for (const SUMOTrafficObject* foe : bidiApproachFoes) {
1001 242 : maxOncomingWidth = MAX2(maxOncomingWidth, foe->getVehicleType().getWidth());
1002 : }
1003 235 : if (ego->getVehicleType().getWidth() + maxOncomingWidth + MSGlobals::gLateralResolution < myLane->getWidth()) {
1004 : return false;
1005 : }
1006 : } else {
1007 : return false;
1008 : }
1009 : }
1010 18235 : }
1011 : }
1012 : }
1013 30083679 : const bool lastWasContRed = lastWasContState(LINKSTATE_TL_RED);
1014 83754359 : for (const MSLink* const link : foeLinks) {
1015 58664435 : if (MSGlobals::gUseMesoSim) {
1016 2112765 : if (link->haveRed()) {
1017 68120 : continue;
1018 : }
1019 : }
1020 : #ifdef MSLink_DEBUG_OPENED
1021 : if (gDebugFlag1) {
1022 : std::cout << SIMTIME << " foeLink=" << link->getViaLaneOrLane()->getID() << " numApproaching=" << link->getApproaching().size() << "\n";
1023 : if (link->getLane()->isCrossing()) {
1024 : std::cout << SIMTIME << " approachingPersons=" << (link->myApproachingPersons == nullptr ? "NULL" : toString(link->myApproachingPersons->size())) << "\n";
1025 : }
1026 : }
1027 : #endif
1028 58596315 : if (link->blockedAtTime(arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, myLane == link->getLane(),
1029 : impatience, decel, waitingTime, collectFoes, ego, lastWasContRed, dist)) {
1030 : return false;
1031 : }
1032 : }
1033 25089924 : if (collectFoes != nullptr && collectFoes->size() > 0) {
1034 : return false;
1035 : }
1036 : return true;
1037 : }
1038 :
1039 :
1040 : bool
1041 58665871 : MSLink::blockedAtTime(SUMOTime arrivalTime, SUMOTime leaveTime, double arrivalSpeed, double leaveSpeed,
1042 : bool sameTargetLane, double impatience, double decel, SUMOTime waitingTime,
1043 : BlockingFoes* collectFoes, const SUMOTrafficObject* ego, bool lastWasContRed, double dist) const {
1044 220396583 : for (const auto& it : myApproachingVehicles) {
1045 : #ifdef MSLink_DEBUG_OPENED
1046 : if (gDebugFlag1) {
1047 : if (ego != nullptr
1048 : && ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_SPEED, 0) >= it.second.speed
1049 : && ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) > 0) {
1050 : std::stringstream stream; // to reduce output interleaving from different threads
1051 : stream << SIMTIME << " " << myApproachingVehicles.size() << " foe link=" << getViaLaneOrLane()->getID()
1052 : << " foeVeh=" << it.first->getID() << " (below ignore speed)"
1053 : << " ignoreFoeProb=" << ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0)
1054 : << "\n";
1055 : std::cout << stream.str();
1056 : }
1057 : }
1058 : #endif
1059 166621815 : if (it.first != ego
1060 166612541 : && (ego == nullptr
1061 166564728 : || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) == 0
1062 23409 : || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_SPEED, 0) < it.second.speed
1063 1757 : || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) < RandHelper::rand(ego->getRNG()))
1064 166610944 : && !ignoreFoe(ego, it.first)
1065 166610702 : && (!lastWasContRed || it.first->getSpeed() > SUMO_const_haltingSpeed)
1066 333225714 : && blockedByFoe(it.first, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, sameTargetLane,
1067 : impatience, decel, waitingTime, ego)) {
1068 11376122 : if (collectFoes == nullptr) {
1069 : return true;
1070 : } else {
1071 6485019 : collectFoes->push_back(it.first);
1072 : }
1073 : }
1074 : }
1075 53774768 : if (myApproachingPersons != nullptr && !haveRed()) {
1076 : const SUMOTime lookAhead = (ego == nullptr
1077 847533 : ? myLookaheadTime
1078 847358 : : TIME2STEPS(ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_TIMEGAP_MINOR, STEPS2TIME(myLookaheadTime))));
1079 922252 : for (const auto& it : *myApproachingPersons) {
1080 : #ifdef MSLink_DEBUG_OPENED
1081 : if (gDebugFlag1) {
1082 : std::cout << SIMTIME << ": " << ego->getID() << " check person " << it.first->getID() << " aTime=" << arrivalTime << " foeATime=" << it.second.arrivalTime
1083 : << " lTime=" << leaveTime << " foeLTime=" << it.second.leavingTime
1084 : << " dist=" << dist << "\n";
1085 : }
1086 : #endif
1087 : if ((ego == nullptr
1088 204286 : || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) == 0
1089 600 : || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_SPEED, 0) < it.first->getSpeed()
1090 600 : || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) < RandHelper::rand(ego->getRNG()))
1091 203917 : && !ignoreFoe(ego, it.first)
1092 408354 : && !((arrivalTime > it.second.leavingTime + lookAhead) || (leaveTime + lookAhead < it.second.arrivalTime))) {
1093 143196 : if (ego == nullptr) {
1094 : // during insertion
1095 174 : if (myJunction->getType() == SumoXMLNodeType::RAIL_CROSSING) {
1096 174 : continue;
1097 : } else {
1098 : return true;
1099 : }
1100 : }
1101 : // check whether braking is feasible (ego might have started to accelerate already)
1102 143022 : const auto& cfm = ego->getVehicleType().getCarFollowModel();
1103 : #ifdef MSLink_DEBUG_OPENED
1104 : if (gDebugFlag1) {
1105 : 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";
1106 : }
1107 : #endif
1108 143022 : if (dist > cfm.brakeGap(ego->getSpeed(), cfm.getMaxDecel(), 0)) {
1109 : #ifdef MSLink_DEBUG_OPENED
1110 : if (gDebugFlag1) {
1111 : std::cout << SIMTIME << ": " << ego->getID() << " blocked by person " << it.first->getID() << "\n";
1112 : }
1113 : #endif
1114 129798 : if (collectFoes == nullptr) {
1115 : return true;
1116 : } else {
1117 0 : collectFoes->push_back(it.first);
1118 : }
1119 : }
1120 : }
1121 : }
1122 : }
1123 : return false;
1124 : }
1125 :
1126 :
1127 : bool
1128 166897338 : MSLink::blockedByFoe(const SUMOVehicle* veh, const ApproachingVehicleInformation& avi,
1129 : SUMOTime arrivalTime, SUMOTime leaveTime, double arrivalSpeed, double leaveSpeed,
1130 : bool sameTargetLane, double impatience, double decel, SUMOTime waitingTime,
1131 : const SUMOTrafficObject* ego) const {
1132 : #ifdef MSLink_DEBUG_OPENED
1133 : if (gDebugFlag1) {
1134 : std::stringstream stream; // to reduce output interleaving from different threads
1135 : stream << " link=" << getDescription()
1136 : << " foeVeh=" << veh->getID()
1137 : << " req=" << avi.willPass
1138 : << " aT=" << avi.arrivalTime
1139 : << " lT=" << avi.leavingTime
1140 : << "\n";
1141 : std::cout << stream.str();
1142 : }
1143 : #endif
1144 166897338 : if (!avi.willPass) {
1145 : return false;
1146 : }
1147 60571779 : if (myState == LINKSTATE_ALLWAY_STOP) {
1148 : assert(waitingTime > 0);
1149 : #ifdef MSLink_DEBUG_OPENED
1150 : if (gDebugFlag1) {
1151 : std::stringstream stream; // to reduce output interleaving from different threads
1152 : stream << " foeDist=" << avi.dist
1153 : << " foeBGap=" << veh->getBrakeGap(false)
1154 : << " foeWait=" << avi.waitingTime
1155 : << " wait=" << waitingTime
1156 : << "\n";
1157 : std::cout << stream.str();
1158 : }
1159 : #endif
1160 : // when using actionSteps, the foe waiting time may be outdated
1161 1563239 : const SUMOTime actionDelta = SIMSTEP - veh->getLastActionTime();
1162 1563239 : if (waitingTime > avi.waitingTime + actionDelta) {
1163 : return false;
1164 : }
1165 234065 : if (waitingTime == (avi.waitingTime + actionDelta) && arrivalTime < avi.arrivalTime + actionDelta) {
1166 : return false;
1167 : }
1168 : }
1169 59212039 : SUMOTime foeArrivalTime = avi.arrivalTime;
1170 59212039 : double foeArrivalSpeedBraking = avi.arrivalSpeedBraking;
1171 59212039 : if (impatience > 0 && arrivalTime < avi.arrivalTime) {
1172 : #ifdef MSLink_DEBUG_OPENED
1173 : gDebugFlag6 = ((ego == nullptr || ego->isSelected()) && (veh == nullptr || veh->isSelected()));
1174 : #endif
1175 2823307 : const SUMOTime fatb = computeFoeArrivalTimeBraking(arrivalTime, veh, avi.arrivalTime, impatience, avi.dist, foeArrivalSpeedBraking);
1176 2823307 : foeArrivalTime = (SUMOTime)((1. - impatience) * (double)avi.arrivalTime + impatience * (double)fatb);
1177 : #ifdef MSLink_DEBUG_OPENED
1178 : if (gDebugFlag6) {
1179 : std::cout << SIMTIME << " link=" << getDescription() << " ego=" << ego->getID() << " foe=" << veh->getID()
1180 : << " at=" << STEPS2TIME(arrivalTime)
1181 : << " fat=" << STEPS2TIME(avi.arrivalTime)
1182 : << " fatb=" << STEPS2TIME(fatb)
1183 : << " fat2=" << STEPS2TIME(foeArrivalTime)
1184 : << "\n";
1185 : }
1186 : #endif
1187 : }
1188 :
1189 :
1190 59212039 : const SUMOTime lookAhead = (myState == LINKSTATE_ZIPPER
1191 59212039 : ? myLookaheadTimeZipper
1192 : : (ego == nullptr
1193 47510532 : ? myLookaheadTime
1194 47505161 : : TIME2STEPS(ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_TIMEGAP_MINOR, STEPS2TIME(myLookaheadTime)))));
1195 : //if (ego != 0) std::cout << SIMTIME << " ego=" << ego->getID() << " jmTimegapMinor=" << ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_TIMEGAP_MINOR, -1) << " lookAhead=" << lookAhead << "\n";
1196 : #ifdef MSLink_DEBUG_OPENED
1197 : if (gDebugFlag1 || gDebugFlag6) {
1198 : std::stringstream stream; // to reduce output interleaving from different threads
1199 : stream << " imp=" << impatience << " fAT2=" << foeArrivalTime << " fASb=" << foeArrivalSpeedBraking << " lA=" << lookAhead << " egoAT=" << arrivalTime << " egoLT=" << leaveTime << " egoLS=" << leaveSpeed << "\n";
1200 : std::cout << stream.str();
1201 : }
1202 : #endif
1203 59212039 : if (avi.leavingTime < arrivalTime) {
1204 : // ego wants to be follower
1205 43308111 : if (sameTargetLane && (arrivalTime - avi.leavingTime < lookAhead
1206 18936920 : || unsafeMergeSpeeds(avi.leaveSpeed, arrivalSpeed,
1207 18936920 : veh->getVehicleType().getCarFollowModel().getMaxDecel(), decel))) {
1208 : #ifdef MSLink_DEBUG_OPENED
1209 : if (gDebugFlag1 || gDebugFlag6) {
1210 : std::cout << " blocked (cannot follow)\n";
1211 : }
1212 : #endif
1213 4177556 : return true;
1214 : }
1215 15903928 : } else if (foeArrivalTime > leaveTime + lookAhead) {
1216 : // ego wants to be leader.
1217 14618088 : if (sameTargetLane && unsafeMergeSpeeds(leaveSpeed, foeArrivalSpeedBraking,
1218 5804463 : decel, veh->getVehicleType().getCarFollowModel().getMaxDecel())) {
1219 : #ifdef MSLink_DEBUG_OPENED
1220 : if (gDebugFlag1 || gDebugFlag6) {
1221 : std::cout << " blocked (cannot lead)\n";
1222 : }
1223 : #endif
1224 : return true;
1225 : }
1226 : } else {
1227 : // even without considering safeHeadwayTime there is already a conflict
1228 : #ifdef MSLink_DEBUG_OPENED
1229 : if (gDebugFlag1 || gDebugFlag6) {
1230 : std::cout << " blocked (hard conflict)\n";
1231 : }
1232 : #endif
1233 : return true;
1234 : }
1235 : return false;
1236 : }
1237 :
1238 :
1239 : SUMOTime
1240 2823307 : MSLink::computeFoeArrivalTimeBraking(SUMOTime arrivalTime, const SUMOVehicle* foe, SUMOTime foeArrivalTime, double impatience, double dist, double& fasb) {
1241 : // a: distance saved when foe brakes from arrivalTime to foeArrivalTime
1242 : // b: distance driven past foeArrivalTime
1243 : // m: permitted decceleration
1244 : // d: total deceleration until foeArrivalTime
1245 : // dist2: distance of foe at arrivalTime
1246 : // actual arrivalTime must fall on a simulation step
1247 2823307 : if (arrivalTime - arrivalTime % DELTA_T == foeArrivalTime - foeArrivalTime % DELTA_T) {
1248 : // foe enters the junction in the same step
1249 : #ifdef MSLink_DEBUG_OPENED
1250 : if (gDebugFlag6) {
1251 : std::cout << " foeAT before egoAT\n";
1252 : }
1253 : #endif
1254 : return foeArrivalTime;
1255 : }
1256 2466060 : if (arrivalTime % DELTA_T > 0) {
1257 2436664 : arrivalTime = arrivalTime - (arrivalTime % DELTA_T) + DELTA_T;
1258 : }
1259 : //arrivalTime += DELTA_T - arrivalTime % DELTA_T;
1260 2466060 : const double m = foe->getVehicleType().getCarFollowModel().getMaxDecel() * impatience;
1261 2466060 : const double dt = STEPS2TIME(foeArrivalTime - arrivalTime);
1262 2466060 : const double d = dt * m;
1263 2466060 : const double a = dt * d / 2;
1264 2466060 : const double v = dist / STEPS2TIME(foeArrivalTime - SIMSTEP + DELTA_T);
1265 2466060 : const double dist2 = dist - v * STEPS2TIME(arrivalTime - SIMSTEP);
1266 : #ifdef MSLink_DEBUG_OPENED
1267 : if (gDebugFlag6) {
1268 : std::cout << " dist=" << dist << " dist2=" << dist2
1269 : << " at=" << STEPS2TIME(arrivalTime)
1270 : << " fat=" << STEPS2TIME(foeArrivalTime)
1271 : << " dt=" << dt << " v=" << v << " m=" << m << " d=" << d << " a=" << a << "\n";
1272 : }
1273 : #endif
1274 2466060 : if (0.5 * v * v / m <= dist2) {
1275 : #ifdef MSLink_DEBUG_OPENED
1276 : if (gDebugFlag6) {
1277 : std::cout << " canBrakeToStop\n";
1278 : }
1279 : #endif
1280 1227365 : fasb = 0;
1281 1227365 : return foeArrivalTime + TIME2STEPS(30);
1282 : }
1283 : // a = b (foe reaches the original distance to the stop line)
1284 : // x: time driven past foeArrivalTime
1285 : // v: foe speed without braking
1286 : // v2: average foe speed after foeArrivalTime (braking continues for time x)
1287 : // v2 = (v - d - x * m / 2)
1288 : // b = v2 * x
1289 : // solving for x gives:
1290 1238695 : const double x = (sqrt(4 * (v - d) * (v - d) - 8 * m * a) * -0.5 - d + v) / m;
1291 :
1292 : #ifdef MSLink_DEBUG_OPENED
1293 : const double x2 = (sqrt(4 * (v - d) * (v - d) - 8 * m * a) * 0.5 - d + v) / m;
1294 : if (gDebugFlag6 || std::isnan(x)) {
1295 : std::cout << SIMTIME << " dist=" << dist << " dist2=" << dist2 << " at=" << STEPS2TIME(arrivalTime) << " m=" << m << " d=" << d << " v=" << v << " a=" << a << " x=" << x << " x2=" << x2 << "\n";
1296 : }
1297 : #endif
1298 1238695 : fasb = v - (dt + x) * m;
1299 1238695 : return foeArrivalTime + TIME2STEPS(x);
1300 : }
1301 :
1302 :
1303 : bool
1304 437202 : MSLink::hasApproachingFoe(SUMOTime arrivalTime, SUMOTime leaveTime, double speed, double decel) const {
1305 461377 : for (const MSLink* const link : myFoeLinks) {
1306 51321 : if (link->blockedAtTime(arrivalTime, leaveTime, speed, speed, myLane == link->getLane(), 0, decel, 0)) {
1307 : return true;
1308 : }
1309 : }
1310 504591 : for (const MSLane* const lane : myFoeLanes) {
1311 100963 : if (lane->getVehicleNumberWithPartials() > 0) {
1312 : return true;
1313 : }
1314 : }
1315 : return false;
1316 : }
1317 :
1318 :
1319 : std::pair<const SUMOVehicle*, const MSLink*>
1320 5817 : MSLink::getFirstApproachingFoe(const MSLink* wrapAround) const {
1321 : double closetDist = std::numeric_limits<double>::max();
1322 : const SUMOVehicle* closest = nullptr;
1323 : const MSLink* foeLink = nullptr;
1324 18834 : for (MSLink* link : myFoeLinks) {
1325 20374 : for (const auto& it : link->myApproachingVehicles) {
1326 : //std::cout << " link=" << getDescription() << " foeLink_in=" << link->getLaneBefore()->getID() << " wrapAround=" << wrapAround->getDescription() << "\n";
1327 7357 : if (link->getLaneBefore() == wrapAround->getLaneBefore()) {
1328 520 : return std::make_pair(nullptr, wrapAround);
1329 6837 : } else if (it.second.dist < closetDist) {
1330 : closetDist = it.second.dist;
1331 3604 : if (it.second.willPass) {
1332 3322 : closest = it.first;
1333 : foeLink = link;
1334 : }
1335 : }
1336 : }
1337 : }
1338 : return std::make_pair(closest, foeLink);
1339 : }
1340 :
1341 :
1342 : void
1343 79852333 : MSLink::setTLState(LinkState state, SUMOTime t) {
1344 79852333 : if (myState != state) {
1345 17730025 : myLastStateChange = t;
1346 : }
1347 79852333 : myState = state;
1348 79852333 : if (haveGreen()) {
1349 12637645 : myLastGreenState = myState;
1350 : }
1351 79852333 : }
1352 :
1353 :
1354 : void
1355 181243 : MSLink::setTLLogic(const MSTrafficLightLogic* logic) {
1356 181243 : myLogic = logic;
1357 181243 : }
1358 :
1359 :
1360 : bool
1361 1300924468 : MSLink::isCont() const {
1362 : // when a traffic light is switched off minor roads have their cont status revoked
1363 1300924468 : return (myState == LINKSTATE_TL_OFF_BLINKING || myState == LINKSTATE_ALLWAY_STOP || myState == LINKSTATE_STOP) ? myAmContOff : myAmCont;
1364 : }
1365 :
1366 :
1367 : bool
1368 65905093 : MSLink::lastWasContMajor() const {
1369 66587088 : if (isExitLinkAfterInternalJunction()) {
1370 681995 : return myInternalLaneBefore->getIncomingLanes()[0].viaLink->lastWasContMajor();
1371 : }
1372 65905093 : if (myInternalLane == nullptr || myAmCont) {
1373 : return false;
1374 : } else {
1375 48379378 : MSLane* pred = myInternalLane->getLogicalPredecessorLane();
1376 48379378 : if (!pred->getEdge().isInternal()) {
1377 : return false;
1378 : } else {
1379 11634634 : const MSLane* const pred2 = pred->getLogicalPredecessorLane();
1380 : assert(pred2 != nullptr);
1381 11634634 : const MSLink* const predLink = pred2->getLinkTo(pred);
1382 : assert(predLink != nullptr);
1383 11634634 : if (predLink->havePriority()) {
1384 : return true;
1385 : }
1386 10965029 : if (myHavePedestrianCrossingFoe) {
1387 1444475 : return predLink->getLastGreenState() == LINKSTATE_TL_GREEN_MAJOR;
1388 : } else {
1389 9520554 : return predLink->haveYellow();
1390 : }
1391 : }
1392 : }
1393 : }
1394 :
1395 :
1396 : bool
1397 60007871 : MSLink::lastWasContState(LinkState linkState) const {
1398 60007871 : if (myInternalLane == nullptr || myAmCont || myHavePedestrianCrossingFoe) {
1399 : return false;
1400 : } else {
1401 52673155 : MSLane* pred = myInternalLane->getLogicalPredecessorLane();
1402 52673155 : if (!pred->getEdge().isInternal()) {
1403 : return false;
1404 : } else {
1405 15498344 : const MSLane* const pred2 = pred->getLogicalPredecessorLane();
1406 : assert(pred2 != nullptr);
1407 15498344 : const MSLink* const predLink = pred2->getLinkTo(pred);
1408 : assert(predLink != nullptr);
1409 15498344 : return predLink->getState() == linkState;
1410 : }
1411 : }
1412 : }
1413 :
1414 :
1415 : void
1416 113904 : MSLink::writeApproaching(OutputDevice& od, const std::string fromLaneID) const {
1417 113904 : if (myApproachingVehicles.size() > 0) {
1418 9765 : od.openTag("link");
1419 9765 : od.writeAttr(SUMO_ATTR_FROM, fromLaneID);
1420 9765 : const std::string via = getViaLane() == nullptr ? "" : getViaLane()->getID();
1421 9765 : od.writeAttr(SUMO_ATTR_VIA, via);
1422 19530 : od.writeAttr(SUMO_ATTR_TO, getLane() == nullptr ? "" : getLane()->getID());
1423 : std::vector<std::pair<SUMOTime, const SUMOVehicle*> > toSort; // stabilize output
1424 21325 : for (auto it : myApproachingVehicles) {
1425 11560 : toSort.push_back(std::make_pair(it.second.arrivalTime, it.first));
1426 : }
1427 9765 : std::sort(toSort.begin(), toSort.end());
1428 21325 : for (std::vector<std::pair<SUMOTime, const SUMOVehicle*> >::const_iterator it = toSort.begin(); it != toSort.end(); ++it) {
1429 11560 : od.openTag("approaching");
1430 11560 : const ApproachingVehicleInformation& avi = myApproachingVehicles.find(it->second)->second;
1431 11560 : od.writeAttr(SUMO_ATTR_ID, it->second->getID());
1432 11560 : od.writeAttr(SUMO_ATTR_IMPATIENCE, it->second->getImpatience());
1433 11560 : od.writeAttr("arrivalTime", time2string(avi.arrivalTime));
1434 11560 : od.writeAttr("leaveTime", time2string(avi.leavingTime));
1435 11560 : od.writeAttr("arrivalSpeed", toString(avi.arrivalSpeed));
1436 11560 : od.writeAttr("arrivalSpeedBraking", toString(avi.arrivalSpeedBraking));
1437 11560 : od.writeAttr("leaveSpeed", toString(avi.leaveSpeed));
1438 11560 : od.writeAttr("willPass", toString(avi.willPass));
1439 23120 : od.closeTag();
1440 : }
1441 9765 : od.closeTag();
1442 9765 : }
1443 113904 : }
1444 :
1445 :
1446 : double
1447 921483 : MSLink::getInternalLengthsAfter() const {
1448 : double len = 0.;
1449 921483 : MSLane* lane = myInternalLane;
1450 :
1451 1759209 : while (lane != nullptr && lane->isInternal()) {
1452 837726 : len += lane->getLength();
1453 837726 : lane = lane->getLinkCont()[0]->getViaLane();
1454 : }
1455 921483 : return len;
1456 : }
1457 :
1458 : double
1459 0 : MSLink::getInternalLengthsBefore() const {
1460 : double len = 0.;
1461 0 : const MSLane* lane = myInternalLane;
1462 :
1463 0 : while (lane != nullptr && lane->isInternal()) {
1464 0 : len += lane->getLength();
1465 0 : if (lane->getIncomingLanes().size() == 1) {
1466 0 : lane = lane->getIncomingLanes()[0].lane;
1467 : } else {
1468 : break;
1469 : }
1470 : }
1471 0 : return len;
1472 : }
1473 :
1474 :
1475 : double
1476 132198 : MSLink::getLengthsBeforeCrossing(const MSLane* foeLane) const {
1477 132198 : MSLane* via = myInternalLane;
1478 : double totalDist = 0.;
1479 : bool foundCrossing = false;
1480 135054 : while (via != nullptr) {
1481 133694 : MSLink* link = via->getLinkCont()[0];
1482 133694 : double dist = link->getLengthBeforeCrossing(foeLane);
1483 133694 : if (dist != INVALID_DOUBLE) {
1484 : // found conflicting lane
1485 130838 : totalDist += dist;
1486 : foundCrossing = true;
1487 : break;
1488 : } else {
1489 2856 : totalDist += via->getLength();
1490 : via = link->getViaLane();
1491 : }
1492 : }
1493 : if (foundCrossing) {
1494 130838 : return totalDist;
1495 : } else {
1496 : return INVALID_DOUBLE;
1497 : }
1498 : }
1499 :
1500 :
1501 : double
1502 133694 : MSLink::getLengthBeforeCrossing(const MSLane* foeLane) const {
1503 : int foe_ix;
1504 801527 : for (foe_ix = 0; foe_ix != (int)myFoeLanes.size(); ++foe_ix) {
1505 800031 : if (myFoeLanes[foe_ix] == foeLane) {
1506 : break;
1507 : }
1508 : }
1509 133694 : if (foe_ix == (int)myFoeLanes.size()) {
1510 : // no conflict with the given lane, indicate by returning -1
1511 : #ifdef MSLink_DEBUG_CROSSING_POINTS
1512 : std::cout << "No crossing of lanes '" << foeLane->getID() << "' and '" << myInternalLaneBefore->getID() << "'" << std::endl;
1513 : #endif
1514 : return INVALID_DOUBLE;
1515 : } else {
1516 : // found conflicting lane index
1517 132198 : double dist = myInternalLaneBefore->getLength() - myConflicts[foe_ix].getLengthBehindCrossing(this);
1518 132198 : if (dist == -10000.) {
1519 : // this is the value in myConflicts, if the relation allows intersection but none is present for the actual geometry.
1520 : return INVALID_DOUBLE;
1521 : }
1522 : #ifdef MSLink_DEBUG_CROSSING_POINTS
1523 : std::cout << "Crossing of lanes '" << myInternalLaneBefore->getID() << "' and '" << foeLane->getID()
1524 : << "' at distance " << dist << " (approach along '"
1525 : << myInternalLaneBefore->getEntryLink()->getLaneBefore()->getID() << "')" << std::endl;
1526 : #endif
1527 : return dist;
1528 : }
1529 : }
1530 :
1531 :
1532 : bool
1533 40505728 : MSLink::isEntryLink() const {
1534 40505728 : if (MSGlobals::gUsingInternalLanes) {
1535 64491430 : return myInternalLane != nullptr && myInternalLaneBefore == nullptr;
1536 : } else {
1537 : return false;
1538 : }
1539 : }
1540 :
1541 : bool
1542 19693092 : MSLink::isConflictEntryLink() const {
1543 : // either a non-cont entry link or the link after a cont-link
1544 19693092 : return !myAmCont && (isEntryLink() || (myInternalLaneBefore != nullptr && myInternalLane != nullptr));
1545 : }
1546 :
1547 : bool
1548 94563306 : MSLink::isExitLink() const {
1549 94563306 : if (MSGlobals::gUsingInternalLanes) {
1550 162449528 : return myInternalLaneBefore != nullptr && myInternalLane == nullptr;
1551 : } else {
1552 : return false;
1553 : }
1554 : }
1555 :
1556 : bool
1557 156964089 : MSLink::isExitLinkAfterInternalJunction() const {
1558 156964089 : if (MSGlobals::gUsingInternalLanes) {
1559 : return (getInternalLaneBefore() != nullptr
1560 82005144 : && myInternalLaneBefore->getIncomingLanes().size() == 1
1561 238604599 : && myInternalLaneBefore->getIncomingLanes().front().viaLink->isInternalJunctionLink());
1562 : } else {
1563 : return false;
1564 : }
1565 : }
1566 :
1567 :
1568 : const MSLink*
1569 142714 : MSLink::getCorrespondingExitLink() const {
1570 142714 : MSLane* lane = myInternalLane;
1571 : const MSLink* link = this;
1572 290004 : while (lane != nullptr) {
1573 147290 : link = lane->getLinkCont()[0];
1574 : lane = link->getViaLane();
1575 : }
1576 142714 : return link;
1577 : }
1578 :
1579 :
1580 : const MSLink*
1581 826250797 : MSLink::getCorrespondingEntryLink() const {
1582 : const MSLink* link = this;
1583 1111080858 : while (link->myLaneBefore->isInternal()) {
1584 : assert(myLaneBefore->getIncomingLanes().size() == 1);
1585 284830061 : link = link->myLaneBefore->getIncomingLanes().front().viaLink;
1586 : }
1587 826250797 : return link;
1588 : }
1589 :
1590 :
1591 : bool
1592 433948378 : MSLink::isInternalJunctionLink() const {
1593 433948378 : return getInternalLaneBefore() != nullptr && myInternalLane != nullptr;
1594 : }
1595 :
1596 :
1597 : const MSLink::LinkLeaders
1598 899142549 : MSLink::getLeaderInfo(const MSVehicle* ego, double dist, std::vector<const MSPerson*>* collectBlockers, bool isShadowLink) const {
1599 : LinkLeaders result;
1600 : // this link needs to start at an internal lane (either an exit link or between two internal lanes)
1601 : // or it must be queried by the pedestrian model (ego == 0)
1602 899142549 : if (ego != nullptr && (!fromInternalLane() || ego->getLaneChangeModel().isOpposite())) {
1603 : // ignore link leaders
1604 : return result;
1605 : }
1606 : //gDebugFlag1 = true;
1607 304903526 : if (gDebugFlag1) {
1608 0 : std::cout << SIMTIME << " getLeaderInfo link=" << getDescription() << " dist=" << dist << " isShadowLink=" << isShadowLink << "\n";
1609 : }
1610 304903526 : if (MSGlobals::gComputeLC && ego != nullptr && ego->getLane()->isNormal()) {
1611 27269231 : const MSLink* junctionEntry = getLaneBefore()->getEntryLink();
1612 6563837 : if (junctionEntry->haveRed() && !ego->ignoreRed(junctionEntry, true)
1613 : // check oncoming on bidiLane during laneChanging
1614 33827556 : && (!MSGlobals::gComputeLC || junctionEntry->getLaneBefore()->getBidiLane() == nullptr)) {
1615 6513743 : if (gDebugFlag1) {
1616 0 : std::cout << " ignore linkLeaders beyond red light\n";
1617 : }
1618 : return result;
1619 : }
1620 : }
1621 : // this is an exit link
1622 298389783 : const double extraGap = ego != nullptr ? ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_EXTRA_GAP, 0) : 0;
1623 784589430 : for (int i = 0; i < (int)myFoeLanes.size(); ++i) {
1624 486199647 : const MSLane* foeLane = myFoeLanes[i];
1625 486199647 : const MSLink* foeExitLink = foeLane->getLinkCont()[0];
1626 : // distance from the querying vehicle to the crossing point with foeLane
1627 486199647 : double distToCrossing = dist - myConflicts[i].getLengthBehindCrossing(this);
1628 486199647 : const double foeDistToCrossing = foeLane->getLength() - myConflicts[i].getFoeLengthBehindCrossing(foeExitLink);
1629 486199647 : const bool sameTarget = (myLane == foeExitLink->getLane()) && !isInternalJunctionLink() && !foeExitLink->isInternalJunctionLink();
1630 486199647 : const bool sameSource = (myInternalLaneBefore != nullptr && myInternalLaneBefore->getNormalPredecessorLane() == foeLane->getNormalPredecessorLane());
1631 486199647 : const double crossingWidth = (sameTarget || sameSource) ? 0 : myConflicts[i].conflictSize;
1632 486199647 : const double foeCrossingWidth = (sameTarget || sameSource) ? 0 : myConflicts[i].getFoeConflictSize(foeExitLink);
1633 : // special treatment of contLane foe only applies if this lane is not a contLane or contLane follower itself
1634 615442079 : const bool contLane = (foeExitLink->getViaLaneOrLane()->getEdge().isInternal() && !(
1635 129242432 : isInternalJunctionLink() || isExitLinkAfterInternalJunction()));
1636 486199647 : if (gDebugFlag1) {
1637 : std::cout << " distToCrossing=" << distToCrossing << " foeLane=" << foeLane->getID() << " cWidth=" << crossingWidth
1638 0 : << " flag=" << myConflicts[i].flag << " i=" << i << " fcIndex=" << myConflicts[i].foeConflictIndex
1639 0 : << " ijl=" << isInternalJunctionLink() << " sT=" << sameTarget << " sS=" << sameSource
1640 0 : << " lbc=" << myConflicts[i].getLengthBehindCrossing(this)
1641 0 : << " flbc=" << myConflicts[i].getFoeLengthBehindCrossing(foeExitLink)
1642 : << " cw=" << crossingWidth
1643 : << " fcw=" << foeCrossingWidth
1644 : << " contLane=" << contLane
1645 0 : << " state=" << toString(myState)
1646 0 : << " foeState=" << toString(foeExitLink->getState())
1647 0 : << "\n";
1648 : }
1649 107513816 : if (distToCrossing + crossingWidth < 0 && !sameTarget
1650 587974916 : && (ego == nullptr || !MSGlobals::gComputeLC || distToCrossing + crossingWidth + ego->getVehicleType().getLength() < 0)) {
1651 97625361 : if (gDebugFlag1) {
1652 0 : std::cout << " ignore:egoBeyondCrossingPoint\n";
1653 : }
1654 97625361 : continue; // vehicle is behind the crossing point, continue with next foe lane
1655 : }
1656 : bool ignoreGreenCont = false;
1657 : bool foeIndirect = false;
1658 388574286 : if (contLane) {
1659 42043617 : const MSLink* entry = getLaneBefore()->getEntryLink();
1660 42043617 : const MSLink* foeEntry = foeLane->getEntryLink();
1661 42043617 : foeIndirect = foeEntry->myAmIndirect;
1662 41763930 : if (entry != nullptr && entry->haveGreen()
1663 26753103 : && foeEntry != nullptr && foeEntry->haveGreen()
1664 54504469 : && entry->myLaneBefore != foeEntry->myLaneBefore) {
1665 : // ignore vehicles before an internaljunction as long as they are still in green minor mode
1666 : ignoreGreenCont = true;
1667 : }
1668 : }
1669 42043617 : if (foeIndirect && distToCrossing >= NO_INTERSECTION) {
1670 29409 : if (gDebugFlag1) {
1671 0 : std::cout << " ignore:noIntersection\n";
1672 : }
1673 29409 : continue;
1674 : }
1675 : // it is not sufficient to return the last vehicle on the foeLane because ego might be its leader
1676 : // therefore we return all vehicles on the lane
1677 : //
1678 : // special care must be taken for continuation lanes. (next lane is also internal)
1679 : // vehicles on cont. lanes or on internal lanes with the same target as this link can not be ignored
1680 : // and should block (gap = -1) unless they are part of an indirect turn
1681 : MSLane::AnyVehicleIterator end = foeLane->anyVehiclesEnd();
1682 29726203 : for (MSLane::AnyVehicleIterator it_veh = foeLane->anyVehiclesBegin(); it_veh != end; ++it_veh) {
1683 29726203 : MSVehicle* leader = (MSVehicle*)*it_veh;
1684 29726203 : const double leaderBack = leader->getBackPositionOnLane(foeLane) - extraGap;
1685 29726203 : const double leaderBackDist = foeDistToCrossing - leaderBack;
1686 29726203 : const double l2 = ego != nullptr ? ego->getLength() + 2 : 0; // add some slack to account for further meeting-angle effects
1687 29613715 : const double sagitta = ego != nullptr && myRadius != std::numeric_limits<double>::max() ? myRadius - sqrt(myRadius * myRadius - 0.25 * l2 * l2) : 0;
1688 29726203 : const bool pastTheCrossingPoint = leaderBackDist + foeCrossingWidth + sagitta < 0;
1689 29726203 : const bool enteredTheCrossingPoint = leaderBackDist < leader->getVehicleType().getLength();
1690 29726203 : const bool foeIsBicycleTurn = (leader->getVehicleType().getVehicleClass() == SVC_BICYCLE
1691 29726203 : && foeLane->getIncomingLanes().front().viaLink->getDirection() == LinkDirection::LEFT);
1692 29726203 : const bool ignoreIndirectBicycleTurn = pastTheCrossingPoint && foeIsBicycleTurn;
1693 29726203 : const bool cannotIgnore = ((contLane && !ignoreIndirectBicycleTurn) || sameTarget || (sameSource && !MSGlobals::gComputeLC)) && ego != nullptr;
1694 29726203 : const bool inTheWay = ((((!pastTheCrossingPoint && distToCrossing > 0) || (sameTarget && distToCrossing > leaderBackDist - leader->getLength()))
1695 23830066 : && (enteredTheCrossingPoint || (sameSource && !enteredTheCrossingPoint && foeDistToCrossing < distToCrossing))
1696 15794884 : && (!(myConflicts[i].flag == CONFLICT_DUMMY_MERGE) || foeIsBicycleTurn || sameSource))
1697 43701359 : || foeExitLink->getLaneBefore()->getNormalPredecessorLane() == myLane->getBidiLane());
1698 29726203 : const bool isOpposite = leader->getLaneChangeModel().isOpposite();
1699 29726203 : const auto avi = foeExitLink->getApproaching(leader);
1700 : // if leader is not found, assume that it performed a lane change in the last step
1701 29726203 : const bool willPass = avi.willPass || (avi.arrivalTime == INVALID_TIME && sameTarget);
1702 29726203 : if (gDebugFlag1) {
1703 : std::cout << " candidate leader=" << leader->getID()
1704 : << " cannotIgnore=" << cannotIgnore
1705 : << " fdtc=" << foeDistToCrossing
1706 : << " lb=" << leaderBack
1707 : << " lbd=" << leaderBackDist
1708 : << " fcwidth=" << foeCrossingWidth
1709 0 : << " r=" << myRadius
1710 : << " sagitta=" << sagitta
1711 : << " foePastCP=" << pastTheCrossingPoint
1712 : << " foeEnteredCP=" << enteredTheCrossingPoint
1713 : << " inTheWay=" << inTheWay
1714 : << " willPass=" << willPass
1715 0 : << " isFrontOnLane=" << leader->isFrontOnLane(foeLane)
1716 : << " ignoreGreenCont=" << ignoreGreenCont
1717 : << " foeIndirect=" << foeIndirect
1718 : << " foeBikeTurn=" << foeIsBicycleTurn
1719 0 : << " isOpposite=" << isOpposite << "\n";
1720 : }
1721 29726203 : if (leader == ego) {
1722 7963783 : continue;
1723 : }
1724 : // ignore greenCont foe vehicles that are not in the way
1725 28952822 : if (!inTheWay && ignoreGreenCont) {
1726 7088 : if (gDebugFlag1) {
1727 0 : std::cout << " ignoreGreenCont\n";
1728 : }
1729 7088 : continue;
1730 : }
1731 : // after entering the conflict area, ignore foe vehicles that are not in the way
1732 4160863 : if ((!MSGlobals::gComputeLC || (ego != nullptr && ego->getLane() == foeLane) || (MSGlobals::gSublane && !MSGlobals::gComputeLC))
1733 24967103 : && distToCrossing < -POSITION_EPS && !inTheWay
1734 29059997 : && (ego == nullptr || !MSGlobals::gComputeLC || distToCrossing < -ego->getVehicleType().getLength())) {
1735 89961 : if (gDebugFlag1) {
1736 0 : std::cout << " ego entered conflict area\n";
1737 : }
1738 89961 : continue;
1739 : }
1740 28930412 : if (!MSGlobals::gComputeLC
1741 24696650 : && sameSource
1742 7529482 : && &ego->getLane()->getEdge() == &myInternalLaneBefore->getEdge()
1743 29226211 : && leaderBack + leader->getLength() < ego->getPositionOnLane() - ego->getLength()) {
1744 : // ego is already on the junction and clearly ahead of foe
1745 74639 : if (gDebugFlag1) {
1746 0 : std::cout << " ego ahead of same-source foe\n";
1747 : }
1748 74639 : continue;
1749 : }
1750 :
1751 : // ignore foe vehicles that will not pass
1752 18670817 : if ((!cannotIgnore || leader->isStopped() || sameTarget)
1753 19745218 : && !willPass
1754 1590759 : && (avi.arrivalTime == INVALID_TIME || leader->getSpeed() < SUMO_const_haltingSpeed)
1755 1590486 : && leader->isFrontOnLane(foeLane)
1756 : && !isOpposite
1757 768162 : && !inTheWay
1758 : // willPass is false if the vehicle is already on the stopping edge
1759 29207212 : && !leader->willStop()) {
1760 425287 : if (gDebugFlag1) {
1761 0 : std::cout << " foe will not pass\n";
1762 : }
1763 425287 : continue;
1764 : }
1765 28355847 : if (leader->isBidiOn(foeLane)) {
1766 : // conflict resolved via forward lane of the foe
1767 325093 : continue;
1768 : }
1769 : // check whether foe is blocked and might need to change before leaving the junction
1770 28030754 : const bool foeStrategicBlocked = (leader->getLaneChangeModel().isStrategicBlocked() &&
1771 1338494 : leader->getCarFollowModel().brakeGap(leader->getSpeed()) <= foeLane->getLength() - leaderBack);
1772 28030754 : const bool sameInternalEdge = &myInternalLaneBefore->getEdge() == &foeExitLink->getInternalLaneBefore()->getEdge();
1773 :
1774 28030754 : const bool foeLaneIsBidi = myInternalLaneBefore->getBidiLane() == foeLane;
1775 28030754 : if (MSGlobals::gSublane && ego != nullptr && (sameSource || sameTarget || foeLaneIsBidi)
1776 7622714 : && (!foeStrategicBlocked || sameInternalEdge)) {
1777 7466867 : if (ego->getLane() == leader->getLane()) {
1778 172258 : continue;
1779 : }
1780 : // ignore vehicles if not in conflict sublane-wise
1781 7294609 : const double egoLatOffset = isShadowLink ? ego->getLatOffset(ego->getLaneChangeModel().getShadowLane()) : 0;
1782 7294609 : const double posLat = ego->getLateralPositionOnLane() + egoLatOffset;
1783 7294609 : double posLatLeader = leader->getLateralPositionOnLane() + leader->getLatOffset(foeLane);
1784 7294609 : if (foeLaneIsBidi) {
1785 : // leader is oncoming
1786 1473 : posLatLeader = foeLane->getWidth() - posLatLeader;
1787 : }
1788 7294609 : const double latGap = (fabs(posLat - posLatLeader)
1789 7294609 : - 0.5 * (ego->getVehicleType().getWidth() + leader->getVehicleType().getWidth()));
1790 7294609 : const double maneuverDist = leader->getLaneChangeModel().getManeuverDist() * (posLat < posLatLeader ? -1 : 1);
1791 7294609 : if (gDebugFlag1) {
1792 0 : std::cout << " checkIgnore sublaneFoe lane=" << myInternalLaneBefore->getID()
1793 : << " sameSource=" << sameSource
1794 : << " sameTarget=" << sameTarget
1795 : << " foeLaneIsBidi=" << foeLaneIsBidi
1796 : << " foeLane=" << foeLane->getID()
1797 : << " leader=" << leader->getID()
1798 0 : << " egoLane=" << ego->getLane()->getID()
1799 0 : << " leaderLane=" << leader->getLane()->getID()
1800 : << " egoLat=" << posLat
1801 : << " egoLatOffset=" << egoLatOffset
1802 : << " leaderLat=" << posLatLeader
1803 0 : << " leaderLatOffset=" << leader->getLatOffset(foeLane)
1804 : << " latGap=" << latGap
1805 : << " maneuverDist=" << maneuverDist
1806 0 : << " computeLC=" << MSGlobals::gComputeLC
1807 0 : << " egoMaxSpeedLat=" << ego->getVehicleType().getMaxSpeedLat()
1808 0 : << "\n";
1809 : }
1810 1581271 : if (latGap > 0 && (latGap > maneuverDist || !sameTarget || !MSGlobals::gComputeLC)
1811 : // do not perform sublane changes that interfere with the leader vehicle
1812 8869081 : && (!MSGlobals::gComputeLC || latGap > ego->getVehicleType().getMaxSpeedLat())) {
1813 1399613 : const MSLink* foeEntryLink = foeLane->getIncomingLanes().front().viaLink;
1814 1399613 : if (sameSource) {
1815 : // for lanes from the same edge, higer index implies a
1816 : // connection further to the left
1817 1076842 : const bool leaderFromRight = (myIndex > foeEntryLink->getIndex());
1818 1076842 : if ((posLat > posLatLeader) == leaderFromRight) {
1819 : // ignore speed since lanes diverge
1820 616741 : if (gDebugFlag1) {
1821 0 : std::cout << " ignored (same source) leaderFromRight=" << leaderFromRight << "\n";
1822 : }
1823 616741 : continue;
1824 : }
1825 322771 : } else if (sameTarget) {
1826 : // for lanes from different edges we cannot rely on the
1827 : // index due to wrap-around issues
1828 321298 : if (myDirection != foeEntryLink->getDirection()) {
1829 313493 : bool leaderFromRight = foeEntryLink->getDirection() < myDirection;
1830 : // leader vehicle should not move towards ego
1831 313493 : if (MSGlobals::gLefthand) {
1832 0 : leaderFromRight = !leaderFromRight;
1833 : }
1834 313493 : if (gDebugFlag1) {
1835 0 : std::cout << " leaderFromRight=" << leaderFromRight << "\n";
1836 : }
1837 457713 : if ((posLat > posLatLeader) == leaderFromRight
1838 : // leader should keep lateral position or move away from ego
1839 173956 : && (leader->getLaneChangeModel().getSpeedLat() == 0 || leader->getLaneChangeModel().getManeuverDist() == 0
1840 30963 : || leaderFromRight == (leader->getLaneChangeModel().getSpeedLat() < latGap))
1841 462116 : && (ego->getLaneChangeModel().getSpeedLat() == 0 || ego->getLaneChangeModel().getManeuverDist() == 0
1842 10523 : || leaderFromRight == (ego->getLaneChangeModel().getSpeedLat() > -latGap))) {
1843 144220 : if (gDebugFlag1) {
1844 0 : std::cout << " ignored (different source) leaderFromRight=" << leaderFromRight << "\n";
1845 : }
1846 144220 : continue;
1847 : }
1848 : } else {
1849 : // XXX figure out relative direction somehow
1850 : }
1851 : } else {
1852 1473 : if (gDebugFlag1) {
1853 0 : std::cout << " ignored oncoming bidi leader\n";
1854 : }
1855 1473 : continue;
1856 : }
1857 : }
1858 : }
1859 27096062 : if (leader->getWaitingTime() < MSGlobals::gIgnoreJunctionBlocker) {
1860 : // compute distance between vehicles on the superimposition of both lanes
1861 : // where the crossing point is the common point
1862 : double gap;
1863 : bool fromLeft = true;
1864 26593103 : if (ego == nullptr) {
1865 : // request from pedestrian model. return distance between leaderBack and crossing point
1866 : //std::cout << " foeLane=" << foeLane->getID() << " leaderBack=" << leaderBack << " foeDistToCrossing=" << foeDistToCrossing << " foeLength=" << foeLane->getLength() << " foebehind=" << myConflicts[i].second << " dist=" << dist << " behind=" << myConflicts[i].first << "\n";
1867 112078 : gap = leaderBackDist;
1868 : // distToCrossing should not take into account the with of the foe lane
1869 : // (which was subtracted in setRequestInformation)
1870 : // Instead, the width of the foe vehicle is used directly by the caller.
1871 112078 : distToCrossing += myConflicts[i].conflictSize / 2;
1872 112078 : if (gap + foeCrossingWidth < 0) {
1873 : // leader is completely past the crossing point
1874 : // or there is no crossing point
1875 4560261 : continue; // next vehicle
1876 : }
1877 : // we need to determine whether the vehicle passes the
1878 : // crossing from the left or the right (heuristic)
1879 109895 : fromLeft = foeDistToCrossing > 0.5 * foeLane->getLength();
1880 26481025 : } else if ((contLane && !sameSource && !ignoreIndirectBicycleTurn) || isOpposite) {
1881 1497712 : gap = -std::numeric_limits<double>::max(); // always break for vehicles which are on a continuation lane or for opposite-direction vehicles
1882 : } else {
1883 24983313 : if (pastTheCrossingPoint && !sameTarget) {
1884 : // leader is completely past the crossing point
1885 : // or there is no crossing point
1886 4557997 : if (gDebugFlag1) {
1887 0 : std::cout << " foePastCP ignored\n";
1888 : }
1889 4557997 : continue;
1890 : }
1891 : double leaderBackDist2 = leaderBackDist;
1892 20425316 : if (sameTarget && leaderBackDist2 < 0) {
1893 3176192 : const double mismatch = myConflicts[i].getFoeLengthBehindCrossing(foeExitLink) - myConflicts[i].getLengthBehindCrossing(this);
1894 3176192 : if (mismatch > 0) {
1895 1594878 : leaderBackDist2 += mismatch;
1896 : }
1897 : }
1898 20425316 : if (gDebugFlag1) {
1899 : std::cout << " distToCrossing=" << distToCrossing << " leaderBack=" << leaderBack
1900 : << " backDist=" << leaderBackDist
1901 : << " backDist2=" << leaderBackDist2
1902 0 : << " blockedStrategic=" << leader->getLaneChangeModel().isStrategicBlocked()
1903 0 : << "\n";
1904 : }
1905 20425316 : gap = distToCrossing - ego->getVehicleType().getMinGap() - leaderBackDist2 - foeCrossingWidth;
1906 : }
1907 : // if the foe is already moving off the intersection, we may
1908 : // advance up to the crossing point unless we have the same target or same source
1909 : // (for sameSource, the crossing point indicates the point of divergence)
1910 25779213 : const bool stopAsap = ((leader->isFrontOnLane(foeLane) ? cannotIgnore : (sameTarget || sameSource))
1911 22455030 : || (ego != nullptr && ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_ADVANCE, 1.0) == 0.0));
1912 22032923 : if (gDebugFlag1) {
1913 0 : std::cout << " leader=" << leader->getID() << " contLane=" << contLane << " cannotIgnore=" << cannotIgnore << " stopAsap=" << stopAsap << " gap=" << gap << "\n";
1914 : }
1915 22032923 : if (ignoreFoe(ego, leader)) {
1916 81 : continue;
1917 : }
1918 22032842 : const int llFlags = ((fromLeft ? LL_FROM_LEFT : 0) |
1919 22032842 : (inTheWay ? LL_IN_THE_WAY : 0) |
1920 22032842 : (sameSource ? LL_SAME_SOURCE : 0) |
1921 22032842 : (sameTarget ? LL_SAME_TARGET : 0));
1922 29090809 : result.emplace_back(leader, gap, stopAsap ? -1 : distToCrossing, llFlags, leader->getLatOffset(foeLane));
1923 : }
1924 :
1925 : }
1926 388544877 : if (ego != nullptr && MSNet::getInstance()->hasPersons()) {
1927 : // check for crossing pedestrians (keep driving if already on top of the crossing
1928 8919373 : const double distToPeds = distToCrossing - ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPLINE_CROSSING_GAP, MSPModel::SAFETY_GAP);
1929 8919373 : const double vehWidth = ego->getVehicleType().getWidth() + MSPModel::SAFETY_GAP; // + configurable safety gap
1930 : /// @todo consider lateral position (depending on whether the crossing is encountered on the way in or out)
1931 : // @check lefthand?!
1932 8919373 : const bool wayIn = myConflicts[i].lengthBehindCrossing < myLaneBefore->getLength() * 0.5;
1933 8919373 : const double vehCenter = (foeDistToCrossing + myLaneBefore->getWidth() * 0.5
1934 8919373 : + ego->getLateralPositionOnLane() * (wayIn ? -1 : 1));
1935 : // can access the movement model here since we already checked for existing persons above
1936 17373737 : if (distToPeds >= -MSPModel::SAFETY_GAP && MSNet::getInstance()->getPersonControl().getMovementModel()->blockedAtDist(ego, foeLane, vehCenter, vehWidth,
1937 8454364 : ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_CROSSING_GAP, JM_CROSSING_GAP_DEFAULT),
1938 : collectBlockers)) {
1939 519823 : result.emplace_back(nullptr, -1, distToPeds);
1940 8399550 : } else if (foeLane->isCrossing() && ego->getLane()->isInternal() && ego->getLane()->getEdge().getToJunction() == myJunction) {
1941 150521 : const MSLink* crossingLink = foeLane->getIncomingLanes()[0].viaLink;
1942 150521 : if (distToCrossing > 0 && crossingLink->havePriority() && crossingLink->myApproachingPersons != nullptr) {
1943 : // a person might step on the crossing at any moment, since ego
1944 : // is already on the junction, the opened() check is not done anymore
1945 26562 : const double timeToEnterCrossing = distToCrossing / MAX2(ego->getSpeed(), 1.0);
1946 31872 : for (const auto& item : (*crossingLink->myApproachingPersons)) {
1947 6860 : if (!ignoreFoe(ego, item.first) && timeToEnterCrossing > STEPS2TIME(item.second.arrivalTime - SIMSTEP)) {
1948 1550 : if (gDebugFlag1) {
1949 0 : std::cout << SIMTIME << ": " << ego->getID() << " breaking for approaching person " << item.first->getID()
1950 : //<< " dtc=" << distToCrossing << " ttc=" << distToCrossing / MAX2(ego->getSpeed(), 1.0) << " foeAT=" << item.second.arrivalTime << " foeTTC=" << STEPS2TIME(item.second.arrivalTime - SIMSTEP)
1951 0 : << "\n";
1952 : }
1953 1550 : result.emplace_back(nullptr, -1, distToPeds);
1954 1550 : break;
1955 : //} else {
1956 : // if (gDebugFlag1) {
1957 : // std::cout << SIMTIME << ": " << ego->getID() << " notBreaking for approaching person " << item.first->getID()
1958 : // << " dtc=" << distToCrossing << " ttc=" << distToCrossing / MAX2(ego->getSpeed(), 1.0) << " foeAT=" << item.second.arrivalTime << " foeTTC=" << STEPS2TIME(item.second.arrivalTime - SIMSTEP)
1959 : // << "\n";
1960 : // }
1961 : }
1962 : }
1963 : }
1964 : }
1965 : }
1966 : }
1967 :
1968 : //std::cout << SIMTIME << " ego=" << Named::getIDSecure(ego) << " link=" << getViaLaneOrLane()->getID() << " myWalkingAreaFoe=" << Named::getIDSecure(myWalkingAreaFoe) << "\n";
1969 298389783 : if (ego != nullptr) {
1970 297449397 : checkWalkingAreaFoe(ego, myWalkingAreaFoe, collectBlockers, result);
1971 297449397 : checkWalkingAreaFoe(ego, myWalkingAreaFoeExit, collectBlockers, result);
1972 : }
1973 :
1974 298389783 : if (MSGlobals::gLateralResolution > 0 && ego != nullptr && !isShadowLink) {
1975 : // check for foes on the same edge
1976 76330047 : for (std::vector<MSLane*>::const_iterator it = mySublaneFoeLanes.begin(); it != mySublaneFoeLanes.end(); ++it) {
1977 7303188 : const MSLane* foeLane = *it;
1978 : MSLane::AnyVehicleIterator end = foeLane->anyVehiclesEnd();
1979 4823300 : for (MSLane::AnyVehicleIterator it_veh = foeLane->anyVehiclesBegin(); it_veh != end; ++it_veh) {
1980 4823300 : MSVehicle* leader = (MSVehicle*)*it_veh;
1981 4823300 : if (leader == ego) {
1982 3031629 : continue;
1983 : }
1984 4146610 : if (leader->getLane()->isNormal()) {
1985 : // leader is past the conflict point
1986 1925859 : continue;
1987 : }
1988 2220751 : const double maxLength = MAX2(myInternalLaneBefore->getLength(), foeLane->getLength());
1989 2220751 : const double gap = dist - maxLength - ego->getVehicleType().getMinGap() + leader->getBackPositionOnLane(foeLane) - extraGap;
1990 2220751 : if (gap < -(ego->getVehicleType().getMinGap() + leader->getLength())) {
1991 : // ego is ahead of leader
1992 429080 : continue;
1993 : }
1994 1791671 : const double posLat = ego->getLateralPositionOnLane();
1995 1791671 : const double posLatLeader = leader->getLateralPositionOnLane() + leader->getLatOffset(foeLane);
1996 1791671 : if (gDebugFlag1) {
1997 0 : std::cout << " sublaneFoe lane=" << myInternalLaneBefore->getID()
1998 : << " foeLane=" << foeLane->getID()
1999 : << " leader=" << leader->getID()
2000 0 : << " egoLane=" << ego->getLane()->getID()
2001 0 : << " leaderLane=" << leader->getLane()->getID()
2002 : << " gap=" << gap
2003 : << " egoLat=" << posLat
2004 : << " leaderLat=" << posLatLeader
2005 0 : << " leaderLatOffset=" << leader->getLatOffset(foeLane)
2006 0 : << " egoIndex=" << myInternalLaneBefore->getIndex()
2007 0 : << " foeIndex=" << foeLane->getIndex()
2008 0 : << " dist=" << dist
2009 0 : << " leaderBack=" << leader->getBackPositionOnLane(foeLane)
2010 0 : << "\n";
2011 : }
2012 : // there only is a conflict if the paths cross
2013 731777 : if ((posLat < posLatLeader && myInternalLaneBefore->getIndex() > foeLane->getIndex())
2014 2131256 : || (posLat > posLatLeader && myInternalLaneBefore->getIndex() < foeLane->getIndex())) {
2015 816344 : if (gDebugFlag1) {
2016 0 : std::cout << SIMTIME << " blocked by " << leader->getID() << " (sublane split) foeLane=" << foeLane->getID() << "\n";
2017 : }
2018 816344 : if (ignoreFoe(ego, leader)) {
2019 0 : continue;
2020 : }
2021 816344 : result.emplace_back(leader, gap, -1, LL_SAME_SOURCE);
2022 : }
2023 : }
2024 : }
2025 : }
2026 : return result;
2027 0 : }
2028 :
2029 :
2030 : void
2031 594898794 : MSLink::checkWalkingAreaFoe(const MSVehicle* ego, const MSLane* foeLane, std::vector<const MSPerson*>* collectBlockers, LinkLeaders& result) const {
2032 594898794 : if (foeLane != nullptr && foeLane->getEdge().getPersons().size() > 0) {
2033 : // pedestrians may be on an arbitrary path across this
2034 : // walkingarea. make sure to keep enough distance.
2035 : // This is a simple but conservative solution that could be improved
2036 : // by ignoring pedestrians that are "obviously" not on a collision course
2037 124976 : double distToPeds = std::numeric_limits<double>::max();
2038 : assert(myInternalLaneBefore != nullptr);
2039 124976 : PositionVector egoPath = myInternalLaneBefore->getShape();
2040 124976 : if (ego->getLateralPositionOnLane() != 0) {
2041 110480 : egoPath.move2side((MSGlobals::gLefthand ? 1 : -1) * ego->getLateralPositionOnLane());
2042 : }
2043 937689 : for (MSTransportable* t : foeLane->getEdge().getPersons()) {
2044 812713 : MSPerson* p = static_cast<MSPerson*>(t);
2045 812713 : double dist = ego->getPosition().distanceTo2D(p->getPosition()) - p->getVehicleType().getLength();
2046 812713 : const bool inFront = isInFront(ego, egoPath, p->getPosition()) || isInFront(ego, egoPath, getFuturePosition(p));
2047 : if (inFront) {
2048 304424 : dist -= MAX2(ego->getVehicleType().getMinGap(), MSPModel::SAFETY_GAP);
2049 : }
2050 : #ifdef DEBUG_WALKINGAREA
2051 : if (ego->isSelected()) {
2052 : std::cout << SIMTIME << " veh=" << ego->getID() << " ped=" << p->getID()
2053 : << " pos=" << ego->getPosition() << " pedPos=" << p->getPosition()
2054 : << " futurePedPos=" << getFuturePosition(p)
2055 : << " rawDist=" << ego->getPosition().distanceTo2D(p->getPosition())
2056 : << " inFront=" << inFront
2057 : << " dist=" << dist << "\n";
2058 : }
2059 : #endif
2060 812713 : if (dist < ego->getVehicleType().getWidth() / 2 || inFront) {
2061 152547 : if (inFront) {
2062 152212 : const double oncomingFactor = isOnComingPed(ego, p);
2063 152212 : if (oncomingFactor > 0) {
2064 : // account for pedestrian movement while closing in
2065 53382 : const double timeToStop = sqrt(dist) / 2;
2066 53382 : const double pedDist = p->getMaxSpeed() * MAX2(timeToStop, TS) * oncomingFactor;
2067 53382 : dist = MAX2(0.0, dist - pedDist);
2068 : #ifdef DEBUG_WALKINGAREA
2069 : if (ego->isSelected()) {
2070 : std::cout << " timeToStop=" << timeToStop << " pedDist=" << pedDist << " factor=" << oncomingFactor << " dist2=" << dist << "\n";
2071 : }
2072 : #endif
2073 : }
2074 : }
2075 152547 : if (ignoreFoe(ego, p)) {
2076 26165 : continue;
2077 : }
2078 126382 : distToPeds = MIN2(distToPeds, dist);
2079 126382 : if (collectBlockers != nullptr) {
2080 0 : collectBlockers->push_back(p);
2081 : }
2082 : }
2083 : }
2084 124976 : if (distToPeds != std::numeric_limits<double>::max()) {
2085 : // leave extra space in front
2086 85899 : result.emplace_back(nullptr, -1, distToPeds);
2087 : }
2088 124976 : }
2089 594898794 : }
2090 :
2091 : bool
2092 1489385 : MSLink::isInFront(const MSVehicle* ego, const PositionVector& egoPath, const Position& pPos) const {
2093 1489385 : const double pedAngle = ego->getPosition().angleTo2D(pPos);
2094 1489385 : const double angleDiff = fabs(GeomHelper::angleDiff(ego->getAngle(), pedAngle));
2095 : #ifdef DEBUG_WALKINGAREA
2096 : if (ego->isSelected()) {
2097 : std::cout << " angleDiff=" << RAD2DEG(angleDiff) << "\n";
2098 : }
2099 : #endif
2100 1489385 : if (angleDiff < DEG2RAD(75)) {
2101 1057055 : return egoPath.distance2D(pPos) < ego->getVehicleType().getWidth() + MSPModel::SAFETY_GAP;
2102 : }
2103 : return false;
2104 : }
2105 :
2106 :
2107 : double
2108 152212 : MSLink::isOnComingPed(const MSVehicle* ego, const MSPerson* p) const {
2109 152212 : const double pedToEgoAngle = p->getPosition().angleTo2D(ego->getPosition());
2110 152212 : const double angleDiff = fabs(GeomHelper::angleDiff(p->getAngle(), pedToEgoAngle));
2111 : #ifdef DEBUG_WALKINGAREA
2112 : if (ego->isSelected()) {
2113 : std::cout << " ped-angleDiff=" << RAD2DEG(angleDiff) << " res=" << cos(angleDiff) << "\n";
2114 : }
2115 : #endif
2116 152212 : if (angleDiff <= DEG2RAD(90)) {
2117 : ;
2118 53382 : return cos(angleDiff);
2119 : } else {
2120 : return 0;
2121 : }
2122 : }
2123 :
2124 :
2125 : Position
2126 676672 : MSLink::getFuturePosition(const MSPerson* p, double timeHorizon) const {
2127 676672 : const double a = p->getAngle();
2128 676672 : const double dist = timeHorizon * p->getMaxSpeed();
2129 :
2130 676672 : const Position offset(cos(a) * dist, sin(a) * dist);
2131 676672 : return p->getPosition() + offset;
2132 : }
2133 :
2134 :
2135 : MSLink*
2136 10582734 : MSLink::getParallelLink(int direction) const {
2137 10582734 : if (direction == -1) {
2138 4109596 : return myParallelRight;
2139 6473138 : } else if (direction == 1) {
2140 5812532 : return myParallelLeft;
2141 : } else {
2142 : assert(false || myLane->getOpposite() != nullptr || MSGlobals::gComputeLC);
2143 : return nullptr;
2144 : }
2145 : }
2146 :
2147 : MSLink*
2148 167103 : MSLink::getOppositeDirectionLink() const {
2149 167103 : if (myLane->getOpposite() != nullptr && myLaneBefore->getOpposite() != nullptr) {
2150 42868 : for (MSLink* cand : myLane->getOpposite()->getLinkCont()) {
2151 39704 : if (cand->getLane() == myLaneBefore->getOpposite()) {
2152 : return cand;
2153 : }
2154 : }
2155 : }
2156 : return nullptr;
2157 : }
2158 :
2159 :
2160 : MSLink*
2161 5921592 : MSLink::computeParallelLink(int direction) {
2162 5921592 : const MSLane* const before = getLaneBefore()->getParallelLane(direction, false);
2163 5921592 : const MSLane* const after = getLane()->getParallelLane(direction, false);
2164 5921592 : if (before != nullptr && after != nullptr) {
2165 1003301 : for (MSLink* const link : before->getLinkCont()) {
2166 662258 : if (link->getLane() == after) {
2167 : return link;
2168 : }
2169 : }
2170 : }
2171 : return nullptr;
2172 : }
2173 :
2174 :
2175 : double
2176 1326926 : MSLink::getZipperSpeed(const MSVehicle* ego, const double dist, double vSafe,
2177 : SUMOTime arrivalTime,
2178 : const BlockingFoes* foes) const {
2179 1326926 : if (myFoeLinks.size() == 0) {
2180 : // link should have LINKSTATE_MAJOR in this case
2181 : assert(false);
2182 : return vSafe;
2183 : }
2184 1326926 : const double brakeGap = ego->getCarFollowModel().brakeGap(vSafe, ego->getCarFollowModel().getMaxDecel(), TS);
2185 1442377 : if (dist > MAX2(myFoeVisibilityDistance, brakeGap)) {
2186 : #ifdef DEBUG_ZIPPER
2187 : const SUMOTime now = MSNet::getInstance()->getCurrentTimeStep();
2188 : DEBUGOUT(DEBUG_COND_ZIPPER, SIMTIME << " getZipperSpeed ego=" << ego->getID()
2189 : << " dist=" << dist << " bGap=" << brakeGap << " ignoring foes (arrival in " << STEPS2TIME(arrivalTime - now) << ")\n")
2190 : #endif
2191 : return vSafe;
2192 : }
2193 : #ifdef DEBUG_ZIPPER
2194 : DEBUGOUT(DEBUG_COND_ZIPPER, SIMTIME << " getZipperSpeed ego=" << ego->getID()
2195 : << " egoAT=" << arrivalTime
2196 : << " dist=" << dist
2197 : << " brakeGap=" << brakeGap
2198 : << " vSafe=" << vSafe
2199 : << " numFoes=" << foes->size()
2200 : << "\n")
2201 : #endif
2202 : const bool uniqueFoeLink = myFoeLinks.size() == 1;
2203 497258 : MSLink* foeLink = myFoeLinks[0];
2204 2594425 : for (const auto& item : *foes) {
2205 2097167 : if (!item->isVehicle()) {
2206 0 : continue;
2207 : }
2208 2097167 : const MSVehicle* foe = dynamic_cast<const MSVehicle*>(item);
2209 : assert(foe != 0);
2210 : const ApproachingVehicleInformation* aviPtr = nullptr;
2211 2097167 : if (uniqueFoeLink) {
2212 1391940 : aviPtr = foeLink->getApproachingPtr(foe);
2213 : } else {
2214 : // figure out which link is approached by the current foe
2215 1067464 : for (MSLink* fl : myFoeLinks) {
2216 1067464 : aviPtr = fl->getApproachingPtr(foe);
2217 1067464 : if (aviPtr != nullptr) {
2218 : break;
2219 : }
2220 : }
2221 : }
2222 2097167 : if (aviPtr == nullptr) {
2223 0 : continue;
2224 : }
2225 : const ApproachingVehicleInformation& avi = *aviPtr;
2226 2097167 : const double foeDist = (foe->isActive() ? avi.dist : MAX2(0.0, avi.dist -
2227 36 : STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - foe->getLastActionTime()) * avi.speed));
2228 :
2229 1423763 : if ( // ignore vehicles that arrive after us (unless they are ahead and we could easily brake for them)
2230 2197819 : ((avi.arrivalTime > arrivalTime) && !couldBrakeForLeader(dist, foeDist, ego, foe)) ||
2231 : // also ignore vehicles that are behind us and are able to brake for us
2232 2197819 : couldBrakeForLeader(foeDist, dist, foe, ego) ||
2233 : // resolve ties by lane index
2234 673412 : (avi.arrivalTime == arrivalTime && foeDist == dist && ego->getLane()->getIndex() < foe->getLane()->getIndex())) {
2235 : #ifdef DEBUG_ZIPPER
2236 : if (DEBUG_COND_ZIPPER) std::cout
2237 : << " ignoring foe=" << foe->getID()
2238 : << " foeAT=" << avi.arrivalTime
2239 : << " foeDist=" << avi.dist
2240 : << " foeDist2=" << foeDist
2241 : << " foeSpeed=" << avi.speed
2242 : << " egoSpeed=" << ego->getSpeed()
2243 : << " deltaDist=" << foeDist - dist
2244 : << " delteSpeed=" << avi.speed - foe->getCarFollowModel().getMaxDecel() - ego->getSpeed()
2245 : << " egoCouldBrake=" << couldBrakeForLeader(dist, foeDist, ego, foe)
2246 : << " foeCouldBrake=" << couldBrakeForLeader(foeDist, dist, foe, ego)
2247 : << "\n";
2248 : #endif
2249 1423763 : continue;
2250 : }
2251 : // the idea behind speed adaption is three-fold:
2252 : // 1) ego needs to be in a car-following relationship with foe eventually
2253 : // thus, the ego speed should be equal to the follow speed once the foe enters
2254 : // the zipper junction
2255 : // 2) ego vehicle needs to put a certain distance beteen himself and foe (safeGap)
2256 : // achieving this distance can be spread over time but computing
2257 : // safeGap is subject to estimation errors of future speeds
2258 : // 3) deceleration can be spread out over the time until true
2259 : // car-following happens, at the start of speed adaptions, smaller
2260 : // decelerations should be sufficient
2261 :
2262 : // we cannot trust avi.arrivalSpeed if the foe has leader vehicles that are accelerating
2263 : // lets try to extrapolate
2264 673404 : const double uMax = foe->getLane()->getVehicleMaxSpeed(foe);
2265 673404 : const double uAccel = foe->getCarFollowModel().estimateSpeedAfterDistance(foeDist, avi.speed, foe->getCarFollowModel().getMaxAccel());
2266 : const double uEnd = MIN2(uMax, uAccel);
2267 673404 : const double uAvg = (avi.speed + uEnd) / 2;
2268 673404 : const double tf0 = foeDist / MAX2(NUMERICAL_EPS, uAvg);
2269 673404 : const double tf = MAX2(1.0, ceil((tf0) / TS) * TS);
2270 :
2271 673404 : const double vMax = ego->getLane()->getVehicleMaxSpeed(ego);
2272 673404 : const double vAccel = ego->getCarFollowModel().estimateSpeedAfterDistance(dist, ego->getSpeed(), ego->getCarFollowModel().getMaxAccel());
2273 673404 : const double vDecel = ego->getCarFollowModel().estimateSpeedAfterDistance(dist, ego->getSpeed(), -ego->getCarFollowModel().getMaxDecel());
2274 : const double vEnd = MIN3(vMax, vAccel, MAX2(uEnd, vDecel));
2275 673404 : const double vAvg = (ego->getSpeed() + vEnd) / 2;
2276 673404 : const double te0 = dist / MAX2(NUMERICAL_EPS, vAvg);
2277 673404 : const double te = MAX2(1.0, ceil((te0) / TS) * TS);
2278 :
2279 673404 : const double tTarget = tf + ego->getCarFollowModel().getHeadwayTime();
2280 673404 : const double a = ego->getCarFollowModel().avoidArrivalAccel(dist, tTarget, vSafe, ego->getCarFollowModel().getMaxDecel());
2281 :
2282 673404 : const double gap = dist - foe->getVehicleType().getLength() - ego->getVehicleType().getMinGap() - foeDist;
2283 673404 : const double vFollow = ego->getCarFollowModel().followSpeed(
2284 673404 : ego, ego->getSpeed(), gap, avi.speed, foe->getCarFollowModel().getMaxDecel(), foe);
2285 673404 : const double vSafeGap = MAX2(vFollow, ego->getSpeed() + ACCEL2SPEED(a));
2286 :
2287 : // scale behavior based on ego time to link (te)
2288 673404 : const double w = MIN2(1.0, te / 10);
2289 673404 : const double maxDecel = w * ego->getCarFollowModel().getMaxDecel() + (1 - w) * ego->getCarFollowModel().getEmergencyDecel();
2290 673404 : const double vZipper = MAX3(vFollow, ego->getSpeed() - ACCEL2SPEED(maxDecel), vSafeGap);
2291 :
2292 : vSafe = MIN2(vSafe, vZipper);
2293 : #ifdef DEBUG_ZIPPER
2294 : if (DEBUG_COND_ZIPPER) std::cout << " adapting to foe=" << foe->getID()
2295 : << " foeDist=" << foeDist
2296 : << " foeSpeed=" << avi.speed
2297 : << " foeAS=" << avi.arrivalSpeed
2298 : << " egoSpeed=" << ego->getSpeed()
2299 : << " uMax=" << uMax
2300 : << " uAccel=" << uAccel
2301 : << " uEnd=" << uEnd
2302 : << " uAvg=" << uAvg
2303 : << " gap=" << gap
2304 : << "\n "
2305 : << " tf=" << tf
2306 : << " te=" << te
2307 : << " aSafeGap=" << a
2308 : << " vMax=" << vMax
2309 : << " vAccel=" << vAccel
2310 : << " vDecel=" << vDecel
2311 : << " vEnd=" << vEnd
2312 : << " vSafeGap=" << vSafeGap
2313 : << " vFollow=" << vFollow
2314 : << " w=" << w
2315 : << " maxDecel=" << maxDecel
2316 : << " vZipper=" << vZipper
2317 : << " vSafe=" << vSafe
2318 : << "\n";
2319 : #endif
2320 : }
2321 : return vSafe;
2322 : }
2323 :
2324 :
2325 : bool
2326 2197819 : MSLink::couldBrakeForLeader(double followDist, double leaderDist, const MSVehicle* follow, const MSVehicle* leader) {
2327 : return (// leader is ahead of follower
2328 2197819 : followDist > leaderDist &&
2329 : // and follower could brake for 1 s to stay behind leader
2330 393662 : followDist - leaderDist > follow->getSpeed() - follow->getCarFollowModel().getMaxDecel() - leader->getSpeed());
2331 : }
2332 :
2333 :
2334 : void
2335 2960796 : MSLink::initParallelLinks() {
2336 2960796 : myParallelRight = computeParallelLink(-1);
2337 2960796 : myParallelLeft = computeParallelLink(1);
2338 2960796 : }
2339 :
2340 : bool
2341 77124 : MSLink::checkContOff() const {
2342 : // check whether this link gets to keep its cont status switching the tls off
2343 : // @note: this could also be pre-computed in netconvert
2344 : // we check whether there is any major link from this edge
2345 226012 : for (const MSLane* cand : myLaneBefore->getEdge().getLanes()) {
2346 447515 : for (const MSLink* link : cand->getLinkCont()) {
2347 298627 : if (link->getOffState() == LINKSTATE_TL_OFF_NOSIGNAL) {
2348 : return true;
2349 : }
2350 : }
2351 : }
2352 : return false;
2353 : }
2354 :
2355 : bool
2356 4364892 : MSLink::lateralOverlap(double posLat, double width, double posLat2, double width2) {
2357 4364892 : return fabs(posLat2 - posLat) < (width + width2) / 2;
2358 : }
2359 :
2360 : std::string
2361 0 : MSLink::getDescription() const {
2362 0 : return myLaneBefore->getID() + "->" + getViaLaneOrLane()->getID();
2363 : }
2364 :
2365 :
2366 : bool
2367 190343388 : MSLink::ignoreFoe(const SUMOTrafficObject* ego, const SUMOTrafficObject* foe) {
2368 190343388 : if (ego == nullptr || !ego->getParameter().wasSet(VEHPARS_JUNCTIONMODEL_PARAMS_SET)) {
2369 190309835 : return false;
2370 : }
2371 33553 : const SUMOVehicleParameter& param = ego->getParameter();
2372 73462 : for (const std::string& typeID : StringTokenizer(param.getParameter(toString(SUMO_ATTR_JM_IGNORE_TYPES), "")).getVector()) {
2373 32650 : if (typeID == foe->getVehicleType().getID()) {
2374 : return true;
2375 : }
2376 33553 : }
2377 16507 : for (const std::string& id : StringTokenizer(param.getParameter(toString(SUMO_ATTR_JM_IGNORE_IDS), "")).getVector()) {
2378 2293 : if (id == foe->getID()) {
2379 : return true;
2380 : }
2381 7259 : }
2382 6955 : return false;
2383 : }
2384 :
2385 :
2386 : void
2387 112873 : MSLink::updateDistToFoePedCrossing(double dist) {
2388 112873 : myDistToFoePedCrossing = MIN2(myDistToFoePedCrossing, dist);
2389 112873 : }
2390 :
2391 :
2392 : std::pair<const SUMOVehicle* const, const MSLink::ApproachingVehicleInformation>
2393 10499234 : MSLink::getClosest() const {
2394 : assert(getApproaching().size() > 0);
2395 : double minDist = std::numeric_limits<double>::max();
2396 : auto closestIt = getApproaching().begin();
2397 20999197 : for (auto apprIt = getApproaching().begin(); apprIt != getApproaching().end(); apprIt++) {
2398 10499963 : if (apprIt->second.dist < minDist) {
2399 : minDist = apprIt->second.dist;
2400 : closestIt = apprIt;
2401 : }
2402 : }
2403 : // maybe a parallel link has a closer vehicle
2404 : /*
2405 : for (MSLink* link2 : link->getLaneBefore()->getLinkCont()) {
2406 : if (link2 != link) {
2407 : for (auto apprIt2 = link2->getApproaching().begin(); apprIt2 != link2->getApproaching().end(); apprIt2++) {
2408 : if (apprIt2->second.dist < minDist) {
2409 : minDist = apprIt2->second.dist;
2410 : closestIt = apprIt2;
2411 : }
2412 : }
2413 : }
2414 : }
2415 : */
2416 10499234 : return *closestIt;
2417 : }
2418 :
2419 :
2420 : bool
2421 472205 : MSLink::railSignalWasPassed() const {
2422 472205 : if (myJunction != nullptr && myJunction->getType() == SumoXMLNodeType::RAIL_SIGNAL) {
2423 2015 : for (const auto& item : myApproachingVehicles) {
2424 12 : if (item.second.dist < SPEED2DIST(item.first->getSpeed())) {
2425 : return true;
2426 : }
2427 : }
2428 : }
2429 : return false;
2430 : }
2431 :
2432 : /****************************************************************************/
|
