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 483527455 : MSLink::ConflictInfo::getFoeLengthBehindCrossing(const MSLink* foeExitLink) const {
84 483527455 : if (flag == CONFLICT_DUMMY_MERGE) {
85 : return 0;
86 473641527 : } else if (foeConflictIndex >= 0) {
87 451526807 : return foeExitLink->myConflicts[foeConflictIndex].lengthBehindCrossing;
88 : } else {
89 : return -NO_INTERSECTION;
90 : }
91 : }
92 :
93 : double
94 250500214 : MSLink::ConflictInfo::getFoeConflictSize(const MSLink* foeExitLink) const {
95 250500214 : if (foeConflictIndex >= 0) {
96 228454686 : return foeExitLink->myConflicts[foeConflictIndex].conflictSize;
97 : } else {
98 : return 0;
99 : }
100 : }
101 :
102 : double
103 483658469 : MSLink::ConflictInfo::getLengthBehindCrossing(const MSLink* exitLink) const {
104 483658469 : if (flag == CONFLICT_STOP_AT_INTERNAL_JUNCTION) {
105 20032987 : return exitLink->getInternalLaneBefore()->getLength();
106 : } else {
107 463625482 : return lengthBehindCrossing;
108 : }
109 : }
110 :
111 : // ===========================================================================
112 : // member method definitions
113 : // ===========================================================================
114 2920605 : 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 2920605 : bool indirect) :
118 2920605 : myLane(succLane),
119 2920605 : myLaneBefore(predLane),
120 2920605 : myApproachingPersons(nullptr),
121 2920605 : myIndex(-1),
122 2920605 : myTLIndex(tlIndex),
123 2920605 : myLogic(logic),
124 2920605 : myState(state),
125 2920605 : myLastGreenState(LINKSTATE_TL_GREEN_MINOR),
126 2920605 : myOffState(state),
127 2920605 : myLastStateChange(SUMOTime_MIN / 2), // a large negative value, but avoid overflows when subtracting
128 2920605 : myDirection(dir),
129 2920605 : myLength(length),
130 2920605 : myFoeVisibilityDistance(foeVisibilityDistance),
131 2920605 : myDistToFoePedCrossing(std::numeric_limits<double>::max()),
132 2920605 : myHasFoes(false),
133 2920605 : myAmCont(false),
134 2920605 : myAmContOff(false),
135 2920605 : myKeepClear(keepClear),
136 2920605 : myInternalLane(via),
137 2920605 : myInternalLaneBefore(nullptr),
138 2920605 : myMesoTLSPenalty(0),
139 2920605 : myGreenFraction(1),
140 2920605 : myLateralShift(0),
141 2920605 : myOffFoeLinks(nullptr),
142 2920605 : myWalkingAreaFoe(nullptr),
143 2920605 : myWalkingAreaFoeExit(nullptr),
144 2920605 : myHavePedestrianCrossingFoe(false),
145 2920605 : myParallelRight(nullptr),
146 2920605 : myParallelLeft(nullptr),
147 2920605 : myAmIndirect(indirect),
148 2920605 : myRadius(std::numeric_limits<double>::max()),
149 2920605 : myPermissions(myLaneBefore->getPermissions() & myLane->getPermissions() & (via == nullptr ? SVCAll : via->getPermissions())),
150 2920605 : myJunction(nullptr) {
151 :
152 2920605 : if (MSGlobals::gLateralResolution > 0) {
153 : // detect lateral shift from lane geometries
154 : //std::cout << "DEBUG link=" << myLaneBefore->getID() << "->" << getViaLaneOrLane()->getID() << " hasInternal=" << MSNet::getInstance()->hasInternalLinks() << " shapeBefore=" << myLaneBefore->getShape().back() << " shapeFront=" << getViaLaneOrLane()->getShape().front() << "\n";
155 206049 : if ((myInternalLane != nullptr || predLane->isInternal())
156 514865 : && myLaneBefore->getShape().back() != getViaLaneOrLane()->getShape().front()) {
157 : PositionVector from = myLaneBefore->getShape();
158 : const PositionVector& to = getViaLaneOrLane()->getShape();
159 : const double dist = from.back().distanceTo2D(to.front());
160 : // figure out direction of shift
161 : try {
162 652 : from.move2side(dist);
163 0 : } catch (InvalidArgument&) {
164 0 : }
165 652 : myLateralShift = (from.back().distanceTo2D(to.front()) < dist) ? dist : -dist;
166 652 : if (MSGlobals::gLefthand) {
167 90 : myLateralShift *= -1;
168 : }
169 : //std::cout << " lateral shift link=" << myLaneBefore->getID() << "->" << getViaLaneOrLane()->getID() << " dist=" << dist << " shift=" << myLateralShift << "\n";
170 652 : }
171 : }
172 2920605 : }
173 :
174 :
175 2892886 : MSLink::~MSLink() {
176 2892886 : delete myOffFoeLinks;
177 2894220 : delete myApproachingPersons;
178 2892886 : }
179 :
180 :
181 : void
182 6 : MSLink::addCustomConflict(const MSLane* from, const MSLane* to, double startPos, double endPos) {
183 6 : myCustomConflicts.push_back(CustomConflict(from, to, startPos, endPos));
184 6 : }
185 :
186 : const MSLink::CustomConflict*
187 3977340 : MSLink::getCustomConflict(const MSLane* foeLane) const {
188 3977340 : if (myCustomConflicts.size() > 0) {
189 24 : const MSLane* foeFrom = foeLane->getNormalPredecessorLane();
190 24 : const MSLane* foeTo = foeLane->getNormalSuccessorLane();
191 36 : for (const CustomConflict& cc : myCustomConflicts) {
192 24 : if (cc.from == foeFrom && cc.to == foeTo) {
193 : return &cc;
194 : }
195 : }
196 :
197 : }
198 : return nullptr;
199 : }
200 :
201 : void
202 2640370 : MSLink::setRequestInformation(int index, bool hasFoes, bool isCont,
203 : const std::vector<MSLink*>& foeLinks,
204 : const std::vector<MSLane*>& foeLanes,
205 : MSLane* internalLaneBefore) {
206 : //#ifdef MSLink_DEBUG_CROSSING_POINTS
207 : // std::cout << " setRequestInformation() for junction " << getViaLaneOrLane()->getEdge().getFromJunction()->getID()
208 : // << "\nInternalLanes = " << toString(getViaLaneOrLane()->getEdge().getFromJunction()->getInternalLanes())
209 : // << std::endl;
210 : //#endif
211 2640370 : myIndex = index;
212 2640370 : myHasFoes = hasFoes;
213 2640370 : myAmCont = isCont && MSGlobals::gUsingInternalLanes;
214 2640370 : myFoeLinks = foeLinks;
215 9954182 : for (MSLane* foeLane : foeLanes) {
216 : // cannot assign vector due to const-ness
217 7313812 : myFoeLanes.push_back(foeLane);
218 : }
219 2640370 : myJunction = const_cast<MSJunction*>(myLane->getEdge().getFromJunction()); // junctionGraph is initialized after the whole network is loaded
220 2640370 : myAmContOff = isCont && myLogic != nullptr && internalLaneBefore == nullptr && checkContOff();
221 2640370 : myInternalLaneBefore = internalLaneBefore;
222 : MSLane* lane = nullptr;
223 : if (internalLaneBefore != nullptr) {
224 : // this is an exit link. compute crossing points with all foeLanes
225 : lane = internalLaneBefore;
226 : //} else if (myLane->isCrossing()) {
227 : // // this is the link to a pedestrian crossing. compute crossing points with all foeLanes
228 : // // @note not currently used by pedestrians
229 : // lane = myLane;
230 : }
231 2640370 : const MSLink* entryLink = getCorrespondingEntryLink();
232 2640370 : if (entryLink->getOffState() == LinkState::LINKSTATE_ALLWAY_STOP && entryLink->getTLLogic() != nullptr) {
233 : // TLS has "normal" right of way rules but all conflicting links are foes when switching TLS off
234 : // (unless it's an internal junction link which should ignore all foes and should be ignored by all foes
235 5762 : myOffFoeLinks = new std::vector<MSLink*>();
236 5762 : if (isEntryLink()) {
237 15648 : for (MSLane* foeLane : foeLanes) {
238 : assert(foeLane->isInternal() || foeLane->isCrossing());
239 13140 : MSLink* viaLink = foeLane->getIncomingLanes().front().viaLink;
240 13140 : if (viaLink->getLaneBefore()->isNormal()) {
241 7960 : myOffFoeLinks->push_back(viaLink);
242 : }
243 : }
244 : }
245 : }
246 : #ifdef MSLink_DEBUG_CROSSING_POINTS
247 : std::cout << "link " << myIndex << " to " << getViaLaneOrLane()->getID() << " internalLaneBefore=" << (lane == 0 ? "NULL" : lane->getID()) << " has foes: " << toString(foeLanes) << "\n";
248 : #endif
249 2640370 : if (lane != nullptr) {
250 1009259 : const bool beforeInternalJunction = lane->getLinkCont()[0]->getViaLaneOrLane()->getEdge().isInternal();
251 1009259 : if (lane->getIncomingLanes().size() != 1) {
252 0 : throw ProcessError(TLF("Internal lane '%' has % predecessors", lane->getID(), toString(lane->getIncomingLanes().size())));
253 : }
254 1009259 : const MSLink* junctionEntryLink = lane->getEntryLink();
255 1009259 : const bool isSecondPart = isExitLinkAfterInternalJunction();
256 : // compute crossing points
257 5144936 : for (const MSLane* foeLane : myFoeLanes) {
258 4135677 : const CustomConflict* cc = junctionEntryLink != nullptr ? junctionEntryLink->getCustomConflict(foeLane) : nullptr;
259 3977328 : if (cc != nullptr) {
260 : // handle custom conflict definition
261 12 : double startPos = cc->startPos;
262 12 : const double conflictSize = cc->endPos - cc->startPos;
263 12 : if (isSecondPart) {
264 0 : startPos -= junctionEntryLink->getViaLane()->getLength();
265 : }
266 : // the foe connection may be split at an internal
267 : // junction, we need to figure out whether the current
268 : // foeLane is the intended target for the custom conflict
269 : // There are two possibilities:
270 : // a) We have no custom conflict for the reverse pair of connections
271 : // -> just check whether lane and foeLane intersect
272 : // b) We have a "reverse" custom conflict
273 : // -> check whether it covers the foeLane
274 12 : const CustomConflict* rcc = foeLane->getEntryLink()->getCustomConflict(lane);
275 : bool haveIntersection = false;
276 12 : if (rcc == nullptr) {
277 : // a)
278 12 : haveIntersection = lane->getShape().intersectsAtLengths2D(foeLane->getShape()).size() > 0;
279 : } else {
280 : // b)
281 0 : const bool foeIsSecondPart = foeLane->getLogicalPredecessorLane()->isInternal();
282 0 : double foeStartPos = rcc->startPos;
283 0 : const double foeConflictSize = rcc->endPos - rcc->startPos;
284 0 : if (foeIsSecondPart) {
285 0 : foeStartPos -= foeLane->getLogicalPredecessorLane()->getLength();
286 : }
287 0 : const double foeEndPos = foeStartPos + foeConflictSize;
288 0 : haveIntersection = ((foeStartPos > 0 && foeStartPos < foeLane->getLength())
289 0 : || (foeEndPos > 0 && foeEndPos < foeLane->getLength()));
290 : }
291 12 : if (haveIntersection) {
292 6 : myConflicts.push_back(ConflictInfo(lane->getLength() - startPos, conflictSize));
293 : } else {
294 6 : myConflicts.push_back(ConflictInfo(-NO_INTERSECTION, 0));
295 : }
296 : #ifdef MSLink_DEBUG_CROSSING_POINTS
297 : std::cout << " " << lane->getID() << " custom conflict with " << foeLane->getID() << " customReverse=" << (rcc != nullptr)
298 : << " haveIntersection=" << haveIntersection
299 : << " startPos=" << startPos << " conflictSize=" << conflictSize
300 : << " lbc=" << myConflicts.back().lengthBehindCrossing
301 : << "\n";
302 : #endif
303 12 : continue;
304 12 : }
305 4135665 : myHavePedestrianCrossingFoe = myHavePedestrianCrossingFoe || foeLane->isCrossing();
306 4135665 : const bool sameTarget = myLane == foeLane->getLinkCont()[0]->getLane();
307 4135665 : if (sameTarget && !beforeInternalJunction && !contIntersect(lane, foeLane)) {
308 : //if (myLane == foeLane->getLinkCont()[0]->getLane()) {
309 : // this foeLane has the same target and merges at the end (lane exits the junction)
310 1378943 : const double minDist = MIN2(DIVERGENCE_MIN_WIDTH, 0.5 * (lane->getWidth() + foeLane->getWidth()));
311 1378943 : if (lane->getShape().back().distanceTo2D(foeLane->getShape().back()) >= minDist) {
312 : // account for lateral shift by the entry links
313 125769 : if (foeLane->getEntryLink()->isIndirect()) {
314 41 : myConflicts.push_back(ConflictInfo(-NO_INTERSECTION, 0)); // dummy value, never used
315 : #ifdef MSLink_DEBUG_CROSSING_POINTS
316 : std::cout << " " << lane->getID() << " dummy merge with indirect" << foeLane->getID() << "\n";
317 : #endif
318 : } else {
319 125728 : myConflicts.push_back(ConflictInfo(0, foeLane->getWidth(), CONFLICT_DUMMY_MERGE)); // dummy value, never used
320 : #ifdef MSLink_DEBUG_CROSSING_POINTS
321 : std::cout << " " << lane->getID() << " dummy merge with " << foeLane->getID() << "\n";
322 : #endif
323 : }
324 : } else {
325 1253174 : const double distAfterDivergence = computeDistToDivergence(lane, foeLane, minDist, false);
326 : const double lbcLane = lane->interpolateGeometryPosToLanePos(distAfterDivergence);
327 1253174 : myConflicts.push_back(ConflictInfo(lbcLane, foeLane->getWidth()));
328 : #ifdef MSLink_DEBUG_CROSSING_POINTS
329 : std::cout
330 : << " " << lane->getID()
331 : << " merges with " << foeLane->getID()
332 : << " nextLane " << lane->getLinkCont()[0]->getViaLaneOrLane()->getID()
333 : << " dist1=" << myConflicts.back().lengthBehindCrossing
334 : << "\n";
335 : #endif
336 : }
337 : } else {
338 2756722 : std::vector<double> intersections1 = lane->getShape().intersectsAtLengths2D(foeLane->getShape());
339 : #ifdef MSLink_DEBUG_CROSSING_POINTS_DETAILS
340 : std::cout << " intersections1=" << toString(intersections1) << "\n";
341 : #endif
342 : bool haveIntersection = true;
343 2756722 : if (intersections1.size() == 0) {
344 1429561 : intersections1.push_back(-NO_INTERSECTION); // disregard this foe (using maxdouble leads to nasty problems down the line)
345 : haveIntersection = false;
346 1327161 : } else if (intersections1.size() > 1) {
347 1306 : std::sort(intersections1.begin(), intersections1.end());
348 : }
349 2756722 : std::vector<double> intersections2 = foeLane->getShape().intersectsAtLengths2D(lane->getShape());
350 : #ifdef MSLink_DEBUG_CROSSING_POINTS_DETAILS
351 : std::cout << " intersections2=" << toString(intersections2) << "\n";
352 : #endif
353 2756722 : if (intersections2.size() == 0) {
354 1429561 : intersections2.push_back(0);
355 1327161 : } else if (intersections2.size() > 1) {
356 1306 : std::sort(intersections2.begin(), intersections2.end());
357 : }
358 :
359 : // check for near-intersection (internal junctions for a side road which are only relevant when they have stranded vehicles))
360 2756722 : if (!haveIntersection && foeLane->getLinkCont()[0]->getViaLane() != nullptr) {
361 257575 : const Position waitPos = foeLane->getShape().back();
362 257575 : const double dist = lane->getShape().distance2D(waitPos, true);
363 257575 : if (dist != GeomHelper::INVALID_OFFSET && dist < lane->getWidth() / 2) {
364 : // risk of collision
365 : intersections1.clear();
366 : intersections2.clear();
367 29540 : intersections1.push_back(lane->getShape().nearest_offset_to_point2D(waitPos));
368 29540 : intersections2.push_back(foeLane->getShape().length());
369 : haveIntersection = true;
370 : #ifdef MSLink_DEBUG_CROSSING_POINTS_DETAILS
371 : std::cout << " link=" << myIndex << " " << getDescription() << " almostIntersection with foeLane " << foeLane->getID() << " offset=" << intersections1.back() << "\n";
372 : #endif
373 : }
374 : }
375 :
376 : double conflictSize = foeLane->getWidth();
377 : ConflictFlag flag = CONFLICT_NO_INTERSECTION;
378 2727182 : if (haveIntersection) {
379 : flag = CONFLICT_DEFAULT;
380 1356701 : const double angle1 = GeomHelper::naviDegree(lane->getShape().rotationAtOffset(intersections1.back()));
381 1356701 : const double angle2 = GeomHelper::naviDegree(foeLane->getShape().rotationAtOffset(intersections2.back()));
382 1356701 : const double angleDiff = GeomHelper::getMinAngleDiff(angle1, angle2);
383 : //const double angleDiff = MIN2(GeomHelper::getMinAngleDiff(angle1, angle2),
384 : // GeomHelper::getMinAngleDiff(angle1, angle2 + 180));
385 1356701 : const double widthFactor = 1 / MAX2(sin(DEG2RAD(angleDiff)), 0.2) * 2 - 1;
386 : //std::cout << " intersection of " << lane->getID() << " with " << foeLane->getID() << " angle1=" << angle1 << " angle2=" << angle2 << " angleDiff=" << angleDiff << " widthFactor=" << widthFactor << "\n";
387 1356701 : conflictSize *= widthFactor;
388 : conflictSize = MIN2(conflictSize, lane->getLength());
389 : // lane width affects the crossing point
390 1356701 : intersections1.back() -= conflictSize / 2;
391 : // ensure non-negative offset for weird geometries
392 1356701 : intersections1.back() = MAX2(0.0, intersections1.back());
393 :
394 : // also length/geometry factor. (XXX: Why subtract width/2 *before* converting geometric position to lane pos? refs #3031)
395 1356701 : intersections1.back() = lane->interpolateGeometryPosToLanePos(intersections1.back());
396 :
397 1356701 : if (internalLaneBefore->getLogicalPredecessorLane()->getEdge().isInternal() && !foeLane->isCrossing()) {
398 : flag = CONFLICT_STOP_AT_INTERNAL_JUNCTION;
399 : }
400 :
401 1356701 : if (foeLane->isCrossing()) {
402 110858 : const MSLink* before = myInternalLaneBefore->getCanonicalPredecessorLane()->getLinkTo(myInternalLaneBefore);
403 110858 : const_cast<MSLink*>(before)->updateDistToFoePedCrossing(intersections1.back());
404 : };
405 : }
406 :
407 2756722 : myConflicts.push_back(ConflictInfo(
408 2756722 : lane->getLength() - intersections1.back(),
409 : conflictSize, flag));
410 :
411 : #ifdef MSLink_DEBUG_CROSSING_POINTS
412 : std::cout
413 : << " intersection of " << lane->getID()
414 : << " totalLength=" << lane->getLength()
415 : << " with " << foeLane->getID()
416 : << " totalLength=" << foeLane->getLength()
417 : << " dist1=" << myConflicts.back().lengthBehindCrossing
418 : << " widthFactor=" << myConflicts.back().conflictSize / foeLane->getWidth()
419 : << "\n";
420 : #endif
421 2756722 : }
422 : }
423 : // check for overlap with internal lanes from the same source lane
424 1009259 : const MSLane* pred = lane->getLogicalPredecessorLane();
425 : // to avoid overlap with vehicles that came from pred (especially when pred has endOffset > 0)
426 : // we add all other internal lanes from pred as foeLanes
427 3290090 : for (const MSLink* const link : pred->getLinkCont()) {
428 2280831 : const MSLane* const sibling = link->getViaLane();
429 2280831 : if (sibling != lane && sibling != nullptr) {
430 1246784 : const double minDist = MIN2(DIVERGENCE_MIN_WIDTH, 0.5 * (lane->getWidth() + sibling->getWidth()));
431 1246784 : if (lane->getShape().front().distanceTo2D(sibling->getShape().front()) >= minDist) {
432 : // account for lateral shift by the entry links
433 630 : continue;
434 : }
435 1246154 : const double distToDivergence = computeDistToDivergence(lane, sibling, minDist, true);
436 : double lbcLane;
437 1246154 : if (lane->getLength() == sibling->getLength() && &lane->getEdge() == &sibling->getEdge()) {
438 : // for parallel lanes, avoid inconsistency in distance estimation (#10988)
439 : // between forward distance (getLeaderInfo)
440 : // and backward distance used in lane-changing (getFollowersOnConsecutive)
441 18150 : lbcLane = lane->getLength() - distToDivergence;
442 : } else {
443 1228004 : lbcLane = MAX2(0.0, lane->getLength() - lane->interpolateGeometryPosToLanePos(distToDivergence));
444 : }
445 : ConflictInfo ci = ConflictInfo(lbcLane, sibling->getWidth());
446 1246154 : auto it = std::find(myFoeLanes.begin(), myFoeLanes.end(), sibling);
447 1246154 : if (it != myFoeLanes.end()) {
448 : // avoid duplicate foeLane
449 90 : const int replacedIndex = (int)(it - myFoeLanes.begin());
450 90 : myConflicts[replacedIndex] = ci;
451 : } else {
452 1246064 : myConflicts.push_back(ci);
453 1246064 : myFoeLanes.push_back(sibling);
454 : }
455 : #ifdef MSLink_DEBUG_CROSSING_POINTS
456 : std::cout << " adding same-origin foe" << sibling->getID()
457 : << " dist1=" << myConflicts.back().lengthBehindCrossing
458 : << "\n";
459 : #endif
460 1246154 : const MSLane* const siblingCont = sibling->getLinkCont().front()->getViaLaneOrLane();
461 1246154 : if (siblingCont->isInternal() && lane->getShape().distance2D(siblingCont->getShape().front()) < minDist) {
462 : // there may still be overlap with siblingCont (when considering vehicle widths)
463 283041 : const double maxCommonLength = MIN2(lane->getLength(), sibling->getLength() + siblingCont->getLength());
464 283041 : const double lengthBehindDivergence = MAX2(0.0, lane->getLength() - maxCommonLength);
465 : ConflictInfo ci2 = ConflictInfo(lengthBehindDivergence, siblingCont->getWidth(), CONFLICT_SIBLING_CONTINUATION);
466 283041 : myConflicts.push_back(ci2);
467 283041 : myFoeLanes.push_back(siblingCont);
468 283041 : myRecheck.insert({this, siblingCont->getLinkCont().front()});
469 :
470 : #ifdef MSLink_DEBUG_CROSSING_POINTS
471 : std::cout << " adding same-origin foeContinuation" << siblingCont->getID()
472 : << " dist1=" << myConflicts.back().lengthBehindCrossing
473 : << "\n";
474 : #endif
475 : }
476 : }
477 : }
478 : // init points for the symmetrical conflict
479 : // for each pair of conflicting lanes, the link that gets second, sets the pointers
480 6674041 : for (int i = 0; i < (int)myFoeLanes.size(); i++) {
481 5664782 : const MSLane* foeLane = myFoeLanes[i];
482 5664782 : MSLink* foeExitLink = foeLane->getLinkCont()[0];
483 : int foundIndex = -1;
484 21315327 : for (int i2 = 0; i2 < (int)foeExitLink->myFoeLanes.size(); i2++) {
485 18088894 : if (foeExitLink->myFoeLanes[i2] == lane) {
486 2438349 : myConflicts[i].foeConflictIndex = i2;
487 2438349 : foeExitLink->myConflicts[i2].foeConflictIndex = i;
488 2438349 : myRecheck.erase({foeExitLink, this});
489 : foundIndex = i2;
490 2438349 : break;
491 : }
492 : }
493 : #ifdef MSLink_DEBUG_CROSSING_POINTS
494 : std::cout << lane->getID() << " foeLane=" << foeLane->getID() << " index=" << i << " foundIndex=" << foundIndex << "\n";
495 : #endif
496 5664782 : if (foundIndex < 0) {
497 3226433 : if (myConflicts[i].flag != CONFLICT_NO_INTERSECTION) {
498 2342808 : myRecheck.insert({this, foeExitLink});
499 : }
500 : }
501 : }
502 : }
503 2640370 : if (MSGlobals::gLateralResolution > 0) {
504 : // check for links with the same origin lane and the same destination edge
505 296545 : const MSEdge* myTarget = &myLane->getEdge();
506 : // save foes for entry links
507 936988 : for (MSLink* const it : myLaneBefore->getLinkCont()) {
508 : const MSEdge* target = &(it->getLane()->getEdge());
509 640443 : if (it == this) {
510 296545 : continue;
511 : }
512 343898 : if (target == myTarget) {
513 6800 : mySublaneFoeLinks.push_back(it);
514 : #ifdef MSLink_DEBUG_CROSSING_POINTS
515 : std::cout << " sublaneFoeLink (same target): " << it->getViaLaneOrLane()->getID() << "\n";
516 : #endif
517 337098 : } else if (myDirection != LinkDirection::STRAIGHT && it->getDirection() == LinkDirection::STRAIGHT) {
518 : // potential turn conflict
519 82471 : mySublaneFoeLinks2.push_back(it);
520 : #ifdef MSLink_DEBUG_CROSSING_POINTS
521 : std::cout << " sublaneFoeLink2 (other target: " << it->getViaLaneOrLane()->getID() << "\n";
522 : #endif
523 : }
524 : }
525 : // save foes for exit links
526 296545 : if (fromInternalLane()) {
527 : //std::cout << " setRequestInformation link=" << getViaLaneOrLane()->getID() << " before=" << myLaneBefore->getID() << " before2=" << myLaneBefore->getIncomingLanes().front().lane->getID() << "\n";
528 370520 : for (const MSLink* const link : myLaneBefore->getIncomingLanes().front().lane->getLinkCont()) {
529 260431 : if (link->getViaLane() != myInternalLaneBefore && &link->getLane()->getEdge() == myTarget) {
530 : //std::cout << " add sublaneFoe=" << (*it)->getViaLane()->getID() << "\n";
531 4356 : mySublaneFoeLanes.push_back(link->getViaLane());
532 : }
533 : }
534 : }
535 : }
536 2640370 : if (myInternalLaneBefore != nullptr
537 1009259 : && myDirection != LinkDirection::STRAIGHT
538 : // for right turns, the curvature helps rather than restricts the linkLeader check
539 583171 : && (
540 583171 : (!MSGlobals::gLefthand && myDirection != LinkDirection::RIGHT)
541 172383 : || (MSGlobals::gLefthand && myDirection != LinkDirection::LEFT))) {
542 822486 : const double angle = fabs(GeomHelper::angleDiff(
543 411243 : myLaneBefore->getNormalPredecessorLane()->getShape().angleAt2D(-2),
544 411243 : myLane->getShape().angleAt2D(0)));
545 411243 : if (angle > 0) {
546 411243 : double length = myInternalLaneBefore->getShape().length2D();
547 822486 : if (myInternalLaneBefore->getIncomingLanes().size() == 1 &&
548 411243 : myInternalLaneBefore->getIncomingLanes()[0].lane->isInternal()) {
549 116270 : length += myInternalLaneBefore->getIncomingLanes()[0].lane->getShape().length2D();
550 294973 : } else if (myInternalLane != nullptr) {
551 116270 : length += myInternalLane->getShape().length2D();
552 : }
553 411243 : myRadius = length / angle;
554 : //std::cout << getDescription() << " a=" << RAD2DEG(angle) << " l=" << length << " r=" << myRadius << "\n";
555 : }
556 : }
557 2640370 : }
558 :
559 :
560 : void
561 42364 : MSLink::recheckSetRequestInformation() {
562 346290 : for (auto item : myRecheck) {
563 : #ifdef MSLink_DEBUG_CROSSING_POINTS
564 : std::cout << " recheck l1=" << item.first->getDescription() << " l2=" << item.second->getDescription() << "\n";
565 : #endif
566 : MSLink* const link = item.first;
567 : MSLink* const foeExitLink = item.second;
568 : const MSLane* const lane = link->getInternalLaneBefore();
569 : const MSLane* const foeLane = foeExitLink->getInternalLaneBefore();
570 : int conflictIndex = -1;
571 2660301 : for (int i = 0; i < (int)link->myFoeLanes.size(); i++) {
572 2660301 : if (link->myFoeLanes[i] == foeLane) {
573 : conflictIndex = i;
574 : break;
575 : }
576 : }
577 303926 : if (conflictIndex == -1) {
578 0 : WRITE_WARNING("Could not recheck ConflictInfo for " + link->getDescription() + " and " + foeExitLink->getDescription() + "\n");
579 283285 : continue;
580 : }
581 303926 : ConflictInfo& ci = link->myConflicts[conflictIndex];
582 303926 : if (ci.flag & CONFLICT_SIBLING_CONTINUATION) {
583 283035 : const MSLane* const intLane = link->getInternalLaneBefore();
584 : const MSLane* const siblingCont = foeExitLink->getInternalLaneBefore();
585 283035 : const MSLane* const sibling = siblingCont->getLogicalPredecessorLane();
586 : // this is an approximation because intLane and sibling+siblingCont are still close to each other but may have different curvature
587 283035 : const double distToDivergence = intLane->getLength() - ci.lengthBehindCrossing;
588 283035 : double lbcSibCont = MIN2(siblingCont->getLength(), MAX2(0.0, sibling->getLength() + siblingCont->getLength() - distToDivergence));
589 : #ifdef MSLink_DEBUG_CROSSING_POINTS
590 : std::cout << " siblingContinuation: distToDivergence=" << distToDivergence << " lbcSibCont=" << lbcSibCont << "\n";
591 : std::cout << " conflictIndex=" << conflictIndex << " foeLane=" << foeLane->getID() << " foeExitLink=" << foeExitLink->getDescription() << " intLane=" << intLane->getID() << "\n";
592 : #endif
593 : ConflictInfo ci2 = ConflictInfo(lbcSibCont, intLane->getWidth());
594 283035 : ci2.foeConflictIndex = conflictIndex;
595 283035 : ci.foeConflictIndex = (int)foeExitLink->myConflicts.size();
596 283035 : foeExitLink->myFoeLanes.push_back(intLane);
597 283035 : foeExitLink->myConflicts.push_back(ci2);
598 : continue;
599 283035 : }
600 :
601 20891 : std::vector<double> intersections1 = foeLane->getShape().intersectsAtLengths2D(lane->getShape());
602 20891 : if (intersections1.size() == 0) {
603 : #ifdef MSLink_DEBUG_CROSSING_POINTS
604 : std::cout << " no intersection\n";
605 : #endif
606 : continue;
607 : }
608 20641 : const double widthFactor = ci.conflictSize / foeLane->getWidth();
609 20641 : const double conflictSize2 = lane->getWidth() * widthFactor;
610 20641 : std::sort(intersections1.begin(), intersections1.end());
611 20641 : intersections1.back() -= conflictSize2 / 2;
612 20641 : intersections1.back() = MAX2(0.0, intersections1.back());
613 20641 : ci.foeConflictIndex = (int)foeExitLink->myConflicts.size();
614 20641 : foeExitLink->myConflicts.push_back(ConflictInfo(foeLane->getLength() - intersections1.back(), conflictSize2));
615 : #ifdef MSLink_DEBUG_CROSSING_POINTS
616 : std::cout << " ci=" << conflictIndex << " wf=" << widthFactor << " flag=" << ci.flag << " flbc=" << foeExitLink->myConflicts.back().lengthBehindCrossing << "\n";
617 : #endif
618 20891 : }
619 : myRecheck.clear();
620 42364 : }
621 :
622 : double
623 2499328 : MSLink::computeDistToDivergence(const MSLane* lane, const MSLane* sibling, double minDist, bool sameSource, double siblingPredLength) const {
624 : double lbcSibling = 0;
625 : double lbcLane = 0;
626 :
627 : PositionVector l = lane->getShape();
628 : PositionVector s = sibling->getShape();
629 2499328 : double length = l.length2D();
630 2499328 : double sibLength = s.length2D();
631 2499328 : if (!sameSource) {
632 2506348 : l = l.reverse();
633 2506348 : s = s.reverse();
634 1246154 : } else if (sibling->getEntryLink()->myAmIndirect) {
635 : // ignore final waiting position since it may be quite close to the lane
636 : // shape but the waiting position is perpendicular (so the minDist
637 : // requirement is not necessary
638 94 : lbcSibling += s[-1].distanceTo2D(s[-2]);
639 : s.pop_back();
640 1246060 : } else if (lane->getEntryLink()->myAmIndirect) {
641 : // ignore final waiting position since it may be quite close to the lane
642 : // shape but the waiting position is perpendicular (so the minDist
643 : // requirement is not necessary
644 94 : lbcLane += l[-1].distanceTo2D(l[-2]);
645 : l.pop_back();
646 : }
647 :
648 : #ifdef MSLink_DEBUG_CROSSING_POINTS_DETAILS
649 : std::cout << " sameSource=" << sameSource << " lane=" << lane->getID() << " sib=" << sibling->getID() << " minDist=" << minDist << " backDist=" << l.back().distanceTo2D(s.back()) << "\n";
650 : #endif
651 2499328 : if (l.back().distanceTo2D(s.back()) > minDist) {
652 : // compute the final divergence point
653 : // this position serves two purposes:
654 : // 1) once the foe vehicle back (on sibling) has passed this point, we can safely ignore it
655 : // 2) both vehicles are put into a cf-relationship while before the point.
656 : // Since the actual crossing point is at the start of the junction,
657 : // we want to make sure that both vehicles have the same distance to the crossing point and thus follow each other naturally
658 2441470 : std::vector<double> distances = l.distances(s);
659 : #ifdef MSLink_DEBUG_CROSSING_POINTS
660 : std::cout << " distances=" << toString(distances) << "\n";
661 : #endif
662 : assert(distances.size() == l.size() + s.size());
663 2441470 : if (distances.back() > minDist && distances[l.size() - 1] > minDist) {
664 : // do a pairwise check between lane and sibling to make because we do not know which of them bends more
665 2323041 : for (int j = (int)s.size() - 2; j >= 0; j--) {
666 2323041 : const int i = j + (int)l.size();
667 2323041 : const double segLength = s[j].distanceTo2D(s[j + 1]);
668 2323041 : if (distances[i] > minDist) {
669 903171 : lbcSibling += segLength;
670 : } else {
671 : // assume no sharp bends and just interpolate the last segment
672 1419870 : lbcSibling += segLength - (minDist - distances[i]) * segLength / (distances[i + 1] - distances[i]);
673 1419870 : break;
674 : }
675 : }
676 2323537 : for (int i = (int)l.size() - 2; i >= 0; i--) {
677 2323537 : const double segLength = l[i].distanceTo2D(l[i + 1]);
678 2323537 : if (distances[i] > minDist) {
679 903667 : lbcLane += segLength;
680 : } else {
681 : // assume no sharp bends and just interpolate the last segment
682 1419870 : lbcLane += segLength - (minDist - distances[i]) * segLength / (distances[i + 1] - distances[i]);
683 1419870 : break;
684 : }
685 : }
686 : }
687 : assert(lbcSibling >= -NUMERICAL_EPS);
688 : assert(lbcLane >= -NUMERICAL_EPS);
689 2441470 : }
690 2499328 : const double distToDivergence1 = sibling->getLength() + siblingPredLength - lbcSibling;
691 2499328 : const double distToDivergence2 = lane->getLength() - lbcLane;
692 : const double distToDivergence = MIN3(
693 : MAX2(distToDivergence1, distToDivergence2),
694 : sibLength, length);
695 : #ifdef MSLink_DEBUG_CROSSING_POINTS
696 : std::cout << " distToDivergence=" << distToDivergence
697 : << " distTD1=" << distToDivergence1
698 : << " distTD2=" << distToDivergence2
699 : << " length=" << length
700 : << " sibLength=" << sibLength
701 : << "\n";
702 : #endif
703 2499328 : return distToDivergence;
704 2499328 : }
705 :
706 :
707 : bool
708 1379174 : MSLink::contIntersect(const MSLane* lane, const MSLane* foe) {
709 1379174 : if (foe->getLinkCont()[0]->getViaLane() != nullptr) {
710 126278 : std::vector<double> intersections = lane->getShape().intersectsAtLengths2D(foe->getShape());
711 126278 : return intersections.size() > 0;
712 126278 : }
713 : return false;
714 : }
715 :
716 :
717 : void
718 879395042 : MSLink::setApproaching(const SUMOVehicle* approaching, const SUMOTime arrivalTime, const double arrivalSpeed, const double leaveSpeed,
719 : const bool setRequest, const double arrivalSpeedBraking, const SUMOTime waitingTime, double dist, double latOffset) {
720 879395042 : const SUMOTime leaveTime = getLeaveTime(arrivalTime, arrivalSpeed, leaveSpeed, approaching->getVehicleType().getLength());
721 : #ifdef DEBUG_APPROACHING
722 : if (DEBUG_COND2(approaching)) {
723 : std::cout << SIMTIME << " link=" << getDescription() << " setApproaching veh=" << approaching->getID();
724 : if (myApproachingVehicles.size() > 0) {
725 : std::cout << " curApproaching=";
726 : for (auto i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
727 : std::cout << i->first->getID() << " ";
728 : }
729 : }
730 : std::cout << "\n";
731 : }
732 : #endif
733 879395042 : myApproachingVehicles.emplace(approaching,
734 1758790084 : ApproachingVehicleInformation(arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, setRequest,
735 879395042 : arrivalSpeedBraking, waitingTime, dist, approaching->getSpeed(), latOffset));
736 879395042 : }
737 :
738 :
739 : void
740 1127882 : MSLink::setApproaching(const SUMOVehicle* approaching, ApproachingVehicleInformation ai) {
741 : #ifdef DEBUG_APPROACHING
742 : if (DEBUG_COND2(approaching)) {
743 : std::cout << SIMTIME << " link=" << getDescription() << " setApproaching veh=" << approaching->getID();
744 : if (myApproachingVehicles.size() > 0) {
745 : std::cout << " curApproaching=";
746 : for (auto i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
747 : std::cout << i->first->getID() << " ";
748 : }
749 : }
750 : std::cout << "\n";
751 : }
752 : #endif
753 1127882 : myApproachingVehicles.emplace(approaching, ai);
754 1127882 : }
755 :
756 : void
757 487881 : MSLink::setApproachingPerson(const MSPerson* approaching, const SUMOTime arrivalTime, const SUMOTime leaveTime) {
758 487881 : if (myApproachingPersons == nullptr) {
759 1334 : myApproachingPersons = new PersonApproachInfos();
760 : }
761 487881 : myApproachingPersons->emplace(approaching, ApproachingPersonInformation(arrivalTime, leaveTime));
762 487881 : }
763 :
764 : void
765 870076619 : MSLink::removeApproaching(const SUMOVehicle* veh) {
766 : #ifdef DEBUG_APPROACHING
767 : if (DEBUG_COND2(veh)) {
768 : std::cout << SIMTIME << " link=" << getDescription() << " removeApproaching veh=" << veh->getID();
769 : if (myApproachingVehicles.size() > 0) {
770 : std::cout << " curApproaching=";
771 : for (auto i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
772 : std::cout << i->first->getID() << " ";
773 : }
774 : }
775 : std::cout << "\n";
776 : }
777 : #endif
778 : myApproachingVehicles.erase(veh);
779 870076619 : }
780 :
781 :
782 : void
783 31244 : MSLink::removeApproachingPerson(const MSPerson* person) {
784 31244 : if (myApproachingPersons == nullptr) {
785 12 : WRITE_WARNINGF("Person '%' entered crossing lane '%' without registering approach, time=%", person->getID(), myLane->getID(), time2string(SIMSTEP));
786 3 : return;
787 : }
788 : #ifdef DEBUG_APPROACHING
789 : if (DEBUG_COND2(person)) {
790 : std::cout << SIMTIME << " Link '" << (myLaneBefore == 0 ? "NULL" : myLaneBefore->getID()) << "'->'" << (myLane == 0 ? "NULL" : myLane->getID()) << std::endl;
791 : std::cout << "' Removing approaching person '" << person->getID() << "'\nCurrently registered persons:" << std::endl;
792 : for (auto i = myApproachingPersons->begin(); i != myApproachingPersons->end(); ++i) {
793 : std::cout << "'" << i->first->getID() << "'" << std::endl;
794 : }
795 : }
796 : #endif
797 : myApproachingPersons->erase(person);
798 : }
799 :
800 :
801 : MSLink::ApproachingVehicleInformation
802 31752398 : MSLink::getApproaching(const SUMOVehicle* veh) const {
803 : auto i = myApproachingVehicles.find(veh);
804 31752398 : if (i != myApproachingVehicles.end()) {
805 26994610 : return i->second;
806 : } else {
807 : return ApproachingVehicleInformation(INVALID_TIME, INVALID_TIME, 0, 0, false, 0, 0, 0, 0, 0);
808 : }
809 : }
810 :
811 :
812 : const MSLink::ApproachingVehicleInformation*
813 2889587 : MSLink::getApproachingPtr(const SUMOVehicle* veh) const {
814 : auto i = myApproachingVehicles.find(veh);
815 2889587 : if (i != myApproachingVehicles.end()) {
816 2527343 : return &i->second;
817 : } else {
818 : return nullptr;
819 : }
820 : }
821 :
822 :
823 : void
824 11288 : MSLink::clearState() {
825 : myApproachingVehicles.clear();
826 11288 : }
827 :
828 :
829 : SUMOTime
830 1551416049 : MSLink::getLeaveTime(const SUMOTime arrivalTime, const double arrivalSpeed,
831 : const double leaveSpeed, const double vehicleLength) const {
832 1585218282 : return arrivalTime == SUMOTime_MAX ? SUMOTime_MAX : arrivalTime + TIME2STEPS((getLength() + vehicleLength) / MAX2(0.5 * (arrivalSpeed + leaveSpeed), NUMERICAL_EPS));
833 : }
834 :
835 :
836 : bool
837 685228283 : MSLink::opened(SUMOTime arrivalTime, double arrivalSpeed, double leaveSpeed, double vehicleLength,
838 : double impatience, double decel, SUMOTime waitingTime, double posLat,
839 : BlockingFoes* collectFoes, bool ignoreRed, const SUMOTrafficObject* ego, double dist) const {
840 : #ifdef MSLink_DEBUG_OPENED
841 : if (gDebugFlag1) {
842 : std::cout << SIMTIME << " opened? link=" << getDescription() << " red=" << haveRed() << " cont=" << isCont() << " numFoeLinks=" << myFoeLinks.size() << " havePrio=" << havePriority() << " lastWasContMajorGreen=" << lastWasContState(LINKSTATE_TL_GREEN_MAJOR) << "\n";
843 : }
844 : #endif
845 685228283 : if (haveRed() && !ignoreRed) {
846 : return false;
847 : }
848 676049234 : if (isCont() && MSGlobals::gUsingInternalLanes) {
849 : return true;
850 : }
851 671583208 : const SUMOTime leaveTime = getLeaveTime(arrivalTime, arrivalSpeed, leaveSpeed, vehicleLength);
852 671583208 : if (MSGlobals::gLateralResolution > 0) {
853 : // check for foes on the same lane with the same target edge
854 137907051 : for (const MSLink* foeLink : mySublaneFoeLinks) {
855 : assert(myLane != foeLink->getLane());
856 9993865 : for (const auto& it : foeLink->myApproachingVehicles) {
857 6799308 : const SUMOVehicle* foe = it.first;
858 : if (
859 : // there only is a conflict if the paths cross
860 7399754 : ((posLat < foe->getLateralPositionOnLane() + it.second.latOffset && myLane->getIndex() > foeLink->myLane->getIndex())
861 6553388 : || (posLat > foe->getLateralPositionOnLane() + it.second.latOffset && myLane->getIndex() < foeLink->myLane->getIndex()))
862 : // the vehicle that arrives later must yield
863 7397315 : && (arrivalTime > it.second.arrivalTime
864 : // if both vehicles arrive at the same time, the one
865 : // to the left must yield
866 386486 : || (arrivalTime == it.second.arrivalTime && posLat > foe->getLateralPositionOnLane()))) {
867 212166 : if (blockedByFoe(foe, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, false,
868 : impatience, decel, waitingTime, ego)) {
869 : #ifdef MSLink_DEBUG_OPENED
870 : if (gDebugFlag1) {
871 : std::cout << SIMTIME << " blocked by " << foe->getID() << " arrival=" << arrivalTime << " foeArrival=" << it.second.arrivalTime << "\n";
872 : }
873 : #endif
874 42189 : if (collectFoes == nullptr) {
875 : #ifdef MSLink_DEBUG_OPENED
876 : if (gDebugFlag1) {
877 : std::cout << " link=" << getViaLaneOrLane()->getID() << " blocked by sublaneFoe=" << foe->getID() << " foeLink=" << foeLink->getViaLaneOrLane()->getID() << " posLat=" << posLat << "\n";
878 : }
879 : #endif
880 : return false;
881 : } else {
882 0 : collectFoes->push_back(it.first);
883 : }
884 : }
885 : }
886 : }
887 : }
888 : // check for foes on the same lane with a different target edge
889 : // (straight movers take precedence if the paths cross)
890 134670305 : const int lhSign = MSGlobals::gLefthand ? -1 : 1;
891 136015972 : for (const MSLink* foeLink : mySublaneFoeLinks2) {
892 : assert(myDirection != LinkDirection::STRAIGHT);
893 5706618 : for (const auto& it : foeLink->myApproachingVehicles) {
894 4360951 : const SUMOVehicle* foe = it.first;
895 : // there only is a conflict if the paths cross
896 : // and if the vehicles are not currently in a car-following relationship
897 4360951 : const double egoWidth = ego == nullptr ? 1.8 : ego->getVehicleType().getWidth();
898 4360951 : if (!lateralOverlap(posLat, egoWidth, foe->getLateralPositionOnLane() + it.second.latOffset, foe->getVehicleType().getWidth())
899 4360951 : && (((myDirection == LinkDirection::RIGHT || myDirection == LinkDirection::PARTRIGHT)
900 494691 : && (posLat * lhSign > (foe->getLateralPositionOnLane() + it.second.latOffset) * lhSign))
901 447039 : || ((myDirection == LinkDirection::LEFT || myDirection == LinkDirection::PARTLEFT)
902 33197 : && (posLat * lhSign < (foe->getLateralPositionOnLane() + it.second.latOffset) * lhSign)))) {
903 81775 : if (blockedByFoe(foe, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, false,
904 : impatience, decel, waitingTime, ego)) {
905 : #ifdef MSLink_DEBUG_OPENED
906 : if (gDebugFlag1) {
907 : std::cout << SIMTIME << " blocked by sublane foe " << foe->getID() << " arrival=" << arrivalTime << " foeArrival=" << it.second.arrivalTime << "\n";
908 : }
909 : #endif
910 8944 : if (collectFoes == nullptr) {
911 : #ifdef MSLink_DEBUG_OPENED
912 : if (gDebugFlag1) {
913 : std::cout << " link=" << getViaLaneOrLane()->getID() << " blocked by sublaneFoe2=" << foe->getID() << " foeLink=" << foeLink->getViaLaneOrLane()->getID() << " posLat=" << posLat << "\n";
914 : }
915 : #endif
916 : return false;
917 : } else {
918 0 : collectFoes->push_back(it.first);
919 : }
920 : }
921 : }
922 : }
923 : }
924 : }
925 : #ifdef MSLink_DEBUG_OPENED
926 : /*
927 : if (gDebugFlag1) {
928 : std::cout << SIMTIME << " isExitLinkAfterInternalJunction=" << isExitLinkAfterInternalJunction()
929 : << " entryLink=" << getCorrespondingEntryLink()->getDescription()
930 : << " entryState=" << getCorrespondingEntryLink()->getState()
931 : << "\n";
932 : }
933 : */
934 : #endif
935 : if ((havePriority()
936 29903813 : || lastWasContState(LINKSTATE_TL_GREEN_MAJOR)
937 29461435 : || (isExitLinkAfterInternalJunction() && getCorrespondingEntryLink()->getState() == LINKSTATE_TL_GREEN_MAJOR))
938 672045641 : && myState != LINKSTATE_ZIPPER) {
939 : // priority usually means the link is open but there are exceptions:
940 : // zipper still needs to collect foes
941 : // sublane model could have detected a conflict
942 640426185 : return collectFoes == nullptr || collectFoes->size() == 0;
943 : }
944 31105890 : if (myState == LINKSTATE_ALLWAY_STOP && waitingTime < TIME2STEPS(ego == nullptr ? TS : ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_ALLWAYSTOP_WAIT, TS))) {
945 : return false;
946 30129728 : } else if (myState == LINKSTATE_STOP && waitingTime < TIME2STEPS(ego == nullptr ? TS : ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPSIGN_WAIT, TS))) {
947 : return false;
948 : }
949 :
950 30065694 : const std::vector<MSLink*>& foeLinks = (myOffFoeLinks == nullptr || getCorrespondingEntryLink()->getState() != LINKSTATE_ALLWAY_STOP) ? myFoeLinks : *myOffFoeLinks;
951 :
952 30065694 : if (MSGlobals::gUseMesoSim && impatience == 1 && !myLane->getEdge().isRoundabout()) {
953 : return true;
954 : }
955 30065560 : if (myLane->getBidiLane() != nullptr) {
956 131896 : MSLane* bidi = myLane->getBidiLane();
957 131896 : if (bidi->getVehicleNumber() > 0) {
958 19955 : if (ego == nullptr) {
959 : return false;
960 : }
961 : double maxOncomingWidth = 0;
962 19955 : const MSLane::VehCont& vehs = bidi->getVehiclesSecure();
963 127733 : for (MSVehicle* foe : vehs) {
964 107778 : maxOncomingWidth = MAX2(maxOncomingWidth, foe->getVehicleType().getWidth());
965 : }
966 19955 : bidi->releaseVehicles();
967 19955 : if (MSGlobals::gLateralResolution > 0) {
968 16779 : if (ego->getVehicleType().getWidth() + maxOncomingWidth + MSGlobals::gLateralResolution < myLane->getWidth()) {
969 : return false;
970 : }
971 3176 : } else if (maxOncomingWidth > 0) {
972 : // do not enter a lane that has any oncoming vehicles
973 : return false;
974 : }
975 : }
976 266583 : for (auto ili : bidi->getIncomingLanes()) {
977 138445 : if (ili.lane->getEdge().getPriority() > myLaneBefore->getEdge().getPriority()
978 138445 : || (ili.lane->getEdge().getPriority() == myLaneBefore->getEdge().getPriority()
979 107448 : && ili.lane->getID() > myLaneBefore->getID())) {
980 : BlockingFoes bidiApproachFoes;
981 : double maxOncomingWidth = 0;
982 21199 : if (ili.viaLink->blockedAtTime(arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, false, 0, decel, 0,
983 42398 : MSGlobals::gLateralResolution ? &bidiApproachFoes : nullptr, ego) || bidiApproachFoes.size() > 0) {
984 506 : if (MSGlobals::gLateralResolution > 0) {
985 437 : for (const SUMOTrafficObject* foe : bidiApproachFoes) {
986 226 : maxOncomingWidth = MAX2(maxOncomingWidth, foe->getVehicleType().getWidth());
987 : }
988 211 : if (ego->getVehicleType().getWidth() + maxOncomingWidth + MSGlobals::gLateralResolution < myLane->getWidth()) {
989 : return false;
990 : }
991 : } else {
992 : return false;
993 : }
994 : }
995 21199 : }
996 : }
997 : }
998 30061802 : const bool lastWasContRed = lastWasContState(LINKSTATE_TL_RED);
999 83564730 : for (const MSLink* const link : foeLinks) {
1000 58437899 : if (MSGlobals::gUseMesoSim) {
1001 1829960 : if (link->haveRed()) {
1002 70073 : continue;
1003 : }
1004 : }
1005 : #ifdef MSLink_DEBUG_OPENED
1006 : if (gDebugFlag1) {
1007 : std::cout << SIMTIME << " foeLink=" << link->getViaLaneOrLane()->getID() << " numApproaching=" << link->getApproaching().size() << "\n";
1008 : if (link->getLane()->isCrossing()) {
1009 : std::cout << SIMTIME << " approachingPersons=" << (link->myApproachingPersons == nullptr ? "NULL" : toString(link->myApproachingPersons->size())) << "\n";
1010 : }
1011 : }
1012 : #endif
1013 58367826 : if (link->blockedAtTime(arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, myLane == link->getLane(),
1014 : impatience, decel, waitingTime, collectFoes, ego, lastWasContRed, dist)) {
1015 : return false;
1016 : }
1017 : }
1018 25126831 : if (collectFoes != nullptr && collectFoes->size() > 0) {
1019 : return false;
1020 : }
1021 : return true;
1022 : }
1023 :
1024 :
1025 : bool
1026 58440326 : MSLink::blockedAtTime(SUMOTime arrivalTime, SUMOTime leaveTime, double arrivalSpeed, double leaveSpeed,
1027 : bool sameTargetLane, double impatience, double decel, SUMOTime waitingTime,
1028 : BlockingFoes* collectFoes, const SUMOTrafficObject* ego, bool lastWasContRed, double dist) const {
1029 219422093 : for (const auto& it : myApproachingVehicles) {
1030 : #ifdef MSLink_DEBUG_OPENED
1031 : if (gDebugFlag1) {
1032 : if (ego != nullptr
1033 : && ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_SPEED, 0) >= it.second.speed
1034 : && ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) > 0) {
1035 : std::stringstream stream; // to reduce output interleaving from different threads
1036 : stream << SIMTIME << " " << myApproachingVehicles.size() << " foe link=" << getViaLaneOrLane()->getID()
1037 : << " foeVeh=" << it.first->getID() << " (below ignore speed)"
1038 : << " ignoreFoeProb=" << ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0)
1039 : << "\n";
1040 : std::cout << stream.str();
1041 : }
1042 : }
1043 : #endif
1044 165814090 : if (it.first != ego
1045 165804817 : && (ego == nullptr
1046 165756610 : || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) == 0
1047 23251 : || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_SPEED, 0) < it.second.speed
1048 1746 : || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) < RandHelper::rand(ego->getRNG()))
1049 165803227 : && !ignoreFoe(ego, it.first)
1050 165802985 : && (!lastWasContRed || it.first->getSpeed() > SUMO_const_haltingSpeed)
1051 331600960 : && blockedByFoe(it.first, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, sameTargetLane,
1052 : impatience, decel, waitingTime, ego)) {
1053 11317848 : if (collectFoes == nullptr) {
1054 : return true;
1055 : } else {
1056 6485525 : collectFoes->push_back(it.first);
1057 : }
1058 : }
1059 : }
1060 53608003 : if (myApproachingPersons != nullptr && !haveRed()) {
1061 : const SUMOTime lookAhead = (ego == nullptr
1062 847530 : ? myLookaheadTime
1063 847358 : : TIME2STEPS(ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_TIMEGAP_MINOR, STEPS2TIME(myLookaheadTime))));
1064 922244 : for (const auto& it : *myApproachingPersons) {
1065 : #ifdef MSLink_DEBUG_OPENED
1066 : if (gDebugFlag1) {
1067 : std::cout << SIMTIME << ": " << ego->getID() << " check person " << it.first->getID() << " aTime=" << arrivalTime << " foeATime=" << it.second.arrivalTime
1068 : << " lTime=" << leaveTime << " foeLTime=" << it.second.leavingTime
1069 : << " dist=" << dist << "\n";
1070 : }
1071 : #endif
1072 : if ((ego == nullptr
1073 204291 : || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) == 0
1074 600 : || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_SPEED, 0) < it.first->getSpeed()
1075 600 : || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) < RandHelper::rand(ego->getRNG()))
1076 203912 : && !ignoreFoe(ego, it.first)
1077 408344 : && !((arrivalTime > it.second.leavingTime + lookAhead) || (leaveTime + lookAhead < it.second.arrivalTime))) {
1078 143187 : if (ego == nullptr) {
1079 : // during insertion
1080 165 : if (myJunction->getType() == SumoXMLNodeType::RAIL_CROSSING) {
1081 165 : continue;
1082 : } else {
1083 : return true;
1084 : }
1085 : }
1086 : // check whether braking is feasible (ego might have started to accelerate already)
1087 143022 : const auto& cfm = ego->getVehicleType().getCarFollowModel();
1088 : #ifdef MSLink_DEBUG_OPENED
1089 : if (gDebugFlag1) {
1090 : 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";
1091 : }
1092 : #endif
1093 143022 : if (dist > cfm.brakeGap(ego->getSpeed(), cfm.getMaxDecel(), 0)) {
1094 : #ifdef MSLink_DEBUG_OPENED
1095 : if (gDebugFlag1) {
1096 : std::cout << SIMTIME << ": " << ego->getID() << " blocked by person " << it.first->getID() << "\n";
1097 : }
1098 : #endif
1099 129798 : if (collectFoes == nullptr) {
1100 : return true;
1101 : } else {
1102 0 : collectFoes->push_back(it.first);
1103 : }
1104 : }
1105 : }
1106 : }
1107 : }
1108 : return false;
1109 : }
1110 :
1111 :
1112 : bool
1113 166080811 : MSLink::blockedByFoe(const SUMOVehicle* veh, const ApproachingVehicleInformation& avi,
1114 : SUMOTime arrivalTime, SUMOTime leaveTime, double arrivalSpeed, double leaveSpeed,
1115 : bool sameTargetLane, double impatience, double decel, SUMOTime waitingTime,
1116 : const SUMOTrafficObject* ego) const {
1117 : #ifdef MSLink_DEBUG_OPENED
1118 : if (gDebugFlag1) {
1119 : std::stringstream stream; // to reduce output interleaving from different threads
1120 : stream << " link=" << getDescription()
1121 : << " foeVeh=" << veh->getID()
1122 : << " req=" << avi.willPass
1123 : << " aT=" << avi.arrivalTime
1124 : << " lT=" << avi.leavingTime
1125 : << "\n";
1126 : std::cout << stream.str();
1127 : }
1128 : #endif
1129 166080811 : if (!avi.willPass) {
1130 : return false;
1131 : }
1132 60564704 : if (myState == LINKSTATE_ALLWAY_STOP) {
1133 : assert(waitingTime > 0);
1134 : #ifdef MSLink_DEBUG_OPENED
1135 : if (gDebugFlag1) {
1136 : std::stringstream stream; // to reduce output interleaving from different threads
1137 : stream << " foeDist=" << avi.dist
1138 : << " foeBGap=" << veh->getBrakeGap(false)
1139 : << " foeWait=" << avi.waitingTime
1140 : << " wait=" << waitingTime
1141 : << "\n";
1142 : std::cout << stream.str();
1143 : }
1144 : #endif
1145 : // when using actionSteps, the foe waiting time may be outdated
1146 1566781 : const SUMOTime actionDelta = SIMSTEP - veh->getLastActionTime();
1147 1566781 : if (waitingTime > avi.waitingTime + actionDelta) {
1148 : return false;
1149 : }
1150 232841 : if (waitingTime == (avi.waitingTime + actionDelta) && arrivalTime < avi.arrivalTime + actionDelta) {
1151 : return false;
1152 : }
1153 : }
1154 59201190 : SUMOTime foeArrivalTime = avi.arrivalTime;
1155 59201190 : double foeArrivalSpeedBraking = avi.arrivalSpeedBraking;
1156 59201190 : if (impatience > 0 && arrivalTime < avi.arrivalTime) {
1157 : #ifdef MSLink_DEBUG_OPENED
1158 : gDebugFlag6 = ((ego == nullptr || ego->isSelected()) && (veh == nullptr || veh->isSelected()));
1159 : #endif
1160 2771089 : const SUMOTime fatb = computeFoeArrivalTimeBraking(arrivalTime, veh, avi.arrivalTime, impatience, avi.dist, foeArrivalSpeedBraking);
1161 2771089 : foeArrivalTime = (SUMOTime)((1. - impatience) * (double)avi.arrivalTime + impatience * (double)fatb);
1162 : #ifdef MSLink_DEBUG_OPENED
1163 : if (gDebugFlag6) {
1164 : std::cout << SIMTIME << " link=" << getDescription() << " ego=" << ego->getID() << " foe=" << veh->getID()
1165 : << " at=" << STEPS2TIME(arrivalTime)
1166 : << " fat=" << STEPS2TIME(avi.arrivalTime)
1167 : << " fatb=" << STEPS2TIME(fatb)
1168 : << " fat2=" << STEPS2TIME(foeArrivalTime)
1169 : << "\n";
1170 : }
1171 : #endif
1172 : }
1173 :
1174 :
1175 59201190 : const SUMOTime lookAhead = (myState == LINKSTATE_ZIPPER
1176 59201190 : ? myLookaheadTimeZipper
1177 : : (ego == nullptr
1178 47479586 : ? myLookaheadTime
1179 47474198 : : TIME2STEPS(ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_TIMEGAP_MINOR, STEPS2TIME(myLookaheadTime)))));
1180 : //if (ego != 0) std::cout << SIMTIME << " ego=" << ego->getID() << " jmTimegapMinor=" << ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_TIMEGAP_MINOR, -1) << " lookAhead=" << lookAhead << "\n";
1181 : #ifdef MSLink_DEBUG_OPENED
1182 : if (gDebugFlag1 || gDebugFlag6) {
1183 : std::stringstream stream; // to reduce output interleaving from different threads
1184 : stream << " imp=" << impatience << " fAT2=" << foeArrivalTime << " fASb=" << foeArrivalSpeedBraking << " lA=" << lookAhead << " egoAT=" << arrivalTime << " egoLT=" << leaveTime << " egoLS=" << leaveSpeed << "\n";
1185 : std::cout << stream.str();
1186 : }
1187 : #endif
1188 59201190 : if (avi.leavingTime < arrivalTime) {
1189 : // ego wants to be follower
1190 43351277 : if (sameTargetLane && (arrivalTime - avi.leavingTime < lookAhead
1191 18982644 : || unsafeMergeSpeeds(avi.leaveSpeed, arrivalSpeed,
1192 18982644 : veh->getVehicleType().getCarFollowModel().getMaxDecel(), decel))) {
1193 : #ifdef MSLink_DEBUG_OPENED
1194 : if (gDebugFlag1 || gDebugFlag6) {
1195 : std::cout << " blocked (cannot follow)\n";
1196 : }
1197 : #endif
1198 4175524 : return true;
1199 : }
1200 15849913 : } else if (foeArrivalTime > leaveTime + lookAhead) {
1201 : // ego wants to be leader.
1202 14617938 : if (sameTargetLane && unsafeMergeSpeeds(leaveSpeed, foeArrivalSpeedBraking,
1203 5804240 : decel, veh->getVehicleType().getCarFollowModel().getMaxDecel())) {
1204 : #ifdef MSLink_DEBUG_OPENED
1205 : if (gDebugFlag1 || gDebugFlag6) {
1206 : std::cout << " blocked (cannot lead)\n";
1207 : }
1208 : #endif
1209 : return true;
1210 : }
1211 : } else {
1212 : // even without considering safeHeadwayTime there is already a conflict
1213 : #ifdef MSLink_DEBUG_OPENED
1214 : if (gDebugFlag1 || gDebugFlag6) {
1215 : std::cout << " blocked (hard conflict)\n";
1216 : }
1217 : #endif
1218 : return true;
1219 : }
1220 : return false;
1221 : }
1222 :
1223 :
1224 : SUMOTime
1225 2771089 : MSLink::computeFoeArrivalTimeBraking(SUMOTime arrivalTime, const SUMOVehicle* foe, SUMOTime foeArrivalTime, double impatience, double dist, double& fasb) {
1226 : // a: distance saved when foe brakes from arrivalTime to foeArrivalTime
1227 : // b: distance driven past foeArrivalTime
1228 : // m: permitted decceleration
1229 : // d: total deceleration until foeArrivalTime
1230 : // dist2: distance of foe at arrivalTime
1231 : // actual arrivalTime must fall on a simulation step
1232 2771089 : if (arrivalTime - arrivalTime % DELTA_T == foeArrivalTime - foeArrivalTime % DELTA_T) {
1233 : // foe enters the junction in the same step
1234 : #ifdef MSLink_DEBUG_OPENED
1235 : if (gDebugFlag6) {
1236 : std::cout << " foeAT before egoAT\n";
1237 : }
1238 : #endif
1239 : return foeArrivalTime;
1240 : }
1241 2416054 : if (arrivalTime % DELTA_T > 0) {
1242 2386304 : arrivalTime = arrivalTime - (arrivalTime % DELTA_T) + DELTA_T;
1243 : }
1244 : //arrivalTime += DELTA_T - arrivalTime % DELTA_T;
1245 2416054 : const double m = foe->getVehicleType().getCarFollowModel().getMaxDecel() * impatience;
1246 2416054 : const double dt = STEPS2TIME(foeArrivalTime - arrivalTime);
1247 2416054 : const double d = dt * m;
1248 2416054 : const double a = dt * d / 2;
1249 2416054 : const double v = dist / STEPS2TIME(foeArrivalTime - SIMSTEP + DELTA_T);
1250 2416054 : const double dist2 = dist - v * STEPS2TIME(arrivalTime - SIMSTEP);
1251 : #ifdef MSLink_DEBUG_OPENED
1252 : if (gDebugFlag6) {
1253 : std::cout << " dist=" << dist << " dist2=" << dist2
1254 : << " at=" << STEPS2TIME(arrivalTime)
1255 : << " fat=" << STEPS2TIME(foeArrivalTime)
1256 : << " dt=" << dt << " v=" << v << " m=" << m << " d=" << d << " a=" << a << "\n";
1257 : }
1258 : #endif
1259 2416054 : if (0.5 * v * v / m <= dist2) {
1260 : #ifdef MSLink_DEBUG_OPENED
1261 : if (gDebugFlag6) {
1262 : std::cout << " canBrakeToStop\n";
1263 : }
1264 : #endif
1265 1216959 : fasb = 0;
1266 1216959 : return foeArrivalTime + TIME2STEPS(30);
1267 : }
1268 : // a = b (foe reaches the original distance to the stop line)
1269 : // x: time driven past foeArrivalTime
1270 : // v: foe speed without braking
1271 : // v2: average foe speed after foeArrivalTime (braking continues for time x)
1272 : // v2 = (v - d - x * m / 2)
1273 : // b = v2 * x
1274 : // solving for x gives:
1275 1199095 : const double x = (sqrt(4 * (v - d) * (v - d) - 8 * m * a) * -0.5 - d + v) / m;
1276 :
1277 : #ifdef MSLink_DEBUG_OPENED
1278 : const double x2 = (sqrt(4 * (v - d) * (v - d) - 8 * m * a) * 0.5 - d + v) / m;
1279 : if (gDebugFlag6 || std::isnan(x)) {
1280 : std::cout << SIMTIME << " dist=" << dist << " dist2=" << dist2 << " at=" << STEPS2TIME(arrivalTime) << " m=" << m << " d=" << d << " v=" << v << " a=" << a << " x=" << x << " x2=" << x2 << "\n";
1281 : }
1282 : #endif
1283 1199095 : fasb = v - (dt + x) * m;
1284 1199095 : return foeArrivalTime + TIME2STEPS(x);
1285 : }
1286 :
1287 :
1288 : bool
1289 437945 : MSLink::hasApproachingFoe(SUMOTime arrivalTime, SUMOTime leaveTime, double speed, double decel) const {
1290 462096 : for (const MSLink* const link : myFoeLinks) {
1291 51301 : if (link->blockedAtTime(arrivalTime, leaveTime, speed, speed, myLane == link->getLane(), 0, decel, 0)) {
1292 : return true;
1293 : }
1294 : }
1295 508294 : for (const MSLane* const lane : myFoeLanes) {
1296 104355 : if (lane->getVehicleNumberWithPartials() > 0) {
1297 : return true;
1298 : }
1299 : }
1300 : return false;
1301 : }
1302 :
1303 :
1304 : std::pair<const SUMOVehicle*, const MSLink*>
1305 5622 : MSLink::getFirstApproachingFoe(const MSLink* wrapAround) const {
1306 : double closetDist = std::numeric_limits<double>::max();
1307 : const SUMOVehicle* closest = nullptr;
1308 : const MSLink* foeLink = nullptr;
1309 18308 : for (MSLink* link : myFoeLinks) {
1310 19750 : for (const auto& it : link->myApproachingVehicles) {
1311 : //std::cout << " link=" << getDescription() << " foeLink_in=" << link->getLaneBefore()->getID() << " wrapAround=" << wrapAround->getDescription() << "\n";
1312 7064 : if (link->getLaneBefore() == wrapAround->getLaneBefore()) {
1313 489 : return std::make_pair(nullptr, wrapAround);
1314 6575 : } else if (it.second.dist < closetDist) {
1315 : closetDist = it.second.dist;
1316 3423 : if (it.second.willPass) {
1317 3162 : closest = it.first;
1318 : foeLink = link;
1319 : }
1320 : }
1321 : }
1322 : }
1323 : return std::make_pair(closest, foeLink);
1324 : }
1325 :
1326 :
1327 : void
1328 65816548 : MSLink::setTLState(LinkState state, SUMOTime t) {
1329 65816548 : if (myState != state) {
1330 15560606 : myLastStateChange = t;
1331 : }
1332 65816548 : myState = state;
1333 65816548 : if (haveGreen()) {
1334 11106416 : myLastGreenState = myState;
1335 : }
1336 65816548 : }
1337 :
1338 :
1339 : void
1340 179943 : MSLink::setTLLogic(const MSTrafficLightLogic* logic) {
1341 179943 : myLogic = logic;
1342 179943 : }
1343 :
1344 :
1345 : bool
1346 1282938078 : MSLink::isCont() const {
1347 : // when a traffic light is switched off minor roads have their cont status revoked
1348 1282938078 : return (myState == LINKSTATE_TL_OFF_BLINKING || myState == LINKSTATE_ALLWAY_STOP || myState == LINKSTATE_STOP) ? myAmContOff : myAmCont;
1349 : }
1350 :
1351 :
1352 : bool
1353 66704402 : MSLink::lastWasContMajor() const {
1354 67381527 : if (isExitLinkAfterInternalJunction()) {
1355 677125 : return myInternalLaneBefore->getIncomingLanes()[0].viaLink->lastWasContMajor();
1356 : }
1357 66704402 : if (myInternalLane == nullptr || myAmCont) {
1358 : return false;
1359 : } else {
1360 48711184 : MSLane* pred = myInternalLane->getLogicalPredecessorLane();
1361 48711184 : if (!pred->getEdge().isInternal()) {
1362 : return false;
1363 : } else {
1364 11735728 : const MSLane* const pred2 = pred->getLogicalPredecessorLane();
1365 : assert(pred2 != nullptr);
1366 11735728 : const MSLink* const predLink = pred2->getLinkTo(pred);
1367 : assert(predLink != nullptr);
1368 11735728 : if (predLink->havePriority()) {
1369 : return true;
1370 : }
1371 11063796 : if (myHavePedestrianCrossingFoe) {
1372 1433421 : return predLink->getLastGreenState() == LINKSTATE_TL_GREEN_MAJOR;
1373 : } else {
1374 9630375 : return predLink->haveYellow();
1375 : }
1376 : }
1377 : }
1378 : }
1379 :
1380 :
1381 : bool
1382 59965615 : MSLink::lastWasContState(LinkState linkState) const {
1383 59965615 : if (myInternalLane == nullptr || myAmCont || myHavePedestrianCrossingFoe) {
1384 : return false;
1385 : } else {
1386 52339572 : MSLane* pred = myInternalLane->getLogicalPredecessorLane();
1387 52339572 : if (!pred->getEdge().isInternal()) {
1388 : return false;
1389 : } else {
1390 15391642 : const MSLane* const pred2 = pred->getLogicalPredecessorLane();
1391 : assert(pred2 != nullptr);
1392 15391642 : const MSLink* const predLink = pred2->getLinkTo(pred);
1393 : assert(predLink != nullptr);
1394 15391642 : return predLink->getState() == linkState;
1395 : }
1396 : }
1397 : }
1398 :
1399 :
1400 : void
1401 113904 : MSLink::writeApproaching(OutputDevice& od, const std::string fromLaneID) const {
1402 113904 : if (myApproachingVehicles.size() > 0) {
1403 9765 : od.openTag("link");
1404 9765 : od.writeAttr(SUMO_ATTR_FROM, fromLaneID);
1405 9765 : const std::string via = getViaLane() == nullptr ? "" : getViaLane()->getID();
1406 9765 : od.writeAttr(SUMO_ATTR_VIA, via);
1407 19530 : od.writeAttr(SUMO_ATTR_TO, getLane() == nullptr ? "" : getLane()->getID());
1408 : std::vector<std::pair<SUMOTime, const SUMOVehicle*> > toSort; // stabilize output
1409 21325 : for (auto it : myApproachingVehicles) {
1410 11560 : toSort.push_back(std::make_pair(it.second.arrivalTime, it.first));
1411 : }
1412 9765 : std::sort(toSort.begin(), toSort.end());
1413 21325 : for (std::vector<std::pair<SUMOTime, const SUMOVehicle*> >::const_iterator it = toSort.begin(); it != toSort.end(); ++it) {
1414 11560 : od.openTag("approaching");
1415 11560 : const ApproachingVehicleInformation& avi = myApproachingVehicles.find(it->second)->second;
1416 11560 : od.writeAttr(SUMO_ATTR_ID, it->second->getID());
1417 11560 : od.writeAttr(SUMO_ATTR_IMPATIENCE, it->second->getImpatience());
1418 11560 : od.writeAttr("arrivalTime", time2string(avi.arrivalTime));
1419 11560 : od.writeAttr("leaveTime", time2string(avi.leavingTime));
1420 11560 : od.writeAttr("arrivalSpeed", toString(avi.arrivalSpeed));
1421 11560 : od.writeAttr("arrivalSpeedBraking", toString(avi.arrivalSpeedBraking));
1422 11560 : od.writeAttr("leaveSpeed", toString(avi.leaveSpeed));
1423 11560 : od.writeAttr("willPass", toString(avi.willPass));
1424 23120 : od.closeTag();
1425 : }
1426 9765 : od.closeTag();
1427 9765 : }
1428 113904 : }
1429 :
1430 :
1431 : double
1432 922014 : MSLink::getInternalLengthsAfter() const {
1433 : double len = 0.;
1434 922014 : MSLane* lane = myInternalLane;
1435 :
1436 1760241 : while (lane != nullptr && lane->isInternal()) {
1437 838227 : len += lane->getLength();
1438 838227 : lane = lane->getLinkCont()[0]->getViaLane();
1439 : }
1440 922014 : return len;
1441 : }
1442 :
1443 : double
1444 0 : MSLink::getInternalLengthsBefore() const {
1445 : double len = 0.;
1446 0 : const MSLane* lane = myInternalLane;
1447 :
1448 0 : while (lane != nullptr && lane->isInternal()) {
1449 0 : len += lane->getLength();
1450 0 : if (lane->getIncomingLanes().size() == 1) {
1451 0 : lane = lane->getIncomingLanes()[0].lane;
1452 : } else {
1453 : break;
1454 : }
1455 : }
1456 0 : return len;
1457 : }
1458 :
1459 :
1460 : double
1461 131134 : MSLink::getLengthsBeforeCrossing(const MSLane* foeLane) const {
1462 131134 : MSLane* via = myInternalLane;
1463 : double totalDist = 0.;
1464 : bool foundCrossing = false;
1465 133990 : while (via != nullptr) {
1466 132630 : MSLink* link = via->getLinkCont()[0];
1467 132630 : double dist = link->getLengthBeforeCrossing(foeLane);
1468 132630 : if (dist != INVALID_DOUBLE) {
1469 : // found conflicting lane
1470 129774 : totalDist += dist;
1471 : foundCrossing = true;
1472 : break;
1473 : } else {
1474 2856 : totalDist += via->getLength();
1475 : via = link->getViaLane();
1476 : }
1477 : }
1478 : if (foundCrossing) {
1479 129774 : return totalDist;
1480 : } else {
1481 : return INVALID_DOUBLE;
1482 : }
1483 : }
1484 :
1485 :
1486 : double
1487 132630 : MSLink::getLengthBeforeCrossing(const MSLane* foeLane) const {
1488 : int foe_ix;
1489 795143 : for (foe_ix = 0; foe_ix != (int)myFoeLanes.size(); ++foe_ix) {
1490 793647 : if (myFoeLanes[foe_ix] == foeLane) {
1491 : break;
1492 : }
1493 : }
1494 132630 : if (foe_ix == (int)myFoeLanes.size()) {
1495 : // no conflict with the given lane, indicate by returning -1
1496 : #ifdef MSLink_DEBUG_CROSSING_POINTS
1497 : std::cout << "No crossing of lanes '" << foeLane->getID() << "' and '" << myInternalLaneBefore->getID() << "'" << std::endl;
1498 : #endif
1499 : return INVALID_DOUBLE;
1500 : } else {
1501 : // found conflicting lane index
1502 131134 : double dist = myInternalLaneBefore->getLength() - myConflicts[foe_ix].getLengthBehindCrossing(this);
1503 131134 : if (dist == -10000.) {
1504 : // this is the value in myConflicts, if the relation allows intersection but none is present for the actual geometry.
1505 : return INVALID_DOUBLE;
1506 : }
1507 : #ifdef MSLink_DEBUG_CROSSING_POINTS
1508 : std::cout << "Crossing of lanes '" << myInternalLaneBefore->getID() << "' and '" << foeLane->getID()
1509 : << "' at distance " << dist << " (approach along '"
1510 : << myInternalLaneBefore->getEntryLink()->getLaneBefore()->getID() << "')" << std::endl;
1511 : #endif
1512 : return dist;
1513 : }
1514 : }
1515 :
1516 :
1517 : bool
1518 40189439 : MSLink::isEntryLink() const {
1519 40189439 : if (MSGlobals::gUsingInternalLanes) {
1520 63900129 : return myInternalLane != nullptr && myInternalLaneBefore == nullptr;
1521 : } else {
1522 : return false;
1523 : }
1524 : }
1525 :
1526 : bool
1527 19516409 : MSLink::isConflictEntryLink() const {
1528 : // either a non-cont entry link or the link after a cont-link
1529 19516409 : return !myAmCont && (isEntryLink() || (myInternalLaneBefore != nullptr && myInternalLane != nullptr));
1530 : }
1531 :
1532 : bool
1533 94889297 : MSLink::isExitLink() const {
1534 94889297 : if (MSGlobals::gUsingInternalLanes) {
1535 163055798 : return myInternalLaneBefore != nullptr && myInternalLane == nullptr;
1536 : } else {
1537 : return false;
1538 : }
1539 : }
1540 :
1541 : bool
1542 156855446 : MSLink::isExitLinkAfterInternalJunction() const {
1543 156855446 : if (MSGlobals::gUsingInternalLanes) {
1544 : return (getInternalLaneBefore() != nullptr
1545 81164503 : && myInternalLaneBefore->getIncomingLanes().size() == 1
1546 237507040 : && myInternalLaneBefore->getIncomingLanes().front().viaLink->isInternalJunctionLink());
1547 : } else {
1548 : return false;
1549 : }
1550 : }
1551 :
1552 :
1553 : const MSLink*
1554 141642 : MSLink::getCorrespondingExitLink() const {
1555 141642 : MSLane* lane = myInternalLane;
1556 : const MSLink* link = this;
1557 287732 : while (lane != nullptr) {
1558 146090 : link = lane->getLinkCont()[0];
1559 : lane = link->getViaLane();
1560 : }
1561 141642 : return link;
1562 : }
1563 :
1564 :
1565 : const MSLink*
1566 809500741 : MSLink::getCorrespondingEntryLink() const {
1567 : const MSLink* link = this;
1568 1088467719 : while (link->myLaneBefore->isInternal()) {
1569 : assert(myLaneBefore->getIncomingLanes().size() == 1);
1570 278966978 : link = link->myLaneBefore->getIncomingLanes().front().viaLink;
1571 : }
1572 809500741 : return link;
1573 : }
1574 :
1575 :
1576 : bool
1577 424452773 : MSLink::isInternalJunctionLink() const {
1578 424452773 : return getInternalLaneBefore() != nullptr && myInternalLane != nullptr;
1579 : }
1580 :
1581 :
1582 : const MSLink::LinkLeaders
1583 879401438 : MSLink::getLeaderInfo(const MSVehicle* ego, double dist, std::vector<const MSPerson*>* collectBlockers, bool isShadowLink) const {
1584 : LinkLeaders result;
1585 : // this link needs to start at an internal lane (either an exit link or between two internal lanes)
1586 : // or it must be queried by the pedestrian model (ego == 0)
1587 879401438 : if (ego != nullptr && (!fromInternalLane() || ego->getLaneChangeModel().isOpposite())) {
1588 : // ignore link leaders
1589 : return result;
1590 : }
1591 : //gDebugFlag1 = true;
1592 297028948 : if (gDebugFlag1) {
1593 0 : std::cout << SIMTIME << " getLeaderInfo link=" << getDescription() << " dist=" << dist << " isShadowLink=" << isShadowLink << "\n";
1594 : }
1595 297028948 : if (MSGlobals::gComputeLC && ego != nullptr && ego->getLane()->isNormal()) {
1596 25814476 : const MSLink* junctionEntry = getLaneBefore()->getEntryLink();
1597 6475182 : if (junctionEntry->haveRed() && !ego->ignoreRed(junctionEntry, true)
1598 : // check oncoming on bidiLane during laneChanging
1599 32284972 : && (!MSGlobals::gComputeLC || junctionEntry->getLaneBefore()->getBidiLane() == nullptr)) {
1600 6421179 : if (gDebugFlag1) {
1601 0 : std::cout << " ignore linkLeaders beyond red light\n";
1602 : }
1603 : return result;
1604 : }
1605 : }
1606 : // this is an exit link
1607 290607769 : const double extraGap = ego != nullptr ? ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_EXTRA_GAP, 0) : 0;
1608 771019665 : for (int i = 0; i < (int)myFoeLanes.size(); ++i) {
1609 480411897 : const MSLane* foeLane = myFoeLanes[i];
1610 480411897 : const MSLink* foeExitLink = foeLane->getLinkCont()[0];
1611 : // distance from the querying vehicle to the crossing point with foeLane
1612 480411897 : double distToCrossing = dist - myConflicts[i].getLengthBehindCrossing(this);
1613 480411897 : const double foeDistToCrossing = foeLane->getLength() - myConflicts[i].getFoeLengthBehindCrossing(foeExitLink);
1614 480411897 : const bool sameTarget = (myLane == foeExitLink->getLane()) && !isInternalJunctionLink() && !foeExitLink->isInternalJunctionLink();
1615 480411897 : const bool sameSource = (myInternalLaneBefore != nullptr && myInternalLaneBefore->getNormalPredecessorLane() == foeLane->getNormalPredecessorLane());
1616 480411897 : const double crossingWidth = (sameTarget || sameSource) ? 0 : myConflicts[i].conflictSize;
1617 480411897 : const double foeCrossingWidth = (sameTarget || sameSource) ? 0 : myConflicts[i].getFoeConflictSize(foeExitLink);
1618 : // special treatment of contLane foe only applies if this lane is not a contLane or contLane follower itself
1619 607823523 : const bool contLane = (foeExitLink->getViaLaneOrLane()->getEdge().isInternal() && !(
1620 127411626 : isInternalJunctionLink() || isExitLinkAfterInternalJunction()));
1621 480411897 : if (gDebugFlag1) {
1622 : std::cout << " distToCrossing=" << distToCrossing << " foeLane=" << foeLane->getID() << " cWidth=" << crossingWidth
1623 0 : << " flag=" << myConflicts[i].flag << " i=" << i << " fcIndex=" << myConflicts[i].foeConflictIndex
1624 0 : << " ijl=" << isInternalJunctionLink() << " sT=" << sameTarget << " sS=" << sameSource
1625 0 : << " lbc=" << myConflicts[i].getLengthBehindCrossing(this)
1626 0 : << " flbc=" << myConflicts[i].getFoeLengthBehindCrossing(foeExitLink)
1627 : << " cw=" << crossingWidth
1628 : << " fcw=" << foeCrossingWidth
1629 : << " contLane=" << contLane
1630 0 : << " state=" << toString(myState)
1631 0 : << " foeState=" << toString(foeExitLink->getState())
1632 0 : << "\n";
1633 : }
1634 106068008 : if (distToCrossing + crossingWidth < 0 && !sameTarget
1635 580763211 : && (ego == nullptr || !MSGlobals::gComputeLC || distToCrossing + crossingWidth + ego->getVehicleType().getLength() < 0)) {
1636 96266289 : if (gDebugFlag1) {
1637 0 : std::cout << " ignore:egoBeyondCrossingPoint\n";
1638 : }
1639 96266289 : continue; // vehicle is behind the crossing point, continue with next foe lane
1640 : }
1641 : bool ignoreGreenCont = false;
1642 : bool foeIndirect = false;
1643 384145608 : if (contLane) {
1644 41878649 : const MSLink* entry = getLaneBefore()->getEntryLink();
1645 41878649 : const MSLink* foeEntry = foeLane->getEntryLink();
1646 41878649 : foeIndirect = foeEntry->myAmIndirect;
1647 41598344 : if (entry != nullptr && entry->haveGreen()
1648 26548394 : && foeEntry != nullptr && foeEntry->haveGreen()
1649 54266236 : && entry->myLaneBefore != foeEntry->myLaneBefore) {
1650 : // ignore vehicles before an internaljunction as long as they are still in green minor mode
1651 : ignoreGreenCont = true;
1652 : }
1653 : }
1654 41878649 : if (foeIndirect && distToCrossing >= NO_INTERSECTION) {
1655 31466 : if (gDebugFlag1) {
1656 0 : std::cout << " ignore:noIntersection\n";
1657 : }
1658 31466 : continue;
1659 : }
1660 : // it is not sufficient to return the last vehicle on the foeLane because ego might be its leader
1661 : // therefore we return all vehicles on the lane
1662 : //
1663 : // special care must be taken for continuation lanes. (next lane is also internal)
1664 : // vehicles on cont. lanes or on internal lanes with the same target as this link can not be ignored
1665 : // and should block (gap = -1) unless they are part of an indirect turn
1666 : MSLane::AnyVehicleIterator end = foeLane->anyVehiclesEnd();
1667 30587909 : for (MSLane::AnyVehicleIterator it_veh = foeLane->anyVehiclesBegin(); it_veh != end; ++it_veh) {
1668 30587910 : MSVehicle* leader = (MSVehicle*)*it_veh;
1669 30587910 : const double leaderBack = leader->getBackPositionOnLane(foeLane) - extraGap;
1670 30587910 : const double leaderBackDist = foeDistToCrossing - leaderBack;
1671 30587910 : const double l2 = ego != nullptr ? ego->getLength() + 2 : 0; // add some slack to account for further meeting-angle effects
1672 30475418 : const double sagitta = ego != nullptr && myRadius != std::numeric_limits<double>::max() ? myRadius - sqrt(myRadius * myRadius - 0.25 * l2 * l2) : 0;
1673 30587910 : const bool pastTheCrossingPoint = leaderBackDist + foeCrossingWidth + sagitta < 0;
1674 30587910 : const bool enteredTheCrossingPoint = leaderBackDist < leader->getVehicleType().getLength();
1675 30587910 : const bool foeIsBicycleTurn = (leader->getVehicleType().getVehicleClass() == SVC_BICYCLE
1676 30587910 : && foeLane->getIncomingLanes().front().viaLink->getDirection() == LinkDirection::LEFT);
1677 30587910 : const bool ignoreIndirectBicycleTurn = pastTheCrossingPoint && foeIsBicycleTurn;
1678 30587910 : const bool cannotIgnore = ((contLane && !ignoreIndirectBicycleTurn) || sameTarget || (sameSource && !MSGlobals::gComputeLC)) && ego != nullptr;
1679 30587910 : const bool inTheWay = ((((!pastTheCrossingPoint && distToCrossing > 0) || (sameTarget && distToCrossing > leaderBackDist - leader->getLength()))
1680 24191855 : && (enteredTheCrossingPoint || (sameSource && !enteredTheCrossingPoint && foeDistToCrossing < distToCrossing))
1681 16305319 : && (!(myConflicts[i].flag == CONFLICT_DUMMY_MERGE) || foeIsBicycleTurn || sameSource))
1682 44912159 : || foeExitLink->getLaneBefore()->getNormalPredecessorLane() == myLane->getBidiLane());
1683 30587910 : const bool isOpposite = leader->getLaneChangeModel().isOpposite();
1684 30587910 : const auto avi = foeExitLink->getApproaching(leader);
1685 : // if leader is not found, assume that it performed a lane change in the last step
1686 30587910 : const bool willPass = avi.willPass || (avi.arrivalTime == INVALID_TIME && sameTarget);
1687 30587910 : if (gDebugFlag1) {
1688 : std::cout << " candidate leader=" << leader->getID()
1689 : << " cannotIgnore=" << cannotIgnore
1690 : << " fdtc=" << foeDistToCrossing
1691 : << " lb=" << leaderBack
1692 : << " lbd=" << leaderBackDist
1693 : << " fcwidth=" << foeCrossingWidth
1694 0 : << " r=" << myRadius
1695 : << " sagitta=" << sagitta
1696 : << " foePastCP=" << pastTheCrossingPoint
1697 : << " foeEnteredCP=" << enteredTheCrossingPoint
1698 : << " inTheWay=" << inTheWay
1699 : << " willPass=" << willPass
1700 0 : << " isFrontOnLane=" << leader->isFrontOnLane(foeLane)
1701 : << " ignoreGreenCont=" << ignoreGreenCont
1702 : << " foeIndirect=" << foeIndirect
1703 : << " foeBikeTurn=" << foeIsBicycleTurn
1704 0 : << " isOpposite=" << isOpposite << "\n";
1705 : }
1706 30587910 : if (leader == ego) {
1707 9243953 : continue;
1708 : }
1709 : // ignore greenCont foe vehicles that are not in the way
1710 29645122 : if (!inTheWay && ignoreGreenCont) {
1711 6890 : if (gDebugFlag1) {
1712 0 : std::cout << " ignoreGreenCont\n";
1713 : }
1714 6890 : continue;
1715 : }
1716 : // after entering the conflict area, ignore foe vehicles that are not in the way
1717 3816666 : if ((!MSGlobals::gComputeLC || (ego != nullptr && ego->getLane() == foeLane) || MSGlobals::gSublane)
1718 28248911 : && distToCrossing < -POSITION_EPS && !inTheWay
1719 29967294 : && (ego == nullptr || !MSGlobals::gComputeLC || distToCrossing < -ego->getVehicleType().getLength())) {
1720 132386 : if (gDebugFlag1) {
1721 0 : std::cout << " ego entered conflict area\n";
1722 : }
1723 132386 : continue;
1724 : }
1725 29605655 : if (!MSGlobals::gComputeLC
1726 25733043 : && sameSource
1727 8047624 : && &ego->getLane()->getEdge() == &myInternalLaneBefore->getEdge()
1728 29973684 : && leaderBack + leader->getLength() < ego->getPositionOnLane() - ego->getLength()) {
1729 : // ego is already on the junction and clearly ahead of foe
1730 99809 : if (gDebugFlag1) {
1731 0 : std::cout << " ego ahead of same-source foe\n";
1732 : }
1733 99809 : continue;
1734 : }
1735 :
1736 : // ignore foe vehicles that will not pass
1737 18721863 : if ((!cannotIgnore || leader->isStopped() || sameTarget)
1738 20029951 : && !willPass
1739 2332106 : && (avi.arrivalTime == INVALID_TIME || leader->getSpeed() < SUMO_const_haltingSpeed)
1740 2331834 : && leader->isFrontOnLane(foeLane)
1741 : && !isOpposite
1742 1172855 : && !inTheWay
1743 : // willPass is false if the vehicle is already on the stopping edge
1744 29857476 : && !leader->willStop()) {
1745 450650 : if (gDebugFlag1) {
1746 0 : std::cout << " foe will not pass\n";
1747 : }
1748 450650 : continue;
1749 : }
1750 28955387 : if (leader->isBidiOn(foeLane)) {
1751 : // conflict resolved via forward lane of the foe
1752 1277058 : continue;
1753 : }
1754 : // check whether foe is blocked and might need to change before leaving the junction
1755 27678329 : const bool foeStrategicBlocked = (leader->getLaneChangeModel().isStrategicBlocked() &&
1756 1422028 : leader->getCarFollowModel().brakeGap(leader->getSpeed()) <= foeLane->getLength() - leaderBack);
1757 27678329 : const bool sameInternalEdge = &myInternalLaneBefore->getEdge() == &foeExitLink->getInternalLaneBefore()->getEdge();
1758 :
1759 27678329 : const bool foeLaneIsBidi = myInternalLaneBefore->getBidiLane() == foeLane;
1760 27678329 : if (MSGlobals::gSublane && ego != nullptr && (sameSource || sameTarget || foeLaneIsBidi)
1761 7144805 : && (!foeStrategicBlocked || sameInternalEdge)) {
1762 7006090 : if (ego->getLane() == leader->getLane()) {
1763 160016 : continue;
1764 : }
1765 : // ignore vehicles if not in conflict sublane-wise
1766 6846074 : const double egoLatOffset = isShadowLink ? ego->getLatOffset(ego->getLaneChangeModel().getShadowLane()) : 0;
1767 6846074 : const double posLat = ego->getLateralPositionOnLane() + egoLatOffset;
1768 6846074 : double posLatLeader = leader->getLateralPositionOnLane() + leader->getLatOffset(foeLane);
1769 6846074 : if (foeLaneIsBidi) {
1770 : // leader is oncoming
1771 431 : posLatLeader = foeLane->getWidth() - posLatLeader;
1772 : }
1773 6846074 : const double latGap = (fabs(posLat - posLatLeader)
1774 6846074 : - 0.5 * (ego->getVehicleType().getWidth() + leader->getVehicleType().getWidth()));
1775 6846074 : const double maneuverDist = leader->getLaneChangeModel().getManeuverDist() * (posLat < posLatLeader ? -1 : 1);
1776 6846074 : if (gDebugFlag1) {
1777 0 : std::cout << " checkIgnore sublaneFoe lane=" << myInternalLaneBefore->getID()
1778 : << " sameSource=" << sameSource
1779 : << " sameTarget=" << sameTarget
1780 : << " foeLaneIsBidi=" << foeLaneIsBidi
1781 : << " foeLane=" << foeLane->getID()
1782 : << " leader=" << leader->getID()
1783 0 : << " egoLane=" << ego->getLane()->getID()
1784 0 : << " leaderLane=" << leader->getLane()->getID()
1785 : << " egoLat=" << posLat
1786 : << " egoLatOffset=" << egoLatOffset
1787 : << " leaderLat=" << posLatLeader
1788 0 : << " leaderLatOffset=" << leader->getLatOffset(foeLane)
1789 : << " latGap=" << latGap
1790 : << " maneuverDist=" << maneuverDist
1791 0 : << " computeLC=" << MSGlobals::gComputeLC
1792 0 : << " egoMaxSpeedLat=" << ego->getVehicleType().getMaxSpeedLat()
1793 0 : << "\n";
1794 : }
1795 1483356 : if (latGap > 0 && (latGap > maneuverDist || !sameTarget || !MSGlobals::gComputeLC)
1796 : // do not perform sublane changes that interfere with the leader vehicle
1797 8322893 : && (!MSGlobals::gComputeLC || latGap > ego->getVehicleType().getMaxSpeedLat())) {
1798 1342454 : const MSLink* foeEntryLink = foeLane->getIncomingLanes().front().viaLink;
1799 1342454 : if (sameSource) {
1800 : // for lanes from the same edge, higer index implies a
1801 : // connection further to the left
1802 1033556 : const bool leaderFromRight = (myIndex > foeEntryLink->getIndex());
1803 1033556 : if ((posLat > posLatLeader) == leaderFromRight) {
1804 : // ignore speed since lanes diverge
1805 594651 : if (gDebugFlag1) {
1806 0 : std::cout << " ignored (same source) leaderFromRight=" << leaderFromRight << "\n";
1807 : }
1808 594651 : continue;
1809 : }
1810 308898 : } else if (sameTarget) {
1811 : // for lanes from different edges we cannot rely on the
1812 : // index due to wrap-around issues
1813 308467 : if (myDirection != foeEntryLink->getDirection()) {
1814 301655 : bool leaderFromRight = foeEntryLink->getDirection() < myDirection;
1815 : // leader vehicle should not move towards ego
1816 301655 : if (MSGlobals::gLefthand) {
1817 0 : leaderFromRight = !leaderFromRight;
1818 : }
1819 419892 : if ((posLat > posLatLeader) == leaderFromRight
1820 : // leader should keep lateral position or move away from ego
1821 166877 : && (leader->getLaneChangeModel().getSpeedLat() == 0
1822 49284 : || leaderFromRight == (leader->getLaneChangeModel().getSpeedLat() < latGap))
1823 429001 : && (ego->getLaneChangeModel().getSpeedLat() == 0
1824 16492 : || leaderFromRight == (ego->getLaneChangeModel().getSpeedLat() > -latGap))) {
1825 118237 : if (gDebugFlag1) {
1826 0 : std::cout << " ignored (different source) leaderFromRight=" << leaderFromRight << "\n";
1827 : }
1828 118237 : continue;
1829 : }
1830 : } else {
1831 : // XXX figure out relative direction somehow
1832 : }
1833 : } else {
1834 431 : if (gDebugFlag1) {
1835 0 : std::cout << " ignored oncoming bidi leader\n";
1836 : }
1837 431 : continue;
1838 : }
1839 : }
1840 : }
1841 26804994 : if (leader->getWaitingTime() < MSGlobals::gIgnoreJunctionBlocker) {
1842 : // compute distance between vehicles on the superimposition of both lanes
1843 : // where the crossing point is the common point
1844 : double gap;
1845 : bool fromLeft = true;
1846 26313762 : if (ego == nullptr) {
1847 : // request from pedestrian model. return distance between leaderBack and crossing point
1848 : //std::cout << " foeLane=" << foeLane->getID() << " leaderBack=" << leaderBack << " foeDistToCrossing=" << foeDistToCrossing << " foeLength=" << foeLane->getLength() << " foebehind=" << myConflicts[i].second << " dist=" << dist << " behind=" << myConflicts[i].first << "\n";
1849 112082 : gap = leaderBackDist;
1850 : // distToCrossing should not take into account the with of the foe lane
1851 : // (which was subtracted in setRequestInformation)
1852 : // Instead, the width of the foe vehicle is used directly by the caller.
1853 112082 : distToCrossing += myConflicts[i].conflictSize / 2;
1854 112082 : if (gap + foeCrossingWidth < 0) {
1855 : // leader is completely past the crossing point
1856 : // or there is no crossing point
1857 4518249 : continue; // next vehicle
1858 : }
1859 : // we need to determine whether the vehicle passes the
1860 : // crossing from the left or the right (heuristic)
1861 109899 : fromLeft = foeDistToCrossing > 0.5 * foeLane->getLength();
1862 26201680 : } else if ((contLane && !sameSource && !ignoreIndirectBicycleTurn) || isOpposite) {
1863 1355488 : gap = -std::numeric_limits<double>::max(); // always break for vehicles which are on a continuation lane or for opposite-direction vehicles
1864 : } else {
1865 24846192 : if (pastTheCrossingPoint && !sameTarget) {
1866 : // leader is completely past the crossing point
1867 : // or there is no crossing point
1868 4515987 : if (gDebugFlag1) {
1869 0 : std::cout << " foePastCP ignored\n";
1870 : }
1871 4515987 : continue;
1872 : }
1873 : double leaderBackDist2 = leaderBackDist;
1874 20330205 : if (sameTarget && leaderBackDist2 < 0) {
1875 3115438 : const double mismatch = myConflicts[i].getFoeLengthBehindCrossing(foeExitLink) - myConflicts[i].getLengthBehindCrossing(this);
1876 3115438 : if (mismatch > 0) {
1877 1562835 : leaderBackDist2 += mismatch;
1878 : }
1879 : }
1880 20330205 : if (gDebugFlag1) {
1881 : std::cout << " distToCrossing=" << distToCrossing << " leaderBack=" << leaderBack
1882 : << " backDist=" << leaderBackDist
1883 : << " backDist2=" << leaderBackDist2
1884 0 : << " blockedStrategic=" << leader->getLaneChangeModel().isStrategicBlocked()
1885 0 : << "\n";
1886 : }
1887 20330205 : gap = distToCrossing - ego->getVehicleType().getMinGap() - leaderBackDist2 - foeCrossingWidth;
1888 : }
1889 : // if the foe is already moving off the intersection, we may
1890 : // advance up to the crossing point unless we have the same target or same source
1891 : // (for sameSource, the crossing point indicates the point of divergence)
1892 25577265 : const bool stopAsap = ((leader->isFrontOnLane(foeLane) ? cannotIgnore : (sameTarget || sameSource))
1893 22351551 : || (ego != nullptr && ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_ADVANCE, 1.0) == 0.0));
1894 21795592 : if (gDebugFlag1) {
1895 0 : std::cout << " leader=" << leader->getID() << " contLane=" << contLane << " cannotIgnore=" << cannotIgnore << " stopAsap=" << stopAsap << " gap=" << gap << "\n";
1896 : }
1897 21795592 : if (ignoreFoe(ego, leader)) {
1898 79 : continue;
1899 : }
1900 21795513 : const int llFlags = ((fromLeft ? LL_FROM_LEFT : 0) |
1901 21795513 : (inTheWay ? LL_IN_THE_WAY : 0) |
1902 21795513 : (sameSource ? LL_SAME_SOURCE : 0) |
1903 21795513 : (sameTarget ? LL_SAME_TARGET : 0));
1904 28890849 : result.emplace_back(leader, gap, stopAsap ? -1 : distToCrossing, llFlags, leader->getLatOffset(foeLane));
1905 : }
1906 :
1907 : }
1908 384114141 : if (ego != nullptr && MSNet::getInstance()->hasPersons()) {
1909 : // check for crossing pedestrians (keep driving if already on top of the crossing
1910 8797165 : const double distToPeds = distToCrossing - ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPLINE_CROSSING_GAP, MSPModel::SAFETY_GAP);
1911 8797165 : const double vehWidth = ego->getVehicleType().getWidth() + MSPModel::SAFETY_GAP; // + configurable safety gap
1912 : /// @todo consider lateral position (depending on whether the crossing is encountered on the way in or out)
1913 : // @check lefthand?!
1914 8797165 : const bool wayIn = myConflicts[i].lengthBehindCrossing < myLaneBefore->getLength() * 0.5;
1915 8797165 : const double vehCenter = (foeDistToCrossing + myLaneBefore->getWidth() * 0.5
1916 8797165 : + ego->getLateralPositionOnLane() * (wayIn ? -1 : 1));
1917 : // can access the movement model here since we already checked for existing persons above
1918 17138774 : if (distToPeds >= -MSPModel::SAFETY_GAP && MSNet::getInstance()->getPersonControl().getMovementModel()->blockedAtDist(ego, foeLane, vehCenter, vehWidth,
1919 8341609 : ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_CROSSING_GAP, JM_CROSSING_GAP_DEFAULT),
1920 : collectBlockers)) {
1921 519427 : result.emplace_back(nullptr, -1, distToPeds);
1922 8277738 : } else if (foeLane->isCrossing() && ego->getLane()->isInternal() && ego->getLane()->getEdge().getToJunction() == myJunction) {
1923 146928 : const MSLink* crossingLink = foeLane->getIncomingLanes()[0].viaLink;
1924 146928 : if (distToCrossing > 0 && crossingLink->havePriority() && crossingLink->myApproachingPersons != nullptr) {
1925 : // a person might step on the crossing at any moment, since ego
1926 : // is already on the junction, the opened() check is not done anymore
1927 26500 : const double timeToEnterCrossing = distToCrossing / MAX2(ego->getSpeed(), 1.0);
1928 31806 : for (const auto& item : (*crossingLink->myApproachingPersons)) {
1929 6853 : if (!ignoreFoe(ego, item.first) && timeToEnterCrossing > STEPS2TIME(item.second.arrivalTime - SIMSTEP)) {
1930 1547 : if (gDebugFlag1) {
1931 0 : std::cout << SIMTIME << ": " << ego->getID() << " breaking for approaching person " << item.first->getID()
1932 : //<< " dtc=" << distToCrossing << " ttc=" << distToCrossing / MAX2(ego->getSpeed(), 1.0) << " foeAT=" << item.second.arrivalTime << " foeTTC=" << STEPS2TIME(item.second.arrivalTime - SIMSTEP)
1933 0 : << "\n";
1934 : }
1935 1547 : result.emplace_back(nullptr, -1, distToPeds);
1936 1547 : break;
1937 : //} else {
1938 : // if (gDebugFlag1) {
1939 : // std::cout << SIMTIME << ": " << ego->getID() << " notBreaking for approaching person " << item.first->getID()
1940 : // << " dtc=" << distToCrossing << " ttc=" << distToCrossing / MAX2(ego->getSpeed(), 1.0) << " foeAT=" << item.second.arrivalTime << " foeTTC=" << STEPS2TIME(item.second.arrivalTime - SIMSTEP)
1941 : // << "\n";
1942 : // }
1943 : }
1944 : }
1945 : }
1946 : }
1947 : }
1948 : }
1949 :
1950 : //std::cout << SIMTIME << " ego=" << Named::getIDSecure(ego) << " link=" << getViaLaneOrLane()->getID() << " myWalkingAreaFoe=" << Named::getIDSecure(myWalkingAreaFoe) << "\n";
1951 290607768 : if (ego != nullptr) {
1952 289667121 : checkWalkingAreaFoe(ego, myWalkingAreaFoe, collectBlockers, result);
1953 289667121 : checkWalkingAreaFoe(ego, myWalkingAreaFoeExit, collectBlockers, result);
1954 : }
1955 :
1956 290607768 : if (MSGlobals::gLateralResolution > 0 && ego != nullptr && !isShadowLink) {
1957 : // check for foes on the same edge
1958 71909208 : for (std::vector<MSLane*>::const_iterator it = mySublaneFoeLanes.begin(); it != mySublaneFoeLanes.end(); ++it) {
1959 6026256 : const MSLane* foeLane = *it;
1960 : MSLane::AnyVehicleIterator end = foeLane->anyVehiclesEnd();
1961 4209228 : for (MSLane::AnyVehicleIterator it_veh = foeLane->anyVehiclesBegin(); it_veh != end; ++it_veh) {
1962 4209228 : MSVehicle* leader = (MSVehicle*)*it_veh;
1963 4209228 : if (leader == ego) {
1964 2456863 : continue;
1965 : }
1966 3572685 : if (leader->getLane()->isNormal()) {
1967 : // leader is past the conflict point
1968 1361320 : continue;
1969 : }
1970 2211365 : const double maxLength = MAX2(myInternalLaneBefore->getLength(), foeLane->getLength());
1971 2211365 : const double gap = dist - maxLength - ego->getVehicleType().getMinGap() + leader->getBackPositionOnLane(foeLane) - extraGap;
1972 2211365 : if (gap < -(ego->getVehicleType().getMinGap() + leader->getLength())) {
1973 : // ego is ahead of leader
1974 459000 : continue;
1975 : }
1976 1752365 : const double posLat = ego->getLateralPositionOnLane();
1977 1752365 : const double posLatLeader = leader->getLateralPositionOnLane() + leader->getLatOffset(foeLane);
1978 1752365 : if (gDebugFlag1) {
1979 0 : std::cout << " sublaneFoe lane=" << myInternalLaneBefore->getID()
1980 : << " foeLane=" << foeLane->getID()
1981 : << " leader=" << leader->getID()
1982 0 : << " egoLane=" << ego->getLane()->getID()
1983 0 : << " leaderLane=" << leader->getLane()->getID()
1984 : << " gap=" << gap
1985 : << " egoLat=" << posLat
1986 : << " leaderLat=" << posLatLeader
1987 0 : << " leaderLatOffset=" << leader->getLatOffset(foeLane)
1988 0 : << " egoIndex=" << myInternalLaneBefore->getIndex()
1989 0 : << " foeIndex=" << foeLane->getIndex()
1990 0 : << " dist=" << dist
1991 0 : << " leaderBack=" << leader->getBackPositionOnLane(foeLane)
1992 0 : << "\n";
1993 : }
1994 : // there only is a conflict if the paths cross
1995 695109 : if ((posLat < posLatLeader && myInternalLaneBefore->getIndex() > foeLane->getIndex())
1996 2079807 : || (posLat > posLatLeader && myInternalLaneBefore->getIndex() < foeLane->getIndex())) {
1997 796171 : if (gDebugFlag1) {
1998 0 : std::cout << SIMTIME << " blocked by " << leader->getID() << " (sublane split) foeLane=" << foeLane->getID() << "\n";
1999 : }
2000 796171 : if (ignoreFoe(ego, leader)) {
2001 0 : continue;
2002 : }
2003 796171 : result.emplace_back(leader, gap, -1);
2004 : }
2005 : }
2006 : }
2007 : }
2008 : return result;
2009 1 : }
2010 :
2011 :
2012 : void
2013 579334242 : MSLink::checkWalkingAreaFoe(const MSVehicle* ego, const MSLane* foeLane, std::vector<const MSPerson*>* collectBlockers, LinkLeaders& result) const {
2014 579334242 : if (foeLane != nullptr && foeLane->getEdge().getPersons().size() > 0) {
2015 : // pedestrians may be on an arbitrary path across this
2016 : // walkingarea. make sure to keep enough distance.
2017 : // This is a simple but conservative solution that could be improved
2018 : // by ignoring pedestrians that are "obviously" not on a collision course
2019 125665 : double distToPeds = std::numeric_limits<double>::max();
2020 : assert(myInternalLaneBefore != nullptr);
2021 125665 : PositionVector egoPath = myInternalLaneBefore->getShape();
2022 125665 : if (ego->getLateralPositionOnLane() != 0) {
2023 107434 : egoPath.move2side((MSGlobals::gLefthand ? 1 : -1) * ego->getLateralPositionOnLane());
2024 : }
2025 959753 : for (MSTransportable* t : foeLane->getEdge().getPersons()) {
2026 834088 : MSPerson* p = static_cast<MSPerson*>(t);
2027 834088 : double dist = ego->getPosition().distanceTo2D(p->getPosition()) - p->getVehicleType().getLength();
2028 834088 : const bool inFront = isInFront(ego, egoPath, p->getPosition()) || isInFront(ego, egoPath, getFuturePosition(p));
2029 : if (inFront) {
2030 305886 : dist -= MAX2(ego->getVehicleType().getMinGap(), MSPModel::SAFETY_GAP);
2031 : }
2032 : #ifdef DEBUG_WALKINGAREA
2033 : if (ego->isSelected()) {
2034 : std::cout << SIMTIME << " veh=" << ego->getID() << " ped=" << p->getID()
2035 : << " pos=" << ego->getPosition() << " pedPos=" << p->getPosition()
2036 : << " futurePedPos=" << getFuturePosition(p)
2037 : << " rawDist=" << ego->getPosition().distanceTo2D(p->getPosition())
2038 : << " inFront=" << inFront
2039 : << " dist=" << dist << "\n";
2040 : }
2041 : #endif
2042 834088 : if (dist < ego->getVehicleType().getWidth() / 2 || inFront) {
2043 153234 : if (inFront) {
2044 152943 : const double oncomingFactor = isOnComingPed(ego, p);
2045 152943 : if (oncomingFactor > 0) {
2046 : // account for pedestrian movement while closing in
2047 53949 : const double timeToStop = sqrt(dist) / 2;
2048 53949 : const double pedDist = p->getMaxSpeed() * MAX2(timeToStop, TS) * oncomingFactor;
2049 53949 : dist = MAX2(0.0, dist - pedDist);
2050 : #ifdef DEBUG_WALKINGAREA
2051 : if (ego->isSelected()) {
2052 : std::cout << " timeToStop=" << timeToStop << " pedDist=" << pedDist << " factor=" << oncomingFactor << " dist2=" << dist << "\n";
2053 : }
2054 : #endif
2055 : }
2056 : }
2057 153234 : if (ignoreFoe(ego, p)) {
2058 25237 : continue;
2059 : }
2060 127997 : distToPeds = MIN2(distToPeds, dist);
2061 127997 : if (collectBlockers != nullptr) {
2062 0 : collectBlockers->push_back(p);
2063 : }
2064 : }
2065 : }
2066 125665 : if (distToPeds != std::numeric_limits<double>::max()) {
2067 : // leave extra space in front
2068 87077 : result.emplace_back(nullptr, -1, distToPeds);
2069 : }
2070 125665 : }
2071 579334242 : }
2072 :
2073 : bool
2074 1531611 : MSLink::isInFront(const MSVehicle* ego, const PositionVector& egoPath, const Position& pPos) const {
2075 1531611 : const double pedAngle = ego->getPosition().angleTo2D(pPos);
2076 1531611 : const double angleDiff = fabs(GeomHelper::angleDiff(ego->getAngle(), pedAngle));
2077 : #ifdef DEBUG_WALKINGAREA
2078 : if (ego->isSelected()) {
2079 : std::cout << " angleDiff=" << RAD2DEG(angleDiff) << "\n";
2080 : }
2081 : #endif
2082 1531611 : if (angleDiff < DEG2RAD(75)) {
2083 1082758 : return egoPath.distance2D(pPos) < ego->getVehicleType().getWidth() + MSPModel::SAFETY_GAP;
2084 : }
2085 : return false;
2086 : }
2087 :
2088 :
2089 : double
2090 152943 : MSLink::isOnComingPed(const MSVehicle* ego, const MSPerson* p) const {
2091 152943 : const double pedToEgoAngle = p->getPosition().angleTo2D(ego->getPosition());
2092 152943 : const double angleDiff = fabs(GeomHelper::angleDiff(p->getAngle(), pedToEgoAngle));
2093 : #ifdef DEBUG_WALKINGAREA
2094 : if (ego->isSelected()) {
2095 : std::cout << " ped-angleDiff=" << RAD2DEG(angleDiff) << " res=" << cos(angleDiff) << "\n";
2096 : }
2097 : #endif
2098 152943 : if (angleDiff <= DEG2RAD(90)) {
2099 : ;
2100 53949 : return cos(angleDiff);
2101 : } else {
2102 : return 0;
2103 : }
2104 : }
2105 :
2106 :
2107 : Position
2108 697523 : MSLink::getFuturePosition(const MSPerson* p, double timeHorizon) const {
2109 697523 : const double a = p->getAngle();
2110 697523 : const double dist = timeHorizon * p->getMaxSpeed();
2111 :
2112 697523 : const Position offset(cos(a) * dist, sin(a) * dist);
2113 697523 : return p->getPosition() + offset;
2114 : }
2115 :
2116 :
2117 : MSLink*
2118 10503301 : MSLink::getParallelLink(int direction) const {
2119 10503301 : if (direction == -1) {
2120 4027845 : return myParallelRight;
2121 6475456 : } else if (direction == 1) {
2122 5783239 : return myParallelLeft;
2123 : } else {
2124 : assert(false || myLane->getOpposite() != nullptr || MSGlobals::gComputeLC);
2125 : return nullptr;
2126 : }
2127 : }
2128 :
2129 : MSLink*
2130 169746 : MSLink::getOppositeDirectionLink() const {
2131 169746 : if (myLane->getOpposite() != nullptr && myLaneBefore->getOpposite() != nullptr) {
2132 42821 : for (MSLink* cand : myLane->getOpposite()->getLinkCont()) {
2133 39649 : if (cand->getLane() == myLaneBefore->getOpposite()) {
2134 : return cand;
2135 : }
2136 : }
2137 : }
2138 : return nullptr;
2139 : }
2140 :
2141 :
2142 : MSLink*
2143 5828690 : MSLink::computeParallelLink(int direction) {
2144 5828690 : const MSLane* const before = getLaneBefore()->getParallelLane(direction, false);
2145 5828690 : const MSLane* const after = getLane()->getParallelLane(direction, false);
2146 5828690 : if (before != nullptr && after != nullptr) {
2147 997630 : for (MSLink* const link : before->getLinkCont()) {
2148 657711 : if (link->getLane() == after) {
2149 : return link;
2150 : }
2151 : }
2152 : }
2153 : return nullptr;
2154 : }
2155 :
2156 :
2157 : double
2158 1327404 : MSLink::getZipperSpeed(const MSVehicle* ego, const double dist, double vSafe,
2159 : SUMOTime arrivalTime,
2160 : const BlockingFoes* foes) const {
2161 1327404 : if (myFoeLinks.size() == 0) {
2162 : // link should have LINKSTATE_MAJOR in this case
2163 : assert(false);
2164 : return vSafe;
2165 : }
2166 1327404 : const double brakeGap = ego->getCarFollowModel().brakeGap(vSafe, ego->getCarFollowModel().getMaxDecel(), TS);
2167 1442784 : if (dist > MAX2(myFoeVisibilityDistance, brakeGap)) {
2168 : #ifdef DEBUG_ZIPPER
2169 : const SUMOTime now = MSNet::getInstance()->getCurrentTimeStep();
2170 : DEBUGOUT(DEBUG_COND_ZIPPER, SIMTIME << " getZipperSpeed ego=" << ego->getID()
2171 : << " dist=" << dist << " bGap=" << brakeGap << " ignoring foes (arrival in " << STEPS2TIME(arrivalTime - now) << ")\n")
2172 : #endif
2173 : return vSafe;
2174 : }
2175 : #ifdef DEBUG_ZIPPER
2176 : DEBUGOUT(DEBUG_COND_ZIPPER, SIMTIME << " getZipperSpeed ego=" << ego->getID()
2177 : << " egoAT=" << arrivalTime
2178 : << " dist=" << dist
2179 : << " brakeGap=" << brakeGap
2180 : << " vSafe=" << vSafe
2181 : << " numFoes=" << foes->size()
2182 : << "\n")
2183 : #endif
2184 : const bool uniqueFoeLink = myFoeLinks.size() == 1;
2185 496377 : MSLink* foeLink = myFoeLinks[0];
2186 2598432 : for (const auto& item : *foes) {
2187 2102055 : if (!item->isVehicle()) {
2188 0 : continue;
2189 : }
2190 2102055 : const MSVehicle* foe = dynamic_cast<const MSVehicle*>(item);
2191 : assert(foe != 0);
2192 : const ApproachingVehicleInformation* aviPtr = nullptr;
2193 2102055 : if (uniqueFoeLink) {
2194 1396823 : aviPtr = foeLink->getApproachingPtr(foe);
2195 : } else {
2196 : // figure out which link is approached by the current foe
2197 1067476 : for (MSLink* fl : myFoeLinks) {
2198 1067476 : aviPtr = fl->getApproachingPtr(foe);
2199 1067476 : if (aviPtr != nullptr) {
2200 : break;
2201 : }
2202 : }
2203 : }
2204 2102055 : if (aviPtr == nullptr) {
2205 0 : continue;
2206 : }
2207 : const ApproachingVehicleInformation& avi = *aviPtr;
2208 2102055 : const double foeDist = (foe->isActive() ? avi.dist : MAX2(0.0, avi.dist -
2209 36 : STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - foe->getLastActionTime()) * avi.speed));
2210 :
2211 1428779 : if ( // ignore vehicles that arrive after us (unless they are ahead and we could easily brake for them)
2212 2202786 : ((avi.arrivalTime > arrivalTime) && !couldBrakeForLeader(dist, foeDist, ego, foe)) ||
2213 : // also ignore vehicles that are behind us and are able to brake for us
2214 2202786 : couldBrakeForLeader(foeDist, dist, foe, ego) ||
2215 : // resolve ties by lane index
2216 673284 : (avi.arrivalTime == arrivalTime && foeDist == dist && ego->getLane()->getIndex() < foe->getLane()->getIndex())) {
2217 : #ifdef DEBUG_ZIPPER
2218 : if (DEBUG_COND_ZIPPER) std::cout
2219 : << " ignoring foe=" << foe->getID()
2220 : << " foeAT=" << avi.arrivalTime
2221 : << " foeDist=" << avi.dist
2222 : << " foeDist2=" << foeDist
2223 : << " foeSpeed=" << avi.speed
2224 : << " egoSpeed=" << ego->getSpeed()
2225 : << " deltaDist=" << foeDist - dist
2226 : << " delteSpeed=" << avi.speed - foe->getCarFollowModel().getMaxDecel() - ego->getSpeed()
2227 : << " egoCouldBrake=" << couldBrakeForLeader(dist, foeDist, ego, foe)
2228 : << " foeCouldBrake=" << couldBrakeForLeader(foeDist, dist, foe, ego)
2229 : << "\n";
2230 : #endif
2231 1428779 : continue;
2232 : }
2233 : // the idea behind speed adaption is three-fold:
2234 : // 1) ego needs to be in a car-following relationship with foe eventually
2235 : // thus, the ego speed should be equal to the follow speed once the foe enters
2236 : // the zipper junction
2237 : // 2) ego vehicle needs to put a certain distance beteen himself and foe (safeGap)
2238 : // achieving this distance can be spread over time but computing
2239 : // safeGap is subject to estimation errors of future speeds
2240 : // 3) deceleration can be spread out over the time until true
2241 : // car-following happens, at the start of speed adaptions, smaller
2242 : // decelerations should be sufficient
2243 :
2244 : // we cannot trust avi.arrivalSpeed if the foe has leader vehicles that are accelerating
2245 : // lets try to extrapolate
2246 673276 : const double uMax = foe->getLane()->getVehicleMaxSpeed(foe);
2247 673276 : const double uAccel = foe->getCarFollowModel().estimateSpeedAfterDistance(foeDist, avi.speed, foe->getCarFollowModel().getMaxAccel());
2248 : const double uEnd = MIN2(uMax, uAccel);
2249 673276 : const double uAvg = (avi.speed + uEnd) / 2;
2250 673276 : const double tf0 = foeDist / MAX2(NUMERICAL_EPS, uAvg);
2251 673276 : const double tf = MAX2(1.0, ceil((tf0) / TS) * TS);
2252 :
2253 673276 : const double vMax = ego->getLane()->getVehicleMaxSpeed(ego);
2254 673276 : const double vAccel = ego->getCarFollowModel().estimateSpeedAfterDistance(dist, ego->getSpeed(), ego->getCarFollowModel().getMaxAccel());
2255 673276 : const double vDecel = ego->getCarFollowModel().estimateSpeedAfterDistance(dist, ego->getSpeed(), -ego->getCarFollowModel().getMaxDecel());
2256 : const double vEnd = MIN3(vMax, vAccel, MAX2(uEnd, vDecel));
2257 673276 : const double vAvg = (ego->getSpeed() + vEnd) / 2;
2258 673276 : const double te0 = dist / MAX2(NUMERICAL_EPS, vAvg);
2259 673276 : const double te = MAX2(1.0, ceil((te0) / TS) * TS);
2260 :
2261 673276 : const double tTarget = tf + ego->getCarFollowModel().getHeadwayTime();
2262 673276 : const double a = ego->getCarFollowModel().avoidArrivalAccel(dist, tTarget, vSafe, ego->getCarFollowModel().getMaxDecel());
2263 :
2264 673276 : const double gap = dist - foe->getVehicleType().getLength() - ego->getVehicleType().getMinGap() - foeDist;
2265 673276 : const double vFollow = ego->getCarFollowModel().followSpeed(
2266 673276 : ego, ego->getSpeed(), gap, avi.speed, foe->getCarFollowModel().getMaxDecel(), foe);
2267 673276 : const double vSafeGap = MAX2(vFollow, ego->getSpeed() + ACCEL2SPEED(a));
2268 :
2269 : // scale behavior based on ego time to link (te)
2270 673276 : const double w = MIN2(1.0, te / 10);
2271 673276 : const double maxDecel = w * ego->getCarFollowModel().getMaxDecel() + (1 - w) * ego->getCarFollowModel().getEmergencyDecel();
2272 673276 : const double vZipper = MAX3(vFollow, ego->getSpeed() - ACCEL2SPEED(maxDecel), vSafeGap);
2273 :
2274 : vSafe = MIN2(vSafe, vZipper);
2275 : #ifdef DEBUG_ZIPPER
2276 : if (DEBUG_COND_ZIPPER) std::cout << " adapting to foe=" << foe->getID()
2277 : << " foeDist=" << foeDist
2278 : << " foeSpeed=" << avi.speed
2279 : << " foeAS=" << avi.arrivalSpeed
2280 : << " egoSpeed=" << ego->getSpeed()
2281 : << " uMax=" << uMax
2282 : << " uAccel=" << uAccel
2283 : << " uEnd=" << uEnd
2284 : << " uAvg=" << uAvg
2285 : << " gap=" << gap
2286 : << "\n "
2287 : << " tf=" << tf
2288 : << " te=" << te
2289 : << " aSafeGap=" << a
2290 : << " vMax=" << vMax
2291 : << " vAccel=" << vAccel
2292 : << " vDecel=" << vDecel
2293 : << " vEnd=" << vEnd
2294 : << " vSafeGap=" << vSafeGap
2295 : << " vFollow=" << vFollow
2296 : << " w=" << w
2297 : << " maxDecel=" << maxDecel
2298 : << " vZipper=" << vZipper
2299 : << " vSafe=" << vSafe
2300 : << "\n";
2301 : #endif
2302 : }
2303 : return vSafe;
2304 : }
2305 :
2306 :
2307 : bool
2308 2202786 : MSLink::couldBrakeForLeader(double followDist, double leaderDist, const MSVehicle* follow, const MSVehicle* leader) {
2309 : return (// leader is ahead of follower
2310 2202786 : followDist > leaderDist &&
2311 : // and follower could brake for 1 s to stay behind leader
2312 397274 : followDist - leaderDist > follow->getSpeed() - follow->getCarFollowModel().getMaxDecel() - leader->getSpeed());
2313 : }
2314 :
2315 :
2316 : void
2317 2914345 : MSLink::initParallelLinks() {
2318 2914345 : myParallelRight = computeParallelLink(-1);
2319 2914345 : myParallelLeft = computeParallelLink(1);
2320 2914345 : }
2321 :
2322 : bool
2323 76702 : MSLink::checkContOff() const {
2324 : // check whether this link gets to keep its cont status switching the tls off
2325 : // @note: this could also be pre-computed in netconvert
2326 : // we check whether there is any major link from this edge
2327 223898 : for (const MSLane* cand : myLaneBefore->getEdge().getLanes()) {
2328 443157 : for (const MSLink* link : cand->getLinkCont()) {
2329 295961 : if (link->getOffState() == LINKSTATE_TL_OFF_NOSIGNAL) {
2330 : return true;
2331 : }
2332 : }
2333 : }
2334 : return false;
2335 : }
2336 :
2337 : bool
2338 4360951 : MSLink::lateralOverlap(double posLat, double width, double posLat2, double width2) {
2339 4360951 : return fabs(posLat2 - posLat) < (width + width2) / 2;
2340 : }
2341 :
2342 : std::string
2343 0 : MSLink::getDescription() const {
2344 0 : return myLaneBefore->getID() + "->" + getViaLaneOrLane()->getID();
2345 : }
2346 :
2347 :
2348 : bool
2349 189278446 : MSLink::ignoreFoe(const SUMOTrafficObject* ego, const SUMOTrafficObject* foe) {
2350 189278446 : if (ego == nullptr || !ego->getParameter().wasSet(VEHPARS_JUNCTIONMODEL_PARAMS_SET)) {
2351 189246007 : return false;
2352 : }
2353 32439 : const SUMOVehicleParameter& param = ego->getParameter();
2354 71050 : for (const std::string& typeID : StringTokenizer(param.getParameter(toString(SUMO_ATTR_JM_IGNORE_TYPES), "")).getVector()) {
2355 31537 : if (typeID == foe->getVehicleType().getID()) {
2356 : return true;
2357 : }
2358 32439 : }
2359 16136 : for (const std::string& id : StringTokenizer(param.getParameter(toString(SUMO_ATTR_JM_IGNORE_IDS), "")).getVector()) {
2360 2291 : if (id == foe->getID()) {
2361 : return true;
2362 : }
2363 7074 : }
2364 6771 : return false;
2365 : }
2366 :
2367 :
2368 : void
2369 110858 : MSLink::updateDistToFoePedCrossing(double dist) {
2370 110858 : myDistToFoePedCrossing = MIN2(myDistToFoePedCrossing, dist);
2371 110858 : }
2372 :
2373 :
2374 : std::pair<const SUMOVehicle* const, const MSLink::ApproachingVehicleInformation>
2375 8766799 : MSLink::getClosest() const {
2376 : assert(getApproaching().size() > 0);
2377 : double minDist = std::numeric_limits<double>::max();
2378 : auto closestIt = getApproaching().begin();
2379 17534327 : for (auto apprIt = getApproaching().begin(); apprIt != getApproaching().end(); apprIt++) {
2380 8767528 : if (apprIt->second.dist < minDist) {
2381 : minDist = apprIt->second.dist;
2382 : closestIt = apprIt;
2383 : }
2384 : }
2385 : // maybe a parallel link has a closer vehicle
2386 : /*
2387 : for (MSLink* link2 : link->getLaneBefore()->getLinkCont()) {
2388 : if (link2 != link) {
2389 : for (auto apprIt2 = link2->getApproaching().begin(); apprIt2 != link2->getApproaching().end(); apprIt2++) {
2390 : if (apprIt2->second.dist < minDist) {
2391 : minDist = apprIt2->second.dist;
2392 : closestIt = apprIt2;
2393 : }
2394 : }
2395 : }
2396 : }
2397 : */
2398 8766799 : return *closestIt;
2399 : }
2400 :
2401 :
2402 : bool
2403 473250 : MSLink::railSignalWasPassed() const {
2404 473250 : if (myJunction != nullptr && myJunction->getType() == SumoXMLNodeType::RAIL_SIGNAL) {
2405 2009 : for (const auto& item : myApproachingVehicles) {
2406 12 : if (item.second.dist < SPEED2DIST(item.first->getSpeed())) {
2407 : return true;
2408 : }
2409 : }
2410 : }
2411 : return false;
2412 : }
2413 :
2414 : /****************************************************************************/
|
