SUMO - Simulation of Urban MObility
MSLink.cpp
Go to the documentation of this file.
1 /****************************************************************************/
10 // A connnection between lanes
11 /****************************************************************************/
12 // SUMO, Simulation of Urban MObility; see http://sumo.dlr.de/
13 // Copyright (C) 2001-2017 DLR (http://www.dlr.de/) and contributors
14 /****************************************************************************/
15 //
16 // This file is part of SUMO.
17 // SUMO is free software: you can redistribute it and/or modify
18 // it under the terms of the GNU General Public License as published by
19 // the Free Software Foundation, either version 3 of the License, or
20 // (at your option) any later version.
21 //
22 /****************************************************************************/
23 
24 // ===========================================================================
25 // included modules
26 // ===========================================================================
27 #ifdef _MSC_VER
28 #include <windows_config.h>
29 #else
30 #include <config.h>
31 #endif
32 
33 #include <iostream>
34 #include <algorithm>
35 #include <limits>
37 #include "MSNet.h"
38 #include "MSJunction.h"
39 #include "MSLink.h"
40 #include "MSLane.h"
42 #include "MSEdge.h"
43 #include "MSGlobals.h"
44 #include "MSVehicle.h"
47 
48 //#define MSLink_DEBUG_CROSSING_POINTS
49 
50 // ===========================================================================
51 // static member variables
52 // ===========================================================================
54 // additional caution is needed when approaching a zipper link
56 
57 const double MSLink::ZIPPER_ADAPT_DIST(100);
58 
59 // time to link in seconds below which adaptation should take place
60 #define ZIPPER_ADAPT_TIME 10
61 
62 #define INVALID_DOUBLE std::numeric_limits<double>::max()
63 
64 // ===========================================================================
65 // member method definitions
66 // ===========================================================================
67 MSLink::MSLink(MSLane* predLane, MSLane* succLane, MSLane* via, LinkDirection dir, LinkState state, double length, double foeVisibilityDistance, bool keepClear, MSTrafficLightLogic* logic, int tlIndex) :
68  myLane(succLane),
69  myLaneBefore(predLane),
70  myIndex(-1),
71  myTLIndex(tlIndex),
72  myLogic(logic),
73  myState(state),
74  myLastStateChange(SUMOTime_MIN),
75  myDirection(dir),
76  myLength(length),
77  myFoeVisibilityDistance(foeVisibilityDistance),
78  myHasFoes(false),
79  myAmCont(false),
80  myKeepClear(keepClear),
81  myInternalLane(via),
82  myInternalLaneBefore(0),
83  myMesoTLSPenalty(0),
84  myGreenFraction(1),
85  myParallelRight(0),
86  myParallelLeft(0),
87  myJunction(0) {
88 }
89 
90 
92 
93 
94 void
96  const std::vector<MSLink*>& foeLinks,
97  const std::vector<MSLane*>& foeLanes,
98  MSLane* internalLaneBefore) {
99 //#ifdef MSLink_DEBUG_CROSSING_POINTS
100 // std::cout << " setRequestInformation() for junction " << getViaLaneOrLane()->getEdge().getFromJunction()->getID()
101 // << "\nInternalLanes = " << toString(getViaLaneOrLane()->getEdge().getFromJunction()->getInternalLanes())
102 // << std::endl;
103 //#endif
104  myIndex = index;
105  myHasFoes = hasFoes;
106  myAmCont = isCont;
107  myFoeLinks = foeLinks;
108  for (std::vector<MSLane*>::const_iterator it_lane = foeLanes.begin(); it_lane != foeLanes.end(); ++it_lane) {
109  // cannot assign vector due to const-ness
110  myFoeLanes.push_back(*it_lane);
111  }
112  myJunction = const_cast<MSJunction*>(myLane->getEdge().getFromJunction()); // junctionGraph is initialized after the whole network is loaded
113  myInternalLaneBefore = internalLaneBefore;
114  MSLane* lane = 0;
115  if (internalLaneBefore != 0) {
116  // this is an exit link. compute crossing points with all foeLanes
117  lane = internalLaneBefore;
118  //} else if (myLane->getEdge().isCrossing()) {
119  // // this is the link to a pedestrian crossing. compute crossing points with all foeLanes
120  // // @note not currently used by pedestrians
121  // lane = myLane;
122  }
123 #ifdef MSLink_DEBUG_CROSSING_POINTS
124  std::cout << " link " << myIndex << " to " << getViaLaneOrLane()->getID() << " internalLaneBefore=" << (lane == 0 ? "NULL" : lane->getID()) << " has foes: " << toString(foeLanes) << "\n";
125 #endif
126  if (lane != 0) {
127  const bool beforeInternalJunction = lane->getLinkCont()[0]->getViaLaneOrLane()->getEdge().isInternal();
128  assert(lane->getIncomingLanes().size() == 1);
129  // compute crossing points
130  for (std::vector<const MSLane*>::const_iterator it_lane = myFoeLanes.begin(); it_lane != myFoeLanes.end(); ++it_lane) {
131  const bool sameTarget = myLane == (*it_lane)->getLinkCont()[0]->getLane();
132  if (sameTarget && !beforeInternalJunction) {
133  //if (myLane == (*it_lane)->getLinkCont()[0]->getLane()) {
134  // this foeLane has the same target and merges at the end (lane exits the junction)
135  myLengthsBehindCrossing.push_back(std::make_pair(0, 0)); // dummy value, never used
136 #ifdef MSLink_DEBUG_CROSSING_POINTS
137  std::cout
138  << " " << lane->getID()
139  << " merges with " << (*it_lane)->getID()
140  << " nextLane " << lane->getLinkCont()[0]->getViaLaneOrLane()->getID()
141  << " dist1=" << myLengthsBehindCrossing.back().first
142  << " dist2=" << myLengthsBehindCrossing.back().second
143  << "\n";
144 #endif
145  } else {
146  std::vector<double> intersections1 = lane->getShape().intersectsAtLengths2D((*it_lane)->getShape());
147 #ifdef MSLink_DEBUG_CROSSING_POINTS
148 // std::cout << " intersections1=" << toString(intersections1) << "\n";
149 #endif
150  bool haveIntersection = true;
151  if (intersections1.size() == 0) {
152  intersections1.push_back(-10000.0); // disregard this foe (using maxdouble leads to nasty problems down the line)
153  haveIntersection = false;
154  } else if (intersections1.size() > 1) {
155  std::sort(intersections1.begin(), intersections1.end());
156  }
157  std::vector<double> intersections2 = (*it_lane)->getShape().intersectsAtLengths2D(lane->getShape());
158 #ifdef MSLink_DEBUG_CROSSING_POINTS
159  //std::cout << " intersections2=" << toString(intersections2) << "\n";
160 #endif
161  if (intersections2.size() == 0) {
162  intersections2.push_back(0);
163  } else if (intersections2.size() > 1) {
164  std::sort(intersections2.begin(), intersections2.end());
165  }
166  if (haveIntersection) {
167  // lane width affects the crossing point
168  intersections1.back() -= (*it_lane)->getWidth() / 2;
169  intersections2.back() -= lane->getWidth() / 2;
170  // also length/geometry factor. (XXX: Why subtract width/2 *before* converting geometric position to lane pos?)
171  intersections1.back() = lane->interpolateGeometryPosToLanePos(intersections1.back());
172  intersections2.back() = (*it_lane)->interpolateGeometryPosToLanePos(intersections2.back());
173 
174  if (internalLaneBefore->getLogicalPredecessorLane()->getEdge().isInternal() && !(*it_lane)->getEdge().isCrossing()) {
175  // wait at the internal junction
176  // (except for foes that are crossings since there is no internal junction)
177  intersections1.back() = 0;
178  }
179  }
180 
181  myLengthsBehindCrossing.push_back(std::make_pair(
182  lane->getLength() - intersections1.back(),
183  (*it_lane)->getLength() - intersections2.back()));
184 
185 #ifdef MSLink_DEBUG_CROSSING_POINTS
186  std::cout
187  << " intersection of " << lane->getID()
188  << " totalLength=" << lane->getLength()
189  << " with " << (*it_lane)->getID()
190  << " totalLength=" << (*it_lane)->getLength()
191  << " dist1=" << myLengthsBehindCrossing.back().first
192  << " dist2=" << myLengthsBehindCrossing.back().second
193  << "\n";
194 #endif
195  }
196  }
197  // check for overlap with internal lanes from the same source lane
198  const MSLane* pred = lane->getLogicalPredecessorLane();
199  // to avoid overlap with vehicles that came from pred (especially when pred has endOffset > 0)
200  // we add all other internal lanes from pred as foeLanes
201  const MSLinkCont& predLinks = pred->getLinkCont();
202  for (MSLinkCont::const_iterator it = predLinks.begin(); it != predLinks.end(); ++it) {
203  const MSLane* sibling = (*it)->getViaLane();
204  if (sibling != lane && sibling != 0) {
205  std::vector<double> intersections1 = lane->getShape().intersectsAtLengths2D(sibling->getShape());
206 #ifdef MSLink_DEBUG_CROSSING_POINTS
207  //std::cout << " intersections1=" << toString(intersections1) << "\n";
208 #endif
209  if (intersections1.size() > 0) {
210  std::sort(intersections1.begin(), intersections1.end());
211  if (intersections1.back() > NUMERICAL_EPS) {
212  // siblings share a common shape up to the last crossing point so intersections are identical and only need to be computed once
213  myLengthsBehindCrossing.push_back(std::make_pair(
214  lane->getLength() - intersections1.back(),
215  sibling->getLength() - intersections1.back()));
216  myFoeLanes.push_back(sibling);
217 #ifdef MSLink_DEBUG_CROSSING_POINTS
218  std::cout << " adding same-origin foe" << sibling->getID()
219  << " dist1=" << myLengthsBehindCrossing.back().first
220  << " dist2=" << myLengthsBehindCrossing.back().second
221  << "\n";
222 #endif
223  }
224  }
225  }
226  }
227  }
229  // check for links with the same origin lane and the same destination edge
230  const MSEdge* myTarget = &myLane->getEdge();
231  const MSLinkCont& predLinks = myLaneBefore->getLinkCont();
232  for (MSLinkCont::const_iterator it = predLinks.begin(); it != predLinks.end(); ++it) {
233  const MSEdge* target = &((*it)->getLane()->getEdge());
234  if (*it != this && target == myTarget) {
235  mySublaneFoeLinks.push_back(*it);
236  }
237  }
238  }
239 }
240 
241 
242 std::pair<double, double>
243 getLastIntersections(const MSLane* lane, const MSLane* foe);
244 
245 void
246 MSLink::setApproaching(const SUMOVehicle* approaching, const SUMOTime arrivalTime, const double arrivalSpeed, const double leaveSpeed,
247  const bool setRequest, const SUMOTime arrivalTimeBraking, const double arrivalSpeedBraking, const SUMOTime waitingTime, double dist) {
248  const SUMOTime leaveTime = getLeaveTime(arrivalTime, arrivalSpeed, leaveSpeed, approaching->getVehicleType().getLength());
249  myApproachingVehicles.insert(std::make_pair(approaching,
250  ApproachingVehicleInformation(arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, setRequest,
251  arrivalTimeBraking, arrivalSpeedBraking, waitingTime, dist)));
252 }
253 
254 
255 void
257  myBlockedFoeLinks.insert(link);
258 }
259 
260 
261 
262 bool
264  for (std::set<MSLink*>::const_iterator i = myBlockedFoeLinks.begin(); i != myBlockedFoeLinks.end(); ++i) {
265  if ((*i)->isBlockingAnyone()) {
266  return true;
267  }
268  }
269  return false;
270 }
271 
272 
273 void
275  myApproachingVehicles.erase(veh);
276 }
277 
278 
281  std::map<const SUMOVehicle*, ApproachingVehicleInformation>::const_iterator i = myApproachingVehicles.find(veh);
282  if (i != myApproachingVehicles.end()) {
283  return i->second;
284  } else {
285  return ApproachingVehicleInformation(-1000, -1000, 0, 0, false, -1000, 0, 0, 0);
286  }
287 }
288 
289 
290 SUMOTime
291 MSLink::getLeaveTime(const SUMOTime arrivalTime, const double arrivalSpeed,
292  const double leaveSpeed, const double vehicleLength) const {
293  return arrivalTime + TIME2STEPS((getLength() + vehicleLength) / MAX2(0.5 * (arrivalSpeed + leaveSpeed), NUMERICAL_EPS));
294 }
295 
296 
297 bool
298 MSLink::opened(SUMOTime arrivalTime, double arrivalSpeed, double leaveSpeed, double vehicleLength,
299  double impatience, double decel, SUMOTime waitingTime, double posLat,
300  std::vector<const SUMOVehicle*>* collectFoes) const {
301  if (haveRed()) {
302  return false;
303  }
305  return true;
306  }
307  const SUMOTime leaveTime = getLeaveTime(arrivalTime, arrivalSpeed, leaveSpeed, vehicleLength);
309  // check for foes on the same lane
310  for (std::vector<MSLink*>::const_iterator it = mySublaneFoeLinks.begin(); it != mySublaneFoeLinks.end(); ++it) {
311  const MSLink* foeLink = *it;
312  assert(myLane != foeLink->getLane());
313  for (std::map<const SUMOVehicle*, ApproachingVehicleInformation>::const_iterator i = foeLink->myApproachingVehicles.begin(); i != foeLink->myApproachingVehicles.end(); ++i) {
314  const SUMOVehicle* foe = i->first;
315  if (
316  // there only is a conflict if the paths cross
317  ((posLat < foe->getLateralPositionOnLane() && myLane->getIndex() > foeLink->myLane->getIndex())
318  || (posLat > foe->getLateralPositionOnLane() && myLane->getIndex() < foeLink->myLane->getIndex()))
319  // the vehicle that arrives later must yield
320  && (arrivalTime > i->second.arrivalTime
321  // if both vehicles arrive at the same time, the one
322  // to the left must yield
323  || (arrivalTime == i->second.arrivalTime && posLat > foe->getLateralPositionOnLane()))) {
324  if (blockedByFoe(i->first, i->second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, false,
325  impatience, decel, waitingTime)) {
326  //std::cout << SIMTIME << " blocked by " << foe->getID() << " arrival=" << arrivalTime << " foeArrival=" << i->second.arrivalTime << "\n";
327  if (collectFoes == 0) {
328  //std::cout << " link=" << getViaLaneOrLane()->getID() << " blocked by sublaneFoe=" << foe->getID() << " foeLink=" << foeLink->getViaLaneOrLane()->getID() << " posLat=" << posLat << "\n";
329  return false;
330  } else {
331  collectFoes->push_back(i->first);
332  }
333  }
334  }
335  }
336  }
337  }
338  if (havePriority() && myState != LINKSTATE_ZIPPER) {
339  // priority usually means the link is open but there are exceptions:
340  // zipper still needs to collect foes
341  // sublane model could have detected a conflict
342  return collectFoes == 0 || collectFoes->size() == 0;
343  }
344  if ((myState == LINKSTATE_STOP || myState == LINKSTATE_ALLWAY_STOP) && waitingTime == 0) {
345  return false;
346  }
347  for (std::vector<MSLink*>::const_iterator i = myFoeLinks.begin(); i != myFoeLinks.end(); ++i) {
349  if ((*i)->haveRed()) {
350  continue;
351  }
352  }
353  if ((*i)->blockedAtTime(arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, myLane == (*i)->getLane(),
354  impatience, decel, waitingTime, collectFoes)) {
355  return false;
356  }
357  }
358  if (collectFoes != 0 && collectFoes->size() > 0) {
359  return false;
360  }
361  return true;
362 }
363 
364 
365 bool
366 MSLink::blockedAtTime(SUMOTime arrivalTime, SUMOTime leaveTime, double arrivalSpeed, double leaveSpeed,
367  bool sameTargetLane, double impatience, double decel, SUMOTime waitingTime,
368  std::vector<const SUMOVehicle*>* collectFoes) const {
369  for (std::map<const SUMOVehicle*, ApproachingVehicleInformation>::const_iterator i = myApproachingVehicles.begin(); i != myApproachingVehicles.end(); ++i) {
370  if (blockedByFoe(i->first, i->second, arrivalTime, leaveTime, arrivalSpeed, leaveSpeed, sameTargetLane,
371  impatience, decel, waitingTime)) {
372  if (collectFoes == 0) {
373  return true;
374  } else {
375  collectFoes->push_back(i->first);
376  }
377  }
378  }
379  return false;
380 }
381 
382 
383 bool
384 MSLink::blockedByFoe(const SUMOVehicle* veh, const ApproachingVehicleInformation& avi, SUMOTime arrivalTime, SUMOTime leaveTime, double arrivalSpeed, double leaveSpeed,
385  bool sameTargetLane, double impatience, double decel, SUMOTime waitingTime) const {
386  if (!avi.willPass) {
387  return false;
388  }
390  assert(waitingTime > 0);
391  if (waitingTime > avi.waitingTime) {
392  return false;
393  }
394  if (waitingTime == avi.waitingTime && arrivalTime < avi.arrivalTime) {
395  return false;
396  }
397  }
398  const SUMOTime foeArrivalTime = (SUMOTime)((1.0 - impatience) * avi.arrivalTime + impatience * avi.arrivalTimeBraking);
400  if (avi.leavingTime < arrivalTime) {
401  // ego wants to be follower
402  if (sameTargetLane && (arrivalTime - avi.leavingTime < lookAhead
403  || unsafeMergeSpeeds(avi.leaveSpeed, arrivalSpeed,
404  veh->getVehicleType().getCarFollowModel().getMaxDecel(), decel))) {
405  return true;
406  }
407  } else if (foeArrivalTime > leaveTime) {
408  // ego wants to be leader.
409  if (sameTargetLane && (foeArrivalTime - leaveTime < lookAhead
410  || unsafeMergeSpeeds(leaveSpeed, avi.arrivalSpeedBraking,
411  decel, veh->getVehicleType().getCarFollowModel().getMaxDecel()))) {
412  return true;
413  }
414  } else {
415  // even without considering safeHeadwayTime there is already a conflict
416  return true;
417  }
418  return false;
419 }
420 
421 
422 bool
424  MSVehicle* veh = lane->getLastAnyVehicle();
425  double distLeft = 0;
426  if (veh == 0) {
427  return false;
428  } else {
429  distLeft = lane->getLength() - veh->getBackPositionOnLane(lane);
430  assert(distLeft > 0);
431  // can we be sure that the vehicle leaves this lane in the next step?
432  bool result = distLeft > (veh->getSpeed() - veh->getCarFollowModel().getMaxDecel());
433  return result;
434  }
435 }
436 
437 
438 bool
439 MSLink::hasApproachingFoe(SUMOTime arrivalTime, SUMOTime leaveTime, double speed, double decel) const {
440  for (std::vector<MSLink*>::const_iterator i = myFoeLinks.begin(); i != myFoeLinks.end(); ++i) {
441  if ((*i)->blockedAtTime(arrivalTime, leaveTime, speed, speed, myLane == (*i)->getLane(), 0, decel, 0)) {
442  return true;
443  }
444  }
445  for (std::vector<const MSLane*>::const_iterator i = myFoeLanes.begin(); i != myFoeLanes.end(); ++i) {
446  if ((*i)->getVehicleNumberWithPartials() > 0) {
447  return true;
448  }
449  }
450  return false;
451 }
452 
453 
456  return myDirection;
457 }
458 
459 
460 void
462  if (myState != state) {
463  myLastStateChange = t;
464  }
465  myState = state;
466 }
467 
468 
469 MSLane*
471  return myLane;
472 }
473 
474 bool
476  if (myInternalLane == 0 || myAmCont) {
477  return false;
478  } else {
481  return false;
482  } else {
483  MSLane* pred2 = pred->getLogicalPredecessorLane();
484  assert(pred2 != 0);
485  MSLink* predLink = MSLinkContHelper::getConnectingLink(*pred2, *pred);
486  assert(predLink != 0);
487  return predLink->havePriority();
488  }
489  }
490 }
491 
492 
493 void
494 MSLink::writeApproaching(OutputDevice& od, const std::string fromLaneID) const {
495  if (myApproachingVehicles.size() > 0) {
496  od.openTag("link");
497  od.writeAttr(SUMO_ATTR_FROM, fromLaneID);
498  const std::string via = getViaLane() == 0 ? "" : getViaLane()->getID();
499  od.writeAttr(SUMO_ATTR_VIA, via);
500  od.writeAttr(SUMO_ATTR_TO, getLane() == 0 ? "" : getLane()->getID());
501  std::vector<std::pair<SUMOTime, const SUMOVehicle*> > toSort; // stabilize output
502  for (std::map<const SUMOVehicle*, ApproachingVehicleInformation>::const_iterator it = myApproachingVehicles.begin(); it != myApproachingVehicles.end(); ++it) {
503  toSort.push_back(std::make_pair(it->second.arrivalTime, it->first));
504  }
505  std::sort(toSort.begin(), toSort.end());
506  for (std::vector<std::pair<SUMOTime, const SUMOVehicle*> >::const_iterator it = toSort.begin(); it != toSort.end(); ++it) {
507  od.openTag("approaching");
508  const ApproachingVehicleInformation& avi = myApproachingVehicles.find(it->second)->second;
509  od.writeAttr(SUMO_ATTR_ID, it->second->getID());
510  od.writeAttr(SUMO_ATTR_IMPATIENCE, it->second->getImpatience());
511  od.writeAttr("arrivalTime", time2string(avi.arrivalTime));
512  od.writeAttr("arrivalTimeBraking", time2string(avi.arrivalTimeBraking));
513  od.writeAttr("leaveTime", time2string(avi.leavingTime));
514  od.writeAttr("arrivalSpeed", toString(avi.arrivalSpeed));
515  od.writeAttr("arrivalSpeedBraking", toString(avi.arrivalSpeedBraking));
516  od.writeAttr("leaveSpeed", toString(avi.leaveSpeed));
517  od.writeAttr("willPass", toString(avi.willPass));
518  od.closeTag();
519  }
520  od.closeTag();
521  }
522 }
523 
524 
525 double
527  double len = 0.;
528  MSLane* lane = myInternalLane;
529 
530  while (lane != 0 && lane->isInternal()) {
531  len += lane->getLength();
532  lane = lane->getLinkCont()[0]->getViaLane();
533  }
534  return len;
535 }
536 
537 
538 double
539 MSLink::getLengthBeforeCrossing(const MSLink* foeEntryLink) const {
540  // to be called for entry links
541  assert(foeEntryLink->getInternalLaneBefore() == 0);
542 
543  // TODO: Understand myLengthsBehindCrossing and extract relevant information -> take a look at getLeaderInfo()!
544  // -> myLengthsBehindCrossing does not treat lanes behind internal junctions, these should be extracted from the internal lane of that junction?!
545 
546  // first internal lane for foe after entering the junction
547  const MSLane* foeEntryLane = foeEntryLink->getViaLane();
548  assert(foeEntryLane != 0);
549 
550  // first internal lane for ego after entering the junction
551  const MSLane* egoEntryLane = getViaLane();
552  assert(egoEntryLane != 0);
553 
554  int foe_ix = 0;
555  double res = INVALID_DOUBLE;
556  for (std::vector<const MSLane*>::const_iterator i = myFoeLanes.begin(); i != myFoeLanes.end(); ++i) {
557  if (*i == foeEntryLane) {
558  // Found foe lane index
559  // note: myLengthsBehindcrossing is the length behind the foe lane's border, not its center
560  // (therefore the term -foeEntryLane->getWidth()/2.)
561  // XXX: recheck definition of myLengthsBehindCrossing in setRequestInformation(), especially order of position scaling and subtracting the lane width/2
562  res = egoEntryLane->getLength() - myLengthsBehindCrossing[foe_ix].first - foeEntryLane->getWidth() / 2.;
563  break;
564  }
565  ++foe_ix;
566  }
567 
568 
569 #ifdef MSLink_DEBUG_CROSSING_POINTS
570  std::cout << "getLengthBeforeCrossing() for link " << toString(getLaneBefore()) << "->" << toString(getViaLane())
571  << " and foeLink " << toString(foeEntryLink->getLaneBefore()) << "->" << toString(foeEntryLink->getViaLane())
572  << "\nLength on toString(getLaneBefore()) before crossing = " << res
573  << std::endl;
574 #endif
575 
576  if (foe_ix == (int)myFoeLanes.size() && egoEntryLane->getLinkCont().size() > 0) {
577  // Did not find crossing for first egoLane. Check internal cont lanes
578  assert(egoEntryLane->getLinkCont().size() == 1);
579  MSLink* contLink = egoEntryLane->getLinkCont()[0];
580  MSLane* contLane = contLink->getViaLane();
581  if (contLane != 0) {
582  // Yes, there is another internal lane after the entry lane
583  // Assert, there are no more internal lanes than two along the conection (in case the code should be modified)
584  assert(contLane->getLinkCont().size() == 0
585  || (contLane->getLinkCont().size() == 1 && contLane->getLinkCont()[0]->getViaLane() == 0));
586 #ifdef MSLink_DEBUG_CROSSING_POINTS
587  std::cout << "No crossing with entryLane.\nChecking crossing of contLane '" << contLane->getID()
588  << "' with '" << toString(foeEntryLink->getLaneBefore()) << "'"
589  << std::endl;
590 #endif
591  // TODO: really check for crossing as above
592  }
593  }
594 
595  return res;
596 }
597 
598 
599 MSLane*
601  return myInternalLane;
602 }
603 
604 
605 bool
609 }
610 
611 
612 bool
614  return getInternalLaneBefore() != 0 && myInternalLane != 0;
615 }
616 
617 bool
619  return isExitLink() || isInternalJunctionLink();
620 }
621 
623 MSLink::getLeaderInfo(const MSVehicle* ego, double dist, std::vector<const MSPerson*>* collectBlockers) const {
624  LinkLeaders result;
625  if (ego->getLaneChangeModel().isOpposite()) {
626  // ignore link leaders
627  return result;
628  }
629  //gDebugFlag1 = true;
630  // this link needs to start at an internal lane (either an exit link or between two internal lanes)
631  if (fromInternalLane()) {
632  //if (gDebugFlag1) std::cout << SIMTIME << " getLeaderInfo link=" << getViaLaneOrLane()->getID() << "\n";
633  // this is an exit link
634  for (int i = 0; i < (int)myFoeLanes.size(); ++i) {
635  const MSLane* foeLane = myFoeLanes[i];
636  // distance from the querying vehicle to the crossing point with foeLane
637  const double distToCrossing = dist - myLengthsBehindCrossing[i].first;
638  const bool sameTarget = (myLane == foeLane->getLinkCont()[0]->getLane());
639  const bool sameSource = (myInternalLaneBefore != 0 && myInternalLaneBefore->getLogicalPredecessorLane() == foeLane->getLogicalPredecessorLane());
640  const double crossingWidth = (sameTarget || sameSource) ? 0 : foeLane->getWidth();
641  const double foeCrossingWidth = (sameTarget || sameSource) ? 0 : myInternalLaneBefore->getWidth();
642  if (gDebugFlag1) {
643  std::cout << " distToCrossing=" << distToCrossing << " foeLane=" << foeLane->getID() << "\n";
644  }
645  if (distToCrossing + crossingWidth < 0) {
646  continue; // vehicle is behind the crossing point, continue with next foe lane
647  }
648  const double foeDistToCrossing = foeLane->getLength() - myLengthsBehindCrossing[i].second;
649  // it is not sufficient to return the last vehicle on the foeLane because ego might be its leader
650  // therefore we return all vehicles on the lane
651  //
652  // special care must be taken for continuation lanes. (next lane is also internal)
653  // vehicles on these lanes should always block (gap = -1)
654  const bool contLane = (foeLane->getLinkCont()[0]->getViaLaneOrLane()->getEdge().getPurpose() == MSEdge::EDGEFUNCTION_INTERNAL);
655  // vehicles on cont. lanes or on internal lanes with the same target as this link can never be ignored
656  const bool cannotIgnore = contLane || sameTarget || sameSource;
658  for (MSLane::AnyVehicleIterator it_veh = foeLane->anyVehiclesBegin(); it_veh != end; ++it_veh) {
659  MSVehicle* leader = (MSVehicle*)*it_veh;
660  const bool isOpposite = leader->getLaneChangeModel().isOpposite();
661  if (gDebugFlag1) {
662  std::cout << " candiate leader=" << leader->getID()
663  << " cannotIgnore=" << cannotIgnore
664  << " willPass=" << foeLane->getLinkCont()[0]->getApproaching(leader).willPass
665  << " isFrontOnLane=" << leader->isFrontOnLane(foeLane)
666  << " isOpposite=" << isOpposite << "\n";
667  }
668  if (!cannotIgnore && !foeLane->getLinkCont()[0]->getApproaching(leader).willPass && leader->isFrontOnLane(foeLane) && !isOpposite) {
669  continue;
670  }
671  if (cannotIgnore || leader->getWaitingTime() < MSGlobals::gIgnoreJunctionBlocker) {
672  // compute distance between vehicles on the the superimposition of both lanes
673  // where the crossing point is the common point
674  double gap;
675  if ((contLane && !sameSource) || isOpposite) {
676  gap = -1; // always break for vehicles which are on a continuation lane or for opposite-direction vehicles
677  } else {
678  const double leaderBack = leader->getBackPositionOnLane(foeLane);
679  const double leaderBackDist = foeDistToCrossing - leaderBack;
680  if (gDebugFlag1) {
681  std::cout << " distToCrossing=" << distToCrossing << " leader back=" << leaderBack << " backDist=" << leaderBackDist << "\n";
682  }
683  if (leaderBackDist + foeCrossingWidth < 0) {
684  // leader is completely past the crossing point
685  // or there is no crossing point
686  continue; // next vehicle
687  }
688  gap = distToCrossing - leaderBackDist - (sameTarget ? ego->getVehicleType().getMinGap() : 0);
689  }
690  if (gDebugFlag1) {
691  std::cout << " leader=" << leader->getID() << " contLane=" << contLane << " cannotIgnore=" << cannotIgnore << "\n";
692  }
693  // if the foe is already moving off the intersection, we may
694  // advance up to the crossing point unless we have the same target
695  const bool stopAsap = leader->isFrontOnLane(foeLane) ? cannotIgnore : sameTarget;
696  result.push_back(LinkLeader(leader, gap, stopAsap ? -1 : distToCrossing));
697  }
698 
699  }
700  // check for crossing pedestrians (keep driving if already on top of the crossing
701  const double distToPeds = distToCrossing - MSPModel::SAFETY_GAP;
702  if (distToPeds >= -MSPModel::SAFETY_GAP && MSPModel::getModel()->blockedAtDist(foeLane, foeDistToCrossing, collectBlockers)) {
703  result.push_back(LinkLeader((MSVehicle*)0, -1, distToPeds));
704  }
705  }
706  }
707  return result;
708 }
709 
710 
711 MSLane*
713  if (myInternalLane != 0) {
714  return myInternalLane;
715  }
716  return myLane;
717 }
718 
719 
720 const MSLane*
722  if (myInternalLaneBefore != 0) {
724  throw ProcessError("lane before mismatch!");
725  }
726  }
727  return myLaneBefore;
728 }
729 
730 
731 MSLink*
732 MSLink::getParallelLink(int direction) const {
733  if (direction == -1) {
734  return myParallelRight;
735  } else if (direction == 1) {
736  return myParallelLeft;
737  } else {
738  assert(false);
739  return 0;
740  }
741 }
742 
743 
744 MSLink*
746  MSLane* before = getLaneBefore()->getParallelLane(direction);
747  MSLane* after = getViaLaneOrLane()->getParallelLane(direction);
748  if (before != 0 && after != 0) {
749  return MSLinkContHelper::getConnectingLink(*before, *after);
750  } else {
751  return 0;
752  }
753 }
754 
755 void
757  if (myJunction != 0) {
758  myJunction->passedJunction(vehicle);
759  }
760 }
761 
762 
763 bool
764 MSLink::isLeader(const MSVehicle* ego, const MSVehicle* foe) const {
765  if (myJunction != 0) {
766  return myJunction->isLeader(ego, foe);
767  } else {
768  // unregulated junction
769  return false;
770  }
771 }
772 
773 const MSLane*
775  return myInternalLaneBefore;
776 }
777 
778 
779 double
780 MSLink::getZipperSpeed(const MSVehicle* ego, const double dist, double vSafe,
781  SUMOTime arrivalTime,
782  std::vector<const SUMOVehicle*>* collectFoes) const {
783  //gDebugFlag1 = ego->getID() == "left.7";
784  if (myFoeLinks.size() == 0) {
785  // link should have LINKSTATE_MAJOR in this case
786  assert(false);
787  return vSafe;
788  } else if (myFoeLinks.size() > 1) {
789  throw ProcessError("Zipper junctions with more than two conflicting lanes are not supported (at junction '"
790  + myJunction->getID() + "')");
791  }
793  const double secondsToArrival = STEPS2TIME(arrivalTime - now);
794  if (secondsToArrival > ZIPPER_ADAPT_TIME && dist > ZIPPER_ADAPT_DIST) {
795  //if (gDebugFlag1) std::cout << SIMTIME << " getZipperSpeed ego=" << ego->getID()
796  // << " dist=" << dist
797  // << " ignoring foes (arrival in " << STEPS2TIME(arrivalTime - now) << ")\n";
798  return vSafe;
799  }
800  //if (gDebugFlag1) std::cout << SIMTIME << " getZipperSpeed ego=" << ego->getID()
801  // << " egoAT=" << arrivalTime
802  // << " dist=" << dist
803  // << " vSafe=" << vSafe
804  // << " numFoes=" << collectFoes->size()
805  // << "\n";
806  MSLink* foeLink = myFoeLinks[0];
807  const double vSafeOrig = vSafe;
808  for (std::vector<const SUMOVehicle*>::const_iterator i = collectFoes->begin(); i != collectFoes->end(); ++i) {
809  const MSVehicle* foe = dynamic_cast<const MSVehicle*>(*i);
810  assert(foe != 0);
811  const ApproachingVehicleInformation& avi = foeLink->getApproaching(foe);
812  if ( // ignore vehicles that arrive after us (unless they are ahead and we could easily brake for them)
813  ((avi.arrivalTime > arrivalTime) && !couldBrakeForLeader(dist, avi.dist, ego, foe)) ||
814  // also ignore vehicles that are behind us and are able to brake for us
815  couldBrakeForLeader(avi.dist, dist, foe, ego) ||
816  // resolve ties by lane index
817  (avi.arrivalTime == arrivalTime && avi.dist == dist && ego->getLane()->getIndex() < foe->getLane()->getIndex())) {
818  //if (gDebugFlag1) std::cout
819  // << " ignoring foe=" << foe->getID()
820  // << " foeAT=" << avi.arrivalTime
821  // << " foeDist=" << avi.dist
822  // << " foeSpeed=" << foe->getSpeed()
823  // << " egoSpeed=" << ego->getSpeed()
824  // << " deltaDist=" << avi.dist - dist
825  // << " delteSpeed=" << foe->getSpeed() - foe->getCarFollowModel().getMaxDecel() - ego->getSpeed()
826  // << "\n";
827  continue;
828  }
829  const double gap = dist - foe->getVehicleType().getLength() - ego->getVehicleType().getMinGap() - avi.dist;
830  const double follow = ego->getCarFollowModel().followSpeed(
831  ego, ego->getSpeed(), gap, foe->getSpeed(), foe->getCarFollowModel().getMaxDecel());
832  // speed adaption to follow the foe can be spread over secondsToArrival
833  const double followInTime = vSafeOrig + (follow - vSafeOrig) / MAX2((double)1, secondsToArrival / TS);
834  vSafe = MIN2(vSafe, followInTime);
835  //if (gDebugFlag1) std::cout << " adapting to foe=" << foe->getID()
836  // << " foeDist=" << avi.dist
837  // << " follow=" << follow
838  // << " followInTime=" << followInTime
839  // << " gap=" << gap
840  // << " foeSpeed=" << foe->getSpeed()
841  // << " follow=" << follow
842  // << " foeAT=" << avi.arrivalTime
843  // << " foeLT=" << avi.leavingTime
844  // << " foeAS=" << avi.arrivalSpeed
845  // << " vSafe=" << vSafe
846  // << "\n";
847  }
848  return vSafe;
849 }
850 
851 
852 bool
853 MSLink::couldBrakeForLeader(double followDist, double leaderDist, const MSVehicle* follow, const MSVehicle* leader) {
854  return (// leader is ahead of follower
855  followDist > leaderDist &&
856  // and follower could brake for 1 s to stay behind leader
857  followDist - leaderDist > follow->getSpeed() - follow->getCarFollowModel().getMaxDecel() - leader->getSpeed());
858 }
859 
860 
861 void
865 }
866 
867 /****************************************************************************/
868 
const std::vector< IncomingLaneInfo > & getIncomingLanes() const
Definition: MSLane.h:719
bool gDebugFlag1
global utility flags for debugging
Definition: StdDefs.cpp:33
static double gLateralResolution
Definition: MSGlobals.h:92
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:1979
MSEdge & getEdge() const
Returns the lane&#39;s edge.
Definition: MSLane.h:582
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:83
SUMOTime getWaitingTime() const
Returns the SUMOTime waited (speed was lesser than 0.1m/s)
Definition: MSVehicle.h:536
MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:488
virtual double followSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel) const =0
Computes the vehicle&#39;s follow speed (no dawdling)
This is an uncontrolled, minor link, has to stop.
The base class for an intersection.
Definition: MSJunction.h:64
std::string time2string(SUMOTime t)
Definition: SUMOTime.cpp:60
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:158
T MAX2(T a, T b)
Definition: StdDefs.h:70
double getLength() const
Returns the lane&#39;s length.
Definition: MSLane.h:484
const PositionVector & getShape() const
Returns this lane&#39;s shape.
Definition: MSLane.h:426
const std::string & getID() const
Returns the id.
Definition: Named.h:66
#define TIME2STEPS(x)
Definition: SUMOTime.h:66
#define TS
Definition: SUMOTime.h:52
This is an uncontrolled, all-way stop link.
double getWidth() const
Returns the lane&#39;s width.
Definition: MSLane.h:500
This is an uncontrolled, zipper-merge link.
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:2921
bool isFrontOnLane(const MSLane *lane) const
Returns the information whether the front of the vehicle is on the given lane.
Definition: MSVehicle.cpp:2465
LinkDirection
The different directions a link between two lanes may take (or a stream between two edges)...
bool isInternal() const
Definition: MSLane.cpp:1497
#define SUMOTime_MIN
Definition: SUMOTime.h:45
A road/street connecting two junctions.
Definition: MSEdge.h:80
const MSCFModel & getCarFollowModel() const
Returns the vehicle&#39;s car following model definition.
Definition: MSVehicle.h:796
int getIndex() const
Returns the lane&#39;s index.
Definition: MSLane.h:507
const MSCFModel & getCarFollowModel() const
Returns the vehicle type&#39;s car following model definition (const version)
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:56
void passedJunction(const MSVehicle *vehicle)
erase vehicle from myLinkLeaders
Definition: MSJunction.cpp:73
Representation of a vehicle.
Definition: SUMOVehicle.h:67
static MSPModel * getModel()
Definition: MSPModel.cpp:63
double interpolateGeometryPosToLanePos(double geometryPos) const
Definition: MSLane.h:454
#define STEPS2TIME(x)
Definition: SUMOTime.h:65
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:257
T MIN2(T a, T b)
Definition: StdDefs.h:64
AnyVehicleIterator anyVehiclesEnd() const
end iterator for iterating over all vehicles touching this lane in downstream direction ...
Definition: MSLane.h:390
AnyVehicleIterator anyVehiclesBegin() const
begin iterator for iterating over all vehicles touching this lane in downstream direction ...
Definition: MSLane.h:385
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:76
double getMinGap() const
Get the free space in front of vehicles of this class.
double getMaxDecel() const
Get the vehicle type&#39;s maximum deceleration [m/s^2].
Definition: MSCFModel.h:201
EdgeBasicFunction getPurpose() const
Returns the edge type (EdgeBasicFunction)
Definition: MSEdge.h:249
bool isInternal() const
return whether this edge is an internal edge
Definition: MSEdge.h:254
const MSVehicleType & getVehicleType() const
Returns the vehicle&#39;s type definition.
The edge is a normal street.
Definition: MSEdge.h:93
const MSJunction * getFromJunction() const
Definition: MSEdge.h:368
MSVehicle * getLastAnyVehicle() const
returns the last vehicle that is fully or partially on this lane
Definition: MSLane.cpp:1520
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:80
double getBackPositionOnLane(const MSLane *lane) const
Get the vehicle&#39;s position relative to the given lane.
Definition: MSVehicle.cpp:2403
The parent class for traffic light logics.
Static storage of an output device and its base (abstract) implementation.
Definition: OutputDevice.h:71
bool closeTag()
Closes the most recently opened tag.
static const double SAFETY_GAP
Definition: MSPModel.h:81
long long int SUMOTime
Definition: TraCIDefs.h:52
#define NUMERICAL_EPS
Definition: config.h:151
const MSLinkCont & getLinkCont() const
returns the container with all links !!!
Definition: MSLane.cpp:1597
double getSpeed() const
Returns the vehicle&#39;s current speed.
Definition: MSVehicle.h:442
The edge is an internal edge.
Definition: MSEdge.h:97
const std::string & getID() const
Returns the name of the vehicle.
static bool gUseMesoSim
Definition: MSGlobals.h:98
Representation of a lane in the micro simulation.
Definition: MSLane.h:79
bool isLeader(const MSVehicle *ego, const MSVehicle *foe)
Definition: MSJunction.cpp:79
OutputDevice & openTag(const std::string &xmlElement)
Opens an XML tag.
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:1685
virtual const MSVehicleType & getVehicleType() const =0
Returns the vehicle&#39;s type.