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