LCOV - code coverage report
Current view: top level - src/microsim/transportables - MSPModel_Striping.cpp (source / functions) Coverage Total Hit
Test: lcov.info Lines: 86.3 % 1322 1141
Test Date: 2024-12-21 15:45:41 Functions: 98.6 % 69 68

            Line data    Source code
       1              : /****************************************************************************/
       2              : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
       3              : // Copyright (C) 2014-2024 German Aerospace Center (DLR) and others.
       4              : // This program and the accompanying materials are made available under the
       5              : // terms of the Eclipse Public License 2.0 which is available at
       6              : // https://www.eclipse.org/legal/epl-2.0/
       7              : // This Source Code may also be made available under the following Secondary
       8              : // Licenses when the conditions for such availability set forth in the Eclipse
       9              : // Public License 2.0 are satisfied: GNU General Public License, version 2
      10              : // or later which is available at
      11              : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
      12              : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
      13              : /****************************************************************************/
      14              : /// @file    MSPModel_Striping.cpp
      15              : /// @author  Jakob Erdmann
      16              : /// @author  Michael Behrisch
      17              : /// @date    Mon, 13 Jan 2014
      18              : ///
      19              : // The pedestrian following model (prototype)
      20              : /****************************************************************************/
      21              : #include <config.h>
      22              : 
      23              : #include <cmath>
      24              : #include <algorithm>
      25              : #include <utils/common/RandHelper.h>
      26              : #include <utils/geom/GeomHelper.h>
      27              : #include <utils/options/OptionsCont.h>
      28              : #include <utils/router/PedestrianRouter.h>
      29              : #include <microsim/MSNet.h>
      30              : #include <microsim/MSEdge.h>
      31              : #include <microsim/MSEventControl.h>
      32              : #include <microsim/MSLane.h>
      33              : #include <microsim/MSLink.h>
      34              : #include <microsim/MSJunction.h>
      35              : #include <microsim/MSStoppingPlace.h>
      36              : #include <microsim/MSVehicleControl.h>
      37              : #include <microsim/MSGlobals.h>
      38              : #include <microsim/transportables/MSStage.h>
      39              : #include <microsim/transportables/MSTransportableControl.h>
      40              : #include "MSPModel_Striping.h"
      41              : 
      42              : 
      43              : // ===========================================================================
      44              : // DEBUGGING HELPERS
      45              : // ===========================================================================
      46              : //
      47              : #define DEBUGID1 ""
      48              : #define DEBUGID2 ""
      49              : //#define DEBUGCOND(PED) (false)
      50              : //#define DEBUGCOND(PED) ((PED).getPerson()->getID() == DEBUGID1 || (PED).getPerson()->getID() == DEBUGID2)
      51              : #define DEBUGCOND(PED) ((PED).getPerson()->isSelected())
      52              : #define DEBUGCOND2(LANE) ((LANE)->isSelected())
      53              : //#define LOG_ALL 1
      54              : //#define DEBUG_MOVETOXY
      55              : 
      56            0 : void MSPModel_Striping::DEBUG_PRINT(const Obstacles& obs) {
      57            0 :     for (int i = 0; i < (int)obs.size(); ++i) {
      58              :         std::cout
      59            0 :                 << "(" << obs[i].description
      60            0 :                 << " x=(" << obs[i].xBack << "," << obs[i].xFwd
      61            0 :                 << ") s=" << obs[i].speed
      62            0 :                 << ")   ";
      63              :     }
      64            0 :     std::cout << "\n";
      65            0 : }
      66              : 
      67              : // ===========================================================================
      68              : // named (internal) constants
      69              : // ===========================================================================
      70              : 
      71              : // distances are comparable with lower values being "more important"
      72              : const double MSPModel_Striping::DIST_FAR_AWAY(10000);
      73              : const double MSPModel_Striping::DIST_BEHIND(1000);
      74              : const double MSPModel_Striping::DIST_OVERLAP(-1);
      75              : 
      76              : // ===========================================================================
      77              : // static members
      78              : // ===========================================================================
      79              : 
      80              : MSPModel_Striping::WalkingAreaPaths MSPModel_Striping::myWalkingAreaPaths;
      81              : std::map<const MSEdge*, std::vector<const MSLane*> >  MSPModel_Striping::myWalkingAreaFoes;
      82              : MSPModel_Striping::MinNextLengths MSPModel_Striping::myMinNextLengths;
      83              : 
      84              : // model parameters (static to simplify access from class PState
      85              : double MSPModel_Striping::stripeWidth;
      86              : double MSPModel_Striping::dawdling;
      87              : double MSPModel_Striping::minGapToVehicle;
      88              : int MSPModel_Striping::myWalkingAreaDetail;
      89              : SUMOTime MSPModel_Striping::jamTime;
      90              : SUMOTime MSPModel_Striping::jamTimeCrossing;
      91              : SUMOTime MSPModel_Striping::jamTimeNarrow;
      92              : double MSPModel_Striping::jamFactor;
      93              : bool MSPModel_Striping::myLegacyPosLat;
      94              : const double MSPModel_Striping::LOOKAHEAD_SAMEDIR(4.0); // seconds
      95              : const double MSPModel_Striping::LOOKAHEAD_ONCOMING(10.0); // seconds
      96              : const double MSPModel_Striping::LOOKAHEAD_ONCOMING_DIST(10.0); // m
      97              : const double MSPModel_Striping::LOOKAROUND_VEHICLES(60.0); // meters
      98              : const double MSPModel_Striping::LATERAL_PENALTY(-1.); // meters
      99              : const double MSPModel_Striping::OBSTRUCTED_PENALTY(-300000.); // meters
     100              : const double MSPModel_Striping::INAPPROPRIATE_PENALTY(-20000.); // meters
     101              : const double MSPModel_Striping::ONCOMING_CONFLICT_PENALTY(-1000.); // meters
     102              : const double MSPModel_Striping::OBSTRUCTION_THRESHOLD(MSPModel_Striping::OBSTRUCTED_PENALTY * 0.5); // despite obstruction, additional utility may have been added
     103              : const double MSPModel_Striping::SQUEEZE(0.7);
     104              : double MSPModel_Striping::RESERVE_FOR_ONCOMING_FACTOR;
     105              : double MSPModel_Striping::RESERVE_FOR_ONCOMING_FACTOR_JUNCTIONS;
     106              : double MSPModel_Striping::RESERVE_FOR_ONCOMING_MAX;
     107              : bool MSPModel_Striping::USE_NET_SPEEDS(false);
     108              : const double MSPModel_Striping::MAX_WAIT_TOLERANCE(120.); // seconds
     109              : const double MSPModel_Striping::LATERAL_SPEED_FACTOR(0.4);
     110              : const double MSPModel_Striping::MIN_STARTUP_DIST(0.4); // meters
     111              : 
     112              : 
     113              : // ===========================================================================
     114              : // MSPModel_Striping method definitions
     115              : // ===========================================================================
     116              : 
     117         5163 : MSPModel_Striping::MSPModel_Striping(const OptionsCont& oc, MSNet* net) {
     118         5163 :     myWalkingAreaDetail = oc.getInt("pedestrian.striping.walkingarea-detail");
     119         5163 :     initWalkingAreaPaths(net);
     120              :     // configurable parameters
     121         5163 :     stripeWidth = oc.getFloat("pedestrian.striping.stripe-width");
     122         5163 :     MSVehicleType* defaultPedType = MSNet::getInstance()->getVehicleControl().getVType(DEFAULT_PEDTYPE_ID, nullptr, true);
     123         5163 :     if (defaultPedType != nullptr && defaultPedType->getWidth() > stripeWidth) {
     124           12 :         WRITE_WARNINGF(TL("Pedestrian vType '%' width % is larger than pedestrian.striping.stripe-width and this may cause collisions with vehicles."),
     125              :                        DEFAULT_PEDTYPE_ID, defaultPedType->getWidth());
     126              :     }
     127              : 
     128         5163 :     dawdling = oc.getFloat("pedestrian.striping.dawdling");
     129         5163 :     minGapToVehicle = oc.getFloat("pedestrian.striping.mingap-to-vehicle");
     130         5163 :     RESERVE_FOR_ONCOMING_FACTOR = oc.getFloat("pedestrian.striping.reserve-oncoming");
     131         5163 :     RESERVE_FOR_ONCOMING_FACTOR_JUNCTIONS = oc.getFloat("pedestrian.striping.reserve-oncoming.junctions");
     132         5163 :     RESERVE_FOR_ONCOMING_MAX = oc.getFloat("pedestrian.striping.reserve-oncoming.max");
     133              :     // beginning with 1.20.0, sensible default speeds were set for crossings and walkingareas
     134         5163 :     USE_NET_SPEEDS = net->getNetworkVersion() >= MMVersion(1, 20);
     135              : 
     136         5163 :     jamTime = string2time(oc.getString("pedestrian.striping.jamtime"));
     137         5163 :     if (jamTime <= 0) {
     138            4 :         jamTime = SUMOTime_MAX;
     139              :     }
     140         5163 :     jamTimeCrossing = string2time(oc.getString("pedestrian.striping.jamtime.crossing"));
     141         5163 :     if (jamTimeCrossing <= 0) {
     142            0 :         jamTimeCrossing = SUMOTime_MAX;
     143              :     }
     144         5163 :     jamTimeNarrow = string2time(oc.getString("pedestrian.striping.jamtime.narrow"));
     145         5163 :     if (jamTimeNarrow <= 0) {
     146            0 :         jamTimeNarrow = SUMOTime_MAX;
     147              :     }
     148         5163 :     jamFactor = oc.getFloat("pedestrian.striping.jamfactor");
     149         5163 :     myLegacyPosLat = oc.getBool("pedestrian.striping.legacy-departposlat");
     150         5163 : }
     151              : 
     152              : 
     153        10308 : MSPModel_Striping::~MSPModel_Striping() {
     154              :     myWalkingAreaPaths.clear(); // need to recompute when lane pointers change
     155              :     myWalkingAreaFoes.clear();
     156              :     myMinNextLengths.clear();
     157        10308 : }
     158              : 
     159              : 
     160              : MSTransportableStateAdapter*
     161       243767 : MSPModel_Striping::add(MSTransportable* transportable, MSStageMoving* stage, SUMOTime) {
     162       243767 :     if (!transportable->isPerson()) {
     163              :         // containers are not supported (TODO add a warning here?)
     164              :         return nullptr;
     165              :     }
     166              :     MSPerson* person = static_cast<MSPerson*>(transportable);
     167       243767 :     MSNet* net = MSNet::getInstance();
     168       243767 :     if (!myAmActive) {
     169         3873 :         net->getBeginOfTimestepEvents()->addEvent(new MovePedestrians(this), net->getCurrentTimeStep() + DELTA_T);
     170         3873 :         myAmActive = true;
     171              :     }
     172              :     assert(person->getCurrentStageType() == MSStageType::WALKING);
     173       243767 :     const MSLane* lane = stage->checkDepartLane(person->getEdge(), person->getVClass(), stage->getDepartLane(), person->getID());
     174       243767 :     if (lane == nullptr) {
     175            8 :         const char* error = TL("Person '%' could not find sidewalk on edge '%', time=%.");
     176           16 :         if (OptionsCont::getOptions().getBool("ignore-route-errors")) {
     177           24 :             WRITE_WARNINGF(error, person->getID(), person->getEdge()->getID(), time2string(net->getCurrentTimeStep()));
     178            8 :             return nullptr;
     179              :         } else {
     180            0 :             throw ProcessError(TLF(error, person->getID(), person->getEdge()->getID(), time2string(net->getCurrentTimeStep())));
     181              :         }
     182              :     }
     183       243759 :     PState* ped = new PState(person, stage, lane);
     184       243755 :     myActiveLanes[lane].push_back(ped);
     185       243755 :     myNumActivePedestrians++;
     186       243755 :     return ped;
     187              : }
     188              : 
     189              : 
     190              : MSTransportableStateAdapter*
     191           14 : MSPModel_Striping::loadState(MSTransportable* transportable, MSStageMoving* stage, std::istringstream& in) {
     192              :     MSPerson* person = static_cast<MSPerson*>(transportable);
     193           14 :     MSNet* net = MSNet::getInstance();
     194           14 :     if (!myAmActive) {
     195           12 :         net->getBeginOfTimestepEvents()->addEvent(new MovePedestrians(this), SIMSTEP);
     196           12 :         myAmActive = true;
     197              :     }
     198           14 :     PState* ped = new PState(person, stage, &in);
     199           14 :     myActiveLanes[ped->getLane()].push_back(ped);
     200           14 :     myNumActivePedestrians++;
     201           14 :     return ped;
     202              : }
     203              : 
     204              : 
     205              : int
     206   7775603838 : MSPModel_Striping::numStripes(const MSLane* lane) {
     207   7775603838 :     return MAX2(1, (int)floor(lane->getWidth() / stripeWidth));
     208              : }
     209              : 
     210              : 
     211              : int
     212       879003 : MSPModel_Striping::connectedDirection(const MSLane* from, const MSLane* to) {
     213       879003 :     if (from == nullptr || to == nullptr) {
     214            0 :         return UNDEFINED_DIRECTION;
     215       879003 :     } else if (from->getLinkTo(to) != nullptr) {
     216       595760 :         return FORWARD;
     217       283243 :     } else if (to->getLinkTo(from) != nullptr) {
     218       283243 :         return BACKWARD;
     219              :     } else {
     220            0 :         return UNDEFINED_DIRECTION;
     221              :     }
     222              : }
     223              : 
     224              : 
     225              : void
     226         5163 : MSPModel_Striping::initWalkingAreaPaths(const MSNet*) {
     227         5163 :     if (myWalkingAreaPaths.size() > 0) {
     228              :         return;
     229              :     }
     230              :     // collect vehicle lanes that cross walkingareas
     231       393509 :     for (MSEdgeVector::const_iterator i = MSEdge::getAllEdges().begin(); i != MSEdge::getAllEdges().end(); ++i) {
     232       388346 :         const MSEdge* edge = *i;
     233       388346 :         if (!edge->isWalkingArea() && !edge->isCrossing()) {
     234       693066 :             for (MSLane* lane : edge->getLanes()) {
     235       806391 :                 for (MSLink* link : lane->getLinkCont()) {
     236       444972 :                     if (link->getWalkingAreaFoe() != nullptr) {
     237              :                         // link is an exit link
     238         2846 :                         myWalkingAreaFoes[&link->getWalkingAreaFoe()->getEdge()].push_back(link->getLaneBefore());
     239              :                         //std::cout << " wa=" << link->getWalkingAreaFoe()->getEdge().getID() << " foe=" << link->getLaneBefore()->getID() << "\n";
     240              :                     }
     241       444972 :                     if (link->getWalkingAreaFoeExit() != nullptr) {
     242              :                         // link is an exit link
     243         2932 :                         myWalkingAreaFoes[&link->getWalkingAreaFoeExit()->getEdge()].push_back(link->getLaneBefore());
     244              :                         //std::cout << " wa=" << link->getWalkingAreaFoeExit()->getEdge().getID() << " foe=" << link->getLaneBefore()->getID() << "\n";
     245              :                     }
     246              :                 }
     247              :             }
     248              :         }
     249              :     }
     250              : 
     251              :     // build walkingareaPaths
     252       393509 :     for (MSEdgeVector::const_iterator i = MSEdge::getAllEdges().begin(); i != MSEdge::getAllEdges().end(); ++i) {
     253       388346 :         insertWalkArePaths(*i, myWalkingAreaPaths);
     254              :     }
     255              : }
     256              : 
     257              : 
     258              : void
     259       388346 : MSPModel_Striping::insertWalkArePaths(const MSEdge* edge, WalkingAreaPaths& into) {
     260       388346 :     if (edge->isWalkingArea()) {
     261        49218 :         const MSLane* walkingArea = getSidewalk<MSEdge, MSLane>(edge);
     262        49218 :         myMinNextLengths[walkingArea] = walkingArea->getLength();
     263              :         // build all possible paths across this walkingArea
     264              :         // gather all incident lanes
     265              :         std::vector<const MSLane*> lanes;
     266       128946 :         for (const MSEdge* in : edge->getPredecessors()) {
     267        79728 :             if (!in->isTazConnector()) {
     268        57434 :                 lanes.push_back(getSidewalk<MSEdge, MSLane>(in));
     269        57434 :                 if (lanes.back() == nullptr) {
     270            0 :                     throw ProcessError("Invalid connection from edge '" + in->getID() + "' to walkingarea edge '" + edge->getID() + "'");
     271              :                 }
     272              :             }
     273              :         }
     274       129131 :         for (const MSEdge* out : edge->getSuccessors()) {
     275        79913 :             if (!out->isTazConnector()) {
     276        57619 :                 lanes.push_back(getSidewalk<MSEdge, MSLane>(out));
     277        57619 :                 if (lanes.back() == nullptr) {
     278            0 :                     throw ProcessError("Invalid connection from walkingarea edge '" + edge->getID() + "' to edge '" + out->getID() + "'");
     279              :                 }
     280              :             }
     281              :         }
     282              :         // build all combinations
     283       164271 :         for (int j = 0; j < (int)lanes.size(); ++j) {
     284       408258 :             for (int k = 0; k < (int)lanes.size(); ++k) {
     285       293205 :                 if (j != k) {
     286              :                     // build the walkingArea
     287       178152 :                     const MSLane* const from = lanes[j];
     288       178152 :                     const MSLane* const to = lanes[k];
     289       178152 :                     const int fromDir = from->getLinkTo(walkingArea) != nullptr ? FORWARD : BACKWARD;
     290       178152 :                     const int toDir = walkingArea->getLinkTo(to) != nullptr ? FORWARD : BACKWARD;
     291       178152 :                     PositionVector shape;
     292       267493 :                     Position fromPos = from->getShape()[fromDir == FORWARD ? -1 : 0];
     293       266963 :                     Position toPos = to->getShape()[toDir == FORWARD ? 0 : -1];
     294       178152 :                     const double maxExtent = fromPos.distanceTo2D(toPos) / 4; // prevent sharp corners
     295       178152 :                     const double extrapolateBy = MIN2(maxExtent, walkingArea->getWidth() / 2);
     296              :                     // assemble shape
     297       178152 :                     shape.push_back(fromPos);
     298       178152 :                     if (extrapolateBy > POSITION_EPS) {
     299              :                         PositionVector fromShp = from->getShape();
     300       177172 :                         fromShp.extrapolate(extrapolateBy);
     301       354344 :                         shape.push_back_noDoublePos(fromDir == FORWARD ? fromShp.back() : fromShp.front());
     302              :                         PositionVector nextShp = to->getShape();
     303       177172 :                         nextShp.extrapolate(extrapolateBy);
     304       354344 :                         shape.push_back_noDoublePos(toDir == FORWARD ? nextShp.front() : nextShp.back());
     305       177172 :                     }
     306       178152 :                     shape.push_back_noDoublePos(toPos);
     307       178152 :                     if (shape.size() < 2) {
     308              :                         PositionVector fromShp = from->getShape();
     309          546 :                         fromShp.extrapolate(1.5 * POSITION_EPS); // noDoublePos requires a difference of POSITION_EPS in at least one coordinate
     310         1092 :                         shape.push_back_noDoublePos(fromDir == FORWARD ? fromShp.back() : fromShp.front());
     311              :                         assert(shape.size() == 2);
     312       178152 :                     } else if (myWalkingAreaDetail > 4) {
     313          448 :                         shape = shape.bezier(myWalkingAreaDetail);
     314              :                     }
     315              :                     double angleOverride = INVALID_DOUBLE;
     316       178152 :                     if (shape.size() >= 4 && shape.length() < walkingArea->getWidth()) {
     317        45380 :                         const double aStart = shape.angleAt2D(0);
     318        45380 :                         const double aEnd = shape.angleAt2D((int)shape.size() - 2);
     319        45380 :                         if (fabs(aStart - aEnd) < DEG2RAD(10)) {
     320         7414 :                             angleOverride = (aStart + aEnd) / 2;
     321              :                         }
     322              :                     }
     323       178152 :                     if (fromDir == BACKWARD) {
     324              :                         // will be walking backward on walkingArea
     325       178682 :                         shape = shape.reverse();
     326              :                     }
     327       178152 :                     WalkingAreaPath wap = WalkingAreaPath(from, walkingArea, to, shape, fromDir, angleOverride);
     328            0 :                     into.insert(std::make_pair(std::make_pair(from, to), wap));
     329       356304 :                     myMinNextLengths[walkingArea] = MIN2(myMinNextLengths[walkingArea], wap.length);
     330       178152 :                 }
     331              :             }
     332              :         }
     333        49218 :     }
     334       388346 : }
     335              : 
     336              : 
     337              : const MSPModel_Striping::WalkingAreaPath*
     338            8 : MSPModel_Striping::getArbitraryPath(const MSEdge* walkingArea) {
     339              :     assert(walkingArea->isWalkingArea());
     340              :     std::vector<const MSLane*> lanes;
     341           24 :     for (const MSEdge* const pred : walkingArea->getPredecessors()) {
     342           16 :         lanes.push_back(getSidewalk<MSEdge, MSLane>(pred));
     343              :     }
     344           24 :     for (const MSEdge* const succ : walkingArea->getSuccessors()) {
     345           16 :         lanes.push_back(getSidewalk<MSEdge, MSLane>(succ));
     346              :     }
     347            8 :     if (lanes.size() < 1) {
     348            0 :         throw ProcessError(TLF("Invalid walkingarea '%' does not allow continuation.", walkingArea->getID()));
     349              :     }
     350            8 :     return &myWalkingAreaPaths.find(std::make_pair(lanes.front(), lanes.back()))->second;
     351            8 : }
     352              : 
     353              : const MSPModel_Striping::WalkingAreaPath*
     354           59 : MSPModel_Striping::guessPath(const MSEdge* walkingArea, const MSEdge* before, const MSEdge* after) {
     355              :     assert(walkingArea->isWalkingArea());
     356           59 :     const MSLane* swBefore = getSidewalk<MSEdge, MSLane>(before);
     357           59 :     const MSLane* swAfter = getSidewalk<MSEdge, MSLane>(after);
     358           59 :     const auto pathIt = myWalkingAreaPaths.find(std::make_pair(swBefore, swAfter));
     359           59 :     if (pathIt != myWalkingAreaPaths.end()) {
     360           24 :         return &pathIt->second;
     361              :     }
     362              :     const MSEdgeVector& preds = walkingArea->getPredecessors();
     363           35 :     const MSEdgeVector& succs = walkingArea->getSuccessors();
     364           35 :     bool useBefore = swBefore != nullptr && std::find(preds.begin(), preds.end(), before) != preds.end();
     365           35 :     bool useAfter = swAfter != nullptr && std::find(succs.begin(), succs.end(), after) != succs.end();
     366           35 :     if (useBefore) {
     367           27 :         if (useAfter) {
     368            0 :             return getWalkingAreaPath(walkingArea, swBefore, swAfter);
     369           27 :         } else if (succs.size() > 0) {
     370              :             // could also try to exploit direction
     371           27 :             return getWalkingAreaPath(walkingArea, swBefore, getSidewalk<MSEdge, MSLane>(succs.front()));
     372              :         }
     373            8 :     } else if (useAfter && preds.size() > 0) {
     374              :         // could also try to exploit direction
     375            8 :         return getWalkingAreaPath(walkingArea, getSidewalk<MSEdge, MSLane>(preds.front()), swAfter);
     376              :     }
     377            0 :     return getArbitraryPath(walkingArea);
     378              : }
     379              : 
     380              : 
     381              : const MSPModel_Striping::WalkingAreaPath*
     382       913402 : MSPModel_Striping::getWalkingAreaPath(const MSEdge* walkingArea, const MSLane* before, const MSLane* after) {
     383              :     assert(walkingArea->isWalkingArea());
     384       913402 :     const auto pathIt = myWalkingAreaPaths.find(std::make_pair(before, after));
     385       913402 :     if (pathIt != myWalkingAreaPaths.end()) {
     386       913361 :         return &pathIt->second;
     387              :     } else {
     388              :         // this can happen in case of moveToXY where before can point anywhere
     389              :         const MSEdgeVector& preds = walkingArea->getPredecessors();
     390           41 :         if (preds.size() > 0) {
     391           41 :             const MSEdge* const pred = walkingArea->getPredecessors().front();
     392           41 :             const auto pathIt2 = myWalkingAreaPaths.find(std::make_pair(getSidewalk<MSEdge, MSLane>(pred), after));
     393              :             assert(pathIt2 != myWalkingAreaPaths.end());
     394           41 :             return &pathIt2->second;
     395              :         } else {
     396            0 :             return getArbitraryPath(walkingArea);
     397              :         }
     398              :     }
     399              : }
     400              : 
     401              : 
     402              : 
     403              : MSPModel_Striping::NextLaneInfo
     404      1528106 : MSPModel_Striping::getNextLane(const PState& ped, const MSLane* currentLane, const MSLane* prevLane) {
     405              :     const MSEdge* currentEdge = &currentLane->getEdge();
     406      1528106 :     const MSJunction* junction = ped.getDirection() == FORWARD ? currentEdge->getToJunction() : currentEdge->getFromJunction();
     407      1528106 :     const MSEdge* nextRouteEdge = ped.getStage()->getNextRouteEdge();
     408      1528106 :     const MSLane* nextRouteLane = getSidewalk<MSEdge, MSLane>(nextRouteEdge, ped.getPerson()->getVClass());
     409              :     // result values
     410              :     const MSLane* nextLane = nextRouteLane;
     411      1528106 :     const MSLink* link = nullptr;
     412      1528106 :     int nextDir = UNDEFINED_DIRECTION;
     413              : 
     414              :     //if DEBUGCOND(ped) {
     415              :     //    std::cout << "  nextRouteLane=" << Named::getIDSecure(nextRouteLane) << " junction=" << junction->getID() << "\n";
     416              :     //}
     417      1528106 :     if (nextRouteLane == nullptr && nextRouteEdge != nullptr) {
     418           56 :         std::string error = "Person '" + ped.getPerson()->getID() + "' could not find sidewalk on edge '" + nextRouteEdge->getID() + "', time="
     419           56 :                             + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".";
     420           56 :         if (OptionsCont::getOptions().getBool("ignore-route-errors")) {
     421           76 :             WRITE_WARNING(error);
     422           24 :             nextRouteLane = nextRouteEdge->getLanes().front();
     423              :         } else {
     424            8 :             throw ProcessError(error);
     425              :         }
     426              :     }
     427              : 
     428      1528102 :     if (nextRouteLane != nullptr) {
     429      1304661 :         if (currentEdge->isInternal()) {
     430              :             assert(junction == currentEdge->getFromJunction());
     431        25268 :             nextDir = junction == nextRouteEdge->getFromJunction() ? FORWARD : BACKWARD;
     432        25268 :             if (nextDir == FORWARD) {
     433        24295 :                 nextLane = currentLane->getLinkCont()[0]->getViaLaneOrLane();
     434              :             } else {
     435          973 :                 nextLane = currentLane->getLogicalPredecessorLane();
     436              :             }
     437        25268 :             if DEBUGCOND(ped) {
     438            0 :                 std::cout << "  internal\n";
     439              :             }
     440      1279393 :         } else if (currentEdge->isCrossing()) {
     441        34296 :             nextDir = ped.getDirection();
     442        34296 :             if (nextDir == FORWARD) {
     443        16691 :                 nextLane = currentLane->getLinkCont()[0]->getLane();
     444              :             } else {
     445        17605 :                 nextLane = currentLane->getLogicalPredecessorLane();
     446              :             }
     447        34296 :             if DEBUGCOND(ped) {
     448            0 :                 std::cout << "  crossing\n";
     449              :             }
     450        34296 :             unregisterCrossingApproach(ped, currentLane);
     451      1245097 :         } else if (currentEdge->isWalkingArea())  {
     452              :             ConstMSEdgeVector crossingRoute;
     453              :             // departPos can be 0 because the direction of the walkingArea does not matter
     454              :             // for the arrivalPos, we need to make sure that the route does not deviate across other junctions
     455       879040 :             const int nextRouteEdgeDir = nextRouteEdge->getFromJunction() == junction ? FORWARD : BACKWARD;
     456       879040 :             const double arrivalPos = (nextRouteEdge == ped.getStage()->getRoute().back()
     457       879040 :                                        ? ped.getStage()->getArrivalPos()
     458       247959 :                                        : (nextRouteEdgeDir == FORWARD ? 0 : nextRouteEdge->getLength()));
     459              :             MSEdgeVector prohibited;
     460       879040 :             if (prevLane != nullptr) {
     461       878981 :                 prohibited.push_back(&prevLane->getEdge());
     462              :             }
     463       879040 :             MSNet::getInstance()->getPedestrianRouter(0, prohibited).compute(currentEdge, nextRouteEdge, 0, arrivalPos, ped.getStage()->getMaxSpeed(ped.getPerson()), 0, junction, crossingRoute, true);
     464       879040 :             if DEBUGCOND(ped) {
     465              :                 std::cout
     466              :                         << "   nre=" << nextRouteEdge->getID()
     467              :                         << "   nreDir=" << nextRouteEdgeDir
     468            0 :                         << "   aPos=" << arrivalPos
     469            0 :                         << " crossingRoute=" << toString(crossingRoute)
     470            0 :                         << "\n";
     471              :             }
     472       879040 :             if (crossingRoute.size() > 1) {
     473       879003 :                 const MSEdge* nextEdge = crossingRoute[1];
     474       879003 :                 nextLane = getSidewalk<MSEdge, MSLane>(crossingRoute[1], ped.getPerson()->getVClass());
     475              :                 assert((nextEdge->getFromJunction() == junction || nextEdge->getToJunction() == junction));
     476              :                 assert(nextLane != prevLane);
     477       879003 :                 nextDir = connectedDirection(currentLane, nextLane);
     478       879003 :                 if DEBUGCOND(ped) {
     479            0 :                     std::cout << " nextDir=" << nextDir << "\n";
     480              :                 }
     481              :                 assert(nextDir != UNDEFINED_DIRECTION);
     482       879003 :                 if (nextDir == FORWARD) {
     483       595760 :                     link = currentLane->getLinkTo(nextLane);
     484              :                 } else {
     485       283243 :                     link = nextLane->getLinkTo(currentLane);
     486       283243 :                     if (nextEdge->isCrossing() && link->getTLLogic() == nullptr) {
     487       235983 :                         const MSLane* oppositeWalkingArea = nextLane->getLogicalPredecessorLane();
     488       235983 :                         link = oppositeWalkingArea->getLinkTo(nextLane);
     489              :                     }
     490              :                 }
     491              :                 assert(link != nullptr);
     492       879003 :                 if (nextLane->isCrossing()) {
     493       573975 :                     registerCrossingApproach(ped, nextLane, prevLane);
     494              :                 }
     495              :             } else {
     496           37 :                 if DEBUGCOND(ped) {
     497            0 :                     std::cout << SIMTIME
     498            0 :                               << " no route from '" << (currentEdge == nullptr ? "NULL" : currentEdge->getID())
     499            0 :                               << "' to '" << (nextRouteEdge == nullptr ? "NULL" : nextRouteEdge->getID())
     500            0 :                               << "\n";
     501              :                 }
     502              :                 // check if a direct connection exists (moving onto the walkingarea was the wrong choice)
     503           37 :                 if (ped.getDirection() == FORWARD) {
     504           24 :                     link = prevLane->getLinkTo(nextRouteLane);
     505              :                 } else {
     506           13 :                     link = nextRouteLane->getLinkTo(prevLane);
     507              :                 }
     508           37 :                 if (link != nullptr) {
     509              :                     // leave direction as UNDEFINED_DIRECTION to signal that currentLane must be changed
     510              :                     nextLane = link->getViaLaneOrLane();
     511              :                 } else {
     512           80 :                     WRITE_WARNING("Person '" + ped.getPerson()->getID() + "' could not find route across junction '" + junction->getID()
     513              :                                   + "' from walkingArea '" + currentEdge->getID()
     514              :                                   + "' to edge '" + nextRouteEdge->getID() + "', time=" +
     515              :                                   time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
     516              :                     // error indicated by nextDir == UNDEFINED_DIRECTION
     517              :                     nextLane = nextRouteLane;
     518              :                 }
     519              :             }
     520      1245097 :         } else if (currentEdge == nextRouteEdge) {
     521              :             // strange loop in this route. No need to use walkingArea
     522            8 :             nextDir = -ped.getDirection();
     523              :         } else {
     524              :             // normal edge. by default use next / previous walking area
     525       366049 :             nextDir = ped.getDirection();
     526       366049 :             nextLane = getNextWalkingArea(currentLane, ped.getDirection(), link);
     527       366049 :             if (nextLane != nullptr) {
     528              :                 // walking area found
     529       324881 :                 if DEBUGCOND(ped) {
     530            0 :                     std::cout << "  next walkingArea " << (nextDir == FORWARD ? "forward" : "backward") << "\n";
     531              :                 }
     532              :             } else {
     533              :                 // walk forward by default
     534        41168 :                 if (junction == nextRouteEdge->getToJunction()) {
     535         3938 :                     nextDir = BACKWARD;
     536        37230 :                 } else if (junction == nextRouteEdge->getFromJunction()) {
     537              :                     nextDir = FORWARD;
     538              :                 } else {
     539              :                     // topological disconnect, find a direction that makes sense
     540              :                     // for the future part of the route
     541           56 :                     ConstMSEdgeVector futureRoute = ped.getStage()->getRoute();
     542           56 :                     futureRoute.erase(futureRoute.begin(), futureRoute.begin() + ped.getStage()->getRoutePosition() + 1);
     543           56 :                     int passedFwd = 0;
     544           56 :                     int passedBwd = 0;
     545           56 :                     canTraverse(FORWARD, futureRoute, passedFwd);
     546           56 :                     canTraverse(BACKWARD, futureRoute, passedBwd);
     547           56 :                     nextDir = (passedFwd >= passedBwd) ? FORWARD : BACKWARD;
     548           56 :                     if DEBUGCOND(ped) {
     549            0 :                         std::cout << " nextEdge=" << nextRouteEdge->getID() << " passedFwd=" << passedFwd << " passedBwd=" << passedBwd << " futureRoute=" << toString(futureRoute) << " nextDir=" << nextDir << "\n";
     550              :                     }
     551           56 :                 }
     552              :                 // try to use a direct link as fallback
     553              :                 // direct links only exist if built explicitly. They are used to model tl-controlled links if there are no crossings
     554        41168 :                 if (ped.getDirection() == FORWARD) {
     555        36765 :                     link = currentLane->getLinkTo(nextRouteLane);
     556        36765 :                     if (link != nullptr) {
     557        35193 :                         if DEBUGCOND(ped) {
     558            0 :                             std::cout << "  direct forward\n";
     559              :                         }
     560        35193 :                         nextLane = currentLane->getInternalFollowingLane(nextRouteLane);
     561              :                     }
     562              :                 } else {
     563         4403 :                     link = nextRouteLane->getLinkTo(currentLane);
     564         4403 :                     if (link != nullptr) {
     565         3176 :                         if DEBUGCOND(ped) {
     566            0 :                             std::cout << "  direct backward\n";
     567              :                         }
     568         3176 :                         nextLane = nextRouteLane->getInternalFollowingLane(currentLane);
     569         3176 :                         if (nextLane != nullptr) {
     570              :                             // advance to the end of consecutive internal lanes
     571         5343 :                             while (nextLane->getLinkCont()[0]->getViaLaneOrLane()->isInternal()) {
     572            5 :                                 nextLane = nextLane->getLinkCont()[0]->getViaLaneOrLane();
     573              :                             }
     574              :                         }
     575              :                     }
     576              :                 }
     577              :             }
     578       365542 :             if (nextLane == nullptr) {
     579              :                 // no internal lane found
     580              :                 nextLane = nextRouteLane;
     581         8439 :                 if DEBUGCOND(ped) {
     582            0 :                     std::cout << SIMTIME << " no next lane found for " << currentLane->getID() << " dir=" << ped.getDirection() << "\n";
     583              :                 }
     584         8439 :                 if (usingInternalLanesStatic() && currentLane->getLinkCont().size() > 0) {
     585            0 :                     WRITE_WARNING("Person '" + ped.getPerson()->getID() + "' could not find route across junction '" + junction->getID()
     586              :                                   + "' from edge '" + currentEdge->getID()
     587              :                                   + "' to edge '" + nextRouteEdge->getID() + "', time=" +
     588              :                                   time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
     589              :                 }
     590       357610 :             } else if (nextLane->getLength() <= POSITION_EPS) {
     591              :                 // internal lane too short
     592              :                 // most often this is due to a zero-size junction. However, if
     593              :                 // the person needs to pass a crossing we cannot skip ahead
     594        10969 :                 if ((nextLane->getCanonicalSuccessorLane() == nullptr
     595        10961 :                         || !nextLane->getCanonicalSuccessorLane()->getEdge().isCrossing())
     596        32887 :                         && (nextLane->getLogicalPredecessorLane() == nullptr ||
     597        10957 :                             !nextLane->getLogicalPredecessorLane()->getEdge().isCrossing())) {
     598              :                     //WRITE_WARNING("Person '" + ped.getID()
     599              :                     //        + "' skips short lane '" + nextLane->getID()
     600              :                     //        + "' length=" + toString(nextLane->getLength())
     601              :                     //        + " time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
     602              :                     nextLane = nextRouteLane;
     603        10969 :                     nextDir = nextRouteEdge->getFromJunction() == junction ? FORWARD : BACKWARD;
     604              :                 }
     605              :             }
     606              :         }
     607              :     }
     608      1528102 :     if DEBUGCOND(ped) {
     609            0 :         std::cout << SIMTIME
     610              :                   << " p=" << ped.getPerson()->getID()
     611              :                   << " l=" << currentLane->getID()
     612            0 :                   << " nl=" << (nextLane == nullptr ? "NULL" : nextLane->getID())
     613            0 :                   << " nrl=" << (nextRouteLane == nullptr ? "NULL" : nextRouteLane->getID())
     614              :                   << " d=" << nextDir
     615            0 :                   << " link=" << (link == nullptr ? "NULL" : link->getViaLaneOrLane()->getID())
     616            0 :                   << " pedDir=" << ped.getDirection()
     617            0 :                   << "\n";
     618              :     }
     619              :     assert(nextLane != 0 || nextRouteLane == 0);
     620      1528102 :     return NextLaneInfo(nextLane, link, nextDir);
     621              : }
     622              : 
     623              : 
     624              : const MSLane*
     625       366049 : MSPModel_Striping::getNextWalkingArea(const MSLane* currentLane, const int dir, const MSLink*& link) {
     626       366049 :     if (dir == FORWARD) {
     627       373149 :         for (const MSLink* const l : currentLane->getLinkCont()) {
     628       336384 :             if (l->getLane()->getEdge().isWalkingArea()) {
     629       273260 :                 link = l;
     630              :                 return l->getLane();
     631              :             }
     632              :         }
     633              :     } else {
     634              :         const std::vector<MSLane::IncomingLaneInfo>& laneInfos = currentLane->getIncomingLanes();
     635        62587 :         for (std::vector<MSLane::IncomingLaneInfo>::const_iterator it = laneInfos.begin(); it != laneInfos.end(); ++it) {
     636        58184 :             if ((*it).lane->getEdge().isWalkingArea()) {
     637        51621 :                 link = (*it).viaLink;
     638              :                 return (*it).lane;
     639              :             }
     640              :         }
     641              :     }
     642              :     return nullptr;
     643              : }
     644              : 
     645              : 
     646              : MSPModel_Striping::Obstacles
     647     68125297 : MSPModel_Striping::getNeighboringObstacles(const Pedestrians& pedestrians, int egoIndex, int stripes) {
     648     68125297 :     const PState& ego = *static_cast<PState*>(pedestrians[egoIndex]);
     649     68125297 :     const int egoStripe = ego.stripe();
     650     68125297 :     Obstacles obs(stripes, Obstacle(ego.getDirection()));
     651     68125297 :     std::vector<bool> haveBlocker(stripes, false);
     652  10231265305 :     for (int index = egoIndex + 1; index < (int)pedestrians.size(); index++) {
     653  10217332860 :         const PState& p = *static_cast<PState*>(pedestrians[index]);
     654  10217332860 :         if DEBUGCOND(ego) {
     655            0 :             std::cout << SIMTIME << " ped=" << ego.getID() << " cur=" << egoStripe << " checking neighbor " << p.getID()
     656            0 :                       << " nCur=" << p.stripe() << " nOth=" << p.otherStripe();
     657              :         }
     658  10217332860 :         if (!p.isWaitingToEnter() && !p.isJammed()) {
     659    136661272 :             const Obstacle o(p);
     660    136661272 :             if DEBUGCOND(ego) {
     661            0 :                 std::cout << " dist=" << ego.distanceTo(o) << std::endl;
     662              :             }
     663    136661272 :             if (ego.distanceTo(o) == DIST_BEHIND) {
     664              :                 break;
     665              :             }
     666     82468420 :             if (ego.distanceTo(o) == DIST_OVERLAP) {
     667     82468420 :                 if (p.stripe() != egoStripe || p.getDirection() != ego.getDirection()) {
     668     62811385 :                     obs[p.stripe()] = o;
     669     62811385 :                     haveBlocker[p.stripe()] = true;
     670              :                 } else {
     671              :                     //std::cout << SIMTIME << "   ignoring overlap between " << ego.getID() << " and " << p.getID() << " on stripe=" << egoStripe << "\n";
     672              :                 }
     673     82468420 :                 if (p.otherStripe() != egoStripe || p.getDirection() != ego.getDirection()) {
     674     62811385 :                     obs[p.otherStripe()] = o;
     675     62811385 :                     haveBlocker[p.otherStripe()] = true;
     676              :                 } else {
     677              :                     //std::cout << SIMTIME << "   ignoring overlap between " << ego.getID() << " and " << p.getID() << " on stripe2=" << egoStripe << "\n";
     678              :                 }
     679              :             } else {
     680            0 :                 if (!haveBlocker[p.stripe()]) {
     681            0 :                     obs[p.stripe()] = o;
     682              :                 }
     683            0 :                 if (!haveBlocker[p.otherStripe()]) {
     684            0 :                     obs[p.otherStripe()] = o;
     685              :                 }
     686              :             }
     687              :         }
     688              :     }
     689     68125297 :     if DEBUGCOND(ego) {
     690            0 :         std::cout << SIMTIME << " ped=" << ego.getPerson()->getID() << "  neighObs=";
     691            0 :         DEBUG_PRINT(obs);
     692              :     }
     693     68125297 :     return obs;
     694            0 : }
     695              : 
     696              : 
     697              : int
     698      3054235 : MSPModel_Striping::getStripeOffset(int origStripes, int destStripes, bool addRemainder) {
     699      3054235 :     int offset = (destStripes - origStripes) / 2;
     700      3054235 :     if (addRemainder) {
     701        14626 :         offset += (destStripes - origStripes) % 2;
     702              :     }
     703      3054235 :     return offset;
     704              : }
     705              : 
     706              : 
     707              : const MSPModel_Striping::Obstacles&
     708     17252351 : MSPModel_Striping::getNextLaneObstacles(NextLanesObstacles& nextLanesObs, const
     709              :                                         MSLane* lane, const MSLane* nextLane, int stripes, int nextDir,
     710              :                                         double currentLength, int currentDir) {
     711              :     if (nextLanesObs.count(nextLane) == 0) {
     712      2309717 :         const double nextLength = nextLane->getEdge().isWalkingArea() ? myMinNextLengths[nextLane] : nextLane->getLength();
     713              :         // figure out the which pedestrians are ahead on the next lane
     714      2309717 :         const int nextStripes = numStripes(nextLane);
     715              :         // do not move past the end of the next lane in a single step
     716      4619434 :         Obstacles obs(stripes, Obstacle(nextDir == FORWARD ? nextLength : 0, 0, OBSTACLE_NEXTEND, "nextEnd", 0));
     717              : 
     718      2309717 :         const int offset = getStripeOffset(nextStripes, stripes, currentDir != nextDir && nextStripes > stripes);
     719              :         //std::cout << SIMTIME << " getNextLaneObstacles"
     720              :         //    << " nextLane=" << nextLane->getID()
     721              :         //    << " nextLength=" << nextLength
     722              :         //    << " nextDir=" << nextDir
     723              :         //    << " currentLength=" << currentLength
     724              :         //    << " currentDir=" << currentDir
     725              :         //    << " stripes=" << stripes
     726              :         //    << " nextStripes=" << nextStripes
     727              :         //    << " offset=" << offset
     728              :         //    << "\n";
     729      2309717 :         if (nextStripes < stripes) {
     730              :             // some stripes do not continue
     731      2039843 :             for (int ii = 0; ii < stripes; ++ii) {
     732      1760449 :                 if (ii < offset || ii >= nextStripes + offset) {
     733      2419641 :                     obs[ii] = Obstacle(nextDir == FORWARD ? 0 : nextLength, 0, OBSTACLE_END, "stripeEnd", 0);
     734              :                 }
     735              :             }
     736              :         }
     737      2309717 :         Pedestrians& pedestrians = getPedestrians(nextLane);
     738      2309717 :         if (nextLane->getEdge().isWalkingArea()) {
     739      1189569 :             transformToCurrentLanePositions(obs, currentDir, nextDir, currentLength, nextLength);
     740              :             // complex transformation into the coordinate system of the current lane
     741              :             // (pedestrians on next lane may walk at arbitrary angles relative to the current lane)
     742      1189569 :             double lateral_offset = (lane->getWidth() - stripeWidth) * 0.5;
     743      1189569 :             if ((stripes - nextStripes) % 2 != 0) {
     744       459307 :                 lateral_offset += 0.5 * stripeWidth;
     745              :             }
     746              :             nextDir = currentDir;
     747              :             // transform pedestrians into the current coordinate system
     748      8471539 :             for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
     749      7281970 :                 const PState& p = *static_cast<PState*>(pedestrians[ii]);
     750      7281970 :                 if (p.isWaitingToEnter() || p.isJammed()) {
     751      2249046 :                     continue;
     752              :                 }
     753      5032924 :                 Position pPos = p.getPosition(*p.getStage(), -1);
     754      5032924 :                 Position relPos = lane->getShape().transformToVectorCoordinates(pPos, true);
     755              :                 if (relPos == Position::INVALID) {
     756            0 :                     WRITE_WARNINGF("Could not map position % onto lane '%'", pPos, lane->getID());
     757              :                 }
     758      5032924 :                 const double newY = relPos.y() + lateral_offset;
     759              :                 //if (p.getPerson()->getID() == "ped200") std::cout << "    ped=" << p.getPerson()->getID() << "  relX=" << relPos.x() << " relY=" << newY << " latOff=" << lateral_offset << " s=" << p.stripe(newY) << " os=" << p.otherStripe(newY) << "\n";
     760      5032924 :                 if ((currentDir == FORWARD && relPos.x() >= lane->getLength()) || (currentDir == BACKWARD && relPos.x() < 0)) {
     761      5028243 :                     addCloserObstacle(obs, relPos.x(), p.stripe(newY), stripes, p.getPerson()->getID(), p.getPerson()->getVehicleType().getWidth(), currentDir, OBSTACLE_PED);
     762      5028243 :                     addCloserObstacle(obs, relPos.x(), p.otherStripe(newY), stripes, p.getPerson()->getID(), p.getPerson()->getVehicleType().getWidth(), currentDir, OBSTACLE_PED);
     763              :                 }
     764              :             }
     765              :         } else {
     766              :             // simple transformation into the coordinate system of the current lane
     767              :             // (only need to worry about currentDir and nextDir)
     768              :             // XXX consider waitingToEnter on nextLane
     769      1120148 :             sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(nextDir));
     770     33944651 :             for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
     771     32824503 :                 const PState& p = *static_cast<PState*>(pedestrians[ii]);
     772     32824503 :                 if (p.isWaitingToEnter() || p.isJammed()) {
     773     21610116 :                     continue;
     774              :                 }
     775              :                 double newY = p.getPosLat();
     776     11214387 :                 Obstacle pObs(p);
     777     11214387 :                 if (nextDir != currentDir) {
     778       509142 :                     newY = (nextStripes - 1) * stripeWidth - newY;
     779       509142 :                     pObs.speed *= -1;
     780              :                 }
     781     11214387 :                 newY += offset * stripeWidth;
     782     11214387 :                 const int stripe = p.stripe(newY);
     783     11214387 :                 if (stripe >= 0 && stripe < stripes) {
     784     11214384 :                     obs[stripe] = pObs;
     785              :                 }
     786     11214387 :                 const int otherStripe = p.otherStripe(newY);
     787     11214387 :                 if (otherStripe >= 0 && otherStripe < stripes) {
     788     11214384 :                     obs[otherStripe] = pObs;
     789              :                 }
     790              :             }
     791      1120148 :             if (nextLane->getEdge().isCrossing()) {
     792              :                 // add vehicle obstacles
     793       326030 :                 const MSLink* crossingEntryLink = nextLane->getIncomingLanes().front().viaLink;
     794       326030 :                 const bool prio = crossingEntryLink->havePriority() || crossingEntryLink->getTLLogic() != nullptr;
     795       326030 :                 addCrossingVehs(nextLane, stripes, offset, nextDir, obs, prio);
     796              :             }
     797      1120148 :             if (nextLane->getVehicleNumberWithPartials() > 0) {
     798        18640 :                 Obstacles vehObs = getVehicleObstacles(nextLane, nextDir);
     799        18640 :                 PState::mergeObstacles(obs, vehObs, nextDir, offset);
     800        18640 :             }
     801      1120148 :             transformToCurrentLanePositions(obs, currentDir, nextDir, currentLength, nextLength);
     802              :         }
     803      2309717 :         nextLanesObs[nextLane] = obs;
     804      2309717 :     }
     805     17252351 :     return nextLanesObs[nextLane];
     806              : }
     807              : 
     808              : void
     809      2309717 : MSPModel_Striping::transformToCurrentLanePositions(Obstacles& obs, int currentDir, int nextDir, double currentLength, double nextLength) {
     810     13134848 :     for (Obstacle& o : obs) {
     811     10825131 :         if (currentDir == FORWARD) {
     812      7234212 :             if (nextDir == FORWARD) {
     813      6858577 :                 o.xFwd += currentLength;
     814      6858577 :                 o.xBack += currentLength;
     815              :             } else {
     816       375635 :                 const double tmp = o.xFwd;
     817       375635 :                 o.xFwd = currentLength + nextLength - o.xBack;
     818       375635 :                 o.xBack = currentLength + nextLength - tmp;
     819              :             }
     820              :         } else {
     821      3590919 :             if (nextDir == FORWARD) {
     822       848051 :                 const double tmp = o.xFwd;
     823       848051 :                 o.xFwd = -o.xBack;
     824       848051 :                 o.xBack = -tmp;
     825              :             } else {
     826      2742868 :                 o.xFwd -= nextLength;
     827      2742868 :                 o.xBack -= nextLength;
     828              :             }
     829              :         }
     830              :     }
     831      2309717 : }
     832              : 
     833              : 
     834              : void
     835     10056486 : MSPModel_Striping::addCloserObstacle(Obstacles& obs, double x, int stripe, int numStripes, const std::string& id, double width, int dir, ObstacleType type) {
     836     10056486 :     if (stripe >= 0 && stripe < numStripes) {
     837      2493101 :         if ((dir == FORWARD && x - width / 2. < obs[stripe].xBack) || (dir == BACKWARD && x + width / 2. > obs[stripe].xFwd)) {
     838      2104430 :             obs[stripe] = Obstacle(x, 0, type, id, width);
     839              :         }
     840              :     }
     841     10056486 : }
     842              : 
     843              : void
     844     18036734 : MSPModel_Striping::moveInDirection(SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir) {
     845    140196897 :     for (ActiveLanes::iterator it_lane = myActiveLanes.begin(); it_lane != myActiveLanes.end(); ++it_lane) {
     846    122160167 :         const MSLane* lane = it_lane->first;
     847    122160167 :         Pedestrians& pedestrians = it_lane->second;
     848    122160167 :         if (pedestrians.size() == 0) {
     849    113076942 :             continue;
     850              :         }
     851              :         //std::cout << SIMTIME << ">>> lane=" << lane->getID() << " numPeds=" << pedestrians.size() << "\n";
     852      9083225 :         if (lane->getEdge().isWalkingArea()) {
     853      2040019 :             const double lateral_offset = (lane->getWidth() - stripeWidth) * 0.5;
     854      2040019 :             const double minY = stripeWidth * - 0.5 + NUMERICAL_EPS;
     855      2040019 :             const double maxY = stripeWidth * (numStripes(lane) - 0.5) - NUMERICAL_EPS;
     856              :             const WalkingAreaPath* debugPath = nullptr;
     857              :             // need to handle each walkingAreaPath separately and transform
     858              :             // coordinates beforehand
     859              :             std::set<const WalkingAreaPath*, walkingarea_path_sorter> paths;
     860     17863092 :             for (Pedestrians::iterator it = pedestrians.begin(); it != pedestrians.end(); ++it) {
     861     15823073 :                 const PState* p = static_cast<PState*>(*it);
     862              :                 assert(p->myWalkingAreaPath != 0);
     863     15823073 :                 if (p->getDirection() == dir) {
     864      7927393 :                     paths.insert(p->myWalkingAreaPath);
     865      7927393 :                     if DEBUGCOND(*p) {
     866            0 :                         debugPath = p->myWalkingAreaPath;
     867            0 :                         std::cout << SIMTIME << " debugging WalkingAreaPath from=" << debugPath->from->getID() << " to=" << debugPath->to->getID() << " minY=" << minY << " maxY=" << maxY << " latOffset=" << lateral_offset << "\n";
     868              :                     }
     869              :                 }
     870              :             }
     871      2040019 :             const double usableWidth = (numStripes(lane) - 1) * stripeWidth;
     872      3139597 :             for (std::set<const WalkingAreaPath*, walkingarea_path_sorter>::iterator it = paths.begin(); it != paths.end(); ++it) {
     873      1099578 :                 const WalkingAreaPath* path = *it;
     874              :                 Pedestrians toDelete;
     875              :                 Pedestrians transformedPeds;
     876      1099578 :                 transformedPeds.reserve(pedestrians.size());
     877      9988814 :                 for (Pedestrians::iterator it_p = pedestrians.begin(); it_p != pedestrians.end(); ++it_p) {
     878      8889236 :                     PState* p = static_cast<PState*>(*it_p);
     879      8889236 :                     if (p->myWalkingAreaPath == path) {
     880      7927393 :                         transformedPeds.push_back(p);
     881      7927393 :                         if (path == debugPath) std::cout << "  ped=" << p->getPerson()->getID() << "  relX=" << p->getEdgePos(0) << " relY=" << p->getPosLat() << " (untransformed), vecCoord="
     882            0 :                                                              << path->shape.transformToVectorCoordinates(p->getPosition(*p->getStage(), -1)) << "\n";
     883       961843 :                     } else if (p->myWalkingAreaPath->from == path->to && p->myWalkingAreaPath->to == path->from) {
     884        59323 :                         if (p->myWalkingAreaPath->dir != path->dir) {
     885              :                             // opposite direction is already in the correct coordinate system
     886        39507 :                             transformedPeds.push_back(p);
     887        39507 :                             if (path == debugPath) std::cout << "  ped=" << p->getPerson()->getID() << "  relX=" << p->getEdgePos(0) << " relY=" << p->getPosLat() << " (untransformed), vecCoord="
     888            0 :                                                                  << path->shape.transformToVectorCoordinates(p->getPosition(*p->getStage(), -1)) << "\n";
     889              :                         } else {
     890              :                             // x position must be reversed
     891        19816 :                             PState* tp = new PState(*p);
     892        19816 :                             tp->reverse(path->length, usableWidth);
     893        19816 :                             toDelete.push_back(tp);
     894        19816 :                             transformedPeds.push_back(tp);
     895        19816 :                             if (path == debugPath) std::cout << "  ped=" << p->getPerson()->getID() << "  relX=" << p->getEdgePos(0) << " relY=" << p->getPosLat() << " (semi-transformed), vecCoord="
     896            0 :                                                                  << path->shape.transformToVectorCoordinates(p->getPosition(*p->getStage(), -1)) << "\n";
     897              :                         }
     898              :                     } else {
     899       902520 :                         const Position relPos = path->shape.transformToVectorCoordinates(p->getPosition(*p->getStage(), -1));
     900       902520 :                         const double newY = relPos.y() + lateral_offset;
     901       901958 :                         if (relPos != Position::INVALID && newY >= minY && newY <= maxY) {
     902       243480 :                             PState* tp = new PState(*p);
     903       243480 :                             tp->reset(relPos.x(), newY);
     904       243480 :                             toDelete.push_back(tp);
     905       243480 :                             transformedPeds.push_back(tp);
     906       243480 :                             if (path == debugPath) {
     907            0 :                                 std::cout << "  ped=" << p->getPerson()->getID() << "  relX=" << relPos.x() << " relY=" << newY << " (transformed), vecCoord=" << relPos << "\n";
     908              :                             }
     909              :                         } else {
     910       659040 :                             if (path == debugPath) {
     911            0 :                                 std::cout << "  ped=" << p->getPerson()->getID() << "  relX=" << relPos.x() << " relY=" << newY << " (invalid), vecCoord=" << relPos << "\n";
     912              :                             }
     913              :                         }
     914              :                     }
     915              :                 }
     916              :                 auto itFoe = myWalkingAreaFoes.find(&lane->getEdge());
     917      1099578 :                 if (itFoe != myWalkingAreaFoes.end()) {
     918              :                     // add vehicle foes on paths which cross this walkingarea
     919              :                     // translate the vehicle into a number of dummy-pedestrians
     920              :                     // that occupy the same space
     921      2238820 :                     for (const MSLane* foeLane : itFoe->second) {
     922       160963 :                         for (auto itVeh = foeLane->anyVehiclesBegin(); itVeh != foeLane->anyVehiclesEnd(); ++itVeh) {
     923       160963 :                             const MSVehicle* veh = *itVeh;
     924       160963 :                             const double vehWidth = veh->getVehicleType().getWidth();
     925       160963 :                             Boundary relCorners;
     926       160963 :                             Position relFront = path->shape.transformToVectorCoordinates(veh->getPosition(), true);
     927       160963 :                             Position relBack = path->shape.transformToVectorCoordinates(veh->getBackPosition(), true);
     928              :                             if (relFront == Position::INVALID) {
     929            0 :                                 WRITE_WARNINGF("Could not vehicle '%' front position % onto walkingarea '%' path=%, time=%.",
     930              :                                                veh->getID(), veh->getPosition(), lane->getID(), path->shape, time2string(SIMSTEP));
     931              :                             }
     932              :                             if (relBack == Position::INVALID) {
     933            0 :                                 WRITE_WARNINGF("Could not vehicle '%' back position % onto walkingarea '%' path=%, time=%.",
     934              :                                                veh->getID(), veh->getBackPosition(), lane->getID(), path->shape, time2string(SIMSTEP));
     935              :                             }
     936       160963 :                             PositionVector relCenter;
     937       160963 :                             relCenter.push_back(relFront);
     938       160963 :                             relCenter.push_back(relBack);
     939       160963 :                             relCenter.move2side(vehWidth / 2);
     940       160963 :                             relCorners.add(relCenter[0]);
     941       160963 :                             relCorners.add(relCenter[1]);
     942       160963 :                             relCenter.move2side(-vehWidth);
     943       160963 :                             relCorners.add(relCenter[0]);
     944       160963 :                             relCorners.add(relCenter[1]);
     945              :                             // persons should require less gap than the vehicles to prevent getting stuck
     946              :                             // when a vehicles moves towards them
     947       160963 :                             relCorners.growWidth(SAFETY_GAP / 2);
     948       160963 :                             const double xWidth = relCorners.getWidth();
     949       160963 :                             const double vehYmin = MAX2(minY - lateral_offset, relCorners.ymin());
     950       160963 :                             const double vehYmax = MIN2(maxY - lateral_offset, relCorners.ymax());
     951       160963 :                             const double xCenter = relCorners.getCenter().x();
     952              :                             Position yMinPos(xCenter, vehYmin);
     953              :                             Position yMaxPos(xCenter, vehYmax);
     954       160963 :                             const bool addFront = addVehicleFoe(veh, lane, yMinPos, dir * xWidth, 0, lateral_offset, minY, maxY, toDelete, transformedPeds);
     955       160963 :                             const bool addBack = addVehicleFoe(veh, lane, yMaxPos, dir * xWidth, 0, lateral_offset, minY, maxY, toDelete, transformedPeds);
     956       160963 :                             if (path == debugPath) {
     957              :                                 std::cout << "  veh=" << veh->getID()
     958            0 :                                           << " corners=" << relCorners
     959              :                                           << " xWidth=" << xWidth
     960            0 :                                           << " ymin=" << relCorners.ymin()
     961            0 :                                           << " ymax=" << relCorners.ymax()
     962              :                                           << " vehYmin=" << vehYmin
     963              :                                           << " vehYmax=" << vehYmax
     964            0 :                                           << "\n";
     965              :                             }
     966       160963 :                             if (addFront && addBack) {
     967              :                                 // add in-between positions
     968       160963 :                                 const double yDist = vehYmax - vehYmin;
     969       374779 :                                 for (double dist = stripeWidth; dist < yDist; dist += stripeWidth) {
     970       213816 :                                     const double relDist = dist / yDist;
     971       213816 :                                     Position between = (yMinPos * relDist) + (yMaxPos * (1 - relDist));
     972       213816 :                                     if (path == debugPath) {
     973            0 :                                         std::cout << "  vehBetween=" << veh->getID() << " pos=" << between << "\n";
     974              :                                     }
     975       213816 :                                     addVehicleFoe(veh, lane, between, dir * xWidth, stripeWidth, lateral_offset, minY, maxY, toDelete, transformedPeds);
     976              :                                 }
     977              :                             }
     978       160963 :                         }
     979              :                     }
     980              :                 }
     981      1099578 :                 moveInDirectionOnLane(transformedPeds, lane, currentTime, changedLane, dir, path == debugPath);
     982      1099578 :                 arriveAndAdvance(pedestrians, currentTime, changedLane, dir);
     983              :                 // clean up
     984      1800724 :                 for (Pedestrians::iterator it_p = toDelete.begin(); it_p != toDelete.end(); ++it_p) {
     985       701146 :                     delete *it_p;
     986              :                 }
     987      1099578 :             }
     988              :         } else {
     989      7043206 :             moveInDirectionOnLane(pedestrians, lane, currentTime, changedLane, dir, false);
     990      7043206 :             arriveAndAdvance(pedestrians, currentTime, changedLane, dir);
     991              :         }
     992              :     }
     993     18036730 : }
     994              : 
     995              : 
     996              : bool
     997       535742 : MSPModel_Striping::addVehicleFoe(const MSVehicle* veh, const MSLane* walkingarea, const Position& relPos, double xWidth, double yWidth, double lateral_offset,
     998              :                                  double minY, double maxY, Pedestrians& toDelete, Pedestrians& transformedPeds) {
     999              :     if (relPos != Position::INVALID) {
    1000       535742 :         const double newY = relPos.y() + lateral_offset;
    1001       535742 :         if (newY >= minY && newY <= maxY) {
    1002       437850 :             PState* tp = new PStateVehicle(veh, walkingarea, relPos.x(), newY, xWidth, yWidth);
    1003              :             //std::cout << SIMTIME << " addVehicleFoe=" << veh->getID() << " rx=" << relPos.x() << " ry=" << newY << " s=" << tp->stripe() << " o=" << tp->otherStripe() << "\n";
    1004       437850 :             toDelete.push_back(tp);
    1005       437850 :             transformedPeds.push_back(tp);
    1006              :         }
    1007       535742 :         return true;
    1008              :     } else {
    1009              :         return false;
    1010              :     }
    1011              : }
    1012              : 
    1013              : void
    1014      8142784 : MSPModel_Striping::arriveAndAdvance(Pedestrians& pedestrians, SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir) {
    1015              :     // advance to the next lane / arrive at destination
    1016      8142784 :     sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(dir));
    1017              :     // can't use iterators because we do concurrent modification
    1018    137647537 :     for (int i = 0; i < (int)pedestrians.size(); i++) {
    1019    129504757 :         PState* const p = static_cast<PState*>(pedestrians[i]);
    1020    129504757 :         if (p->isRemoteControlled()) {
    1021        20291 :             continue;
    1022              :         }
    1023    129484466 :         if (p->getDirection() == dir && p->distToLaneEnd() < 0) {
    1024              :             // moveToNextLane may trigger re-insertion (for consecutive
    1025              :             // walks) so erase must be called first
    1026              :             pedestrians.erase(pedestrians.begin() + i);
    1027       965615 :             i--;
    1028       965615 :             p->moveToNextLane(currentTime);
    1029       965611 :             if (p->getLane() != nullptr) {
    1030       744518 :                 changedLane.insert(p->getPerson());
    1031       744518 :                 myActiveLanes[p->getLane()].push_back(p);
    1032              :             } else {
    1033              :                 // end walking stage and destroy PState
    1034       221093 :                 p->getStage()->moveToNextEdge(p->getPerson(), currentTime, dir);
    1035       221093 :                 myNumActivePedestrians--;
    1036              :             }
    1037              :         }
    1038              :     }
    1039      8142780 : }
    1040              : 
    1041              : 
    1042              : void
    1043      8142784 : MSPModel_Striping::moveInDirectionOnLane(Pedestrians& pedestrians, const MSLane* lane, SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir, bool debug) {
    1044      8142784 :     const int stripes = numStripes(lane);
    1045              :     //std::cout << " laneWidth=" << lane->getWidth() << " stripeWidth=" << stripeWidth << " stripes=" << stripes << "\n";
    1046     16285568 :     Obstacles obs(stripes, Obstacle(dir)); // continuously updated
    1047              :     NextLanesObstacles nextLanesObs; // continuously updated
    1048      8142784 :     sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(dir));
    1049              : 
    1050     16285568 :     Obstacles crossingVehs(stripes, Obstacle(dir));
    1051              :     bool hasCrossingVehObs = false;
    1052      8142784 :     if (lane->getEdge().isCrossing()) {
    1053              :         // assume that vehicles will brake when already on the crossing
    1054       609893 :         hasCrossingVehObs = addCrossingVehs(lane, stripes, 0, dir, crossingVehs, true);
    1055              :     }
    1056              : 
    1057    137425075 :     for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
    1058    129282291 :         PState& p = *static_cast<PState*>(pedestrians[ii]);
    1059              :         UNUSED_PARAMETER(debug);
    1060              :         //if (debug) {
    1061              :         //    std::cout << SIMTIME << " CHECKING d=" << dir << " p=" << p.getID() << " relX=" << p.myRelX << " xmin=" << p.getMinX() << " xmax=" << p.getMaxX() << " pdir=" << p.getDirection() << "\n";
    1062              :         //}
    1063    129282291 :         Obstacles currentObs = obs;
    1064    129282291 :         if (p.getDirection() != dir || changedLane.count(p.getPerson()) != 0 || p.getRemotePosition() != Position::INVALID) {
    1065     61156994 :             if (!p.isWaitingToEnter() && !p.isJammed()) {
    1066              :                 //if DEBUGCOND(p) {
    1067              :                 //    std::cout << "   obs=" << p.getPerson()->getID() << "  y=" << p.getPosLat() << "  stripe=" << p.stripe() << " oStripe=" << p.otherStripe() << "\n";
    1068              :                 //}
    1069     30144686 :                 Obstacle o(p);
    1070     30144686 :                 if (p.getDirection() != dir && p.getSpeed(*p.getStage()) == 0.) {
    1071              :                     // ensure recognition of oncoming
    1072      7952680 :                     o.speed = (p.getDirection() == FORWARD ? 0.1 : -0.1);
    1073              :                 }
    1074     30144686 :                 if (o.closer(obs[p.stripe()], dir)) {
    1075     30140473 :                     obs[p.stripe()] = o;
    1076              :                 }
    1077     30144686 :                 if (o.closer(obs[p.otherStripe()], dir)) {
    1078     30140307 :                     obs[p.otherStripe()] = o;
    1079              :                 }
    1080              :             }
    1081              :             continue;
    1082     61156994 :         }
    1083     68125297 :         if DEBUGCOND(p) {
    1084            0 :             std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << "  currentObs=";
    1085            0 :             gDebugFlag1 = true;
    1086            0 :             DEBUG_PRINT(currentObs);
    1087              :         }
    1088     68125297 :         const MSLane* nextLane = p.myNLI.lane;
    1089     68125297 :         const MSLink* link = p.myNLI.link;
    1090     68125297 :         const double dist = p.distToLaneEnd();
    1091              :         const double speed (p.getStage()->getConfiguredSpeed() >= 0
    1092     68125297 :                 ? p.getStage()->getConfiguredSpeed()
    1093     56167057 :                 : ((nextLane != nullptr && (USE_NET_SPEEDS || nextLane->isNormal() || nextLane->isInternal()))
    1094    122693801 :                     ? nextLane->getVehicleMaxSpeed(p.getPerson())
    1095     13448005 :                     : p.getStage()->getMaxSpeed(p.getPerson())));
    1096              : 
    1097              : 
    1098     68125297 :         if (nextLane != nullptr && dist <= LOOKAHEAD_ONCOMING_DIST) {
    1099     17252351 :             const double currentLength = (p.myWalkingAreaPath == nullptr ? lane->getLength() : p.myWalkingAreaPath->length);
    1100     17252351 :             const Obstacles& nextObs = getNextLaneObstacles(
    1101              :                                            nextLanesObs, lane, nextLane, stripes,
    1102              :                                            p.myNLI.dir, currentLength, dir);
    1103              : 
    1104     17252351 :             if DEBUGCOND(p) {
    1105            0 :                 std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << "  nextObs=";
    1106            0 :                 DEBUG_PRINT(nextObs);
    1107              :             }
    1108     17252351 :             p.mergeObstacles(currentObs, nextObs);
    1109              :         }
    1110     68125297 :         if DEBUGCOND(p) {
    1111            0 :             std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << "  obsWithNext=";
    1112            0 :             DEBUG_PRINT(currentObs);
    1113              :         }
    1114     68125297 :         p.mergeObstacles(currentObs, getNeighboringObstacles(pedestrians, ii, stripes));
    1115     68125297 :         if DEBUGCOND(p) {
    1116            0 :             std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << "  obsWithNeigh=";
    1117            0 :             DEBUG_PRINT(currentObs);
    1118              :         }
    1119              :         // time gap to pass the intersection ahead of a vehicle.
    1120     68125297 :         const double passingLength = p.getLength() + p.getPerson()->getTimegapCrossing() * speed;
    1121              :         // check link state
    1122     68125297 :         if DEBUGCOND(p) {
    1123            0 :             gDebugFlag1 = true; // get debug output from MSLink
    1124            0 :             std::cout << "   link=" << (link == nullptr ? "NULL" : link->getViaLaneOrLane()->getID())
    1125            0 :                       << " dist=" << dist << " d2=" << dist - p.getMinGap() << " la=" << LOOKAHEAD_SAMEDIR* speed << "\n";
    1126              :         }
    1127              :         if (link != nullptr
    1128              :                 // only check close before junction, @todo we should take deceleration into account here
    1129     55257007 :                 && dist - p.getMinGap() < LOOKAHEAD_SAMEDIR * speed
    1130              :                 // persons move before vehicles so we subtract DELTA_TO because they cannot rely on vehicles having passed the intersection in the current time step
    1131     79748900 :                 && (!link->opened(currentTime - DELTA_T, speed, speed, passingLength, p.getImpatience(currentTime), speed, 0, 0, nullptr, p.ignoreRed(link), p.getPerson())
    1132     10926878 :                     || p.stopForYellow(link))) {
    1133              :             // prevent movement passed a closed link
    1134       697691 :             Obstacles closedLink(stripes, Obstacle(p.getEdgePos(0) + dir * (dist - NUMERICAL_EPS), 0, OBSTACLE_LINKCLOSED, "closedLink_" + link->getViaLaneOrLane()->getID(), 0));
    1135       697691 :             p.mergeObstacles(currentObs, closedLink);
    1136       697691 :             if DEBUGCOND(p) {
    1137            0 :                 std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << "  obsWithTLS=";
    1138            0 :                 DEBUG_PRINT(currentObs);
    1139              :             }
    1140              :             // consider rerouting over another crossing
    1141       697691 :             if (p.myWalkingAreaPath != nullptr) {
    1142              :                 // @todo actually another path would be needed starting at the current position
    1143       539552 :                 const MSLane* oldNext = p.myNLI.lane;
    1144       539552 :                 p.myNLI = getNextLane(p, p.getLane(), p.myWalkingAreaPath->from);
    1145       539552 :                 if (p.myNLI.lane != oldNext) {
    1146          160 :                     unregisterCrossingApproach(p, oldNext);
    1147              :                 }
    1148              :             }
    1149       697691 :         }
    1150     68125297 :         if DEBUGCOND(p) {
    1151            0 :             gDebugFlag1 = false;
    1152              :         }
    1153     68125297 :         if (&lane->getEdge() == p.getStage()->getDestination() && p.getStage()->getDestinationStop() != nullptr) {
    1154              :             Obstacles arrival;
    1155      1322241 :             if (p.getStage()->getDestinationStop()->getWaitingCapacity() > p.getStage()->getDestinationStop()->getNumWaitingPersons()) {
    1156      2426436 :                 arrival = Obstacles(stripes, Obstacle(p.getStage()->getArrivalPos() + dir * p.getMinGap(), 0, OBSTACLE_ARRIVALPOS, "arrival", 0));
    1157              :             } else {
    1158       218046 :                 arrival = Obstacles(stripes, Obstacle(p.getStage()->getArrivalPos() - dir * p.getMinGap(), 0, OBSTACLE_ARRIVALPOS, "arrival_blocked", 0));
    1159              :             }
    1160      1322241 :             p.mergeObstacles(currentObs, arrival);
    1161      1322241 :         }
    1162              : 
    1163     68125297 :         if (lane->getVehicleNumberWithPartials() > 0) {
    1164              :             // react to vehicles on the same lane
    1165              :             // @todo: improve efficiency by using the same iterator for all pedestrians on this lane
    1166       271141 :             Obstacles vehObs = getVehicleObstacles(lane, dir, &p);
    1167       271141 :             p.mergeObstacles(currentObs, vehObs);
    1168       271141 :             if DEBUGCOND(p) {
    1169            0 :                 std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << "  obsWithVehs=";
    1170            0 :                 DEBUG_PRINT(currentObs);
    1171              :             }
    1172       271141 :         }
    1173     68125297 :         if (hasCrossingVehObs) {
    1174        23952 :             p.mergeObstacles(currentObs, crossingVehs);
    1175        23952 :             if DEBUGCOND(p) {
    1176            0 :                 std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << "  obsWithVehs2=";
    1177            0 :                 DEBUG_PRINT(currentObs);
    1178              :             }
    1179              :         }
    1180              : 
    1181              :         // walk, taking into account all obstacles
    1182     68125297 :         p.walk(currentObs, currentTime);
    1183     68125297 :         gDebugFlag1 = false;
    1184     68125297 :         if (!p.isWaitingToEnter() && !p.isJammed()) {
    1185     34887778 :             Obstacle o(p);
    1186     34887778 :             if (o.closer(obs[p.stripe()], dir)) {
    1187     34886218 :                 obs[p.stripe()] = o;
    1188              :             }
    1189     34887778 :             if (o.closer(obs[p.otherStripe()], dir)) {
    1190     34886210 :                 obs[p.otherStripe()] = o;
    1191              :             }
    1192     34887778 :             if (MSGlobals::gCheck4Accidents && p.myWalkingAreaPath == nullptr && !p.isJammed()) {
    1193   2760375482 :                 for (int coll = 0; coll < ii; ++coll) {
    1194   2731118420 :                     PState& c = *static_cast<PState*>(pedestrians[coll]);
    1195   2731118420 :                     if (!c.isWaitingToEnter() && c.myWalkingAreaPath == nullptr && !c.isJammed()) {
    1196   1202938110 :                         if (c.stripe() == p.stripe() || p.stripe() == c.otherStripe() || p.otherStripe() == c.stripe() || p.otherStripe() == c.otherStripe()) {
    1197    459709662 :                             Obstacle cObs(c);
    1198              :                             // we check only for real collisions, no min gap violations
    1199    459709662 :                             if (p.distanceTo(cObs, false) == DIST_OVERLAP) {
    1200       599880 :                                 WRITE_WARNING("Collision of person '" + p.getPerson()->getID() + "' and person '" + c.getPerson()->getID()
    1201              :                                               + "', lane='" + lane->getID() + "', time=" + time2string(currentTime) + ".");
    1202              :                             }
    1203              :                         }
    1204              :                     }
    1205              :                 }
    1206              :             }
    1207              :         }
    1208              :         //std::cout << SIMTIME << p.getPerson()->getID() << " lane=" << lane->getID() << " x=" << p.myRelX << "\n";
    1209    129282291 :     }
    1210     16285568 : }
    1211              : 
    1212              : 
    1213              : void
    1214       573975 : MSPModel_Striping::registerCrossingApproach(const PState& ped, const MSLane* crossing, const MSLane* beforeWA) {
    1215              :     // person has entered the walkingarea
    1216       573975 :     SUMOTime arrivalTime = SIMSTEP;
    1217              :     assert(ped.getLane()->isWalkingArea());
    1218       573975 :     const WalkingAreaPath* wa = getWalkingAreaPath(&ped.getLane()->getEdge(), beforeWA, crossing);
    1219       573975 :     const double speed = ped.getStage()->getMaxSpeed(ped.getPerson()) * (1 - dawdling / 2);
    1220       573975 :     arrivalTime += TIME2STEPS(wa->length / speed);
    1221       573975 :     SUMOTime leavingTime = arrivalTime + TIME2STEPS(crossing->getLength() / speed);
    1222       573975 :     crossing->getIncomingLanes()[0].viaLink->setApproachingPerson(ped.getPerson(), arrivalTime, leavingTime);
    1223       573975 :     if DEBUGCOND(ped) {
    1224            0 :         std::cout << SIMTIME << " register " << ped.getPerson()->getID() << " at crossing " << crossing->getID() << "\n";
    1225              :     }
    1226       573975 : }
    1227              : 
    1228              : 
    1229              : bool
    1230       935923 : MSPModel_Striping::addCrossingVehs(const MSLane* crossing, int stripes, double lateral_offset, int dir, Obstacles& obs, bool prio) {
    1231              :     bool hasCrossingVehObs = false;
    1232       935923 :     const MSLink* crossingExitLink = crossing->getLinkCont().front();
    1233       935923 :     gDebugFlag1 = DEBUGCOND2(crossing);
    1234       935923 :     const MSLink::LinkLeaders linkLeaders = crossingExitLink->getLeaderInfo(nullptr, crossing->getLength());
    1235       935923 :     gDebugFlag1 = false;
    1236       935923 :     if (linkLeaders.size() > 0) {
    1237       211448 :         for (MSLink::LinkLeaders::const_iterator it = linkLeaders.begin(); it != linkLeaders.end(); ++it) {
    1238              :             // the vehicle to enter the junction first has priority
    1239       107322 :             const MSVehicle* veh = (*it).vehAndGap.first;
    1240       107322 :             if (veh != nullptr) {
    1241       107322 :                 Obstacle vo((*it).distToCrossing, 0, OBSTACLE_VEHICLE, veh->getID(), veh->getVehicleType().getWidth() + 2 * minGapToVehicle, veh);
    1242              :                 // block entry to the crossing in walking direction but allow leaving it
    1243       107322 :                 Obstacle voBlock = vo;
    1244       107322 :                 if (dir == FORWARD) {
    1245        59749 :                     voBlock.xBack = NUMERICAL_EPS;
    1246              :                 } else {
    1247        47573 :                     voBlock.xFwd = crossing->getLength() - NUMERICAL_EPS;
    1248              :                 }
    1249              :                 // when approaching a priority crossings, vehicles must be able
    1250              :                 // to brake, otherwise the person must be able to cross in time
    1251       107322 :                 const double distToCrossBeforeVeh = (dir == FORWARD ? vo.xFwd : crossing->getLength() - vo.xBack);
    1252              :                 const double bGap = (prio
    1253       108991 :                                      ? veh->getCarFollowModel().brakeGap(veh->getSpeed(), veh->getCarFollowModel().getMaxDecel(), 0)
    1254         1669 :                                      : veh->getSpeed() * distToCrossBeforeVeh); // walking 1m/s
    1255              :                 double vehYmin;
    1256              :                 double vehYmax;
    1257              :                 // relY increases from left to right (the other way around from vehicles)
    1258       107322 :                 if ((*it).fromLeft()) {
    1259        70000 :                     vehYmin = -(*it).vehAndGap.second + lateral_offset; // vehicle back
    1260        70000 :                     vehYmax = vehYmin + veh->getVehicleType().getLength() + bGap + minGapToVehicle;
    1261        70000 :                     vehYmin -= minGapToVehicle;
    1262              :                 } else {
    1263        37322 :                     vehYmax = crossing->getWidth() + (*it).vehAndGap.second - lateral_offset; // vehicle back
    1264        37322 :                     vehYmin = vehYmax - veh->getVehicleType().getLength() - bGap - minGapToVehicle;
    1265        37322 :                     vehYmax += minGapToVehicle;
    1266              : 
    1267              :                 }
    1268       429027 :                 for (int s = MAX2(0, PState::stripe(vehYmin)); s < MIN2(PState::stripe(vehYmax), stripes); ++s) {
    1269       179493 :                     if ((dir == FORWARD && obs[s].xBack > vo.xBack)
    1270       321966 :                             || (dir == BACKWARD && obs[s].xFwd < vo.xFwd)) {
    1271       321102 :                         if (!prio && veh->getSpeed() > SUMO_const_haltingSpeed) {
    1272              :                             // do not enter the crossing
    1273         9292 :                             obs[s] = voBlock;
    1274              :                         } else {
    1275       311810 :                             obs[s] = vo;
    1276              :                         }
    1277              :                         hasCrossingVehObs = true;
    1278              :                     }
    1279              :                 }
    1280       107322 :                 if (DEBUGCOND2(crossing)) {
    1281            0 :                     std::cout << SIMTIME
    1282              :                               << " crossingVeh=" << veh->getID()
    1283              :                               << " lane=" << crossing->getID()
    1284              :                               << " prio=" << prio
    1285              :                               << " latOffset=" << lateral_offset
    1286              :                               << " dir=" << dir
    1287            0 :                               << " stripes=" << stripes
    1288            0 :                               << " dist=" << (*it).distToCrossing
    1289            0 :                               << " gap=" << (*it).vehAndGap.second
    1290              :                               << " brakeGap=" << bGap
    1291              :                               << " fromLeft=" << (*it).fromLeft()
    1292              :                               << " distToCrossBefore=" << distToCrossBeforeVeh
    1293              :                               << " ymin=" << vehYmin
    1294              :                               << " ymax=" << vehYmax
    1295              :                               << " smin=" << PState::stripe(vehYmin)
    1296            0 :                               << " smax=" << PState::stripe(vehYmax)
    1297            0 :                               << "\n";
    1298            0 :                     DEBUG_PRINT(obs);
    1299              :                 }
    1300              :             }
    1301              :         }
    1302       104126 :         if (hasCrossingVehObs) {
    1303              :             // check whether the crossing is fully blocked
    1304        69603 :             const int reserved = getReserved((int)obs.size(), RESERVE_FOR_ONCOMING_FACTOR_JUNCTIONS);
    1305              :             bool allBlocked = true;
    1306              : 
    1307       385258 :             for (int i = 0; i < (int)obs.size(); i++) {
    1308       342382 :                 const Obstacle& o = obs[i];
    1309       342382 :                 if (o.type != OBSTACLE_VEHICLE && (
    1310        49789 :                             (dir == FORWARD && i >= reserved) ||
    1311        34459 :                             (dir == BACKWARD && i < (int)obs.size() - reserved))) {
    1312              :                     allBlocked = false;
    1313              :                     break;
    1314              :                 }
    1315              :             }
    1316        69603 :             if (allBlocked) {
    1317        42876 :                 if (DEBUGCOND2(crossing)) {
    1318            0 :                     std::cout << SIMTIME << " crossing=" << crossing->getID() << " allBlocked\n";
    1319              :                 }
    1320       304252 :                 for (Obstacle& o : obs) {
    1321       261376 :                     if (dir == FORWARD) {
    1322       142446 :                         o.xBack = NUMERICAL_EPS;
    1323              :                     } else {
    1324       118930 :                         o.xFwd = crossing->getLength() - NUMERICAL_EPS;
    1325              :                     }
    1326              :                 }
    1327              :             }
    1328              :         }
    1329              :     }
    1330       935923 :     return hasCrossingVehObs;
    1331       935923 : }
    1332              : 
    1333              : 
    1334              : MSPModel_Striping::Obstacles
    1335       289781 : MSPModel_Striping::getVehicleObstacles(const MSLane* lane, int dir, PState* ped) {
    1336       289781 :     const int stripes = numStripes(lane);
    1337       579562 :     Obstacles vehObs(stripes, Obstacle(dir));
    1338              :     int current = -1;
    1339              :     double minX = 0.;
    1340              :     double maxX = 0.;
    1341              :     double pRelY = -1.;
    1342              :     double pWidth = 0.;
    1343              :     std::string pID;
    1344       289781 :     bool debug = DEBUGCOND2(lane);
    1345       289781 :     if (ped != nullptr) {
    1346       271141 :         current = ped->stripe();
    1347       271141 :         minX = ped->getMinX();
    1348       271141 :         maxX = ped->getMaxX();
    1349              :         pRelY = ped->getPosLat();
    1350       271141 :         pWidth = ped->getPerson()->getVehicleType().getWidth();
    1351              :         pID = ped->getPerson()->getID();
    1352       271141 :         debug = DEBUGCOND(*ped);
    1353        18640 :     } else if (dir == BACKWARD) {
    1354              :         // checking vehicles on the next lane. Use entry point as reference
    1355              :         minX = lane->getLength();
    1356              :         maxX = lane->getLength();
    1357              :     }
    1358       289781 :     MSLane::AnyVehicleIterator begin = (dir == FORWARD ? lane->anyVehiclesUpstreamBegin() : lane->anyVehiclesBegin());
    1359              :     MSLane::AnyVehicleIterator end = (dir == FORWARD ? lane->anyVehiclesUpstreamEnd() : lane->anyVehiclesEnd());
    1360       756568 :     for (MSLane::AnyVehicleIterator it = begin; it != end; ++it) {
    1361       466787 :         const MSVehicle* veh = *it;
    1362       466787 :         const bool bidi = veh->getLane() == lane->getBidiLane();
    1363       466787 :         const double vehBack = veh->getBackPositionOnLane(lane);
    1364       466787 :         double vehFront = vehBack + veh->getVehicleType().getLength();
    1365              :         // ensure that vehicles are not blocked
    1366       466787 :         const double vehNextSpeed = veh->getWaitingTime() > DELTA_T ? 0 : MAX2(veh->getSpeed(), 1.0);
    1367       466787 :         const double clearance = SAFETY_GAP + vehNextSpeed * LOOKAHEAD_SAMEDIR;
    1368              :         // boundaries for range checking
    1369              :         double vehXMax;
    1370              :         double vehXMin;
    1371              :         double vehXMaxCheck;
    1372              :         double vehXMinCheck;
    1373       466787 :         if (bidi) {
    1374         1571 :             vehFront = vehBack - veh->getVehicleType().getLength();
    1375         1571 :             vehXMax = vehBack + SAFETY_GAP;
    1376         1571 :             vehXMin = vehFront - clearance;
    1377         1571 :             if (dir == FORWARD) {
    1378         1571 :                 vehXMaxCheck = vehBack + NUMERICAL_EPS;
    1379         1571 :                 vehXMinCheck = vehFront - LOOKAROUND_VEHICLES;
    1380              :             } else  {
    1381            0 :                 vehXMaxCheck = vehBack + LOOKAHEAD_SAMEDIR;
    1382              :                 vehXMinCheck = vehFront - clearance;
    1383              :             }
    1384              :         } else {
    1385       465216 :             vehXMax = vehFront + clearance;
    1386       465216 :             vehXMin = vehBack - SAFETY_GAP;
    1387       465216 :             if (dir == FORWARD) {
    1388              :                 vehXMaxCheck = vehFront + clearance;
    1389       349478 :                 vehXMinCheck = vehBack - LOOKAHEAD_SAMEDIR;
    1390              :             } else  {
    1391       115738 :                 vehXMaxCheck = vehFront + LOOKAROUND_VEHICLES;
    1392       115738 :                 vehXMinCheck = vehBack - NUMERICAL_EPS;
    1393              :             }
    1394              :         }
    1395       466787 :         if (debug) {
    1396            0 :             std::cout << SIMTIME << " ped=" << pID << " veh=" << veh->getID() << " check obstacle on lane=" << lane->getID()
    1397              :                       << "\n"
    1398              :                       << " vehXMin=" << vehXMin
    1399              :                       << " vehXMax=" << vehXMax
    1400              :                       << " vehXMinC=" << vehXMinCheck
    1401              :                       << " vehXMaxC=" << vehXMaxCheck
    1402              :                       << " minX=" << minX
    1403              :                       << " maxX=" << maxX
    1404              :                       << " bidi=" << bidi
    1405              :                       << " vFront=" << vehFront
    1406              :                       << " vBack=" << vehBack
    1407            0 :                       << "\n";
    1408              :         }
    1409       466787 :         if (vehXMaxCheck > minX && vehXMinCheck && vehXMinCheck <= maxX) {
    1410       356117 :             Obstacle vo(vehBack, veh->getSpeed() * (bidi ? -1 : 1), OBSTACLE_VEHICLE, veh->getID(), 0, veh);
    1411              :             // moving vehicles block space along their path
    1412       178392 :             vo.xFwd = vehXMax;
    1413       178392 :             vo.xBack = vehXMin;
    1414              :             // relY increases from left to right (the other way around from vehicles)
    1415              :             // XXX lateral offset for partial vehicles
    1416       178392 :             const double posLat = veh->getLateralPositionOnLane() * (bidi ? -1 : 1);
    1417       178392 :             const double vehYmax = 0.5 * (lane->getWidth() + veh->getVehicleType().getWidth() - stripeWidth) - posLat;
    1418       178392 :             const double vehYmin = vehYmax - veh->getVehicleType().getWidth();
    1419       761449 :             for (int s = MAX2(0, PState::stripe(vehYmin)); s < MIN2(PState::stripe(vehYmax) + 1, stripes); ++s) {
    1420       583057 :                 Obstacle prior = vehObs[s];
    1421              :                 vehObs[s] = vo;
    1422       583057 :                 if (s == current && vehFront + SAFETY_GAP < minX) {
    1423              :                     // ignore if already overlapping while vehicle is still behind
    1424        16459 :                     if (pRelY - pWidth < vehYmax &&
    1425        16176 :                             pRelY + pWidth > vehYmin && dir == FORWARD) {
    1426        10681 :                         if (debug) {
    1427            0 :                             std::cout << "   ignoring vehicle '" << veh->getID() << " on stripe " << s << " vehFrontSG=" << vehFront + SAFETY_GAP << " minX=" << minX << "\n";
    1428              :                         }
    1429              :                         if (dir == FORWARD) {
    1430              :                             vehObs[s] = prior;
    1431              :                         } else {
    1432              :                             vehObs[s].xFwd = MIN2(vo.xFwd, vehFront + SAFETY_GAP);
    1433              :                         }
    1434              :                     }
    1435              :                 }
    1436              :             }
    1437       178392 :             if (debug) {
    1438            0 :                 std::cout << SIMTIME << " ped=" << pID << " veh=" << veh->getID() << " obstacle on lane=" << lane->getID()
    1439              :                           << "\n"
    1440              :                           << "     ymin=" << vehYmin
    1441              :                           << " ymax=" << vehYmax
    1442              :                           << " smin=" << PState::stripe(vehYmin)
    1443            0 :                           << " smax=" << PState::stripe(vehYmax)
    1444            0 :                           << " relY=" << pRelY
    1445              :                           << " current=" << current
    1446            0 :                           << " vo.xFwd=" << vo.xFwd
    1447            0 :                           << " vo.xBack=" << vo.xBack
    1448              :                           << " vFront=" << vehFront
    1449              :                           << " vBack=" << vehBack
    1450            0 :                           << "\n";
    1451              :             }
    1452              :         }
    1453              :     }
    1454       289781 :     return vehObs;
    1455            0 : }
    1456              : 
    1457              : 
    1458              : // ===========================================================================
    1459              : // MSPModel_Striping::Obstacle method definitions
    1460              : // ===========================================================================
    1461     84700646 : MSPModel_Striping::Obstacle::Obstacle(int dir, double dist) :
    1462     84700646 :     xFwd(dir * dist),  // by default, far away when seen in dir
    1463     84700646 :     xBack(dir * dist),  // by default, far away when seen in dir
    1464     84700646 :     speed(0),
    1465     84700646 :     type(OBSTACLE_NONE),
    1466     84700646 :     description("") {
    1467     84700646 : }
    1468              : 
    1469              : 
    1470    672617785 : MSPModel_Striping::Obstacle::Obstacle(const PState& ped) :
    1471    672617785 :     xFwd(ped.getMaxX()),
    1472    672617785 :     xBack(ped.getMinX()),
    1473    672617785 :     speed(ped.getDirection() * ped.getSpeed(*ped.getStage())),
    1474    672617785 :     type(ped.getOType()),
    1475    672617785 :     description(ped.getID()) {
    1476              :     assert(!ped.isWaitingToEnter());
    1477    672617785 :     if (type == OBSTACLE_VEHICLE) {
    1478       463388 :         vehicle = static_cast<const PStateVehicle&>(ped).getVehicle();
    1479              :     }
    1480    672617785 : }
    1481              : 
    1482              : 
    1483              : bool
    1484    130064928 : MSPModel_Striping::Obstacle::closer(const Obstacle& o, int dir) {
    1485    130064928 :     if (dir == FORWARD) {
    1486     70246372 :         return xBack <= o.xBack;
    1487              :     } else {
    1488     59818556 :         return xFwd >= o.xFwd;
    1489              :     }
    1490              : }
    1491              : 
    1492              : 
    1493              : // ===========================================================================
    1494              : // MSPModel_Striping::PState method definitions
    1495              : // ===========================================================================
    1496       243759 : MSPModel_Striping::PState::PState(MSPerson* person, MSStageMoving* stage, const MSLane* lane):
    1497              :     MSPModel_InteractingState(person, stage, lane),
    1498       243759 :     myWalkingAreaPath(nullptr) {
    1499              :     const MSEdge* currentEdge = &lane->getEdge();
    1500              :     const ConstMSEdgeVector& route = myStage->getRoute();
    1501              :     assert(!route.empty());
    1502       243759 :     myDir = FORWARD;
    1503       243759 :     if (route.size() == 1) {
    1504              :         // only a single edge, move towards end pos
    1505        13182 :         myDir = (myEdgePos <= myStage->getArrivalPos()) ? FORWARD : BACKWARD;
    1506       230577 :     } else if (route.front()->getFunction() != SumoXMLEdgeFunc::NORMAL) {
    1507              :         // start on an intersection
    1508            8 :         if (route.front()->isWalkingArea()) {
    1509            8 :             myWalkingAreaPath = getArbitraryPath(route.front());
    1510              :         }
    1511              :     } else {
    1512       230569 :         int passedFwd = 0;
    1513       230569 :         int passedBwd = 0;
    1514       230569 :         const bool mayStartForward = canTraverse(FORWARD, route, passedFwd) != UNDEFINED_DIRECTION;
    1515       230569 :         const bool mayStartBackward = canTraverse(BACKWARD, route, passedBwd) != UNDEFINED_DIRECTION;
    1516       230569 :         if DEBUGCOND(*this) {
    1517            0 :             std::cout << "  initialize dir for " << myPerson->getID() << " forward=" << mayStartForward << " backward=" << mayStartBackward << "\n";
    1518              :         }
    1519       230569 :         if (mayStartForward && mayStartBackward) {
    1520              :             // figure out the best direction via routing
    1521              :             ConstMSEdgeVector crossingRoute;
    1522         2462 :             MSNet::getInstance()->getPedestrianRouter(0).compute(currentEdge, route.back(), myEdgePos, myStage->getArrivalPos(), myStage->getMaxSpeed(person), 0, nullptr, crossingRoute, true);
    1523         2462 :             if (crossingRoute.size() > 1) {
    1524              :                 // route found
    1525         2454 :                 const MSEdge* nextEdge = crossingRoute[1];
    1526         2454 :                 if (nextEdge->getFromJunction() == currentEdge->getFromJunction() || nextEdge->getToJunction() == currentEdge->getFromJunction()) {
    1527         2079 :                     myDir = BACKWARD;
    1528              :                 }
    1529              :             }
    1530         2462 :             if DEBUGCOND(*this) {
    1531            0 :                 std::cout << " crossingRoute=" << toString(crossingRoute) << "\n";
    1532              :             }
    1533       230569 :         } else if (!mayStartForward && !mayStartBackward) {
    1534           61 :             int lastDisconnect = passedFwd >= passedBwd ? passedFwd : passedBwd;
    1535              :             std::string dLoc;
    1536           61 :             if (route.size() > 2) {
    1537           20 :                 dLoc = TLF(" between edge '%' and edge '%'", route[lastDisconnect - 1]->getID(), route[lastDisconnect]->getID());
    1538              :             }
    1539          366 :             WRITE_WARNINGF(TL("Person '%' walking from edge '%' to edge '%' has a disconnect%, time=%."),
    1540              :                            myPerson->getID(), route.front()->getID(), route.back()->getID(), dLoc, SIMTIME);
    1541           66 :             myDir =  passedFwd >= passedBwd ? FORWARD : BACKWARD;
    1542              :         } else {
    1543       428230 :             myDir = !mayStartBackward ? FORWARD : BACKWARD;
    1544              :         }
    1545              :     }
    1546       243759 :     if (myPosLat == UNSPECIFIED_POS_LAT || myLegacyPosLat) {
    1547       243599 :         if (myPosLat == UNSPECIFIED_POS_LAT) {
    1548       243413 :             myPosLat = 0;
    1549              :         }
    1550       243599 :         if (lane->getVehicleNumberWithPartials() > 0 && myPosLat == 0) {
    1551              :             // better start next to the road if nothing was specified
    1552         2133 :             myPosLat -= stripeWidth;
    1553              :         }
    1554       243599 :         if (myDir == FORWARD || lane->getPermissions() != SVC_PEDESTRIAN) {
    1555              :             // start at the right side of the sidewalk on shared roads
    1556       209358 :             myPosLat = stripeWidth * (numStripes(lane) - 1) - myPosLat;
    1557              :         }
    1558          160 :     } else if (myPosLat == RANDOM_POS_LAT) {
    1559           20 :         myPosLat = RandHelper::rand() * stripeWidth * (numStripes(lane) - 1);
    1560              :     } else {
    1561              :         // vehicle to striping coordinate system
    1562          140 :         myPosLat = posLatConversion(myPosLat, lane->getWidth());
    1563              :     }
    1564       243759 :     if DEBUGCOND(*this) {
    1565            0 :         std::cout << "  added new pedestrian " << myPerson->getID() << " on " << lane->getID() << " myEdgePos=" << myEdgePos << " myPosLat=" << myPosLat << " dir=" << myDir << " route=" << toString(myStage->getRoute()) << "\n";
    1566              :     }
    1567              : 
    1568       243759 :     myNLI = getNextLane(*this, lane, nullptr);
    1569       243755 : }
    1570              : 
    1571              : 
    1572           14 : MSPModel_Striping::PState::PState(MSPerson* person, MSStageMoving* stage, std::istringstream* in) :
    1573              :     MSPModel_InteractingState(person, stage, nullptr),
    1574           14 :     myWalkingAreaPath(nullptr) {
    1575           14 :     if (in != nullptr) {
    1576              :         std::string laneID;
    1577              :         std::string wapLaneFrom;
    1578              :         std::string wapLaneTo;
    1579              :         std::string nextLaneID;
    1580              :         std::string nextLinkFrom;
    1581              :         std::string nextLinkTo;
    1582              :         int nextDir;
    1583              : 
    1584           14 :         (*in) >> laneID
    1585           14 :               >> myEdgePos >> myPosLat >> myDir >> mySpeed >> mySpeedLat >> myWaitingToEnter >> myWaitingTime
    1586           14 :               >> wapLaneFrom >> wapLaneTo
    1587           14 :               >> myAmJammed
    1588              :               >> nextLaneID
    1589              :               >> nextLinkFrom
    1590           14 :               >> nextLinkTo
    1591           14 :               >> nextDir;
    1592              : 
    1593              : 
    1594           14 :         myLane = MSLane::dictionary(laneID);
    1595           14 :         if (myLane == nullptr) {
    1596            0 :             throw ProcessError("Unknown lane '" + laneID + "' when loading walk for person '" + myPerson->getID() + "' from state.");
    1597              :         }
    1598              : 
    1599              :         MSLane* nextLane = nullptr;
    1600           14 :         if (nextLaneID != "null") {
    1601           13 :             nextLane = MSLane::dictionary(nextLaneID);
    1602           13 :             if (nextLane == nullptr) {
    1603            0 :                 throw ProcessError("Unknown next lane '" + nextLaneID + "' when loading walk for person '" + myPerson->getID() + "' from state.");
    1604              :             }
    1605              :         }
    1606              :         const MSLink* link = nullptr;
    1607           14 :         if (nextLinkFrom != "null") {
    1608            2 :             MSLane* from = MSLane::dictionary(nextLinkFrom);
    1609            2 :             MSLane* to = MSLane::dictionary(nextLinkTo);
    1610            2 :             if (from == nullptr) {
    1611            0 :                 throw ProcessError("Unknown link origin lane '" + nextLinkFrom + "' when loading walk for person '" + myPerson->getID() + "' from state.");
    1612              :             }
    1613            2 :             if (to == nullptr) {
    1614            0 :                 throw ProcessError("Unknown link destination lane '" + nextLinkTo + "' when loading walk for person '" + myPerson->getID() + "' from state.");
    1615              :             }
    1616            2 :             link = from->getLinkTo(to);
    1617              :         }
    1618           14 :         myNLI =  NextLaneInfo(nextLane, link, nextDir);
    1619              : 
    1620           14 :         if (wapLaneFrom != "null") {
    1621            1 :             MSLane* from = MSLane::dictionary(wapLaneFrom);
    1622            1 :             MSLane* to = MSLane::dictionary(wapLaneTo);
    1623            1 :             if (from == nullptr) {
    1624            0 :                 throw ProcessError("Unknown walkingAreaPath origin lane '" + wapLaneFrom + "' when loading walk for person '" + myPerson->getID() + "' from state.");
    1625              :             }
    1626            1 :             if (to == nullptr) {
    1627            0 :                 throw ProcessError("Unknown walkingAreaPath destination lane '" + wapLaneTo + "' when loading walk for person '" + myPerson->getID() + "' from state.");
    1628              :             }
    1629            1 :             const auto pathIt = myWalkingAreaPaths.find(std::make_pair(from, to));
    1630            1 :             if (pathIt != myWalkingAreaPaths.end()) {
    1631            1 :                 myWalkingAreaPath = &pathIt->second;
    1632              :             } else {
    1633            0 :                 throw ProcessError("Unknown walkingAreaPath from lane '" + wapLaneFrom + "' to lane '" + wapLaneTo + "' when loading walk for person '" + myPerson->getID() + "' from state.");
    1634              :             }
    1635              :         }
    1636              :     }
    1637           14 : }
    1638              : 
    1639              : 
    1640       437850 : MSPModel_Striping::PState::PState() :
    1641       437850 :     MSPModel_InteractingState(nullptr, nullptr, nullptr) {}
    1642              : 
    1643              : 
    1644              : void
    1645            9 : MSPModel_Striping::PState::saveState(std::ostringstream& out) {
    1646            9 :     std::string wapLaneFrom = "null";
    1647            9 :     std::string wapLaneTo = "null";
    1648            9 :     if (myWalkingAreaPath != nullptr) {
    1649            1 :         wapLaneFrom = myWalkingAreaPath->from->getID();
    1650            1 :         wapLaneTo = myWalkingAreaPath->to->getID();
    1651              :     }
    1652            9 :     std::string nextLaneID = "null";
    1653            9 :     std::string nextLinkFrom = "null";
    1654            9 :     std::string nextLinkTo = "null";
    1655            9 :     if (myNLI.lane != nullptr) {
    1656              :         nextLaneID = myNLI.lane->getID();
    1657              :     }
    1658            9 :     if (myNLI.link != nullptr) {
    1659              :         nextLinkFrom = myNLI.link->getLaneBefore()->getID();
    1660            2 :         nextLinkTo = myNLI.link->getViaLaneOrLane()->getID();
    1661              :     }
    1662            9 :     out << " " << myLane->getID()
    1663           18 :         << " " << myEdgePos
    1664            9 :         << " " << myPosLat
    1665              :         << " " << myDir
    1666           18 :         << " " << mySpeed
    1667            9 :         << " " << mySpeedLat
    1668            9 :         << " " << myWaitingToEnter
    1669            9 :         << " " << myWaitingTime
    1670              :         << " " << wapLaneFrom
    1671              :         << " " << wapLaneTo
    1672            9 :         << " " << myAmJammed
    1673              :         << " " << nextLaneID
    1674              :         << " " << nextLinkFrom
    1675              :         << " " << nextLinkTo
    1676            9 :         << " " << myNLI.dir;
    1677            9 : }
    1678              : 
    1679              : double
    1680   2174541060 : MSPModel_Striping::PState::getMinX(const bool includeMinGap) const {
    1681              :     // @todo speed should have an influence here because faster persons need more space
    1682   2174541060 :     if (myDir == FORWARD) {
    1683   2010452345 :         return myEdgePos - getLength();
    1684              :     }
    1685    164088715 :     return myEdgePos - (includeMinGap ? getMinGap() : 0.);
    1686              : }
    1687              : 
    1688              : 
    1689              : double
    1690   2174541060 : MSPModel_Striping::PState::getMaxX(const bool includeMinGap) const {
    1691              :     // @todo speed should have an influence here because faster persons need more space
    1692   2174541060 :     if (myDir == FORWARD) {
    1693   2010452345 :         return myEdgePos + (includeMinGap ? getMinGap() : 0.);
    1694              :     }
    1695    164088715 :     return myEdgePos + getLength();
    1696              : }
    1697              : 
    1698              : 
    1699              : double
    1700   2242666357 : MSPModel_Striping::PState::getLength() const {
    1701   2242666357 :     return myPerson->getVehicleType().getLength();
    1702              : }
    1703              : 
    1704              : 
    1705              : double
    1706   1463732807 : MSPModel_Striping::PState::getMinGap() const {
    1707   1463732807 :     return myPerson->getVehicleType().getMinGap();
    1708              : }
    1709              : 
    1710              : 
    1711              : int
    1712   7809893648 : MSPModel_Striping::PState::stripe(double relY) {
    1713   7809893648 :     return (int)floor(relY / stripeWidth + 0.5);
    1714              : }
    1715              : 
    1716              : 
    1717              : int
    1718   3395589849 : MSPModel_Striping::PState::otherStripe(double relY) const {
    1719   3395589849 :     const int s = stripe(relY);
    1720   3395589849 :     const double offset = relY - s * stripeWidth;
    1721   3395589849 :     const double threshold = MAX2(NUMERICAL_EPS, stripeWidth - SQUEEZE * getWidth());
    1722              :     int result;
    1723   3395589849 :     if (offset > threshold) {
    1724       497855 :         result = s + 1;
    1725   3395091994 :     } else if (offset < -threshold) {
    1726       416662 :         result = s - 1;
    1727              :     } else {
    1728              :         result = s;
    1729              :     }
    1730              :     //std::cout.setf(std::ios::fixed , std::ios::floatfield);
    1731              :     //std::cout << std::setprecision(5);
    1732              :     //if DEBUGCOND(*this) std::cout << "  otherStripe " << myPerson->getID() << " offset=" << offset << " threshold=" << threshold << " rawResult=" << result << "\n";
    1733   3395589849 :     return result;
    1734              : }
    1735              : 
    1736              : int
    1737   4379628857 : MSPModel_Striping::PState::stripe() const {
    1738   4379628857 :     return MIN2(MAX2(0, stripe(myPosLat)), numStripes(myLane) - 1);
    1739              : }
    1740              : 
    1741              : 
    1742              : int
    1743   3379347219 : MSPModel_Striping::PState::otherStripe() const {
    1744   3379347219 :     return MIN2(MAX2(0, otherStripe(myPosLat)), numStripes(myLane) - 1);
    1745              : }
    1746              : 
    1747              : 
    1748              : double
    1749    138466349 : MSPModel_Striping::PState::distToLaneEnd() const {
    1750    138466349 :     if (myStage->getNextRouteEdge() == nullptr) {
    1751     24320875 :         return myDir * (myStage->getArrivalPos() - myEdgePos) - POSITION_EPS - (
    1752      2143857 :                    (myWaitingTime > DELTA_T && (myStage->getDestinationStop() == nullptr ||
    1753              :                                                 myStage->getDestinationStop()->getWaitingCapacity() > myStage->getDestinationStop()->getNumWaitingPersons()))
    1754     26464732 :                    ? getMinGap() : 0);
    1755              :     } else {
    1756    114145474 :         const double length = myWalkingAreaPath == nullptr ? myLane->getLength() : myWalkingAreaPath->length;
    1757    114145474 :         return myDir == FORWARD ? length - myEdgePos : myEdgePos;
    1758              :     }
    1759              : }
    1760              : 
    1761              : 
    1762              : bool
    1763       965615 : MSPModel_Striping::PState::moveToNextLane(SUMOTime currentTime) {
    1764       965615 :     double dist = distToLaneEnd();
    1765       965615 :     if (DEBUGCOND(*this)) {
    1766            0 :         std::cout << SIMTIME << " ped=" << myPerson->getID() << " myEdgePos=" << myEdgePos << " dist=" << dist << "\n";
    1767              :     }
    1768       965615 :     if (dist <= 0) {
    1769              :         //if (ped.getPerson()->getID() == DEBUG1) {
    1770              :         //    std::cout << SIMTIME << " addToLane x=" << ped.myEdgePos << " newDir=" << newDir << " newLane=" << newLane->getID() << " walkingAreaShape=" << walkingAreaShape << "\n";
    1771              :         //}
    1772              :         //std::cout << " changing to " << newLane->getID() << " myPosLat=" << ped.myPosLat << " oldStripes=" << numStripes(myLane) << " newStripes=" << numStripes(newLane);
    1773              :         //std::cout << " newY=" << ped.myPosLat << " myDir=" << ped.getDirection() << " newDir=" << newDir;
    1774       965615 :         const int oldDir = myDir;
    1775       965615 :         const MSLane* oldLane = myLane;
    1776       965615 :         myLane = myNLI.lane;
    1777       965615 :         myDir = myNLI.dir;
    1778       965615 :         const bool normalLane = (myLane == nullptr || myLane->getEdge().getFunction() == SumoXMLEdgeFunc::NORMAL || &myLane->getEdge() == myStage->getNextRouteEdge());
    1779       965615 :         if DEBUGCOND(*this) {
    1780            0 :             std::cout << SIMTIME
    1781            0 :                       << " ped=" << myPerson->getID()
    1782              :                       << " moveToNextLane old=" << oldLane->getID()
    1783            0 :                       << " new=" << (myLane == nullptr ? "NULL" : myLane->getID())
    1784              :                       << " oldDir=" << oldDir
    1785            0 :                       << " newDir=" << myDir
    1786            0 :                       << " myEdgePos=" << myEdgePos
    1787              :                       << " dist=" << dist
    1788            0 :                       << "\n";
    1789              :         }
    1790       965615 :         if (myLane == nullptr) {
    1791       221093 :             myEdgePos = myStage->getArrivalPos();
    1792              :         }
    1793              :         // moveToNextEdge might destroy the person and thus mess up the heap. Better check first
    1794       965615 :         if (myStage->getRouteStep() == myStage->getRoute().end() - 1) {
    1795       221093 :             myLane = nullptr;
    1796              :         } else {
    1797       744522 :             const bool arrived = myStage->moveToNextEdge(myPerson, currentTime, oldDir, normalLane ? nullptr : &myLane->getEdge());
    1798              :             UNUSED_PARAMETER(arrived);
    1799              :             assert(!arrived);
    1800              :             assert(myDir != UNDEFINED_DIRECTION);
    1801       744522 :             myNLI = getNextLane(*this, myLane, oldLane);
    1802              :             // reminders must be called after updated myNLI (to ensure that getNextEdgePtr returns the correct edge)
    1803       744522 :             myStage->activateEntryReminders(myPerson);
    1804              :             assert(myNLI.lane != oldLane); // do not turn around
    1805       744522 :             if DEBUGCOND(*this) {
    1806            0 :                 std::cout << "    nextLane=" << (myNLI.lane == nullptr ? "NULL" : myNLI.lane->getID()) << "\n";
    1807              :             }
    1808       744522 :             if (myLane->getEdge().isWalkingArea()) {
    1809       339429 :                 if (myNLI.dir != UNDEFINED_DIRECTION) {
    1810       339392 :                     myWalkingAreaPath = getWalkingAreaPath(&myLane->getEdge(), oldLane, myNLI.lane);
    1811              :                     assert(myWalkingAreaPath->shape.size() >= 2);
    1812       339392 :                     if DEBUGCOND(*this) {
    1813            0 :                         std::cout << "  mWAPath shape=" << myWalkingAreaPath->shape << " length=" << myWalkingAreaPath->length << "\n";
    1814              :                     }
    1815           37 :                 } else if (myNLI.link != nullptr) {
    1816              :                     // using direct connection (instead of using walkingarea)
    1817           17 :                     myLane = myNLI.lane;
    1818              :                     assert(!myLane->getEdge().isWalkingArea());
    1819           17 :                     myStage->moveToNextEdge(myPerson, currentTime, myDir, &myLane->getEdge());
    1820           17 :                     myWalkingAreaPath = nullptr;
    1821           17 :                     myNLI = getNextLane(*this, myLane, oldLane);
    1822              :                 } else {
    1823              :                     // disconnected route. move to the next edge
    1824           40 :                     if (OptionsCont::getOptions().getBool("ignore-route-errors")) {
    1825              :                         // try to determine direction from topology, otherwise maintain current direction
    1826           16 :                         const MSEdge* currRouteEdge = *myStage->getRouteStep();
    1827           16 :                         const MSEdge* nextRouteEdge = myStage->getNextRouteEdge();
    1828              :                         if ((nextRouteEdge->getToJunction() == currRouteEdge->getFromJunction())
    1829           16 :                                 || nextRouteEdge->getToJunction() == currRouteEdge->getToJunction()) {
    1830            4 :                             myDir = BACKWARD;
    1831              :                         } else if ((nextRouteEdge->getFromJunction() == currRouteEdge->getFromJunction())
    1832           12 :                                    || nextRouteEdge->getFromJunction() == currRouteEdge->getToJunction()) {
    1833           12 :                             myDir = FORWARD;
    1834              :                         }
    1835           16 :                         myStage->moveToNextEdge(myPerson, currentTime, oldDir, nullptr);
    1836           16 :                         myLane = myNLI.lane;
    1837              :                         assert(myLane != 0);
    1838              :                         assert(myLane->getEdge().getFunction() == SumoXMLEdgeFunc::NORMAL);
    1839           16 :                         myNLI = getNextLane(*this, myLane, oldLane);
    1840           16 :                         myWalkingAreaPath = nullptr;
    1841              :                     } else {
    1842           12 :                         throw ProcessError(TLF("Disconnected walk for person '%'.", myPerson->getID()));
    1843              :                     }
    1844              :                 }
    1845              :             } else {
    1846       405093 :                 myWalkingAreaPath = nullptr;
    1847              :             }
    1848              :             // adapt x to fit onto the new lane
    1849              :             // (make sure we do not move past the end of the new lane since that
    1850              :             // lane was not checked for obstacles)
    1851       744518 :             const double newLength = (myWalkingAreaPath == nullptr ? myLane->getLength() : myWalkingAreaPath->length);
    1852       744518 :             if (-dist > newLength) {
    1853              :                 assert(OptionsCont::getOptions().getBool("ignore-route-errors"));
    1854              :                 // should not happen because the end of myLane should have been an obstacle as well
    1855              :                 // (only when the route is broken)
    1856            1 :                 dist = -newLength;
    1857              :             }
    1858       744518 :             if (myDir == BACKWARD) {
    1859       138247 :                 myEdgePos = newLength + dist;
    1860              :             } else {
    1861       606271 :                 myEdgePos = -dist;
    1862              :             }
    1863       744518 :             if DEBUGCOND(*this) {
    1864            0 :                 std::cout << SIMTIME << " update myEdgePos ped=" << myPerson->getID()
    1865              :                           << " newLength=" << newLength
    1866              :                           << " dist=" << dist
    1867            0 :                           << " myEdgePos=" << myEdgePos
    1868            0 :                           << "\n";
    1869              :             }
    1870              :             // adjust to change in direction
    1871       744518 :             if (myDir != oldDir) {
    1872        35676 :                 myPosLat = (numStripes(oldLane) - 1) * stripeWidth - myPosLat;
    1873              :             }
    1874              :             // adjust to differences in sidewalk width
    1875      1474538 :             const int offset = getStripeOffset(numStripes(oldLane), numStripes(myLane), oldDir != myDir && numStripes(myLane) < numStripes(oldLane));
    1876       744518 :             myPosLat += offset * stripeWidth;
    1877       744518 :             if DEBUGCOND(*this) {
    1878            0 :                 std::cout << SIMTIME << " transformY ped=" << myPerson->getID()
    1879            0 :                           << " newLane=" << Named::getIDSecure(myLane)
    1880            0 :                           << " newY=" << myPosLat
    1881            0 :                           << " os=" << numStripes(oldLane) << " ns=" << numStripes(myLane)
    1882            0 :                           << " od=" << oldDir << " nd=" << myDir
    1883            0 :                           << " offset=" << offset << "\n";
    1884              :             }
    1885              :         }
    1886       965611 :         myAngle = std::numeric_limits<double>::max(); // see #9014
    1887       965611 :         return true;
    1888              :     } else {
    1889              :         return false;
    1890              :     }
    1891              : }
    1892              : 
    1893              : 
    1894              : int
    1895     68194900 : MSPModel_Striping::getReserved(int stripes, double factor) {
    1896     68194900 :     return MIN2(
    1897     68194900 :                (int)floor(stripes * factor),
    1898     68194900 :                (int)floor(RESERVE_FOR_ONCOMING_MAX / stripeWidth));
    1899              : }
    1900              : 
    1901              : void
    1902     68125297 : MSPModel_Striping::PState::walk(const Obstacles& obs, SUMOTime currentTime) {
    1903     68125297 :     const int stripes = (int)obs.size();
    1904     68125297 :     const int sMax =  stripes - 1;
    1905              :     assert(stripes == numStripes(myLane));
    1906              :     // account stage-specific max speed but also for normal lane speed limit
    1907              :     // (speed limits on crossings and walkingareas ignored due to #11527)
    1908     68125297 :     const double vMax = (myStage->getConfiguredSpeed() >= 0
    1909     68125297 :                          ? myStage->getConfiguredSpeed()
    1910      9389952 :                          : (USE_NET_SPEEDS || myLane->isNormal() || myLane->isInternal()
    1911     76675820 :                             ? myLane->getVehicleMaxSpeed(myPerson)
    1912       785035 :                             : myStage->getMaxSpeed(myPerson)));
    1913              :     // ultimate goal is to choose the preferred stripe (chosen)
    1914     68125297 :     const int current = stripe();
    1915     68125297 :     const int other = otherStripe();
    1916              :     // compute distances
    1917     68125297 :     std::vector<double> distance(stripes);
    1918    295616235 :     for (int i = 0; i < stripes; ++i) {
    1919    227490938 :         distance[i] = distanceTo(obs[i], obs[i].type == OBSTACLE_PED);
    1920              :     }
    1921              :     // compute utility for all stripes
    1922     68125297 :     std::vector<double> utility(stripes, 0);
    1923              :     // forbid stripes which are blocked and also all stripes behind them
    1924    295616235 :     for (int i = 0; i < stripes; ++i) {
    1925    227490938 :         if (distance[i] == DIST_OVERLAP) {
    1926    102488259 :             if (i == current && (!isWaitingToEnter() || stripe() != stripe(myPosLat))) {
    1927      7500011 :                 utility[i] += OBSTRUCTED_PENALTY;
    1928              :             }
    1929    102488259 :             if (i < current) {
    1930    144043391 :                 for (int j = 0; j <= i; ++j) {
    1931     87840366 :                     utility[j] += OBSTRUCTED_PENALTY;
    1932              :                 }
    1933              :             }
    1934    102488259 :             if (i > current) {
    1935     62563956 :                 for (int j = i; j < stripes; ++j) {
    1936     36232173 :                     utility[j] += OBSTRUCTED_PENALTY;
    1937              :                 }
    1938              :             }
    1939              :         }
    1940              :     }
    1941              :     // forbid a portion of the leftmost stripes (in walking direction).
    1942              :     // lanes with stripes less than 1 / RESERVE_FOR_ONCOMING_FACTOR
    1943              :     // may still deadlock in heavy pedestrian traffic
    1944     68125297 :     const bool onJunction = myLane->getEdge().isWalkingArea() || myLane->getEdge().isCrossing();
    1945     68125297 :     const int reserved = getReserved(stripes, (onJunction ? RESERVE_FOR_ONCOMING_FACTOR_JUNCTIONS : RESERVE_FOR_ONCOMING_FACTOR));
    1946     68125297 :     if (myDir == FORWARD) {
    1947     71871051 :         for (int i = 0; i < reserved; ++i) {
    1948     17165373 :             utility[i] += INAPPROPRIATE_PENALTY * (i == current ? 0.5 : 1);
    1949              :         }
    1950              :     } else {
    1951      6619839 :         for (int i = sMax; i > sMax - reserved; --i) {
    1952      2786306 :             utility[i] += INAPPROPRIATE_PENALTY * (i == current ? 0.5 : 1);
    1953              :         }
    1954              :     }
    1955              :     // adapt utility based on obstacles
    1956    295616235 :     for (int i = 0; i < stripes; ++i) {
    1957    227490938 :         if (obs[i].speed * myDir < 0) {
    1958              :             // penalize evasion to the left unless the obstacle is a vehicle
    1959      4303464 :             if ((myDir == FORWARD || obs[i].type == OBSTACLE_VEHICLE) && i > 0) {
    1960       803001 :                 utility[i - 1] -= 0.5;
    1961      3500463 :             } else if (myDir == BACKWARD && i < sMax) {
    1962       598007 :                 utility[i + 1] -= 0.5;
    1963              :             }
    1964              :         }
    1965              :         // compute expected distance achievable by staying on this stripe for a time horizon
    1966    227490938 :         const double walkDist = MAX2(0., distance[i]); // disregard special distance flags
    1967    227490938 :         const double lookAhead = obs[i].speed * myDir >= 0 ? LOOKAHEAD_SAMEDIR : LOOKAHEAD_ONCOMING;
    1968    227490938 :         const double expectedDist = MIN2(vMax * LOOKAHEAD_SAMEDIR, walkDist + obs[i].speed * myDir * lookAhead);
    1969    227490938 :         if (expectedDist >= 0) {
    1970    224391576 :             utility[i] += expectedDist;
    1971              :         } else {
    1972              :             // let only the distance count
    1973      3099362 :             utility[i] += ONCOMING_CONFLICT_PENALTY + distance[i];
    1974              :         }
    1975              :     }
    1976              :     // discourage use of the leftmost stripe (in walking direction) if there are oncoming
    1977     68125297 :     if (myDir == FORWARD && obs[0].speed < 0) {
    1978      1438971 :         utility[0] += ONCOMING_CONFLICT_PENALTY;
    1979     66686326 :     } else if (myDir == BACKWARD && obs[sMax].speed > 0) {
    1980      1466573 :         utility[sMax] += ONCOMING_CONFLICT_PENALTY;
    1981              :     }
    1982              :     // penalize lateral movement (if the current stripe permits walking)
    1983     68125297 :     if (distance[current] > 0 && myWaitingTime == 0) {
    1984    128241845 :         for (int i = 0; i < stripes; ++i) {
    1985    100504310 :             utility[i] += abs(i - current) * LATERAL_PENALTY;
    1986              :         }
    1987              :     }
    1988              :     // walk on the right side on shared space
    1989     68125297 :     if (myLane->getPermissions() != SVC_PEDESTRIAN && myDir == BACKWARD) {
    1990      2287310 :         for (int i = 0; i < stripes; ++i) {
    1991      1906378 :             if (i <= current) {
    1992      1817927 :                 utility[i] += (sMax - i + 1) * LATERAL_PENALTY;
    1993              :             }
    1994              :         }
    1995              :     }
    1996              : 
    1997              :     // select best stripe
    1998              :     int chosen = current;
    1999    295616235 :     for (int i = 0; i < stripes; ++i) {
    2000    227490938 :         if (utility[i] > utility[chosen] && utility[i] >= INAPPROPRIATE_PENALTY * 0.5) {
    2001              :             chosen = i;
    2002              :         }
    2003              :     }
    2004              :     // compute speed components along both axes
    2005     68125297 :     const int next = (chosen == current ? current : (chosen < current ? current - 1 : current + 1));
    2006     68125297 :     double xDist = MIN3(distance[current], distance[other], distance[next]);
    2007     68125297 :     if (next != chosen) {
    2008              :         // ensure that we do not collide with an obstacle in the stripe beyond
    2009              :         // next as this might become the 'other' stripe in the next step
    2010       476553 :         const int nextOther = chosen < current ? current - 2 : current + 2;
    2011       476553 :         xDist = MIN2(xDist, distance[nextOther]);
    2012              :     }
    2013              :     // XXX preferred gap differs between approaching a standing obstacle or a moving obstacle
    2014              :     const double preferredGap = NUMERICAL_EPS;
    2015     68125297 :     double xSpeed = MIN2(vMax, MAX2(0., DIST2SPEED(xDist - preferredGap)));
    2016     68125297 :     if (xSpeed < NUMERICAL_EPS) {
    2017              :         xSpeed = 0.;
    2018              :     }
    2019     68125297 :     if (DEBUGCOND(*this)) {
    2020            0 :         std::cout << " xSpeedPotential=" << xSpeed << "\n";
    2021              :     }
    2022              :     // avoid tiny steps
    2023              :     // XXX pressure from behind?
    2024     68125297 :     if (mySpeed == 0 && xDist < MIN_STARTUP_DIST &&
    2025              :             // unless walking towards a short lane
    2026     31266001 :             !(
    2027     31566754 :                 (xDist == distance[current] && obs[current].type >= OBSTACLE_END)
    2028     31266020 :                 || (xDist == distance[other] && obs[other].type >= OBSTACLE_END)
    2029     31266001 :                 || (xDist == distance[next] && obs[next].type >= OBSTACLE_END))
    2030              :        ) {
    2031              :         xSpeed = 0;
    2032              :     }
    2033     36859298 :     if (xSpeed == 0) {
    2034     38556802 :         if (DEBUGCOND(*this)) {
    2035            0 :             std::cout << " sharedWA=" << (myWalkingAreaFoes.find(&myLane->getEdge()) != myWalkingAreaFoes.end())
    2036            0 :                       << " vehObs=" << Named::getIDSecure(obs[current].vehicle)
    2037            0 :                       << " vehWait=" << STEPS2TIME(obs[current].vehicle ? obs[current].vehicle->getWaitingTime() : 0)
    2038            0 :                       << "\n";
    2039              :         }
    2040     38556802 :         if (myWaitingTime > ((myLane->getEdge().isCrossing()
    2041              :                               // treat shared walkingarea like a crossing to avoid deadlocking vehicles
    2042     38548952 :                               || (myLane->getEdge().isWalkingArea() && obs[current].vehicle != nullptr && obs[current].vehicle->getWaitingTime() > jamTimeCrossing
    2043     77106068 :                                   && myWalkingAreaFoes.find(&myLane->getEdge()) != myWalkingAreaFoes.end())) ? jamTimeCrossing : jamTime)
    2044     38476343 :                 || (sMax == 0 && obs[0].speed * myDir < 0 && myWaitingTime > jamTimeNarrow)
    2045     77004561 :                 || myAmJammed) {
    2046              :             // squeeze slowly through the crowd ignoring others
    2047      8677544 :             if (!myAmJammed) {
    2048        91558 :                 MSNet::getInstance()->getPersonControl().registerJammed();
    2049       366232 :                 WRITE_WARNINGF(TL("Person '%' is jammed on edge '%', time=%."),
    2050              :                                myPerson->getID(), myStage->getEdge()->getID(), time2string(SIMSTEP));
    2051        91558 :                 myAmJammed = true;
    2052              :             }
    2053      8677544 :             xSpeed = vMax * jamFactor;
    2054              :         }
    2055     29568495 :     } else if (myAmJammed && stripe(myPosLat) >= 0 && stripe(myPosLat) <= sMax && xDist >= MIN_STARTUP_DIST)  {
    2056        80736 :         myAmJammed = false;
    2057              :     }
    2058              :     // dawdling
    2059     68125297 :     const double dawdle = MIN2(xSpeed, RandHelper::rand() * vMax * dawdling);
    2060     68125297 :     xSpeed -= dawdle;
    2061              : 
    2062              :     // XXX ensure that diagonal speed <= vMax
    2063              :     // avoid deadlocks on narrow sidewalks
    2064              :     //if (oncoming && xSpeed == 0 && myStage->getWaitingTime(currentTime) > TIME2STEPS(ONCOMIN_PATIENCE)) {
    2065              :     //    if DEBUGCOND(*this) std::cout << "  stepping aside to resolve oncoming deadlock\n";
    2066              :     //    xSpeed = POSITION_EPS; // reset myWaitingTime
    2067              :     //     if (myDir == FORWARD && chosen < sMax) {
    2068              :     //         chosen += 1;
    2069              :     //     } else if (myDir == BACKWARD && chosen > 0) {
    2070              :     //         chosen -= 1;
    2071              :     //     }
    2072              :     //}
    2073     68125297 :     const double maxYSpeed = MIN2(MAX2(vMax * LATERAL_SPEED_FACTOR, vMax - xSpeed), stripeWidth);
    2074              :     double ySpeed = 0;
    2075              :     double yDist = 0;
    2076     68125297 :     if (utility[next] > OBSTRUCTION_THRESHOLD && utility[chosen] > OBSTRUCTION_THRESHOLD) {
    2077              :         // don't move laterally if the stripes are blocked
    2078     60672503 :         yDist = (chosen * stripeWidth) - myPosLat;
    2079     60672503 :         if (fabs(yDist) > NUMERICAL_EPS) {
    2080      2726397 :             ySpeed = (yDist > 0 ?
    2081      1252110 :                       MIN2(maxYSpeed, DIST2SPEED(yDist)) :
    2082      1474287 :                       MAX2(-maxYSpeed, DIST2SPEED(yDist)));
    2083              :         }
    2084      7452794 :     } else if (utility[next] <= OBSTRUCTION_THRESHOLD && obs[next].type == OBSTACLE_VEHICLE
    2085              :                // still on the road
    2086         9651 :                && stripe() == stripe(myPosLat)
    2087              :                // only when the vehicle is moving on the same lane
    2088      7458033 :                && !(myLane->getEdge().isCrossing() || myLane->getEdge().isWalkingArea())) {
    2089              :         // step aside to let the vehicle pass
    2090         2823 :         int stepAsideDir = myDir;
    2091         2823 :         if (myLane->getEdge().getLanes().size() > 1 || current > sMax / 2) {
    2092              :             // always step to the right on multi-lane edges or when closer to
    2093              :             // the right side
    2094              :             stepAsideDir = FORWARD;
    2095              :         }
    2096         2823 :         myAmJammed = true; // ignore pedestrian-pedestrian collisions
    2097         2823 :         ySpeed = stepAsideDir * vMax;
    2098              :     }
    2099              : 
    2100              :     // DEBUG
    2101     68125297 :     if DEBUGCOND(*this) {
    2102            0 :         std::cout << SIMTIME
    2103            0 :                   << " ped=" << myPerson->getID()
    2104            0 :                   << " edge=" << myStage->getEdge()->getID()
    2105            0 :                   << " x=" << myEdgePos
    2106            0 :                   << " y=" << myPosLat
    2107              :                   << " d=" << myDir
    2108            0 :                   << " pvx=" << mySpeed
    2109              :                   << " cur=" << current
    2110            0 :                   << " cho=" << chosen
    2111            0 :                   << " oth=" << other
    2112            0 :                   << " nxt=" << next
    2113            0 :                   << " vx=" << xSpeed
    2114              :                   << " dawdle=" << dawdle
    2115              :                   << " vy=" << ySpeed
    2116              :                   << " xd=" << xDist
    2117              :                   << " yd=" << yDist
    2118              :                   << " vMax=" << vMax
    2119            0 :                   << " wTime=" << myStage->getWaitingTime(currentTime)
    2120            0 :                   << " jammed=" << myAmJammed
    2121            0 :                   << "\n";
    2122            0 :         if (DEBUGCOND(*this)) {
    2123            0 :             for (int i = 0; i < stripes; ++i) {
    2124            0 :                 const Obstacle& o = obs[i];
    2125            0 :                 std::cout << " util=" << utility[i] << " dist=" << distance[i] << " o=" << o.description;
    2126            0 :                 if (o.description != "") {
    2127            0 :                     std::cout << " xF=" << o.xFwd << " xB=" << o.xBack << " v=" << o.speed;
    2128              :                 }
    2129            0 :                 if (i == current) {
    2130            0 :                     std::cout << " current";
    2131              :                 }
    2132            0 :                 if (i == other && i != current) {
    2133            0 :                     std::cout << " other";
    2134              :                 }
    2135            0 :                 if (i == chosen) {
    2136            0 :                     std::cout << " chosen";
    2137              :                 }
    2138            0 :                 if (i == next) {
    2139            0 :                     std::cout << " next";
    2140              :                 }
    2141            0 :                 std::cout << "\n";
    2142              :             }
    2143              :         }
    2144              :     }
    2145     68125297 :     myEdgePos += SPEED2DIST(xSpeed * myDir);
    2146     68125297 :     myPosLat += SPEED2DIST(ySpeed);
    2147     68125297 :     mySpeedLat = ySpeed;
    2148     68125297 :     mySpeed = xSpeed;
    2149     68125297 :     if (xSpeed >= SUMO_const_haltingSpeed) {
    2150     34099401 :         myWaitingToEnter = false;
    2151     34099401 :         myWaitingTime = 0;
    2152              :     } else {
    2153     34025896 :         myWaitingTime += DELTA_T;
    2154              :     }
    2155     68125297 :     myAngle = std::numeric_limits<double>::max(); // set on first access or via remote control
    2156     68125297 : }
    2157              : 
    2158              : 
    2159              : double
    2160     11623603 : MSPModel_Striping::PState::getImpatience(SUMOTime now) const {
    2161     11623603 :     return MAX2(0., MIN2(1., myPerson->getVehicleType().getImpatience()
    2162     11623603 :                          + STEPS2TIME(myStage->getWaitingTime(now)) / MAX_WAIT_TOLERANCE));
    2163              : }
    2164              : 
    2165              : 
    2166              : Position
    2167     10271124 : MSPModel_Striping::PState::getPosition(const MSStageMoving& stage, SUMOTime) const {
    2168              :     if (myRemoteXYPos != Position::INVALID) {
    2169          362 :         return myRemoteXYPos;
    2170              :     }
    2171     10270762 :     if (myLane == nullptr) {
    2172              :         // pedestrian has already finished
    2173          228 :         return Position::INVALID;
    2174              :     }
    2175     10270534 :     const double lateral_offset = -getLatOffset();  // the minus is hunting me in my dreams but seems to be here for historical reasons
    2176     10270534 :     if (myWalkingAreaPath == nullptr) {
    2177       863964 :         return stage.getLanePosition(myLane, myEdgePos, lateral_offset);
    2178              :     } else {
    2179              :         //if DEBUGCOND(*this) {
    2180              :         //    std::cout << SIMTIME
    2181              :         //        << " getPosition (walkingArea)"
    2182              :         //        << " p=" << myPerson->getID()
    2183              :         //        << " x=" << myEdgePos
    2184              :         //        << " y=" << myPosLat
    2185              :         //        << " latOffset=" << lateral_offset
    2186              :         //        << " shape=" << myWalkingAreaPath->shape
    2187              :         //        << " pos=" << myWalkingAreaPath->shape.positionAtOffset(myEdgePos, lateral_offset)
    2188              :         //        << "\n";
    2189              :         //}
    2190      9406570 :         if (myWalkingAreaPath->angleOverride == INVALID_DOUBLE) {
    2191      9327797 :             return myWalkingAreaPath->shape.positionAtOffset(myEdgePos, lateral_offset);
    2192              :         } else {
    2193        78773 :             const double rotationOffset = myDir == FORWARD ? 0 : DEG2RAD(180);
    2194        78773 :             return myWalkingAreaPath->shape.sidePositionAtAngle(myEdgePos, lateral_offset, myWalkingAreaPath->angleOverride + rotationOffset);
    2195              :         }
    2196              :     }
    2197              : }
    2198              : 
    2199              : 
    2200              : double
    2201      2452153 : MSPModel_Striping::PState::getAngle(const MSStageMoving&, SUMOTime) const {
    2202      2452153 :     if (myAngle != std::numeric_limits<double>::max()) {
    2203              :         return myAngle;
    2204              :     }
    2205       636836 :     if (myLane == nullptr) {
    2206              :         // pedestrian has already finished
    2207              :         return 0;
    2208              :     }
    2209       636607 :     if (myWalkingAreaPath != nullptr && myWalkingAreaPath->angleOverride != INVALID_DOUBLE) {
    2210              :         return myWalkingAreaPath->angleOverride;
    2211              :     }
    2212       623556 :     const PositionVector& shp = myWalkingAreaPath == nullptr ? myLane->getShape() : myWalkingAreaPath->shape;
    2213       623556 :     double geomX = myWalkingAreaPath == nullptr ? myLane->interpolateLanePosToGeometryPos(myEdgePos) : myEdgePos;
    2214       623556 :     double angle = shp.rotationAtOffset(geomX) + (myDir == MSPModel::BACKWARD ? M_PI : 0);
    2215       623556 :     if (myDir == MSPModel::BACKWARD) {
    2216       174893 :         angle += atan2(mySpeedLat, MAX2(mySpeed, NUMERICAL_EPS));
    2217              :     } else { // myDir == MSPModel::FORWARD
    2218       580085 :         angle -= atan2(mySpeedLat, MAX2(mySpeed, NUMERICAL_EPS));
    2219              :     }
    2220       623556 :     if (angle > M_PI) {
    2221       120707 :         angle -= 2 * M_PI;
    2222              :     }
    2223       623556 :     myAngle = angle;
    2224       623556 :     return angle;
    2225              : }
    2226              : 
    2227              : 
    2228              : const MSEdge*
    2229       166312 : MSPModel_Striping::PState::getNextEdge(const MSStageMoving&) const {
    2230       166312 :     return myNLI.lane == nullptr ? nullptr : &myNLI.lane->getEdge();
    2231              : }
    2232              : 
    2233              : 
    2234              : const MSLane*
    2235         1498 : MSPModel_Striping::PState::getNextCrossing() const {
    2236         1498 :     return myNLI.lane != nullptr && myNLI.lane->isCrossing() ? myNLI.lane : nullptr;
    2237              : }
    2238              : 
    2239              : 
    2240              : void
    2241        19816 : MSPModel_Striping::PState::reverse(const double pathLength, const double usableWidth) {
    2242        19816 :     myEdgePos = pathLength - myEdgePos;
    2243        19816 :     myPosLat = usableWidth - myPosLat;
    2244        19816 :     myDir = -myWalkingAreaPath->dir;
    2245        19816 :     mySpeedLat = -mySpeedLat;
    2246        19816 : }
    2247              : 
    2248              : 
    2249              : void
    2250       243480 : MSPModel_Striping::PState::reset(const double edgePos, const double latPos) {
    2251       243480 :     myEdgePos = edgePos;
    2252       243480 :     myPosLat = latPos;
    2253       243480 :     myDir = UNDEFINED_DIRECTION;  // only an obstacle, speed may be orthogonal to dir
    2254       243480 :     mySpeed = 0.;
    2255       243480 :     mySpeedLat = 0.;
    2256       243480 : }
    2257              : 
    2258              : 
    2259              : void
    2260           17 : MSPModel_Striping::PState::moveTo(MSPerson* p, MSLane* lane, double lanePos, double lanePosLat, SUMOTime t) {
    2261              :     ConstMSEdgeVector newEdges; // keep route
    2262              :     int routeOffset = 0;
    2263              :     bool laneOnRoute = false;
    2264           17 :     const MSJunction* laneOnJunction = lane->isNormal() ? nullptr : lane->getEdge().getToJunction();
    2265           17 :     for (const MSEdge* edge : myStage->getRoute()) {
    2266              :         if (edge == &lane->getEdge()
    2267           12 :                 || edge->getToJunction() == laneOnJunction
    2268           17 :                 || edge->getFromJunction() == laneOnJunction) {
    2269              :             laneOnRoute = true;
    2270              :             break;
    2271              :         }
    2272            0 :         routeOffset++;
    2273              :     }
    2274           17 :     if (!laneOnRoute) {
    2275            0 :         throw ProcessError("Lane '" + lane->getID() + "' is not on the route of person '" + getID() + "'.");
    2276              :     }
    2277           17 :     Position pos = lane->geometryPositionAtOffset(lanePos, lanePosLat);
    2278           17 :     if (lane->getEdge().isWalkingArea() && (myWalkingAreaPath == nullptr || myWalkingAreaPath->lane != lane)) {
    2279              :         // entered new walkingarea. Determine path to guess position
    2280            8 :         const MSEdge* prevEdge = myStage->getRoute()[routeOffset];
    2281            8 :         const MSEdge* nextEdge = routeOffset + 1 < (int)myStage->getRoute().size() ? myStage->getRoute()[routeOffset + 1] : nullptr;
    2282            8 :         const WalkingAreaPath* guessed = guessPath(&lane->getEdge(), prevEdge, nextEdge);
    2283            8 :         const double maxPos = guessed->shape.length() - NUMERICAL_EPS;
    2284            8 :         if (lanePos > maxPos + POSITION_EPS || lanePos < -POSITION_EPS) {
    2285            0 :             throw ProcessError("Lane position " + toString(lanePos) + " cannot be mapped onto walkingarea '" + lane->getID()
    2286            0 :                                + "' (fromLane='" + guessed->from->getID()
    2287            0 :                                + "' toLane='" + guessed->to->getID() + "') for person '" + getID() + "' time=" + time2string(t) + ".");
    2288              :         }
    2289              :         // give some slack
    2290            8 :         lanePos = MIN2(maxPos, MAX2(NUMERICAL_EPS, lanePos));
    2291            8 :         pos = guessed->shape.positionAtOffset(lanePos, lanePosLat);
    2292              :     }
    2293           17 :     const double angle = GeomHelper::naviDegree(p->getPosition().angleTo2D(pos));
    2294           17 :     moveToXY(p, pos, lane, lanePos, lanePosLat, angle, routeOffset, newEdges, t);
    2295           17 : }
    2296              : 
    2297              : 
    2298              : void
    2299        10317 : MSPModel_Striping::PState::moveToXY(MSPerson* p, Position pos, MSLane* lane, double lanePos,
    2300              :                                     double lanePosLat, double angle, int routeOffset,
    2301              :                                     const ConstMSEdgeVector& edges, SUMOTime t) {
    2302        10317 :     MSPModel_Striping* pm = dynamic_cast<MSPModel_Striping*>(MSNet::getInstance()->getPersonControl().getMovementModel());
    2303              :     assert(p == myPerson);
    2304              :     assert(pm != nullptr);
    2305        10317 :     const double oldAngle = GeomHelper::naviDegree(getAngle(*myStage, t));
    2306              :     // person already walking in this step. undo this to obtain the previous position
    2307        10317 :     const double oldX = myEdgePos - SPEED2DIST(mySpeed * myDir);
    2308              :     const double tmp = myEdgePos;
    2309        10317 :     myEdgePos = oldX;
    2310        10317 :     Position oldPos = getPosition(*myStage, t);
    2311        10317 :     myEdgePos = tmp;
    2312              :     //if (oldPos == Position::INVALID) {
    2313              :     //    oldPos = pos
    2314              :     //}
    2315        10317 :     myAngle = GeomHelper::fromNaviDegree(angle);
    2316              : #ifdef DEBUG_MOVETOXY
    2317              :     std::cout << SIMTIME << " ped=" << p->getID()
    2318              :               << " moveToXY"
    2319              :               << " pos=" << pos
    2320              :               << " lane=" << lane->getID()
    2321              :               << " lanePos=" << lanePos
    2322              :               << " lanePosLat=" << lanePosLat
    2323              :               << " angle=" << angle
    2324              :               << " routeOffset=" << routeOffset
    2325              :               << " edges=" << toString(edges)
    2326              :               << " oldLane=" << Named::getIDSecure(myLane)
    2327              :               << " path=" << (myWalkingAreaPath == nullptr ? "null" : (myWalkingAreaPath->from->getID() + "->" + myWalkingAreaPath->to->getID())) << "\n";
    2328              : #endif
    2329              : 
    2330        10317 :     if (lane != myLane && myLane != nullptr) {
    2331          290 :         pm->remove(this);
    2332              :         pm->registerActive();
    2333              :     }
    2334        10317 :     if (lane != nullptr &&
    2335        10317 :             fabs(lanePosLat) < (0.5 * (lane->getWidth() + p->getVehicleType().getWidth()) + SIDEWALK_OFFSET)) {
    2336        10312 :         myRemoteXYPos = Position::INVALID;
    2337        10312 :         const MSEdge* old = myStage->getEdge();
    2338        10312 :         const MSLane* oldLane = myLane;
    2339        10312 :         if (lane != myLane) {
    2340              :             // implicitly adds new active lane if necessary
    2341          290 :             pm->myActiveLanes[lane].push_back(this);
    2342              :         }
    2343        10312 :         if (edges.empty()) {
    2344              :             // map within route
    2345        10242 :             myStage->setRouteIndex(myPerson, routeOffset);
    2346              :         } else {
    2347           70 :             myStage->replaceRoute(myPerson, edges, routeOffset);
    2348              :         }
    2349        10312 :         if (!lane->getEdge().isNormal()) {
    2350          525 :             myStage->moveToNextEdge(myPerson, t, myDir, &lane->getEdge());
    2351              :         }
    2352              : 
    2353        10312 :         myLane = lane;
    2354        10312 :         const double lateral_offset = (lane->getWidth() - stripeWidth) * 0.5;
    2355        10312 :         if (lane->getEdge().isWalkingArea()) {
    2356          288 :             if (myWalkingAreaPath == nullptr || myWalkingAreaPath->lane != lane) {
    2357              :                 // entered new walkingarea. Determine path
    2358           51 :                 myWalkingAreaPath = guessPath(&lane->getEdge(), old, myStage->getNextRouteEdge());
    2359              : #ifdef DEBUG_MOVETOXY
    2360              :                 std::cout << " guessPath old=" << old->getID() << " next=" << Named::getIDSecure(myStage->getNextRouteEdge())
    2361              :                           << " path=" << myWalkingAreaPath->from->getID() << "->" << myWalkingAreaPath->to->getID() << "\n";
    2362              : #endif
    2363              :             }
    2364              :             // lanePos and lanePosLat are matched onto the circumference of the
    2365              :             // walkingarea. Use pos instead
    2366          288 :             const Position relPos = myWalkingAreaPath->shape.transformToVectorCoordinates(pos);
    2367              :             if (relPos == Position::INVALID) {
    2368          168 :                 WRITE_WARNING("Could not map position " + toString(pos) + " onto lane '" + myLane->getID()
    2369              :                               + "' (fromLane='" + myWalkingAreaPath->from->getID()
    2370              :                               + "' toLane='" + myWalkingAreaPath->to->getID() + "') for person '" + getID() + "' time=" + time2string(t) + ".");
    2371           42 :                 myRemoteXYPos = pos;
    2372              :             } else {
    2373          246 :                 myEdgePos = relPos.x();
    2374          246 :                 myPosLat = lateral_offset + relPos.y();
    2375              :             }
    2376              :         } else {
    2377        10024 :             myWalkingAreaPath = nullptr;
    2378        10024 :             myEdgePos = lanePos;
    2379        10024 :             myPosLat = lateral_offset - lanePosLat;
    2380              :         }
    2381              :         // guess direction
    2382        10312 :         const double angleDiff = GeomHelper::getMinAngleDiff(angle, oldAngle);
    2383        10312 :         if (myStage->getNextRouteEdge() != nullptr) {
    2384         5544 :             if (myStage->getEdge()->getToJunction() == myStage->getNextRouteEdge()->getFromJunction() ||
    2385          420 :                     myStage->getEdge()->getToJunction() == myStage->getNextRouteEdge()->getToJunction()) {
    2386         4824 :                 myDir = FORWARD;
    2387              :             } else {
    2388          300 :                 myDir = BACKWARD;
    2389              :             }
    2390              :         } else {
    2391              :             // guess from angle
    2392         5188 :             if (angleDiff <= 90) {
    2393              :                 // keep direction
    2394         5168 :                 if (myDir == UNDEFINED_DIRECTION) {
    2395            0 :                     myDir = FORWARD;
    2396              :                 }
    2397              :             } else {
    2398              :                 // change direction
    2399           20 :                 myDir = (myDir == BACKWARD) ? FORWARD : BACKWARD;
    2400              :             }
    2401              :         }
    2402              :         // update next lane info (after guessing direction)
    2403        10312 :         if (oldLane == nullptr || &oldLane->getEdge() != &myLane->getEdge()) {
    2404          240 :             const MSLane* sidewalk = getSidewalk<MSEdge, MSLane>(&myLane->getEdge(), p->getVClass());
    2405              :             // assume that we will eventually move back onto the sidewalk if
    2406              :             // there is one
    2407          240 :             myNLI = getNextLane(*this, sidewalk == nullptr ? myLane : sidewalk, nullptr);
    2408          240 :             myStage->activateEntryReminders(myPerson);
    2409              : #ifdef DEBUG_MOVETOXY
    2410              :             std::cout << " myNLI=" << Named::getIDSecure(myNLI.lane) << " link=" << (myNLI.link == nullptr ? "NULL" : myNLI.link->getDescription()) << " dir=" << myNLI.dir << "\n";
    2411              : #endif
    2412              :         }
    2413              : #ifdef DEBUG_MOVETOXY
    2414              :         std::cout << " newRelPos=" << Position(myEdgePos, myPosLat) << " edge=" << myPerson->getEdge()->getID() << " newPos=" << myPerson->getPosition()
    2415              :                   << " oldAngle=" << oldAngle << " angleDiff=" << angleDiff << " newDir=" << myDir << "\n";
    2416              : #endif
    2417        10312 :         if (oldLane == myLane) {
    2418        10022 :             mySpeed = DIST2SPEED(fabs(oldX - myEdgePos));
    2419              :         } else {
    2420              :             //std::cout << SIMTIME << " oldX=" << oldX << " oldSpeed=" << mySpeed << " oldPos=" << oldPos << " pos=" << pos << " dist=" << oldPos.distanceTo2D(pos) << " oldLane=" << Named::getIDSecure(oldLane) << " lane=" << lane->getID() << "\n";
    2421          290 :             mySpeed = DIST2SPEED(oldPos.distanceTo2D(pos));
    2422              :         }
    2423              :     } else {
    2424              :         // map outside the network
    2425            5 :         myRemoteXYPos = pos;
    2426            5 :         mySpeed = DIST2SPEED(oldPos.distanceTo2D(pos));
    2427              :     }
    2428              : 
    2429        10317 : }
    2430              : 
    2431              : 
    2432              : double
    2433       733621 : MSPModel_Striping::PState::getPathLength() const {
    2434       733621 :     if (myWalkingAreaPath != nullptr) {
    2435       674002 :         return myWalkingAreaPath->length;
    2436              :     } else {
    2437              :         return 0;
    2438              :     }
    2439              : }
    2440              : 
    2441              : double
    2442   1502115522 : MSPModel_Striping::PState::distanceTo(const Obstacle& obs, const bool includeMinGap) const {
    2443              :     // check for overlap
    2444   1502115522 :     const double maxX = getMaxX(includeMinGap);
    2445   1502115522 :     const double minX = getMinX(includeMinGap);
    2446              :     //if (DEBUGCOND(*this)) {
    2447              :     //    std::cout << std::setprecision(2) <<   "   distanceTo=" << obs.description << " maxX=" << maxX << " minX=" << minX << " obs.xFwd=" << obs.xFwd << " obs.xBack=" << obs.xBack << "\n";
    2448              :     //}
    2449   1502115522 :     if ((obs.xFwd >= maxX && obs.xBack <= maxX) || (obs.xFwd <= maxX && obs.xFwd >= minX)) {
    2450              :         // avoid blocking by itself on looped route
    2451    428801445 :         return (obs.type == OBSTACLE_PED && obs.description == myPerson->getID()) ? DIST_FAR_AWAY : DIST_OVERLAP;
    2452              :     }
    2453   1073314077 :     if (myDir == FORWARD) {
    2454    966098566 :         return obs.xFwd < minX ? DIST_BEHIND : obs.xBack - maxX;
    2455              :     } else {
    2456    107215511 :         return obs.xBack > maxX ? DIST_BEHIND : minX - obs.xFwd;
    2457              :     }
    2458              : }
    2459              : 
    2460              : 
    2461              : void
    2462     87692673 : MSPModel_Striping::PState::mergeObstacles(Obstacles& into, const Obstacles& obs2) {
    2463    385585288 :     for (int i = 0; i < (int)into.size(); ++i) {
    2464    297892615 :         if (gDebugFlag1) {
    2465            0 :             std::cout << "     i=" << i << " maxX=" << getMaxX(true) << " minX=" << getMinX(true)
    2466            0 :                       << " into=" << into[i].description << " iDist=" << distanceTo(into[i], into[i].type == OBSTACLE_PED)
    2467            0 :                       << " obs2=" << obs2[i].description << " oDist=" << distanceTo(obs2[i], obs2[i].type == OBSTACLE_PED) << "\n";
    2468              :         }
    2469    297892615 :         const double dO = distanceTo(obs2[i], obs2[i].type == OBSTACLE_PED);
    2470    297892615 :         const double dI = distanceTo(into[i], into[i].type == OBSTACLE_PED);
    2471    297892615 :         if (dO < dI) {
    2472              :             into[i] = obs2[i];
    2473              :         } else if (dO == dI
    2474     72943089 :                    && into[i].type != OBSTACLE_PED
    2475     33221078 :                    && into[i].type != OBSTACLE_VEHICLE
    2476    286246952 :                    && (obs2[i].type == OBSTACLE_PED ||
    2477              :                        obs2[i].type == OBSTACLE_VEHICLE)) {
    2478              :             into[i] = obs2[i];
    2479              :         }
    2480              :     }
    2481     87692673 : }
    2482              : 
    2483              : void
    2484        18640 : MSPModel_Striping::PState::mergeObstacles(Obstacles& into, const Obstacles& obs2, int dir, int offset) {
    2485       146342 :     for (int i = 0; i < (int)into.size(); ++i) {
    2486       127702 :         int i2 = i + offset;
    2487       127702 :         if (i2 >= 0 && i2 < (int)obs2.size()) {
    2488       126636 :             if (dir == FORWARD) {
    2489        91728 :                 if (obs2[i2].xBack < into[i].xBack) {
    2490              :                     into[i] = obs2[i2];
    2491              :                 }
    2492              :             } else {
    2493        34908 :                 if (obs2[i2].xFwd > into[i].xFwd) {
    2494              :                     into[i] = obs2[i2];
    2495              :                 }
    2496              :             }
    2497              :         }
    2498              :     }
    2499        18640 : }
    2500              : 
    2501              : 
    2502              : bool
    2503     11623603 : MSPModel_Striping::PState::ignoreRed(const MSLink* link) const {
    2504     11623603 :     if (link->haveRed()) {
    2505       651707 :         const double ignoreRedTime = myPerson->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_AFTER_RED_TIME, -1);
    2506       651707 :         if (ignoreRedTime >= 0) {
    2507           40 :             const double redDuration = STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - link->getLastStateChange());
    2508           40 :             if (DEBUGCOND(*this)) {
    2509            0 :                 std::cout << SIMTIME << "  ignoreRedTime=" << ignoreRedTime << " redDuration=" << redDuration << "\n";
    2510              :             }
    2511           40 :             return ignoreRedTime > redDuration;
    2512              :         } else {
    2513              :             return false;
    2514              :         }
    2515              :     } else {
    2516              :         return false;
    2517              :     }
    2518              : }
    2519              : 
    2520              : 
    2521              : bool
    2522     10926878 : MSPModel_Striping::PState::stopForYellow(const MSLink* link) const {
    2523              :     // main use case is at rail_crossing
    2524     10926878 :     if (link->haveYellow()) {
    2525          966 :         const double ignoreYellowTime = myPerson->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_AFTER_YELLOW_TIME, -1);
    2526          966 :         if (ignoreYellowTime >= 0) {
    2527            0 :             const double yellowDuration = STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - link->getLastStateChange());
    2528            0 :             if (DEBUGCOND(*this)) {
    2529            0 :                 std::cout << SIMTIME << "  ignoreYellowTime=" << ignoreYellowTime << " yellowDuration=" << yellowDuration << "\n";
    2530              :             }
    2531            0 :             return ignoreYellowTime < yellowDuration;
    2532              :         } else {
    2533              :             return true;
    2534              :         }
    2535              :     } else {
    2536              :         return false;
    2537              :     }
    2538              : }
    2539              : 
    2540              : double
    2541   3394706432 : MSPModel_Striping::PState::getWidth() const {
    2542   3394706432 :     return myPerson->getVehicleType().getWidth();
    2543              : }
    2544              : 
    2545              : 
    2546              : bool
    2547    129504757 : MSPModel_Striping::PState::isRemoteControlled() const {
    2548    129504757 :     return myPerson->hasInfluencer() && myPerson->getInfluencer().isRemoteControlled();
    2549              : }
    2550              : 
    2551              : // ===========================================================================
    2552              : // MSPModel_Striping::PStateVehicle method definitions
    2553              : // ===========================================================================
    2554              : 
    2555       437850 : MSPModel_Striping::PStateVehicle::PStateVehicle(const MSVehicle* veh, const MSLane* walkingarea, double relX, double relY, double xWidth, double yWidth):
    2556       437850 :     myVehicle(veh), myXWidth(xWidth), myYWidth(yWidth) {
    2557       437850 :     myLane = walkingarea; // to ensure correct limits when calling otherStripe()
    2558              :     // relX is the center but we want it to be the max value if the movement direction is forward
    2559              :     // and the min value otherwise (indicated by xWidth sign)
    2560       437850 :     myEdgePos = relX + xWidth / 2;
    2561       437850 :     myPosLat = relY;
    2562       437850 : }
    2563              : 
    2564              : const std::string&
    2565      1751322 : MSPModel_Striping::PStateVehicle::getID() const {
    2566      1751322 :     return myVehicle->getID();
    2567              : }
    2568              : 
    2569              : double
    2570       883417 : MSPModel_Striping::PStateVehicle::getWidth() const {
    2571       883417 :     return myYWidth;
    2572              : }
    2573              : 
    2574              : double
    2575       463388 : MSPModel_Striping::PStateVehicle::getMinX(const bool /*includeMinGap*/) const {
    2576       463388 :     return myXWidth > 0 ? myEdgePos - myXWidth : myEdgePos;
    2577              : }
    2578              : 
    2579              : double
    2580       463388 : MSPModel_Striping::PStateVehicle::getMaxX(const bool /*includeMinGap*/) const {
    2581       463388 :     return myXWidth > 0 ? myEdgePos : myEdgePos - myXWidth;
    2582              : }
    2583              : 
    2584              : // ===========================================================================
    2585              : // MSPModel_Striping::MovePedestrians method definitions
    2586              : // ===========================================================================
    2587              : 
    2588              : SUMOTime
    2589      9018369 : MSPModel_Striping::MovePedestrians::execute(SUMOTime currentTime) {
    2590              :     std::set<MSPerson*> changedLane;
    2591      9018369 :     myModel->moveInDirection(currentTime, changedLane, FORWARD);
    2592      9018365 :     myModel->moveInDirection(currentTime, changedLane, BACKWARD);
    2593              :     // DEBUG
    2594              : #ifdef LOG_ALL
    2595              :     for (ActiveLanes::const_iterator it_lane = myModel->getActiveLanes().begin(); it_lane != myModel->getActiveLanes().end(); ++it_lane) {
    2596              :         const MSLane* lane = it_lane->first;
    2597              :         Pedestrians pedestrians = it_lane->second;
    2598              :         if (pedestrians.size() == 0) {
    2599              :             continue;
    2600              :         }
    2601              :         sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(FORWARD));
    2602              :         std::cout << SIMTIME << " lane=" << lane->getID();
    2603              :         for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
    2604              :             const PState& p = *pedestrians[ii];
    2605              :             std::cout << " (" << p.getPerson()->getID() << " " << p.myEdgePos << "," << p.myPosLat << " " << p.getDirection() << ")";
    2606              :         }
    2607              :         std::cout << "\n";
    2608              :     }
    2609              : #endif
    2610      9018365 :     return DELTA_T;
    2611              : }
        

Generated by: LCOV version 2.0-1