Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2014-2026 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 6119 : MSPModel_Striping::MSPModel_Striping(const OptionsCont& oc, MSNet* net) {
118 6119 : myWalkingAreaDetail = oc.getInt("pedestrian.striping.walkingarea-detail");
119 6119 : initWalkingAreaPaths(net);
120 : // configurable parameters
121 6119 : stripeWidth = oc.getFloat("pedestrian.striping.stripe-width");
122 6119 : MSVehicleType* defaultPedType = MSNet::getInstance()->getVehicleControl().getVType(DEFAULT_PEDTYPE_ID, nullptr, true);
123 6119 : 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 6119 : dawdling = oc.getFloat("pedestrian.striping.dawdling");
129 6119 : minGapToVehicle = oc.getFloat("pedestrian.striping.mingap-to-vehicle");
130 6119 : RESERVE_FOR_ONCOMING_FACTOR = oc.getFloat("pedestrian.striping.reserve-oncoming");
131 6119 : RESERVE_FOR_ONCOMING_FACTOR_JUNCTIONS = oc.getFloat("pedestrian.striping.reserve-oncoming.junctions");
132 6119 : 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 6119 : USE_NET_SPEEDS = net->getNetworkVersion() >= MMVersion(1, 20);
135 :
136 6119 : jamTime = string2time(oc.getString("pedestrian.striping.jamtime"));
137 6119 : if (jamTime <= 0) {
138 4 : jamTime = SUMOTime_MAX;
139 : }
140 6119 : jamTimeCrossing = string2time(oc.getString("pedestrian.striping.jamtime.crossing"));
141 6119 : if (jamTimeCrossing <= 0) {
142 0 : jamTimeCrossing = SUMOTime_MAX;
143 : }
144 6119 : jamTimeNarrow = string2time(oc.getString("pedestrian.striping.jamtime.narrow"));
145 6119 : if (jamTimeNarrow <= 0) {
146 0 : jamTimeNarrow = SUMOTime_MAX;
147 : }
148 6119 : jamFactor = oc.getFloat("pedestrian.striping.jamfactor");
149 6119 : myLegacyPosLat = oc.getBool("pedestrian.striping.legacy-departposlat");
150 6119 : }
151 :
152 :
153 12206 : MSPModel_Striping::~MSPModel_Striping() {
154 : myWalkingAreaPaths.clear(); // need to recompute when lane pointers change
155 : myWalkingAreaFoes.clear();
156 : myMinNextLengths.clear();
157 12206 : }
158 :
159 :
160 : MSTransportableStateAdapter*
161 204051 : MSPModel_Striping::add(MSTransportable* transportable, MSStageMoving* stage, SUMOTime) {
162 204051 : 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 204051 : MSNet* net = MSNet::getInstance();
168 204051 : if (!myAmActive) {
169 4249 : net->getBeginOfTimestepEvents()->addEvent(new MovePedestrians(this), net->getCurrentTimeStep() + DELTA_T);
170 4249 : myAmActive = true;
171 : }
172 : assert(person->getCurrentStageType() == MSStageType::WALKING);
173 204051 : const MSLane* lane = stage->checkDepartLane(person->getEdge(), person->getVClass(), stage->getDepartLane(), person->getID());
174 204051 : 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 204043 : PState* ped = new PState(person, stage, lane);
184 204039 : myActiveLanes[lane].push_back(ped);
185 204039 : myNumActivePedestrians++;
186 204039 : return ped;
187 : }
188 :
189 :
190 : MSTransportableStateAdapter*
191 19 : MSPModel_Striping::loadState(MSTransportable* transportable, MSStageMoving* stage, std::istringstream& in) {
192 : MSPerson* person = static_cast<MSPerson*>(transportable);
193 19 : MSNet* net = MSNet::getInstance();
194 19 : if (!myAmActive) {
195 13 : net->getBeginOfTimestepEvents()->addEvent(new MovePedestrians(this), SIMSTEP);
196 13 : myAmActive = true;
197 : }
198 19 : PState* ped = new PState(person, stage, &in);
199 19 : myActiveLanes[ped->getLane()].push_back(ped);
200 19 : myNumActivePedestrians++;
201 19 : return ped;
202 : }
203 :
204 :
205 : int
206 6078834715 : MSPModel_Striping::numStripes(const MSLane* lane) {
207 6078834715 : return MAX2(1, (int)floor(lane->getWidth() / stripeWidth));
208 : }
209 :
210 :
211 : int
212 812106 : MSPModel_Striping::connectedDirection(const MSLane* from, const MSLane* to) {
213 812106 : if (from == nullptr || to == nullptr) {
214 0 : return UNDEFINED_DIRECTION;
215 812106 : } else if (from->getLinkTo(to) != nullptr) {
216 564946 : return FORWARD;
217 247160 : } else if (to->getLinkTo(from) != nullptr) {
218 247160 : return BACKWARD;
219 : } else {
220 0 : return UNDEFINED_DIRECTION;
221 : }
222 : }
223 :
224 :
225 : void
226 6119 : MSPModel_Striping::initWalkingAreaPaths(const MSNet*) {
227 6119 : if (myWalkingAreaPaths.size() > 0) {
228 : return;
229 : }
230 : // collect vehicle lanes that cross walkingareas
231 431289 : for (MSEdgeVector::const_iterator i = MSEdge::getAllEdges().begin(); i != MSEdge::getAllEdges().end(); ++i) {
232 425171 : const MSEdge* edge = *i;
233 425171 : if (!edge->isWalkingArea() && !edge->isCrossing()) {
234 786104 : for (MSLane* lane : edge->getLanes()) {
235 963708 : for (MSLink* link : lane->getLinkCont()) {
236 532364 : if (link->getWalkingAreaFoe() != nullptr) {
237 : // link is an exit link
238 2936 : myWalkingAreaFoes[&link->getWalkingAreaFoe()->getEdge()].push_back(link->getLaneBefore());
239 : //std::cout << " wa=" << link->getWalkingAreaFoe()->getEdge().getID() << " foe=" << link->getLaneBefore()->getID() << "\n";
240 : }
241 532364 : if (link->getWalkingAreaFoeExit() != nullptr) {
242 : // link is an exit link
243 3020 : 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 431289 : for (MSEdgeVector::const_iterator i = MSEdge::getAllEdges().begin(); i != MSEdge::getAllEdges().end(); ++i) {
253 425171 : insertWalkArePaths(*i, myWalkingAreaPaths);
254 : }
255 : }
256 :
257 :
258 : void
259 425171 : MSPModel_Striping::insertWalkArePaths(const MSEdge* edge, WalkingAreaPaths& into) {
260 425171 : if (edge->isWalkingArea()) {
261 61791 : const MSLane* walkingArea = getSidewalk<MSEdge, MSLane>(edge);
262 61791 : myMinNextLengths[walkingArea] = walkingArea->getLength();
263 : // build all possible paths across this walkingArea
264 : // gather all incident lanes
265 : std::vector<const MSLane*> lanes;
266 137285 : for (const MSEdge* in : edge->getPredecessors()) {
267 75494 : if (!in->isTazConnector()) {
268 71148 : lanes.push_back(getSidewalk<MSEdge, MSLane>(in));
269 71148 : if (lanes.back() == nullptr) {
270 0 : throw ProcessError("Invalid connection from edge '" + in->getID() + "' to walkingarea edge '" + edge->getID() + "'");
271 : }
272 : }
273 : }
274 137474 : for (const MSEdge* out : edge->getSuccessors()) {
275 75683 : if (!out->isTazConnector()) {
276 71337 : lanes.push_back(getSidewalk<MSEdge, MSLane>(out));
277 71337 : 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 204276 : for (int j = 0; j < (int)lanes.size(); ++j) {
284 498292 : for (int k = 0; k < (int)lanes.size(); ++k) {
285 355807 : if (j != k) {
286 : // build the walkingArea
287 213322 : const MSLane* const from = lanes[j];
288 213322 : const MSLane* const to = lanes[k];
289 213322 : const int fromDir = from->getLinkTo(walkingArea) != nullptr ? FORWARD : BACKWARD;
290 213322 : const int toDir = walkingArea->getLinkTo(to) != nullptr ? FORWARD : BACKWARD;
291 213322 : PositionVector shape;
292 320243 : Position fromPos = from->getShape()[fromDir == FORWARD ? -1 : 0];
293 319723 : Position toPos = to->getShape()[toDir == FORWARD ? 0 : -1];
294 213322 : const double maxExtent = fromPos.distanceTo2D(toPos) / 4; // prevent sharp corners
295 213322 : const double extrapolateBy = MIN2(maxExtent, walkingArea->getWidth() / 2);
296 : // assemble shape
297 213322 : shape.push_back(fromPos);
298 213322 : if (extrapolateBy > POSITION_EPS) {
299 : PositionVector fromShp = from->getShape();
300 212312 : fromShp.extrapolate(extrapolateBy);
301 424624 : shape.push_back_noDoublePos(fromDir == FORWARD ? fromShp.back() : fromShp.front());
302 : PositionVector nextShp = to->getShape();
303 212312 : nextShp.extrapolate(extrapolateBy);
304 424624 : shape.push_back_noDoublePos(toDir == FORWARD ? nextShp.front() : nextShp.back());
305 212312 : }
306 213322 : shape.push_back_noDoublePos(toPos);
307 213322 : if (shape.size() < 2) {
308 : PositionVector fromShp = from->getShape();
309 558 : fromShp.extrapolate(1.5 * POSITION_EPS); // noDoublePos requires a difference of POSITION_EPS in at least one coordinate
310 1116 : shape.push_back_noDoublePos(fromDir == FORWARD ? fromShp.back() : fromShp.front());
311 : assert(shape.size() == 2);
312 213322 : } else if (myWalkingAreaDetail > 4) {
313 448 : shape = shape.bezier(myWalkingAreaDetail);
314 : }
315 : double angleOverride = INVALID_DOUBLE;
316 213322 : if (shape.size() >= 4 && shape.length() < walkingArea->getWidth()) {
317 55288 : const double aStart = shape.angleAt2D(0);
318 55288 : const double aEnd = shape.angleAt2D((int)shape.size() - 2);
319 55288 : if (fabs(aStart - aEnd) < DEG2RAD(10)) {
320 9061 : angleOverride = (aStart + aEnd) / 2;
321 : }
322 : }
323 213322 : if (fromDir == BACKWARD) {
324 : // will be walking backward on walkingArea
325 213842 : shape = shape.reverse();
326 : }
327 213322 : WalkingAreaPath wap = WalkingAreaPath(from, walkingArea, to, shape, fromDir, angleOverride);
328 213322 : into.insert(std::make_pair(std::make_pair(from, to), wap));
329 247109 : myMinNextLengths[walkingArea] = MIN2(myMinNextLengths[walkingArea], wap.length);
330 213322 : }
331 : }
332 : }
333 61791 : }
334 425171 : }
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 46 : MSPModel_Striping::guessPath(const MSEdge* walkingArea, const MSEdge* before, const MSEdge* after) {
355 : assert(walkingArea->isWalkingArea());
356 46 : const MSLane* swBefore = getSidewalk<MSEdge, MSLane>(before);
357 46 : const MSLane* swAfter = getSidewalk<MSEdge, MSLane>(after);
358 46 : const auto pathIt = myWalkingAreaPaths.find(std::make_pair(swBefore, swAfter));
359 46 : if (pathIt != myWalkingAreaPaths.end()) {
360 19 : return &pathIt->second;
361 : }
362 : const MSEdgeVector& preds = walkingArea->getPredecessors();
363 27 : const MSEdgeVector& succs = walkingArea->getSuccessors();
364 27 : bool useBefore = swBefore != nullptr && std::find(preds.begin(), preds.end(), before) != preds.end();
365 27 : bool useAfter = swAfter != nullptr && std::find(succs.begin(), succs.end(), after) != succs.end();
366 27 : if (useBefore) {
367 21 : if (useAfter) {
368 0 : return getWalkingAreaPath(walkingArea, swBefore, swAfter);
369 21 : } else if (succs.size() > 0) {
370 : // could also try to exploit direction
371 21 : return getWalkingAreaPath(walkingArea, swBefore, getSidewalk<MSEdge, MSLane>(succs.front()));
372 : }
373 6 : } else if (useAfter && preds.size() > 0) {
374 : // could also try to exploit direction
375 6 : return getWalkingAreaPath(walkingArea, getSidewalk<MSEdge, MSLane>(preds.front()), swAfter);
376 : }
377 0 : return getArbitraryPath(walkingArea);
378 : }
379 :
380 :
381 : const MSPModel_Striping::WalkingAreaPath*
382 797403 : MSPModel_Striping::getWalkingAreaPath(const MSEdge* walkingArea, const MSLane* before, const MSLane* after) {
383 : assert(walkingArea->isWalkingArea());
384 797403 : const auto pathIt = myWalkingAreaPaths.find(std::make_pair(before, after));
385 797403 : if (pathIt != myWalkingAreaPaths.end()) {
386 797369 : return &pathIt->second;
387 : }
388 : // this can happen in case of moveToXY where before can point anywhere
389 : // or when a person starts directly on a walking area (before == nullptr)
390 38 : for (const MSEdge* const pred : walkingArea->getPredecessors()) {
391 38 : const auto pathIt2 = myWalkingAreaPaths.find(std::make_pair(getSidewalk<MSEdge, MSLane>(pred), after));
392 38 : if (pathIt2 != myWalkingAreaPaths.end()) {
393 34 : return &pathIt2->second;
394 : }
395 : }
396 0 : return getArbitraryPath(walkingArea);
397 : }
398 :
399 :
400 :
401 : MSPModel_Striping::NextLaneInfo
402 1390208 : MSPModel_Striping::getNextLane(const PState& ped, const MSLane* currentLane, const MSLane* prevLane) {
403 : const MSEdge* currentEdge = ¤tLane->getEdge();
404 1390208 : const MSJunction* junction = ped.getDirection() == FORWARD ? currentEdge->getToJunction() : currentEdge->getFromJunction();
405 1390208 : const MSEdge* nextRouteEdge = ped.getStage()->getNextRouteEdge();
406 1390208 : const MSLane* nextRouteLane = getSidewalk<MSEdge, MSLane>(nextRouteEdge, ped.getPerson()->getVClass());
407 : // result values
408 : const MSLane* nextLane = nextRouteLane;
409 1390208 : const MSLink* link = nullptr;
410 1390208 : int nextDir = UNDEFINED_DIRECTION;
411 :
412 : //if DEBUGCOND(ped) {
413 : // std::cout << " nextRouteLane=" << Named::getIDSecure(nextRouteLane) << " junction=" << junction->getID() << "\n";
414 : //}
415 1390208 : if (nextRouteLane == nullptr && nextRouteEdge != nullptr) {
416 56 : std::string error = "Person '" + ped.getPerson()->getID() + "' could not find sidewalk on edge '" + nextRouteEdge->getID() + "', time="
417 56 : + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".";
418 56 : if (OptionsCont::getOptions().getBool("ignore-route-errors")) {
419 76 : WRITE_WARNING(error);
420 24 : nextRouteLane = nextRouteEdge->getLanes().front();
421 : } else {
422 4 : throw ProcessError(error);
423 : }
424 : }
425 :
426 1390204 : if (nextRouteLane != nullptr) {
427 1194442 : if (currentEdge->isInternal()) {
428 : assert(junction == currentEdge->getFromJunction());
429 23625 : nextDir = junction == nextRouteEdge->getFromJunction() ? FORWARD : BACKWARD;
430 23625 : if (nextDir == FORWARD) {
431 22654 : nextLane = currentLane->getLinkCont()[0]->getViaLaneOrLane();
432 : } else {
433 971 : nextLane = currentLane->getLogicalPredecessorLane();
434 : }
435 23625 : if DEBUGCOND(ped) {
436 0 : std::cout << " internal\n";
437 : }
438 1170817 : } else if (currentEdge->isCrossing()) {
439 33113 : nextDir = ped.getDirection();
440 33113 : if (nextDir == FORWARD) {
441 16825 : nextLane = currentLane->getLinkCont()[0]->getLane();
442 : } else {
443 16288 : nextLane = currentLane->getLogicalPredecessorLane();
444 : }
445 33113 : if DEBUGCOND(ped) {
446 0 : std::cout << " crossing\n";
447 : }
448 33113 : if (currentLane->isPriorityCrossing()) {
449 31075 : unregisterCrossingApproach(ped, currentLane);
450 : }
451 1137704 : } 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 812143 : const int nextRouteEdgeDir = nextRouteEdge->getFromJunction() == junction ? FORWARD : BACKWARD;
456 812143 : const double arrivalPos = (nextRouteEdge == ped.getStage()->getRoute().back()
457 812143 : ? ped.getStage()->getArrivalPos()
458 205041 : : (nextRouteEdgeDir == FORWARD ? 0 : nextRouteEdge->getLength()));
459 : SUMOAbstractRouter<MSEdge, SUMOVehicle>::Prohibitions prohibited;
460 812143 : if (prevLane != nullptr) {
461 812095 : prohibited[&prevLane->getEdge()].end = std::numeric_limits<double>::max();
462 : }
463 1624286 : MSNet::getInstance()->getPedestrianRouter(0, prohibited).compute(currentEdge, nextRouteEdge, 0, arrivalPos,
464 812143 : ped.getStage()->getMaxSpeed(ped.getPerson()),
465 812143 : 0, junction, ped.getPerson()->getVTypeParameter(), crossingRoute, true);
466 812143 : if DEBUGCOND(ped) {
467 : std::cout
468 : << " nre=" << nextRouteEdge->getID()
469 : << " nreDir=" << nextRouteEdgeDir
470 0 : << " aPos=" << arrivalPos
471 0 : << " crossingRoute=" << toString(crossingRoute)
472 0 : << "\n";
473 : }
474 812143 : if (crossingRoute.size() > 1) {
475 812106 : const MSEdge* nextEdge = crossingRoute[1];
476 812106 : nextLane = getSidewalk<MSEdge, MSLane>(crossingRoute[1], ped.getPerson()->getVClass());
477 : assert((nextEdge->getFromJunction() == junction || nextEdge->getToJunction() == junction));
478 : assert(nextLane != prevLane);
479 812106 : nextDir = connectedDirection(currentLane, nextLane);
480 812106 : if DEBUGCOND(ped) {
481 0 : std::cout << " nextDir=" << nextDir << "\n";
482 : }
483 : assert(nextDir != UNDEFINED_DIRECTION);
484 812106 : if (nextDir == FORWARD) {
485 564946 : link = currentLane->getLinkTo(nextLane);
486 : } else {
487 247160 : link = nextLane->getLinkTo(currentLane);
488 247160 : if (nextEdge->isCrossing() && link->getTLLogic() == nullptr) {
489 194974 : const MSLane* oppositeWalkingArea = nextLane->getLogicalPredecessorLane();
490 194974 : link = oppositeWalkingArea->getLinkTo(nextLane);
491 : }
492 : }
493 : assert(link != nullptr);
494 812106 : if (nextLane->isPriorityCrossing()) {
495 487747 : registerCrossingApproach(ped, nextLane, prevLane);
496 : }
497 : } else {
498 37 : if DEBUGCOND(ped) {
499 0 : std::cout << SIMTIME
500 0 : << " no route from '" << (currentEdge == nullptr ? "NULL" : currentEdge->getID())
501 0 : << "' to '" << (nextRouteEdge == nullptr ? "NULL" : nextRouteEdge->getID())
502 0 : << "\n";
503 : }
504 : // check if a direct connection exists (moving onto the walkingarea was the wrong choice)
505 37 : if (ped.getDirection() == FORWARD) {
506 24 : link = prevLane->getLinkTo(nextRouteLane);
507 : } else {
508 13 : link = nextRouteLane->getLinkTo(prevLane);
509 : }
510 37 : if (link != nullptr) {
511 : // leave direction as UNDEFINED_DIRECTION to signal that currentLane must be changed
512 : nextLane = link->getViaLaneOrLane();
513 : } else {
514 80 : WRITE_WARNING("Person '" + ped.getPerson()->getID() + "' could not find route across junction '" + junction->getID()
515 : + "' from walkingArea '" + currentEdge->getID()
516 : + "' to edge '" + nextRouteEdge->getID() + "', time=" +
517 : time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
518 : // error indicated by nextDir == UNDEFINED_DIRECTION
519 : nextLane = nextRouteLane;
520 : }
521 : }
522 1137704 : } else if (currentEdge == nextRouteEdge) {
523 : // strange loop in this route. No need to use walkingArea
524 8 : nextDir = -ped.getDirection();
525 : } else {
526 : // normal edge. by default use next / previous walking area
527 325553 : nextDir = ped.getDirection();
528 325553 : nextLane = getNextWalkingArea(currentLane, ped.getDirection(), link);
529 325553 : if (nextLane != nullptr) {
530 : // walking area found
531 286787 : if DEBUGCOND(ped) {
532 0 : std::cout << " next walkingArea " << (nextDir == FORWARD ? "forward" : "backward") << "\n";
533 : }
534 : } else {
535 : // walk forward by default
536 38766 : if (junction == nextRouteEdge->getToJunction()) {
537 3948 : nextDir = BACKWARD;
538 34818 : } else if (junction == nextRouteEdge->getFromJunction()) {
539 : nextDir = FORWARD;
540 : } else {
541 : // topological disconnect, find a direction that makes sense
542 : // for the future part of the route
543 212 : ConstMSEdgeVector futureRoute = ped.getStage()->getRoute();
544 212 : futureRoute.erase(futureRoute.begin(), futureRoute.begin() + ped.getStage()->getRoutePosition() + 1);
545 212 : int passedFwd = 0;
546 212 : int passedBwd = 0;
547 212 : canTraverse(FORWARD, futureRoute, passedFwd);
548 212 : canTraverse(BACKWARD, futureRoute, passedBwd);
549 212 : nextDir = (passedFwd >= passedBwd) ? FORWARD : BACKWARD;
550 212 : if DEBUGCOND(ped) {
551 0 : std::cout << " nextEdge=" << nextRouteEdge->getID() << " passedFwd=" << passedFwd << " passedBwd=" << passedBwd << " futureRoute=" << toString(futureRoute) << " nextDir=" << nextDir << "\n";
552 : }
553 212 : }
554 : // try to use a direct link as fallback
555 : // direct links only exist if built explicitly. They are used to model tl-controlled links if there are no crossings
556 38766 : if (ped.getDirection() == FORWARD) {
557 34301 : link = currentLane->getLinkTo(nextRouteLane);
558 34301 : if (link != nullptr) {
559 32632 : if DEBUGCOND(ped) {
560 0 : std::cout << " direct forward\n";
561 : }
562 32632 : nextLane = currentLane->getInternalFollowingLane(nextRouteLane);
563 : }
564 : } else {
565 4465 : link = nextRouteLane->getLinkTo(currentLane);
566 4465 : if (link != nullptr) {
567 3185 : if DEBUGCOND(ped) {
568 0 : std::cout << " direct backward\n";
569 : }
570 3185 : nextLane = nextRouteLane->getInternalFollowingLane(currentLane);
571 3185 : if (nextLane != nullptr) {
572 : // advance to the end of consecutive internal lanes
573 5309 : while (nextLane->getLinkCont()[0]->getViaLaneOrLane()->isInternal()) {
574 5 : nextLane = nextLane->getLinkCont()[0]->getViaLaneOrLane();
575 : }
576 : }
577 : }
578 : }
579 : }
580 325020 : if (nextLane == nullptr) {
581 : // no internal lane found
582 : nextLane = nextRouteLane;
583 8798 : if DEBUGCOND(ped) {
584 0 : std::cout << SIMTIME << " no next lane found for " << currentLane->getID() << " dir=" << ped.getDirection() << "\n";
585 : }
586 8798 : if (usingInternalLanesStatic() && currentLane->getLinkCont().size() > 0) {
587 0 : WRITE_WARNING("Person '" + ped.getPerson()->getID() + "' could not find route across junction '" + junction->getID()
588 : + "' from edge '" + currentEdge->getID()
589 : + "' to edge '" + nextRouteEdge->getID() + "', time=" +
590 : time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
591 : }
592 316755 : } else if (nextLane->getLength() <= POSITION_EPS) {
593 : // internal lane too short
594 : // most often this is due to a zero-size junction. However, if
595 : // the person needs to pass a crossing we cannot skip ahead
596 10955 : if ((nextLane->getCanonicalSuccessorLane() == nullptr
597 10947 : || !nextLane->getCanonicalSuccessorLane()->isCrossing())
598 32845 : && (nextLane->getLogicalPredecessorLane() == nullptr ||
599 10943 : !nextLane->getLogicalPredecessorLane()->isCrossing())) {
600 : //WRITE_WARNING("Person '" + ped.getID()
601 : // + "' skips short lane '" + nextLane->getID()
602 : // + "' length=" + toString(nextLane->getLength())
603 : // + " time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
604 : nextLane = nextRouteLane;
605 10955 : nextDir = nextRouteEdge->getFromJunction() == junction ? FORWARD : BACKWARD;
606 : }
607 : }
608 : }
609 : }
610 1390204 : if DEBUGCOND(ped) {
611 0 : std::cout << SIMTIME
612 : << " p=" << ped.getPerson()->getID()
613 : << " l=" << currentLane->getID()
614 0 : << " nl=" << (nextLane == nullptr ? "NULL" : nextLane->getID())
615 0 : << " nrl=" << (nextRouteLane == nullptr ? "NULL" : nextRouteLane->getID())
616 : << " d=" << nextDir
617 0 : << " link=" << (link == nullptr ? "NULL" : link->getViaLaneOrLane()->getID())
618 0 : << " pedDir=" << ped.getDirection()
619 0 : << "\n";
620 : }
621 : assert(nextLane != 0 || nextRouteLane == 0);
622 1390204 : return NextLaneInfo(nextLane, link, nextDir);
623 : }
624 :
625 :
626 : const MSLane*
627 325553 : MSPModel_Striping::getNextWalkingArea(const MSLane* currentLane, const int dir, const MSLink*& link) {
628 325553 : if (dir == FORWARD) {
629 325413 : for (const MSLink* const l : currentLane->getLinkCont()) {
630 291112 : if (l->getLane()->isWalkingArea()) {
631 230433 : link = l;
632 : return l->getLane();
633 : }
634 : }
635 : } else {
636 : const std::vector<MSLane::IncomingLaneInfo>& laneInfos = currentLane->getIncomingLanes();
637 67444 : for (std::vector<MSLane::IncomingLaneInfo>::const_iterator it = laneInfos.begin(); it != laneInfos.end(); ++it) {
638 62979 : if ((*it).lane->isWalkingArea()) {
639 56354 : link = (*it).viaLink;
640 56354 : return (*it).lane;
641 : }
642 : }
643 : }
644 : return nullptr;
645 : }
646 :
647 :
648 : MSPModel_Striping::Obstacles
649 51419925 : MSPModel_Striping::getNeighboringObstacles(const Pedestrians& pedestrians, int egoIndex, int stripes) {
650 51419925 : const PState& ego = *static_cast<PState*>(pedestrians[egoIndex]);
651 51419925 : const int egoStripe = ego.stripe();
652 51419925 : Obstacles obs(stripes, Obstacle(ego.getDirection()));
653 51419925 : std::vector<bool> haveBlocker(stripes, false);
654 5928880348 : for (int index = egoIndex + 1; index < (int)pedestrians.size(); index++) {
655 5915173295 : const PState& p = *static_cast<PState*>(pedestrians[index]);
656 5915173295 : if DEBUGCOND(ego) {
657 0 : std::cout << SIMTIME << " ped=" << ego.getID() << " cur=" << egoStripe << " checking neighbor " << p.getID()
658 0 : << " nCur=" << p.stripe() << " nOth=" << p.otherStripe();
659 : }
660 5915173295 : if (!p.isWaitingToEnter() && !p.isJammed()) {
661 94423218 : const Obstacle o(p);
662 94423218 : if DEBUGCOND(ego) {
663 0 : std::cout << " dist=" << ego.distanceTo(o) << std::endl;
664 : }
665 94423218 : if (ego.distanceTo(o) == DIST_BEHIND) {
666 : break;
667 : }
668 56710346 : if (ego.distanceTo(o) == DIST_OVERLAP) {
669 56710346 : if (p.stripe() != egoStripe || p.getDirection() != ego.getDirection()) {
670 42833080 : obs[p.stripe()] = o;
671 42833080 : haveBlocker[p.stripe()] = true;
672 : } else {
673 : //std::cout << SIMTIME << " ignoring overlap between " << ego.getID() << " and " << p.getID() << " on stripe=" << egoStripe << "\n";
674 : }
675 56710346 : if (p.otherStripe() != egoStripe || p.getDirection() != ego.getDirection()) {
676 42833080 : obs[p.otherStripe()] = o;
677 42833080 : haveBlocker[p.otherStripe()] = true;
678 : } else {
679 : //std::cout << SIMTIME << " ignoring overlap between " << ego.getID() << " and " << p.getID() << " on stripe2=" << egoStripe << "\n";
680 : }
681 : } else {
682 0 : if (!haveBlocker[p.stripe()]) {
683 0 : obs[p.stripe()] = o;
684 : }
685 0 : if (!haveBlocker[p.otherStripe()]) {
686 0 : obs[p.otherStripe()] = o;
687 : }
688 : }
689 : }
690 : }
691 51419925 : if DEBUGCOND(ego) {
692 0 : std::cout << SIMTIME << " ped=" << ego.getPerson()->getID() << " neighObs=";
693 0 : DEBUG_PRINT(obs);
694 : }
695 51419925 : return obs;
696 0 : }
697 :
698 :
699 : int
700 2987621 : MSPModel_Striping::getStripeOffset(int origStripes, int destStripes, bool addRemainder) {
701 2987621 : int offset = (destStripes - origStripes) / 2;
702 2987621 : if (addRemainder) {
703 13284 : offset += (destStripes - origStripes) % 2;
704 : }
705 2987621 : return offset;
706 : }
707 :
708 :
709 : const MSPModel_Striping::Obstacles&
710 10311199 : MSPModel_Striping::getNextLaneObstacles(NextLanesObstacles& nextLanesObs, const
711 : MSLane* lane, const MSLane* nextLane, int stripes, int nextDir,
712 : double currentLength, int currentDir) {
713 : if (nextLanesObs.count(nextLane) == 0) {
714 2304198 : const double nextLength = nextLane->isWalkingArea() ? myMinNextLengths[nextLane] : nextLane->getLength();
715 : // figure out the which pedestrians are ahead on the next lane
716 2304198 : const int nextStripes = numStripes(nextLane);
717 : // do not move past the end of the next lane in a single step
718 4608396 : Obstacles obs(stripes, Obstacle(nextDir == FORWARD ? nextLength : 0, 0, OBSTACLE_NEXTEND, "nextEnd", 0));
719 :
720 2304198 : const int offset = getStripeOffset(nextStripes, stripes, currentDir != nextDir && nextStripes > stripes);
721 : //std::cout << SIMTIME << " getNextLaneObstacles"
722 : // << " nextLane=" << nextLane->getID()
723 : // << " nextLength=" << nextLength
724 : // << " nextDir=" << nextDir
725 : // << " currentLength=" << currentLength
726 : // << " currentDir=" << currentDir
727 : // << " stripes=" << stripes
728 : // << " nextStripes=" << nextStripes
729 : // << " offset=" << offset
730 : // << "\n";
731 2304198 : if (nextStripes < stripes) {
732 : // some stripes do not continue
733 2040460 : for (int ii = 0; ii < stripes; ++ii) {
734 1761105 : if (ii < offset || ii >= nextStripes + offset) {
735 2416380 : obs[ii] = Obstacle(nextDir == FORWARD ? 0 : nextLength, 0, OBSTACLE_END, "stripeEnd", 0);
736 : }
737 : }
738 : }
739 2304198 : Pedestrians& pedestrians = getPedestrians(nextLane);
740 2304198 : if (nextLane->isWalkingArea()) {
741 1187268 : transformToCurrentLanePositions(obs, currentDir, nextDir, currentLength, nextLength);
742 : // complex transformation into the coordinate system of the current lane
743 : // (pedestrians on next lane may walk at arbitrary angles relative to the current lane)
744 1187268 : double lateral_offset = (lane->getWidth() - stripeWidth) * 0.5;
745 1187268 : if ((stripes - nextStripes) % 2 != 0) {
746 457403 : lateral_offset += 0.5 * stripeWidth;
747 : }
748 : nextDir = currentDir;
749 : // transform pedestrians into the current coordinate system
750 6238272 : for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
751 5051004 : const PState& p = *static_cast<PState*>(pedestrians[ii]);
752 5051004 : if (p.isWaitingToEnter() || p.isJammed()) {
753 1027789 : continue;
754 : }
755 4023215 : Position pPos = p.getPosition(*p.getStage(), -1);
756 4023215 : Position relPos = lane->getShape().transformToVectorCoordinates(pPos, true);
757 : if (relPos == Position::INVALID) {
758 0 : WRITE_WARNINGF("Could not map position % onto lane '%'", pPos, lane->getID());
759 : }
760 4023215 : const double newY = relPos.y() + lateral_offset;
761 : //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";
762 4023215 : if ((currentDir == FORWARD && relPos.x() >= lane->getLength()) || (currentDir == BACKWARD && relPos.x() < 0)) {
763 4021773 : addCloserObstacle(obs, relPos.x(), p.stripe(newY), stripes, p.getPerson()->getID(), p.getPerson()->getVehicleType().getWidth(), currentDir, OBSTACLE_PED);
764 4021773 : addCloserObstacle(obs, relPos.x(), p.otherStripe(newY), stripes, p.getPerson()->getID(), p.getPerson()->getVehicleType().getWidth(), currentDir, OBSTACLE_PED);
765 : }
766 : }
767 : } else {
768 : // simple transformation into the coordinate system of the current lane
769 : // (only need to worry about currentDir and nextDir)
770 : // XXX consider waitingToEnter on nextLane
771 1116930 : sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(nextDir));
772 21214625 : for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
773 20097695 : const PState& p = *static_cast<PState*>(pedestrians[ii]);
774 20097695 : if (p.isWaitingToEnter() || p.isJammed()) {
775 10678231 : continue;
776 : }
777 : double newY = p.getPosLat();
778 9419464 : Obstacle pObs(p);
779 9419464 : if (nextDir != currentDir) {
780 290663 : newY = (nextStripes - 1) * stripeWidth - newY;
781 290663 : pObs.speed *= -1;
782 : }
783 9419464 : newY += offset * stripeWidth;
784 9419464 : const int stripe = p.stripe(newY);
785 9419464 : if (stripe >= 0 && stripe < stripes) {
786 9419461 : obs[stripe] = pObs;
787 : }
788 9419464 : const int otherStripe = p.otherStripe(newY);
789 9419464 : if (otherStripe >= 0 && otherStripe < stripes) {
790 9419461 : obs[otherStripe] = pObs;
791 : }
792 : }
793 1116930 : if (nextLane->isCrossing()) {
794 : // add vehicle obstacles
795 329380 : const MSLink* crossingEntryLink = nextLane->getIncomingLanes().front().viaLink;
796 329380 : const bool prio = crossingEntryLink->havePriority() || crossingEntryLink->getTLLogic() != nullptr;
797 329380 : addCrossingVehs(nextLane, stripes, offset, nextDir, obs, prio, currentDir != nextDir);
798 : }
799 1116930 : if (nextLane->getVehicleNumberWithPartials() > 0) {
800 18631 : Obstacles vehObs = getVehicleObstacles(nextLane, nextDir);
801 18631 : PState::mergeObstacles(obs, vehObs, nextDir, offset);
802 18631 : }
803 1116930 : transformToCurrentLanePositions(obs, currentDir, nextDir, currentLength, nextLength);
804 : }
805 2304198 : nextLanesObs[nextLane] = obs;
806 2304198 : }
807 10311199 : return nextLanesObs[nextLane];
808 : }
809 :
810 : void
811 2304198 : MSPModel_Striping::transformToCurrentLanePositions(Obstacles& obs, int currentDir, int nextDir, double currentLength, double nextLength) {
812 13127572 : for (Obstacle& o : obs) {
813 10823374 : if (currentDir == FORWARD) {
814 7135699 : if (nextDir == FORWARD) {
815 6790699 : o.xFwd += currentLength;
816 6790699 : o.xBack += currentLength;
817 : } else {
818 345000 : const double tmp = o.xFwd;
819 345000 : o.xFwd = currentLength + nextLength - o.xBack;
820 345000 : o.xBack = currentLength + nextLength - tmp;
821 : }
822 : } else {
823 3687675 : if (nextDir == FORWARD) {
824 851238 : const double tmp = o.xFwd;
825 851238 : o.xFwd = -o.xBack;
826 851238 : o.xBack = -tmp;
827 : } else {
828 2836437 : o.xFwd -= nextLength;
829 2836437 : o.xBack -= nextLength;
830 : }
831 : }
832 : }
833 2304198 : }
834 :
835 :
836 : void
837 8043546 : MSPModel_Striping::addCloserObstacle(Obstacles& obs, double x, int stripe, int numStripes, const std::string& id, double width, int dir, ObstacleType type) {
838 8043546 : if (stripe >= 0 && stripe < numStripes) {
839 2000644 : if ((dir == FORWARD && x - width / 2. < obs[stripe].xBack) || (dir == BACKWARD && x + width / 2. > obs[stripe].xFwd)) {
840 1711502 : obs[stripe] = Obstacle(x, 0, type, id, width);
841 : }
842 : }
843 8043546 : }
844 :
845 : void
846 6267806 : MSPModel_Striping::moveInDirection(SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir) {
847 50476565 : for (ActiveLanes::iterator it_lane = myActiveLanes.begin(); it_lane != myActiveLanes.end(); ++it_lane) {
848 44208763 : const MSLane* lane = it_lane->first;
849 44208763 : Pedestrians& pedestrians = it_lane->second;
850 44208763 : if (pedestrians.size() == 0) {
851 35030509 : continue;
852 : }
853 : //std::cout << SIMTIME << ">>> lane=" << lane->getID() << " numPeds=" << pedestrians.size() << "\n";
854 9178254 : if (lane->isWalkingArea()) {
855 2046829 : const double lateral_offset = (lane->getWidth() - stripeWidth) * 0.5;
856 2046829 : const double minY = stripeWidth * - 0.5 + NUMERICAL_EPS;
857 2046829 : const double maxY = stripeWidth * (numStripes(lane) - 0.5) - NUMERICAL_EPS;
858 : const WalkingAreaPath* debugPath = nullptr;
859 : // need to handle each walkingAreaPath separately and transform
860 : // coordinates beforehand
861 : std::set<const WalkingAreaPath*, walkingarea_path_sorter> paths;
862 13437908 : for (Pedestrians::iterator it = pedestrians.begin(); it != pedestrians.end(); ++it) {
863 11391079 : const PState* p = static_cast<PState*>(*it);
864 : assert(p->myWalkingAreaPath != 0);
865 11391079 : if (p->getDirection() == dir) {
866 5711495 : paths.insert(p->myWalkingAreaPath);
867 5711495 : if DEBUGCOND(*p) {
868 0 : debugPath = p->myWalkingAreaPath;
869 0 : std::cout << SIMTIME << " debugging WalkingAreaPath from=" << debugPath->from->getID() << " to=" << debugPath->to->getID() << " minY=" << minY << " maxY=" << maxY << " latOffset=" << lateral_offset << "\n";
870 : }
871 : }
872 : }
873 2046829 : const double usableWidth = (numStripes(lane) - 1) * stripeWidth;
874 3150735 : for (std::set<const WalkingAreaPath*, walkingarea_path_sorter>::iterator it = paths.begin(); it != paths.end(); ++it) {
875 1103906 : const WalkingAreaPath* path = *it;
876 : Pedestrians toDelete;
877 : Pedestrians transformedPeds;
878 1103906 : transformedPeds.reserve(pedestrians.size());
879 7776762 : for (Pedestrians::iterator it_p = pedestrians.begin(); it_p != pedestrians.end(); ++it_p) {
880 6672856 : PState* p = static_cast<PState*>(*it_p);
881 6672856 : if (p->myWalkingAreaPath == path) {
882 5711495 : transformedPeds.push_back(p);
883 5711495 : if (path == debugPath) std::cout << " ped=" << p->getPerson()->getID() << " relX=" << p->getEdgePos(0) << " relY=" << p->getPosLat() << " (untransformed), vecCoord="
884 0 : << path->shape.transformToVectorCoordinates(p->getPosition(*p->getStage(), -1)) << "\n";
885 961361 : } else if (p->myWalkingAreaPath->from == path->to && p->myWalkingAreaPath->to == path->from) {
886 43242 : if (p->myWalkingAreaPath->dir != path->dir) {
887 : // opposite direction is already in the correct coordinate system
888 39746 : transformedPeds.push_back(p);
889 39746 : if (path == debugPath) std::cout << " ped=" << p->getPerson()->getID() << " relX=" << p->getEdgePos(0) << " relY=" << p->getPosLat() << " (untransformed), vecCoord="
890 0 : << path->shape.transformToVectorCoordinates(p->getPosition(*p->getStage(), -1)) << "\n";
891 : } else {
892 : // x position must be reversed
893 3496 : PState* tp = new PState(*p);
894 3496 : tp->reverse(path->length, usableWidth);
895 3496 : toDelete.push_back(tp);
896 3496 : transformedPeds.push_back(tp);
897 3496 : if (path == debugPath) std::cout << " ped=" << p->getPerson()->getID() << " relX=" << p->getEdgePos(0) << " relY=" << p->getPosLat() << " (semi-transformed), vecCoord="
898 0 : << path->shape.transformToVectorCoordinates(p->getPosition(*p->getStage(), -1)) << "\n";
899 : }
900 : } else {
901 918119 : const Position relPos = path->shape.transformToVectorCoordinates(p->getPosition(*p->getStage(), -1));
902 918119 : const double newY = relPos.y() + lateral_offset;
903 917003 : if (relPos != Position::INVALID && newY >= minY && newY <= maxY) {
904 256782 : PState* tp = new PState(*p);
905 256782 : tp->reset(relPos.x(), newY);
906 256782 : toDelete.push_back(tp);
907 256782 : transformedPeds.push_back(tp);
908 256782 : if (path == debugPath) {
909 0 : std::cout << " ped=" << p->getPerson()->getID() << " relX=" << relPos.x() << " relY=" << newY << " (transformed), vecCoord=" << relPos << "\n";
910 : }
911 : } else {
912 661337 : if (path == debugPath) {
913 0 : std::cout << " ped=" << p->getPerson()->getID() << " relX=" << relPos.x() << " relY=" << newY << " (invalid), vecCoord=" << relPos << "\n";
914 : }
915 : }
916 : }
917 : }
918 : auto itFoe = myWalkingAreaFoes.find(&lane->getEdge());
919 1103906 : if (itFoe != myWalkingAreaFoes.end()) {
920 : // add vehicle foes on paths which cross this walkingarea
921 : // translate the vehicle into a number of dummy-pedestrians
922 : // that occupy the same space
923 2264282 : for (const MSLane* foeLane : itFoe->second) {
924 184879 : for (auto itVeh = foeLane->anyVehiclesBegin(); itVeh != foeLane->anyVehiclesEnd(); ++itVeh) {
925 184879 : const MSVehicle* veh = *itVeh;
926 184879 : const double vehWidth = veh->getVehicleType().getWidth();
927 184879 : Boundary relCorners;
928 184879 : Position relFront = path->shape.transformToVectorCoordinates(veh->getPosition(), true);
929 184879 : Position relBack = path->shape.transformToVectorCoordinates(veh->getBackPosition(), true);
930 : if (relFront == Position::INVALID) {
931 0 : WRITE_WARNINGF("Could not vehicle '%' front position % onto walkingarea '%' path=%, time=%.",
932 : veh->getID(), veh->getPosition(), lane->getID(), path->shape, time2string(SIMSTEP));
933 : }
934 : if (relBack == Position::INVALID) {
935 0 : WRITE_WARNINGF("Could not vehicle '%' back position % onto walkingarea '%' path=%, time=%.",
936 : veh->getID(), veh->getBackPosition(), lane->getID(), path->shape, time2string(SIMSTEP));
937 : }
938 184879 : PositionVector relCenter;
939 184879 : relCenter.push_back(relFront);
940 184879 : relCenter.push_back(relBack);
941 184879 : relCenter.move2side(vehWidth / 2);
942 184879 : relCorners.add(relCenter[0]);
943 184879 : relCorners.add(relCenter[1]);
944 184879 : relCenter.move2side(-vehWidth);
945 184879 : relCorners.add(relCenter[0]);
946 184879 : relCorners.add(relCenter[1]);
947 : // persons should require less gap than the vehicles to prevent getting stuck
948 : // when a vehicles moves towards them
949 184879 : relCorners.growWidth(SAFETY_GAP / 2);
950 184879 : const double xWidth = relCorners.getWidth();
951 184879 : const double vehYmin = MAX2(minY - lateral_offset, relCorners.ymin());
952 184879 : const double vehYmax = MIN2(maxY - lateral_offset, relCorners.ymax());
953 184879 : const double xCenter = relCorners.getCenter().x();
954 : Position yMinPos(xCenter, vehYmin);
955 : Position yMaxPos(xCenter, vehYmax);
956 184879 : const bool addFront = addVehicleFoe(veh, lane, yMinPos, dir * xWidth, 0, lateral_offset, minY, maxY, toDelete, transformedPeds);
957 184879 : const bool addBack = addVehicleFoe(veh, lane, yMaxPos, dir * xWidth, 0, lateral_offset, minY, maxY, toDelete, transformedPeds);
958 184879 : if (path == debugPath) {
959 : std::cout << " veh=" << veh->getID()
960 0 : << " corners=" << relCorners
961 : << " xWidth=" << xWidth
962 0 : << " ymin=" << relCorners.ymin()
963 0 : << " ymax=" << relCorners.ymax()
964 : << " vehYmin=" << vehYmin
965 : << " vehYmax=" << vehYmax
966 0 : << "\n";
967 : }
968 184879 : if (addFront && addBack) {
969 : // add in-between positions
970 184879 : const double yDist = vehYmax - vehYmin;
971 421752 : for (double dist = stripeWidth; dist < yDist; dist += stripeWidth) {
972 236873 : const double relDist = dist / yDist;
973 236873 : Position between = (yMinPos * relDist) + (yMaxPos * (1 - relDist));
974 236873 : if (path == debugPath) {
975 0 : std::cout << " vehBetween=" << veh->getID() << " pos=" << between << "\n";
976 : }
977 236873 : addVehicleFoe(veh, lane, between, dir * xWidth, stripeWidth, lateral_offset, minY, maxY, toDelete, transformedPeds);
978 : }
979 : }
980 184879 : }
981 : }
982 : }
983 1103906 : moveInDirectionOnLane(transformedPeds, lane, currentTime, changedLane, dir, path == debugPath);
984 1103906 : arriveAndAdvance(pedestrians, currentTime, changedLane, dir);
985 : // clean up
986 1869025 : for (Pedestrians::iterator it_p = toDelete.begin(); it_p != toDelete.end(); ++it_p) {
987 765119 : delete *it_p;
988 : }
989 1103906 : }
990 : } else {
991 7131425 : moveInDirectionOnLane(pedestrians, lane, currentTime, changedLane, dir, false);
992 7131425 : arriveAndAdvance(pedestrians, currentTime, changedLane, dir);
993 : }
994 : }
995 6267802 : }
996 :
997 :
998 : bool
999 606631 : MSPModel_Striping::addVehicleFoe(const MSVehicle* veh, const MSLane* walkingarea, const Position& relPos, double xWidth, double yWidth, double lateral_offset,
1000 : double minY, double maxY, Pedestrians& toDelete, Pedestrians& transformedPeds) {
1001 : if (relPos != Position::INVALID) {
1002 606631 : const double newY = relPos.y() + lateral_offset;
1003 606631 : if (newY >= minY && newY <= maxY) {
1004 504841 : PState* tp = new PStateVehicle(veh, walkingarea, relPos.x(), newY, xWidth, yWidth);
1005 : //std::cout << SIMTIME << " addVehicleFoe=" << veh->getID() << " rx=" << relPos.x() << " ry=" << newY << " s=" << tp->stripe() << " o=" << tp->otherStripe() << "\n";
1006 504841 : toDelete.push_back(tp);
1007 504841 : transformedPeds.push_back(tp);
1008 : }
1009 606631 : return true;
1010 : } else {
1011 : return false;
1012 : }
1013 : }
1014 :
1015 : void
1016 8235331 : MSPModel_Striping::arriveAndAdvance(Pedestrians& pedestrians, SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir) {
1017 : // advance to the next lane / arrive at destination
1018 8235331 : sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(dir));
1019 : // can't use iterators because we do concurrent modification
1020 106549495 : for (int i = 0; i < (int)pedestrians.size(); i++) {
1021 98314168 : PState* const p = static_cast<PState*>(pedestrians[i]);
1022 98314168 : if (p->isRemoteControlled()) {
1023 26760 : continue;
1024 : }
1025 98287408 : if (p->getDirection() == dir && p->distToLaneEnd() < 0) {
1026 : // moveToNextLane may trigger re-insertion (for consecutive
1027 : // walks) so erase must be called first
1028 : pedestrians.erase(pedestrians.begin() + i);
1029 878133 : i--;
1030 878133 : p->moveToNextLane(currentTime);
1031 878129 : if (p->getLane() != nullptr) {
1032 683423 : changedLane.insert(p->getPerson());
1033 683423 : myActiveLanes[p->getLane()].push_back(p);
1034 : } else {
1035 : // end walking stage and destroy PState
1036 194706 : p->getStage()->moveToNextEdge(p->getPerson(), currentTime, dir);
1037 194706 : myNumActivePedestrians--;
1038 : }
1039 : }
1040 : }
1041 8235327 : }
1042 :
1043 :
1044 : void
1045 8235331 : MSPModel_Striping::moveInDirectionOnLane(Pedestrians& pedestrians, const MSLane* lane, SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir, bool debug) {
1046 8235331 : const int stripes = numStripes(lane);
1047 : //std::cout << " laneWidth=" << lane->getWidth() << " stripeWidth=" << stripeWidth << " stripes=" << stripes << "\n";
1048 16470662 : Obstacles obs(stripes, Obstacle(dir)); // continuously updated
1049 : NextLanesObstacles nextLanesObs; // continuously updated
1050 8235331 : sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(dir));
1051 :
1052 8235331 : Obstacles crossingVehs(stripes, Obstacle(dir));
1053 : bool hasCrossingVehObs = false;
1054 8235331 : if (lane->isCrossing()) {
1055 : // assume that vehicles will brake when already on the crossing
1056 611487 : hasCrossingVehObs = addCrossingVehs(lane, stripes, 0, dir, crossingVehs, true, false);
1057 : }
1058 :
1059 106391708 : for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
1060 98156377 : PState& p = *static_cast<PState*>(pedestrians[ii]);
1061 : UNUSED_PARAMETER(debug);
1062 : //if (debug) {
1063 : // std::cout << SIMTIME << " CHECKING d=" << dir << " p=" << p.getID() << " relX=" << p.myRelX << " xmin=" << p.getMinX() << " xmax=" << p.getMaxX() << " pdir=" << p.getDirection() << "\n";
1064 : //}
1065 98156377 : Obstacles currentObs = obs;
1066 98156377 : if (p.getDirection() != dir || changedLane.count(p.getPerson()) != 0 || p.getRemotePosition() != Position::INVALID) {
1067 46736452 : if (!p.isWaitingToEnter() && !p.isJammed()) {
1068 : //if DEBUGCOND(p) {
1069 : // std::cout << " obs=" << p.getPerson()->getID() << " y=" << p.getPosLat() << " stripe=" << p.stripe() << " oStripe=" << p.otherStripe() << "\n";
1070 : //}
1071 27448357 : Obstacle o(p);
1072 27448357 : if (p.getDirection() != dir && p.getSpeed() == 0.) {
1073 : // ensure recognition of oncoming
1074 6029911 : o.speed = (p.getDirection() == FORWARD ? 0.1 : -0.1);
1075 : }
1076 27448357 : if (o.closer(obs[p.stripe()], dir)) {
1077 27444052 : obs[p.stripe()] = o;
1078 : }
1079 27448357 : if (o.closer(obs[p.otherStripe()], dir)) {
1080 27443930 : obs[p.otherStripe()] = o;
1081 : }
1082 : }
1083 : continue;
1084 46736452 : }
1085 51419925 : if DEBUGCOND(p) {
1086 0 : std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << " currentObs=";
1087 0 : gDebugFlag1 = true;
1088 0 : DEBUG_PRINT(currentObs);
1089 : }
1090 51419925 : const MSLane* nextLane = p.myNLI.lane;
1091 51419925 : const MSLink* link = p.myNLI.link;
1092 51419925 : const double dist = p.distToLaneEnd();
1093 : const double speed(p.getStage()->getConfiguredSpeed() >= 0
1094 51419925 : ? p.getStage()->getConfiguredSpeed()
1095 40578854 : : ((nextLane != nullptr && (USE_NET_SPEEDS || nextLane->isNormal() || nextLane->isInternal()))
1096 90588847 : ? nextLane->getVehicleMaxSpeed(p.getPerson())
1097 12140923 : : p.getStage()->getMaxSpeed(p.getPerson())));
1098 :
1099 :
1100 51419925 : if (nextLane != nullptr && dist <= LOOKAHEAD_ONCOMING_DIST) {
1101 10311199 : const double currentLength = (p.myWalkingAreaPath == nullptr ? lane->getLength() : p.myWalkingAreaPath->length);
1102 10311199 : const Obstacles& nextObs = getNextLaneObstacles(
1103 : nextLanesObs, lane, nextLane, stripes,
1104 : p.myNLI.dir, currentLength, dir);
1105 :
1106 10311199 : if DEBUGCOND(p) {
1107 0 : std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << " nextObs=";
1108 0 : DEBUG_PRINT(nextObs);
1109 : }
1110 10311199 : p.mergeObstacles(currentObs, nextObs);
1111 : }
1112 51419925 : if DEBUGCOND(p) {
1113 0 : std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << " obsWithNext=";
1114 0 : DEBUG_PRINT(currentObs);
1115 : }
1116 51419925 : p.mergeObstacles(currentObs, getNeighboringObstacles(pedestrians, ii, stripes));
1117 51419925 : if DEBUGCOND(p) {
1118 0 : std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << " obsWithNeigh=";
1119 0 : DEBUG_PRINT(currentObs);
1120 : }
1121 : // time gap to pass the intersection ahead of a vehicle.
1122 51419925 : const double passingLength = p.getLength() + p.getPerson()->getTimegapCrossing() * speed;
1123 : // check link state
1124 51419925 : if DEBUGCOND(p) {
1125 0 : gDebugFlag1 = true; // get debug output from MSLink
1126 0 : std::cout << " link=" << (link == nullptr ? "NULL" : link->getViaLaneOrLane()->getID())
1127 0 : << " dist=" << dist << " d2=" << dist - p.getMinGap() << " la=" << LOOKAHEAD_SAMEDIR* speed << "\n";
1128 : }
1129 : if (link != nullptr
1130 : // only check close before junction, @todo we should take deceleration into account here
1131 39663994 : && dist - p.getMinGap() < LOOKAHEAD_SAMEDIR * speed
1132 : // persons move before vehicles so we subtract DELTA_TO because they cannot rely on vehicles having passed the intersection in the current time step
1133 61112177 : && (!link->opened(currentTime - DELTA_T, speed, speed, passingLength, p.getImpatience(), speed, 0, 0, nullptr, p.ignoreRed(link), p.getPerson())
1134 9033055 : || p.stopForYellow(link))) {
1135 : // prevent movement passed a closed link
1136 660115 : Obstacles closedLink(stripes, Obstacle(p.getEdgePos(0) + dir * (dist - NUMERICAL_EPS), 0, OBSTACLE_LINKCLOSED, "closedLink_" + link->getViaLaneOrLane()->getID(), 0));
1137 660115 : p.mergeObstacles(currentObs, closedLink);
1138 660115 : if DEBUGCOND(p) {
1139 0 : std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << " obsWithTLS=";
1140 0 : DEBUG_PRINT(currentObs);
1141 : }
1142 : // consider rerouting over another crossing
1143 660115 : if (p.myWalkingAreaPath != nullptr) {
1144 : // @todo actually another path would be needed starting at the current position
1145 502429 : const MSLane* oldNext = p.myNLI.lane;
1146 502429 : p.myNLI = getNextLane(p, p.getLane(), p.myWalkingAreaPath->from);
1147 502429 : if (p.myNLI.lane != oldNext && oldNext->isPriorityCrossing()) {
1148 151 : unregisterCrossingApproach(p, oldNext);
1149 : }
1150 : }
1151 660115 : }
1152 51419925 : if DEBUGCOND(p) {
1153 0 : gDebugFlag1 = false;
1154 : }
1155 51419925 : if (&lane->getEdge() == p.getStage()->getDestination() && p.getStage()->getDestinationStop() != nullptr) {
1156 : Obstacles arrival;
1157 1387401 : if (p.getStage()->getDestinationStop()->getWaitingCapacity() > p.getStage()->getDestinationStop()->getNumWaitingPersons() ||
1158 : (!p.getStage()->getDestinationStop()->checkPersonCapacity())) {
1159 2570476 : arrival = Obstacles(stripes, Obstacle(p.getStage()->getArrivalPos() + dir * p.getMinGap(), 0, OBSTACLE_ARRIVALPOS, "arrival", 0));
1160 : } else {
1161 204326 : arrival = Obstacles(stripes, Obstacle(p.getStage()->getArrivalPos() - dir * p.getMinGap(), 0, OBSTACLE_ARRIVALPOS, "arrival_blocked", 0));
1162 : }
1163 1387401 : p.mergeObstacles(currentObs, arrival);
1164 1387401 : }
1165 :
1166 51419925 : if (lane->getVehicleNumberWithPartials() > 0) {
1167 : // react to vehicles on the same lane
1168 : // @todo: improve efficiency by using the same iterator for all pedestrians on this lane
1169 276026 : Obstacles vehObs = getVehicleObstacles(lane, dir, &p);
1170 276026 : p.mergeObstacles(currentObs, vehObs);
1171 276026 : if DEBUGCOND(p) {
1172 0 : std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << " obsWithVehs=";
1173 0 : DEBUG_PRINT(currentObs);
1174 : }
1175 276026 : }
1176 51419925 : if (hasCrossingVehObs) {
1177 24810 : p.mergeObstacles(currentObs, crossingVehs);
1178 24810 : if DEBUGCOND(p) {
1179 0 : std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << " obsWithVehs2=";
1180 0 : DEBUG_PRINT(currentObs);
1181 : }
1182 : }
1183 :
1184 : // walk, taking into account all obstacles
1185 51419925 : p.walk(currentObs);
1186 51419925 : gDebugFlag1 = false;
1187 51419925 : if (!p.isWaitingToEnter() && !p.isJammed()) {
1188 31146418 : Obstacle o(p);
1189 31146418 : if (o.closer(obs[p.stripe()], dir)) {
1190 31144863 : obs[p.stripe()] = o;
1191 : }
1192 31146418 : if (o.closer(obs[p.otherStripe()], dir)) {
1193 31144858 : obs[p.otherStripe()] = o;
1194 : }
1195 31146418 : if (MSGlobals::gCheck4Accidents && p.myWalkingAreaPath == nullptr && !p.isJammed()) {
1196 1975241647 : for (int coll = 0; coll < ii; ++coll) {
1197 1948737599 : PState& c = *static_cast<PState*>(pedestrians[coll]);
1198 1948737599 : if (!c.isWaitingToEnter() && c.myWalkingAreaPath == nullptr && !c.isJammed()) {
1199 956331958 : if (c.stripe() == p.stripe() || p.stripe() == c.otherStripe() || p.otherStripe() == c.stripe() || p.otherStripe() == c.otherStripe()) {
1200 378299367 : Obstacle cObs(c);
1201 : // we check only for real collisions, no min gap violations
1202 378299367 : if (p.distanceTo(cObs, false) == DIST_OVERLAP) {
1203 1164584 : WRITE_WARNING("Collision of person '" + p.getPerson()->getID() + "' and person '" + c.getPerson()->getID()
1204 : + "', lane='" + lane->getID() + "', time=" + time2string(currentTime) + ".");
1205 : }
1206 : }
1207 : }
1208 : }
1209 : }
1210 : }
1211 : //std::cout << SIMTIME << p.getPerson()->getID() << " lane=" << lane->getID() << " x=" << p.myRelX << "\n";
1212 98156377 : }
1213 16470662 : }
1214 :
1215 :
1216 : void
1217 487747 : MSPModel_Striping::registerCrossingApproach(const PState& ped, const MSLane* crossing, const MSLane* beforeWA) {
1218 : // person has entered the walkingarea
1219 487747 : SUMOTime arrivalTime = SIMSTEP;
1220 : assert(ped.getLane()->isWalkingArea());
1221 487747 : const WalkingAreaPath* wa = getWalkingAreaPath(&ped.getLane()->getEdge(), beforeWA, crossing);
1222 487747 : const double speed = ped.getStage()->getMaxSpeed(ped.getPerson()) * (1 - dawdling / 2);
1223 487747 : arrivalTime += TIME2STEPS(wa->length / speed);
1224 487747 : SUMOTime leavingTime = arrivalTime + TIME2STEPS(crossing->getLength() / speed);
1225 487747 : crossing->getIncomingLanes()[0].viaLink->setApproachingPerson(ped.getPerson(), arrivalTime, leavingTime);
1226 487747 : if DEBUGCOND(ped) {
1227 0 : std::cout << SIMTIME << " register " << ped.getPerson()->getID() << " at crossing " << crossing->getID() << "\n";
1228 : }
1229 487747 : }
1230 :
1231 :
1232 : bool
1233 940867 : MSPModel_Striping::addCrossingVehs(const MSLane* crossing, int stripes, double lateral_offset, int dir, Obstacles& obs, bool prio, bool flipY) {
1234 : bool hasCrossingVehObs = false;
1235 940867 : const MSLink* crossingExitLink = crossing->getLinkCont().front();
1236 940867 : gDebugFlag1 = DEBUGCOND2(crossing);
1237 940867 : const MSLink::LinkLeaders linkLeaders = crossingExitLink->getLeaderInfo(nullptr, crossing->getLength());
1238 940867 : gDebugFlag1 = false;
1239 940867 : if (linkLeaders.size() > 0) {
1240 215223 : for (MSLink::LinkLeaders::const_iterator it = linkLeaders.begin(); it != linkLeaders.end(); ++it) {
1241 : // the vehicle to enter the junction first has priority
1242 109690 : const MSVehicle* veh = (*it).vehAndGap.first;
1243 109690 : if (veh != nullptr) {
1244 109690 : Obstacle vo((*it).distToCrossing, 0, OBSTACLE_VEHICLE, veh->getID(), veh->getVehicleType().getWidth() + 2 * minGapToVehicle, veh);
1245 : // block entry to the crossing in walking direction but allow leaving it
1246 109690 : Obstacle voBlock = vo;
1247 109690 : if (dir == FORWARD) {
1248 61517 : voBlock.xBack = NUMERICAL_EPS;
1249 : } else {
1250 48173 : voBlock.xFwd = crossing->getLength() - NUMERICAL_EPS;
1251 : }
1252 : // when approaching a priority crossings, vehicles must be able
1253 : // to brake, otherwise the person must be able to cross in time
1254 109690 : const double distToCrossBeforeVeh = (dir == FORWARD ? vo.xFwd : crossing->getLength() - vo.xBack);
1255 : const double bGap = (prio
1256 111290 : ? veh->getCarFollowModel().brakeGap(veh->getSpeed(), veh->getCarFollowModel().getMaxDecel(), 0)
1257 1600 : : veh->getSpeed() * distToCrossBeforeVeh); // walking 1m/s
1258 : double vehYmin;
1259 : double vehYmax;
1260 : // relY increases from left to right (the other way around from vehicles)
1261 109690 : if ((*it).fromLeft()) {
1262 71258 : vehYmin = -(*it).vehAndGap.second + lateral_offset; // vehicle back
1263 71258 : vehYmax = vehYmin + veh->getVehicleType().getLength() + bGap + minGapToVehicle;
1264 71258 : vehYmin -= minGapToVehicle;
1265 : } else {
1266 38432 : vehYmax = crossing->getWidth() + (*it).vehAndGap.second - lateral_offset; // vehicle back
1267 38432 : vehYmin = vehYmax - veh->getVehicleType().getLength() - bGap - minGapToVehicle;
1268 38432 : vehYmax += minGapToVehicle;
1269 :
1270 : }
1271 438893 : for (int s = MAX2(0, PState::stripe(vehYmin)); s < MIN2(PState::stripe(vehYmax), stripes); ++s) {
1272 185577 : if ((dir == FORWARD && obs[s].xBack > vo.xBack)
1273 329466 : || (dir == BACKWARD && obs[s].xFwd < vo.xFwd)) {
1274 327860 : if (!prio && veh->getSpeed() > SUMO_const_haltingSpeed) {
1275 : // do not enter the crossing
1276 9103 : obs[s] = voBlock;
1277 : } else {
1278 318757 : obs[s] = vo;
1279 : }
1280 : hasCrossingVehObs = true;
1281 : }
1282 : }
1283 109690 : if (flipY) {
1284 24277 : Obstacles tmp = obs;
1285 174395 : for (int i = 0; i < (int)obs.size(); i++) {
1286 150118 : obs[i] = tmp[obs.size() - 1 - i];
1287 : }
1288 24277 : }
1289 109690 : if (DEBUGCOND2(crossing)) {
1290 0 : std::cout << SIMTIME
1291 : << " crossingVeh=" << veh->getID()
1292 : << " lane=" << crossing->getID()
1293 : << " prio=" << prio
1294 : << " latOffset=" << lateral_offset
1295 : << " dir=" << dir
1296 0 : << " flipY=" << flipY
1297 : << " stripes=" << stripes
1298 0 : << " dist=" << (*it).distToCrossing
1299 0 : << " gap=" << (*it).vehAndGap.second
1300 : << " brakeGap=" << bGap
1301 : << " fromLeft=" << (*it).fromLeft()
1302 : << " distToCrossBefore=" << distToCrossBeforeVeh
1303 : << " ymin=" << vehYmin
1304 : << " ymax=" << vehYmax
1305 : << " smin=" << PState::stripe(vehYmin)
1306 0 : << " smax=" << PState::stripe(vehYmax)
1307 0 : << "\n";
1308 0 : DEBUG_PRINT(obs);
1309 : }
1310 : }
1311 : }
1312 105533 : if (hasCrossingVehObs) {
1313 : // check whether the crossing is fully blocked
1314 70991 : const int reserved = getReserved((int)obs.size(), RESERVE_FOR_ONCOMING_FACTOR_JUNCTIONS);
1315 : bool allBlocked = true;
1316 :
1317 391751 : for (int i = 0; i < (int)obs.size(); i++) {
1318 347883 : const Obstacle& o = obs[i];
1319 347883 : if (o.type != OBSTACLE_VEHICLE && (
1320 54696 : (dir == FORWARD && i >= reserved) ||
1321 39177 : (dir == BACKWARD && i < (int)obs.size() - reserved))) {
1322 : allBlocked = false;
1323 : break;
1324 : }
1325 : }
1326 70991 : if (allBlocked) {
1327 43868 : if (DEBUGCOND2(crossing)) {
1328 0 : std::cout << SIMTIME << " crossing=" << crossing->getID() << " allBlocked\n";
1329 : }
1330 311753 : for (Obstacle& o : obs) {
1331 267885 : if (dir == FORWARD) {
1332 149040 : o.xBack = NUMERICAL_EPS;
1333 : } else {
1334 118845 : o.xFwd = crossing->getLength() - NUMERICAL_EPS;
1335 : }
1336 : }
1337 : }
1338 : }
1339 : }
1340 940867 : return hasCrossingVehObs;
1341 940867 : }
1342 :
1343 :
1344 : MSPModel_Striping::Obstacles
1345 294657 : MSPModel_Striping::getVehicleObstacles(const MSLane* lane, int dir, PState* ped) {
1346 294657 : const int stripes = numStripes(lane);
1347 589314 : Obstacles vehObs(stripes, Obstacle(dir));
1348 : int current = -1;
1349 : double minX = 0.;
1350 : double maxX = 0.;
1351 : double pRelY = -1.;
1352 : double pWidth = 0.;
1353 : std::string pID;
1354 294657 : bool debug = DEBUGCOND2(lane);
1355 294657 : if (ped != nullptr) {
1356 276026 : current = ped->stripe();
1357 276026 : minX = ped->getMinX();
1358 276026 : maxX = ped->getMaxX();
1359 : pRelY = ped->getPosLat();
1360 276026 : pWidth = ped->getPerson()->getVehicleType().getWidth();
1361 : pID = ped->getPerson()->getID();
1362 276026 : debug = DEBUGCOND(*ped);
1363 18631 : } else if (dir == BACKWARD) {
1364 : // checking vehicles on the next lane. Use entry point as reference
1365 : minX = lane->getLength();
1366 : maxX = lane->getLength();
1367 : }
1368 294657 : MSLane::AnyVehicleIterator begin = (dir == FORWARD ? lane->anyVehiclesUpstreamBegin() : lane->anyVehiclesBegin());
1369 : MSLane::AnyVehicleIterator end = (dir == FORWARD ? lane->anyVehiclesUpstreamEnd() : lane->anyVehiclesEnd());
1370 780867 : for (MSLane::AnyVehicleIterator it = begin; it != end; ++it) {
1371 486210 : const MSVehicle* veh = *it;
1372 486210 : const bool bidi = veh->getLane() == lane->getBidiLane();
1373 486210 : const double vehBack = veh->getBackPositionOnLane(lane);
1374 486210 : double vehFront = vehBack + veh->getVehicleType().getLength();
1375 : // ensure that vehicles are not blocked
1376 486210 : const double vehNextSpeed = veh->getWaitingTime() > DELTA_T ? 0 : MAX2(veh->getSpeed(), 1.0);
1377 486210 : const double clearance = SAFETY_GAP + vehNextSpeed * LOOKAHEAD_SAMEDIR;
1378 : // boundaries for range checking
1379 : double vehXMax;
1380 : double vehXMin;
1381 : double vehXMaxCheck;
1382 : double vehXMinCheck;
1383 486210 : if (bidi) {
1384 1617 : vehFront = vehBack - veh->getVehicleType().getLength();
1385 1617 : vehXMax = vehBack + SAFETY_GAP;
1386 1617 : vehXMin = vehFront - clearance;
1387 1617 : if (dir == FORWARD) {
1388 1617 : vehXMaxCheck = vehBack + NUMERICAL_EPS;
1389 1617 : vehXMinCheck = vehFront - LOOKAROUND_VEHICLES;
1390 : } else {
1391 0 : vehXMaxCheck = vehBack + LOOKAHEAD_SAMEDIR;
1392 : vehXMinCheck = vehFront - clearance;
1393 : }
1394 : } else {
1395 484593 : vehXMax = vehFront + clearance;
1396 484593 : vehXMin = vehBack - SAFETY_GAP;
1397 484593 : if (dir == FORWARD) {
1398 : vehXMaxCheck = vehFront + clearance;
1399 362058 : vehXMinCheck = vehBack - LOOKAHEAD_SAMEDIR;
1400 : } else {
1401 122535 : vehXMaxCheck = vehFront + LOOKAROUND_VEHICLES;
1402 122535 : vehXMinCheck = vehBack - NUMERICAL_EPS;
1403 : }
1404 : }
1405 486210 : if (debug) {
1406 0 : std::cout << SIMTIME << " ped=" << pID << " veh=" << veh->getID() << " check obstacle on lane=" << lane->getID()
1407 : << "\n"
1408 : << " vehXMin=" << vehXMin
1409 : << " vehXMax=" << vehXMax
1410 : << " vehXMinC=" << vehXMinCheck
1411 : << " vehXMaxC=" << vehXMaxCheck
1412 : << " minX=" << minX
1413 : << " maxX=" << maxX
1414 : << " bidi=" << bidi
1415 : << " vFront=" << vehFront
1416 : << " vBack=" << vehBack
1417 0 : << "\n";
1418 : }
1419 486210 : if (vehXMaxCheck > minX && vehXMinCheck && vehXMinCheck <= maxX) {
1420 369974 : Obstacle vo(vehBack, veh->getSpeed() * (bidi ? -1 : 1), OBSTACLE_VEHICLE, veh->getID(), 0, veh);
1421 : // moving vehicles block space along their path
1422 185341 : vo.xFwd = vehXMax;
1423 185341 : vo.xBack = vehXMin;
1424 : // relY increases from left to right (the other way around from vehicles)
1425 : // XXX lateral offset for partial vehicles
1426 185341 : const double posLat = veh->getLateralPositionOnLane() * (bidi ? -1 : 1);
1427 185341 : const double vehYmax = 0.5 * (lane->getWidth() + veh->getVehicleType().getWidth() - stripeWidth) - posLat;
1428 185341 : const double vehYmin = vehYmax - veh->getVehicleType().getWidth();
1429 804111 : for (int s = MAX2(0, PState::stripe(vehYmin)); s < MIN2(PState::stripe(vehYmax) + 1, stripes); ++s) {
1430 618770 : Obstacle prior = vehObs[s];
1431 : vehObs[s] = vo;
1432 618770 : if (s == current && vehFront + SAFETY_GAP < minX) {
1433 : // ignore if already overlapping while vehicle is still behind
1434 16165 : if (pRelY - pWidth < vehYmax &&
1435 15863 : pRelY + pWidth > vehYmin && dir == FORWARD) {
1436 9997 : if (debug) {
1437 0 : std::cout << " ignoring vehicle '" << veh->getID() << " on stripe " << s << " vehFrontSG=" << vehFront + SAFETY_GAP << " minX=" << minX << "\n";
1438 : }
1439 : if (dir == FORWARD) {
1440 : vehObs[s] = prior;
1441 : } else {
1442 : vehObs[s].xFwd = MIN2(vo.xFwd, vehFront + SAFETY_GAP);
1443 : }
1444 : }
1445 : }
1446 : }
1447 185341 : if (debug) {
1448 0 : std::cout << SIMTIME << " ped=" << pID << " veh=" << veh->getID() << " obstacle on lane=" << lane->getID()
1449 : << "\n"
1450 : << " ymin=" << vehYmin
1451 : << " ymax=" << vehYmax
1452 : << " smin=" << PState::stripe(vehYmin)
1453 0 : << " smax=" << PState::stripe(vehYmax)
1454 0 : << " relY=" << pRelY
1455 : << " current=" << current
1456 0 : << " vo.xFwd=" << vo.xFwd
1457 0 : << " vo.xBack=" << vo.xBack
1458 : << " vFront=" << vehFront
1459 : << " vBack=" << vehBack
1460 0 : << "\n";
1461 : }
1462 : }
1463 : }
1464 294657 : return vehObs;
1465 0 : }
1466 :
1467 :
1468 : // ===========================================================================
1469 : // MSPModel_Striping::Obstacle method definitions
1470 : // ===========================================================================
1471 68185244 : MSPModel_Striping::Obstacle::Obstacle(int dir, double dist) :
1472 68185244 : xFwd(dir * dist), // by default, far away when seen in dir
1473 68185244 : xBack(dir * dist), // by default, far away when seen in dir
1474 68185244 : speed(0),
1475 68185244 : type(OBSTACLE_NONE),
1476 68185244 : description("") {
1477 68185244 : }
1478 :
1479 :
1480 540736824 : MSPModel_Striping::Obstacle::Obstacle(const PState& ped) :
1481 540736824 : xFwd(ped.getMaxX()),
1482 540736824 : xBack(ped.getMinX()),
1483 540736824 : speed(ped.getDirection() * ped.getSpeed()),
1484 540736824 : type(ped.getOType()),
1485 540736824 : description(ped.getID()) {
1486 : assert(!ped.isWaitingToEnter());
1487 540736824 : if (type == OBSTACLE_VEHICLE) {
1488 539629 : vehicle = static_cast<const PStateVehicle&>(ped).getVehicle();
1489 : }
1490 540736824 : }
1491 :
1492 :
1493 : bool
1494 117189550 : MSPModel_Striping::Obstacle::closer(const Obstacle& o, int dir) {
1495 117189550 : if (dir == FORWARD) {
1496 62727952 : return xBack <= o.xBack;
1497 : } else {
1498 54461598 : return xFwd >= o.xFwd;
1499 : }
1500 : }
1501 :
1502 :
1503 : // ===========================================================================
1504 : // MSPModel_Striping::PState method definitions
1505 : // ===========================================================================
1506 204043 : MSPModel_Striping::PState::PState(MSPerson* person, MSStageMoving* stage, const MSLane* lane):
1507 : MSPModel_InteractingState(person, stage, lane),
1508 204043 : myWalkingAreaPath(nullptr) {
1509 : const MSEdge* currentEdge = &lane->getEdge();
1510 : const ConstMSEdgeVector& route = myStage->getRoute();
1511 : assert(!route.empty());
1512 204043 : myDir = FORWARD;
1513 204043 : if (route.size() == 1) {
1514 : // only a single edge, move towards end pos
1515 13492 : myDir = (myEdgePos <= myStage->getArrivalPos()) ? FORWARD : BACKWARD;
1516 190551 : } else if (route.front()->getFunction() != SumoXMLEdgeFunc::NORMAL) {
1517 : // start on an intersection
1518 8 : if (route.front()->isWalkingArea()) {
1519 8 : myWalkingAreaPath = getArbitraryPath(route.front());
1520 : }
1521 : } else {
1522 190543 : int passedFwd = 0;
1523 190543 : int passedBwd = 0;
1524 190543 : const bool mayStartForward = canTraverse(FORWARD, route, passedFwd) != UNDEFINED_DIRECTION;
1525 190543 : const bool mayStartBackward = canTraverse(BACKWARD, route, passedBwd) != UNDEFINED_DIRECTION;
1526 190543 : if DEBUGCOND(*this) {
1527 0 : std::cout << " initialize dir for " << myPerson->getID() << " forward=" << mayStartForward << " backward=" << mayStartBackward << "\n";
1528 : }
1529 190543 : if (mayStartForward && mayStartBackward) {
1530 : // figure out the best direction via routing
1531 : ConstMSEdgeVector crossingRoute;
1532 7434 : MSNet::getInstance()->getPedestrianRouter(0).compute(currentEdge, route.back(), myEdgePos, myStage->getArrivalPos(),
1533 2478 : myStage->getMaxSpeed(person), 0, nullptr, person->getVTypeParameter(), crossingRoute, true);
1534 2478 : if (crossingRoute.size() > 1) {
1535 : // route found
1536 2470 : const MSEdge* nextEdge = crossingRoute[1];
1537 2470 : if (nextEdge->getFromJunction() == currentEdge->getFromJunction() || nextEdge->getToJunction() == currentEdge->getFromJunction()) {
1538 1883 : myDir = BACKWARD;
1539 : }
1540 : }
1541 2478 : if DEBUGCOND(*this) {
1542 0 : std::cout << " crossingRoute=" << toString(crossingRoute) << "\n";
1543 : }
1544 190543 : } else if (!mayStartForward && !mayStartBackward) {
1545 157 : int lastDisconnect = passedFwd >= passedBwd ? passedFwd : passedBwd;
1546 : std::string dLoc;
1547 157 : if (route.size() > 2) {
1548 260 : dLoc = TLF(" between edge '%' and edge '%'", route[lastDisconnect - 1]->getID(), route[lastDisconnect]->getID());
1549 : }
1550 942 : WRITE_WARNINGF(TL("Person '%' walking from edge '%' to edge '%' has a disconnect%, time=%."),
1551 : myPerson->getID(), route.front()->getID(), route.back()->getID(), dLoc, SIMTIME);
1552 162 : myDir = passedFwd >= passedBwd ? FORWARD : BACKWARD;
1553 : } else {
1554 346228 : myDir = !mayStartBackward ? FORWARD : BACKWARD;
1555 : }
1556 : }
1557 204043 : if (myPosLat == UNSPECIFIED_POS_LAT || myLegacyPosLat) {
1558 203759 : if (myPosLat == UNSPECIFIED_POS_LAT) {
1559 203573 : myPosLat = 0;
1560 : }
1561 203759 : if (lane->getVehicleNumberWithPartials() > 0 && myPosLat == 0) {
1562 : // better start next to the road if nothing was specified
1563 2162 : myPosLat -= stripeWidth;
1564 : }
1565 203759 : if (myDir == FORWARD || lane->getPermissions() != SVC_PEDESTRIAN) {
1566 : // start at the right side of the sidewalk on shared roads
1567 168016 : myPosLat = stripeWidth * (numStripes(lane) - 1) - myPosLat;
1568 : }
1569 284 : } else if (myPosLat == RANDOM_POS_LAT) {
1570 20 : myPosLat = RandHelper::rand() * stripeWidth * (numStripes(lane) - 1);
1571 : } else {
1572 : // vehicle to striping coordinate system
1573 264 : myPosLat = posLatConversion(myPosLat, lane->getWidth());
1574 : }
1575 204043 : if DEBUGCOND(*this) {
1576 0 : std::cout << " added new pedestrian " << myPerson->getID() << " on " << lane->getID() << " myEdgePos=" << myEdgePos << " myPosLat=" << myPosLat << " dir=" << myDir << " route=" << toString(myStage->getRoute()) << "\n";
1577 : }
1578 :
1579 204043 : myNLI = getNextLane(*this, lane, nullptr);
1580 204039 : }
1581 :
1582 :
1583 19 : MSPModel_Striping::PState::PState(MSPerson* person, MSStageMoving* stage, std::istringstream* in) :
1584 : MSPModel_InteractingState(person, stage, nullptr),
1585 19 : myWalkingAreaPath(nullptr) {
1586 19 : if (in != nullptr) {
1587 : std::string laneID;
1588 : std::string wapLaneFrom;
1589 : std::string wapLaneTo;
1590 : std::string nextLaneID;
1591 : std::string nextLinkFrom;
1592 : std::string nextLinkTo;
1593 : int nextDir;
1594 :
1595 19 : (*in) >> laneID
1596 19 : >> myEdgePos >> myPosLat >> myDir >> mySpeed >> mySpeedLat >> myWaitingToEnter
1597 19 : >> myWaitingTime >> myTotalWaitingTime
1598 19 : >> wapLaneFrom >> wapLaneTo
1599 19 : >> myAmJammed
1600 : >> nextLaneID
1601 : >> nextLinkFrom
1602 19 : >> nextLinkTo
1603 19 : >> nextDir;
1604 :
1605 :
1606 19 : myLane = MSLane::dictionary(laneID);
1607 19 : if (myLane == nullptr) {
1608 0 : throw ProcessError("Unknown lane '" + laneID + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1609 : }
1610 :
1611 : MSLane* nextLane = nullptr;
1612 19 : if (nextLaneID != "null") {
1613 16 : nextLane = MSLane::dictionary(nextLaneID);
1614 16 : if (nextLane == nullptr) {
1615 0 : throw ProcessError("Unknown next lane '" + nextLaneID + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1616 : }
1617 : }
1618 : const MSLink* link = nullptr;
1619 19 : if (nextLinkFrom != "null") {
1620 7 : MSLane* from = MSLane::dictionary(nextLinkFrom);
1621 7 : MSLane* to = MSLane::dictionary(nextLinkTo);
1622 7 : if (from == nullptr) {
1623 0 : throw ProcessError("Unknown link origin lane '" + nextLinkFrom + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1624 : }
1625 7 : if (to == nullptr) {
1626 0 : throw ProcessError("Unknown link destination lane '" + nextLinkTo + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1627 : }
1628 7 : link = from->getLinkTo(to);
1629 : }
1630 19 : myNLI = NextLaneInfo(nextLane, link, nextDir);
1631 :
1632 19 : if (wapLaneFrom != "null") {
1633 1 : MSLane* from = MSLane::dictionary(wapLaneFrom);
1634 1 : MSLane* to = MSLane::dictionary(wapLaneTo);
1635 1 : if (from == nullptr) {
1636 0 : throw ProcessError("Unknown walkingAreaPath origin lane '" + wapLaneFrom + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1637 : }
1638 1 : if (to == nullptr) {
1639 0 : throw ProcessError("Unknown walkingAreaPath destination lane '" + wapLaneTo + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1640 : }
1641 1 : const auto pathIt = myWalkingAreaPaths.find(std::make_pair(from, to));
1642 1 : if (pathIt != myWalkingAreaPaths.end()) {
1643 1 : myWalkingAreaPath = &pathIt->second;
1644 : } else {
1645 0 : throw ProcessError("Unknown walkingAreaPath from lane '" + wapLaneFrom + "' to lane '" + wapLaneTo + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1646 : }
1647 : }
1648 : }
1649 19 : }
1650 :
1651 :
1652 504841 : MSPModel_Striping::PState::PState() :
1653 504841 : MSPModel_InteractingState(nullptr, nullptr, nullptr) {}
1654 :
1655 :
1656 : void
1657 15 : MSPModel_Striping::PState::saveState(std::ostringstream& out) {
1658 15 : std::string wapLaneFrom = "null";
1659 15 : std::string wapLaneTo = "null";
1660 15 : if (myWalkingAreaPath != nullptr) {
1661 1 : wapLaneFrom = myWalkingAreaPath->from->getID();
1662 1 : wapLaneTo = myWalkingAreaPath->to->getID();
1663 : }
1664 15 : std::string nextLaneID = "null";
1665 15 : std::string nextLinkFrom = "null";
1666 15 : std::string nextLinkTo = "null";
1667 15 : if (myNLI.lane != nullptr) {
1668 : nextLaneID = myNLI.lane->getID();
1669 : }
1670 15 : if (myNLI.link != nullptr) {
1671 : nextLinkFrom = myNLI.link->getLaneBefore()->getID();
1672 7 : nextLinkTo = myNLI.link->getViaLaneOrLane()->getID();
1673 : }
1674 15 : out << " " << myLane->getID()
1675 30 : << " " << myEdgePos
1676 15 : << " " << myPosLat
1677 : << " " << myDir
1678 30 : << " " << mySpeed
1679 15 : << " " << mySpeedLat
1680 15 : << " " << myWaitingToEnter
1681 15 : << " " << myWaitingTime
1682 15 : << " " << myTotalWaitingTime
1683 : << " " << wapLaneFrom
1684 : << " " << wapLaneTo
1685 15 : << " " << myAmJammed
1686 : << " " << nextLaneID
1687 : << " " << nextLinkFrom
1688 : << " " << nextLinkTo
1689 15 : << " " << myNLI.dir;
1690 15 : }
1691 :
1692 : double
1693 1701440091 : MSPModel_Striping::PState::getMinX(const bool includeMinGap) const {
1694 : // @todo speed should have an influence here because faster persons need more space
1695 1701440091 : if (myDir == FORWARD) {
1696 1532276327 : return myEdgePos - getLength();
1697 : }
1698 169163764 : return myEdgePos - (includeMinGap ? getMinGap() : 0.);
1699 : }
1700 :
1701 :
1702 : double
1703 1701440091 : MSPModel_Striping::PState::getMaxX(const bool includeMinGap) const {
1704 : // @todo speed should have an influence here because faster persons need more space
1705 1701440091 : if (myDir == FORWARD) {
1706 1532276327 : return myEdgePos + (includeMinGap ? getMinGap() : 0.);
1707 : }
1708 169163764 : return myEdgePos + getLength();
1709 : }
1710 :
1711 :
1712 : double
1713 1752860016 : MSPModel_Striping::PState::getLength() const {
1714 1752860016 : return myPerson->getVehicleType().getLength();
1715 : }
1716 :
1717 :
1718 : double
1719 1085271671 : MSPModel_Striping::PState::getMinGap() const {
1720 1085271671 : return myPerson->getVehicleType().getMinGap();
1721 : }
1722 :
1723 :
1724 : int
1725 6100938775 : MSPModel_Striping::PState::stripe(double relY) {
1726 6100938775 : return (int)floor(relY / stripeWidth + 0.5);
1727 : }
1728 :
1729 :
1730 : int
1731 2636709432 : MSPModel_Striping::PState::otherStripe(double relY) const {
1732 2636709432 : const int s = stripe(relY);
1733 2636709432 : const double offset = relY - s * stripeWidth;
1734 2636709432 : const double threshold = MAX2(NUMERICAL_EPS, stripeWidth - SQUEEZE * getWidth());
1735 : int result;
1736 2636709432 : if (offset > threshold) {
1737 475634 : result = s + 1;
1738 2636233798 : } else if (offset < -threshold) {
1739 404372 : result = s - 1;
1740 : } else {
1741 : result = s;
1742 : }
1743 : //std::cout.setf(std::ios::fixed , std::ios::floatfield);
1744 : //std::cout << std::setprecision(5);
1745 : //if DEBUGCOND(*this) std::cout << " otherStripe " << myPerson->getID() << " offset=" << offset << " threshold=" << threshold << " rawResult=" << result << "\n";
1746 2636709432 : return result;
1747 : }
1748 :
1749 : int
1750 3439005964 : MSPModel_Striping::PState::stripe() const {
1751 3439005964 : return MIN2(MAX2(0, stripe(myPosLat)), numStripes(myLane) - 1);
1752 : }
1753 :
1754 :
1755 : int
1756 2623268195 : MSPModel_Striping::PState::otherStripe() const {
1757 2623268195 : return MIN2(MAX2(0, otherStripe(myPosLat)), numStripes(myLane) - 1);
1758 : }
1759 :
1760 :
1761 : double
1762 104929856 : MSPModel_Striping::PState::distToLaneEnd() const {
1763 104929856 : if (myStage->getNextRouteEdge() == nullptr) {
1764 22031154 : return myDir * (myStage->getArrivalPos() - myEdgePos) - POSITION_EPS - (
1765 1394047 : (myWaitingTime > DELTA_T && (myStage->getDestinationStop() == nullptr ||
1766 : myStage->getDestinationStop()->getWaitingCapacity() > myStage->getDestinationStop()->getNumWaitingPersons()))
1767 23425201 : ? getMinGap() : 0);
1768 : } else {
1769 82898702 : const double length = myWalkingAreaPath == nullptr ? myLane->getLength() : myWalkingAreaPath->length;
1770 82898702 : return myDir == FORWARD ? length - myEdgePos : myEdgePos;
1771 : }
1772 : }
1773 :
1774 :
1775 : bool
1776 878133 : MSPModel_Striping::PState::moveToNextLane(SUMOTime currentTime) {
1777 878133 : double dist = distToLaneEnd();
1778 878133 : if (DEBUGCOND(*this)) {
1779 0 : std::cout << SIMTIME << " ped=" << myPerson->getID() << " myEdgePos=" << myEdgePos << " dist=" << dist << "\n";
1780 : }
1781 878133 : if (dist <= 0) {
1782 : //if (ped.getPerson()->getID() == DEBUG1) {
1783 : // std::cout << SIMTIME << " addToLane x=" << ped.myEdgePos << " newDir=" << newDir << " newLane=" << newLane->getID() << " walkingAreaShape=" << walkingAreaShape << "\n";
1784 : //}
1785 : //std::cout << " changing to " << newLane->getID() << " myPosLat=" << ped.myPosLat << " oldStripes=" << numStripes(myLane) << " newStripes=" << numStripes(newLane);
1786 : //std::cout << " newY=" << ped.myPosLat << " myDir=" << ped.getDirection() << " newDir=" << newDir;
1787 878133 : const int oldDir = myDir;
1788 878133 : const MSLane* oldLane = myLane;
1789 878133 : myLane = myNLI.lane;
1790 878133 : myDir = myNLI.dir;
1791 878133 : const bool normalLane = (myLane == nullptr || myLane->getEdge().getFunction() == SumoXMLEdgeFunc::NORMAL || &myLane->getEdge() == myStage->getNextRouteEdge());
1792 878133 : if DEBUGCOND(*this) {
1793 0 : std::cout << SIMTIME
1794 0 : << " ped=" << myPerson->getID()
1795 : << " moveToNextLane old=" << oldLane->getID()
1796 0 : << " new=" << (myLane == nullptr ? "NULL" : myLane->getID())
1797 : << " oldDir=" << oldDir
1798 0 : << " newDir=" << myDir
1799 0 : << " myEdgePos=" << myEdgePos
1800 : << " dist=" << dist
1801 0 : << "\n";
1802 : }
1803 878133 : if (myLane == nullptr) {
1804 194706 : myEdgePos = myStage->getArrivalPos();
1805 : }
1806 : // moveToNextEdge might destroy the person and thus mess up the heap. Better check first
1807 878133 : if (myStage->getRouteStep() == myStage->getRoute().end() - 1) {
1808 194706 : myLane = nullptr;
1809 : } else {
1810 683427 : const bool arrived = myStage->moveToNextEdge(myPerson, currentTime, oldDir, normalLane ? nullptr : &myLane->getEdge());
1811 : UNUSED_PARAMETER(arrived);
1812 : assert(!arrived);
1813 : assert(myDir != UNDEFINED_DIRECTION);
1814 683427 : myNLI = getNextLane(*this, myLane, oldLane);
1815 : // reminders must be called after updated myNLI (to ensure that getNextEdgePtr returns the correct edge)
1816 683427 : myStage->activateEntryReminders(myPerson);
1817 : assert(myNLI.lane != oldLane); // do not turn around
1818 683427 : if DEBUGCOND(*this) {
1819 0 : std::cout << " nextLane=" << (myNLI.lane == nullptr ? "NULL" : myNLI.lane->getID()) << "\n";
1820 : }
1821 683427 : if (myLane->isWalkingArea()) {
1822 309666 : if (myNLI.dir != UNDEFINED_DIRECTION) {
1823 309629 : myWalkingAreaPath = getWalkingAreaPath(&myLane->getEdge(), oldLane, myNLI.lane);
1824 : assert(myWalkingAreaPath->shape.size() >= 2);
1825 309629 : if DEBUGCOND(*this) {
1826 0 : std::cout << " mWAPath shape=" << myWalkingAreaPath->shape << " length=" << myWalkingAreaPath->length << "\n";
1827 : }
1828 37 : } else if (myNLI.link != nullptr) {
1829 : // using direct connection (instead of using walkingarea)
1830 17 : myLane = myNLI.lane;
1831 : assert(!myLane->isWalkingArea());
1832 17 : myStage->moveToNextEdge(myPerson, currentTime, myDir, &myLane->getEdge());
1833 17 : myWalkingAreaPath = nullptr;
1834 17 : myNLI = getNextLane(*this, myLane, oldLane);
1835 : } else {
1836 : // disconnected route. move to the next edge
1837 40 : if (OptionsCont::getOptions().getBool("ignore-route-errors")) {
1838 : // try to determine direction from topology, otherwise maintain current direction
1839 16 : const MSEdge* currRouteEdge = *myStage->getRouteStep();
1840 16 : const MSEdge* nextRouteEdge = myStage->getNextRouteEdge();
1841 : if ((nextRouteEdge->getToJunction() == currRouteEdge->getFromJunction())
1842 16 : || nextRouteEdge->getToJunction() == currRouteEdge->getToJunction()) {
1843 4 : myDir = BACKWARD;
1844 : } else if ((nextRouteEdge->getFromJunction() == currRouteEdge->getFromJunction())
1845 12 : || nextRouteEdge->getFromJunction() == currRouteEdge->getToJunction()) {
1846 12 : myDir = FORWARD;
1847 : }
1848 16 : myStage->moveToNextEdge(myPerson, currentTime, oldDir, nullptr);
1849 16 : myLane = myNLI.lane;
1850 : assert(myLane != 0);
1851 : assert(myLane->getEdge().getFunction() == SumoXMLEdgeFunc::NORMAL);
1852 16 : myNLI = getNextLane(*this, myLane, oldLane);
1853 16 : myWalkingAreaPath = nullptr;
1854 : } else {
1855 12 : throw ProcessError(TLF("Disconnected walk for person '%'.", myPerson->getID()));
1856 : }
1857 : }
1858 : } else {
1859 373761 : myWalkingAreaPath = nullptr;
1860 : }
1861 : // adapt x to fit onto the new lane
1862 : // (make sure we do not move past the end of the new lane since that
1863 : // lane was not checked for obstacles)
1864 683423 : const double newLength = (myWalkingAreaPath == nullptr ? myLane->getLength() : myWalkingAreaPath->length);
1865 683423 : if (-dist > newLength) {
1866 : assert(OptionsCont::getOptions().getBool("ignore-route-errors"));
1867 : // should not happen because the end of myLane should have been an obstacle as well
1868 : // (only when the route is broken)
1869 1 : dist = -newLength;
1870 : }
1871 683423 : if (myDir == BACKWARD) {
1872 145654 : myEdgePos = newLength + dist;
1873 : } else {
1874 537769 : myEdgePos = -dist;
1875 : }
1876 683423 : if DEBUGCOND(*this) {
1877 0 : std::cout << SIMTIME << " update myEdgePos ped=" << myPerson->getID()
1878 : << " newLength=" << newLength
1879 : << " dist=" << dist
1880 0 : << " myEdgePos=" << myEdgePos
1881 0 : << "\n";
1882 : }
1883 : // adjust to change in direction
1884 683423 : if (myDir != oldDir) {
1885 32610 : myPosLat = (numStripes(oldLane) - 1) * stripeWidth - myPosLat;
1886 : }
1887 : // adjust to differences in sidewalk width
1888 1353690 : const int offset = getStripeOffset(numStripes(oldLane), numStripes(myLane), oldDir != myDir && numStripes(myLane) < numStripes(oldLane));
1889 683423 : myPosLat += offset * stripeWidth;
1890 683423 : if DEBUGCOND(*this) {
1891 0 : std::cout << SIMTIME << " transformY ped=" << myPerson->getID()
1892 0 : << " newLane=" << Named::getIDSecure(myLane)
1893 0 : << " newY=" << myPosLat
1894 0 : << " os=" << numStripes(oldLane) << " ns=" << numStripes(myLane)
1895 0 : << " od=" << oldDir << " nd=" << myDir
1896 0 : << " offset=" << offset << "\n";
1897 : }
1898 : }
1899 878129 : myAngle = std::numeric_limits<double>::max(); // see #9014
1900 878129 : return true;
1901 : } else {
1902 : return false;
1903 : }
1904 : }
1905 :
1906 :
1907 : int
1908 51490916 : MSPModel_Striping::getReserved(int stripes, double factor) {
1909 51490916 : return MIN2(
1910 51490916 : (int)floor(stripes * factor),
1911 51490916 : (int)floor(RESERVE_FOR_ONCOMING_MAX / stripeWidth));
1912 : }
1913 :
1914 : void
1915 51419925 : MSPModel_Striping::PState::walk(const Obstacles& obs) {
1916 51419925 : const int stripes = (int)obs.size();
1917 51419925 : const int sMax = stripes - 1;
1918 : assert(stripes == numStripes(myLane));
1919 : // account stage-specific max speed but also for normal lane speed limit
1920 : // (speed limits on crossings and walkingareas ignored due to #11527)
1921 51419925 : const double vMax = (myStage->getConfiguredSpeed() >= 0
1922 51419925 : ? myStage->getConfiguredSpeed()
1923 9182965 : : (USE_NET_SPEEDS || myLane->isNormal() || myLane->isInternal()
1924 59823111 : ? myLane->getVehicleMaxSpeed(myPerson)
1925 724739 : : myStage->getMaxSpeed(myPerson)));
1926 : // ultimate goal is to choose the preferred stripe (chosen)
1927 51419925 : const int current = stripe();
1928 51419925 : const int other = otherStripe();
1929 : // compute distances
1930 51419925 : std::vector<double> distance(stripes);
1931 229044052 : for (int i = 0; i < stripes; ++i) {
1932 177624127 : distance[i] = distanceTo(obs[i], obs[i].type == OBSTACLE_PED);
1933 : }
1934 : // compute utility for all stripes
1935 51419925 : std::vector<double> utility(stripes, 0);
1936 : // forbid stripes which are blocked and also all stripes behind them
1937 229044052 : for (int i = 0; i < stripes; ++i) {
1938 177624127 : if (distance[i] == DIST_OVERLAP) {
1939 68619595 : if (i == current && (!isWaitingToEnter() || stripe() != stripe(myPosLat))) {
1940 5025936 : utility[i] += OBSTRUCTED_PENALTY;
1941 : }
1942 68619595 : if (i < current) {
1943 94705852 : for (int j = 0; j <= i; ++j) {
1944 58748297 : utility[j] += OBSTRUCTED_PENALTY;
1945 : }
1946 : }
1947 68619595 : if (i > current) {
1948 48597015 : for (int j = i; j < stripes; ++j) {
1949 28391485 : utility[j] += OBSTRUCTED_PENALTY;
1950 : }
1951 : }
1952 : }
1953 : }
1954 : // forbid a portion of the leftmost stripes (in walking direction).
1955 : // lanes with stripes less than 1 / RESERVE_FOR_ONCOMING_FACTOR
1956 : // may still deadlock in heavy pedestrian traffic
1957 51419925 : const bool onJunction = myLane->isWalkingArea() || myLane->isCrossing();
1958 51419925 : const int reserved = getReserved(stripes, (onJunction ? RESERVE_FOR_ONCOMING_FACTOR_JUNCTIONS : RESERVE_FOR_ONCOMING_FACTOR));
1959 51419925 : if (myDir == FORWARD) {
1960 52547858 : for (int i = 0; i < reserved; ++i) {
1961 12760206 : utility[i] += INAPPROPRIATE_PENALTY * (i == current ? 0.5 : 1);
1962 : }
1963 : } else {
1964 6905177 : for (int i = sMax; i > sMax - reserved; --i) {
1965 2827702 : utility[i] += INAPPROPRIATE_PENALTY * (i == current ? 0.5 : 1);
1966 : }
1967 : }
1968 : // adapt utility based on obstacles
1969 229044052 : for (int i = 0; i < stripes; ++i) {
1970 177624127 : if (obs[i].speed * myDir < 0) {
1971 : // penalize evasion to the left unless the obstacle is a vehicle
1972 4452780 : if ((myDir == FORWARD || obs[i].type == OBSTACLE_VEHICLE) && i > 0) {
1973 857400 : utility[i - 1] -= 0.5;
1974 3595380 : } else if (myDir == BACKWARD && i < sMax) {
1975 591016 : utility[i + 1] -= 0.5;
1976 : }
1977 : }
1978 : // compute expected distance achievable by staying on this stripe for a time horizon
1979 177624127 : const double walkDist = MAX2(0., distance[i]); // disregard special distance flags
1980 177624127 : const double lookAhead = obs[i].speed * myDir >= 0 ? LOOKAHEAD_SAMEDIR : LOOKAHEAD_ONCOMING;
1981 177624127 : const double expectedDist = MIN2(vMax * LOOKAHEAD_SAMEDIR, walkDist + obs[i].speed * myDir * lookAhead);
1982 177624127 : if (expectedDist >= 0) {
1983 174507093 : utility[i] += expectedDist;
1984 : } else {
1985 : // let only the distance count
1986 3117034 : utility[i] += ONCOMING_CONFLICT_PENALTY + distance[i];
1987 : }
1988 : }
1989 : // discourage use of the leftmost stripe (in walking direction) if there are oncoming
1990 51419925 : if (myDir == FORWARD && obs[0].speed < 0) {
1991 1501229 : utility[0] += ONCOMING_CONFLICT_PENALTY;
1992 49918696 : } else if (myDir == BACKWARD && obs[sMax].speed > 0) {
1993 1506539 : utility[sMax] += ONCOMING_CONFLICT_PENALTY;
1994 : }
1995 : // penalize lateral movement (if the current stripe permits walking)
1996 51419925 : if (distance[current] > 0 && myWaitingTime == 0) {
1997 122377376 : for (int i = 0; i < stripes; ++i) {
1998 96128059 : utility[i] += abs(i - current) * LATERAL_PENALTY;
1999 : }
2000 : }
2001 : // walk on the right side on shared space
2002 51419925 : if (myLane->getPermissions() != SVC_PEDESTRIAN && myDir == BACKWARD) {
2003 2396508 : for (int i = 0; i < stripes; ++i) {
2004 1997270 : if (i <= current) {
2005 1903287 : utility[i] += (sMax - i + 1) * LATERAL_PENALTY;
2006 : }
2007 : }
2008 : }
2009 :
2010 : // select best stripe
2011 : int chosen = current;
2012 229044052 : for (int i = 0; i < stripes; ++i) {
2013 177624127 : if (utility[i] > utility[chosen] && utility[i] >= INAPPROPRIATE_PENALTY * 0.5) {
2014 : chosen = i;
2015 : }
2016 : }
2017 : // compute speed components along both axes
2018 51419925 : const int next = (chosen == current ? current : (chosen < current ? current - 1 : current + 1));
2019 51419925 : double xDist = MIN3(distance[current], distance[other], distance[next]);
2020 51419925 : if (next != chosen) {
2021 : // ensure that we do not collide with an obstacle in the stripe beyond
2022 : // next as this might become the 'other' stripe in the next step
2023 343819 : const int nextOther = chosen < current ? current - 2 : current + 2;
2024 343819 : xDist = MIN2(xDist, distance[nextOther]);
2025 : }
2026 : // XXX preferred gap differs between approaching a standing obstacle or a moving obstacle
2027 : const double preferredGap = NUMERICAL_EPS;
2028 51419925 : double xSpeed = MIN2(vMax, MAX2(0., DIST2SPEED(xDist - preferredGap)));
2029 51419925 : if (xSpeed < NUMERICAL_EPS) {
2030 : xSpeed = 0.;
2031 : }
2032 51419925 : if (DEBUGCOND(*this)) {
2033 0 : std::cout << " xSpeedPotential=" << xSpeed << "\n";
2034 : }
2035 : // avoid tiny steps
2036 : // XXX pressure from behind?
2037 51419925 : if (mySpeed == 0 && xDist < MIN_STARTUP_DIST &&
2038 : // unless walking towards a short lane
2039 18843707 : !(
2040 19128016 : (xDist == distance[current] && obs[current].type >= OBSTACLE_END)
2041 18843723 : || (xDist == distance[other] && obs[other].type >= OBSTACLE_END)
2042 18843707 : || (xDist == distance[next] && obs[next].type >= OBSTACLE_END))
2043 : ) {
2044 : xSpeed = 0;
2045 : }
2046 32576220 : if (xSpeed == 0) {
2047 23714521 : if (DEBUGCOND(*this)) {
2048 0 : std::cout << " sharedWA=" << (myWalkingAreaFoes.find(&myLane->getEdge()) != myWalkingAreaFoes.end())
2049 0 : << " vehObs=" << Named::getIDSecure(obs[current].vehicle)
2050 0 : << " vehWait=" << STEPS2TIME(obs[current].vehicle ? obs[current].vehicle->getWaitingTime() : 0)
2051 0 : << "\n";
2052 : }
2053 23714521 : if (myWaitingTime > ((myLane->isCrossing()
2054 : // treat shared walkingarea like a crossing to avoid deadlocking vehicles
2055 23710574 : || (myLane->isWalkingArea() && obs[current].vehicle != nullptr && obs[current].vehicle->getWaitingTime() > jamTimeCrossing
2056 47425378 : && myWalkingAreaFoes.find(&myLane->getEdge()) != myWalkingAreaFoes.end())) ? jamTimeCrossing : jamTime)
2057 23665782 : || (sMax == 0 && obs[0].speed * myDir < 0 && myWaitingTime > jamTimeNarrow)
2058 47351718 : || myAmJammed) {
2059 : // squeeze slowly through the crowd ignoring others
2060 5704494 : if (!myAmJammed) {
2061 63179 : MSNet::getInstance()->getPersonControl().registerJammed();
2062 252716 : WRITE_WARNINGF(TL("Person '%' is jammed on edge '%', time=%."),
2063 : myPerson->getID(), myStage->getEdge()->getID(), time2string(SIMSTEP));
2064 63179 : myAmJammed = true;
2065 : }
2066 5704494 : xSpeed = vMax * jamFactor;
2067 : }
2068 27705404 : } else if (myAmJammed && stripe(myPosLat) >= 0 && stripe(myPosLat) <= sMax && xDist >= MIN_STARTUP_DIST) {
2069 56393 : myAmJammed = false;
2070 : }
2071 : // dawdling
2072 51419925 : const double dawdle = MIN2(xSpeed, RandHelper::rand() * vMax * dawdling);
2073 51419925 : xSpeed -= dawdle;
2074 :
2075 : // XXX ensure that diagonal speed <= vMax
2076 : // avoid deadlocks on narrow sidewalks
2077 : //if (oncoming && xSpeed == 0 && myStage->getWaitingTime(currentTime) > TIME2STEPS(ONCOMIN_PATIENCE)) {
2078 : // if DEBUGCOND(*this) std::cout << " stepping aside to resolve oncoming deadlock\n";
2079 : // xSpeed = POSITION_EPS; // reset myWaitingTime
2080 : // if (myDir == FORWARD && chosen < sMax) {
2081 : // chosen += 1;
2082 : // } else if (myDir == BACKWARD && chosen > 0) {
2083 : // chosen -= 1;
2084 : // }
2085 : //}
2086 51419925 : const double maxYSpeed = MIN2(MAX2(vMax * LATERAL_SPEED_FACTOR, vMax - xSpeed), stripeWidth);
2087 : double ySpeed = 0;
2088 : double yDist = 0;
2089 51419925 : if (utility[next] > OBSTRUCTION_THRESHOLD && utility[chosen] > OBSTRUCTION_THRESHOLD) {
2090 : // don't move laterally if the stripes are blocked
2091 46424894 : yDist = (chosen * stripeWidth) - myPosLat;
2092 46424894 : if (fabs(yDist) > NUMERICAL_EPS) {
2093 2108231 : ySpeed = (yDist > 0 ?
2094 961609 : MIN2(maxYSpeed, DIST2SPEED(yDist)) :
2095 1146622 : MAX2(-maxYSpeed, DIST2SPEED(yDist)));
2096 : }
2097 4995031 : } else if (utility[next] <= OBSTRUCTION_THRESHOLD && obs[next].type == OBSTACLE_VEHICLE
2098 : // still on the road
2099 9765 : && stripe() == stripe(myPosLat)
2100 : // only when the vehicle is moving on the same lane
2101 5000417 : && !(myLane->isCrossing() || myLane->isWalkingArea())) {
2102 : // step aside to let the vehicle pass
2103 3125 : int stepAsideDir = myDir;
2104 3125 : if (myLane->getEdge().getLanes().size() > 1 || current > sMax / 2) {
2105 : // always step to the right on multi-lane edges or when closer to
2106 : // the right side
2107 : stepAsideDir = FORWARD;
2108 : }
2109 3125 : myAmJammed = true; // ignore pedestrian-pedestrian collisions
2110 3125 : ySpeed = stepAsideDir * vMax;
2111 : }
2112 :
2113 : // DEBUG
2114 51419925 : if DEBUGCOND(*this) {
2115 0 : std::cout << SIMTIME
2116 0 : << " ped=" << myPerson->getID()
2117 0 : << " edge=" << myStage->getEdge()->getID()
2118 0 : << " x=" << myEdgePos
2119 0 : << " y=" << myPosLat
2120 : << " d=" << myDir
2121 0 : << " pvx=" << mySpeed
2122 : << " cur=" << current
2123 0 : << " cho=" << chosen
2124 0 : << " oth=" << other
2125 0 : << " nxt=" << next
2126 0 : << " vx=" << xSpeed
2127 : << " dawdle=" << dawdle
2128 : << " vy=" << ySpeed
2129 : << " xd=" << xDist
2130 : << " yd=" << yDist
2131 : << " vMax=" << vMax
2132 0 : << " wTime=" << myStage->getWaitingTime()
2133 0 : << " jammed=" << myAmJammed
2134 0 : << "\n";
2135 0 : if (DEBUGCOND(*this)) {
2136 0 : for (int i = 0; i < stripes; ++i) {
2137 0 : const Obstacle& o = obs[i];
2138 0 : std::cout << " util=" << utility[i] << " dist=" << distance[i] << " o=" << o.description;
2139 0 : if (o.description != "") {
2140 0 : std::cout << " xF=" << o.xFwd << " xB=" << o.xBack << " v=" << o.speed;
2141 : }
2142 0 : if (i == current) {
2143 0 : std::cout << " current";
2144 : }
2145 0 : if (i == other && i != current) {
2146 0 : std::cout << " other";
2147 : }
2148 0 : if (i == chosen) {
2149 0 : std::cout << " chosen";
2150 : }
2151 0 : if (i == next) {
2152 0 : std::cout << " next";
2153 : }
2154 0 : std::cout << "\n";
2155 : }
2156 : }
2157 : }
2158 51419925 : myEdgePos += SPEED2DIST(xSpeed * myDir);
2159 51419925 : myPosLat += SPEED2DIST(ySpeed);
2160 51419925 : mySpeedLat = ySpeed;
2161 51419925 : mySpeed = xSpeed;
2162 51419925 : if (xSpeed >= SUMO_const_haltingSpeed) {
2163 30491469 : myWaitingToEnter = false;
2164 30491469 : myWaitingTime = 0;
2165 : } else {
2166 20928456 : myWaitingTime += DELTA_T;
2167 20928456 : myTotalWaitingTime += DELTA_T;
2168 : }
2169 51419925 : myAngle = std::numeric_limits<double>::max(); // set on first access or via remote control
2170 51419925 : }
2171 :
2172 :
2173 : double
2174 9692252 : MSPModel_Striping::PState::getImpatience() const {
2175 9692252 : return MAX2(0., MIN2(1., myPerson->getVehicleType().getImpatience()
2176 9692252 : + STEPS2TIME(myStage->getWaitingTime()) / MAX_WAIT_TOLERANCE));
2177 : }
2178 :
2179 :
2180 : Position
2181 9810286 : MSPModel_Striping::PState::getPosition(const MSStageMoving& stage, SUMOTime) const {
2182 : if (myRemoteXYPos != Position::INVALID) {
2183 6405 : return myRemoteXYPos;
2184 : }
2185 9803881 : if (myLane == nullptr) {
2186 : // pedestrian has already finished
2187 206 : return Position::INVALID;
2188 : }
2189 9803675 : const double lateral_offset = -getLatOffset(); // the minus is hunting me in my dreams but seems to be here for historical reasons
2190 9803675 : if (myWalkingAreaPath == nullptr) {
2191 1122766 : return stage.getLanePosition(myLane, myEdgePos, lateral_offset);
2192 : } else {
2193 : //if DEBUGCOND(*this) {
2194 : // std::cout << SIMTIME
2195 : // << " getPosition (walkingArea)"
2196 : // << " p=" << myPerson->getID()
2197 : // << " x=" << myEdgePos
2198 : // << " y=" << myPosLat
2199 : // << " latOffset=" << lateral_offset
2200 : // << " shape=" << myWalkingAreaPath->shape
2201 : // << " pos=" << myWalkingAreaPath->shape.positionAtOffset(myEdgePos, lateral_offset)
2202 : // << "\n";
2203 : //}
2204 8680909 : if (myWalkingAreaPath->angleOverride == INVALID_DOUBLE) {
2205 8491720 : return myWalkingAreaPath->shape.positionAtOffset(myEdgePos, lateral_offset);
2206 : } else {
2207 189189 : const double rotationOffset = myDir == FORWARD ? 0 : DEG2RAD(180);
2208 189189 : return myWalkingAreaPath->shape.sidePositionAtAngle(myEdgePos, lateral_offset, myWalkingAreaPath->angleOverride + rotationOffset);
2209 : }
2210 : }
2211 : }
2212 :
2213 :
2214 : double
2215 2862093 : MSPModel_Striping::PState::getAngle(const MSStageMoving&, SUMOTime) const {
2216 2862093 : if (myAngle != std::numeric_limits<double>::max()) {
2217 : return myAngle;
2218 : }
2219 932725 : if (myLane == nullptr) {
2220 : // pedestrian has already finished
2221 : return 0;
2222 : }
2223 932518 : if (myWalkingAreaPath != nullptr && myWalkingAreaPath->angleOverride != INVALID_DOUBLE) {
2224 : return myWalkingAreaPath->angleOverride;
2225 : }
2226 881815 : const PositionVector& shp = myWalkingAreaPath == nullptr ? myLane->getShape() : myWalkingAreaPath->shape;
2227 881815 : double geomX = myWalkingAreaPath == nullptr ? myLane->interpolateLanePosToGeometryPos(myEdgePos) : myEdgePos;
2228 881815 : double angle = shp.rotationAtOffset(geomX) + (myDir == MSPModel::BACKWARD ? M_PI : 0);
2229 881815 : if (myDir == MSPModel::BACKWARD) {
2230 288438 : angle += atan2(mySpeedLat, MAX2(mySpeed, NUMERICAL_EPS));
2231 : } else { // myDir == MSPModel::FORWARD
2232 747196 : angle -= atan2(mySpeedLat, MAX2(mySpeed, NUMERICAL_EPS));
2233 : }
2234 881815 : if (angle > M_PI) {
2235 205856 : angle -= 2 * M_PI;
2236 : }
2237 881815 : myAngle = angle;
2238 881815 : return angle;
2239 : }
2240 :
2241 :
2242 : const MSEdge*
2243 166692 : MSPModel_Striping::PState::getNextEdge(const MSStageMoving&) const {
2244 166692 : return myNLI.lane == nullptr ? nullptr : &myNLI.lane->getEdge();
2245 : }
2246 :
2247 :
2248 : const MSLane*
2249 1634 : MSPModel_Striping::PState::getNextCrossing() const {
2250 1634 : return myNLI.lane != nullptr && myNLI.lane->isCrossing() ? myNLI.lane : nullptr;
2251 : }
2252 :
2253 :
2254 : void
2255 3496 : MSPModel_Striping::PState::reverse(const double pathLength, const double usableWidth) {
2256 3496 : myEdgePos = pathLength - myEdgePos;
2257 3496 : myPosLat = usableWidth - myPosLat;
2258 3496 : myDir = -myWalkingAreaPath->dir;
2259 3496 : mySpeedLat = -mySpeedLat;
2260 3496 : }
2261 :
2262 :
2263 : void
2264 256782 : MSPModel_Striping::PState::reset(const double edgePos, const double latPos) {
2265 256782 : myEdgePos = edgePos;
2266 256782 : myPosLat = latPos;
2267 256782 : myDir = UNDEFINED_DIRECTION; // only an obstacle, speed may be orthogonal to dir
2268 256782 : mySpeed = 0.;
2269 256782 : mySpeedLat = 0.;
2270 256782 : }
2271 :
2272 :
2273 : void
2274 12 : MSPModel_Striping::PState::moveTo(MSPerson* p, MSLane* lane, double lanePos, double lanePosLat, SUMOTime t) {
2275 : ConstMSEdgeVector newEdges; // keep route
2276 : int routeOffset = 0;
2277 : bool laneOnRoute = false;
2278 12 : const MSJunction* laneOnJunction = lane->isNormal() ? nullptr : lane->getEdge().getToJunction();
2279 12 : for (const MSEdge* edge : myStage->getRoute()) {
2280 : if (edge == &lane->getEdge()
2281 9 : || edge->getToJunction() == laneOnJunction
2282 12 : || edge->getFromJunction() == laneOnJunction) {
2283 : laneOnRoute = true;
2284 : break;
2285 : }
2286 0 : routeOffset++;
2287 : }
2288 12 : if (!laneOnRoute) {
2289 0 : throw ProcessError("Lane '" + lane->getID() + "' is not on the route of person '" + getID() + "'.");
2290 : }
2291 12 : Position pos = lane->geometryPositionAtOffset(lanePos, lanePosLat);
2292 12 : if (lane->isWalkingArea() && (myWalkingAreaPath == nullptr || myWalkingAreaPath->lane != lane)) {
2293 : // entered new walkingarea. Determine path to guess position
2294 6 : const MSEdge* prevEdge = myStage->getRoute()[routeOffset];
2295 6 : const MSEdge* nextEdge = routeOffset + 1 < (int)myStage->getRoute().size() ? myStage->getRoute()[routeOffset + 1] : nullptr;
2296 6 : const WalkingAreaPath* guessed = guessPath(&lane->getEdge(), prevEdge, nextEdge);
2297 6 : const double maxPos = guessed->shape.length() - NUMERICAL_EPS;
2298 6 : if (lanePos > maxPos + POSITION_EPS || lanePos < -POSITION_EPS) {
2299 0 : throw ProcessError("Lane position " + toString(lanePos) + " cannot be mapped onto walkingarea '" + lane->getID()
2300 0 : + "' (fromLane='" + guessed->from->getID()
2301 0 : + "' toLane='" + guessed->to->getID() + "') for person '" + getID() + "' time=" + time2string(t) + ".");
2302 : }
2303 : // give some slack
2304 6 : lanePos = MIN2(maxPos, MAX2(NUMERICAL_EPS, lanePos));
2305 6 : pos = guessed->shape.positionAtOffset(lanePos, lanePosLat);
2306 : }
2307 12 : const double angle = GeomHelper::naviDegree(p->getPosition().angleTo2D(pos));
2308 12 : moveToXY(p, pos, lane, lanePos, lanePosLat, angle, routeOffset, newEdges, t);
2309 12 : }
2310 :
2311 :
2312 : void
2313 13495 : MSPModel_Striping::PState::moveToXY(MSPerson* p, Position pos, MSLane* lane, double lanePos,
2314 : double lanePosLat, double angle, int routeOffset,
2315 : const ConstMSEdgeVector& edges, SUMOTime t) {
2316 13495 : MSPModel_Striping* pm = dynamic_cast<MSPModel_Striping*>(MSNet::getInstance()->getPersonControl().getMovementModel());
2317 : assert(p == myPerson);
2318 : assert(pm != nullptr);
2319 13495 : const double oldAngle = GeomHelper::naviDegree(getAngle(*myStage, t));
2320 : // person already walking in this step. undo this to obtain the previous position
2321 13495 : const double oldX = myEdgePos - SPEED2DIST(mySpeed * myDir);
2322 : const double tmp = myEdgePos;
2323 13495 : myEdgePos = oldX;
2324 13495 : Position oldPos = getPosition(*myStage, t);
2325 13495 : myEdgePos = tmp;
2326 : //if (oldPos == Position::INVALID) {
2327 : // oldPos = pos
2328 : //}
2329 13495 : myAngle = GeomHelper::fromNaviDegree(angle);
2330 : #ifdef DEBUG_MOVETOXY
2331 : std::cout << SIMTIME << " ped=" << p->getID()
2332 : << " moveToXY"
2333 : << " pos=" << pos
2334 : << " lane=" << lane->getID()
2335 : << " lanePos=" << lanePos
2336 : << " lanePosLat=" << lanePosLat
2337 : << " angle=" << angle
2338 : << " routeOffset=" << routeOffset
2339 : << " edges=" << toString(edges)
2340 : << " oldLane=" << Named::getIDSecure(myLane)
2341 : << " path=" << (myWalkingAreaPath == nullptr ? "null" : (myWalkingAreaPath->from->getID() + "->" + myWalkingAreaPath->to->getID())) << "\n";
2342 : #endif
2343 :
2344 13495 : if (lane != myLane && myLane != nullptr && lane != nullptr) {
2345 308 : pm->remove(this);
2346 : pm->registerActive();
2347 : }
2348 13495 : if (lane != nullptr &&
2349 10363 : fabs(lanePosLat) < (0.5 * (lane->getWidth() + p->getVehicleType().getWidth()) + SIDEWALK_OFFSET)) {
2350 10359 : myRemoteXYPos = Position::INVALID;
2351 10359 : const MSEdge* old = myStage->getEdge();
2352 10359 : const MSLane* oldLane = myLane;
2353 10359 : if (lane != myLane) {
2354 : // implicitly adds new active lane if necessary
2355 308 : pm->myActiveLanes[lane].push_back(this);
2356 : }
2357 10359 : if (edges.empty()) {
2358 : // map within route
2359 10320 : myStage->setRouteIndex(myPerson, routeOffset);
2360 : } else {
2361 39 : myStage->replaceRoute(myPerson, edges, routeOffset);
2362 : }
2363 10359 : if (!lane->getEdge().isNormal()) {
2364 389 : myStage->moveToNextEdge(myPerson, t, myDir, &lane->getEdge());
2365 : }
2366 :
2367 10359 : myLane = lane;
2368 10359 : const double lateral_offset = (lane->getWidth() - stripeWidth) * 0.5;
2369 10359 : if (lane->isWalkingArea()) {
2370 190 : if (myWalkingAreaPath == nullptr || myWalkingAreaPath->lane != lane) {
2371 : // entered new walkingarea. Determine path
2372 40 : myWalkingAreaPath = guessPath(&lane->getEdge(), old, myStage->getNextRouteEdge());
2373 : #ifdef DEBUG_MOVETOXY
2374 : std::cout << " guessPath old=" << old->getID() << " next=" << Named::getIDSecure(myStage->getNextRouteEdge())
2375 : << " path=" << myWalkingAreaPath->from->getID() << "->" << myWalkingAreaPath->to->getID() << "\n";
2376 : #endif
2377 : }
2378 : // lanePos and lanePosLat are matched onto the circumference of the
2379 : // walkingarea. Use pos instead
2380 190 : const Position relPos = myWalkingAreaPath->shape.transformToVectorCoordinates(pos);
2381 : if (relPos == Position::INVALID) {
2382 0 : WRITE_WARNING("Could not map position " + toString(pos) + " onto lane '" + myLane->getID()
2383 : + "' (fromLane='" + myWalkingAreaPath->from->getID()
2384 : + "' toLane='" + myWalkingAreaPath->to->getID() + "') for person '" + getID() + "' time=" + time2string(t) + ".");
2385 0 : myRemoteXYPos = pos;
2386 : } else {
2387 190 : myEdgePos = relPos.x();
2388 190 : myPosLat = lateral_offset + relPos.y();
2389 : }
2390 : } else {
2391 10169 : myWalkingAreaPath = nullptr;
2392 10169 : myEdgePos = lanePos;
2393 10169 : myPosLat = lateral_offset - lanePosLat;
2394 10169 : lane->requireCollisionCheck();
2395 : }
2396 : // guess direction
2397 10359 : const double angleDiff = GeomHelper::getMinAngleDiff(angle, oldAngle);
2398 10359 : if (myStage->getNextRouteEdge() != nullptr) {
2399 6147 : if (myStage->getEdge()->getToJunction() == myStage->getNextRouteEdge()->getFromJunction() ||
2400 856 : myStage->getEdge()->getToJunction() == myStage->getNextRouteEdge()->getToJunction()) {
2401 4520 : myDir = FORWARD;
2402 : } else {
2403 771 : myDir = BACKWARD;
2404 : }
2405 : } else {
2406 : // guess from angle
2407 5068 : if (angleDiff <= 90) {
2408 : // keep direction
2409 5008 : if (myDir == UNDEFINED_DIRECTION) {
2410 0 : myDir = FORWARD;
2411 : }
2412 : } else {
2413 : // change direction
2414 60 : myDir = (myDir == BACKWARD) ? FORWARD : BACKWARD;
2415 : }
2416 : }
2417 : // update next lane info (after guessing direction)
2418 10359 : if (oldLane == nullptr || &oldLane->getEdge() != &myLane->getEdge()) {
2419 276 : const MSLane* sidewalk = getSidewalk<MSEdge, MSLane>(&myLane->getEdge(), p->getVClass());
2420 : // assume that we will eventually move back onto the sidewalk if
2421 : // there is one
2422 276 : myNLI = getNextLane(*this, sidewalk == nullptr ? myLane : sidewalk, nullptr);
2423 276 : myStage->activateEntryReminders(myPerson);
2424 : #ifdef DEBUG_MOVETOXY
2425 : std::cout << " myNLI=" << Named::getIDSecure(myNLI.lane) << " link=" << (myNLI.link == nullptr ? "NULL" : myNLI.link->getDescription()) << " dir=" << myNLI.dir << "\n";
2426 : #endif
2427 : }
2428 : #ifdef DEBUG_MOVETOXY
2429 : std::cout << " newRelPos=" << Position(myEdgePos, myPosLat) << " edge=" << myPerson->getEdge()->getID()
2430 : << " newPos=" << myPerson->getPosition()
2431 : << " latOffset=" << getLatOffset()
2432 : << " oldAngle=" << oldAngle << " angleDiff=" << angleDiff << " newDir=" << myDir << "\n";
2433 : #endif
2434 10359 : if (oldLane == myLane) {
2435 10051 : mySpeed = DIST2SPEED(fabs(oldX - myEdgePos));
2436 : } else {
2437 : //std::cout << SIMTIME << " oldX=" << oldX << " oldSpeed=" << mySpeed << " oldPos=" << oldPos << " pos=" << pos << " dist=" << oldPos.distanceTo2D(pos) << " oldLane=" << Named::getIDSecure(oldLane) << " lane=" << lane->getID() << "\n";
2438 308 : mySpeed = DIST2SPEED(oldPos.distanceTo2D(pos));
2439 : }
2440 : } else {
2441 : // map outside the network
2442 3136 : myRemoteXYPos = pos;
2443 3136 : mySpeed = DIST2SPEED(oldPos.distanceTo2D(pos));
2444 : }
2445 :
2446 13495 : }
2447 :
2448 :
2449 : double
2450 674039 : MSPModel_Striping::PState::getPathLength() const {
2451 674039 : if (myWalkingAreaPath != nullptr) {
2452 617238 : return myWalkingAreaPath->length;
2453 : } else {
2454 : return 0;
2455 : }
2456 : }
2457 :
2458 : double
2459 1160966870 : MSPModel_Striping::PState::distanceTo(const Obstacle& obs, const bool includeMinGap) const {
2460 : // check for overlap
2461 1160966870 : const double maxX = getMaxX(includeMinGap);
2462 1160966870 : const double minX = getMinX(includeMinGap);
2463 : //if (DEBUGCOND(*this)) {
2464 : // std::cout << std::setprecision(2) << " distanceTo=" << obs.description << " maxX=" << maxX << " minX=" << minX << " obs.xFwd=" << obs.xFwd << " obs.xBack=" << obs.xBack << "\n";
2465 : //}
2466 1160966870 : if ((obs.xFwd >= maxX && obs.xBack <= maxX) || (obs.xFwd <= maxX && obs.xFwd >= minX)) {
2467 : // avoid blocking by itself on looped route
2468 289202548 : return (obs.type == OBSTACLE_PED && obs.description == myPerson->getID()) ? DIST_FAR_AWAY : DIST_OVERLAP;
2469 : }
2470 871764322 : if (myDir == FORWARD) {
2471 760758637 : return obs.xFwd < minX ? DIST_BEHIND : obs.xBack - maxX;
2472 : } else {
2473 111005685 : return obs.xBack > maxX ? DIST_BEHIND : minX - obs.xFwd;
2474 : }
2475 : }
2476 :
2477 :
2478 : void
2479 64079476 : MSPModel_Striping::PState::mergeObstacles(Obstacles& into, const Obstacles& obs2) {
2480 291034382 : for (int i = 0; i < (int)into.size(); ++i) {
2481 226954906 : if (gDebugFlag1) {
2482 0 : std::cout << " i=" << i << " maxX=" << getMaxX(true) << " minX=" << getMinX(true)
2483 0 : << " into=" << into[i].description << " iDist=" << distanceTo(into[i], into[i].type == OBSTACLE_PED)
2484 0 : << " obs2=" << obs2[i].description << " oDist=" << distanceTo(obs2[i], obs2[i].type == OBSTACLE_PED) << "\n";
2485 : }
2486 226954906 : const double dO = distanceTo(obs2[i], obs2[i].type == OBSTACLE_PED);
2487 226954906 : const double dI = distanceTo(into[i], into[i].type == OBSTACLE_PED);
2488 226954906 : if (dO < dI) {
2489 : into[i] = obs2[i];
2490 : } else if (dO == dI
2491 61140205 : && into[i].type != OBSTACLE_PED
2492 33708314 : && into[i].type != OBSTACLE_VEHICLE
2493 223791979 : && (obs2[i].type == OBSTACLE_PED ||
2494 : obs2[i].type == OBSTACLE_VEHICLE)) {
2495 : into[i] = obs2[i];
2496 : }
2497 : }
2498 64079476 : }
2499 :
2500 : void
2501 18631 : MSPModel_Striping::PState::mergeObstacles(Obstacles& into, const Obstacles& obs2, int dir, int offset) {
2502 146381 : for (int i = 0; i < (int)into.size(); ++i) {
2503 127750 : int i2 = i + offset;
2504 127750 : if (i2 >= 0 && i2 < (int)obs2.size()) {
2505 126429 : if (dir == FORWARD) {
2506 90416 : if (obs2[i2].xBack < into[i].xBack) {
2507 : into[i] = obs2[i2];
2508 : }
2509 : } else {
2510 36013 : if (obs2[i2].xFwd > into[i].xFwd) {
2511 : into[i] = obs2[i2];
2512 : }
2513 : }
2514 : }
2515 : }
2516 18631 : }
2517 :
2518 :
2519 : bool
2520 9692252 : MSPModel_Striping::PState::ignoreRed(const MSLink* link) const {
2521 9692252 : if (link->haveRed()) {
2522 614205 : const double ignoreRedTime = myPerson->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_AFTER_RED_TIME, -1);
2523 614205 : if (ignoreRedTime >= 0) {
2524 40 : const double redDuration = STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - link->getLastStateChange());
2525 40 : if (DEBUGCOND(*this)) {
2526 0 : std::cout << SIMTIME << " ignoreRedTime=" << ignoreRedTime << " redDuration=" << redDuration << "\n";
2527 : }
2528 40 : return ignoreRedTime > redDuration;
2529 : } else {
2530 : return false;
2531 : }
2532 : } else {
2533 : return false;
2534 : }
2535 : }
2536 :
2537 :
2538 : bool
2539 9033055 : MSPModel_Striping::PState::stopForYellow(const MSLink* link) const {
2540 : // main use case is at rail_crossing
2541 9033055 : if (link->haveYellow()) {
2542 918 : const double ignoreYellowTime = myPerson->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_AFTER_YELLOW_TIME, -1);
2543 918 : if (ignoreYellowTime >= 0) {
2544 0 : const double yellowDuration = STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - link->getLastStateChange());
2545 0 : if (DEBUGCOND(*this)) {
2546 0 : std::cout << SIMTIME << " ignoreYellowTime=" << ignoreYellowTime << " yellowDuration=" << yellowDuration << "\n";
2547 : }
2548 0 : return ignoreYellowTime < yellowDuration;
2549 : } else {
2550 : return true;
2551 : }
2552 : } else {
2553 : return false;
2554 : }
2555 : }
2556 :
2557 : double
2558 2635690643 : MSPModel_Striping::PState::getWidth() const {
2559 2635690643 : return myPerson->getVehicleType().getWidth();
2560 : }
2561 :
2562 :
2563 : bool
2564 98314168 : MSPModel_Striping::PState::isRemoteControlled() const {
2565 98314168 : return myPerson->hasInfluencer() && myPerson->getInfluencer().isRemoteControlled();
2566 : }
2567 :
2568 : // ===========================================================================
2569 : // MSPModel_Striping::PStateVehicle method definitions
2570 : // ===========================================================================
2571 :
2572 504841 : MSPModel_Striping::PStateVehicle::PStateVehicle(const MSVehicle* veh, const MSLane* walkingarea, double relX, double relY, double xWidth, double yWidth):
2573 504841 : myVehicle(veh), myXWidth(xWidth), myYWidth(yWidth) {
2574 504841 : myLane = walkingarea; // to ensure correct limits when calling otherStripe()
2575 : // relX is the center but we want it to be the max value if the movement direction is forward
2576 : // and the min value otherwise (indicated by xWidth sign)
2577 504841 : myEdgePos = relX + xWidth / 2;
2578 504841 : myPosLat = relY;
2579 504841 : }
2580 :
2581 : const std::string&
2582 1970527 : MSPModel_Striping::PStateVehicle::getID() const {
2583 1970527 : return myVehicle->getID();
2584 : }
2585 :
2586 : double
2587 1018789 : MSPModel_Striping::PStateVehicle::getWidth() const {
2588 1018789 : return myYWidth;
2589 : }
2590 :
2591 : double
2592 539629 : MSPModel_Striping::PStateVehicle::getMinX(const bool /*includeMinGap*/) const {
2593 539629 : return myXWidth > 0 ? myEdgePos - myXWidth : myEdgePos;
2594 : }
2595 :
2596 : double
2597 539629 : MSPModel_Striping::PStateVehicle::getMaxX(const bool /*includeMinGap*/) const {
2598 539629 : return myXWidth > 0 ? myEdgePos : myEdgePos - myXWidth;
2599 : }
2600 :
2601 : // ===========================================================================
2602 : // MSPModel_Striping::MovePedestrians method definitions
2603 : // ===========================================================================
2604 :
2605 : SUMOTime
2606 3133905 : MSPModel_Striping::MovePedestrians::execute(SUMOTime currentTime) {
2607 : std::set<MSPerson*> changedLane;
2608 3133905 : myModel->moveInDirection(currentTime, changedLane, FORWARD);
2609 3133901 : myModel->moveInDirection(currentTime, changedLane, BACKWARD);
2610 : // DEBUG
2611 : #ifdef LOG_ALL
2612 : for (ActiveLanes::const_iterator it_lane = myModel->getActiveLanes().begin(); it_lane != myModel->getActiveLanes().end(); ++it_lane) {
2613 : const MSLane* lane = it_lane->first;
2614 : Pedestrians pedestrians = it_lane->second;
2615 : if (pedestrians.size() == 0) {
2616 : continue;
2617 : }
2618 : sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(FORWARD));
2619 : std::cout << SIMTIME << " lane=" << lane->getID();
2620 : for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
2621 : const PState& p = *pedestrians[ii];
2622 : std::cout << " (" << p.getPerson()->getID() << " " << p.myEdgePos << "," << p.myPosLat << " " << p.getDirection() << ")";
2623 : }
2624 : std::cout << "\n";
2625 : }
2626 : #endif
2627 3133901 : return DELTA_T;
2628 : }
|
