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