SUMO - Simulation of Urban MObility
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
MSVehicle.cpp
Go to the documentation of this file.
1 /****************************************************************************/
17 // Representation of a vehicle in the micro simulation
18 /****************************************************************************/
19 // SUMO, Simulation of Urban MObility; see http://sumo-sim.org/
20 // Copyright (C) 2001-2014 DLR (http://www.dlr.de/) and contributors
21 /****************************************************************************/
22 //
23 // This file is part of SUMO.
24 // SUMO is free software: you can redistribute it and/or modify
25 // it under the terms of the GNU General Public License as published by
26 // the Free Software Foundation, either version 3 of the License, or
27 // (at your option) any later version.
28 //
29 /****************************************************************************/
30 
31 // ===========================================================================
32 // included modules
33 // ===========================================================================
34 #ifdef _MSC_VER
35 #include <windows_config.h>
36 #else
37 #include <config.h>
38 #endif
39 
40 #include <iostream>
41 #include <cassert>
42 #include <cmath>
43 #include <cstdlib>
44 #include <algorithm>
45 #include <map>
47 #include <utils/common/ToString.h>
54 #include <utils/common/StdDefs.h>
55 #include <utils/geom/GeomHelper.h>
56 #include <utils/geom/Line.h>
62 #include <microsim/MSGlobals.h>
63 #include "trigger/MSBusStop.h"
65 #include "MSEdgeWeightsStorage.h"
67 #include "MSMoveReminder.h"
68 #include "MSPerson.h"
69 #include "MSPersonControl.h"
70 #include "MSLane.h"
71 #include "MSVehicle.h"
72 #include "MSEdge.h"
73 #include "MSVehicleType.h"
74 #include "MSNet.h"
75 #include "MSRoute.h"
76 #include "MSLinkCont.h"
77 
78 #ifdef HAVE_INTERNAL
79 #include <mesosim/MESegment.h>
80 #include <mesosim/MELoop.h>
81 #include "MSGlobals.h"
82 #endif
83 
84 #ifdef CHECK_MEMORY_LEAKS
85 #include <foreign/nvwa/debug_new.h>
86 #endif // CHECK_MEMORY_LEAKS
87 
88 //#define DEBUG_VEHICLE_GUI_SELECTION 1
89 #ifdef DEBUG_VEHICLE_GUI_SELECTION
90 #undef ID_LIST
92 #include <guisim/GUIVehicle.h>
93 #include <guisim/GUILane.h>
94 #endif
95 
96 #define BUS_STOP_OFFSET 0.5
97 
98 #define CRLL_LOOK_AHEAD 5
99 
100 // @todo Calibrate with real-world values / make configurable
101 #define DIST_TO_STOPLINE_EXPECT_PRIORITY 1.0
102 
103 // ===========================================================================
104 // static value definitions
105 // ===========================================================================
106 std::vector<MSLane*> MSVehicle::myEmptyLaneVector;
107 
108 
109 // ===========================================================================
110 // method definitions
111 // ===========================================================================
112 /* -------------------------------------------------------------------------
113  * methods of MSVehicle::State
114  * ----------------------------------------------------------------------- */
116  myPos = state.myPos;
117  mySpeed = state.mySpeed;
118 }
119 
120 
123  myPos = state.myPos;
124  mySpeed = state.mySpeed;
125  return *this;
126 }
127 
128 
129 bool
131  return (myPos != state.myPos ||
132  mySpeed != state.mySpeed);
133 }
134 
135 
136 SUMOReal
138  return myPos;
139 }
140 
141 
143  myPos(pos), mySpeed(speed) {}
144 
145 
146 /* -------------------------------------------------------------------------
147  * methods of MSVehicle::Influencer
148  * ----------------------------------------------------------------------- */
149 #ifndef NO_TRACI
151  mySpeedAdaptationStarted(true),
152  myConsiderSafeVelocity(true),
153  myConsiderMaxAcceleration(true),
154  myConsiderMaxDeceleration(true),
155  myRespectJunctionPriority(true),
156  myEmergencyBrakeRedLight(true),
157  myAmVTDControlled(false),
158  myStrategicLC(LC_NOCONFLICT),
159  myCooperativeLC(LC_NOCONFLICT),
160  mySpeedGainLC(LC_NOCONFLICT),
161  myRightDriveLC(LC_NOCONFLICT),
162  myTraciLaneChangePriority(LCP_URGENT)
163 {}
164 
165 
167 
168 
169 void
170 MSVehicle::Influencer::setSpeedTimeLine(const std::vector<std::pair<SUMOTime, SUMOReal> >& speedTimeLine) {
171  mySpeedAdaptationStarted = true;
172  mySpeedTimeLine = speedTimeLine;
173 }
174 
175 
176 void
177 MSVehicle::Influencer::setLaneTimeLine(const std::vector<std::pair<SUMOTime, unsigned int> >& laneTimeLine) {
178  myLaneTimeLine = laneTimeLine;
179 }
180 
181 
182 SUMOReal
184  // keep original speed
185  myOriginalSpeed = speed;
186  // remove leading commands which are no longer valid
187  while (mySpeedTimeLine.size() == 1 || (mySpeedTimeLine.size() > 1 && currentTime > mySpeedTimeLine[1].first)) {
188  mySpeedTimeLine.erase(mySpeedTimeLine.begin());
189  }
190  // do nothing if the time line does not apply for the current time
191  if (mySpeedTimeLine.size() < 2 || currentTime < mySpeedTimeLine[0].first) {
192  return speed;
193  }
194  // compute and set new speed
195  if (!mySpeedAdaptationStarted) {
196  mySpeedTimeLine[0].second = speed;
197  mySpeedAdaptationStarted = true;
198  }
199  currentTime += DELTA_T;
200  const SUMOReal td = STEPS2TIME(currentTime - mySpeedTimeLine[0].first) / STEPS2TIME(mySpeedTimeLine[1].first + DELTA_T - mySpeedTimeLine[0].first);
201  speed = mySpeedTimeLine[0].second - (mySpeedTimeLine[0].second - mySpeedTimeLine[1].second) * td;
202  if (myConsiderSafeVelocity) {
203  speed = MIN2(speed, vSafe);
204  }
205  if (myConsiderMaxAcceleration) {
206  speed = MIN2(speed, vMax);
207  }
208  if (myConsiderMaxDeceleration) {
209  speed = MAX2(speed, vMin);
210  }
211  return speed;
212 }
213 
214 
215 int
216 MSVehicle::Influencer::influenceChangeDecision(const SUMOTime currentTime, const MSEdge& currentEdge, const unsigned int currentLaneIndex, int state) {
217  // remove leading commands which are no longer valid
218  while (myLaneTimeLine.size() == 1 || (myLaneTimeLine.size() > 1 && currentTime > myLaneTimeLine[1].first)) {
219  myLaneTimeLine.erase(myLaneTimeLine.begin());
220  }
221  ChangeRequest changeRequest = REQUEST_NONE;
222  // do nothing if the time line does not apply for the current time
223  if (myLaneTimeLine.size() >= 2 && currentTime >= myLaneTimeLine[0].first) {
224  const unsigned int destinationLaneIndex = myLaneTimeLine[1].second;
225  if (destinationLaneIndex < (unsigned int)currentEdge.getLanes().size()) {
226  if (currentLaneIndex > destinationLaneIndex) {
227  changeRequest = REQUEST_RIGHT;
228  } else if (currentLaneIndex < destinationLaneIndex) {
229  changeRequest = REQUEST_LEFT;
230  } else {
231  changeRequest = REQUEST_HOLD;
232  }
233  }
234  }
235  // check whether the current reason shall be canceled / overridden
236  if ((state & LCA_WANTS_LANECHANGE_OR_STAY) != 0) {
237  // flags for the current reason
238  LaneChangeMode mode = LC_NEVER;
239  if ((state & LCA_STRATEGIC) != 0) {
240  mode = myStrategicLC;
241  } else if ((state & LCA_COOPERATIVE) != 0) {
242  mode = myCooperativeLC;
243  } else if ((state & LCA_SPEEDGAIN) != 0) {
244  mode = mySpeedGainLC;
245  } else if ((state & LCA_KEEPRIGHT) != 0) {
246  mode = myRightDriveLC;
247  } else if ((state & LCA_TRACI) != 0) {
248  mode = LC_NEVER;
249  } else {
250  WRITE_WARNING("Lane change model did not provide a reason for changing (state=" + toString(state) + ", time=" + time2string(currentTime) + "\n");
251  }
252  if (mode == LC_NEVER) {
253  // cancel all lcModel requests
254  state &= ~LCA_WANTS_LANECHANGE_OR_STAY;
255  state &= ~LCA_URGENT;
256  } else if (mode == LC_NOCONFLICT && changeRequest != REQUEST_NONE) {
257  if (
258  ((state & LCA_LEFT) != 0 && changeRequest != REQUEST_LEFT) ||
259  ((state & LCA_RIGHT) != 0 && changeRequest != REQUEST_RIGHT) ||
260  ((state & LCA_STAY) != 0 && changeRequest != REQUEST_HOLD)) {
261  // cancel conflicting lcModel request
262  state &= ~LCA_WANTS_LANECHANGE_OR_STAY;
263  state &= ~LCA_URGENT;
264  }
265  } else if (mode == LC_ALWAYS) {
266  // ignore any TraCI requests
267  return state;
268  }
269  }
270  // apply traci requests
271  if (changeRequest == REQUEST_NONE) {
272  return state;
273  } else {
274  state |= LCA_TRACI;
275  // security checks
276  if ((myTraciLaneChangePriority == LCP_ALWAYS)
277  || (myTraciLaneChangePriority == LCP_NOOVERLAP && (state & LCA_OVERLAPPING) == 0)) {
278  state &= ~(LCA_BLOCKED | LCA_OVERLAPPING);
279  }
280  if (changeRequest != REQUEST_HOLD && myTraciLaneChangePriority != LCP_OPPORTUNISTIC) {
281  state |= LCA_URGENT;
282  }
283  switch (changeRequest) {
284  case REQUEST_HOLD:
285  return state | LCA_STAY;
286  case REQUEST_LEFT:
287  return state | LCA_LEFT;
288  case REQUEST_RIGHT:
289  return state | LCA_RIGHT;
290  default:
291  throw ProcessError("should not happen");
292  }
293  }
294 }
295 
296 
297 SUMOReal
299  assert(myLaneTimeLine.size() >= 2);
300  assert(currentTime >= myLaneTimeLine[0].first);
301  return STEPS2TIME(myLaneTimeLine[1].first - currentTime);
302 }
303 
304 
305 void
307  myConsiderSafeVelocity = value;
308 }
309 
310 
311 void
313  myConsiderMaxAcceleration = value;
314 }
315 
316 
317 void
319  myConsiderMaxDeceleration = value;
320 }
321 
322 
323 void
325  myRespectJunctionPriority = value;
326 }
327 
328 
329 void
331  myEmergencyBrakeRedLight = value;
332 }
333 
334 
335 void
337  myStrategicLC = (LaneChangeMode)(value & (1 + 2));
338  myCooperativeLC = (LaneChangeMode)((value & (4 + 8)) >> 2);
339  mySpeedGainLC = (LaneChangeMode)((value & (16 + 32)) >> 4);
340  myRightDriveLC = (LaneChangeMode)((value & (64 + 128)) >> 6);
341  myTraciLaneChangePriority = (TraciLaneChangePriority)((value & (256 + 512)) >> 8);
342 }
343 
344 
345 void
349  if (myVTDRoute.size() != 0) {
350  v->replaceRouteEdges(myVTDRoute, true);
351  }
352  v->myCurrEdge += myVTDEdgeOffset;
353  if (myVTDPos > myVTDLane->getLength()) {
354  myVTDPos = myVTDLane->getLength();
355  }
356  myVTDLane->forceVehicleInsertion(v, myVTDPos);
357  v->updateBestLanes();
358  myAmVTDControlled = false;
359 }
360 
361 #endif
362 
363 
364 /* -------------------------------------------------------------------------
365  * MSVehicle-methods
366  * ----------------------------------------------------------------------- */
368  delete myLaneChangeModel;
369  // other
370  delete myEdgeWeights;
371  for (std::vector<MSLane*>::iterator i = myFurtherLanes.begin(); i != myFurtherLanes.end(); ++i) {
372  (*i)->resetPartialOccupation(this);
373  }
374  myFurtherLanes.clear();
375  for (DriveItemVector::iterator i = myLFLinkLanes.begin(); i != myLFLinkLanes.end(); ++i) {
376  if ((*i).myLink != 0) {
377  (*i).myLink->removeApproaching(this);
378  }
379  }
380  //
381  if (myType->amVehicleSpecific()) {
382  delete myType;
383  }
384 #ifndef NO_TRACI
385  delete myInfluencer;
386 #endif
387 }
388 
389 
391  const MSRoute* route,
392  const MSVehicleType* type,
393  SUMOReal speedFactor) :
394  MSBaseVehicle(pars, route, type, speedFactor),
395  myWaitingTime(0),
396  myState(0, 0), //
397  myLane(0),
400  myPersonDevice(0),
401  myAcceleration(0),
402  mySignals(0),
403  myAmOnNet(false),
405  myHaveToWaitOnNextLink(false),
406  myCachedPosition(Position::INVALID),
407  myEdgeWeights(0)
408 #ifndef NO_TRACI
409  , myInfluencer(0)
410 #endif
411 {
412  for (std::vector<SUMOVehicleParameter::Stop>::iterator i = pars->stops.begin(); i != pars->stops.end(); ++i) {
413  if (!addStop(*i)) {
414  throw ProcessError("Stop for vehicle '" + pars->id +
415  "' on lane '" + i->lane + "' is too close or not downstream the current route.");
416  }
417  }
418  for (std::vector<SUMOVehicleParameter::Stop>::const_iterator i = route->getStops().begin(); i != route->getStops().end(); ++i) {
419  if (!addStop(*i)) {
420  throw ProcessError("Stop for vehicle '" + pars->id +
421  "' on lane '" + i->lane + "' is too close or not downstream the current route.");
422  }
423  }
424  updateBestLanes(true, (*myCurrEdge)->getLanes()[0]); // must be called before getBestLanes in getDepartLane
425  const MSLane* const depLane = (*myCurrEdge)->getDepartLane(*this);
426  if (depLane == 0) {
427  throw ProcessError("Invalid departlane definition for vehicle '" + pars->id + "'.");
428  }
429  if (pars->departSpeedProcedure == DEPART_SPEED_GIVEN && pars->departSpeed > depLane->getVehicleMaxSpeed(this)) {
430  if (type->getSpeedDeviation() > 0 && pars->departSpeed <= type->getSpeedFactor() * depLane->getSpeedLimit() * (2 * type->getSpeedDeviation() + 1.)) {
431  WRITE_WARNING("Choosing new speed factor for vehicle '" + pars->id + "' to match departure speed.");
433  } else {
434  throw ProcessError("Departure speed for vehicle '" + pars->id +
435  "' is too high for the departure lane '" + depLane->getID() + "'.");
436  }
437  }
438  if (pars->departSpeedProcedure == DEPART_SPEED_GIVEN && pars->departSpeed > type->getMaxSpeed()) {
439  throw ProcessError("Departure speed for vehicle '" + pars->id +
440  "' is too high for the vehicle type '" + type->getID() + "'.");
441  }
444 }
445 
446 
447 void
451  for (DriveItemVector::iterator i = myLFLinkLanes.begin(); i != myLFLinkLanes.end(); ++i) {
452  if ((*i).myLink != 0) {
453  (*i).myLink->removeApproaching(this);
454  }
455  }
456  leaveLane(reason);
457 }
458 
459 
460 // ------------ interaction with the route
461 bool
463  return myCurrEdge == myRoute->end() - 1 && (myStops.empty() || myStops.front().edge != myCurrEdge)
465 }
466 
467 
468 bool
469 MSVehicle::replaceRoute(const MSRoute* newRoute, bool onInit, int offset) {
470  const MSEdgeVector& edges = newRoute->getEdges();
471  // assert the vehicle may continue (must not be "teleported" or whatever to another position)
472  if (!onInit && !newRoute->contains(*myCurrEdge)) {
473  return false;
474  }
475 
476  // rebuild in-vehicle route information
477  if (onInit) {
478  myCurrEdge = newRoute->begin();
479  } else {
480  myCurrEdge = find(edges.begin() + offset, edges.end(), *myCurrEdge);
481  }
482  // check whether the old route may be deleted (is not used by anyone else)
483  newRoute->addReference();
484  myRoute->release();
485  // assign new route
486  myRoute = newRoute;
489  if (!onInit) {
490  updateBestLanes(true);
491  }
492  // update arrival definition
494  // save information that the vehicle was rerouted
497  // recheck old stops
498  for (std::list<Stop>::iterator iter = myStops.begin(); iter != myStops.end();) {
499  if (find(myCurrEdge, edges.end(), &iter->lane->getEdge()) == edges.end()) {
500  iter = myStops.erase(iter);
501  } else {
502  iter->edge = find(myCurrEdge, edges.end(), &iter->lane->getEdge());
503  ++iter;
504  }
505  }
506  // add new stops
507  for (std::vector<SUMOVehicleParameter::Stop>::const_iterator i = newRoute->getStops().begin(); i != newRoute->getStops().end(); ++i) {
508  if (!addStop(*i)) {
509  WRITE_WARNING("Stop for vehicle '" + getID() +
510  "' on lane '" + i->lane + "' is too close or not downstream the new route.");
511  }
512  }
513  return true;
514 }
515 
516 
517 bool
518 MSVehicle::willPass(const MSEdge* const edge) const {
519  return find(myCurrEdge, myRoute->end(), edge) != myRoute->end();
520 }
521 
522 
523 unsigned int
525  return (unsigned int) std::distance(myRoute->begin(), myCurrEdge);
526 }
527 
528 
529 void
530 MSVehicle::resetRoutePosition(unsigned int index) {
531  myCurrEdge = myRoute->begin() + index;
532  // !!! hack
533  myArrivalPos = (*(myRoute->end() - 1))->getLanes()[0]->getLength();
534 }
535 
536 
537 
540  return _getWeightsStorage();
541 }
542 
543 
546  return _getWeightsStorage();
547 }
548 
549 
552  if (myEdgeWeights == 0) {
554  }
555  return *myEdgeWeights;
556 }
557 
558 
559 // ------------ Interaction with move reminders
560 void
562  // This erasure-idiom works for all stl-sequence-containers
563  // See Meyers: Effective STL, Item 9
564  for (MoveReminderCont::iterator rem = myMoveReminders.begin(); rem != myMoveReminders.end();) {
565  if (!rem->first->notifyMove(*this, oldPos + rem->second, newPos + rem->second, newSpeed)) {
566 #ifdef _DEBUG
567  if (myTraceMoveReminders) {
568  traceMoveReminder("notifyMove", rem->first, rem->second, false);
569  }
570 #endif
571  rem = myMoveReminders.erase(rem);
572  } else {
573 #ifdef _DEBUG
574  if (myTraceMoveReminders) {
575  traceMoveReminder("notifyMove", rem->first, rem->second, true);
576  }
577 #endif
578  ++rem;
579  }
580  }
581 }
582 
583 
584 void
586  // save the old work reminders, patching the position information
587  // add the information about the new offset to the old lane reminders
588  const SUMOReal oldLaneLength = myLane->getLength();
589  for (MoveReminderCont::iterator rem = myMoveReminders.begin(); rem != myMoveReminders.end(); ++rem) {
590  rem->second += oldLaneLength;
591 #ifdef _DEBUG
592  if (myTraceMoveReminders) {
593  traceMoveReminder("adaptedPos", rem->first, rem->second, true);
594  }
595 #endif
596  }
597  for (std::vector< MSMoveReminder* >::const_iterator rem = enteredLane.getMoveReminders().begin(); rem != enteredLane.getMoveReminders().end(); ++rem) {
598  addReminder(*rem);
599  }
600 }
601 
602 
603 // ------------ Other getter methods
604 SUMOReal
606  if (myLane == 0) {
607  return 0;
608  }
609  const SUMOReal lp = getPositionOnLane();
611  return myLane->getShape().slopeDegreeAtOffset(gp);
612 }
613 
614 
615 Position
616 MSVehicle::getPosition(const SUMOReal offset) const {
617  if (myLane == 0) {
618  return Position::INVALID;
619  }
620  if (isParking()) {
621  PositionVector shp = myLane->getEdge().getLanes()[0]->getShape();
624  }
625  const bool changingLanes = getLaneChangeModel().isChangingLanes();
626  if (offset == 0. && !changingLanes) {
629  }
630  return myCachedPosition;
631  }
633  if (changingLanes) {
635  Line line = getLaneChangeModel().isLaneChangeMidpointPassed() ? Line(other, result) : Line(result, other);
636  return line.getPositionAtDistance(getLaneChangeModel().getLaneChangeCompletion() * line.length());
637  }
638  return result;
639 }
640 
641 
642 SUMOReal
644  Position p1;
645  Position p2;
646  if (getLaneChangeModel().isChangingLanes()) {
647  // cannot use getPosition() because it already includes the offset to the side and thus messes up the angle
649  } else {
650  p1 = getPosition();
651  }
653  // vehicle is fully on the new lane (visually)
655  } else {
656  p2 = myFurtherLanes.size() > 0
657  ? myFurtherLanes.back()->geometryPositionAtOffset(myFurtherLanes.back()->getPartialOccupatorEnd())
659  }
660  SUMOReal result = (p1 != p2 ?
661  atan2(p1.x() - p2.x(), p2.y() - p1.y()) * 180. / M_PI :
665  result += getLaneChangeModel().getLaneChangeDirection() * angleOffset;
666  }
667  return result;
668 }
669 
670 
671 // ------------
672 bool
674  Stop stop;
675  stop.lane = MSLane::dictionary(stopPar.lane);
676  stop.busstop = MSNet::getInstance()->getBusStop(stopPar.busstop);
677  stop.startPos = stopPar.startPos;
678  stop.endPos = stopPar.endPos;
679  stop.duration = stopPar.duration;
680  stop.until = stopPar.until;
681  stop.awaitedPersons = stopPar.awaitedPersons;
682  if (stop.until != -1) {
683  stop.until += untilOffset;
684  }
685  stop.triggered = stopPar.triggered;
686  stop.parking = stopPar.parking;
687  stop.reached = false;
688  if (stop.startPos < 0 || stop.endPos > stop.lane->getLength()) {
689  return false;
690  }
691  stop.edge = find(myCurrEdge, myRoute->end(), &stop.lane->getEdge());
692  MSRouteIterator prevStopEdge = myCurrEdge;
693  SUMOReal prevStopPos = myState.myPos;
694  // where to insert the stop
695  std::list<Stop>::iterator iter = myStops.begin();
696  if (stopPar.index == STOP_INDEX_END || stopPar.index >= static_cast<int>(myStops.size())) {
697  if (myStops.size() > 0) {
698  prevStopEdge = myStops.back().edge;
699  prevStopPos = myStops.back().endPos;
700  iter = myStops.end();
701  stop.edge = find(prevStopEdge, myRoute->end(), &stop.lane->getEdge());
702  if (prevStopEdge == stop.edge && prevStopPos > stop.endPos) {
703  stop.edge = find(prevStopEdge + 1, myRoute->end(), &stop.lane->getEdge());
704  }
705  }
706  } else {
707  if (stopPar.index == STOP_INDEX_FIT) {
708  while (iter != myStops.end() && (iter->edge < stop.edge ||
709  (iter->endPos < stop.endPos && iter->edge == stop.edge))) {
710  prevStopEdge = iter->edge;
711  prevStopPos = iter->endPos;
712  ++iter;
713  }
714  } else {
715  int index = stopPar.index;
716  while (index > 0) {
717  prevStopEdge = iter->edge;
718  prevStopPos = iter->endPos;
719  ++iter;
720  --index;
721  }
722  stop.edge = find(prevStopEdge, myRoute->end(), &stop.lane->getEdge());
723  }
724  }
725  if (stop.edge == myRoute->end() || prevStopEdge > stop.edge ||
726  (prevStopEdge == stop.edge && prevStopPos > stop.endPos)) {
727  return false;
728  }
729  // David.C:
730  //if (!stop.parking && (myCurrEdge == stop.edge && myState.myPos > stop.endPos - getCarFollowModel().brakeGap(myState.mySpeed))) {
732  return false;
733  }
734  if (!hasDeparted() && myCurrEdge == stop.edge) {
735  SUMOReal pos = -1;
737  pos = myParameter->departPos;
738  if (pos < 0.) {
739  pos += (*myCurrEdge)->getLength();
740  }
741  }
743  pos = MIN2(static_cast<SUMOReal>(getVehicleType().getLength() + POSITION_EPS), (*myCurrEdge)->getLength());
744  }
745  if (pos > stop.endPos) {
746  return false;
747  }
748  }
749  myStops.insert(iter, stop);
750  return true;
751 }
752 
753 
754 bool
756  return !myStops.empty() && myStops.begin()->reached;
757 }
758 
759 
760 bool
762  return isStopped() && myStops.begin()->parking;
763 }
764 
765 
766 bool
768  return isStopped() && myStops.begin()->triggered;
769 }
770 
771 
772 SUMOReal
774  if (myStops.empty()) {
775  // no stops; pass
776  return currentVelocity;
777  }
778  Stop& stop = myStops.front();
779  if (stop.reached) {
780  // ok, we have already reached the next stop
781  // any waiting persons may board now
782  bool boarded = MSNet::getInstance()->getPersonControl().boardAnyWaiting(&myLane->getEdge(), this);
783  boarded &= stop.awaitedPersons.size() == 0;
784  if (boarded) {
785  if (stop.busstop != 0) {
786  const std::vector<MSPerson*>& persons = myPersonDevice->getPersons();
787  for (std::vector<MSPerson*>::const_iterator i = persons.begin(); i != persons.end(); ++i) {
788  stop.busstop->removePerson(*i);
789  }
790  }
791  // the triggering condition has been fulfilled. Maybe we want to wait a bit longer for additional riders (car pooling)
792  stop.triggered = false;
796  }
797  }
798  if (stop.duration <= 0 && !stop.triggered) {
800  } else {
801  // we have to wait some more time
803  // we can only register after waiting for one step. otherwise we might falsely signal a deadlock
806  }
807  stop.duration -= DELTA_T;
808  return 0;
809  }
810  } else {
811  // is the next stop on the current lane?
812  if (stop.edge == myCurrEdge) {
813  // get the stopping position
814  SUMOReal endPos = stop.endPos;
815  bool busStopsMustHaveSpace = true;
816  if (stop.busstop != 0) {
817  // on bus stops, we have to wait for free place if they are in use...
818  endPos = stop.busstop->getLastFreePos(*this);
819  if (endPos - 5. < stop.busstop->getBeginLanePosition()) { // !!! explicit offset
820  busStopsMustHaveSpace = false;
821  }
822  }
823  if (myState.pos() + getVehicleType().getMinGap() >= endPos - BUS_STOP_OFFSET && busStopsMustHaveSpace
824  && myLane == stop.lane) {
825  // ok, we may stop (have reached the stop)
826  stop.reached = true;
827  MSNet::getInstance()->getVehicleControl().addWaiting(&myLane->getEdge(), this);
829  // compute stopping time
830  if (stop.until >= 0) {
831  if (stop.duration == -1) {
833  } else {
835  }
836  }
837  if (stop.busstop != 0) {
838  // let the bus stop know the vehicle
840  }
841  }
842  // decelerate
843  return getCarFollowModel().stopSpeed(this, getSpeed(), endPos - myState.pos());
844  }
845  }
846  return currentVelocity;
847 }
848 
849 
850 void
851 MSVehicle::planMove(const SUMOTime t, const MSVehicle* pred, const SUMOReal lengthsInFront) {
853  checkRewindLinkLanes(lengthsInFront, myLFLinkLanes);
855 }
856 
857 
858 void
859 MSVehicle::planMoveInternal(const SUMOTime t, const MSVehicle* pred, DriveItemVector& lfLinks) const {
860 #ifdef DEBUG_VEHICLE_GUI_SELECTION
861  if (gSelected.isSelected(GLO_VEHICLE, static_cast<const GUIVehicle*>(this)->getGlID())) {
862  int bla = 0;
863  }
864 #endif
865  // remove information about approaching links, will be reset later in this step
866  for (DriveItemVector::iterator i = lfLinks.begin(); i != lfLinks.end(); ++i) {
867  if ((*i).myLink != 0) {
868  (*i).myLink->removeApproaching(this);
869  }
870  }
871  lfLinks.clear();
872 #ifdef HAVE_INTERNAL_LANES
873  myLinkLeaders.clear();
874 #endif
875  //
876  const MSCFModel& cfModel = getCarFollowModel();
877  const SUMOReal vehicleLength = getVehicleType().getLength();
878  const SUMOReal maxV = cfModel.maxNextSpeed(myState.mySpeed, this);
879  SUMOReal laneMaxV = myLane->getVehicleMaxSpeed(this);
880  // vBeg is the initial maximum velocity of this vehicle in this step
881  SUMOReal v = MIN2(maxV, laneMaxV);
882 #ifndef NO_TRACI
883  if (myInfluencer != 0) {
884  const SUMOReal vMin = MAX2(SUMOReal(0), cfModel.getSpeedAfterMaxDecel(myState.mySpeed));
885  v = myInfluencer->influenceSpeed(MSNet::getInstance()->getCurrentTimeStep(), v, v, vMin, maxV);
886  // !!! recheck - why is it done, here?
887  if (myInfluencer->isVTDControlled()) {
888  return; // !!! temporary
889  }
890  }
891 #endif
892 
893  const SUMOReal dist = SPEED2DIST(maxV) + cfModel.brakeGap(maxV);
894  const std::vector<MSLane*>& bestLaneConts = getBestLanesContinuation();
895  assert(bestLaneConts.size() > 0);
896 #ifdef HAVE_INTERNAL_LANES
897  bool hadNonInternal = false;
898 #else
899  bool hadNonInternal = true;
900 #endif
901  SUMOReal seen = myLane->getLength() - myState.myPos; // the distance already "seen"; in the following always up to the end of the current "lane"
902  SUMOReal seenNonInternal = 0;
903  SUMOReal vLinkPass = MIN2(estimateSpeedAfterDistance(seen, v, getVehicleType().getCarFollowModel().getMaxAccel()), laneMaxV); // upper bound
904  unsigned int view = 0;
905  DriveProcessItem* lastLink = 0;
906  SUMOReal gap = 0;
907  if (pred != 0) {
908  if (pred == myLane->getPartialOccupator()) {
910  } else {
912  }
913  }
914  std::pair<const MSVehicle*, SUMOReal> leaderInfo = std::make_pair(pred, gap);
915  // iterator over subsequent lanes and fill lfLinks until stopping distance or stopped
916  const MSLane* lane = myLane;
917  while (true) {
918  // check leader on lane
919  // leader is given for the first edge only
920  adaptToLeader(leaderInfo, seen, lastLink, lane, v, vLinkPass);
921 
922  // process stops
923  if (!myStops.empty() && &myStops.begin()->lane->getEdge() == &lane->getEdge()) {
924  // we are approaching a stop on the edge; must not drive further
925  const Stop& stop = *myStops.begin();
926  const SUMOReal endPos = stop.busstop == 0 ? stop.endPos : stop.busstop->getLastFreePos(*this);
927  const SUMOReal stopDist = seen + endPos - lane->getLength();
928  const SUMOReal stopSpeed = cfModel.stopSpeed(this, getSpeed(), stopDist);
929  if (lastLink != 0) {
930  lastLink->adaptLeaveSpeed(stopSpeed);
931  }
932  v = MIN2(v, stopSpeed);
933  lfLinks.push_back(DriveProcessItem(v, stopDist));
934  break;
935  }
936 
937  // move to next lane
938  // get the next link used
939  MSLinkCont::const_iterator link = MSLane::succLinkSec(*this, view + 1, *lane, bestLaneConts);
940  // check whether the vehicle is on its final edge
941  if (myCurrEdge + view + 1 == myRoute->end()) {
943  myParameter->arrivalSpeed : laneMaxV);
944  // subtract the arrival speed from the remaining distance so we get one additional driving step with arrival speed
945  const SUMOReal distToArrival = seen + myArrivalPos - lane->getLength() - SPEED2DIST(arrivalSpeed);
946  const SUMOReal va = cfModel.freeSpeed(this, getSpeed(), distToArrival, arrivalSpeed);
947  v = MIN2(v, va);
948  if (lastLink != 0) {
949  lastLink->adaptLeaveSpeed(va);
950  }
951  lfLinks.push_back(DriveProcessItem(v, seen));
952  break;
953  }
954  // check whether the lane is a dead end
955  if (lane->isLinkEnd(link)) {
956  SUMOReal va = MIN2(cfModel.stopSpeed(this, getSpeed(), seen), laneMaxV);
957  if (lastLink != 0) {
958  lastLink->adaptLeaveSpeed(va);
959  }
960  v = MIN2(va, v);
961  lfLinks.push_back(DriveProcessItem(v, seen));
962  break;
963  }
964  const bool yellowOrRed = (*link)->getState() == LINKSTATE_TL_RED ||
965  (*link)->getState() == LINKSTATE_TL_YELLOW_MAJOR ||
966  (*link)->getState() == LINKSTATE_TL_YELLOW_MINOR;
967  // We distinguish 3 cases when determining the point at which a vehicle stops:
968  // - links that require stopping: here the vehicle needs to stop close to the stop line
969  // to ensure it gets onto the junction in the next step. Othwise the vehicle would 'forget'
970  // that it already stopped and need to stop again. This is necessary pending implementation of #999
971  // - red/yellow light: here the vehicle 'knows' that it will have priority eventually and does not need to stop on a precise spot
972  // - other types of minor links: the vehicle needs to stop as close to the junction as necessary
973  // to minimize the time window for passing the junction. If the
974  // vehicle 'decides' to accelerate and cannot enter the junction in
975  // the next step, new foes may appear and cause a collision (see #1096)
976  // - major links: stopping point is irrelevant
977  const SUMOReal laneStopOffset = yellowOrRed || (*link)->havePriority() ? DIST_TO_STOPLINE_EXPECT_PRIORITY : POSITION_EPS;
978  const SUMOReal stopDist = MAX2(SUMOReal(0), seen - laneStopOffset);
979  // check whether we need to slow down in order to finish a continuous lane change
980  if (getLaneChangeModel().isChangingLanes()) {
981  if ( // slow down to finish lane change before a turn lane
982  ((*link)->getDirection() == LINKDIR_LEFT || (*link)->getDirection() == LINKDIR_RIGHT) ||
983  // slow down to finish lane change before the shadow lane ends
985  (*link)->getViaLaneOrLane()->getParallelLane(-getLaneChangeModel().getLaneChangeDirection()) == 0)) {
986  const SUMOReal timeRemaining = STEPS2TIME((1 - getLaneChangeModel().getLaneChangeCompletion()) * MSGlobals::gLaneChangeDuration);
987  const SUMOReal va = seen / timeRemaining;
988  v = MIN2(va, v);
989  }
990  }
991 
992  const bool setRequest = v > 0; // even if red, if we cannot break we should issue a request
993  SUMOReal vLinkWait = MIN2(v, cfModel.stopSpeed(this, getSpeed(), stopDist));
994  if (yellowOrRed && seen >= cfModel.brakeGap(myState.mySpeed) - myState.mySpeed * cfModel.getHeadwayTime()) {
995  // the vehicle is able to brake in front of a yellow/red traffic light
996  lfLinks.push_back(DriveProcessItem(*link, vLinkWait, vLinkWait, false, t + TIME2STEPS(seen / MAX2(vLinkWait, NUMERICAL_EPS)), vLinkWait, 0, SUMOTime_MAX, stopDist));
997  //lfLinks.push_back(DriveProcessItem(0, vLinkWait, vLinkWait, false, 0, 0, stopDist));
998  break;
999  }
1000 
1001 #ifdef HAVE_INTERNAL_LANES
1002  // we want to pass the link but need to check for foes on internal lanes
1003  const MSLink::LinkLeaders linkLeaders = (*link)->getLeaderInfo(seen, getVehicleType().getMinGap());
1004  for (MSLink::LinkLeaders::const_iterator it = linkLeaders.begin(); it != linkLeaders.end(); ++it) {
1005  // the vehicle to enter the junction first has priority
1006  const MSVehicle* leader = (*it).vehAndGap.first;
1007  if (leader == 0) {
1008  // leader is a pedestrian. Passing 'this' as a dummy.
1009  //std::cout << SIMTIME << " veh=" << getID() << " is blocked on link to " << (*link)->getViaLaneOrLane()->getID() << " by pedestrian. dist=" << it->distToCrossing << "\n";
1010  adaptToLeader(std::make_pair(this, -1), seen, lastLink, lane, v, vLinkPass, it->distToCrossing);
1011  } else if (leader->myLinkLeaders.count(getID()) == 0) {
1012  // leader isn't already following us, now we follow it
1013  myLinkLeaders.insert(leader->getID());
1014  adaptToLeader(it->vehAndGap, seen, lastLink, lane, v, vLinkPass, it->distToCrossing);
1015  if (lastLink != 0) {
1016  // we are not yet on the junction with this linkLeader.
1017  // at least we can drive up to the previous link and stop there
1018  v = MAX2(v, lastLink->myVLinkWait);
1019  }
1020  }
1021  }
1022  // if this is the link between two internal lanes we may have to slow down for pedestrians
1023  vLinkWait = MIN2(vLinkWait, v);
1024 #endif
1025 
1026  if (lastLink != 0) {
1027  lastLink->adaptLeaveSpeed(laneMaxV);
1028  }
1029  SUMOReal arrivalSpeed = vLinkPass;
1030  SUMOTime arrivalTime;
1031  // vehicles should decelerate when approaching a minor link
1032  if (!(*link)->havePriority() && stopDist > cfModel.getMaxDecel()) {
1033  // vehicle decelerates just enough to be able to stop if necessary and then accelerates
1034  arrivalSpeed = cfModel.getMaxDecel() + cfModel.getMaxAccel();
1035  const SUMOReal v1 = MAX2(vLinkWait, arrivalSpeed);
1036  // now + time spent decelerating + time spent at full speed
1037  arrivalTime = t + TIME2STEPS((v1 - arrivalSpeed) / cfModel.getMaxDecel()
1038  + (seen - (v1 * v1 - arrivalSpeed * arrivalSpeed) * 0.5 / cfModel.getMaxDecel()) / MAX2(vLinkWait, NUMERICAL_EPS));
1039  } else {
1040  const SUMOReal accel = (vLinkPass >= v) ? cfModel.getMaxAccel() : -cfModel.getMaxDecel();
1041  const SUMOReal accelTime = (vLinkPass - v) / accel;
1042  const SUMOReal accelWay = accelTime * (vLinkPass + v) * 0.5;
1043  arrivalTime = t + TIME2STEPS(accelTime + MAX2(SUMOReal(0), seen - accelWay) / MAX2(vLinkPass, NUMERICAL_EPS));
1044  }
1045  // compute speed, time if vehicle starts braking now
1046  // if stopping is possible, arrivalTime can be arbitrarily large. A small value keeps fractional times (impatience) meaningful
1047  SUMOReal arrivalSpeedBraking = 0;
1048  SUMOTime arrivalTimeBraking = arrivalTime + TIME2STEPS(30);
1049  if (seen < cfModel.brakeGap(v)) {
1050  // vehicle cannot come to a complete stop in time
1051  // Because we use a continuous formula for computiing the possible slow-down
1052  // we need to handle the mismatch with the discrete dynamics
1053  if (seen < v) {
1054  arrivalSpeedBraking = arrivalSpeed; // no time left for braking after this step
1055  } else if (2 * (seen - v * cfModel.getHeadwayTime()) * -cfModel.getMaxDecel() + v * v >= 0) {
1056  arrivalSpeedBraking = estimateSpeedAfterDistance(seen - v * cfModel.getHeadwayTime(), v, -cfModel.getMaxDecel());
1057  } else {
1058  arrivalSpeedBraking = cfModel.getMaxDecel();
1059  }
1060  // due to discrecte/continuous mismatch we have to ensure that braking actually helps
1061  arrivalSpeedBraking = MIN2(arrivalSpeedBraking, arrivalSpeed);
1062  arrivalTimeBraking = MAX2(arrivalTime, t + TIME2STEPS(seen / ((v + arrivalSpeedBraking) * 0.5)));
1063  }
1064  lfLinks.push_back(DriveProcessItem(*link, v, vLinkWait, setRequest,
1065  arrivalTime, arrivalSpeed,
1066  arrivalTimeBraking, arrivalSpeedBraking,
1067  stopDist,
1068  estimateLeaveSpeed(*link, vLinkPass)));
1069 #ifdef HAVE_INTERNAL_LANES
1070  if ((*link)->getViaLane() == 0) {
1071  hadNonInternal = true;
1072  ++view;
1073  }
1074 #else
1075  ++view;
1076 #endif
1077  // we need to look ahead far enough to see available space for checkRewindLinkLanes
1078  if (!setRequest || ((v <= 0 || seen > dist) && hadNonInternal && seenNonInternal > vehicleLength * CRLL_LOOK_AHEAD)) {
1079  break;
1080  }
1081  // get the following lane
1082  lane = (*link)->getViaLaneOrLane();
1083  laneMaxV = lane->getVehicleMaxSpeed(this);
1084  // the link was passed
1085  // compute the velocity to use when the link is not blocked by other vehicles
1086  // the vehicle shall be not faster when reaching the next lane than allowed
1087  const SUMOReal va = MAX2(laneMaxV, cfModel.freeSpeed(this, getSpeed(), seen, laneMaxV));
1088  v = MIN2(va, v);
1089  seenNonInternal += lane->getEdge().getPurpose() == MSEdge::EDGEFUNCTION_INTERNAL ? 0 : lane->getLength();
1090  seen += lane->getLength();
1091  leaderInfo = lane->getLastVehicleInformation();
1092  leaderInfo.second = leaderInfo.second + seen - lane->getLength() - getVehicleType().getMinGap();
1093  vLinkPass = MIN2(estimateSpeedAfterDistance(lane->getLength(), v, getVehicleType().getCarFollowModel().getMaxAccel()), laneMaxV); // upper bound
1094  lastLink = &lfLinks.back();
1095  }
1096 
1097 }
1098 
1099 
1100 void
1101 MSVehicle::adaptToLeader(const std::pair<const MSVehicle*, SUMOReal> leaderInfo,
1102  const SUMOReal seen, DriveProcessItem* const lastLink,
1103  const MSLane* const lane, SUMOReal& v, SUMOReal& vLinkPass,
1104  SUMOReal distToCrossing) const {
1105  if (leaderInfo.first != 0) {
1106  const MSCFModel& cfModel = getCarFollowModel();
1107  SUMOReal vsafeLeader = 0;
1108  if (leaderInfo.second >= 0) {
1109  vsafeLeader = cfModel.followSpeed(this, getSpeed(), leaderInfo.second, leaderInfo.first->getSpeed(), leaderInfo.first->getCarFollowModel().getMaxDecel());
1110  } else {
1111  // the leading, in-lapping vehicle is occupying the complete next lane
1112  // stop before entering this lane
1113  vsafeLeader = cfModel.stopSpeed(this, getSpeed(), seen - lane->getLength() - POSITION_EPS);
1114  }
1115  if (distToCrossing >= 0) {
1116  // drive up to the crossing point with the current link leader
1117  vsafeLeader = MAX2(vsafeLeader, cfModel.stopSpeed(this, getSpeed(), distToCrossing));
1118  }
1119  if (lastLink != 0) {
1120  lastLink->adaptLeaveSpeed(vsafeLeader);
1121  }
1122  v = MIN2(v, vsafeLeader);
1123  vLinkPass = MIN2(vLinkPass, vsafeLeader);
1124  }
1125 }
1126 
1127 
1128 bool
1130 #ifdef DEBUG_VEHICLE_GUI_SELECTION
1131  if (gSelected.isSelected(GLO_VEHICLE, static_cast<const GUIVehicle*>(this)->getGlID())) {
1132  int bla = 0;
1133  }
1134 #endif
1135  // get safe velocities from DriveProcessItems
1136  SUMOReal vSafe = 0; // maximum safe velocity
1137  SUMOReal vSafeMin = 0; // minimum safe velocity
1138  // the distance to a link which should either be crossed this step or in
1139  // front of which we need to stop
1140  SUMOReal vSafeMinDist = 0;
1141  myHaveToWaitOnNextLink = false;
1142 #ifndef NO_TRACI
1143  if (myInfluencer != 0) {
1144  if (myInfluencer->isVTDControlled()) {
1145  return false;
1146  }
1147  }
1148 #endif
1149 
1150  assert(myLFLinkLanes.size() != 0 || (myInfluencer != 0 && myInfluencer->isVTDControlled()));
1151  DriveItemVector::iterator i;
1152  for (i = myLFLinkLanes.begin(); i != myLFLinkLanes.end(); ++i) {
1153  MSLink* link = (*i).myLink;
1154  // the vehicle must change the lane on one of the next lanes
1155  if (link != 0 && (*i).mySetRequest) {
1156  const LinkState ls = link->getState();
1157  // vehicles should brake when running onto a yellow light if the distance allows to halt in front
1158  const bool yellow = ls == LINKSTATE_TL_YELLOW_MAJOR || ls == LINKSTATE_TL_YELLOW_MINOR;
1160  if (yellow && ((*i).myDistance > brakeGap || myState.mySpeed < ACCEL2SPEED(getCarFollowModel().getMaxDecel()))) {
1161  vSafe = (*i).myVLinkWait;
1162  myHaveToWaitOnNextLink = true;
1163  link->removeApproaching(this);
1164  break;
1165  }
1166  //
1167 #ifdef NO_TRACI
1168  const bool influencerPrio = false;
1169 #else
1170  const bool influencerPrio = (myInfluencer != 0 && !myInfluencer->getRespectJunctionPriority());
1171 #endif
1172  const bool opened = yellow || influencerPrio ||
1173  link->opened((*i).myArrivalTime, (*i).myArrivalSpeed, (*i).getLeaveSpeed(),
1176  // vehicles should decelerate when approaching a minor link
1177  if (opened && !influencerPrio && !link->havePriority() && !link->lastWasContMajor()) {
1178  if ((*i).myDistance > getCarFollowModel().getMaxDecel()) {
1179  vSafe = (*i).myVLinkWait;
1180  myHaveToWaitOnNextLink = true;
1181  if (ls == LINKSTATE_EQUAL) {
1182  link->removeApproaching(this);
1183  }
1184  break; // could be revalidated
1185  } else {
1186  // past the point of no return. we need to drive fast enough
1187  // to make it across the link. However, minor slowdowns
1188  // should be permissible to follow leading traffic safely
1189  // There is a problem in subsecond simulation: If we cannot
1190  // make it across the minor link in one step, new traffic
1191  // could appear on a major foe link and cause a collision
1192  vSafeMin = MIN2((SUMOReal) DIST2SPEED(myLane->getLength() - getPositionOnLane() + POSITION_EPS), (*i).myVLinkPass);
1193  vSafeMinDist = myLane->getLength() - getPositionOnLane();
1194  }
1195  }
1196  // have waited; may pass if opened...
1197  if (opened) {
1198  vSafe = (*i).myVLinkPass;
1199  if (vSafe < getCarFollowModel().getMaxDecel() && vSafe <= (*i).myVLinkWait && vSafe < getCarFollowModel().maxNextSpeed(getSpeed(), this)) {
1200  // this vehicle is probably not gonna drive accross the next junction (heuristic)
1201  myHaveToWaitOnNextLink = true;
1202  }
1203  } else {
1204  vSafe = (*i).myVLinkWait;
1205  myHaveToWaitOnNextLink = true;
1206  if (ls == LINKSTATE_EQUAL) {
1207  link->removeApproaching(this);
1208  }
1209  break;
1210  }
1211  } else {
1212  vSafe = (*i).myVLinkWait;
1213  if (vSafe < getSpeed()) {
1214  myHaveToWaitOnNextLink = true;
1215  }
1216  break;
1217  }
1218  }
1219  if (vSafe + NUMERICAL_EPS < vSafeMin) {
1220  // cannot drive across a link so we need to stop before it
1221  vSafe = MIN2(vSafe, getCarFollowModel().stopSpeed(this, getSpeed(), vSafeMinDist));
1222  vSafeMin = 0;
1223  myHaveToWaitOnNextLink = true;
1224  }
1225  // vehicles inside a roundabout should maintain their requests
1226  if (myLane->getEdge().isRoundabout()) {
1227  myHaveToWaitOnNextLink = false;
1228  }
1229 
1230  // XXX braking due to lane-changing is not registered
1231  bool braking = vSafe < getSpeed();
1232  // apply speed reduction due to dawdling / lane changing but ensure minimum save speed
1233  SUMOReal vNext = MAX2(getCarFollowModel().moveHelper(this, vSafe), vSafeMin);
1234 
1235  //if (vNext > vSafe) {
1236  // WRITE_WARNING("vehicle '" + getID() + "' cannot brake hard enough to reach safe speed "
1237  // + toString(vSafe) + ", moving at " + toString(vNext) + " instead. time="
1238  // + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
1239  //}
1240  vNext = MAX2(vNext, (SUMOReal) 0.);
1241 #ifndef NO_TRACI
1242  if (myInfluencer != 0) {
1244  const SUMOReal vMin = MAX2(SUMOReal(0), getVehicleType().getCarFollowModel().getSpeedAfterMaxDecel(myState.mySpeed));
1245  vNext = myInfluencer->influenceSpeed(MSNet::getInstance()->getCurrentTimeStep(), vNext, vSafe, vMin, vMax);
1246  if (myInfluencer->isVTDControlled()) {
1247  vNext = 0;
1248  }
1249  }
1250 #endif
1251  // visit waiting time
1252  if (vNext <= SUMO_const_haltingSpeed) {
1254  braking = true;
1255  } else {
1256  myWaitingTime = 0;
1257  }
1258  if (braking) {
1260  } else {
1262  }
1263  // call reminders after vNext is set
1264  const SUMOReal pos = myState.myPos;
1265 
1266  // update position and speed
1268  myState.myPos += SPEED2DIST(vNext);
1269  myState.mySpeed = vNext;
1271  std::vector<MSLane*> passedLanes;
1272  for (std::vector<MSLane*>::reverse_iterator i = myFurtherLanes.rbegin(); i != myFurtherLanes.rend(); ++i) {
1273  passedLanes.push_back(*i);
1274  }
1275  if (passedLanes.size() == 0 || passedLanes.back() != myLane) {
1276  passedLanes.push_back(myLane);
1277  }
1278  bool moved = false;
1279  std::string emergencyReason = " for unknown reasons";
1280  // move on lane(s)
1281  if (myState.myPos <= myLane->getLength()) {
1282  // we are staying at our lane
1283  // there is no need to go over succeeding lanes
1284  workOnMoveReminders(pos, pos + SPEED2DIST(vNext), vNext);
1285  } else {
1286  // we are moving at least to the next lane (maybe pass even more than one)
1287  if (myCurrEdge != myRoute->end() - 1) {
1288  MSLane* approachedLane = myLane;
1289  // move the vehicle forward
1290  for (i = myLFLinkLanes.begin(); i != myLFLinkLanes.end() && approachedLane != 0 && myState.myPos > approachedLane->getLength(); ++i) {
1292  MSLink* link = (*i).myLink;
1293  // check whether the vehicle was allowed to enter lane
1294  // otherwise it is decelareted and we do not need to test for it's
1295  // approach on the following lanes when a lane changing is performed
1296  // proceed to the next lane
1297  if (link != 0) {
1298  approachedLane = link->getViaLaneOrLane();
1299 #ifndef NO_TRACI
1301 #endif
1302  if (link->getState() == LINKSTATE_TL_RED) {
1303  emergencyReason = " because of a red traffic light";
1304  break;
1305  }
1306 #ifndef NO_TRACI
1307  }
1308 #endif
1309  } else {
1310  emergencyReason = " because there is no connection to the next edge";
1311  approachedLane = 0;
1312  break;
1313  }
1314  if (approachedLane != myLane && approachedLane != 0) {
1315  myState.myPos -= myLane->getLength();
1316  assert(myState.myPos > 0);
1317  enterLaneAtMove(approachedLane);
1318  myLane = approachedLane;
1319  if (getLaneChangeModel().isChangingLanes()) {
1320  if (link->getDirection() == LINKDIR_LEFT || link->getDirection() == LINKDIR_RIGHT) {
1321  // abort lane change
1322  WRITE_WARNING("Vehicle '" + getID() + "' could not finish continuous lane change (turn lane) time=" +
1323  time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
1325  }
1326  }
1327  moved = true;
1328  if (approachedLane->getEdge().isVaporizing()) {
1330  break;
1331  }
1332  }
1333  passedLanes.push_back(approachedLane);
1334  }
1335  }
1336  }
1337  // clear previously set information
1338  for (std::vector<MSLane*>::iterator i = myFurtherLanes.begin(); i != myFurtherLanes.end(); ++i) {
1339  (*i)->resetPartialOccupation(this);
1340  }
1341  myFurtherLanes.clear();
1342 
1343  if (myInfluencer != 0 && myInfluencer->isVTDControlled()) {
1344  myWaitingTime = 0;
1345 // myInfluencer->setVTDControlled(false);
1346  return false;
1347  }
1348 
1349  if (!hasArrived() && !myLane->getEdge().isVaporizing()) {
1350  if (myState.myPos > myLane->getLength()) {
1351  WRITE_WARNING("Vehicle '" + getID() + "' performs emergency stop at the end of lane '" + myLane->getID()
1352  + emergencyReason
1353  + " (decel=" + toString(myAcceleration - myState.mySpeed)
1354  + "), time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
1357  myState.mySpeed = 0;
1358  }
1359  if (myState.myPos - getVehicleType().getLength() < 0 && passedLanes.size() > 0) {
1360  SUMOReal leftLength = getVehicleType().getLength() - myState.myPos;
1361  std::vector<MSLane*>::reverse_iterator i = passedLanes.rbegin() + 1;
1362  while (leftLength > 0 && i != passedLanes.rend()) {
1363  myFurtherLanes.push_back(*i);
1364  leftLength -= (*i)->setPartialOccupation(this, leftLength);
1365  ++i;
1366  }
1367  }
1368  updateBestLanes();
1369  // bestLanes need to be updated before lane changing starts
1370  if (getLaneChangeModel().isChangingLanes()) {
1372  }
1373  setBlinkerInformation(); // needs updated bestLanes
1374  // State needs to be reset for all vehicles before the next call to MSEdgeControl::changeLanes
1376  }
1377  // State needs to be reset for all vehicles before the next call to MSEdgeControl::changeLanes
1378  return moved;
1379 }
1380 
1381 
1382 SUMOReal
1383 MSVehicle::getSpaceTillLastStanding(const MSLane* l, bool& foundStopped) const {
1384  SUMOReal lengths = 0;
1385  const MSLane::VehCont& vehs = l->getVehiclesSecure();
1386  for (MSLane::VehCont::const_iterator i = vehs.begin(); i != vehs.end(); ++i) {
1387  if ((*i)->getSpeed() < SUMO_const_haltingSpeed && !(*i)->getLane()->getEdge().isRoundabout()) {
1388  foundStopped = true;
1389  const SUMOReal ret = (*i)->getPositionOnLane() - (*i)->getVehicleType().getLengthWithGap() - lengths;
1390  l->releaseVehicles();
1391  return ret;
1392  }
1393  lengths += (*i)->getVehicleType().getLengthWithGap();
1394  }
1395  l->releaseVehicles();
1396  return l->getLength() - lengths;
1397 }
1398 
1399 
1400 void
1401 MSVehicle::checkRewindLinkLanes(const SUMOReal lengthsInFront, DriveItemVector& lfLinks) const {
1402 #ifdef DEBUG_VEHICLE_GUI_SELECTION
1403  if (gSelected.isSelected(GLO_VEHICLE, static_cast<const GUIVehicle*>(this)->getGlID())) {
1404  int bla = 0;
1405  if (MSNet::getInstance()->getCurrentTimeStep() == 152000) {
1406  bla = 0;
1407  }
1408  }
1409 #endif
1410 #ifdef HAVE_INTERNAL_LANES
1412  bool hadVehicle = false;
1413  SUMOReal seenSpace = -lengthsInFront;
1414 
1415  bool foundStopped = false;
1416  // compute available space until a stopped vehicle is found
1417  // this is the sum of non-interal lane length minus in-between vehicle lenghts
1418  for (unsigned int i = 0; i < lfLinks.size(); ++i) {
1419  // skip unset links
1420  DriveProcessItem& item = lfLinks[i];
1421  if (item.myLink == 0 || foundStopped) {
1422  item.availableSpace = seenSpace;
1423  item.hadVehicle = hadVehicle;
1424  continue;
1425  }
1426  // get the next lane, determine whether it is an internal lane
1427  const MSLane* approachedLane = item.myLink->getViaLane();
1428  if (approachedLane != 0) {
1429  if (item.myLink->hasFoes()/* && item.myLink->willHaveBlockedFoe()*/) {
1430  seenSpace = seenSpace - approachedLane->getBruttoVehLenSum();
1431  hadVehicle |= approachedLane->getVehicleNumber() != 0;
1432  } else {
1433  seenSpace = seenSpace + getSpaceTillLastStanding(approachedLane, foundStopped);// - approachedLane->getBruttoVehLenSum() + approachedLane->getLength();
1434  hadVehicle |= approachedLane->getVehicleNumber() != 0;
1435  }
1436  item.availableSpace = seenSpace;
1437  item.hadVehicle = hadVehicle;
1438  continue;
1439  }
1440  approachedLane = item.myLink->getLane();
1441  const MSVehicle* last = approachedLane->getLastVehicle();
1442  if (last == 0) {
1443  last = approachedLane->getPartialOccupator();
1444  if (last != 0) {
1446  item.availableSpace = MAX2(seenSpace, seenSpace + approachedLane->getPartialOccupatorEnd() + last->getCarFollowModel().brakeGap(last->getSpeed()));
1447  hadVehicle = true;
1449  seenSpace = seenSpace + getSpaceTillLastStanding(approachedLane, foundStopped);// - approachedLane->getBruttoVehLenSum() + approachedLane->getLength();
1451  if (last->myHaveToWaitOnNextLink) {
1452  foundStopped = true;
1453  }
1454  } else {
1455  seenSpace += approachedLane->getLength();
1456  item.availableSpace = seenSpace;
1457  }
1458  } else {
1459  if (last->signalSet(VEH_SIGNAL_BRAKELIGHT)) {
1460  const SUMOReal lastBrakeGap = last->getCarFollowModel().brakeGap(approachedLane->getLastVehicle()->getSpeed());
1461  const SUMOReal lastGap = last->getPositionOnLane() - last->getVehicleType().getLengthWithGap() + lastBrakeGap - last->getSpeed() * last->getCarFollowModel().getHeadwayTime();
1462  item.availableSpace = MAX2(seenSpace, seenSpace + lastGap);
1463  seenSpace += getSpaceTillLastStanding(approachedLane, foundStopped);// - approachedLane->getBruttoVehLenSum() + approachedLane->getLength();
1464  } else {
1465  seenSpace += getSpaceTillLastStanding(approachedLane, foundStopped);
1466  item.availableSpace = seenSpace;
1467  }
1468  if (last->myHaveToWaitOnNextLink) {
1469  foundStopped = true;
1470  }
1471  hadVehicle = true;
1472  }
1473  item.hadVehicle = hadVehicle;
1474  }
1475 
1476 
1477 #ifdef DEBUG_VEHICLE_GUI_SELECTION
1478  if (gSelected.isSelected(GLO_VEHICLE, static_cast<const GUIVehicle*>(this)->getGlID())) {
1479  int bla = 0;
1480  }
1481 #endif
1482  // check which links allow continuation and add pass available to the previous item
1483  for (int i = (int)(lfLinks.size() - 1); i > 0; --i) {
1484  DriveProcessItem& item = lfLinks[i - 1];
1485  const bool opened = item.myLink != 0 && (item.myLink->havePriority() ||
1486 #ifndef NO_TRACI
1488 #endif
1489  item.myLink->opened(item.myArrivalTime, item.myArrivalSpeed,
1492  bool allowsContinuation = item.myLink == 0 || item.myLink->isCont() || !lfLinks[i].hadVehicle || opened;
1493  if (!opened && item.myLink != 0) {
1494  if (i > 1) {
1495  DriveProcessItem& item2 = lfLinks[i - 2];
1496  if (item2.myLink != 0 && item2.myLink->isCont()) {
1497  allowsContinuation = true;
1498  }
1499  }
1500  }
1501  if (allowsContinuation) {
1502  item.availableSpace = lfLinks[i].availableSpace;
1503  }
1504  }
1505 
1506  // find removalBegin
1507  int removalBegin = -1;
1508  for (unsigned int i = 0; hadVehicle && i < lfLinks.size() && removalBegin < 0; ++i) {
1509  // skip unset links
1510  const DriveProcessItem& item = lfLinks[i];
1511  if (item.myLink == 0) {
1512  continue;
1513  }
1514  /*
1515  SUMOReal impatienceCorrection = MAX2(SUMOReal(0), SUMOReal(SUMOReal(myWaitingTime)));
1516  if (seenSpace<getVehicleType().getLengthWithGap()-impatienceCorrection/10.&&nextSeenNonInternal!=0) {
1517  removalBegin = lastLinkToInternal;
1518  }
1519  */
1520 
1521  const SUMOReal leftSpace = item.availableSpace - getVehicleType().getLengthWithGap();
1522  if (leftSpace < 0/* && item.myLink->willHaveBlockedFoe()*/) {
1523  SUMOReal impatienceCorrection = 0;
1524  /*
1525  if(item.myLink->getState()==LINKSTATE_MINOR) {
1526  impatienceCorrection = MAX2(SUMOReal(0), STEPS2TIME(myWaitingTime));
1527  }
1528  */
1529  if (leftSpace < -impatienceCorrection / 10. && item.myLink->hasFoes()) {
1530  removalBegin = i;
1531  }
1532  //removalBegin = i;
1533  }
1534  }
1535  // abort requests
1536  if (removalBegin != -1 && !(removalBegin == 0 && myLane->getEdge().getPurpose() == MSEdge::EDGEFUNCTION_INTERNAL)) {
1537  while (removalBegin < (int)(lfLinks.size())) {
1539  lfLinks[removalBegin].myVLinkPass = lfLinks[removalBegin].myVLinkWait;
1540  if (lfLinks[removalBegin].myDistance >= brakeGap || (lfLinks[removalBegin].myDistance > 0 && myState.mySpeed < ACCEL2SPEED(getCarFollowModel().getMaxDecel()))) {
1541  lfLinks[removalBegin].mySetRequest = false;
1542  }
1543  ++removalBegin;
1544  }
1545  }
1546  }
1547 #else
1548  UNUSED_PARAMETER(lengthsInFront);
1549 #endif
1550  for (DriveItemVector::iterator i = lfLinks.begin(); i != lfLinks.end(); ++i) {
1551  if ((*i).myLink != 0) {
1552  if ((*i).myLink->getState() == LINKSTATE_ALLWAY_STOP) {
1553  (*i).myArrivalTime += (SUMOTime)RandHelper::rand((size_t)2); // tie braker
1554  }
1555  (*i).myLink->setApproaching(this, (*i).myArrivalTime, (*i).myArrivalSpeed, (*i).getLeaveSpeed(),
1556  (*i).mySetRequest, (*i).myArrivalTimeBraking, (*i).myArrivalSpeedBraking, getWaitingTime());
1557  }
1558  }
1559 }
1560 
1561 
1562 void
1564  for (MoveReminderCont::iterator rem = myMoveReminders.begin(); rem != myMoveReminders.end();) {
1565  if (rem->first->getLane() != 0 && rem->first->getLane() != getLane()) {
1566 #ifdef _DEBUG
1567  if (myTraceMoveReminders) {
1568  traceMoveReminder("notifyEnter_skipped", rem->first, rem->second, true);
1569  }
1570 #endif
1571  ++rem;
1572  } else {
1573  if (rem->first->notifyEnter(*this, reason)) {
1574 #ifdef _DEBUG
1575  if (myTraceMoveReminders) {
1576  traceMoveReminder("notifyEnter", rem->first, rem->second, true);
1577  }
1578 #endif
1579  ++rem;
1580  } else {
1581 #ifdef _DEBUG
1582  if (myTraceMoveReminders) {
1583  traceMoveReminder("notifyEnter", rem->first, rem->second, false);
1584  }
1585 #endif
1586  rem = myMoveReminders.erase(rem);
1587  }
1588  }
1589  }
1590 }
1591 
1592 
1593 bool
1594 MSVehicle::enterLaneAtMove(MSLane* enteredLane, bool onTeleporting) {
1595  myAmOnNet = !onTeleporting;
1596  // vaporizing edge?
1597  /*
1598  if (enteredLane->getEdge().isVaporizing()) {
1599  // yep, let's do the vaporization...
1600  myLane = enteredLane;
1601  return true;
1602  }
1603  */
1604  // move mover reminder one lane further
1605  adaptLaneEntering2MoveReminder(*enteredLane);
1606  // set the entered lane as the current lane
1607  myLane = enteredLane;
1608 
1609  // internal edges are not a part of the route...
1610  if (enteredLane->getEdge().getPurpose() != MSEdge::EDGEFUNCTION_INTERNAL) {
1611  ++myCurrEdge;
1612  }
1613  if (!onTeleporting) {
1615  } else {
1617  // normal move() isn't called so reset position here
1618  myState.myPos = 0;
1620  }
1621  return hasArrived();
1622 }
1623 
1624 
1625 void
1627  myAmOnNet = true;
1628  myLane = enteredLane;
1629  // need to update myCurrentLaneInBestLanes
1630  updateBestLanes();
1631  // switch to and activate the new lane's reminders
1632  // keep OldLaneReminders
1633  for (std::vector< MSMoveReminder* >::const_iterator rem = enteredLane->getMoveReminders().begin(); rem != enteredLane->getMoveReminders().end(); ++rem) {
1634  addReminder(*rem);
1635  }
1637  /*
1638  for (std::vector<MSLane*>::iterator i = myFurtherLanes.begin(); i != myFurtherLanes.end(); ++i) {
1639  (*i)->resetPartialOccupation(this);
1640  }
1641  myFurtherLanes.clear();
1642  */
1643  if (myState.myPos - getVehicleType().getLength() < 0) {
1644  // we have to rebuild "further lanes"
1645  const MSRoute& route = getRoute();
1647  MSLane* lane = myLane;
1648  SUMOReal leftLength = getVehicleType().getLength() - myState.myPos;
1649  while (i != route.begin() && leftLength > 0) {
1650  /* const MSEdge* const prev = */ *(--i);
1651  lane = lane->getLogicalPredecessorLane();
1652  if (lane == 0) {
1653  break;
1654  }
1655  myFurtherLanes.push_back(lane);
1656  leftLength -= (lane)->setPartialOccupation(this, leftLength);
1657  /*
1658  const std::vector<MSLane::IncomingLaneInfo> &incomingLanes = lane->getIncomingLanes();
1659  for (std::vector<MSLane::IncomingLaneInfo>::const_iterator j = incomingLanes.begin(); j != incomingLanes.end(); ++j) {
1660  if (&(*j).lane->getEdge() == prev) {
1661  #ifdef HAVE_INTERNAL_LANES
1662  (*j).lane->setPartialOccupation(this, leftLength);
1663  #else
1664  leftLength -= (*j).length;
1665  (*j).lane->setPartialOccupation(this, leftLength);
1666  #endif
1667  leftLength -= (*j).lane->getLength();
1668  break;
1669  }
1670  }
1671  */
1672  }
1673  }
1674 }
1675 
1676 
1677 void
1679  myState = State(pos, speed);
1681  assert(myState.myPos >= 0);
1682  assert(myState.mySpeed >= 0);
1683  myWaitingTime = 0;
1684  myLane = enteredLane;
1685  myAmOnNet = true;
1686  // set and activate the new lane's reminders
1687  for (std::vector< MSMoveReminder* >::const_iterator rem = enteredLane->getMoveReminders().begin(); rem != enteredLane->getMoveReminders().end(); ++rem) {
1688  addReminder(*rem);
1689  }
1690  activateReminders(notification);
1691  std::string msg;
1692  if (MSGlobals::gCheckRoutes && !hasValidRoute(msg)) {
1693  throw ProcessError("Vehicle '" + getID() + "' has no valid route. " + msg);
1694  }
1695  // build the list of lanes the vehicle is lapping into
1696  SUMOReal leftLength = myType->getLength() - pos;
1697  MSLane* clane = enteredLane;
1698  while (leftLength > 0) {
1699  clane = clane->getLogicalPredecessorLane();
1700  if (clane == 0) {
1701  break;
1702  }
1703  myFurtherLanes.push_back(clane);
1704  leftLength -= (clane)->setPartialOccupation(this, leftLength);
1705  }
1706 }
1707 
1708 
1709 void
1711  for (MoveReminderCont::iterator rem = myMoveReminders.begin(); rem != myMoveReminders.end();) {
1712  if (rem->first->notifyLeave(*this, myState.myPos + rem->second, reason)) {
1713 #ifdef _DEBUG
1714  if (myTraceMoveReminders) {
1715  traceMoveReminder("notifyLeave", rem->first, rem->second, true);
1716  }
1717 #endif
1718  ++rem;
1719  } else {
1720 #ifdef _DEBUG
1721  if (myTraceMoveReminders) {
1722  traceMoveReminder("notifyLeave", rem->first, rem->second, false);
1723  }
1724 #endif
1725  rem = myMoveReminders.erase(rem);
1726  }
1727  }
1728  if (reason != MSMoveReminder::NOTIFICATION_JUNCTION) {
1729  for (std::vector<MSLane*>::iterator i = myFurtherLanes.begin(); i != myFurtherLanes.end(); ++i) {
1730  (*i)->resetPartialOccupation(this);
1731  }
1732  myFurtherLanes.clear();
1733  }
1734  if (reason >= MSMoveReminder::NOTIFICATION_TELEPORT) {
1735  myAmOnNet = false;
1736  }
1738  WRITE_WARNING("Vehicle '" + getID() + "' aborts stop.");
1739  }
1741  while (!myStops.empty() && myStops.front().edge == myCurrEdge) {
1742  WRITE_WARNING("Vehicle '" + getID() + "' skips stop on lane '" + myStops.front().lane->getID() + "'.");
1743  myStops.pop_front();
1744  }
1745  }
1746 }
1747 
1748 
1751  return *myLaneChangeModel;
1752 }
1753 
1754 
1757  return *myLaneChangeModel;
1758 }
1759 
1760 
1761 const std::vector<MSVehicle::LaneQ>&
1763  return *myBestLanes.begin();
1764 }
1765 
1766 
1767 void
1768 MSVehicle::updateBestLanes(bool forceRebuild, const MSLane* startLane) {
1769 #ifdef DEBUG_VEHICLE_GUI_SELECTION
1770  if (gSelected.isSelected(GLO_VEHICLE, static_cast<const GUIVehicle*>(this)->getGlID())) {
1771  int bla = 0;
1772  myLastBestLanesEdge = 0;
1773  }
1774 #endif
1775 
1776  if (startLane == 0) {
1777  startLane = myLane;
1778  }
1779  assert(startLane != 0);
1780  if (myBestLanes.size() > 0 && !forceRebuild && myLastBestLanesEdge == &startLane->getEdge()) {
1782  return;
1783  }
1784  if (startLane->getEdge().getPurpose() == MSEdge::EDGEFUNCTION_INTERNAL) {
1785  if (myBestLanes.size() == 0 || forceRebuild) {
1786  // rebuilt from previous non-internal lane (may backtrack twice if behind an internal junction)
1787  updateBestLanes(true, startLane->getLogicalPredecessorLane());
1788  }
1789  if (myLastBestLanesInternalLane == startLane && !forceRebuild) {
1790  return;
1791  }
1792  // adapt best lanes to fit the current internal edge:
1793  // keep the entries that are reachable from this edge
1794  const MSEdge* nextEdge = startLane->getInternalFollower();
1795  assert(nextEdge->getPurpose() != MSEdge::EDGEFUNCTION_INTERNAL);
1796  for (std::vector<std::vector<LaneQ> >::iterator it = myBestLanes.begin(); it != myBestLanes.end();) {
1797  std::vector<LaneQ>& lanes = *it;
1798  assert(lanes.size() > 0);
1799  if (&(lanes[0].lane->getEdge()) == nextEdge) {
1800  // keep those lanes which are successors of internal lanes from the edge of startLane
1801  std::vector<LaneQ> oldLanes = lanes;
1802  lanes.clear();
1803  const std::vector<MSLane*>& sourceLanes = startLane->getEdge().getLanes();
1804  for (std::vector<MSLane*>::const_iterator it_source = sourceLanes.begin(); it_source != sourceLanes.end(); ++it_source) {
1805  for (std::vector<LaneQ>::iterator it_lane = oldLanes.begin(); it_lane != oldLanes.end(); ++it_lane) {
1806  if ((*it_source)->getLinkCont()[0]->getLane() == (*it_lane).lane) {
1807  lanes.push_back(*it_lane);
1808  break;
1809  }
1810  }
1811  }
1812  assert(lanes.size() == startLane->getEdge().getLanes().size());
1813  // patch invalid bestLaneOffset and updated myCurrentLaneInBestLanes
1814  for (int i = 0; i < (int)lanes.size(); ++i) {
1815  if (i + lanes[i].bestLaneOffset < 0) {
1816  lanes[i].bestLaneOffset = -i;
1817  }
1818  if (i + lanes[i].bestLaneOffset >= (int)lanes.size()) {
1819  lanes[i].bestLaneOffset = (int)lanes.size() - i - 1;
1820  }
1821  assert(i + lanes[i].bestLaneOffset >= 0);
1822  assert(i + lanes[i].bestLaneOffset < (int)lanes.size());
1823  if (lanes[i].bestContinuations[0] != 0) {
1824  // patch length of bestContinuation to match expectations (only once)
1825  lanes[i].bestContinuations.insert(lanes[i].bestContinuations.begin(), (MSLane*)0);
1826  }
1827  if (startLane->getLinkCont()[0]->getLane() == lanes[i].lane) {
1828  myCurrentLaneInBestLanes = lanes.begin() + i;
1829  }
1830  assert(&(lanes[i].lane->getEdge()) == nextEdge);
1831  }
1832  myLastBestLanesInternalLane = startLane;
1834  return;
1835  } else {
1836  // remove passed edges
1837  it = myBestLanes.erase(it);
1838  }
1839  }
1840  assert(false); // should always find the next edge
1841  }
1842  // start rebuilding
1843  myLastBestLanesEdge = &startLane->getEdge();
1844  myBestLanes.clear();
1845 
1846  // get information about the next stop
1847  const MSEdge* nextStopEdge = 0;
1848  const MSLane* nextStopLane = 0;
1849  SUMOReal nextStopPos = 0;
1850  if (!myStops.empty()) {
1851  const Stop& nextStop = myStops.front();
1852  nextStopLane = nextStop.lane;
1853  nextStopEdge = &nextStopLane->getEdge();
1854  nextStopPos = nextStop.startPos;
1855  }
1856  if (myParameter->arrivalLaneProcedure == ARRIVAL_LANE_GIVEN && nextStopEdge == 0) {
1857  nextStopEdge = *(myRoute->end() - 1);
1858  nextStopLane = nextStopEdge->getLanes()[myParameter->arrivalLane];
1859  nextStopPos = myArrivalPos;
1860  }
1861  if (nextStopEdge != 0) {
1862  // make sure that the "wrong" lanes get a penalty. (penalty needs to be
1863  // large enough to overcome a magic threshold in MSLCM_DK2004.cpp:383)
1864  nextStopPos = MIN2((SUMOReal)nextStopPos, (SUMOReal)(nextStopEdge->getLength() - 2 * POSITION_EPS));
1865  }
1866 
1867  // go forward along the next lanes;
1868  int seen = 0;
1869  SUMOReal seenLength = 0;
1870  bool progress = true;
1871  for (MSRouteIterator ce = myCurrEdge; progress;) {
1872  std::vector<LaneQ> currentLanes;
1873  const std::vector<MSLane*>* allowed = 0;
1874  const MSEdge* nextEdge = 0;
1875  if (ce != myRoute->end() && ce + 1 != myRoute->end()) {
1876  nextEdge = *(ce + 1);
1877  allowed = (*ce)->allowedLanes(*nextEdge, myType->getVehicleClass());
1878  }
1879  const std::vector<MSLane*>& lanes = (*ce)->getLanes();
1880  for (std::vector<MSLane*>::const_iterator i = lanes.begin(); i != lanes.end(); ++i) {
1881  LaneQ q;
1882  MSLane* cl = *i;
1883  q.lane = cl;
1884  q.bestContinuations.push_back(cl);
1885  q.bestLaneOffset = 0;
1886  q.length = cl->getLength();
1887  q.allowsContinuation = allowed == 0 || find(allowed->begin(), allowed->end(), cl) != allowed->end();
1888  currentLanes.push_back(q);
1889  }
1890  //
1891  if (nextStopEdge == *ce) {
1892  progress = false;
1893  for (std::vector<LaneQ>::iterator q = currentLanes.begin(); q != currentLanes.end(); ++q) {
1894  if (nextStopLane != 0 && nextStopLane != (*q).lane) {
1895  (*q).allowsContinuation = false;
1896  (*q).length = nextStopPos;
1897  }
1898  }
1899  }
1900 
1901  myBestLanes.push_back(currentLanes);
1902  ++seen;
1903  seenLength += currentLanes[0].lane->getLength();
1904  ++ce;
1905  progress &= (seen <= 4 || seenLength < 3000);
1906  progress &= seen <= 8;
1907  progress &= ce != myRoute->end();
1908  /*
1909  if(progress) {
1910  progress &= (currentLanes.size()!=1||(*ce)->getLanes().size()!=1);
1911  }
1912  */
1913  }
1914 
1915  // we are examining the last lane explicitly
1916  if (myBestLanes.size() != 0) {
1917  SUMOReal bestLength = -1;
1918  int bestThisIndex = 0;
1919  int index = 0;
1920  std::vector<LaneQ>& last = myBestLanes.back();
1921  for (std::vector<LaneQ>::iterator j = last.begin(); j != last.end(); ++j, ++index) {
1922  if ((*j).length > bestLength) {
1923  bestLength = (*j).length;
1924  bestThisIndex = index;
1925  }
1926  }
1927  index = 0;
1928  for (std::vector<LaneQ>::iterator j = last.begin(); j != last.end(); ++j, ++index) {
1929  if ((*j).length < bestLength) {
1930  (*j).bestLaneOffset = bestThisIndex - index;
1931  }
1932  }
1933  }
1934 
1935  // go backward through the lanes
1936  // track back best lane and compute the best prior lane(s)
1937  for (std::vector<std::vector<LaneQ> >::reverse_iterator i = myBestLanes.rbegin() + 1; i != myBestLanes.rend(); ++i) {
1938  std::vector<LaneQ>& nextLanes = (*(i - 1));
1939  std::vector<LaneQ>& clanes = (*i);
1940  MSEdge& cE = clanes[0].lane->getEdge();
1941  int index = 0;
1942  SUMOReal bestConnectedLength = -1;
1943  SUMOReal bestLength = -1;
1944  for (std::vector<LaneQ>::iterator j = nextLanes.begin(); j != nextLanes.end(); ++j, ++index) {
1945  if ((*j).lane->isApproachedFrom(&cE) && bestConnectedLength < (*j).length) {
1946  bestConnectedLength = (*j).length;
1947  }
1948  if (bestLength < (*j).length) {
1949  bestLength = (*j).length;
1950  }
1951  }
1952  // compute index of the best lane (highest length and least offset from the best next lane)
1953  int bestThisIndex = 0;
1954  if (bestConnectedLength > 0) {
1955  index = 0;
1956  for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
1957  LaneQ bestConnectedNext;
1958  bestConnectedNext.length = -1;
1959  if ((*j).allowsContinuation) {
1960  for (std::vector<LaneQ>::const_iterator m = nextLanes.begin(); m != nextLanes.end(); ++m) {
1961  if ((*m).lane->isApproachedFrom(&cE, (*j).lane)) {
1962  if (bestConnectedNext.length < (*m).length || (bestConnectedNext.length == (*m).length && abs(bestConnectedNext.bestLaneOffset) > abs((*m).bestLaneOffset))) {
1963  bestConnectedNext = *m;
1964  }
1965  }
1966  }
1967  if (bestConnectedNext.length == bestConnectedLength && abs(bestConnectedNext.bestLaneOffset) < 2) {
1968  (*j).length += bestLength;
1969  } else {
1970  (*j).length += bestConnectedNext.length;
1971  }
1972  (*j).bestLaneOffset = bestConnectedNext.bestLaneOffset;
1973  }
1974  if (clanes[bestThisIndex].length < (*j).length || (clanes[bestThisIndex].length == (*j).length && abs(clanes[bestThisIndex].bestLaneOffset) > abs((*j).bestLaneOffset))) {
1975  bestThisIndex = index;
1976  }
1977  copy(bestConnectedNext.bestContinuations.begin(), bestConnectedNext.bestContinuations.end(), back_inserter((*j).bestContinuations));
1978  }
1979 
1980  } else {
1981  int bestNextIndex = 0;
1982  int bestDistToNeeded = (int) clanes.size();
1983  index = 0;
1984  for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
1985  if ((*j).allowsContinuation) {
1986  int nextIndex = 0;
1987  for (std::vector<LaneQ>::const_iterator m = nextLanes.begin(); m != nextLanes.end(); ++m, ++nextIndex) {
1988  if ((*m).lane->isApproachedFrom(&cE, (*j).lane)) {
1989  if (bestDistToNeeded > abs((*m).bestLaneOffset)) {
1990  bestDistToNeeded = abs((*m).bestLaneOffset);
1991  bestThisIndex = index;
1992  bestNextIndex = nextIndex;
1993  }
1994  }
1995  }
1996  }
1997  }
1998  clanes[bestThisIndex].length += nextLanes[bestNextIndex].length;
1999  copy(nextLanes[bestNextIndex].bestContinuations.begin(), nextLanes[bestNextIndex].bestContinuations.end(), back_inserter(clanes[bestThisIndex].bestContinuations));
2000 
2001  }
2002  // set bestLaneOffset for all lanes
2003  index = 0;
2004  for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
2005  if ((*j).length < clanes[bestThisIndex].length || ((*j).length == clanes[bestThisIndex].length && abs((*j).bestLaneOffset) > abs(clanes[bestThisIndex].bestLaneOffset))) {
2006  (*j).bestLaneOffset = bestThisIndex - index;
2007  } else {
2008  (*j).bestLaneOffset = 0;
2009  }
2010  }
2011  }
2013  return;
2014 }
2015 
2016 
2017 void
2019  std::vector<LaneQ>& currLanes = *myBestLanes.begin();
2020  std::vector<LaneQ>::iterator i;
2021  for (i = currLanes.begin(); i != currLanes.end(); ++i) {
2022  SUMOReal nextOccupation = 0;
2023  for (std::vector<MSLane*>::const_iterator j = (*i).bestContinuations.begin() + 1; j != (*i).bestContinuations.end(); ++j) {
2024  nextOccupation += (*j)->getBruttoVehLenSum();
2025  }
2026  (*i).nextOccupation = nextOccupation;
2027  if ((*i).lane == startLane) {
2029  }
2030  }
2031 }
2032 
2033 
2034 const std::vector<MSLane*>&
2036  if (myBestLanes.empty() || myBestLanes[0].empty()) {
2037  return myEmptyLaneVector;
2038  }
2039  return (*myCurrentLaneInBestLanes).bestContinuations;
2040 }
2041 
2042 
2043 const std::vector<MSLane*>&
2045  const MSLane* lane = l;
2047  // internal edges are not kept inside the bestLanes structure
2048  lane = lane->getLinkCont()[0]->getLane();
2049  }
2050  if (myBestLanes.size() == 0) {
2051  return myEmptyLaneVector;
2052  }
2053  for (std::vector<LaneQ>::const_iterator i = myBestLanes[0].begin(); i != myBestLanes[0].end(); ++i) {
2054  if ((*i).lane == lane) {
2055  return (*i).bestContinuations;
2056  }
2057  }
2058  return myEmptyLaneVector;
2059 }
2060 
2061 
2062 int
2064  if (myBestLanes.empty() || myBestLanes[0].empty()) {
2065  return 0;
2066  } else {
2067  return (*myCurrentLaneInBestLanes).bestLaneOffset;
2068  }
2069 }
2070 
2071 
2072 void
2074  std::vector<MSVehicle::LaneQ>& preb = myBestLanes.front();
2075  assert(laneIndex < preb.size());
2076  preb[laneIndex].occupation = density + preb[laneIndex].nextOccupation;
2077 }
2078 
2079 
2080 bool
2082  if (getPositionOnLane() > myLane->getLength()) {
2085  return true;
2086  }
2087  return false;
2088 }
2089 
2090 
2091 SUMOReal
2093 #ifdef DEBUG_VEHICLE_GUI_SELECTION
2094  SUMOReal distance = 1000000.;
2095 #else
2097 #endif
2098  if (isOnRoad() && destEdge != NULL) {
2099  if (&myLane->getEdge() == *myCurrEdge) {
2100  // vehicle is on a normal edge
2101  distance = myRoute->getDistanceBetween(getPositionOnLane(), destPos, *myCurrEdge, destEdge);
2102  } else {
2103  // vehicle is on inner junction edge
2104  distance = myLane->getLength() - getPositionOnLane();
2105  distance += myRoute->getDistanceBetween(0, destPos, *(myCurrEdge + 1), destEdge);
2106  }
2107  }
2108  return distance;
2109 }
2110 
2111 
2112 std::pair<const MSVehicle* const, SUMOReal>
2114  if (myLane == 0) {
2115  return std::make_pair(static_cast<const MSVehicle*>(0), -1);
2116  }
2117  if (dist == 0) {
2119  }
2120  const MSVehicle* lead = 0;
2121  const MSLane::VehCont& vehs = myLane->getVehiclesSecure();
2122  MSLane::VehCont::const_iterator pred = std::find(vehs.begin(), vehs.end(), this) + 1;
2123  if (pred != vehs.end()) {
2124  lead = *pred;
2125  }
2127  if (lead != 0) {
2128  return std::make_pair(lead, lead->getPositionOnLane() - lead->getVehicleType().getLength() - getPositionOnLane() - getVehicleType().getMinGap());
2129  }
2130  const SUMOReal seen = myLane->getLength() - getPositionOnLane();
2131  const std::vector<MSLane*>& bestLaneConts = getBestLanesContinuation(myLane);
2132  return myLane->getLeaderOnConsecutive(dist, seen, getSpeed(), *this, bestLaneConts);
2133 }
2134 
2135 
2136 SUMOReal
2138  std::pair<const MSVehicle* const, SUMOReal> leaderInfo = getLeader();
2139  if (leaderInfo.first == 0 || getSpeed() == 0) {
2140  return -1;
2141  }
2142  return (leaderInfo.second + getVehicleType().getMinGap()) / getSpeed();
2143 }
2144 
2145 
2146 SUMOReal
2149 }
2150 
2151 
2152 SUMOReal
2155 }
2156 
2157 
2158 SUMOReal
2161 }
2162 
2163 
2164 SUMOReal
2167 }
2168 
2169 
2170 SUMOReal
2173 }
2174 
2175 
2176 SUMOReal
2179 }
2180 
2181 
2182 SUMOReal
2185 }
2186 
2187 
2188 void
2190  if (myPersonDevice == 0) {
2192  myMoveReminders.push_back(std::make_pair(myPersonDevice, 0.));
2193  }
2194  myPersonDevice->addPerson(person);
2195  if (myStops.size() > 0 && myStops.front().reached && myStops.front().triggered) {
2196  unsigned int numExpected = (unsigned int) myStops.front().awaitedPersons.size();
2197  if (numExpected != 0) {
2198  // I added the if-statement and number retrieval, assuming that it should be a "conditional short jump" only and
2199  // in most cases we won't have the list of expected passenger - only for simulating car-sharing, probably.
2200  // Bus drivers usually do not know the names of the passengers.
2201  myStops.front().awaitedPersons.erase(person->getID());
2202  numExpected = (unsigned int) myStops.front().awaitedPersons.size();
2203  }
2204  if (numExpected == 0) {
2205  myStops.front().duration = 0;
2206  }
2207  }
2208 }
2209 
2210 
2211 unsigned int
2213  unsigned int boarded = myPersonDevice == 0 ? 0 : myPersonDevice->size();
2214  return boarded + myParameter->personNumber;
2215 }
2216 
2217 
2218 void
2221  int state = getLaneChangeModel().getOwnState();
2222  if ((state & LCA_LEFT) != 0) {
2224  } else if ((state & LCA_RIGHT) != 0) {
2226  } else {
2227  const MSLane* lane = getLane();
2228  MSLinkCont::const_iterator link = MSLane::succLinkSec(*this, 1, *lane, getBestLanesContinuation());
2229  if (link != lane->getLinkCont().end() && lane->getLength() - getPositionOnLane() < lane->getVehicleMaxSpeed(this) * (SUMOReal) 7.) {
2230  switch ((*link)->getDirection()) {
2231  case LINKDIR_TURN:
2232  case LINKDIR_LEFT:
2233  case LINKDIR_PARTLEFT:
2235  break;
2236  case LINKDIR_RIGHT:
2237  case LINKDIR_PARTRIGHT:
2239  break;
2240  default:
2241  break;
2242  }
2243  }
2244  }
2245 
2246 }
2247 
2248 
2249 void
2251  if (myType->amVehicleSpecific()) {
2252  delete myType;
2253  }
2254  myType = type;
2255 }
2256 
2257 unsigned int
2259  std::vector<MSLane*>::const_iterator laneP = std::find(myLane->getEdge().getLanes().begin(), myLane->getEdge().getLanes().end(), myLane);
2260  return (unsigned int) std::distance(myLane->getEdge().getLanes().begin(), laneP);
2261 }
2262 
2263 
2264 #ifndef NO_TRACI
2265 bool
2266 MSVehicle::addTraciStop(MSLane* lane, SUMOReal pos, SUMOReal /*radius*/, SUMOTime duration, bool parking, bool triggered) {
2267  //if the stop exists update the duration
2268  for (std::list<Stop>::iterator iter = myStops.begin(); iter != myStops.end(); iter++) {
2269  if (iter->lane == lane && fabs(iter->endPos - pos) < POSITION_EPS) {
2270  if (duration == 0 && !iter->reached) {
2271  myStops.erase(iter);
2272  } else {
2273  iter->duration = duration;
2274  }
2275  return true;
2276  }
2277  }
2278 
2280  newStop.lane = lane->getID();
2281  newStop.busstop = MSNet::getInstance()->getBusStopID(lane, pos);
2282  newStop.startPos = pos - POSITION_EPS;
2283  newStop.endPos = pos;
2284  newStop.duration = duration;
2285  newStop.until = -1;
2286  newStop.triggered = triggered;
2287  newStop.parking = parking;
2288  newStop.index = STOP_INDEX_FIT;
2289  return addStop(newStop);
2290 }
2291 
2292 
2293 bool
2295  if (isStopped()) {
2299  }
2300  // we have waited long enough and fulfilled any passenger-requirements
2301  if (myStops.front().busstop != 0) {
2302  // inform bus stop about leaving it
2303  myStops.front().busstop->leaveFrom(this);
2304  }
2305  // the current stop is no longer valid
2307  myStops.pop_front();
2308  // do not count the stopping time towards gridlock time.
2309  // Other outputs use an independent counter and are not affected.
2310  myWaitingTime = 0;
2311  // maybe the next stop is on the same edge; let's rebuild best lanes
2312  updateBestLanes(true);
2313  // continue as wished...
2315  return true;
2316  }
2317  return false;
2318 }
2319 
2320 
2323  return myStops.front();
2324 }
2325 
2326 
2329  if (myInfluencer == 0) {
2330  myInfluencer = new Influencer();
2331  }
2332  return *myInfluencer;
2333 }
2334 
2335 
2336 SUMOReal
2338  if (myInfluencer != 0) {
2339  return myInfluencer->getOriginalSpeed();
2340  }
2341  return myState.mySpeed;
2342 }
2343 
2344 
2345 int
2347  if (hasInfluencer()) {
2349  MSNet::getInstance()->getCurrentTimeStep(),
2350  myLane->getEdge(),
2351  getLaneIndex(),
2352  state);
2353  }
2354  return state;
2355 }
2356 #endif
2357 
2358 
2359 void
2362  // here starts the vehicle internal part (see loading)
2363  std::vector<int> internals;
2364  internals.push_back(myDeparture);
2365  internals.push_back((int)distance(myRoute->begin(), myCurrEdge));
2366  out.writeAttr(SUMO_ATTR_STATE, toString(internals));
2369  out.closeTag();
2370 }
2371 
2372 
2373 void
2374 MSVehicle::loadState(const SUMOSAXAttributes& attrs, const SUMOTime offset) {
2375  if (!attrs.hasAttribute(SUMO_ATTR_POSITION)) {
2376  throw ProcessError("Error: Invalid vehicles in state (may be a meso state)!");
2377  }
2378  unsigned int routeOffset;
2379  std::istringstream bis(attrs.getString(SUMO_ATTR_STATE));
2380  bis >> myDeparture;
2381  bis >> routeOffset;
2382  if (hasDeparted()) {
2383  myDeparture -= offset;
2384  myCurrEdge += routeOffset;
2385  }
2388  // no need to reset myCachedPosition here since state loading happens directly after creation
2389 }
2390 
2391 
2392 /****************************************************************************/