Eclipse SUMO - Simulation of Urban MObility
Loading...
Searching...
No Matches
MSLink.cpp
Go to the documentation of this file.
1/****************************************************************************/
2// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3// Copyright (C) 2001-2025 German Aerospace Center (DLR) and others.
4// This program and the accompanying materials are made available under the
5// terms of the Eclipse Public License 2.0 which is available at
6// https://www.eclipse.org/legal/epl-2.0/
7// This Source Code may also be made available under the following Secondary
8// Licenses when the conditions for such availability set forth in the Eclipse
9// Public License 2.0 are satisfied: GNU General Public License, version 2
10// or later which is available at
11// https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12// SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13/****************************************************************************/
21// A connection between lanes
22/****************************************************************************/
23#include <config.h>
24
25#include <iostream>
26#include <algorithm>
27#include <limits>
28#include <utils/iodevices/OutputDevice.h>
29#include <utils/common/RandHelper.h>
30#include <utils/common/StringTokenizer.h>
31#include "MSNet.h"
32#include "MSJunction.h"
33#include "MSJunctionLogic.h"
34#include "MSLink.h"
35#include "MSLane.h"
36#include <microsim/transportables/MSPerson.h>
37#include <microsim/transportables/MSTransportableControl.h>
38#include "MSEdge.h"
39#include "MSGlobals.h"
40#include "MSVehicle.h"
41#include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
42#include <microsim/transportables/MSPModel.h>
43
44//#define MSLink_DEBUG_CROSSING_POINTS
45//#define MSLink_DEBUG_CROSSING_POINTS_DETAILS
46//#define MSLink_DEBUG_OPENED
47//#define DEBUG_APPROACHING
48//#define DEBUG_ZIPPER
49//#define DEBUG_WALKINGAREA
50//#define DEBUG_COND (myLane->getID()=="43[0]_0" && myLaneBefore->getID()==":33_0_0")
51//#define DEBUG_COND (myLane->getID()=="end_0")
52//#define DEBUG_COND (true)
53#define DEBUG_COND2(obj) (obj->isSelected())
54//#define DEBUG_COND2(obj) (obj->getID() == "train2")
55//#define DEBUG_COND2(obj) (true)
56//#define DEBUG_COND_ZIPPER (gDebugFlag1)
57//#define DEBUG_COND_ZIPPER (true)
58#define DEBUG_COND_ZIPPER (ego->isSelected())
59
60// ===========================================================================
61// static member variables
62// ===========================================================================
63
64#define INVALID_TIME -1000
65
66// the default safety gap when passing before oncoming pedestrians
67#define JM_CROSSING_GAP_DEFAULT 10
68
69// minimim width between sibling lanes to qualify as non-overlapping
70#define DIVERGENCE_MIN_WIDTH 2.5
71
72const SUMOTime MSLink::myLookaheadTime = TIME2STEPS(1);
73// additional caution is needed when approaching a zipper link
74const SUMOTime MSLink::myLookaheadTimeZipper = TIME2STEPS(16);
75std::set<std::pair<MSLink*, MSLink*> > MSLink::myRecheck;
76const double MSLink::NO_INTERSECTION(10000);
77
78// ===========================================================================
79// ConflictInfo member method definitions
80// ===========================================================================
81
82double
83MSLink::ConflictInfo::getFoeLengthBehindCrossing(const MSLink* foeExitLink) const {
84 if (flag == CONFLICT_DUMMY_MERGE) {
85 return 0;
86 } else if (foeConflictIndex >= 0) {
87 return foeExitLink->myConflicts[foeConflictIndex].lengthBehindCrossing;
88 } else {
89 return -NO_INTERSECTION;
90 }
91}
92
93double
94MSLink::ConflictInfo::getFoeConflictSize(const MSLink* foeExitLink) const {
95 if (foeConflictIndex >= 0) {
96 return foeExitLink->myConflicts[foeConflictIndex].conflictSize;
97 } else {
98 return 0;
99 }
100}
101
102double
103MSLink::ConflictInfo::getLengthBehindCrossing(const MSLink* exitLink) const {
104 if (flag == CONFLICT_STOP_AT_INTERNAL_JUNCTION) {
105 return exitLink->getInternalLaneBefore()->getLength();
106 } else {
107 return lengthBehindCrossing;
108 }
109}
110
111// ===========================================================================
112// member method definitions
113// ===========================================================================
114MSLink::MSLink(MSLane* predLane, MSLane* succLane, MSLane* via, LinkDirection dir, LinkState state,
115 double length, double foeVisibilityDistance, bool keepClear,
116 MSTrafficLightLogic* logic, int tlIndex,
117 bool indirect) :
118 myLane(succLane),
119 myLaneBefore(predLane),
120 myApproachingPersons(nullptr),
121 myIndex(-1),
122 myTLIndex(tlIndex),
123 myLogic(logic),
124 myState(state),
125 myLastGreenState(LINKSTATE_TL_GREEN_MINOR),
126 myOffState(state),
127 myLastStateChange(SUMOTime_MIN / 2), // a large negative value, but avoid overflows when subtracting
128 myDirection(dir),
129 myLength(length),
130 myFoeVisibilityDistance(foeVisibilityDistance),
131 myDistToFoePedCrossing(std::numeric_limits<double>::max()),
132 myHasFoes(false),
133 myAmCont(false),
134 myAmContOff(false),
135 myKeepClear(keepClear),
136 myInternalLane(via),
137 myInternalLaneBefore(nullptr),
138 myMesoTLSPenalty(0),
139 myGreenFraction(1),
140 myLateralShift(0),
141 myOffFoeLinks(nullptr),
142 myWalkingAreaFoe(nullptr),
143 myWalkingAreaFoeExit(nullptr),
144 myHavePedestrianCrossingFoe(false),
145 myParallelRight(nullptr),
146 myParallelLeft(nullptr),
147 myAmIndirect(indirect),
148 myRadius(std::numeric_limits<double>::max()),
149 myPermissions(myLaneBefore->getPermissions() & myLane->getPermissions() & (via == nullptr ? SVCAll : via->getPermissions())),
150 myJunction(nullptr) {
151
152 if (MSGlobals::gLateralResolution > 0) {
153 // detect lateral shift from lane geometries
154 //std::cout << "DEBUG link=" << myLaneBefore->getID() << "->" << getViaLaneOrLane()->getID() << " hasInternal=" << MSNet::getInstance()->hasInternalLinks() << " shapeBefore=" << myLaneBefore->getShape().back() << " shapeFront=" << getViaLaneOrLane()->getShape().front() << "\n";
155 if ((myInternalLane != nullptr || predLane->isInternal())
156 && myLaneBefore->getShape().back() != getViaLaneOrLane()->getShape().front()) {
157 PositionVector from = myLaneBefore->getShape();
158 const PositionVector& to = getViaLaneOrLane()->getShape();
159 const double dist = from.back().distanceTo2D(to.front());
160 // figure out direction of shift
161 try {
162 from.move2side(dist);
163 } catch (InvalidArgument&) {
164 }
165 myLateralShift = (from.back().distanceTo2D(to.front()) < dist) ? dist : -dist;
166 if (MSGlobals::gLefthand) {
167 myLateralShift *= -1;
168 }
169 //std::cout << " lateral shift link=" << myLaneBefore->getID() << "->" << getViaLaneOrLane()->getID() << " dist=" << dist << " shift=" << myLateralShift << "\n";
170 }
171 }
172}
173
174
175MSLink::~MSLink() {
176 delete myOffFoeLinks;
177 delete myApproachingPersons;
178}
179
180
181void
182MSLink::addCustomConflict(const MSLane* from, const MSLane* to, double startPos, double endPos) {
183 myCustomConflicts.push_back(CustomConflict(from, to, startPos, endPos));
184}
185
186const MSLink::CustomConflict*
187MSLink::getCustomConflict(const MSLane* foeLane) const {
188 if (myCustomConflicts.size() > 0) {
189 const MSLane* foeFrom = foeLane->getNormalPredecessorLane();
190 const MSLane* foeTo = foeLane->getNormalSuccessorLane();
191 for (const CustomConflict& cc : myCustomConflicts) {
192 if (cc.from == foeFrom && cc.to == foeTo) {
193 return &cc;
194 }
195 }
196
197 }
198 return nullptr;
199}
200
201void
202MSLink::setRequestInformation(int index, bool hasFoes, bool isCont,
203 const std::vector<MSLink*>& foeLinks,
204 const std::vector<MSLane*>& foeLanes,
205 MSLane* internalLaneBefore) {
206//#ifdef MSLink_DEBUG_CROSSING_POINTS
207// std::cout << " setRequestInformation() for junction " << getViaLaneOrLane()->getEdge().getFromJunction()->getID()
208// << "\nInternalLanes = " << toString(getViaLaneOrLane()->getEdge().getFromJunction()->getInternalLanes())
209// << std::endl;
210//#endif
211 myIndex = index;
212 myHasFoes = hasFoes;
213 myAmCont = isCont && MSGlobals::gUsingInternalLanes;
214 myFoeLinks = foeLinks;
215 for (MSLane* foeLane : foeLanes) {
216 // cannot assign vector due to const-ness
217 myFoeLanes.push_back(foeLane);
218 }
219 myJunction = const_cast<MSJunction*>(myLane->getEdge().getFromJunction()); // junctionGraph is initialized after the whole network is loaded
220 myAmContOff = isCont && myLogic != nullptr && internalLaneBefore == nullptr && checkContOff();
221 myInternalLaneBefore = internalLaneBefore;
222 MSLane* lane = nullptr;
223 if (internalLaneBefore != nullptr) {
224 // this is an exit link. compute crossing points with all foeLanes
225 lane = internalLaneBefore;
226 //} else if (myLane->getEdge().isCrossing()) {
227 // // this is the link to a pedestrian crossing. compute crossing points with all foeLanes
228 // // @note not currently used by pedestrians
229 // lane = myLane;
230 }
231 const MSLink* entryLink = getCorrespondingEntryLink();
232 if (entryLink->getOffState() == LinkState::LINKSTATE_ALLWAY_STOP && entryLink->getTLLogic() != nullptr) {
233 // TLS has "normal" right of way rules but all conflicting links are foes when switching TLS off
234 // (unless it's an internal junction link which should ignore all foes and should be ignored by all foes
235 myOffFoeLinks = new std::vector<MSLink*>();
236 if (isEntryLink()) {
237 for (MSLane* foeLane : foeLanes) {
238 assert(foeLane->isInternal() || foeLane->isCrossing());
239 MSLink* viaLink = foeLane->getIncomingLanes().front().viaLink;
240 if (viaLink->getLaneBefore()->isNormal()) {
241 myOffFoeLinks->push_back(viaLink);
242 }
243 }
244 }
245 }
246#ifdef MSLink_DEBUG_CROSSING_POINTS
247 std::cout << "link " << myIndex << " to " << getViaLaneOrLane()->getID() << " internalLaneBefore=" << (lane == 0 ? "NULL" : lane->getID()) << " has foes: " << toString(foeLanes) << "\n";
248#endif
249 if (lane != nullptr) {
250 const bool beforeInternalJunction = lane->getLinkCont()[0]->getViaLaneOrLane()->getEdge().isInternal();
251 if (lane->getIncomingLanes().size() != 1) {
252 throw ProcessError(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->getEdge().isCrossing();
306 const bool sameTarget = myLane == foeLane->getLinkCont()[0]->getLane();
307 if (sameTarget && !beforeInternalJunction && !contIntersect(lane, foeLane)) {
308 //if (myLane == foeLane->getLinkCont()[0]->getLane()) {
309 // this foeLane has the same target and merges at the end (lane exits the junction)
310 const double minDist = MIN2(DIVERGENCE_MIN_WIDTH, 0.5 * (lane->getWidth() + foeLane->getWidth()));
311 if (lane->getShape().back().distanceTo2D(foeLane->getShape().back()) >= minDist) {
312 // account for lateral shift by the entry links
313 if (foeLane->getEntryLink()->isIndirect()) {
314 myConflicts.push_back(ConflictInfo(-NO_INTERSECTION, 0)); // dummy value, never used
315#ifdef MSLink_DEBUG_CROSSING_POINTS
316 std::cout << " " << lane->getID() << " dummy merge with indirect" << foeLane->getID() << "\n";
317#endif
318 } else {
319 myConflicts.push_back(ConflictInfo(0, foeLane->getWidth(), CONFLICT_DUMMY_MERGE)); // dummy value, never used
320#ifdef MSLink_DEBUG_CROSSING_POINTS
321 std::cout << " " << lane->getID() << " dummy merge with " << foeLane->getID() << "\n";
322#endif
323 }
324 } else {
325 const double distAfterDivergence = computeDistToDivergence(lane, foeLane, minDist, false);
326 const double lbcLane = lane->interpolateGeometryPosToLanePos(distAfterDivergence);
327 myConflicts.push_back(ConflictInfo(lbcLane, foeLane->getWidth()));
328#ifdef MSLink_DEBUG_CROSSING_POINTS
329 std::cout
330 << " " << lane->getID()
331 << " merges with " << foeLane->getID()
332 << " nextLane " << lane->getLinkCont()[0]->getViaLaneOrLane()->getID()
333 << " dist1=" << myConflicts.back().lengthBehindCrossing
334 << "\n";
335#endif
336 }
337 } else {
338 std::vector<double> intersections1 = lane->getShape().intersectsAtLengths2D(foeLane->getShape());
339#ifdef MSLink_DEBUG_CROSSING_POINTS_DETAILS
340 std::cout << " intersections1=" << toString(intersections1) << "\n";
341#endif
342 bool haveIntersection = true;
343 if (intersections1.size() == 0) {
344 intersections1.push_back(-NO_INTERSECTION); // disregard this foe (using maxdouble leads to nasty problems down the line)
345 haveIntersection = false;
346 } else if (intersections1.size() > 1) {
347 std::sort(intersections1.begin(), intersections1.end());
348 }
349 std::vector<double> intersections2 = foeLane->getShape().intersectsAtLengths2D(lane->getShape());
350#ifdef MSLink_DEBUG_CROSSING_POINTS_DETAILS
351 std::cout << " intersections2=" << toString(intersections2) << "\n";
352#endif
353 if (intersections2.size() == 0) {
354 intersections2.push_back(0);
355 } else if (intersections2.size() > 1) {
356 std::sort(intersections2.begin(), intersections2.end());
357 }
358
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->getEdge().isCrossing()) {
398 flag = CONFLICT_STOP_AT_INTERNAL_JUNCTION;
399 }
400
401 if (foeLane->getEdge().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#ifdef MSLink_DEBUG_OPENED
951 if (gDebugFlag1) {
952 std::cout << SIMTIME << " opened link=" << getViaLaneOrLane()->getID() << " foeLinks=" << foeLinks.size() << "\n";
953 }
954#endif
955
956 if (MSGlobals::gUseMesoSim && impatience == 1 && !myLane->getEdge().isRoundabout()) {
957 return true;
958 }
959 const bool lastWasContRed = lastWasContState(LINKSTATE_TL_RED);
960 for (const MSLink* const link : foeLinks) {
961 if (MSGlobals::gUseMesoSim) {
962 if (link->haveRed()) {
963 continue;
964 }
965 }
966#ifdef MSLink_DEBUG_OPENED
967 if (gDebugFlag1) {
968 std::cout << SIMTIME << " foeLink=" << link->getViaLaneOrLane()->getID() << " numApproaching=" << link->getApproaching().size() << "\n";
969 if (link->getLane()->isCrossing()) {
970 std::cout << SIMTIME << " approachingPersons=" << (link->myApproachingPersons == nullptr ? "NULL" : toString(link->myApproachingPersons->size())) << "\n";
971 }
972 }
973#endif
974 if (link->blockedAtTime(arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, myLane == link->getLane(),
975 impatience, decel, waitingTime, collectFoes, ego, lastWasContRed, dist)) {
976 return false;
977 }
978 }
979 if (collectFoes != nullptr && collectFoes->size() > 0) {
980 return false;
981 }
982 return true;
983}
984
985
986bool
987MSLink::blockedAtTime(SUMOTime arrivalTime, SUMOTime leaveTime, double arrivalSpeed, double leaveSpeed,
988 bool sameTargetLane, double impatience, double decel, SUMOTime waitingTime,
989 BlockingFoes* collectFoes, const SUMOTrafficObject* ego, bool lastWasContRed, double dist) const {
990 for (const auto& it : myApproachingVehicles) {
991#ifdef MSLink_DEBUG_OPENED
992 if (gDebugFlag1) {
993 if (ego != nullptr
994 && ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_SPEED, 0) >= it.second.speed
995 && ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) > 0) {
996 std::stringstream stream; // to reduce output interleaving from different threads
997 stream << SIMTIME << " " << myApproachingVehicles.size() << " foe link=" << getViaLaneOrLane()->getID()
998 << " foeVeh=" << it.first->getID() << " (below ignore speed)"
999 << " ignoreFoeProb=" << ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0)
1000 << "\n";
1001 std::cout << stream.str();
1002 }
1003 }
1004#endif
1005 if (it.first != ego
1006 && (ego == nullptr
1007 || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) == 0
1008 || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_SPEED, 0) < it.second.speed
1009 || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) < RandHelper::rand(ego->getRNG()))
1010 && !ignoreFoe(ego, it.first)
1011 && (!lastWasContRed || it.first->getSpeed() > SUMO_const_haltingSpeed)
1012 && blockedByFoe(it.first, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, sameTargetLane,
1013 impatience, decel, waitingTime, ego)) {
1014 if (collectFoes == nullptr) {
1015 return true;
1016 } else {
1017 collectFoes->push_back(it.first);
1018 }
1019 }
1020 }
1021 if (myApproachingPersons != nullptr && !haveRed()) {
1022 for (const auto& it : *myApproachingPersons) {
1023//#ifdef MSLink_DEBUG_OPENED
1024// if (gDebugFlag1) {
1025// std::cout << SIMTIME << ": " << ego->getID() << " check person " << it.first->getID() << " aTime=" << arrivalTime << " foeATime=" << it.second.arrivalTime
1026// << " lTime=" << leaveTime << " foeLTime=" << it.second.leavingTime
1027// << " dist=" << dist << "\n";
1028// }
1029//#endif
1030 if ((ego == nullptr
1031 || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) == 0
1032 || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_SPEED, 0) < it.first->getSpeed()
1033 || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0) < RandHelper::rand(ego->getRNG()))
1034 && !ignoreFoe(ego, it.first)
1035 && !((arrivalTime > it.second.leavingTime) || (leaveTime < it.second.arrivalTime))) {
1036 if (ego == nullptr) {
1037 // during insertion
1038 if (myJunction->getType() == SumoXMLNodeType::RAIL_CROSSING) {
1039 continue;
1040 } else {
1041 return true;
1042 }
1043 }
1044 // check whether braking is feasible (ego might have started to accelerate already)
1045 const auto& cfm = ego->getVehicleType().getCarFollowModel();
1046#ifdef MSLink_DEBUG_OPENED
1047 if (gDebugFlag1) {
1048 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";
1049 }
1050#endif
1051 if (dist > cfm.brakeGap(ego->getSpeed(), cfm.getMaxDecel(), 0)) {
1052#ifdef MSLink_DEBUG_OPENED
1053 if (gDebugFlag1) {
1054 std::cout << SIMTIME << ": " << ego->getID() << " blocked by person " << it.first->getID() << "\n";
1055 }
1056#endif
1057 if (collectFoes == nullptr) {
1058 return true;
1059 } else {
1060 collectFoes->push_back(it.first);
1061 }
1062 }
1063 }
1064 }
1065 }
1066 return false;
1067}
1068
1069
1070bool
1071MSLink::blockedByFoe(const SUMOVehicle* veh, const ApproachingVehicleInformation& avi,
1072 SUMOTime arrivalTime, SUMOTime leaveTime, double arrivalSpeed, double leaveSpeed,
1073 bool sameTargetLane, double impatience, double decel, SUMOTime waitingTime,
1074 const SUMOTrafficObject* ego) const {
1075#ifdef MSLink_DEBUG_OPENED
1076 if (gDebugFlag1) {
1077 std::stringstream stream; // to reduce output interleaving from different threads
1078 stream << " link=" << getDescription()
1079 << " foeVeh=" << veh->getID()
1080 << " req=" << avi.willPass
1081 << " aT=" << avi.arrivalTime
1082 << " lT=" << avi.leavingTime
1083 << "\n";
1084 std::cout << stream.str();
1085 }
1086#endif
1087 if (!avi.willPass) {
1088 return false;
1089 }
1090 if (myState == LINKSTATE_ALLWAY_STOP) {
1091 assert(waitingTime > 0);
1092#ifdef MSLink_DEBUG_OPENED
1093 if (gDebugFlag1) {
1094 std::stringstream stream; // to reduce output interleaving from different threads
1095 stream << " foeDist=" << avi.dist
1096 << " foeBGap=" << veh->getBrakeGap(false)
1097 << " foeWait=" << avi.waitingTime
1098 << " wait=" << waitingTime
1099 << "\n";
1100 std::cout << stream.str();
1101 }
1102#endif
1103 // when using actionSteps, the foe waiting time may be outdated
1104 const SUMOTime actionDelta = SIMSTEP - veh->getLastActionTime();
1105 if (waitingTime > avi.waitingTime + actionDelta) {
1106 return false;
1107 }
1108 if (waitingTime == (avi.waitingTime + actionDelta) && arrivalTime < avi.arrivalTime + actionDelta) {
1109 return false;
1110 }
1111 }
1112 SUMOTime foeArrivalTime = avi.arrivalTime;
1113 double foeArrivalSpeedBraking = avi.arrivalSpeedBraking;
1114 if (impatience > 0 && arrivalTime < avi.arrivalTime) {
1115#ifdef MSLink_DEBUG_OPENED
1116 gDebugFlag6 = ((ego == nullptr || ego->isSelected()) && (veh == nullptr || veh->isSelected()));
1117#endif
1118 const SUMOTime fatb = computeFoeArrivalTimeBraking(arrivalTime, veh, avi.arrivalTime, impatience, avi.dist, foeArrivalSpeedBraking);
1119 foeArrivalTime = (SUMOTime)((1. - impatience) * (double)avi.arrivalTime + impatience * (double)fatb);
1120#ifdef MSLink_DEBUG_OPENED
1121 if (gDebugFlag6) {
1122 std::cout << SIMTIME << " link=" << getDescription() << " ego=" << ego->getID() << " foe=" << veh->getID()
1123 << " at=" << STEPS2TIME(arrivalTime)
1124 << " fat=" << STEPS2TIME(avi.arrivalTime)
1125 << " fatb=" << STEPS2TIME(fatb)
1126 << " fat2=" << STEPS2TIME(foeArrivalTime)
1127 << "\n";
1128 }
1129#endif
1130 }
1131
1132
1133 const SUMOTime lookAhead = (myState == LINKSTATE_ZIPPER
1134 ? myLookaheadTimeZipper
1135 : (ego == nullptr
1136 ? myLookaheadTime
1137 : TIME2STEPS(ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_TIMEGAP_MINOR, STEPS2TIME(myLookaheadTime)))));
1138 //if (ego != 0) std::cout << SIMTIME << " ego=" << ego->getID() << " jmTimegapMinor=" << ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_TIMEGAP_MINOR, -1) << " lookAhead=" << lookAhead << "\n";
1139#ifdef MSLink_DEBUG_OPENED
1140 if (gDebugFlag1 || gDebugFlag6) {
1141 std::stringstream stream; // to reduce output interleaving from different threads
1142 stream << " imp=" << impatience << " fAT2=" << foeArrivalTime << " fASb=" << foeArrivalSpeedBraking << " lA=" << lookAhead << " egoAT=" << arrivalTime << " egoLT=" << leaveTime << " egoLS=" << leaveSpeed << "\n";
1143 std::cout << stream.str();
1144 }
1145#endif
1146 if (avi.leavingTime < arrivalTime) {
1147 // ego wants to be follower
1148 if (sameTargetLane && (arrivalTime - avi.leavingTime < lookAhead
1149 || unsafeMergeSpeeds(avi.leaveSpeed, arrivalSpeed,
1150 veh->getVehicleType().getCarFollowModel().getMaxDecel(), decel))) {
1151#ifdef MSLink_DEBUG_OPENED
1152 if (gDebugFlag1 || gDebugFlag6) {
1153 std::cout << " blocked (cannot follow)\n";
1154 }
1155#endif
1156 return true;
1157 }
1158 } else if (foeArrivalTime > leaveTime + lookAhead) {
1159 // ego wants to be leader.
1160 if (sameTargetLane && unsafeMergeSpeeds(leaveSpeed, foeArrivalSpeedBraking,
1161 decel, veh->getVehicleType().getCarFollowModel().getMaxDecel())) {
1162#ifdef MSLink_DEBUG_OPENED
1163 if (gDebugFlag1 || gDebugFlag6) {
1164 std::cout << " blocked (cannot lead)\n";
1165 }
1166#endif
1167 return true;
1168 }
1169 } else {
1170 // even without considering safeHeadwayTime there is already a conflict
1171#ifdef MSLink_DEBUG_OPENED
1172 if (gDebugFlag1 || gDebugFlag6) {
1173 std::cout << " blocked (hard conflict)\n";
1174 }
1175#endif
1176 return true;
1177 }
1178 return false;
1179}
1180
1181
1182SUMOTime
1183MSLink::computeFoeArrivalTimeBraking(SUMOTime arrivalTime, const SUMOVehicle* foe, SUMOTime foeArrivalTime, double impatience, double dist, double& fasb) {
1184 // a: distance saved when foe brakes from arrivalTime to foeArrivalTime
1185 // b: distance driven past foeArrivalTime
1186 // m: permitted decceleration
1187 // d: total deceleration until foeArrivalTime
1188 // dist2: distance of foe at arrivalTime
1189 // actual arrivalTime must fall on a simulation step
1190 if (arrivalTime - arrivalTime % DELTA_T == foeArrivalTime - foeArrivalTime % DELTA_T) {
1191 // foe enters the junction in the same step
1192#ifdef MSLink_DEBUG_OPENED
1193 if (gDebugFlag6) {
1194 std::cout << " foeAT before egoAT\n";
1195 }
1196#endif
1197 return foeArrivalTime;
1198 }
1199 if (arrivalTime % DELTA_T > 0) {
1200 arrivalTime = arrivalTime - (arrivalTime % DELTA_T) + DELTA_T;
1201 }
1202 //arrivalTime += DELTA_T - arrivalTime % DELTA_T;
1203 const double m = foe->getVehicleType().getCarFollowModel().getMaxDecel() * impatience;
1204 const double dt = STEPS2TIME(foeArrivalTime - arrivalTime);
1205 const double d = dt * m;
1206 const double a = dt * d / 2;
1207 const double v = dist / STEPS2TIME(foeArrivalTime - SIMSTEP + DELTA_T);
1208 const double dist2 = dist - v * STEPS2TIME(arrivalTime - SIMSTEP);
1209#ifdef MSLink_DEBUG_OPENED
1210 if (gDebugFlag6) {
1211 std::cout << " dist=" << dist << " dist2=" << dist2
1212 << " at=" << STEPS2TIME(arrivalTime)
1213 << " fat=" << STEPS2TIME(foeArrivalTime)
1214 << " dt=" << dt << " v=" << v << " m=" << m << " d=" << d << " a=" << a << "\n";
1215 }
1216#endif
1217 if (0.5 * v * v / m <= dist2) {
1218#ifdef MSLink_DEBUG_OPENED
1219 if (gDebugFlag6) {
1220 std::cout << " canBrakeToStop\n";
1221 }
1222#endif
1223 fasb = 0;
1224 return foeArrivalTime + TIME2STEPS(30);
1225 }
1226 // a = b (foe reaches the original distance to the stop line)
1227 // x: time driven past foeArrivalTime
1228 // v: foe speed without braking
1229 // v2: average foe speed after foeArrivalTime (braking continues for time x)
1230 // v2 = (v - d - x * m / 2)
1231 // b = v2 * x
1232 // solving for x gives:
1233 const double x = (sqrt(4 * (v - d) * (v - d) - 8 * m * a) * -0.5 - d + v) / m;
1234
1235#ifdef MSLink_DEBUG_OPENED
1236 const double x2 = (sqrt(4 * (v - d) * (v - d) - 8 * m * a) * 0.5 - d + v) / m;
1237 if (gDebugFlag6 || std::isnan(x)) {
1238 std::cout << SIMTIME << " dist=" << dist << " dist2=" << dist2 << " at=" << STEPS2TIME(arrivalTime) << " m=" << m << " d=" << d << " v=" << v << " a=" << a << " x=" << x << " x2=" << x2 << "\n";
1239 }
1240#endif
1241 fasb = v - (dt + x) * m;
1242 return foeArrivalTime + TIME2STEPS(x);
1243}
1244
1245
1246bool
1247MSLink::hasApproachingFoe(SUMOTime arrivalTime, SUMOTime leaveTime, double speed, double decel) const {
1248 for (const MSLink* const link : myFoeLinks) {
1249 if (link->blockedAtTime(arrivalTime, leaveTime, speed, speed, myLane == link->getLane(), 0, decel, 0)) {
1250 return true;
1251 }
1252 }
1253 for (const MSLane* const lane : myFoeLanes) {
1254 if (lane->getVehicleNumberWithPartials() > 0) {
1255 return true;
1256 }
1257 }
1258 return false;
1259}
1260
1261
1262std::pair<const SUMOVehicle*, const MSLink*>
1263MSLink::getFirstApproachingFoe(const MSLink* wrapAround) const {
1264 double closetDist = std::numeric_limits<double>::max();
1265 const SUMOVehicle* closest = nullptr;
1266 const MSLink* foeLink = nullptr;
1267 for (MSLink* link : myFoeLinks) {
1268 for (const auto& it : link->myApproachingVehicles) {
1269 //std::cout << " link=" << getDescription() << " foeLink_in=" << link->getLaneBefore()->getID() << " wrapAround=" << wrapAround->getDescription() << "\n";
1270 if (link->getLaneBefore() == wrapAround->getLaneBefore()) {
1271 return std::make_pair(nullptr, wrapAround);
1272 } else if (it.second.dist < closetDist) {
1273 closetDist = it.second.dist;
1274 if (it.second.willPass) {
1275 closest = it.first;
1276 foeLink = link;
1277 }
1278 }
1279 }
1280 }
1281 return std::make_pair(closest, foeLink);
1282}
1283
1284
1285void
1286MSLink::setTLState(LinkState state, SUMOTime t) {
1287 if (myState != state) {
1288 myLastStateChange = t;
1289 }
1290 myState = state;
1291 if (haveGreen()) {
1292 myLastGreenState = myState;
1293 }
1294}
1295
1296
1297void
1298MSLink::setTLLogic(const MSTrafficLightLogic* logic) {
1299 myLogic = logic;
1300}
1301
1302
1303bool
1304MSLink::isCont() const {
1305 // when a traffic light is switched off minor roads have their cont status revoked
1306 return (myState == LINKSTATE_TL_OFF_BLINKING || myState == LINKSTATE_ALLWAY_STOP || myState == LINKSTATE_STOP) ? myAmContOff : myAmCont;
1307}
1308
1309
1310bool
1311MSLink::lastWasContMajor() const {
1312 if (isExitLinkAfterInternalJunction()) {
1313 return myInternalLaneBefore->getIncomingLanes()[0].viaLink->lastWasContMajor();
1314 }
1315 if (myInternalLane == nullptr || myAmCont) {
1316 return false;
1317 } else {
1318 MSLane* pred = myInternalLane->getLogicalPredecessorLane();
1319 if (!pred->getEdge().isInternal()) {
1320 return false;
1321 } else {
1322 const MSLane* const pred2 = pred->getLogicalPredecessorLane();
1323 assert(pred2 != nullptr);
1324 const MSLink* const predLink = pred2->getLinkTo(pred);
1325 assert(predLink != nullptr);
1326 if (predLink->havePriority()) {
1327 return true;
1328 }
1329 if (myHavePedestrianCrossingFoe) {
1330 return predLink->getLastGreenState() == LINKSTATE_TL_GREEN_MAJOR;
1331 } else {
1332 return predLink->haveYellow();
1333 }
1334 }
1335 }
1336}
1337
1338
1339bool
1340MSLink::lastWasContState(LinkState linkState) const {
1341 if (myInternalLane == nullptr || myAmCont || myHavePedestrianCrossingFoe) {
1342 return false;
1343 } else {
1344 MSLane* pred = myInternalLane->getLogicalPredecessorLane();
1345 if (!pred->getEdge().isInternal()) {
1346 return false;
1347 } else {
1348 const MSLane* const pred2 = pred->getLogicalPredecessorLane();
1349 assert(pred2 != nullptr);
1350 const MSLink* const predLink = pred2->getLinkTo(pred);
1351 assert(predLink != nullptr);
1352 return predLink->getState() == linkState;
1353 }
1354 }
1355}
1356
1357
1358void
1359MSLink::writeApproaching(OutputDevice& od, const std::string fromLaneID) const {
1360 if (myApproachingVehicles.size() > 0) {
1361 od.openTag("link");
1362 od.writeAttr(SUMO_ATTR_FROM, fromLaneID);
1363 const std::string via = getViaLane() == nullptr ? "" : getViaLane()->getID();
1364 od.writeAttr(SUMO_ATTR_VIA, via);
1365 od.writeAttr(SUMO_ATTR_TO, getLane() == nullptr ? "" : getLane()->getID());
1366 std::vector<std::pair<SUMOTime, const SUMOVehicle*> > toSort; // stabilize output
1367 for (auto it : myApproachingVehicles) {
1368 toSort.push_back(std::make_pair(it.second.arrivalTime, it.first));
1369 }
1370 std::sort(toSort.begin(), toSort.end());
1371 for (std::vector<std::pair<SUMOTime, const SUMOVehicle*> >::const_iterator it = toSort.begin(); it != toSort.end(); ++it) {
1372 od.openTag("approaching");
1373 const ApproachingVehicleInformation& avi = myApproachingVehicles.find(it->second)->second;
1374 od.writeAttr(SUMO_ATTR_ID, it->second->getID());
1375 od.writeAttr(SUMO_ATTR_IMPATIENCE, it->second->getImpatience());
1376 od.writeAttr("arrivalTime", time2string(avi.arrivalTime));
1377 od.writeAttr("leaveTime", time2string(avi.leavingTime));
1378 od.writeAttr("arrivalSpeed", toString(avi.arrivalSpeed));
1379 od.writeAttr("arrivalSpeedBraking", toString(avi.arrivalSpeedBraking));
1380 od.writeAttr("leaveSpeed", toString(avi.leaveSpeed));
1381 od.writeAttr("willPass", toString(avi.willPass));
1382 od.closeTag();
1383 }
1384 od.closeTag();
1385 }
1386}
1387
1388
1389double
1390MSLink::getInternalLengthsAfter() const {
1391 double len = 0.;
1392 MSLane* lane = myInternalLane;
1393
1394 while (lane != nullptr && lane->isInternal()) {
1395 len += lane->getLength();
1396 lane = lane->getLinkCont()[0]->getViaLane();
1397 }
1398 return len;
1399}
1400
1401double
1402MSLink::getInternalLengthsBefore() const {
1403 double len = 0.;
1404 const MSLane* lane = myInternalLane;
1405
1406 while (lane != nullptr && lane->isInternal()) {
1407 len += lane->getLength();
1408 if (lane->getIncomingLanes().size() == 1) {
1409 lane = lane->getIncomingLanes()[0].lane;
1410 } else {
1411 break;
1412 }
1413 }
1414 return len;
1415}
1416
1417
1418double
1419MSLink::getLengthsBeforeCrossing(const MSLane* foeLane) const {
1420 MSLane* via = myInternalLane;
1421 double totalDist = 0.;
1422 bool foundCrossing = false;
1423 while (via != nullptr) {
1424 MSLink* link = via->getLinkCont()[0];
1425 double dist = link->getLengthBeforeCrossing(foeLane);
1426 if (dist != INVALID_DOUBLE) {
1427 // found conflicting lane
1428 totalDist += dist;
1429 foundCrossing = true;
1430 break;
1431 } else {
1432 totalDist += via->getLength();
1433 via = link->getViaLane();
1434 }
1435 }
1436 if (foundCrossing) {
1437 return totalDist;
1438 } else {
1439 return INVALID_DOUBLE;
1440 }
1441}
1442
1443
1444double
1445MSLink::getLengthBeforeCrossing(const MSLane* foeLane) const {
1446 int foe_ix;
1447 for (foe_ix = 0; foe_ix != (int)myFoeLanes.size(); ++foe_ix) {
1448 if (myFoeLanes[foe_ix] == foeLane) {
1449 break;
1450 }
1451 }
1452 if (foe_ix == (int)myFoeLanes.size()) {
1453 // no conflict with the given lane, indicate by returning -1
1454#ifdef MSLink_DEBUG_CROSSING_POINTS
1455 std::cout << "No crossing of lanes '" << foeLane->getID() << "' and '" << myInternalLaneBefore->getID() << "'" << std::endl;
1456#endif
1457 return INVALID_DOUBLE;
1458 } else {
1459 // found conflicting lane index
1460 double dist = myInternalLaneBefore->getLength() - myConflicts[foe_ix].getLengthBehindCrossing(this);
1461 if (dist == -10000.) {
1462 // this is the value in myConflicts, if the relation allows intersection but none is present for the actual geometry.
1463 return INVALID_DOUBLE;
1464 }
1465#ifdef MSLink_DEBUG_CROSSING_POINTS
1466 std::cout << "Crossing of lanes '" << myInternalLaneBefore->getID() << "' and '" << foeLane->getID()
1467 << "' at distance " << dist << " (approach along '"
1468 << myInternalLaneBefore->getEntryLink()->getLaneBefore()->getID() << "')" << std::endl;
1469#endif
1470 return dist;
1471 }
1472}
1473
1474
1475bool
1476MSLink::isEntryLink() const {
1477 if (MSGlobals::gUsingInternalLanes) {
1478 return myInternalLane != nullptr && myInternalLaneBefore == nullptr;
1479 } else {
1480 return false;
1481 }
1482}
1483
1484bool
1485MSLink::isConflictEntryLink() const {
1486 // either a non-cont entry link or the link after a cont-link
1487 return !myAmCont && (isEntryLink() || (myInternalLaneBefore != nullptr && myInternalLane != nullptr));
1488}
1489
1490bool
1491MSLink::isExitLink() const {
1492 if (MSGlobals::gUsingInternalLanes) {
1493 return myInternalLaneBefore != nullptr && myInternalLane == nullptr;
1494 } else {
1495 return false;
1496 }
1497}
1498
1499bool
1500MSLink::isExitLinkAfterInternalJunction() const {
1501 if (MSGlobals::gUsingInternalLanes) {
1502 return (getInternalLaneBefore() != nullptr
1503 && myInternalLaneBefore->getIncomingLanes().size() == 1
1504 && myInternalLaneBefore->getIncomingLanes().front().viaLink->isInternalJunctionLink());
1505 } else {
1506 return false;
1507 }
1508}
1509
1510
1511const MSLink*
1512MSLink::getCorrespondingExitLink() const {
1513 MSLane* lane = myInternalLane;
1514 const MSLink* link = this;
1515 while (lane != nullptr) {
1516 link = lane->getLinkCont()[0];
1517 lane = link->getViaLane();
1518 }
1519 return link;
1520}
1521
1522
1523const MSLink*
1524MSLink::getCorrespondingEntryLink() const {
1525 const MSLink* link = this;
1526 while (link->myLaneBefore->isInternal()) {
1527 assert(myLaneBefore->getIncomingLanes().size() == 1);
1528 link = link->myLaneBefore->getIncomingLanes().front().viaLink;
1529 }
1530 return link;
1531}
1532
1533
1534bool
1535MSLink::isInternalJunctionLink() const {
1536 return getInternalLaneBefore() != nullptr && myInternalLane != nullptr;
1537}
1538
1539
1540const MSLink::LinkLeaders
1541MSLink::getLeaderInfo(const MSVehicle* ego, double dist, std::vector<const MSPerson*>* collectBlockers, bool isShadowLink) const {
1542 LinkLeaders result;
1543 // this link needs to start at an internal lane (either an exit link or between two internal lanes)
1544 // or it must be queried by the pedestrian model (ego == 0)
1545 if (ego != nullptr && (!fromInternalLane() || ego->getLaneChangeModel().isOpposite())) {
1546 // ignore link leaders
1547 return result;
1548 }
1549 //gDebugFlag1 = true;
1550 if (gDebugFlag1) {
1551 std::cout << SIMTIME << " getLeaderInfo link=" << getDescription() << " dist=" << dist << " isShadowLink=" << isShadowLink << "\n";
1552 }
1553 if (MSGlobals::gComputeLC && ego != nullptr && ego->getLane()->isNormal()) {
1554 const MSLink* junctionEntry = getLaneBefore()->getEntryLink();
1555 if (junctionEntry->haveRed() && !ego->ignoreRed(junctionEntry, true)
1556 // check oncoming on bidiLane during laneChanging
1557 && (!MSGlobals::gComputeLC || junctionEntry->getLaneBefore()->getBidiLane() == nullptr)) {
1558 if (gDebugFlag1) {
1559 std::cout << " ignore linkLeaders beyond red light\n";
1560 }
1561 return result;
1562 }
1563 }
1564 // this is an exit link
1565 const double extraGap = ego != nullptr ? ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_EXTRA_GAP, 0) : 0;
1566 for (int i = 0; i < (int)myFoeLanes.size(); ++i) {
1567 const MSLane* foeLane = myFoeLanes[i];
1568 const MSLink* foeExitLink = foeLane->getLinkCont()[0];
1569 // distance from the querying vehicle to the crossing point with foeLane
1570 double distToCrossing = dist - myConflicts[i].getLengthBehindCrossing(this);
1571 const double foeDistToCrossing = foeLane->getLength() - myConflicts[i].getFoeLengthBehindCrossing(foeExitLink);
1572 const bool sameTarget = (myLane == foeExitLink->getLane()) && !isInternalJunctionLink() && !foeExitLink->isInternalJunctionLink();
1573 const bool sameSource = (myInternalLaneBefore != nullptr && myInternalLaneBefore->getNormalPredecessorLane() == foeLane->getNormalPredecessorLane());
1574 const double crossingWidth = (sameTarget || sameSource) ? 0 : myConflicts[i].conflictSize;
1575 const double foeCrossingWidth = (sameTarget || sameSource) ? 0 : myConflicts[i].getFoeConflictSize(foeExitLink);
1576 // special treatment of contLane foe only applies if this lane is not a contLane or contLane follower itself
1577 const bool contLane = (foeExitLink->getViaLaneOrLane()->getEdge().isInternal() && !(
1578 isInternalJunctionLink() || isExitLinkAfterInternalJunction()));
1579 if (gDebugFlag1) {
1580 std::cout << " distToCrossing=" << distToCrossing << " foeLane=" << foeLane->getID() << " cWidth=" << crossingWidth
1581 << " ijl=" << isInternalJunctionLink() << " sT=" << sameTarget << " sS=" << sameSource
1582 << " lbc=" << myConflicts[i].getLengthBehindCrossing(this)
1583 << " flbc=" << myConflicts[i].getFoeLengthBehindCrossing(foeExitLink)
1584 << " cw=" << crossingWidth
1585 << " fcw=" << foeCrossingWidth
1586 << " contLane=" << contLane
1587 << " state=" << toString(myState)
1588 << " foeState=" << toString(foeExitLink->getState())
1589 << "\n";
1590 }
1591 if (distToCrossing + crossingWidth < 0 && !sameTarget
1592 && (ego == nullptr || !MSGlobals::gComputeLC || distToCrossing + crossingWidth + ego->getVehicleType().getLength() < 0)) {
1593 if (gDebugFlag1) {
1594 std::cout << " ignore:egoBeyondCrossingPoint\n";
1595 }
1596 continue; // vehicle is behind the crossing point, continue with next foe lane
1597 }
1598 bool ignoreGreenCont = false;
1599 bool foeIndirect = false;
1600 if (contLane) {
1601 const MSLink* entry = getLaneBefore()->getEntryLink();
1602 const MSLink* foeEntry = foeLane->getEntryLink();
1603 foeIndirect = foeEntry->myAmIndirect;
1604 if (entry != nullptr && entry->haveGreen()
1605 && foeEntry != nullptr && foeEntry->haveGreen()
1606 && entry->myLaneBefore != foeEntry->myLaneBefore) {
1607 // ignore vehicles before an internaljunction as long as they are still in green minor mode
1608 ignoreGreenCont = true;
1609 }
1610 }
1611 if (foeIndirect && distToCrossing >= NO_INTERSECTION) {
1612 if (gDebugFlag1) {
1613 std::cout << " ignore:noIntersection\n";
1614 }
1615 continue;
1616 }
1617 // it is not sufficient to return the last vehicle on the foeLane because ego might be its leader
1618 // therefore we return all vehicles on the lane
1619 //
1620 // special care must be taken for continuation lanes. (next lane is also internal)
1621 // vehicles on cont. lanes or on internal lanes with the same target as this link can not be ignored
1622 // and should block (gap = -1) unless they are part of an indirect turn
1623 MSLane::AnyVehicleIterator end = foeLane->anyVehiclesEnd();
1624 for (MSLane::AnyVehicleIterator it_veh = foeLane->anyVehiclesBegin(); it_veh != end; ++it_veh) {
1625 MSVehicle* leader = (MSVehicle*)*it_veh;
1626 const double leaderBack = leader->getBackPositionOnLane(foeLane) - extraGap;
1627 const double leaderBackDist = foeDistToCrossing - leaderBack;
1628 const double l2 = ego != nullptr ? ego->getLength() + 2 : 0; // add some slack to account for further meeting-angle effects
1629 const double sagitta = ego != nullptr && myRadius != std::numeric_limits<double>::max() ? myRadius - sqrt(myRadius * myRadius - 0.25 * l2 * l2) : 0;
1630 const bool pastTheCrossingPoint = leaderBackDist + foeCrossingWidth + sagitta < 0;
1631 const bool enteredTheCrossingPoint = leaderBackDist < leader->getVehicleType().getLength();
1632 const bool foeIsBicycleTurn = (leader->getVehicleType().getVehicleClass() == SVC_BICYCLE
1633 && foeLane->getIncomingLanes().front().viaLink->getDirection() == LinkDirection::LEFT);
1634 const bool ignoreIndirectBicycleTurn = pastTheCrossingPoint && foeIsBicycleTurn;
1635 const bool cannotIgnore = ((contLane && !ignoreIndirectBicycleTurn) || sameTarget || (sameSource && !MSGlobals::gComputeLC)) && ego != nullptr;
1636 const bool inTheWay = ((((!pastTheCrossingPoint && distToCrossing > 0) || (sameTarget && distToCrossing > leaderBackDist - leader->getLength()))
1637 && (enteredTheCrossingPoint || (sameSource && !enteredTheCrossingPoint && foeDistToCrossing < distToCrossing))
1638 && (!foeExitLink->isInternalJunctionLink() || foeIsBicycleTurn || sameSource))
1639 || foeExitLink->getLaneBefore()->getNormalPredecessorLane() == myLane->getBidiLane());
1640 const bool isOpposite = leader->getLaneChangeModel().isOpposite();
1641 const auto avi = foeExitLink->getApproaching(leader);
1642 // if leader is not found, assume that it performed a lane change in the last step
1643 const bool willPass = avi.willPass || (avi.arrivalTime == INVALID_TIME && sameTarget);
1644 if (gDebugFlag1) {
1645 std::cout << " candidate leader=" << leader->getID()
1646 << " cannotIgnore=" << cannotIgnore
1647 << " fdtc=" << foeDistToCrossing
1648 << " lb=" << leaderBack
1649 << " lbd=" << leaderBackDist
1650 << " fcwidth=" << foeCrossingWidth
1651 << " r=" << myRadius
1652 << " sagitta=" << sagitta
1653 << " foePastCP=" << pastTheCrossingPoint
1654 << " foeEnteredCP=" << enteredTheCrossingPoint
1655 << " inTheWay=" << inTheWay
1656 << " willPass=" << willPass
1657 << " isFrontOnLane=" << leader->isFrontOnLane(foeLane)
1658 << " ignoreGreenCont=" << ignoreGreenCont
1659 << " foeIndirect=" << foeIndirect
1660 << " foeBikeTurn=" << foeIsBicycleTurn
1661 << " isOpposite=" << isOpposite << "\n";
1662 }
1663 if (leader == ego) {
1664 continue;
1665 }
1666 // ignore greenCont foe vehicles that are not in the way
1667 if (!inTheWay && ignoreGreenCont) {
1668 if (gDebugFlag1) {
1669 std::cout << " ignoreGreenCont\n";
1670 }
1671 continue;
1672 }
1673 // after entering the conflict area, ignore foe vehicles that are not in the way
1674 if ((!MSGlobals::gComputeLC || (ego != nullptr && ego->getLane() == foeLane) || MSGlobals::gSublane)
1675 && distToCrossing < -POSITION_EPS && !inTheWay
1676 && (ego == nullptr || !MSGlobals::gComputeLC || distToCrossing < -ego->getVehicleType().getLength())) {
1677 if (gDebugFlag1) {
1678 std::cout << " ego entered conflict area\n";
1679 }
1680 continue;
1681 }
1682 if (!MSGlobals::gComputeLC
1683 && sameSource
1684 && &ego->getLane()->getEdge() == &myInternalLaneBefore->getEdge()
1685 && leaderBack + leader->getLength() < ego->getPositionOnLane() - ego->getLength()) {
1686 // ego is already on the junction and clearly ahead of foe
1687 if (gDebugFlag1) {
1688 std::cout << " ego ahead of same-source foe\n";
1689 }
1690 continue;
1691 }
1692
1693 // ignore foe vehicles that will not pass
1694 if ((!cannotIgnore || leader->isStopped() || sameTarget)
1695 && !willPass
1696 && (avi.arrivalTime == INVALID_TIME || leader->getSpeed() < SUMO_const_haltingSpeed)
1697 && leader->isFrontOnLane(foeLane)
1698 && !isOpposite
1699 && !inTheWay
1700 // willPass is false if the vehicle is already on the stopping edge
1701 && !leader->willStop()) {
1702 if (gDebugFlag1) {
1703 std::cout << " foe will not pass\n";
1704 }
1705 continue;
1706 }
1707 if (leader->isBidiOn(foeLane)) {
1708 // conflict resolved via forward lane of the foe
1709 continue;
1710 }
1711 // check whether foe is blocked and might need to change before leaving the junction
1712 const bool foeStrategicBlocked = (leader->getLaneChangeModel().isStrategicBlocked() &&
1713 leader->getCarFollowModel().brakeGap(leader->getSpeed()) <= foeLane->getLength() - leaderBack);
1714 const bool sameInternalEdge = &myInternalLaneBefore->getEdge() == &foeExitLink->getInternalLaneBefore()->getEdge();
1715
1716 const bool foeLaneIsBidi = myInternalLaneBefore->getBidiLane() == foeLane;
1717 if (MSGlobals::gSublane && ego != nullptr && (sameSource || sameTarget || foeLaneIsBidi)
1718 && (!foeStrategicBlocked || sameInternalEdge)) {
1719 if (ego->getLane() == leader->getLane()) {
1720 continue;
1721 }
1722 // ignore vehicles if not in conflict sublane-wise
1723 const double egoLatOffset = isShadowLink ? ego->getLatOffset(ego->getLaneChangeModel().getShadowLane()) : 0;
1724 const double posLat = ego->getLateralPositionOnLane() + egoLatOffset;
1725 double posLatLeader = leader->getLateralPositionOnLane() + leader->getLatOffset(foeLane);
1726 if (foeLaneIsBidi) {
1727 // leader is oncoming
1728 posLatLeader = foeLane->getWidth() - posLatLeader;
1729 }
1730 const double latGap = (fabs(posLat - posLatLeader)
1731 - 0.5 * (ego->getVehicleType().getWidth() + leader->getVehicleType().getWidth()));
1732 const double maneuverDist = leader->getLaneChangeModel().getManeuverDist() * (posLat < posLatLeader ? -1 : 1);
1733 if (gDebugFlag1) {
1734 std::cout << " checkIgnore sublaneFoe lane=" << myInternalLaneBefore->getID()
1735 << " sameSource=" << sameSource
1736 << " sameTarget=" << sameTarget
1737 << " foeLaneIsBidi=" << foeLaneIsBidi
1738 << " foeLane=" << foeLane->getID()
1739 << " leader=" << leader->getID()
1740 << " egoLane=" << ego->getLane()->getID()
1741 << " leaderLane=" << leader->getLane()->getID()
1742 << " egoLat=" << posLat
1743 << " egoLatOffset=" << egoLatOffset
1744 << " leaderLat=" << posLatLeader
1745 << " leaderLatOffset=" << leader->getLatOffset(foeLane)
1746 << " latGap=" << latGap
1747 << " maneuverDist=" << maneuverDist
1748 << " computeLC=" << MSGlobals::gComputeLC
1749 << " egoMaxSpeedLat=" << ego->getVehicleType().getMaxSpeedLat()
1750 << "\n";
1751 }
1752 if (latGap > 0 && (latGap > maneuverDist || !sameTarget || !MSGlobals::gComputeLC)
1753 // do not perform sublane changes that interfere with the leader vehicle
1754 && (!MSGlobals::gComputeLC || latGap > ego->getVehicleType().getMaxSpeedLat())) {
1755 const MSLink* foeEntryLink = foeLane->getIncomingLanes().front().viaLink;
1756 if (sameSource) {
1757 // for lanes from the same edge, higer index implies a
1758 // connection further to the left
1759 const bool leaderFromRight = (myIndex > foeEntryLink->getIndex());
1760 if ((posLat > posLatLeader) == leaderFromRight) {
1761 // ignore speed since lanes diverge
1762 if (gDebugFlag1) {
1763 std::cout << " ignored (same source) leaderFromRight=" << leaderFromRight << "\n";
1764 }
1765 continue;
1766 }
1767 } else if (sameTarget) {
1768 // for lanes from different edges we cannot rely on the
1769 // index due to wrap-around issues
1770 if (myDirection != foeEntryLink->getDirection()) {
1771 bool leaderFromRight = foeEntryLink->getDirection() < myDirection;
1772 // leader vehicle should not move towards ego
1773 if (MSGlobals::gLefthand) {
1774 leaderFromRight = !leaderFromRight;
1775 }
1776 if ((posLat > posLatLeader) == leaderFromRight
1777 // leader should keep lateral position or move away from ego
1778 && (leader->getLaneChangeModel().getSpeedLat() == 0
1779 || leaderFromRight == (leader->getLaneChangeModel().getSpeedLat() < latGap))
1780 && (ego->getLaneChangeModel().getSpeedLat() == 0
1781 || leaderFromRight == (ego->getLaneChangeModel().getSpeedLat() > latGap))) {
1782 if (gDebugFlag1) {
1783 std::cout << " ignored (different source) leaderFromRight=" << leaderFromRight << "\n";
1784 }
1785 continue;
1786 }
1787 } else {
1788 // XXX figure out relative direction somehow
1789 }
1790 } else {
1791 if (gDebugFlag1) {
1792 std::cout << " ignored oncoming bidi leader\n";
1793 }
1794 continue;
1795 }
1796 }
1797 }
1798 if (leader->getWaitingTime() < MSGlobals::gIgnoreJunctionBlocker) {
1799 // compute distance between vehicles on the superimposition of both lanes
1800 // where the crossing point is the common point
1801 double gap;
1802 bool fromLeft = true;
1803 if (ego == nullptr) {
1804 // request from pedestrian model. return distance between leaderBack and crossing point
1805 //std::cout << " foeLane=" << foeLane->getID() << " leaderBack=" << leaderBack << " foeDistToCrossing=" << foeDistToCrossing << " foeLength=" << foeLane->getLength() << " foebehind=" << myConflicts[i].second << " dist=" << dist << " behind=" << myConflicts[i].first << "\n";
1806 gap = leaderBackDist;
1807 // distToCrossing should not take into account the with of the foe lane
1808 // (which was subtracted in setRequestInformation)
1809 // Instead, the width of the foe vehicle is used directly by the caller.
1810 distToCrossing += myConflicts[i].conflictSize / 2;
1811 if (gap + foeCrossingWidth < 0) {
1812 // leader is completely past the crossing point
1813 // or there is no crossing point
1814 continue; // next vehicle
1815 }
1816 // we need to determine whether the vehicle passes the
1817 // crossing from the left or the right (heuristic)
1818 fromLeft = foeDistToCrossing > 0.5 * foeLane->getLength();
1819 } else if ((contLane && !sameSource && !ignoreIndirectBicycleTurn) || isOpposite) {
1820 gap = -std::numeric_limits<double>::max(); // always break for vehicles which are on a continuation lane or for opposite-direction vehicles
1821 } else {
1822 if (pastTheCrossingPoint && !sameTarget) {
1823 // leader is completely past the crossing point
1824 // or there is no crossing point
1825 if (gDebugFlag1) {
1826 std::cout << " foePastCP ignored\n";
1827 }
1828 continue;
1829 }
1830 double leaderBackDist2 = leaderBackDist;
1831 if (sameTarget && leaderBackDist2 < 0) {
1832 const double mismatch = myConflicts[i].getFoeLengthBehindCrossing(foeExitLink) - myConflicts[i].getLengthBehindCrossing(this);
1833 if (mismatch > 0) {
1834 leaderBackDist2 += mismatch;
1835 }
1836 }
1837 if (gDebugFlag1) {
1838 std::cout << " distToCrossing=" << distToCrossing << " leaderBack=" << leaderBack
1839 << " backDist=" << leaderBackDist
1840 << " backDist2=" << leaderBackDist2
1841 << " blockedStrategic=" << leader->getLaneChangeModel().isStrategicBlocked()
1842 << "\n";
1843 }
1844 gap = distToCrossing - ego->getVehicleType().getMinGap() - leaderBackDist2 - foeCrossingWidth;
1845 }
1846 // if the foe is already moving off the intersection, we may
1847 // advance up to the crossing point unless we have the same target or same source
1848 // (for sameSource, the crossing point indicates the point of divergence)
1849 const bool stopAsap = ((leader->isFrontOnLane(foeLane) ? cannotIgnore : (sameTarget || sameSource))
1850 || (ego != nullptr && ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_ADVANCE, 1.0) == 0.0));
1851 if (gDebugFlag1) {
1852 std::cout << " leader=" << leader->getID() << " contLane=" << contLane << " cannotIgnore=" << cannotIgnore << " stopAsap=" << stopAsap << " gap=" << gap << "\n";
1853 }
1854 if (ignoreFoe(ego, leader)) {
1855 continue;
1856 }
1857 const int llFlags = ((fromLeft ? LL_FROM_LEFT : 0) |
1858 (inTheWay ? LL_IN_THE_WAY : 0) |
1859 (sameSource ? LL_SAME_SOURCE : 0) |
1860 (sameTarget ? LL_SAME_TARGET : 0));
1861 result.emplace_back(leader, gap, stopAsap ? -1 : distToCrossing, llFlags, leader->getLatOffset(foeLane));
1862 }
1863
1864 }
1865 if (ego != nullptr && MSNet::getInstance()->hasPersons()) {
1866 // check for crossing pedestrians (keep driving if already on top of the crossing
1867 const double distToPeds = distToCrossing - ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_STOPLINE_CROSSING_GAP, MSPModel::SAFETY_GAP);
1868 const double vehWidth = ego->getVehicleType().getWidth() + MSPModel::SAFETY_GAP; // + configurable safety gap
1870 // @check lefthand?!
1871 const bool wayIn = myConflicts[i].lengthBehindCrossing < myLaneBefore->getLength() * 0.5;
1872 const double vehCenter = (foeDistToCrossing + myLaneBefore->getWidth() * 0.5
1873 + ego->getLateralPositionOnLane() * (wayIn ? -1 : 1));
1874 // can access the movement model here since we already checked for existing persons above
1875 if (distToPeds >= -MSPModel::SAFETY_GAP && MSNet::getInstance()->getPersonControl().getMovementModel()->blockedAtDist(ego, foeLane, vehCenter, vehWidth,
1876 ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_CROSSING_GAP, JM_CROSSING_GAP_DEFAULT),
1877 collectBlockers)) {
1878 result.emplace_back(nullptr, -1, distToPeds);
1879 } else if (foeLane->isCrossing() && ego->getLane()->isInternal() && ego->getLane()->getEdge().getToJunction() == myJunction) {
1880 const MSLink* crossingLink = foeLane->getIncomingLanes()[0].viaLink;
1881 if (distToCrossing > 0 && crossingLink->havePriority() && crossingLink->myApproachingPersons != nullptr) {
1882 // a person might step on the crossing at any moment, since ego
1883 // is already on the junction, the opened() check is not done anymore
1884 const double timeToEnterCrossing = distToCrossing / MAX2(ego->getSpeed(), 1.0);
1885 for (const auto& item : (*crossingLink->myApproachingPersons)) {
1886 if (!ignoreFoe(ego, item.first) && timeToEnterCrossing > STEPS2TIME(item.second.arrivalTime - SIMSTEP)) {
1887 if (gDebugFlag1) {
1888 std::cout << SIMTIME << ": " << ego->getID() << " breaking for approaching person " << item.first->getID()
1889 //<< " dtc=" << distToCrossing << " ttc=" << distToCrossing / MAX2(ego->getSpeed(), 1.0) << " foeAT=" << item.second.arrivalTime << " foeTTC=" << STEPS2TIME(item.second.arrivalTime - SIMSTEP)
1890 << "\n";
1891 }
1892 result.emplace_back(nullptr, -1, distToPeds);
1893 break;
1894 //} else {
1895 // if (gDebugFlag1) {
1896 // std::cout << SIMTIME << ": " << ego->getID() << " notBreaking for approaching person " << item.first->getID()
1897 // << " dtc=" << distToCrossing << " ttc=" << distToCrossing / MAX2(ego->getSpeed(), 1.0) << " foeAT=" << item.second.arrivalTime << " foeTTC=" << STEPS2TIME(item.second.arrivalTime - SIMSTEP)
1898 // << "\n";
1899 // }
1900 }
1901 }
1902 }
1903 }
1904 }
1905 }
1906
1907 //std::cout << SIMTIME << " ego=" << Named::getIDSecure(ego) << " link=" << getViaLaneOrLane()->getID() << " myWalkingAreaFoe=" << Named::getIDSecure(myWalkingAreaFoe) << "\n";
1908 if (ego != nullptr) {
1909 checkWalkingAreaFoe(ego, myWalkingAreaFoe, collectBlockers, result);
1910 checkWalkingAreaFoe(ego, myWalkingAreaFoeExit, collectBlockers, result);
1911 }
1912
1913 if (MSGlobals::gLateralResolution > 0 && ego != nullptr && !isShadowLink) {
1914 // check for foes on the same edge
1915 for (std::vector<MSLane*>::const_iterator it = mySublaneFoeLanes.begin(); it != mySublaneFoeLanes.end(); ++it) {
1916 const MSLane* foeLane = *it;
1917 MSLane::AnyVehicleIterator end = foeLane->anyVehiclesEnd();
1918 for (MSLane::AnyVehicleIterator it_veh = foeLane->anyVehiclesBegin(); it_veh != end; ++it_veh) {
1919 MSVehicle* leader = (MSVehicle*)*it_veh;
1920 if (leader == ego) {
1921 continue;
1922 }
1923 if (leader->getLane()->isNormal()) {
1924 // leader is past the conflict point
1925 continue;
1926 }
1927 const double maxLength = MAX2(myInternalLaneBefore->getLength(), foeLane->getLength());
1928 const double gap = dist - maxLength - ego->getVehicleType().getMinGap() + leader->getBackPositionOnLane(foeLane) - extraGap;
1929 if (gap < -(ego->getVehicleType().getMinGap() + leader->getLength())) {
1930 // ego is ahead of leader
1931 continue;
1932 }
1933 const double posLat = ego->getLateralPositionOnLane();
1934 const double posLatLeader = leader->getLateralPositionOnLane() + leader->getLatOffset(foeLane);
1935 if (gDebugFlag1) {
1936 std::cout << " sublaneFoe lane=" << myInternalLaneBefore->getID()
1937 << " foeLane=" << foeLane->getID()
1938 << " leader=" << leader->getID()
1939 << " egoLane=" << ego->getLane()->getID()
1940 << " leaderLane=" << leader->getLane()->getID()
1941 << " gap=" << gap
1942 << " egoLat=" << posLat
1943 << " leaderLat=" << posLatLeader
1944 << " leaderLatOffset=" << leader->getLatOffset(foeLane)
1945 << " egoIndex=" << myInternalLaneBefore->getIndex()
1946 << " foeIndex=" << foeLane->getIndex()
1947 << " dist=" << dist
1948 << " leaderBack=" << leader->getBackPositionOnLane(foeLane)
1949 << "\n";
1950 }
1951 // there only is a conflict if the paths cross
1952 if ((posLat < posLatLeader && myInternalLaneBefore->getIndex() > foeLane->getIndex())
1953 || (posLat > posLatLeader && myInternalLaneBefore->getIndex() < foeLane->getIndex())) {
1954 if (gDebugFlag1) {
1955 std::cout << SIMTIME << " blocked by " << leader->getID() << " (sublane split) foeLane=" << foeLane->getID() << "\n";
1956 }
1957 if (ignoreFoe(ego, leader)) {
1958 continue;
1959 }
1960 result.emplace_back(leader, gap, -1);
1961 }
1962 }
1963 }
1964 }
1965 return result;
1966}
1967
1968
1969void
1970MSLink::checkWalkingAreaFoe(const MSVehicle* ego, const MSLane* foeLane, std::vector<const MSPerson*>* collectBlockers, LinkLeaders& result) const {
1971 if (foeLane != nullptr && foeLane->getEdge().getPersons().size() > 0) {
1972 // pedestrians may be on an arbitrary path across this
1973 // walkingarea. make sure to keep enough distance.
1974 // This is a simple but conservative solution that could be improved
1975 // by ignoring pedestrians that are "obviously" not on a collision course
1976 double distToPeds = std::numeric_limits<double>::max();
1977 assert(myInternalLaneBefore != nullptr);
1978 PositionVector egoPath = myInternalLaneBefore->getShape();
1979 if (ego->getLateralPositionOnLane() != 0) {
1980 egoPath.move2side((MSGlobals::gLefthand ? 1 : -1) * ego->getLateralPositionOnLane());
1981 }
1982 for (MSTransportable* t : foeLane->getEdge().getPersons()) {
1983 MSPerson* p = static_cast<MSPerson*>(t);
1984 double dist = ego->getPosition().distanceTo2D(p->getPosition()) - p->getVehicleType().getLength();
1985 const bool inFront = isInFront(ego, egoPath, p->getPosition()) || isInFront(ego, egoPath, getFuturePosition(p));
1986 if (inFront) {
1987 dist -= MAX2(ego->getVehicleType().getMinGap(), MSPModel::SAFETY_GAP);
1988 }
1989#ifdef DEBUG_WALKINGAREA
1990 if (ego->isSelected()) {
1991 std::cout << SIMTIME << " veh=" << ego->getID() << " ped=" << p->getID()
1992 << " pos=" << ego->getPosition() << " pedPos=" << p->getPosition()
1993 << " futurePedPos=" << getFuturePosition(p)
1994 << " rawDist=" << ego->getPosition().distanceTo2D(p->getPosition())
1995 << " inFront=" << inFront
1996 << " dist=" << dist << "\n";
1997 }
1998#endif
1999 if (dist < ego->getVehicleType().getWidth() / 2 || inFront) {
2000 if (inFront) {
2001 const double oncomingFactor = isOnComingPed(ego, p);
2002 if (oncomingFactor > 0) {
2003 // account for pedestrian movement while closing in
2004 const double timeToStop = sqrt(dist) / 2;
2005 const double pedDist = p->getMaxSpeed() * MAX2(timeToStop, TS) * oncomingFactor;
2006 dist = MAX2(0.0, dist - pedDist);
2007#ifdef DEBUG_WALKINGAREA
2008 if (ego->isSelected()) {
2009 std::cout << " timeToStop=" << timeToStop << " pedDist=" << pedDist << " factor=" << oncomingFactor << " dist2=" << dist << "\n";
2010 }
2011#endif
2012 }
2013 }
2014 if (ignoreFoe(ego, p)) {
2015 continue;
2016 }
2017 distToPeds = MIN2(distToPeds, dist);
2018 if (collectBlockers != nullptr) {
2019 collectBlockers->push_back(p);
2020 }
2021 }
2022 }
2023 if (distToPeds != std::numeric_limits<double>::max()) {
2024 // leave extra space in front
2025 result.emplace_back(nullptr, -1, distToPeds);
2026 }
2027 }
2028}
2029
2030bool
2031MSLink::isInFront(const MSVehicle* ego, const PositionVector& egoPath, const Position& pPos) const {
2032 const double pedAngle = ego->getPosition().angleTo2D(pPos);
2033 const double angleDiff = fabs(GeomHelper::angleDiff(ego->getAngle(), pedAngle));
2034#ifdef DEBUG_WALKINGAREA
2035 if (ego->isSelected()) {
2036 std::cout << " angleDiff=" << RAD2DEG(angleDiff) << "\n";
2037 }
2038#endif
2039 if (angleDiff < DEG2RAD(75)) {
2040 return egoPath.distance2D(pPos) < ego->getVehicleType().getWidth() + MSPModel::SAFETY_GAP;
2041 }
2042 return false;
2043}
2044
2045
2046double
2047MSLink::isOnComingPed(const MSVehicle* ego, const MSPerson* p) const {
2048 const double pedToEgoAngle = p->getPosition().angleTo2D(ego->getPosition());
2049 const double angleDiff = fabs(GeomHelper::angleDiff(p->getAngle(), pedToEgoAngle));
2050#ifdef DEBUG_WALKINGAREA
2051 if (ego->isSelected()) {
2052 std::cout << " ped-angleDiff=" << RAD2DEG(angleDiff) << " res=" << cos(angleDiff) << "\n";
2053 }
2054#endif
2055 if (angleDiff <= DEG2RAD(90)) {
2056 ;
2057 return cos(angleDiff);
2058 } else {
2059 return 0;
2060 }
2061}
2062
2063
2064Position
2065MSLink::getFuturePosition(const MSPerson* p, double timeHorizon) const {
2066 const double a = p->getAngle();
2067 const double dist = timeHorizon * p->getMaxSpeed();
2068
2069 const Position offset(cos(a) * dist, sin(a) * dist);
2070 return p->getPosition() + offset;
2071}
2072
2073
2074MSLink*
2075MSLink::getParallelLink(int direction) const {
2076 if (direction == -1) {
2077 return myParallelRight;
2078 } else if (direction == 1) {
2079 return myParallelLeft;
2080 } else {
2081 assert(false || myLane->getOpposite() != nullptr);
2082 return nullptr;
2083 }
2084}
2085
2086MSLink*
2087MSLink::getOppositeDirectionLink() const {
2088 if (myLane->getOpposite() != nullptr && myLaneBefore->getOpposite() != nullptr) {
2089 for (MSLink* cand : myLane->getOpposite()->getLinkCont()) {
2090 if (cand->getLane() == myLaneBefore->getOpposite()) {
2091 return cand;
2092 }
2093 }
2094 }
2095 return nullptr;
2096}
2097
2098
2099MSLink*
2100MSLink::computeParallelLink(int direction) {
2101 const MSLane* const before = getLaneBefore()->getParallelLane(direction, false);
2102 const MSLane* const after = getLane()->getParallelLane(direction, false);
2103 if (before != nullptr && after != nullptr) {
2104 for (MSLink* const link : before->getLinkCont()) {
2105 if (link->getLane() == after) {
2106 return link;
2107 }
2108 }
2109 }
2110 return nullptr;
2111}
2112
2113
2114double
2115MSLink::getZipperSpeed(const MSVehicle* ego, const double dist, double vSafe,
2116 SUMOTime arrivalTime,
2117 const BlockingFoes* foes) const {
2118 if (myFoeLinks.size() == 0) {
2119 // link should have LINKSTATE_MAJOR in this case
2120 assert(false);
2121 return vSafe;
2122 }
2123 const double brakeGap = ego->getCarFollowModel().brakeGap(vSafe, ego->getCarFollowModel().getMaxDecel(), TS);
2124 if (dist > MAX2(myFoeVisibilityDistance, brakeGap)) {
2125#ifdef DEBUG_ZIPPER
2126 const SUMOTime now = MSNet::getInstance()->getCurrentTimeStep();
2127 DEBUGOUT(DEBUG_COND_ZIPPER, SIMTIME << " getZipperSpeed ego=" << ego->getID()
2128 << " dist=" << dist << " bGap=" << brakeGap << " ignoring foes (arrival in " << STEPS2TIME(arrivalTime - now) << ")\n")
2129#endif
2130 return vSafe;
2131 }
2132#ifdef DEBUG_ZIPPER
2133 DEBUGOUT(DEBUG_COND_ZIPPER, SIMTIME << " getZipperSpeed ego=" << ego->getID()
2134 << " egoAT=" << arrivalTime
2135 << " dist=" << dist
2136 << " brakeGap=" << brakeGap
2137 << " vSafe=" << vSafe
2138 << " numFoes=" << foes->size()
2139 << "\n")
2140#endif
2141 const bool uniqueFoeLink = myFoeLinks.size() == 1;
2142 MSLink* foeLink = myFoeLinks[0];
2143 for (const auto& item : *foes) {
2144 if (!item->isVehicle()) {
2145 continue;
2146 }
2147 const MSVehicle* foe = dynamic_cast<const MSVehicle*>(item);
2148 assert(foe != 0);
2149 const ApproachingVehicleInformation* aviPtr = nullptr;
2150 if (uniqueFoeLink) {
2151 aviPtr = foeLink->getApproachingPtr(foe);
2152 } else {
2153 // figure out which link is approached by the current foe
2154 for (MSLink* fl : myFoeLinks) {
2155 aviPtr = fl->getApproachingPtr(foe);
2156 if (aviPtr != nullptr) {
2157 break;
2158 }
2159 }
2160 }
2161 if (aviPtr == nullptr) {
2162 continue;
2163 }
2164 const ApproachingVehicleInformation& avi = *aviPtr;
2165 const double foeDist = (foe->isActive() ? avi.dist : MAX2(0.0, avi.dist -
2166 STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - foe->getLastActionTime()) * avi.speed));
2167
2168 if ( // ignore vehicles that arrive after us (unless they are ahead and we could easily brake for them)
2169 ((avi.arrivalTime > arrivalTime) && !couldBrakeForLeader(dist, foeDist, ego, foe)) ||
2170 // also ignore vehicles that are behind us and are able to brake for us
2171 couldBrakeForLeader(foeDist, dist, foe, ego) ||
2172 // resolve ties by lane index
2173 (avi.arrivalTime == arrivalTime && foeDist == dist && ego->getLane()->getIndex() < foe->getLane()->getIndex())) {
2174#ifdef DEBUG_ZIPPER
2175 if (DEBUG_COND_ZIPPER) std::cout
2176 << " ignoring foe=" << foe->getID()
2177 << " foeAT=" << avi.arrivalTime
2178 << " foeDist=" << avi.dist
2179 << " foeDist2=" << foeDist
2180 << " foeSpeed=" << avi.speed
2181 << " egoSpeed=" << ego->getSpeed()
2182 << " deltaDist=" << foeDist - dist
2183 << " delteSpeed=" << avi.speed - foe->getCarFollowModel().getMaxDecel() - ego->getSpeed()
2184 << " egoCouldBrake=" << couldBrakeForLeader(dist, foeDist, ego, foe)
2185 << " foeCouldBrake=" << couldBrakeForLeader(foeDist, dist, foe, ego)
2186 << "\n";
2187#endif
2188 continue;
2189 }
2190 // the idea behind speed adaption is three-fold:
2191 // 1) ego needs to be in a car-following relationship with foe eventually
2192 // thus, the ego speed should be equal to the follow speed once the foe enters
2193 // the zipper junction
2194 // 2) ego vehicle needs to put a certain distance beteen himself and foe (safeGap)
2195 // achieving this distance can be spread over time but computing
2196 // safeGap is subject to estimation errors of future speeds
2197 // 3) deceleration can be spread out over the time until true
2198 // car-following happens, at the start of speed adaptions, smaller
2199 // decelerations should be sufficient
2200
2201 // we cannot trust avi.arrivalSpeed if the foe has leader vehicles that are accelerating
2202 // lets try to extrapolate
2203 const double uMax = foe->getLane()->getVehicleMaxSpeed(foe);
2204 const double uAccel = foe->getCarFollowModel().estimateSpeedAfterDistance(foeDist, avi.speed, foe->getCarFollowModel().getMaxAccel());
2205 const double uEnd = MIN2(uMax, uAccel);
2206 const double uAvg = (avi.speed + uEnd) / 2;
2207 const double tf0 = foeDist / MAX2(NUMERICAL_EPS, uAvg);
2208 const double tf = MAX2(1.0, ceil((tf0) / TS) * TS);
2209
2210 const double vMax = ego->getLane()->getVehicleMaxSpeed(ego);
2211 const double vAccel = ego->getCarFollowModel().estimateSpeedAfterDistance(dist, ego->getSpeed(), ego->getCarFollowModel().getMaxAccel());
2212 const double vDecel = ego->getCarFollowModel().estimateSpeedAfterDistance(dist, ego->getSpeed(), -ego->getCarFollowModel().getMaxDecel());
2213 const double vEnd = MIN3(vMax, vAccel, MAX2(uEnd, vDecel));
2214 const double vAvg = (ego->getSpeed() + vEnd) / 2;
2215 const double te0 = dist / MAX2(NUMERICAL_EPS, vAvg);
2216 const double te = MAX2(1.0, ceil((te0) / TS) * TS);
2217
2218 const double tTarget = tf + ego->getCarFollowModel().getHeadwayTime();
2219 const double a = ego->getCarFollowModel().avoidArrivalAccel(dist, tTarget, vSafe, ego->getCarFollowModel().getMaxDecel());
2220
2221 const double gap = dist - foe->getVehicleType().getLength() - ego->getVehicleType().getMinGap() - foeDist;
2222 const double vFollow = ego->getCarFollowModel().followSpeed(
2223 ego, ego->getSpeed(), gap, avi.speed, foe->getCarFollowModel().getMaxDecel(), foe);
2224 const double vSafeGap = MAX2(vFollow, ego->getSpeed() + ACCEL2SPEED(a));
2225
2226 // scale behavior based on ego time to link (te)
2227 const double w = MIN2(1.0, te / 10);
2228 const double maxDecel = w * ego->getCarFollowModel().getMaxDecel() + (1 - w) * ego->getCarFollowModel().getEmergencyDecel();
2229 const double vZipper = MAX3(vFollow, ego->getSpeed() - ACCEL2SPEED(maxDecel), vSafeGap);
2230
2231 vSafe = MIN2(vSafe, vZipper);
2232#ifdef DEBUG_ZIPPER
2233 if (DEBUG_COND_ZIPPER) std::cout << " adapting to foe=" << foe->getID()
2234 << " foeDist=" << foeDist
2235 << " foeSpeed=" << avi.speed
2236 << " foeAS=" << avi.arrivalSpeed
2237 << " egoSpeed=" << ego->getSpeed()
2238 << " uMax=" << uMax
2239 << " uAccel=" << uAccel
2240 << " uEnd=" << uEnd
2241 << " uAvg=" << uAvg
2242 << " gap=" << gap
2243 << "\n "
2244 << " tf=" << tf
2245 << " te=" << te
2246 << " aSafeGap=" << a
2247 << " vMax=" << vMax
2248 << " vAccel=" << vAccel
2249 << " vDecel=" << vDecel
2250 << " vEnd=" << vEnd
2251 << " vSafeGap=" << vSafeGap
2252 << " vFollow=" << vFollow
2253 << " w=" << w
2254 << " maxDecel=" << maxDecel
2255 << " vZipper=" << vZipper
2256 << " vSafe=" << vSafe
2257 << "\n";
2258#endif
2259 }
2260 return vSafe;
2261}
2262
2263
2264bool
2265MSLink::couldBrakeForLeader(double followDist, double leaderDist, const MSVehicle* follow, const MSVehicle* leader) {
2266 return (// leader is ahead of follower
2267 followDist > leaderDist &&
2268 // and follower could brake for 1 s to stay behind leader
2269 followDist - leaderDist > follow->getSpeed() - follow->getCarFollowModel().getMaxDecel() - leader->getSpeed());
2270}
2271
2272
2273void
2278
2279bool
2280MSLink::checkContOff() const {
2281 // check whether this link gets to keep its cont status switching the tls off
2282 // @note: this could also be pre-computed in netconvert
2283 // we check whether there is any major link from this edge
2284 for (const MSLane* cand : myLaneBefore->getEdge().getLanes()) {
2285 for (const MSLink* link : cand->getLinkCont()) {
2286 if (link->getOffState() == LINKSTATE_TL_OFF_NOSIGNAL) {
2287 return true;
2288 }
2289 }
2290 }
2291 return false;
2292}
2293
2294bool
2295MSLink::lateralOverlap(double posLat, double width, double posLat2, double width2) {
2296 return fabs(posLat2 - posLat) < (width + width2) / 2;
2297}
2298
2299std::string
2300MSLink::getDescription() const {
2301 return myLaneBefore->getID() + "->" + getViaLaneOrLane()->getID();
2302}
2303
2304
2305bool
2306MSLink::ignoreFoe(const SUMOTrafficObject* ego, const SUMOTrafficObject* foe) {
2307 if (ego == nullptr || !ego->getParameter().wasSet(VEHPARS_JUNCTIONMODEL_PARAMS_SET)) {
2308 return false;
2309 }
2310 const SUMOVehicleParameter& param = ego->getParameter();
2311 for (const std::string& typeID : StringTokenizer(param.getParameter(toString(SUMO_ATTR_JM_IGNORE_TYPES), "")).getVector()) {
2312 if (typeID == foe->getVehicleType().getID()) {
2313 return true;
2314 }
2315 }
2316 for (const std::string& id : StringTokenizer(param.getParameter(toString(SUMO_ATTR_JM_IGNORE_IDS), "")).getVector()) {
2317 if (id == foe->getID()) {
2318 return true;
2319 }
2320 }
2321 return false;
2322}
2323
2324
2325void
2329
2330
2331std::pair<const SUMOVehicle* const, const MSLink::ApproachingVehicleInformation>
2332MSLink::getClosest() const {
2333 assert(getApproaching().size() > 0);
2334 double minDist = std::numeric_limits<double>::max();
2335 auto closestIt = getApproaching().begin();
2336 for (auto apprIt = getApproaching().begin(); apprIt != getApproaching().end(); apprIt++) {
2337 if (apprIt->second.dist < minDist) {
2338 minDist = apprIt->second.dist;
2339 closestIt = apprIt;
2340 }
2341 }
2342 // maybe a parallel link has a closer vehicle
2343 /*
2344 for (MSLink* link2 : link->getLaneBefore()->getLinkCont()) {
2345 if (link2 != link) {
2346 for (auto apprIt2 = link2->getApproaching().begin(); apprIt2 != link2->getApproaching().end(); apprIt2++) {
2347 if (apprIt2->second.dist < minDist) {
2348 minDist = apprIt2->second.dist;
2349 closestIt = apprIt2;
2350 }
2351 }
2352 }
2353 }
2354 */
2355 return *closestIt;
2356}
2357
2358
2359/****************************************************************************/
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
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:2142
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:2103
LINKSTATE_ALLWAY_STOP
@ LINKSTATE_ALLWAY_STOP
This is an uncontrolled, all-way stop link.
Definition SUMOXMLDefinitions.h:2129
LINKSTATE_STOP
@ LINKSTATE_STOP
This is an uncontrolled, minor link, has to stop.
Definition SUMOXMLDefinitions.h:2127
LINKSTATE_TL_GREEN_MAJOR
@ LINKSTATE_TL_GREEN_MAJOR
The link has green light, may pass.
Definition SUMOXMLDefinitions.h:2105
LINKSTATE_ZIPPER
@ LINKSTATE_ZIPPER
This is an uncontrolled, zipper-merge link.
Definition SUMOXMLDefinitions.h:2131
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:2117
LINKSTATE_TL_RED
@ LINKSTATE_TL_RED
The link has red light (must brake)
Definition SUMOXMLDefinitions.h:2109
LINKSTATE_TL_GREEN_MINOR
@ LINKSTATE_TL_GREEN_MINOR
The link has green light, has to brake.
Definition SUMOXMLDefinitions.h:2107
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:2119
SUMO_ATTR_JM_IGNORE_FOE_SPEED
@ SUMO_ATTR_JM_IGNORE_FOE_SPEED
Definition SUMOXMLDefinitions.h:1365
SUMO_ATTR_JM_STOPLINE_CROSSING_GAP
@ SUMO_ATTR_JM_STOPLINE_CROSSING_GAP
Definition SUMOXMLDefinitions.h:1371
SUMO_ATTR_JM_STOPSIGN_WAIT
@ SUMO_ATTR_JM_STOPSIGN_WAIT
Definition SUMOXMLDefinitions.h:1375
SUMO_ATTR_VIA
@ SUMO_ATTR_VIA
Definition SUMOXMLDefinitions.h:1505
SUMO_ATTR_JM_IGNORE_IDS
@ SUMO_ATTR_JM_IGNORE_IDS
Definition SUMOXMLDefinitions.h:1377
SUMO_ATTR_JM_IGNORE_TYPES
@ SUMO_ATTR_JM_IGNORE_TYPES
Definition SUMOXMLDefinitions.h:1378
SUMO_ATTR_JM_ALLWAYSTOP_WAIT
@ SUMO_ATTR_JM_ALLWAYSTOP_WAIT
Definition SUMOXMLDefinitions.h:1376
SUMO_ATTR_JM_EXTRA_GAP
@ SUMO_ATTR_JM_EXTRA_GAP
Definition SUMOXMLDefinitions.h:1373
SUMO_ATTR_JM_IGNORE_FOE_PROB
@ SUMO_ATTR_JM_IGNORE_FOE_PROB
Definition SUMOXMLDefinitions.h:1366
SUMO_ATTR_TO
@ SUMO_ATTR_TO
Definition SUMOXMLDefinitions.h:1406
SUMO_ATTR_FROM
@ SUMO_ATTR_FROM
Definition SUMOXMLDefinitions.h:1405
SUMO_ATTR_JM_CROSSING_GAP
@ SUMO_ATTR_JM_CROSSING_GAP
Definition SUMOXMLDefinitions.h:1360
SUMO_ATTR_IMPATIENCE
@ SUMO_ATTR_IMPATIENCE
Definition SUMOXMLDefinitions.h:1619
SUMO_ATTR_ID
@ SUMO_ATTR_ID
Definition SUMOXMLDefinitions.h:958
SUMO_ATTR_JM_TIMEGAP_MINOR
@ SUMO_ATTR_JM_TIMEGAP_MINOR
Definition SUMOXMLDefinitions.h:1372
SUMO_ATTR_JM_ADVANCE
@ SUMO_ATTR_JM_ADVANCE
Definition SUMOXMLDefinitions.h:1374
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:1394
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:2851
MSLane::getNormalSuccessorLane
const MSLane * getNormalSuccessorLane() const
get normal lane following this internal lane, for normal lanes, the lane itself is returned
Definition MSLane.cpp:3252
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:2769
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:2746
MSLane::getIncomingLanes
const std::vector< IncomingLaneInfo > & getIncomingLanes() const
Definition MSLane.h:955
MSLane::getCanonicalPredecessorLane
MSLane * getCanonicalPredecessorLane() const
Definition MSLane.cpp:3273
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:3217
MSLane::isNormal
bool isNormal() const
Definition MSLane.cpp:2627
MSLane::isCrossing
bool isCrossing() const
Definition MSLane.cpp:2633
MSLane::isInternal
bool isInternal() const
Definition MSLane.cpp:2621
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:4384
MSLane::getBidiLane
MSLane * getBidiLane() const
retrieve bidirectional lane or nullptr
Definition MSLane.cpp:4680
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:3242
MSLane::getWidth
double getWidth() const
Returns the lane's width.
Definition MSLane.h:640
MSLane::getLinkCont
const std::vector< MSLink * > & getLinkCont() const
returns the container with all links !!!
Definition MSLane.h:729
MSNet::getInstance
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition MSNet.cpp:186
MSNet::getCurrentTimeStep
SUMOTime getCurrentTimeStep() const
Returns the current simulation step.
Definition MSNet.h: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:1250
MSPModel::blockedAtDist
virtual bool blockedAtDist(const SUMOTrafficObject *ego, const MSLane *lane, double vehCenter, double vehWidth, double oncomingGap, std::vector< const MSPerson * > *collectBlockers)
whether a pedestrian is blocking the crossing of lane for the given vehicle bondaries
Definition MSPModel.h:98
MSPModel::SAFETY_GAP
static const double SAFETY_GAP
Definition MSPModel.h:59
MSTrafficLightLogic
The parent class for traffic light logics.
Definition MSTrafficLightLogic.h:55
MSTransportableControl::getMovementModel
MSPModel * getMovementModel()
Returns the default movement model for this kind of transportables.
Definition MSTransportableControl.h:267
MSTransportable::getAngle
virtual double getAngle() const
return the current angle of the transportable
Definition MSTransportable.cpp:197
MSTransportable::getPosition
Position getPosition(const double) const
Return current position (x/y, cartesian)
Definition MSTransportable.h:113
MSTransportable::getVehicleType
const MSVehicleType & getVehicleType() const
Returns the object's "vehicle" type.
Definition MSTransportable.h:144
MSTransportable::getMaxSpeed
double getMaxSpeed() const
Returns the maximum speed (the minimum of desired and physical maximum speed)
Definition MSTransportable.cpp:638
MSVehicle
Representation of a vehicle in the micro simulation.
Definition MSVehicle.h:77
MSVehicle::willStop
bool willStop() const
Returns whether the vehicle will stop on the current edge.
Definition MSVehicle.cpp:1582
MSVehicle::getLastActionTime
SUMOTime getLastActionTime() const
Returns the time of the vehicle's last action point.
Definition MSVehicle.h:541
MSVehicle::isActive
bool isActive() const
Returns whether the current simulation step is an action point for the vehicle.
Definition MSVehicle.h:628
MSVehicle::getWaitingTime
SUMOTime getWaitingTime(const bool accumulated=false) const
Returns the SUMOTime waited (speed was lesser than 0.1m/s)
Definition MSVehicle.h:670
MSVehicle::isFrontOnLane
bool isFrontOnLane(const MSLane *lane) const
Returns the information whether the front of the vehicle is on the given lane.
Definition MSVehicle.cpp:5142
MSVehicle::getLaneChangeModel
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition MSVehicle.cpp:5872
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:6883
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:7184
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:7449
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:260
OutputDevice::closeTag
bool closeTag(const std::string &comment="")
Closes the most recently opened tag and optionally adds a comment.
Definition OutputDevice.cpp:274
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:94
SUMOTrafficObject
Representation of a vehicle, person, or container.
Definition SUMOTrafficObject.h:54
SUMOTrafficObject::getVehicleType
virtual const MSVehicleType & getVehicleType() const =0
Returns the object's "vehicle" type.
SUMOTrafficObject::getSpeed
virtual double getSpeed() const =0
Returns the object's current speed.
SUMOTrafficObject::getParameter
virtual const SUMOVehicleParameter & getParameter() const =0
Returns the vehicle's parameter (including departure definition)
SUMOTrafficObject::getRNG
virtual SumoRNG * getRNG() const =0
Returns the associated RNG for this object.
SUMOTrafficObject::isSelected
virtual bool isSelected() const =0
whether this object is selected in the GUI
SUMOVTypeParameter::getJMParam
double getJMParam(const SumoXMLAttr attr, const double defaultValue) const
Returns the named value from the map, or the default if it is not contained there.
Definition SUMOVTypeParameter.cpp:717
SUMOVehicle
Representation of a vehicle.
Definition SUMOVehicle.h:62
SUMOVehicle::getLateralPositionOnLane
virtual double getLateralPositionOnLane() const =0
Get the vehicle's lateral position on the lane.
SUMOVehicle::getLastActionTime
virtual SUMOTime getLastActionTime() const =0
SUMOVehicle::getBrakeGap
virtual double getBrakeGap(bool delayed=false) const =0
get distance for coming to a stop (used for rerouting checks)
SUMOVehicleParameter
Structure representing possible vehicle parameter.
Definition SUMOVehicleParameter.h:338
SUMOVehicleParameter::wasSet
bool wasSet(long long int what) const
Returns whether the given parameter was set.
Definition SUMOVehicleParameter.h:501
StringTokenizer::getVector
std::vector< std::string > getVector()
return vector of strings
Definition StringTokenizer.cpp:190
std
Definition json.hpp:4471