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