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-2018 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials
5 // are made available under the terms of the Eclipse Public License v2.0
6 // which accompanies this distribution, and is available at
7 // http://www.eclipse.org/legal/epl-v20.html
8 // SPDX-License-Identifier: EPL-2.0
9 /****************************************************************************/
18 // A connnection between lanes
19 /****************************************************************************/
20 
21 // ===========================================================================
22 // included modules
23 // ===========================================================================
24 #include <config.h>
25 
26 #include <iostream>
27 #include <algorithm>
28 #include <limits>
30 #include "MSNet.h"
31 #include "MSJunction.h"
32 #include "MSLink.h"
33 #include "MSLane.h"
35 #include "MSEdge.h"
36 #include "MSGlobals.h"
37 #include "MSVehicle.h"
40 
41 //#define MSLink_DEBUG_CROSSING_POINTS
42 //#define MSLink_DEBUG_OPENED
43 //#define DEBUG_APPROACHING
44 //#define DEBUG_ZIPPER
45 //#define DEBUG_COND (myLane->getID()=="43[0]_0" && myLaneBefore->getID()==":33_0_0")
46 //#define DEBUG_COND (myLane->getID()=="end_0")
47 //#define DEBUG_COND (true)
48 
49 // ===========================================================================
50 // static member variables
51 // ===========================================================================
53 // additional caution is needed when approaching a zipper link
55 
56 const double MSLink::ZIPPER_ADAPT_DIST(100);
57 
58 // time to link in seconds below which adaptation should take place
59 #define ZIPPER_ADAPT_TIME 10
60 // the default safety gap when passing before oncoming pedestrians
61 #define JM_CROSSING_GAP_DEFAULT 10
62 
63 // minimim width between sibling lanes to qualify as non-overlapping
64 #define DIVERGENCE_MIN_WIDTH 2.5
65 
66 // ===========================================================================
67 // member method definitions
68 // ===========================================================================
69 MSLink::MSLink(MSLane* predLane, MSLane* succLane, MSLane* via, LinkDirection dir, LinkState state, double length, double foeVisibilityDistance, bool keepClear, MSTrafficLightLogic* logic, int tlIndex) :
70  myLane(succLane),
71  myLaneBefore(predLane),
72  myIndex(-1),
73  myTLIndex(tlIndex),
74  myLogic(logic),
75  myState(state),
76  myOffState(state),
77  myLastStateChange(SUMOTime_MIN / 2), // a large negative value, but avoid overflows when subtracting
78  myDirection(dir),
79  myLength(length),
80  myFoeVisibilityDistance(foeVisibilityDistance),
81  myHasFoes(false),
82  myAmCont(false),
83  myAmContOff(false),
84  myKeepClear(keepClear),
85  myInternalLane(via),
86  myInternalLaneBefore(nullptr),
87  myMesoTLSPenalty(0),
88  myGreenFraction(1),
89  myLateralShift(0),
90  myWalkingAreaFoe(nullptr),
91  myHavePedestrianCrossingFoe(false),
92  myParallelRight(nullptr),
93  myParallelLeft(nullptr),
94  myJunction(nullptr) {
95 
97  // detect lateral shift from lane geometries
98  //std::cout << "DEBUG link=" << myLaneBefore->getID() << "->" << getViaLaneOrLane()->getID() << " hasInternal=" << MSNet::getInstance()->hasInternalLinks() << " shapeBefore=" << myLaneBefore->getShape().back() << " shapeFront=" << getViaLaneOrLane()->getShape().front() << "\n";
99  if ((myInternalLane != nullptr || predLane->isInternal())
100  && myLaneBefore->getShape().back() != getViaLaneOrLane()->getShape().front()) {
102  const PositionVector& to = getViaLaneOrLane()->getShape();
103  const double dist = from.back().distanceTo2D(to.front());
104  // figure out direction of shift
105  try {
106  from.move2side(dist);
107  } catch (InvalidArgument&) {
108  }
109  myLateralShift = (from.back().distanceTo2D(to.front()) < dist) ? dist : -dist;
110  //std::cout << " lateral shift link=" << myLaneBefore->getID() << "->" << getViaLaneOrLane()->getID() << " dist=" << dist << " shift=" << myLateralShift << "\n";
111  }
112  }
113 }
114 
115 
117 
118 
119 void
121  const std::vector<MSLink*>& foeLinks,
122  const std::vector<MSLane*>& foeLanes,
123  MSLane* internalLaneBefore) {
124 //#ifdef MSLink_DEBUG_CROSSING_POINTS
125 // std::cout << " setRequestInformation() for junction " << getViaLaneOrLane()->getEdge().getFromJunction()->getID()
126 // << "\nInternalLanes = " << toString(getViaLaneOrLane()->getEdge().getFromJunction()->getInternalLanes())
127 // << std::endl;
128 //#endif
129  myIndex = index;
130  myHasFoes = hasFoes;
131  myAmCont = isCont;
132  myFoeLinks = foeLinks;
133  for (std::vector<MSLane*>::const_iterator it_lane = foeLanes.begin(); it_lane != foeLanes.end(); ++it_lane) {
134  // cannot assign vector due to const-ness
135  myFoeLanes.push_back(*it_lane);
136  }
137  myJunction = const_cast<MSJunction*>(myLane->getEdge().getFromJunction()); // junctionGraph is initialized after the whole network is loaded
138  myAmContOff = isCont && myLogic != nullptr && internalLaneBefore == nullptr && checkContOff();
139  myInternalLaneBefore = internalLaneBefore;
140  MSLane* lane = nullptr;
141  if (internalLaneBefore != nullptr) {
142  // this is an exit link. compute crossing points with all foeLanes
143  lane = internalLaneBefore;
144  //} else if (myLane->getEdge().isCrossing()) {
145  // // this is the link to a pedestrian crossing. compute crossing points with all foeLanes
146  // // @note not currently used by pedestrians
147  // lane = myLane;
148  }
149 #ifdef MSLink_DEBUG_CROSSING_POINTS
150  std::cout << " link " << myIndex << " to " << getViaLaneOrLane()->getID() << " internalLaneBefore=" << (lane == 0 ? "NULL" : lane->getID()) << " has foes: " << toString(foeLanes) << "\n";
151 #endif
152  if (lane != nullptr) {
153  const bool beforeInternalJunction = lane->getLinkCont()[0]->getViaLaneOrLane()->getEdge().isInternal();
154  if (lane->getIncomingLanes().size() != 1) {
155  throw ProcessError("Internal lane '" + lane->getID() + "' has " + toString(lane->getIncomingLanes().size()) + " predecessors");
156  }
157  // compute crossing points
158  for (std::vector<const MSLane*>::const_iterator it_lane = myFoeLanes.begin(); it_lane != myFoeLanes.end(); ++it_lane) {
159  myHavePedestrianCrossingFoe = myHavePedestrianCrossingFoe || (*it_lane)->getEdge().isCrossing();
160  const bool sameTarget = myLane == (*it_lane)->getLinkCont()[0]->getLane();
161  if (sameTarget && !beforeInternalJunction && !contIntersect(lane, *it_lane)) {
162  //if (myLane == (*it_lane)->getLinkCont()[0]->getLane()) {
163  // this foeLane has the same target and merges at the end (lane exits the junction)
164  myLengthsBehindCrossing.push_back(std::make_pair(0, 0)); // dummy value, never used
165 #ifdef MSLink_DEBUG_CROSSING_POINTS
166  std::cout
167  << " " << lane->getID()
168  << " merges with " << (*it_lane)->getID()
169  << " nextLane " << lane->getLinkCont()[0]->getViaLaneOrLane()->getID()
170  << " dist1=" << myLengthsBehindCrossing.back().first
171  << " dist2=" << myLengthsBehindCrossing.back().second
172  << "\n";
173 #endif
174  } else {
175  std::vector<double> intersections1 = lane->getShape().intersectsAtLengths2D((*it_lane)->getShape());
176 #ifdef MSLink_DEBUG_CROSSING_POINTS
177 // std::cout << " intersections1=" << toString(intersections1) << "\n";
178 #endif
179  bool haveIntersection = true;
180  if (intersections1.size() == 0) {
181  intersections1.push_back(-10000.0); // disregard this foe (using maxdouble leads to nasty problems down the line)
182  haveIntersection = false;
183  } else if (intersections1.size() > 1) {
184  std::sort(intersections1.begin(), intersections1.end());
185  }
186  std::vector<double> intersections2 = (*it_lane)->getShape().intersectsAtLengths2D(lane->getShape());
187 #ifdef MSLink_DEBUG_CROSSING_POINTS
188  //std::cout << " intersections2=" << toString(intersections2) << "\n";
189 #endif
190  if (intersections2.size() == 0) {
191  intersections2.push_back(0);
192  } else if (intersections2.size() > 1) {
193  std::sort(intersections2.begin(), intersections2.end());
194  }
195  if (haveIntersection) {
196  // lane width affects the crossing point
197  intersections1.back() -= (*it_lane)->getWidth() / 2;
198  intersections2.back() -= lane->getWidth() / 2;
199  // also length/geometry factor. (XXX: Why subtract width/2 *before* converting geometric position to lane pos? refs #3031)
200  intersections1.back() = lane->interpolateGeometryPosToLanePos(intersections1.back());
201  intersections2.back() = (*it_lane)->interpolateGeometryPosToLanePos(intersections2.back());
202 
203  if (internalLaneBefore->getLogicalPredecessorLane()->getEdge().isInternal() && !(*it_lane)->getEdge().isCrossing()) {
204  // wait at the internal junction
205  // (except for foes that are crossings since there is no internal junction)
206  intersections1.back() = 0;
207  }
208  }
209 
210  myLengthsBehindCrossing.push_back(std::make_pair(
211  lane->getLength() - intersections1.back(),
212  (*it_lane)->getLength() - intersections2.back()));
213 
214 #ifdef MSLink_DEBUG_CROSSING_POINTS
215  std::cout
216  << " intersection of " << lane->getID()
217  << " totalLength=" << lane->getLength()
218  << " with " << (*it_lane)->getID()
219  << " totalLength=" << (*it_lane)->getLength()
220  << " dist1=" << myLengthsBehindCrossing.back().first
221  << " dist2=" << myLengthsBehindCrossing.back().second
222  << "\n";
223 #endif
224  }
225  }
226  // check for overlap with internal lanes from the same source lane
227  const MSLane* pred = lane->getLogicalPredecessorLane();
228  // to avoid overlap with vehicles that came from pred (especially when pred has endOffset > 0)
229  // we add all other internal lanes from pred as foeLanes
230  for (const MSLink* const it : pred->getLinkCont()) {
231  const MSLane* sibling = it->getViaLane();
232  if (sibling != lane && sibling != nullptr) {
233  const double minDist = MIN2(DIVERGENCE_MIN_WIDTH, 0.5 * (lane->getWidth() + sibling->getWidth()));
234  const PositionVector& l = lane->getShape();
235  const PositionVector& s = sibling->getShape();
236  if (l.front().distanceTo2D(s.front()) >= minDist) {
237  // account for lateral shift by the entry links
238  continue;
239  }
240  double lbcSibling = 0;
241  double lbcLane = 0;
242  if (l.back().distanceTo2D(s.back()) > minDist) {
243  // compute the final divergence point
244  // this position serves two purposes:
245  // 1) once the foe vehicle back (on sibling) has passed this point, we can safely ignore it
246  // 2) both vehicles are put into a cf-relationship while before the point.
247  // Since the actual crossing point is at the start of the junction,
248  // we want to make sure that both vehicles have the same distance to the crossing point and thus follow each other naturally
249  std::vector<double> distances = l.distances(s);
250 #ifdef MSLink_DEBUG_CROSSING_POINTS
251  std::cout << " distances=" << toString(distances) << "\n";
252 #endif
253  assert(distances.size() == l.size() + s.size());
254  if (distances.back() > minDist && distances[l.size() - 1] > minDist) {
255  // do a pairwise check between lane and sibling to make because we do not know which of them bends more
256  for (int j = (int)s.size() - 2; j >= 0; j--) {
257  const int i = j + (int)l.size();
258  const double segLength = s[j].distanceTo2D(s[j + 1]);
259  if (distances[i] > minDist) {
260  lbcSibling += segLength;
261  } else {
262  // assume no sharp bends and just interpolate the last segment
263  lbcSibling += segLength - (minDist - distances[i]) * segLength / (distances[i + 1] - distances[i]);
264  break;
265  }
266  }
267  for (int i = (int)l.size() - 2; i >= 0; i--) {
268  const double segLength = l[i].distanceTo2D(l[i + 1]);
269  if (distances[i] > minDist) {
270  lbcLane += segLength;
271  } else {
272  // assume no sharp bends and just interpolate the last segment
273  lbcLane += segLength - (minDist - distances[i]) * segLength / (distances[i + 1] - distances[i]);
274  break;
275  }
276  }
277  }
278  assert(lbcSibling >= -NUMERICAL_EPS);
279  assert(lbcLane >= -NUMERICAL_EPS);
280  }
281  const double distToDivergence1 = sibling->getLength() - lbcSibling;
282  const double distToDivergence2 = lane->getLength() - lbcLane;
283  const double distToDivergence = MIN3(
284  MAX2(distToDivergence1, distToDivergence2),
285  sibling->getLength(), lane->getLength());
286  lbcLane = MAX2(0.0, lane->getLength() - distToDivergence);
287  lbcLane = lane->interpolateGeometryPosToLanePos(lbcLane);
288  lbcSibling = MAX2(0.0, sibling->getLength() - distToDivergence);
289  lbcSibling = lane->interpolateGeometryPosToLanePos(lbcSibling);
290  myLengthsBehindCrossing.push_back(std::make_pair(lbcLane, lbcSibling));
291  myFoeLanes.push_back(sibling);
292 #ifdef MSLink_DEBUG_CROSSING_POINTS
293  std::cout << " distToDivergence=" << distToDivergence
294  << " distTD1=" << distToDivergence1
295  << " distTD2=" << distToDivergence2
296  << " length=" << lane->getLength()
297  << " sibLength=" << sibling->getLength()
298  << "\n";
299  std::cout << " adding same-origin foe" << sibling->getID()
300  << " dist1=" << myLengthsBehindCrossing.back().first
301  << " dist2=" << myLengthsBehindCrossing.back().second
302  << "\n";
303 #endif
304  }
305  }
306  }
308  // check for links with the same origin lane and the same destination edge
309  const MSEdge* myTarget = &myLane->getEdge();
310  // save foes for entry links
311  for (MSLink* const it : myLaneBefore->getLinkCont()) {
312  const MSEdge* target = &(it->getLane()->getEdge());
313  if (it == this) {
314  continue;
315  }
316  if (target == myTarget) {
317  mySublaneFoeLinks.push_back(it);
318  } else if (myDirection != LINKDIR_STRAIGHT && it->getDirection() == LINKDIR_STRAIGHT) {
319  // potential turn conflicht
320  mySublaneFoeLinks2.push_back(it);
321  }
322  }
323  // save foes for exit links
324  if (fromInternalLane()) {
325  //std::cout << " setRequestInformation link=" << getViaLaneOrLane()->getID() << " before=" << myLaneBefore->getID() << " before2=" << myLaneBefore->getIncomingLanes().front().lane->getID() << "\n";
326  const MSLinkCont& predLinks2 = myLaneBefore->getIncomingLanes().front().lane->getLinkCont();
327  for (MSLinkCont::const_iterator it = predLinks2.begin(); it != predLinks2.end(); ++it) {
328  const MSEdge* target = &((*it)->getLane()->getEdge());
329  if ((*it)->getViaLane() != myInternalLaneBefore && target == myTarget) {
330  //std::cout << " add sublaneFoe=" << (*it)->getViaLane()->getID() << "\n";
331  mySublaneFoeLanes.push_back((*it)->getViaLane());
332  }
333  }
334  }
335  }
336 }
337 
338 
339 bool
340 MSLink::contIntersect(const MSLane* lane, const MSLane* foe) {
341  if (foe->getLinkCont()[0]->getViaLane() != nullptr) {
342  std::vector<double> intersections = lane->getShape().intersectsAtLengths2D(foe->getShape());
343  return intersections.size() > 0;
344  }
345  return false;
346 
347 
348 }
349 
350 void
351 MSLink::setApproaching(const SUMOVehicle* approaching, const SUMOTime arrivalTime, const double arrivalSpeed, const double leaveSpeed,
352  const bool setRequest, const SUMOTime arrivalTimeBraking, const double arrivalSpeedBraking, const SUMOTime waitingTime, double dist) {
353  const SUMOTime leaveTime = getLeaveTime(arrivalTime, arrivalSpeed, leaveSpeed, approaching->getVehicleType().getLength());
354 #ifdef DEBUG_APPROACHING
355  if (DEBUG_COND) {
356  std::cout << SIMTIME << " Link ''" << (myLaneBefore == 0 ? "NULL" : myLaneBefore->getID()) << "'->'" << (myLane == 0 ? "NULL" : myLane->getID()) << "' Adding approaching vehicle '" << approaching->getID() << "'\nCurrently registered vehicles:" << std::endl;
357  for (auto i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
358  std::cout << "'" << i->first->getID() << "'" << std::endl;
359  }
360  }
361 #endif
362  myApproachingVehicles.insert(std::make_pair(approaching,
363  ApproachingVehicleInformation(arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, setRequest,
364  arrivalTimeBraking, arrivalSpeedBraking, waitingTime, dist)));
365 }
366 
367 
368 void
370 
371 #ifdef DEBUG_APPROACHING
372  if (DEBUG_COND) {
373  std::cout << SIMTIME << " Link ''" << (myLaneBefore == 0 ? "NULL" : myLaneBefore->getID()) << "'->'" << (myLane == 0 ? "NULL" : myLane->getID()) << "' Adding approaching vehicle '" << approaching->getID() << "'\nCurrently registered vehicles:" << std::endl;
374  for (auto i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
375  std::cout << "'" << i->first->getID() << "'" << std::endl;
376  }
377  }
378 #endif
379  myApproachingVehicles.insert(std::make_pair(approaching, ai));
380 }
381 
382 
383 void
385  myBlockedFoeLinks.insert(link);
386 }
387 
388 
389 
390 bool
392  for (std::set<MSLink*>::const_iterator i = myBlockedFoeLinks.begin(); i != myBlockedFoeLinks.end(); ++i) {
393  if ((*i)->isBlockingAnyone()) {
394  return true;
395  }
396  }
397  return false;
398 }
399 
400 
401 void
403 
404 #ifdef DEBUG_APPROACHING
405  if (DEBUG_COND) {
406  std::cout << SIMTIME << " Link ''" << (myLaneBefore == 0 ? "NULL" : myLaneBefore->getID()) << "'->'" << (myLane == 0 ? "NULL" : myLane->getID()) << std::endl;
407  std::cout << "' Removing approaching vehicle '" << veh->getID() << "'\nCurrently registered vehicles:" << std::endl;
408  for (auto i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
409  std::cout << "'" << i->first->getID() << "'" << std::endl;
410  }
411  }
412 #endif
413  myApproachingVehicles.erase(veh);
414 }
415 
416 
419  auto i = myApproachingVehicles.find(veh);
420  if (i != myApproachingVehicles.end()) {
421  return i->second;
422  } else {
423  return ApproachingVehicleInformation(-1000, -1000, 0, 0, false, -1000, 0, 0, 0);
424  }
425 }
426 
427 
428 SUMOTime
429 MSLink::getLeaveTime(const SUMOTime arrivalTime, const double arrivalSpeed,
430  const double leaveSpeed, const double vehicleLength) const {
431  return arrivalTime + TIME2STEPS((getLength() + vehicleLength) / MAX2(0.5 * (arrivalSpeed + leaveSpeed), NUMERICAL_EPS));
432 }
433 
434 
435 bool
436 MSLink::opened(SUMOTime arrivalTime, double arrivalSpeed, double leaveSpeed, double vehicleLength,
437  double impatience, double decel, SUMOTime waitingTime, double posLat,
438  std::vector<const SUMOVehicle*>* collectFoes, bool ignoreRed, const SUMOVehicle* ego) const {
439  if (haveRed() && !ignoreRed) {
440  return false;
441  }
443  return true;
444  }
445  const SUMOTime leaveTime = getLeaveTime(arrivalTime, arrivalSpeed, leaveSpeed, vehicleLength);
447  // check for foes on the same lane with the same target edge
448  for (const MSLink* foeLink : mySublaneFoeLinks) {
449  assert(myLane != foeLink->getLane());
450  for (auto& it : foeLink->myApproachingVehicles) {
451  const SUMOVehicle* foe = it.first;
452  if (
453  // there only is a conflict if the paths cross
454  ((posLat < foe->getLateralPositionOnLane() && myLane->getIndex() > foeLink->myLane->getIndex())
455  || (posLat > foe->getLateralPositionOnLane() && myLane->getIndex() < foeLink->myLane->getIndex()))
456  // the vehicle that arrives later must yield
457  && (arrivalTime > it.second.arrivalTime
458  // if both vehicles arrive at the same time, the one
459  // to the left must yield
460  || (arrivalTime == it.second.arrivalTime && posLat > foe->getLateralPositionOnLane()))) {
461  if (blockedByFoe(foe, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, false,
462  impatience, decel, waitingTime, ego)) {
463 #ifdef MSLink_DEBUG_OPENED
464  if (gDebugFlag1) {
465  std::cout << SIMTIME << " blocked by " << foe->getID() << " arrival=" << arrivalTime << " foeArrival=" << it.second.arrivalTime << "\n";
466  }
467 #endif
468  if (collectFoes == nullptr) {
469 #ifdef MSLink_DEBUG_OPENED
470  if (gDebugFlag1) {
471  std::cout << " link=" << getViaLaneOrLane()->getID() << " blocked by sublaneFoe=" << foe->getID() << " foeLink=" << foeLink->getViaLaneOrLane()->getID() << " posLat=" << posLat << "\n";
472  }
473 #endif
474  return false;
475  } else {
476  collectFoes->push_back(foe);
477  }
478  }
479  }
480  }
481  }
482  // check for foes on the same lane with a different target edge
483  // (straight movers take precedence if the paths cross)
484  for (const MSLink* foeLink : mySublaneFoeLinks2) {
485  assert(myDirection != LINKDIR_STRAIGHT);
486  for (auto& it : foeLink->myApproachingVehicles) {
487  const SUMOVehicle* foe = it.first;
488  // there only is a conflict if the paths cross
490  && (posLat > foe->getLateralPositionOnLane()))
492  && (posLat < foe->getLateralPositionOnLane()))) {
493  if (blockedByFoe(foe, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, false,
494  impatience, decel, waitingTime, ego)) {
495 #ifdef MSLink_DEBUG_OPENED
496  if (gDebugFlag1) {
497  std::cout << SIMTIME << " blocked by sublane foe " << foe->getID() << " arrival=" << arrivalTime << " foeArrival=" << it.second.arrivalTime << "\n";
498  }
499 #endif
500  if (collectFoes == nullptr) {
501  return false;
502  } else {
503  collectFoes->push_back(foe);
504  }
505  }
506  }
507  }
508  }
509  }
510  if (havePriority() && myState != LINKSTATE_ZIPPER) {
511  // priority usually means the link is open but there are exceptions:
512  // zipper still needs to collect foes
513  // sublane model could have detected a conflict
514  return collectFoes == nullptr || collectFoes->size() == 0;
515  }
516  if ((myState == LINKSTATE_STOP || myState == LINKSTATE_ALLWAY_STOP) && waitingTime == 0) {
517  return false;
518  }
519 
520 #ifdef MSLink_DEBUG_OPENED
521  if (gDebugFlag1) {
522  std::cout << SIMTIME << " opened link=" << getViaLaneOrLane()->getID() << " foeLinks=" << myFoeLinks.size() << "\n";
523  }
524 #endif
525 
526  for (std::vector<MSLink*>::const_iterator i = myFoeLinks.begin(); i != myFoeLinks.end(); ++i) {
528  if ((*i)->haveRed()) {
529  continue;
530  }
531  }
532 #ifdef MSLink_DEBUG_OPENED
533  if (gDebugFlag1) {
534  std::cout << " foeLink=" << (*i)->getViaLaneOrLane()->getID() << " numApproaching=" << (*i)->getApproaching().size() << "\n";
535  }
536 #endif
537  if ((*i)->blockedAtTime(arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, myLane == (*i)->getLane(),
538  impatience, decel, waitingTime, collectFoes, ego)) {
539  return false;
540  }
541  }
542  if (collectFoes != nullptr && collectFoes->size() > 0) {
543  return false;
544  }
545  return true;
546 }
547 
548 
549 bool
550 MSLink::blockedAtTime(SUMOTime arrivalTime, SUMOTime leaveTime, double arrivalSpeed, double leaveSpeed,
551  bool sameTargetLane, double impatience, double decel, SUMOTime waitingTime,
552  std::vector<const SUMOVehicle*>* collectFoes, const SUMOVehicle* ego) const {
553  for (auto it : myApproachingVehicles) {
554 #ifdef MSLink_DEBUG_OPENED
555  if (gDebugFlag1) {
556  if (ego != 0
557  && ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_SPEED, 0) >= it.first->getSpeed()
559  std::cout << " foe link=" << getViaLaneOrLane()->getID()
560  << " foeVeh=" << it.first->getID() << " (below ignore speed)"
561  << " ignoreFoeProb=" << ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_PROB, 0)
562  << "\n";
563  }
564  }
565 #endif
566  if (it.first != ego
567  && (ego == nullptr
569  || ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_IGNORE_FOE_SPEED, 0) < it.first->getSpeed()
571  && blockedByFoe(it.first, it.second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, sameTargetLane,
572  impatience, decel, waitingTime, ego)) {
573  if (collectFoes == nullptr) {
574  return true;
575  } else {
576  collectFoes->push_back(it.first);
577  }
578  }
579  }
580  return false;
581 }
582 
583 
584 bool
586  SUMOTime arrivalTime, SUMOTime leaveTime, double arrivalSpeed, double leaveSpeed,
587  bool sameTargetLane, double impatience, double decel, SUMOTime waitingTime,
588  const SUMOVehicle* ego) const {
589 #ifdef MSLink_DEBUG_OPENED
590  if (gDebugFlag1) {
591  std::cout << " foe link=" << getViaLaneOrLane()->getID()
592  << " foeVeh=" << veh->getID()
593  << " req=" << avi.willPass
594  << " aT=" << avi.arrivalTime
595  << " lT=" << avi.leavingTime
596  << "\n";
597  }
598 #endif
599  if (!avi.willPass) {
600  return false;
601  }
603  assert(waitingTime > 0);
604  if (waitingTime > avi.waitingTime) {
605  return false;
606  }
607  if (waitingTime == avi.waitingTime && arrivalTime < avi.arrivalTime) {
608  return false;
609  }
610  }
611  const SUMOTime foeArrivalTime = (SUMOTime)((1.0 - impatience) * avi.arrivalTime + impatience * avi.arrivalTimeBraking);
612  const SUMOTime lookAhead = (myState == LINKSTATE_ZIPPER
614  : (ego == nullptr
617  //if (ego != 0) std::cout << SIMTIME << " ego=" << ego->getID() << " jmTimegapMinor=" << ego->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_TIMEGAP_MINOR, -1) << " lookAhead=" << lookAhead << "\n";
618 #ifdef MSLink_DEBUG_OPENED
619  if (gDebugFlag1) {
620  std::cout << " imp=" << impatience << " fATb=" << avi.arrivalTimeBraking << " fAT2=" << foeArrivalTime << " lA=" << lookAhead << " egoAT=" << arrivalTime << " egoLT=" << leaveTime << "\n";
621  }
622 #endif
623  if (avi.leavingTime < arrivalTime) {
624  // ego wants to be follower
625  if (sameTargetLane && (arrivalTime - avi.leavingTime < lookAhead
626  || unsafeMergeSpeeds(avi.leaveSpeed, arrivalSpeed,
627  veh->getVehicleType().getCarFollowModel().getMaxDecel(), decel))) {
628 #ifdef MSLink_DEBUG_OPENED
629  if (gDebugFlag1) {
630  std::cout << " blocked (cannot follow)\n";
631  }
632 #endif
633  return true;
634  }
635  } else if (foeArrivalTime > leaveTime + lookAhead) {
636  // ego wants to be leader.
637  if (sameTargetLane && unsafeMergeSpeeds(leaveSpeed, avi.arrivalSpeedBraking,
638  decel, veh->getVehicleType().getCarFollowModel().getMaxDecel())) {
639 #ifdef MSLink_DEBUG_OPENED
640  if (gDebugFlag1) {
641  std::cout << " blocked (cannot lead)\n";
642  }
643 #endif
644  return true;
645  }
646  } else {
647  // even without considering safeHeadwayTime there is already a conflict
648 #ifdef MSLink_DEBUG_OPENED
649  if (gDebugFlag1) {
650  std::cout << " blocked (hard conflict)\n";
651  }
652 #endif
653  return true;
654  }
655  return false;
656 }
657 
658 
659 bool
661  MSVehicle* veh = lane->getLastAnyVehicle();
662  double distLeft = 0;
663  if (veh == nullptr) {
664  return false;
665  } else {
666  distLeft = lane->getLength() - veh->getBackPositionOnLane(lane);
667  assert(distLeft > 0);
668  // can we be sure that the vehicle leaves this lane in the next step?
669  bool result = distLeft > (veh->getSpeed() - veh->getCarFollowModel().getMaxDecel());
670  return result;
671  }
672 }
673 
674 
675 bool
676 MSLink::hasApproachingFoe(SUMOTime arrivalTime, SUMOTime leaveTime, double speed, double decel) const {
677  for (std::vector<MSLink*>::const_iterator i = myFoeLinks.begin(); i != myFoeLinks.end(); ++i) {
678  if ((*i)->blockedAtTime(arrivalTime, leaveTime, speed, speed, myLane == (*i)->getLane(), 0, decel, 0)) {
679  return true;
680  }
681  }
682  for (std::vector<const MSLane*>::const_iterator i = myFoeLanes.begin(); i != myFoeLanes.end(); ++i) {
683  if ((*i)->getVehicleNumberWithPartials() > 0) {
684  return true;
685  }
686  }
687  return false;
688 }
689 
690 
693  return myDirection;
694 }
695 
696 
697 void
699  if (myState != state) {
700  myLastStateChange = t;
701  }
702  myState = state;
703 }
704 
705 
706 MSLane*
708  return myLane;
709 }
710 
711 
712 bool
713 MSLink::isCont() const {
714  // when a traffic light is switched off minor roads have their cont status revoked
716 }
717 
718 
719 bool
721  if (myInternalLane == nullptr || myAmCont || myHavePedestrianCrossingFoe) {
722  return false;
723  } else {
725  if (!pred->getEdge().isInternal()) {
726  return false;
727  } else {
728  MSLane* pred2 = pred->getLogicalPredecessorLane();
729  assert(pred2 != 0);
730  MSLink* predLink = MSLinkContHelper::getConnectingLink(*pred2, *pred);
731  assert(predLink != 0);
732  return predLink->havePriority() || predLink->haveYellow();
733  }
734  }
735 }
736 
737 
738 void
739 MSLink::writeApproaching(OutputDevice& od, const std::string fromLaneID) const {
740  if (myApproachingVehicles.size() > 0) {
741  od.openTag("link");
742  od.writeAttr(SUMO_ATTR_FROM, fromLaneID);
743  const std::string via = getViaLane() == nullptr ? "" : getViaLane()->getID();
744  od.writeAttr(SUMO_ATTR_VIA, via);
745  od.writeAttr(SUMO_ATTR_TO, getLane() == nullptr ? "" : getLane()->getID());
746  std::vector<std::pair<SUMOTime, const SUMOVehicle*> > toSort; // stabilize output
747  for (auto it : myApproachingVehicles) {
748  toSort.push_back(std::make_pair(it.second.arrivalTime, it.first));
749  }
750  std::sort(toSort.begin(), toSort.end());
751  for (std::vector<std::pair<SUMOTime, const SUMOVehicle*> >::const_iterator it = toSort.begin(); it != toSort.end(); ++it) {
752  od.openTag("approaching");
753  const ApproachingVehicleInformation& avi = myApproachingVehicles.find(it->second)->second;
754  od.writeAttr(SUMO_ATTR_ID, it->second->getID());
755  od.writeAttr(SUMO_ATTR_IMPATIENCE, it->second->getImpatience());
756  od.writeAttr("arrivalTime", time2string(avi.arrivalTime));
757  od.writeAttr("arrivalTimeBraking", time2string(avi.arrivalTimeBraking));
758  od.writeAttr("leaveTime", time2string(avi.leavingTime));
759  od.writeAttr("arrivalSpeed", toString(avi.arrivalSpeed));
760  od.writeAttr("arrivalSpeedBraking", toString(avi.arrivalSpeedBraking));
761  od.writeAttr("leaveSpeed", toString(avi.leaveSpeed));
762  od.writeAttr("willPass", toString(avi.willPass));
763  od.closeTag();
764  }
765  od.closeTag();
766  }
767 }
768 
769 
770 double
772  double len = 0.;
773  MSLane* lane = myInternalLane;
774 
775  while (lane != nullptr && lane->isInternal()) {
776  len += lane->getLength();
777  lane = lane->getLinkCont()[0]->getViaLane();
778  }
779  return len;
780 }
781 
782 double
784  double len = 0.;
785  const MSLane* lane = myInternalLane;
786 
787  while (lane != nullptr && lane->isInternal()) {
788  len += lane->getLength();
789  if (lane->getIncomingLanes().size() == 1) {
790  lane = lane->getIncomingLanes()[0].lane;
791  } else {
792  break;
793  }
794  }
795  return len;
796 }
797 
798 
799 double
801  MSLane* via = myInternalLane;
802  double totalDist = 0.;
803  bool foundCrossing = false;
804  while (via != nullptr) {
805  MSLink* link = via->getLinkCont()[0];
806  double dist = link->getLengthBeforeCrossing(foeLane);
807  if (dist != INVALID_DOUBLE) {
808  // found conflicting lane
809  totalDist += dist;
810  foundCrossing = true;
811  break;
812  } else {
813  totalDist += via->getLength();
814  via = link->getViaLane();
815  }
816  }
817  if (foundCrossing) {
818  return totalDist;
819  } else {
820  return INVALID_DOUBLE;
821  }
822 }
823 
824 
825 double
827  int foe_ix;
828  for (foe_ix = 0; foe_ix != (int)myFoeLanes.size(); ++foe_ix) {
829  if (myFoeLanes[foe_ix] == foeLane) {
830  break;
831  }
832  }
833  if (foe_ix == (int)myFoeLanes.size()) {
834  // no conflict with the given lane, indicate by returning -1
835 #ifdef MSLink_DEBUG_CROSSING_POINTS
836  std::cout << "No crossing of lanes '" << foeLane->getID() << "' and '" << myInternalLaneBefore->getID() << "'" << std::endl;
837 #endif
838  return INVALID_DOUBLE;
839  } else {
840  // found conflicting lane index
841  double dist = myInternalLaneBefore->getLength() - myLengthsBehindCrossing[foe_ix].first;
842  if (dist == -10000.) {
843  // this is the value in myLengthsBehindCrossing, if the relation allows intersection but none is present for the actual geometry.
844  return INVALID_DOUBLE;
845  }
846 #ifdef MSLink_DEBUG_CROSSING_POINTS
847  std::cout << "Crossing of lanes '" << myInternalLaneBefore->getID() << "' and '" << foeLane->getID()
848  << "' at distance " << dist << " (approach along '"
849  << myInternalLaneBefore->getEntryLink()->getLaneBefore()->getID() << "')" << std::endl;
850 #endif
851  return dist;
852  }
853 }
854 
855 
856 MSLane*
858  return myInternalLane;
859 }
860 
861 
862 bool
865  return myInternalLane != nullptr && myInternalLaneBefore == nullptr;
866  } else {
867  return false;
868  }
869 }
870 
871 bool
873  // either a non-cont entry link or the link after a cont-link
874  return !myAmCont && (isEntryLink() || (myInternalLaneBefore != nullptr && myInternalLane != nullptr));
875 }
876 
877 bool
880  return myInternalLaneBefore != nullptr && myInternalLane == nullptr;
881  } else {
882  return false;
883  }
884 }
885 
886 bool
889  return (getInternalLaneBefore() != nullptr
890  && myInternalLaneBefore->getIncomingLanes().size() == 1
891  && myInternalLaneBefore->getIncomingLanes().front().viaLink->isInternalJunctionLink());
892  } else {
893  return false;
894  }
895 }
896 
897 
898 MSLink*
900  MSLane* lane = myInternalLane;
901  MSLink* link = nullptr;
902  while (lane != nullptr) {
903  link = lane->getLinkCont()[0];
904  lane = link->getViaLane();
905  }
906  return link;
907 }
908 
909 
910 bool
912  return getInternalLaneBefore() != nullptr && myInternalLane != nullptr;
913 }
914 
915 bool
917  return myInternalLaneBefore != nullptr;
918 }
919 
921 MSLink::getLeaderInfo(const MSVehicle* ego, double dist, std::vector<const MSPerson*>* collectBlockers, bool isShadowLink) const {
922  LinkLeaders result;
923  if (ego != nullptr && ego->getLaneChangeModel().isOpposite()) {
924  // ignore link leaders
925  return result;
926  }
927  //gDebugFlag1 = true;
928  // this link needs to start at an internal lane (either an exit link or between two internal lanes)
929  // or it must be queried by the pedestrian model (ego == 0)
930  if (fromInternalLane() || ego == nullptr) {
931  if (gDebugFlag1) {
932  std::cout << SIMTIME << " getLeaderInfo link=" << getViaLaneOrLane()->getID() << " dist=" << dist << " isShadowLink=" << isShadowLink << "\n";
933  }
934  // this is an exit link
935  for (int i = 0; i < (int)myFoeLanes.size(); ++i) {
936  const MSLane* foeLane = myFoeLanes[i];
937  // distance from the querying vehicle to the crossing point with foeLane
938  double distToCrossing = dist - myLengthsBehindCrossing[i].first;
939  const bool sameTarget = (myLane == foeLane->getLinkCont()[0]->getLane()) && !isInternalJunctionLink();
940  const bool sameSource = (myInternalLaneBefore != nullptr && myInternalLaneBefore->getLogicalPredecessorLane() == foeLane->getLogicalPredecessorLane());
941  const double crossingWidth = (sameTarget || sameSource) ? 0 : foeLane->getWidth();
942  const double foeCrossingWidth = (sameTarget || sameSource) ? 0 : myInternalLaneBefore->getWidth();
943  if (gDebugFlag1) {
944  std::cout << " distToCrossing=" << distToCrossing << " foeLane=" << foeLane->getID() << " cWidth=" << crossingWidth
945  << " ijl=" << isInternalJunctionLink() << " sT=" << sameTarget << " sS=" << sameSource
946  << " lbc=" << myLengthsBehindCrossing[i].first
947  << " flbc=" << myLengthsBehindCrossing[i].second
948  << "\n";
949  }
950  // special treatment of contLane foe only applies if this lane is not a contLane or contLane follower itself
951  const bool contLane = (foeLane->getLinkCont()[0]->getViaLaneOrLane()->getEdge().isInternal() && !(
953  if (distToCrossing + crossingWidth < 0
954  && (ego == nullptr || !MSGlobals::gComputeLC || distToCrossing + crossingWidth + ego->getVehicleType().getLength() < 0)) {
955  continue; // vehicle is behind the crossing point, continue with next foe lane
956  }
957  const double foeDistToCrossing = foeLane->getLength() - myLengthsBehindCrossing[i].second;
958  // it is not sufficient to return the last vehicle on the foeLane because ego might be its leader
959  // therefore we return all vehicles on the lane
960  //
961  // special care must be taken for continuation lanes. (next lane is also internal)
962  // vehicles on these lanes should always block (gap = -1)
963  // vehicles on cont. lanes or on internal lanes with the same target as this link can never be ignored
965  for (MSLane::AnyVehicleIterator it_veh = foeLane->anyVehiclesBegin(); it_veh != end; ++it_veh) {
966  MSVehicle* leader = (MSVehicle*)*it_veh;
967  const double leaderBack = leader->getBackPositionOnLane(foeLane);
968  const double leaderBackDist = foeDistToCrossing - leaderBack;
969  const bool pastTheCrossingPoint = leaderBackDist + foeCrossingWidth < 0;
970  const bool ignoreIndirectBicycleTurn = (pastTheCrossingPoint
971  && leader->getVehicleType().getVehicleClass() == SVC_BICYCLE
972  && foeLane->getIncomingLanes().front().viaLink->getDirection() == LINKDIR_LEFT);
973  const bool cannotIgnore = ((contLane && !ignoreIndirectBicycleTurn) || sameTarget || sameSource) && ego != nullptr;
974  const bool inTheWay = !pastTheCrossingPoint && leaderBackDist < leader->getVehicleType().getLength();
975  const bool isOpposite = leader->getLaneChangeModel().isOpposite();
976  if (gDebugFlag1) {
977  std::cout << " candiate leader=" << leader->getID()
978  << " cannotIgnore=" << cannotIgnore
979  << " fdtc=" << foeDistToCrossing
980  << " lb=" << leaderBack
981  << " lbd=" << leaderBackDist
982  << " fcwidth=" << foeCrossingWidth
983  << " foePastCP=" << pastTheCrossingPoint
984  << " inTheWay=" << inTheWay
985  << " willPass=" << foeLane->getLinkCont()[0]->getApproaching(leader).willPass
986  << " isFrontOnLane=" << leader->isFrontOnLane(foeLane)
987  << " isOpposite=" << isOpposite << "\n";
988  }
989  if (leader == ego) {
990  continue;
991  }
992  // after entering the conflict area, ignore foe vehicles that are not in the way
993  if (distToCrossing < -POSITION_EPS && !inTheWay
994  && (ego == nullptr || !MSGlobals::gComputeLC || distToCrossing < -ego->getVehicleType().getLength())) {
995  continue;
996  }
997  // ignore foe vehicles that will not pass
998  if ((!cannotIgnore || leader->isStopped())
999  && !foeLane->getLinkCont()[0]->getApproaching(leader).willPass
1000  && leader->isFrontOnLane(foeLane)
1001  && !isOpposite
1002  && !inTheWay
1003  // willPass is false if the vehicle is already on the stopping edge
1004  && !leader->willStop()) {
1005  continue;
1006  }
1007  if (cannotIgnore || inTheWay || leader->getWaitingTime() < MSGlobals::gIgnoreJunctionBlocker) {
1008  // compute distance between vehicles on the the superimposition of both lanes
1009  // where the crossing point is the common point
1010  double gap;
1011  bool fromLeft = true;
1012  if (ego == nullptr) {
1013  // request from pedestrian model. return distance between leaderBack and crossing point
1014  //std::cout << " foeLane=" << foeLane->getID() << " leaderBack=" << leaderBack << " foeDistToCrossing=" << foeDistToCrossing << " foeLength=" << foeLane->getLength() << " foebehind=" << myLengthsBehindCrossing[i].second << " dist=" << dist << " behind=" << myLengthsBehindCrossing[i].first << "\n";
1015  gap = leaderBackDist;
1016  // distToCrossing should not take into account the with of the foe lane
1017  // (which was subtracted in setRequestInformation)
1018  // Instead, the width of the foe vehicle is used directly by the caller.
1019  distToCrossing += foeLane->getWidth() / 2;
1020  if (gap + foeCrossingWidth < 0) {
1021  // leader is completely past the crossing point
1022  // or there is no crossing point
1023  continue; // next vehicle
1024  }
1025  // we need to determine whether the vehicle passes the
1026  // crossing from the left or the right (heuristic)
1027  fromLeft = foeDistToCrossing > 0.5 * foeLane->getLength();
1028  } else if ((contLane && !sameSource && !ignoreIndirectBicycleTurn) || isOpposite) {
1029  gap = -1; // always break for vehicles which are on a continuation lane or for opposite-direction vehicles
1030  } else {
1031  if (gDebugFlag1) {
1032  std::cout << " distToCrossing=" << distToCrossing << " leader back=" << leaderBack << " backDist=" << leaderBackDist << "\n";
1033  }
1034  if (leaderBackDist + foeCrossingWidth < 0) {
1035  // leader is completely past the crossing point
1036  // or there is no crossing point
1037  continue; // next vehicle
1038  }
1039  gap = distToCrossing - ego->getVehicleType().getMinGap() - leaderBackDist - foeCrossingWidth;
1040  }
1041  // if the foe is already moving off the intersection, we may
1042  // advance up to the crossing point unless we have the same target or same source
1043  // (for sameSource, the crossing point indicates the point of divergence)
1044  const bool stopAsap = leader->isFrontOnLane(foeLane) ? cannotIgnore : (sameTarget || sameSource);
1045  if (gDebugFlag1) {
1046  std::cout << " leader=" << leader->getID() << " contLane=" << contLane << " cannotIgnore=" << cannotIgnore << " stopAsap=" << stopAsap << "\n";
1047  }
1048  result.push_back(LinkLeader(leader, gap, stopAsap ? -1 : distToCrossing, fromLeft));
1049  }
1050 
1051  }
1052  if (ego != nullptr) {
1053  // check for crossing pedestrians (keep driving if already on top of the crossing
1054  const double distToPeds = distToCrossing - MSPModel::SAFETY_GAP;
1055  const double vehWidth = ego->getVehicleType().getWidth() + MSPModel::SAFETY_GAP; // + configurable safety gap
1057  // @check lefthand?!
1058  const bool wayIn = myLengthsBehindCrossing[i].first < myLaneBefore->getLength() * 0.5;
1059  const double vehSideOffset = (foeDistToCrossing + myLaneBefore->getWidth() * 0.5 - vehWidth * 0.5
1060  + ego->getLateralPositionOnLane() * (wayIn ? -1 : 1));
1061  if (distToPeds >= -MSPModel::SAFETY_GAP && MSPModel::getModel()->blockedAtDist(foeLane, vehSideOffset, vehWidth,
1063  collectBlockers)) {
1064  result.push_back(LinkLeader((MSVehicle*)nullptr, -1, distToPeds));
1065  }
1066  }
1067  }
1068 
1069  //std::cout << SIMTIME << " ego=" << Named::getIDSecure(ego) << " link=" << getViaLaneOrLane()->getID() << " myWalkingAreaFoe=" << Named::getIDSecure(myWalkingAreaFoe) << "\n";
1070  if (ego != nullptr && myWalkingAreaFoe != nullptr && myWalkingAreaFoe->getEdge().getPersons().size() > 0) {
1071  // pedestrians may be on an arbitrary path across this
1072  // walkingarea. make sure to keep enough distance.
1073  // This is a simple but conservative solution that could be improved
1074  // by ignoring pedestrians that are "obviously" not on a collision course
1075  double distToPeds = std::numeric_limits<double>::max();
1076  const std::set<MSTransportable*>& persons = myWalkingAreaFoe->getEdge().getPersons();
1077  for (std::set<MSTransportable*>::const_iterator it = persons.begin(); it != persons.end(); ++it) {
1078  MSPerson* p = dynamic_cast<MSPerson*>(*it);
1079  distToPeds = MIN2(distToPeds, ego->getPosition().distanceTo2D(p->getPosition()) - p->getVehicleType().getLength() - MSPModel::SAFETY_GAP);
1080  if (collectBlockers != nullptr) {
1081  collectBlockers->push_back(p);
1082  }
1083  }
1084  result.push_back(LinkLeader((MSVehicle*)nullptr, -1, distToPeds));
1085  }
1086 
1087  if (MSGlobals::gLateralResolution > 0 && ego != nullptr && !isShadowLink) {
1088  // check for foes on the same lane
1089  for (std::vector<MSLane*>::const_iterator it = mySublaneFoeLanes.begin(); it != mySublaneFoeLanes.end(); ++it) {
1090  const MSLane* foeLane = *it;
1091  MSLane::AnyVehicleIterator end = foeLane->anyVehiclesEnd();
1092  for (MSLane::AnyVehicleIterator it_veh = foeLane->anyVehiclesBegin(); it_veh != end; ++it_veh) {
1093  MSVehicle* leader = (MSVehicle*)*it_veh;
1094  if (leader == ego) {
1095  continue;
1096  }
1097  const double maxLength = MAX2(myInternalLaneBefore->getLength(), foeLane->getLength());
1098  const double gap = dist - maxLength - ego->getVehicleType().getMinGap() + leader->getBackPositionOnLane(foeLane);
1099  if (gap < -(ego->getVehicleType().getMinGap() + leader->getLength())) {
1100  // ego is ahead of leader
1101  continue;
1102  }
1103 
1104  const double posLat = ego->getLateralPositionOnLane();
1105  const double posLatLeader = leader->getLateralPositionOnLane() + leader->getLatOffset(foeLane);
1106  if (gDebugFlag1) {
1107  std::cout << " sublaneFoe lane=" << myInternalLaneBefore->getID()
1108  << " foeLane=" << foeLane->getID()
1109  << " leader=" << leader->getID()
1110  << " egoLane=" << ego->getLane()->getID()
1111  << " leaderLane=" << leader->getLane()->getID()
1112  << " egoLat=" << posLat
1113  << " leaderLat=" << posLatLeader
1114  << " leaderLatOffset=" << leader->getLatOffset(foeLane)
1115  << " egoIndex=" << myInternalLaneBefore->getIndex()
1116  << " foeIndex=" << foeLane->getIndex()
1117  << " dist=" << dist
1118  << " leaderBack=" << leader->getBackPositionOnLane(foeLane)
1119  << "\n";
1120  }
1121  // there only is a conflict if the paths cross
1122  if ((posLat < posLatLeader && myInternalLaneBefore->getIndex() > foeLane->getIndex())
1123  || (posLat > posLatLeader && myInternalLaneBefore->getIndex() < foeLane->getIndex())) {
1124  if (gDebugFlag1) {
1125  std::cout << SIMTIME << " blocked by " << leader->getID() << " (sublane split) foeLane=" << foeLane->getID() << "\n";
1126  }
1127  result.push_back(LinkLeader(leader, gap, -1));
1128  }
1129  }
1130  }
1131  }
1132  }
1133  return result;
1134 }
1135 
1136 
1137 MSLane*
1139  if (myInternalLane != nullptr) {
1140  return myInternalLane;
1141  }
1142  return myLane;
1143 }
1144 
1145 
1146 const MSLane*
1148  if (myInternalLaneBefore != nullptr) {
1150  throw ProcessError("lane before mismatch!");
1151  }
1152  }
1153  return myLaneBefore;
1154 }
1155 
1156 
1157 MSLink*
1158 MSLink::getParallelLink(int direction) const {
1159  if (direction == -1) {
1160  return myParallelRight;
1161  } else if (direction == 1) {
1162  return myParallelLeft;
1163  } else {
1164  assert(false);
1165  return nullptr;
1166  }
1167 }
1168 
1169 
1170 MSLink*
1172  MSLane* before = getLaneBefore()->getParallelLane(direction);
1173  MSLane* after = getLane()->getParallelLane(direction);
1174  if (before != nullptr && after != nullptr) {
1175  return MSLinkContHelper::getConnectingLink(*before, *after);
1176  } else {
1177  return nullptr;
1178  }
1179 }
1180 
1181 
1182 const MSLane*
1184  return myInternalLaneBefore;
1185 }
1186 
1187 
1188 double
1189 MSLink::getZipperSpeed(const MSVehicle* ego, const double dist, double vSafe,
1190  SUMOTime arrivalTime,
1191  std::vector<const SUMOVehicle*>* collectFoes) const {
1192  if (myFoeLinks.size() == 0) {
1193  // link should have LINKSTATE_MAJOR in this case
1194  assert(false);
1195  return vSafe;
1196  } else if (myFoeLinks.size() > 1) {
1197  throw ProcessError("Zipper junctions with more than two conflicting lanes are not supported (at junction '"
1198  + myJunction->getID() + "')");
1199  }
1201  const double secondsToArrival = STEPS2TIME(arrivalTime - now);
1202  if (secondsToArrival > ZIPPER_ADAPT_TIME && dist > ZIPPER_ADAPT_DIST) {
1203 #ifdef DEBUG_ZIPPER
1204  if (gDebugFlag1) std::cout << SIMTIME << " getZipperSpeed ego=" << ego->getID()
1205  << " dist=" << dist << " ignoring foes (arrival in " << STEPS2TIME(arrivalTime - now) << ")\n";
1206 #endif
1207  return vSafe;
1208  }
1209 #ifdef DEBUG_ZIPPER
1210  if (gDebugFlag1) std::cout << SIMTIME << " getZipperSpeed ego=" << ego->getID()
1211  << " egoAT=" << arrivalTime
1212  << " dist=" << dist
1213  << " vSafe=" << vSafe
1214  << " numFoes=" << collectFoes->size()
1215  << "\n";
1216 #endif
1217  MSLink* foeLink = myFoeLinks[0];
1218  const double vSafeOrig = vSafe;
1219  for (std::vector<const SUMOVehicle*>::const_iterator i = collectFoes->begin(); i != collectFoes->end(); ++i) {
1220  const MSVehicle* foe = dynamic_cast<const MSVehicle*>(*i);
1221  assert(foe != 0);
1222  const ApproachingVehicleInformation& avi = foeLink->getApproaching(foe);
1223  if ( // ignore vehicles that arrive after us (unless they are ahead and we could easily brake for them)
1224  ((avi.arrivalTime > arrivalTime) && !couldBrakeForLeader(dist, avi.dist, ego, foe)) ||
1225  // also ignore vehicles that are behind us and are able to brake for us
1226  couldBrakeForLeader(avi.dist, dist, foe, ego) ||
1227  // resolve ties by lane index
1228  (avi.arrivalTime == arrivalTime && avi.dist == dist && ego->getLane()->getIndex() < foe->getLane()->getIndex())) {
1229 #ifdef DEBUG_ZIPPER
1230  if (gDebugFlag1) std::cout
1231  << " ignoring foe=" << foe->getID()
1232  << " foeAT=" << avi.arrivalTime
1233  << " foeDist=" << avi.dist
1234  << " foeSpeed=" << foe->getSpeed()
1235  << " egoSpeed=" << ego->getSpeed()
1236  << " deltaDist=" << avi.dist - dist
1237  << " delteSpeed=" << foe->getSpeed() - foe->getCarFollowModel().getMaxDecel() - ego->getSpeed()
1238  << "\n";
1239 #endif
1240  continue;
1241  }
1242  const double gap = dist - foe->getVehicleType().getLength() - ego->getVehicleType().getMinGap() - avi.dist;
1243  const double follow = ego->getCarFollowModel().followSpeed(
1244  ego, ego->getSpeed(), gap, foe->getSpeed(), foe->getCarFollowModel().getMaxDecel(), foe);
1245  // speed adaption to follow the foe can be spread over secondsToArrival
1246  const double followInTime = vSafeOrig + (follow - vSafeOrig) / MAX2(1.0, secondsToArrival);
1247  vSafe = MIN2(vSafe, followInTime);
1248 #ifdef DEBUG_ZIPPER
1249  if (gDebugFlag1) std::cout << " adapting to foe=" << foe->getID()
1250  << " foeDist=" << avi.dist
1251  << " follow=" << follow
1252  << " followInTime=" << followInTime
1253  << " gap=" << gap
1254  << " foeSpeed=" << foe->getSpeed()
1255  << " follow=" << follow
1256  << " foeAT=" << avi.arrivalTime
1257  << " foeLT=" << avi.leavingTime
1258  << " foeAS=" << avi.arrivalSpeed
1259  << " vSafe=" << vSafe
1260  << "\n";
1261 #endif
1262  }
1263  return vSafe;
1264 }
1265 
1266 
1267 bool
1268 MSLink::couldBrakeForLeader(double followDist, double leaderDist, const MSVehicle* follow, const MSVehicle* leader) {
1269  return (// leader is ahead of follower
1270  followDist > leaderDist &&
1271  // and follower could brake for 1 s to stay behind leader
1272  followDist - leaderDist > follow->getSpeed() - follow->getCarFollowModel().getMaxDecel() - leader->getSpeed());
1273 }
1274 
1275 
1276 void
1280 }
1281 
1282 bool
1284  // check whether this link gets to keep its cont status switching the tls off
1285  // @note: this could also be pre-computed in netconvert
1286  // we check whether there is any major link from this edge
1287  for (const MSLane* cand : myLaneBefore->getEdge().getLanes()) {
1288  for (const MSLink* link : cand->getLinkCont()) {
1289  if (link->getOffState() == LINKSTATE_TL_OFF_NOSIGNAL) {
1290  return true;
1291  }
1292  }
1293  }
1294  return false;
1295 }
1296 
1297 /****************************************************************************/
1298 
const std::vector< IncomingLaneInfo > & getIncomingLanes() const
Definition: MSLane.h:785
bool gDebugFlag1
global utility flags for debugging
Definition: StdDefs.cpp:32
The link is a partial left direction.
static double gLateralResolution
Definition: MSGlobals.h:85
OutputDevice & writeAttr(const SumoXMLAttr attr, const T &val)
writes a named attribute
Definition: OutputDevice.h:256
MSLane * getLogicalPredecessorLane() const
get the most likely precedecessor lane (sorted using by_connections_to_sorter). The result is cached ...
Definition: MSLane.cpp:2407
double getLength() const
Returns the vehicle&#39;s length.
MSEdge & getEdge() const
Returns the lane&#39;s edge.
Definition: MSLane.h:640
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:79
long long int SUMOTime
Definition: SUMOTime.h:36
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.
SUMOTime getWaitingTime() const
Returns the SUMOTime waited (speed was lesser than 0.1m/s)
Definition: MSVehicle.h:629
MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:565
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:244
This is an uncontrolled, minor link, has to stop.
The base class for an intersection.
Definition: MSJunction.h:61
vehicle is a bicycle
static bool gComputeLC
whether the simulationLoop is in the lane changing phase
Definition: MSGlobals.h:121
static double rand(std::mt19937 *rng=0)
Returns a random real number in [0, 1)
Definition: RandHelper.h:61
AnyVehicleIterator is a structure, which manages the iteration through all vehicles on the lane...
Definition: MSLane.h:107
std::string time2string(SUMOTime t)
Definition: SUMOTime.cpp:65
const std::vector< MSLane * > & getLanes() const
Returns this edge&#39;s lanes.
Definition: MSEdge.h:162
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:165
T MAX2(T a, T b)
Definition: StdDefs.h:76
double getLength() const
Returns the lane&#39;s length.
Definition: MSLane.h:514
const PositionVector & getShape() const
Returns this lane&#39;s shape.
Definition: MSLane.h:456
Position getPosition(const double offset=0) const
Return current position (x/y, cartesian)
Definition: MSVehicle.cpp:1126
const std::string & getID() const
Returns the id.
Definition: Named.h:78
#define TIME2STEPS(x)
Definition: SUMOTime.h:60
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
The link is controlled by a tls which is off, not blinking, may pass.
This is an uncontrolled, all-way stop link.
double getWidth() const
Returns the lane&#39;s width.
Definition: MSLane.h:530
This is an uncontrolled, zipper-merge link.
The link is a (hard) left direction.
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:4307
#define SIMTIME
Definition: SUMOTime.h:65
bool isFrontOnLane(const MSLane *lane) const
Returns the information whether the front of the vehicle is on the given lane.
Definition: MSVehicle.cpp:3790
LinkDirection
The different directions a link between two lanes may take (or a stream between two edges)...
The link is a straight direction.
bool willStop() const
Returns whether the vehicle will stop on the current edge.
Definition: MSVehicle.cpp:1591
bool isInternal() const
Definition: MSLane.cpp:1875
#define SUMOTime_MIN
Definition: SUMOTime.h:38
A road/street connecting two junctions.
Definition: MSEdge.h:75
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:4975
const MSCFModel & getCarFollowModel() const
Returns the vehicle&#39;s car following model definition.
Definition: MSVehicle.h:891
int getIndex() const
Returns the lane&#39;s index.
Definition: MSLane.h:537
MSLink * getEntryLink() const
Returns the entry link if this is an internal lane, else 0.
Definition: MSLane.cpp:2003
const MSCFModel & getCarFollowModel() const
Returns the vehicle type&#39;s car following model definition (const version)
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:74
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:49
Representation of a vehicle.
Definition: SUMOVehicle.h:60
static MSPModel * getModel()
Definition: MSPModel.cpp:59
double interpolateGeometryPosToLanePos(double geometryPos) const
Definition: MSLane.h:484
A list of positions.
const std::set< MSTransportable * > & getPersons() const
Returns this edge&#39;s persons set.
Definition: MSEdge.h:171
#define STEPS2TIME(x)
Definition: SUMOTime.h:58
LinkState
The right-of-way state of a link between two lanes used when constructing a NBTrafficLightLogic, in MSLink and GNEInternalLane.
SUMOTime getCurrentTimeStep() const
Returns the current simulation step.
Definition: MSNet.h:263
T MIN2(T a, T b)
Definition: StdDefs.h:70
AnyVehicleIterator anyVehiclesEnd() const
end iterator for iterating over all vehicles touching this lane in downstream direction ...
Definition: MSLane.h:418
The link is a (hard) right direction.
#define POSITION_EPS
Definition: config.h:172
AnyVehicleIterator anyVehiclesBegin() const
begin iterator for iterating over all vehicles touching this lane in downstream direction ...
Definition: MSLane.h:412
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 ...
static bool gUsingInternalLanes
Information whether the simulation regards internal lanes.
Definition: MSGlobals.h:69
double getMinGap() const
Get the free space in front of vehicles of this class.
double getMaxDecel() const
Get the vehicle type&#39;s maximal comfortable deceleration [m/s^2].
Definition: MSCFModel.h:218
bool isInternal() const
return whether this edge is an internal edge
Definition: MSEdge.h:225
The link is a partial right direction.
const SUMOVTypeParameter & getParameter() const
void move2side(double amount)
move position vector to side using certain ammount
double getLateralPositionOnLane() const
Get the vehicle&#39;s lateral position on the lane.
Definition: MSVehicle.h:440
double getWidth() const
Get the width which vehicles of this class shall have when being drawn.
const MSVehicleType & getVehicleType() const
Returns the vehicle&#39;s type definition.
virtual Position getPosition() const
Return the Network coordinate of the transportable.
The link is controlled by a tls which is off and blinks, has to brake.
const MSJunction * getFromJunction() const
Definition: MSEdge.h:343
MSVehicle * getLastAnyVehicle() const
returns the last vehicle that is fully or partially on this lane
Definition: MSLane.cpp:1898
virtual double getLateralPositionOnLane() const =0
Get the vehicle&#39;s lateral position on the lane.
double getLength() const
Get vehicle&#39;s length [m].
static SUMOTime gIgnoreJunctionBlocker
Definition: MSGlobals.h:73
const MSVehicleType & getVehicleType() const
double getBackPositionOnLane(const MSLane *lane) const
Get the vehicle&#39;s position relative to the given lane.
Definition: MSVehicle.cpp:3701
The parent class for traffic light logics.
Static storage of an output device and its base (abstract) implementation.
Definition: OutputDevice.h:64
bool closeTag(const std::string &comment="")
Closes the most recently opened tag and optionally adds a comment.
static const double SAFETY_GAP
Definition: MSPModel.h:108
T MIN3(T a, T b, T c)
Definition: StdDefs.h:83
#define NUMERICAL_EPS
Definition: config.h:148
bool isStopped() const
Returns whether the vehicle is at a stop.
Definition: MSVehicle.cpp:1585
const double INVALID_DOUBLE
Definition: StdDefs.h:62
const MSLinkCont & getLinkCont() const
returns the container with all links !!!
Definition: MSLane.cpp:1975
double getSpeed() const
Returns the vehicle&#39;s current speed.
Definition: MSVehicle.h:483
#define DEBUG_COND
Definition: MESegment.cpp:54
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&#39;s follow speed (no dawdling)
const std::string & getID() const
Returns the name of the vehicle.
static bool gUseMesoSim
Definition: MSGlobals.h:91
Representation of a lane in the micro simulation.
Definition: MSLane.h:78
virtual const std::string & getID() const =0
Get the vehicle&#39;s ID.
OutputDevice & openTag(const std::string &xmlElement)
Opens an XML tag.
SUMOVehicleClass getVehicleClass() const
Get this vehicle type&#39;s vehicle class.
MSLane * getParallelLane(int offset) const
Returns the lane with the given offset parallel to this one or 0 if it does not exist.
Definition: MSLane.cpp:2063
virtual const MSVehicleType & getVehicleType() const =0
Returns the vehicle&#39;s type.