SUMO - Simulation of Urban MObility
MSVehicle.cpp
Go to the documentation of this file.
00001 /****************************************************************************/
00017 // Representation of a vehicle in the micro simulation
00018 /****************************************************************************/
00019 // SUMO, Simulation of Urban MObility; see http://sumo.sourceforge.net/
00020 // Copyright (C) 2001-2012 DLR (http://www.dlr.de/) and contributors
00021 /****************************************************************************/
00022 //
00023 //   This file is part of SUMO.
00024 //   SUMO is free software: you can redistribute it and/or modify
00025 //   it under the terms of the GNU General Public License as published by
00026 //   the Free Software Foundation, either version 3 of the License, or
00027 //   (at your option) any later version.
00028 //
00029 /****************************************************************************/
00030 
00031 // ===========================================================================
00032 // included modules
00033 // ===========================================================================
00034 #ifdef _MSC_VER
00035 #include <windows_config.h>
00036 #else
00037 #include <config.h>
00038 #endif
00039 
00040 #include "MSLane.h"
00041 #include "MSVehicle.h"
00042 #include "MSEdge.h"
00043 #include "MSVehicleType.h"
00044 #include "MSNet.h"
00045 #include "MSRoute.h"
00046 #include "MSLinkCont.h"
00047 #include <utils/common/StringUtils.h>
00048 #include <utils/common/StdDefs.h>
00049 #include <microsim/MSVehicleControl.h>
00050 #include <microsim/MSGlobals.h>
00051 #include <iostream>
00052 #include <cassert>
00053 #include <cmath>
00054 #include <cstdlib>
00055 #include <algorithm>
00056 #include <map>
00057 #include "MSMoveReminder.h"
00058 #include <utils/options/OptionsCont.h>
00059 #include "MSLCM_DK2004.h"
00060 #include <utils/common/ToString.h>
00061 #include <utils/common/FileHelpers.h>
00062 #include <utils/iodevices/OutputDevice.h>
00063 #include <utils/iodevices/BinaryInputDevice.h>
00064 #include "trigger/MSBusStop.h"
00065 #include <utils/common/DijkstraRouterTT.h>
00066 #include "MSPerson.h"
00067 #include "MSPersonControl.h"
00068 #include <utils/common/RandHelper.h>
00069 #include "devices/MSDevice_Person.h"
00070 #include "MSEdgeWeightsStorage.h"
00071 #include <utils/common/HelpersHBEFA.h>
00072 #include <utils/common/HelpersHarmonoise.h>
00073 
00074 #ifdef _MESSAGES
00075 #include "MSMessageEmitter.h"
00076 #endif
00077 
00078 #ifdef HAVE_MESOSIM
00079 #include <mesosim/MESegment.h>
00080 #include <mesosim/MELoop.h>
00081 #include "MSGlobals.h"
00082 #endif
00083 
00084 #ifdef CHECK_MEMORY_LEAKS
00085 #include <foreign/nvwa/debug_new.h>
00086 #endif // CHECK_MEMORY_LEAKS
00087 
00088 //#define DEBUG_VEHICLE_GUI_SELECTION 1
00089 #ifdef DEBUG_VEHICLE_GUI_SELECTION
00090 #undef ID_LIST
00091 #include <utils/gui/div/GUIGlobalSelection.h>
00092 #include <guisim/GUIVehicle.h>
00093 #include <guisim/GUILane.h>
00094 #endif
00095 
00096 #define BUS_STOP_OFFSET 0.5
00097 
00098 
00099 // ===========================================================================
00100 // static value definitions
00101 // ===========================================================================
00102 std::vector<MSLane*> MSVehicle::myEmptyLaneVector;
00103 
00104 
00105 // ===========================================================================
00106 // method definitions
00107 // ===========================================================================
00108 /* -------------------------------------------------------------------------
00109  * methods of MSVehicle::State
00110  * ----------------------------------------------------------------------- */
00111 MSVehicle::State::State(const State& state) {
00112     myPos = state.myPos;
00113     mySpeed = state.mySpeed;
00114 }
00115 
00116 
00117 MSVehicle::State&
00118 MSVehicle::State::operator=(const State& state) {
00119     myPos   = state.myPos;
00120     mySpeed = state.mySpeed;
00121     return *this;
00122 }
00123 
00124 
00125 bool
00126 MSVehicle::State::operator!=(const State& state) {
00127     return (myPos   != state.myPos ||
00128             mySpeed != state.mySpeed);
00129 }
00130 
00131 
00132 SUMOReal
00133 MSVehicle::State::pos() const {
00134     return myPos;
00135 }
00136 
00137 
00138 MSVehicle::State::State(SUMOReal pos, SUMOReal speed) :
00139     myPos(pos), mySpeed(speed) {}
00140 
00141 
00142 /* -------------------------------------------------------------------------
00143  * methods of MSVehicle::Influencer
00144  * ----------------------------------------------------------------------- */
00145 #ifndef NO_TRACI
00146 MSVehicle::Influencer::Influencer()
00147     : mySpeedAdaptationStarted(true), myConsiderSafeVelocity(true),
00148       myConsiderMaxAcceleration(true), myConsiderMaxDeceleration(true) {}
00149 
00150 
00151 MSVehicle::Influencer::~Influencer() {}
00152 
00153 
00154 void
00155 MSVehicle::Influencer::setSpeedTimeLine(const std::vector<std::pair<SUMOTime, SUMOReal> > &speedTimeLine) {
00156     mySpeedAdaptationStarted = true;
00157     mySpeedTimeLine = speedTimeLine;
00158 }
00159 
00160 
00161 void
00162 MSVehicle::Influencer::setLaneTimeLine(const std::vector<std::pair<SUMOTime, unsigned int> > &laneTimeLine) {
00163     myLaneTimeLine = laneTimeLine;
00164 }
00165 
00166 
00167 SUMOReal
00168 MSVehicle::Influencer::influenceSpeed(SUMOTime currentTime, SUMOReal speed, SUMOReal vSafe, SUMOReal vMin, SUMOReal vMax) {
00169     // keep original speed
00170     myOriginalSpeed = speed;
00171     // remove leading commands which are no longer valid
00172     while (mySpeedTimeLine.size() == 1 || (mySpeedTimeLine.size() > 1 && currentTime > mySpeedTimeLine[1].first)) {
00173         mySpeedTimeLine.erase(mySpeedTimeLine.begin());
00174     }
00175     // do nothing if the time line does not apply for the current time
00176     if (mySpeedTimeLine.size() < 2 || currentTime < mySpeedTimeLine[0].first) {
00177         return speed;
00178     }
00179     // compute and set new speed
00180     if (!mySpeedAdaptationStarted) {
00181         mySpeedTimeLine[0].second = speed;
00182         mySpeedAdaptationStarted = true;
00183     }
00184     currentTime += DELTA_T;
00185     const SUMOReal td = STEPS2TIME(currentTime - mySpeedTimeLine[0].first) / STEPS2TIME(mySpeedTimeLine[1].first + DELTA_T - mySpeedTimeLine[0].first);
00186     speed = mySpeedTimeLine[0].second - (mySpeedTimeLine[0].second - mySpeedTimeLine[1].second) * td;
00187     if (myConsiderSafeVelocity) {
00188         speed = MIN2(speed, vSafe);
00189     }
00190     if (myConsiderMaxAcceleration) {
00191         speed = MIN2(speed, vMax);
00192     }
00193     if (myConsiderMaxDeceleration) {
00194         speed = MAX2(speed, vMin);
00195     }
00196     return speed;
00197 }
00198 
00199 
00200 MSVehicle::ChangeRequest
00201 MSVehicle::Influencer::checkForLaneChanges(SUMOTime currentTime, const MSEdge& currentEdge, unsigned int currentLaneIndex) {
00202     // remove leading commands which are no longer valid
00203     while (myLaneTimeLine.size() == 1 || (myLaneTimeLine.size() > 1 && currentTime > myLaneTimeLine[1].first)) {
00204         myLaneTimeLine.erase(myLaneTimeLine.begin());
00205     }
00206     // do nothing if the time line does not apply for the current time
00207     if (myLaneTimeLine.size() < 2 || currentTime < myLaneTimeLine[0].first) {
00208         return REQUEST_NONE;
00209     }
00210     unsigned int destinationLaneIndex = myLaneTimeLine[1].second;
00211     if ((unsigned int)currentEdge.getLanes().size() <= destinationLaneIndex) {
00212         return REQUEST_NONE;
00213     }
00214     if (currentLaneIndex > destinationLaneIndex) {
00215         return REQUEST_RIGHT;
00216     } else if (currentLaneIndex < destinationLaneIndex) {
00217         return REQUEST_LEFT;
00218     } else {
00219         return REQUEST_HOLD;
00220     }
00221 }
00222 
00223 
00224 void
00225 MSVehicle::Influencer::setConsiderSafeVelocity(bool value) {
00226     myConsiderSafeVelocity = value;
00227 }
00228 
00229 
00230 void
00231 MSVehicle::Influencer::setConsiderMaxAcceleration(bool value) {
00232     myConsiderMaxAcceleration = value;
00233 }
00234 
00235 
00236 void
00237 MSVehicle::Influencer::setConsiderMaxDeceleration(bool value) {
00238     myConsiderMaxDeceleration = value;
00239 }
00240 #endif
00241 
00242 
00243 /* -------------------------------------------------------------------------
00244  * MSVehicle-methods
00245  * ----------------------------------------------------------------------- */
00246 MSVehicle::~MSVehicle() {
00247     delete myLaneChangeModel;
00248     // other
00249     delete myEdgeWeights;
00250     for (std::vector<MSLane*>::iterator i = myFurtherLanes.begin(); i != myFurtherLanes.end(); ++i) {
00251         (*i)->resetPartialOccupation(this);
00252     }
00253     for (DriveItemVector::iterator i = myLFLinkLanes.begin(); i != myLFLinkLanes.end(); ++i) {
00254         if ((*i).myLink != 0) {
00255             (*i).myLink->removeApproaching(this);
00256         }
00257     }
00258     myFurtherLanes.clear();
00259     //
00260     if (myType->amVehicleSpecific()) {
00261         delete myType;
00262     }
00263 #ifndef NO_TRACI
00264     delete myInfluencer;
00265 #endif
00266 }
00267 
00268 
00269 MSVehicle::MSVehicle(SUMOVehicleParameter* pars,
00270                      const MSRoute* route,
00271                      const MSVehicleType* type,
00272                      int /*vehicleIndex*/) :
00273     MSBaseVehicle(pars, route, type),
00274     myLastLaneChangeOffset(0),
00275     myWaitingTime(0),
00276     myState(0, 0), //
00277     myLane(0),
00278     myLastBestLanesEdge(0),
00279     myPersonDevice(0),
00280     myPreDawdleAcceleration(0),
00281     mySignals(0),
00282     myAmOnNet(false),
00283     myAmRegisteredAsWaitingForPerson(false),
00284     myEdgeWeights(0),
00285     myHaveToWaitOnNextLink(false)
00286 #ifndef NO_TRACI
00287     , myInfluencer(0)
00288 #endif
00289 {
00290     for (std::vector<SUMOVehicleParameter::Stop>::iterator i = pars->stops.begin(); i != pars->stops.end(); ++i) {
00291         if (!addStop(*i)) {
00292             throw ProcessError("Stop for vehicle '" + pars->id +
00293                                "' on lane '" + i->lane + "' is not downstream the current route.");
00294         }
00295     }
00296     for (std::vector<SUMOVehicleParameter::Stop>::const_iterator i = route->getStops().begin(); i != route->getStops().end(); ++i) {
00297         if (!addStop(*i)) {
00298             throw ProcessError("Stop for vehicle '" + pars->id +
00299                                "' on lane '" + i->lane + "' is not downstream the current route.");
00300         }
00301     }
00302     const MSLane* const depLane = (*myCurrEdge)->getDepartLane(*this);
00303     if (depLane == 0) {
00304         throw ProcessError("Invalid departlane definition for vehicle '" + pars->id + "'.");
00305     }
00306     if (pars->departSpeedProcedure == DEPART_SPEED_GIVEN && pars->departSpeed > depLane->getMaxSpeed()) {
00307         throw ProcessError("Departure speed for vehicle '" + pars->id +
00308                            "' is too high for the departure lane '" + depLane->getID() + "'.");
00309     }
00310     if (pars->departSpeedProcedure == DEPART_SPEED_GIVEN && pars->departSpeed > type->getMaxSpeed()) {
00311         throw ProcessError("Departure speed for vehicle '" + pars->id +
00312                            "' is too high for the vehicle type '" + type->getID() + "'.");
00313     }
00314 #ifdef _MESSAGES
00315     myLCMsgEmitter = MSNet::getInstance()->getMsgEmitter("lanechange");
00316     myBMsgEmitter = MSNet::getInstance()->getMsgEmitter("break");
00317     myHBMsgEmitter = MSNet::getInstance()->getMsgEmitter("heartbeat");
00318 #endif
00319     myLaneChangeModel = new MSLCM_DK2004(*this);
00320     myCFVariables = type->getCarFollowModel().createVehicleVariables();
00321 }
00322 
00323 
00324 void
00325 MSVehicle::onRemovalFromNet(const MSMoveReminder::Notification reason) {
00326     workOnMoveReminders(myState.myPos - SPEED2DIST(myState.mySpeed), myState.myPos, myState.mySpeed);
00327     for (DriveItemVector::iterator i = myLFLinkLanes.begin(); i != myLFLinkLanes.end(); ++i) {
00328         if ((*i).myLink != 0) {
00329             (*i).myLink->removeApproaching(this);
00330         }
00331     }
00332     leaveLane(reason);
00333 }
00334 
00335 
00336 // ------------ interaction with the route
00337 bool
00338 MSVehicle::ends() const {
00339     return myCurrEdge == myRoute->end() - 1 && myState.myPos > myArrivalPos - POSITION_EPS;
00340 }
00341 
00342 
00343 bool
00344 MSVehicle::replaceRoute(const MSRoute* newRoute, bool onInit) {
00345     const MSEdgeVector& edges = newRoute->getEdges();
00346     // assert the vehicle may continue (must not be "teleported" or whatever to another position)
00347     if (!onInit && !newRoute->contains(*myCurrEdge)) {
00348         return false;
00349     }
00350 
00351     // rebuild in-vehicle route information
00352     if (onInit) {
00353         myCurrEdge = newRoute->begin();
00354     } else {
00355         myCurrEdge = find(edges.begin(), edges.end(), *myCurrEdge);
00356     }
00357     // check whether the old route may be deleted (is not used by anyone else)
00358     newRoute->addReference();
00359     myRoute->release();
00360     // assign new route
00361     myRoute = newRoute;
00362     myLastBestLanesEdge = 0;
00363     // update arrival definition
00364     calculateArrivalPos();
00365     // save information that the vehicle was rerouted
00366     myNumberReroutes++;
00367     MSNet::getInstance()->informVehicleStateListener(this, MSNet::VEHICLE_STATE_NEWROUTE);
00368     // recheck stops
00369     for (std::list<Stop>::iterator iter = myStops.begin(); iter != myStops.end();) {
00370         if (find(edges.begin(), edges.end(), &iter->lane->getEdge()) == edges.end()) {
00371             iter = myStops.erase(iter);
00372         } else {
00373             iter->edge = find(edges.begin(), edges.end(), &iter->lane->getEdge());
00374             ++iter;
00375         }
00376     }
00377     return true;
00378 }
00379 
00380 
00381 bool
00382 MSVehicle::willPass(const MSEdge* const edge) const {
00383     return find(myCurrEdge, myRoute->end(), edge) != myRoute->end();
00384 }
00385 
00386 
00387 MSEdgeWeightsStorage&
00388 MSVehicle::getWeightsStorage() {
00389     if (myEdgeWeights == 0) {
00390         myEdgeWeights = new MSEdgeWeightsStorage();
00391     }
00392     return *myEdgeWeights;
00393 }
00394 
00395 
00396 // ------------ Interaction with move reminders
00397 void
00398 MSVehicle::workOnMoveReminders(SUMOReal oldPos, SUMOReal newPos, SUMOReal newSpeed) {
00399     // This erasure-idiom works for all stl-sequence-containers
00400     // See Meyers: Effective STL, Item 9
00401     for (MoveReminderCont::iterator rem = myMoveReminders.begin(); rem != myMoveReminders.end();) {
00402         if (!rem->first->notifyMove(*this, oldPos + rem->second, newPos + rem->second, newSpeed)) {
00403             rem = myMoveReminders.erase(rem);
00404         } else {
00405             ++rem;
00406         }
00407     }
00408 }
00409 
00410 
00411 void
00412 MSVehicle::adaptLaneEntering2MoveReminder(const MSLane& enteredLane) {
00413     // save the old work reminders, patching the position information
00414     //  add the information about the new offset to the old lane reminders
00415     const SUMOReal oldLaneLength = myLane->getLength();
00416     for (MoveReminderCont::iterator rem = myMoveReminders.begin(); rem != myMoveReminders.end(); ++rem) {
00417         rem->second += oldLaneLength;
00418     }
00419     for (std::vector< MSMoveReminder* >::const_iterator rem = enteredLane.getMoveReminders().begin(); rem != enteredLane.getMoveReminders().end(); ++rem) {
00420         addReminder(*rem);
00421     }
00422 }
00423 
00424 
00425 // ------------ Other getter methods
00426 Position
00427 MSVehicle::getPosition() const {
00428     if (myLane == 0) {
00429         return Position(-1000, -1000);
00430     }
00431     return myLane->getShape().positionAtLengthPosition(myState.pos());
00432 }
00433 
00434 
00435 SUMOReal
00436 MSVehicle::getAngle() const {
00437     Position p1 = myLane->getShape().positionAtLengthPosition(myState.pos());
00438     Position p2 = myFurtherLanes.size() > 0
00439                   ? myFurtherLanes.front()->getShape().positionAtLengthPosition(myFurtherLanes.front()->getPartialOccupatorEnd())
00440                   : myLane->getShape().positionAtLengthPosition(myState.pos() - myType->getLength());
00441     if (p1 != p2) {
00442         return atan2(p1.x() - p2.x(), p2.y() - p1.y()) * 180. / PI;
00443     } else {
00444         return -myLane->getShape().rotationDegreeAtLengthPosition(getPositionOnLane());
00445     }
00446 }
00447 
00448 
00449 // ------------
00450 bool
00451 MSVehicle::addStop(const SUMOVehicleParameter::Stop& stopPar, SUMOTime untilOffset) {
00452     Stop stop;
00453     stop.lane = MSLane::dictionary(stopPar.lane);
00454     stop.busstop = MSNet::getInstance()->getBusStop(stopPar.busstop);
00455     stop.startPos = stopPar.startPos;
00456     stop.endPos = stopPar.endPos;
00457     stop.duration = stopPar.duration;
00458     stop.until = stopPar.until;
00459     if (stop.until != -1) {
00460         stop.until += untilOffset;
00461     }
00462     stop.triggered = stopPar.triggered;
00463     stop.parking = stopPar.parking;
00464     stop.reached = false;
00465     if (stop.startPos < 0 || stop.endPos > stop.lane->getLength()) {
00466         return false;
00467     }
00468     stop.edge = find(myCurrEdge, myRoute->end(), &stop.lane->getEdge());
00469     MSRouteIterator prevStopEdge = myCurrEdge;
00470     SUMOReal prevStopPos = myState.myPos;
00471     // where to insert the stop
00472     std::list<Stop>::iterator iter = myStops.begin();
00473     if (stopPar.index == STOP_INDEX_END || stopPar.index >= static_cast<int>(myStops.size())) {
00474         if (myStops.size() > 0) {
00475             prevStopEdge = myStops.back().edge;
00476             prevStopPos = myStops.back().endPos;
00477             iter = myStops.end();
00478             stop.edge = find(prevStopEdge, myRoute->end(), &stop.lane->getEdge());
00479         }
00480     } else {
00481         if (stopPar.index == STOP_INDEX_FIT) {
00482             while (iter != myStops.end() && (iter->edge < stop.edge ||
00483                                              (iter->endPos < stop.endPos && iter->edge == stop.edge))) {
00484                 prevStopEdge = iter->edge;
00485                 prevStopPos = iter->endPos;
00486                 ++iter;
00487             }
00488         } else {
00489             int index = stopPar.index;
00490             while (index > 0) {
00491                 prevStopEdge = iter->edge;
00492                 prevStopPos = iter->endPos;
00493                 ++iter;
00494                 --index;
00495             }
00496             stop.edge = find(prevStopEdge, myRoute->end(), &stop.lane->getEdge());
00497         }
00498     }
00499     if (stop.edge == myRoute->end() || prevStopEdge > stop.edge || prevStopEdge == stop.edge && prevStopPos > stop.endPos) {
00500         return false;
00501     }
00502     if (myCurrEdge == stop.edge && myState.myPos > stop.endPos - getCarFollowModel().brakeGap(myState.mySpeed)) {
00503         return false;
00504     }
00505     myStops.insert(iter, stop);
00506     return true;
00507 }
00508 
00509 
00510 bool
00511 MSVehicle::isStopped() const {
00512     return !myStops.empty() && myStops.begin()->reached;
00513 }
00514 
00515 
00516 bool
00517 MSVehicle::isParking() const {
00518     return isStopped() && myStops.begin()->parking;
00519 }
00520 
00521 
00522 SUMOReal
00523 MSVehicle::processNextStop(SUMOReal currentVelocity) {
00524     if (myStops.empty()) {
00525         // no stops; pass
00526         return currentVelocity;
00527     }
00528     Stop& stop = myStops.front();
00529     if (stop.reached) {
00530         // ok, we have already reached the next stop
00531         // any waiting persons may board now
00532         bool boarded = MSNet::getInstance()->getPersonControl().boardAnyWaiting(&myLane->getEdge(), this);
00533         if (boarded) {
00534             // the triggering condition has been fulfilled. Maybe we want to wait a bit longer for additional riders (car pooling)
00535             stop.triggered = false;
00536             if (myAmRegisteredAsWaitingForPerson) {
00537                 MSNet::getInstance()->getVehicleControl().unregisterOneWaitingForPerson();
00538                 myAmRegisteredAsWaitingForPerson = false;
00539             }
00540         }
00541         if (stop.duration <= 0 && !stop.triggered) {
00542             // we have waited long enough and fulfilled any passenger-requirements
00543             if (stop.busstop != 0) {
00544                 // inform bus stop about leaving it
00545                 stop.busstop->leaveFrom(this);
00546             }
00547             // the current stop is no longer valid
00548             MSNet::getInstance()->getVehicleControl().removeWaiting(&myLane->getEdge(), this);
00549             myStops.pop_front();
00550             // do not count the stopping time towards gridlock time.
00551             // Other outputs use an independent counter and are not affected.
00552             myWaitingTime = 0;
00553             // maybe the next stop is on the same edge; let's rebuild best lanes
00554             getBestLanes(true);
00555             // continue as wished...
00556         } else {
00557             // we have to wait some more time
00558             if (stop.triggered && !myAmRegisteredAsWaitingForPerson) {
00559                 // we can only register after waiting for one step. otherwise we might falsely signal a deadlock
00560                 MSNet::getInstance()->getVehicleControl().registerOneWaitingForPerson();
00561                 myAmRegisteredAsWaitingForPerson = true;
00562             }
00563             stop.duration -= DELTA_T;
00564             return 0;
00565         }
00566     } else {
00567         // is the next stop on the current lane?
00568         if (stop.edge == myCurrEdge) {
00569             // get the stopping position
00570             SUMOReal endPos = stop.endPos;
00571             bool busStopsMustHaveSpace = true;
00572             if (stop.busstop != 0) {
00573                 // on bus stops, we have to wait for free place if they are in use...
00574                 endPos = stop.busstop->getLastFreePos(*this);
00575                 if (endPos - 5. < stop.busstop->getBeginLanePosition()) { // !!! explicit offset
00576                     busStopsMustHaveSpace = false;
00577                 }
00578             }
00579             if (myState.pos() + getVehicleType().getMinGap() >= endPos - BUS_STOP_OFFSET && busStopsMustHaveSpace) {
00580                 // ok, we may stop (have reached the stop)
00581                 stop.reached = true;
00582                 MSNet::getInstance()->getVehicleControl().addWaiting(&myLane->getEdge(), this);
00583                 // compute stopping time
00584                 if (stop.until >= 0) {
00585                     if (stop.duration == -1) {
00586                         stop.duration = stop.until - MSNet::getInstance()->getCurrentTimeStep();
00587                     } else {
00588                         stop.duration = MAX2(stop.duration, stop.until - MSNet::getInstance()->getCurrentTimeStep());
00589                     }
00590                 }
00591                 if (stop.busstop != 0) {
00592                     // let the bus stop know the vehicle
00593                     stop.busstop->enter(this, myState.pos() + getVehicleType().getMinGap(), myState.pos() - myType->getLength());
00594                 }
00595             }
00596             // decelerate
00597             return getCarFollowModel().stopSpeed(this, endPos - myState.pos());
00598         }
00599     }
00600     return currentVelocity;
00601 }
00602 
00603 
00604 bool
00605 MSVehicle::moveRegardingCritical(SUMOTime t, const MSLane* const lane,
00606                                  const MSVehicle* const pred,
00607                                  const MSVehicle* const neigh,
00608                                  SUMOReal lengthsInFront) {
00609 #ifdef _MESSAGES
00610     if (myHBMsgEmitter != 0) {
00611         if (isOnRoad()) {
00612             SUMOReal timeStep = MSNet::getInstance()->getCurrentTimeStep();
00613             myHBMsgEmitter->writeHeartBeatEvent(myParameter->id, timeStep, myLane, myState.pos(), myState.speed(), getPosition().x(), getPosition().y());
00614         }
00615     }
00616 #endif
00617 #ifdef DEBUG_VEHICLE_GUI_SELECTION
00618     if (gSelected.isSelected(GLO_VEHICLE, static_cast<const GUIVehicle*>(this)->getGlID())) {
00619         int bla = 0;
00620     }
00621 #endif
00622     // remove information about approaching links, will be reset later in this step
00623     for (DriveItemVector::iterator i = myLFLinkLanes.begin(); i != myLFLinkLanes.end(); ++i) {
00624         if ((*i).myLink != 0) {
00625             (*i).myLink->removeApproaching(this);
00626         }
00627     }
00628     myLFLinkLanes.clear();
00629     //
00630     const MSCFModel& cfModel = getCarFollowModel();
00631     SUMOReal vBeg = MIN2(cfModel.maxNextSpeed(myState.mySpeed), lane->getMaxSpeed());
00632     // check whether the vehicle is not on an appropriate lane
00633     bool onAppropriateLane = myLane->appropriate(this);
00634     if (!onAppropriateLane) {
00635         // decelerate to lane end when yes
00636         SUMOReal place = MIN2(myLane->getLength() - (SUMOReal)POSITION_EPS, getVehicleType().getMinGap());
00637         vBeg = MIN2(cfModel.stopSpeed(this, myLane->getLength() - myState.myPos - place), myLane->getMaxSpeed());
00638     }
00639     if (myCurrEdge == myRoute->end() - 1) {
00640         if (myParameter->arrivalSpeedProcedure == ARRIVAL_SPEED_GIVEN) {
00641             vBeg = MIN2(cfModel.freeSpeed(this, getSpeed(), myArrivalPos - myState.myPos, myParameter->arrivalSpeed), myLane->getMaxSpeed());
00642         } else {
00643             vBeg = myLane->getMaxSpeed();
00644         }
00645     }
00646     // interaction with leader
00647     if (pred != 0) {
00648         // interaction with leader if one exists on same lane
00649         SUMOReal gap = gap2pred(*pred);
00650         if (MSGlobals::gCheck4Accidents && gap < 0) {
00651             // collision occured!
00652             return true;
00653         }
00654         vBeg = MIN2(vBeg, cfModel.followSpeed(this, getSpeed(), gap2pred(*pred), pred->getSpeed(), pred->getCarFollowModel().getMaxDecel()));
00655     } else {
00656         // (potential) interaction with a vehicle extending partially into this lane
00657         MSVehicle* predP = myLane->getPartialOccupator();
00658         if (predP != 0 && predP != this) {
00659             SUMOReal gap = myLane->getPartialOccupatorEnd() - myState.myPos - getVehicleType().getMinGap();
00660             if (MSGlobals::gCheck4Accidents && gap < 0) {
00661                 // collision occured!
00662                 return true;
00663             }
00664             vBeg = MIN2(vBeg, cfModel.followSpeed(this, getSpeed(), gap, predP->getSpeed(), predP->getCarFollowModel().getMaxDecel()));
00665         }
00666     }
00667     // interaction with left-lane leader (do not overtake right)
00668     cfModel.leftVehicleVsafe(this, neigh, vBeg);
00669     // check whether the vehicle wants to stop somewhere
00670     if (!myStops.empty() && &myStops.begin()->lane->getEdge() == &lane->getEdge()) {
00671         const Stop& stop = *myStops.begin();
00672         SUMOReal stopPos = stop.busstop == 0 ? stop.endPos : stop.busstop->getLastFreePos(*this) - POSITION_EPS;
00673         SUMOReal seen = lane->getLength() - myState.pos();
00674         SUMOReal vsafeStop = cfModel.stopSpeed(this, seen - (lane->getLength() - stopPos));
00675         vBeg = MIN2(vBeg, vsafeStop);
00676     }
00677     vBeg = MAX2((SUMOReal) 0, vBeg);
00678 #ifdef _DEBUG
00679     if (vBeg < cfModel.getSpeedAfterMaxDecel(myState.mySpeed)) {
00680         WRITE_WARNING("Vehicle '" + getID() + "' is decelerating too much (#1; is: " + toString(myState.mySpeed - vBeg) + ", may: " + toString(cfModel.getSpeedAfterMaxDecel(myState.mySpeed)) + ")");
00681     }
00682 #endif
00683     if (!onAppropriateLane) {
00684         myLFLinkLanes.push_back(DriveProcessItem(0, vBeg, vBeg, false, 0, 0, myLane->getLength() - myState.myPos - getVehicleType().getMinGap()));
00685     } else {
00686         // check whether the driver wants to let someone in
00687         vsafeCriticalCont(t, vBeg);
00688         checkRewindLinkLanes(lengthsInFront);
00689     }
00690     return false;
00691 }
00692 
00693 
00694 bool
00695 MSVehicle::moveChecked() {
00696 #ifdef DEBUG_VEHICLE_GUI_SELECTION
00697     if (gSelected.isSelected(GLO_VEHICLE, static_cast<const GUIVehicle*>(this)->getGlID())) {
00698         int bla = 0;
00699     }
00700 #endif
00701     // get vsafe
00702     SUMOReal vSafe = 0;
00703     myHaveToWaitOnNextLink = false;
00704 
00705     assert(myLFLinkLanes.size() != 0);
00706     DriveItemVector::iterator i;
00707     bool braking = false;
00708     bool lastWasGreenCont = false;
00709     for (i = myLFLinkLanes.begin(); i != myLFLinkLanes.end(); ++i) {
00710         MSLink* link = (*i).myLink;
00711         // the vehicle must change the lane on one of the next lanes
00712         if (link != 0 && (*i).mySetRequest) {
00713             const LinkState ls = link->getState();
00714             // vehicles should brake when running onto a yellow light if the distance allows to halt in front
00715             const bool yellow = ls == LINKSTATE_TL_YELLOW_MAJOR || ls == LINKSTATE_TL_YELLOW_MINOR;
00716             const SUMOReal brakeGap = getCarFollowModel().brakeGap(myState.mySpeed) - getCarFollowModel().getHeadwayTime() * myState.mySpeed;
00717             if (yellow && ((*i).myDistance > brakeGap || myState.mySpeed < ACCEL2SPEED(getCarFollowModel().getMaxDecel()))) {
00718                 vSafe = (*i).myVLinkWait;
00719                 braking = true;
00720                 lastWasGreenCont = false;
00721                 link->removeApproaching(this);
00722                 break;
00723             }
00724             //
00725             const bool opened = yellow || link->opened((*i).myArrivalTime, (*i).myArrivalSpeed, getVehicleType().getLengthWithGap());
00726             // vehicles should decelerate when approaching a minor link
00727             if (opened && !lastWasGreenCont && !link->havePriority() && (*i).myDistance > getCarFollowModel().getMaxDecel()) {
00728                 vSafe = (*i).myVLinkWait;
00729                 braking = true;
00730                 lastWasGreenCont = false;
00731                 if (ls == LINKSTATE_EQUAL) {
00732                     link->removeApproaching(this);
00733                 }
00734                 break; // could be revalidated
00735             }
00736             // have waited; may pass if opened...
00737             if (opened) {
00738                 vSafe = (*i).myVLinkPass;
00739                 lastWasGreenCont = link->isCont() && (ls == LINKSTATE_TL_GREEN_MAJOR);
00740             } else {
00741                 lastWasGreenCont = false;
00742                 vSafe = (*i).myVLinkWait;
00743                 braking = true;
00744                 if (ls == LINKSTATE_EQUAL) {
00745                     link->removeApproaching(this);
00746                 }
00747                 break;
00748             }
00749         } else {
00750             vSafe = (*i).myVLinkWait;
00751             braking = true;
00752             break;
00753         }
00754     }
00755 
00756     if (braking) {
00757         myHaveToWaitOnNextLink = true;
00758     }
00759 
00760     SUMOReal vNext = getCarFollowModel().moveHelper(this, vSafe);
00761     vNext = MAX2(vNext, (SUMOReal) 0.);
00762 #ifndef NO_TRACI
00763     if (myInfluencer != 0) {
00764         SUMOReal vMin = MAX2(SUMOReal(0), getVehicleType().getCarFollowModel().getSpeedAfterMaxDecel(myState.mySpeed));
00765         SUMOReal vMax = getVehicleType().getCarFollowModel().maxNextSpeed(myState.mySpeed);
00766         vNext = myInfluencer->influenceSpeed(MSNet::getInstance()->getCurrentTimeStep(), vNext, vSafe, vMin, vMax);
00767     }
00768 #endif
00769     // visit waiting time
00770     if (vNext <= 0.1) {
00771         myWaitingTime += DELTA_T;
00772         braking = true;
00773     } else {
00774         myWaitingTime = 0;
00775     }
00776     if (myState.mySpeed < vNext) {
00777         braking = false;
00778     }
00779     if (braking) {
00780         switchOnSignal(VEH_SIGNAL_BRAKELIGHT);
00781     } else {
00782         switchOffSignal(VEH_SIGNAL_BRAKELIGHT);
00783     }
00784     // call reminders after vNext is set
00785     SUMOReal pos = myState.myPos;
00786     vNext = MIN2(vNext, getMaxSpeed());
00787 
00788 #ifdef _MESSAGES
00789     if (myHBMsgEmitter != 0) {
00790         if (isOnRoad()) {
00791             SUMOReal timeStep = MSNet::getInstance()->getCurrentTimeStep();
00792             myHBMsgEmitter->writeHeartBeatEvent(myParameter->id, timeStep, myLane, myState.pos(), myState.speed(), getPosition().x(), getPosition().y());
00793         }
00794     }
00795     if (myBMsgEmitter != 0) {
00796         if (vNext < myState.mySpeed) {
00797             SUMOReal timeStep = MSNet::getInstance()->getCurrentTimeStep();
00798             myBMsgEmitter->writeBreakEvent(myParameter->id, timeStep, myLane, myState.pos(), myState.speed(), getPosition().x(), getPosition().y());
00799         }
00800     }
00801 #endif
00802     // update position and speed
00803     myState.myPos += SPEED2DIST(vNext);
00804     myState.mySpeed = vNext;
00805     std::vector<MSLane*> passedLanes;
00806     for (std::vector<MSLane*>::reverse_iterator i = myFurtherLanes.rbegin(); i != myFurtherLanes.rend(); ++i) {
00807         passedLanes.push_back(*i);
00808     }
00809     if (passedLanes.size() == 0 || passedLanes.back() != myLane) {
00810         passedLanes.push_back(myLane);
00811     }
00812     bool moved = true;
00813     // move on lane(s)
00814     if (myState.myPos <= myLane->getLength()) {
00815         // we are staying at our lane
00816         //  there is no need to go over succeeding lanes
00817         workOnMoveReminders(pos, pos + SPEED2DIST(vNext), vNext);
00818         moved = false;
00819     } else {
00820         // we are moving at least to the next lane (maybe pass even more than one)
00821         if (myCurrEdge != myRoute->end() - 1) {
00822             MSLane* approachedLane = myLane;
00823             // move the vehicle forward
00824             for (i = myLFLinkLanes.begin(); i != myLFLinkLanes.end() && approachedLane != 0 && myState.myPos > approachedLane->getLength(); ++i) {
00825                 leaveLane(MSMoveReminder::NOTIFICATION_JUNCTION);
00826                 MSLink* link = (*i).myLink;
00827                 // check whether the vehicle was allowed to enter lane
00828                 //  otherwise it is decelareted and we do not need to test for it's
00829                 //  approach on the following lanes when a lane changing is performed
00830                 myState.myPos -= approachedLane->getLength();
00831                 assert(myState.myPos > 0);
00832                 // proceed to the next lane
00833                 if (link != 0) {
00834 #ifdef HAVE_INTERNAL_LANES
00835                     approachedLane = link->getViaLane();
00836                     if (approachedLane == 0) {
00837                         approachedLane = link->getLane();
00838                     }
00839 #else
00840                     approachedLane = link->getLane();
00841 #endif
00842                 } else {
00843                     approachedLane = 0;
00844                 }
00845                 if (approachedLane != myLane && approachedLane != 0) {
00846                     enterLaneAtMove(approachedLane);
00847                     myLane = approachedLane;
00848                     if (approachedLane->getEdge().isVaporizing()) {
00849                         break;
00850                     }
00851                 }
00852                 passedLanes.push_back(approachedLane);
00853             }
00854         }
00855     }
00856     // clear previously set information
00857     for (std::vector<MSLane*>::iterator i = myFurtherLanes.begin(); i != myFurtherLanes.end(); ++i) {
00858         (*i)->resetPartialOccupation(this);
00859     }
00860     myFurtherLanes.clear();
00861     if (!ends()) {
00862         if (myState.myPos - getVehicleType().getLength() < 0 && passedLanes.size() > 0) {
00863             SUMOReal leftLength = getVehicleType().getLength() - myState.myPos;
00864             std::vector<MSLane*>::reverse_iterator i = passedLanes.rbegin() + 1;
00865             while (leftLength > 0 && i != passedLanes.rend()) {
00866                 myFurtherLanes.push_back(*i);
00867                 leftLength -= (*i)->setPartialOccupation(this, leftLength);
00868                 ++i;
00869             }
00870         }
00871         setBlinkerInformation();
00872     }
00873     return moved;
00874 }
00875 
00876 
00877 SUMOReal
00878 MSVehicle::getSpaceTillLastStanding(MSLane* l, bool& foundStopped) {
00879     SUMOReal lengths = 0;
00880     const MSLane::VehCont& vehs = l->getVehiclesSecure();
00881     for (MSLane::VehCont::const_iterator i = vehs.begin(); i != vehs.end(); ++i) {
00882         if ((*i)->getSpeed() < .1) {
00883             foundStopped = true;
00884             SUMOReal ret = (*i)->getPositionOnLane() - (*i)->getVehicleType().getLengthWithGap() - lengths;
00885             l->releaseVehicles();
00886             return ret;
00887         }
00888         lengths += (*i)->getVehicleType().getLengthWithGap();
00889     }
00890     l->releaseVehicles();
00891     return l->getLength() - lengths;
00892 }
00893 
00894 
00895 void
00896 MSVehicle::checkRewindLinkLanes(SUMOReal lengthsInFront) {
00897 #ifdef DEBUG_VEHICLE_GUI_SELECTION
00898     if (gSelected.isSelected(GLO_VEHICLE, static_cast<const GUIVehicle*>(this)->getGlID())) {
00899         int bla = 0;
00900         if (MSNet::getInstance()->getCurrentTimeStep() == 152000) {
00901             bla = 0;
00902         }
00903     }
00904 #endif
00905 #ifdef HAVE_INTERNAL_LANES
00906     if (MSGlobals::gUsingInternalLanes) {
00907         int removalBegin = -1;
00908         bool hadVehicle = false;
00909         SUMOReal seenSpace = -lengthsInFront;
00910 
00911         std::vector<SUMOReal> availableSpace;
00912         std::vector<bool> hadVehicles;
00913         bool foundStopped = false;
00914 
00915         for (unsigned int i = 0; i < myLFLinkLanes.size(); ++i) {
00916             // skip unset links
00917             DriveProcessItem& item = myLFLinkLanes[i];
00918             if (item.myLink == 0 || foundStopped) {
00919                 availableSpace.push_back(seenSpace);
00920                 hadVehicles.push_back(hadVehicle);
00921                 continue;
00922             }
00923             // get the next lane, determine whether it is an internal lane
00924             MSLane* approachedLane = item.myLink->getViaLane();
00925             if (approachedLane != 0) {
00926                 if (item.myLink->isCrossing()/* && item.myLink->willHaveBlockedFoe()*/) {
00927                     seenSpace = seenSpace - approachedLane->getVehLenSum();
00928                     hadVehicle |= approachedLane->getVehicleNumber() != 0;
00929                 } else {
00930                     seenSpace = seenSpace + getSpaceTillLastStanding(approachedLane, foundStopped);// - approachedLane->getVehLenSum() + approachedLane->getLength();
00931                     hadVehicle |= approachedLane->getVehicleNumber() != 0;
00932                 }
00933                 availableSpace.push_back(seenSpace);
00934                 hadVehicles.push_back(hadVehicle);
00935                 continue;
00936             }
00937             approachedLane = item.myLink->getLane();
00938             MSVehicle* last = approachedLane->getLastVehicle();
00939             if (last == 0) {
00940                 last = approachedLane->getPartialOccupator();
00941                 if (last != 0) {
00942                     SUMOReal m = MAX2(seenSpace, seenSpace + approachedLane->getPartialOccupatorEnd() + last->getCarFollowModel().brakeGap(last->getSpeed()));
00943                     availableSpace.push_back(m);
00944                     hadVehicle = true;
00945                     seenSpace = seenSpace + getSpaceTillLastStanding(approachedLane, foundStopped);// - approachedLane->getVehLenSum() + approachedLane->getLength();
00946                     if (last->myHaveToWaitOnNextLink) {
00947                         foundStopped = true;
00948                     }
00949                 } else {
00950 //                    seenSpace = seenSpace - approachedLane->getVehLenSum() + approachedLane->getLength();
00951 //                    availableSpace.push_back(seenSpace);
00952                     availableSpace.push_back(seenSpace + getSpaceTillLastStanding(approachedLane, foundStopped));
00953                     if (!foundStopped) {
00954                         seenSpace = seenSpace - approachedLane->getVehLenSum() + approachedLane->getLength();
00955                     } else {
00956                         seenSpace = availableSpace.back();
00957                     }
00958                 }
00959             } else {
00960                 if (last->signalSet(VEH_SIGNAL_BRAKELIGHT)) {
00961                     SUMOReal lastBrakeGap = last->getCarFollowModel().brakeGap(approachedLane->getLastVehicle()->getSpeed());
00962                     SUMOReal lastGap = last->getPositionOnLane() - last->getVehicleType().getLengthWithGap() + lastBrakeGap - last->getSpeed() * last->getCarFollowModel().getHeadwayTime();
00963                     SUMOReal m = MAX2(seenSpace, seenSpace + lastGap);
00964                     availableSpace.push_back(m);
00965                     seenSpace = seenSpace + getSpaceTillLastStanding(approachedLane, foundStopped);// - approachedLane->getVehLenSum() + approachedLane->getLength();
00966                 } else {
00967 //                    seenSpace = seenSpace - approachedLane->getVehLenSum() + approachedLane->getLength();
00968 //                    availableSpace.push_back(seenSpace);
00969                     availableSpace.push_back(seenSpace + getSpaceTillLastStanding(approachedLane, foundStopped));
00970                     if (!foundStopped) {
00971                         seenSpace = seenSpace - approachedLane->getVehLenSum() + approachedLane->getLength();
00972                     } else {
00973                         seenSpace = availableSpace.back();
00974                     }
00975                 }
00976                 if (last->myHaveToWaitOnNextLink) {
00977                     foundStopped = true;
00978                 }
00979                 hadVehicle = true;
00980             }
00981             hadVehicles.push_back(hadVehicle);
00982         }
00983 #ifdef DEBUG_VEHICLE_GUI_SELECTION
00984         if (gSelected.isSelected(GLO_VEHICLE, static_cast<const GUIVehicle*>(this)->getGlID())) {
00985             int bla = 0;
00986         }
00987 #endif
00988         SUMOTime t = MSNet::getInstance()->getCurrentTimeStep();
00989         for (int i = (int)(myLFLinkLanes.size() - 1); i > 0; --i) {
00990             DriveProcessItem& item = myLFLinkLanes[i - 1];
00991             bool opened = item.myLink != 0 && (item.myLink->havePriority() || item.myLink->opened(item.myArrivalTime, item.myArrivalSpeed,/*t, .1,*/ getVehicleType().getLengthWithGap()));
00992             bool check1 = item.myLink == 0 || item.myLink->isCont() || !hadVehicles[i];
00993             bool allowsContinuation = check1 || opened;
00994             if (!opened && item.myLink != 0) {
00995                 if (i > 1) {
00996                     DriveProcessItem& item2 = myLFLinkLanes[i - 2];
00997                     if (item2.myLink != 0 && item2.myLink->isCont()) {
00998                         allowsContinuation = true;
00999                     }
01000                 }
01001             }
01002             if (allowsContinuation) {
01003                 availableSpace[i - 1] = availableSpace[i];
01004             }
01005         }
01006 
01007         for (unsigned int i = 0; hadVehicle && i < myLFLinkLanes.size() && removalBegin < 0; ++i) {
01008             // skip unset links
01009             DriveProcessItem& item = myLFLinkLanes[i];
01010             if (item.myLink == 0) {
01011                 continue;
01012             }
01013             /*
01014             SUMOReal impatienceCorrection = MAX2(SUMOReal(0), SUMOReal(SUMOReal(myWaitingTime)));
01015             if (seenSpace<getVehicleType().getLengthWithGap()-impatienceCorrection/10.&&nextSeenNonInternal!=0) {
01016                 removalBegin = lastLinkToInternal;
01017             }
01018             */
01019 
01020             SUMOReal leftSpace = availableSpace[i] - getVehicleType().getLengthWithGap();
01021             if (leftSpace < 0/* && item.myLink->willHaveBlockedFoe()*/) {
01022                 SUMOReal impatienceCorrection = 0;
01023                 /*
01024                 if(item.myLink->getState()==LINKSTATE_MINOR) {
01025                     impatienceCorrection = MAX2(SUMOReal(0), STEPS2TIME(myWaitingTime));
01026                 }
01027                 */
01028                 if (leftSpace < -impatienceCorrection / 10.) {
01029                     removalBegin = i;
01030                 }
01031                 //removalBegin = i;
01032             }
01033         }
01034         if (removalBegin != -1 && !(removalBegin == 0 && myLane->getEdge().getPurpose() == MSEdge::EDGEFUNCTION_INTERNAL)) {
01035             while (removalBegin < (int)(myLFLinkLanes.size())) {
01036                 const SUMOReal brakeGap = getCarFollowModel().brakeGap(myState.mySpeed) - getCarFollowModel().getHeadwayTime() * myState.mySpeed;
01037                 myLFLinkLanes[removalBegin].myVLinkPass = myLFLinkLanes[removalBegin].myVLinkWait;
01038                 if(myLFLinkLanes[removalBegin].myDistance>=brakeGap||(myLFLinkLanes[removalBegin].myDistance>0&&myState.mySpeed<ACCEL2SPEED(getCarFollowModel().getMaxDecel()))) {
01039                     myLFLinkLanes[removalBegin].mySetRequest = false;
01040                 }
01041                 ++removalBegin;
01042             }
01043         }
01044     }
01045 #endif
01046     for (DriveItemVector::iterator i = myLFLinkLanes.begin(); i != myLFLinkLanes.end(); ++i) {
01047         if ((*i).myLink != 0) {
01048             (*i).myLink->setApproaching(this, (*i).myArrivalTime, (*i).myArrivalSpeed, (*i).mySetRequest);
01049         }
01050     }
01051 }
01052 
01053 
01054 
01055 void
01056 MSVehicle::vsafeCriticalCont(SUMOTime t, SUMOReal boundVSafe) {
01057 #ifdef DEBUG_VEHICLE_GUI_SELECTION
01058     if (gSelected.isSelected(GLO_VEHICLE, static_cast<const GUIVehicle*>(this)->getGlID())) {
01059         int bla = 0;
01060     }
01061 #endif
01062 #ifndef NO_TRACI
01063     if (myInfluencer != 0) {
01064         SUMOReal vMin = MAX2(SUMOReal(0), getVehicleType().getCarFollowModel().getSpeedAfterMaxDecel(myState.mySpeed));
01065         SUMOReal vMax = getVehicleType().getCarFollowModel().maxNextSpeed(myState.mySpeed);
01066         boundVSafe = myInfluencer->influenceSpeed(MSNet::getInstance()->getCurrentTimeStep(), boundVSafe, boundVSafe, vMin, vMax);
01067     }
01068 #endif
01069     const MSCFModel& cfModel = getCarFollowModel();
01070     // the vehicle may have just to look into the next lane
01071     //  compute this information and use it only once in the next loop
01072     SUMOReal seen = myLane->getLength() - myState.myPos;
01073     SUMOReal seenNonInternal = 0;
01074     //
01075     if (this != myLane->getFirstVehicle() && seen - cfModel.brakeGap(myState.mySpeed) > 0 && seen - SPEED2DIST(boundVSafe) - ACCEL2DIST(cfModel.getMaxAccel()) > 0) {
01076         // not "reaching critical"
01077         SUMOReal place = MIN2(myLane->getLength() - (SUMOReal)POSITION_EPS, getVehicleType().getMinGap());
01078         myLFLinkLanes.push_back(DriveProcessItem(0, boundVSafe, boundVSafe, false, 0, 0, seen - place));
01079         return;
01080     }
01081 
01082     MSLane* nextLane = myLane;
01083     // compute the way the vehicle would drive if it would use the current speed and then
01084     //  decelerate
01085     SUMOReal maxV = cfModel.maxNextSpeed(myState.mySpeed);
01086     SUMOReal dist = SPEED2DIST(maxV) + cfModel.brakeGap(maxV);
01087     SUMOReal vLinkPass = boundVSafe;
01088     SUMOReal vLinkWait = vLinkPass;
01089     const std::vector<MSLane*> &bestLaneConts = getBestLanesContinuation();
01090 #ifdef HAVE_INTERNAL_LANES
01091     bool hadNonInternal = false;
01092 #else
01093     bool hadNonInternal = true;
01094 #endif
01095 
01096     unsigned int view = 1;
01097     // loop over following lanes
01098     while (true) {
01099         // process stops
01100         if (!myStops.empty() && &myStops.begin()->lane->getEdge() == &nextLane->getEdge()) {
01101             const Stop& stop = *myStops.begin();
01102             const SUMOReal vsafeStop = stop.busstop == 0
01103                                        ? cfModel.stopSpeed(this, seen + stop.endPos)
01104                                        : cfModel.stopSpeed(this, seen + stop.busstop->getLastFreePos(*this) - POSITION_EPS);
01105             vLinkPass = MIN2(vLinkPass, vsafeStop);
01106             vLinkWait = MIN2(vLinkWait, vsafeStop);
01107         }
01108 
01109         // get the next link used
01110         MSLinkCont::const_iterator link = myLane->succLinkSec(*this, view, *nextLane, bestLaneConts);
01111 
01112         // check whether the lane is a dead end
01113         //  (should be valid only on further loop iterations
01114         SUMOReal place = MIN2(nextLane->getLength() - (SUMOReal)POSITION_EPS, getVehicleType().getMinGap());
01115         if (nextLane->isLinkEnd(link)) {
01116             SUMOReal laneEndVSafe = cfModel.stopSpeed(this, seen - place);
01117             if (myCurrEdge + view == myRoute->end()) {
01118                 if (myParameter->arrivalSpeedProcedure == ARRIVAL_SPEED_GIVEN) {
01119                     laneEndVSafe = cfModel.freeSpeed(this, getSpeed(), seen, myParameter->arrivalSpeed);
01120                 } else {
01121                     laneEndVSafe = vLinkPass;
01122                 }
01123             }
01124             // the vehicle will not drive further
01125             assert(MIN2(vLinkPass, laneEndVSafe) >= cfModel.getSpeedAfterMaxDecel(myState.mySpeed));
01126             myLFLinkLanes.push_back(DriveProcessItem(0, MIN2(vLinkPass, laneEndVSafe), MIN2(vLinkPass, laneEndVSafe), false, 0, 0, seen));
01127             return;
01128         }
01129         // the link was passed
01130         vLinkWait = vLinkPass;
01131 
01132 
01133         // get the following lane
01134 #ifdef HAVE_INTERNAL_LANES
01135         nextLane = (*link)->getViaLane();
01136         if (nextLane == 0) {
01137             nextLane = (*link)->getLane();
01138             hadNonInternal = true;
01139             view++;
01140         }
01141 #else
01142         nextLane = (*link)->getLane();
01143         view++;
01144 #endif
01145 
01146         // compute the velocity to use when the link is not blocked by other vehicles
01147         //  the vehicle shall be not faster when reaching the next lane than allowed
01148         SUMOReal vmaxNextLane = MAX2(cfModel.freeSpeed(this, getSpeed(), seen, nextLane->getMaxSpeed()), nextLane->getMaxSpeed());
01149 
01150         // the vehicle shall keep a secure distance to its predecessor
01151         //  (or approach the lane end if the predeccessor is too near)
01152         SUMOReal vsafePredNextLane = 100000;
01153         std::pair<MSVehicle*, SUMOReal> lastOnNext = nextLane->getLastVehicleInformation();
01154         if (lastOnNext.first != 0) {
01155             if (seen + lastOnNext.second >= 0) {
01156                 vsafePredNextLane = cfModel.followSpeed(this, getSpeed(), seen + lastOnNext.second - getVehicleType().getMinGap(), lastOnNext.first->getSpeed(), lastOnNext.first->getCarFollowModel().getMaxDecel());
01157             } else {
01158                 vsafePredNextLane = cfModel.stopSpeed(this, seen - place);
01159             }
01160         }
01161 #ifdef DEBUG_VEHICLE_GUI_SELECTION
01162         if (gSelected.isSelected(GLO_VEHICLE, static_cast<const GUIVehicle*>(this)->getGlID())) {
01163             int bla = 0;
01164         }
01165 #endif
01166         // compute the velocity to use when the link may be used
01167         vLinkPass = MIN3(vLinkPass, vmaxNextLane, vsafePredNextLane);
01168 
01169         // if the link may not be used (is blocked by another vehicle) then let the
01170         //  vehicle decelerate until the end of the street
01171         vLinkWait = MIN3(vLinkPass, vLinkWait, cfModel.stopSpeed(this, seen - place));
01172 #ifdef _DEBUG
01173         if (vLinkWait < cfModel.getSpeedAfterMaxDecel(myState.mySpeed)) {
01174             WRITE_WARNING("Vehicle '" + getID() + "' is decelerating too much (#2; is: " + toString(myState.mySpeed - vLinkWait) + ", may: " + toString(cfModel.getSpeedAfterMaxDecel(myState.mySpeed)) + ")");
01175         }
01176 #endif
01177 
01178         // behaviour in front of not priorised intersections (waiting for priorised foe vehicles)
01179         bool setRequest = false;
01180         // process stops
01181         if (!myStops.empty() && &myStops.begin()->lane->getEdge() == &nextLane->getEdge()) {
01182             const Stop& stop = *myStops.begin();
01183             const SUMOReal vsafeStop = stop.busstop == 0
01184                                        ? cfModel.stopSpeed(this, seen + stop.endPos)
01185                                        : cfModel.stopSpeed(this, seen + stop.busstop->getLastFreePos(*this) - POSITION_EPS);
01186             vLinkPass = MIN2(vLinkPass, vsafeStop);
01187             vLinkWait = MIN2(vLinkWait, vsafeStop);
01188         }
01189         // check whether we approach the final edge
01190         if (myCurrEdge + view == myRoute->end()) {
01191             if (myParameter->arrivalSpeedProcedure == ARRIVAL_SPEED_GIVEN) {
01192                 const SUMOReal vsafe = cfModel.freeSpeed(this, getSpeed(), seen + myArrivalPos, myParameter->arrivalSpeed);
01193                 vLinkPass = MIN2(vLinkPass, vsafe);
01194                 vLinkWait = MIN2(vLinkWait, vsafe);
01195             }
01196         }
01197 
01198         setRequest |= ((*link)->getState() != LINKSTATE_TL_RED && vLinkPass > 0);
01199 //        setRequest |= (seen < cfModel.brakeGap(myState.mySpeed) + SPEED2DIST(myState.mySpeed)*cfModel.getHeadwayTime());
01200         bool yellow = (*link)->getState() == LINKSTATE_TL_YELLOW_MAJOR || (*link)->getState() == LINKSTATE_TL_YELLOW_MINOR;
01201         bool red = (*link)->getState() == LINKSTATE_TL_RED;
01202         if ((yellow || red) && seen > cfModel.brakeGap(myState.mySpeed) - myState.mySpeed*cfModel.getHeadwayTime()) {
01203             vLinkPass = vLinkWait;
01204             setRequest = false;
01205             assert(vLinkWait >= cfModel.getSpeedAfterMaxDecel(myState.mySpeed));
01206             myLFLinkLanes.push_back(DriveProcessItem(*link, vLinkWait, vLinkWait, false, t + TIME2STEPS(seen / vLinkPass), vLinkPass, seen));
01207         }
01208         // the next condition matches the previously one used for determining the difference
01209         //  between critical/non-critical vehicles. Though, one should assume that a vehicle
01210         //  should want to move over an intersection even though it could brake before it!?
01211         //setRequest &= dist - seen > 0;
01212 #ifdef _DEBUG
01213         if (MIN2(vLinkPass, vLinkWait) < cfModel.getSpeedAfterMaxDecel(myState.mySpeed)) {
01214             WRITE_WARNING("Vehicle '" + getID() + "' is decelerating too much (#3; is: " + toString(myState.mySpeed - MIN2(vLinkPass, vLinkWait)) + ", may: " + toString(cfModel.getSpeedAfterMaxDecel(myState.mySpeed)) + ")");
01215         }
01216 #endif
01217         myLFLinkLanes.push_back(DriveProcessItem(*link, vLinkPass, vLinkWait, setRequest, t + TIME2STEPS(seen / vLinkPass), vLinkPass, seen));
01218         seen += nextLane->getLength();
01219         seenNonInternal += nextLane->getEdge().getPurpose() == MSEdge::EDGEFUNCTION_INTERNAL ? 0 : nextLane->getLength();
01220         if (!setRequest || ((vLinkPass <= 0 || seen > dist) && hadNonInternal && seenNonInternal > 50)) {
01221             return;
01222         }
01223     }
01224 }
01225 
01226 
01227 void
01228 MSVehicle::activateReminders(const MSMoveReminder::Notification reason) {
01229     for (MoveReminderCont::iterator rem = myMoveReminders.begin(); rem != myMoveReminders.end();) {
01230         if (rem->first->getLane() != 0 && rem->first->getLane() != getLane()) {
01231             ++rem;
01232         } else {
01233             if (rem->first->notifyEnter(*this, reason)) {
01234                 ++rem;
01235             } else {
01236                 rem = myMoveReminders.erase(rem);
01237             }
01238         }
01239     }
01240 }
01241 
01242 
01243 bool
01244 MSVehicle::enterLaneAtMove(MSLane* enteredLane, bool onTeleporting) {
01245     myAmOnNet = true;
01246     // vaporizing edge?
01247     /*
01248     if (enteredLane->getEdge().isVaporizing()) {
01249         // yep, let's do the vaporization...
01250         myLane = enteredLane;
01251         return true;
01252     }
01253     */
01254     if (!onTeleporting) {
01255         // move mover reminder one lane further
01256         adaptLaneEntering2MoveReminder(*enteredLane);
01257         // set the entered lane as the current lane
01258         myLane = enteredLane;
01259     }
01260 
01261     // internal edges are not a part of the route...
01262     if (enteredLane->getEdge().getPurpose() != MSEdge::EDGEFUNCTION_INTERNAL) {
01263         assert(&enteredLane->getEdge() == *(myCurrEdge + 1));
01264         ++myCurrEdge;
01265     }
01266     if (!onTeleporting) {
01267         // may be optimized: compute only, if the current or the next have more than one lane...!!!
01268         getBestLanes(true);
01269         activateReminders(MSMoveReminder::NOTIFICATION_JUNCTION);
01270 #ifndef NO_TRACI
01271         if (myInfluencer != 0) {
01272             myLaneChangeModel->requestLaneChange(myInfluencer->checkForLaneChanges(MSNet::getInstance()->getCurrentTimeStep(), **myCurrEdge, getLaneIndex()));
01273         }
01274 #endif
01275     }
01276     return ends();
01277 }
01278 
01279 
01280 void
01281 MSVehicle::enterLaneAtLaneChange(MSLane* enteredLane) {
01282     myAmOnNet = true;
01283 #ifdef _MESSAGES
01284     if (myLCMsgEmitter != 0) {
01285         SUMOReal timeStep = MSNet::getInstance()->getCurrentTimeStep();
01286         myLCMsgEmitter->writeLaneChangeEvent(myParameter->id, timeStep, myLane, myState.pos(), myState.speed(), enteredLane, getPosition().x(), getPosition().y());
01287     }
01288 #endif
01289     myLane = enteredLane;
01290     // switch to and activate the new lane's reminders
01291     // keep OldLaneReminders
01292     for (std::vector< MSMoveReminder* >::const_iterator rem = enteredLane->getMoveReminders().begin(); rem != enteredLane->getMoveReminders().end(); ++rem) {
01293         addReminder(*rem);
01294     }
01295     activateReminders(MSMoveReminder::NOTIFICATION_LANE_CHANGE);
01296     SUMOReal leftLength = myState.myPos - getVehicleType().getLength();
01297     if (leftLength < 0) {
01298         // we have to rebuild "further lanes"
01299         const MSRoute& route = getRoute();
01300         MSRouteIterator i = myCurrEdge;
01301         MSLane* lane = myLane;
01302         while (i != route.begin() && leftLength > 0) {
01303             const MSEdge* const prev = *(--i);
01304             const std::vector<MSLane::IncomingLaneInfo> &incomingLanes = lane->getIncomingLanes();
01305             for (std::vector<MSLane::IncomingLaneInfo>::const_iterator j = incomingLanes.begin(); j != incomingLanes.end(); ++j) {
01306                 if (&(*j).lane->getEdge() == prev) {
01307 #ifdef HAVE_INTERNAL_LANES
01308                     (*j).lane->setPartialOccupation(this, leftLength);
01309 #else
01310                     leftLength -= (*j).length;
01311                     (*j).lane->setPartialOccupation(this, leftLength);
01312 #endif
01313                     leftLength -= (*j).lane->getLength();
01314                     break;
01315                 }
01316             }
01317         }
01318     }
01319 #ifndef NO_TRACI
01320     // check if further changes are necessary
01321     if (myInfluencer != 0) {
01322         myLaneChangeModel->requestLaneChange(myInfluencer->checkForLaneChanges(MSNet::getInstance()->getCurrentTimeStep(), **myCurrEdge, getLaneIndex()));
01323     }
01324 #endif
01325 }
01326 
01327 
01328 void
01329 MSVehicle::enterLaneAtInsertion(MSLane* enteredLane, SUMOReal pos, SUMOReal speed, MSMoveReminder::Notification notification) {
01330     myState = State(pos, speed);
01331     assert(myState.myPos >= 0);
01332     assert(myState.mySpeed >= 0);
01333     myWaitingTime = 0;
01334     myLane = enteredLane;
01335     // set and activate the new lane's reminders
01336     for (std::vector< MSMoveReminder* >::const_iterator rem = enteredLane->getMoveReminders().begin(); rem != enteredLane->getMoveReminders().end(); ++rem) {
01337         addReminder(*rem);
01338     }
01339     activateReminders(notification);
01340     std::string msg;
01341     if (MSGlobals::gCheckRoutes && !hasValidRoute(msg)) {
01342         throw ProcessError("Vehicle '" + getID() + "' has no valid route. " + msg);
01343     }
01344     myAmOnNet = true;
01345     // build the list of lanes the vehicle is lapping into
01346     SUMOReal leftLength = myType->getLength() - pos;
01347     MSLane* clane = enteredLane;
01348     while (leftLength > 0) {
01349         clane = clane->getLogicalPredecessorLane();
01350         if (clane == 0) {
01351             break;
01352         }
01353         myFurtherLanes.push_back(clane);
01354         leftLength -= (clane)->setPartialOccupation(this, leftLength);
01355     }
01356 }
01357 
01358 
01359 void
01360 MSVehicle::leaveLane(const MSMoveReminder::Notification reason) {
01361     for (MoveReminderCont::iterator rem = myMoveReminders.begin(); rem != myMoveReminders.end();) {
01362         if (rem->first->notifyLeave(*this, myState.myPos + rem->second, reason)) {
01363             ++rem;
01364         } else {
01365             rem = myMoveReminders.erase(rem);
01366         }
01367     }
01368     if (reason != MSMoveReminder::NOTIFICATION_JUNCTION) {
01369         for (std::vector<MSLane*>::iterator i = myFurtherLanes.begin(); i != myFurtherLanes.end(); ++i) {
01370             (*i)->resetPartialOccupation(this);
01371         }
01372         myFurtherLanes.clear();
01373     }
01374     if (reason >= MSMoveReminder::NOTIFICATION_TELEPORT) {
01375         myAmOnNet = false;
01376     }
01377 }
01378 
01379 
01380 MSAbstractLaneChangeModel&
01381 MSVehicle::getLaneChangeModel() {
01382     return *myLaneChangeModel;
01383 }
01384 
01385 
01386 const MSAbstractLaneChangeModel&
01387 MSVehicle::getLaneChangeModel() const {
01388     return *myLaneChangeModel;
01389 }
01390 
01391 
01392 const std::vector<MSVehicle::LaneQ> &
01393 MSVehicle::getBestLanes(bool forceRebuild, MSLane* startLane) const {
01394 #ifdef DEBUG_VEHICLE_GUI_SELECTION
01395     if (gSelected.isSelected(GLO_VEHICLE, static_cast<const GUIVehicle*>(this)->getGlID())) {
01396         int bla = 0;
01397         myLastBestLanesEdge = 0;
01398     }
01399 #endif
01400 
01401     if (startLane == 0) {
01402         startLane = myLane;
01403     }
01404     // update occupancy and current lane index, only, if the vehicle has not moved to a new lane
01405     if (myLastBestLanesEdge == &startLane->getEdge() && !forceRebuild) {
01406         std::vector<LaneQ> &lanes = *myBestLanes.begin();
01407         std::vector<LaneQ>::iterator i;
01408         for (i = lanes.begin(); i != lanes.end(); ++i) {
01409             SUMOReal nextOccupation = 0;
01410             for (std::vector<MSLane*>::const_iterator j = (*i).bestContinuations.begin() + 1; j != (*i).bestContinuations.end(); ++j) {
01411                 nextOccupation += (*j)->getVehLenSum();
01412             }
01413             (*i).nextOccupation = nextOccupation;
01414             if ((*i).lane == startLane) {
01415                 myCurrentLaneInBestLanes = i;
01416             }
01417         }
01418         return *myBestLanes.begin();
01419     }
01420     // start rebuilding
01421     myLastBestLanesEdge = &startLane->getEdge();
01422     myBestLanes.clear();
01423 
01424     // get information about the next stop
01425     const MSEdge* nextStopEdge = 0;
01426     const MSLane* nextStopLane = 0;
01427     SUMOReal nextStopPos = 0;
01428     if (!myStops.empty()) {
01429         const Stop& nextStop = myStops.front();
01430         nextStopLane = nextStop.lane;
01431         nextStopEdge = &nextStopLane->getEdge();
01432         nextStopPos = nextStop.startPos;
01433     }
01434     if (myParameter->arrivalLaneProcedure == ARRIVAL_LANE_GIVEN && nextStopEdge == 0) {
01435         nextStopEdge = *(myRoute->end() - 1);
01436         nextStopLane = nextStopEdge->getLanes()[myParameter->arrivalLane];
01437         nextStopPos = myArrivalPos;
01438     }
01439 
01440     // go forward along the next lanes;
01441     int seen = 0;
01442     SUMOReal seenLength = 0;
01443     bool progress = true;
01444     for (MSRouteIterator ce = myCurrEdge; progress;) {
01445         std::vector<LaneQ> currentLanes;
01446         const std::vector<MSLane*> *allowed = 0;
01447         const MSEdge* nextEdge = 0;
01448         if (ce != myRoute->end() && ce + 1 != myRoute->end()) {
01449             nextEdge = *(ce + 1);
01450             allowed = (*ce)->allowedLanes(*nextEdge, myType->getVehicleClass());
01451         }
01452         const std::vector<MSLane*> &lanes = (*ce)->getLanes();
01453         for (std::vector<MSLane*>::const_iterator i = lanes.begin(); i != lanes.end(); ++i) {
01454             LaneQ q;
01455             MSLane* cl = *i;
01456             q.lane = cl;
01457             q.bestContinuations.push_back(cl);
01458             q.bestLaneOffset = 0;
01459             q.length = cl->getLength();
01460             q.allowsContinuation = allowed == 0 || find(allowed->begin(), allowed->end(), cl) != allowed->end();
01461             currentLanes.push_back(q);
01462         }
01463         //
01464         if (nextStopEdge == *ce) {
01465             progress = false;
01466             for (std::vector<LaneQ>::iterator q = currentLanes.begin(); q != currentLanes.end(); ++q) {
01467                 if (nextStopLane != (*q).lane) {
01468                     (*q).allowsContinuation = false;
01469                     (*q).length = nextStopPos;
01470                 }
01471             }
01472         }
01473 
01474         myBestLanes.push_back(currentLanes);
01475         ++seen;
01476         seenLength += currentLanes[0].lane->getLength();
01477         ++ce;
01478         progress &= (seen <= 4 || seenLength < 3000);
01479         progress &= seen <= 8;
01480         progress &= ce != myRoute->end();
01481         /*
01482         if(progress) {
01483             progress &= (currentLanes.size()!=1||(*ce)->getLanes().size()!=1);
01484         }
01485         */
01486     }
01487 
01488     // we are examining the last lane explicitly
01489     if (myBestLanes.size() != 0) {
01490         SUMOReal bestLength = -1;
01491         int bestThisIndex = 0;
01492         int index = 0;
01493         std::vector<LaneQ> &last = myBestLanes.back();
01494         for (std::vector<LaneQ>::iterator j = last.begin(); j != last.end(); ++j, ++index) {
01495             if ((*j).length > bestLength) {
01496                 bestLength = (*j).length;
01497                 bestThisIndex = index;
01498             }
01499         }
01500         index = 0;
01501         for (std::vector<LaneQ>::iterator j = last.begin(); j != last.end(); ++j, ++index) {
01502             if ((*j).length < bestLength) {
01503                 (*j).bestLaneOffset = bestThisIndex - index;
01504             }
01505         }
01506     }
01507 
01508     // go backward through the lanes
01509     // track back best lane and compute the best prior lane(s)
01510     for (std::vector<std::vector<LaneQ> >::reverse_iterator i = myBestLanes.rbegin() + 1; i != myBestLanes.rend(); ++i) {
01511         std::vector<LaneQ> &nextLanes = (*(i - 1));
01512         std::vector<LaneQ> &clanes = (*i);
01513         MSEdge& cE = clanes[0].lane->getEdge();
01514         int index = 0;
01515         SUMOReal bestConnectedLength = -1;
01516         SUMOReal bestLength = -1;
01517         for (std::vector<LaneQ>::iterator j = nextLanes.begin(); j != nextLanes.end(); ++j, ++index) {
01518             if ((*j).lane->isApproachedFrom(&cE) && bestConnectedLength < (*j).length) {
01519                 bestConnectedLength = (*j).length;
01520             }
01521             if (bestLength < (*j).length) {
01522                 bestLength = (*j).length;
01523             }
01524         }
01525         if (bestConnectedLength > 0) {
01526             int bestThisIndex = 0;
01527             index = 0;
01528             for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
01529                 LaneQ bestConnectedNext;
01530                 bestConnectedNext.length = -1;
01531                 if ((*j).allowsContinuation) {
01532                     for (std::vector<LaneQ>::const_iterator m = nextLanes.begin(); m != nextLanes.end(); ++m) {
01533                         if ((*m).lane->isApproachedFrom(&cE, (*j).lane)) {
01534                             if (bestConnectedNext.length < (*m).length || (bestConnectedNext.length == (*m).length && abs(bestConnectedNext.bestLaneOffset) > abs((*m).bestLaneOffset))) {
01535                                 bestConnectedNext = *m;
01536                             }
01537                         }
01538                     }
01539                     if (bestConnectedNext.length == bestConnectedLength && abs(bestConnectedNext.bestLaneOffset) < 2) {
01540                         (*j).length += bestLength;
01541                     } else {
01542                         (*j).length += bestConnectedNext.length;
01543                     }
01544                 }
01545                 if (clanes[bestThisIndex].length < (*j).length || (clanes[bestThisIndex].length == (*j).length && abs(abs(clanes[bestThisIndex].bestLaneOffset > (*j).bestLaneOffset)))) {
01546                     bestThisIndex = index;
01547                 }
01548                 copy(bestConnectedNext.bestContinuations.begin(), bestConnectedNext.bestContinuations.end(), back_inserter((*j).bestContinuations));
01549             }
01550 
01551             index = 0;
01552             for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
01553                 if ((*j).length < clanes[bestThisIndex].length || ((*j).length == clanes[bestThisIndex].length && abs((*j).bestLaneOffset) < abs(clanes[bestThisIndex].bestLaneOffset))) {
01554                     (*j).bestLaneOffset = bestThisIndex - index;
01555                 } else {
01556                     (*j).bestLaneOffset = 0;
01557                 }
01558             }
01559 
01560         } else {
01561 
01562             int bestThisIndex = 0;
01563             int bestNextIndex = 0;
01564             int bestDistToNeeded = (int) clanes.size();
01565             index = 0;
01566             for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
01567                 if ((*j).allowsContinuation) {
01568                     int nextIndex = 0;
01569                     for (std::vector<LaneQ>::const_iterator m = nextLanes.begin(); m != nextLanes.end(); ++m, ++nextIndex) {
01570                         if ((*m).lane->isApproachedFrom(&cE, (*j).lane)) {
01571                             if (bestDistToNeeded > abs((*m).bestLaneOffset)) {
01572                                 bestDistToNeeded = abs((*m).bestLaneOffset);
01573                                 bestThisIndex = index;
01574                                 bestNextIndex = nextIndex;
01575                             }
01576                         }
01577                     }
01578                 }
01579             }
01580             clanes[bestThisIndex].length += nextLanes[bestNextIndex].length;
01581             copy(nextLanes[bestNextIndex].bestContinuations.begin(), nextLanes[bestNextIndex].bestContinuations.end(), back_inserter(clanes[bestThisIndex].bestContinuations));
01582             index = 0;
01583             for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
01584                 if ((*j).length < clanes[bestThisIndex].length || ((*j).length == clanes[bestThisIndex].length && abs((*j).bestLaneOffset) < abs(clanes[bestThisIndex].bestLaneOffset))) {
01585                     (*j).bestLaneOffset = bestThisIndex - index;
01586                 } else {
01587                     (*j).bestLaneOffset = 0;
01588                 }
01589             }
01590 
01591         }
01592 
01593     }
01594 
01595     // update occupancy and current lane index
01596     std::vector<LaneQ> &currLanes = *myBestLanes.begin();
01597     std::vector<LaneQ>::iterator i;
01598     for (i = currLanes.begin(); i != currLanes.end(); ++i) {
01599         SUMOReal nextOccupation = 0;
01600         for (std::vector<MSLane*>::const_iterator j = (*i).bestContinuations.begin() + 1; j != (*i).bestContinuations.end(); ++j) {
01601             nextOccupation += (*j)->getVehLenSum();
01602         }
01603         (*i).nextOccupation = nextOccupation;
01604         if ((*i).lane == startLane) {
01605             myCurrentLaneInBestLanes = i;
01606         }
01607     }
01608     return *myBestLanes.begin();
01609 }
01610 
01611 
01612 const std::vector<MSLane*> &
01613 MSVehicle::getBestLanesContinuation() const {
01614     if (myBestLanes.empty() || myBestLanes[0].empty() || myLane->getEdge().getPurpose() == MSEdge::EDGEFUNCTION_INTERNAL) {
01615         return myEmptyLaneVector;
01616     }
01617     return (*myCurrentLaneInBestLanes).bestContinuations;
01618 }
01619 
01620 
01621 const std::vector<MSLane*> &
01622 MSVehicle::getBestLanesContinuation(const MSLane* const l) const {
01623     for (std::vector<std::vector<LaneQ> >::const_iterator i = myBestLanes.begin(); i != myBestLanes.end(); ++i) {
01624         if ((*i).size() != 0 && (*i)[0].lane == l) {
01625             return (*i)[0].bestContinuations;
01626         }
01627     }
01628     return myEmptyLaneVector;
01629 }
01630 
01631 
01632 
01633 SUMOReal
01634 MSVehicle::getDistanceToPosition(SUMOReal destPos, const MSEdge* destEdge) {
01635 #ifdef DEBUG_VEHICLE_GUI_SELECTION
01636     SUMOReal distance = 1000000.;
01637 #else
01638     SUMOReal distance = std::numeric_limits<SUMOReal>::max();
01639 #endif
01640     if (isOnRoad() && destEdge != NULL) {
01641         if (&myLane->getEdge() == *myCurrEdge) {
01642             // vehicle is on a normal edge
01643             distance = myRoute->getDistanceBetween(getPositionOnLane(), destPos, *myCurrEdge, destEdge);
01644         } else {
01645             // vehicle is on inner junction edge
01646             distance = myLane->getLength() - getPositionOnLane();
01647             distance += myRoute->getDistanceBetween(0, destPos, *(myCurrEdge + 1), destEdge);
01648         }
01649     }
01650     return distance;
01651 }
01652 
01653 
01654 SUMOReal
01655 MSVehicle::getHBEFA_CO2Emissions() const {
01656     return HelpersHBEFA::computeCO2(myType->getEmissionClass(), myState.speed(), myPreDawdleAcceleration);
01657 }
01658 
01659 
01660 SUMOReal
01661 MSVehicle::getHBEFA_COEmissions() const {
01662     return HelpersHBEFA::computeCO(myType->getEmissionClass(), myState.speed(), myPreDawdleAcceleration);
01663 }
01664 
01665 
01666 SUMOReal
01667 MSVehicle::getHBEFA_HCEmissions() const {
01668     return HelpersHBEFA::computeHC(myType->getEmissionClass(), myState.speed(), myPreDawdleAcceleration);
01669 }
01670 
01671 
01672 SUMOReal
01673 MSVehicle::getHBEFA_NOxEmissions() const {
01674     return HelpersHBEFA::computeNOx(myType->getEmissionClass(), myState.speed(), myPreDawdleAcceleration);
01675 }
01676 
01677 
01678 SUMOReal
01679 MSVehicle::getHBEFA_PMxEmissions() const {
01680     return HelpersHBEFA::computePMx(myType->getEmissionClass(), myState.speed(), myPreDawdleAcceleration);
01681 }
01682 
01683 
01684 SUMOReal
01685 MSVehicle::getHBEFA_FuelConsumption() const {
01686     return HelpersHBEFA::computeFuel(myType->getEmissionClass(), myState.speed(), myPreDawdleAcceleration);
01687 }
01688 
01689 
01690 SUMOReal
01691 MSVehicle::getHarmonoise_NoiseEmissions() const {
01692     return HelpersHarmonoise::computeNoise(myType->getEmissionClass(), myState.speed(), myPreDawdleAcceleration);
01693 }
01694 
01695 
01696 void
01697 MSVehicle::addPerson(MSPerson* person) {
01698     if (myPersonDevice == 0) {
01699         myPersonDevice = MSDevice_Person::buildVehicleDevices(*this, myDevices);
01700         myMoveReminders.push_back(std::make_pair(myPersonDevice, 0.));
01701     }
01702     myPersonDevice->addPerson(person);
01703     if (myStops.size() > 0 && myStops.front().reached && myStops.front().triggered) {
01704         myStops.front().duration = 0;
01705     }
01706 }
01707 
01708 
01709 void
01710 MSVehicle::setBlinkerInformation() {
01711     switchOffSignal(VEH_SIGNAL_BLINKER_RIGHT | VEH_SIGNAL_BLINKER_LEFT);
01712     int state = getLaneChangeModel().getOwnState();
01713     if ((state & LCA_LEFT) != 0) {
01714         switchOnSignal(VEH_SIGNAL_BLINKER_LEFT);
01715     } else if ((state & LCA_RIGHT) != 0) {
01716         switchOnSignal(VEH_SIGNAL_BLINKER_RIGHT);
01717     } else {
01718         const MSLane* lane = getLane();
01719         MSLinkCont::const_iterator link = lane->succLinkSec(*this, 1, *lane, getBestLanesContinuation());
01720         if (link != lane->getLinkCont().end() && lane->getLength() - getPositionOnLane() < lane->getMaxSpeed() * (SUMOReal) 7.) {
01721             switch ((*link)->getDirection()) {
01722                 case LINKDIR_TURN:
01723                 case LINKDIR_LEFT:
01724                 case LINKDIR_PARTLEFT:
01725                     switchOnSignal(VEH_SIGNAL_BLINKER_LEFT);
01726                     break;
01727                 case LINKDIR_RIGHT:
01728                 case LINKDIR_PARTRIGHT:
01729                     switchOnSignal(VEH_SIGNAL_BLINKER_RIGHT);
01730                     break;
01731                 default:
01732                     break;
01733             }
01734         }
01735     }
01736 
01737 }
01738 
01739 
01740 void
01741 MSVehicle::replaceVehicleType(MSVehicleType* type) {
01742     if (myType->amVehicleSpecific()) {
01743         delete myType;
01744     }
01745     myType = type;
01746 }
01747 
01748 unsigned int
01749 MSVehicle::getLaneIndex() const {
01750     std::vector<MSLane*>::const_iterator laneP = std::find((*myCurrEdge)->getLanes().begin(), (*myCurrEdge)->getLanes().end(), myLane);
01751     return (unsigned int) std::distance((*myCurrEdge)->getLanes().begin(), laneP);
01752 }
01753 
01754 
01755 #ifndef NO_TRACI
01756 bool
01757 MSVehicle::addTraciStop(MSLane* lane, SUMOReal pos, SUMOReal /*radius*/, SUMOTime duration) {
01758     //if the stop exists update the duration
01759     for (std::list<Stop>::iterator iter = myStops.begin(); iter != myStops.end(); iter++) {
01760         if (iter->lane == lane && fabs(iter->endPos - pos) < POSITION_EPS) {
01761             if (duration == 0 && !iter->reached) {
01762                 myStops.erase(iter);
01763             } else {
01764                 iter->duration = duration;
01765             }
01766             return true;
01767         }
01768     }
01769 
01770     SUMOVehicleParameter::Stop newStop;
01771     newStop.lane = lane->getID();
01772     newStop.busstop = MSNet::getInstance()->getBusStopID(lane, pos);
01773     newStop.startPos = pos - POSITION_EPS;
01774     newStop.endPos = pos;
01775     newStop.duration = duration;
01776     newStop.until = -1;
01777     newStop.triggered = false;
01778     newStop.parking = false;
01779     newStop.index = STOP_INDEX_END;
01780     return addStop(newStop);
01781 }
01782 
01783 
01784 MSVehicle::Influencer&
01785 MSVehicle::getInfluencer() {
01786     if (myInfluencer == 0) {
01787         myInfluencer = new Influencer();
01788     }
01789     return *myInfluencer;
01790 }
01791 
01792 
01793 SUMOReal
01794 MSVehicle::getSpeedWithoutTraciInfluence() const {
01795     if (myInfluencer != 0) {
01796         return myInfluencer->getOriginalSpeed();
01797     }
01798     return myState.mySpeed;
01799 }
01800 
01801 
01802 #endif
01803 
01804 
01805 /****************************************************************************/
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Friends Defines