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 6083 : MSPModel_Striping::MSPModel_Striping(const OptionsCont& oc, MSNet* net) {
118 6083 : myWalkingAreaDetail = oc.getInt("pedestrian.striping.walkingarea-detail");
119 6083 : initWalkingAreaPaths(net);
120 : // configurable parameters
121 6083 : stripeWidth = oc.getFloat("pedestrian.striping.stripe-width");
122 6083 : MSVehicleType* defaultPedType = MSNet::getInstance()->getVehicleControl().getVType(DEFAULT_PEDTYPE_ID, nullptr, true);
123 6083 : 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 6083 : dawdling = oc.getFloat("pedestrian.striping.dawdling");
129 6083 : minGapToVehicle = oc.getFloat("pedestrian.striping.mingap-to-vehicle");
130 6083 : RESERVE_FOR_ONCOMING_FACTOR = oc.getFloat("pedestrian.striping.reserve-oncoming");
131 6083 : RESERVE_FOR_ONCOMING_FACTOR_JUNCTIONS = oc.getFloat("pedestrian.striping.reserve-oncoming.junctions");
132 6083 : 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 6083 : USE_NET_SPEEDS = net->getNetworkVersion() >= MMVersion(1, 20);
135 :
136 6083 : jamTime = string2time(oc.getString("pedestrian.striping.jamtime"));
137 6083 : if (jamTime <= 0) {
138 4 : jamTime = SUMOTime_MAX;
139 : }
140 6083 : jamTimeCrossing = string2time(oc.getString("pedestrian.striping.jamtime.crossing"));
141 6083 : if (jamTimeCrossing <= 0) {
142 0 : jamTimeCrossing = SUMOTime_MAX;
143 : }
144 6083 : jamTimeNarrow = string2time(oc.getString("pedestrian.striping.jamtime.narrow"));
145 6083 : if (jamTimeNarrow <= 0) {
146 0 : jamTimeNarrow = SUMOTime_MAX;
147 : }
148 6083 : jamFactor = oc.getFloat("pedestrian.striping.jamfactor");
149 6083 : myLegacyPosLat = oc.getBool("pedestrian.striping.legacy-departposlat");
150 6083 : }
151 :
152 :
153 12146 : MSPModel_Striping::~MSPModel_Striping() {
154 : myWalkingAreaPaths.clear(); // need to recompute when lane pointers change
155 : myWalkingAreaFoes.clear();
156 : myMinNextLengths.clear();
157 12146 : }
158 :
159 :
160 : MSTransportableStateAdapter*
161 243922 : MSPModel_Striping::add(MSTransportable* transportable, MSStageMoving* stage, SUMOTime) {
162 243922 : 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 243922 : MSNet* net = MSNet::getInstance();
168 243922 : if (!myAmActive) {
169 4277 : net->getBeginOfTimestepEvents()->addEvent(new MovePedestrians(this), net->getCurrentTimeStep() + DELTA_T);
170 4277 : myAmActive = true;
171 : }
172 : assert(person->getCurrentStageType() == MSStageType::WALKING);
173 243922 : const MSLane* lane = stage->checkDepartLane(person->getEdge(), person->getVClass(), stage->getDepartLane(), person->getID());
174 243922 : 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 243914 : PState* ped = new PState(person, stage, lane);
184 243910 : myActiveLanes[lane].push_back(ped);
185 243910 : myNumActivePedestrians++;
186 243910 : return ped;
187 : }
188 :
189 :
190 : MSTransportableStateAdapter*
191 14 : MSPModel_Striping::loadState(MSTransportable* transportable, MSStageMoving* stage, std::istringstream& in) {
192 : MSPerson* person = static_cast<MSPerson*>(transportable);
193 14 : MSNet* net = MSNet::getInstance();
194 14 : if (!myAmActive) {
195 12 : net->getBeginOfTimestepEvents()->addEvent(new MovePedestrians(this), SIMSTEP);
196 12 : myAmActive = true;
197 : }
198 14 : PState* ped = new PState(person, stage, &in);
199 14 : myActiveLanes[ped->getLane()].push_back(ped);
200 14 : myNumActivePedestrians++;
201 14 : return ped;
202 : }
203 :
204 :
205 : int
206 8379379178 : MSPModel_Striping::numStripes(const MSLane* lane) {
207 8379379178 : return MAX2(1, (int)floor(lane->getWidth() / stripeWidth));
208 : }
209 :
210 :
211 : int
212 849589 : MSPModel_Striping::connectedDirection(const MSLane* from, const MSLane* to) {
213 849589 : if (from == nullptr || to == nullptr) {
214 0 : return UNDEFINED_DIRECTION;
215 849589 : } else if (from->getLinkTo(to) != nullptr) {
216 602108 : return FORWARD;
217 247481 : } else if (to->getLinkTo(from) != nullptr) {
218 247481 : return BACKWARD;
219 : } else {
220 0 : return UNDEFINED_DIRECTION;
221 : }
222 : }
223 :
224 :
225 : void
226 6083 : MSPModel_Striping::initWalkingAreaPaths(const MSNet*) {
227 6083 : if (myWalkingAreaPaths.size() > 0) {
228 : return;
229 : }
230 : // collect vehicle lanes that cross walkingareas
231 421279 : for (MSEdgeVector::const_iterator i = MSEdge::getAllEdges().begin(); i != MSEdge::getAllEdges().end(); ++i) {
232 415197 : const MSEdge* edge = *i;
233 415197 : if (!edge->isWalkingArea() && !edge->isCrossing()) {
234 768681 : for (MSLane* lane : edge->getLanes()) {
235 941025 : for (MSLink* link : lane->getLinkCont()) {
236 519040 : 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 519040 : if (link->getWalkingAreaFoeExit() != nullptr) {
242 : // link is an exit link
243 3022 : 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 421279 : for (MSEdgeVector::const_iterator i = MSEdge::getAllEdges().begin(); i != MSEdge::getAllEdges().end(); ++i) {
253 415197 : insertWalkArePaths(*i, myWalkingAreaPaths);
254 : }
255 : }
256 :
257 :
258 : void
259 415197 : MSPModel_Striping::insertWalkArePaths(const MSEdge* edge, WalkingAreaPaths& into) {
260 415197 : if (edge->isWalkingArea()) {
261 60045 : const MSLane* walkingArea = getSidewalk<MSEdge, MSLane>(edge);
262 60045 : myMinNextLengths[walkingArea] = walkingArea->getLength();
263 : // build all possible paths across this walkingArea
264 : // gather all incident lanes
265 : std::vector<const MSLane*> lanes;
266 133626 : for (const MSEdge* in : edge->getPredecessors()) {
267 73581 : if (!in->isTazConnector()) {
268 69235 : lanes.push_back(getSidewalk<MSEdge, MSLane>(in));
269 69235 : if (lanes.back() == nullptr) {
270 0 : throw ProcessError("Invalid connection from edge '" + in->getID() + "' to walkingarea edge '" + edge->getID() + "'");
271 : }
272 : }
273 : }
274 133812 : for (const MSEdge* out : edge->getSuccessors()) {
275 73767 : if (!out->isTazConnector()) {
276 69421 : lanes.push_back(getSidewalk<MSEdge, MSLane>(out));
277 69421 : 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 198701 : for (int j = 0; j < (int)lanes.size(); ++j) {
284 485516 : for (int k = 0; k < (int)lanes.size(); ++k) {
285 346860 : if (j != k) {
286 : // build the walkingArea
287 208204 : const MSLane* const from = lanes[j];
288 208204 : const MSLane* const to = lanes[k];
289 208204 : const int fromDir = from->getLinkTo(walkingArea) != nullptr ? FORWARD : BACKWARD;
290 208204 : const int toDir = walkingArea->getLinkTo(to) != nullptr ? FORWARD : BACKWARD;
291 208204 : PositionVector shape;
292 312563 : Position fromPos = from->getShape()[fromDir == FORWARD ? -1 : 0];
293 312049 : Position toPos = to->getShape()[toDir == FORWARD ? 0 : -1];
294 208204 : const double maxExtent = fromPos.distanceTo2D(toPos) / 4; // prevent sharp corners
295 208204 : const double extrapolateBy = MIN2(maxExtent, walkingArea->getWidth() / 2);
296 : // assemble shape
297 208204 : shape.push_back(fromPos);
298 208204 : if (extrapolateBy > POSITION_EPS) {
299 : PositionVector fromShp = from->getShape();
300 207194 : fromShp.extrapolate(extrapolateBy);
301 414388 : shape.push_back_noDoublePos(fromDir == FORWARD ? fromShp.back() : fromShp.front());
302 : PositionVector nextShp = to->getShape();
303 207194 : nextShp.extrapolate(extrapolateBy);
304 414388 : shape.push_back_noDoublePos(toDir == FORWARD ? nextShp.front() : nextShp.back());
305 207194 : }
306 208204 : shape.push_back_noDoublePos(toPos);
307 208204 : if (shape.size() < 2) {
308 : PositionVector fromShp = from->getShape();
309 554 : fromShp.extrapolate(1.5 * POSITION_EPS); // noDoublePos requires a difference of POSITION_EPS in at least one coordinate
310 1108 : shape.push_back_noDoublePos(fromDir == FORWARD ? fromShp.back() : fromShp.front());
311 : assert(shape.size() == 2);
312 208204 : } else if (myWalkingAreaDetail > 4) {
313 448 : shape = shape.bezier(myWalkingAreaDetail);
314 : }
315 : double angleOverride = INVALID_DOUBLE;
316 208204 : if (shape.size() >= 4 && shape.length() < walkingArea->getWidth()) {
317 53894 : const double aStart = shape.angleAt2D(0);
318 53894 : const double aEnd = shape.angleAt2D((int)shape.size() - 2);
319 53894 : if (fabs(aStart - aEnd) < DEG2RAD(10)) {
320 9032 : angleOverride = (aStart + aEnd) / 2;
321 : }
322 : }
323 208204 : if (fromDir == BACKWARD) {
324 : // will be walking backward on walkingArea
325 208718 : shape = shape.reverse();
326 : }
327 208204 : WalkingAreaPath wap = WalkingAreaPath(from, walkingArea, to, shape, fromDir, angleOverride);
328 208204 : into.insert(std::make_pair(std::make_pair(from, to), wap));
329 241311 : myMinNextLengths[walkingArea] = MIN2(myMinNextLengths[walkingArea], wap.length);
330 208204 : }
331 : }
332 : }
333 60045 : }
334 415197 : }
335 :
336 :
337 : const MSPModel_Striping::WalkingAreaPath*
338 6 : MSPModel_Striping::getArbitraryPath(const MSEdge* walkingArea) {
339 : assert(walkingArea->isWalkingArea());
340 : std::vector<const MSLane*> lanes;
341 18 : for (const MSEdge* const pred : walkingArea->getPredecessors()) {
342 12 : lanes.push_back(getSidewalk<MSEdge, MSLane>(pred));
343 : }
344 18 : for (const MSEdge* const succ : walkingArea->getSuccessors()) {
345 12 : lanes.push_back(getSidewalk<MSEdge, MSLane>(succ));
346 : }
347 6 : if (lanes.size() < 1) {
348 0 : throw ProcessError(TLF("Invalid walkingarea '%' does not allow continuation.", walkingArea->getID()));
349 : }
350 6 : return &myWalkingAreaPaths.find(std::make_pair(lanes.front(), lanes.back()))->second;
351 6 : }
352 :
353 : const MSPModel_Striping::WalkingAreaPath*
354 61 : MSPModel_Striping::guessPath(const MSEdge* walkingArea, const MSEdge* before, const MSEdge* after) {
355 : assert(walkingArea->isWalkingArea());
356 61 : const MSLane* swBefore = getSidewalk<MSEdge, MSLane>(before);
357 61 : const MSLane* swAfter = getSidewalk<MSEdge, MSLane>(after);
358 61 : const auto pathIt = myWalkingAreaPaths.find(std::make_pair(swBefore, swAfter));
359 61 : if (pathIt != myWalkingAreaPaths.end()) {
360 24 : return &pathIt->second;
361 : }
362 : const MSEdgeVector& preds = walkingArea->getPredecessors();
363 37 : const MSEdgeVector& succs = walkingArea->getSuccessors();
364 37 : bool useBefore = swBefore != nullptr && std::find(preds.begin(), preds.end(), before) != preds.end();
365 37 : bool useAfter = swAfter != nullptr && std::find(succs.begin(), succs.end(), after) != succs.end();
366 37 : if (useBefore) {
367 29 : if (useAfter) {
368 0 : return getWalkingAreaPath(walkingArea, swBefore, swAfter);
369 29 : } else if (succs.size() > 0) {
370 : // could also try to exploit direction
371 29 : return getWalkingAreaPath(walkingArea, swBefore, getSidewalk<MSEdge, MSLane>(succs.front()));
372 : }
373 8 : } else if (useAfter && preds.size() > 0) {
374 : // could also try to exploit direction
375 8 : return getWalkingAreaPath(walkingArea, getSidewalk<MSEdge, MSLane>(preds.front()), swAfter);
376 : }
377 0 : return getArbitraryPath(walkingArea);
378 : }
379 :
380 :
381 : const MSPModel_Striping::WalkingAreaPath*
382 834858 : MSPModel_Striping::getWalkingAreaPath(const MSEdge* walkingArea, const MSLane* before, const MSLane* after) {
383 : assert(walkingArea->isWalkingArea());
384 834858 : const auto pathIt = myWalkingAreaPaths.find(std::make_pair(before, after));
385 834858 : if (pathIt != myWalkingAreaPaths.end()) {
386 834818 : return &pathIt->second;
387 : } else {
388 : // this can happen in case of moveToXY where before can point anywhere
389 : const MSEdgeVector& preds = walkingArea->getPredecessors();
390 40 : if (preds.size() > 0) {
391 40 : const MSEdge* const pred = walkingArea->getPredecessors().front();
392 40 : const auto pathIt2 = myWalkingAreaPaths.find(std::make_pair(getSidewalk<MSEdge, MSLane>(pred), after));
393 : assert(pathIt2 != myWalkingAreaPaths.end());
394 40 : return &pathIt2->second;
395 : } else {
396 0 : return getArbitraryPath(walkingArea);
397 : }
398 : }
399 : }
400 :
401 :
402 :
403 : MSPModel_Striping::NextLaneInfo
404 1503905 : MSPModel_Striping::getNextLane(const PState& ped, const MSLane* currentLane, const MSLane* prevLane) {
405 : const MSEdge* currentEdge = ¤tLane->getEdge();
406 1503905 : const MSJunction* junction = ped.getDirection() == FORWARD ? currentEdge->getToJunction() : currentEdge->getFromJunction();
407 1503905 : const MSEdge* nextRouteEdge = ped.getStage()->getNextRouteEdge();
408 1503905 : const MSLane* nextRouteLane = getSidewalk<MSEdge, MSLane>(nextRouteEdge, ped.getPerson()->getVClass());
409 : // result values
410 : const MSLane* nextLane = nextRouteLane;
411 1503905 : const MSLink* link = nullptr;
412 1503905 : int nextDir = UNDEFINED_DIRECTION;
413 :
414 : //if DEBUGCOND(ped) {
415 : // std::cout << " nextRouteLane=" << Named::getIDSecure(nextRouteLane) << " junction=" << junction->getID() << "\n";
416 : //}
417 1503905 : if (nextRouteLane == nullptr && nextRouteEdge != nullptr) {
418 56 : std::string error = "Person '" + ped.getPerson()->getID() + "' could not find sidewalk on edge '" + nextRouteEdge->getID() + "', time="
419 56 : + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".";
420 56 : if (OptionsCont::getOptions().getBool("ignore-route-errors")) {
421 76 : WRITE_WARNING(error);
422 24 : nextRouteLane = nextRouteEdge->getLanes().front();
423 : } else {
424 4 : throw ProcessError(error);
425 : }
426 : }
427 :
428 1503901 : if (nextRouteLane != nullptr) {
429 1271681 : if (currentEdge->isInternal()) {
430 : assert(junction == currentEdge->getFromJunction());
431 23638 : nextDir = junction == nextRouteEdge->getFromJunction() ? FORWARD : BACKWARD;
432 23638 : if (nextDir == FORWARD) {
433 22663 : nextLane = currentLane->getLinkCont()[0]->getViaLaneOrLane();
434 : } else {
435 975 : nextLane = currentLane->getLogicalPredecessorLane();
436 : }
437 23638 : if DEBUGCOND(ped) {
438 0 : std::cout << " internal\n";
439 : }
440 1248043 : } else if (currentEdge->isCrossing()) {
441 33163 : nextDir = ped.getDirection();
442 33163 : if (nextDir == FORWARD) {
443 16848 : nextLane = currentLane->getLinkCont()[0]->getLane();
444 : } else {
445 16315 : nextLane = currentLane->getLogicalPredecessorLane();
446 : }
447 33163 : if DEBUGCOND(ped) {
448 0 : std::cout << " crossing\n";
449 : }
450 33163 : if (currentLane->isPriorityCrossing()) {
451 31130 : unregisterCrossingApproach(ped, currentLane);
452 : }
453 1214880 : } else if (currentEdge->isWalkingArea()) {
454 : ConstMSEdgeVector crossingRoute;
455 : // departPos can be 0 because the direction of the walkingArea does not matter
456 : // for the arrivalPos, we need to make sure that the route does not deviate across other junctions
457 849626 : const int nextRouteEdgeDir = nextRouteEdge->getFromJunction() == junction ? FORWARD : BACKWARD;
458 849626 : const double arrivalPos = (nextRouteEdge == ped.getStage()->getRoute().back()
459 849626 : ? ped.getStage()->getArrivalPos()
460 204835 : : (nextRouteEdgeDir == FORWARD ? 0 : nextRouteEdge->getLength()));
461 : SUMOAbstractRouter<MSEdge, SUMOVehicle>::Prohibitions prohibited;
462 849626 : if (prevLane != nullptr) {
463 849568 : prohibited[&prevLane->getEdge()].end = std::numeric_limits<double>::max();
464 : }
465 1699252 : MSNet::getInstance()->getPedestrianRouter(0, prohibited).compute(currentEdge, nextRouteEdge, 0, arrivalPos,
466 849626 : ped.getStage()->getMaxSpeed(ped.getPerson()),
467 849626 : 0, junction, ped.getPerson()->getVTypeParameter(), crossingRoute, true);
468 849626 : if DEBUGCOND(ped) {
469 : std::cout
470 : << " nre=" << nextRouteEdge->getID()
471 : << " nreDir=" << nextRouteEdgeDir
472 0 : << " aPos=" << arrivalPos
473 0 : << " crossingRoute=" << toString(crossingRoute)
474 0 : << "\n";
475 : }
476 849626 : if (crossingRoute.size() > 1) {
477 849589 : const MSEdge* nextEdge = crossingRoute[1];
478 849589 : nextLane = getSidewalk<MSEdge, MSLane>(crossingRoute[1], ped.getPerson()->getVClass());
479 : assert((nextEdge->getFromJunction() == junction || nextEdge->getToJunction() == junction));
480 : assert(nextLane != prevLane);
481 849589 : nextDir = connectedDirection(currentLane, nextLane);
482 849589 : if DEBUGCOND(ped) {
483 0 : std::cout << " nextDir=" << nextDir << "\n";
484 : }
485 : assert(nextDir != UNDEFINED_DIRECTION);
486 849589 : if (nextDir == FORWARD) {
487 602108 : link = currentLane->getLinkTo(nextLane);
488 : } else {
489 247481 : link = nextLane->getLinkTo(currentLane);
490 247481 : if (nextEdge->isCrossing() && link->getTLLogic() == nullptr) {
491 195219 : const MSLane* oppositeWalkingArea = nextLane->getLogicalPredecessorLane();
492 195219 : link = oppositeWalkingArea->getLinkTo(nextLane);
493 : }
494 : }
495 : assert(link != nullptr);
496 849589 : if (nextLane->isPriorityCrossing()) {
497 488017 : registerCrossingApproach(ped, nextLane, prevLane);
498 : }
499 : } else {
500 37 : if DEBUGCOND(ped) {
501 0 : std::cout << SIMTIME
502 0 : << " no route from '" << (currentEdge == nullptr ? "NULL" : currentEdge->getID())
503 0 : << "' to '" << (nextRouteEdge == nullptr ? "NULL" : nextRouteEdge->getID())
504 0 : << "\n";
505 : }
506 : // check if a direct connection exists (moving onto the walkingarea was the wrong choice)
507 37 : if (ped.getDirection() == FORWARD) {
508 24 : link = prevLane->getLinkTo(nextRouteLane);
509 : } else {
510 13 : link = nextRouteLane->getLinkTo(prevLane);
511 : }
512 37 : if (link != nullptr) {
513 : // leave direction as UNDEFINED_DIRECTION to signal that currentLane must be changed
514 : nextLane = link->getViaLaneOrLane();
515 : } else {
516 80 : WRITE_WARNING("Person '" + ped.getPerson()->getID() + "' could not find route across junction '" + junction->getID()
517 : + "' from walkingArea '" + currentEdge->getID()
518 : + "' to edge '" + nextRouteEdge->getID() + "', time=" +
519 : time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
520 : // error indicated by nextDir == UNDEFINED_DIRECTION
521 : nextLane = nextRouteLane;
522 : }
523 : }
524 1214880 : } else if (currentEdge == nextRouteEdge) {
525 : // strange loop in this route. No need to use walkingArea
526 8 : nextDir = -ped.getDirection();
527 : } else {
528 : // normal edge. by default use next / previous walking area
529 365246 : nextDir = ped.getDirection();
530 365246 : nextLane = getNextWalkingArea(currentLane, ped.getDirection(), link);
531 365246 : if (nextLane != nullptr) {
532 : // walking area found
533 326457 : if DEBUGCOND(ped) {
534 0 : std::cout << " next walkingArea " << (nextDir == FORWARD ? "forward" : "backward") << "\n";
535 : }
536 : } else {
537 : // walk forward by default
538 38789 : if (junction == nextRouteEdge->getToJunction()) {
539 3949 : nextDir = BACKWARD;
540 34840 : } else if (junction == nextRouteEdge->getFromJunction()) {
541 : nextDir = FORWARD;
542 : } else {
543 : // topological disconnect, find a direction that makes sense
544 : // for the future part of the route
545 212 : ConstMSEdgeVector futureRoute = ped.getStage()->getRoute();
546 212 : futureRoute.erase(futureRoute.begin(), futureRoute.begin() + ped.getStage()->getRoutePosition() + 1);
547 212 : int passedFwd = 0;
548 212 : int passedBwd = 0;
549 212 : canTraverse(FORWARD, futureRoute, passedFwd);
550 212 : canTraverse(BACKWARD, futureRoute, passedBwd);
551 212 : nextDir = (passedFwd >= passedBwd) ? FORWARD : BACKWARD;
552 212 : if DEBUGCOND(ped) {
553 0 : std::cout << " nextEdge=" << nextRouteEdge->getID() << " passedFwd=" << passedFwd << " passedBwd=" << passedBwd << " futureRoute=" << toString(futureRoute) << " nextDir=" << nextDir << "\n";
554 : }
555 212 : }
556 : // try to use a direct link as fallback
557 : // direct links only exist if built explicitly. They are used to model tl-controlled links if there are no crossings
558 38789 : if (ped.getDirection() == FORWARD) {
559 34315 : link = currentLane->getLinkTo(nextRouteLane);
560 34315 : if (link != nullptr) {
561 32640 : if DEBUGCOND(ped) {
562 0 : std::cout << " direct forward\n";
563 : }
564 32640 : nextLane = currentLane->getInternalFollowingLane(nextRouteLane);
565 : }
566 : } else {
567 4474 : link = nextRouteLane->getLinkTo(currentLane);
568 4474 : if (link != nullptr) {
569 3183 : if DEBUGCOND(ped) {
570 0 : std::cout << " direct backward\n";
571 : }
572 3183 : nextLane = nextRouteLane->getInternalFollowingLane(currentLane);
573 3183 : if (nextLane != nullptr) {
574 : // advance to the end of consecutive internal lanes
575 5317 : while (nextLane->getLinkCont()[0]->getViaLaneOrLane()->isInternal()) {
576 5 : nextLane = nextLane->getLinkCont()[0]->getViaLaneOrLane();
577 : }
578 : }
579 : }
580 : }
581 : }
582 364719 : if (nextLane == nullptr) {
583 : // no internal lane found
584 : nextLane = nextRouteLane;
585 8804 : if DEBUGCOND(ped) {
586 0 : std::cout << SIMTIME << " no next lane found for " << currentLane->getID() << " dir=" << ped.getDirection() << "\n";
587 : }
588 8804 : if (usingInternalLanesStatic() && currentLane->getLinkCont().size() > 0) {
589 0 : WRITE_WARNING("Person '" + ped.getPerson()->getID() + "' could not find route across junction '" + junction->getID()
590 : + "' from edge '" + currentEdge->getID()
591 : + "' to edge '" + nextRouteEdge->getID() + "', time=" +
592 : time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
593 : }
594 356442 : } else if (nextLane->getLength() <= POSITION_EPS) {
595 : // internal lane too short
596 : // most often this is due to a zero-size junction. However, if
597 : // the person needs to pass a crossing we cannot skip ahead
598 10955 : if ((nextLane->getCanonicalSuccessorLane() == nullptr
599 10947 : || !nextLane->getCanonicalSuccessorLane()->isCrossing())
600 32845 : && (nextLane->getLogicalPredecessorLane() == nullptr ||
601 10943 : !nextLane->getLogicalPredecessorLane()->isCrossing())) {
602 : //WRITE_WARNING("Person '" + ped.getID()
603 : // + "' skips short lane '" + nextLane->getID()
604 : // + "' length=" + toString(nextLane->getLength())
605 : // + " time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
606 : nextLane = nextRouteLane;
607 10955 : nextDir = nextRouteEdge->getFromJunction() == junction ? FORWARD : BACKWARD;
608 : }
609 : }
610 : }
611 : }
612 1503901 : if DEBUGCOND(ped) {
613 0 : std::cout << SIMTIME
614 : << " p=" << ped.getPerson()->getID()
615 : << " l=" << currentLane->getID()
616 0 : << " nl=" << (nextLane == nullptr ? "NULL" : nextLane->getID())
617 0 : << " nrl=" << (nextRouteLane == nullptr ? "NULL" : nextRouteLane->getID())
618 : << " d=" << nextDir
619 0 : << " link=" << (link == nullptr ? "NULL" : link->getViaLaneOrLane()->getID())
620 0 : << " pedDir=" << ped.getDirection()
621 0 : << "\n";
622 : }
623 : assert(nextLane != 0 || nextRouteLane == 0);
624 1503901 : return NextLaneInfo(nextLane, link, nextDir);
625 : }
626 :
627 :
628 : const MSLane*
629 365246 : MSPModel_Striping::getNextWalkingArea(const MSLane* currentLane, const int dir, const MSLink*& link) {
630 365246 : if (dir == FORWARD) {
631 365057 : for (const MSLink* const l : currentLane->getLinkCont()) {
632 330742 : if (l->getLane()->isWalkingArea()) {
633 270035 : link = l;
634 : return l->getLane();
635 : }
636 : }
637 : } else {
638 : const std::vector<MSLane::IncomingLaneInfo>& laneInfos = currentLane->getIncomingLanes();
639 67538 : for (std::vector<MSLane::IncomingLaneInfo>::const_iterator it = laneInfos.begin(); it != laneInfos.end(); ++it) {
640 63064 : if ((*it).lane->isWalkingArea()) {
641 56422 : link = (*it).viaLink;
642 56422 : return (*it).lane;
643 : }
644 : }
645 : }
646 : return nullptr;
647 : }
648 :
649 :
650 : MSPModel_Striping::Obstacles
651 71106446 : MSPModel_Striping::getNeighboringObstacles(const Pedestrians& pedestrians, int egoIndex, int stripes) {
652 71106446 : const PState& ego = *static_cast<PState*>(pedestrians[egoIndex]);
653 71106446 : const int egoStripe = ego.stripe();
654 71106446 : Obstacles obs(stripes, Obstacle(ego.getDirection()));
655 71106446 : std::vector<bool> haveBlocker(stripes, false);
656 10795040390 : for (int index = egoIndex + 1; index < (int)pedestrians.size(); index++) {
657 10780633723 : const PState& p = *static_cast<PState*>(pedestrians[index]);
658 10780633723 : if DEBUGCOND(ego) {
659 0 : std::cout << SIMTIME << " ped=" << ego.getID() << " cur=" << egoStripe << " checking neighbor " << p.getID()
660 0 : << " nCur=" << p.stripe() << " nOth=" << p.otherStripe();
661 : }
662 10780633723 : if (!p.isWaitingToEnter() && !p.isJammed()) {
663 147680856 : const Obstacle o(p);
664 147680856 : if DEBUGCOND(ego) {
665 0 : std::cout << " dist=" << ego.distanceTo(o) << std::endl;
666 : }
667 147680856 : if (ego.distanceTo(o) == DIST_BEHIND) {
668 : break;
669 : }
670 90981077 : if (ego.distanceTo(o) == DIST_OVERLAP) {
671 90981077 : if (p.stripe() != egoStripe || p.getDirection() != ego.getDirection()) {
672 67485445 : obs[p.stripe()] = o;
673 67485445 : haveBlocker[p.stripe()] = true;
674 : } else {
675 : //std::cout << SIMTIME << " ignoring overlap between " << ego.getID() << " and " << p.getID() << " on stripe=" << egoStripe << "\n";
676 : }
677 90981077 : if (p.otherStripe() != egoStripe || p.getDirection() != ego.getDirection()) {
678 67485445 : obs[p.otherStripe()] = o;
679 67485445 : haveBlocker[p.otherStripe()] = true;
680 : } else {
681 : //std::cout << SIMTIME << " ignoring overlap between " << ego.getID() << " and " << p.getID() << " on stripe2=" << egoStripe << "\n";
682 : }
683 : } else {
684 0 : if (!haveBlocker[p.stripe()]) {
685 0 : obs[p.stripe()] = o;
686 : }
687 0 : if (!haveBlocker[p.otherStripe()]) {
688 0 : obs[p.otherStripe()] = o;
689 : }
690 : }
691 : }
692 : }
693 71106446 : if DEBUGCOND(ego) {
694 0 : std::cout << SIMTIME << " ped=" << ego.getPerson()->getID() << " neighObs=";
695 0 : DEBUG_PRINT(obs);
696 : }
697 71106446 : return obs;
698 0 : }
699 :
700 :
701 : int
702 3109180 : MSPModel_Striping::getStripeOffset(int origStripes, int destStripes, bool addRemainder) {
703 3109180 : int offset = (destStripes - origStripes) / 2;
704 3109180 : if (addRemainder) {
705 13342 : offset += (destStripes - origStripes) % 2;
706 : }
707 3109180 : return offset;
708 : }
709 :
710 :
711 : const MSPModel_Striping::Obstacles&
712 18404794 : MSPModel_Striping::getNextLaneObstacles(NextLanesObstacles& nextLanesObs, const
713 : MSLane* lane, const MSLane* nextLane, int stripes, int nextDir,
714 : double currentLength, int currentDir) {
715 : if (nextLanesObs.count(nextLane) == 0) {
716 2352263 : const double nextLength = nextLane->isWalkingArea() ? myMinNextLengths[nextLane] : nextLane->getLength();
717 : // figure out the which pedestrians are ahead on the next lane
718 2352263 : const int nextStripes = numStripes(nextLane);
719 : // do not move past the end of the next lane in a single step
720 4704526 : Obstacles obs(stripes, Obstacle(nextDir == FORWARD ? nextLength : 0, 0, OBSTACLE_NEXTEND, "nextEnd", 0));
721 :
722 2352263 : const int offset = getStripeOffset(nextStripes, stripes, currentDir != nextDir && nextStripes > stripes);
723 : //std::cout << SIMTIME << " getNextLaneObstacles"
724 : // << " nextLane=" << nextLane->getID()
725 : // << " nextLength=" << nextLength
726 : // << " nextDir=" << nextDir
727 : // << " currentLength=" << currentLength
728 : // << " currentDir=" << currentDir
729 : // << " stripes=" << stripes
730 : // << " nextStripes=" << nextStripes
731 : // << " offset=" << offset
732 : // << "\n";
733 2352263 : if (nextStripes < stripes) {
734 : // some stripes do not continue
735 2041323 : for (int ii = 0; ii < stripes; ++ii) {
736 1761870 : if (ii < offset || ii >= nextStripes + offset) {
737 2418408 : obs[ii] = Obstacle(nextDir == FORWARD ? 0 : nextLength, 0, OBSTACLE_END, "stripeEnd", 0);
738 : }
739 : }
740 : }
741 2352263 : Pedestrians& pedestrians = getPedestrians(nextLane);
742 2352263 : if (nextLane->isWalkingArea()) {
743 1211080 : transformToCurrentLanePositions(obs, currentDir, nextDir, currentLength, nextLength);
744 : // complex transformation into the coordinate system of the current lane
745 : // (pedestrians on next lane may walk at arbitrary angles relative to the current lane)
746 1211080 : double lateral_offset = (lane->getWidth() - stripeWidth) * 0.5;
747 1211080 : if ((stripes - nextStripes) % 2 != 0) {
748 457699 : lateral_offset += 0.5 * stripeWidth;
749 : }
750 : nextDir = currentDir;
751 : // transform pedestrians into the current coordinate system
752 9051774 : for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
753 7840694 : const PState& p = *static_cast<PState*>(pedestrians[ii]);
754 7840694 : if (p.isWaitingToEnter() || p.isJammed()) {
755 2663777 : continue;
756 : }
757 5176917 : Position pPos = p.getPosition(*p.getStage(), -1);
758 5176917 : Position relPos = lane->getShape().transformToVectorCoordinates(pPos, true);
759 : if (relPos == Position::INVALID) {
760 0 : WRITE_WARNINGF("Could not map position % onto lane '%'", pPos, lane->getID());
761 : }
762 5176917 : const double newY = relPos.y() + lateral_offset;
763 : //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";
764 5176917 : if ((currentDir == FORWARD && relPos.x() >= lane->getLength()) || (currentDir == BACKWARD && relPos.x() < 0)) {
765 5175474 : addCloserObstacle(obs, relPos.x(), p.stripe(newY), stripes, p.getPerson()->getID(), p.getPerson()->getVehicleType().getWidth(), currentDir, OBSTACLE_PED);
766 5175474 : addCloserObstacle(obs, relPos.x(), p.otherStripe(newY), stripes, p.getPerson()->getID(), p.getPerson()->getVehicleType().getWidth(), currentDir, OBSTACLE_PED);
767 : }
768 : }
769 : } else {
770 : // simple transformation into the coordinate system of the current lane
771 : // (only need to worry about currentDir and nextDir)
772 : // XXX consider waitingToEnter on nextLane
773 1141183 : sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(nextDir));
774 35652142 : for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
775 34510959 : const PState& p = *static_cast<PState*>(pedestrians[ii]);
776 34510959 : if (p.isWaitingToEnter() || p.isJammed()) {
777 22968988 : continue;
778 : }
779 : double newY = p.getPosLat();
780 11541971 : Obstacle pObs(p);
781 11541971 : if (nextDir != currentDir) {
782 290461 : newY = (nextStripes - 1) * stripeWidth - newY;
783 290461 : pObs.speed *= -1;
784 : }
785 11541971 : newY += offset * stripeWidth;
786 11541971 : const int stripe = p.stripe(newY);
787 11541971 : if (stripe >= 0 && stripe < stripes) {
788 11541968 : obs[stripe] = pObs;
789 : }
790 11541971 : const int otherStripe = p.otherStripe(newY);
791 11541971 : if (otherStripe >= 0 && otherStripe < stripes) {
792 11541968 : obs[otherStripe] = pObs;
793 : }
794 : }
795 1141183 : if (nextLane->isCrossing()) {
796 : // add vehicle obstacles
797 329916 : const MSLink* crossingEntryLink = nextLane->getIncomingLanes().front().viaLink;
798 329916 : const bool prio = crossingEntryLink->havePriority() || crossingEntryLink->getTLLogic() != nullptr;
799 329916 : addCrossingVehs(nextLane, stripes, offset, nextDir, obs, prio, currentDir != nextDir);
800 : }
801 1141183 : if (nextLane->getVehicleNumberWithPartials() > 0) {
802 18641 : Obstacles vehObs = getVehicleObstacles(nextLane, nextDir);
803 18641 : PState::mergeObstacles(obs, vehObs, nextDir, offset);
804 18641 : }
805 1141183 : transformToCurrentLanePositions(obs, currentDir, nextDir, currentLength, nextLength);
806 : }
807 2352263 : nextLanesObs[nextLane] = obs;
808 2352263 : }
809 18404794 : return nextLanesObs[nextLane];
810 : }
811 :
812 : void
813 2352263 : MSPModel_Striping::transformToCurrentLanePositions(Obstacles& obs, int currentDir, int nextDir, double currentLength, double nextLength) {
814 13323054 : for (Obstacle& o : obs) {
815 10970791 : if (currentDir == FORWARD) {
816 7279730 : if (nextDir == FORWARD) {
817 6932733 : o.xFwd += currentLength;
818 6932733 : o.xBack += currentLength;
819 : } else {
820 346997 : const double tmp = o.xFwd;
821 346997 : o.xFwd = currentLength + nextLength - o.xBack;
822 346997 : o.xBack = currentLength + nextLength - tmp;
823 : }
824 : } else {
825 3691061 : if (nextDir == FORWARD) {
826 851818 : const double tmp = o.xFwd;
827 851818 : o.xFwd = -o.xBack;
828 851818 : o.xBack = -tmp;
829 : } else {
830 2839243 : o.xFwd -= nextLength;
831 2839243 : o.xBack -= nextLength;
832 : }
833 : }
834 : }
835 2352263 : }
836 :
837 :
838 : void
839 10350948 : MSPModel_Striping::addCloserObstacle(Obstacles& obs, double x, int stripe, int numStripes, const std::string& id, double width, int dir, ObstacleType type) {
840 10350948 : if (stripe >= 0 && stripe < numStripes) {
841 2401026 : if ((dir == FORWARD && x - width / 2. < obs[stripe].xBack) || (dir == BACKWARD && x + width / 2. > obs[stripe].xFwd)) {
842 2099796 : obs[stripe] = Obstacle(x, 0, type, id, width);
843 : }
844 : }
845 10350948 : }
846 :
847 : void
848 6258488 : MSPModel_Striping::moveInDirection(SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir) {
849 48451143 : for (ActiveLanes::iterator it_lane = myActiveLanes.begin(); it_lane != myActiveLanes.end(); ++it_lane) {
850 42192659 : const MSLane* lane = it_lane->first;
851 42192659 : Pedestrians& pedestrians = it_lane->second;
852 42192659 : if (pedestrians.size() == 0) {
853 32907056 : continue;
854 : }
855 : //std::cout << SIMTIME << ">>> lane=" << lane->getID() << " numPeds=" << pedestrians.size() << "\n";
856 9285603 : if (lane->isWalkingArea()) {
857 2095187 : const double lateral_offset = (lane->getWidth() - stripeWidth) * 0.5;
858 2095187 : const double minY = stripeWidth * - 0.5 + NUMERICAL_EPS;
859 2095187 : const double maxY = stripeWidth * (numStripes(lane) - 0.5) - NUMERICAL_EPS;
860 : const WalkingAreaPath* debugPath = nullptr;
861 : // need to handle each walkingAreaPath separately and transform
862 : // coordinates beforehand
863 : std::set<const WalkingAreaPath*, walkingarea_path_sorter> paths;
864 19143411 : for (Pedestrians::iterator it = pedestrians.begin(); it != pedestrians.end(); ++it) {
865 17048224 : const PState* p = static_cast<PState*>(*it);
866 : assert(p->myWalkingAreaPath != 0);
867 17048224 : if (p->getDirection() == dir) {
868 8539649 : paths.insert(p->myWalkingAreaPath);
869 8539649 : if DEBUGCOND(*p) {
870 0 : debugPath = p->myWalkingAreaPath;
871 0 : std::cout << SIMTIME << " debugging WalkingAreaPath from=" << debugPath->from->getID() << " to=" << debugPath->to->getID() << " minY=" << minY << " maxY=" << maxY << " latOffset=" << lateral_offset << "\n";
872 : }
873 : }
874 : }
875 2095187 : const double usableWidth = (numStripes(lane) - 1) * stripeWidth;
876 3223330 : for (std::set<const WalkingAreaPath*, walkingarea_path_sorter>::iterator it = paths.begin(); it != paths.end(); ++it) {
877 1128143 : const WalkingAreaPath* path = *it;
878 : Pedestrians toDelete;
879 : Pedestrians transformedPeds;
880 1128143 : transformedPeds.reserve(pedestrians.size());
881 10629241 : for (Pedestrians::iterator it_p = pedestrians.begin(); it_p != pedestrians.end(); ++it_p) {
882 9501098 : PState* p = static_cast<PState*>(*it_p);
883 9501098 : if (p->myWalkingAreaPath == path) {
884 8539649 : transformedPeds.push_back(p);
885 8539649 : if (path == debugPath) std::cout << " ped=" << p->getPerson()->getID() << " relX=" << p->getEdgePos(0) << " relY=" << p->getPosLat() << " (untransformed), vecCoord="
886 0 : << path->shape.transformToVectorCoordinates(p->getPosition(*p->getStage(), -1)) << "\n";
887 961449 : } else if (p->myWalkingAreaPath->from == path->to && p->myWalkingAreaPath->to == path->from) {
888 43290 : if (p->myWalkingAreaPath->dir != path->dir) {
889 : // opposite direction is already in the correct coordinate system
890 39843 : transformedPeds.push_back(p);
891 39843 : if (path == debugPath) std::cout << " ped=" << p->getPerson()->getID() << " relX=" << p->getEdgePos(0) << " relY=" << p->getPosLat() << " (untransformed), vecCoord="
892 0 : << path->shape.transformToVectorCoordinates(p->getPosition(*p->getStage(), -1)) << "\n";
893 : } else {
894 : // x position must be reversed
895 3447 : PState* tp = new PState(*p);
896 3447 : tp->reverse(path->length, usableWidth);
897 3447 : toDelete.push_back(tp);
898 3447 : transformedPeds.push_back(tp);
899 3447 : if (path == debugPath) std::cout << " ped=" << p->getPerson()->getID() << " relX=" << p->getEdgePos(0) << " relY=" << p->getPosLat() << " (semi-transformed), vecCoord="
900 0 : << path->shape.transformToVectorCoordinates(p->getPosition(*p->getStage(), -1)) << "\n";
901 : }
902 : } else {
903 918159 : const Position relPos = path->shape.transformToVectorCoordinates(p->getPosition(*p->getStage(), -1));
904 918159 : const double newY = relPos.y() + lateral_offset;
905 917055 : if (relPos != Position::INVALID && newY >= minY && newY <= maxY) {
906 256806 : PState* tp = new PState(*p);
907 256806 : tp->reset(relPos.x(), newY);
908 256806 : toDelete.push_back(tp);
909 256806 : transformedPeds.push_back(tp);
910 256806 : if (path == debugPath) {
911 0 : std::cout << " ped=" << p->getPerson()->getID() << " relX=" << relPos.x() << " relY=" << newY << " (transformed), vecCoord=" << relPos << "\n";
912 : }
913 : } else {
914 661353 : if (path == debugPath) {
915 0 : std::cout << " ped=" << p->getPerson()->getID() << " relX=" << relPos.x() << " relY=" << newY << " (invalid), vecCoord=" << relPos << "\n";
916 : }
917 : }
918 : }
919 : }
920 : auto itFoe = myWalkingAreaFoes.find(&lane->getEdge());
921 1128143 : if (itFoe != myWalkingAreaFoes.end()) {
922 : // add vehicle foes on paths which cross this walkingarea
923 : // translate the vehicle into a number of dummy-pedestrians
924 : // that occupy the same space
925 2265707 : for (const MSLane* foeLane : itFoe->second) {
926 184869 : for (auto itVeh = foeLane->anyVehiclesBegin(); itVeh != foeLane->anyVehiclesEnd(); ++itVeh) {
927 184869 : const MSVehicle* veh = *itVeh;
928 184869 : const double vehWidth = veh->getVehicleType().getWidth();
929 184869 : Boundary relCorners;
930 184869 : Position relFront = path->shape.transformToVectorCoordinates(veh->getPosition(), true);
931 184869 : Position relBack = path->shape.transformToVectorCoordinates(veh->getBackPosition(), true);
932 : if (relFront == Position::INVALID) {
933 0 : WRITE_WARNINGF("Could not vehicle '%' front position % onto walkingarea '%' path=%, time=%.",
934 : veh->getID(), veh->getPosition(), lane->getID(), path->shape, time2string(SIMSTEP));
935 : }
936 : if (relBack == Position::INVALID) {
937 0 : WRITE_WARNINGF("Could not vehicle '%' back position % onto walkingarea '%' path=%, time=%.",
938 : veh->getID(), veh->getBackPosition(), lane->getID(), path->shape, time2string(SIMSTEP));
939 : }
940 184869 : PositionVector relCenter;
941 184869 : relCenter.push_back(relFront);
942 184869 : relCenter.push_back(relBack);
943 184869 : relCenter.move2side(vehWidth / 2);
944 184869 : relCorners.add(relCenter[0]);
945 184869 : relCorners.add(relCenter[1]);
946 184869 : relCenter.move2side(-vehWidth);
947 184869 : relCorners.add(relCenter[0]);
948 184869 : relCorners.add(relCenter[1]);
949 : // persons should require less gap than the vehicles to prevent getting stuck
950 : // when a vehicles moves towards them
951 184869 : relCorners.growWidth(SAFETY_GAP / 2);
952 184869 : const double xWidth = relCorners.getWidth();
953 184869 : const double vehYmin = MAX2(minY - lateral_offset, relCorners.ymin());
954 184869 : const double vehYmax = MIN2(maxY - lateral_offset, relCorners.ymax());
955 184869 : const double xCenter = relCorners.getCenter().x();
956 : Position yMinPos(xCenter, vehYmin);
957 : Position yMaxPos(xCenter, vehYmax);
958 184869 : const bool addFront = addVehicleFoe(veh, lane, yMinPos, dir * xWidth, 0, lateral_offset, minY, maxY, toDelete, transformedPeds);
959 184869 : const bool addBack = addVehicleFoe(veh, lane, yMaxPos, dir * xWidth, 0, lateral_offset, minY, maxY, toDelete, transformedPeds);
960 184869 : if (path == debugPath) {
961 : std::cout << " veh=" << veh->getID()
962 0 : << " corners=" << relCorners
963 : << " xWidth=" << xWidth
964 0 : << " ymin=" << relCorners.ymin()
965 0 : << " ymax=" << relCorners.ymax()
966 : << " vehYmin=" << vehYmin
967 : << " vehYmax=" << vehYmax
968 0 : << "\n";
969 : }
970 184869 : if (addFront && addBack) {
971 : // add in-between positions
972 184869 : const double yDist = vehYmax - vehYmin;
973 421737 : for (double dist = stripeWidth; dist < yDist; dist += stripeWidth) {
974 236868 : const double relDist = dist / yDist;
975 236868 : Position between = (yMinPos * relDist) + (yMaxPos * (1 - relDist));
976 236868 : if (path == debugPath) {
977 0 : std::cout << " vehBetween=" << veh->getID() << " pos=" << between << "\n";
978 : }
979 236868 : addVehicleFoe(veh, lane, between, dir * xWidth, stripeWidth, lateral_offset, minY, maxY, toDelete, transformedPeds);
980 : }
981 : }
982 184869 : }
983 : }
984 : }
985 1128143 : moveInDirectionOnLane(transformedPeds, lane, currentTime, changedLane, dir, path == debugPath);
986 1128143 : arriveAndAdvance(pedestrians, currentTime, changedLane, dir);
987 : // clean up
988 1893218 : for (Pedestrians::iterator it_p = toDelete.begin(); it_p != toDelete.end(); ++it_p) {
989 765075 : delete *it_p;
990 : }
991 1128143 : }
992 : } else {
993 7190416 : moveInDirectionOnLane(pedestrians, lane, currentTime, changedLane, dir, false);
994 7190416 : arriveAndAdvance(pedestrians, currentTime, changedLane, dir);
995 : }
996 : }
997 6258484 : }
998 :
999 :
1000 : bool
1001 606606 : MSPModel_Striping::addVehicleFoe(const MSVehicle* veh, const MSLane* walkingarea, const Position& relPos, double xWidth, double yWidth, double lateral_offset,
1002 : double minY, double maxY, Pedestrians& toDelete, Pedestrians& transformedPeds) {
1003 : if (relPos != Position::INVALID) {
1004 606606 : const double newY = relPos.y() + lateral_offset;
1005 606606 : if (newY >= minY && newY <= maxY) {
1006 504822 : PState* tp = new PStateVehicle(veh, walkingarea, relPos.x(), newY, xWidth, yWidth);
1007 : //std::cout << SIMTIME << " addVehicleFoe=" << veh->getID() << " rx=" << relPos.x() << " ry=" << newY << " s=" << tp->stripe() << " o=" << tp->otherStripe() << "\n";
1008 504822 : toDelete.push_back(tp);
1009 504822 : transformedPeds.push_back(tp);
1010 : }
1011 606606 : return true;
1012 : } else {
1013 : return false;
1014 : }
1015 : }
1016 :
1017 : void
1018 8318559 : MSPModel_Striping::arriveAndAdvance(Pedestrians& pedestrians, SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir) {
1019 : // advance to the next lane / arrive at destination
1020 8318559 : sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(dir));
1021 : // can't use iterators because we do concurrent modification
1022 143177079 : for (int i = 0; i < (int)pedestrians.size(); i++) {
1023 134858524 : PState* const p = static_cast<PState*>(pedestrians[i]);
1024 134858524 : if (p->isRemoteControlled()) {
1025 27778 : continue;
1026 : }
1027 134830746 : if (p->getDirection() == dir && p->distToLaneEnd() < 0) {
1028 : // moveToNextLane may trigger re-insertion (for consecutive
1029 : // walks) so erase must be called first
1030 : pedestrians.erase(pedestrians.begin() + i);
1031 987732 : i--;
1032 987732 : p->moveToNextLane(currentTime);
1033 987728 : if (p->getLane() != nullptr) {
1034 756917 : changedLane.insert(p->getPerson());
1035 756917 : myActiveLanes[p->getLane()].push_back(p);
1036 : } else {
1037 : // end walking stage and destroy PState
1038 230811 : p->getStage()->moveToNextEdge(p->getPerson(), currentTime, dir);
1039 230811 : myNumActivePedestrians--;
1040 : }
1041 : }
1042 : }
1043 8318555 : }
1044 :
1045 :
1046 : void
1047 8318559 : MSPModel_Striping::moveInDirectionOnLane(Pedestrians& pedestrians, const MSLane* lane, SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir, bool debug) {
1048 8318559 : const int stripes = numStripes(lane);
1049 : //std::cout << " laneWidth=" << lane->getWidth() << " stripeWidth=" << stripeWidth << " stripes=" << stripes << "\n";
1050 16637118 : Obstacles obs(stripes, Obstacle(dir)); // continuously updated
1051 : NextLanesObstacles nextLanesObs; // continuously updated
1052 8318559 : sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(dir));
1053 :
1054 8318559 : Obstacles crossingVehs(stripes, Obstacle(dir));
1055 : bool hasCrossingVehObs = false;
1056 8318559 : if (lane->isCrossing()) {
1057 : // assume that vehicles will brake when already on the crossing
1058 611820 : hasCrossingVehObs = addCrossingVehs(lane, stripes, 0, dir, crossingVehs, true, false);
1059 : }
1060 :
1061 143019257 : for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
1062 134700698 : PState& p = *static_cast<PState*>(pedestrians[ii]);
1063 : UNUSED_PARAMETER(debug);
1064 : //if (debug) {
1065 : // std::cout << SIMTIME << " CHECKING d=" << dir << " p=" << p.getID() << " relX=" << p.myRelX << " xmin=" << p.getMinX() << " xmax=" << p.getMaxX() << " pdir=" << p.getDirection() << "\n";
1066 : //}
1067 134700698 : Obstacles currentObs = obs;
1068 134700698 : if (p.getDirection() != dir || changedLane.count(p.getPerson()) != 0 || p.getRemotePosition() != Position::INVALID) {
1069 63594252 : if (!p.isWaitingToEnter() && !p.isJammed()) {
1070 : //if DEBUGCOND(p) {
1071 : // std::cout << " obs=" << p.getPerson()->getID() << " y=" << p.getPosLat() << " stripe=" << p.stripe() << " oStripe=" << p.otherStripe() << "\n";
1072 : //}
1073 31153757 : Obstacle o(p);
1074 31153757 : if (p.getDirection() != dir && p.getSpeed(*p.getStage()) == 0.) {
1075 : // ensure recognition of oncoming
1076 8528189 : o.speed = (p.getDirection() == FORWARD ? 0.1 : -0.1);
1077 : }
1078 31153757 : if (o.closer(obs[p.stripe()], dir)) {
1079 31149452 : obs[p.stripe()] = o;
1080 : }
1081 31153757 : if (o.closer(obs[p.otherStripe()], dir)) {
1082 31149330 : obs[p.otherStripe()] = o;
1083 : }
1084 : }
1085 : continue;
1086 63594252 : }
1087 71106446 : if DEBUGCOND(p) {
1088 0 : std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << " currentObs=";
1089 0 : gDebugFlag1 = true;
1090 0 : DEBUG_PRINT(currentObs);
1091 : }
1092 71106446 : const MSLane* nextLane = p.myNLI.lane;
1093 71106446 : const MSLink* link = p.myNLI.link;
1094 71106446 : const double dist = p.distToLaneEnd();
1095 : const double speed(p.getStage()->getConfiguredSpeed() >= 0
1096 71106446 : ? p.getStage()->getConfiguredSpeed()
1097 58915147 : : ((nextLane != nullptr && (USE_NET_SPEEDS || nextLane->isNormal() || nextLane->isInternal()))
1098 128611852 : ? nextLane->getVehicleMaxSpeed(p.getPerson())
1099 13490950 : : p.getStage()->getMaxSpeed(p.getPerson())));
1100 :
1101 :
1102 71106446 : if (nextLane != nullptr && dist <= LOOKAHEAD_ONCOMING_DIST) {
1103 18404794 : const double currentLength = (p.myWalkingAreaPath == nullptr ? lane->getLength() : p.myWalkingAreaPath->length);
1104 18404794 : const Obstacles& nextObs = getNextLaneObstacles(
1105 : nextLanesObs, lane, nextLane, stripes,
1106 : p.myNLI.dir, currentLength, dir);
1107 :
1108 18404794 : if DEBUGCOND(p) {
1109 0 : std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << " nextObs=";
1110 0 : DEBUG_PRINT(nextObs);
1111 : }
1112 18404794 : p.mergeObstacles(currentObs, nextObs);
1113 : }
1114 71106446 : if DEBUGCOND(p) {
1115 0 : std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << " obsWithNext=";
1116 0 : DEBUG_PRINT(currentObs);
1117 : }
1118 71106446 : p.mergeObstacles(currentObs, getNeighboringObstacles(pedestrians, ii, stripes));
1119 71106446 : if DEBUGCOND(p) {
1120 0 : std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << " obsWithNeigh=";
1121 0 : DEBUG_PRINT(currentObs);
1122 : }
1123 : // time gap to pass the intersection ahead of a vehicle.
1124 71106446 : const double passingLength = p.getLength() + p.getPerson()->getTimegapCrossing() * speed;
1125 : // check link state
1126 71106446 : if DEBUGCOND(p) {
1127 0 : gDebugFlag1 = true; // get debug output from MSLink
1128 0 : std::cout << " link=" << (link == nullptr ? "NULL" : link->getViaLaneOrLane()->getID())
1129 0 : << " dist=" << dist << " d2=" << dist - p.getMinGap() << " la=" << LOOKAHEAD_SAMEDIR* speed << "\n";
1130 : }
1131 : if (link != nullptr
1132 : // only check close before junction, @todo we should take deceleration into account here
1133 57998888 : && dist - p.getMinGap() < LOOKAHEAD_SAMEDIR * speed
1134 : // persons move before vehicles so we subtract DELTA_TO because they cannot rely on vehicles having passed the intersection in the current time step
1135 83454927 : && (!link->opened(currentTime - DELTA_T, speed, speed, passingLength, p.getImpatience(), speed, 0, 0, nullptr, p.ignoreRed(link), p.getPerson())
1136 11688910 : || p.stopForYellow(link))) {
1137 : // prevent movement passed a closed link
1138 660489 : Obstacles closedLink(stripes, Obstacle(p.getEdgePos(0) + dir * (dist - NUMERICAL_EPS), 0, OBSTACLE_LINKCLOSED, "closedLink_" + link->getViaLaneOrLane()->getID(), 0));
1139 660489 : p.mergeObstacles(currentObs, closedLink);
1140 660489 : if DEBUGCOND(p) {
1141 0 : std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << " obsWithTLS=";
1142 0 : DEBUG_PRINT(currentObs);
1143 : }
1144 : // consider rerouting over another crossing
1145 660489 : if (p.myWalkingAreaPath != nullptr) {
1146 : // @todo actually another path would be needed starting at the current position
1147 502727 : const MSLane* oldNext = p.myNLI.lane;
1148 502727 : p.myNLI = getNextLane(p, p.getLane(), p.myWalkingAreaPath->from);
1149 502727 : if (p.myNLI.lane != oldNext && oldNext->isPriorityCrossing()) {
1150 152 : unregisterCrossingApproach(p, oldNext);
1151 : }
1152 : }
1153 660489 : }
1154 71106446 : if DEBUGCOND(p) {
1155 0 : gDebugFlag1 = false;
1156 : }
1157 71106446 : if (&lane->getEdge() == p.getStage()->getDestination() && p.getStage()->getDestinationStop() != nullptr) {
1158 : Obstacles arrival;
1159 1386168 : if (p.getStage()->getDestinationStop()->getWaitingCapacity() > p.getStage()->getDestinationStop()->getNumWaitingPersons() ||
1160 : (!p.getStage()->getDestinationStop()->checkPersonCapacity())) {
1161 2568458 : arrival = Obstacles(stripes, Obstacle(p.getStage()->getArrivalPos() + dir * p.getMinGap(), 0, OBSTACLE_ARRIVALPOS, "arrival", 0));
1162 : } else {
1163 203878 : arrival = Obstacles(stripes, Obstacle(p.getStage()->getArrivalPos() - dir * p.getMinGap(), 0, OBSTACLE_ARRIVALPOS, "arrival_blocked", 0));
1164 : }
1165 1386168 : p.mergeObstacles(currentObs, arrival);
1166 1386168 : }
1167 :
1168 71106446 : if (lane->getVehicleNumberWithPartials() > 0) {
1169 : // react to vehicles on the same lane
1170 : // @todo: improve efficiency by using the same iterator for all pedestrians on this lane
1171 275864 : Obstacles vehObs = getVehicleObstacles(lane, dir, &p);
1172 275864 : p.mergeObstacles(currentObs, vehObs);
1173 275864 : if DEBUGCOND(p) {
1174 0 : std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << " obsWithVehs=";
1175 0 : DEBUG_PRINT(currentObs);
1176 : }
1177 275864 : }
1178 71106446 : if (hasCrossingVehObs) {
1179 24832 : p.mergeObstacles(currentObs, crossingVehs);
1180 24832 : if DEBUGCOND(p) {
1181 0 : std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << " obsWithVehs2=";
1182 0 : DEBUG_PRINT(currentObs);
1183 : }
1184 : }
1185 :
1186 : // walk, taking into account all obstacles
1187 71106446 : p.walk(currentObs);
1188 71106446 : gDebugFlag1 = false;
1189 71106446 : if (!p.isWaitingToEnter() && !p.isJammed()) {
1190 36029064 : Obstacle o(p);
1191 36029064 : if (o.closer(obs[p.stripe()], dir)) {
1192 36027509 : obs[p.stripe()] = o;
1193 : }
1194 36029064 : if (o.closer(obs[p.otherStripe()], dir)) {
1195 36027504 : obs[p.otherStripe()] = o;
1196 : }
1197 36029064 : if (MSGlobals::gCheck4Accidents && p.myWalkingAreaPath == nullptr && !p.isJammed()) {
1198 2936513965 : for (int coll = 0; coll < ii; ++coll) {
1199 2906309958 : PState& c = *static_cast<PState*>(pedestrians[coll]);
1200 2906309958 : if (!c.isWaitingToEnter() && c.myWalkingAreaPath == nullptr && !c.isJammed()) {
1201 1294872892 : if (c.stripe() == p.stripe() || p.stripe() == c.otherStripe() || p.otherStripe() == c.stripe() || p.otherStripe() == c.otherStripe()) {
1202 490601977 : Obstacle cObs(c);
1203 : // we check only for real collisions, no min gap violations
1204 490601977 : if (p.distanceTo(cObs, false) == DIST_OVERLAP) {
1205 1165868 : WRITE_WARNING("Collision of person '" + p.getPerson()->getID() + "' and person '" + c.getPerson()->getID()
1206 : + "', lane='" + lane->getID() + "', time=" + time2string(currentTime) + ".");
1207 : }
1208 : }
1209 : }
1210 : }
1211 : }
1212 : }
1213 : //std::cout << SIMTIME << p.getPerson()->getID() << " lane=" << lane->getID() << " x=" << p.myRelX << "\n";
1214 134700698 : }
1215 16637118 : }
1216 :
1217 :
1218 : void
1219 488017 : MSPModel_Striping::registerCrossingApproach(const PState& ped, const MSLane* crossing, const MSLane* beforeWA) {
1220 : // person has entered the walkingarea
1221 488017 : SUMOTime arrivalTime = SIMSTEP;
1222 : assert(ped.getLane()->isWalkingArea());
1223 488017 : const WalkingAreaPath* wa = getWalkingAreaPath(&ped.getLane()->getEdge(), beforeWA, crossing);
1224 488017 : const double speed = ped.getStage()->getMaxSpeed(ped.getPerson()) * (1 - dawdling / 2);
1225 488017 : arrivalTime += TIME2STEPS(wa->length / speed);
1226 488017 : SUMOTime leavingTime = arrivalTime + TIME2STEPS(crossing->getLength() / speed);
1227 488017 : crossing->getIncomingLanes()[0].viaLink->setApproachingPerson(ped.getPerson(), arrivalTime, leavingTime);
1228 488017 : if DEBUGCOND(ped) {
1229 0 : std::cout << SIMTIME << " register " << ped.getPerson()->getID() << " at crossing " << crossing->getID() << "\n";
1230 : }
1231 488017 : }
1232 :
1233 :
1234 : bool
1235 941736 : MSPModel_Striping::addCrossingVehs(const MSLane* crossing, int stripes, double lateral_offset, int dir, Obstacles& obs, bool prio, bool flipY) {
1236 : bool hasCrossingVehObs = false;
1237 941736 : const MSLink* crossingExitLink = crossing->getLinkCont().front();
1238 941736 : gDebugFlag1 = DEBUGCOND2(crossing);
1239 941736 : const MSLink::LinkLeaders linkLeaders = crossingExitLink->getLeaderInfo(nullptr, crossing->getLength());
1240 941736 : gDebugFlag1 = false;
1241 941736 : if (linkLeaders.size() > 0) {
1242 215348 : for (MSLink::LinkLeaders::const_iterator it = linkLeaders.begin(); it != linkLeaders.end(); ++it) {
1243 : // the vehicle to enter the junction first has priority
1244 109771 : const MSVehicle* veh = (*it).vehAndGap.first;
1245 109771 : if (veh != nullptr) {
1246 109771 : Obstacle vo((*it).distToCrossing, 0, OBSTACLE_VEHICLE, veh->getID(), veh->getVehicleType().getWidth() + 2 * minGapToVehicle, veh);
1247 : // block entry to the crossing in walking direction but allow leaving it
1248 109771 : Obstacle voBlock = vo;
1249 109771 : if (dir == FORWARD) {
1250 61608 : voBlock.xBack = NUMERICAL_EPS;
1251 : } else {
1252 48163 : voBlock.xFwd = crossing->getLength() - NUMERICAL_EPS;
1253 : }
1254 : // when approaching a priority crossings, vehicles must be able
1255 : // to brake, otherwise the person must be able to cross in time
1256 109771 : const double distToCrossBeforeVeh = (dir == FORWARD ? vo.xFwd : crossing->getLength() - vo.xBack);
1257 : const double bGap = (prio
1258 111411 : ? veh->getCarFollowModel().brakeGap(veh->getSpeed(), veh->getCarFollowModel().getMaxDecel(), 0)
1259 1640 : : veh->getSpeed() * distToCrossBeforeVeh); // walking 1m/s
1260 : double vehYmin;
1261 : double vehYmax;
1262 : // relY increases from left to right (the other way around from vehicles)
1263 109771 : if ((*it).fromLeft()) {
1264 71329 : vehYmin = -(*it).vehAndGap.second + lateral_offset; // vehicle back
1265 71329 : vehYmax = vehYmin + veh->getVehicleType().getLength() + bGap + minGapToVehicle;
1266 71329 : vehYmin -= minGapToVehicle;
1267 : } else {
1268 38442 : vehYmax = crossing->getWidth() + (*it).vehAndGap.second - lateral_offset; // vehicle back
1269 38442 : vehYmin = vehYmax - veh->getVehicleType().getLength() - bGap - minGapToVehicle;
1270 38442 : vehYmax += minGapToVehicle;
1271 :
1272 : }
1273 439379 : for (int s = MAX2(0, PState::stripe(vehYmin)); s < MIN2(PState::stripe(vehYmax), stripes); ++s) {
1274 185966 : if ((dir == FORWARD && obs[s].xBack > vo.xBack)
1275 329945 : || (dir == BACKWARD && obs[s].xFwd < vo.xFwd)) {
1276 328176 : if (!prio && veh->getSpeed() > SUMO_const_haltingSpeed) {
1277 : // do not enter the crossing
1278 9257 : obs[s] = voBlock;
1279 : } else {
1280 318919 : obs[s] = vo;
1281 : }
1282 : hasCrossingVehObs = true;
1283 : }
1284 : }
1285 109771 : if (flipY) {
1286 24259 : Obstacles tmp = obs;
1287 174275 : for (int i = 0; i < (int)obs.size(); i++) {
1288 150016 : obs[i] = tmp[obs.size() - 1 - i];
1289 : }
1290 24259 : }
1291 109771 : if (DEBUGCOND2(crossing)) {
1292 0 : std::cout << SIMTIME
1293 : << " crossingVeh=" << veh->getID()
1294 : << " lane=" << crossing->getID()
1295 : << " prio=" << prio
1296 : << " latOffset=" << lateral_offset
1297 : << " dir=" << dir
1298 0 : << " flipY=" << flipY
1299 : << " stripes=" << stripes
1300 0 : << " dist=" << (*it).distToCrossing
1301 0 : << " gap=" << (*it).vehAndGap.second
1302 : << " brakeGap=" << bGap
1303 : << " fromLeft=" << (*it).fromLeft()
1304 : << " distToCrossBefore=" << distToCrossBeforeVeh
1305 : << " ymin=" << vehYmin
1306 : << " ymax=" << vehYmax
1307 : << " smin=" << PState::stripe(vehYmin)
1308 0 : << " smax=" << PState::stripe(vehYmax)
1309 0 : << "\n";
1310 0 : DEBUG_PRINT(obs);
1311 : }
1312 : }
1313 : }
1314 105577 : if (hasCrossingVehObs) {
1315 : // check whether the crossing is fully blocked
1316 71035 : const int reserved = getReserved((int)obs.size(), RESERVE_FOR_ONCOMING_FACTOR_JUNCTIONS);
1317 : bool allBlocked = true;
1318 :
1319 392074 : for (int i = 0; i < (int)obs.size(); i++) {
1320 348166 : const Obstacle& o = obs[i];
1321 348166 : if (o.type != OBSTACLE_VEHICLE && (
1322 54715 : (dir == FORWARD && i >= reserved) ||
1323 39198 : (dir == BACKWARD && i < (int)obs.size() - reserved))) {
1324 : allBlocked = false;
1325 : break;
1326 : }
1327 : }
1328 71035 : if (allBlocked) {
1329 43908 : if (DEBUGCOND2(crossing)) {
1330 0 : std::cout << SIMTIME << " crossing=" << crossing->getID() << " allBlocked\n";
1331 : }
1332 312069 : for (Obstacle& o : obs) {
1333 268161 : if (dir == FORWARD) {
1334 149329 : o.xBack = NUMERICAL_EPS;
1335 : } else {
1336 118832 : o.xFwd = crossing->getLength() - NUMERICAL_EPS;
1337 : }
1338 : }
1339 : }
1340 : }
1341 : }
1342 941736 : return hasCrossingVehObs;
1343 941736 : }
1344 :
1345 :
1346 : MSPModel_Striping::Obstacles
1347 294505 : MSPModel_Striping::getVehicleObstacles(const MSLane* lane, int dir, PState* ped) {
1348 294505 : const int stripes = numStripes(lane);
1349 589010 : Obstacles vehObs(stripes, Obstacle(dir));
1350 : int current = -1;
1351 : double minX = 0.;
1352 : double maxX = 0.;
1353 : double pRelY = -1.;
1354 : double pWidth = 0.;
1355 : std::string pID;
1356 294505 : bool debug = DEBUGCOND2(lane);
1357 294505 : if (ped != nullptr) {
1358 275864 : current = ped->stripe();
1359 275864 : minX = ped->getMinX();
1360 275864 : maxX = ped->getMaxX();
1361 : pRelY = ped->getPosLat();
1362 275864 : pWidth = ped->getPerson()->getVehicleType().getWidth();
1363 : pID = ped->getPerson()->getID();
1364 275864 : debug = DEBUGCOND(*ped);
1365 18641 : } else if (dir == BACKWARD) {
1366 : // checking vehicles on the next lane. Use entry point as reference
1367 : minX = lane->getLength();
1368 : maxX = lane->getLength();
1369 : }
1370 294505 : MSLane::AnyVehicleIterator begin = (dir == FORWARD ? lane->anyVehiclesUpstreamBegin() : lane->anyVehiclesBegin());
1371 : MSLane::AnyVehicleIterator end = (dir == FORWARD ? lane->anyVehiclesUpstreamEnd() : lane->anyVehiclesEnd());
1372 781919 : for (MSLane::AnyVehicleIterator it = begin; it != end; ++it) {
1373 487414 : const MSVehicle* veh = *it;
1374 487414 : const bool bidi = veh->getLane() == lane->getBidiLane();
1375 487414 : const double vehBack = veh->getBackPositionOnLane(lane);
1376 487414 : double vehFront = vehBack + veh->getVehicleType().getLength();
1377 : // ensure that vehicles are not blocked
1378 487414 : const double vehNextSpeed = veh->getWaitingTime() > DELTA_T ? 0 : MAX2(veh->getSpeed(), 1.0);
1379 487414 : const double clearance = SAFETY_GAP + vehNextSpeed * LOOKAHEAD_SAMEDIR;
1380 : // boundaries for range checking
1381 : double vehXMax;
1382 : double vehXMin;
1383 : double vehXMaxCheck;
1384 : double vehXMinCheck;
1385 487414 : if (bidi) {
1386 1617 : vehFront = vehBack - veh->getVehicleType().getLength();
1387 1617 : vehXMax = vehBack + SAFETY_GAP;
1388 1617 : vehXMin = vehFront - clearance;
1389 1617 : if (dir == FORWARD) {
1390 1617 : vehXMaxCheck = vehBack + NUMERICAL_EPS;
1391 1617 : vehXMinCheck = vehFront - LOOKAROUND_VEHICLES;
1392 : } else {
1393 0 : vehXMaxCheck = vehBack + LOOKAHEAD_SAMEDIR;
1394 : vehXMinCheck = vehFront - clearance;
1395 : }
1396 : } else {
1397 485797 : vehXMax = vehFront + clearance;
1398 485797 : vehXMin = vehBack - SAFETY_GAP;
1399 485797 : if (dir == FORWARD) {
1400 : vehXMaxCheck = vehFront + clearance;
1401 363616 : vehXMinCheck = vehBack - LOOKAHEAD_SAMEDIR;
1402 : } else {
1403 122181 : vehXMaxCheck = vehFront + LOOKAROUND_VEHICLES;
1404 122181 : vehXMinCheck = vehBack - NUMERICAL_EPS;
1405 : }
1406 : }
1407 487414 : if (debug) {
1408 0 : std::cout << SIMTIME << " ped=" << pID << " veh=" << veh->getID() << " check obstacle on lane=" << lane->getID()
1409 : << "\n"
1410 : << " vehXMin=" << vehXMin
1411 : << " vehXMax=" << vehXMax
1412 : << " vehXMinC=" << vehXMinCheck
1413 : << " vehXMaxC=" << vehXMaxCheck
1414 : << " minX=" << minX
1415 : << " maxX=" << maxX
1416 : << " bidi=" << bidi
1417 : << " vFront=" << vehFront
1418 : << " vBack=" << vehBack
1419 0 : << "\n";
1420 : }
1421 487414 : if (vehXMaxCheck > minX && vehXMinCheck && vehXMinCheck <= maxX) {
1422 370128 : Obstacle vo(vehBack, veh->getSpeed() * (bidi ? -1 : 1), OBSTACLE_VEHICLE, veh->getID(), 0, veh);
1423 : // moving vehicles block space along their path
1424 185418 : vo.xFwd = vehXMax;
1425 185418 : vo.xBack = vehXMin;
1426 : // relY increases from left to right (the other way around from vehicles)
1427 : // XXX lateral offset for partial vehicles
1428 185418 : const double posLat = veh->getLateralPositionOnLane() * (bidi ? -1 : 1);
1429 185418 : const double vehYmax = 0.5 * (lane->getWidth() + veh->getVehicleType().getWidth() - stripeWidth) - posLat;
1430 185418 : const double vehYmin = vehYmax - veh->getVehicleType().getWidth();
1431 804232 : for (int s = MAX2(0, PState::stripe(vehYmin)); s < MIN2(PState::stripe(vehYmax) + 1, stripes); ++s) {
1432 618814 : Obstacle prior = vehObs[s];
1433 : vehObs[s] = vo;
1434 618814 : if (s == current && vehFront + SAFETY_GAP < minX) {
1435 : // ignore if already overlapping while vehicle is still behind
1436 16132 : if (pRelY - pWidth < vehYmax &&
1437 15831 : pRelY + pWidth > vehYmin && dir == FORWARD) {
1438 9966 : if (debug) {
1439 0 : std::cout << " ignoring vehicle '" << veh->getID() << " on stripe " << s << " vehFrontSG=" << vehFront + SAFETY_GAP << " minX=" << minX << "\n";
1440 : }
1441 : if (dir == FORWARD) {
1442 : vehObs[s] = prior;
1443 : } else {
1444 : vehObs[s].xFwd = MIN2(vo.xFwd, vehFront + SAFETY_GAP);
1445 : }
1446 : }
1447 : }
1448 : }
1449 185418 : if (debug) {
1450 0 : std::cout << SIMTIME << " ped=" << pID << " veh=" << veh->getID() << " obstacle on lane=" << lane->getID()
1451 : << "\n"
1452 : << " ymin=" << vehYmin
1453 : << " ymax=" << vehYmax
1454 : << " smin=" << PState::stripe(vehYmin)
1455 0 : << " smax=" << PState::stripe(vehYmax)
1456 0 : << " relY=" << pRelY
1457 : << " current=" << current
1458 0 : << " vo.xFwd=" << vo.xFwd
1459 0 : << " vo.xBack=" << vo.xBack
1460 : << " vFront=" << vehFront
1461 : << " vBack=" << vehBack
1462 0 : << "\n";
1463 : }
1464 : }
1465 : }
1466 294505 : return vehObs;
1467 0 : }
1468 :
1469 :
1470 : // ===========================================================================
1471 : // MSPModel_Striping::Obstacle method definitions
1472 : // ===========================================================================
1473 88038069 : MSPModel_Striping::Obstacle::Obstacle(int dir, double dist) :
1474 88038069 : xFwd(dir * dist), // by default, far away when seen in dir
1475 88038069 : xBack(dir * dist), // by default, far away when seen in dir
1476 88038069 : speed(0),
1477 88038069 : type(OBSTACLE_NONE),
1478 88038069 : description("") {
1479 88038069 : }
1480 :
1481 :
1482 717007625 : MSPModel_Striping::Obstacle::Obstacle(const PState& ped) :
1483 717007625 : xFwd(ped.getMaxX()),
1484 717007625 : xBack(ped.getMinX()),
1485 717007625 : speed(ped.getDirection() * ped.getSpeed(*ped.getStage())),
1486 717007625 : type(ped.getOType()),
1487 717007625 : description(ped.getID()) {
1488 : assert(!ped.isWaitingToEnter());
1489 717007625 : if (type == OBSTACLE_VEHICLE) {
1490 539616 : vehicle = static_cast<const PStateVehicle&>(ped).getVehicle();
1491 : }
1492 717007625 : }
1493 :
1494 :
1495 : bool
1496 134365642 : MSPModel_Striping::Obstacle::closer(const Obstacle& o, int dir) {
1497 134365642 : if (dir == FORWARD) {
1498 72546076 : return xBack <= o.xBack;
1499 : } else {
1500 61819566 : return xFwd >= o.xFwd;
1501 : }
1502 : }
1503 :
1504 :
1505 : // ===========================================================================
1506 : // MSPModel_Striping::PState method definitions
1507 : // ===========================================================================
1508 243914 : MSPModel_Striping::PState::PState(MSPerson* person, MSStageMoving* stage, const MSLane* lane):
1509 : MSPModel_InteractingState(person, stage, lane),
1510 243914 : myWalkingAreaPath(nullptr) {
1511 : const MSEdge* currentEdge = &lane->getEdge();
1512 : const ConstMSEdgeVector& route = myStage->getRoute();
1513 : assert(!route.empty());
1514 243914 : myDir = FORWARD;
1515 243914 : if (route.size() == 1) {
1516 : // only a single edge, move towards end pos
1517 13472 : myDir = (myEdgePos <= myStage->getArrivalPos()) ? FORWARD : BACKWARD;
1518 230442 : } else if (route.front()->getFunction() != SumoXMLEdgeFunc::NORMAL) {
1519 : // start on an intersection
1520 6 : if (route.front()->isWalkingArea()) {
1521 6 : myWalkingAreaPath = getArbitraryPath(route.front());
1522 : }
1523 : } else {
1524 230436 : int passedFwd = 0;
1525 230436 : int passedBwd = 0;
1526 230436 : const bool mayStartForward = canTraverse(FORWARD, route, passedFwd) != UNDEFINED_DIRECTION;
1527 230436 : const bool mayStartBackward = canTraverse(BACKWARD, route, passedBwd) != UNDEFINED_DIRECTION;
1528 230436 : if DEBUGCOND(*this) {
1529 0 : std::cout << " initialize dir for " << myPerson->getID() << " forward=" << mayStartForward << " backward=" << mayStartBackward << "\n";
1530 : }
1531 230436 : if (mayStartForward && mayStartBackward) {
1532 : // figure out the best direction via routing
1533 : ConstMSEdgeVector crossingRoute;
1534 7437 : MSNet::getInstance()->getPedestrianRouter(0).compute(currentEdge, route.back(), myEdgePos, myStage->getArrivalPos(),
1535 2479 : myStage->getMaxSpeed(person), 0, nullptr, person->getVTypeParameter(), crossingRoute, true);
1536 2479 : if (crossingRoute.size() > 1) {
1537 : // route found
1538 2471 : const MSEdge* nextEdge = crossingRoute[1];
1539 2471 : if (nextEdge->getFromJunction() == currentEdge->getFromJunction() || nextEdge->getToJunction() == currentEdge->getFromJunction()) {
1540 1884 : myDir = BACKWARD;
1541 : }
1542 : }
1543 2479 : if DEBUGCOND(*this) {
1544 0 : std::cout << " crossingRoute=" << toString(crossingRoute) << "\n";
1545 : }
1546 230436 : } else if (!mayStartForward && !mayStartBackward) {
1547 156 : int lastDisconnect = passedFwd >= passedBwd ? passedFwd : passedBwd;
1548 : std::string dLoc;
1549 156 : if (route.size() > 2) {
1550 260 : dLoc = TLF(" between edge '%' and edge '%'", route[lastDisconnect - 1]->getID(), route[lastDisconnect]->getID());
1551 : }
1552 936 : WRITE_WARNINGF(TL("Person '%' walking from edge '%' to edge '%' has a disconnect%, time=%."),
1553 : myPerson->getID(), route.front()->getID(), route.back()->getID(), dLoc, SIMTIME);
1554 161 : myDir = passedFwd >= passedBwd ? FORWARD : BACKWARD;
1555 : } else {
1556 426023 : myDir = !mayStartBackward ? FORWARD : BACKWARD;
1557 : }
1558 : }
1559 243914 : if (myPosLat == UNSPECIFIED_POS_LAT || myLegacyPosLat) {
1560 243630 : if (myPosLat == UNSPECIFIED_POS_LAT) {
1561 243444 : myPosLat = 0;
1562 : }
1563 243630 : if (lane->getVehicleNumberWithPartials() > 0 && myPosLat == 0) {
1564 : // better start next to the road if nothing was specified
1565 2162 : myPosLat -= stripeWidth;
1566 : }
1567 243630 : if (myDir == FORWARD || lane->getPermissions() != SVC_PEDESTRIAN) {
1568 : // start at the right side of the sidewalk on shared roads
1569 207903 : myPosLat = stripeWidth * (numStripes(lane) - 1) - myPosLat;
1570 : }
1571 284 : } else if (myPosLat == RANDOM_POS_LAT) {
1572 20 : myPosLat = RandHelper::rand() * stripeWidth * (numStripes(lane) - 1);
1573 : } else {
1574 : // vehicle to striping coordinate system
1575 264 : myPosLat = posLatConversion(myPosLat, lane->getWidth());
1576 : }
1577 243914 : if DEBUGCOND(*this) {
1578 0 : std::cout << " added new pedestrian " << myPerson->getID() << " on " << lane->getID() << " myEdgePos=" << myEdgePos << " myPosLat=" << myPosLat << " dir=" << myDir << " route=" << toString(myStage->getRoute()) << "\n";
1579 : }
1580 :
1581 243914 : myNLI = getNextLane(*this, lane, nullptr);
1582 243910 : }
1583 :
1584 :
1585 14 : MSPModel_Striping::PState::PState(MSPerson* person, MSStageMoving* stage, std::istringstream* in) :
1586 : MSPModel_InteractingState(person, stage, nullptr),
1587 14 : myWalkingAreaPath(nullptr) {
1588 14 : if (in != nullptr) {
1589 : std::string laneID;
1590 : std::string wapLaneFrom;
1591 : std::string wapLaneTo;
1592 : std::string nextLaneID;
1593 : std::string nextLinkFrom;
1594 : std::string nextLinkTo;
1595 : int nextDir;
1596 :
1597 14 : (*in) >> laneID
1598 14 : >> myEdgePos >> myPosLat >> myDir >> mySpeed >> mySpeedLat >> myWaitingToEnter >> myWaitingTime
1599 14 : >> wapLaneFrom >> wapLaneTo
1600 14 : >> myAmJammed
1601 : >> nextLaneID
1602 : >> nextLinkFrom
1603 14 : >> nextLinkTo
1604 14 : >> nextDir;
1605 :
1606 :
1607 14 : myLane = MSLane::dictionary(laneID);
1608 14 : if (myLane == nullptr) {
1609 0 : throw ProcessError("Unknown lane '" + laneID + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1610 : }
1611 :
1612 : MSLane* nextLane = nullptr;
1613 14 : if (nextLaneID != "null") {
1614 13 : nextLane = MSLane::dictionary(nextLaneID);
1615 13 : if (nextLane == nullptr) {
1616 0 : throw ProcessError("Unknown next lane '" + nextLaneID + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1617 : }
1618 : }
1619 : const MSLink* link = nullptr;
1620 14 : if (nextLinkFrom != "null") {
1621 2 : MSLane* from = MSLane::dictionary(nextLinkFrom);
1622 2 : MSLane* to = MSLane::dictionary(nextLinkTo);
1623 2 : if (from == nullptr) {
1624 0 : throw ProcessError("Unknown link origin lane '" + nextLinkFrom + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1625 : }
1626 2 : if (to == nullptr) {
1627 0 : throw ProcessError("Unknown link destination lane '" + nextLinkTo + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1628 : }
1629 2 : link = from->getLinkTo(to);
1630 : }
1631 14 : myNLI = NextLaneInfo(nextLane, link, nextDir);
1632 :
1633 14 : if (wapLaneFrom != "null") {
1634 1 : MSLane* from = MSLane::dictionary(wapLaneFrom);
1635 1 : MSLane* to = MSLane::dictionary(wapLaneTo);
1636 1 : if (from == nullptr) {
1637 0 : throw ProcessError("Unknown walkingAreaPath origin lane '" + wapLaneFrom + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1638 : }
1639 1 : if (to == nullptr) {
1640 0 : throw ProcessError("Unknown walkingAreaPath destination lane '" + wapLaneTo + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1641 : }
1642 1 : const auto pathIt = myWalkingAreaPaths.find(std::make_pair(from, to));
1643 1 : if (pathIt != myWalkingAreaPaths.end()) {
1644 1 : myWalkingAreaPath = &pathIt->second;
1645 : } else {
1646 0 : throw ProcessError("Unknown walkingAreaPath from lane '" + wapLaneFrom + "' to lane '" + wapLaneTo + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1647 : }
1648 : }
1649 : }
1650 14 : }
1651 :
1652 :
1653 504822 : MSPModel_Striping::PState::PState() :
1654 504822 : MSPModel_InteractingState(nullptr, nullptr, nullptr) {}
1655 :
1656 :
1657 : void
1658 9 : MSPModel_Striping::PState::saveState(std::ostringstream& out) {
1659 9 : std::string wapLaneFrom = "null";
1660 9 : std::string wapLaneTo = "null";
1661 9 : if (myWalkingAreaPath != nullptr) {
1662 1 : wapLaneFrom = myWalkingAreaPath->from->getID();
1663 1 : wapLaneTo = myWalkingAreaPath->to->getID();
1664 : }
1665 9 : std::string nextLaneID = "null";
1666 9 : std::string nextLinkFrom = "null";
1667 9 : std::string nextLinkTo = "null";
1668 9 : if (myNLI.lane != nullptr) {
1669 : nextLaneID = myNLI.lane->getID();
1670 : }
1671 9 : if (myNLI.link != nullptr) {
1672 : nextLinkFrom = myNLI.link->getLaneBefore()->getID();
1673 2 : nextLinkTo = myNLI.link->getViaLaneOrLane()->getID();
1674 : }
1675 9 : out << " " << myLane->getID()
1676 18 : << " " << myEdgePos
1677 9 : << " " << myPosLat
1678 : << " " << myDir
1679 18 : << " " << mySpeed
1680 9 : << " " << mySpeedLat
1681 9 : << " " << myWaitingToEnter
1682 9 : << " " << myWaitingTime
1683 : << " " << wapLaneFrom
1684 : << " " << wapLaneTo
1685 9 : << " " << myAmJammed
1686 : << " " << nextLaneID
1687 : << " " << nextLinkFrom
1688 : << " " << nextLinkTo
1689 9 : << " " << myNLI.dir;
1690 9 : }
1691 :
1692 : double
1693 2303251246 : MSPModel_Striping::PState::getMinX(const bool includeMinGap) const {
1694 : // @todo speed should have an influence here because faster persons need more space
1695 2303251246 : if (myDir == FORWARD) {
1696 2134145958 : return myEdgePos - getLength();
1697 : }
1698 169105288 : return myEdgePos - (includeMinGap ? getMinGap() : 0.);
1699 : }
1700 :
1701 :
1702 : double
1703 2303251246 : MSPModel_Striping::PState::getMaxX(const bool includeMinGap) const {
1704 : // @todo speed should have an influence here because faster persons need more space
1705 2303251246 : if (myDir == FORWARD) {
1706 2134145958 : return myEdgePos + (includeMinGap ? getMinGap() : 0.);
1707 : }
1708 169105288 : return myEdgePos + getLength();
1709 : }
1710 :
1711 :
1712 : double
1713 2374357692 : MSPModel_Striping::PState::getLength() const {
1714 2374357692 : return myPerson->getVehicleType().getLength();
1715 : }
1716 :
1717 :
1718 : double
1719 1558447390 : MSPModel_Striping::PState::getMinGap() const {
1720 1558447390 : return myPerson->getVehicleType().getMinGap();
1721 : }
1722 :
1723 :
1724 : int
1725 8414695689 : MSPModel_Striping::PState::stripe(double relY) {
1726 8414695689 : return (int)floor(relY / stripeWidth + 0.5);
1727 : }
1728 :
1729 :
1730 : int
1731 3665377174 : MSPModel_Striping::PState::otherStripe(double relY) const {
1732 3665377174 : const int s = stripe(relY);
1733 3665377174 : const double offset = relY - s * stripeWidth;
1734 3665377174 : const double threshold = MAX2(NUMERICAL_EPS, stripeWidth - SQUEEZE * getWidth());
1735 : int result;
1736 3665377174 : if (offset > threshold) {
1737 504045 : result = s + 1;
1738 3664873129 : } else if (offset < -threshold) {
1739 426621 : 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 3665377174 : return result;
1747 : }
1748 :
1749 : int
1750 4713743867 : MSPModel_Striping::PState::stripe() const {
1751 4713743867 : return MIN2(MAX2(0, stripe(myPosLat)), numStripes(myLane) - 1);
1752 : }
1753 :
1754 :
1755 : int
1756 3648659729 : MSPModel_Striping::PState::otherStripe() const {
1757 3648659729 : return MIN2(MAX2(0, otherStripe(myPosLat)), numStripes(myLane) - 1);
1758 : }
1759 :
1760 :
1761 : double
1762 144448159 : MSPModel_Striping::PState::distToLaneEnd() const {
1763 144448159 : if (myStage->getNextRouteEdge() == nullptr) {
1764 24803824 : return myDir * (myStage->getArrivalPos() - myEdgePos) - POSITION_EPS - (
1765 2223618 : (myWaitingTime > DELTA_T && (myStage->getDestinationStop() == nullptr ||
1766 : myStage->getDestinationStop()->getWaitingCapacity() > myStage->getDestinationStop()->getNumWaitingPersons()))
1767 27027442 : ? getMinGap() : 0);
1768 : } else {
1769 119644335 : const double length = myWalkingAreaPath == nullptr ? myLane->getLength() : myWalkingAreaPath->length;
1770 119644335 : return myDir == FORWARD ? length - myEdgePos : myEdgePos;
1771 : }
1772 : }
1773 :
1774 :
1775 : bool
1776 987732 : MSPModel_Striping::PState::moveToNextLane(SUMOTime currentTime) {
1777 987732 : double dist = distToLaneEnd();
1778 987732 : if (DEBUGCOND(*this)) {
1779 0 : std::cout << SIMTIME << " ped=" << myPerson->getID() << " myEdgePos=" << myEdgePos << " dist=" << dist << "\n";
1780 : }
1781 987732 : 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 987732 : const int oldDir = myDir;
1788 987732 : const MSLane* oldLane = myLane;
1789 987732 : myLane = myNLI.lane;
1790 987732 : myDir = myNLI.dir;
1791 987732 : const bool normalLane = (myLane == nullptr || myLane->getEdge().getFunction() == SumoXMLEdgeFunc::NORMAL || &myLane->getEdge() == myStage->getNextRouteEdge());
1792 987732 : 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 987732 : if (myLane == nullptr) {
1804 230811 : myEdgePos = myStage->getArrivalPos();
1805 : }
1806 : // moveToNextEdge might destroy the person and thus mess up the heap. Better check first
1807 987732 : if (myStage->getRouteStep() == myStage->getRoute().end() - 1) {
1808 230811 : myLane = nullptr;
1809 : } else {
1810 756921 : 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 756921 : myNLI = getNextLane(*this, myLane, oldLane);
1815 : // reminders must be called after updated myNLI (to ensure that getNextEdgePtr returns the correct edge)
1816 756921 : myStage->activateEntryReminders(myPerson);
1817 : assert(myNLI.lane != oldLane); // do not turn around
1818 756921 : if DEBUGCOND(*this) {
1819 0 : std::cout << " nextLane=" << (myNLI.lane == nullptr ? "NULL" : myNLI.lane->getID()) << "\n";
1820 : }
1821 756921 : if (myLane->isWalkingArea()) {
1822 346841 : if (myNLI.dir != UNDEFINED_DIRECTION) {
1823 346804 : myWalkingAreaPath = getWalkingAreaPath(&myLane->getEdge(), oldLane, myNLI.lane);
1824 : assert(myWalkingAreaPath->shape.size() >= 2);
1825 346804 : 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 410080 : 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 756917 : const double newLength = (myWalkingAreaPath == nullptr ? myLane->getLength() : myWalkingAreaPath->length);
1865 756917 : 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 756917 : if (myDir == BACKWARD) {
1872 145858 : myEdgePos = newLength + dist;
1873 : } else {
1874 611059 : myEdgePos = -dist;
1875 : }
1876 756917 : 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 756917 : if (myDir != oldDir) {
1885 32708 : myPosLat = (numStripes(oldLane) - 1) * stripeWidth - myPosLat;
1886 : }
1887 : // adjust to differences in sidewalk width
1888 1500620 : const int offset = getStripeOffset(numStripes(oldLane), numStripes(myLane), oldDir != myDir && numStripes(myLane) < numStripes(oldLane));
1889 756917 : myPosLat += offset * stripeWidth;
1890 756917 : 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 987728 : myAngle = std::numeric_limits<double>::max(); // see #9014
1900 987728 : return true;
1901 : } else {
1902 : return false;
1903 : }
1904 : }
1905 :
1906 :
1907 : int
1908 71177481 : MSPModel_Striping::getReserved(int stripes, double factor) {
1909 71177481 : return MIN2(
1910 71177481 : (int)floor(stripes * factor),
1911 71177481 : (int)floor(RESERVE_FOR_ONCOMING_MAX / stripeWidth));
1912 : }
1913 :
1914 : void
1915 71106446 : MSPModel_Striping::PState::walk(const Obstacles& obs) {
1916 71106446 : const int stripes = (int)obs.size();
1917 71106446 : 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 71106446 : const double vMax = (myStage->getConfiguredSpeed() >= 0
1922 71106446 : ? myStage->getConfiguredSpeed()
1923 9173763 : : (USE_NET_SPEEDS || myLane->isNormal() || myLane->isInternal()
1924 79499229 : ? myLane->getVehicleMaxSpeed(myPerson)
1925 725935 : : myStage->getMaxSpeed(myPerson)));
1926 : // ultimate goal is to choose the preferred stripe (chosen)
1927 71106446 : const int current = stripe();
1928 71106446 : const int other = otherStripe();
1929 : // compute distances
1930 71106446 : std::vector<double> distance(stripes);
1931 307778547 : for (int i = 0; i < stripes; ++i) {
1932 236672101 : distance[i] = distanceTo(obs[i], obs[i].type == OBSTACLE_PED);
1933 : }
1934 : // compute utility for all stripes
1935 71106446 : std::vector<double> utility(stripes, 0);
1936 : // forbid stripes which are blocked and also all stripes behind them
1937 307778547 : for (int i = 0; i < stripes; ++i) {
1938 236672101 : if (distance[i] == DIST_OVERLAP) {
1939 108839656 : if (i == current && (!isWaitingToEnter() || stripe() != stripe(myPosLat))) {
1940 8070916 : utility[i] += OBSTRUCTED_PENALTY;
1941 : }
1942 108839656 : if (i < current) {
1943 149593957 : for (int j = 0; j <= i; ++j) {
1944 90945551 : utility[j] += OBSTRUCTED_PENALTY;
1945 : }
1946 : }
1947 108839656 : if (i > current) {
1948 69934483 : for (int j = i; j < stripes; ++j) {
1949 40299485 : 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 71106446 : const bool onJunction = myLane->isWalkingArea() || myLane->isCrossing();
1958 71106446 : const int reserved = getReserved(stripes, (onJunction ? RESERVE_FOR_ONCOMING_FACTOR_JUNCTIONS : RESERVE_FOR_ONCOMING_FACTOR));
1959 71106446 : if (myDir == FORWARD) {
1960 75066174 : for (int i = 0; i < reserved; ++i) {
1961 17835553 : utility[i] += INAPPROPRIATE_PENALTY * (i == current ? 0.5 : 1);
1962 : }
1963 : } else {
1964 6903635 : for (int i = sMax; i > sMax - reserved; --i) {
1965 2829532 : utility[i] += INAPPROPRIATE_PENALTY * (i == current ? 0.5 : 1);
1966 : }
1967 : }
1968 : // adapt utility based on obstacles
1969 307778547 : for (int i = 0; i < stripes; ++i) {
1970 236672101 : if (obs[i].speed * myDir < 0) {
1971 : // penalize evasion to the left unless the obstacle is a vehicle
1972 4450268 : if ((myDir == FORWARD || obs[i].type == OBSTACLE_VEHICLE) && i > 0) {
1973 857216 : utility[i - 1] -= 0.5;
1974 3593052 : } else if (myDir == BACKWARD && i < sMax) {
1975 589519 : utility[i + 1] -= 0.5;
1976 : }
1977 : }
1978 : // compute expected distance achievable by staying on this stripe for a time horizon
1979 236672101 : const double walkDist = MAX2(0., distance[i]); // disregard special distance flags
1980 236672101 : const double lookAhead = obs[i].speed * myDir >= 0 ? LOOKAHEAD_SAMEDIR : LOOKAHEAD_ONCOMING;
1981 236672101 : const double expectedDist = MIN2(vMax * LOOKAHEAD_SAMEDIR, walkDist + obs[i].speed * myDir * lookAhead);
1982 236672101 : if (expectedDist >= 0) {
1983 233555392 : utility[i] += expectedDist;
1984 : } else {
1985 : // let only the distance count
1986 3116709 : utility[i] += ONCOMING_CONFLICT_PENALTY + distance[i];
1987 : }
1988 : }
1989 : // discourage use of the leftmost stripe (in walking direction) if there are oncoming
1990 71106446 : if (myDir == FORWARD && obs[0].speed < 0) {
1991 1500929 : utility[0] += ONCOMING_CONFLICT_PENALTY;
1992 69605517 : } else if (myDir == BACKWARD && obs[sMax].speed > 0) {
1993 1506008 : utility[sMax] += ONCOMING_CONFLICT_PENALTY;
1994 : }
1995 : // penalize lateral movement (if the current stripe permits walking)
1996 71106446 : if (distance[current] > 0 && myWaitingTime == 0) {
1997 131110315 : for (int i = 0; i < stripes; ++i) {
1998 102674542 : utility[i] += abs(i - current) * LATERAL_PENALTY;
1999 : }
2000 : }
2001 : // walk on the right side on shared space
2002 71106446 : if (myLane->getPermissions() != SVC_PEDESTRIAN && myDir == BACKWARD) {
2003 2372940 : for (int i = 0; i < stripes; ++i) {
2004 1977634 : if (i <= current) {
2005 1884352 : utility[i] += (sMax - i + 1) * LATERAL_PENALTY;
2006 : }
2007 : }
2008 : }
2009 :
2010 : // select best stripe
2011 : int chosen = current;
2012 307778547 : for (int i = 0; i < stripes; ++i) {
2013 236672101 : if (utility[i] > utility[chosen] && utility[i] >= INAPPROPRIATE_PENALTY * 0.5) {
2014 : chosen = i;
2015 : }
2016 : }
2017 : // compute speed components along both axes
2018 71106446 : const int next = (chosen == current ? current : (chosen < current ? current - 1 : current + 1));
2019 71106446 : double xDist = MIN3(distance[current], distance[other], distance[next]);
2020 71106446 : 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 487583 : const int nextOther = chosen < current ? current - 2 : current + 2;
2024 487583 : 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 71106446 : double xSpeed = MIN2(vMax, MAX2(0., DIST2SPEED(xDist - preferredGap)));
2029 71106446 : if (xSpeed < NUMERICAL_EPS) {
2030 : xSpeed = 0.;
2031 : }
2032 71106446 : if (DEBUGCOND(*this)) {
2033 0 : std::cout << " xSpeedPotential=" << xSpeed << "\n";
2034 : }
2035 : // avoid tiny steps
2036 : // XXX pressure from behind?
2037 71106446 : if (mySpeed == 0 && xDist < MIN_STARTUP_DIST &&
2038 : // unless walking towards a short lane
2039 32953419 : !(
2040 33237790 : (xDist == distance[current] && obs[current].type >= OBSTACLE_END)
2041 32953435 : || (xDist == distance[other] && obs[other].type >= OBSTACLE_END)
2042 32953419 : || (xDist == distance[next] && obs[next].type >= OBSTACLE_END))
2043 : ) {
2044 : xSpeed = 0;
2045 : }
2046 38153029 : if (xSpeed == 0) {
2047 40795958 : 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 40795958 : if (myWaitingTime > ((myLane->isCrossing()
2054 : // treat shared walkingarea like a crossing to avoid deadlocking vehicles
2055 40791998 : || (myLane->isWalkingArea() && obs[current].vehicle != nullptr && obs[current].vehicle->getWaitingTime() > jamTimeCrossing
2056 81588239 : && myWalkingAreaFoes.find(&myLane->getEdge()) != myWalkingAreaFoes.end())) ? jamTimeCrossing : jamTime)
2057 40708942 : || (sMax == 0 && obs[0].speed * myDir < 0 && myWaitingTime > jamTimeNarrow)
2058 81476315 : || myAmJammed) {
2059 : // squeeze slowly through the crowd ignoring others
2060 9334342 : if (!myAmJammed) {
2061 97858 : MSNet::getInstance()->getPersonControl().registerJammed();
2062 391432 : WRITE_WARNINGF(TL("Person '%' is jammed on edge '%', time=%."),
2063 : myPerson->getID(), myStage->getEdge()->getID(), time2string(SIMSTEP));
2064 97858 : myAmJammed = true;
2065 : }
2066 9334342 : xSpeed = vMax * jamFactor;
2067 : }
2068 30310488 : } else if (myAmJammed && stripe(myPosLat) >= 0 && stripe(myPosLat) <= sMax && xDist >= MIN_STARTUP_DIST) {
2069 88797 : myAmJammed = false;
2070 : }
2071 : // dawdling
2072 71106446 : const double dawdle = MIN2(xSpeed, RandHelper::rand() * vMax * dawdling);
2073 71106446 : 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 71106446 : const double maxYSpeed = MIN2(MAX2(vMax * LATERAL_SPEED_FACTOR, vMax - xSpeed), stripeWidth);
2087 : double ySpeed = 0;
2088 : double yDist = 0;
2089 71106446 : if (utility[next] > OBSTRUCTION_THRESHOLD && utility[chosen] > OBSTRUCTION_THRESHOLD) {
2090 : // don't move laterally if the stripes are blocked
2091 63084945 : yDist = (chosen * stripeWidth) - myPosLat;
2092 63084945 : if (fabs(yDist) > NUMERICAL_EPS) {
2093 2755210 : ySpeed = (yDist > 0 ?
2094 1264684 : MIN2(maxYSpeed, DIST2SPEED(yDist)) :
2095 1490526 : MAX2(-maxYSpeed, DIST2SPEED(yDist)));
2096 : }
2097 8021501 : } else if (utility[next] <= OBSTRUCTION_THRESHOLD && obs[next].type == OBSTACLE_VEHICLE
2098 : // still on the road
2099 9793 : && stripe() == stripe(myPosLat)
2100 : // only when the vehicle is moving on the same lane
2101 8026913 : && !(myLane->isCrossing() || myLane->isWalkingArea())) {
2102 : // step aside to let the vehicle pass
2103 3126 : int stepAsideDir = myDir;
2104 3126 : 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 3126 : myAmJammed = true; // ignore pedestrian-pedestrian collisions
2110 3126 : ySpeed = stepAsideDir * vMax;
2111 : }
2112 :
2113 : // DEBUG
2114 71106446 : 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 71106446 : myEdgePos += SPEED2DIST(xSpeed * myDir);
2159 71106446 : myPosLat += SPEED2DIST(ySpeed);
2160 71106446 : mySpeedLat = ySpeed;
2161 71106446 : mySpeed = xSpeed;
2162 71106446 : if (xSpeed >= SUMO_const_haltingSpeed) {
2163 35288750 : myWaitingToEnter = false;
2164 35288750 : myWaitingTime = 0;
2165 : } else {
2166 35817696 : myWaitingTime += DELTA_T;
2167 35817696 : myTotalWaitingTime += DELTA_T;
2168 : }
2169 71106446 : myAngle = std::numeric_limits<double>::max(); // set on first access or via remote control
2170 71106446 : }
2171 :
2172 :
2173 : double
2174 12348481 : MSPModel_Striping::PState::getImpatience() const {
2175 12348481 : return MAX2(0., MIN2(1., myPerson->getVehicleType().getImpatience()
2176 12348481 : + STEPS2TIME(myStage->getWaitingTime()) / MAX_WAIT_TOLERANCE));
2177 : }
2178 :
2179 :
2180 : Position
2181 10959675 : MSPModel_Striping::PState::getPosition(const MSStageMoving& stage, SUMOTime) const {
2182 : if (myRemoteXYPos != Position::INVALID) {
2183 6275 : return myRemoteXYPos;
2184 : }
2185 10953400 : if (myLane == nullptr) {
2186 : // pedestrian has already finished
2187 229 : return Position::INVALID;
2188 : }
2189 10953171 : const double lateral_offset = -getLatOffset(); // the minus is hunting me in my dreams but seems to be here for historical reasons
2190 10953171 : if (myWalkingAreaPath == nullptr) {
2191 1116373 : 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 9836798 : if (myWalkingAreaPath->angleOverride == INVALID_DOUBLE) {
2205 9649247 : return myWalkingAreaPath->shape.positionAtOffset(myEdgePos, lateral_offset);
2206 : } else {
2207 187551 : const double rotationOffset = myDir == FORWARD ? 0 : DEG2RAD(180);
2208 187551 : return myWalkingAreaPath->shape.sidePositionAtAngle(myEdgePos, lateral_offset, myWalkingAreaPath->angleOverride + rotationOffset);
2209 : }
2210 : }
2211 : }
2212 :
2213 :
2214 : double
2215 2870167 : MSPModel_Striping::PState::getAngle(const MSStageMoving&, SUMOTime) const {
2216 2870167 : if (myAngle != std::numeric_limits<double>::max()) {
2217 : return myAngle;
2218 : }
2219 934464 : if (myLane == nullptr) {
2220 : // pedestrian has already finished
2221 : return 0;
2222 : }
2223 934233 : if (myWalkingAreaPath != nullptr && myWalkingAreaPath->angleOverride != INVALID_DOUBLE) {
2224 : return myWalkingAreaPath->angleOverride;
2225 : }
2226 883934 : const PositionVector& shp = myWalkingAreaPath == nullptr ? myLane->getShape() : myWalkingAreaPath->shape;
2227 883934 : double geomX = myWalkingAreaPath == nullptr ? myLane->interpolateLanePosToGeometryPos(myEdgePos) : myEdgePos;
2228 883934 : double angle = shp.rotationAtOffset(geomX) + (myDir == MSPModel::BACKWARD ? M_PI : 0);
2229 883934 : if (myDir == MSPModel::BACKWARD) {
2230 277079 : angle += atan2(mySpeedLat, MAX2(mySpeed, NUMERICAL_EPS));
2231 : } else { // myDir == MSPModel::FORWARD
2232 769399 : angle -= atan2(mySpeedLat, MAX2(mySpeed, NUMERICAL_EPS));
2233 : }
2234 883934 : if (angle > M_PI) {
2235 193571 : angle -= 2 * M_PI;
2236 : }
2237 883934 : myAngle = angle;
2238 883934 : return angle;
2239 : }
2240 :
2241 :
2242 : const MSEdge*
2243 166743 : MSPModel_Striping::PState::getNextEdge(const MSStageMoving&) const {
2244 166743 : return myNLI.lane == nullptr ? nullptr : &myNLI.lane->getEdge();
2245 : }
2246 :
2247 :
2248 : const MSLane*
2249 1686 : MSPModel_Striping::PState::getNextCrossing() const {
2250 1686 : return myNLI.lane != nullptr && myNLI.lane->isCrossing() ? myNLI.lane : nullptr;
2251 : }
2252 :
2253 :
2254 : void
2255 3447 : MSPModel_Striping::PState::reverse(const double pathLength, const double usableWidth) {
2256 3447 : myEdgePos = pathLength - myEdgePos;
2257 3447 : myPosLat = usableWidth - myPosLat;
2258 3447 : myDir = -myWalkingAreaPath->dir;
2259 3447 : mySpeedLat = -mySpeedLat;
2260 3447 : }
2261 :
2262 :
2263 : void
2264 256806 : MSPModel_Striping::PState::reset(const double edgePos, const double latPos) {
2265 256806 : myEdgePos = edgePos;
2266 256806 : myPosLat = latPos;
2267 256806 : myDir = UNDEFINED_DIRECTION; // only an obstacle, speed may be orthogonal to dir
2268 256806 : mySpeed = 0.;
2269 256806 : mySpeedLat = 0.;
2270 256806 : }
2271 :
2272 :
2273 : void
2274 19 : 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 19 : const MSJunction* laneOnJunction = lane->isNormal() ? nullptr : lane->getEdge().getToJunction();
2279 19 : for (const MSEdge* edge : myStage->getRoute()) {
2280 : if (edge == &lane->getEdge()
2281 14 : || edge->getToJunction() == laneOnJunction
2282 19 : || edge->getFromJunction() == laneOnJunction) {
2283 : laneOnRoute = true;
2284 : break;
2285 : }
2286 0 : routeOffset++;
2287 : }
2288 19 : if (!laneOnRoute) {
2289 0 : throw ProcessError("Lane '" + lane->getID() + "' is not on the route of person '" + getID() + "'.");
2290 : }
2291 19 : Position pos = lane->geometryPositionAtOffset(lanePos, lanePosLat);
2292 19 : if (lane->isWalkingArea() && (myWalkingAreaPath == nullptr || myWalkingAreaPath->lane != lane)) {
2293 : // entered new walkingarea. Determine path to guess position
2294 9 : const MSEdge* prevEdge = myStage->getRoute()[routeOffset];
2295 9 : const MSEdge* nextEdge = routeOffset + 1 < (int)myStage->getRoute().size() ? myStage->getRoute()[routeOffset + 1] : nullptr;
2296 9 : const WalkingAreaPath* guessed = guessPath(&lane->getEdge(), prevEdge, nextEdge);
2297 9 : const double maxPos = guessed->shape.length() - NUMERICAL_EPS;
2298 9 : 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 9 : lanePos = MIN2(maxPos, MAX2(NUMERICAL_EPS, lanePos));
2305 9 : pos = guessed->shape.positionAtOffset(lanePos, lanePosLat);
2306 : }
2307 19 : const double angle = GeomHelper::naviDegree(p->getPosition().angleTo2D(pos));
2308 19 : moveToXY(p, pos, lane, lanePos, lanePosLat, angle, routeOffset, newEdges, t);
2309 19 : }
2310 :
2311 :
2312 : void
2313 14041 : 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 14041 : MSPModel_Striping* pm = dynamic_cast<MSPModel_Striping*>(MSNet::getInstance()->getPersonControl().getMovementModel());
2317 : assert(p == myPerson);
2318 : assert(pm != nullptr);
2319 14041 : const double oldAngle = GeomHelper::naviDegree(getAngle(*myStage, t));
2320 : // person already walking in this step. undo this to obtain the previous position
2321 14041 : const double oldX = myEdgePos - SPEED2DIST(mySpeed * myDir);
2322 : const double tmp = myEdgePos;
2323 14041 : myEdgePos = oldX;
2324 14041 : Position oldPos = getPosition(*myStage, t);
2325 14041 : myEdgePos = tmp;
2326 : //if (oldPos == Position::INVALID) {
2327 : // oldPos = pos
2328 : //}
2329 14041 : 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 14041 : if (lane != myLane && myLane != nullptr && lane != nullptr) {
2345 360 : pm->remove(this);
2346 : pm->registerActive();
2347 : }
2348 14041 : if (lane != nullptr &&
2349 10909 : fabs(lanePosLat) < (0.5 * (lane->getWidth() + p->getVehicleType().getWidth()) + SIDEWALK_OFFSET)) {
2350 10904 : myRemoteXYPos = Position::INVALID;
2351 10904 : const MSEdge* old = myStage->getEdge();
2352 10904 : const MSLane* oldLane = myLane;
2353 10904 : if (lane != myLane) {
2354 : // implicitly adds new active lane if necessary
2355 360 : pm->myActiveLanes[lane].push_back(this);
2356 : }
2357 10904 : if (edges.empty()) {
2358 : // map within route
2359 10854 : myStage->setRouteIndex(myPerson, routeOffset);
2360 : } else {
2361 50 : myStage->replaceRoute(myPerson, edges, routeOffset);
2362 : }
2363 10904 : if (!lane->getEdge().isNormal()) {
2364 496 : myStage->moveToNextEdge(myPerson, t, myDir, &lane->getEdge());
2365 : }
2366 :
2367 10904 : myLane = lane;
2368 10904 : const double lateral_offset = (lane->getWidth() - stripeWidth) * 0.5;
2369 10904 : if (lane->isWalkingArea()) {
2370 242 : if (myWalkingAreaPath == nullptr || myWalkingAreaPath->lane != lane) {
2371 : // entered new walkingarea. Determine path
2372 52 : 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 242 : 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 242 : myEdgePos = relPos.x();
2388 242 : myPosLat = lateral_offset + relPos.y();
2389 : }
2390 : } else {
2391 10662 : myWalkingAreaPath = nullptr;
2392 10662 : myEdgePos = lanePos;
2393 10662 : myPosLat = lateral_offset - lanePosLat;
2394 10662 : lane->requireCollisionCheck();
2395 : }
2396 : // guess direction
2397 10904 : const double angleDiff = GeomHelper::getMinAngleDiff(angle, oldAngle);
2398 10904 : if (myStage->getNextRouteEdge() != nullptr) {
2399 6511 : if (myStage->getEdge()->getToJunction() == myStage->getNextRouteEdge()->getFromJunction() ||
2400 963 : myStage->getEdge()->getToJunction() == myStage->getNextRouteEdge()->getToJunction()) {
2401 4710 : myDir = FORWARD;
2402 : } else {
2403 838 : myDir = BACKWARD;
2404 : }
2405 : } else {
2406 : // guess from angle
2407 5356 : if (angleDiff <= 90) {
2408 : // keep direction
2409 5292 : if (myDir == UNDEFINED_DIRECTION) {
2410 0 : myDir = FORWARD;
2411 : }
2412 : } else {
2413 : // change direction
2414 64 : myDir = (myDir == BACKWARD) ? FORWARD : BACKWARD;
2415 : }
2416 : }
2417 : // update next lane info (after guessing direction)
2418 10904 : if (oldLane == nullptr || &oldLane->getEdge() != &myLane->getEdge()) {
2419 310 : 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 310 : myNLI = getNextLane(*this, sidewalk == nullptr ? myLane : sidewalk, nullptr);
2423 310 : 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 10904 : if (oldLane == myLane) {
2435 10544 : 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 360 : mySpeed = DIST2SPEED(oldPos.distanceTo2D(pos));
2439 : }
2440 : } else {
2441 : // map outside the network
2442 3137 : myRemoteXYPos = pos;
2443 3137 : mySpeed = DIST2SPEED(oldPos.distanceTo2D(pos));
2444 : }
2445 :
2446 14041 : }
2447 :
2448 :
2449 : double
2450 746828 : MSPModel_Striping::PState::getPathLength() const {
2451 746828 : if (myWalkingAreaPath != nullptr) {
2452 689914 : return myWalkingAreaPath->length;
2453 : } else {
2454 : return 0;
2455 : }
2456 : }
2457 :
2458 : double
2459 1586507373 : MSPModel_Striping::PState::distanceTo(const Obstacle& obs, const bool includeMinGap) const {
2460 : // check for overlap
2461 1586507373 : const double maxX = getMaxX(includeMinGap);
2462 1586507373 : 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 1586507373 : if ((obs.xFwd >= maxX && obs.xBack <= maxX) || (obs.xFwd <= maxX && obs.xFwd >= minX)) {
2467 : // avoid blocking by itself on looped route
2468 464710851 : return (obs.type == OBSTACLE_PED && obs.description == myPerson->getID()) ? DIST_FAR_AWAY : DIST_OVERLAP;
2469 : }
2470 1121796522 : if (myDir == FORWARD) {
2471 1010841956 : return obs.xFwd < minX ? DIST_BEHIND : obs.xBack - maxX;
2472 : } else {
2473 110954566 : return obs.xBack > maxX ? DIST_BEHIND : minX - obs.xFwd;
2474 : }
2475 : }
2476 :
2477 :
2478 : void
2479 91858593 : MSPModel_Striping::PState::mergeObstacles(Obstacles& into, const Obstacles& obs2) {
2480 402144274 : for (int i = 0; i < (int)into.size(); ++i) {
2481 310285681 : 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 310285681 : const double dO = distanceTo(obs2[i], obs2[i].type == OBSTACLE_PED);
2487 310285681 : const double dI = distanceTo(into[i], into[i].type == OBSTACLE_PED);
2488 310285681 : if (dO < dI) {
2489 : into[i] = obs2[i];
2490 : } else if (dO == dI
2491 77050120 : && into[i].type != OBSTACLE_PED
2492 33845065 : && into[i].type != OBSTACLE_VEHICLE
2493 297876171 : && (obs2[i].type == OBSTACLE_PED ||
2494 : obs2[i].type == OBSTACLE_VEHICLE)) {
2495 : into[i] = obs2[i];
2496 : }
2497 : }
2498 91858593 : }
2499 :
2500 : void
2501 18641 : MSPModel_Striping::PState::mergeObstacles(Obstacles& into, const Obstacles& obs2, int dir, int offset) {
2502 146450 : for (int i = 0; i < (int)into.size(); ++i) {
2503 127809 : int i2 = i + offset;
2504 127809 : if (i2 >= 0 && i2 < (int)obs2.size()) {
2505 126473 : if (dir == FORWARD) {
2506 90460 : 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 18641 : }
2517 :
2518 :
2519 : bool
2520 12348481 : MSPModel_Striping::PState::ignoreRed(const MSLink* link) const {
2521 12348481 : if (link->haveRed()) {
2522 614526 : const double ignoreRedTime = myPerson->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_AFTER_RED_TIME, -1);
2523 614526 : 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 11688910 : MSPModel_Striping::PState::stopForYellow(const MSLink* link) const {
2540 : // main use case is at rail_crossing
2541 11688910 : 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 3664358423 : MSPModel_Striping::PState::getWidth() const {
2559 3664358423 : return myPerson->getVehicleType().getWidth();
2560 : }
2561 :
2562 :
2563 : bool
2564 134858524 : MSPModel_Striping::PState::isRemoteControlled() const {
2565 134858524 : return myPerson->hasInfluencer() && myPerson->getInfluencer().isRemoteControlled();
2566 : }
2567 :
2568 : // ===========================================================================
2569 : // MSPModel_Striping::PStateVehicle method definitions
2570 : // ===========================================================================
2571 :
2572 504822 : MSPModel_Striping::PStateVehicle::PStateVehicle(const MSVehicle* veh, const MSLane* walkingarea, double relX, double relY, double xWidth, double yWidth):
2573 504822 : myVehicle(veh), myXWidth(xWidth), myYWidth(yWidth) {
2574 504822 : 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 504822 : myEdgePos = relX + xWidth / 2;
2578 504822 : myPosLat = relY;
2579 504822 : }
2580 :
2581 : const std::string&
2582 1970448 : MSPModel_Striping::PStateVehicle::getID() const {
2583 1970448 : return myVehicle->getID();
2584 : }
2585 :
2586 : double
2587 1018751 : MSPModel_Striping::PStateVehicle::getWidth() const {
2588 1018751 : return myYWidth;
2589 : }
2590 :
2591 : double
2592 539616 : MSPModel_Striping::PStateVehicle::getMinX(const bool /*includeMinGap*/) const {
2593 539616 : return myXWidth > 0 ? myEdgePos - myXWidth : myEdgePos;
2594 : }
2595 :
2596 : double
2597 539616 : MSPModel_Striping::PStateVehicle::getMaxX(const bool /*includeMinGap*/) const {
2598 539616 : return myXWidth > 0 ? myEdgePos : myEdgePos - myXWidth;
2599 : }
2600 :
2601 : // ===========================================================================
2602 : // MSPModel_Striping::MovePedestrians method definitions
2603 : // ===========================================================================
2604 :
2605 : SUMOTime
2606 3129246 : MSPModel_Striping::MovePedestrians::execute(SUMOTime currentTime) {
2607 : std::set<MSPerson*> changedLane;
2608 3129246 : myModel->moveInDirection(currentTime, changedLane, FORWARD);
2609 3129242 : 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 3129242 : return DELTA_T;
2628 : }
|
