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