Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2014-2024 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file MSPModel_Striping.cpp
15 : /// @author Jakob Erdmann
16 : /// @author Michael Behrisch
17 : /// @date Mon, 13 Jan 2014
18 : ///
19 : // The pedestrian following model (prototype)
20 : /****************************************************************************/
21 : #include <config.h>
22 :
23 : #include <cmath>
24 : #include <algorithm>
25 : #include <utils/common/RandHelper.h>
26 : #include <utils/geom/GeomHelper.h>
27 : #include <utils/options/OptionsCont.h>
28 : #include <utils/router/PedestrianRouter.h>
29 : #include <microsim/MSNet.h>
30 : #include <microsim/MSEdge.h>
31 : #include <microsim/MSEventControl.h>
32 : #include <microsim/MSLane.h>
33 : #include <microsim/MSLink.h>
34 : #include <microsim/MSJunction.h>
35 : #include <microsim/MSStoppingPlace.h>
36 : #include <microsim/MSVehicleControl.h>
37 : #include <microsim/MSGlobals.h>
38 : #include <microsim/transportables/MSStage.h>
39 : #include <microsim/transportables/MSTransportableControl.h>
40 : #include "MSPModel_Striping.h"
41 :
42 :
43 : // ===========================================================================
44 : // DEBUGGING HELPERS
45 : // ===========================================================================
46 : //
47 : #define DEBUGID1 ""
48 : #define DEBUGID2 ""
49 : //#define DEBUGCOND(PED) (false)
50 : //#define DEBUGCOND(PED) ((PED).myPerson->getID() == DEBUGID1 || (PED).myPerson->getID() == DEBUGID2)
51 : #define DEBUGCOND(PED) ((PED).myPerson->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 : MSPModel_Striping::Pedestrians MSPModel_Striping::noPedestrians;
84 :
85 : // model parameters (static to simplify access from class PState
86 : double MSPModel_Striping::stripeWidth;
87 : double MSPModel_Striping::dawdling;
88 : double MSPModel_Striping::minGapToVehicle;
89 : int MSPModel_Striping::myWalkingAreaDetail;
90 : SUMOTime MSPModel_Striping::jamTime;
91 : SUMOTime MSPModel_Striping::jamTimeCrossing;
92 : SUMOTime MSPModel_Striping::jamTimeNarrow;
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::LOOKAROUND_VEHICLES(60.0); // meters
97 : const double MSPModel_Striping::LATERAL_PENALTY(-1.); // meters
98 : const double MSPModel_Striping::OBSTRUCTED_PENALTY(-300000.); // meters
99 : const double MSPModel_Striping::INAPPROPRIATE_PENALTY(-20000.); // meters
100 : const double MSPModel_Striping::ONCOMING_CONFLICT_PENALTY(-1000.); // meters
101 : const double MSPModel_Striping::OBSTRUCTION_THRESHOLD(MSPModel_Striping::OBSTRUCTED_PENALTY * 0.5); // despite obstruction, additional utility may have been added
102 : const double MSPModel_Striping::SQUEEZE(0.7);
103 : double MSPModel_Striping::RESERVE_FOR_ONCOMING_FACTOR;
104 : double MSPModel_Striping::RESERVE_FOR_ONCOMING_FACTOR_JUNCTIONS;
105 : double MSPModel_Striping::RESERVE_FOR_ONCOMING_MAX;
106 : const double MSPModel_Striping::MAX_WAIT_TOLERANCE(120.); // seconds
107 : const double MSPModel_Striping::LATERAL_SPEED_FACTOR(0.4);
108 : const double MSPModel_Striping::MIN_STARTUP_DIST(0.4); // meters
109 :
110 :
111 : // ===========================================================================
112 : // MSPModel_Striping method definitions
113 : // ===========================================================================
114 :
115 3246 : MSPModel_Striping::MSPModel_Striping(const OptionsCont& oc, MSNet* net) :
116 3246 : myNumActivePedestrians(0),
117 3246 : myAmActive(false) {
118 3246 : myWalkingAreaDetail = oc.getInt("pedestrian.striping.walkingarea-detail");
119 3246 : initWalkingAreaPaths(net);
120 : // configurable parameters
121 3246 : stripeWidth = oc.getFloat("pedestrian.striping.stripe-width");
122 3246 : MSVehicleType* defaultPedType = MSNet::getInstance()->getVehicleControl().getVType(DEFAULT_PEDTYPE_ID, nullptr, true);
123 3246 : 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 3246 : dawdling = oc.getFloat("pedestrian.striping.dawdling");
129 3246 : minGapToVehicle = oc.getFloat("pedestrian.striping.mingap-to-vehicle");
130 3246 : RESERVE_FOR_ONCOMING_FACTOR = oc.getFloat("pedestrian.striping.reserve-oncoming");
131 3246 : RESERVE_FOR_ONCOMING_FACTOR_JUNCTIONS = oc.getFloat("pedestrian.striping.reserve-oncoming.junctions");
132 3246 : RESERVE_FOR_ONCOMING_MAX = oc.getFloat("pedestrian.striping.reserve-oncoming.max");
133 :
134 3246 : jamTime = string2time(oc.getString("pedestrian.striping.jamtime"));
135 3246 : if (jamTime <= 0) {
136 4 : jamTime = SUMOTime_MAX;
137 : }
138 3246 : jamTimeCrossing = string2time(oc.getString("pedestrian.striping.jamtime.crossing"));
139 3246 : if (jamTimeCrossing <= 0) {
140 0 : jamTimeCrossing = SUMOTime_MAX;
141 : }
142 3246 : jamTimeNarrow = string2time(oc.getString("pedestrian.striping.jamtime.narrow"));
143 3246 : if (jamTimeNarrow <= 0) {
144 0 : jamTimeNarrow = SUMOTime_MAX;
145 : }
146 3246 : myLegacyPosLat = oc.getBool("pedestrian.striping.legacy-departposlat");
147 3246 : }
148 :
149 :
150 6476 : MSPModel_Striping::~MSPModel_Striping() {
151 3238 : clearState();
152 : myWalkingAreaPaths.clear(); // need to recompute when lane pointers change
153 : myWalkingAreaFoes.clear();
154 : myMinNextLengths.clear();
155 6476 : }
156 :
157 : void
158 6506 : MSPModel_Striping::clearState() {
159 : myActiveLanes.clear();
160 6506 : myNumActivePedestrians = 0;
161 6506 : myAmActive = false;
162 6506 : }
163 :
164 : MSTransportableStateAdapter*
165 269559 : MSPModel_Striping::add(MSTransportable* transportable, MSStageMoving* stage, SUMOTime) {
166 269559 : if (!transportable->isPerson()) {
167 : // containers are not supported (TODO add a warning here?)
168 : return nullptr;
169 : }
170 : MSPerson* person = static_cast<MSPerson*>(transportable);
171 269559 : MSNet* net = MSNet::getInstance();
172 269559 : if (!myAmActive) {
173 2158 : net->getBeginOfTimestepEvents()->addEvent(new MovePedestrians(this), net->getCurrentTimeStep() + DELTA_T);
174 2158 : myAmActive = true;
175 : }
176 : assert(person->getCurrentStageType() == MSStageType::WALKING);
177 269559 : const MSLane* lane = stage->checkDepartLane(person->getEdge(), person->getVClass(), stage->getDepartLane(), person->getID());
178 269559 : if (lane == nullptr) {
179 8 : const char* error = TL("Person '%' could not find sidewalk on edge '%', time=%.");
180 16 : if (OptionsCont::getOptions().getBool("ignore-route-errors")) {
181 24 : WRITE_WARNINGF(error, person->getID(), person->getEdge()->getID(), time2string(net->getCurrentTimeStep()));
182 8 : return nullptr;
183 : } else {
184 0 : throw ProcessError(TLF(error, person->getID(), person->getEdge()->getID(), time2string(net->getCurrentTimeStep())));
185 : }
186 : }
187 269551 : PState* ped = new PState(person, stage, lane);
188 269547 : myActiveLanes[lane].push_back(ped);
189 269547 : myNumActivePedestrians++;
190 269547 : return ped;
191 : }
192 :
193 :
194 : MSTransportableStateAdapter*
195 20 : MSPModel_Striping::loadState(MSTransportable* transportable, MSStageMoving* stage, std::istringstream& in) {
196 : MSPerson* person = static_cast<MSPerson*>(transportable);
197 20 : MSNet* net = MSNet::getInstance();
198 20 : if (!myAmActive) {
199 16 : net->getBeginOfTimestepEvents()->addEvent(new MovePedestrians(this), SIMSTEP);
200 16 : myAmActive = true;
201 : }
202 20 : PState* ped = new PState(person, stage, &in);
203 20 : myActiveLanes[ped->getLane()].push_back(ped);
204 20 : myNumActivePedestrians++;
205 20 : return ped;
206 : }
207 :
208 :
209 : void
210 2276 : MSPModel_Striping::remove(MSTransportableStateAdapter* state) {
211 2276 : const MSLane* lane = dynamic_cast<PState*>(state)->myLane;
212 2276 : Pedestrians& pedestrians = myActiveLanes[lane];
213 52788 : for (Pedestrians::iterator it = pedestrians.begin(); it != pedestrians.end(); ++it) {
214 52788 : if (*it == state) {
215 : pedestrians.erase(it);
216 2276 : myNumActivePedestrians--;
217 : return;
218 : }
219 : }
220 : }
221 :
222 :
223 : bool
224 3155613 : MSPModel_Striping::blockedAtDist(const SUMOTrafficObject* ego, const MSLane* lane, double vehSide, double vehWidth,
225 : double oncomingGap, std::vector<const MSPerson*>* collectBlockers) {
226 3155613 : const Pedestrians& pedestrians = getPedestrians(lane);
227 3162815 : for (Pedestrians::const_iterator it_ped = pedestrians.begin(); it_ped != pedestrians.end(); ++it_ped) {
228 33147 : const PState& ped = **it_ped;
229 33147 : const double leaderFrontDist = (ped.myDir == FORWARD ? vehSide - ped.myRelX : ped.myRelX - vehSide);
230 33147 : const double leaderBackDist = leaderFrontDist + ped.getLength();
231 33147 : if DEBUGCOND(ped) {
232 0 : std::cout << SIMTIME << " lane=" << lane->getID() << " dir=" << ped.myDir << " pX=" << ped.myRelX << " pL=" << ped.getLength()
233 : << " vehSide=" << vehSide
234 : << " vehWidth=" << vehWidth
235 : << " lBD=" << leaderBackDist
236 : << " lFD=" << leaderFrontDist
237 0 : << "\n";
238 : }
239 33147 : if (leaderBackDist >= -vehWidth
240 27898 : && (leaderFrontDist < 0
241 : // give right of way to (close) approaching pedestrians unless they are standing
242 22218 : || (leaderFrontDist <= oncomingGap && ped.myWaitingTime < TIME2STEPS(2.0)))) {
243 25975 : if (MSLink::ignoreFoe(ego, ped.myPerson)) {
244 30 : continue;
245 : }
246 : // found one pedestrian that is not completely past the crossing point
247 : //std::cout << SIMTIME << " blocking pedestrian foeLane=" << lane->getID() << " ped=" << ped.myPerson->getID() << " dir=" << ped.myDir << " pX=" << ped.myRelX << " pL=" << ped.getLength() << " fDTC=" << distToCrossing << " lBD=" << leaderBackDist << "\n";
248 25945 : if (collectBlockers == nullptr) {
249 : return true;
250 : } else {
251 0 : collectBlockers->push_back(ped.myPerson);
252 : }
253 : }
254 : }
255 3129668 : if (collectBlockers == nullptr) {
256 : return false;
257 : } else {
258 0 : return collectBlockers->size() > 0;
259 : }
260 : }
261 :
262 :
263 : bool
264 3606600 : MSPModel_Striping::hasPedestrians(const MSLane* lane) {
265 3606600 : return getPedestrians(lane).size() > 0;
266 : }
267 :
268 :
269 : bool
270 94657 : MSPModel_Striping::usingInternalLanes() {
271 94657 : return usingInternalLanesStatic();
272 : }
273 :
274 : bool
275 269121 : MSPModel_Striping::usingInternalLanesStatic() {
276 269121 : return MSGlobals::gUsingInternalLanes && MSNet::getInstance()->hasInternalLinks() && MSNet::getInstance()->hasPedestrianNetwork();
277 : }
278 :
279 : PersonDist
280 722676 : MSPModel_Striping::nextBlocking(const MSLane* lane, double minPos, double minRight, double maxLeft, double stopTime, bool bidi) {
281 : PersonDist result((const MSPerson*)nullptr, std::numeric_limits<double>::max());
282 722676 : const Pedestrians& pedestrians = getPedestrians(lane);
283 3952937 : for (Pedestrians::const_iterator it_ped = pedestrians.begin(); it_ped != pedestrians.end(); ++it_ped) {
284 3230261 : const PState& ped = **it_ped;
285 : // account for distance covered by oncoming pedestrians
286 3230261 : double relX2 = ped.myRelX - (ped.myDir == FORWARD ? 0 : stopTime * ped.myPerson->getMaxSpeed());
287 3230261 : double dist = ((relX2 - minPos) * (bidi ? -1 : 1)
288 3230261 : - (ped.myDir == FORWARD ? ped.myPerson->getVehicleType().getLength() : 0));
289 3230261 : const bool aheadOfVehicle = bidi ? ped.myRelX < minPos : ped.myRelX > minPos;
290 3230261 : if (aheadOfVehicle && dist < result.second) {
291 1378793 : const double center = lane->getWidth() - (ped.myRelY + stripeWidth * 0.5);
292 1378793 : const double halfWidth = 0.5 * ped.myPerson->getVehicleType().getWidth();
293 1378793 : const bool overlap = (center + halfWidth > minRight && center - halfWidth < maxLeft);
294 1378793 : if DEBUGCOND(ped) {
295 0 : std::cout << " nextBlocking lane=" << lane->getID() << " bidi=" << bidi
296 : << " minPos=" << minPos << " minRight=" << minRight << " maxLeft=" << maxLeft
297 : << " stopTime=" << stopTime
298 0 : << " pedY=" << ped.myRelY
299 0 : << " pedX=" << ped.myRelX
300 : << " relX2=" << relX2
301 : << " center=" << center
302 : << " pedLeft=" << center + halfWidth
303 0 : << " pedRight=" << center - halfWidth
304 0 : << " overlap=" << overlap
305 0 : << "\n";
306 : }
307 1378793 : if (overlap) {
308 125656 : result.first = ped.myPerson;
309 : result.second = dist;
310 : }
311 : }
312 : }
313 722676 : return result;
314 : }
315 :
316 :
317 : MSPModel_Striping::Pedestrians&
318 8606986 : MSPModel_Striping::getPedestrians(const MSLane* lane) {
319 : ActiveLanes::iterator it = myActiveLanes.find(lane);
320 8606986 : if (it != myActiveLanes.end()) {
321 : //std::cout << " found lane=" << lane->getID() << " n=" << it->second.size() << "\n";
322 3310259 : return (it->second);
323 : } else {
324 : return noPedestrians;
325 : }
326 : }
327 :
328 :
329 : int
330 3283778522 : MSPModel_Striping::numStripes(const MSLane* lane) {
331 3283778522 : return MAX2(1, (int)floor(lane->getWidth() / stripeWidth));
332 : }
333 :
334 : int
335 404614 : MSPModel_Striping::connectedDirection(const MSLane* from, const MSLane* to) {
336 404614 : if (from == nullptr || to == nullptr) {
337 0 : return UNDEFINED_DIRECTION;
338 404614 : } else if (from->getLinkTo(to) != nullptr) {
339 301426 : return FORWARD;
340 103188 : } else if (to->getLinkTo(from) != nullptr) {
341 103188 : return BACKWARD;
342 : } else {
343 0 : return UNDEFINED_DIRECTION;
344 : }
345 : }
346 :
347 :
348 : void
349 3246 : MSPModel_Striping::initWalkingAreaPaths(const MSNet*) {
350 3246 : if (myWalkingAreaPaths.size() > 0) {
351 : return;
352 : }
353 : // collect vehicle lanes that cross walkingareas
354 279199 : for (MSEdgeVector::const_iterator i = MSEdge::getAllEdges().begin(); i != MSEdge::getAllEdges().end(); ++i) {
355 275953 : const MSEdge* edge = *i;
356 275953 : if (!edge->isWalkingArea() && !edge->isCrossing()) {
357 528000 : for (MSLane* lane : edge->getLanes()) {
358 655653 : for (MSLink* link : lane->getLinkCont()) {
359 368254 : if (link->getWalkingAreaFoe() != nullptr) {
360 : // link is an exit link
361 4652 : myWalkingAreaFoes[&link->getWalkingAreaFoe()->getEdge()].push_back(link->getLaneBefore());
362 : //std::cout << " wa=" << link->getWalkingAreaFoe()->getEdge().getID() << " foe=" << link->getLaneBefore()->getID() << "\n";
363 : }
364 368254 : if (link->getWalkingAreaFoeExit() != nullptr) {
365 : // link is an exit link
366 4728 : myWalkingAreaFoes[&link->getWalkingAreaFoeExit()->getEdge()].push_back(link->getLaneBefore());
367 : //std::cout << " wa=" << link->getWalkingAreaFoeExit()->getEdge().getID() << " foe=" << link->getLaneBefore()->getID() << "\n";
368 : }
369 : }
370 : }
371 : }
372 : }
373 :
374 : // build walkingareaPaths
375 279199 : for (MSEdgeVector::const_iterator i = MSEdge::getAllEdges().begin(); i != MSEdge::getAllEdges().end(); ++i) {
376 275953 : insertWalkArePaths(*i, myWalkingAreaPaths);
377 : }
378 : }
379 :
380 :
381 : void
382 275953 : MSPModel_Striping::insertWalkArePaths(const MSEdge* edge, WalkingAreaPaths& into) {
383 275953 : if (edge->isWalkingArea()) {
384 29854 : const MSLane* walkingArea = getSidewalk<MSEdge, MSLane>(edge);
385 29854 : myMinNextLengths[walkingArea] = walkingArea->getLength();
386 : // build all possible paths across this walkingArea
387 : // gather all incident lanes
388 : std::vector<const MSLane*> lanes;
389 67204 : for (const MSEdge* in : edge->getPredecessors()) {
390 37350 : if (!in->isTazConnector()) {
391 36858 : lanes.push_back(getSidewalk<MSEdge, MSLane>(in));
392 36858 : if (lanes.back() == nullptr) {
393 0 : throw ProcessError("Invalid connection from edge '" + in->getID() + "' to walkingarea edge '" + edge->getID() + "'");
394 : }
395 : }
396 : }
397 67435 : for (const MSEdge* out : edge->getSuccessors()) {
398 37581 : if (!out->isTazConnector()) {
399 37089 : lanes.push_back(getSidewalk<MSEdge, MSLane>(out));
400 37089 : if (lanes.back() == nullptr) {
401 0 : throw ProcessError("Invalid connection from walkingarea edge '" + edge->getID() + "' to edge '" + out->getID() + "'");
402 : }
403 : }
404 : }
405 : // build all combinations
406 103801 : for (int j = 0; j < (int)lanes.size(); ++j) {
407 281824 : for (int k = 0; k < (int)lanes.size(); ++k) {
408 207877 : if (j != k) {
409 : // build the walkingArea
410 133930 : const MSLane* const from = lanes[j];
411 133930 : const MSLane* const to = lanes[k];
412 133930 : const int fromDir = from->getLinkTo(walkingArea) != nullptr ? FORWARD : BACKWARD;
413 133930 : const int toDir = walkingArea->getLinkTo(to) != nullptr ? FORWARD : BACKWARD;
414 133930 : PositionVector shape;
415 201256 : Position fromPos = from->getShape()[fromDir == FORWARD ? -1 : 0];
416 200534 : Position toPos = to->getShape()[toDir == FORWARD ? 0 : -1];
417 133930 : const double maxExtent = fromPos.distanceTo2D(toPos) / 4; // prevent sharp corners
418 133930 : const double extrapolateBy = MIN2(maxExtent, walkingArea->getWidth() / 2);
419 : // assemble shape
420 133930 : shape.push_back(fromPos);
421 133930 : if (extrapolateBy > POSITION_EPS) {
422 : PositionVector fromShp = from->getShape();
423 132538 : fromShp.extrapolate(extrapolateBy);
424 265076 : shape.push_back_noDoublePos(fromDir == FORWARD ? fromShp.back() : fromShp.front());
425 : PositionVector nextShp = to->getShape();
426 132538 : nextShp.extrapolate(extrapolateBy);
427 265076 : shape.push_back_noDoublePos(toDir == FORWARD ? nextShp.front() : nextShp.back());
428 132538 : }
429 133930 : shape.push_back_noDoublePos(toPos);
430 133930 : if (shape.size() < 2) {
431 : PositionVector fromShp = from->getShape();
432 622 : fromShp.extrapolate(1.5 * POSITION_EPS); // noDoublePos requires a difference of POSITION_EPS in at least one coordinate
433 1244 : shape.push_back_noDoublePos(fromDir == FORWARD ? fromShp.back() : fromShp.front());
434 : assert(shape.size() == 2);
435 133930 : } else if (myWalkingAreaDetail > 4) {
436 448 : shape = shape.bezier(myWalkingAreaDetail);
437 : }
438 : double angleOverride = INVALID_DOUBLE;
439 133930 : if (shape.size() >= 4 && shape.length() < walkingArea->getWidth()) {
440 27770 : const double aStart = shape.angleAt2D(0);
441 27770 : const double aEnd = shape.angleAt2D((int)shape.size() - 2);
442 27770 : if (fabs(aStart - aEnd) < DEG2RAD(10)) {
443 3632 : angleOverride = (aStart + aEnd) / 2;
444 : }
445 : }
446 133930 : if (fromDir == BACKWARD) {
447 : // will be walking backward on walkingArea
448 134652 : shape = shape.reverse();
449 : }
450 133930 : WalkingAreaPath wap = WalkingAreaPath(from, walkingArea, to, shape, fromDir, angleOverride);
451 0 : into.insert(std::make_pair(std::make_pair(from, to), wap));
452 267860 : myMinNextLengths[walkingArea] = MIN2(myMinNextLengths[walkingArea], wap.length);
453 133930 : }
454 : }
455 : }
456 : }
457 275953 : }
458 :
459 :
460 : const MSPModel_Striping::WalkingAreaPath*
461 10 : MSPModel_Striping::getArbitraryPath(const MSEdge* walkingArea) {
462 : assert(walkingArea->isWalkingArea());
463 : std::vector<const MSLane*> lanes;
464 30 : for (const MSEdge* const pred : walkingArea->getPredecessors()) {
465 20 : lanes.push_back(getSidewalk<MSEdge, MSLane>(pred));
466 : }
467 30 : for (const MSEdge* const succ : walkingArea->getSuccessors()) {
468 20 : lanes.push_back(getSidewalk<MSEdge, MSLane>(succ));
469 : }
470 10 : if (lanes.size() < 1) {
471 0 : throw ProcessError(TLF("Invalid walkingarea '%' does not allow continuation.", walkingArea->getID()));
472 : }
473 10 : return &myWalkingAreaPaths.find(std::make_pair(lanes.front(), lanes.back()))->second;
474 : }
475 :
476 : const MSPModel_Striping::WalkingAreaPath*
477 72 : MSPModel_Striping::guessPath(const MSEdge* walkingArea, const MSEdge* before, const MSEdge* after) {
478 : assert(walkingArea->isWalkingArea());
479 72 : const MSLane* swBefore = getSidewalk<MSEdge, MSLane>(before);
480 72 : const MSLane* swAfter = getSidewalk<MSEdge, MSLane>(after);
481 72 : const auto pathIt = myWalkingAreaPaths.find(std::make_pair(swBefore, swAfter));
482 72 : if (pathIt != myWalkingAreaPaths.end()) {
483 29 : return &pathIt->second;
484 : }
485 : const MSEdgeVector& preds = walkingArea->getPredecessors();
486 43 : const MSEdgeVector& succs = walkingArea->getSuccessors();
487 43 : bool useBefore = swBefore != nullptr && std::find(preds.begin(), preds.end(), before) != preds.end();
488 43 : bool useAfter = swAfter != nullptr && std::find(succs.begin(), succs.end(), after) != succs.end();
489 43 : if (useBefore) {
490 33 : if (useAfter) {
491 0 : return getWalkingAreaPath(walkingArea, swBefore, swAfter);
492 33 : } else if (succs.size() > 0) {
493 : // could also try to exploit direction
494 33 : return getWalkingAreaPath(walkingArea, swBefore, getSidewalk<MSEdge, MSLane>(succs.front()));
495 : }
496 10 : } else if (useAfter && preds.size() > 0) {
497 : // could also try to exploit direction
498 10 : return getWalkingAreaPath(walkingArea, getSidewalk<MSEdge, MSLane>(preds.front()), swAfter);
499 : }
500 0 : return getArbitraryPath(walkingArea);
501 : }
502 :
503 :
504 : const MSPModel_Striping::WalkingAreaPath*
505 103744 : MSPModel_Striping::getWalkingAreaPath(const MSEdge* walkingArea, const MSLane* before, const MSLane* after) {
506 : assert(walkingArea->isWalkingArea());
507 103744 : const auto pathIt = myWalkingAreaPaths.find(std::make_pair(before, after));
508 103744 : if (pathIt != myWalkingAreaPaths.end()) {
509 103738 : return &pathIt->second;
510 : } else {
511 : // this can happen in case of moveToXY where before can point anywhere
512 : const MSEdgeVector& preds = walkingArea->getPredecessors();
513 6 : if (preds.size() > 0) {
514 6 : const MSEdge* const pred = walkingArea->getPredecessors().front();
515 6 : const auto pathIt2 = myWalkingAreaPaths.find(std::make_pair(getSidewalk<MSEdge, MSLane>(pred), after));
516 : assert(pathIt2 != myWalkingAreaPaths.end());
517 6 : return &pathIt2->second;
518 : } else {
519 0 : return getArbitraryPath(walkingArea);
520 : }
521 : }
522 : }
523 :
524 :
525 :
526 : MSPModel_Striping::NextLaneInfo
527 1253409 : MSPModel_Striping::getNextLane(const PState& ped, const MSLane* currentLane, const MSLane* prevLane) {
528 : const MSEdge* currentEdge = ¤tLane->getEdge();
529 1253409 : const MSJunction* junction = ped.myDir == FORWARD ? currentEdge->getToJunction() : currentEdge->getFromJunction();
530 1253409 : const MSEdge* nextRouteEdge = ped.myStage->getNextRouteEdge();
531 1253409 : const MSLane* nextRouteLane = getSidewalk<MSEdge, MSLane>(nextRouteEdge, ped.myPerson->getVClass());
532 : // result values
533 : const MSLane* nextLane = nextRouteLane;
534 1253409 : const MSLink* link = nullptr;
535 1253409 : int nextDir = UNDEFINED_DIRECTION;
536 :
537 : //if DEBUGCOND(ped) {
538 : // std::cout << " nextRouteLane=" << Named::getIDSecure(nextRouteLane) << " junction=" << junction->getID() << "\n";
539 : //}
540 1253409 : if (nextRouteLane == nullptr && nextRouteEdge != nullptr) {
541 56 : std::string error = "Person '" + ped.myPerson->getID() + "' could not find sidewalk on edge '" + nextRouteEdge->getID() + "', time="
542 84 : + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".";
543 56 : if (OptionsCont::getOptions().getBool("ignore-route-errors")) {
544 76 : WRITE_WARNING(error);
545 24 : nextRouteLane = nextRouteEdge->getLanes().front();
546 : } else {
547 8 : throw ProcessError(error);
548 : }
549 : }
550 :
551 1253405 : if (nextRouteLane != nullptr) {
552 986324 : if (currentEdge->isInternal()) {
553 : assert(junction == currentEdge->getFromJunction());
554 145618 : nextDir = junction == nextRouteEdge->getFromJunction() ? FORWARD : BACKWARD;
555 145618 : if (nextDir == FORWARD) {
556 84518 : nextLane = currentLane->getLinkCont()[0]->getViaLaneOrLane();
557 : } else {
558 61100 : nextLane = currentLane->getLogicalPredecessorLane();
559 : }
560 145618 : if DEBUGCOND(ped) {
561 0 : std::cout << " internal\n";
562 : }
563 840706 : } else if (currentEdge->isCrossing()) {
564 12211 : nextDir = ped.myDir;
565 12211 : if (ped.myDir == FORWARD) {
566 6588 : nextLane = currentLane->getLinkCont()[0]->getLane();
567 : } else {
568 5623 : nextLane = currentLane->getLogicalPredecessorLane();
569 : }
570 12211 : if DEBUGCOND(ped) {
571 0 : std::cout << " crossing\n";
572 : }
573 828495 : } else if (currentEdge->isWalkingArea()) {
574 : ConstMSEdgeVector crossingRoute;
575 : // departPos can be 0 because the direction of the walkingArea does not matter
576 : // for the arrivalPos, we need to make sure that the route does not deviate across other junctions
577 404651 : const int nextRouteEdgeDir = nextRouteEdge->getFromJunction() == junction ? FORWARD : BACKWARD;
578 404651 : const double arrivalPos = (nextRouteEdge == ped.myStage->getRoute().back()
579 404651 : ? ped.myStage->getArrivalPos()
580 13043 : : (nextRouteEdgeDir == FORWARD ? 0 : nextRouteEdge->getLength()));
581 : MSEdgeVector prohibited;
582 404651 : if (prevLane != nullptr) {
583 404579 : prohibited.push_back(&prevLane->getEdge());
584 : }
585 404651 : MSNet::getInstance()->getPedestrianRouter(0, prohibited).compute(currentEdge, nextRouteEdge, 0, arrivalPos, ped.myStage->getMaxSpeed(ped.myPerson), 0, junction, crossingRoute, true);
586 404651 : if DEBUGCOND(ped) {
587 : std::cout
588 : << " nre=" << nextRouteEdge->getID()
589 : << " nreDir=" << nextRouteEdgeDir
590 0 : << " aPos=" << arrivalPos
591 0 : << " crossingRoute=" << toString(crossingRoute)
592 0 : << "\n";
593 : }
594 404651 : if (crossingRoute.size() > 1) {
595 404614 : const MSEdge* nextEdge = crossingRoute[1];
596 404614 : nextLane = getSidewalk<MSEdge, MSLane>(crossingRoute[1], ped.myPerson->getVClass());
597 : assert((nextEdge->getFromJunction() == junction || nextEdge->getToJunction() == junction));
598 : assert(nextLane != prevLane);
599 404614 : nextDir = connectedDirection(currentLane, nextLane);
600 404614 : if DEBUGCOND(ped) {
601 0 : std::cout << " nextDir=" << nextDir << "\n";
602 : }
603 : assert(nextDir != UNDEFINED_DIRECTION);
604 404614 : if (nextDir == FORWARD) {
605 301426 : link = currentLane->getLinkTo(nextLane);
606 : } else {
607 103188 : link = nextLane->getLinkTo(currentLane);
608 103188 : if (nextEdge->isCrossing() && link->getTLLogic() == nullptr) {
609 88554 : const MSLane* oppositeWalkingArea = nextLane->getLogicalPredecessorLane();
610 88554 : link = oppositeWalkingArea->getLinkTo(nextLane);
611 : }
612 : }
613 : assert(link != nullptr);
614 : } else {
615 37 : if DEBUGCOND(ped) {
616 0 : std::cout << SIMTIME
617 0 : << " no route from '" << (currentEdge == nullptr ? "NULL" : currentEdge->getID())
618 0 : << "' to '" << (nextRouteEdge == nullptr ? "NULL" : nextRouteEdge->getID())
619 0 : << "\n";
620 : }
621 : // check if a direct connection exists (moving onto the walkingarea was the wrong choice)
622 37 : if (ped.myDir == FORWARD) {
623 24 : link = prevLane->getLinkTo(nextRouteLane);
624 : } else {
625 13 : link = nextRouteLane->getLinkTo(prevLane);
626 : }
627 37 : if (link != nullptr) {
628 : // leave direction as UNDEFINED_DIRECTION to signal that currentLane must be changed
629 : nextLane = link->getViaLaneOrLane();
630 : } else {
631 40 : WRITE_WARNING("Person '" + ped.myPerson->getID() + "' could not find route across junction '" + junction->getID()
632 : + "' from walkingArea '" + currentEdge->getID()
633 : + "' to edge '" + nextRouteEdge->getID() + "', time=" +
634 : time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
635 : // error indicated by nextDir == UNDEFINED_DIRECTION
636 : nextLane = nextRouteLane;
637 : }
638 : }
639 423844 : } else if (currentEdge == nextRouteEdge) {
640 : // strange loop in this route. No need to use walkingArea
641 8 : nextDir = -ped.myDir;
642 : } else {
643 : // normal edge. by default use next / previous walking area
644 423836 : nextDir = ped.myDir;
645 423836 : nextLane = getNextWalkingArea(currentLane, ped.myDir, link);
646 423836 : if (nextLane != nullptr) {
647 : // walking area found
648 97064 : if DEBUGCOND(ped) {
649 0 : std::cout << " next walkingArea " << (nextDir == FORWARD ? "forward" : "backward") << "\n";
650 : }
651 : } else {
652 : // walk forward by default
653 326772 : if (junction == nextRouteEdge->getToJunction()) {
654 145743 : nextDir = BACKWARD;
655 181029 : } else if (junction == nextRouteEdge->getFromJunction()) {
656 : nextDir = FORWARD;
657 : } else {
658 : // topological disconnect, find a direction that makes sense
659 : // for the future part of the route
660 776 : ConstMSEdgeVector futureRoute = ped.myStage->getRoute();
661 776 : futureRoute.erase(futureRoute.begin(), futureRoute.begin() + ped.myStage->getRoutePosition() + 1);
662 776 : int passedFwd = 0;
663 776 : int passedBwd = 0;
664 776 : canTraverse(FORWARD, futureRoute, passedFwd);
665 776 : canTraverse(BACKWARD, futureRoute, passedBwd);
666 776 : nextDir = (passedFwd >= passedBwd) ? FORWARD : BACKWARD;
667 776 : if DEBUGCOND(ped) {
668 0 : std::cout << " nextEdge=" << nextRouteEdge->getID() << " passedFwd=" << passedFwd << " passedBwd=" << passedBwd << " futureRoute=" << toString(futureRoute) << " nextDir=" << nextDir << "\n";
669 : }
670 : }
671 : // try to use a direct link as fallback
672 : // direct links only exist if built explicitly. They are used to model tl-controlled links if there are no crossings
673 326772 : if (ped.myDir == FORWARD) {
674 101753 : link = currentLane->getLinkTo(nextRouteLane);
675 101753 : if (link != nullptr) {
676 94426 : if DEBUGCOND(ped) {
677 0 : std::cout << " direct forward\n";
678 : }
679 94426 : nextLane = currentLane->getInternalFollowingLane(nextRouteLane);
680 : }
681 : } else {
682 225019 : link = nextRouteLane->getLinkTo(currentLane);
683 225019 : if (link != nullptr) {
684 63397 : if DEBUGCOND(ped) {
685 0 : std::cout << " direct backward\n";
686 : }
687 63397 : nextLane = nextRouteLane->getInternalFollowingLane(currentLane);
688 63397 : if (nextLane != nullptr) {
689 : // advance to the end of consecutive internal lanes
690 125805 : while (nextLane->getLinkCont()[0]->getViaLaneOrLane()->isInternal()) {
691 5 : nextLane = nextLane->getLinkCont()[0]->getViaLaneOrLane();
692 : }
693 : }
694 : }
695 : }
696 : }
697 423836 : if (nextLane == nullptr) {
698 : // no internal lane found
699 : nextLane = nextRouteLane;
700 174464 : if DEBUGCOND(ped) {
701 0 : std::cout << SIMTIME << " no next lane found for " << currentLane->getID() << " dir=" << ped.myDir << "\n";
702 : }
703 174464 : if (usingInternalLanesStatic() && currentLane->getLinkCont().size() > 0) {
704 0 : WRITE_WARNING("Person '" + ped.myPerson->getID() + "' could not find route across junction '" + junction->getID()
705 : + "' from edge '" + currentEdge->getID()
706 : + "' to edge '" + nextRouteEdge->getID() + "', time=" +
707 : time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
708 : }
709 249372 : } else if (nextLane->getLength() <= POSITION_EPS) {
710 : // internal lane too short
711 : // most often this is due to a zero-size junction. However, if
712 : // the person needs to pass a crossing we cannot skip ahead
713 11369 : if ((nextLane->getCanonicalSuccessorLane() == nullptr
714 11361 : || !nextLane->getCanonicalSuccessorLane()->getEdge().isCrossing())
715 34087 : && (nextLane->getLogicalPredecessorLane() == nullptr ||
716 11357 : !nextLane->getLogicalPredecessorLane()->getEdge().isCrossing())) {
717 : //WRITE_WARNING("Person '" + ped.getID()
718 : // + "' skips short lane '" + nextLane->getID()
719 : // + "' length=" + toString(nextLane->getLength())
720 : // + " time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
721 : nextLane = nextRouteLane;
722 11369 : nextDir = nextRouteEdge->getFromJunction() == junction ? FORWARD : BACKWARD;
723 : }
724 : }
725 : }
726 : }
727 1253405 : if DEBUGCOND(ped) {
728 0 : std::cout << SIMTIME
729 0 : << " p=" << ped.myPerson->getID()
730 : << " l=" << currentLane->getID()
731 0 : << " nl=" << (nextLane == nullptr ? "NULL" : nextLane->getID())
732 0 : << " nrl=" << (nextRouteLane == nullptr ? "NULL" : nextRouteLane->getID())
733 : << " d=" << nextDir
734 0 : << " link=" << (link == nullptr ? "NULL" : link->getViaLaneOrLane()->getID())
735 0 : << " pedDir=" << ped.myDir
736 0 : << "\n";
737 : }
738 : assert(nextLane != 0 || nextRouteLane == 0);
739 1253405 : return NextLaneInfo(nextLane, link, nextDir);
740 : }
741 :
742 :
743 : const MSLane*
744 423836 : MSPModel_Striping::getNextWalkingArea(const MSLane* currentLane, const int dir, const MSLink*& link) {
745 423836 : if (dir == FORWARD) {
746 313979 : for (const MSLink* const l : currentLane->getLinkCont()) {
747 212226 : if (l->getLane()->getEdge().isWalkingArea()) {
748 83426 : link = l;
749 : return l->getLane();
750 : }
751 : }
752 : } else {
753 : const std::vector<MSLane::IncomingLaneInfo>& laneInfos = currentLane->getIncomingLanes();
754 389493 : for (std::vector<MSLane::IncomingLaneInfo>::const_iterator it = laneInfos.begin(); it != laneInfos.end(); ++it) {
755 164474 : if ((*it).lane->getEdge().isWalkingArea()) {
756 13638 : link = (*it).viaLink;
757 : return (*it).lane;
758 : }
759 : }
760 : }
761 : return nullptr;
762 : }
763 :
764 :
765 : MSPModel_Striping::Obstacles
766 49720913 : MSPModel_Striping::getNeighboringObstacles(const Pedestrians& pedestrians, int egoIndex, int stripes) {
767 49720913 : const PState& ego = *pedestrians[egoIndex];
768 49720913 : const int egoStripe = ego.stripe();
769 49720913 : Obstacles obs(stripes, Obstacle(ego.myDir));
770 49720913 : std::vector<bool> haveBlocker(stripes, false);
771 8688806049 : for (int index = egoIndex + 1; index < (int)pedestrians.size(); index++) {
772 8675247753 : const PState& p = *pedestrians[index];
773 8675247753 : if DEBUGCOND(ego) {
774 0 : std::cout << SIMTIME << " ped=" << ego.getID() << " cur=" << egoStripe << " checking neighbor " << p.getID()
775 0 : << " nCur=" << p.stripe() << " nOth=" << p.otherStripe();
776 : }
777 8675247753 : if (!p.myWaitingToEnter && !p.myAmJammed) {
778 58243719 : const Obstacle o(p);
779 58243719 : if DEBUGCOND(ego) {
780 0 : std::cout << " dist=" << ego.distanceTo(o) << std::endl;
781 : }
782 58243719 : if (ego.distanceTo(o) == DIST_BEHIND) {
783 : break;
784 : }
785 22081102 : if (ego.distanceTo(o) == DIST_OVERLAP) {
786 22081102 : if (p.stripe() != egoStripe || p.myDir != ego.myDir) {
787 18157240 : obs[p.stripe()] = o;
788 18157240 : haveBlocker[p.stripe()] = true;
789 : } else {
790 : //std::cout << SIMTIME << " ignoring overlap between " << ego.getID() << " and " << p.getID() << " on stripe=" << egoStripe << "\n";
791 : }
792 22081102 : if (p.otherStripe() != egoStripe || p.myDir != ego.myDir) {
793 18157240 : obs[p.otherStripe()] = o;
794 18157240 : haveBlocker[p.otherStripe()] = true;
795 : } else {
796 : //std::cout << SIMTIME << " ignoring overlap between " << ego.getID() << " and " << p.getID() << " on stripe2=" << egoStripe << "\n";
797 : }
798 : } else {
799 0 : if (!haveBlocker[p.stripe()]) {
800 0 : obs[p.stripe()] = o;
801 : }
802 0 : if (!haveBlocker[p.otherStripe()]) {
803 0 : obs[p.otherStripe()] = o;
804 : }
805 : }
806 : }
807 : }
808 49720913 : if DEBUGCOND(ego) {
809 0 : std::cout << SIMTIME << " ped=" << ego.myPerson->getID() << " neighObs=";
810 0 : DEBUG_PRINT(obs);
811 : }
812 49720913 : return obs;
813 0 : }
814 :
815 :
816 : int
817 1804003 : MSPModel_Striping::getStripeOffset(int origStripes, int destStripes, bool addRemainder) {
818 1804003 : int offset = (destStripes - origStripes) / 2;
819 1804003 : if (addRemainder) {
820 4905 : offset += (destStripes - origStripes) % 2;
821 : }
822 1804003 : return offset;
823 : }
824 :
825 :
826 : const MSPModel_Striping::Obstacles&
827 28850248 : MSPModel_Striping::getNextLaneObstacles(NextLanesObstacles& nextLanesObs, const
828 : MSLane* lane, const MSLane* nextLane, int stripes, int nextDir,
829 : double currentLength, int currentDir) {
830 : if (nextLanesObs.count(nextLane) == 0) {
831 1122097 : const double nextLength = nextLane->getEdge().isWalkingArea() ? myMinNextLengths[nextLane] : nextLane->getLength();
832 : // figure out the which pedestrians are ahead on the next lane
833 1122097 : const int nextStripes = numStripes(nextLane);
834 : // do not move past the end of the next lane in a single step
835 3366291 : Obstacles obs(stripes, Obstacle(nextDir == FORWARD ? nextLength : 0, 0, OBSTACLE_NEXTEND, "nextEnd", 0));
836 :
837 1122097 : const int offset = getStripeOffset(nextStripes, stripes, currentDir != nextDir && nextStripes > stripes);
838 : //std::cout << SIMTIME << " getNextLaneObstacles"
839 : // << " nextLane=" << nextLane->getID()
840 : // << " nextLength=" << nextLength
841 : // << " nextDir=" << nextDir
842 : // << " currentLength=" << currentLength
843 : // << " currentDir=" << currentDir
844 : // << " stripes=" << stripes
845 : // << " nextStripes=" << nextStripes
846 : // << " offset=" << offset
847 : // << "\n";
848 1122097 : if (nextStripes < stripes) {
849 : // some stripes do not continue
850 1406088 : for (int ii = 0; ii < stripes; ++ii) {
851 1211017 : if (ii < offset || ii >= nextStripes + offset) {
852 1657590 : obs[ii] = Obstacle(nextDir == FORWARD ? 0 : nextLength, 0, OBSTACLE_END, "stripeEnd", 0);
853 : }
854 : }
855 : }
856 1122097 : Pedestrians& pedestrians = getPedestrians(nextLane);
857 1122097 : if (nextLane->getEdge().isWalkingArea()) {
858 290356 : transformToCurrentLanePositions(obs, currentDir, nextDir, currentLength, nextLength);
859 : // complex transformation into the coordinate system of the current lane
860 : // (pedestrians on next lane may walk at arbitrary angles relative to the current lane)
861 290356 : double lateral_offset = (lane->getWidth() - stripeWidth) * 0.5;
862 290356 : if ((stripes - nextStripes) % 2 != 0) {
863 180231 : lateral_offset += 0.5 * stripeWidth;
864 : }
865 : nextDir = currentDir;
866 : // transform pedestrians into the current coordinate system
867 1734159 : for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
868 1443803 : PState& p = *pedestrians[ii];
869 1443803 : if (p.myWaitingToEnter || p.myAmJammed) {
870 3062 : continue;
871 : }
872 1440741 : Position relPos = lane->getShape().transformToVectorCoordinates(p.getPosition(*p.myStage, -1), true);
873 1440741 : const double newY = relPos.y() + lateral_offset;
874 : //if (p.myPerson->getID() == "ped200") std::cout << " ped=" << p.myPerson->getID() << " relX=" << relPos.x() << " relY=" << newY << " latOff=" << lateral_offset << " s=" << p.stripe(newY) << " os=" << p.otherStripe(newY) << "\n";
875 1440741 : if ((currentDir == FORWARD && relPos.x() >= lane->getLength()) || (currentDir == BACKWARD && relPos.x() < 0)) {
876 1436961 : addCloserObstacle(obs, relPos.x(), p.stripe(newY), stripes, p.myPerson->getID(), p.myPerson->getVehicleType().getWidth(), currentDir, OBSTACLE_PED);
877 1436961 : addCloserObstacle(obs, relPos.x(), p.otherStripe(newY), stripes, p.myPerson->getID(), p.myPerson->getVehicleType().getWidth(), currentDir, OBSTACLE_PED);
878 : }
879 : }
880 : } else {
881 : // simple transformation into the coordinate system of the current lane
882 : // (only need to worry about currentDir and nextDir)
883 : // XXX consider waitingToEnter on nextLane
884 831741 : sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(nextDir));
885 26471530 : for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
886 25639789 : const PState& p = *pedestrians[ii];
887 25639789 : if (p.myWaitingToEnter || p.myAmJammed) {
888 14273502 : continue;
889 : }
890 11366287 : double newY = p.myRelY;
891 11366287 : Obstacle pObs(p);
892 11366287 : if (nextDir != currentDir) {
893 2408228 : newY = (nextStripes - 1) * stripeWidth - newY;
894 2408228 : pObs.speed *= -1;
895 : }
896 11366287 : newY += offset * stripeWidth;
897 11366287 : const int stripe = p.stripe(newY);
898 11366287 : if (stripe >= 0 && stripe < stripes) {
899 11366284 : obs[stripe] = pObs;
900 : }
901 11366287 : const int otherStripe = p.otherStripe(newY);
902 11366287 : if (otherStripe >= 0 && otherStripe < stripes) {
903 11366284 : obs[otherStripe] = pObs;
904 : }
905 : }
906 831741 : if (nextLane->getEdge().isCrossing()) {
907 : // add vehicle obstacles
908 79766 : const MSLink* crossingEntryLink = nextLane->getIncomingLanes().front().viaLink;
909 79766 : const bool prio = crossingEntryLink->havePriority() || crossingEntryLink->getTLLogic() != nullptr;
910 79766 : addCrossingVehs(nextLane, stripes, offset, nextDir, obs, prio);
911 : }
912 831741 : if (nextLane->getVehicleNumberWithPartials() > 0) {
913 19193 : Obstacles vehObs = getVehicleObstacles(nextLane, nextDir);
914 19193 : PState::mergeObstacles(obs, vehObs, nextDir, offset);
915 19193 : }
916 831741 : transformToCurrentLanePositions(obs, currentDir, nextDir, currentLength, nextLength);
917 : }
918 1122097 : nextLanesObs[nextLane] = obs;
919 1122097 : }
920 28850248 : return nextLanesObs[nextLane];
921 : }
922 :
923 : void
924 1122097 : MSPModel_Striping::transformToCurrentLanePositions(Obstacles& obs, int currentDir, int nextDir, double currentLength, double nextLength) {
925 6504567 : for (int ii = 0; ii < (int)obs.size(); ++ii) {
926 5382470 : Obstacle& o = obs[ii];
927 5382470 : if (currentDir == FORWARD) {
928 3518521 : if (nextDir == FORWARD) {
929 3356320 : o.xFwd += currentLength;
930 3356320 : o.xBack += currentLength;
931 : } else {
932 162201 : const double tmp = o.xFwd;
933 162201 : o.xFwd = currentLength + nextLength - o.xBack;
934 162201 : o.xBack = currentLength + nextLength - tmp;
935 : }
936 : } else {
937 1863949 : if (nextDir == FORWARD) {
938 487461 : const double tmp = o.xFwd;
939 487461 : o.xFwd = -o.xBack;
940 487461 : o.xBack = -tmp;
941 : } else {
942 1376488 : o.xFwd -= nextLength;
943 1376488 : o.xBack -= nextLength;
944 : }
945 : }
946 : }
947 1122097 : }
948 :
949 :
950 : void
951 2873922 : MSPModel_Striping::addCloserObstacle(Obstacles& obs, double x, int stripe, int numStripes, const std::string& id, double width, int dir, ObstacleType type) {
952 2873922 : if (stripe >= 0 && stripe < numStripes) {
953 1068467 : if ((dir == FORWARD && x - width / 2. < obs[stripe].xBack) || (dir == BACKWARD && x + width / 2. > obs[stripe].xFwd)) {
954 723690 : obs[stripe] = Obstacle(x, 0, type, id, width);
955 : }
956 : }
957 2873922 : }
958 :
959 : void
960 3704788 : MSPModel_Striping::moveInDirection(SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir) {
961 48411843 : for (ActiveLanes::iterator it_lane = myActiveLanes.begin(); it_lane != myActiveLanes.end(); ++it_lane) {
962 44707059 : const MSLane* lane = it_lane->first;
963 44707059 : Pedestrians& pedestrians = it_lane->second;
964 44707059 : if (pedestrians.size() == 0) {
965 39631827 : continue;
966 : }
967 : //std::cout << SIMTIME << ">>> lane=" << lane->getID() << " numPeds=" << pedestrians.size() << "\n";
968 5075232 : if (lane->getEdge().isWalkingArea()) {
969 604922 : const double lateral_offset = (lane->getWidth() - stripeWidth) * 0.5;
970 604922 : const double minY = stripeWidth * - 0.5 + NUMERICAL_EPS;
971 604922 : const double maxY = stripeWidth * (numStripes(lane) - 0.5) - NUMERICAL_EPS;
972 : const WalkingAreaPath* debugPath = nullptr;
973 : // need to handle each walkingAreaPath separately and transform
974 : // coordinates beforehand
975 : std::set<const WalkingAreaPath*, walkingarea_path_sorter> paths;
976 3437887 : for (Pedestrians::iterator it = pedestrians.begin(); it != pedestrians.end(); ++it) {
977 2832965 : const PState* p = *it;
978 : assert(p->myWalkingAreaPath != 0);
979 2832965 : if (p->myDir == dir) {
980 1422451 : paths.insert(p->myWalkingAreaPath);
981 1422451 : if DEBUGCOND(*p) {
982 0 : debugPath = p->myWalkingAreaPath;
983 0 : std::cout << SIMTIME << " debugging WalkingAreaPath from=" << debugPath->from->getID() << " to=" << debugPath->to->getID() << " minY=" << minY << " maxY=" << maxY << " latOffset=" << lateral_offset << "\n";
984 : }
985 : }
986 : }
987 604922 : const double usableWidth = (numStripes(lane) - 1) * stripeWidth;
988 981329 : for (std::set<const WalkingAreaPath*, walkingarea_path_sorter>::iterator it = paths.begin(); it != paths.end(); ++it) {
989 376407 : const WalkingAreaPath* path = *it;
990 : Pedestrians toDelete;
991 : Pedestrians transformedPeds;
992 376407 : transformedPeds.reserve(pedestrians.size());
993 2754129 : for (Pedestrians::iterator it_p = pedestrians.begin(); it_p != pedestrians.end(); ++it_p) {
994 2377722 : PState* p = *it_p;
995 2377722 : if (p->myWalkingAreaPath == path) {
996 1422451 : transformedPeds.push_back(p);
997 1422451 : if (path == debugPath) std::cout << " ped=" << p->myPerson->getID() << " relX=" << p->myRelX << " relY=" << p->myRelY << " (untransformed), vecCoord="
998 0 : << path->shape.transformToVectorCoordinates(p->getPosition(*p->myStage, -1)) << "\n";
999 955271 : } else if (p->myWalkingAreaPath->from == path->to && p->myWalkingAreaPath->to == path->from) {
1000 53933 : if (p->myWalkingAreaPath->dir != path->dir) {
1001 : // opposite direction is already in the correct coordinate system
1002 39899 : transformedPeds.push_back(p);
1003 39899 : if (path == debugPath) std::cout << " ped=" << p->myPerson->getID() << " relX=" << p->myRelX << " relY=" << p->myRelY << " (untransformed), vecCoord="
1004 0 : << path->shape.transformToVectorCoordinates(p->getPosition(*p->myStage, -1)) << "\n";
1005 : } else {
1006 : // x position must be reversed
1007 14034 : PState* tp = new PState(*p);
1008 14034 : tp->myRelX = path->length - p->myRelX;
1009 14034 : tp->myRelY = usableWidth - p->myRelY;
1010 14034 : tp->myDir = !path->dir;
1011 14034 : tp->mySpeed = -p->mySpeed;
1012 14034 : tp->mySpeedLat = -p->mySpeedLat;
1013 14034 : toDelete.push_back(tp);
1014 14034 : transformedPeds.push_back(tp);
1015 14034 : if (path == debugPath) std::cout << " ped=" << p->myPerson->getID() << " relX=" << p->myRelX << " relY=" << p->myRelY << " (semi-transformed), vecCoord="
1016 0 : << path->shape.transformToVectorCoordinates(p->getPosition(*p->myStage, -1)) << "\n";
1017 : }
1018 : } else {
1019 901338 : const Position relPos = path->shape.transformToVectorCoordinates(p->getPosition(*p->myStage, -1));
1020 901338 : const double newY = relPos.y() + lateral_offset;
1021 900890 : if (relPos != Position::INVALID && newY >= minY && newY <= maxY) {
1022 241988 : PState* tp = new PState(*p);
1023 241988 : tp->myRelX = relPos.x();
1024 241988 : tp->myRelY = newY;
1025 : // only an obstacle, speed may be orthogonal to dir
1026 241988 : tp->myDir = !dir;
1027 241988 : tp->mySpeed = 0;
1028 241988 : tp->mySpeedLat = 0;
1029 241988 : toDelete.push_back(tp);
1030 241988 : transformedPeds.push_back(tp);
1031 241988 : if (path == debugPath) {
1032 0 : std::cout << " ped=" << p->myPerson->getID() << " relX=" << relPos.x() << " relY=" << newY << " (transformed), vecCoord=" << relPos << "\n";
1033 : }
1034 : } else {
1035 659350 : if (path == debugPath) {
1036 0 : std::cout << " ped=" << p->myPerson->getID() << " relX=" << relPos.x() << " relY=" << newY << " (invalid), vecCoord=" << relPos << "\n";
1037 : }
1038 : }
1039 : }
1040 : }
1041 : auto itFoe = myWalkingAreaFoes.find(&lane->getEdge());
1042 376407 : if (itFoe != myWalkingAreaFoes.end()) {
1043 : // add vehicle foes on paths which cross this walkingarea
1044 : // translate the vehicle into a number of dummy-pedestrians
1045 : // that occupy the same space
1046 2296156 : for (const MSLane* foeLane : itFoe->second) {
1047 171221 : for (auto itVeh = foeLane->anyVehiclesBegin(); itVeh != foeLane->anyVehiclesEnd(); ++itVeh) {
1048 171221 : const MSVehicle* veh = *itVeh;
1049 171221 : const double vehWidth = veh->getVehicleType().getWidth();
1050 171221 : Boundary relCorners;
1051 171221 : Position relFront = path->shape.transformToVectorCoordinates(veh->getPosition());
1052 171221 : Position relBack = path->shape.transformToVectorCoordinates(veh->getBackPosition());
1053 171221 : PositionVector relCenter;
1054 171221 : relCenter.push_back(relFront);
1055 171221 : relCenter.push_back(relBack);
1056 171221 : relCenter.move2side(vehWidth / 2);
1057 171221 : relCorners.add(relCenter[0]);
1058 171221 : relCorners.add(relCenter[1]);
1059 171221 : relCenter.move2side(-vehWidth);
1060 171221 : relCorners.add(relCenter[0]);
1061 171221 : relCorners.add(relCenter[1]);
1062 : // persons should requier less gap than the vehicles to prevent getting stuck
1063 : // when a vehicles moves towards them
1064 171221 : relCorners.growWidth(SAFETY_GAP / 2);
1065 171221 : const double xWidth = relCorners.getWidth();
1066 171221 : const double vehYmin = MAX2(minY - lateral_offset, relCorners.ymin());
1067 171221 : const double vehYmax = MIN2(maxY - lateral_offset, relCorners.ymax());
1068 171221 : const double xCenter = relCorners.getCenter().x();
1069 : Position yMinPos(xCenter, vehYmin);
1070 : Position yMaxPos(xCenter, vehYmax);
1071 171221 : const bool addFront = addVehicleFoe(veh, lane, yMinPos, dir * xWidth, 0, lateral_offset, minY, maxY, toDelete, transformedPeds);
1072 171221 : const bool addBack = addVehicleFoe(veh, lane, yMaxPos, dir * xWidth, 0, lateral_offset, minY, maxY, toDelete, transformedPeds);
1073 171221 : if (path == debugPath) {
1074 : std::cout << " veh=" << veh->getID()
1075 0 : << " corners=" << relCorners
1076 : << " xWidth=" << xWidth
1077 0 : << " ymin=" << relCorners.ymin()
1078 0 : << " ymax=" << relCorners.ymax()
1079 : << " vehYmin=" << vehYmin
1080 : << " vehYmax=" << vehYmax
1081 0 : << "\n";
1082 : }
1083 171221 : if (addFront && addBack) {
1084 : // add in-between positions
1085 171221 : const double yDist = vehYmax - vehYmin;
1086 479336 : for (double dist = stripeWidth; dist < yDist; dist += stripeWidth) {
1087 308115 : const double relDist = dist / yDist;
1088 308115 : Position between = (yMinPos * relDist) + (yMaxPos * (1 - relDist));
1089 308115 : if (path == debugPath) {
1090 0 : std::cout << " vehBetween=" << veh->getID() << " pos=" << between << "\n";
1091 : }
1092 308115 : addVehicleFoe(veh, lane, between, dir * xWidth, stripeWidth, lateral_offset, minY, maxY, toDelete, transformedPeds);
1093 : }
1094 : }
1095 171221 : }
1096 : }
1097 : }
1098 376407 : moveInDirectionOnLane(transformedPeds, lane, currentTime, changedLane, dir, path == debugPath);
1099 376407 : arriveAndAdvance(pedestrians, currentTime, changedLane, dir);
1100 : // clean up
1101 1182626 : for (Pedestrians::iterator it_p = toDelete.begin(); it_p != toDelete.end(); ++it_p) {
1102 806219 : delete *it_p;
1103 : }
1104 : }
1105 : } else {
1106 4470310 : moveInDirectionOnLane(pedestrians, lane, currentTime, changedLane, dir, false);
1107 4470310 : arriveAndAdvance(pedestrians, currentTime, changedLane, dir);
1108 : }
1109 : }
1110 3704784 : }
1111 :
1112 :
1113 : bool
1114 650557 : MSPModel_Striping::addVehicleFoe(const MSVehicle* veh, const MSLane* walkingarea, const Position& relPos, double xWidth, double yWidth, double lateral_offset,
1115 : double minY, double maxY, Pedestrians& toDelete, Pedestrians& transformedPeds) {
1116 : if (relPos != Position::INVALID) {
1117 650557 : const double newY = relPos.y() + lateral_offset;
1118 650557 : if (newY >= minY && newY <= maxY) {
1119 550197 : PState* tp = new PStateVehicle(veh, walkingarea, relPos.x(), newY, xWidth, yWidth);
1120 : //std::cout << SIMTIME << " addVehicleFoe=" << veh->getID() << " rx=" << relPos.x() << " ry=" << newY << " s=" << tp->stripe() << " o=" << tp->otherStripe() << "\n";
1121 550197 : toDelete.push_back(tp);
1122 550197 : transformedPeds.push_back(tp);
1123 : }
1124 650557 : return true;
1125 : } else {
1126 : return false;
1127 : }
1128 : }
1129 :
1130 : void
1131 4846717 : MSPModel_Striping::arriveAndAdvance(Pedestrians& pedestrians, SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir) {
1132 : // advance to the next lane / arrive at destination
1133 4846717 : sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(dir));
1134 : // can't use iterators because we do concurrent modification
1135 103931039 : for (int i = 0; i < (int)pedestrians.size(); i++) {
1136 99084326 : PState* const p = pedestrians[i];
1137 99084326 : if (p->isRemoteControlled()) {
1138 32302 : continue;
1139 : }
1140 99052024 : if (p->myDir == dir && p->distToLaneEnd() < 0) {
1141 : // moveToNextLane may trigger re-insertion (for consecutive
1142 : // walks) so erase must be called first
1143 : pedestrians.erase(pedestrians.begin() + i);
1144 948585 : i--;
1145 948585 : p->moveToNextLane(currentTime);
1146 948581 : if (p->myLane != nullptr) {
1147 681906 : changedLane.insert(p->myPerson);
1148 681906 : myActiveLanes[p->myLane].push_back(p);
1149 : } else {
1150 : // end walking stage and destroy PState
1151 266675 : p->myStage->moveToNextEdge(p->myPerson, currentTime, dir);
1152 266675 : myNumActivePedestrians--;
1153 : }
1154 : }
1155 : }
1156 4846713 : }
1157 :
1158 :
1159 : void
1160 4846717 : MSPModel_Striping::moveInDirectionOnLane(Pedestrians& pedestrians, const MSLane* lane, SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir, bool debug) {
1161 4846717 : const int stripes = numStripes(lane);
1162 : //std::cout << " laneWidth=" << lane->getWidth() << " stripeWidth=" << stripeWidth << " stripes=" << stripes << "\n";
1163 9693434 : Obstacles obs(stripes, Obstacle(dir)); // continously updated
1164 : NextLanesObstacles nextLanesObs; // continously updated
1165 4846717 : sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(dir));
1166 :
1167 9693434 : Obstacles crossingVehs(stripes, Obstacle(dir));
1168 : bool hasCrossingVehObs = false;
1169 4846717 : if (lane->getEdge().isCrossing()) {
1170 : // assume that vehicles will brake when already on the crossing
1171 80574 : hasCrossingVehObs = addCrossingVehs(lane, stripes, 0, dir, crossingVehs, true);
1172 : }
1173 :
1174 103821614 : for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
1175 98974897 : PState& p = *pedestrians[ii];
1176 : UNUSED_PARAMETER(debug);
1177 : //if (debug) {
1178 : // std::cout << SIMTIME << " CHECKING d=" << dir << " p=" << p.getID() << " relX=" << p.myRelX << " xmin=" << p.getMinX() << " xmax=" << p.getMaxX() << " pdir=" << p.myDir << "\n";
1179 : //}
1180 98974897 : Obstacles currentObs = obs;
1181 98974897 : if (p.myDir != dir || changedLane.count(p.myPerson) != 0 || p.myRemoteXYPos != Position::INVALID) {
1182 49253984 : if (!p.myWaitingToEnter && !p.myAmJammed) {
1183 : //if DEBUGCOND(p) {
1184 : // std::cout << " obs=" << p.myPerson->getID() << " y=" << p.myRelY << " stripe=" << p.stripe() << " oStripe=" << p.otherStripe() << "\n";
1185 : //}
1186 24315658 : Obstacle o(p);
1187 24315658 : if (p.myDir != dir && p.mySpeed == 0) {
1188 : // ensure recognition of oncoming
1189 4724158 : o.speed = (p.myDir == FORWARD ? 0.1 : -0.1);
1190 : }
1191 24315658 : if (o.closer(obs[p.stripe()], dir)) {
1192 24303900 : obs[p.stripe()] = o;
1193 : }
1194 24315658 : if (o.closer(obs[p.otherStripe()], dir)) {
1195 24304077 : obs[p.otherStripe()] = o;
1196 : }
1197 : }
1198 : continue;
1199 49253984 : }
1200 49720913 : if DEBUGCOND(p) {
1201 0 : std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " currentObs=";
1202 0 : gDebugFlag1 = true;
1203 0 : DEBUG_PRINT(currentObs);
1204 : }
1205 49720913 : const MSLane* nextLane = p.myNLI.lane;
1206 49720913 : const MSLink* link = p.myNLI.link;
1207 49720913 : const double dist = p.distToLaneEnd();
1208 49720913 : const double speed = p.myStage->getMaxSpeed(p.myPerson);
1209 49720913 : if (nextLane != nullptr && dist <= LOOKAHEAD_ONCOMING) {
1210 28850248 : const double currentLength = (p.myWalkingAreaPath == nullptr ? lane->getLength() : p.myWalkingAreaPath->length);
1211 28850248 : const Obstacles& nextObs = getNextLaneObstacles(
1212 : nextLanesObs, lane, nextLane, stripes,
1213 : p.myNLI.dir, currentLength, dir);
1214 :
1215 28850248 : if DEBUGCOND(p) {
1216 0 : std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " nextObs=";
1217 0 : DEBUG_PRINT(nextObs);
1218 : }
1219 28850248 : p.mergeObstacles(currentObs, nextObs);
1220 : }
1221 49720913 : if DEBUGCOND(p) {
1222 0 : std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWithNext=";
1223 0 : DEBUG_PRINT(currentObs);
1224 : }
1225 49720913 : p.mergeObstacles(currentObs, getNeighboringObstacles(pedestrians, ii, stripes));
1226 49720913 : if DEBUGCOND(p) {
1227 0 : std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWithNeigh=";
1228 0 : DEBUG_PRINT(currentObs);
1229 : }
1230 : // time gap to pass the intersection ahead of a vehicle.
1231 : const double passingClearanceTime = 2;
1232 49720913 : const double passingLength = p.getLength() + passingClearanceTime * speed;
1233 : // check link state
1234 49720913 : if DEBUGCOND(p) {
1235 0 : gDebugFlag1 = true; // get debug output from MSLink
1236 0 : std::cout << " link=" << (link == nullptr ? "NULL" : link->getViaLaneOrLane()->getID())
1237 0 : << " dist=" << dist << " d2=" << dist - p.getMinGap() << " la=" << LOOKAHEAD_SAMEDIR* speed << "\n";
1238 : }
1239 : if (link != nullptr
1240 : // only check close before junction, @todo we should take deceleration into account here
1241 29820838 : && dist - p.getMinGap() < LOOKAHEAD_SAMEDIR * speed
1242 : // persons move before vehicles so we subtract DELTA_TO because they cannot rely on vehicles having passed the intersection in the current time step
1243 59399576 : && !link->opened(currentTime - DELTA_T, speed, speed, passingLength, p.getImpatience(currentTime), speed, 0, 0, nullptr, p.ignoreRed(link), p.myPerson)) {
1244 : // prevent movement passed a closed link
1245 911038 : Obstacles closedLink(stripes, Obstacle(p.myRelX + dir * (dist - NUMERICAL_EPS), 0, OBSTACLE_LINKCLOSED, "closedLink_" + link->getViaLaneOrLane()->getID(), 0));
1246 455519 : p.mergeObstacles(currentObs, closedLink);
1247 455519 : if DEBUGCOND(p) {
1248 0 : std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWithTLS=";
1249 0 : DEBUG_PRINT(currentObs);
1250 : }
1251 : // consider rerouting over another crossing
1252 455519 : if (p.myWalkingAreaPath != nullptr) {
1253 : // @todo actually another path would be needed starting at the current position
1254 300841 : p.myNLI = getNextLane(p, p.myLane, p.myWalkingAreaPath->from);
1255 : }
1256 455519 : }
1257 49720913 : if DEBUGCOND(p) {
1258 0 : gDebugFlag1 = false;
1259 : }
1260 49720913 : if (&lane->getEdge() == p.myStage->getDestination() && p.myStage->getDestinationStop() != nullptr) {
1261 : Obstacles arrival;
1262 1160663 : if (p.myStage->getDestinationStop()->getWaitingCapacity() > p.myStage->getDestinationStop()->getNumWaitingPersons()) {
1263 1012000 : arrival = Obstacles(stripes, Obstacle(p.myStage->getArrivalPos() + dir * p.getMinGap(), 0, OBSTACLE_ARRIVALPOS, "arrival", 0));
1264 : } else {
1265 148663 : arrival = Obstacles(stripes, Obstacle(p.myStage->getArrivalPos() - dir * p.getMinGap(), 0, OBSTACLE_ARRIVALPOS, "arrival_blocked", 0));
1266 : }
1267 1160663 : p.mergeObstacles(currentObs, arrival);
1268 1160663 : }
1269 :
1270 49720913 : if (lane->getVehicleNumberWithPartials() > 0) {
1271 : // react to vehicles on the same lane
1272 : // @todo: improve efficiency by using the same iterator for all pedestrians on this lane
1273 272764 : Obstacles vehObs = getVehicleObstacles(lane, dir, &p);
1274 272764 : p.mergeObstacles(currentObs, vehObs);
1275 272764 : if DEBUGCOND(p) {
1276 0 : std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWithVehs=";
1277 0 : DEBUG_PRINT(currentObs);
1278 : }
1279 272764 : }
1280 49720913 : if (hasCrossingVehObs) {
1281 1489 : p.mergeObstacles(currentObs, crossingVehs);
1282 1489 : if DEBUGCOND(p) {
1283 0 : std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWithVehs2=";
1284 0 : DEBUG_PRINT(currentObs);
1285 : }
1286 : }
1287 :
1288 : // walk, taking into account all obstacles
1289 49720913 : p.walk(currentObs, currentTime);
1290 49720913 : gDebugFlag1 = false;
1291 49720913 : if (!p.myWaitingToEnter && !p.myAmJammed) {
1292 24905101 : Obstacle o(p);
1293 24905101 : if (o.closer(obs[p.stripe()], dir)) {
1294 24903486 : obs[p.stripe()] = o;
1295 : }
1296 24905101 : if (o.closer(obs[p.otherStripe()], dir)) {
1297 24903482 : obs[p.otherStripe()] = o;
1298 : }
1299 24905101 : if (MSGlobals::gCheck4Accidents && p.myWalkingAreaPath == nullptr && !p.myAmJammed) {
1300 813781302 : for (int coll = 0; coll < ii; ++coll) {
1301 790284400 : PState& c = *pedestrians[coll];
1302 790284400 : if (!c.myWaitingToEnter && c.myWalkingAreaPath == nullptr && !c.myAmJammed) {
1303 550469925 : if (c.stripe() == p.stripe() || p.stripe() == c.otherStripe() || p.otherStripe() == c.stripe() || p.otherStripe() == c.otherStripe()) {
1304 266610815 : Obstacle cObs(c);
1305 : // we check only for real collisions, no min gap violations
1306 266610815 : if (p.distanceTo(cObs, false) == DIST_OVERLAP) {
1307 52564 : WRITE_WARNING("Collision of person '" + p.myPerson->getID() + "' and person '" + c.myPerson->getID()
1308 : + "', lane='" + lane->getID() + "', time=" + time2string(currentTime) + ".");
1309 : }
1310 : }
1311 : }
1312 : }
1313 : }
1314 : }
1315 : //std::cout << SIMTIME << p.myPerson->getID() << " lane=" << lane->getID() << " x=" << p.myRelX << "\n";
1316 98974897 : }
1317 9693434 : }
1318 :
1319 : bool
1320 160340 : MSPModel_Striping::addCrossingVehs(const MSLane* crossing, int stripes, double lateral_offset, int dir, Obstacles& obs, bool prio) {
1321 : bool hasCrossingVehObs = false;
1322 160340 : const MSLink* crossingExitLink = crossing->getLinkCont().front();
1323 160340 : gDebugFlag1 = DEBUGCOND2(crossing);
1324 160340 : const MSLink::LinkLeaders linkLeaders = crossingExitLink->getLeaderInfo(nullptr, crossing->getLength());
1325 160340 : gDebugFlag1 = false;
1326 160340 : if (linkLeaders.size() > 0) {
1327 21740 : for (MSLink::LinkLeaders::const_iterator it = linkLeaders.begin(); it != linkLeaders.end(); ++it) {
1328 : // the vehicle to enter the junction first has priority
1329 11119 : const MSVehicle* veh = (*it).vehAndGap.first;
1330 11119 : if (veh != nullptr) {
1331 11119 : Obstacle vo((*it).distToCrossing, 0, OBSTACLE_VEHICLE, veh->getID(), veh->getVehicleType().getWidth() + 2 * minGapToVehicle, veh);
1332 : // block entry to the crossing in walking direction but allow leaving it
1333 11119 : Obstacle voBlock = vo;
1334 11119 : if (dir == FORWARD) {
1335 5769 : voBlock.xBack = NUMERICAL_EPS;
1336 : } else {
1337 5350 : voBlock.xFwd = crossing->getLength() - NUMERICAL_EPS;
1338 : }
1339 : // when approaching a priority crossings, vehicles must be able
1340 : // to brake, otherwise the person must be able to cross in time
1341 11119 : const double distToCrossBeforeVeh = (dir == FORWARD ? vo.xFwd : crossing->getLength() - vo.xBack);
1342 : const double bGap = (prio
1343 12849 : ? veh->getCarFollowModel().brakeGap(veh->getSpeed(), veh->getCarFollowModel().getMaxDecel(), 0)
1344 1730 : : veh->getSpeed() * distToCrossBeforeVeh); // walking 1m/s
1345 : double vehYmin;
1346 : double vehYmax;
1347 : // relY increases from left to right (the other way around from vehicles)
1348 11119 : if ((*it).fromLeft()) {
1349 7128 : vehYmin = -(*it).vehAndGap.second + lateral_offset; // vehicle back
1350 7128 : vehYmax = vehYmin + veh->getVehicleType().getLength() + bGap + minGapToVehicle;
1351 7128 : vehYmin -= minGapToVehicle;
1352 : } else {
1353 3991 : vehYmax = crossing->getWidth() + (*it).vehAndGap.second - lateral_offset; // vehicle back
1354 3991 : vehYmin = vehYmax - veh->getVehicleType().getLength() - bGap - minGapToVehicle;
1355 3991 : vehYmax += minGapToVehicle;
1356 :
1357 : }
1358 41050 : for (int s = MAX2(0, PState::stripe(vehYmin)); s < MIN2(PState::stripe(vehYmax), stripes); ++s) {
1359 14097 : if ((dir == FORWARD && obs[s].xBack > vo.xBack)
1360 30203 : || (dir == BACKWARD && obs[s].xFwd < vo.xFwd)) {
1361 29192 : if (!prio && veh->getSpeed() > SUMO_const_haltingSpeed) {
1362 : // do not enter the crossing
1363 9738 : obs[s] = voBlock;
1364 : } else {
1365 19454 : obs[s] = vo;
1366 : }
1367 : hasCrossingVehObs = true;
1368 : }
1369 : }
1370 11119 : if (DEBUGCOND2(crossing)) {
1371 0 : std::cout << SIMTIME
1372 : << " crossingVeh=" << veh->getID()
1373 : << " lane=" << crossing->getID()
1374 0 : << " prio=" << prio
1375 : << " latOffset=" << lateral_offset
1376 : << " dir=" << dir
1377 0 : << " stripes=" << stripes
1378 0 : << " dist=" << (*it).distToCrossing
1379 0 : << " gap=" << (*it).vehAndGap.second
1380 : << " brakeGap=" << bGap
1381 0 : << " fromLeft=" << (*it).fromLeft()
1382 : << " distToCrossBefore=" << distToCrossBeforeVeh
1383 : << " ymin=" << vehYmin
1384 : << " ymax=" << vehYmax
1385 : << " smin=" << PState::stripe(vehYmin)
1386 0 : << " smax=" << PState::stripe(vehYmax)
1387 0 : << "\n";
1388 0 : DEBUG_PRINT(obs);
1389 : }
1390 : }
1391 : }
1392 10621 : if (hasCrossingVehObs) {
1393 : // check whether the crossing is fully blocked
1394 6046 : const int reserved = getReserved((int)obs.size(), RESERVE_FOR_ONCOMING_FACTOR_JUNCTIONS);
1395 : bool allBlocked = true;
1396 :
1397 32993 : for (int i = 0; i < (int)obs.size(); i++) {
1398 29680 : const Obstacle& o = obs[i];
1399 29680 : if (o.type != OBSTACLE_VEHICLE && (
1400 4206 : (dir == FORWARD && i >= reserved) ||
1401 3001 : (dir == BACKWARD && i < (int)obs.size() - reserved))) {
1402 : allBlocked = false;
1403 : break;
1404 : }
1405 : }
1406 6046 : if (allBlocked) {
1407 3313 : if (DEBUGCOND2(crossing)) {
1408 0 : std::cout << SIMTIME << " crossing=" << crossing->getID() << " allBlocked\n";
1409 : }
1410 24818 : for (Obstacle& o : obs) {
1411 21505 : if (dir == FORWARD) {
1412 9911 : o.xBack = NUMERICAL_EPS;
1413 : } else {
1414 11594 : o.xFwd = crossing->getLength() - NUMERICAL_EPS;
1415 : }
1416 : }
1417 : }
1418 : }
1419 : }
1420 160340 : return hasCrossingVehObs;
1421 : }
1422 :
1423 :
1424 : MSPModel_Striping::Obstacles
1425 291957 : MSPModel_Striping::getVehicleObstacles(const MSLane* lane, int dir, PState* ped) {
1426 291957 : const int stripes = numStripes(lane);
1427 583914 : Obstacles vehObs(stripes, Obstacle(dir));
1428 : int current = -1;
1429 : double minX = 0.;
1430 : double maxX = 0.;
1431 : double pRelY = -1.;
1432 : double pWidth = 0.;
1433 : std::string pID;
1434 291957 : bool debug = DEBUGCOND2(lane);
1435 291957 : if (ped != nullptr) {
1436 272764 : current = ped->stripe();
1437 272764 : minX = ped->getMinX();
1438 272764 : maxX = ped->getMaxX();
1439 272764 : pRelY = ped->myRelY;
1440 272764 : pWidth = ped->myPerson->getVehicleType().getWidth();
1441 272764 : pID = ped->myPerson->getID();
1442 272764 : debug = DEBUGCOND(*ped);
1443 19193 : } else if (dir == BACKWARD) {
1444 : // checking vehicles on the next lane. Use entry point as reference
1445 : minX = lane->getLength();
1446 : maxX = lane->getLength();
1447 : }
1448 291957 : MSLane::AnyVehicleIterator begin = (dir == FORWARD ? lane->anyVehiclesUpstreamBegin() : lane->anyVehiclesBegin());
1449 : MSLane::AnyVehicleIterator end = (dir == FORWARD ? lane->anyVehiclesUpstreamEnd() : lane->anyVehiclesEnd());
1450 760260 : for (MSLane::AnyVehicleIterator it = begin; it != end; ++it) {
1451 468303 : const MSVehicle* veh = *it;
1452 468303 : const bool bidi = veh->getLane() == lane->getBidiLane();
1453 468303 : const double vehBack = veh->getBackPositionOnLane(lane);
1454 468303 : double vehFront = vehBack + veh->getVehicleType().getLength();
1455 : // ensure that vehicles are not blocked
1456 468303 : const double vehNextSpeed = MAX2(veh->getSpeed(), 1.0);
1457 468303 : const double clearance = SAFETY_GAP + vehNextSpeed * LOOKAHEAD_SAMEDIR;
1458 : // boundaries for range checking
1459 : double vehXMax;
1460 : double vehXMin;
1461 : double vehXMaxCheck;
1462 : double vehXMinCheck;
1463 468303 : if (bidi) {
1464 1571 : vehFront = vehBack - veh->getVehicleType().getLength();
1465 1571 : vehXMax = vehBack + SAFETY_GAP;
1466 1571 : vehXMin = vehFront - clearance;
1467 1571 : if (dir == FORWARD) {
1468 1571 : vehXMaxCheck = vehBack + NUMERICAL_EPS;
1469 1571 : vehXMinCheck = vehFront - LOOKAROUND_VEHICLES;
1470 : } else {
1471 0 : vehXMaxCheck = vehBack + LOOKAHEAD_SAMEDIR;
1472 : vehXMinCheck = vehFront - clearance;
1473 : }
1474 : } else {
1475 466732 : vehXMax = vehFront + clearance;
1476 466732 : vehXMin = vehBack - SAFETY_GAP;
1477 466732 : if (dir == FORWARD) {
1478 : vehXMaxCheck = vehFront + clearance;
1479 343975 : vehXMinCheck = vehBack - LOOKAHEAD_SAMEDIR;
1480 : } else {
1481 122757 : vehXMaxCheck = vehFront + LOOKAROUND_VEHICLES;
1482 122757 : vehXMinCheck = vehBack - NUMERICAL_EPS;
1483 : }
1484 : }
1485 468303 : if (debug) {
1486 0 : std::cout << SIMTIME << " ped=" << pID << " veh=" << veh->getID() << " check obstacle on lane=" << lane->getID()
1487 : << "\n"
1488 : << " vehXMin=" << vehXMin
1489 : << " vehXMax=" << vehXMax
1490 : << " vehXMinC=" << vehXMinCheck
1491 : << " vehXMaxC=" << vehXMaxCheck
1492 : << " minX=" << minX
1493 : << " maxX=" << maxX
1494 0 : << " bidi=" << bidi
1495 : << " vFront=" << vehFront
1496 : << " vBack=" << vehBack
1497 0 : << "\n";
1498 : }
1499 468303 : if (vehXMaxCheck > minX && vehXMinCheck && vehXMinCheck <= maxX) {
1500 358077 : Obstacle vo(vehBack, veh->getSpeed() * (bidi ? -1 : 1), OBSTACLE_VEHICLE, veh->getID(), 0, veh);
1501 : // moving vehicles block space along their path
1502 179372 : vo.xFwd = vehXMax;
1503 179372 : vo.xBack = vehXMin;
1504 : // relY increases from left to right (the other way around from vehicles)
1505 : // XXX lateral offset for partial vehicles
1506 179372 : const double posLat = veh->getLateralPositionOnLane() * (bidi ? -1 : 1);
1507 179372 : const double vehYmax = 0.5 * (lane->getWidth() + veh->getVehicleType().getWidth() - stripeWidth) - posLat;
1508 179372 : const double vehYmin = vehYmax - veh->getVehicleType().getWidth();
1509 765956 : for (int s = MAX2(0, PState::stripe(vehYmin)); s < MIN2(PState::stripe(vehYmax) + 1, stripes); ++s) {
1510 586584 : Obstacle prior = vehObs[s];
1511 : vehObs[s] = vo;
1512 586584 : if (s == current && vehFront + SAFETY_GAP < minX) {
1513 : // ignore if aleady overlapping while vehicle is still behind
1514 18009 : if (pRelY - pWidth < vehYmax &&
1515 17888 : pRelY + pWidth > vehYmin && dir == FORWARD) {
1516 11770 : if (debug) {
1517 0 : std::cout << " ignoring vehicle '" << veh->getID() << " on stripe " << s << " vehFrontSG=" << vehFront + SAFETY_GAP << " minX=" << minX << "\n";
1518 : }
1519 : if (dir == FORWARD) {
1520 : vehObs[s] = prior;
1521 : } else {
1522 : vehObs[s].xFwd = MIN2(vo.xFwd, vehFront + SAFETY_GAP);
1523 : }
1524 : }
1525 : }
1526 : }
1527 179372 : if (debug) {
1528 0 : std::cout << SIMTIME << " ped=" << pID << " veh=" << veh->getID() << " obstacle on lane=" << lane->getID()
1529 : << "\n"
1530 : << " ymin=" << vehYmin
1531 : << " ymax=" << vehYmax
1532 : << " smin=" << PState::stripe(vehYmin)
1533 0 : << " smax=" << PState::stripe(vehYmax)
1534 0 : << " relY=" << pRelY
1535 : << " current=" << current
1536 0 : << " vo.xFwd=" << vo.xFwd
1537 0 : << " vo.xBack=" << vo.xBack
1538 : << " vFront=" << vehFront
1539 : << " vBack=" << vehBack
1540 0 : << "\n";
1541 : }
1542 : }
1543 : }
1544 291957 : return vehObs;
1545 0 : }
1546 :
1547 :
1548 : // ===========================================================================
1549 : // MSPModel_Striping::Obstacle method definitions
1550 : // ===========================================================================
1551 59706304 : MSPModel_Striping::Obstacle::Obstacle(int dir, double dist) :
1552 59706304 : xFwd(dir * dist), // by default, far away when seen in dir
1553 59706304 : xBack(dir * dist), // by default, far away when seen in dir
1554 59706304 : speed(0),
1555 59706304 : type(OBSTACLE_NONE),
1556 59706304 : description("") {
1557 59706304 : }
1558 :
1559 :
1560 385441580 : MSPModel_Striping::Obstacle::Obstacle(const PState& ped) :
1561 385441580 : xFwd(ped.getMaxX()),
1562 385441580 : xBack(ped.getMinX()),
1563 385441580 : speed(ped.myDir * ped.mySpeed),
1564 385441580 : type(ped.getOType()),
1565 385441580 : description(ped.getID()) {
1566 : assert(!ped.myWaitingToEnter);
1567 385441580 : if (type == OBSTACLE_VEHICLE) {
1568 585143 : vehicle = static_cast<const PStateVehicle&>(ped).getVehicle();
1569 : }
1570 385441580 : }
1571 :
1572 :
1573 : bool
1574 98441518 : MSPModel_Striping::Obstacle::closer(const Obstacle& o, int dir) {
1575 98441518 : if (dir == FORWARD) {
1576 51249788 : return xBack <= o.xBack;
1577 : } else {
1578 47191730 : return xFwd >= o.xFwd;
1579 : }
1580 : }
1581 :
1582 :
1583 : // ===========================================================================
1584 : // MSPModel_Striping::PState method definitions
1585 : // ===========================================================================
1586 269551 : MSPModel_Striping::PState::PState(MSPerson* person, MSStageMoving* stage, const MSLane* lane):
1587 269551 : myPerson(person),
1588 269551 : myStage(stage),
1589 269551 : myLane(lane),
1590 269551 : myRelX(stage->getDepartPos()),
1591 269551 : myRelY(stage->getDepartPosLat()),
1592 269551 : myDir(FORWARD),
1593 269551 : mySpeed(0),
1594 269551 : mySpeedLat(0),
1595 269551 : myWaitingToEnter(true),
1596 269551 : myWaitingTime(0),
1597 269551 : myWalkingAreaPath(nullptr),
1598 269551 : myAmJammed(false),
1599 269551 : myRemoteXYPos(Position::INVALID),
1600 269551 : myAngle(std::numeric_limits<double>::max()) {
1601 : const MSEdge* currentEdge = &lane->getEdge();
1602 : const ConstMSEdgeVector& route = myStage->getRoute();
1603 : assert(!route.empty());
1604 269551 : if (route.size() == 1) {
1605 : // only a single edge, move towards end pos
1606 11649 : myDir = (myRelX <= myStage->getArrivalPos()) ? FORWARD : BACKWARD;
1607 257902 : } else if (route.front()->getFunction() != SumoXMLEdgeFunc::NORMAL) {
1608 : // start on an intersection
1609 : myDir = FORWARD;
1610 10 : if (route.front()->isWalkingArea()) {
1611 10 : myWalkingAreaPath = getArbitraryPath(route.front());
1612 : }
1613 : } else {
1614 257892 : int passedFwd = 0;
1615 257892 : int passedBwd = 0;
1616 257892 : const bool mayStartForward = canTraverse(FORWARD, route, passedFwd) != UNDEFINED_DIRECTION;
1617 257892 : const bool mayStartBackward = canTraverse(BACKWARD, route, passedBwd) != UNDEFINED_DIRECTION;
1618 257892 : if DEBUGCOND(*this) {
1619 0 : std::cout << " initialize dir for " << myPerson->getID() << " forward=" << mayStartForward << " backward=" << mayStartBackward << "\n";
1620 : }
1621 257892 : if (mayStartForward && mayStartBackward) {
1622 : // figure out the best direction via routing
1623 : ConstMSEdgeVector crossingRoute;
1624 6110 : MSNet::getInstance()->getPedestrianRouter(0).compute(currentEdge, route.back(), myRelX, myStage->getArrivalPos(), myStage->getMaxSpeed(person), 0, nullptr, crossingRoute, true);
1625 3055 : if (crossingRoute.size() > 1) {
1626 : // route found
1627 3047 : const MSEdge* nextEdge = crossingRoute[1];
1628 3047 : if (nextEdge->getFromJunction() == currentEdge->getFromJunction() || nextEdge->getToJunction() == currentEdge->getFromJunction()) {
1629 2307 : myDir = BACKWARD;
1630 : }
1631 : }
1632 3055 : if DEBUGCOND(*this) {
1633 0 : std::cout << " crossingRoute=" << toString(crossingRoute) << "\n";
1634 : }
1635 254837 : } else if (!mayStartForward && !mayStartBackward) {
1636 103 : int lastDisconnect = passedFwd >= passedBwd ? passedFwd : passedBwd;
1637 206 : std::string dLoc = TLF(", time=%.", SIMTIME);
1638 103 : if (route.size() > 2) {
1639 225 : dLoc = TLF("between edge '%' and edge '%', time=%.", route[lastDisconnect - 1]->getID(), route[lastDisconnect]->getID(), SIMTIME);
1640 : }
1641 618 : WRITE_WARNINGF(TL("Person '%' walking from edge '%' to edge '%' has a disconnect%"),
1642 : myPerson->getID(), route.front()->getID(), route.back()->getID(), dLoc);
1643 108 : myDir = passedFwd >= passedBwd ? FORWARD : BACKWARD;
1644 : } else {
1645 421072 : myDir = !mayStartBackward ? FORWARD : BACKWARD;
1646 : }
1647 : }
1648 269551 : if (myRelY == UNSPECIFIED_POS_LAT || myLegacyPosLat) {
1649 268771 : if (myRelY == UNSPECIFIED_POS_LAT) {
1650 268601 : myRelY = 0;
1651 : }
1652 268771 : if (lane->getVehicleNumberWithPartials() > 0 && myRelY == 0) {
1653 : // better start next to the road if nothing was specified
1654 1949 : myRelY -= stripeWidth;
1655 : }
1656 268771 : if (myDir == FORWARD || lane->getPermissions() != SVC_PEDESTRIAN) {
1657 : // start at the right side of the sidewalk on shared roads
1658 252298 : myRelY = stripeWidth * (numStripes(lane) - 1) - myRelY;
1659 : }
1660 780 : } else if (myRelY == RANDOM_POS_LAT) {
1661 20 : myRelY = RandHelper::rand() * stripeWidth * (numStripes(lane) - 1);
1662 : } else {
1663 : // convert vehicle-style posLat (0 is center, left is larger)
1664 : // into striping coordinates (0 is on the leftmost stripe, right is larger)
1665 760 : myRelY = lane->getWidth() / 2 - myRelY - stripeWidth / 2;
1666 : }
1667 269551 : if DEBUGCOND(*this) {
1668 0 : std::cout << " added new pedestrian " << myPerson->getID() << " on " << lane->getID() << " myRelX=" << myRelX << " myRelY=" << myRelY << " dir=" << myDir << " route=" << toString(myStage->getRoute()) << "\n";
1669 : }
1670 :
1671 269551 : myNLI = getNextLane(*this, lane, nullptr);
1672 269547 : }
1673 :
1674 :
1675 550197 : MSPModel_Striping::PState::PState():
1676 550197 : myPerson(nullptr),
1677 550197 : myStage(nullptr),
1678 550197 : myLane(nullptr),
1679 550197 : myRelX(0),
1680 550197 : myRelY(0),
1681 550197 : myDir(UNDEFINED_DIRECTION),
1682 550197 : mySpeed(0),
1683 550197 : mySpeedLat(0),
1684 550197 : myWaitingToEnter(false),
1685 550197 : myWaitingTime(0),
1686 550197 : myWalkingAreaPath(nullptr),
1687 550197 : myAmJammed(false),
1688 550197 : myRemoteXYPos(Position::INVALID),
1689 550197 : myAngle(std::numeric_limits<double>::max()) {
1690 550197 : }
1691 :
1692 :
1693 20 : MSPModel_Striping::PState::PState(MSPerson* person, MSStageMoving* stage, std::istringstream* in):
1694 20 : myPerson(person),
1695 20 : myStage(stage),
1696 20 : myLane(nullptr),
1697 20 : myWalkingAreaPath(nullptr),
1698 20 : myRemoteXYPos(Position::INVALID),
1699 20 : myAngle(std::numeric_limits<double>::max()) {
1700 20 : if (in != nullptr) {
1701 : std::string laneID;
1702 : std::string wapLaneFrom;
1703 : std::string wapLaneTo;
1704 : std::string nextLaneID;
1705 : std::string nextLinkFrom;
1706 : std::string nextLinkTo;
1707 : int nextDir;
1708 :
1709 20 : (*in) >> laneID
1710 20 : >> myRelX >> myRelY >> myDir >> mySpeed >> mySpeedLat >> myWaitingToEnter >> myWaitingTime
1711 20 : >> wapLaneFrom >> wapLaneTo
1712 20 : >> myAmJammed
1713 : >> nextLaneID
1714 : >> nextLinkFrom
1715 20 : >> nextLinkTo
1716 20 : >> nextDir;
1717 :
1718 :
1719 20 : myLane = MSLane::dictionary(laneID);
1720 20 : if (myLane == nullptr) {
1721 0 : throw ProcessError("Unknown lane '" + laneID + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1722 : }
1723 :
1724 : MSLane* nextLane = nullptr;
1725 20 : if (nextLaneID != "null") {
1726 18 : nextLane = MSLane::dictionary(nextLaneID);
1727 18 : if (nextLane == nullptr) {
1728 0 : throw ProcessError("Unknown next lane '" + nextLaneID + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1729 : }
1730 : }
1731 : const MSLink* link = nullptr;
1732 20 : if (nextLinkFrom != "null") {
1733 4 : MSLane* from = MSLane::dictionary(nextLinkFrom);
1734 4 : MSLane* to = MSLane::dictionary(nextLinkTo);
1735 4 : if (from == nullptr) {
1736 0 : throw ProcessError("Unknown link origin lane '" + nextLinkFrom + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1737 : }
1738 4 : if (to == nullptr) {
1739 0 : throw ProcessError("Unknown link destination lane '" + nextLinkTo + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1740 : }
1741 4 : link = from->getLinkTo(to);
1742 : }
1743 20 : myNLI = NextLaneInfo(nextLane, link, nextDir);
1744 :
1745 20 : if (wapLaneFrom != "null") {
1746 2 : MSLane* from = MSLane::dictionary(wapLaneFrom);
1747 2 : MSLane* to = MSLane::dictionary(wapLaneTo);
1748 2 : if (from == nullptr) {
1749 0 : throw ProcessError("Unknown walkingAreaPath origin lane '" + wapLaneFrom + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1750 : }
1751 2 : if (to == nullptr) {
1752 0 : throw ProcessError("Unknown walkingAreaPath destination lane '" + wapLaneTo + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1753 : }
1754 2 : const auto pathIt = myWalkingAreaPaths.find(std::make_pair(from, to));
1755 2 : if (pathIt != myWalkingAreaPaths.end()) {
1756 2 : myWalkingAreaPath = &pathIt->second;
1757 : } else {
1758 0 : throw ProcessError("Unknown walkingAreaPath from lane '" + wapLaneFrom + "' to lane '" + wapLaneTo + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1759 : }
1760 : }
1761 : }
1762 20 : }
1763 :
1764 : void
1765 14 : MSPModel_Striping::PState::saveState(std::ostringstream& out) {
1766 14 : std::string wapLaneFrom = "null";
1767 14 : std::string wapLaneTo = "null";
1768 14 : if (myWalkingAreaPath != nullptr) {
1769 2 : wapLaneFrom = myWalkingAreaPath->from->getID();
1770 2 : wapLaneTo = myWalkingAreaPath->to->getID();
1771 : }
1772 14 : std::string nextLaneID = "null";
1773 14 : std::string nextLinkFrom = "null";
1774 14 : std::string nextLinkTo = "null";
1775 14 : if (myNLI.lane != nullptr) {
1776 : nextLaneID = myNLI.lane->getID();
1777 : }
1778 14 : if (myNLI.link != nullptr) {
1779 : nextLinkFrom = myNLI.link->getLaneBefore()->getID();
1780 4 : nextLinkTo = myNLI.link->getViaLaneOrLane()->getID();
1781 : }
1782 14 : out << " " << myLane->getID()
1783 28 : << " " << myRelX
1784 14 : << " " << myRelY
1785 : << " " << myDir
1786 28 : << " " << mySpeed
1787 14 : << " " << mySpeedLat
1788 14 : << " " << myWaitingToEnter
1789 14 : << " " << myWaitingTime
1790 : << " " << wapLaneFrom
1791 : << " " << wapLaneTo
1792 14 : << " " << myAmJammed
1793 : << " " << nextLaneID
1794 : << " " << nextLinkFrom
1795 : << " " << nextLinkTo
1796 14 : << " " << myNLI.dir;
1797 14 : }
1798 :
1799 : double
1800 1472016297 : MSPModel_Striping::PState::getMinX(const bool includeMinGap) const {
1801 : // @todo speed should have an influence here because faster persons need more space
1802 1472016297 : if (myDir == FORWARD) {
1803 986353271 : return myRelX - getLength();
1804 : }
1805 485663026 : return myRelX - (includeMinGap ? getMinGap() : 0.);
1806 : }
1807 :
1808 :
1809 : double
1810 1472016297 : MSPModel_Striping::PState::getMaxX(const bool includeMinGap) const {
1811 : // @todo speed should have an influence here because faster persons need more space
1812 1472016297 : if (myDir == FORWARD) {
1813 986353271 : return myRelX + (includeMinGap ? getMinGap() : 0.);
1814 : }
1815 485663026 : return myRelX + getLength();
1816 : }
1817 :
1818 :
1819 : double
1820 1521770357 : MSPModel_Striping::PState::getLength() const {
1821 1521770357 : return myPerson->getVehicleType().getLength();
1822 : }
1823 :
1824 :
1825 : double
1826 923936095 : MSPModel_Striping::PState::getMinGap() const {
1827 923936095 : return myPerson->getVehicleType().getMinGap();
1828 : }
1829 :
1830 :
1831 : int
1832 3309934933 : MSPModel_Striping::PState::stripe(double relY) {
1833 3309934933 : return (int)floor(relY / stripeWidth + 0.5);
1834 : }
1835 :
1836 :
1837 : int
1838 1354946753 : MSPModel_Striping::PState::otherStripe(double relY) const {
1839 1354946753 : const int s = stripe(relY);
1840 1354946753 : const double offset = relY - s * stripeWidth;
1841 1354946753 : const double threshold = MAX2(NUMERICAL_EPS, stripeWidth - SQUEEZE * getWidth());
1842 : int result;
1843 1354946753 : if (offset > threshold) {
1844 447188 : result = s + 1;
1845 1354499565 : } else if (offset < -threshold) {
1846 311964 : result = s - 1;
1847 : } else {
1848 : result = s;
1849 : }
1850 : //std::cout.setf(std::ios::fixed , std::ios::floatfield);
1851 : //std::cout << std::setprecision(5);
1852 : //if DEBUGCOND(*this) std::cout << " otherStripe " << myPerson->getID() << " offset=" << offset << " threshold=" << threshold << " rawResult=" << result << "\n";
1853 1354946753 : return result;
1854 : }
1855 :
1856 : int
1857 1932239896 : MSPModel_Striping::PState::stripe() const {
1858 1932239896 : return MIN2(MAX2(0, stripe(myRelY)), numStripes(myLane) - 1);
1859 : }
1860 :
1861 :
1862 : int
1863 1342143505 : MSPModel_Striping::PState::otherStripe() const {
1864 1342143505 : return MIN2(MAX2(0, otherStripe(myRelY)), numStripes(myLane) - 1);
1865 : }
1866 :
1867 :
1868 : double
1869 101596609 : MSPModel_Striping::PState::distToLaneEnd() const {
1870 101596609 : if (myStage->getNextRouteEdge() == nullptr) {
1871 16861592 : return myDir * (myStage->getArrivalPos() - myRelX) - POSITION_EPS - (
1872 573562 : (myWaitingTime > DELTA_T && (myStage->getDestinationStop() == nullptr ||
1873 : myStage->getDestinationStop()->getWaitingCapacity() > myStage->getDestinationStop()->getNumWaitingPersons()))
1874 17435154 : ? getMinGap() : 0);
1875 : } else {
1876 84735017 : const double length = myWalkingAreaPath == nullptr ? myLane->getLength() : myWalkingAreaPath->length;
1877 84735017 : return myDir == FORWARD ? length - myRelX : myRelX;
1878 : }
1879 : }
1880 :
1881 :
1882 : bool
1883 948585 : MSPModel_Striping::PState::moveToNextLane(SUMOTime currentTime) {
1884 948585 : double dist = distToLaneEnd();
1885 948585 : if (DEBUGCOND(*this)) {
1886 0 : std::cout << SIMTIME << " ped=" << myPerson->getID() << " myRelX=" << myRelX << " dist=" << dist << "\n";
1887 : }
1888 948585 : if (dist <= 0) {
1889 : //if (ped.myPerson->getID() == DEBUG1) {
1890 : // std::cout << SIMTIME << " addToLane x=" << ped.myRelX << " newDir=" << newDir << " newLane=" << newLane->getID() << " walkingAreaShape=" << walkingAreaShape << "\n";
1891 : //}
1892 : //std::cout << " changing to " << newLane->getID() << " myRelY=" << ped.myRelY << " oldStripes=" << numStripes(myLane) << " newStripes=" << numStripes(newLane);
1893 : //std::cout << " newY=" << ped.myRelY << " myDir=" << ped.myDir << " newDir=" << newDir;
1894 948585 : const int oldDir = myDir;
1895 948585 : const MSLane* oldLane = myLane;
1896 948585 : myLane = myNLI.lane;
1897 948585 : myDir = myNLI.dir;
1898 948585 : const bool normalLane = (myLane == nullptr || myLane->getEdge().getFunction() == SumoXMLEdgeFunc::NORMAL || &myLane->getEdge() == myStage->getNextRouteEdge());
1899 948585 : if DEBUGCOND(*this) {
1900 0 : std::cout << SIMTIME
1901 0 : << " ped=" << myPerson->getID()
1902 : << " moveToNextLane old=" << oldLane->getID()
1903 0 : << " new=" << (myLane == nullptr ? "NULL" : myLane->getID())
1904 : << " oldDir=" << oldDir
1905 0 : << " newDir=" << myDir
1906 0 : << " myRelX=" << myRelX
1907 : << " dist=" << dist
1908 0 : << "\n";
1909 : }
1910 948585 : if (myLane == nullptr) {
1911 266675 : myRelX = myStage->getArrivalPos();
1912 : }
1913 : // moveToNextEdge might destroy the person and thus mess up the heap. Better check first
1914 948585 : if (myStage->getRouteStep() == myStage->getRoute().end() - 1) {
1915 266675 : myLane = nullptr;
1916 : } else {
1917 681910 : const bool arrived = myStage->moveToNextEdge(myPerson, currentTime, oldDir, normalLane ? nullptr : &myLane->getEdge());
1918 : UNUSED_PARAMETER(arrived);
1919 : assert(!arrived);
1920 : assert(myDir != UNDEFINED_DIRECTION);
1921 681910 : myNLI = getNextLane(*this, myLane, oldLane);
1922 : // reminders must be called after updated myNLI (to ensure that getNextEdgePtr returns the correct edge)
1923 681910 : myStage->activateEntryReminders(myPerson);
1924 : assert(myNLI.lane != oldLane); // do not turn around
1925 681910 : if DEBUGCOND(*this) {
1926 0 : std::cout << " nextLane=" << (myNLI.lane == nullptr ? "NULL" : myNLI.lane->getID()) << "\n";
1927 : }
1928 681910 : if (myLane->getEdge().isWalkingArea()) {
1929 103738 : if (myNLI.dir != UNDEFINED_DIRECTION) {
1930 103701 : myWalkingAreaPath = getWalkingAreaPath(&myLane->getEdge(), oldLane, myNLI.lane);
1931 : assert(myWalkingAreaPath->shape.size() >= 2);
1932 103701 : if DEBUGCOND(*this) {
1933 0 : std::cout << " mWAPath shape=" << myWalkingAreaPath->shape << " length=" << myWalkingAreaPath->length << "\n";
1934 : }
1935 37 : } else if (myNLI.link != nullptr) {
1936 : // using direct connection (instead of using walkingarea)
1937 17 : myLane = myNLI.lane;
1938 : assert(!myLane->getEdge().isWalkingArea());
1939 17 : myStage->moveToNextEdge(myPerson, currentTime, myDir, &myLane->getEdge());
1940 17 : myWalkingAreaPath = nullptr;
1941 17 : myNLI = getNextLane(*this, myLane, oldLane);
1942 : } else {
1943 : // disconnnected route. move to the next edge
1944 40 : if (OptionsCont::getOptions().getBool("ignore-route-errors")) {
1945 : // try to determine direction from topology, otherwise maintain current direction
1946 16 : const MSEdge* currRouteEdge = *myStage->getRouteStep();
1947 16 : const MSEdge* nextRouteEdge = myStage->getNextRouteEdge();
1948 : if ((nextRouteEdge->getToJunction() == currRouteEdge->getFromJunction())
1949 16 : || nextRouteEdge->getToJunction() == currRouteEdge->getToJunction()) {
1950 4 : myDir = BACKWARD;
1951 : } else if ((nextRouteEdge->getFromJunction() == currRouteEdge->getFromJunction())
1952 12 : || nextRouteEdge->getFromJunction() == currRouteEdge->getToJunction()) {
1953 12 : myDir = FORWARD;
1954 : }
1955 16 : myStage->moveToNextEdge(myPerson, currentTime, oldDir, nullptr);
1956 16 : myLane = myNLI.lane;
1957 : assert(myLane != 0);
1958 : assert(myLane->getEdge().getFunction() == SumoXMLEdgeFunc::NORMAL);
1959 16 : myNLI = getNextLane(*this, myLane, oldLane);
1960 16 : myWalkingAreaPath = nullptr;
1961 : } else {
1962 16 : throw ProcessError(TLF("Disconnected walk for person '%'.", myPerson->getID()));
1963 : }
1964 : }
1965 : } else {
1966 578172 : myWalkingAreaPath = nullptr;
1967 : }
1968 : // adapt x to fit onto the new lane
1969 : // (make sure we do not move past the end of the new lane since that
1970 : // lane was not checked for obstacles)
1971 681906 : const double newLength = (myWalkingAreaPath == nullptr ? myLane->getLength() : myWalkingAreaPath->length);
1972 681906 : if (-dist > newLength) {
1973 : assert(OptionsCont::getOptions().getBool("ignore-route-errors"));
1974 : // should not happen because the end of myLane should have been an obstacle as well
1975 : // (only when the route is broken)
1976 0 : dist = -newLength;
1977 : }
1978 681906 : if (myDir == BACKWARD) {
1979 246068 : myRelX = newLength + dist;
1980 : } else {
1981 435838 : myRelX = -dist;
1982 : }
1983 681906 : if DEBUGCOND(*this) {
1984 0 : std::cout << SIMTIME << " update myRelX ped=" << myPerson->getID()
1985 : << " newLength=" << newLength
1986 : << " dist=" << dist
1987 0 : << " myRelX=" << myRelX
1988 0 : << "\n";
1989 : }
1990 : // adjust to change in direction
1991 681906 : if (myDir != oldDir) {
1992 102792 : myRelY = (numStripes(oldLane) - 1) * stripeWidth - myRelY;
1993 : }
1994 : // adjust to differences in sidewalk width
1995 779921 : const int offset = getStripeOffset(numStripes(oldLane), numStripes(myLane), oldDir != myDir && numStripes(myLane) < numStripes(oldLane));
1996 681906 : myRelY += offset * stripeWidth;
1997 681906 : if DEBUGCOND(*this) {
1998 0 : std::cout << SIMTIME << " transformY ped=" << myPerson->getID()
1999 0 : << " newLane=" << Named::getIDSecure(myLane)
2000 0 : << " newY=" << myRelY
2001 0 : << " os=" << numStripes(oldLane) << " ns=" << numStripes(myLane)
2002 0 : << " od=" << oldDir << " nd=" << myDir
2003 0 : << " offset=" << offset << "\n";
2004 : }
2005 : }
2006 948581 : myAngle = std::numeric_limits<double>::max(); // see #9014
2007 948581 : return true;
2008 : } else {
2009 : return false;
2010 : }
2011 : }
2012 :
2013 :
2014 : int
2015 49726959 : MSPModel_Striping::getReserved(int stripes, double factor) {
2016 49726959 : return MIN2(
2017 49726959 : (int)floor(stripes * factor),
2018 49726959 : (int)floor(RESERVE_FOR_ONCOMING_MAX / stripeWidth));
2019 : }
2020 :
2021 : void
2022 49720913 : MSPModel_Striping::PState::walk(const Obstacles& obs, SUMOTime currentTime) {
2023 49720913 : const int stripes = (int)obs.size();
2024 49720913 : const int sMax = stripes - 1;
2025 : assert(stripes == numStripes(myLane));
2026 : // account stage-specific max speed but also for normal lane speed limit
2027 : // (speed limits on crossings and walkingareas ignored due to #11527)
2028 49720913 : const double vMax = (myStage->getConfiguredSpeed() >= 0
2029 49720913 : ? myStage->getConfiguredSpeed()
2030 55490723 : : (myLane->isNormal() || myLane->isInternal()
2031 53916894 : ? myLane->getVehicleMaxSpeed(myPerson)
2032 1573829 : : myStage->getMaxSpeed(myPerson)));
2033 : // ultimate goal is to choose the prefered stripe (chosen)
2034 49720913 : const int current = stripe();
2035 49720913 : const int other = otherStripe();
2036 : // compute distances
2037 49720913 : std::vector<double> distance(stripes);
2038 224334165 : for (int i = 0; i < stripes; ++i) {
2039 174613252 : distance[i] = distanceTo(obs[i], obs[i].type == OBSTACLE_PED);
2040 : }
2041 : // compute utility for all stripes
2042 49720913 : std::vector<double> utility(stripes, 0);
2043 : // forbid stripes which are blocked and also all stripes behind them
2044 224334165 : for (int i = 0; i < stripes; ++i) {
2045 174613252 : if (distance[i] == DIST_OVERLAP) {
2046 53567763 : if (i == current && (!myWaitingToEnter || stripe() != stripe(myRelY))) {
2047 3358814 : utility[i] += OBSTRUCTED_PENALTY;
2048 : }
2049 53567763 : if (i < current) {
2050 61573846 : for (int j = 0; j <= i; ++j) {
2051 39147085 : utility[j] += OBSTRUCTED_PENALTY;
2052 : }
2053 : }
2054 53567763 : if (i > current) {
2055 53666981 : for (int j = i; j < stripes; ++j) {
2056 32753623 : utility[j] += OBSTRUCTED_PENALTY;
2057 : }
2058 : }
2059 : }
2060 : }
2061 : // forbid a portion of the leftmost stripes (in walking direction).
2062 : // lanes with stripes less than 1 / RESERVE_FOR_ONCOMING_FACTOR
2063 : // may still deadlock in heavy pedestrian traffic
2064 49720913 : const bool onJunction = myLane->getEdge().isWalkingArea() || myLane->getEdge().isCrossing();
2065 49720913 : const int reserved = getReserved(stripes, (onJunction ? RESERVE_FOR_ONCOMING_FACTOR_JUNCTIONS : RESERVE_FOR_ONCOMING_FACTOR));
2066 49720913 : if (myDir == FORWARD) {
2067 38613366 : for (int i = 0; i < reserved; ++i) {
2068 5443776 : utility[i] += INAPPROPRIATE_PENALTY * (i == current ? 0.5 : 1);
2069 : }
2070 : } else {
2071 14199220 : for (int i = sMax; i > sMax - reserved; --i) {
2072 713748 : utility[i] += INAPPROPRIATE_PENALTY * (i == current ? 0.5 : 1);
2073 : }
2074 : }
2075 : // adapt utility based on obstacles
2076 224334165 : for (int i = 0; i < stripes; ++i) {
2077 174613252 : if (obs[i].speed * myDir < 0) {
2078 : // penalize evasion to the left unless the obstacle is a vehicle
2079 4231137 : if ((myDir == FORWARD || obs[i].type == OBSTACLE_VEHICLE) && i > 0) {
2080 842648 : utility[i - 1] -= 0.5;
2081 3388489 : } else if (myDir == BACKWARD && i < sMax) {
2082 587670 : utility[i + 1] -= 0.5;
2083 : }
2084 : }
2085 : // compute expected distance achievable by staying on this stripe for a time horizon
2086 174613252 : const double walkDist = MAX2(0., distance[i]); // disregard special distance flags
2087 174613252 : const double lookAhead = obs[i].speed * myDir >= 0 ? LOOKAHEAD_SAMEDIR : LOOKAHEAD_ONCOMING;
2088 174613252 : const double expectedDist = MIN2(vMax * LOOKAHEAD_SAMEDIR, walkDist + obs[i].speed * myDir * lookAhead);
2089 174613252 : if (expectedDist >= 0) {
2090 171593485 : utility[i] += expectedDist;
2091 : } else {
2092 : // let only the distance count
2093 3019767 : utility[i] += ONCOMING_CONFLICT_PENALTY + distance[i];
2094 : }
2095 : }
2096 : // discourage use of the leftmost stripe (in walking direction) if there are oncoming
2097 49720913 : if (myDir == FORWARD && obs[0].speed < 0) {
2098 1386619 : utility[0] += ONCOMING_CONFLICT_PENALTY;
2099 48334294 : } else if (myDir == BACKWARD && obs[sMax].speed > 0) {
2100 1419331 : utility[sMax] += ONCOMING_CONFLICT_PENALTY;
2101 : }
2102 : // penalize lateral movement (if the current stripe permits walking)
2103 49720913 : if (distance[current] > 0 && myWaitingTime == 0) {
2104 106898128 : for (int i = 0; i < stripes; ++i) {
2105 84323459 : utility[i] += abs(i - current) * LATERAL_PENALTY;
2106 : }
2107 : }
2108 : // walk on the right side on shared space
2109 49720913 : if (myLane->getPermissions() != SVC_PEDESTRIAN && myDir == BACKWARD) {
2110 47361568 : for (int i = 0; i < stripes; ++i) {
2111 35739465 : if (i <= current) {
2112 30035129 : utility[i] += (sMax - i + 1) * LATERAL_PENALTY;
2113 : }
2114 : }
2115 : }
2116 :
2117 : // select best stripe
2118 : int chosen = current;
2119 224334165 : for (int i = 0; i < stripes; ++i) {
2120 174613252 : if (utility[i] > utility[chosen] && utility[i] >= INAPPROPRIATE_PENALTY * 0.5) {
2121 : chosen = i;
2122 : }
2123 : }
2124 : // compute speed components along both axes
2125 49720913 : const int next = (chosen == current ? current : (chosen < current ? current - 1 : current + 1));
2126 49720913 : double xDist = MIN3(distance[current], distance[other], distance[next]);
2127 49720913 : if (next != chosen) {
2128 : // ensure that we do not collide with an obstacle in the stripe beyond
2129 : // next as this might become the 'other' stripe in the next step
2130 1208310 : const int nextOther = chosen < current ? current - 2 : current + 2;
2131 1208310 : xDist = MIN2(xDist, distance[nextOther]);
2132 : }
2133 : // XXX preferred gap differs between approaching a standing obstacle or a moving obstacle
2134 : const double preferredGap = NUMERICAL_EPS;
2135 49720913 : double xSpeed = MIN2(vMax, MAX2(0., DIST2SPEED(xDist - preferredGap)));
2136 49720913 : if (xSpeed < NUMERICAL_EPS) {
2137 : xSpeed = 0.;
2138 : }
2139 49720913 : if (DEBUGCOND(*this)) {
2140 0 : std::cout << " xSpeedPotential=" << xSpeed << "\n";
2141 : }
2142 : // avoid tiny steps
2143 : // XXX pressure from behind?
2144 49720913 : if (mySpeed == 0 && xDist < MIN_STARTUP_DIST &&
2145 : // unless walking towards a short lane
2146 22990822 : !(
2147 23177225 : (xDist == distance[current] && obs[current].type >= OBSTACLE_END)
2148 22990830 : || (xDist == distance[other] && obs[other].type >= OBSTACLE_END)
2149 22990822 : || (xDist == distance[next] && obs[next].type >= OBSTACLE_END))
2150 : ) {
2151 : xSpeed = 0;
2152 : }
2153 49720913 : if (xSpeed == 0) {
2154 26262912 : if (DEBUGCOND(*this)) {
2155 0 : std::cout << " sharedWA=" << (myWalkingAreaFoes.find(&myLane->getEdge()) != myWalkingAreaFoes.end())
2156 0 : << " vehObs=" << Named::getIDSecure(obs[current].vehicle)
2157 0 : << " vehWait=" << STEPS2TIME(obs[current].vehicle ? obs[current].vehicle->getWaitingTime() : 0)
2158 0 : << "\n";
2159 : }
2160 26262912 : if (myWaitingTime > ((myLane->getEdge().isCrossing()
2161 : // treat shared walkingarea like a crossing to avoid deadlocking vehicles
2162 26256791 : || (myLane->getEdge().isWalkingArea() && obs[current].vehicle != nullptr && obs[current].vehicle->getWaitingTime() > jamTimeCrossing
2163 52520033 : && myWalkingAreaFoes.find(&myLane->getEdge()) != myWalkingAreaFoes.end())) ? jamTimeCrossing : jamTime)
2164 26199528 : || (sMax == 0 && obs[0].speed * myDir < 0 && myWaitingTime > jamTimeNarrow)
2165 52447164 : || myAmJammed) {
2166 : // squeeze slowly through the crowd ignoring others
2167 3841177 : if (!myAmJammed) {
2168 63115 : MSNet::getInstance()->getPersonControl().registerJammed();
2169 252460 : WRITE_WARNINGF(TL("Person '%' is jammed on edge '%', time=%."),
2170 : myPerson->getID(), myStage->getEdge()->getID(), time2string(SIMSTEP));
2171 63115 : myAmJammed = true;
2172 : }
2173 3841177 : xSpeed = vMax / 4;
2174 : }
2175 23458001 : } else if (myAmJammed && stripe(myRelY) >= 0 && stripe(myRelY) <= sMax && xDist >= MIN_STARTUP_DIST) {
2176 64631 : myAmJammed = false;
2177 : }
2178 : // dawdling
2179 49720913 : const double dawdle = MIN2(xSpeed, RandHelper::rand() * vMax * dawdling);
2180 49720913 : xSpeed -= dawdle;
2181 :
2182 : // XXX ensure that diagonal speed <= vMax
2183 : // avoid deadlocks on narrow sidewalks
2184 : //if (oncoming && xSpeed == 0 && myStage->getWaitingTime(currentTime) > TIME2STEPS(ONCOMIN_PATIENCE)) {
2185 : // if DEBUGCOND(*this) std::cout << " stepping asside to resolve oncoming deadlock\n";
2186 : // xSpeed = POSITION_EPS; // reset myWaitingTime
2187 : // if (myDir == FORWARD && chosen < sMax) {
2188 : // chosen += 1;
2189 : // } else if (myDir == BACKWARD && chosen > 0) {
2190 : // chosen -= 1;
2191 : // }
2192 : //}
2193 49720913 : const double maxYSpeed = MIN2(MAX2(vMax * LATERAL_SPEED_FACTOR, vMax - xSpeed), stripeWidth);
2194 : double ySpeed = 0;
2195 : double yDist = 0;
2196 49720913 : if (utility[next] > OBSTRUCTION_THRESHOLD && utility[chosen] > OBSTRUCTION_THRESHOLD) {
2197 : // don't move laterally if the stripes are blocked
2198 46418767 : yDist = (chosen * stripeWidth) - myRelY;
2199 46418767 : if (fabs(yDist) > NUMERICAL_EPS) {
2200 6038209 : ySpeed = (yDist > 0 ?
2201 2966792 : MIN2(maxYSpeed, DIST2SPEED(yDist)) :
2202 3071417 : MAX2(-maxYSpeed, DIST2SPEED(yDist)));
2203 : }
2204 3302146 : } else if (utility[next] <= OBSTRUCTION_THRESHOLD && obs[next].type == OBSTACLE_VEHICLE
2205 : // still on the road
2206 9944 : && stripe() == stripe(myRelY)
2207 : // only when the vehicle is moving on the same lane
2208 3307803 : && !(myLane->getEdge().isCrossing() || myLane->getEdge().isWalkingArea())) {
2209 : // step aside to let the vehicle pass
2210 3435 : int stepAsideDir = myDir;
2211 3435 : if (myLane->getEdge().getLanes().size() > 1 || current > sMax / 2) {
2212 : // always step to the right on multi-lane edges or when closer to
2213 : // the right side
2214 : stepAsideDir = FORWARD;
2215 : }
2216 3435 : myAmJammed = true; // ignore pedestrian-pedestrian collisions
2217 3435 : ySpeed = stepAsideDir * vMax;
2218 : }
2219 :
2220 : // DEBUG
2221 49720913 : if DEBUGCOND(*this) {
2222 0 : std::cout << SIMTIME
2223 0 : << " ped=" << myPerson->getID()
2224 0 : << " edge=" << myStage->getEdge()->getID()
2225 0 : << " x=" << myRelX
2226 0 : << " y=" << myRelY
2227 : << " d=" << myDir
2228 0 : << " pvx=" << mySpeed
2229 : << " cur=" << current
2230 0 : << " cho=" << chosen
2231 0 : << " oth=" << other
2232 0 : << " nxt=" << next
2233 0 : << " vx=" << xSpeed
2234 : << " dawdle=" << dawdle
2235 : << " vy=" << ySpeed
2236 : << " xd=" << xDist
2237 : << " yd=" << yDist
2238 : << " vMax=" << vMax
2239 0 : << " wTime=" << myStage->getWaitingTime(currentTime)
2240 0 : << " jammed=" << myAmJammed
2241 0 : << "\n";
2242 0 : if (DEBUGCOND(*this)) {
2243 0 : for (int i = 0; i < stripes; ++i) {
2244 0 : const Obstacle& o = obs[i];
2245 0 : std::cout << " util=" << utility[i] << " dist=" << distance[i] << " o=" << o.description;
2246 0 : if (o.description != "") {
2247 0 : std::cout << " xF=" << o.xFwd << " xB=" << o.xBack << " v=" << o.speed;
2248 : }
2249 0 : if (i == current) {
2250 0 : std::cout << " current";
2251 : }
2252 0 : if (i == other && i != current) {
2253 0 : std::cout << " other";
2254 : }
2255 0 : if (i == chosen) {
2256 0 : std::cout << " chosen";
2257 : }
2258 0 : if (i == next) {
2259 0 : std::cout << " next";
2260 : }
2261 0 : std::cout << "\n";
2262 : }
2263 : }
2264 : }
2265 49720913 : myRelX += SPEED2DIST(xSpeed * myDir);
2266 49720913 : myRelY += SPEED2DIST(ySpeed);
2267 49720913 : mySpeedLat = ySpeed;
2268 49720913 : mySpeed = xSpeed;
2269 49720913 : if (xSpeed >= SUMO_const_haltingSpeed) {
2270 25406344 : myWaitingToEnter = false;
2271 25406344 : myWaitingTime = 0;
2272 : } else {
2273 24314569 : myWaitingTime += DELTA_T;
2274 : }
2275 49720913 : myAngle = std::numeric_limits<double>::max(); // set on first access or via remote control
2276 49720913 : }
2277 :
2278 :
2279 : double
2280 9678663 : MSPModel_Striping::PState::getImpatience(SUMOTime now) const {
2281 9678663 : return MAX2(0., MIN2(1., myPerson->getVehicleType().getImpatience()
2282 9678663 : + STEPS2TIME(myStage->getWaitingTime(now)) / MAX_WAIT_TOLERANCE));
2283 : }
2284 :
2285 :
2286 : double
2287 11307954 : MSPModel_Striping::PState::getEdgePos(const MSStageMoving&, SUMOTime) const {
2288 11307954 : return myRelX;
2289 : }
2290 :
2291 :
2292 : int
2293 359922 : MSPModel_Striping::PState::getDirection(const MSStageMoving&, SUMOTime) const {
2294 359922 : return myDir;
2295 : }
2296 :
2297 :
2298 : Position
2299 6734005 : MSPModel_Striping::PState::getPosition(const MSStageMoving& stage, SUMOTime) const {
2300 : if (myRemoteXYPos != Position::INVALID) {
2301 445 : return myRemoteXYPos;
2302 : }
2303 6733560 : if (myLane == nullptr) {
2304 : // pedestrian has already finished
2305 244 : return Position::INVALID;
2306 : }
2307 6733316 : const double lateral_offset = myRelY + (stripeWidth - myLane->getWidth()) * 0.5;
2308 6733316 : if (myWalkingAreaPath == nullptr) {
2309 827013 : return stage.getLanePosition(myLane, myRelX, lateral_offset);
2310 : } else {
2311 : //if DEBUGCOND(*this) {
2312 : // std::cout << SIMTIME
2313 : // << " getPosition (walkingArea)"
2314 : // << " p=" << myPerson->getID()
2315 : // << " x=" << myRelX
2316 : // << " y=" << myRelY
2317 : // << " latOffset=" << lateral_offset
2318 : // << " shape=" << myWalkingAreaPath->shape
2319 : // << " pos=" << myWalkingAreaPath->shape.positionAtOffset(myRelX, lateral_offset)
2320 : // << "\n";
2321 : //}
2322 5906303 : if (myWalkingAreaPath->angleOverride == INVALID_DOUBLE) {
2323 5787373 : return myWalkingAreaPath->shape.positionAtOffset(myRelX, lateral_offset);
2324 : } else {
2325 118930 : const double rotationOffset = myDir == FORWARD ? 0 : DEG2RAD(180);
2326 118930 : return myWalkingAreaPath->shape.sidePositionAtAngle(myRelX, lateral_offset, myWalkingAreaPath->angleOverride + rotationOffset);
2327 : }
2328 : }
2329 : }
2330 :
2331 :
2332 : double
2333 2406728 : MSPModel_Striping::PState::getAngle(const MSStageMoving&, SUMOTime) const {
2334 2406728 : if (myAngle != std::numeric_limits<double>::max()) {
2335 : return myAngle;
2336 : }
2337 555873 : if (myLane == nullptr) {
2338 : // pedestrian has already finished
2339 : return 0;
2340 : }
2341 555629 : if (myWalkingAreaPath != nullptr && myWalkingAreaPath->angleOverride != INVALID_DOUBLE) {
2342 : return myWalkingAreaPath->angleOverride;
2343 : }
2344 528331 : const PositionVector& shp = myWalkingAreaPath == nullptr ? myLane->getShape() : myWalkingAreaPath->shape;
2345 528331 : double geomX = myWalkingAreaPath == nullptr ? myLane->interpolateLanePosToGeometryPos(myRelX) : myRelX;
2346 528331 : double angle = shp.rotationAtOffset(geomX) + (myDir == MSPModel::BACKWARD ? M_PI : 0);
2347 528331 : if (myDir == MSPModel::BACKWARD) {
2348 146950 : angle += atan2(mySpeedLat, MAX2(mySpeed, NUMERICAL_EPS));
2349 : } else { // myDir == MSPModel::FORWARD
2350 497286 : angle -= atan2(mySpeedLat, MAX2(mySpeed, NUMERICAL_EPS));
2351 : }
2352 528331 : if (angle > M_PI) {
2353 96573 : angle -= 2 * M_PI;
2354 : }
2355 528331 : myAngle = angle;
2356 528331 : return angle;
2357 : }
2358 :
2359 :
2360 : SUMOTime
2361 9679859 : MSPModel_Striping::PState::getWaitingTime(const MSStageMoving&, SUMOTime) const {
2362 9679859 : return myWaitingTime;
2363 : }
2364 :
2365 :
2366 : double
2367 548660 : MSPModel_Striping::PState::getSpeed(const MSStageMoving&) const {
2368 548660 : return mySpeed;
2369 : }
2370 :
2371 :
2372 : const MSEdge*
2373 164052 : MSPModel_Striping::PState::getNextEdge(const MSStageMoving&) const {
2374 164052 : return myNLI.lane == nullptr ? nullptr : &myNLI.lane->getEdge();
2375 : }
2376 :
2377 :
2378 : void
2379 21 : MSPModel_Striping::PState::moveTo(MSPerson* p, MSLane* lane, double lanePos, double lanePosLat, SUMOTime t) {
2380 : ConstMSEdgeVector newEdges; // keep route
2381 : int routeOffset = 0;
2382 : bool laneOnRoute = false;
2383 21 : const MSJunction* laneOnJunction = lane->isNormal() ? nullptr : lane->getEdge().getToJunction();
2384 21 : for (const MSEdge* edge : myStage->getRoute()) {
2385 : if (edge == &lane->getEdge()
2386 16 : || edge->getToJunction() == laneOnJunction
2387 21 : || edge->getFromJunction() == laneOnJunction) {
2388 : laneOnRoute = true;
2389 : break;
2390 : }
2391 0 : routeOffset++;
2392 : }
2393 21 : if (!laneOnRoute) {
2394 0 : throw ProcessError("Lane '" + lane->getID() + "' is not on the route of person '" + getID() + "'.");
2395 : }
2396 21 : Position pos = lane->geometryPositionAtOffset(lanePos, lanePosLat);
2397 21 : if (lane->getEdge().isWalkingArea() && (myWalkingAreaPath == nullptr || myWalkingAreaPath->lane != lane)) {
2398 : // entered new walkingarea. Determine path to guess position
2399 10 : const MSEdge* prevEdge = myStage->getRoute()[routeOffset];
2400 10 : const MSEdge* nextEdge = routeOffset + 1 < (int)myStage->getRoute().size() ? myStage->getRoute()[routeOffset + 1] : nullptr;
2401 10 : const WalkingAreaPath* guessed = guessPath(&lane->getEdge(), prevEdge, nextEdge);
2402 10 : const double maxPos = guessed->shape.length() - NUMERICAL_EPS;
2403 10 : if (lanePos > maxPos + POSITION_EPS || lanePos < -POSITION_EPS) {
2404 0 : throw ProcessError("Lane position " + toString(lanePos) + " cannot be mapped onto walkingarea '" + lane->getID()
2405 0 : + "' (fromLane='" + guessed->from->getID()
2406 0 : + "' toLane='" + guessed->to->getID() + "') for person '" + getID() + "' time=" + time2string(t) + ".");
2407 : }
2408 : // give some slack
2409 10 : lanePos = MIN2(maxPos, MAX2(NUMERICAL_EPS, lanePos));
2410 10 : pos = guessed->shape.positionAtOffset(lanePos, lanePosLat);
2411 : }
2412 21 : const double angle = GeomHelper::naviDegree(p->getPosition().angleTo2D(pos));
2413 21 : moveToXY(p, pos, lane, lanePos, lanePosLat, angle, routeOffset, newEdges, t);
2414 21 : }
2415 :
2416 :
2417 : void
2418 16360 : MSPModel_Striping::PState::moveToXY(MSPerson* p, Position pos, MSLane* lane, double lanePos,
2419 : double lanePosLat, double angle, int routeOffset,
2420 : const ConstMSEdgeVector& edges, SUMOTime t) {
2421 16360 : MSPModel_Striping* pm = dynamic_cast<MSPModel_Striping*>(MSNet::getInstance()->getPersonControl().getMovementModel());
2422 : assert(p == myPerson);
2423 : assert(pm != nullptr);
2424 16360 : const double oldAngle = GeomHelper::naviDegree(getAngle(*myStage, t));
2425 : // person already walking in this step. undo this to obtain the previous position
2426 16360 : const double oldX = myRelX - SPEED2DIST(mySpeed * myDir);
2427 : const double tmp = myRelX;
2428 16360 : myRelX = oldX;
2429 16360 : Position oldPos = getPosition(*myStage, t);
2430 16360 : myRelX = tmp;
2431 : //if (oldPos == Position::INVALID) {
2432 : // oldPos = pos
2433 : //}
2434 16360 : myAngle = GeomHelper::fromNaviDegree(angle);
2435 : #ifdef DEBUG_MOVETOXY
2436 : std::cout << SIMTIME << " ped=" << p->getID()
2437 : << " moveToXY"
2438 : << " pos=" << pos
2439 : << " lane=" << lane->getID()
2440 : << " lanePos=" << lanePos
2441 : << " lanePosLat=" << lanePosLat
2442 : << " angle=" << angle
2443 : << " routeOffset=" << routeOffset
2444 : << " edges=" << toString(edges)
2445 : << " oldLane=" << Named::getIDSecure(myLane)
2446 : << " path=" << (myWalkingAreaPath == nullptr ? "null" : (myWalkingAreaPath->from->getID() + "->" + myWalkingAreaPath->to->getID())) << "\n";
2447 : #endif
2448 :
2449 16360 : if (lane != myLane && myLane != nullptr) {
2450 1126 : pm->remove(this);
2451 : pm->registerActive();
2452 : }
2453 16360 : if (lane != nullptr &&
2454 16360 : fabs(lanePosLat) < (0.5 * (lane->getWidth() + p->getVehicleType().getWidth()) + SIDEWALK_OFFSET)) {
2455 16354 : myRemoteXYPos = Position::INVALID;
2456 16354 : const MSEdge* old = myStage->getEdge();
2457 16354 : const MSLane* oldLane = myLane;
2458 16354 : if (lane != myLane) {
2459 : // implicitly adds new active lane if necessary
2460 1126 : pm->myActiveLanes[lane].push_back(this);
2461 : }
2462 16354 : if (edges.empty()) {
2463 : // map within route
2464 16270 : myStage->setRouteIndex(myPerson, routeOffset);
2465 : } else {
2466 84 : myStage->replaceRoute(myPerson, edges, routeOffset);
2467 : }
2468 16354 : if (!lane->getEdge().isNormal()) {
2469 718 : myStage->moveToNextEdge(myPerson, t, myDir, &lane->getEdge());
2470 : }
2471 :
2472 16354 : myLane = lane;
2473 16354 : const double lateral_offset = (lane->getWidth() - stripeWidth) * 0.5;
2474 16354 : if (lane->getEdge().isWalkingArea()) {
2475 350 : if (myWalkingAreaPath == nullptr || myWalkingAreaPath->lane != lane) {
2476 : // entered new walkingarea. Determine path
2477 62 : myWalkingAreaPath = guessPath(&lane->getEdge(), old, myStage->getNextRouteEdge());
2478 : #ifdef DEBUG_MOVETOXY
2479 : std::cout << " guessPath old=" << old->getID() << " next=" << Named::getIDSecure(myStage->getNextRouteEdge())
2480 : << " path=" << myWalkingAreaPath->from->getID() << "->" << myWalkingAreaPath->to->getID() << "\n";
2481 : #endif
2482 : }
2483 : // lanePos and lanePosLat are matched onto the circumference of the
2484 : // walkingarea. Use pos instead
2485 350 : const Position relPos = myWalkingAreaPath->shape.transformToVectorCoordinates(pos);
2486 : if (relPos == Position::INVALID) {
2487 104 : WRITE_WARNING("Could not map position " + toString(pos) + " onto lane '" + myLane->getID()
2488 : + "' (fromLane='" + myWalkingAreaPath->from->getID()
2489 : + "' toLane='" + myWalkingAreaPath->to->getID() + "') for person '" + getID() + "' time=" + time2string(t) + ".");
2490 52 : myRemoteXYPos = pos;
2491 : } else {
2492 298 : myRelX = relPos.x();
2493 298 : myRelY = lateral_offset + relPos.y();
2494 : }
2495 : } else {
2496 16004 : myWalkingAreaPath = nullptr;
2497 16004 : myRelX = lanePos;
2498 16004 : myRelY = lateral_offset - lanePosLat;
2499 : }
2500 : // guess direction
2501 16354 : const double angleDiff = GeomHelper::getMinAngleDiff(angle, oldAngle);
2502 16354 : if (myStage->getNextRouteEdge() != nullptr) {
2503 20809 : if (myStage->getEdge()->getToJunction() == myStage->getNextRouteEdge()->getFromJunction() ||
2504 6868 : myStage->getEdge()->getToJunction() == myStage->getNextRouteEdge()->getToJunction()) {
2505 7203 : myDir = FORWARD;
2506 : } else {
2507 6738 : myDir = BACKWARD;
2508 : }
2509 : } else {
2510 : // guess from angle
2511 2413 : if (angleDiff <= 90) {
2512 : // keep direction
2513 2349 : if (myDir == UNDEFINED_DIRECTION) {
2514 0 : myDir = FORWARD;
2515 : }
2516 : } else {
2517 : // change direction
2518 64 : myDir = (myDir == BACKWARD) ? FORWARD : BACKWARD;
2519 : }
2520 : }
2521 : // update next lane info (after guessing direction)
2522 16354 : if (oldLane == nullptr || &oldLane->getEdge() != &myLane->getEdge()) {
2523 1074 : const MSLane* sidewalk = getSidewalk<MSEdge, MSLane>(&myLane->getEdge(), p->getVClass());
2524 : // assume that we will eventually move back onto the sidewalk if
2525 : // there is one
2526 1074 : myNLI = getNextLane(*this, sidewalk == nullptr ? myLane : sidewalk, nullptr);
2527 1074 : myStage->activateEntryReminders(myPerson);
2528 : #ifdef DEBUG_MOVETOXY
2529 : std::cout << " myNLI=" << Named::getIDSecure(myNLI.lane) << " link=" << (myNLI.link == nullptr ? "NULL" : myNLI.link->getDescription()) << " dir=" << myNLI.dir << "\n";
2530 : #endif
2531 : }
2532 : #ifdef DEBUG_MOVETOXY
2533 : std::cout << " newRelPos=" << Position(myRelX, myRelY) << " edge=" << myPerson->getEdge()->getID() << " newPos=" << myPerson->getPosition()
2534 : << " oldAngle=" << oldAngle << " angleDiff=" << angleDiff << " newDir=" << myDir << "\n";
2535 : #endif
2536 16354 : if (oldLane == myLane) {
2537 15228 : mySpeed = DIST2SPEED(fabs(oldX - myRelX));
2538 : } else {
2539 : //std::cout << SIMTIME << " oldX=" << oldX << " oldSpeed=" << mySpeed << " oldPos=" << oldPos << " pos=" << pos << " dist=" << oldPos.distanceTo2D(pos) << " oldLane=" << Named::getIDSecure(oldLane) << " lane=" << lane->getID() << "\n";
2540 1126 : mySpeed = DIST2SPEED(oldPos.distanceTo2D(pos));
2541 : }
2542 : } else {
2543 : // map outside the network
2544 6 : myRemoteXYPos = pos;
2545 6 : mySpeed = DIST2SPEED(oldPos.distanceTo2D(pos));
2546 : }
2547 :
2548 16360 : }
2549 :
2550 :
2551 : bool
2552 1110602 : MSPModel_Striping::PState::isJammed() const {
2553 1110602 : return myAmJammed;
2554 : }
2555 :
2556 : const MSLane*
2557 852419 : MSPModel_Striping::PState::getLane() const {
2558 852419 : return myLane;
2559 : }
2560 :
2561 : double
2562 365577 : MSPModel_Striping::PState::getPathLength() const {
2563 365577 : if (myWalkingAreaPath != nullptr) {
2564 207522 : return myWalkingAreaPath->length;
2565 : } else {
2566 : return 0;
2567 : }
2568 : }
2569 :
2570 : double
2571 1086887096 : MSPModel_Striping::PState::distanceTo(const Obstacle& obs, const bool includeMinGap) const {
2572 : // check for overlap
2573 1086887096 : const double maxX = getMaxX(includeMinGap);
2574 1086887096 : const double minX = getMinX(includeMinGap);
2575 : //if (DEBUGCOND(*this)) {
2576 : // std::cout << std::setprecision(2) << " distanceTo=" << obs.description << " maxX=" << maxX << " minX=" << minX << " obs.xFwd=" << obs.xFwd << " obs.xBack=" << obs.xBack << "\n";
2577 : //}
2578 1086887096 : if ((obs.xFwd >= maxX && obs.xBack <= maxX) || (obs.xFwd <= maxX && obs.xFwd >= minX)) {
2579 : // avoid blocking by itself on looped route
2580 199016739 : return (obs.type == OBSTACLE_PED && obs.description == myPerson->getID()) ? DIST_FAR_AWAY : DIST_OVERLAP;
2581 : }
2582 887870357 : if (myDir == FORWARD) {
2583 600897788 : return obs.xFwd < minX ? DIST_BEHIND : obs.xBack - maxX;
2584 : } else {
2585 286972569 : return obs.xBack > maxX ? DIST_BEHIND : minX - obs.xFwd;
2586 : }
2587 : }
2588 :
2589 :
2590 : void
2591 80461596 : MSPModel_Striping::PState::mergeObstacles(Obstacles& into, const Obstacles& obs2) {
2592 363130700 : for (int i = 0; i < (int)into.size(); ++i) {
2593 282669104 : if (gDebugFlag1) {
2594 0 : std::cout << " i=" << i << " maxX=" << getMaxX(true) << " minX=" << getMinX(true)
2595 0 : << " into=" << into[i].description << " iDist=" << distanceTo(into[i], into[i].type == OBSTACLE_PED)
2596 0 : << " obs2=" << obs2[i].description << " oDist=" << distanceTo(obs2[i], obs2[i].type == OBSTACLE_PED) << "\n";
2597 : }
2598 282669104 : const double dO = distanceTo(obs2[i], obs2[i].type == OBSTACLE_PED);
2599 282669104 : const double dI = distanceTo(into[i], into[i].type == OBSTACLE_PED);
2600 282669104 : if (dO < dI) {
2601 : into[i] = obs2[i];
2602 : } else if (dO == dI
2603 39882640 : && into[i].type != OBSTACLE_PED
2604 31001122 : && into[i].type != OBSTACLE_VEHICLE
2605 253130246 : && (obs2[i].type == OBSTACLE_PED ||
2606 : obs2[i].type == OBSTACLE_VEHICLE)) {
2607 : into[i] = obs2[i];
2608 : }
2609 : }
2610 80461596 : }
2611 :
2612 : void
2613 19193 : MSPModel_Striping::PState::mergeObstacles(Obstacles& into, const Obstacles& obs2, int dir, int offset) {
2614 148734 : for (int i = 0; i < (int)into.size(); ++i) {
2615 129541 : int i2 = i + offset;
2616 129541 : if (i2 >= 0 && i2 < (int)obs2.size()) {
2617 128301 : if (dir == FORWARD) {
2618 92778 : if (obs2[i2].xBack < into[i].xBack) {
2619 : into[i] = obs2[i2];
2620 : }
2621 : } else {
2622 35523 : if (obs2[i2].xFwd > into[i].xFwd) {
2623 : into[i] = obs2[i2];
2624 : }
2625 : }
2626 : }
2627 : }
2628 19193 : }
2629 :
2630 :
2631 : bool
2632 9678663 : MSPModel_Striping::PState::ignoreRed(const MSLink* link) const {
2633 9678663 : if (link->haveRed()) {
2634 410441 : const double ignoreRedTime = myPerson->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_AFTER_RED_TIME, -1);
2635 410441 : if (ignoreRedTime >= 0) {
2636 40 : const double redDuration = STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - link->getLastStateChange());
2637 40 : if (DEBUGCOND(*this)) {
2638 0 : std::cout << SIMTIME << " ignoreRedTime=" << ignoreRedTime << " redDuration=" << redDuration << "\n";
2639 : }
2640 40 : return ignoreRedTime > redDuration;
2641 : } else {
2642 : return false;
2643 : }
2644 : } else {
2645 : return false;
2646 : }
2647 : }
2648 :
2649 : const std::string&
2650 2721765209 : MSPModel_Striping::PState::getID() const {
2651 2721765209 : return myPerson->getID();
2652 : }
2653 :
2654 : double
2655 1353841339 : MSPModel_Striping::PState::getWidth() const {
2656 1353841339 : return myPerson->getVehicleType().getWidth();
2657 : }
2658 :
2659 :
2660 : bool
2661 99084326 : MSPModel_Striping::PState::isRemoteControlled() const {
2662 99084326 : return myPerson->hasInfluencer() && myPerson->getInfluencer().isRemoteControlled();
2663 : }
2664 :
2665 : // ===========================================================================
2666 : // MSPModel_Striping::PStateVehicle method definitions
2667 : // ===========================================================================
2668 :
2669 550197 : MSPModel_Striping::PStateVehicle::PStateVehicle(const MSVehicle* veh, const MSLane* walkingarea, double relX, double relY, double xWidth, double yWidth):
2670 550197 : myVehicle(veh), myXWidth(xWidth), myYWidth(yWidth) {
2671 550197 : myLane = walkingarea; // to ensure correct limits when calling otherStripe()
2672 : // relX is the center but we want it to be the max value if the movement direction is forward
2673 : // and the min value otherwise (indicated by xWidth sign)
2674 550197 : myRelX = relX + xWidth / 2;
2675 550197 : myRelY = relY;
2676 550197 : }
2677 :
2678 : const std::string&
2679 2152823 : MSPModel_Striping::PStateVehicle::getID() const {
2680 2152823 : return myVehicle->getID();
2681 : }
2682 :
2683 : double
2684 1105414 : MSPModel_Striping::PStateVehicle::getWidth() const {
2685 1105414 : return myYWidth;
2686 : }
2687 :
2688 : double
2689 585143 : MSPModel_Striping::PStateVehicle::getMinX(const bool /*includeMinGap*/) const {
2690 585143 : return myXWidth > 0 ? myRelX - myXWidth : myRelX;
2691 : }
2692 :
2693 : double
2694 585143 : MSPModel_Striping::PStateVehicle::getMaxX(const bool /*includeMinGap*/) const {
2695 585143 : return myXWidth > 0 ? myRelX : myRelX - myXWidth;
2696 : }
2697 :
2698 : // ===========================================================================
2699 : // MSPModel_Striping::MovePedestrians method definitions
2700 : // ===========================================================================
2701 :
2702 : SUMOTime
2703 1852396 : MSPModel_Striping::MovePedestrians::execute(SUMOTime currentTime) {
2704 : std::set<MSPerson*> changedLane;
2705 1852396 : myModel->moveInDirection(currentTime, changedLane, FORWARD);
2706 1852392 : myModel->moveInDirection(currentTime, changedLane, BACKWARD);
2707 : // DEBUG
2708 : #ifdef LOG_ALL
2709 : for (ActiveLanes::const_iterator it_lane = myModel->getActiveLanes().begin(); it_lane != myModel->getActiveLanes().end(); ++it_lane) {
2710 : const MSLane* lane = it_lane->first;
2711 : Pedestrians pedestrians = it_lane->second;
2712 : if (pedestrians.size() == 0) {
2713 : continue;
2714 : }
2715 : sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(FORWARD));
2716 : std::cout << SIMTIME << " lane=" << lane->getID();
2717 : for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
2718 : const PState& p = *pedestrians[ii];
2719 : std::cout << " (" << p.myPerson->getID() << " " << p.myRelX << "," << p.myRelY << " " << p.myDir << ")";
2720 : }
2721 : std::cout << "\n";
2722 : }
2723 : #endif
2724 1852392 : return DELTA_T;
2725 : }
|
