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 // ===========================================================================
78 MSLink::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()) {
118  const PositionVector& to = getViaLaneOrLane()->getShape();
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;
126  if (MSGlobals::gLefthand) {
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 
138 void
139 MSLink::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;
149  myHasFoes = hasFoes;
150  myAmCont = isCont;
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  const double lbcLane = MAX2(0.0, lane->getLength() - lane->interpolateGeometryPosToLanePos(distToDivergence));
283  const double lbcSibling = MAX2(0.0, sibling->getLength() - sibling->interpolateGeometryPosToLanePos(distToDivergence));
284  myLengthsBehindCrossing.push_back(std::make_pair(lbcLane, lbcSibling));
285  myFoeLanes.push_back(sibling);
286 #ifdef MSLink_DEBUG_CROSSING_POINTS
287  std::cout << " adding same-origin foe" << sibling->getID()
288  << " dist1=" << myLengthsBehindCrossing.back().first
289  << " dist2=" << myLengthsBehindCrossing.back().second
290  << "\n";
291 #endif
292  }
293  }
294  }
296  // check for links with the same origin lane and the same destination edge
297  const MSEdge* myTarget = &myLane->getEdge();
298  // save foes for entry links
299  for (MSLink* const it : myLaneBefore->getLinkCont()) {
300  const MSEdge* target = &(it->getLane()->getEdge());
301  if (it == this) {
302  continue;
303  }
304  if (target == myTarget) {
305  mySublaneFoeLinks.push_back(it);
306 #ifdef MSLink_DEBUG_CROSSING_POINTS
307  std::cout << " sublaneFoeLink (same target): " << it->getViaLaneOrLane()->getID() << "\n";
308 #endif
309  } else if (myDirection != LinkDirection::STRAIGHT && it->getDirection() == LinkDirection::STRAIGHT) {
310  // potential turn conflicht
311  mySublaneFoeLinks2.push_back(it);
312 #ifdef MSLink_DEBUG_CROSSING_POINTS
313  std::cout << " sublaneFoeLink2 (other target: " << it->getViaLaneOrLane()->getID() << "\n";
314 #endif
315  }
316  }
317  // save foes for exit links
318  if (fromInternalLane()) {
319  //std::cout << " setRequestInformation link=" << getViaLaneOrLane()->getID() << " before=" << myLaneBefore->getID() << " before2=" << myLaneBefore->getIncomingLanes().front().lane->getID() << "\n";
320  for (const MSLink* const link : myLaneBefore->getIncomingLanes().front().lane->getLinkCont()) {
321  if (link->getViaLane() != myInternalLaneBefore && &link->getLane()->getEdge() == myTarget) {
322  //std::cout << " add sublaneFoe=" << (*it)->getViaLane()->getID() << "\n";
323  mySublaneFoeLanes.push_back(link->getViaLane());
324  }
325  }
326  }
327  }
328  if (myInternalLaneBefore != nullptr
330  // for right turns, the curvature helps rather than restricts the linkLeader check
331  && (
334  const double angle = fabs(GeomHelper::angleDiff(
336  myLane->getShape().angleAt2D(0)));
337  if (angle > 0) {
338  double length = myInternalLaneBefore->getShape().length2D();
339  if (myInternalLaneBefore->getIncomingLanes().size() == 1 &&
340  myInternalLaneBefore->getIncomingLanes()[0].lane->isInternal()) {
341  length += myInternalLaneBefore->getIncomingLanes()[0].lane->getShape().length2D();
342  } else if (myInternalLane != nullptr) {
343  length += myInternalLane->getShape().length2D();
344  }
345  myRadius = length / angle;
346  //std::cout << getDescription() << " a=" << RAD2DEG(angle) << " l=" << length << " r=" << myRadius << "\n";
347  }
348  }
349 }
350 
351 
352 double
353 MSLink::computeDistToDivergence(const MSLane* lane, const MSLane* sibling, double minDist, bool sameSource) const {
354  double lbcSibling = 0;
355  double lbcLane = 0;
356 
357  PositionVector l = lane->getShape();
358  PositionVector s = sibling->getShape();
359  double length = l.length2D();
360  double sibLength = s.length2D();
361  if (!sameSource) {
362  l = l.reverse();
363  s = s.reverse();
364  } else if (sibling->getEntryLink()->myAmIndirect) {
365  // ignore final waiting position since it may be quite close to the lane
366  // shape but the waiting position is perpendicular (so the minDist
367  // requirement is not necessary
368  lbcSibling += s[-1].distanceTo2D(s[-2]);
369  s.pop_back();
370  } else if (lane->getEntryLink()->myAmIndirect) {
371  // ignore final waiting position since it may be quite close to the lane
372  // shape but the waiting position is perpendicular (so the minDist
373  // requirement is not necessary
374  lbcLane += l[-1].distanceTo2D(l[-2]);
375  l.pop_back();
376  }
377 
378 #ifdef MSLink_DEBUG_CROSSING_POINTS_DETAILS
379  std::cout << " sameSource=" << sameSource << " minDist=" << minDist << " backDist=" << l.back().distanceTo2D(s.back()) << "\n";
380 #endif
381  if (l.back().distanceTo2D(s.back()) > minDist) {
382  // compute the final divergence point
383  // this position serves two purposes:
384  // 1) once the foe vehicle back (on sibling) has passed this point, we can safely ignore it
385  // 2) both vehicles are put into a cf-relationship while before the point.
386  // Since the actual crossing point is at the start of the junction,
387  // we want to make sure that both vehicles have the same distance to the crossing point and thus follow each other naturally
388  std::vector<double> distances = l.distances(s);
389 #ifdef MSLink_DEBUG_CROSSING_POINTS
390  std::cout << " distances=" << toString(distances) << "\n";
391 #endif
392  assert(distances.size() == l.size() + s.size());
393  if (distances.back() > minDist && distances[l.size() - 1] > minDist) {
394  // do a pairwise check between lane and sibling to make because we do not know which of them bends more
395  for (int j = (int)s.size() - 2; j >= 0; j--) {
396  const int i = j + (int)l.size();
397  const double segLength = s[j].distanceTo2D(s[j + 1]);
398  if (distances[i] > minDist) {
399  lbcSibling += segLength;
400  } else {
401  // assume no sharp bends and just interpolate the last segment
402  lbcSibling += segLength - (minDist - distances[i]) * segLength / (distances[i + 1] - distances[i]);
403  break;
404  }
405  }
406  for (int i = (int)l.size() - 2; i >= 0; i--) {
407  const double segLength = l[i].distanceTo2D(l[i + 1]);
408  if (distances[i] > minDist) {
409  lbcLane += segLength;
410  } else {
411  // assume no sharp bends and just interpolate the last segment
412  lbcLane += segLength - (minDist - distances[i]) * segLength / (distances[i + 1] - distances[i]);
413  break;
414  }
415  }
416  }
417  assert(lbcSibling >= -NUMERICAL_EPS);
418  assert(lbcLane >= -NUMERICAL_EPS);
419  }
420  const double distToDivergence1 = sibling->getLength() - lbcSibling;
421  const double distToDivergence2 = lane->getLength() - lbcLane;
422  const double distToDivergence = MIN3(
423  MAX2(distToDivergence1, distToDivergence2),
424  sibLength, length);
425 #ifdef MSLink_DEBUG_CROSSING_POINTS
426  std::cout << " distToDivergence=" << distToDivergence
427  << " distTD1=" << distToDivergence1
428  << " distTD2=" << distToDivergence2
429  << " length=" << length
430  << " sibLength=" << sibLength
431  << "\n";
432 #endif
433  return distToDivergence;
434 }
435 
436 
437 bool
438 MSLink::contIntersect(const MSLane* lane, const MSLane* foe) {
439  if (foe->getLinkCont()[0]->getViaLane() != nullptr) {
440  std::vector<double> intersections = lane->getShape().intersectsAtLengths2D(foe->getShape());
441  return intersections.size() > 0;
442  }
443  return false;
444 }
445 
446 
447 void
448 MSLink::setApproaching(const SUMOVehicle* approaching, const SUMOTime arrivalTime, const double arrivalSpeed, const double leaveSpeed,
449  const bool setRequest, const SUMOTime arrivalTimeBraking, const double arrivalSpeedBraking, const SUMOTime waitingTime, double dist, double latOffset) {
450  const SUMOTime leaveTime = getLeaveTime(arrivalTime, arrivalSpeed, leaveSpeed, approaching->getVehicleType().getLength());
451 #ifdef DEBUG_APPROACHING
452  if (DEBUG_COND2(approaching)) {
453  std::cout << SIMTIME << " Link '" << (myLaneBefore == 0 ? "NULL" : myLaneBefore->getID()) << "'->'" << (myLane == 0 ? "NULL" : myLane->getID()) << "' Adding approaching vehicle '" << approaching->getID() << "'\nCurrently registered vehicles:" << std::endl;
454  for (auto i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
455  std::cout << "'" << i->first->getID() << "'" << std::endl;
456  }
457  }
458 #endif
459  myApproachingVehicles.emplace(approaching,
460  ApproachingVehicleInformation(arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, setRequest,
461  arrivalTimeBraking, arrivalSpeedBraking, waitingTime, dist, approaching->getSpeed(), latOffset));
462 }
463 
464 
465 void
467 
468 #ifdef DEBUG_APPROACHING
469  if (DEBUG_COND2(approaching)) {
470  std::cout << SIMTIME << " Link '" << (myLaneBefore == 0 ? "NULL" : myLaneBefore->getID()) << "'->'" << (myLane == 0 ? "NULL" : myLane->getID()) << "' Adding approaching vehicle '" << approaching->getID() << "'\nCurrently registered vehicles:" << std::endl;
471  for (auto i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
472  std::cout << "'" << i->first->getID() << "'" << std::endl;
473  }
474  }
475 #endif
476  myApproachingVehicles.emplace(approaching, ai);
477 }
478 
479 
480 void
482  myBlockedFoeLinks.insert(link);
483 }
484 
485 
486 
487 bool
489  for (std::set<MSLink*>::const_iterator i = myBlockedFoeLinks.begin(); i != myBlockedFoeLinks.end(); ++i) {
490  if ((*i)->isBlockingAnyone()) {
491  return true;
492  }
493  }
494  return false;
495 }
496 
497 
498 void
500 
501 #ifdef DEBUG_APPROACHING
502  if (DEBUG_COND2(veh)) {
503  std::cout << SIMTIME << " Link '" << (myLaneBefore == 0 ? "NULL" : myLaneBefore->getID()) << "'->'" << (myLane == 0 ? "NULL" : myLane->getID()) << std::endl;
504  std::cout << "' Removing approaching vehicle '" << veh->getID() << "'\nCurrently registered vehicles:" << std::endl;
505  for (auto i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
506  std::cout << "'" << i->first->getID() << "'" << std::endl;
507  }
508  }
509 #endif
510  myApproachingVehicles.erase(veh);
511 }
512 
513 
516  auto i = myApproachingVehicles.find(veh);
517  if (i != myApproachingVehicles.end()) {
518  return i->second;
519  } else {
520  return ApproachingVehicleInformation(INVALID_TIME, INVALID_TIME, 0, 0, false, INVALID_TIME, 0, 0, 0, 0, 0);
521  }
522 }
523 
524 void
526  myApproachingVehicles.clear();
527 }
528 
529 SUMOTime
530 MSLink::getLeaveTime(const SUMOTime arrivalTime, const double arrivalSpeed,
531  const double leaveSpeed, const double vehicleLength) const {
532  return arrivalTime + TIME2STEPS((getLength() + vehicleLength) / MAX2(0.5 * (arrivalSpeed + leaveSpeed), NUMERICAL_EPS));
533 }
534 
535 
536 bool
537 MSLink::opened(SUMOTime arrivalTime, double arrivalSpeed, double leaveSpeed, double vehicleLength,
538  double impatience, double decel, SUMOTime waitingTime, double posLat,
539  BlockingFoes* collectFoes, bool ignoreRed, const SUMOTrafficObject* ego) const {
540  if (haveRed() && !ignoreRed) {
541  return false;
542  }
544  return true;
545  }
546  const SUMOTime leaveTime = getLeaveTime(arrivalTime, arrivalSpeed, leaveSpeed, vehicleLength);
548  // check for foes on the same lane with the same target edge
549  for (const MSLink* foeLink : mySublaneFoeLinks) {
550  assert(myLane != foeLink->getLane());
551  for (const auto& it : foeLink->myApproachingVehicles) {
552  const SUMOVehicle* foe = it.first;
553  if (
554  // there only is a conflict if the paths cross
555  ((posLat < foe->getLateralPositionOnLane() + it.second.latOffset && myLane->getIndex() > foeLink->myLane->getIndex())
556  || (posLat > foe->getLateralPositionOnLane() + it.second.latOffset && myLane->getIndex() < foeLink->myLane->getIndex()))
557  // the vehicle that arrives later must yield
558  && (arrivalTime > it.second.arrivalTime
559  // if both vehicles arrive at the same time, the one
560  // to the left must yield
561  || (arrivalTime == it.second.arrivalTime && posLat > foe->getLateralPositionOnLane()))) {
562  if (blockedByFoe(foe, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, false,
563  impatience, decel, waitingTime, ego)) {
564 #ifdef MSLink_DEBUG_OPENED
565  if (gDebugFlag1) {
566  std::cout << SIMTIME << " blocked by " << foe->getID() << " arrival=" << arrivalTime << " foeArrival=" << it.second.arrivalTime << "\n";
567  }
568 #endif
569  if (collectFoes == nullptr) {
570 #ifdef MSLink_DEBUG_OPENED
571  if (gDebugFlag1) {
572  std::cout << " link=" << getViaLaneOrLane()->getID() << " blocked by sublaneFoe=" << foe->getID() << " foeLink=" << foeLink->getViaLaneOrLane()->getID() << " posLat=" << posLat << "\n";
573  }
574 #endif
575  return false;
576  } else {
577  collectFoes->push_back(it.first);
578  }
579  }
580  }
581  }
582  }
583  // check for foes on the same lane with a different target edge
584  // (straight movers take precedence if the paths cross)
585  const int lhSign = MSGlobals::gLefthand ? -1 : 1;
586  for (const MSLink* foeLink : mySublaneFoeLinks2) {
588  for (const auto& it : foeLink->myApproachingVehicles) {
589  const SUMOVehicle* foe = it.first;
590  // there only is a conflict if the paths cross
591  // and if the vehicles are not currently in a car-following relationship
592  const double egoWidth = ego == nullptr ? 1.8 : ego->getVehicleType().getWidth();
593  if (!lateralOverlap(posLat, egoWidth, foe->getLateralPositionOnLane() + it.second.latOffset, foe->getVehicleType().getWidth())
595  && (posLat * lhSign > (foe->getLateralPositionOnLane() + it.second.latOffset) * lhSign))
597  && (posLat * lhSign < (foe->getLateralPositionOnLane() + it.second.latOffset) * lhSign)))) {
598  if (blockedByFoe(foe, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, false,
599  impatience, decel, waitingTime, ego)) {
600 #ifdef MSLink_DEBUG_OPENED
601  if (gDebugFlag1) {
602  std::cout << SIMTIME << " blocked by sublane foe " << foe->getID() << " arrival=" << arrivalTime << " foeArrival=" << it.second.arrivalTime << "\n";
603  }
604 #endif
605  if (collectFoes == nullptr) {
606 #ifdef MSLink_DEBUG_OPENED
607  if (gDebugFlag1) {
608  std::cout << " link=" << getViaLaneOrLane()->getID() << " blocked by sublaneFoe2=" << foe->getID() << " foeLink=" << foeLink->getViaLaneOrLane()->getID() << " posLat=" << posLat << "\n";
609  }
610 #endif
611  return false;
612  } else {
613  collectFoes->push_back(it.first);
614  }
615  }
616  }
617  }
618  }
619  }
621  // priority usually means the link is open but there are exceptions:
622  // zipper still needs to collect foes
623  // sublane model could have detected a conflict
624  return collectFoes == nullptr || collectFoes->size() == 0;
625  }
626  if ((myState == LINKSTATE_STOP || myState == LINKSTATE_ALLWAY_STOP) && waitingTime == 0) {
627  return false;
628  }
629 
630 #ifdef MSLink_DEBUG_OPENED
631  if (gDebugFlag1) {
632  std::cout << SIMTIME << " opened link=" << getViaLaneOrLane()->getID() << " foeLinks=" << myFoeLinks.size() << "\n";
633  }
634 #endif
635 
636  if (MSGlobals::gUseMesoSim && impatience == 1) {
637  return true;
638  }
639  for (const MSLink* const link : myFoeLinks) {
641  if (link->haveRed()) {
642  continue;
643  }
644  }
645 #ifdef MSLink_DEBUG_OPENED
646  if (gDebugFlag1) {
647  std::cout << " foeLink=" << link->getViaLaneOrLane()->getID() << " numApproaching=" << link->getApproaching().size() << "\n";
648  }
649 #endif
650  if (link->blockedAtTime(arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, myLane == link->getLane(),
651  impatience, decel, waitingTime, collectFoes, ego)) {
652  return false;
653  }
654  }
655  if (collectFoes != nullptr && collectFoes->size() > 0) {
656  return false;
657  }
658  return true;
659 }
660 
661 
662 bool
663 MSLink::blockedAtTime(SUMOTime arrivalTime, SUMOTime leaveTime, double arrivalSpeed, double leaveSpeed,
664  bool sameTargetLane, double impatience, double decel, SUMOTime waitingTime,
665  BlockingFoes* collectFoes, const SUMOTrafficObject* ego) const {
666  for (const auto& it : myApproachingVehicles) {
667 #ifdef MSLink_DEBUG_OPENED
668  if (gDebugFlag1) {
669  if (ego != nullptr
670  && ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_SPEED, 0) >= it.second.speed
672  std::stringstream stream; // to reduce output interleaving from different threads
673  stream << SIMTIME << " " << myApproachingVehicles.size() << " foe link=" << getViaLaneOrLane()->getID()
674  << " foeVeh=" << it.first->getID() << " (below ignore speed)"
675  << " ignoreFoeProb=" << ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0)
676  << "\n";
677  std::cout << stream.str();
678  }
679  }
680 #endif
681  if (it.first != ego
682  && (ego == nullptr
684  || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_SPEED, 0) < it.second.speed
686  && !ignoreFoe(ego, it.first)
687  && blockedByFoe(it.first, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, sameTargetLane,
688  impatience, decel, waitingTime, ego)) {
689  if (collectFoes == nullptr) {
690  return true;
691  } else {
692  collectFoes->push_back(it.first);
693  }
694  }
695  }
696  return false;
697 }
698 
699 
700 bool
702  SUMOTime arrivalTime, SUMOTime leaveTime, double arrivalSpeed, double leaveSpeed,
703  bool sameTargetLane, double impatience, double decel, SUMOTime waitingTime,
704  const SUMOTrafficObject* ego) const {
705 #ifdef MSLink_DEBUG_OPENED
706  if (gDebugFlag1) {
707  std::stringstream stream; // to reduce output interleaving from different threads
708  stream << " link=" << getDescription()
709  << " foeVeh=" << veh->getID()
710  << " req=" << avi.willPass
711  << " aT=" << avi.arrivalTime
712  << " lT=" << avi.leavingTime
713  << "\n";
714  std::cout << stream.str();
715  }
716 #endif
717  if (!avi.willPass) {
718  return false;
719  }
721  assert(waitingTime > 0);
722  if (waitingTime > avi.waitingTime) {
723  return false;
724  }
725  if (waitingTime == avi.waitingTime && arrivalTime < avi.arrivalTime) {
726  return false;
727  }
728  }
729  const SUMOTime foeArrivalTime = (SUMOTime)((1.0 - impatience) * avi.arrivalTime + impatience * avi.arrivalTimeBraking);
730  const SUMOTime lookAhead = (myState == LINKSTATE_ZIPPER
732  : (ego == nullptr
735  //if (ego != 0) std::cout << SIMTIME << " ego=" << ego->getID() << " jmTimegapMinor=" << ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_TIMEGAP_MINOR, -1) << " lookAhead=" << lookAhead << "\n";
736 #ifdef MSLink_DEBUG_OPENED
737  if (gDebugFlag1) {
738  std::stringstream stream; // to reduce output interleaving from different threads
739  stream << " imp=" << impatience << " fATb=" << avi.arrivalTimeBraking << " fAT2=" << foeArrivalTime << " lA=" << lookAhead << " egoAT=" << arrivalTime << " egoLT=" << leaveTime << "\n";
740  std::cout << stream.str();
741  }
742 #endif
743  if (avi.leavingTime < arrivalTime) {
744  // ego wants to be follower
745  if (sameTargetLane && (arrivalTime - avi.leavingTime < lookAhead
746  || unsafeMergeSpeeds(avi.leaveSpeed, arrivalSpeed,
747  veh->getVehicleType().getCarFollowModel().getMaxDecel(), decel))) {
748 #ifdef MSLink_DEBUG_OPENED
749  if (gDebugFlag1) {
750  std::cout << " blocked (cannot follow)\n";
751  }
752 #endif
753  return true;
754  }
755  } else if (foeArrivalTime > leaveTime + lookAhead) {
756  // ego wants to be leader.
757  if (sameTargetLane && unsafeMergeSpeeds(leaveSpeed, avi.arrivalSpeedBraking,
758  decel, veh->getVehicleType().getCarFollowModel().getMaxDecel())) {
759 #ifdef MSLink_DEBUG_OPENED
760  if (gDebugFlag1) {
761  std::cout << " blocked (cannot lead)\n";
762  }
763 #endif
764  return true;
765  }
766  } else {
767  // even without considering safeHeadwayTime there is already a conflict
768 #ifdef MSLink_DEBUG_OPENED
769  if (gDebugFlag1) {
770  std::cout << " blocked (hard conflict)\n";
771  }
772 #endif
773  return true;
774  }
775  return false;
776 }
777 
778 
779 bool
780 MSLink::hasApproachingFoe(SUMOTime arrivalTime, SUMOTime leaveTime, double speed, double decel) const {
781  for (const MSLink* const link : myFoeLinks) {
782  if (link->blockedAtTime(arrivalTime, leaveTime, speed, speed, myLane == link->getLane(), 0, decel, 0)) {
783  return true;
784  }
785  }
786  for (const MSLane* const lane : myFoeLanes) {
787  if (lane->getVehicleNumberWithPartials() > 0) {
788  return true;
789  }
790  }
791  return false;
792 }
793 
794 
795 std::pair<const SUMOVehicle*, const MSLink*>
796 MSLink::getFirstApproachingFoe(const MSLink* wrapAround) const {
797  double closetDist = std::numeric_limits<double>::max();
798  const SUMOVehicle* closest = nullptr;
799  const MSLink* foeLink = nullptr;
800  for (MSLink* link : myFoeLinks) {
801  for (const auto& it : link->myApproachingVehicles) {
802  //std::cout << " link=" << getDescription() << " foeLink_in=" << link->getLaneBefore()->getID() << " wrapAround=" << wrapAround->getDescription() << "\n";
803  if (link->getLaneBefore() == wrapAround->getLaneBefore()) {
804  return std::make_pair(nullptr, wrapAround);
805  } else if (it.second.dist < closetDist) {
806  closetDist = it.second.dist;
807  if (it.second.willPass) {
808  closest = it.first;
809  foeLink = link;
810  }
811  }
812  }
813  }
814  return std::make_pair(closest, foeLink);
815 }
816 
817 
818 void
820  if (myState != state) {
821  myLastStateChange = t;
822  }
823  myState = state;
824  if (haveGreen()) {
826  }
827 }
828 
829 
830 bool
831 MSLink::isCont() const {
832  // when a traffic light is switched off minor roads have their cont status revoked
834 }
835 
836 
837 bool
839  if (myInternalLane == nullptr || myAmCont) {
840  return false;
841  } else {
843  if (!pred->getEdge().isInternal()) {
844  return false;
845  } else {
846  const MSLane* const pred2 = pred->getLogicalPredecessorLane();
847  assert(pred2 != nullptr);
848  const MSLink* const predLink = pred2->getLinkTo(pred);
849  assert(predLink != nullptr);
850  if (predLink->havePriority()) {
851  return true;
852  }
854  return predLink->getLastGreenState() == LINKSTATE_TL_GREEN_MAJOR;
855  } else {
856  return predLink->haveYellow();
857  }
858  }
859  }
860 }
861 
862 
863 bool
865  if (myInternalLane == nullptr || myAmCont || myHavePedestrianCrossingFoe) {
866  return false;
867  } else {
869  if (!pred->getEdge().isInternal()) {
870  return false;
871  } else {
872  const MSLane* const pred2 = pred->getLogicalPredecessorLane();
873  assert(pred2 != nullptr);
874  const MSLink* const predLink = pred2->getLinkTo(pred);
875  assert(predLink != nullptr);
876  return predLink->getState() == LINKSTATE_TL_GREEN_MAJOR || predLink->getState() == LINKSTATE_TL_RED;
877  }
878  }
879 }
880 
881 
882 void
883 MSLink::writeApproaching(OutputDevice& od, const std::string fromLaneID) const {
884  if (myApproachingVehicles.size() > 0) {
885  od.openTag("link");
886  od.writeAttr(SUMO_ATTR_FROM, fromLaneID);
887  const std::string via = getViaLane() == nullptr ? "" : getViaLane()->getID();
888  od.writeAttr(SUMO_ATTR_VIA, via);
889  od.writeAttr(SUMO_ATTR_TO, getLane() == nullptr ? "" : getLane()->getID());
890  std::vector<std::pair<SUMOTime, const SUMOVehicle*> > toSort; // stabilize output
891  for (auto it : myApproachingVehicles) {
892  toSort.push_back(std::make_pair(it.second.arrivalTime, it.first));
893  }
894  std::sort(toSort.begin(), toSort.end());
895  for (std::vector<std::pair<SUMOTime, const SUMOVehicle*> >::const_iterator it = toSort.begin(); it != toSort.end(); ++it) {
896  od.openTag("approaching");
897  const ApproachingVehicleInformation& avi = myApproachingVehicles.find(it->second)->second;
898  od.writeAttr(SUMO_ATTR_ID, it->second->getID());
899  od.writeAttr(SUMO_ATTR_IMPATIENCE, it->second->getImpatience());
900  od.writeAttr("arrivalTime", time2string(avi.arrivalTime));
901  od.writeAttr("arrivalTimeBraking", time2string(avi.arrivalTimeBraking));
902  od.writeAttr("leaveTime", time2string(avi.leavingTime));
903  od.writeAttr("arrivalSpeed", toString(avi.arrivalSpeed));
904  od.writeAttr("arrivalSpeedBraking", toString(avi.arrivalSpeedBraking));
905  od.writeAttr("leaveSpeed", toString(avi.leaveSpeed));
906  od.writeAttr("willPass", toString(avi.willPass));
907  od.closeTag();
908  }
909  od.closeTag();
910  }
911 }
912 
913 
914 double
916  double len = 0.;
917  MSLane* lane = myInternalLane;
918 
919  while (lane != nullptr && lane->isInternal()) {
920  len += lane->getLength();
921  lane = lane->getLinkCont()[0]->getViaLane();
922  }
923  return len;
924 }
925 
926 double
928  double len = 0.;
929  const MSLane* lane = myInternalLane;
930 
931  while (lane != nullptr && lane->isInternal()) {
932  len += lane->getLength();
933  if (lane->getIncomingLanes().size() == 1) {
934  lane = lane->getIncomingLanes()[0].lane;
935  } else {
936  break;
937  }
938  }
939  return len;
940 }
941 
942 
943 double
945  MSLane* via = myInternalLane;
946  double totalDist = 0.;
947  bool foundCrossing = false;
948  while (via != nullptr) {
949  MSLink* link = via->getLinkCont()[0];
950  double dist = link->getLengthBeforeCrossing(foeLane);
951  if (dist != INVALID_DOUBLE) {
952  // found conflicting lane
953  totalDist += dist;
954  foundCrossing = true;
955  break;
956  } else {
957  totalDist += via->getLength();
958  via = link->getViaLane();
959  }
960  }
961  if (foundCrossing) {
962  return totalDist;
963  } else {
964  return INVALID_DOUBLE;
965  }
966 }
967 
968 
969 double
971  int foe_ix;
972  for (foe_ix = 0; foe_ix != (int)myFoeLanes.size(); ++foe_ix) {
973  if (myFoeLanes[foe_ix] == foeLane) {
974  break;
975  }
976  }
977  if (foe_ix == (int)myFoeLanes.size()) {
978  // no conflict with the given lane, indicate by returning -1
979 #ifdef MSLink_DEBUG_CROSSING_POINTS
980  std::cout << "No crossing of lanes '" << foeLane->getID() << "' and '" << myInternalLaneBefore->getID() << "'" << std::endl;
981 #endif
982  return INVALID_DOUBLE;
983  } else {
984  // found conflicting lane index
985  double dist = myInternalLaneBefore->getLength() - myLengthsBehindCrossing[foe_ix].first;
986  if (dist == -10000.) {
987  // this is the value in myLengthsBehindCrossing, if the relation allows intersection but none is present for the actual geometry.
988  return INVALID_DOUBLE;
989  }
990 #ifdef MSLink_DEBUG_CROSSING_POINTS
991  std::cout << "Crossing of lanes '" << myInternalLaneBefore->getID() << "' and '" << foeLane->getID()
992  << "' at distance " << dist << " (approach along '"
993  << myInternalLaneBefore->getEntryLink()->getLaneBefore()->getID() << "')" << std::endl;
994 #endif
995  return dist;
996  }
997 }
998 
999 
1000 bool
1003  return myInternalLane != nullptr && myInternalLaneBefore == nullptr;
1004  } else {
1005  return false;
1006  }
1007 }
1008 
1009 bool
1011  // either a non-cont entry link or the link after a cont-link
1012  return !myAmCont && (isEntryLink() || (myInternalLaneBefore != nullptr && myInternalLane != nullptr));
1013 }
1014 
1015 bool
1018  return myInternalLaneBefore != nullptr && myInternalLane == nullptr;
1019  } else {
1020  return false;
1021  }
1022 }
1023 
1024 bool
1027  return (getInternalLaneBefore() != nullptr
1028  && myInternalLaneBefore->getIncomingLanes().size() == 1
1029  && myInternalLaneBefore->getIncomingLanes().front().viaLink->isInternalJunctionLink());
1030  } else {
1031  return false;
1032  }
1033 }
1034 
1035 
1036 const MSLink*
1038  MSLane* lane = myInternalLane;
1039  const MSLink* link = nullptr;
1040  while (lane != nullptr) {
1041  link = lane->getLinkCont()[0];
1042  lane = link->getViaLane();
1043  }
1044  return link;
1045 }
1046 
1047 
1048 const MSLink*
1050  const MSLink* link = this;
1051  while (link->myLaneBefore->isInternal()) {
1052  assert(myLaneBefore->getIncomingLanes().size() == 1);
1053  link = link->myLaneBefore->getIncomingLanes().front().viaLink;
1054  }
1055  return link;
1056 }
1057 
1058 
1059 bool
1061  return getInternalLaneBefore() != nullptr && myInternalLane != nullptr;
1062 }
1063 
1064 
1065 const MSLink::LinkLeaders
1066 MSLink::getLeaderInfo(const MSVehicle* ego, double dist, std::vector<const MSPerson*>* collectBlockers, bool isShadowLink) const {
1067  LinkLeaders result;
1068  // this link needs to start at an internal lane (either an exit link or between two internal lanes)
1069  // or it must be queried by the pedestrian model (ego == 0)
1070  if (ego != nullptr && (!fromInternalLane() || ego->getLaneChangeModel().isOpposite())) {
1071  // ignore link leaders
1072  return result;
1073  }
1074  //gDebugFlag1 = true;
1075  if (gDebugFlag1) {
1076  std::cout << SIMTIME << " getLeaderInfo link=" << getViaLaneOrLane()->getID() << " dist=" << dist << " isShadowLink=" << isShadowLink << "\n";
1077  }
1078  // this is an exit link
1079  for (int i = 0; i < (int)myFoeLanes.size(); ++i) {
1080  const MSLane* foeLane = myFoeLanes[i];
1081  const MSLink* foeExitLink = foeLane->getLinkCont()[0];
1082  // distance from the querying vehicle to the crossing point with foeLane
1083  double distToCrossing = dist - myLengthsBehindCrossing[i].first;
1084  const bool sameTarget = (myLane == foeExitLink->getLane()) && !isInternalJunctionLink();
1085  const bool sameSource = (myInternalLaneBefore != nullptr && myInternalLaneBefore->getLogicalPredecessorLane() == foeLane->getLogicalPredecessorLane());
1086  const double crossingWidth = (sameTarget || sameSource) ? 0 : foeLane->getWidth();
1087  const double foeCrossingWidth = (sameTarget || sameSource) ? 0 : myInternalLaneBefore->getWidth();
1088  // special treatment of contLane foe only applies if this lane is not a contLane or contLane follower itself
1089  const bool contLane = (foeExitLink->getViaLaneOrLane()->getEdge().isInternal() && !(
1091  if (gDebugFlag1) {
1092  std::cout << " distToCrossing=" << distToCrossing << " foeLane=" << foeLane->getID() << " cWidth=" << crossingWidth
1093  << " ijl=" << isInternalJunctionLink() << " sT=" << sameTarget << " sS=" << sameSource
1094  << " lbc=" << myLengthsBehindCrossing[i].first
1095  << " flbc=" << myLengthsBehindCrossing[i].second
1096  << " contLane=" << contLane
1097  << " state=" << toString(myState)
1098  << " foeState=" << toString(foeExitLink->getState())
1099  << "\n";
1100  }
1101  if (distToCrossing + crossingWidth < 0 && !sameTarget
1102  && (ego == nullptr || !MSGlobals::gComputeLC || distToCrossing + crossingWidth + ego->getVehicleType().getLength() < 0)) {
1103  continue; // vehicle is behind the crossing point, continue with next foe lane
1104  }
1105  bool ignoreGreenCont = false;
1106  bool foeIndirect = false;
1107  if (contLane) {
1108  const MSLink* entry = getLaneBefore()->getEntryLink();
1109  const MSLink* foeEntry = foeLane->getEntryLink();
1110  foeIndirect = foeEntry->myAmIndirect;
1111  if (entry != nullptr && entry->haveGreen()
1112  && foeEntry != nullptr && foeEntry->haveGreen()
1113  && entry->myLaneBefore != foeEntry->myLaneBefore) {
1114  // ignore vehicles before an internaljunction as long as they are still in green minor mode
1115  ignoreGreenCont = true;
1116  }
1117  }
1118  if (foeIndirect && distToCrossing >= NO_INTERSECTION) {
1119  if (gDebugFlag1) {
1120  std::cout << " ignore:noIntersection\n";
1121  }
1122  continue;
1123  }
1124  const double foeDistToCrossing = foeLane->getLength() - myLengthsBehindCrossing[i].second;
1125  // it is not sufficient to return the last vehicle on the foeLane because ego might be its leader
1126  // therefore we return all vehicles on the lane
1127  //
1128  // special care must be taken for continuation lanes. (next lane is also internal)
1129  // vehicles on cont. lanes or on internal lanes with the same target as this link can not be ignored
1130  // and should block (gap = -1) unless they are part of an indirect turn
1131  MSLane::AnyVehicleIterator end = foeLane->anyVehiclesEnd();
1132  for (MSLane::AnyVehicleIterator it_veh = foeLane->anyVehiclesBegin(); it_veh != end; ++it_veh) {
1133  MSVehicle* leader = (MSVehicle*)*it_veh;
1134  const double leaderBack = leader->getBackPositionOnLane(foeLane);
1135  const double leaderBackDist = foeDistToCrossing - leaderBack;
1136  const double l2 = ego != nullptr ? ego->getLength() + 2 : 0; // add some slack to account for further meeting-angle effects
1137  const double sagitta = ego != nullptr && myRadius != std::numeric_limits<double>::max() ? myRadius - sqrt(myRadius * myRadius - 0.25 * l2 * l2) : 0;
1138  const bool pastTheCrossingPoint = leaderBackDist + foeCrossingWidth + sagitta < 0;
1139  const bool foeIsBicycleTurn = (leader->getVehicleType().getVehicleClass() == SVC_BICYCLE
1140  && foeLane->getIncomingLanes().front().viaLink->getDirection() == LinkDirection::LEFT);
1141  const bool ignoreIndirectBicycleTurn = pastTheCrossingPoint && foeIsBicycleTurn;
1142  const bool cannotIgnore = ((contLane && !ignoreIndirectBicycleTurn) || sameTarget || sameSource) && ego != nullptr;
1143  const bool inTheWay = (((!pastTheCrossingPoint && distToCrossing > 0) || (sameTarget && distToCrossing > leaderBackDist - leader->getLength()))
1144  && leaderBackDist < leader->getVehicleType().getLength()
1145  && (!foeExitLink->isInternalJunctionLink() || foeIsBicycleTurn));
1146  const bool isOpposite = leader->getLaneChangeModel().isOpposite();
1147  const auto avi = foeExitLink->getApproaching(leader);
1148  // if leader is not found, assume that it performed a lane change in the last step
1149  const bool willPass = avi.willPass || (avi.arrivalTime == INVALID_TIME && sameTarget);
1150  if (gDebugFlag1) {
1151  std::cout << " candidate leader=" << leader->getID()
1152  << " cannotIgnore=" << cannotIgnore
1153  << " fdtc=" << foeDistToCrossing
1154  << " lb=" << leaderBack
1155  << " lbd=" << leaderBackDist
1156  << " fcwidth=" << foeCrossingWidth
1157  << " r=" << myRadius
1158  << " sagitta=" << sagitta
1159  << " foePastCP=" << pastTheCrossingPoint
1160  << " inTheWay=" << inTheWay
1161  << " willPass=" << willPass
1162  << " isFrontOnLane=" << leader->isFrontOnLane(foeLane)
1163  << " ignoreGreenCont=" << ignoreGreenCont
1164  << " foeIndirect=" << foeIndirect
1165  << " foeBikeTurn=" << foeIsBicycleTurn
1166  << " isOpposite=" << isOpposite << "\n";
1167  }
1168  if (leader == ego) {
1169  continue;
1170  }
1171  // ignore greenCont foe vehicles that are not in the way
1172  if (!inTheWay && ignoreGreenCont) {
1173  if (gDebugFlag1) {
1174  std::cout << " ignoreGreenCont\n";
1175  }
1176  continue;
1177  }
1178  // after entering the conflict area, ignore foe vehicles that are not in the way
1179  if (distToCrossing < -POSITION_EPS && !inTheWay
1180  && (ego == nullptr || !MSGlobals::gComputeLC || distToCrossing < -ego->getVehicleType().getLength())) {
1181  if (gDebugFlag1) {
1182  std::cout << " ego entered conflict area\n";
1183  }
1184  continue;
1185  }
1186  // ignore foe vehicles that will not pass
1187  if ((!cannotIgnore || leader->isStopped() || sameTarget)
1188  && !willPass
1189  && leader->isFrontOnLane(foeLane)
1190  && !isOpposite
1191  && !inTheWay
1192  // willPass is false if the vehicle is already on the stopping edge
1193  && !leader->willStop()) {
1194  if (gDebugFlag1) {
1195  std::cout << " foe will not pass\n";
1196  }
1197  continue;
1198  }
1199  // check whether foe is blocked and might need to change before leaving the junction
1200  const bool foeStrategicBlocked = (leader->getLaneChangeModel().isStrategicBlocked() &&
1201  leader->getCarFollowModel().brakeGap(leader->getSpeed()) <= foeLane->getLength() - leaderBack);
1202 
1203  if (MSGlobals::gSublane && ego != nullptr && (sameSource || sameTarget)
1204  && (!foeStrategicBlocked)) {
1205  // ignore vehicles if not in conflict sublane-wise
1206  const double posLat = ego->getLateralPositionOnLane();
1207  const double posLatLeader = leader->getLateralPositionOnLane() + leader->getLatOffset(foeLane);
1208  const double latGap = (fabs(posLat - posLatLeader)
1209  - 0.5 * (ego->getVehicleType().getWidth() + leader->getVehicleType().getWidth()));
1210  const double maneuverDist = leader->getLaneChangeModel().getManeuverDist() * (posLat < posLatLeader ? -1 : 1);
1211  if (gDebugFlag1) {
1212  std::cout << " sublaneFoe lane=" << myInternalLaneBefore->getID()
1213  << " sameSource=" << sameSource
1214  << " sameTarget=" << sameTarget
1215  << " foeLane=" << foeLane->getID()
1216  << " leader=" << leader->getID()
1217  << " egoLane=" << ego->getLane()->getID()
1218  << " leaderLane=" << leader->getLane()->getID()
1219  << " egoLat=" << posLat
1220  << " leaderLat=" << posLatLeader
1221  << " leaderLatOffset=" << leader->getLatOffset(foeLane)
1222  << " latGap=" << latGap
1223  << " maneuverDist=" << maneuverDist
1224  << " computeLC=" << MSGlobals::gComputeLC
1225  << " egoMaxSpeedLat=" << ego->getVehicleType().getMaxSpeedLat()
1226  << "\n";
1227  }
1228  if (latGap > 0 && (latGap > maneuverDist || !sameTarget)
1229  // do not perform sublane changes that interfere with the leader vehicle
1230  && (!MSGlobals::gComputeLC || latGap > ego->getVehicleType().getMaxSpeedLat())) {
1231  const MSLink* foeEntryLink = foeLane->getIncomingLanes().front().viaLink;
1232  if (sameSource) {
1233  // for lanes from the same edge, higer index implies a
1234  // connection further to the left
1235  const bool leaderFromRight = (myIndex > foeEntryLink->getIndex());
1236  if ((posLat > posLatLeader) == leaderFromRight) {
1237  // ignore speed since lanes diverge
1238  if (gDebugFlag1) {
1239  std::cout << " ignored (same source) leaderFromRight=" << leaderFromRight << "\n";
1240  }
1241  continue;
1242  }
1243  } else {
1244  // for lanes from different edges we cannot rely on the
1245  // index due to wrap-around issues
1246  if (myDirection != foeEntryLink->getDirection()) {
1247  bool leaderFromRight = foeEntryLink->getDirection() < myDirection;
1248  // leader vehicle should not move towards ego
1249  if (MSGlobals::gLefthand) {
1250  leaderFromRight = !leaderFromRight;
1251  }
1252  if ((posLat > posLatLeader) == leaderFromRight
1253  // leader should keep lateral position or move away from ego
1254  && (leader->getLaneChangeModel().getSpeedLat() == 0
1255  || leaderFromRight == (leader->getLaneChangeModel().getSpeedLat() < latGap))
1256  && (ego->getLaneChangeModel().getSpeedLat() == 0
1257  || leaderFromRight == (ego->getLaneChangeModel().getSpeedLat() > latGap))) {
1258  if (gDebugFlag1) {
1259  std::cout << " ignored (different source) leaderFromRight=" << leaderFromRight << "\n";
1260  }
1261  continue;
1262  }
1263  } else {
1264  // XXX figure out relative direction somehow
1265  }
1266  }
1267  }
1268  }
1270  // compute distance between vehicles on the the superimposition of both lanes
1271  // where the crossing point is the common point
1272  double gap;
1273  bool fromLeft = true;
1274  if (ego == nullptr) {
1275  // request from pedestrian model. return distance between leaderBack and crossing point
1276  //std::cout << " foeLane=" << foeLane->getID() << " leaderBack=" << leaderBack << " foeDistToCrossing=" << foeDistToCrossing << " foeLength=" << foeLane->getLength() << " foebehind=" << myLengthsBehindCrossing[i].second << " dist=" << dist << " behind=" << myLengthsBehindCrossing[i].first << "\n";
1277  gap = leaderBackDist;
1278  // distToCrossing should not take into account the with of the foe lane
1279  // (which was subtracted in setRequestInformation)
1280  // Instead, the width of the foe vehicle is used directly by the caller.
1281  distToCrossing += foeLane->getWidth() / 2;
1282  if (gap + foeCrossingWidth < 0) {
1283  // leader is completely past the crossing point
1284  // or there is no crossing point
1285  continue; // next vehicle
1286  }
1287  // we need to determine whether the vehicle passes the
1288  // crossing from the left or the right (heuristic)
1289  fromLeft = foeDistToCrossing > 0.5 * foeLane->getLength();
1290  } else if ((contLane && !sameSource && !ignoreIndirectBicycleTurn) || isOpposite) {
1291  gap = -std::numeric_limits<double>::max(); // always break for vehicles which are on a continuation lane or for opposite-direction vehicles
1292  } else {
1293  if (gDebugFlag1) {
1294  std::cout << " distToCrossing=" << distToCrossing << " leader back=" << leaderBack << " backDist=" << leaderBackDist
1295  << " blockedStrategic=" << leader->getLaneChangeModel().isStrategicBlocked()
1296  //<< " stateRight=" << toString((LaneChangeAction)leader->getLaneChangeModel().getSavedState(-1).second)
1297  << "\n";
1298  }
1299  if (pastTheCrossingPoint && !sameTarget) {
1300  // leader is completely past the crossing point
1301  // or there is no crossing point
1302  continue; // next vehicle
1303  }
1304  gap = distToCrossing - ego->getVehicleType().getMinGap() - leaderBackDist - foeCrossingWidth;
1305  // factor 2 is to give some slack for lane-changing
1306  if (gap < leader->getVehicleType().getLength() * 2 && leader->getLaneChangeModel().isStrategicBlocked()) {
1307  // do not encroach on leader when it tries to change lanes
1308  gap = -std::numeric_limits<double>::max();
1309  }
1310  }
1311  // if the foe is already moving off the intersection, we may
1312  // advance up to the crossing point unless we have the same target or same source
1313  // (for sameSource, the crossing point indicates the point of divergence)
1314  const bool stopAsap = leader->isFrontOnLane(foeLane) ? cannotIgnore : (sameTarget || sameSource);
1315  if (gDebugFlag1) {
1316  std::cout << " leader=" << leader->getID() << " contLane=" << contLane << " cannotIgnore=" << cannotIgnore << " stopAsap=" << stopAsap << "\n";
1317  }
1318  if (ignoreFoe(ego, leader)) {
1319  continue;
1320  }
1321  result.emplace_back(leader, gap, stopAsap ? -1 : distToCrossing, fromLeft, inTheWay);
1322  }
1323 
1324  }
1325  if (ego != nullptr && MSNet::getInstance()->hasPersons()) {
1326  // check for crossing pedestrians (keep driving if already on top of the crossing
1327  const double distToPeds = distToCrossing - MSPModel::SAFETY_GAP;
1328  const double vehWidth = ego->getVehicleType().getWidth() + MSPModel::SAFETY_GAP; // + configurable safety gap
1330  // @check lefthand?!
1331  const bool wayIn = myLengthsBehindCrossing[i].first < myLaneBefore->getLength() * 0.5;
1332  const double vehSideOffset = (foeDistToCrossing + myLaneBefore->getWidth() * 0.5 - vehWidth * 0.5
1333  + ego->getLateralPositionOnLane() * (wayIn ? -1 : 1));
1334  // can access the movement model here since we already checked for existing persons above
1335  if (distToPeds >= -MSPModel::SAFETY_GAP && MSNet::getInstance()->getPersonControl().getMovementModel()->blockedAtDist(foeLane, vehSideOffset, vehWidth,
1337  collectBlockers)) {
1338  //if (ignoreFoe(ego, leader)) {
1339  // continue;
1340  //}
1341  result.emplace_back(nullptr, -1, distToPeds);
1342  }
1343  }
1344  }
1345 
1346  //std::cout << SIMTIME << " ego=" << Named::getIDSecure(ego) << " link=" << getViaLaneOrLane()->getID() << " myWalkingAreaFoe=" << Named::getIDSecure(myWalkingAreaFoe) << "\n";
1347  if (ego != nullptr) {
1348  checkWalkingAreaFoe(ego, myWalkingAreaFoe, collectBlockers, result);
1349  checkWalkingAreaFoe(ego, myWalkingAreaFoeExit, collectBlockers, result);
1350  }
1351 
1352  if (MSGlobals::gLateralResolution > 0 && ego != nullptr && !isShadowLink) {
1353  // check for foes on the same lane
1354  for (std::vector<MSLane*>::const_iterator it = mySublaneFoeLanes.begin(); it != mySublaneFoeLanes.end(); ++it) {
1355  const MSLane* foeLane = *it;
1356  MSLane::AnyVehicleIterator end = foeLane->anyVehiclesEnd();
1357  for (MSLane::AnyVehicleIterator it_veh = foeLane->anyVehiclesBegin(); it_veh != end; ++it_veh) {
1358  MSVehicle* leader = (MSVehicle*)*it_veh;
1359  if (leader == ego) {
1360  continue;
1361  }
1362  const double maxLength = MAX2(myInternalLaneBefore->getLength(), foeLane->getLength());
1363  const double gap = dist - maxLength - ego->getVehicleType().getMinGap() + leader->getBackPositionOnLane(foeLane);
1364  if (gap < -(ego->getVehicleType().getMinGap() + leader->getLength())) {
1365  // ego is ahead of leader
1366  continue;
1367  }
1368 
1369  const double posLat = ego->getLateralPositionOnLane();
1370  const double posLatLeader = leader->getLateralPositionOnLane() + leader->getLatOffset(foeLane);
1371  if (gDebugFlag1) {
1372  std::cout << " sublaneFoe lane=" << myInternalLaneBefore->getID()
1373  << " foeLane=" << foeLane->getID()
1374  << " leader=" << leader->getID()
1375  << " egoLane=" << ego->getLane()->getID()
1376  << " leaderLane=" << leader->getLane()->getID()
1377  << " egoLat=" << posLat
1378  << " leaderLat=" << posLatLeader
1379  << " leaderLatOffset=" << leader->getLatOffset(foeLane)
1380  << " egoIndex=" << myInternalLaneBefore->getIndex()
1381  << " foeIndex=" << foeLane->getIndex()
1382  << " dist=" << dist
1383  << " leaderBack=" << leader->getBackPositionOnLane(foeLane)
1384  << "\n";
1385  }
1386  // there only is a conflict if the paths cross
1387  if ((posLat < posLatLeader && myInternalLaneBefore->getIndex() > foeLane->getIndex())
1388  || (posLat > posLatLeader && myInternalLaneBefore->getIndex() < foeLane->getIndex())) {
1389  if (gDebugFlag1) {
1390  std::cout << SIMTIME << " blocked by " << leader->getID() << " (sublane split) foeLane=" << foeLane->getID() << "\n";
1391  }
1392  if (ignoreFoe(ego, leader)) {
1393  continue;
1394  }
1395  result.emplace_back(leader, gap, -1);
1396  }
1397  }
1398  }
1399  }
1400  return result;
1401 }
1402 
1403 
1404 void
1405 MSLink::checkWalkingAreaFoe(const MSVehicle* ego, const MSLane* foeLane, std::vector<const MSPerson*>* collectBlockers, LinkLeaders& result) const {
1406  if (foeLane != nullptr && foeLane->getEdge().getPersons().size() > 0) {
1407  // pedestrians may be on an arbitrary path across this
1408  // walkingarea. make sure to keep enough distance.
1409  // This is a simple but conservative solution that could be improved
1410  // by ignoring pedestrians that are "obviously" not on a collision course
1411  double distToPeds = std::numeric_limits<double>::max();
1412  assert(myInternalLaneBefore != nullptr);
1414  if (ego->getLateralPositionOnLane() != 0) {
1415  egoPath.move2side((MSGlobals::gLefthand ? 1 : -1) * ego->getLateralPositionOnLane());
1416  }
1417  for (MSTransportable* t : foeLane->getEdge().getPersons()) {
1418  MSPerson* p = static_cast<MSPerson*>(t);
1419  const double dist = ego->getPosition().distanceTo2D(p->getPosition()) - p->getVehicleType().getLength() - MSPModel::SAFETY_GAP;
1420 #ifdef DEBUG_WALKINGAREA
1421  if (ego->isSelected()) {
1422  std::cout << SIMTIME << " veh=" << ego->getID() << " ped=" << p->getID()
1423  << " pos=" << ego->getPosition() << " pedPos=" << p->getPosition()
1424  << " rawDist=" << ego->getPosition().distanceTo2D(p->getPosition())
1425  << " dist=" << dist << "\n";
1426  }
1427 #endif
1428  if (dist < ego->getVehicleType().getWidth() / 2 || isInFront(ego, egoPath, p)) {
1429  distToPeds = MIN2(distToPeds, dist);
1430  if (collectBlockers != nullptr) {
1431  collectBlockers->push_back(p);
1432  }
1433  }
1434  }
1435  if (distToPeds != std::numeric_limits<double>::max()) {
1436  // leave extra space in front
1437  result.emplace_back(nullptr, -1, distToPeds - ego->getVehicleType().getMinGap());
1438  }
1439  }
1440 }
1441 
1442 bool
1443 MSLink::isInFront(const MSVehicle* ego, const PositionVector& egoPath, const MSPerson* p) const {
1444  const double pedAngle = ego->getPosition().angleTo2D(p->getPosition());
1445  const double angleDiff = fabs(GeomHelper::angleDiff(ego->getAngle(), pedAngle));
1446 #ifdef DEBUG_WALKINGAREA
1447  if (ego->isSelected()) {
1448  std::cout << " angleDiff=" << RAD2DEG(angleDiff) << "\n";
1449  }
1450 #endif
1451  if (angleDiff < DEG2RAD(75)) {
1452  return egoPath.distance2D(p->getPosition()) < ego->getVehicleType().getWidth() + MSPModel::SAFETY_GAP;
1453  }
1454  return false;
1455 }
1456 
1457 
1458 MSLink*
1459 MSLink::getParallelLink(int direction) const {
1460  if (direction == -1) {
1461  return myParallelRight;
1462  } else if (direction == 1) {
1463  return myParallelLeft;
1464  } else {
1465  assert(false || myLane->getOpposite() != nullptr);
1466  return nullptr;
1467  }
1468 }
1469 
1470 MSLink*
1472  if (myLane->getOpposite() != nullptr && myLaneBefore->getOpposite() != nullptr) {
1473  for (MSLink* cand : myLane->getOpposite()->getLinkCont()) {
1474  if (cand->getLane() == myLaneBefore->getOpposite()) {
1475  return cand;
1476  }
1477  }
1478  }
1479  return nullptr;
1480 }
1481 
1482 
1483 MSLink*
1485  const MSLane* const before = getLaneBefore()->getParallelLane(direction, false);
1486  const MSLane* const after = getLane()->getParallelLane(direction, false);
1487  if (before != nullptr && after != nullptr) {
1488  for (MSLink* const link : before->getLinkCont()) {
1489  if (link->getLane() == after) {
1490  return link;
1491  }
1492  }
1493  }
1494  return nullptr;
1495 }
1496 
1497 
1498 double
1499 MSLink::getZipperSpeed(const MSVehicle* ego, const double dist, double vSafe,
1500  SUMOTime arrivalTime,
1501  BlockingFoes* collectFoes) const {
1502  if (myFoeLinks.size() == 0) {
1503  // link should have LINKSTATE_MAJOR in this case
1504  assert(false);
1505  return vSafe;
1506  } else if (myFoeLinks.size() > 1) {
1507  throw ProcessError("Zipper junctions with more than two conflicting lanes are not supported (at junction '"
1508  + myJunction->getID() + "')");
1509  }
1510  const double brakeGap = ego->getCarFollowModel().brakeGap(ego->getSpeed(), ego->getCarFollowModel().getMaxDecel(), 0);
1511  if (dist > MAX2(myFoeVisibilityDistance, brakeGap)) {
1512 #ifdef DEBUG_ZIPPER
1514  if (DEBUG_COND_ZIPPER) DEBUGOUT(SIMTIME << " getZipperSpeed ego=" << ego->getID()
1515  << " dist=" << dist << " ignoring foes (arrival in " << STEPS2TIME(arrivalTime - now) << ")\n")
1516 #endif
1517  return vSafe;
1518  }
1519 #ifdef DEBUG_ZIPPER
1520  if (DEBUG_COND_ZIPPER) DEBUGOUT(SIMTIME << " getZipperSpeed ego=" << ego->getID()
1521  << " egoAT=" << arrivalTime
1522  << " dist=" << dist
1523  << " brakeGap=" << brakeGap
1524  << " vSafe=" << vSafe
1525  << " numFoes=" << collectFoes->size()
1526  << "\n")
1527 #endif
1528  MSLink* foeLink = myFoeLinks[0];
1529  for (const auto& item : *collectFoes) {
1530  const MSVehicle* foe = dynamic_cast<const MSVehicle*>(item);
1531  assert(foe != 0);
1532  const ApproachingVehicleInformation& avi = foeLink->getApproaching(foe);
1533  const double foeDist = (foe->isActive() ? avi.dist : MAX2(0.0, avi.dist -
1534  STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - foe->getLastActionTime()) * avi.speed));
1535 
1536  if ( // ignore vehicles that arrive after us (unless they are ahead and we could easily brake for them)
1537  ((avi.arrivalTime > arrivalTime) && !couldBrakeForLeader(dist, foeDist, ego, foe)) ||
1538  // also ignore vehicles that are behind us and are able to brake for us
1539  couldBrakeForLeader(foeDist, dist, foe, ego) ||
1540  // resolve ties by lane index
1541  (avi.arrivalTime == arrivalTime && foeDist == dist && ego->getLane()->getIndex() < foe->getLane()->getIndex())) {
1542 #ifdef DEBUG_ZIPPER
1543  if (DEBUG_COND_ZIPPER) std::cout
1544  << " ignoring foe=" << foe->getID()
1545  << " foeAT=" << avi.arrivalTime
1546  << " foeDist=" << avi.dist
1547  << " foeDist2=" << foeDist
1548  << " foeSpeed=" << avi.speed
1549  << " egoSpeed=" << ego->getSpeed()
1550  << " deltaDist=" << foeDist - dist
1551  << " delteSpeed=" << avi.speed - foe->getCarFollowModel().getMaxDecel() - ego->getSpeed()
1552  << " egoCouldBrake=" << couldBrakeForLeader(dist, foeDist, ego, foe)
1553  << " foeCouldBrake=" << couldBrakeForLeader(foeDist, dist, foe, ego)
1554  << "\n";
1555 #endif
1556  continue;
1557  }
1558  // the idea behind speed adaption is three-fold:
1559  // 1) ego needs to be in a car-following relationship with foe eventually
1560  // thus, the ego speed should be equal to the follow speed once the foe enters
1561  // the zipper junction
1562  // 2) ego vehicle needs to put a certain distance beteen himself and foe (safeGap)
1563  // achieving this distance can be spread over time but computing
1564  // safeGap is subject to estimation errors of future speeds
1565  // 3) deceleration can be spread out over the time until true
1566  // car-following happens, at the start of speed adaptions, smaller
1567  // decelerations should be sufficient
1568 
1569  // we cannot trust avi.arrivalSpeed if the foe has leader vehicles that are accelerating
1570  // lets try to extrapolate
1571  const double uMax = foe->getLane()->getVehicleMaxSpeed(foe);
1572  const double uAccel = foe->getCarFollowModel().estimateSpeedAfterDistance(foeDist, avi.speed, foe->getCarFollowModel().getMaxAccel());
1573  const double uEnd = MIN2(uMax, uAccel);
1574  const double uAvg = (avi.speed + uEnd) / 2;
1575  const double tf0 = foeDist / MAX2(NUMERICAL_EPS, uAvg);
1576  const double tf = MAX2(1.0, ceil((tf0) / TS) * TS);
1577 
1578  const double vMax = ego->getLane()->getVehicleMaxSpeed(ego);
1579  const double vAccel = ego->getCarFollowModel().estimateSpeedAfterDistance(dist, ego->getSpeed(), ego->getCarFollowModel().getMaxAccel());
1580  const double vDecel = ego->getCarFollowModel().estimateSpeedAfterDistance(dist, ego->getSpeed(), ego->getCarFollowModel().getMaxDecel());
1581  const double vEnd = MIN3(vMax, vAccel, MAX2(uEnd, vDecel));
1582  const double vAvg = (ego->getSpeed() + vEnd) / 2;
1583  const double te0 = dist / MAX2(NUMERICAL_EPS, vAvg);
1584  const double te = MAX2(1.0, ceil((te0) / TS) * TS);
1585 
1586  const double gap = dist - foe->getVehicleType().getLength() - ego->getVehicleType().getMinGap() - foeDist;
1587  const double safeGap = ego->getCarFollowModel().getSecureGap(ego, foe, vEnd, uEnd, foe->getCarFollowModel().getMaxDecel());
1588  // round t to next step size
1589  // increase gap to safeGap by the time foe reaches link
1590  // gap + u*t - (t * v + a * t^2 / 2) = safeGap
1591  const double deltaGap = gap + tf * uAvg - safeGap - vAvg * tf;
1592  const double a = 2 * deltaGap / (tf * tf);
1593  const double vSafeGap = ego->getSpeed() + ACCEL2SPEED(a);
1594  const double vFollow = ego->getCarFollowModel().followSpeed(
1595  ego, ego->getSpeed(), gap, avi.speed, foe->getCarFollowModel().getMaxDecel(), foe);
1596 
1597  // scale behavior based on ego time to link (te)
1598  const double w = MIN2(1.0, te / 10);
1599  const double maxDecel = w * ego->getCarFollowModel().getMaxDecel() + (1 - w) * ego->getCarFollowModel().getEmergencyDecel();
1600  const double vZipper = MAX3(vFollow, ego->getSpeed() - ACCEL2SPEED(maxDecel), w * vSafeGap + (1 - w) * vFollow);
1601 
1602  vSafe = MIN2(vSafe, vZipper);
1603 #ifdef DEBUG_ZIPPER
1604  if (DEBUG_COND_ZIPPER) std::cout << " adapting to foe=" << foe->getID()
1605  << " foeDist=" << foeDist
1606  << " foeSpeed=" << avi.speed
1607  << " foeAS=" << avi.arrivalSpeed
1608  << " egoSpeed=" << ego->getSpeed()
1609  << " uMax=" << uMax
1610  << " uAccel=" << uAccel
1611  << " uEnd=" << uEnd
1612  << " uAvg=" << uAvg
1613  << " gap=" << gap
1614  << " safeGap=" << safeGap
1615  << "\n "
1616  << " tf=" << tf
1617  << " te=" << te
1618  << " dg=" << deltaGap
1619  << " aSafeGap=" << a
1620  << " vMax=" << vMax
1621  << " vAccel=" << vAccel
1622  << " vDecel=" << vDecel
1623  << " vEnd=" << vEnd
1624  << " vSafeGap=" << vSafeGap
1625  << " vFollow=" << vFollow
1626  << " w=" << w
1627  << " maxDecel=" << maxDecel
1628  << " vZipper=" << vZipper
1629  << " vSafe=" << vSafe
1630  << "\n";
1631 #endif
1632  }
1633  return vSafe;
1634 }
1635 
1636 
1637 bool
1638 MSLink::couldBrakeForLeader(double followDist, double leaderDist, const MSVehicle* follow, const MSVehicle* leader) {
1639  return (// leader is ahead of follower
1640  followDist > leaderDist &&
1641  // and follower could brake for 1 s to stay behind leader
1642  followDist - leaderDist > follow->getSpeed() - follow->getCarFollowModel().getMaxDecel() - leader->getSpeed());
1643 }
1644 
1645 
1646 void
1650 }
1651 
1652 bool
1654  // check whether this link gets to keep its cont status switching the tls off
1655  // @note: this could also be pre-computed in netconvert
1656  // we check whether there is any major link from this edge
1657  for (const MSLane* cand : myLaneBefore->getEdge().getLanes()) {
1658  for (const MSLink* link : cand->getLinkCont()) {
1659  if (link->getOffState() == LINKSTATE_TL_OFF_NOSIGNAL) {
1660  return true;
1661  }
1662  }
1663  }
1664  return false;
1665 }
1666 
1667 bool
1668 MSLink::lateralOverlap(double posLat, double width, double posLat2, double width2) {
1669  return fabs(posLat2 - posLat) < (width + width2) / 2;
1670 }
1671 
1672 std::string
1674  return myLaneBefore->getID() + "->" + getViaLaneOrLane()->getID();
1675 }
1676 
1677 
1678 bool
1680  if (ego == nullptr || !ego->getParameter().wasSet(VEHPARS_JUNCTIONMODEL_PARAMS_SET)) {
1681  return false;
1682  }
1683  const SUMOVehicleParameter& param = ego->getParameter();
1684  for (const std::string& typeID : StringTokenizer(param.getParameter(toString(SUMO_ATTR_JM_IGNORE_TYPES), "")).getVector()) {
1685  if (typeID == foe->getVehicleType().getID()) {
1686  return true;
1687  }
1688  }
1689  for (const std::string& id : StringTokenizer(param.getParameter(toString(SUMO_ATTR_JM_IGNORE_IDS), "")).getVector()) {
1690  if (id == foe->getID()) {
1691  return true;
1692  }
1693  }
1694  return false;
1695 }
1696 
1697 /****************************************************************************/
#define DEG2RAD(x)
Definition: GeomHelper.h:35
#define RAD2DEG(x)
Definition: GeomHelper.h:36
std::string time2string(SUMOTime t)
convert SUMOTime to string
Definition: SUMOTime.cpp:68
#define STEPS2TIME(x)
Definition: SUMOTime.h:53
#define ACCEL2SPEED(x)
Definition: SUMOTime.h:49
#define SUMOTime_MIN
Definition: SUMOTime.h:34
#define TS
Definition: SUMOTime.h:40
#define SIMTIME
Definition: SUMOTime.h:60
#define TIME2STEPS(x)
Definition: SUMOTime.h:55
long long int SUMOTime
Definition: SUMOTime.h:32
@ 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 gDebugFlag1
global utility flags for debugging
Definition: StdDefs.cpp:32
const double INVALID_DOUBLE
Definition: StdDefs.h:63
T MIN3(T a, T b, T c)
Definition: StdDefs.h:87
#define DEBUGOUT(msg)
Definition: StdDefs.h:138
T MIN2(T a, T b)
Definition: StdDefs.h:74
T MAX2(T a, T b)
Definition: StdDefs.h:80
T MAX3(T a, T b, T c)
Definition: StdDefs.h:94
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:707
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:351
double getEmergencyDecel() const
Get the vehicle type's maximal phisically possible deceleration [m/s^2].
Definition: MSCFModel.h:247
virtual double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver's reaction time tau (i....
Definition: MSCFModel.h:334
virtual double followSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0) const =0
Computes the vehicle's follow speed (no dawdling)
double getMaxAccel() const
Get the vehicle type's maximum acceleration [m/s^2].
Definition: MSCFModel.h:231
double getMaxDecel() const
Get the vehicle type's maximal comfortable deceleration [m/s^2].
Definition: MSCFModel.h:239
A road/street connecting two junctions.
Definition: MSEdge.h:77
const std::set< MSTransportable * > & getPersons() const
Returns this edge's persons set.
Definition: MSEdge.h:198
const std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
Definition: MSEdge.h:168
const MSJunction * getFromJunction() const
Definition: MSEdge.h:397
bool isInternal() const
return whether this edge is an internal edge
Definition: MSEdge.h:262
static bool gUseMesoSim
Definition: MSGlobals.h:94
static double gLateralResolution
Definition: MSGlobals.h:88
static bool gComputeLC
whether the simulationLoop is in the lane changing phase
Definition: MSGlobals.h:127
static bool gLefthand
Whether lefthand-drive is being simulated.
Definition: MSGlobals.h:157
static SUMOTime gIgnoreJunctionBlocker
Definition: MSGlobals.h:76
static bool gSublane
whether sublane simulation is enabled (sublane model or continuous lanechanging)
Definition: MSGlobals.h:148
static bool gUsingInternalLanes
Information whether the simulation regards internal lanes.
Definition: MSGlobals.h:72
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:102
Representation of a lane in the micro simulation.
Definition: MSLane.h:82
const std::vector< MSLink * > & getLinkCont() const
returns the container with all links !!!
Definition: MSLane.h:641
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:2300
AnyVehicleIterator anyVehiclesEnd() const
end iterator for iterating over all vehicles touching this lane in downstream direction
Definition: MSLane.h:440
const MSLink * getEntryLink() const
Returns the entry link if this is an internal lane, else nullptr.
Definition: MSLane.cpp:2237
const MSLink * getLinkTo(const MSLane *const) const
returns the link to the given lane or nullptr, if it is not connected
Definition: MSLane.cpp:2214
double getLength() const
Returns the lane's length.
Definition: MSLane.h:541
double getVehicleMaxSpeed(const SUMOTrafficObject *const veh) const
Returns the lane's maximum speed, given a vehicle's speed limit adaptation.
Definition: MSLane.h:519
int getIndex() const
Returns the lane's index.
Definition: MSLane.h:563
const std::vector< IncomingLaneInfo > & getIncomingLanes() const
Definition: MSLane.h:837
MSLane * getLogicalPredecessorLane() const
get the most likely precedecessor lane (sorted using by_connections_to_sorter). The result is cached ...
Definition: MSLane.cpp:2637
bool isInternal() const
Definition: MSLane.cpp:2122
MSEdge & getEdge() const
Returns the lane's edge.
Definition: MSLane.h:674
const PositionVector & getShape() const
Returns this lane's shape.
Definition: MSLane.h:478
double interpolateGeometryPosToLanePos(double geometryPos) const
Definition: MSLane.h:511
AnyVehicleIterator anyVehiclesBegin() const
begin iterator for iterating over all vehicles touching this lane in downstream direction
Definition: MSLane.h:434
MSLane * getOpposite() const
return the neighboring opposite direction lane for lane changing or nullptr
Definition: MSLane.cpp:3802
const MSLane * getNormalPredecessorLane() const
get normal lane leading to this internal lane, for normal lanes, the lane itself is returned
Definition: MSLane.cpp:2662
double getWidth() const
Returns the lane's width.
Definition: MSLane.h:556
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:174
SUMOTime getCurrentTimeStep() const
Returns the current simulation step.
Definition: MSNet.h:318
bool hasPersons() const
Returns whether persons are simulated.
Definition: MSNet.h:393
virtual MSTransportableControl & getPersonControl()
Returns the person control.
Definition: MSNet.cpp:1059
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:113
The parent class for traffic light logics.
MSPModel * getMovementModel()
Returns the default movement model for this kind of transportables.
const MSVehicleType & getVehicleType() const
Returns the object's "vehicle" type.
Position getPosition(const double) const
Return current position (x/y, cartesian)
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:75
bool willStop() const
Returns whether the vehicle will stop on the current edge.
Definition: MSVehicle.cpp:1472
SUMOTime getLastActionTime() const
Returns the time of the vehicle's last action point.
Definition: MSVehicle.h:512
SUMOTime getWaitingTime() const
Returns the SUMOTime waited (speed was lesser than 0.1m/s)
Definition: MSVehicle.h:631
bool isActive() const
Returns whether the current simulation step is an action point for the vehicle.
Definition: MSVehicle.h:597
bool isFrontOnLane(const MSLane *lane) const
Returns the information whether the front of the vehicle is on the given lane.
Definition: MSVehicle.cpp:4465
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:5051
Position getPosition(const double offset=0) const
Return current position (x/y, cartesian)
Definition: MSVehicle.cpp:1142
double getBackPositionOnLane(const MSLane *lane) const
Get the vehicle's position relative to the given lane.
Definition: MSVehicle.h:396
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:5941
double getLateralPositionOnLane() const
Get the vehicle's lateral position on the lane.
Definition: MSVehicle.h:411
double getSpeed() const
Returns the vehicle's current speed.
Definition: MSVehicle.h:462
const MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:552
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
Definition: MSVehicle.h:917
double getAngle() const
Returns the vehicle's direction in radians.
Definition: MSVehicle.h:683
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.
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 std::string & getID() const
Returns the name of the vehicle type.
Definition: MSVehicleType.h:90
double getLength() const
Get vehicle's length [m].
const MSCFModel & getCarFollowModel() const
Returns the vehicle type's car following model definition (const version)
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 & openTag(const std::string &xmlElement)
Opens an XML tag.
OutputDevice & writeAttr(const SumoXMLAttr attr, const T &val)
writes a named attribute
Definition: OutputDevice.h:248
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.h:119
Representation of a vehicle, person, or container.
virtual double getSpeed() const =0
Returns the object's current speed.
virtual SumoRNG * getRNG() const =0
Returns the associated RNG for this object.
virtual const MSVehicleType & getVehicleType() const =0
Returns the object's "vehicle" type.
virtual const SUMOVehicleParameter & getParameter() const =0
Returns the vehicle's parameter (including departure definition)
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.