Eclipse SUMO - Simulation of Urban MObility
Loading...
Searching...
No Matches
MSLink.cpp
Go to the documentation of this file.
1/****************************************************************************/
2// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3// Copyright (C) 2001-2025 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/****************************************************************************/
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
72const SUMOTime MSLink::myLookaheadTime = TIME2STEPS(1);
73// additional caution is needed when approaching a zipper link
74const SUMOTime MSLink::myLookaheadTimeZipper = TIME2STEPS(16);
75std::set<std::pair<MSLink*, MSLink*> > MSLink::myRecheck;
76const double MSLink::NO_INTERSECTION(10000);
77
78// ===========================================================================
79// ConflictInfo member method definitions
80// ===========================================================================
81
82double
83MSLink::ConflictInfo::getFoeLengthBehindCrossing(const MSLink* foeExitLink) const {
84 if (flag == CONFLICT_DUMMY_MERGE) {
85 return 0;
86 } else if (foeConflictIndex >= 0) {
87 return foeExitLink->myConflicts[foeConflictIndex].lengthBehindCrossing;
88 } else {
89 return -NO_INTERSECTION;
90 }
91}
92
93double
94MSLink::ConflictInfo::getFoeConflictSize(const MSLink* foeExitLink) const {
95 if (foeConflictIndex >= 0) {
96 return foeExitLink->myConflicts[foeConflictIndex].conflictSize;
97 } else {
98 return 0;
99 }
100}
101
102double
103MSLink::ConflictInfo::getLengthBehindCrossing(const MSLink* exitLink) const {
104 if (flag == CONFLICT_STOP_AT_INTERNAL_JUNCTION) {
105 return exitLink->getInternalLaneBefore()->getLength();
106 } else {
107 return lengthBehindCrossing;
108 }
109}
110
111// ===========================================================================
112// member method definitions
113// ===========================================================================
114MSLink::MSLink(MSLane* predLane, MSLane* succLane, MSLane* via, LinkDirection dir, LinkState state,
115 double length, double foeVisibilityDistance, bool keepClear,
116 MSTrafficLightLogic* logic, int tlIndex,
117 bool indirect) :
118 myLane(succLane),
119 myLaneBefore(predLane),
120 myApproachingPersons(nullptr),
121 myIndex(-1),
122 myTLIndex(tlIndex),
123 myLogic(logic),
124 myState(state),
125 myLastGreenState(LINKSTATE_TL_GREEN_MINOR),
126 myOffState(state),
127 myLastStateChange(SUMOTime_MIN / 2), // a large negative value, but avoid overflows when subtracting
128 myDirection(dir),
129 myLength(length),
130 myFoeVisibilityDistance(foeVisibilityDistance),
131 myDistToFoePedCrossing(std::numeric_limits<double>::max()),
132 myHasFoes(false),
133 myAmCont(false),
134 myAmContOff(false),
135 myKeepClear(keepClear),
136 myInternalLane(via),
137 myInternalLaneBefore(nullptr),
138 myMesoTLSPenalty(0),
139 myGreenFraction(1),
140 myLateralShift(0),
141 myOffFoeLinks(nullptr),
142 myWalkingAreaFoe(nullptr),
143 myWalkingAreaFoeExit(nullptr),
144 myHavePedestrianCrossingFoe(false),
145 myParallelRight(nullptr),
146 myParallelLeft(nullptr),
147 myAmIndirect(indirect),
148 myRadius(std::numeric_limits<double>::max()),
149 myPermissions(myLaneBefore->getPermissions() & myLane->getPermissions() & (via == nullptr ? SVCAll : via->getPermissions())),
150 myJunction(nullptr) {
151
152 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 if ((myInternalLane != nullptr || predLane->isInternal())
156 && 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 from.move2side(dist);
163 } catch (InvalidArgument&) {
164 }
165 myLateralShift = (from.back().distanceTo2D(to.front()) < dist) ? dist : -dist;
166 if (MSGlobals::gLefthand) {
167 myLateralShift *= -1;
168 }
169 //std::cout << " lateral shift link=" << myLaneBefore->getID() << "->" << getViaLaneOrLane()->getID() << " dist=" << dist << " shift=" << myLateralShift << "\n";
170 }
171 }
172}
173
174
175MSLink::~MSLink() {
176 delete myOffFoeLinks;
177 delete myApproachingPersons;
178}
179
180
181void
182MSLink::addCustomConflict(const MSLane* from, const MSLane* to, double startPos, double endPos) {
183 myCustomConflicts.push_back(CustomConflict(from, to, startPos, endPos));
184}
185
186const MSLink::CustomConflict*
187MSLink::getCustomConflict(const MSLane* foeLane) const {
188 if (myCustomConflicts.size() > 0) {
189 const MSLane* foeFrom = foeLane->getNormalPredecessorLane();
190 const MSLane* foeTo = foeLane->getNormalSuccessorLane();
191 for (const CustomConflict& cc : myCustomConflicts) {
192 if (cc.from == foeFrom && cc.to == foeTo) {
193 return &cc;
194 }
195 }
196
197 }
198 return nullptr;
199}
200
201void
202MSLink::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 myIndex = index;
212 myHasFoes = hasFoes;
213 myAmCont = isCont && MSGlobals::gUsingInternalLanes;
214 myFoeLinks = foeLinks;
215 for (MSLane* foeLane : foeLanes) {
216 // cannot assign vector due to const-ness
217 myFoeLanes.push_back(foeLane);
218 }
219 myJunction = const_cast<MSJunction*>(myLane->getEdge().getFromJunction()); // junctionGraph is initialized after the whole network is loaded
220 myAmContOff = isCont && myLogic != nullptr && internalLaneBefore == nullptr && checkContOff();
221 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->getEdge().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 const MSLink* entryLink = getCorrespondingEntryLink();
232 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 myOffFoeLinks = new std::vector<MSLink*>();
236 if (isEntryLink()) {
237 for (MSLane* foeLane : foeLanes) {
238 assert(foeLane->isInternal() || foeLane->isCrossing());
239 MSLink* viaLink = foeLane->getIncomingLanes().front().viaLink;
240 if (viaLink->getLaneBefore()->isNormal()) {
241 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 if (lane != nullptr) {
250 const bool beforeInternalJunction = lane->getLinkCont()[0]->getViaLaneOrLane()->getEdge().isInternal();
251 if (lane->getIncomingLanes().size() != 1) {
252 throw ProcessError("Internal lane '" + lane->getID() + "' has " + toString(lane->getIncomingLanes().size()) + " predecessors");
253 }
254 const MSLink* junctionEntryLink = lane->getEntryLink();
255 const bool isSecondPart = isExitLinkAfterInternalJunction();
256 // compute crossing points
257 for (const MSLane* foeLane : myFoeLanes) {
258 const CustomConflict* cc = junctionEntryLink != nullptr ? junctionEntryLink->getCustomConflict(foeLane) : nullptr;
259 if (cc != nullptr) {
260 // handle custom conflict definition
261 double startPos = cc->startPos;
262 const double conflictSize = cc->endPos - cc->startPos;
263 if (isSecondPart) {
264 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 const CustomConflict* rcc = foeLane->getEntryLink()->getCustomConflict(lane);
275 bool haveIntersection = false;
276 if (rcc == nullptr) {
277 // a)
278 haveIntersection = lane->getShape().intersectsAtLengths2D(foeLane->getShape()).size() > 0;
279 } else {
280 // b)
281 const bool foeIsSecondPart = foeLane->getLogicalPredecessorLane()->isInternal();
282 double foeStartPos = rcc->startPos;
283 const double foeConflictSize = rcc->endPos - rcc->startPos;
284 if (foeIsSecondPart) {
285 foeStartPos -= foeLane->getLogicalPredecessorLane()->getLength();
286 }
287 const double foeEndPos = foeStartPos + foeConflictSize;
288 haveIntersection = ((foeStartPos > 0 && foeStartPos < foeLane->getLength())
289 || (foeEndPos > 0 && foeEndPos < foeLane->getLength()));
290 }
291 if (haveIntersection) {
292 myConflicts.push_back(ConflictInfo(lane->getLength() - startPos, conflictSize));
293 } else {
294 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 continue;
304 }
305 myHavePedestrianCrossingFoe = myHavePedestrianCrossingFoe || foeLane->getEdge().isCrossing();
306 const bool sameTarget = myLane == foeLane->getLinkCont()[0]->getLane();
307 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 const double minDist = MIN2(DIVERGENCE_MIN_WIDTH, 0.5 * (lane->getWidth() + foeLane->getWidth()));
311 if (lane->getShape().back().distanceTo2D(foeLane->getShape().back()) >= minDist) {
312 // account for lateral shift by the entry links
313 if (foeLane->getEntryLink()->isIndirect()) {
314 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 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 const double distAfterDivergence = computeDistToDivergence(lane, foeLane, minDist, false);
326 const double lbcLane = lane->interpolateGeometryPosToLanePos(distAfterDivergence);
327 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 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 if (intersections1.size() == 0) {
344 intersections1.push_back(-NO_INTERSECTION); // disregard this foe (using maxdouble leads to nasty problems down the line)
345 haveIntersection = false;
346 } else if (intersections1.size() > 1) {
347 std::sort(intersections1.begin(), intersections1.end());
348 }
349 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 if (intersections2.size() == 0) {
354 intersections2.push_back(0);
355 } else if (intersections2.size() > 1) {
356 std::sort(intersections2.begin(), intersections2.end());
357 }
358 double conflictSize = foeLane->getWidth();
359 ConflictFlag flag = CONFLICT_NO_INTERSECTION;
360 if (haveIntersection) {
361 flag = CONFLICT_DEFAULT;
362 const double angle1 = GeomHelper::naviDegree(lane->getShape().rotationAtOffset(intersections1.back()));
363 const double angle2 = GeomHelper::naviDegree(foeLane->getShape().rotationAtOffset(intersections2.back()));
364 const double angleDiff = GeomHelper::getMinAngleDiff(angle1, angle2);
365 //const double angleDiff = MIN2(GeomHelper::getMinAngleDiff(angle1, angle2),
366 // GeomHelper::getMinAngleDiff(angle1, angle2 + 180));
367 const double widthFactor = 1 / MAX2(sin(DEG2RAD(angleDiff)), 0.2) * 2 - 1;
368 //std::cout << " intersection of " << lane->getID() << " with " << foeLane->getID() << " angle1=" << angle1 << " angle2=" << angle2 << " angleDiff=" << angleDiff << " widthFactor=" << widthFactor << "\n";
369 conflictSize *= widthFactor;
370 conflictSize = MIN2(conflictSize, lane->getLength());
371 // lane width affects the crossing point
372 intersections1.back() -= conflictSize / 2;
373 // ensure non-negative offset for weird geometries
374 intersections1.back() = MAX2(0.0, intersections1.back());
375
376 // also length/geometry factor. (XXX: Why subtract width/2 *before* converting geometric position to lane pos? refs #3031)
377 intersections1.back() = lane->interpolateGeometryPosToLanePos(intersections1.back());
378
379 if (internalLaneBefore->getLogicalPredecessorLane()->getEdge().isInternal() && !foeLane->getEdge().isCrossing()) {
380 flag = CONFLICT_STOP_AT_INTERNAL_JUNCTION;
381 }
382
383 if (foeLane->getEdge().isCrossing()) {
384 const MSLink* before = myInternalLaneBefore->getCanonicalPredecessorLane()->getLinkTo(myInternalLaneBefore);
385 const_cast<MSLink*>(before)->updateDistToFoePedCrossing(intersections1.back());
386 };
387 }
388
389 myConflicts.push_back(ConflictInfo(
390 lane->getLength() - intersections1.back(),
391 conflictSize, flag));
392
393#ifdef MSLink_DEBUG_CROSSING_POINTS
394 std::cout
395 << " intersection of " << lane->getID()
396 << " totalLength=" << lane->getLength()
397 << " with " << foeLane->getID()
398 << " totalLength=" << foeLane->getLength()
399 << " dist1=" << myConflicts.back().lengthBehindCrossing
400 << " widthFactor=" << myConflicts.back().conflictSize / foeLane->getWidth()
401 << "\n";
402#endif
403 }
404 }
405 // check for overlap with internal lanes from the same source lane
406 const MSLane* pred = lane->getLogicalPredecessorLane();
407 // to avoid overlap with vehicles that came from pred (especially when pred has endOffset > 0)
408 // we add all other internal lanes from pred as foeLanes
409 for (const MSLink* const link : pred->getLinkCont()) {
410 const MSLane* const sibling = link->getViaLane();
411 if (sibling != lane && sibling != nullptr) {
412 const double minDist = MIN2(DIVERGENCE_MIN_WIDTH, 0.5 * (lane->getWidth() + sibling->getWidth()));
413 if (lane->getShape().front().distanceTo2D(sibling->getShape().front()) >= minDist) {
414 // account for lateral shift by the entry links
415 continue;
416 }
417 const double distToDivergence = computeDistToDivergence(lane, sibling, minDist, true);
418 double lbcLane;
419 if (lane->getLength() == sibling->getLength() && &lane->getEdge() == &sibling->getEdge()) {
420 // for parallel lanes, avoid inconsistency in distance estimation (#10988)
421 // between forward distance (getLeaderInfo)
422 // and backward distance used in lane-changing (getFollowersOnConsecutive)
423 lbcLane = lane->getLength() - distToDivergence;
424 } else {
425 lbcLane = MAX2(0.0, lane->getLength() - lane->interpolateGeometryPosToLanePos(distToDivergence));
426 }
427 ConflictInfo ci = ConflictInfo(lbcLane, sibling->getWidth());
428 auto it = std::find(myFoeLanes.begin(), myFoeLanes.end(), sibling);
429 if (it != myFoeLanes.end()) {
430 // avoid duplicate foeLane
431 const int replacedIndex = (int)(it - myFoeLanes.begin());
432 myConflicts[replacedIndex] = ci;
433 } else {
434 myConflicts.push_back(ci);
435 myFoeLanes.push_back(sibling);
436 }
437#ifdef MSLink_DEBUG_CROSSING_POINTS
438 std::cout << " adding same-origin foe" << sibling->getID()
439 << " dist1=" << myConflicts.back().lengthBehindCrossing
440 << "\n";
441#endif
442 const MSLane* const siblingCont = sibling->getLinkCont().front()->getViaLaneOrLane();
443 if (siblingCont->isInternal() && lane->getShape().distance2D(siblingCont->getShape().front()) < minDist) {
444 // there may still be overlap with siblingCont (when considering vehicle widths)
445 const double distToDivergence2 = computeDistToDivergence(lane, siblingCont, minDist, true, sibling->getLength());
446 double lbcLane2 = MAX2(0.0, lane->getLength() - lane->interpolateGeometryPosToLanePos(distToDivergence2));
447 ConflictInfo ci2 = ConflictInfo(lbcLane2, siblingCont->getWidth(), CONFLICT_SIBLING_CONTINUATION);
448 myConflicts.push_back(ci2);
449 myFoeLanes.push_back(siblingCont);
450 myRecheck.insert({this, siblingCont->getLinkCont().front()});
451
452#ifdef MSLink_DEBUG_CROSSING_POINTS
453 std::cout << " adding same-origin foeContinuation" << siblingCont->getID()
454 << " dist1=" << myConflicts.back().lengthBehindCrossing
455 << "\n";
456#endif
457 }
458 }
459 }
460 // init points for the symmetrical conflict
461 // for each pair of conflicting lanes, the link that gets second, sets the pointers
462 for (int i = 0; i < (int)myFoeLanes.size(); i++) {
463 const MSLane* foeLane = myFoeLanes[i];
464 MSLink* foeExitLink = foeLane->getLinkCont()[0];
465 int foundIndex = -1;
466 for (int i2 = 0; i2 < (int)foeExitLink->myFoeLanes.size(); i2++) {
467 if (foeExitLink->myFoeLanes[i2] == lane) {
468 myConflicts[i].foeConflictIndex = i2;
469 foeExitLink->myConflicts[i2].foeConflictIndex = i;
470 myRecheck.erase({foeExitLink, this});
471 foundIndex = i2;
472 break;
473 }
474 }
475#ifdef MSLink_DEBUG_CROSSING_POINTS
476 std::cout << lane->getID() << " foeLane=" << foeLane->getID() << " index=" << i << " foundIndex=" << foundIndex << "\n";
477#endif
478 if (foundIndex < 0) {
479 if (myConflicts[i].flag != CONFLICT_NO_INTERSECTION) {
480 myRecheck.insert({this, foeExitLink});
481 }
482 }
483 }
484 }
485 if (MSGlobals::gLateralResolution > 0) {
486 // check for links with the same origin lane and the same destination edge
487 const MSEdge* myTarget = &myLane->getEdge();
488 // save foes for entry links
489 for (MSLink* const it : myLaneBefore->getLinkCont()) {
490 const MSEdge* target = &(it->getLane()->getEdge());
491 if (it == this) {
492 continue;
493 }
494 if (target == myTarget) {
495 mySublaneFoeLinks.push_back(it);
496#ifdef MSLink_DEBUG_CROSSING_POINTS
497 std::cout << " sublaneFoeLink (same target): " << it->getViaLaneOrLane()->getID() << "\n";
498#endif
499 } else if (myDirection != LinkDirection::STRAIGHT && it->getDirection() == LinkDirection::STRAIGHT) {
500 // potential turn conflict
501 mySublaneFoeLinks2.push_back(it);
502#ifdef MSLink_DEBUG_CROSSING_POINTS
503 std::cout << " sublaneFoeLink2 (other target: " << it->getViaLaneOrLane()->getID() << "\n";
504#endif
505 }
506 }
507 // save foes for exit links
508 if (fromInternalLane()) {
509 //std::cout << " setRequestInformation link=" << getViaLaneOrLane()->getID() << " before=" << myLaneBefore->getID() << " before2=" << myLaneBefore->getIncomingLanes().front().lane->getID() << "\n";
510 for (const MSLink* const link : myLaneBefore->getIncomingLanes().front().lane->getLinkCont()) {
511 if (link->getViaLane() != myInternalLaneBefore && &link->getLane()->getEdge() == myTarget) {
512 //std::cout << " add sublaneFoe=" << (*it)->getViaLane()->getID() << "\n";
513 mySublaneFoeLanes.push_back(link->getViaLane());
514 }
515 }
516 }
517 }
518 if (myInternalLaneBefore != nullptr
519 && myDirection != LinkDirection::STRAIGHT
520 // for right turns, the curvature helps rather than restricts the linkLeader check
521 && (
522 (!MSGlobals::gLefthand && myDirection != LinkDirection::RIGHT)
523 || (MSGlobals::gLefthand && myDirection != LinkDirection::LEFT))) {
524 const double angle = fabs(GeomHelper::angleDiff(
525 myLaneBefore->getNormalPredecessorLane()->getShape().angleAt2D(-2),
526 myLane->getShape().angleAt2D(0)));
527 if (angle > 0) {
528 double length = myInternalLaneBefore->getShape().length2D();
529 if (myInternalLaneBefore->getIncomingLanes().size() == 1 &&
530 myInternalLaneBefore->getIncomingLanes()[0].lane->isInternal()) {
531 length += myInternalLaneBefore->getIncomingLanes()[0].lane->getShape().length2D();
532 } else if (myInternalLane != nullptr) {
533 length += myInternalLane->getShape().length2D();
534 }
535 myRadius = length / angle;
536 //std::cout << getDescription() << " a=" << RAD2DEG(angle) << " l=" << length << " r=" << myRadius << "\n";
537 }
538 }
539}
540
541
542void
543MSLink::recheckSetRequestInformation() {
544 for (auto item : myRecheck) {
545#ifdef MSLink_DEBUG_CROSSING_POINTS
546 std::cout << " recheck l1=" << item.first->getDescription() << " l2=" << item.second->getDescription() << "\n";
547#endif
548 MSLink* const link = item.first;
549 MSLink* const foeExitLink = item.second;
550 const MSLane* const lane = link->getInternalLaneBefore();
551 const MSLane* const foeLane = foeExitLink->getInternalLaneBefore();
552 int conflictIndex = -1;
553 for (int i = 0; i < (int)link->myFoeLanes.size(); i++) {
554 if (link->myFoeLanes[i] == foeLane) {
555 conflictIndex = i;
556 break;
557 }
558 }
559 if (conflictIndex == -1) {
560 WRITE_WARNING("Could not recheck ConflictInfo for " + link->getDescription() + " and " + foeExitLink->getDescription() + "\n");
561 continue;
562 }
563 ConflictInfo& ci = link->myConflicts[conflictIndex];
564 if (ci.flag & CONFLICT_SIBLING_CONTINUATION) {
565 const MSLane* const intLane = link->getInternalLaneBefore();
566 const MSLane* const siblingCont = foeExitLink->getInternalLaneBefore();
567 const MSLane* const sibling = siblingCont->getLogicalPredecessorLane();
568 // this is an approximation because intLane and sibling+siblingCont are still close to each other but may have different curvature
569 const double distToDivergence = intLane->getLength() - ci.lengthBehindCrossing;
570 double lbcSibCont = MIN2(siblingCont->getLength(), MAX2(0.0, sibling->getLength() + siblingCont->getLength() - distToDivergence));
571#ifdef MSLink_DEBUG_CROSSING_POINTS
572 std::cout << " siblingContinuation: distToDivergence=" << distToDivergence << " lbcSibCont=" << lbcSibCont << "\n";
573#endif
574 ConflictInfo ci2 = ConflictInfo(lbcSibCont, intLane->getWidth());
575 ci2.foeConflictIndex = conflictIndex;
576 ci.foeConflictIndex = (int)foeExitLink->myConflicts.size();
577 foeExitLink->myFoeLanes.push_back(intLane);
578 foeExitLink->myConflicts.push_back(ci2);
579 continue;
580 }
581
582 std::vector<double> intersections1 = foeLane->getShape().intersectsAtLengths2D(lane->getShape());
583 if (intersections1.size() == 0) {
584#ifdef MSLink_DEBUG_CROSSING_POINTS
585 std::cout << " no intersection\n";
586#endif
587 continue;
588 }
589 const double widthFactor = ci.conflictSize / foeLane->getWidth();
590 const double conflictSize2 = lane->getWidth() * widthFactor;
591 std::sort(intersections1.begin(), intersections1.end());
592 intersections1.back() -= conflictSize2 / 2;
593 intersections1.back() = MAX2(0.0, intersections1.back());
594 ci.foeConflictIndex = (int)foeExitLink->myConflicts.size();
595 foeExitLink->myConflicts.push_back(ConflictInfo(foeLane->getLength() - intersections1.back(), conflictSize2));
596#ifdef MSLink_DEBUG_CROSSING_POINTS
597 std::cout << " ci=" << conflictIndex << " wf=" << widthFactor << " flag=" << ci.flag << " flbc=" << foeExitLink->myConflicts.back().lengthBehindCrossing << "\n";
598#endif
599 }
600 myRecheck.clear();
601}
602
603double
604MSLink::computeDistToDivergence(const MSLane* lane, const MSLane* sibling, double minDist, bool sameSource, double siblingPredLength) const {
605 double lbcSibling = 0;
606 double lbcLane = 0;
607
608 PositionVector l = lane->getShape();
609 PositionVector s = sibling->getShape();
610 double length = l.length2D();
611 double sibLength = s.length2D();
612 if (!sameSource) {
613 l = l.reverse();
614 s = s.reverse();
615 } else if (sibling->getEntryLink()->myAmIndirect) {
616 // ignore final waiting position since it may be quite close to the lane
617 // shape but the waiting position is perpendicular (so the minDist
618 // requirement is not necessary
619 lbcSibling += s[-1].distanceTo2D(s[-2]);
620 s.pop_back();
621 } else if (lane->getEntryLink()->myAmIndirect) {
622 // ignore final waiting position since it may be quite close to the lane
623 // shape but the waiting position is perpendicular (so the minDist
624 // requirement is not necessary
625 lbcLane += l[-1].distanceTo2D(l[-2]);
626 l.pop_back();
627 }
628
629#ifdef MSLink_DEBUG_CROSSING_POINTS_DETAILS
630 std::cout << " sameSource=" << sameSource << " minDist=" << minDist << " backDist=" << l.back().distanceTo2D(s.back()) << "\n";
631#endif
632 if (l.back().distanceTo2D(s.back()) > minDist) {
633 // compute the final divergence point
634 // this position serves two purposes:
635 // 1) once the foe vehicle back (on sibling) has passed this point, we can safely ignore it
636 // 2) both vehicles are put into a cf-relationship while before the point.
637 // Since the actual crossing point is at the start of the junction,
638 // we want to make sure that both vehicles have the same distance to the crossing point and thus follow each other naturally
639 std::vector<double> distances = l.distances(s);
640#ifdef MSLink_DEBUG_CROSSING_POINTS
641 std::cout << " distances=" << toString(distances) << "\n";
642#endif
643 assert(distances.size() == l.size() + s.size());
644 if (distances.back() > minDist && distances[l.size() - 1] > minDist) {
645 // do a pairwise check between lane and sibling to make because we do not know which of them bends more
646 for (int j = (int)s.size() - 2; j >= 0; j--) {
647 const int i = j + (int)l.size();
648 const double segLength = s[j].distanceTo2D(s[j + 1]);
649 if (distances[i] > minDist) {
650 lbcSibling += segLength;
651 } else {
652 // assume no sharp bends and just interpolate the last segment
653 lbcSibling += segLength - (minDist - distances[i]) * segLength / (distances[i + 1] - distances[i]);
654 break;
655 }
656 }
657 for (int i = (int)l.size() - 2; i >= 0; i--) {
658 const double segLength = l[i].distanceTo2D(l[i + 1]);
659 if (distances[i] > minDist) {
660 lbcLane += segLength;
661 } else {
662 // assume no sharp bends and just interpolate the last segment
663 lbcLane += segLength - (minDist - distances[i]) * segLength / (distances[i + 1] - distances[i]);
664 break;
665 }
666 }
667 }
668 assert(lbcSibling >= -NUMERICAL_EPS);
669 assert(lbcLane >= -NUMERICAL_EPS);
670 }
671 const double distToDivergence1 = sibling->getLength() + siblingPredLength - lbcSibling;
672 const double distToDivergence2 = lane->getLength() - lbcLane;
673 const double distToDivergence = MIN3(
674 MAX2(distToDivergence1, distToDivergence2),
675 sibLength, length);
676#ifdef MSLink_DEBUG_CROSSING_POINTS
677 std::cout << " distToDivergence=" << distToDivergence
678 << " distTD1=" << distToDivergence1
679 << " distTD2=" << distToDivergence2
680 << " length=" << length
681 << " sibLength=" << sibLength
682 << "\n";
683#endif
684 return distToDivergence;
685}
686
687
688bool
689MSLink::contIntersect(const MSLane* lane, const MSLane* foe) {
690 if (foe->getLinkCont()[0]->getViaLane() != nullptr) {
691 std::vector<double> intersections = lane->getShape().intersectsAtLengths2D(foe->getShape());
692 return intersections.size() > 0;
693 }
694 return false;
695}
696
697
698void
699MSLink::setApproaching(const SUMOVehicle* approaching, const SUMOTime arrivalTime, const double arrivalSpeed, const double leaveSpeed,
700 const bool setRequest, const double arrivalSpeedBraking, const SUMOTime waitingTime, double dist, double latOffset) {
701 const SUMOTime leaveTime = getLeaveTime(arrivalTime, arrivalSpeed, leaveSpeed, approaching->getVehicleType().getLength());
702#ifdef DEBUG_APPROACHING
703 if (DEBUG_COND2(approaching)) {
704 std::cout << SIMTIME << " link=" << getDescription() << " setApproaching veh=" << approaching->getID();
705 if (myApproachingVehicles.size() > 0) {
706 std::cout << " curApproaching=";
707 for (auto i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
708 std::cout << i->first->getID() << " ";
709 }
710 }
711 std::cout << "\n";
712 }
713#endif
714 myApproachingVehicles.emplace(approaching,
715 ApproachingVehicleInformation(arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, setRequest,
716 arrivalSpeedBraking, waitingTime, dist, approaching->getSpeed(), latOffset));
717}
718
719
720void
721MSLink::setApproaching(const SUMOVehicle* approaching, ApproachingVehicleInformation ai) {
722#ifdef DEBUG_APPROACHING
723 if (DEBUG_COND2(approaching)) {
724 std::cout << SIMTIME << " link=" << getDescription() << " setApproaching veh=" << approaching->getID();
725 if (myApproachingVehicles.size() > 0) {
726 std::cout << " curApproaching=";
727 for (auto i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
728 std::cout << i->first->getID() << " ";
729 }
730 }
731 std::cout << "\n";
732 }
733#endif
734 myApproachingVehicles.emplace(approaching, ai);
735}
736
737void
738MSLink::setApproachingPerson(const MSPerson* approaching, const SUMOTime arrivalTime, const SUMOTime leaveTime) {
739 if (myApproachingPersons == nullptr) {
740 myApproachingPersons = new PersonApproachInfos();
741 }
742 myApproachingPersons->emplace(approaching, ApproachingPersonInformation(arrivalTime, leaveTime));
743}
744
745void
746MSLink::removeApproaching(const SUMOVehicle* veh) {
747#ifdef DEBUG_APPROACHING
748 if (DEBUG_COND2(veh)) {
749 std::cout << SIMTIME << " link=" << getDescription() << " removeApproaching veh=" << veh->getID();
750 if (myApproachingVehicles.size() > 0) {
751 std::cout << " curApproaching=";
752 for (auto i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
753 std::cout << i->first->getID() << " ";
754 }
755 }
756 std::cout << "\n";
757 }
758#endif
759 myApproachingVehicles.erase(veh);
760}
761
762
763void
764MSLink::removeApproachingPerson(const MSPerson* person) {
765 if (myApproachingPersons == nullptr) {
766 WRITE_WARNINGF("Person '%' entered crossing lane '%' without registering approach, time=%", person->getID(), myLane->getID(), time2string(SIMSTEP));
767 return;
768 }
769#ifdef DEBUG_APPROACHING
770 if (DEBUG_COND2(person)) {
771 std::cout << SIMTIME << " Link '" << (myLaneBefore == 0 ? "NULL" : myLaneBefore->getID()) << "'->'" << (myLane == 0 ? "NULL" : myLane->getID()) << std::endl;
772 std::cout << "' Removing approaching person '" << person->getID() << "'\nCurrently registered persons:" << std::endl;
773 for (auto i = myApproachingPersons->begin(); i != myApproachingPersons->end(); ++i) {
774 std::cout << "'" << i->first->getID() << "'" << std::endl;
775 }
776 }
777#endif
778 myApproachingPersons->erase(person);
779}
780
781
782MSLink::ApproachingVehicleInformation
783MSLink::getApproaching(const SUMOVehicle* veh) const {
784 auto i = myApproachingVehicles.find(veh);
785 if (i != myApproachingVehicles.end()) {
786 return i->second;
787 } else {
788 return ApproachingVehicleInformation(INVALID_TIME, INVALID_TIME, 0, 0, false, 0, 0, 0, 0, 0);
789 }
790}
791
792
793const MSLink::ApproachingVehicleInformation*
794MSLink::getApproachingPtr(const SUMOVehicle* veh) const {
795 auto i = myApproachingVehicles.find(veh);
796 if (i != myApproachingVehicles.end()) {
797 return &i->second;
798 } else {
799 return nullptr;
800 }
801}
802
803
804void
808
809
810SUMOTime
811MSLink::getLeaveTime(const SUMOTime arrivalTime, const double arrivalSpeed,
812 const double leaveSpeed, const double vehicleLength) const {
813 return arrivalTime == SUMOTime_MAX ? SUMOTime_MAX : arrivalTime + TIME2STEPS((getLength() + vehicleLength) / MAX2(0.5 * (arrivalSpeed + leaveSpeed), NUMERICAL_EPS));
814}
815
816
817bool
818MSLink::opened(SUMOTime arrivalTime, double arrivalSpeed, double leaveSpeed, double vehicleLength,
819 double impatience, double decel, SUMOTime waitingTime, double posLat,
820 BlockingFoes* collectFoes, bool ignoreRed, const SUMOTrafficObject* ego, double dist) const {
821#ifdef MSLink_DEBUG_OPENED
822 if (gDebugFlag1) {
823 std::cout << SIMTIME << " opened? link=" << getDescription() << " red=" << haveRed() << " cont=" << isCont() << " numFoeLinks=" << myFoeLinks.size() << " havePrio=" << havePriority() << " lastWasContMajorGreen=" << lastWasContState(LINKSTATE_TL_GREEN_MAJOR) << "\n";
824 }
825#endif
826 if (haveRed() && !ignoreRed) {
827 return false;
828 }
829 if (isCont() && MSGlobals::gUsingInternalLanes) {
830 return true;
831 }
832 const SUMOTime leaveTime = getLeaveTime(arrivalTime, arrivalSpeed, leaveSpeed, vehicleLength);
833 if (MSGlobals::gLateralResolution > 0) {
834 // check for foes on the same lane with the same target edge
835 for (const MSLink* foeLink : mySublaneFoeLinks) {
836 assert(myLane != foeLink->getLane());
837 for (const auto& it : foeLink->myApproachingVehicles) {
838 const SUMOVehicle* foe = it.first;
839 if (
840 // there only is a conflict if the paths cross
841 ((posLat < foe->getLateralPositionOnLane() + it.second.latOffset && myLane->getIndex() > foeLink->myLane->getIndex())
842 || (posLat > foe->getLateralPositionOnLane() + it.second.latOffset && myLane->getIndex() < foeLink->myLane->getIndex()))
843 // the vehicle that arrives later must yield
844 && (arrivalTime > it.second.arrivalTime
845 // if both vehicles arrive at the same time, the one
846 // to the left must yield
847 || (arrivalTime == it.second.arrivalTime && posLat > foe->getLateralPositionOnLane()))) {
848 if (blockedByFoe(foe, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, false,
849 impatience, decel, waitingTime, ego)) {
850#ifdef MSLink_DEBUG_OPENED
851 if (gDebugFlag1) {
852 std::cout << SIMTIME << " blocked by " << foe->getID() << " arrival=" << arrivalTime << " foeArrival=" << it.second.arrivalTime << "\n";
853 }
854#endif
855 if (collectFoes == nullptr) {
856#ifdef MSLink_DEBUG_OPENED
857 if (gDebugFlag1) {
858 std::cout << " link=" << getViaLaneOrLane()->getID() << " blocked by sublaneFoe=" << foe->getID() << " foeLink=" << foeLink->getViaLaneOrLane()->getID() << " posLat=" << posLat << "\n";
859 }
860#endif
861 return false;
862 } else {
863 collectFoes->push_back(it.first);
864 }
865 }
866 }
867 }
868 }
869 // check for foes on the same lane with a different target edge
870 // (straight movers take precedence if the paths cross)
871 const int lhSign = MSGlobals::gLefthand ? -1 : 1;
872 for (const MSLink* foeLink : mySublaneFoeLinks2) {
873 assert(myDirection != LinkDirection::STRAIGHT);
874 for (const auto& it : foeLink->myApproachingVehicles) {
875 const SUMOVehicle* foe = it.first;
876 // there only is a conflict if the paths cross
877 // and if the vehicles are not currently in a car-following relationship
878 const double egoWidth = ego == nullptr ? 1.8 : ego->getVehicleType().getWidth();
879 if (!lateralOverlap(posLat, egoWidth, foe->getLateralPositionOnLane() + it.second.latOffset, foe->getVehicleType().getWidth())
880 && (((myDirection == LinkDirection::RIGHT || myDirection == LinkDirection::PARTRIGHT)
881 && (posLat * lhSign > (foe->getLateralPositionOnLane() + it.second.latOffset) * lhSign))
882 || ((myDirection == LinkDirection::LEFT || myDirection == LinkDirection::PARTLEFT)
883 && (posLat * lhSign < (foe->getLateralPositionOnLane() + it.second.latOffset) * lhSign)))) {
884 if (blockedByFoe(foe, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, false,
885 impatience, decel, waitingTime, ego)) {
886#ifdef MSLink_DEBUG_OPENED
887 if (gDebugFlag1) {
888 std::cout << SIMTIME << " blocked by sublane foe " << foe->getID() << " arrival=" << arrivalTime << " foeArrival=" << it.second.arrivalTime << "\n";
889 }
890#endif
891 if (collectFoes == nullptr) {
892#ifdef MSLink_DEBUG_OPENED
893 if (gDebugFlag1) {
894 std::cout << " link=" << getViaLaneOrLane()->getID() << " blocked by sublaneFoe2=" << foe->getID() << " foeLink=" << foeLink->getViaLaneOrLane()->getID() << " posLat=" << posLat << "\n";
895 }
896#endif
897 return false;
898 } else {
899 collectFoes->push_back(it.first);
900 }
901 }
902 }
903 }
904 }
905 }
906 if ((havePriority() || lastWasContState(LINKSTATE_TL_GREEN_MAJOR)) && myState != LINKSTATE_ZIPPER) {
907 // priority usually means the link is open but there are exceptions:
908 // zipper still needs to collect foes
909 // sublane model could have detected a conflict
910 return collectFoes == nullptr || collectFoes->size() == 0;
911 }
912 if (myState == LINKSTATE_ALLWAY_STOP && waitingTime < TIME2STEPS(ego == nullptr ? TS : ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_ALLWAYSTOP_WAIT, TS))) {
913 return false;
914 } else if (myState == LINKSTATE_STOP && waitingTime < TIME2STEPS(ego == nullptr ? TS : ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPSIGN_WAIT, TS))) {
915 return false;
916 }
917
918 const std::vector<MSLink*>& foeLinks = (myOffFoeLinks == nullptr || getCorrespondingEntryLink()->getState() != LINKSTATE_ALLWAY_STOP) ? myFoeLinks : *myOffFoeLinks;
919#ifdef MSLink_DEBUG_OPENED
920 if (gDebugFlag1) {
921 std::cout << SIMTIME << " opened link=" << getViaLaneOrLane()->getID() << " foeLinks=" << foeLinks.size() << "\n";
922 }
923#endif
924
925 if (MSGlobals::gUseMesoSim && impatience == 1 && !myLane->getEdge().isRoundabout()) {
926 return true;
927 }
928 const bool lastWasContRed = lastWasContState(LINKSTATE_TL_RED);
929 for (const MSLink* const link : foeLinks) {
930 if (MSGlobals::gUseMesoSim) {
931 if (link->haveRed()) {
932 continue;
933 }
934 }
935#ifdef MSLink_DEBUG_OPENED
936 if (gDebugFlag1) {
937 std::cout << SIMTIME << " foeLink=" << link->getViaLaneOrLane()->getID() << " numApproaching=" << link->getApproaching().size() << "\n";
938 if (link->getLane()->isCrossing()) {
939 std::cout << SIMTIME << " approachingPersons=" << (link->myApproachingPersons == nullptr ? "NULL" : toString(link->myApproachingPersons->size())) << "\n";
940 }
941 }
942#endif
943 if (link->blockedAtTime(arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, myLane == link->getLane(),
944 impatience, decel, waitingTime, collectFoes, ego, lastWasContRed, dist)) {
945 return false;
946 }
947 }
948 if (collectFoes != nullptr && collectFoes->size() > 0) {
949 return false;
950 }
951 return true;
952}
953
954
955bool
956MSLink::blockedAtTime(SUMOTime arrivalTime, SUMOTime leaveTime, double arrivalSpeed, double leaveSpeed,
957 bool sameTargetLane, double impatience, double decel, SUMOTime waitingTime,
958 BlockingFoes* collectFoes, const SUMOTrafficObject* ego, bool lastWasContRed, double dist) const {
959 for (const auto& it : myApproachingVehicles) {
960#ifdef MSLink_DEBUG_OPENED
961 if (gDebugFlag1) {
962 if (ego != nullptr
963 && ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_SPEED, 0) >= it.second.speed
964 && ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) > 0) {
965 std::stringstream stream; // to reduce output interleaving from different threads
966 stream << SIMTIME << " " << myApproachingVehicles.size() << " foe link=" << getViaLaneOrLane()->getID()
967 << " foeVeh=" << it.first->getID() << " (below ignore speed)"
968 << " ignoreFoeProb=" << ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0)
969 << "\n";
970 std::cout << stream.str();
971 }
972 }
973#endif
974 if (it.first != ego
975 && (ego == nullptr
976 || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) == 0
977 || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_SPEED, 0) < it.second.speed
978 || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) < RandHelper::rand(ego->getRNG()))
979 && !ignoreFoe(ego, it.first)
980 && (!lastWasContRed || it.first->getSpeed() > SUMO_const_haltingSpeed)
981 && blockedByFoe(it.first, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, sameTargetLane,
982 impatience, decel, waitingTime, ego)) {
983 if (collectFoes == nullptr) {
984 return true;
985 } else {
986 collectFoes->push_back(it.first);
987 }
988 }
989 }
990 if (myApproachingPersons != nullptr && !haveRed()) {
991 for (const auto& it : *myApproachingPersons) {
992//#ifdef MSLink_DEBUG_OPENED
993// if (gDebugFlag1) {
994// std::cout << SIMTIME << ": " << ego->getID() << " check person " << it.first->getID() << " aTime=" << arrivalTime << " foeATime=" << it.second.arrivalTime
995// << " lTime=" << leaveTime << " foeLTime=" << it.second.leavingTime
996// << " dist=" << dist << "\n";
997// }
998//#endif
999 if ((ego == nullptr
1000 || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) == 0
1001 || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_SPEED, 0) < it.first->getSpeed()
1002 || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) < RandHelper::rand(ego->getRNG()))
1003 && !ignoreFoe(ego, it.first)
1004 && !((arrivalTime > it.second.leavingTime) || (leaveTime < it.second.arrivalTime))) {
1005 if (ego == nullptr) {
1006 // during insertion
1007 if (myJunction->getType() == SumoXMLNodeType::RAIL_CROSSING) {
1008 continue;
1009 } else {
1010 return true;
1011 }
1012 }
1013 // check whether braking is feasible (ego might have started to accelerate already)
1014 const auto& cfm = ego->getVehicleType().getCarFollowModel();
1015#ifdef MSLink_DEBUG_OPENED
1016 if (gDebugFlag1) {
1017 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";
1018 }
1019#endif
1020 if (dist > cfm.brakeGap(ego->getSpeed(), cfm.getMaxDecel(), 0)) {
1021#ifdef MSLink_DEBUG_OPENED
1022 if (gDebugFlag1) {
1023 std::cout << SIMTIME << ": " << ego->getID() << " blocked by person " << it.first->getID() << "\n";
1024 }
1025#endif
1026 if (collectFoes == nullptr) {
1027 return true;
1028 } else {
1029 collectFoes->push_back(it.first);
1030 }
1031 }
1032 }
1033 }
1034 }
1035 return false;
1036}
1037
1038
1039bool
1040MSLink::blockedByFoe(const SUMOVehicle* veh, const ApproachingVehicleInformation& avi,
1041 SUMOTime arrivalTime, SUMOTime leaveTime, double arrivalSpeed, double leaveSpeed,
1042 bool sameTargetLane, double impatience, double decel, SUMOTime waitingTime,
1043 const SUMOTrafficObject* ego) const {
1044#ifdef MSLink_DEBUG_OPENED
1045 if (gDebugFlag1) {
1046 std::stringstream stream; // to reduce output interleaving from different threads
1047 stream << " link=" << getDescription()
1048 << " foeVeh=" << veh->getID()
1049 << " req=" << avi.willPass
1050 << " aT=" << avi.arrivalTime
1051 << " lT=" << avi.leavingTime
1052 << "\n";
1053 std::cout << stream.str();
1054 }
1055#endif
1056 if (!avi.willPass) {
1057 return false;
1058 }
1059 if (myState == LINKSTATE_ALLWAY_STOP) {
1060 assert(waitingTime > 0);
1061#ifdef MSLink_DEBUG_OPENED
1062 if (gDebugFlag1) {
1063 std::stringstream stream; // to reduce output interleaving from different threads
1064 stream << " foeDist=" << avi.dist
1065 << " foeBGap=" << veh->getBrakeGap(false)
1066 << " foeWait=" << avi.waitingTime
1067 << " wait=" << waitingTime
1068 << "\n";
1069 std::cout << stream.str();
1070 }
1071#endif
1072 // when using actionSteps, the foe waiting time may be outdated
1073 const SUMOTime actionDelta = SIMSTEP - veh->getLastActionTime();
1074 if (waitingTime > avi.waitingTime + actionDelta) {
1075 return false;
1076 }
1077 if (waitingTime == (avi.waitingTime + actionDelta) && arrivalTime < avi.arrivalTime + actionDelta) {
1078 return false;
1079 }
1080 }
1081 SUMOTime foeArrivalTime = avi.arrivalTime;
1082 double foeArrivalSpeedBraking = avi.arrivalSpeedBraking;
1083 if (impatience > 0 && arrivalTime < avi.arrivalTime) {
1084#ifdef MSLink_DEBUG_OPENED
1085 gDebugFlag6 = ((ego == nullptr || ego->isSelected()) && (veh == nullptr || veh->isSelected()));
1086#endif
1087 const SUMOTime fatb = computeFoeArrivalTimeBraking(arrivalTime, veh, avi.arrivalTime, impatience, avi.dist, foeArrivalSpeedBraking);
1088 foeArrivalTime = (SUMOTime)((1. - impatience) * (double)avi.arrivalTime + impatience * (double)fatb);
1089#ifdef MSLink_DEBUG_OPENED
1090 if (gDebugFlag6) {
1091 std::cout << SIMTIME << " link=" << getDescription() << " ego=" << ego->getID() << " foe=" << veh->getID()
1092 << " at=" << STEPS2TIME(arrivalTime)
1093 << " fat=" << STEPS2TIME(avi.arrivalTime)
1094 << " fatb=" << STEPS2TIME(fatb)
1095 << " fat2=" << STEPS2TIME(foeArrivalTime)
1096 << "\n";
1097 }
1098#endif
1099 }
1100
1101
1102 const SUMOTime lookAhead = (myState == LINKSTATE_ZIPPER
1103 ? myLookaheadTimeZipper
1104 : (ego == nullptr
1105 ? myLookaheadTime
1106 : TIME2STEPS(ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_TIMEGAP_MINOR, STEPS2TIME(myLookaheadTime)))));
1107 //if (ego != 0) std::cout << SIMTIME << " ego=" << ego->getID() << " jmTimegapMinor=" << ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_TIMEGAP_MINOR, -1) << " lookAhead=" << lookAhead << "\n";
1108#ifdef MSLink_DEBUG_OPENED
1109 if (gDebugFlag1 || gDebugFlag6) {
1110 std::stringstream stream; // to reduce output interleaving from different threads
1111 stream << " imp=" << impatience << " fAT2=" << foeArrivalTime << " fASb=" << foeArrivalSpeedBraking << " lA=" << lookAhead << " egoAT=" << arrivalTime << " egoLT=" << leaveTime << " egoLS=" << leaveSpeed << "\n";
1112 std::cout << stream.str();
1113 }
1114#endif
1115 if (avi.leavingTime < arrivalTime) {
1116 // ego wants to be follower
1117 if (sameTargetLane && (arrivalTime - avi.leavingTime < lookAhead
1118 || unsafeMergeSpeeds(avi.leaveSpeed, arrivalSpeed,
1119 veh->getVehicleType().getCarFollowModel().getMaxDecel(), decel))) {
1120#ifdef MSLink_DEBUG_OPENED
1121 if (gDebugFlag1 || gDebugFlag6) {
1122 std::cout << " blocked (cannot follow)\n";
1123 }
1124#endif
1125 return true;
1126 }
1127 } else if (foeArrivalTime > leaveTime + lookAhead) {
1128 // ego wants to be leader.
1129 if (sameTargetLane && unsafeMergeSpeeds(leaveSpeed, foeArrivalSpeedBraking,
1130 decel, veh->getVehicleType().getCarFollowModel().getMaxDecel())) {
1131#ifdef MSLink_DEBUG_OPENED
1132 if (gDebugFlag1 || gDebugFlag6) {
1133 std::cout << " blocked (cannot lead)\n";
1134 }
1135#endif
1136 return true;
1137 }
1138 } else {
1139 // even without considering safeHeadwayTime there is already a conflict
1140#ifdef MSLink_DEBUG_OPENED
1141 if (gDebugFlag1 || gDebugFlag6) {
1142 std::cout << " blocked (hard conflict)\n";
1143 }
1144#endif
1145 return true;
1146 }
1147 return false;
1148}
1149
1150
1151SUMOTime
1152MSLink::computeFoeArrivalTimeBraking(SUMOTime arrivalTime, const SUMOVehicle* foe, SUMOTime foeArrivalTime, double impatience, double dist, double& fasb) {
1153 // a: distance saved when foe brakes from arrivalTime to foeArrivalTime
1154 // b: distance driven past foeArrivalTime
1155 // m: permitted decceleration
1156 // d: total deceleration until foeArrivalTime
1157 // dist2: distance of foe at arrivalTime
1158 // actual arrivalTime must fall on a simulation step
1159 if (arrivalTime - arrivalTime % DELTA_T == foeArrivalTime - foeArrivalTime % DELTA_T) {
1160 // foe enters the junction in the same step
1161#ifdef MSLink_DEBUG_OPENED
1162 if (gDebugFlag6) {
1163 std::cout << " foeAT before egoAT\n";
1164 }
1165#endif
1166 return foeArrivalTime;
1167 }
1168 if (arrivalTime % DELTA_T > 0) {
1169 arrivalTime = arrivalTime - (arrivalTime % DELTA_T) + DELTA_T;
1170 }
1171 //arrivalTime += DELTA_T - arrivalTime % DELTA_T;
1172 const double m = foe->getVehicleType().getCarFollowModel().getMaxDecel() * impatience;
1173 const double dt = STEPS2TIME(foeArrivalTime - arrivalTime);
1174 const double d = dt * m;
1175 const double a = dt * d / 2;
1176 const double v = dist / STEPS2TIME(foeArrivalTime - SIMSTEP + DELTA_T);
1177 const double dist2 = dist - v * STEPS2TIME(arrivalTime - SIMSTEP);
1178#ifdef MSLink_DEBUG_OPENED
1179 if (gDebugFlag6) {
1180 std::cout << " dist=" << dist << " dist2=" << dist2
1181 << " at=" << STEPS2TIME(arrivalTime)
1182 << " fat=" << STEPS2TIME(foeArrivalTime)
1183 << " dt=" << dt << " v=" << v << " m=" << m << " d=" << d << " a=" << a << "\n";
1184 }
1185#endif
1186 if (0.5 * v * v / m <= dist2) {
1187#ifdef MSLink_DEBUG_OPENED
1188 if (gDebugFlag6) {
1189 std::cout << " canBrakeToStop\n";
1190 }
1191#endif
1192 fasb = 0;
1193 return foeArrivalTime + TIME2STEPS(30);
1194 }
1195 // a = b (foe reaches the original distance to the stop line)
1196 // x: time driven past foeArrivalTime
1197 // v: foe speed without braking
1198 // v2: average foe speed after foeArrivalTime (braking continues for time x)
1199 // v2 = (v - d - x * m / 2)
1200 // b = v2 * x
1201 // solving for x gives:
1202 const double x = (sqrt(4 * (v - d) * (v - d) - 8 * m * a) * -0.5 - d + v) / m;
1203
1204#ifdef MSLink_DEBUG_OPENED
1205 const double x2 = (sqrt(4 * (v - d) * (v - d) - 8 * m * a) * 0.5 - d + v) / m;
1206 if (gDebugFlag6 || std::isnan(x)) {
1207 std::cout << SIMTIME << " dist=" << dist << " dist2=" << dist2 << " at=" << STEPS2TIME(arrivalTime) << " m=" << m << " d=" << d << " v=" << v << " a=" << a << " x=" << x << " x2=" << x2 << "\n";
1208 }
1209#endif
1210 fasb = v - (dt + x) * m;
1211 return foeArrivalTime + TIME2STEPS(x);
1212}
1213
1214
1215bool
1216MSLink::hasApproachingFoe(SUMOTime arrivalTime, SUMOTime leaveTime, double speed, double decel) const {
1217 for (const MSLink* const link : myFoeLinks) {
1218 if (link->blockedAtTime(arrivalTime, leaveTime, speed, speed, myLane == link->getLane(), 0, decel, 0)) {
1219 return true;
1220 }
1221 }
1222 for (const MSLane* const lane : myFoeLanes) {
1223 if (lane->getVehicleNumberWithPartials() > 0) {
1224 return true;
1225 }
1226 }
1227 return false;
1228}
1229
1230
1231std::pair<const SUMOVehicle*, const MSLink*>
1232MSLink::getFirstApproachingFoe(const MSLink* wrapAround) const {
1233 double closetDist = std::numeric_limits<double>::max();
1234 const SUMOVehicle* closest = nullptr;
1235 const MSLink* foeLink = nullptr;
1236 for (MSLink* link : myFoeLinks) {
1237 for (const auto& it : link->myApproachingVehicles) {
1238 //std::cout << " link=" << getDescription() << " foeLink_in=" << link->getLaneBefore()->getID() << " wrapAround=" << wrapAround->getDescription() << "\n";
1239 if (link->getLaneBefore() == wrapAround->getLaneBefore()) {
1240 return std::make_pair(nullptr, wrapAround);
1241 } else if (it.second.dist < closetDist) {
1242 closetDist = it.second.dist;
1243 if (it.second.willPass) {
1244 closest = it.first;
1245 foeLink = link;
1246 }
1247 }
1248 }
1249 }
1250 return std::make_pair(closest, foeLink);
1251}
1252
1253
1254void
1255MSLink::setTLState(LinkState state, SUMOTime t) {
1256 if (myState != state) {
1257 myLastStateChange = t;
1258 }
1259 myState = state;
1260 if (haveGreen()) {
1261 myLastGreenState = myState;
1262 }
1263}
1264
1265
1266void
1267MSLink::setTLLogic(const MSTrafficLightLogic* logic) {
1268 myLogic = logic;
1269}
1270
1271
1272bool
1273MSLink::isCont() const {
1274 // when a traffic light is switched off minor roads have their cont status revoked
1275 return (myState == LINKSTATE_TL_OFF_BLINKING || myState == LINKSTATE_ALLWAY_STOP || myState == LINKSTATE_STOP) ? myAmContOff : myAmCont;
1276}
1277
1278
1279bool
1280MSLink::lastWasContMajor() const {
1281 if (myInternalLane == nullptr || myAmCont) {
1282 return false;
1283 } else {
1284 MSLane* pred = myInternalLane->getLogicalPredecessorLane();
1285 if (!pred->getEdge().isInternal()) {
1286 return false;
1287 } else {
1288 const MSLane* const pred2 = pred->getLogicalPredecessorLane();
1289 assert(pred2 != nullptr);
1290 const MSLink* const predLink = pred2->getLinkTo(pred);
1291 assert(predLink != nullptr);
1292 if (predLink->havePriority()) {
1293 return true;
1294 }
1295 if (myHavePedestrianCrossingFoe) {
1296 return predLink->getLastGreenState() == LINKSTATE_TL_GREEN_MAJOR;
1297 } else {
1298 return predLink->haveYellow();
1299 }
1300 }
1301 }
1302}
1303
1304
1305bool
1306MSLink::lastWasContState(LinkState linkState) const {
1307 if (myInternalLane == nullptr || myAmCont || myHavePedestrianCrossingFoe) {
1308 return false;
1309 } else {
1310 MSLane* pred = myInternalLane->getLogicalPredecessorLane();
1311 if (!pred->getEdge().isInternal()) {
1312 return false;
1313 } else {
1314 const MSLane* const pred2 = pred->getLogicalPredecessorLane();
1315 assert(pred2 != nullptr);
1316 const MSLink* const predLink = pred2->getLinkTo(pred);
1317 assert(predLink != nullptr);
1318 return predLink->getState() == linkState;
1319 }
1320 }
1321}
1322
1323
1324void
1325MSLink::writeApproaching(OutputDevice& od, const std::string fromLaneID) const {
1326 if (myApproachingVehicles.size() > 0) {
1327 od.openTag("link");
1328 od.writeAttr(SUMO_ATTR_FROM, fromLaneID);
1329 const std::string via = getViaLane() == nullptr ? "" : getViaLane()->getID();
1330 od.writeAttr(SUMO_ATTR_VIA, via);
1331 od.writeAttr(SUMO_ATTR_TO, getLane() == nullptr ? "" : getLane()->getID());
1332 std::vector<std::pair<SUMOTime, const SUMOVehicle*> > toSort; // stabilize output
1333 for (auto it : myApproachingVehicles) {
1334 toSort.push_back(std::make_pair(it.second.arrivalTime, it.first));
1335 }
1336 std::sort(toSort.begin(), toSort.end());
1337 for (std::vector<std::pair<SUMOTime, const SUMOVehicle*> >::const_iterator it = toSort.begin(); it != toSort.end(); ++it) {
1338 od.openTag("approaching");
1339 const ApproachingVehicleInformation& avi = myApproachingVehicles.find(it->second)->second;
1340 od.writeAttr(SUMO_ATTR_ID, it->second->getID());
1341 od.writeAttr(SUMO_ATTR_IMPATIENCE, it->second->getImpatience());
1342 od.writeAttr("arrivalTime", time2string(avi.arrivalTime));
1343 od.writeAttr("leaveTime", time2string(avi.leavingTime));
1344 od.writeAttr("arrivalSpeed", toString(avi.arrivalSpeed));
1345 od.writeAttr("arrivalSpeedBraking", toString(avi.arrivalSpeedBraking));
1346 od.writeAttr("leaveSpeed", toString(avi.leaveSpeed));
1347 od.writeAttr("willPass", toString(avi.willPass));
1348 od.closeTag();
1349 }
1350 od.closeTag();
1351 }
1352}
1353
1354
1355double
1356MSLink::getInternalLengthsAfter() const {
1357 double len = 0.;
1358 MSLane* lane = myInternalLane;
1359
1360 while (lane != nullptr && lane->isInternal()) {
1361 len += lane->getLength();
1362 lane = lane->getLinkCont()[0]->getViaLane();
1363 }
1364 return len;
1365}
1366
1367double
1368MSLink::getInternalLengthsBefore() const {
1369 double len = 0.;
1370 const MSLane* lane = myInternalLane;
1371
1372 while (lane != nullptr && lane->isInternal()) {
1373 len += lane->getLength();
1374 if (lane->getIncomingLanes().size() == 1) {
1375 lane = lane->getIncomingLanes()[0].lane;
1376 } else {
1377 break;
1378 }
1379 }
1380 return len;
1381}
1382
1383
1384double
1385MSLink::getLengthsBeforeCrossing(const MSLane* foeLane) const {
1386 MSLane* via = myInternalLane;
1387 double totalDist = 0.;
1388 bool foundCrossing = false;
1389 while (via != nullptr) {
1390 MSLink* link = via->getLinkCont()[0];
1391 double dist = link->getLengthBeforeCrossing(foeLane);
1392 if (dist != INVALID_DOUBLE) {
1393 // found conflicting lane
1394 totalDist += dist;
1395 foundCrossing = true;
1396 break;
1397 } else {
1398 totalDist += via->getLength();
1399 via = link->getViaLane();
1400 }
1401 }
1402 if (foundCrossing) {
1403 return totalDist;
1404 } else {
1405 return INVALID_DOUBLE;
1406 }
1407}
1408
1409
1410double
1411MSLink::getLengthBeforeCrossing(const MSLane* foeLane) const {
1412 int foe_ix;
1413 for (foe_ix = 0; foe_ix != (int)myFoeLanes.size(); ++foe_ix) {
1414 if (myFoeLanes[foe_ix] == foeLane) {
1415 break;
1416 }
1417 }
1418 if (foe_ix == (int)myFoeLanes.size()) {
1419 // no conflict with the given lane, indicate by returning -1
1420#ifdef MSLink_DEBUG_CROSSING_POINTS
1421 std::cout << "No crossing of lanes '" << foeLane->getID() << "' and '" << myInternalLaneBefore->getID() << "'" << std::endl;
1422#endif
1423 return INVALID_DOUBLE;
1424 } else {
1425 // found conflicting lane index
1426 double dist = myInternalLaneBefore->getLength() - myConflicts[foe_ix].getLengthBehindCrossing(this);
1427 if (dist == -10000.) {
1428 // this is the value in myConflicts, if the relation allows intersection but none is present for the actual geometry.
1429 return INVALID_DOUBLE;
1430 }
1431#ifdef MSLink_DEBUG_CROSSING_POINTS
1432 std::cout << "Crossing of lanes '" << myInternalLaneBefore->getID() << "' and '" << foeLane->getID()
1433 << "' at distance " << dist << " (approach along '"
1434 << myInternalLaneBefore->getEntryLink()->getLaneBefore()->getID() << "')" << std::endl;
1435#endif
1436 return dist;
1437 }
1438}
1439
1440
1441bool
1442MSLink::isEntryLink() const {
1443 if (MSGlobals::gUsingInternalLanes) {
1444 return myInternalLane != nullptr && myInternalLaneBefore == nullptr;
1445 } else {
1446 return false;
1447 }
1448}
1449
1450bool
1451MSLink::isConflictEntryLink() const {
1452 // either a non-cont entry link or the link after a cont-link
1453 return !myAmCont && (isEntryLink() || (myInternalLaneBefore != nullptr && myInternalLane != nullptr));
1454}
1455
1456bool
1457MSLink::isExitLink() const {
1458 if (MSGlobals::gUsingInternalLanes) {
1459 return myInternalLaneBefore != nullptr && myInternalLane == nullptr;
1460 } else {
1461 return false;
1462 }
1463}
1464
1465bool
1466MSLink::isExitLinkAfterInternalJunction() const {
1467 if (MSGlobals::gUsingInternalLanes) {
1468 return (getInternalLaneBefore() != nullptr
1469 && myInternalLaneBefore->getIncomingLanes().size() == 1
1470 && myInternalLaneBefore->getIncomingLanes().front().viaLink->isInternalJunctionLink());
1471 } else {
1472 return false;
1473 }
1474}
1475
1476
1477const MSLink*
1478MSLink::getCorrespondingExitLink() const {
1479 MSLane* lane = myInternalLane;
1480 const MSLink* link = this;
1481 while (lane != nullptr) {
1482 link = lane->getLinkCont()[0];
1483 lane = link->getViaLane();
1484 }
1485 return link;
1486}
1487
1488
1489const MSLink*
1490MSLink::getCorrespondingEntryLink() const {
1491 const MSLink* link = this;
1492 while (link->myLaneBefore->isInternal()) {
1493 assert(myLaneBefore->getIncomingLanes().size() == 1);
1494 link = link->myLaneBefore->getIncomingLanes().front().viaLink;
1495 }
1496 return link;
1497}
1498
1499
1500bool
1501MSLink::isInternalJunctionLink() const {
1502 return getInternalLaneBefore() != nullptr && myInternalLane != nullptr;
1503}
1504
1505
1506const MSLink::LinkLeaders
1507MSLink::getLeaderInfo(const MSVehicle* ego, double dist, std::vector<const MSPerson*>* collectBlockers, bool isShadowLink) const {
1508 LinkLeaders result;
1509 // this link needs to start at an internal lane (either an exit link or between two internal lanes)
1510 // or it must be queried by the pedestrian model (ego == 0)
1511 if (ego != nullptr && (!fromInternalLane() || ego->getLaneChangeModel().isOpposite())) {
1512 // ignore link leaders
1513 return result;
1514 }
1515 //gDebugFlag1 = true;
1516 if (gDebugFlag1) {
1517 std::cout << SIMTIME << " getLeaderInfo link=" << getDescription() << " dist=" << dist << " isShadowLink=" << isShadowLink << "\n";
1518 }
1519 if (MSGlobals::gComputeLC && ego != nullptr && ego->getLane()->isNormal()) {
1520 const MSLink* junctionEntry = getLaneBefore()->getEntryLink();
1521 if (junctionEntry->haveRed() && !ego->ignoreRed(junctionEntry, true)
1522 // check oncoming on bidiLane during laneChanging
1523 && (!MSGlobals::gComputeLC || junctionEntry->getLaneBefore()->getBidiLane() == nullptr)) {
1524 if (gDebugFlag1) {
1525 std::cout << " ignore linkLeaders beyond red light\n";
1526 }
1527 return result;
1528 }
1529 }
1530 // this is an exit link
1531 const double extraGap = ego != nullptr ? ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_EXTRA_GAP, 0) : 0;
1532 for (int i = 0; i < (int)myFoeLanes.size(); ++i) {
1533 const MSLane* foeLane = myFoeLanes[i];
1534 const MSLink* foeExitLink = foeLane->getLinkCont()[0];
1535 // distance from the querying vehicle to the crossing point with foeLane
1536 double distToCrossing = dist - myConflicts[i].getLengthBehindCrossing(this);
1537 const double foeDistToCrossing = foeLane->getLength() - myConflicts[i].getFoeLengthBehindCrossing(foeExitLink);
1538 const bool sameTarget = (myLane == foeExitLink->getLane()) && !isInternalJunctionLink() && !foeExitLink->isInternalJunctionLink();
1539 const bool sameSource = (myInternalLaneBefore != nullptr && myInternalLaneBefore->getNormalPredecessorLane() == foeLane->getNormalPredecessorLane());
1540 const double crossingWidth = (sameTarget || sameSource) ? 0 : myConflicts[i].conflictSize;
1541 const double foeCrossingWidth = (sameTarget || sameSource) ? 0 : myConflicts[i].getFoeConflictSize(foeExitLink);
1542 // special treatment of contLane foe only applies if this lane is not a contLane or contLane follower itself
1543 const bool contLane = (foeExitLink->getViaLaneOrLane()->getEdge().isInternal() && !(
1544 isInternalJunctionLink() || isExitLinkAfterInternalJunction()));
1545 if (gDebugFlag1) {
1546 std::cout << " distToCrossing=" << distToCrossing << " foeLane=" << foeLane->getID() << " cWidth=" << crossingWidth
1547 << " ijl=" << isInternalJunctionLink() << " sT=" << sameTarget << " sS=" << sameSource
1548 << " lbc=" << myConflicts[i].getLengthBehindCrossing(this)
1549 << " flbc=" << myConflicts[i].getFoeLengthBehindCrossing(foeExitLink)
1550 << " cw=" << crossingWidth
1551 << " fcw=" << foeCrossingWidth
1552 << " contLane=" << contLane
1553 << " state=" << toString(myState)
1554 << " foeState=" << toString(foeExitLink->getState())
1555 << "\n";
1556 }
1557 if (distToCrossing + crossingWidth < 0 && !sameTarget
1558 && (ego == nullptr || !MSGlobals::gComputeLC || distToCrossing + crossingWidth + ego->getVehicleType().getLength() < 0)) {
1559 continue; // vehicle is behind the crossing point, continue with next foe lane
1560 }
1561 bool ignoreGreenCont = false;
1562 bool foeIndirect = false;
1563 if (contLane) {
1564 const MSLink* entry = getLaneBefore()->getEntryLink();
1565 const MSLink* foeEntry = foeLane->getEntryLink();
1566 foeIndirect = foeEntry->myAmIndirect;
1567 if (entry != nullptr && entry->haveGreen()
1568 && foeEntry != nullptr && foeEntry->haveGreen()
1569 && entry->myLaneBefore != foeEntry->myLaneBefore) {
1570 // ignore vehicles before an internaljunction as long as they are still in green minor mode
1571 ignoreGreenCont = true;
1572 }
1573 }
1574 if (foeIndirect && distToCrossing >= NO_INTERSECTION) {
1575 if (gDebugFlag1) {
1576 std::cout << " ignore:noIntersection\n";
1577 }
1578 continue;
1579 }
1580 // it is not sufficient to return the last vehicle on the foeLane because ego might be its leader
1581 // therefore we return all vehicles on the lane
1582 //
1583 // special care must be taken for continuation lanes. (next lane is also internal)
1584 // vehicles on cont. lanes or on internal lanes with the same target as this link can not be ignored
1585 // and should block (gap = -1) unless they are part of an indirect turn
1586 MSLane::AnyVehicleIterator end = foeLane->anyVehiclesEnd();
1587 for (MSLane::AnyVehicleIterator it_veh = foeLane->anyVehiclesBegin(); it_veh != end; ++it_veh) {
1588 MSVehicle* leader = (MSVehicle*)*it_veh;
1589 const double leaderBack = leader->getBackPositionOnLane(foeLane) - extraGap;
1590 const double leaderBackDist = foeDistToCrossing - leaderBack;
1591 const double l2 = ego != nullptr ? ego->getLength() + 2 : 0; // add some slack to account for further meeting-angle effects
1592 const double sagitta = ego != nullptr && myRadius != std::numeric_limits<double>::max() ? myRadius - sqrt(myRadius * myRadius - 0.25 * l2 * l2) : 0;
1593 const bool pastTheCrossingPoint = leaderBackDist + foeCrossingWidth + sagitta < 0;
1594 const bool enteredTheCrossingPoint = leaderBackDist < leader->getVehicleType().getLength();
1595 const bool foeIsBicycleTurn = (leader->getVehicleType().getVehicleClass() == SVC_BICYCLE
1596 && foeLane->getIncomingLanes().front().viaLink->getDirection() == LinkDirection::LEFT);
1597 const bool ignoreIndirectBicycleTurn = pastTheCrossingPoint && foeIsBicycleTurn;
1598 const bool cannotIgnore = ((contLane && !ignoreIndirectBicycleTurn) || sameTarget || (sameSource && !MSGlobals::gComputeLC)) && ego != nullptr;
1599 const bool inTheWay = ((((!pastTheCrossingPoint && distToCrossing > 0) || (sameTarget && distToCrossing > leaderBackDist - leader->getLength()))
1600 && enteredTheCrossingPoint
1601 && (!foeExitLink->isInternalJunctionLink() || foeIsBicycleTurn))
1602 || foeExitLink->getLaneBefore()->getNormalPredecessorLane() == myLane->getBidiLane());
1603 const bool isOpposite = leader->getLaneChangeModel().isOpposite();
1604 const auto avi = foeExitLink->getApproaching(leader);
1605 // if leader is not found, assume that it performed a lane change in the last step
1606 const bool willPass = avi.willPass || (avi.arrivalTime == INVALID_TIME && sameTarget);
1607 if (gDebugFlag1) {
1608 std::cout << " candidate leader=" << leader->getID()
1609 << " cannotIgnore=" << cannotIgnore
1610 << " fdtc=" << foeDistToCrossing
1611 << " lb=" << leaderBack
1612 << " lbd=" << leaderBackDist
1613 << " fcwidth=" << foeCrossingWidth
1614 << " r=" << myRadius
1615 << " sagitta=" << sagitta
1616 << " foePastCP=" << pastTheCrossingPoint
1617 << " foeEnteredCP=" << enteredTheCrossingPoint
1618 << " inTheWay=" << inTheWay
1619 << " willPass=" << willPass
1620 << " isFrontOnLane=" << leader->isFrontOnLane(foeLane)
1621 << " ignoreGreenCont=" << ignoreGreenCont
1622 << " foeIndirect=" << foeIndirect
1623 << " foeBikeTurn=" << foeIsBicycleTurn
1624 << " isOpposite=" << isOpposite << "\n";
1625 }
1626 if (leader == ego) {
1627 continue;
1628 }
1629 // ignore greenCont foe vehicles that are not in the way
1630 if (!inTheWay && ignoreGreenCont) {
1631 if (gDebugFlag1) {
1632 std::cout << " ignoreGreenCont\n";
1633 }
1634 continue;
1635 }
1636 // after entering the conflict area, ignore foe vehicles that are not in the way
1637 if ((!MSGlobals::gComputeLC || (ego != nullptr && ego->getLane() == foeLane) || MSGlobals::gSublane)
1638 && distToCrossing < -POSITION_EPS && !inTheWay
1639 && (ego == nullptr || !MSGlobals::gComputeLC || distToCrossing < -ego->getVehicleType().getLength())) {
1640 if (gDebugFlag1) {
1641 std::cout << " ego entered conflict area\n";
1642 }
1643 continue;
1644 }
1645 if (!MSGlobals::gComputeLC
1646 && sameSource
1647 && &ego->getLane()->getEdge() == &myInternalLaneBefore->getEdge()
1648 && leaderBack + leader->getLength() < ego->getPositionOnLane() - ego->getLength()) {
1649 // ego is already on the junction and clearly ahead of foe
1650 if (gDebugFlag1) {
1651 std::cout << " ego ahead of same-source foe\n";
1652 }
1653 continue;
1654 }
1655
1656 // ignore foe vehicles that will not pass
1657 if ((!cannotIgnore || leader->isStopped() || sameTarget)
1658 && !willPass
1659 && (avi.arrivalTime == INVALID_TIME || leader->getSpeed() < SUMO_const_haltingSpeed)
1660 && leader->isFrontOnLane(foeLane)
1661 && !isOpposite
1662 && !inTheWay
1663 // willPass is false if the vehicle is already on the stopping edge
1664 && !leader->willStop()) {
1665 if (gDebugFlag1) {
1666 std::cout << " foe will not pass\n";
1667 }
1668 continue;
1669 }
1670 if (leader->isBidiOn(foeLane)) {
1671 // conflict resolved via forward lane of the foe
1672 continue;
1673 }
1674 // check whether foe is blocked and might need to change before leaving the junction
1675 const bool foeStrategicBlocked = (leader->getLaneChangeModel().isStrategicBlocked() &&
1676 leader->getCarFollowModel().brakeGap(leader->getSpeed()) <= foeLane->getLength() - leaderBack);
1677 const bool sameInternalEdge = &myInternalLaneBefore->getEdge() == &foeExitLink->getInternalLaneBefore()->getEdge();
1678
1679 const bool foeLaneIsBidi = myInternalLaneBefore->getBidiLane() == foeLane;
1680 if (MSGlobals::gSublane && ego != nullptr && (sameSource || sameTarget || foeLaneIsBidi)
1681 && (!foeStrategicBlocked || sameInternalEdge)) {
1682 if (ego->getLane() == leader->getLane()) {
1683 continue;
1684 }
1685 // ignore vehicles if not in conflict sublane-wise
1686 const double egoLatOffset = isShadowLink ? ego->getLatOffset(ego->getLaneChangeModel().getShadowLane()) : 0;
1687 const double posLat = ego->getLateralPositionOnLane() + egoLatOffset;
1688 double posLatLeader = leader->getLateralPositionOnLane() + leader->getLatOffset(foeLane);
1689 if (foeLaneIsBidi) {
1690 // leader is oncoming
1691 posLatLeader = foeLane->getWidth() - posLatLeader;
1692 }
1693 const double latGap = (fabs(posLat - posLatLeader)
1694 - 0.5 * (ego->getVehicleType().getWidth() + leader->getVehicleType().getWidth()));
1695 const double maneuverDist = leader->getLaneChangeModel().getManeuverDist() * (posLat < posLatLeader ? -1 : 1);
1696 if (gDebugFlag1) {
1697 std::cout << " checkIgnore sublaneFoe lane=" << myInternalLaneBefore->getID()
1698 << " sameSource=" << sameSource
1699 << " sameTarget=" << sameTarget
1700 << " foeLaneIsBidi=" << foeLaneIsBidi
1701 << " foeLane=" << foeLane->getID()
1702 << " leader=" << leader->getID()
1703 << " egoLane=" << ego->getLane()->getID()
1704 << " leaderLane=" << leader->getLane()->getID()
1705 << " egoLat=" << posLat
1706 << " egoLatOffset=" << egoLatOffset
1707 << " leaderLat=" << posLatLeader
1708 << " leaderLatOffset=" << leader->getLatOffset(foeLane)
1709 << " latGap=" << latGap
1710 << " maneuverDist=" << maneuverDist
1711 << " computeLC=" << MSGlobals::gComputeLC
1712 << " egoMaxSpeedLat=" << ego->getVehicleType().getMaxSpeedLat()
1713 << "\n";
1714 }
1715 if (latGap > 0 && (latGap > maneuverDist || !sameTarget || !MSGlobals::gComputeLC)
1716 // do not perform sublane changes that interfere with the leader vehicle
1717 && (!MSGlobals::gComputeLC || latGap > ego->getVehicleType().getMaxSpeedLat())) {
1718 const MSLink* foeEntryLink = foeLane->getIncomingLanes().front().viaLink;
1719 if (sameSource) {
1720 // for lanes from the same edge, higer index implies a
1721 // connection further to the left
1722 const bool leaderFromRight = (myIndex > foeEntryLink->getIndex());
1723 if ((posLat > posLatLeader) == leaderFromRight) {
1724 // ignore speed since lanes diverge
1725 if (gDebugFlag1) {
1726 std::cout << " ignored (same source) leaderFromRight=" << leaderFromRight << "\n";
1727 }
1728 continue;
1729 }
1730 } else if (sameTarget) {
1731 // for lanes from different edges we cannot rely on the
1732 // index due to wrap-around issues
1733 if (myDirection != foeEntryLink->getDirection()) {
1734 bool leaderFromRight = foeEntryLink->getDirection() < myDirection;
1735 // leader vehicle should not move towards ego
1736 if (MSGlobals::gLefthand) {
1737 leaderFromRight = !leaderFromRight;
1738 }
1739 if ((posLat > posLatLeader) == leaderFromRight
1740 // leader should keep lateral position or move away from ego
1741 && (leader->getLaneChangeModel().getSpeedLat() == 0
1742 || leaderFromRight == (leader->getLaneChangeModel().getSpeedLat() < latGap))
1743 && (ego->getLaneChangeModel().getSpeedLat() == 0
1744 || leaderFromRight == (ego->getLaneChangeModel().getSpeedLat() > latGap))) {
1745 if (gDebugFlag1) {
1746 std::cout << " ignored (different source) leaderFromRight=" << leaderFromRight << "\n";
1747 }
1748 continue;
1749 }
1750 } else {
1751 // XXX figure out relative direction somehow
1752 }
1753 } else {
1754 if (gDebugFlag1) {
1755 std::cout << " ignored oncoming bidi leader\n";
1756 }
1757 continue;
1758 }
1759 }
1760 }
1761 if (leader->getWaitingTime() < MSGlobals::gIgnoreJunctionBlocker) {
1762 // compute distance between vehicles on the superimposition of both lanes
1763 // where the crossing point is the common point
1764 double gap;
1765 bool fromLeft = true;
1766 if (ego == nullptr) {
1767 // request from pedestrian model. return distance between leaderBack and crossing point
1768 //std::cout << " foeLane=" << foeLane->getID() << " leaderBack=" << leaderBack << " foeDistToCrossing=" << foeDistToCrossing << " foeLength=" << foeLane->getLength() << " foebehind=" << myConflicts[i].second << " dist=" << dist << " behind=" << myConflicts[i].first << "\n";
1769 gap = leaderBackDist;
1770 // distToCrossing should not take into account the with of the foe lane
1771 // (which was subtracted in setRequestInformation)
1772 // Instead, the width of the foe vehicle is used directly by the caller.
1773 distToCrossing += myConflicts[i].conflictSize / 2;
1774 if (gap + foeCrossingWidth < 0) {
1775 // leader is completely past the crossing point
1776 // or there is no crossing point
1777 continue; // next vehicle
1778 }
1779 // we need to determine whether the vehicle passes the
1780 // crossing from the left or the right (heuristic)
1781 fromLeft = foeDistToCrossing > 0.5 * foeLane->getLength();
1782 } else if ((contLane && !sameSource && !ignoreIndirectBicycleTurn) || isOpposite) {
1783 gap = -std::numeric_limits<double>::max(); // always break for vehicles which are on a continuation lane or for opposite-direction vehicles
1784 } else {
1785 if (pastTheCrossingPoint && !sameTarget) {
1786 // leader is completely past the crossing point
1787 // or there is no crossing point
1788 if (gDebugFlag1) {
1789 std::cout << " foePastCP ignored\n";
1790 }
1791 continue;
1792 }
1793 double leaderBackDist2 = leaderBackDist;
1794 if (sameTarget && leaderBackDist2 < 0) {
1795 const double mismatch = myConflicts[i].getFoeLengthBehindCrossing(foeExitLink) - myConflicts[i].getLengthBehindCrossing(this);
1796 if (mismatch > 0) {
1797 leaderBackDist2 += mismatch;
1798 }
1799 }
1800 if (gDebugFlag1) {
1801 std::cout << " distToCrossing=" << distToCrossing << " leaderBack=" << leaderBack
1802 << " backDist=" << leaderBackDist
1803 << " backDist2=" << leaderBackDist2
1804 << " blockedStrategic=" << leader->getLaneChangeModel().isStrategicBlocked()
1805 << "\n";
1806 }
1807 gap = distToCrossing - ego->getVehicleType().getMinGap() - leaderBackDist2 - foeCrossingWidth;
1808 }
1809 // if the foe is already moving off the intersection, we may
1810 // advance up to the crossing point unless we have the same target or same source
1811 // (for sameSource, the crossing point indicates the point of divergence)
1812 const bool stopAsap = ((leader->isFrontOnLane(foeLane) ? cannotIgnore : (sameTarget || sameSource))
1813 || (ego != nullptr && ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_ADVANCE, 1.0) == 0.0));
1814 if (gDebugFlag1) {
1815 std::cout << " leader=" << leader->getID() << " contLane=" << contLane << " cannotIgnore=" << cannotIgnore << " stopAsap=" << stopAsap << " gap=" << gap << "\n";
1816 }
1817 if (ignoreFoe(ego, leader)) {
1818 continue;
1819 }
1820 const int llFlags = ((fromLeft ? LL_FROM_LEFT : 0) |
1821 (inTheWay ? LL_IN_THE_WAY : 0) |
1822 (sameSource ? LL_SAME_SOURCE : 0) |
1823 (sameTarget ? LL_SAME_TARGET : 0));
1824 result.emplace_back(leader, gap, stopAsap ? -1 : distToCrossing, llFlags, leader->getLatOffset(foeLane));
1825 }
1826
1827 }
1828 if (ego != nullptr && MSNet::getInstance()->hasPersons()) {
1829 // check for crossing pedestrians (keep driving if already on top of the crossing
1830 const double distToPeds = distToCrossing - ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPLINE_CROSSING_GAP, MSPModel::SAFETY_GAP);
1831 const double vehWidth = ego->getVehicleType().getWidth() + MSPModel::SAFETY_GAP; // + configurable safety gap
1833 // @check lefthand?!
1834 const bool wayIn = myConflicts[i].lengthBehindCrossing < myLaneBefore->getLength() * 0.5;
1835 const double vehCenter = (foeDistToCrossing + myLaneBefore->getWidth() * 0.5
1836 + ego->getLateralPositionOnLane() * (wayIn ? -1 : 1));
1837 // can access the movement model here since we already checked for existing persons above
1838 if (distToPeds >= -MSPModel::SAFETY_GAP && MSNet::getInstance()->getPersonControl().getMovementModel()->blockedAtDist(ego, foeLane, vehCenter, vehWidth,
1839 ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_CROSSING_GAP, JM_CROSSING_GAP_DEFAULT),
1840 collectBlockers)) {
1841 result.emplace_back(nullptr, -1, distToPeds);
1842 } else if (foeLane->isCrossing() && ego->getLane()->isInternal() && ego->getLane()->getEdge().getToJunction() == myJunction) {
1843 const MSLink* crossingLink = foeLane->getIncomingLanes()[0].viaLink;
1844 if (distToCrossing > 0 && crossingLink->havePriority() && crossingLink->myApproachingPersons != nullptr) {
1845 // a person might step on the crossing at any moment, since ego
1846 // is already on the junction, the opened() check is not done anymore
1847 const double timeToEnterCrossing = distToCrossing / MAX2(ego->getSpeed(), 1.0);
1848 for (const auto& item : (*crossingLink->myApproachingPersons)) {
1849 if (!ignoreFoe(ego, item.first) && timeToEnterCrossing > STEPS2TIME(item.second.arrivalTime - SIMSTEP)) {
1850 if (gDebugFlag1) {
1851 std::cout << SIMTIME << ": " << ego->getID() << " breaking for approaching person " << item.first->getID()
1852 //<< " dtc=" << distToCrossing << " ttc=" << distToCrossing / MAX2(ego->getSpeed(), 1.0) << " foeAT=" << item.second.arrivalTime << " foeTTC=" << STEPS2TIME(item.second.arrivalTime - SIMSTEP)
1853 << "\n";
1854 }
1855 result.emplace_back(nullptr, -1, distToPeds);
1856 break;
1857 //} else {
1858 // if (gDebugFlag1) {
1859 // std::cout << SIMTIME << ": " << ego->getID() << " notBreaking for approaching person " << item.first->getID()
1860 // << " dtc=" << distToCrossing << " ttc=" << distToCrossing / MAX2(ego->getSpeed(), 1.0) << " foeAT=" << item.second.arrivalTime << " foeTTC=" << STEPS2TIME(item.second.arrivalTime - SIMSTEP)
1861 // << "\n";
1862 // }
1863 }
1864 }
1865 }
1866 }
1867 }
1868 }
1869
1870 //std::cout << SIMTIME << " ego=" << Named::getIDSecure(ego) << " link=" << getViaLaneOrLane()->getID() << " myWalkingAreaFoe=" << Named::getIDSecure(myWalkingAreaFoe) << "\n";
1871 if (ego != nullptr) {
1872 checkWalkingAreaFoe(ego, myWalkingAreaFoe, collectBlockers, result);
1873 checkWalkingAreaFoe(ego, myWalkingAreaFoeExit, collectBlockers, result);
1874 }
1875
1876 if (MSGlobals::gLateralResolution > 0 && ego != nullptr && !isShadowLink) {
1877 // check for foes on the same edge
1878 for (std::vector<MSLane*>::const_iterator it = mySublaneFoeLanes.begin(); it != mySublaneFoeLanes.end(); ++it) {
1879 const MSLane* foeLane = *it;
1880 MSLane::AnyVehicleIterator end = foeLane->anyVehiclesEnd();
1881 for (MSLane::AnyVehicleIterator it_veh = foeLane->anyVehiclesBegin(); it_veh != end; ++it_veh) {
1882 MSVehicle* leader = (MSVehicle*)*it_veh;
1883 if (leader == ego) {
1884 continue;
1885 }
1886 if (leader->getLane()->isNormal()) {
1887 // leader is past the conflict point
1888 continue;
1889 }
1890 const double maxLength = MAX2(myInternalLaneBefore->getLength(), foeLane->getLength());
1891 const double gap = dist - maxLength - ego->getVehicleType().getMinGap() + leader->getBackPositionOnLane(foeLane) - extraGap;
1892 if (gap < -(ego->getVehicleType().getMinGap() + leader->getLength())) {
1893 // ego is ahead of leader
1894 continue;
1895 }
1896 const double posLat = ego->getLateralPositionOnLane();
1897 const double posLatLeader = leader->getLateralPositionOnLane() + leader->getLatOffset(foeLane);
1898 if (gDebugFlag1) {
1899 std::cout << " sublaneFoe lane=" << myInternalLaneBefore->getID()
1900 << " foeLane=" << foeLane->getID()
1901 << " leader=" << leader->getID()
1902 << " egoLane=" << ego->getLane()->getID()
1903 << " leaderLane=" << leader->getLane()->getID()
1904 << " gap=" << gap
1905 << " egoLat=" << posLat
1906 << " leaderLat=" << posLatLeader
1907 << " leaderLatOffset=" << leader->getLatOffset(foeLane)
1908 << " egoIndex=" << myInternalLaneBefore->getIndex()
1909 << " foeIndex=" << foeLane->getIndex()
1910 << " dist=" << dist
1911 << " leaderBack=" << leader->getBackPositionOnLane(foeLane)
1912 << "\n";
1913 }
1914 // there only is a conflict if the paths cross
1915 if ((posLat < posLatLeader && myInternalLaneBefore->getIndex() > foeLane->getIndex())
1916 || (posLat > posLatLeader && myInternalLaneBefore->getIndex() < foeLane->getIndex())) {
1917 if (gDebugFlag1) {
1918 std::cout << SIMTIME << " blocked by " << leader->getID() << " (sublane split) foeLane=" << foeLane->getID() << "\n";
1919 }
1920 if (ignoreFoe(ego, leader)) {
1921 continue;
1922 }
1923 result.emplace_back(leader, gap, -1);
1924 }
1925 }
1926 }
1927 }
1928 return result;
1929}
1930
1931
1932void
1933MSLink::checkWalkingAreaFoe(const MSVehicle* ego, const MSLane* foeLane, std::vector<const MSPerson*>* collectBlockers, LinkLeaders& result) const {
1934 if (foeLane != nullptr && foeLane->getEdge().getPersons().size() > 0) {
1935 // pedestrians may be on an arbitrary path across this
1936 // walkingarea. make sure to keep enough distance.
1937 // This is a simple but conservative solution that could be improved
1938 // by ignoring pedestrians that are "obviously" not on a collision course
1939 double distToPeds = std::numeric_limits<double>::max();
1940 assert(myInternalLaneBefore != nullptr);
1941 PositionVector egoPath = myInternalLaneBefore->getShape();
1942 if (ego->getLateralPositionOnLane() != 0) {
1943 egoPath.move2side((MSGlobals::gLefthand ? 1 : -1) * ego->getLateralPositionOnLane());
1944 }
1945 for (MSTransportable* t : foeLane->getEdge().getPersons()) {
1946 MSPerson* p = static_cast<MSPerson*>(t);
1947 double dist = ego->getPosition().distanceTo2D(p->getPosition()) - p->getVehicleType().getLength();
1948 const bool inFront = isInFront(ego, egoPath, p->getPosition()) || isInFront(ego, egoPath, getFuturePosition(p));
1949 if (inFront) {
1950 dist -= MAX2(ego->getVehicleType().getMinGap(), MSPModel::SAFETY_GAP);
1951 }
1952#ifdef DEBUG_WALKINGAREA
1953 if (ego->isSelected()) {
1954 std::cout << SIMTIME << " veh=" << ego->getID() << " ped=" << p->getID()
1955 << " pos=" << ego->getPosition() << " pedPos=" << p->getPosition()
1956 << " futurePedPos=" << getFuturePosition(p)
1957 << " rawDist=" << ego->getPosition().distanceTo2D(p->getPosition())
1958 << " inFront=" << inFront
1959 << " dist=" << dist << "\n";
1960 }
1961#endif
1962 if (dist < ego->getVehicleType().getWidth() / 2 || inFront) {
1963 if (inFront) {
1964 const double oncomingFactor = isOnComingPed(ego, p);
1965 if (oncomingFactor > 0) {
1966 // account for pedestrian movement while closing in
1967 const double timeToStop = sqrt(dist) / 2;
1968 const double pedDist = p->getMaxSpeed() * MAX2(timeToStop, TS) * oncomingFactor;
1969 dist = MAX2(0.0, dist - pedDist);
1970#ifdef DEBUG_WALKINGAREA
1971 if (ego->isSelected()) {
1972 std::cout << " timeToStop=" << timeToStop << " pedDist=" << pedDist << " factor=" << oncomingFactor << " dist2=" << dist << "\n";
1973 }
1974#endif
1975 }
1976 }
1977 if (ignoreFoe(ego, p)) {
1978 continue;
1979 }
1980 distToPeds = MIN2(distToPeds, dist);
1981 if (collectBlockers != nullptr) {
1982 collectBlockers->push_back(p);
1983 }
1984 }
1985 }
1986 if (distToPeds != std::numeric_limits<double>::max()) {
1987 // leave extra space in front
1988 result.emplace_back(nullptr, -1, distToPeds);
1989 }
1990 }
1991}
1992
1993bool
1994MSLink::isInFront(const MSVehicle* ego, const PositionVector& egoPath, const Position& pPos) const {
1995 const double pedAngle = ego->getPosition().angleTo2D(pPos);
1996 const double angleDiff = fabs(GeomHelper::angleDiff(ego->getAngle(), pedAngle));
1997#ifdef DEBUG_WALKINGAREA
1998 if (ego->isSelected()) {
1999 std::cout << " angleDiff=" << RAD2DEG(angleDiff) << "\n";
2000 }
2001#endif
2002 if (angleDiff < DEG2RAD(75)) {
2003 return egoPath.distance2D(pPos) < ego->getVehicleType().getWidth() + MSPModel::SAFETY_GAP;
2004 }
2005 return false;
2006}
2007
2008
2009double
2010MSLink::isOnComingPed(const MSVehicle* ego, const MSPerson* p) const {
2011 const double pedToEgoAngle = p->getPosition().angleTo2D(ego->getPosition());
2012 const double angleDiff = fabs(GeomHelper::angleDiff(p->getAngle(), pedToEgoAngle));
2013#ifdef DEBUG_WALKINGAREA
2014 if (ego->isSelected()) {
2015 std::cout << " ped-angleDiff=" << RAD2DEG(angleDiff) << " res=" << cos(angleDiff) << "\n";
2016 }
2017#endif
2018 if (angleDiff <= DEG2RAD(90)) {
2019 ;
2020 return cos(angleDiff);
2021 } else {
2022 return 0;
2023 }
2024}
2025
2026
2027Position
2028MSLink::getFuturePosition(const MSPerson* p, double timeHorizon) const {
2029 const double a = p->getAngle();
2030 const double dist = timeHorizon * p->getMaxSpeed();
2031
2032 const Position offset(cos(a) * dist, sin(a) * dist);
2033 return p->getPosition() + offset;
2034}
2035
2036
2037MSLink*
2038MSLink::getParallelLink(int direction) const {
2039 if (direction == -1) {
2040 return myParallelRight;
2041 } else if (direction == 1) {
2042 return myParallelLeft;
2043 } else {
2044 assert(false || myLane->getOpposite() != nullptr);
2045 return nullptr;
2046 }
2047}
2048
2049MSLink*
2050MSLink::getOppositeDirectionLink() const {
2051 if (myLane->getOpposite() != nullptr && myLaneBefore->getOpposite() != nullptr) {
2052 for (MSLink* cand : myLane->getOpposite()->getLinkCont()) {
2053 if (cand->getLane() == myLaneBefore->getOpposite()) {
2054 return cand;
2055 }
2056 }
2057 }
2058 return nullptr;
2059}
2060
2061
2062MSLink*
2063MSLink::computeParallelLink(int direction) {
2064 const MSLane* const before = getLaneBefore()->getParallelLane(direction, false);
2065 const MSLane* const after = getLane()->getParallelLane(direction, false);
2066 if (before != nullptr && after != nullptr) {
2067 for (MSLink* const link : before->getLinkCont()) {
2068 if (link->getLane() == after) {
2069 return link;
2070 }
2071 }
2072 }
2073 return nullptr;
2074}
2075
2076
2077double
2078MSLink::getZipperSpeed(const MSVehicle* ego, const double dist, double vSafe,
2079 SUMOTime arrivalTime,
2080 const BlockingFoes* foes) const {
2081 if (myFoeLinks.size() == 0) {
2082 // link should have LINKSTATE_MAJOR in this case
2083 assert(false);
2084 return vSafe;
2085 }
2086 const double brakeGap = ego->getCarFollowModel().brakeGap(vSafe, ego->getCarFollowModel().getMaxDecel(), TS);
2087 if (dist > MAX2(myFoeVisibilityDistance, brakeGap)) {
2088#ifdef DEBUG_ZIPPER
2089 const SUMOTime now = MSNet::getInstance()->getCurrentTimeStep();
2090 DEBUGOUT(DEBUG_COND_ZIPPER, SIMTIME << " getZipperSpeed ego=" << ego->getID()
2091 << " dist=" << dist << " bGap=" << brakeGap << " ignoring foes (arrival in " << STEPS2TIME(arrivalTime - now) << ")\n")
2092#endif
2093 return vSafe;
2094 }
2095#ifdef DEBUG_ZIPPER
2096 DEBUGOUT(DEBUG_COND_ZIPPER, SIMTIME << " getZipperSpeed ego=" << ego->getID()
2097 << " egoAT=" << arrivalTime
2098 << " dist=" << dist
2099 << " brakeGap=" << brakeGap
2100 << " vSafe=" << vSafe
2101 << " numFoes=" << foes->size()
2102 << "\n")
2103#endif
2104 const bool uniqueFoeLink = myFoeLinks.size() == 1;
2105 MSLink* foeLink = myFoeLinks[0];
2106 for (const auto& item : *foes) {
2107 if (!item->isVehicle()) {
2108 continue;
2109 }
2110 const MSVehicle* foe = dynamic_cast<const MSVehicle*>(item);
2111 assert(foe != 0);
2112 const ApproachingVehicleInformation* aviPtr = nullptr;
2113 if (uniqueFoeLink) {
2114 aviPtr = foeLink->getApproachingPtr(foe);
2115 } else {
2116 // figure out which link is approached by the current foe
2117 for (MSLink* fl : myFoeLinks) {
2118 aviPtr = fl->getApproachingPtr(foe);
2119 if (aviPtr != nullptr) {
2120 break;
2121 }
2122 }
2123 }
2124 if (aviPtr == nullptr) {
2125 continue;
2126 }
2127 const ApproachingVehicleInformation& avi = *aviPtr;
2128 const double foeDist = (foe->isActive() ? avi.dist : MAX2(0.0, avi.dist -
2129 STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - foe->getLastActionTime()) * avi.speed));
2130
2131 if ( // ignore vehicles that arrive after us (unless they are ahead and we could easily brake for them)
2132 ((avi.arrivalTime > arrivalTime) && !couldBrakeForLeader(dist, foeDist, ego, foe)) ||
2133 // also ignore vehicles that are behind us and are able to brake for us
2134 couldBrakeForLeader(foeDist, dist, foe, ego) ||
2135 // resolve ties by lane index
2136 (avi.arrivalTime == arrivalTime && foeDist == dist && ego->getLane()->getIndex() < foe->getLane()->getIndex())) {
2137#ifdef DEBUG_ZIPPER
2138 if (DEBUG_COND_ZIPPER) std::cout
2139 << " ignoring foe=" << foe->getID()
2140 << " foeAT=" << avi.arrivalTime
2141 << " foeDist=" << avi.dist
2142 << " foeDist2=" << foeDist
2143 << " foeSpeed=" << avi.speed
2144 << " egoSpeed=" << ego->getSpeed()
2145 << " deltaDist=" << foeDist - dist
2146 << " delteSpeed=" << avi.speed - foe->getCarFollowModel().getMaxDecel() - ego->getSpeed()
2147 << " egoCouldBrake=" << couldBrakeForLeader(dist, foeDist, ego, foe)
2148 << " foeCouldBrake=" << couldBrakeForLeader(foeDist, dist, foe, ego)
2149 << "\n";
2150#endif
2151 continue;
2152 }
2153 // the idea behind speed adaption is three-fold:
2154 // 1) ego needs to be in a car-following relationship with foe eventually
2155 // thus, the ego speed should be equal to the follow speed once the foe enters
2156 // the zipper junction
2157 // 2) ego vehicle needs to put a certain distance beteen himself and foe (safeGap)
2158 // achieving this distance can be spread over time but computing
2159 // safeGap is subject to estimation errors of future speeds
2160 // 3) deceleration can be spread out over the time until true
2161 // car-following happens, at the start of speed adaptions, smaller
2162 // decelerations should be sufficient
2163
2164 // we cannot trust avi.arrivalSpeed if the foe has leader vehicles that are accelerating
2165 // lets try to extrapolate
2166 const double uMax = foe->getLane()->getVehicleMaxSpeed(foe);
2167 const double uAccel = foe->getCarFollowModel().estimateSpeedAfterDistance(foeDist, avi.speed, foe->getCarFollowModel().getMaxAccel());
2168 const double uEnd = MIN2(uMax, uAccel);
2169 const double uAvg = (avi.speed + uEnd) / 2;
2170 const double tf0 = foeDist / MAX2(NUMERICAL_EPS, uAvg);
2171 const double tf = MAX2(1.0, ceil((tf0) / TS) * TS);
2172
2173 const double vMax = ego->getLane()->getVehicleMaxSpeed(ego);
2174 const double vAccel = ego->getCarFollowModel().estimateSpeedAfterDistance(dist, ego->getSpeed(), ego->getCarFollowModel().getMaxAccel());
2175 const double vDecel = ego->getCarFollowModel().estimateSpeedAfterDistance(dist, ego->getSpeed(), -ego->getCarFollowModel().getMaxDecel());
2176 const double vEnd = MIN3(vMax, vAccel, MAX2(uEnd, vDecel));
2177 const double vAvg = (ego->getSpeed() + vEnd) / 2;
2178 const double te0 = dist / MAX2(NUMERICAL_EPS, vAvg);
2179 const double te = MAX2(1.0, ceil((te0) / TS) * TS);
2180
2181 const double tTarget = tf + ego->getCarFollowModel().getHeadwayTime();
2182 const double a = ego->getCarFollowModel().avoidArrivalAccel(dist, tTarget, vSafe, ego->getCarFollowModel().getMaxDecel());
2183
2184 const double gap = dist - foe->getVehicleType().getLength() - ego->getVehicleType().getMinGap() - foeDist;
2185 const double vFollow = ego->getCarFollowModel().followSpeed(
2186 ego, ego->getSpeed(), gap, avi.speed, foe->getCarFollowModel().getMaxDecel(), foe);
2187 const double vSafeGap = MAX2(vFollow, ego->getSpeed() + ACCEL2SPEED(a));
2188
2189 // scale behavior based on ego time to link (te)
2190 const double w = MIN2(1.0, te / 10);
2191 const double maxDecel = w * ego->getCarFollowModel().getMaxDecel() + (1 - w) * ego->getCarFollowModel().getEmergencyDecel();
2192 const double vZipper = MAX3(vFollow, ego->getSpeed() - ACCEL2SPEED(maxDecel), vSafeGap);
2193
2194 vSafe = MIN2(vSafe, vZipper);
2195#ifdef DEBUG_ZIPPER
2196 if (DEBUG_COND_ZIPPER) std::cout << " adapting to foe=" << foe->getID()
2197 << " foeDist=" << foeDist
2198 << " foeSpeed=" << avi.speed
2199 << " foeAS=" << avi.arrivalSpeed
2200 << " egoSpeed=" << ego->getSpeed()
2201 << " uMax=" << uMax
2202 << " uAccel=" << uAccel
2203 << " uEnd=" << uEnd
2204 << " uAvg=" << uAvg
2205 << " gap=" << gap
2206 << "\n "
2207 << " tf=" << tf
2208 << " te=" << te
2209 << " aSafeGap=" << a
2210 << " vMax=" << vMax
2211 << " vAccel=" << vAccel
2212 << " vDecel=" << vDecel
2213 << " vEnd=" << vEnd
2214 << " vSafeGap=" << vSafeGap
2215 << " vFollow=" << vFollow
2216 << " w=" << w
2217 << " maxDecel=" << maxDecel
2218 << " vZipper=" << vZipper
2219 << " vSafe=" << vSafe
2220 << "\n";
2221#endif
2222 }
2223 return vSafe;
2224}
2225
2226
2227bool
2228MSLink::couldBrakeForLeader(double followDist, double leaderDist, const MSVehicle* follow, const MSVehicle* leader) {
2229 return (// leader is ahead of follower
2230 followDist > leaderDist &&
2231 // and follower could brake for 1 s to stay behind leader
2232 followDist - leaderDist > follow->getSpeed() - follow->getCarFollowModel().getMaxDecel() - leader->getSpeed());
2233}
2234
2235
2236void
2241
2242bool
2243MSLink::checkContOff() const {
2244 // check whether this link gets to keep its cont status switching the tls off
2245 // @note: this could also be pre-computed in netconvert
2246 // we check whether there is any major link from this edge
2247 for (const MSLane* cand : myLaneBefore->getEdge().getLanes()) {
2248 for (const MSLink* link : cand->getLinkCont()) {
2249 if (link->getOffState() == LINKSTATE_TL_OFF_NOSIGNAL) {
2250 return true;
2251 }
2252 }
2253 }
2254 return false;
2255}
2256
2257bool
2258MSLink::lateralOverlap(double posLat, double width, double posLat2, double width2) {
2259 return fabs(posLat2 - posLat) < (width + width2) / 2;
2260}
2261
2262std::string
2263MSLink::getDescription() const {
2264 return myLaneBefore->getID() + "->" + getViaLaneOrLane()->getID();
2265}
2266
2267
2268bool
2269MSLink::ignoreFoe(const SUMOTrafficObject* ego, const SUMOTrafficObject* foe) {
2270 if (ego == nullptr || !ego->getParameter().wasSet(VEHPARS_JUNCTIONMODEL_PARAMS_SET)) {
2271 return false;
2272 }
2273 const SUMOVehicleParameter& param = ego->getParameter();
2274 for (const std::string& typeID : StringTokenizer(param.getParameter(toString(SUMO_ATTR_JM_IGNORE_TYPES), "")).getVector()) {
2275 if (typeID == foe->getVehicleType().getID()) {
2276 return true;
2277 }
2278 }
2279 for (const std::string& id : StringTokenizer(param.getParameter(toString(SUMO_ATTR_JM_IGNORE_IDS), "")).getVector()) {
2280 if (id == foe->getID()) {
2281 return true;
2282 }
2283 }
2284 return false;
2285}
2286
2287
2288void
2292
2293
2294std::pair<const SUMOVehicle* const, const MSLink::ApproachingVehicleInformation>
2295MSLink::getClosest() const {
2296 assert(getApproaching().size() > 0);
2297 double minDist = std::numeric_limits<double>::max();
2298 auto closestIt = getApproaching().begin();
2299 for (auto apprIt = getApproaching().begin(); apprIt != getApproaching().end(); apprIt++) {
2300 if (apprIt->second.dist < minDist) {
2301 minDist = apprIt->second.dist;
2302 closestIt = apprIt;
2303 }
2304 }
2305 // maybe a parallel link has a closer vehicle
2306 /*
2307 for (MSLink* link2 : link->getLaneBefore()->getLinkCont()) {
2308 if (link2 != link) {
2309 for (auto apprIt2 = link2->getApproaching().begin(); apprIt2 != link2->getApproaching().end(); apprIt2++) {
2310 if (apprIt2->second.dist < minDist) {
2311 minDist = apprIt2->second.dist;
2312 closestIt = apprIt2;
2313 }
2314 }
2315 }
2316 }
2317 */
2318 return *closestIt;
2319}
2320
2321
2322/****************************************************************************/
SUMOTime
long long int SUMOTime
Definition GUI.h:36
DEG2RAD
#define DEG2RAD(x)
Definition GeomHelper.h:35
RAD2DEG
#define RAD2DEG(x)
Definition GeomHelper.h:36
DEBUG_COND2
#define DEBUG_COND2(obj)
Definition MESegment.cpp:53
WRITE_WARNINGF
#define WRITE_WARNINGF(...)
Definition MsgHandler.h:288
WRITE_WARNING
#define WRITE_WARNING(msg)
Definition MsgHandler.h:287
DELTA_T
SUMOTime DELTA_T
Definition SUMOTime.cpp:38
time2string
std::string time2string(SUMOTime t, bool humanReadable)
convert SUMOTime to string (independently of global format setting)
Definition SUMOTime.cpp:91
STEPS2TIME
#define STEPS2TIME(x)
Definition SUMOTime.h:55
SIMSTEP
#define SIMSTEP
Definition SUMOTime.h:61
ACCEL2SPEED
#define ACCEL2SPEED(x)
Definition SUMOTime.h:51
SUMOTime_MAX
#define SUMOTime_MAX
Definition SUMOTime.h:34
SUMOTime_MIN
#define SUMOTime_MIN
Definition SUMOTime.h:35
TS
#define TS
Definition SUMOTime.h:42
SIMTIME
#define SIMTIME
Definition SUMOTime.h:62
TIME2STEPS
#define TIME2STEPS(x)
Definition SUMOTime.h:57
SVCAll
const SVCPermissions SVCAll
all VClasses are allowed
Definition SUMOVehicleClass.cpp:158
SVC_BICYCLE
@ SVC_BICYCLE
vehicle is a bicycle
Definition SUMOVehicleClass.h:186
VEHPARS_JUNCTIONMODEL_PARAMS_SET
const long long int VEHPARS_JUNCTIONMODEL_PARAMS_SET
Definition SUMOVehicleParameter.h:72
LinkDirection
LinkDirection
The different directions a link between two lanes may take (or a stream between two edges)....
Definition SUMOXMLDefinitions.h:2134
LinkDirection::PARTLEFT
@ PARTLEFT
The link is a partial left direction.
LinkDirection::RIGHT
@ RIGHT
The link is a (hard) right direction.
LinkDirection::LEFT
@ LEFT
The link is a (hard) left direction.
LinkDirection::STRAIGHT
@ STRAIGHT
The link is a straight direction.
LinkDirection::PARTRIGHT
@ PARTRIGHT
The link is a partial right direction.
LinkState
LinkState
The right-of-way state of a link between two lanes used when constructing a NBTrafficLightLogic,...
Definition SUMOXMLDefinitions.h:2095
LINKSTATE_ALLWAY_STOP
@ LINKSTATE_ALLWAY_STOP
This is an uncontrolled, all-way stop link.
Definition SUMOXMLDefinitions.h:2121
LINKSTATE_STOP
@ LINKSTATE_STOP
This is an uncontrolled, minor link, has to stop.
Definition SUMOXMLDefinitions.h:2119
LINKSTATE_TL_GREEN_MAJOR
@ LINKSTATE_TL_GREEN_MAJOR
The link has green light, may pass.
Definition SUMOXMLDefinitions.h:2097
LINKSTATE_ZIPPER
@ LINKSTATE_ZIPPER
This is an uncontrolled, zipper-merge link.
Definition SUMOXMLDefinitions.h:2123
LINKSTATE_TL_OFF_BLINKING
@ LINKSTATE_TL_OFF_BLINKING
The link is controlled by a tls which is off and blinks, has to brake.
Definition SUMOXMLDefinitions.h:2109
LINKSTATE_TL_RED
@ LINKSTATE_TL_RED
The link has red light (must brake)
Definition SUMOXMLDefinitions.h:2101
LINKSTATE_TL_GREEN_MINOR
@ LINKSTATE_TL_GREEN_MINOR
The link has green light, has to brake.
Definition SUMOXMLDefinitions.h:2099
LINKSTATE_TL_OFF_NOSIGNAL
@ LINKSTATE_TL_OFF_NOSIGNAL
The link is controlled by a tls which is off, not blinking, may pass.
Definition SUMOXMLDefinitions.h:2111
SUMO_ATTR_JM_IGNORE_FOE_SPEED
@ SUMO_ATTR_JM_IGNORE_FOE_SPEED
Definition SUMOXMLDefinitions.h:1358
SUMO_ATTR_JM_STOPLINE_CROSSING_GAP
@ SUMO_ATTR_JM_STOPLINE_CROSSING_GAP
Definition SUMOXMLDefinitions.h:1364
SUMO_ATTR_JM_STOPSIGN_WAIT
@ SUMO_ATTR_JM_STOPSIGN_WAIT
Definition SUMOXMLDefinitions.h:1368
SUMO_ATTR_VIA
@ SUMO_ATTR_VIA
Definition SUMOXMLDefinitions.h:1498
SUMO_ATTR_JM_IGNORE_IDS
@ SUMO_ATTR_JM_IGNORE_IDS
Definition SUMOXMLDefinitions.h:1370
SUMO_ATTR_JM_IGNORE_TYPES
@ SUMO_ATTR_JM_IGNORE_TYPES
Definition SUMOXMLDefinitions.h:1371
SUMO_ATTR_JM_ALLWAYSTOP_WAIT
@ SUMO_ATTR_JM_ALLWAYSTOP_WAIT
Definition SUMOXMLDefinitions.h:1369
SUMO_ATTR_JM_EXTRA_GAP
@ SUMO_ATTR_JM_EXTRA_GAP
Definition SUMOXMLDefinitions.h:1366
SUMO_ATTR_JM_IGNORE_FOE_PROB
@ SUMO_ATTR_JM_IGNORE_FOE_PROB
Definition SUMOXMLDefinitions.h:1359
SUMO_ATTR_TO
@ SUMO_ATTR_TO
Definition SUMOXMLDefinitions.h:1399
SUMO_ATTR_FROM
@ SUMO_ATTR_FROM
Definition SUMOXMLDefinitions.h:1398
SUMO_ATTR_JM_CROSSING_GAP
@ SUMO_ATTR_JM_CROSSING_GAP
Definition SUMOXMLDefinitions.h:1353
SUMO_ATTR_IMPATIENCE
@ SUMO_ATTR_IMPATIENCE
Definition SUMOXMLDefinitions.h:1612
SUMO_ATTR_ID
@ SUMO_ATTR_ID
Definition SUMOXMLDefinitions.h:952
SUMO_ATTR_JM_TIMEGAP_MINOR
@ SUMO_ATTR_JM_TIMEGAP_MINOR
Definition SUMOXMLDefinitions.h:1365
SUMO_ATTR_JM_ADVANCE
@ SUMO_ATTR_JM_ADVANCE
Definition SUMOXMLDefinitions.h:1367
gDebugFlag6
bool gDebugFlag6
Definition StdDefs.cpp:43
gDebugFlag1
bool gDebugFlag1
global utility flags for debugging
Definition StdDefs.cpp:38
INVALID_DOUBLE
const double INVALID_DOUBLE
invalid double
Definition StdDefs.h:64
MIN3
T MIN3(T a, T b, T c)
Definition StdDefs.h:89
MIN2
T MIN2(T a, T b)
Definition StdDefs.h:76
SUMO_const_haltingSpeed
const double SUMO_const_haltingSpeed
the speed threshold at which vehicles are considered as halting
Definition StdDefs.h:58
MAX2
T MAX2(T a, T b)
Definition StdDefs.h:82
MAX3
T MAX3(T a, T b, T c)
Definition StdDefs.h:96
DEBUGOUT
#define DEBUGOUT(cond, msg)
Definition StdDefs.h:142
toString
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition ToString.h:46
GeomHelper::naviDegree
static double naviDegree(const double angle)
Definition GeomHelper.cpp:192
GeomHelper::angleDiff
static double angleDiff(const double angle1, const double angle2)
Returns the difference of the second angle to the first angle in radiants.
Definition GeomHelper.cpp:179
GeomHelper::getMinAngleDiff
static double getMinAngleDiff(double angle1, double angle2)
Returns the minimum distance (clockwise/counter-clockwise) between both angles.
Definition GeomHelper.cpp:173
MSAbstractLaneChangeModel::getManeuverDist
double getManeuverDist() const
Returns the remaining unblocked distance for the current maneuver. (only used by sublane model)
Definition MSAbstractLaneChangeModel.cpp:193
MSAbstractLaneChangeModel::getSpeedLat
double getSpeedLat() const
return the lateral speed of the current lane change maneuver
Definition MSAbstractLaneChangeModel.h:588
MSBaseVehicle::isSelected
virtual bool isSelected() const
whether this vehicle is selected in the GUI
Definition MSBaseVehicle.h:809
MSBaseVehicle::getLength
double getLength() const
Returns the vehicle's length.
Definition MSBaseVehicle.h:565
MSBaseVehicle::getVehicleType
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
Definition MSBaseVehicle.h:153
MSBaseVehicle::isStopped
bool isStopped() const
Returns whether the vehicle is at a stop.
Definition MSBaseVehicle.cpp:1364
MSCFModel::estimateSpeedAfterDistance
double estimateSpeedAfterDistance(const double dist, const double v, const double accel) const
Definition MSCFModel.cpp:787
MSCFModel::getEmergencyDecel
double getEmergencyDecel() const
Get the vehicle type's maximal physically possible deceleration [m/s^2].
Definition MSCFModel.h:277
MSCFModel::avoidArrivalAccel
static double avoidArrivalAccel(double dist, double time, double speed, double maxDecel)
Computes the acceleration needed to arrive not before the given time.
Definition MSCFModel.cpp:543
MSCFModel::getMaxAccel
double getMaxAccel() const
Get the vehicle type's maximum acceleration [m/s^2].
Definition MSCFModel.h:261
MSCFModel::brakeGap
double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver's reaction time tau (i....
Definition MSCFModel.h:408
MSCFModel::getMaxDecel
double getMaxDecel() const
Get the vehicle type's maximal comfortable deceleration [m/s^2].
Definition MSCFModel.h:269
MSCFModel::followSpeed
virtual double followSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0, const CalcReason usage=CalcReason::CURRENT) const =0
Computes the vehicle's follow speed (no dawdling)
MSCFModel::getHeadwayTime
virtual double getHeadwayTime() const
Get the driver's desired headway [s].
Definition MSCFModel.h:339
MSEdge
A road/street connecting two junctions.
Definition MSEdge.h:77
MSEdge::getPersons
const std::set< MSTransportable *, ComparatorNumericalIdLess > & getPersons() const
Returns this edge's persons set.
Definition MSEdge.h:204
MSEdge::getLanes
const std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
Definition MSEdge.h:168
MSEdge::getToJunction
const MSJunction * getToJunction() const
Definition MSEdge.h:418
MSEdge::getFromJunction
const MSJunction * getFromJunction() const
Definition MSEdge.h:414
MSEdge::isRoundabout
bool isRoundabout() const
Definition MSEdge.h:721
MSEdge::isInternal
bool isInternal() const
return whether this edge is an internal edge
Definition MSEdge.h:268
MSGlobals::gUseMesoSim
static bool gUseMesoSim
Definition MSGlobals.h:106
MSGlobals::gLateralResolution
static double gLateralResolution
Definition MSGlobals.h:100
MSGlobals::gComputeLC
static bool gComputeLC
whether the simulationLoop is in the lane changing phase
Definition MSGlobals.h:140
MSGlobals::gLefthand
static bool gLefthand
Whether lefthand-drive is being simulated.
Definition MSGlobals.h:174
MSGlobals::gIgnoreJunctionBlocker
static SUMOTime gIgnoreJunctionBlocker
Definition MSGlobals.h:85
MSGlobals::gSublane
static bool gSublane
whether sublane simulation is enabled (sublane model or continuous lanechanging)
Definition MSGlobals.h:165
MSGlobals::gUsingInternalLanes
static bool gUsingInternalLanes
Information whether the simulation regards internal lanes.
Definition MSGlobals.h:81
MSJunction
The base class for an intersection.
Definition MSJunction.h:58
MSJunction::getType
SumoXMLNodeType getType() const
return the type of this Junction
Definition MSJunction.h:133
MSLane::AnyVehicleIterator
AnyVehicleIterator is a structure, which manages the iteration through all vehicles on the lane,...
Definition MSLane.h:129
MSLane
Representation of a lane in the micro simulation.
Definition MSLane.h:84
MSLane::getParallelLane
MSLane * getParallelLane(int offset, bool includeOpposite=true) const
Returns the lane with the given offset parallel to this one or 0 if it does not exist.
Definition MSLane.cpp:2839
MSLane::getNormalSuccessorLane
const MSLane * getNormalSuccessorLane() const
get normal lane following this internal lane, for normal lanes, the lane itself is returned
Definition MSLane.cpp:3240
MSLane::anyVehiclesEnd
AnyVehicleIterator anyVehiclesEnd() const
end iterator for iterating over all vehicles touching this lane in downstream direction
Definition MSLane.h:495
MSLane::getEntryLink
const MSLink * getEntryLink() const
Returns the entry link if this is an internal lane, else nullptr.
Definition MSLane.cpp:2757
MSLane::getLinkTo
const MSLink * getLinkTo(const MSLane *const) const
returns the link to the given lane or nullptr, if it is not connected
Definition MSLane.cpp:2734
MSLane::getIncomingLanes
const std::vector< IncomingLaneInfo > & getIncomingLanes() const
Definition MSLane.h:955
MSLane::getCanonicalPredecessorLane
MSLane * getCanonicalPredecessorLane() const
Definition MSLane.cpp:3261
MSLane::getLength
double getLength() const
Returns the lane's length.
Definition MSLane.h:611
MSLane::getVehicleMaxSpeed
double getVehicleMaxSpeed(const SUMOTrafficObject *const veh) const
Returns the lane's maximum speed, given a vehicle's speed limit adaptation.
Definition MSLane.h:574
MSLane::getIndex
int getIndex() const
Returns the lane's index.
Definition MSLane.h:647
MSLane::getLogicalPredecessorLane
MSLane * getLogicalPredecessorLane() const
get the most likely precedecessor lane (sorted using by_connections_to_sorter). The result is cached ...
Definition MSLane.cpp:3205
MSLane::isNormal
bool isNormal() const
Definition MSLane.cpp:2615
MSLane::isCrossing
bool isCrossing() const
Definition MSLane.cpp:2621
MSLane::isInternal
bool isInternal() const
Definition MSLane.cpp:2609
MSLane::interpolateGeometryPosToLanePos
double interpolateGeometryPosToLanePos(double geometryPos) const
Definition MSLane.h:566
MSLane::anyVehiclesBegin
AnyVehicleIterator anyVehiclesBegin() const
begin iterator for iterating over all vehicles touching this lane in downstream direction
Definition MSLane.h:489
MSLane::getOpposite
MSLane * getOpposite() const
return the neighboring opposite direction lane for lane changing or nullptr
Definition MSLane.cpp:4372
MSLane::getBidiLane
MSLane * getBidiLane() const
retrieve bidirectional lane or nullptr
Definition MSLane.cpp:4668
MSLane::getShape
virtual const PositionVector & getShape(bool) const
Definition MSLane.h:294
MSLane::getEdge
MSEdge & getEdge() const
Returns the lane's edge.
Definition MSLane.h:769
MSLane::getNormalPredecessorLane
const MSLane * getNormalPredecessorLane() const
get normal lane leading to this internal lane, for normal lanes, the lane itself is returned
Definition MSLane.cpp:3230
MSLane::getWidth
double getWidth() const
Returns the lane's width.
Definition MSLane.h:640
MSLane::getLinkCont
const std::vector< MSLink * > & getLinkCont() const
returns the container with all links !!!
Definition MSLane.h:729
MSNet::getInstance
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition MSNet.cpp:186
MSNet::getCurrentTimeStep
SUMOTime getCurrentTimeStep() const
Returns the current simulation step.
Definition MSNet.h:326
MSNet::hasPersons
bool hasPersons() const
Returns whether persons are simulated.
Definition MSNet.h:401
MSNet::getPersonControl
virtual MSTransportableControl & getPersonControl()
Returns the person control.
Definition MSNet.cpp:1200
MSPModel::blockedAtDist
virtual bool blockedAtDist(const SUMOTrafficObject *ego, const MSLane *lane, double vehCenter, double vehWidth, double oncomingGap, std::vector< const MSPerson * > *collectBlockers)
whether a pedestrian is blocking the crossing of lane for the given vehicle bondaries
Definition MSPModel.h:98
MSPModel::SAFETY_GAP
static const double SAFETY_GAP
Definition MSPModel.h:59
MSTrafficLightLogic
The parent class for traffic light logics.
Definition MSTrafficLightLogic.h:55
MSTransportableControl::getMovementModel
MSPModel * getMovementModel()
Returns the default movement model for this kind of transportables.
Definition MSTransportableControl.h:267
MSTransportable::getAngle
virtual double getAngle() const
return the current angle of the transportable
Definition MSTransportable.cpp:197
MSTransportable::getPosition
Position getPosition(const double) const
Return current position (x/y, cartesian)
Definition MSTransportable.h:113
MSTransportable::getVehicleType
const MSVehicleType & getVehicleType() const
Returns the object's "vehicle" type.
Definition MSTransportable.h:144
MSTransportable::getMaxSpeed
double getMaxSpeed() const
Returns the maximum speed (the minimum of desired and physical maximum speed)
Definition MSTransportable.cpp:638
MSVehicle
Representation of a vehicle in the micro simulation.
Definition MSVehicle.h:77
MSVehicle::willStop
bool willStop() const
Returns whether the vehicle will stop on the current edge.
Definition MSVehicle.cpp:1582
MSVehicle::getLastActionTime
SUMOTime getLastActionTime() const
Returns the time of the vehicle's last action point.
Definition MSVehicle.h:541
MSVehicle::isActive
bool isActive() const
Returns whether the current simulation step is an action point for the vehicle.
Definition MSVehicle.h:628
MSVehicle::getWaitingTime
SUMOTime getWaitingTime(const bool accumulated=false) const
Returns the SUMOTime waited (speed was lesser than 0.1m/s)
Definition MSVehicle.h:670
MSVehicle::isFrontOnLane
bool isFrontOnLane(const MSLane *lane) const
Returns the information whether the front of the vehicle is on the given lane.
Definition MSVehicle.cpp:5119
MSVehicle::getLaneChangeModel
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition MSVehicle.cpp:5847
MSVehicle::getPosition
Position getPosition(const double offset=0) const
Return current position (x/y, cartesian)
Definition MSVehicle.cpp:1249
MSVehicle::getBackPositionOnLane
double getBackPositionOnLane(const MSLane *lane) const
Get the vehicle's position relative to the given lane.
Definition MSVehicle.h:398
MSVehicle::getLatOffset
double getLatOffset(const MSLane *lane) const
Get the offset that that must be added to interpret myState.myPosLat for the given lane.
Definition MSVehicle.cpp:6858
MSVehicle::getLane
const MSLane * getLane() const
Returns the lane the vehicle is on.
Definition MSVehicle.h:581
MSVehicle::isBidiOn
bool isBidiOn(const MSLane *lane) const
whether this vehicle is driving against lane
Definition MSVehicle.cpp:7159
MSVehicle::getLateralPositionOnLane
double getLateralPositionOnLane() const
Get the vehicle's lateral position on the lane.
Definition MSVehicle.h:413
MSVehicle::getSpeed
double getSpeed() const
Returns the vehicle's current speed.
Definition MSVehicle.h:490
MSVehicle::getCarFollowModel
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
Definition MSVehicle.h:969
MSVehicle::ignoreRed
bool ignoreRed(const MSLink *link, bool canBrake) const
decide whether a red (or yellow light) may be ignored
Definition MSVehicle.cpp:7424
MSVehicle::getAngle
double getAngle() const
Returns the vehicle's direction in radians.
Definition MSVehicle.h:735
MSVehicleType::getWidth
double getWidth() const
Get the width which vehicles of this class shall have when being drawn.
Definition MSVehicleType.h:267
MSVehicleType::getVehicleClass
SUMOVehicleClass getVehicleClass() const
Get this vehicle type's vehicle class.
Definition MSVehicleType.h:189
MSVehicleType::getID
const std::string & getID() const
Returns the name of the vehicle type.
Definition MSVehicleType.h:91
MSVehicleType::getMinGap
double getMinGap() const
Get the free space in front of vehicles of this class.
Definition MSVehicleType.h:123
MSVehicleType::getCarFollowModel
const MSCFModel & getCarFollowModel() const
Returns the vehicle type's car following model definition (const version)
Definition MSVehicleType.h:138
MSVehicleType::getLength
double getLength() const
Get vehicle's length [m].
Definition MSVehicleType.h:107
MSVehicleType::getParameter
const SUMOVTypeParameter & getParameter() const
Definition MSVehicleType.h:649
Named::getID
const std::string & getID() const
Returns the id.
Definition Named.h:74
OutputDevice
Static storage of an output device and its base (abstract) implementation.
Definition OutputDevice.h:64
OutputDevice::writeAttr
OutputDevice & writeAttr(const SumoXMLAttr attr, const T &val)
writes a named attribute
Definition OutputDevice.h:236
OutputDevice::openTag
OutputDevice & openTag(const std::string &xmlElement)
Opens an XML tag.
Definition OutputDevice.cpp:259
OutputDevice::closeTag
bool closeTag(const std::string &comment="")
Closes the most recently opened tag and optionally adds a comment.
Definition OutputDevice.cpp:273
Parameterised::getParameter
virtual const std::string getParameter(const std::string &key, const std::string defaultValue="") const
Returns the value for a given key.
Definition Parameterised.cpp:90
Position
A point in 2D or 3D with translation and scaling methods.
Definition Position.h:37
Position::distanceTo2D
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition Position.h:273
Position::angleTo2D
double angleTo2D(const Position &other) const
returns the angle in the plane of the vector pointing from here to the other position (in radians bet...
Definition Position.h:283
PositionVector
A list of positions.
Definition PositionVector.h:43
PositionVector::length2D
double length2D() const
Returns the length.
Definition PositionVector.cpp:550
PositionVector::rotationAtOffset
double rotationAtOffset(double pos) const
Returns the rotation at the given length.
Definition PositionVector.cpp:320
PositionVector::intersectsAtLengths2D
std::vector< double > intersectsAtLengths2D(const PositionVector &other) const
For all intersections between this vector and other, return the 2D-length of the subvector from this ...
Definition PositionVector.cpp:1089
PositionVector::distance2D
double distance2D(const Position &p, bool perpendicular=false) const
closest 2D-distance to point p (or -1 if perpendicular is true and the point is beyond this vector)
Definition PositionVector.cpp:1400
PositionVector::distances
std::vector< double > distances(const PositionVector &s, bool perpendicular=false) const
distances of all my points to s and all of s points to myself
Definition PositionVector.cpp:1380
PositionVector::move2side
void move2side(double amount, double maxExtension=100)
move position vector to side using certain amount
Definition PositionVector.cpp:1184
PositionVector::angleAt2D
double angleAt2D(int pos) const
get angle in certain position of position vector (in radians between -M_PI and M_PI)
Definition PositionVector.cpp:1355
PositionVector::reverse
PositionVector reverse() const
reverse position vector
Definition PositionVector.cpp:1167
RandHelper::rand
static double rand(SumoRNG *rng=nullptr)
Returns a random real number in [0, 1)
Definition RandHelper.cpp:94
SUMOTrafficObject
Representation of a vehicle, person, or container.
Definition SUMOTrafficObject.h:54
SUMOTrafficObject::getVehicleType
virtual const MSVehicleType & getVehicleType() const =0
Returns the object's "vehicle" type.
SUMOTrafficObject::getSpeed
virtual double getSpeed() const =0
Returns the object's current speed.
SUMOTrafficObject::getParameter
virtual const SUMOVehicleParameter & getParameter() const =0
Returns the vehicle's parameter (including departure definition)
SUMOTrafficObject::getRNG
virtual SumoRNG * getRNG() const =0
Returns the associated RNG for this object.
SUMOTrafficObject::isSelected
virtual bool isSelected() const =0
whether this object is selected in the GUI
SUMOVTypeParameter::getJMParam
double getJMParam(const SumoXMLAttr attr, const double defaultValue) const
Returns the named value from the map, or the default if it is not contained there.
Definition SUMOVTypeParameter.cpp:717
SUMOVehicle
Representation of a vehicle.
Definition SUMOVehicle.h:62
SUMOVehicle::getLateralPositionOnLane
virtual double getLateralPositionOnLane() const =0
Get the vehicle's lateral position on the lane.
SUMOVehicle::getLastActionTime
virtual SUMOTime getLastActionTime() const =0
SUMOVehicle::getBrakeGap
virtual double getBrakeGap(bool delayed=false) const =0
get distance for coming to a stop (used for rerouting checks)
SUMOVehicleParameter
Structure representing possible vehicle parameter.
Definition SUMOVehicleParameter.h:338
SUMOVehicleParameter::wasSet
bool wasSet(long long int what) const
Returns whether the given parameter was set.
Definition SUMOVehicleParameter.h:501
StringTokenizer::getVector
std::vector< std::string > getVector()
return vector of strings
Definition StringTokenizer.cpp:190
std
Definition json.hpp:4471