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