Eclipse SUMO - Simulation of Urban MObility
Loading...
Searching...
No Matches
MSLink.cpp
Go to the documentation of this file.
1/****************************************************************************/
2// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3// Copyright (C) 2001-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 conflictSize = MIN2(conflictSize, lane->getLength());
366 // lane width affects the crossing point
367 intersections1.back() -= conflictSize / 2;
368 // ensure non-negative offset for weird geometries
369 intersections1.back() = MAX2(0.0, intersections1.back());
370
371 // also length/geometry factor. (XXX: Why subtract width/2 *before* converting geometric position to lane pos? refs #3031)
372 intersections1.back() = lane->interpolateGeometryPosToLanePos(intersections1.back());
373
374 if (internalLaneBefore->getLogicalPredecessorLane()->getEdge().isInternal() && !foeLane->getEdge().isCrossing()) {
376 }
377 }
378
379 myConflicts.push_back(ConflictInfo(
380 lane->getLength() - intersections1.back(),
381 conflictSize, flag));
382
383#ifdef MSLink_DEBUG_CROSSING_POINTS
384 std::cout
385 << " intersection of " << lane->getID()
386 << " totalLength=" << lane->getLength()
387 << " with " << foeLane->getID()
388 << " totalLength=" << foeLane->getLength()
389 << " dist1=" << myConflicts.back().lengthBehindCrossing
390 << " widthFactor=" << myConflicts.back().conflictSize / foeLane->getWidth()
391 << "\n";
392#endif
393 }
394 }
395 // check for overlap with internal lanes from the same source lane
396 const MSLane* pred = lane->getLogicalPredecessorLane();
397 // to avoid overlap with vehicles that came from pred (especially when pred has endOffset > 0)
398 // we add all other internal lanes from pred as foeLanes
399 for (const MSLink* const link : pred->getLinkCont()) {
400 const MSLane* const sibling = link->getViaLane();
401 if (sibling != lane && sibling != nullptr) {
402 const double minDist = MIN2(DIVERGENCE_MIN_WIDTH, 0.5 * (lane->getWidth() + sibling->getWidth()));
403 if (lane->getShape().front().distanceTo2D(sibling->getShape().front()) >= minDist) {
404 // account for lateral shift by the entry links
405 continue;
406 }
407 const double distToDivergence = computeDistToDivergence(lane, sibling, minDist, true);
408 double lbcLane;
409 if (lane->getLength() == sibling->getLength() && &lane->getEdge() == &sibling->getEdge()) {
410 // for parallel lanes, avoid inconsistency in distance estimation (#10988)
411 // between forward distance (getLeaderInfo)
412 // and backward distance used in lane-changing (getFollowersOnConsecutive)
413 lbcLane = lane->getLength() - distToDivergence;
414 } else {
415 lbcLane = MAX2(0.0, lane->getLength() - lane->interpolateGeometryPosToLanePos(distToDivergence));
416 }
417 ConflictInfo ci = ConflictInfo(lbcLane, sibling->getWidth());
418 auto it = std::find(myFoeLanes.begin(), myFoeLanes.end(), sibling);
419 if (it != myFoeLanes.end()) {
420 // avoid duplicate foeLane
421 const int replacedIndex = (int)(it - myFoeLanes.begin());
422 myConflicts[replacedIndex] = ci;
423 } else {
424 myConflicts.push_back(ci);
425 myFoeLanes.push_back(sibling);
426 }
427#ifdef MSLink_DEBUG_CROSSING_POINTS
428 std::cout << " adding same-origin foe" << sibling->getID()
429 << " dist1=" << myConflicts.back().lengthBehindCrossing
430 << "\n";
431#endif
432 }
433 }
434 // init points for the symmetrical conflict
435 // for each pair of conflicting lanes, the link that gets second, sets the pointers
436 for (int i = 0; i < (int)myFoeLanes.size(); i++) {
437 const MSLane* foeLane = myFoeLanes[i];
438 MSLink* foeExitLink = foeLane->getLinkCont()[0];
439 int foundIndex = -1;
440 for (int i2 = 0; i2 < (int)foeExitLink->myFoeLanes.size(); i2++) {
441 if (foeExitLink->myFoeLanes[i2] == lane) {
442 myConflicts[i].foeConflictIndex = i2;
443 foeExitLink->myConflicts[i2].foeConflictIndex = i;
444 myRecheck.erase({foeExitLink, this});
445 foundIndex = i2;
446 break;
447 }
448 }
449#ifdef MSLink_DEBUG_CROSSING_POINTS
450 std::cout << lane->getID() << " foeLane=" << foeLane->getID() << " index=" << i << " foundIndex=" << foundIndex << "\n";
451#endif
452 if (foundIndex < 0) {
454 myRecheck.insert({this, foeExitLink});
455 }
456 }
457 }
458 }
460 // check for links with the same origin lane and the same destination edge
461 const MSEdge* myTarget = &myLane->getEdge();
462 // save foes for entry links
463 for (MSLink* const it : myLaneBefore->getLinkCont()) {
464 const MSEdge* target = &(it->getLane()->getEdge());
465 if (it == this) {
466 continue;
467 }
468 if (target == myTarget) {
469 mySublaneFoeLinks.push_back(it);
470#ifdef MSLink_DEBUG_CROSSING_POINTS
471 std::cout << " sublaneFoeLink (same target): " << it->getViaLaneOrLane()->getID() << "\n";
472#endif
473 } else if (myDirection != LinkDirection::STRAIGHT && it->getDirection() == LinkDirection::STRAIGHT) {
474 // potential turn conflict
475 mySublaneFoeLinks2.push_back(it);
476#ifdef MSLink_DEBUG_CROSSING_POINTS
477 std::cout << " sublaneFoeLink2 (other target: " << it->getViaLaneOrLane()->getID() << "\n";
478#endif
479 }
480 }
481 // save foes for exit links
482 if (fromInternalLane()) {
483 //std::cout << " setRequestInformation link=" << getViaLaneOrLane()->getID() << " before=" << myLaneBefore->getID() << " before2=" << myLaneBefore->getIncomingLanes().front().lane->getID() << "\n";
484 for (const MSLink* const link : myLaneBefore->getIncomingLanes().front().lane->getLinkCont()) {
485 if (link->getViaLane() != myInternalLaneBefore && &link->getLane()->getEdge() == myTarget) {
486 //std::cout << " add sublaneFoe=" << (*it)->getViaLane()->getID() << "\n";
487 mySublaneFoeLanes.push_back(link->getViaLane());
488 }
489 }
490 }
491 }
492 if (myInternalLaneBefore != nullptr
494 // for right turns, the curvature helps rather than restricts the linkLeader check
495 && (
498 const double angle = fabs(GeomHelper::angleDiff(
500 myLane->getShape().angleAt2D(0)));
501 if (angle > 0) {
502 double length = myInternalLaneBefore->getShape().length2D();
503 if (myInternalLaneBefore->getIncomingLanes().size() == 1 &&
504 myInternalLaneBefore->getIncomingLanes()[0].lane->isInternal()) {
505 length += myInternalLaneBefore->getIncomingLanes()[0].lane->getShape().length2D();
506 } else if (myInternalLane != nullptr) {
507 length += myInternalLane->getShape().length2D();
508 }
509 myRadius = length / angle;
510 //std::cout << getDescription() << " a=" << RAD2DEG(angle) << " l=" << length << " r=" << myRadius << "\n";
511 }
512 }
513}
514
515
516void
518 for (auto item : myRecheck) {
519#ifdef MSLink_DEBUG_CROSSING_POINTS
520 std::cout << " recheck l1=" << item.first->getDescription() << " l2=" << item.second->getDescription() << "\n";
521#endif
522 MSLink* link = item.first;
523 MSLink* foeExitLink = item.second;
524 const MSLane* lane = link->getInternalLaneBefore();
525 const MSLane* foeLane = foeExitLink->getInternalLaneBefore();
526 int conflictIndex = -1;
527 for (int i = 0; i < (int)link->myFoeLanes.size(); i++) {
528 if (link->myFoeLanes[i] == foeLane) {
529 conflictIndex = i;
530 break;
531 }
532 }
533 if (conflictIndex == -1) {
534 WRITE_WARNING("Could not recheck ConflictInfo for " + link->getDescription() + " and " + foeExitLink->getDescription() + "\n");
535 continue;
536 }
537 ConflictInfo& ci = link->myConflicts[conflictIndex];
538 std::vector<double> intersections1 = foeLane->getShape().intersectsAtLengths2D(lane->getShape());
539 if (intersections1.size() == 0) {
540#ifdef MSLink_DEBUG_CROSSING_POINTS
541 std::cout << " no intersection\n";
542#endif
543 continue;
544 }
545 const double widthFactor = ci.conflictSize / foeLane->getWidth();
546 const double conflictSize2 = lane->getWidth() * widthFactor;
547 std::sort(intersections1.begin(), intersections1.end());
548 intersections1.back() -= conflictSize2 / 2;
549 intersections1.back() = MAX2(0.0, intersections1.back());
550 ci.foeConflictIndex = (int)foeExitLink->myConflicts.size();
551 foeExitLink->myConflicts.push_back(ConflictInfo(foeLane->getLength() - intersections1.back(), conflictSize2));
552#ifdef MSLink_DEBUG_CROSSING_POINTS
553 std::cout << " ci=" << conflictIndex << " wf=" << widthFactor << " flag=" << ci.flag << " flbc=" << foeExitLink->myConflicts.back().lengthBehindCrossing << "\n";
554#endif
555 }
556 myRecheck.clear();
557}
558
559double
560MSLink::computeDistToDivergence(const MSLane* lane, const MSLane* sibling, double minDist, bool sameSource) const {
561 double lbcSibling = 0;
562 double lbcLane = 0;
563
564 PositionVector l = lane->getShape();
565 PositionVector s = sibling->getShape();
566 double length = l.length2D();
567 double sibLength = s.length2D();
568 if (!sameSource) {
569 l = l.reverse();
570 s = s.reverse();
571 } else if (sibling->getEntryLink()->myAmIndirect) {
572 // ignore final waiting position since it may be quite close to the lane
573 // shape but the waiting position is perpendicular (so the minDist
574 // requirement is not necessary
575 lbcSibling += s[-1].distanceTo2D(s[-2]);
576 s.pop_back();
577 } else if (lane->getEntryLink()->myAmIndirect) {
578 // ignore final waiting position since it may be quite close to the lane
579 // shape but the waiting position is perpendicular (so the minDist
580 // requirement is not necessary
581 lbcLane += l[-1].distanceTo2D(l[-2]);
582 l.pop_back();
583 }
584
585#ifdef MSLink_DEBUG_CROSSING_POINTS_DETAILS
586 std::cout << " sameSource=" << sameSource << " minDist=" << minDist << " backDist=" << l.back().distanceTo2D(s.back()) << "\n";
587#endif
588 if (l.back().distanceTo2D(s.back()) > minDist) {
589 // compute the final divergence point
590 // this position serves two purposes:
591 // 1) once the foe vehicle back (on sibling) has passed this point, we can safely ignore it
592 // 2) both vehicles are put into a cf-relationship while before the point.
593 // Since the actual crossing point is at the start of the junction,
594 // we want to make sure that both vehicles have the same distance to the crossing point and thus follow each other naturally
595 std::vector<double> distances = l.distances(s);
596#ifdef MSLink_DEBUG_CROSSING_POINTS
597 std::cout << " distances=" << toString(distances) << "\n";
598#endif
599 assert(distances.size() == l.size() + s.size());
600 if (distances.back() > minDist && distances[l.size() - 1] > minDist) {
601 // do a pairwise check between lane and sibling to make because we do not know which of them bends more
602 for (int j = (int)s.size() - 2; j >= 0; j--) {
603 const int i = j + (int)l.size();
604 const double segLength = s[j].distanceTo2D(s[j + 1]);
605 if (distances[i] > minDist) {
606 lbcSibling += segLength;
607 } else {
608 // assume no sharp bends and just interpolate the last segment
609 lbcSibling += segLength - (minDist - distances[i]) * segLength / (distances[i + 1] - distances[i]);
610 break;
611 }
612 }
613 for (int i = (int)l.size() - 2; i >= 0; i--) {
614 const double segLength = l[i].distanceTo2D(l[i + 1]);
615 if (distances[i] > minDist) {
616 lbcLane += segLength;
617 } else {
618 // assume no sharp bends and just interpolate the last segment
619 lbcLane += segLength - (minDist - distances[i]) * segLength / (distances[i + 1] - distances[i]);
620 break;
621 }
622 }
623 }
624 assert(lbcSibling >= -NUMERICAL_EPS);
625 assert(lbcLane >= -NUMERICAL_EPS);
626 }
627 const double distToDivergence1 = sibling->getLength() - lbcSibling;
628 const double distToDivergence2 = lane->getLength() - lbcLane;
629 const double distToDivergence = MIN3(
630 MAX2(distToDivergence1, distToDivergence2),
631 sibLength, length);
632#ifdef MSLink_DEBUG_CROSSING_POINTS
633 std::cout << " distToDivergence=" << distToDivergence
634 << " distTD1=" << distToDivergence1
635 << " distTD2=" << distToDivergence2
636 << " length=" << length
637 << " sibLength=" << sibLength
638 << "\n";
639#endif
640 return distToDivergence;
641}
642
643
644bool
645MSLink::contIntersect(const MSLane* lane, const MSLane* foe) {
646 if (foe->getLinkCont()[0]->getViaLane() != nullptr) {
647 std::vector<double> intersections = lane->getShape().intersectsAtLengths2D(foe->getShape());
648 return intersections.size() > 0;
649 }
650 return false;
651}
652
653
654void
655MSLink::setApproaching(const SUMOVehicle* approaching, const SUMOTime arrivalTime, const double arrivalSpeed, const double leaveSpeed,
656 const bool setRequest, const double arrivalSpeedBraking, const SUMOTime waitingTime, double dist, double latOffset) {
657 const SUMOTime leaveTime = getLeaveTime(arrivalTime, arrivalSpeed, leaveSpeed, approaching->getVehicleType().getLength());
658#ifdef DEBUG_APPROACHING
659 if (DEBUG_COND2(approaching)) {
660 std::cout << SIMTIME << " Link '" << (myLaneBefore == 0 ? "NULL" : myLaneBefore->getID()) << "'->'" << (myLane == 0 ? "NULL" : myLane->getID()) << "' Adding approaching vehicle '" << approaching->getID() << "'\nCurrently registered vehicles:" << std::endl;
661 for (auto i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
662 std::cout << "'" << i->first->getID() << "'" << std::endl;
663 }
664 }
665#endif
666 myApproachingVehicles.emplace(approaching,
667 ApproachingVehicleInformation(arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, setRequest,
668 arrivalSpeedBraking, waitingTime, dist, approaching->getSpeed(), latOffset));
669}
670
671
672void
674
675#ifdef DEBUG_APPROACHING
676 if (DEBUG_COND2(approaching)) {
677 std::cout << SIMTIME << " Link '" << (myLaneBefore == 0 ? "NULL" : myLaneBefore->getID()) << "'->'" << (myLane == 0 ? "NULL" : myLane->getID()) << "' Adding approaching vehicle '" << approaching->getID() << "'\nCurrently registered vehicles:" << std::endl;
678 for (auto i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
679 std::cout << "'" << i->first->getID() << "'" << std::endl;
680 }
681 }
682#endif
683 myApproachingVehicles.emplace(approaching, ai);
684}
685
686
687void
689 myBlockedFoeLinks.insert(link);
690}
691
692
693
694bool
696 for (std::set<MSLink*>::const_iterator i = myBlockedFoeLinks.begin(); i != myBlockedFoeLinks.end(); ++i) {
697 if ((*i)->isBlockingAnyone()) {
698 return true;
699 }
700 }
701 return false;
702}
703
704
705void
707
708#ifdef DEBUG_APPROACHING
709 if (DEBUG_COND2(veh)) {
710 std::cout << SIMTIME << " Link '" << (myLaneBefore == 0 ? "NULL" : myLaneBefore->getID()) << "'->'" << (myLane == 0 ? "NULL" : myLane->getID()) << std::endl;
711 std::cout << "' Removing approaching vehicle '" << veh->getID() << "'\nCurrently registered vehicles:" << std::endl;
712 for (auto i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
713 std::cout << "'" << i->first->getID() << "'" << std::endl;
714 }
715 }
716#endif
717 myApproachingVehicles.erase(veh);
718}
719
720
723 auto i = myApproachingVehicles.find(veh);
724 if (i != myApproachingVehicles.end()) {
725 return i->second;
726 } else {
727 return ApproachingVehicleInformation(INVALID_TIME, INVALID_TIME, 0, 0, false, 0, 0, 0, 0, 0);
728 }
729}
730
731
732void
736
737
739MSLink::getLeaveTime(const SUMOTime arrivalTime, const double arrivalSpeed,
740 const double leaveSpeed, const double vehicleLength) const {
741 return arrivalTime == SUMOTime_MAX ? SUMOTime_MAX : arrivalTime + TIME2STEPS((getLength() + vehicleLength) / MAX2(0.5 * (arrivalSpeed + leaveSpeed), NUMERICAL_EPS));
742}
743
744
745bool
746MSLink::opened(SUMOTime arrivalTime, double arrivalSpeed, double leaveSpeed, double vehicleLength,
747 double impatience, double decel, SUMOTime waitingTime, double posLat,
748 BlockingFoes* collectFoes, bool ignoreRed, const SUMOTrafficObject* ego) const {
749#ifdef MSLink_DEBUG_OPENED
750 if (gDebugFlag1) {
751 std::cout << SIMTIME << " opened? link=" << getDescription() << " red=" << haveRed() << " cont=" << isCont() << " numFoeLinks=" << myFoeLinks.size() << " havePrio=" << havePriority() << " lastWasContMajorGreen=" << lastWasContState(LINKSTATE_TL_GREEN_MAJOR) << "\n";
752 }
753#endif
754 if (haveRed() && !ignoreRed) {
755 return false;
756 }
758 return true;
759 }
760 const SUMOTime leaveTime = getLeaveTime(arrivalTime, arrivalSpeed, leaveSpeed, vehicleLength);
762 // check for foes on the same lane with the same target edge
763 for (const MSLink* foeLink : mySublaneFoeLinks) {
764 assert(myLane != foeLink->getLane());
765 for (const auto& it : foeLink->myApproachingVehicles) {
766 const SUMOVehicle* foe = it.first;
767 if (
768 // there only is a conflict if the paths cross
769 ((posLat < foe->getLateralPositionOnLane() + it.second.latOffset && myLane->getIndex() > foeLink->myLane->getIndex())
770 || (posLat > foe->getLateralPositionOnLane() + it.second.latOffset && myLane->getIndex() < foeLink->myLane->getIndex()))
771 // the vehicle that arrives later must yield
772 && (arrivalTime > it.second.arrivalTime
773 // if both vehicles arrive at the same time, the one
774 // to the left must yield
775 || (arrivalTime == it.second.arrivalTime && posLat > foe->getLateralPositionOnLane()))) {
776 if (blockedByFoe(foe, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, false,
777 impatience, decel, waitingTime, ego)) {
778#ifdef MSLink_DEBUG_OPENED
779 if (gDebugFlag1) {
780 std::cout << SIMTIME << " blocked by " << foe->getID() << " arrival=" << arrivalTime << " foeArrival=" << it.second.arrivalTime << "\n";
781 }
782#endif
783 if (collectFoes == nullptr) {
784#ifdef MSLink_DEBUG_OPENED
785 if (gDebugFlag1) {
786 std::cout << " link=" << getViaLaneOrLane()->getID() << " blocked by sublaneFoe=" << foe->getID() << " foeLink=" << foeLink->getViaLaneOrLane()->getID() << " posLat=" << posLat << "\n";
787 }
788#endif
789 return false;
790 } else {
791 collectFoes->push_back(it.first);
792 }
793 }
794 }
795 }
796 }
797 // check for foes on the same lane with a different target edge
798 // (straight movers take precedence if the paths cross)
799 const int lhSign = MSGlobals::gLefthand ? -1 : 1;
800 for (const MSLink* foeLink : mySublaneFoeLinks2) {
802 for (const auto& it : foeLink->myApproachingVehicles) {
803 const SUMOVehicle* foe = it.first;
804 // there only is a conflict if the paths cross
805 // and if the vehicles are not currently in a car-following relationship
806 const double egoWidth = ego == nullptr ? 1.8 : ego->getVehicleType().getWidth();
807 if (!lateralOverlap(posLat, egoWidth, foe->getLateralPositionOnLane() + it.second.latOffset, foe->getVehicleType().getWidth())
809 && (posLat * lhSign > (foe->getLateralPositionOnLane() + it.second.latOffset) * lhSign))
811 && (posLat * lhSign < (foe->getLateralPositionOnLane() + it.second.latOffset) * lhSign)))) {
812 if (blockedByFoe(foe, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, false,
813 impatience, decel, waitingTime, ego)) {
814#ifdef MSLink_DEBUG_OPENED
815 if (gDebugFlag1) {
816 std::cout << SIMTIME << " blocked by sublane foe " << foe->getID() << " arrival=" << arrivalTime << " foeArrival=" << it.second.arrivalTime << "\n";
817 }
818#endif
819 if (collectFoes == nullptr) {
820#ifdef MSLink_DEBUG_OPENED
821 if (gDebugFlag1) {
822 std::cout << " link=" << getViaLaneOrLane()->getID() << " blocked by sublaneFoe2=" << foe->getID() << " foeLink=" << foeLink->getViaLaneOrLane()->getID() << " posLat=" << posLat << "\n";
823 }
824#endif
825 return false;
826 } else {
827 collectFoes->push_back(it.first);
828 }
829 }
830 }
831 }
832 }
833 }
835 // priority usually means the link is open but there are exceptions:
836 // zipper still needs to collect foes
837 // sublane model could have detected a conflict
838 return collectFoes == nullptr || collectFoes->size() == 0;
839 }
840 if ((myState == LINKSTATE_STOP || myState == LINKSTATE_ALLWAY_STOP) && waitingTime == 0) {
841 return false;
842 }
843
844 const std::vector<MSLink*>& foeLinks = (myOffFoeLinks == nullptr || getCorrespondingEntryLink()->getState() != LINKSTATE_ALLWAY_STOP) ? myFoeLinks : *myOffFoeLinks;
845#ifdef MSLink_DEBUG_OPENED
846 if (gDebugFlag1) {
847 std::cout << SIMTIME << " opened link=" << getViaLaneOrLane()->getID() << " foeLinks=" << foeLinks.size() << "\n";
848 }
849#endif
850
851 if (MSGlobals::gUseMesoSim && impatience == 1) {
852 return true;
853 }
854 const bool lastWasContRed = lastWasContState(LINKSTATE_TL_RED);
855 for (const MSLink* const link : foeLinks) {
857 if (link->haveRed()) {
858 continue;
859 }
860 }
861#ifdef MSLink_DEBUG_OPENED
862 if (gDebugFlag1) {
863 std::cout << " foeLink=" << link->getViaLaneOrLane()->getID() << " numApproaching=" << link->getApproaching().size() << "\n";
864 }
865#endif
866 if (link->blockedAtTime(arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, myLane == link->getLane(),
867 impatience, decel, waitingTime, collectFoes, ego, lastWasContRed)) {
868 return false;
869 }
870 }
871 if (collectFoes != nullptr && collectFoes->size() > 0) {
872 return false;
873 }
874 return true;
875}
876
877
878bool
879MSLink::blockedAtTime(SUMOTime arrivalTime, SUMOTime leaveTime, double arrivalSpeed, double leaveSpeed,
880 bool sameTargetLane, double impatience, double decel, SUMOTime waitingTime,
881 BlockingFoes* collectFoes, const SUMOTrafficObject* ego, bool lastWasContRed) const {
882 for (const auto& it : myApproachingVehicles) {
883#ifdef MSLink_DEBUG_OPENED
884 if (gDebugFlag1) {
885 if (ego != nullptr
888 std::stringstream stream; // to reduce output interleaving from different threads
889 stream << SIMTIME << " " << myApproachingVehicles.size() << " foe link=" << getViaLaneOrLane()->getID()
890 << " foeVeh=" << it.first->getID() << " (below ignore speed)"
891 << " ignoreFoeProb=" << ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0)
892 << "\n";
893 std::cout << stream.str();
894 }
895 }
896#endif
897 if (it.first != ego
898 && (ego == nullptr
902 && !ignoreFoe(ego, it.first)
903 && (!lastWasContRed || it.first->getSpeed() > SUMO_const_haltingSpeed)
904 && blockedByFoe(it.first, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, sameTargetLane,
905 impatience, decel, waitingTime, ego)) {
906 if (collectFoes == nullptr) {
907 return true;
908 } else {
909 collectFoes->push_back(it.first);
910 }
911 }
912 }
913 return false;
914}
915
916
917bool
919 SUMOTime arrivalTime, SUMOTime leaveTime, double arrivalSpeed, double leaveSpeed,
920 bool sameTargetLane, double impatience, double decel, SUMOTime waitingTime,
921 const SUMOTrafficObject* ego) const {
922#ifdef MSLink_DEBUG_OPENED
923 if (gDebugFlag1) {
924 std::stringstream stream; // to reduce output interleaving from different threads
925 stream << " link=" << getDescription()
926 << " foeVeh=" << veh->getID()
927 << " req=" << avi.willPass
928 << " aT=" << avi.arrivalTime
929 << " lT=" << avi.leavingTime
930 << "\n";
931 std::cout << stream.str();
932 }
933#endif
934 if (!avi.willPass) {
935 return false;
936 }
938 assert(waitingTime > 0);
939 if (waitingTime > avi.waitingTime) {
940 return false;
941 }
942 if (waitingTime == avi.waitingTime && arrivalTime < avi.arrivalTime) {
943 return false;
944 }
945 }
946 SUMOTime foeArrivalTime = avi.arrivalTime;
947 double foeArrivalSpeedBraking = avi.arrivalSpeedBraking;
948 if (impatience > 0 && arrivalTime < avi.arrivalTime) {
949#ifdef MSLink_DEBUG_OPENED
950 gDebugFlag6 = ((ego == nullptr || ego->isSelected()) && (veh == nullptr || veh->isSelected()));
951#endif
952 const SUMOTime fatb = computeFoeArrivalTimeBraking(arrivalTime, veh, avi.arrivalTime, impatience, avi.dist, foeArrivalSpeedBraking);
953 foeArrivalTime = (SUMOTime)((1. - impatience) * (double)avi.arrivalTime + impatience * (double)fatb);
954#ifdef MSLink_DEBUG_OPENED
955 if (gDebugFlag6) {
956 std::cout << SIMTIME << " link=" << getDescription() << " ego=" << ego->getID() << " foe=" << veh->getID()
957 << " at=" << STEPS2TIME(arrivalTime)
958 << " fat=" << STEPS2TIME(avi.arrivalTime)
959 << " fatb=" << STEPS2TIME(fatb)
960 << " fat2=" << STEPS2TIME(foeArrivalTime)
961 << "\n";
962 }
963#endif
964 }
965
966
967 const SUMOTime lookAhead = (myState == LINKSTATE_ZIPPER
969 : (ego == nullptr
972 //if (ego != 0) std::cout << SIMTIME << " ego=" << ego->getID() << " jmTimegapMinor=" << ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_TIMEGAP_MINOR, -1) << " lookAhead=" << lookAhead << "\n";
973#ifdef MSLink_DEBUG_OPENED
974 if (gDebugFlag1 || gDebugFlag6) {
975 std::stringstream stream; // to reduce output interleaving from different threads
976 stream << " imp=" << impatience << " fAT2=" << foeArrivalTime << " fASb=" << foeArrivalSpeedBraking << " lA=" << lookAhead << " egoAT=" << arrivalTime << " egoLT=" << leaveTime << " egoLS=" << leaveSpeed << "\n";
977 std::cout << stream.str();
978 }
979#endif
980 if (avi.leavingTime < arrivalTime) {
981 // ego wants to be follower
982 if (sameTargetLane && (arrivalTime - avi.leavingTime < lookAhead
983 || unsafeMergeSpeeds(avi.leaveSpeed, arrivalSpeed,
984 veh->getVehicleType().getCarFollowModel().getMaxDecel(), decel))) {
985#ifdef MSLink_DEBUG_OPENED
986 if (gDebugFlag1 || gDebugFlag6) {
987 std::cout << " blocked (cannot follow)\n";
988 }
989#endif
990 return true;
991 }
992 } else if (foeArrivalTime > leaveTime + lookAhead) {
993 // ego wants to be leader.
994 if (sameTargetLane && unsafeMergeSpeeds(leaveSpeed, foeArrivalSpeedBraking,
995 decel, veh->getVehicleType().getCarFollowModel().getMaxDecel())) {
996#ifdef MSLink_DEBUG_OPENED
997 if (gDebugFlag1 || gDebugFlag6) {
998 std::cout << " blocked (cannot lead)\n";
999 }
1000#endif
1001 return true;
1002 }
1003 } else {
1004 // even without considering safeHeadwayTime there is already a conflict
1005#ifdef MSLink_DEBUG_OPENED
1006 if (gDebugFlag1 || gDebugFlag6) {
1007 std::cout << " blocked (hard conflict)\n";
1008 }
1009#endif
1010 return true;
1011 }
1012 return false;
1013}
1014
1015
1017MSLink::computeFoeArrivalTimeBraking(SUMOTime arrivalTime, const SUMOVehicle* foe, SUMOTime foeArrivalTime, double impatience, double dist, double& fasb) {
1018 // a: distance saved when foe brakes from arrivalTime to foeArrivalTime
1019 // b: distance driven past foeArrivalTime
1020 // m: permitted decceleration
1021 // d: total deceleration until foeArrivalTime
1022 // dist2: distance of foe at arrivalTime
1023 // actual arrivalTime must fall on a simulation step
1024 if (arrivalTime - arrivalTime % DELTA_T == foeArrivalTime - foeArrivalTime % DELTA_T) {
1025 // foe enters the junction in the same step
1026 return foeArrivalTime;
1027 }
1028 //arrivalTime += DELTA_T - arrivalTime % DELTA_T;
1029 const double m = foe->getVehicleType().getCarFollowModel().getMaxDecel() * impatience;
1030 const double dt = STEPS2TIME(foeArrivalTime - arrivalTime);
1031 const double d = dt * m;
1032 const double a = dt * d / 2;
1033 const double v = dist / STEPS2TIME(foeArrivalTime - SIMSTEP + DELTA_T);
1034 const double dist2 = dist - v * STEPS2TIME(arrivalTime - SIMSTEP);
1035 if (0.5 * v * v / m <= dist2) {
1036 if (gDebugFlag6) {
1037 std::cout << " dist=" << dist << " dist2=" << dist2 << " at=" << STEPS2TIME(arrivalTime) << " m=" << m << " d=" << d << " a=" << a << " canBrakeToStop\n";
1038 }
1039 fasb = 0;
1040 return foeArrivalTime + TIME2STEPS(30);
1041 }
1042 // a = b (foe reaches the original distance to the stop line)
1043 // x: time driven past foeArrivalTime
1044 // v: foe speed without braking
1045 // v2: average foe speed after foeArrivalTime (braking continues for time x)
1046 // v2 = (v - d - x * m / 2)
1047 // b = v2 * x
1048 // solving for x gives:
1049 const double x = (sqrt(4 * (v - d) * (v - d) - 8 * m * a) * -0.5 - d + v) / m;
1050
1051#ifdef MSLink_DEBUG_OPENED
1052 const double x2 = (sqrt(4 * (v - d) * (v - d) - 8 * m * a) * 0.5 - d + v) / m;
1053 if (gDebugFlag6 || std::isnan(x)) {
1054 std::cout << SIMTIME << " dist=" << dist << " dist2=" << dist2 << " at=" << STEPS2TIME(arrivalTime) << " m=" << m << " d=" << d << " v=" << v << " a=" << a << " x=" << x << " x2=" << x2 << "\n";
1055 }
1056#endif
1057 fasb = v - (dt + x) * m;
1058 return foeArrivalTime + TIME2STEPS(x);
1059}
1060
1061
1062bool
1063MSLink::hasApproachingFoe(SUMOTime arrivalTime, SUMOTime leaveTime, double speed, double decel) const {
1064 for (const MSLink* const link : myFoeLinks) {
1065 if (link->blockedAtTime(arrivalTime, leaveTime, speed, speed, myLane == link->getLane(), 0, decel, 0)) {
1066 return true;
1067 }
1068 }
1069 for (const MSLane* const lane : myFoeLanes) {
1070 if (lane->getVehicleNumberWithPartials() > 0) {
1071 return true;
1072 }
1073 }
1074 return false;
1075}
1076
1077
1078std::pair<const SUMOVehicle*, const MSLink*>
1079MSLink::getFirstApproachingFoe(const MSLink* wrapAround) const {
1080 double closetDist = std::numeric_limits<double>::max();
1081 const SUMOVehicle* closest = nullptr;
1082 const MSLink* foeLink = nullptr;
1083 for (MSLink* link : myFoeLinks) {
1084 for (const auto& it : link->myApproachingVehicles) {
1085 //std::cout << " link=" << getDescription() << " foeLink_in=" << link->getLaneBefore()->getID() << " wrapAround=" << wrapAround->getDescription() << "\n";
1086 if (link->getLaneBefore() == wrapAround->getLaneBefore()) {
1087 return std::make_pair(nullptr, wrapAround);
1088 } else if (it.second.dist < closetDist) {
1089 closetDist = it.second.dist;
1090 if (it.second.willPass) {
1091 closest = it.first;
1092 foeLink = link;
1093 }
1094 }
1095 }
1096 }
1097 return std::make_pair(closest, foeLink);
1098}
1099
1100
1101void
1103 if (myState != state) {
1105 }
1106 myState = state;
1107 if (haveGreen()) {
1109 }
1110}
1111
1112
1113bool
1115 // when a traffic light is switched off minor roads have their cont status revoked
1117}
1118
1119
1120bool
1122 if (myInternalLane == nullptr || myAmCont) {
1123 return false;
1124 } else {
1126 if (!pred->getEdge().isInternal()) {
1127 return false;
1128 } else {
1129 const MSLane* const pred2 = pred->getLogicalPredecessorLane();
1130 assert(pred2 != nullptr);
1131 const MSLink* const predLink = pred2->getLinkTo(pred);
1132 assert(predLink != nullptr);
1133 if (predLink->havePriority()) {
1134 return true;
1135 }
1137 return predLink->getLastGreenState() == LINKSTATE_TL_GREEN_MAJOR;
1138 } else {
1139 return predLink->haveYellow();
1140 }
1141 }
1142 }
1143}
1144
1145
1146bool
1149 return false;
1150 } else {
1152 if (!pred->getEdge().isInternal()) {
1153 return false;
1154 } else {
1155 const MSLane* const pred2 = pred->getLogicalPredecessorLane();
1156 assert(pred2 != nullptr);
1157 const MSLink* const predLink = pred2->getLinkTo(pred);
1158 assert(predLink != nullptr);
1159 return predLink->getState() == linkState;
1160 }
1161 }
1162}
1163
1164
1165void
1166MSLink::writeApproaching(OutputDevice& od, const std::string fromLaneID) const {
1167 if (myApproachingVehicles.size() > 0) {
1168 od.openTag("link");
1169 od.writeAttr(SUMO_ATTR_FROM, fromLaneID);
1170 const std::string via = getViaLane() == nullptr ? "" : getViaLane()->getID();
1171 od.writeAttr(SUMO_ATTR_VIA, via);
1172 od.writeAttr(SUMO_ATTR_TO, getLane() == nullptr ? "" : getLane()->getID());
1173 std::vector<std::pair<SUMOTime, const SUMOVehicle*> > toSort; // stabilize output
1174 for (auto it : myApproachingVehicles) {
1175 toSort.push_back(std::make_pair(it.second.arrivalTime, it.first));
1176 }
1177 std::sort(toSort.begin(), toSort.end());
1178 for (std::vector<std::pair<SUMOTime, const SUMOVehicle*> >::const_iterator it = toSort.begin(); it != toSort.end(); ++it) {
1179 od.openTag("approaching");
1180 const ApproachingVehicleInformation& avi = myApproachingVehicles.find(it->second)->second;
1181 od.writeAttr(SUMO_ATTR_ID, it->second->getID());
1182 od.writeAttr(SUMO_ATTR_IMPATIENCE, it->second->getImpatience());
1183 od.writeAttr("arrivalTime", time2string(avi.arrivalTime));
1184 od.writeAttr("leaveTime", time2string(avi.leavingTime));
1185 od.writeAttr("arrivalSpeed", toString(avi.arrivalSpeed));
1186 od.writeAttr("arrivalSpeedBraking", toString(avi.arrivalSpeedBraking));
1187 od.writeAttr("leaveSpeed", toString(avi.leaveSpeed));
1188 od.writeAttr("willPass", toString(avi.willPass));
1189 od.closeTag();
1190 }
1191 od.closeTag();
1192 }
1193}
1194
1195
1196double
1198 double len = 0.;
1199 MSLane* lane = myInternalLane;
1200
1201 while (lane != nullptr && lane->isInternal()) {
1202 len += lane->getLength();
1203 lane = lane->getLinkCont()[0]->getViaLane();
1204 }
1205 return len;
1206}
1207
1208double
1210 double len = 0.;
1211 const MSLane* lane = myInternalLane;
1212
1213 while (lane != nullptr && lane->isInternal()) {
1214 len += lane->getLength();
1215 if (lane->getIncomingLanes().size() == 1) {
1216 lane = lane->getIncomingLanes()[0].lane;
1217 } else {
1218 break;
1219 }
1220 }
1221 return len;
1222}
1223
1224
1225double
1227 MSLane* via = myInternalLane;
1228 double totalDist = 0.;
1229 bool foundCrossing = false;
1230 while (via != nullptr) {
1231 MSLink* link = via->getLinkCont()[0];
1232 double dist = link->getLengthBeforeCrossing(foeLane);
1233 if (dist != INVALID_DOUBLE) {
1234 // found conflicting lane
1235 totalDist += dist;
1236 foundCrossing = true;
1237 break;
1238 } else {
1239 totalDist += via->getLength();
1240 via = link->getViaLane();
1241 }
1242 }
1243 if (foundCrossing) {
1244 return totalDist;
1245 } else {
1246 return INVALID_DOUBLE;
1247 }
1248}
1249
1250
1251double
1253 int foe_ix;
1254 for (foe_ix = 0; foe_ix != (int)myFoeLanes.size(); ++foe_ix) {
1255 if (myFoeLanes[foe_ix] == foeLane) {
1256 break;
1257 }
1258 }
1259 if (foe_ix == (int)myFoeLanes.size()) {
1260 // no conflict with the given lane, indicate by returning -1
1261#ifdef MSLink_DEBUG_CROSSING_POINTS
1262 std::cout << "No crossing of lanes '" << foeLane->getID() << "' and '" << myInternalLaneBefore->getID() << "'" << std::endl;
1263#endif
1264 return INVALID_DOUBLE;
1265 } else {
1266 // found conflicting lane index
1267 double dist = myInternalLaneBefore->getLength() - myConflicts[foe_ix].getLengthBehindCrossing(this);
1268 if (dist == -10000.) {
1269 // this is the value in myConflicts, if the relation allows intersection but none is present for the actual geometry.
1270 return INVALID_DOUBLE;
1271 }
1272#ifdef MSLink_DEBUG_CROSSING_POINTS
1273 std::cout << "Crossing of lanes '" << myInternalLaneBefore->getID() << "' and '" << foeLane->getID()
1274 << "' at distance " << dist << " (approach along '"
1275 << myInternalLaneBefore->getEntryLink()->getLaneBefore()->getID() << "')" << std::endl;
1276#endif
1277 return dist;
1278 }
1279}
1280
1281
1282bool
1285 return myInternalLane != nullptr && myInternalLaneBefore == nullptr;
1286 } else {
1287 return false;
1288 }
1289}
1290
1291bool
1293 // either a non-cont entry link or the link after a cont-link
1294 return !myAmCont && (isEntryLink() || (myInternalLaneBefore != nullptr && myInternalLane != nullptr));
1295}
1296
1297bool
1300 return myInternalLaneBefore != nullptr && myInternalLane == nullptr;
1301 } else {
1302 return false;
1303 }
1304}
1305
1306bool
1309 return (getInternalLaneBefore() != nullptr
1310 && myInternalLaneBefore->getIncomingLanes().size() == 1
1311 && myInternalLaneBefore->getIncomingLanes().front().viaLink->isInternalJunctionLink());
1312 } else {
1313 return false;
1314 }
1315}
1316
1317
1318const MSLink*
1320 MSLane* lane = myInternalLane;
1321 const MSLink* link = this;
1322 while (lane != nullptr) {
1323 link = lane->getLinkCont()[0];
1324 lane = link->getViaLane();
1325 }
1326 return link;
1327}
1328
1329
1330const MSLink*
1332 const MSLink* link = this;
1333 while (link->myLaneBefore->isInternal()) {
1334 assert(myLaneBefore->getIncomingLanes().size() == 1);
1335 link = link->myLaneBefore->getIncomingLanes().front().viaLink;
1336 }
1337 return link;
1338}
1339
1340
1341bool
1343 return getInternalLaneBefore() != nullptr && myInternalLane != nullptr;
1344}
1345
1346
1348MSLink::getLeaderInfo(const MSVehicle* ego, double dist, std::vector<const MSPerson*>* collectBlockers, bool isShadowLink) const {
1349 LinkLeaders result;
1350 // this link needs to start at an internal lane (either an exit link or between two internal lanes)
1351 // or it must be queried by the pedestrian model (ego == 0)
1352 if (ego != nullptr && (!fromInternalLane() || ego->getLaneChangeModel().isOpposite())) {
1353 // ignore link leaders
1354 return result;
1355 }
1356 //gDebugFlag1 = true;
1357 if (gDebugFlag1) {
1358 std::cout << SIMTIME << " getLeaderInfo link=" << getDescription() << " dist=" << dist << " isShadowLink=" << isShadowLink << "\n";
1359 }
1360 if (MSGlobals::gComputeLC && ego != nullptr && ego->getLane()->isNormal()) {
1361 const MSLink* junctionEntry = getLaneBefore()->getEntryLink();
1362 if (junctionEntry->haveRed() && !ego->ignoreRed(junctionEntry, true)) {
1363 if (gDebugFlag1) {
1364 std::cout << " ignore linkLeaders beyond red light\n";
1365 }
1366 return result;
1367 }
1368 }
1369 // this is an exit link
1370 for (int i = 0; i < (int)myFoeLanes.size(); ++i) {
1371 const MSLane* foeLane = myFoeLanes[i];
1372 const MSLink* foeExitLink = foeLane->getLinkCont()[0];
1373 // distance from the querying vehicle to the crossing point with foeLane
1374 double distToCrossing = dist - myConflicts[i].getLengthBehindCrossing(this);
1375 const double foeDistToCrossing = foeLane->getLength() - myConflicts[i].getFoeLengthBehindCrossing(foeExitLink);
1376 const bool sameTarget = (myLane == foeExitLink->getLane()) && !isInternalJunctionLink() && !foeExitLink->isInternalJunctionLink();
1377 const bool sameSource = (myInternalLaneBefore != nullptr && myInternalLaneBefore->getLogicalPredecessorLane() == foeLane->getLogicalPredecessorLane());
1378 const double crossingWidth = (sameTarget || sameSource) ? 0 : myConflicts[i].conflictSize;
1379 const double foeCrossingWidth = (sameTarget || sameSource) ? 0 : myConflicts[i].getFoeConflictSize(foeExitLink);
1380 // special treatment of contLane foe only applies if this lane is not a contLane or contLane follower itself
1381 const bool contLane = (foeExitLink->getViaLaneOrLane()->getEdge().isInternal() && !(
1383 if (gDebugFlag1) {
1384 std::cout << " distToCrossing=" << distToCrossing << " foeLane=" << foeLane->getID() << " cWidth=" << crossingWidth
1385 << " ijl=" << isInternalJunctionLink() << " sT=" << sameTarget << " sS=" << sameSource
1386 << " lbc=" << myConflicts[i].getLengthBehindCrossing(this)
1387 << " flbc=" << myConflicts[i].getFoeLengthBehindCrossing(foeExitLink)
1388 << " cw=" << crossingWidth
1389 << " fcw=" << foeCrossingWidth
1390 << " contLane=" << contLane
1391 << " state=" << toString(myState)
1392 << " foeState=" << toString(foeExitLink->getState())
1393 << "\n";
1394 }
1395 if (distToCrossing + crossingWidth < 0 && !sameTarget
1396 && (ego == nullptr || !MSGlobals::gComputeLC || distToCrossing + crossingWidth + ego->getVehicleType().getLength() < 0)) {
1397 continue; // vehicle is behind the crossing point, continue with next foe lane
1398 }
1399 bool ignoreGreenCont = false;
1400 bool foeIndirect = false;
1401 if (contLane) {
1402 const MSLink* entry = getLaneBefore()->getEntryLink();
1403 const MSLink* foeEntry = foeLane->getEntryLink();
1404 foeIndirect = foeEntry->myAmIndirect;
1405 if (entry != nullptr && entry->haveGreen()
1406 && foeEntry != nullptr && foeEntry->haveGreen()
1407 && entry->myLaneBefore != foeEntry->myLaneBefore) {
1408 // ignore vehicles before an internaljunction as long as they are still in green minor mode
1409 ignoreGreenCont = true;
1410 }
1411 }
1412 if (foeIndirect && distToCrossing >= NO_INTERSECTION) {
1413 if (gDebugFlag1) {
1414 std::cout << " ignore:noIntersection\n";
1415 }
1416 continue;
1417 }
1418 // it is not sufficient to return the last vehicle on the foeLane because ego might be its leader
1419 // therefore we return all vehicles on the lane
1420 //
1421 // special care must be taken for continuation lanes. (next lane is also internal)
1422 // vehicles on cont. lanes or on internal lanes with the same target as this link can not be ignored
1423 // and should block (gap = -1) unless they are part of an indirect turn
1425 for (MSLane::AnyVehicleIterator it_veh = foeLane->anyVehiclesBegin(); it_veh != end; ++it_veh) {
1426 MSVehicle* leader = (MSVehicle*)*it_veh;
1427 const double leaderBack = leader->getBackPositionOnLane(foeLane);
1428 const double leaderBackDist = foeDistToCrossing - leaderBack;
1429 const double l2 = ego != nullptr ? ego->getLength() + 2 : 0; // add some slack to account for further meeting-angle effects
1430 const double sagitta = ego != nullptr && myRadius != std::numeric_limits<double>::max() ? myRadius - sqrt(myRadius * myRadius - 0.25 * l2 * l2) : 0;
1431 const bool pastTheCrossingPoint = leaderBackDist + foeCrossingWidth + sagitta < 0;
1432 const bool enteredTheCrossingPoint = leaderBackDist < leader->getVehicleType().getLength();
1433 const bool foeIsBicycleTurn = (leader->getVehicleType().getVehicleClass() == SVC_BICYCLE
1434 && foeLane->getIncomingLanes().front().viaLink->getDirection() == LinkDirection::LEFT);
1435 const bool ignoreIndirectBicycleTurn = pastTheCrossingPoint && foeIsBicycleTurn;
1436 const bool cannotIgnore = ((contLane && !ignoreIndirectBicycleTurn) || sameTarget || sameSource) && ego != nullptr;
1437 const bool inTheWay = ((((!pastTheCrossingPoint && distToCrossing > 0) || (sameTarget && distToCrossing > leaderBackDist - leader->getLength()))
1438 && enteredTheCrossingPoint
1439 && (!foeExitLink->isInternalJunctionLink() || foeIsBicycleTurn))
1440 || foeExitLink->getLaneBefore()->getNormalPredecessorLane() == myLane->getBidiLane());
1441 const bool isOpposite = leader->getLaneChangeModel().isOpposite();
1442 const auto avi = foeExitLink->getApproaching(leader);
1443 // if leader is not found, assume that it performed a lane change in the last step
1444 const bool willPass = avi.willPass || (avi.arrivalTime == INVALID_TIME && sameTarget);
1445 if (gDebugFlag1) {
1446 std::cout << " candidate leader=" << leader->getID()
1447 << " cannotIgnore=" << cannotIgnore
1448 << " fdtc=" << foeDistToCrossing
1449 << " lb=" << leaderBack
1450 << " lbd=" << leaderBackDist
1451 << " fcwidth=" << foeCrossingWidth
1452 << " r=" << myRadius
1453 << " sagitta=" << sagitta
1454 << " foePastCP=" << pastTheCrossingPoint
1455 << " foeEnteredCP=" << enteredTheCrossingPoint
1456 << " inTheWay=" << inTheWay
1457 << " willPass=" << willPass
1458 << " isFrontOnLane=" << leader->isFrontOnLane(foeLane)
1459 << " ignoreGreenCont=" << ignoreGreenCont
1460 << " foeIndirect=" << foeIndirect
1461 << " foeBikeTurn=" << foeIsBicycleTurn
1462 << " isOpposite=" << isOpposite << "\n";
1463 }
1464 if (leader == ego) {
1465 continue;
1466 }
1467 // ignore greenCont foe vehicles that are not in the way
1468 if (!inTheWay && ignoreGreenCont) {
1469 if (gDebugFlag1) {
1470 std::cout << " ignoreGreenCont\n";
1471 }
1472 continue;
1473 }
1474 // after entering the conflict area, ignore foe vehicles that are not in the way
1475 if (distToCrossing < -POSITION_EPS && !inTheWay
1476 && (ego == nullptr || !MSGlobals::gComputeLC || distToCrossing < -ego->getVehicleType().getLength())) {
1477 if (gDebugFlag1) {
1478 std::cout << " ego entered conflict area\n";
1479 }
1480 continue;
1481 }
1483 && sameSource
1484 && &ego->getLane()->getEdge() == &myInternalLaneBefore->getEdge()
1485 && leaderBack + leader->getLength() < ego->getPositionOnLane() - ego->getLength()) {
1486 // ego is already on the junction and clearly ahead of foe
1487 if (gDebugFlag1) {
1488 std::cout << " ego ahead of same-source foe\n";
1489 }
1490 continue;
1491 }
1492
1493 // ignore foe vehicles that will not pass
1494 if ((!cannotIgnore || leader->isStopped() || sameTarget)
1495 && !willPass
1496 && leader->isFrontOnLane(foeLane)
1497 && !isOpposite
1498 && !inTheWay
1499 // willPass is false if the vehicle is already on the stopping edge
1500 && !leader->willStop()) {
1501 if (gDebugFlag1) {
1502 std::cout << " foe will not pass\n";
1503 }
1504 continue;
1505 }
1506 if (leader->isBidiOn(foeLane)) {
1507 // conflict resolved via forward lane of the foe
1508 continue;
1509 }
1510 // check whether foe is blocked and might need to change before leaving the junction
1511 const bool foeStrategicBlocked = (leader->getLaneChangeModel().isStrategicBlocked() &&
1512 leader->getCarFollowModel().brakeGap(leader->getSpeed()) <= foeLane->getLength() - leaderBack);
1513 const bool sameInternalEdge = &myInternalLaneBefore->getEdge() == &foeExitLink->getInternalLaneBefore()->getEdge();
1514
1515 const bool foeLaneIsBidi = myInternalLaneBefore->getBidiLane() == foeLane;
1516 if (MSGlobals::gSublane && ego != nullptr && (sameSource || sameTarget || foeLaneIsBidi)
1517 && (!foeStrategicBlocked || sameInternalEdge)) {
1518 if (ego->getLane() == leader->getLane()) {
1519 continue;
1520 }
1521 // ignore vehicles if not in conflict sublane-wise
1522 const double egoLatOffset = isShadowLink ? ego->getLatOffset(ego->getLaneChangeModel().getShadowLane()) : 0;
1523 const double posLat = ego->getLateralPositionOnLane() + egoLatOffset;
1524 double posLatLeader = leader->getLateralPositionOnLane() + leader->getLatOffset(foeLane);
1525 if (foeLaneIsBidi) {
1526 // leader is oncoming
1527 posLatLeader = foeLane->getWidth() - posLatLeader;
1528 }
1529 const double latGap = (fabs(posLat - posLatLeader)
1530 - 0.5 * (ego->getVehicleType().getWidth() + leader->getVehicleType().getWidth()));
1531 const double maneuverDist = leader->getLaneChangeModel().getManeuverDist() * (posLat < posLatLeader ? -1 : 1);
1532 if (gDebugFlag1) {
1533 std::cout << " checkIgnore sublaneFoe lane=" << myInternalLaneBefore->getID()
1534 << " sameSource=" << sameSource
1535 << " sameTarget=" << sameTarget
1536 << " foeLaneIsBidi=" << foeLaneIsBidi
1537 << " foeLane=" << foeLane->getID()
1538 << " leader=" << leader->getID()
1539 << " egoLane=" << ego->getLane()->getID()
1540 << " leaderLane=" << leader->getLane()->getID()
1541 << " egoLat=" << posLat
1542 << " egoLatOffset=" << egoLatOffset
1543 << " leaderLat=" << posLatLeader
1544 << " leaderLatOffset=" << leader->getLatOffset(foeLane)
1545 << " latGap=" << latGap
1546 << " maneuverDist=" << maneuverDist
1547 << " computeLC=" << MSGlobals::gComputeLC
1548 << " egoMaxSpeedLat=" << ego->getVehicleType().getMaxSpeedLat()
1549 << "\n";
1550 }
1551 if (latGap > 0 && (latGap > maneuverDist || !sameTarget || !MSGlobals::gComputeLC)
1552 // do not perform sublane changes that interfere with the leader vehicle
1553 && (!MSGlobals::gComputeLC || latGap > ego->getVehicleType().getMaxSpeedLat())) {
1554 const MSLink* foeEntryLink = foeLane->getIncomingLanes().front().viaLink;
1555 if (sameSource) {
1556 // for lanes from the same edge, higer index implies a
1557 // connection further to the left
1558 const bool leaderFromRight = (myIndex > foeEntryLink->getIndex());
1559 if ((posLat > posLatLeader) == leaderFromRight) {
1560 // ignore speed since lanes diverge
1561 if (gDebugFlag1) {
1562 std::cout << " ignored (same source) leaderFromRight=" << leaderFromRight << "\n";
1563 }
1564 continue;
1565 }
1566 } else if (sameTarget) {
1567 // for lanes from different edges we cannot rely on the
1568 // index due to wrap-around issues
1569 if (myDirection != foeEntryLink->getDirection()) {
1570 bool leaderFromRight = foeEntryLink->getDirection() < myDirection;
1571 // leader vehicle should not move towards ego
1573 leaderFromRight = !leaderFromRight;
1574 }
1575 if ((posLat > posLatLeader) == leaderFromRight
1576 // leader should keep lateral position or move away from ego
1577 && (leader->getLaneChangeModel().getSpeedLat() == 0
1578 || leaderFromRight == (leader->getLaneChangeModel().getSpeedLat() < latGap))
1579 && (ego->getLaneChangeModel().getSpeedLat() == 0
1580 || leaderFromRight == (ego->getLaneChangeModel().getSpeedLat() > latGap))) {
1581 if (gDebugFlag1) {
1582 std::cout << " ignored (different source) leaderFromRight=" << leaderFromRight << "\n";
1583 }
1584 continue;
1585 }
1586 } else {
1587 // XXX figure out relative direction somehow
1588 }
1589 } else {
1590 if (gDebugFlag1) {
1591 std::cout << " ignored oncoming bidi leader\n";
1592 }
1593 continue;
1594 }
1595 }
1596 }
1598 // compute distance between vehicles on the the superimposition of both lanes
1599 // where the crossing point is the common point
1600 double gap;
1601 bool fromLeft = true;
1602 if (ego == nullptr) {
1603 // request from pedestrian model. return distance between leaderBack and crossing point
1604 //std::cout << " foeLane=" << foeLane->getID() << " leaderBack=" << leaderBack << " foeDistToCrossing=" << foeDistToCrossing << " foeLength=" << foeLane->getLength() << " foebehind=" << myConflicts[i].second << " dist=" << dist << " behind=" << myConflicts[i].first << "\n";
1605 gap = leaderBackDist;
1606 // distToCrossing should not take into account the with of the foe lane
1607 // (which was subtracted in setRequestInformation)
1608 // Instead, the width of the foe vehicle is used directly by the caller.
1609 distToCrossing += myConflicts[i].conflictSize / 2;
1610 if (gap + foeCrossingWidth < 0) {
1611 // leader is completely past the crossing point
1612 // or there is no crossing point
1613 continue; // next vehicle
1614 }
1615 // we need to determine whether the vehicle passes the
1616 // crossing from the left or the right (heuristic)
1617 fromLeft = foeDistToCrossing > 0.5 * foeLane->getLength();
1618 } else if ((contLane && !sameSource && !ignoreIndirectBicycleTurn) || isOpposite) {
1619 gap = -std::numeric_limits<double>::max(); // always break for vehicles which are on a continuation lane or for opposite-direction vehicles
1620 } else {
1621 if (pastTheCrossingPoint && !sameTarget) {
1622 // leader is completely past the crossing point
1623 // or there is no crossing point
1624 if (gDebugFlag1) {
1625 std::cout << " foePastCP ignored\n";
1626 }
1627 continue;
1628 }
1629 double leaderBackDist2 = leaderBackDist;
1630 if (sameTarget && leaderBackDist2 < 0) {
1631 const double mismatch = myConflicts[i].getFoeLengthBehindCrossing(foeExitLink) - myConflicts[i].getLengthBehindCrossing(this);
1632 if (mismatch > 0) {
1633 leaderBackDist2 += mismatch;
1634 }
1635 }
1636 if (gDebugFlag1) {
1637 std::cout << " distToCrossing=" << distToCrossing << " leaderBack=" << leaderBack
1638 << " backDist=" << leaderBackDist
1639 << " backDist2=" << leaderBackDist2
1640 << " blockedStrategic=" << leader->getLaneChangeModel().isStrategicBlocked()
1641 << "\n";
1642 }
1643 gap = distToCrossing - ego->getVehicleType().getMinGap() - leaderBackDist2 - foeCrossingWidth;
1644 if (leader->getLaneChangeModel().isStrategicBlocked()) {
1645 // do not encroach on leader when it tries to change lanes
1646 // factor 2 is to give some slack for lane-changing
1647 gap -= leader->getLength() * 2;
1648 }
1649 }
1650 // if the foe is already moving off the intersection, we may
1651 // advance up to the crossing point unless we have the same target or same source
1652 // (for sameSource, the crossing point indicates the point of divergence)
1653 const bool stopAsap = leader->isFrontOnLane(foeLane) ? cannotIgnore : (sameTarget || sameSource);
1654 if (gDebugFlag1) {
1655 std::cout << " leader=" << leader->getID() << " contLane=" << contLane << " cannotIgnore=" << cannotIgnore << " stopAsap=" << stopAsap << " gap=" << gap << "\n";
1656 }
1657 if (ignoreFoe(ego, leader)) {
1658 continue;
1659 }
1660 const int llFlags = ((fromLeft ? LL_FROM_LEFT : 0) |
1661 (inTheWay ? LL_IN_THE_WAY : 0) |
1662 (sameSource ? LL_SAME_SOURCE : 0) |
1663 (sameTarget ? LL_SAME_TARGET : 0));
1664 result.emplace_back(leader, gap, stopAsap ? -1 : distToCrossing, llFlags, leader->getLatOffset(foeLane));
1665 }
1666
1667 }
1668 if (ego != nullptr && MSNet::getInstance()->hasPersons()) {
1669 // check for crossing pedestrians (keep driving if already on top of the crossing
1670 const double distToPeds = distToCrossing - MSPModel::SAFETY_GAP;
1671 const double vehWidth = ego->getVehicleType().getWidth() + MSPModel::SAFETY_GAP; // + configurable safety gap
1673 // @check lefthand?!
1674 const bool wayIn = myConflicts[i].lengthBehindCrossing < myLaneBefore->getLength() * 0.5;
1675 const double vehSideOffset = (foeDistToCrossing + myLaneBefore->getWidth() * 0.5 - vehWidth * 0.5
1676 + ego->getLateralPositionOnLane() * (wayIn ? -1 : 1));
1677 // can access the movement model here since we already checked for existing persons above
1678 if (distToPeds >= -MSPModel::SAFETY_GAP && MSNet::getInstance()->getPersonControl().getMovementModel()->blockedAtDist(ego, foeLane, vehSideOffset, vehWidth,
1680 collectBlockers)) {
1681 result.emplace_back(nullptr, -1, distToPeds);
1682 }
1683 }
1684 }
1685
1686 //std::cout << SIMTIME << " ego=" << Named::getIDSecure(ego) << " link=" << getViaLaneOrLane()->getID() << " myWalkingAreaFoe=" << Named::getIDSecure(myWalkingAreaFoe) << "\n";
1687 if (ego != nullptr) {
1688 checkWalkingAreaFoe(ego, myWalkingAreaFoe, collectBlockers, result);
1689 checkWalkingAreaFoe(ego, myWalkingAreaFoeExit, collectBlockers, result);
1690 }
1691
1692 if (MSGlobals::gLateralResolution > 0 && ego != nullptr && !isShadowLink) {
1693 // check for foes on the same edge
1694 for (std::vector<MSLane*>::const_iterator it = mySublaneFoeLanes.begin(); it != mySublaneFoeLanes.end(); ++it) {
1695 const MSLane* foeLane = *it;
1697 for (MSLane::AnyVehicleIterator it_veh = foeLane->anyVehiclesBegin(); it_veh != end; ++it_veh) {
1698 MSVehicle* leader = (MSVehicle*)*it_veh;
1699 if (leader == ego) {
1700 continue;
1701 }
1702 if (leader->getLane()->isNormal()) {
1703 // leader is past the conflict point
1704 continue;
1705 }
1706 const double maxLength = MAX2(myInternalLaneBefore->getLength(), foeLane->getLength());
1707 const double gap = dist - maxLength - ego->getVehicleType().getMinGap() + leader->getBackPositionOnLane(foeLane);
1708 if (gap < -(ego->getVehicleType().getMinGap() + leader->getLength())) {
1709 // ego is ahead of leader
1710 continue;
1711 }
1712 const double posLat = ego->getLateralPositionOnLane();
1713 const double posLatLeader = leader->getLateralPositionOnLane() + leader->getLatOffset(foeLane);
1714 if (gDebugFlag1) {
1715 std::cout << " sublaneFoe lane=" << myInternalLaneBefore->getID()
1716 << " foeLane=" << foeLane->getID()
1717 << " leader=" << leader->getID()
1718 << " egoLane=" << ego->getLane()->getID()
1719 << " leaderLane=" << leader->getLane()->getID()
1720 << " gap=" << gap
1721 << " egoLat=" << posLat
1722 << " leaderLat=" << posLatLeader
1723 << " leaderLatOffset=" << leader->getLatOffset(foeLane)
1724 << " egoIndex=" << myInternalLaneBefore->getIndex()
1725 << " foeIndex=" << foeLane->getIndex()
1726 << " dist=" << dist
1727 << " leaderBack=" << leader->getBackPositionOnLane(foeLane)
1728 << "\n";
1729 }
1730 // there only is a conflict if the paths cross
1731 if ((posLat < posLatLeader && myInternalLaneBefore->getIndex() > foeLane->getIndex())
1732 || (posLat > posLatLeader && myInternalLaneBefore->getIndex() < foeLane->getIndex())) {
1733 if (gDebugFlag1) {
1734 std::cout << SIMTIME << " blocked by " << leader->getID() << " (sublane split) foeLane=" << foeLane->getID() << "\n";
1735 }
1736 if (ignoreFoe(ego, leader)) {
1737 continue;
1738 }
1739 result.emplace_back(leader, gap, -1);
1740 }
1741 }
1742 }
1743 }
1744 return result;
1745}
1746
1747
1748void
1749MSLink::checkWalkingAreaFoe(const MSVehicle* ego, const MSLane* foeLane, std::vector<const MSPerson*>* collectBlockers, LinkLeaders& result) const {
1750 if (foeLane != nullptr && foeLane->getEdge().getPersons().size() > 0) {
1751 // pedestrians may be on an arbitrary path across this
1752 // walkingarea. make sure to keep enough distance.
1753 // This is a simple but conservative solution that could be improved
1754 // by ignoring pedestrians that are "obviously" not on a collision course
1755 double distToPeds = std::numeric_limits<double>::max();
1756 assert(myInternalLaneBefore != nullptr);
1758 if (ego->getLateralPositionOnLane() != 0) {
1759 egoPath.move2side((MSGlobals::gLefthand ? 1 : -1) * ego->getLateralPositionOnLane());
1760 }
1761 for (MSTransportable* t : foeLane->getEdge().getPersons()) {
1762 MSPerson* p = static_cast<MSPerson*>(t);
1763 double dist = ego->getPosition().distanceTo2D(p->getPosition()) - p->getVehicleType().getLength();
1764 const bool inFront = isInFront(ego, egoPath, p->getPosition()) || isInFront(ego, egoPath, getFuturePosition(p));
1765 if (inFront) {
1766 dist -= ego->getVehicleType().getMinGap();
1767 }
1768#ifdef DEBUG_WALKINGAREA
1769 if (ego->isSelected()) {
1770 std::cout << SIMTIME << " veh=" << ego->getID() << " ped=" << p->getID()
1771 << " pos=" << ego->getPosition() << " pedPos=" << p->getPosition()
1772 << " futurePedPos=" << getFuturePosition(p)
1773 << " rawDist=" << ego->getPosition().distanceTo2D(p->getPosition())
1774 << " inFront=" << inFront
1775 << " dist=" << dist << "\n";
1776 }
1777#endif
1778 if (dist < ego->getVehicleType().getWidth() / 2 || inFront) {
1779 if (inFront) {
1780 const double oncomingFactor = isOnComingPed(ego, p);
1781 if (oncomingFactor > 0) {
1782 // account for pedestrian movement while closing in
1783 const double timeToStop = sqrt(dist) / 2;
1784 const double pedDist = p->getMaxSpeed() * MAX2(timeToStop, TS) * oncomingFactor;
1785 dist = MAX2(0.0, dist - pedDist);
1786#ifdef DEBUG_WALKINGAREA
1787 if (ego->isSelected()) {
1788 std::cout << " timeToStop=" << timeToStop << " pedDist=" << pedDist << " factor=" << oncomingFactor << " dist2=" << dist << "\n";
1789 }
1790#endif
1791 }
1792 }
1793 if (ignoreFoe(ego, p)) {
1794 continue;
1795 }
1796 distToPeds = MIN2(distToPeds, dist);
1797 if (collectBlockers != nullptr) {
1798 collectBlockers->push_back(p);
1799 }
1800 }
1801 }
1802 if (distToPeds != std::numeric_limits<double>::max()) {
1803 // leave extra space in front
1804 result.emplace_back(nullptr, -1, distToPeds);
1805 }
1806 }
1807}
1808
1809bool
1810MSLink::isInFront(const MSVehicle* ego, const PositionVector& egoPath, const Position& pPos) const {
1811 const double pedAngle = ego->getPosition().angleTo2D(pPos);
1812 const double angleDiff = fabs(GeomHelper::angleDiff(ego->getAngle(), pedAngle));
1813#ifdef DEBUG_WALKINGAREA
1814 if (ego->isSelected()) {
1815 std::cout << " angleDiff=" << RAD2DEG(angleDiff) << "\n";
1816 }
1817#endif
1818 if (angleDiff < DEG2RAD(75)) {
1819 return egoPath.distance2D(pPos) < ego->getVehicleType().getWidth() + MSPModel::SAFETY_GAP;
1820 }
1821 return false;
1822}
1823
1824
1825double
1826MSLink::isOnComingPed(const MSVehicle* ego, const MSPerson* p) const {
1827 const double pedToEgoAngle = p->getPosition().angleTo2D(ego->getPosition());
1828 const double angleDiff = fabs(GeomHelper::angleDiff(p->getAngle(), pedToEgoAngle));
1829#ifdef DEBUG_WALKINGAREA
1830 if (ego->isSelected()) {
1831 std::cout << " ped-angleDiff=" << RAD2DEG(angleDiff) << " res=" << cos(angleDiff) << "\n";
1832 }
1833#endif
1834 if (angleDiff <= DEG2RAD(90)) {
1835 ;
1836 return cos(angleDiff);
1837 } else {
1838 return 0;
1839 }
1840}
1841
1842
1844MSLink::getFuturePosition(const MSPerson* p, double timeHorizon) const {
1845 const double a = p->getAngle();
1846 const double dist = timeHorizon * p->getMaxSpeed();
1847
1848 const Position offset(cos(a) * dist, sin(a) * dist);
1849 return p->getPosition() + offset;
1850}
1851
1852
1853MSLink*
1854MSLink::getParallelLink(int direction) const {
1855 if (direction == -1) {
1856 return myParallelRight;
1857 } else if (direction == 1) {
1858 return myParallelLeft;
1859 } else {
1860 assert(false || myLane->getOpposite() != nullptr);
1861 return nullptr;
1862 }
1863}
1864
1865MSLink*
1867 if (myLane->getOpposite() != nullptr && myLaneBefore->getOpposite() != nullptr) {
1868 for (MSLink* cand : myLane->getOpposite()->getLinkCont()) {
1869 if (cand->getLane() == myLaneBefore->getOpposite()) {
1870 return cand;
1871 }
1872 }
1873 }
1874 return nullptr;
1875}
1876
1877
1878MSLink*
1880 const MSLane* const before = getLaneBefore()->getParallelLane(direction, false);
1881 const MSLane* const after = getLane()->getParallelLane(direction, false);
1882 if (before != nullptr && after != nullptr) {
1883 for (MSLink* const link : before->getLinkCont()) {
1884 if (link->getLane() == after) {
1885 return link;
1886 }
1887 }
1888 }
1889 return nullptr;
1890}
1891
1892
1893double
1894MSLink::getZipperSpeed(const MSVehicle* ego, const double dist, double vSafe,
1895 SUMOTime arrivalTime,
1896 const BlockingFoes* foes) const {
1897 if (myFoeLinks.size() == 0) {
1898 // link should have LINKSTATE_MAJOR in this case
1899 assert(false);
1900 return vSafe;
1901 } else if (myFoeLinks.size() > 1) {
1902 throw ProcessError("Zipper junctions with more than two conflicting lanes are not supported (at junction '"
1903 + myJunction->getID() + "')");
1904 }
1905 const double brakeGap = ego->getCarFollowModel().brakeGap(ego->getSpeed(), ego->getCarFollowModel().getMaxDecel(), 0);
1906 if (dist > MAX2(myFoeVisibilityDistance, brakeGap)) {
1907#ifdef DEBUG_ZIPPER
1909 if (DEBUG_COND_ZIPPER) DEBUGOUT(SIMTIME << " getZipperSpeed ego=" << ego->getID()
1910 << " dist=" << dist << " ignoring foes (arrival in " << STEPS2TIME(arrivalTime - now) << ")\n")
1911#endif
1912 return vSafe;
1913 }
1914#ifdef DEBUG_ZIPPER
1915 if (DEBUG_COND_ZIPPER) DEBUGOUT(SIMTIME << " getZipperSpeed ego=" << ego->getID()
1916 << " egoAT=" << arrivalTime
1917 << " dist=" << dist
1918 << " brakeGap=" << brakeGap
1919 << " vSafe=" << vSafe
1920 << " numFoes=" << foes->size()
1921 << "\n")
1922#endif
1923 MSLink* foeLink = myFoeLinks[0];
1924 for (const auto& item : *foes) {
1925 const MSVehicle* foe = dynamic_cast<const MSVehicle*>(item);
1926 assert(foe != 0);
1927 const ApproachingVehicleInformation& avi = foeLink->getApproaching(foe);
1928 const double foeDist = (foe->isActive() ? avi.dist : MAX2(0.0, avi.dist -
1929 STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - foe->getLastActionTime()) * avi.speed));
1930
1931 if ( // ignore vehicles that arrive after us (unless they are ahead and we could easily brake for them)
1932 ((avi.arrivalTime > arrivalTime) && !couldBrakeForLeader(dist, foeDist, ego, foe)) ||
1933 // also ignore vehicles that are behind us and are able to brake for us
1934 couldBrakeForLeader(foeDist, dist, foe, ego) ||
1935 // resolve ties by lane index
1936 (avi.arrivalTime == arrivalTime && foeDist == dist && ego->getLane()->getIndex() < foe->getLane()->getIndex())) {
1937#ifdef DEBUG_ZIPPER
1938 if (DEBUG_COND_ZIPPER) std::cout
1939 << " ignoring foe=" << foe->getID()
1940 << " foeAT=" << avi.arrivalTime
1941 << " foeDist=" << avi.dist
1942 << " foeDist2=" << foeDist
1943 << " foeSpeed=" << avi.speed
1944 << " egoSpeed=" << ego->getSpeed()
1945 << " deltaDist=" << foeDist - dist
1946 << " delteSpeed=" << avi.speed - foe->getCarFollowModel().getMaxDecel() - ego->getSpeed()
1947 << " egoCouldBrake=" << couldBrakeForLeader(dist, foeDist, ego, foe)
1948 << " foeCouldBrake=" << couldBrakeForLeader(foeDist, dist, foe, ego)
1949 << "\n";
1950#endif
1951 continue;
1952 }
1953 // the idea behind speed adaption is three-fold:
1954 // 1) ego needs to be in a car-following relationship with foe eventually
1955 // thus, the ego speed should be equal to the follow speed once the foe enters
1956 // the zipper junction
1957 // 2) ego vehicle needs to put a certain distance beteen himself and foe (safeGap)
1958 // achieving this distance can be spread over time but computing
1959 // safeGap is subject to estimation errors of future speeds
1960 // 3) deceleration can be spread out over the time until true
1961 // car-following happens, at the start of speed adaptions, smaller
1962 // decelerations should be sufficient
1963
1964 // we cannot trust avi.arrivalSpeed if the foe has leader vehicles that are accelerating
1965 // lets try to extrapolate
1966 const double uMax = foe->getLane()->getVehicleMaxSpeed(foe);
1967 const double uAccel = foe->getCarFollowModel().estimateSpeedAfterDistance(foeDist, avi.speed, foe->getCarFollowModel().getMaxAccel());
1968 const double uEnd = MIN2(uMax, uAccel);
1969 const double uAvg = (avi.speed + uEnd) / 2;
1970 const double tf0 = foeDist / MAX2(NUMERICAL_EPS, uAvg);
1971 const double tf = MAX2(1.0, ceil((tf0) / TS) * TS);
1972
1973 const double vMax = ego->getLane()->getVehicleMaxSpeed(ego);
1974 const double vAccel = ego->getCarFollowModel().estimateSpeedAfterDistance(dist, ego->getSpeed(), ego->getCarFollowModel().getMaxAccel());
1975 const double vDecel = ego->getCarFollowModel().estimateSpeedAfterDistance(dist, ego->getSpeed(), ego->getCarFollowModel().getMaxDecel());
1976 const double vEnd = MIN3(vMax, vAccel, MAX2(uEnd, vDecel));
1977 const double vAvg = (ego->getSpeed() + vEnd) / 2;
1978 const double te0 = dist / MAX2(NUMERICAL_EPS, vAvg);
1979 const double te = MAX2(1.0, ceil((te0) / TS) * TS);
1980
1981 const double gap = dist - foe->getVehicleType().getLength() - ego->getVehicleType().getMinGap() - foeDist;
1982 const double safeGap = ego->getCarFollowModel().getSecureGap(ego, foe, vEnd, uEnd, foe->getCarFollowModel().getMaxDecel());
1983 // round t to next step size
1984 // increase gap to safeGap by the time foe reaches link
1985 // gap + u*t - (t * v + a * t^2 / 2) = safeGap
1986 const double deltaGap = gap + tf * uAvg - safeGap - vAvg * tf;
1987 const double a = 2 * deltaGap / (tf * tf);
1988 const double vSafeGap = ego->getSpeed() + ACCEL2SPEED(a);
1989 const double vFollow = ego->getCarFollowModel().followSpeed(
1990 ego, ego->getSpeed(), gap, avi.speed, foe->getCarFollowModel().getMaxDecel(), foe);
1991
1992 // scale behavior based on ego time to link (te)
1993 const double w = MIN2(1.0, te / 10);
1994 const double maxDecel = w * ego->getCarFollowModel().getMaxDecel() + (1 - w) * ego->getCarFollowModel().getEmergencyDecel();
1995 const double vZipper = MAX3(vFollow, ego->getSpeed() - ACCEL2SPEED(maxDecel), w * vSafeGap + (1 - w) * vFollow);
1996
1997 vSafe = MIN2(vSafe, vZipper);
1998#ifdef DEBUG_ZIPPER
1999 if (DEBUG_COND_ZIPPER) std::cout << " adapting to foe=" << foe->getID()
2000 << " foeDist=" << foeDist
2001 << " foeSpeed=" << avi.speed
2002 << " foeAS=" << avi.arrivalSpeed
2003 << " egoSpeed=" << ego->getSpeed()
2004 << " uMax=" << uMax
2005 << " uAccel=" << uAccel
2006 << " uEnd=" << uEnd
2007 << " uAvg=" << uAvg
2008 << " gap=" << gap
2009 << " safeGap=" << safeGap
2010 << "\n "
2011 << " tf=" << tf
2012 << " te=" << te
2013 << " dg=" << deltaGap
2014 << " aSafeGap=" << a
2015 << " vMax=" << vMax
2016 << " vAccel=" << vAccel
2017 << " vDecel=" << vDecel
2018 << " vEnd=" << vEnd
2019 << " vSafeGap=" << vSafeGap
2020 << " vFollow=" << vFollow
2021 << " w=" << w
2022 << " maxDecel=" << maxDecel
2023 << " vZipper=" << vZipper
2024 << " vSafe=" << vSafe
2025 << "\n";
2026#endif
2027 }
2028 return vSafe;
2029}
2030
2031
2032bool
2033MSLink::couldBrakeForLeader(double followDist, double leaderDist, const MSVehicle* follow, const MSVehicle* leader) {
2034 return (// leader is ahead of follower
2035 followDist > leaderDist &&
2036 // and follower could brake for 1 s to stay behind leader
2037 followDist - leaderDist > follow->getSpeed() - follow->getCarFollowModel().getMaxDecel() - leader->getSpeed());
2038}
2039
2040
2041void
2046
2047bool
2049 // check whether this link gets to keep its cont status switching the tls off
2050 // @note: this could also be pre-computed in netconvert
2051 // we check whether there is any major link from this edge
2052 for (const MSLane* cand : myLaneBefore->getEdge().getLanes()) {
2053 for (const MSLink* link : cand->getLinkCont()) {
2054 if (link->getOffState() == LINKSTATE_TL_OFF_NOSIGNAL) {
2055 return true;
2056 }
2057 }
2058 }
2059 return false;
2060}
2061
2062bool
2063MSLink::lateralOverlap(double posLat, double width, double posLat2, double width2) {
2064 return fabs(posLat2 - posLat) < (width + width2) / 2;
2065}
2066
2067std::string
2069 return myLaneBefore->getID() + "->" + getViaLaneOrLane()->getID();
2070}
2071
2072
2073bool
2075 if (ego == nullptr || !ego->getParameter().wasSet(VEHPARS_JUNCTIONMODEL_PARAMS_SET)) {
2076 return false;
2077 }
2078 const SUMOVehicleParameter& param = ego->getParameter();
2079 for (const std::string& typeID : StringTokenizer(param.getParameter(toString(SUMO_ATTR_JM_IGNORE_TYPES), "")).getVector()) {
2080 if (typeID == foe->getVehicleType().getID()) {
2081 return true;
2082 }
2083 }
2084 for (const std::string& id : StringTokenizer(param.getParameter(toString(SUMO_ATTR_JM_IGNORE_IDS), "")).getVector()) {
2085 if (id == foe->getID()) {
2086 return true;
2087 }
2088 }
2089 return false;
2090}
2091
2092/****************************************************************************/
long long int SUMOTime
Definition GUI.h:36
#define DEG2RAD(x)
Definition GeomHelper.h:35
#define RAD2DEG(x)
Definition GeomHelper.h:36
#define DEBUG_COND2(obj)
Definition MESegment.cpp:52
#define WRITE_WARNING(msg)
Definition MsgHandler.h:270
SUMOTime DELTA_T
Definition SUMOTime.cpp:38
std::string time2string(SUMOTime t, bool humanReadable)
convert SUMOTime to string (independently of global format setting)
Definition SUMOTime.cpp:69
#define STEPS2TIME(x)
Definition SUMOTime.h:55
#define SIMSTEP
Definition SUMOTime.h:61
#define ACCEL2SPEED(x)
Definition SUMOTime.h:51
#define SUMOTime_MAX
Definition SUMOTime.h:34
#define SUMOTime_MIN
Definition SUMOTime.h:35
#define TS
Definition SUMOTime.h:42
#define SIMTIME
Definition SUMOTime.h:62
#define TIME2STEPS(x)
Definition SUMOTime.h:57
@ 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:137
T MIN2(T a, T b)
Definition StdDefs.h:76
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)
static double angleDiff(const double angle1, const double angle2)
Returns the difference of the second angle to the first angle in radiants.
static double getMinAngleDiff(double angle1, double angle2)
Returns the minimum distance (clockwise/counter-clockwise) between both angles.
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
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)
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:2653
const MSLane * getNormalSuccessorLane() const
get normal lane following this internal lane, for normal lanes, the lane itself is returned
Definition MSLane.cpp:3034
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:2583
const MSLink * getLinkTo(const MSLane *const) const
returns the link to the given lane or nullptr, if it is not connected
Definition MSLane.cpp:2560
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:2999
bool isNormal() const
Definition MSLane.cpp:2462
bool isInternal() const
Definition MSLane.cpp:2456
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:4160
MSLane * getBidiLane() const
retrieve bidirectional lane or nullptr
Definition MSLane.cpp:4425
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:3024
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:322
bool hasPersons() const
Returns whether persons are simulated.
Definition MSNet.h:397
virtual MSTransportableControl & getPersonControl()
Returns the person control.
Definition MSNet.cpp:1172
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.
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:672
bool isActive() const
Returns whether the current simulation step is an action point for the vehicle.
Definition MSVehicle.h:631
bool isFrontOnLane(const MSLane *lane) const
Returns the information whether the front of the vehicle is on the given lane.
MSAbstractLaneChangeModel & getLaneChangeModel()
Position getPosition(const double offset=0) const
Return current position (x/y, cartesian)
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.
const MSLane * getLane() const
Returns the lane the vehicle is on.
Definition MSVehicle.h:584
bool isBidiOn(const MSLane *lane) const
whether this vehicle is driving against lane
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:978
bool ignoreRed(const MSLink *link, bool canBrake) const
decide whether a red (or yellow light) may be ignored
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:738
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.
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.
OutputDevice & writeAttr(const SumoXMLAttr attr, const T &val)
writes a named attribute
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)
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.
std::vector< std::string > getVector()
return vector of strings
const unsigned char flag[]
Definition flag.cpp:24
Definition json.hpp:4471