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 = ¤tLane->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 : }
|