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