Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2014-2025 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file MSPModel_Striping.cpp
15 : /// @author Jakob Erdmann
16 : /// @author Michael Behrisch
17 : /// @date Mon, 13 Jan 2014
18 : ///
19 : // The pedestrian following model (prototype)
20 : /****************************************************************************/
21 : #include <config.h>
22 :
23 : #include <cmath>
24 : #include <algorithm>
25 : #include <utils/common/RandHelper.h>
26 : #include <utils/geom/GeomHelper.h>
27 : #include <utils/options/OptionsCont.h>
28 : #include <utils/router/PedestrianRouter.h>
29 : #include <microsim/MSNet.h>
30 : #include <microsim/MSEdge.h>
31 : #include <microsim/MSEventControl.h>
32 : #include <microsim/MSLane.h>
33 : #include <microsim/MSLink.h>
34 : #include <microsim/MSJunction.h>
35 : #include <microsim/MSStoppingPlace.h>
36 : #include <microsim/MSVehicleControl.h>
37 : #include <microsim/MSGlobals.h>
38 : #include <microsim/transportables/MSStage.h>
39 : #include <microsim/transportables/MSTransportableControl.h>
40 : #include "MSPModel_Striping.h"
41 :
42 :
43 : // ===========================================================================
44 : // DEBUGGING HELPERS
45 : // ===========================================================================
46 : //
47 : #define DEBUGID1 ""
48 : #define DEBUGID2 ""
49 : //#define DEBUGCOND(PED) (false)
50 : //#define DEBUGCOND(PED) ((PED).getPerson()->getID() == DEBUGID1 || (PED).getPerson()->getID() == DEBUGID2)
51 : #define DEBUGCOND(PED) ((PED).getPerson()->isSelected())
52 : #define DEBUGCOND2(LANE) ((LANE)->isSelected())
53 : //#define LOG_ALL 1
54 : //#define DEBUG_MOVETOXY
55 :
56 0 : void MSPModel_Striping::DEBUG_PRINT(const Obstacles& obs) {
57 0 : for (int i = 0; i < (int)obs.size(); ++i) {
58 : std::cout
59 0 : << "(" << obs[i].description
60 0 : << " x=(" << obs[i].xBack << "," << obs[i].xFwd
61 0 : << ") s=" << obs[i].speed
62 0 : << ") ";
63 : }
64 0 : std::cout << "\n";
65 0 : }
66 :
67 : // ===========================================================================
68 : // named (internal) constants
69 : // ===========================================================================
70 :
71 : // distances are comparable with lower values being "more important"
72 : const double MSPModel_Striping::DIST_FAR_AWAY(10000);
73 : const double MSPModel_Striping::DIST_BEHIND(1000);
74 : const double MSPModel_Striping::DIST_OVERLAP(-1);
75 :
76 : // ===========================================================================
77 : // static members
78 : // ===========================================================================
79 :
80 : MSPModel_Striping::WalkingAreaPaths MSPModel_Striping::myWalkingAreaPaths;
81 : std::map<const MSEdge*, std::vector<const MSLane*> > MSPModel_Striping::myWalkingAreaFoes;
82 : MSPModel_Striping::MinNextLengths MSPModel_Striping::myMinNextLengths;
83 :
84 : // model parameters (static to simplify access from class PState
85 : double MSPModel_Striping::stripeWidth;
86 : double MSPModel_Striping::dawdling;
87 : double MSPModel_Striping::minGapToVehicle;
88 : int MSPModel_Striping::myWalkingAreaDetail;
89 : SUMOTime MSPModel_Striping::jamTime;
90 : SUMOTime MSPModel_Striping::jamTimeCrossing;
91 : SUMOTime MSPModel_Striping::jamTimeNarrow;
92 : double MSPModel_Striping::jamFactor;
93 : bool MSPModel_Striping::myLegacyPosLat;
94 : const double MSPModel_Striping::LOOKAHEAD_SAMEDIR(4.0); // seconds
95 : const double MSPModel_Striping::LOOKAHEAD_ONCOMING(10.0); // seconds
96 : const double MSPModel_Striping::LOOKAHEAD_ONCOMING_DIST(10.0); // m
97 : const double MSPModel_Striping::LOOKAROUND_VEHICLES(60.0); // meters
98 : const double MSPModel_Striping::LATERAL_PENALTY(-1.); // meters
99 : const double MSPModel_Striping::OBSTRUCTED_PENALTY(-300000.); // meters
100 : const double MSPModel_Striping::INAPPROPRIATE_PENALTY(-20000.); // meters
101 : const double MSPModel_Striping::ONCOMING_CONFLICT_PENALTY(-1000.); // meters
102 : const double MSPModel_Striping::OBSTRUCTION_THRESHOLD(MSPModel_Striping::OBSTRUCTED_PENALTY * 0.5); // despite obstruction, additional utility may have been added
103 : const double MSPModel_Striping::SQUEEZE(0.7);
104 : double MSPModel_Striping::RESERVE_FOR_ONCOMING_FACTOR;
105 : double MSPModel_Striping::RESERVE_FOR_ONCOMING_FACTOR_JUNCTIONS;
106 : double MSPModel_Striping::RESERVE_FOR_ONCOMING_MAX;
107 : bool MSPModel_Striping::USE_NET_SPEEDS(false);
108 : const double MSPModel_Striping::MAX_WAIT_TOLERANCE(120.); // seconds
109 : const double MSPModel_Striping::LATERAL_SPEED_FACTOR(0.4);
110 : const double MSPModel_Striping::MIN_STARTUP_DIST(0.4); // meters
111 :
112 :
113 : // ===========================================================================
114 : // MSPModel_Striping method definitions
115 : // ===========================================================================
116 :
117 5408 : MSPModel_Striping::MSPModel_Striping(const OptionsCont& oc, MSNet* net) {
118 5408 : myWalkingAreaDetail = oc.getInt("pedestrian.striping.walkingarea-detail");
119 5408 : initWalkingAreaPaths(net);
120 : // configurable parameters
121 5408 : stripeWidth = oc.getFloat("pedestrian.striping.stripe-width");
122 5408 : MSVehicleType* defaultPedType = MSNet::getInstance()->getVehicleControl().getVType(DEFAULT_PEDTYPE_ID, nullptr, true);
123 5408 : 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 5408 : dawdling = oc.getFloat("pedestrian.striping.dawdling");
129 5408 : minGapToVehicle = oc.getFloat("pedestrian.striping.mingap-to-vehicle");
130 5408 : RESERVE_FOR_ONCOMING_FACTOR = oc.getFloat("pedestrian.striping.reserve-oncoming");
131 5408 : RESERVE_FOR_ONCOMING_FACTOR_JUNCTIONS = oc.getFloat("pedestrian.striping.reserve-oncoming.junctions");
132 5408 : RESERVE_FOR_ONCOMING_MAX = oc.getFloat("pedestrian.striping.reserve-oncoming.max");
133 : // beginning with 1.20.0, sensible default speeds were set for crossings and walkingareas
134 5408 : USE_NET_SPEEDS = net->getNetworkVersion() >= MMVersion(1, 20);
135 :
136 5408 : jamTime = string2time(oc.getString("pedestrian.striping.jamtime"));
137 5408 : if (jamTime <= 0) {
138 4 : jamTime = SUMOTime_MAX;
139 : }
140 5408 : jamTimeCrossing = string2time(oc.getString("pedestrian.striping.jamtime.crossing"));
141 5408 : if (jamTimeCrossing <= 0) {
142 0 : jamTimeCrossing = SUMOTime_MAX;
143 : }
144 5408 : jamTimeNarrow = string2time(oc.getString("pedestrian.striping.jamtime.narrow"));
145 5408 : if (jamTimeNarrow <= 0) {
146 0 : jamTimeNarrow = SUMOTime_MAX;
147 : }
148 5408 : jamFactor = oc.getFloat("pedestrian.striping.jamfactor");
149 5408 : myLegacyPosLat = oc.getBool("pedestrian.striping.legacy-departposlat");
150 5408 : }
151 :
152 :
153 10798 : MSPModel_Striping::~MSPModel_Striping() {
154 : myWalkingAreaPaths.clear(); // need to recompute when lane pointers change
155 : myWalkingAreaFoes.clear();
156 : myMinNextLengths.clear();
157 10798 : }
158 :
159 :
160 : MSTransportableStateAdapter*
161 243024 : MSPModel_Striping::add(MSTransportable* transportable, MSStageMoving* stage, SUMOTime) {
162 243024 : if (!transportable->isPerson()) {
163 : // containers are not supported (TODO add a warning here?)
164 : return nullptr;
165 : }
166 : MSPerson* person = static_cast<MSPerson*>(transportable);
167 243024 : MSNet* net = MSNet::getInstance();
168 243024 : if (!myAmActive) {
169 4006 : net->getBeginOfTimestepEvents()->addEvent(new MovePedestrians(this), net->getCurrentTimeStep() + DELTA_T);
170 4006 : myAmActive = true;
171 : }
172 : assert(person->getCurrentStageType() == MSStageType::WALKING);
173 243024 : const MSLane* lane = stage->checkDepartLane(person->getEdge(), person->getVClass(), stage->getDepartLane(), person->getID());
174 243024 : if (lane == nullptr) {
175 8 : const char* error = TL("Person '%' could not find sidewalk on edge '%', time=%.");
176 16 : if (OptionsCont::getOptions().getBool("ignore-route-errors")) {
177 24 : WRITE_WARNINGF(error, person->getID(), person->getEdge()->getID(), time2string(net->getCurrentTimeStep()));
178 8 : return nullptr;
179 : } else {
180 0 : throw ProcessError(TLF(error, person->getID(), person->getEdge()->getID(), time2string(net->getCurrentTimeStep())));
181 : }
182 : }
183 243016 : PState* ped = new PState(person, stage, lane);
184 243012 : myActiveLanes[lane].push_back(ped);
185 243012 : myNumActivePedestrians++;
186 243012 : return ped;
187 : }
188 :
189 :
190 : MSTransportableStateAdapter*
191 14 : MSPModel_Striping::loadState(MSTransportable* transportable, MSStageMoving* stage, std::istringstream& in) {
192 : MSPerson* person = static_cast<MSPerson*>(transportable);
193 14 : MSNet* net = MSNet::getInstance();
194 14 : if (!myAmActive) {
195 12 : net->getBeginOfTimestepEvents()->addEvent(new MovePedestrians(this), SIMSTEP);
196 12 : myAmActive = true;
197 : }
198 14 : PState* ped = new PState(person, stage, &in);
199 14 : myActiveLanes[ped->getLane()].push_back(ped);
200 14 : myNumActivePedestrians++;
201 14 : return ped;
202 : }
203 :
204 :
205 : int
206 7419370550 : MSPModel_Striping::numStripes(const MSLane* lane) {
207 7419370550 : return MAX2(1, (int)floor(lane->getWidth() / stripeWidth));
208 : }
209 :
210 :
211 : int
212 878631 : MSPModel_Striping::connectedDirection(const MSLane* from, const MSLane* to) {
213 878631 : if (from == nullptr || to == nullptr) {
214 0 : return UNDEFINED_DIRECTION;
215 878631 : } else if (from->getLinkTo(to) != nullptr) {
216 636607 : return FORWARD;
217 242024 : } else if (to->getLinkTo(from) != nullptr) {
218 242024 : return BACKWARD;
219 : } else {
220 0 : return UNDEFINED_DIRECTION;
221 : }
222 : }
223 :
224 :
225 : void
226 5408 : MSPModel_Striping::initWalkingAreaPaths(const MSNet*) {
227 5408 : if (myWalkingAreaPaths.size() > 0) {
228 : return;
229 : }
230 : // collect vehicle lanes that cross walkingareas
231 422222 : for (MSEdgeVector::const_iterator i = MSEdge::getAllEdges().begin(); i != MSEdge::getAllEdges().end(); ++i) {
232 416814 : const MSEdge* edge = *i;
233 416814 : if (!edge->isWalkingArea() && !edge->isCrossing()) {
234 746062 : for (MSLane* lane : edge->getLanes()) {
235 871923 : for (MSLink* link : lane->getLinkCont()) {
236 482700 : if (link->getWalkingAreaFoe() != nullptr) {
237 : // link is an exit link
238 2942 : myWalkingAreaFoes[&link->getWalkingAreaFoe()->getEdge()].push_back(link->getLaneBefore());
239 : //std::cout << " wa=" << link->getWalkingAreaFoe()->getEdge().getID() << " foe=" << link->getLaneBefore()->getID() << "\n";
240 : }
241 482700 : if (link->getWalkingAreaFoeExit() != nullptr) {
242 : // link is an exit link
243 3028 : myWalkingAreaFoes[&link->getWalkingAreaFoeExit()->getEdge()].push_back(link->getLaneBefore());
244 : //std::cout << " wa=" << link->getWalkingAreaFoeExit()->getEdge().getID() << " foe=" << link->getLaneBefore()->getID() << "\n";
245 : }
246 : }
247 : }
248 : }
249 : }
250 :
251 : // build walkingareaPaths
252 422222 : for (MSEdgeVector::const_iterator i = MSEdge::getAllEdges().begin(); i != MSEdge::getAllEdges().end(); ++i) {
253 416814 : insertWalkArePaths(*i, myWalkingAreaPaths);
254 : }
255 : }
256 :
257 :
258 : void
259 416814 : MSPModel_Striping::insertWalkArePaths(const MSEdge* edge, WalkingAreaPaths& into) {
260 416814 : if (edge->isWalkingArea()) {
261 52284 : const MSLane* walkingArea = getSidewalk<MSEdge, MSLane>(edge);
262 52284 : myMinNextLengths[walkingArea] = walkingArea->getLength();
263 : // build all possible paths across this walkingArea
264 : // gather all incident lanes
265 : std::vector<const MSLane*> lanes;
266 135428 : for (const MSEdge* in : edge->getPredecessors()) {
267 83144 : if (!in->isTazConnector()) {
268 60714 : lanes.push_back(getSidewalk<MSEdge, MSLane>(in));
269 60714 : if (lanes.back() == nullptr) {
270 0 : throw ProcessError("Invalid connection from edge '" + in->getID() + "' to walkingarea edge '" + edge->getID() + "'");
271 : }
272 : }
273 : }
274 135613 : for (const MSEdge* out : edge->getSuccessors()) {
275 83329 : if (!out->isTazConnector()) {
276 60899 : lanes.push_back(getSidewalk<MSEdge, MSLane>(out));
277 60899 : if (lanes.back() == nullptr) {
278 0 : throw ProcessError("Invalid connection from walkingarea edge '" + edge->getID() + "' to edge '" + out->getID() + "'");
279 : }
280 : }
281 : }
282 : // build all combinations
283 173897 : for (int j = 0; j < (int)lanes.size(); ++j) {
284 429920 : for (int k = 0; k < (int)lanes.size(); ++k) {
285 308307 : if (j != k) {
286 : // build the walkingArea
287 186694 : const MSLane* const from = lanes[j];
288 186694 : const MSLane* const to = lanes[k];
289 186694 : const int fromDir = from->getLinkTo(walkingArea) != nullptr ? FORWARD : BACKWARD;
290 186694 : const int toDir = walkingArea->getLinkTo(to) != nullptr ? FORWARD : BACKWARD;
291 186694 : PositionVector shape;
292 280300 : Position fromPos = from->getShape()[fromDir == FORWARD ? -1 : 0];
293 279782 : Position toPos = to->getShape()[toDir == FORWARD ? 0 : -1];
294 186694 : const double maxExtent = fromPos.distanceTo2D(toPos) / 4; // prevent sharp corners
295 186694 : const double extrapolateBy = MIN2(maxExtent, walkingArea->getWidth() / 2);
296 : // assemble shape
297 186694 : shape.push_back(fromPos);
298 186694 : if (extrapolateBy > POSITION_EPS) {
299 : PositionVector fromShp = from->getShape();
300 185684 : fromShp.extrapolate(extrapolateBy);
301 371368 : shape.push_back_noDoublePos(fromDir == FORWARD ? fromShp.back() : fromShp.front());
302 : PositionVector nextShp = to->getShape();
303 185684 : nextShp.extrapolate(extrapolateBy);
304 371368 : shape.push_back_noDoublePos(toDir == FORWARD ? nextShp.front() : nextShp.back());
305 185684 : }
306 186694 : shape.push_back_noDoublePos(toPos);
307 186694 : if (shape.size() < 2) {
308 : PositionVector fromShp = from->getShape();
309 554 : fromShp.extrapolate(1.5 * POSITION_EPS); // noDoublePos requires a difference of POSITION_EPS in at least one coordinate
310 1108 : shape.push_back_noDoublePos(fromDir == FORWARD ? fromShp.back() : fromShp.front());
311 : assert(shape.size() == 2);
312 186694 : } else if (myWalkingAreaDetail > 4) {
313 448 : shape = shape.bezier(myWalkingAreaDetail);
314 : }
315 : double angleOverride = INVALID_DOUBLE;
316 186694 : if (shape.size() >= 4 && shape.length() < walkingArea->getWidth()) {
317 46848 : const double aStart = shape.angleAt2D(0);
318 46848 : const double aEnd = shape.angleAt2D((int)shape.size() - 2);
319 46848 : if (fabs(aStart - aEnd) < DEG2RAD(10)) {
320 7402 : angleOverride = (aStart + aEnd) / 2;
321 : }
322 : }
323 186694 : if (fromDir == BACKWARD) {
324 : // will be walking backward on walkingArea
325 187212 : shape = shape.reverse();
326 : }
327 186694 : WalkingAreaPath wap = WalkingAreaPath(from, walkingArea, to, shape, fromDir, angleOverride);
328 186694 : into.insert(std::make_pair(std::make_pair(from, to), wap));
329 216733 : myMinNextLengths[walkingArea] = MIN2(myMinNextLengths[walkingArea], wap.length);
330 186694 : }
331 : }
332 : }
333 52284 : }
334 416814 : }
335 :
336 :
337 : const MSPModel_Striping::WalkingAreaPath*
338 8 : MSPModel_Striping::getArbitraryPath(const MSEdge* walkingArea) {
339 : assert(walkingArea->isWalkingArea());
340 : std::vector<const MSLane*> lanes;
341 24 : for (const MSEdge* const pred : walkingArea->getPredecessors()) {
342 16 : lanes.push_back(getSidewalk<MSEdge, MSLane>(pred));
343 : }
344 24 : for (const MSEdge* const succ : walkingArea->getSuccessors()) {
345 16 : lanes.push_back(getSidewalk<MSEdge, MSLane>(succ));
346 : }
347 8 : if (lanes.size() < 1) {
348 0 : throw ProcessError(TLF("Invalid walkingarea '%' does not allow continuation.", walkingArea->getID()));
349 : }
350 8 : return &myWalkingAreaPaths.find(std::make_pair(lanes.front(), lanes.back()))->second;
351 8 : }
352 :
353 : const MSPModel_Striping::WalkingAreaPath*
354 65 : MSPModel_Striping::guessPath(const MSEdge* walkingArea, const MSEdge* before, const MSEdge* after) {
355 : assert(walkingArea->isWalkingArea());
356 65 : const MSLane* swBefore = getSidewalk<MSEdge, MSLane>(before);
357 65 : const MSLane* swAfter = getSidewalk<MSEdge, MSLane>(after);
358 65 : const auto pathIt = myWalkingAreaPaths.find(std::make_pair(swBefore, swAfter));
359 65 : if (pathIt != myWalkingAreaPaths.end()) {
360 25 : return &pathIt->second;
361 : }
362 : const MSEdgeVector& preds = walkingArea->getPredecessors();
363 40 : const MSEdgeVector& succs = walkingArea->getSuccessors();
364 40 : bool useBefore = swBefore != nullptr && std::find(preds.begin(), preds.end(), before) != preds.end();
365 40 : bool useAfter = swAfter != nullptr && std::find(succs.begin(), succs.end(), after) != succs.end();
366 40 : if (useBefore) {
367 30 : if (useAfter) {
368 0 : return getWalkingAreaPath(walkingArea, swBefore, swAfter);
369 30 : } else if (succs.size() > 0) {
370 : // could also try to exploit direction
371 30 : return getWalkingAreaPath(walkingArea, swBefore, getSidewalk<MSEdge, MSLane>(succs.front()));
372 : }
373 10 : } else if (useAfter && preds.size() > 0) {
374 : // could also try to exploit direction
375 10 : return getWalkingAreaPath(walkingArea, getSidewalk<MSEdge, MSLane>(preds.front()), swAfter);
376 : }
377 0 : return getArbitraryPath(walkingArea);
378 : }
379 :
380 :
381 : const MSPModel_Striping::WalkingAreaPath*
382 914821 : MSPModel_Striping::getWalkingAreaPath(const MSEdge* walkingArea, const MSLane* before, const MSLane* after) {
383 : assert(walkingArea->isWalkingArea());
384 914821 : const auto pathIt = myWalkingAreaPaths.find(std::make_pair(before, after));
385 914821 : if (pathIt != myWalkingAreaPaths.end()) {
386 914778 : return &pathIt->second;
387 : } else {
388 : // this can happen in case of moveToXY where before can point anywhere
389 : const MSEdgeVector& preds = walkingArea->getPredecessors();
390 43 : if (preds.size() > 0) {
391 43 : const MSEdge* const pred = walkingArea->getPredecessors().front();
392 43 : const auto pathIt2 = myWalkingAreaPaths.find(std::make_pair(getSidewalk<MSEdge, MSLane>(pred), after));
393 : assert(pathIt2 != myWalkingAreaPaths.end());
394 43 : return &pathIt2->second;
395 : } else {
396 0 : return getArbitraryPath(walkingArea);
397 : }
398 : }
399 : }
400 :
401 :
402 :
403 : MSPModel_Striping::NextLaneInfo
404 1520172 : MSPModel_Striping::getNextLane(const PState& ped, const MSLane* currentLane, const MSLane* prevLane) {
405 : const MSEdge* currentEdge = ¤tLane->getEdge();
406 1520172 : const MSJunction* junction = ped.getDirection() == FORWARD ? currentEdge->getToJunction() : currentEdge->getFromJunction();
407 1520172 : const MSEdge* nextRouteEdge = ped.getStage()->getNextRouteEdge();
408 1520172 : const MSLane* nextRouteLane = getSidewalk<MSEdge, MSLane>(nextRouteEdge, ped.getPerson()->getVClass());
409 : // result values
410 : const MSLane* nextLane = nextRouteLane;
411 1520172 : const MSLink* link = nullptr;
412 1520172 : int nextDir = UNDEFINED_DIRECTION;
413 :
414 : //if DEBUGCOND(ped) {
415 : // std::cout << " nextRouteLane=" << Named::getIDSecure(nextRouteLane) << " junction=" << junction->getID() << "\n";
416 : //}
417 1520172 : if (nextRouteLane == nullptr && nextRouteEdge != nullptr) {
418 56 : std::string error = "Person '" + ped.getPerson()->getID() + "' could not find sidewalk on edge '" + nextRouteEdge->getID() + "', time="
419 56 : + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".";
420 56 : if (OptionsCont::getOptions().getBool("ignore-route-errors")) {
421 76 : WRITE_WARNING(error);
422 24 : nextRouteLane = nextRouteEdge->getLanes().front();
423 : } else {
424 4 : throw ProcessError(error);
425 : }
426 : }
427 :
428 1520168 : if (nextRouteLane != nullptr) {
429 1297592 : if (currentEdge->isInternal()) {
430 : assert(junction == currentEdge->getFromJunction());
431 21831 : nextDir = junction == nextRouteEdge->getFromJunction() ? FORWARD : BACKWARD;
432 21831 : if (nextDir == FORWARD) {
433 20853 : nextLane = currentLane->getLinkCont()[0]->getViaLaneOrLane();
434 : } else {
435 978 : nextLane = currentLane->getLogicalPredecessorLane();
436 : }
437 21831 : if DEBUGCOND(ped) {
438 0 : std::cout << " internal\n";
439 : }
440 1275761 : } else if (currentEdge->isCrossing()) {
441 36088 : nextDir = ped.getDirection();
442 36088 : if (nextDir == FORWARD) {
443 19889 : nextLane = currentLane->getLinkCont()[0]->getLane();
444 : } else {
445 16199 : nextLane = currentLane->getLogicalPredecessorLane();
446 : }
447 36088 : if DEBUGCOND(ped) {
448 0 : std::cout << " crossing\n";
449 : }
450 36088 : unregisterCrossingApproach(ped, currentLane);
451 1239673 : } else if (currentEdge->isWalkingArea()) {
452 : ConstMSEdgeVector crossingRoute;
453 : // departPos can be 0 because the direction of the walkingArea does not matter
454 : // for the arrivalPos, we need to make sure that the route does not deviate across other junctions
455 878668 : const int nextRouteEdgeDir = nextRouteEdge->getFromJunction() == junction ? FORWARD : BACKWARD;
456 878668 : const double arrivalPos = (nextRouteEdge == ped.getStage()->getRoute().back()
457 878668 : ? ped.getStage()->getArrivalPos()
458 199508 : : (nextRouteEdgeDir == FORWARD ? 0 : nextRouteEdge->getLength()));
459 : std::map<const MSEdge*, double> prohibited;
460 878668 : if (prevLane != nullptr) {
461 878605 : prohibited[&prevLane->getEdge()] = -1;
462 : }
463 1757336 : MSNet::getInstance()->getPedestrianRouter(0, prohibited).compute(currentEdge, nextRouteEdge, 0, arrivalPos,
464 878668 : ped.getStage()->getMaxSpeed(ped.getPerson()),
465 878668 : 0, junction, ped.getPerson()->getVTypeParameter(), crossingRoute, true);
466 878668 : if DEBUGCOND(ped) {
467 : std::cout
468 : << " nre=" << nextRouteEdge->getID()
469 : << " nreDir=" << nextRouteEdgeDir
470 0 : << " aPos=" << arrivalPos
471 0 : << " crossingRoute=" << toString(crossingRoute)
472 0 : << "\n";
473 : }
474 878668 : if (crossingRoute.size() > 1) {
475 878631 : const MSEdge* nextEdge = crossingRoute[1];
476 878631 : nextLane = getSidewalk<MSEdge, MSLane>(crossingRoute[1], ped.getPerson()->getVClass());
477 : assert((nextEdge->getFromJunction() == junction || nextEdge->getToJunction() == junction));
478 : assert(nextLane != prevLane);
479 878631 : nextDir = connectedDirection(currentLane, nextLane);
480 878631 : if DEBUGCOND(ped) {
481 0 : std::cout << " nextDir=" << nextDir << "\n";
482 : }
483 : assert(nextDir != UNDEFINED_DIRECTION);
484 878631 : if (nextDir == FORWARD) {
485 636607 : link = currentLane->getLinkTo(nextLane);
486 : } else {
487 242024 : link = nextLane->getLinkTo(currentLane);
488 242024 : if (nextEdge->isCrossing() && link->getTLLogic() == nullptr) {
489 190667 : const MSLane* oppositeWalkingArea = nextLane->getLogicalPredecessorLane();
490 190667 : link = oppositeWalkingArea->getLinkTo(nextLane);
491 : }
492 : }
493 : assert(link != nullptr);
494 878631 : if (nextLane->isCrossing()) {
495 575759 : registerCrossingApproach(ped, nextLane, prevLane);
496 : }
497 : } else {
498 37 : if DEBUGCOND(ped) {
499 0 : std::cout << SIMTIME
500 0 : << " no route from '" << (currentEdge == nullptr ? "NULL" : currentEdge->getID())
501 0 : << "' to '" << (nextRouteEdge == nullptr ? "NULL" : nextRouteEdge->getID())
502 0 : << "\n";
503 : }
504 : // check if a direct connection exists (moving onto the walkingarea was the wrong choice)
505 37 : if (ped.getDirection() == FORWARD) {
506 24 : link = prevLane->getLinkTo(nextRouteLane);
507 : } else {
508 13 : link = nextRouteLane->getLinkTo(prevLane);
509 : }
510 37 : if (link != nullptr) {
511 : // leave direction as UNDEFINED_DIRECTION to signal that currentLane must be changed
512 : nextLane = link->getViaLaneOrLane();
513 : } else {
514 80 : WRITE_WARNING("Person '" + ped.getPerson()->getID() + "' could not find route across junction '" + junction->getID()
515 : + "' from walkingArea '" + currentEdge->getID()
516 : + "' to edge '" + nextRouteEdge->getID() + "', time=" +
517 : time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
518 : // error indicated by nextDir == UNDEFINED_DIRECTION
519 : nextLane = nextRouteLane;
520 : }
521 : }
522 1239673 : } else if (currentEdge == nextRouteEdge) {
523 : // strange loop in this route. No need to use walkingArea
524 8 : nextDir = -ped.getDirection();
525 : } else {
526 : // normal edge. by default use next / previous walking area
527 360997 : nextDir = ped.getDirection();
528 360997 : nextLane = getNextWalkingArea(currentLane, ped.getDirection(), link);
529 360997 : if (nextLane != nullptr) {
530 : // walking area found
531 322827 : if DEBUGCOND(ped) {
532 0 : std::cout << " next walkingArea " << (nextDir == FORWARD ? "forward" : "backward") << "\n";
533 : }
534 : } else {
535 : // walk forward by default
536 38170 : if (junction == nextRouteEdge->getToJunction()) {
537 4033 : nextDir = BACKWARD;
538 34137 : } else if (junction == nextRouteEdge->getFromJunction()) {
539 : nextDir = FORWARD;
540 : } else {
541 : // topological disconnect, find a direction that makes sense
542 : // for the future part of the route
543 212 : ConstMSEdgeVector futureRoute = ped.getStage()->getRoute();
544 212 : futureRoute.erase(futureRoute.begin(), futureRoute.begin() + ped.getStage()->getRoutePosition() + 1);
545 212 : int passedFwd = 0;
546 212 : int passedBwd = 0;
547 212 : canTraverse(FORWARD, futureRoute, passedFwd);
548 212 : canTraverse(BACKWARD, futureRoute, passedBwd);
549 212 : nextDir = (passedFwd >= passedBwd) ? FORWARD : BACKWARD;
550 212 : if DEBUGCOND(ped) {
551 0 : std::cout << " nextEdge=" << nextRouteEdge->getID() << " passedFwd=" << passedFwd << " passedBwd=" << passedBwd << " futureRoute=" << toString(futureRoute) << " nextDir=" << nextDir << "\n";
552 : }
553 212 : }
554 : // try to use a direct link as fallback
555 : // direct links only exist if built explicitly. They are used to model tl-controlled links if there are no crossings
556 38170 : if (ped.getDirection() == FORWARD) {
557 33612 : link = currentLane->getLinkTo(nextRouteLane);
558 33612 : if (link != nullptr) {
559 31937 : if DEBUGCOND(ped) {
560 0 : std::cout << " direct forward\n";
561 : }
562 31937 : nextLane = currentLane->getInternalFollowingLane(nextRouteLane);
563 : }
564 : } else {
565 4558 : link = nextRouteLane->getLinkTo(currentLane);
566 4558 : if (link != nullptr) {
567 3267 : if DEBUGCOND(ped) {
568 0 : std::cout << " direct backward\n";
569 : }
570 3267 : nextLane = nextRouteLane->getInternalFollowingLane(currentLane);
571 3267 : if (nextLane != nullptr) {
572 : // advance to the end of consecutive internal lanes
573 5485 : while (nextLane->getLinkCont()[0]->getViaLaneOrLane()->isInternal()) {
574 5 : nextLane = nextLane->getLinkCont()[0]->getViaLaneOrLane();
575 : }
576 : }
577 : }
578 : }
579 : }
580 360470 : if (nextLane == nullptr) {
581 : // no internal lane found
582 : nextLane = nextRouteLane;
583 8804 : if DEBUGCOND(ped) {
584 0 : std::cout << SIMTIME << " no next lane found for " << currentLane->getID() << " dir=" << ped.getDirection() << "\n";
585 : }
586 8804 : if (usingInternalLanesStatic() && currentLane->getLinkCont().size() > 0) {
587 0 : WRITE_WARNING("Person '" + ped.getPerson()->getID() + "' could not find route across junction '" + junction->getID()
588 : + "' from edge '" + currentEdge->getID()
589 : + "' to edge '" + nextRouteEdge->getID() + "', time=" +
590 : time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
591 : }
592 352193 : } else if (nextLane->getLength() <= POSITION_EPS) {
593 : // internal lane too short
594 : // most often this is due to a zero-size junction. However, if
595 : // the person needs to pass a crossing we cannot skip ahead
596 11073 : if ((nextLane->getCanonicalSuccessorLane() == nullptr
597 11065 : || !nextLane->getCanonicalSuccessorLane()->getEdge().isCrossing())
598 33199 : && (nextLane->getLogicalPredecessorLane() == nullptr ||
599 11061 : !nextLane->getLogicalPredecessorLane()->getEdge().isCrossing())) {
600 : //WRITE_WARNING("Person '" + ped.getID()
601 : // + "' skips short lane '" + nextLane->getID()
602 : // + "' length=" + toString(nextLane->getLength())
603 : // + " time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
604 : nextLane = nextRouteLane;
605 11073 : nextDir = nextRouteEdge->getFromJunction() == junction ? FORWARD : BACKWARD;
606 : }
607 : }
608 : }
609 : }
610 1520168 : if DEBUGCOND(ped) {
611 0 : std::cout << SIMTIME
612 : << " p=" << ped.getPerson()->getID()
613 : << " l=" << currentLane->getID()
614 0 : << " nl=" << (nextLane == nullptr ? "NULL" : nextLane->getID())
615 0 : << " nrl=" << (nextRouteLane == nullptr ? "NULL" : nextRouteLane->getID())
616 : << " d=" << nextDir
617 0 : << " link=" << (link == nullptr ? "NULL" : link->getViaLaneOrLane()->getID())
618 0 : << " pedDir=" << ped.getDirection()
619 0 : << "\n";
620 : }
621 : assert(nextLane != 0 || nextRouteLane == 0);
622 1520168 : return NextLaneInfo(nextLane, link, nextDir);
623 : }
624 :
625 :
626 : const MSLane*
627 360997 : MSPModel_Striping::getNextWalkingArea(const MSLane* currentLane, const int dir, const MSLink*& link) {
628 360997 : if (dir == FORWARD) {
629 360555 : for (const MSLink* const l : currentLane->getLinkCont()) {
630 326943 : if (l->getLane()->getEdge().isWalkingArea()) {
631 267063 : link = l;
632 : return l->getLane();
633 : }
634 : }
635 : } else {
636 : const std::vector<MSLane::IncomingLaneInfo>& laneInfos = currentLane->getIncomingLanes();
637 67028 : for (std::vector<MSLane::IncomingLaneInfo>::const_iterator it = laneInfos.begin(); it != laneInfos.end(); ++it) {
638 62470 : if ((*it).lane->getEdge().isWalkingArea()) {
639 55764 : link = (*it).viaLink;
640 : return (*it).lane;
641 : }
642 : }
643 : }
644 : return nullptr;
645 : }
646 :
647 :
648 : MSPModel_Striping::Obstacles
649 65696538 : MSPModel_Striping::getNeighboringObstacles(const Pedestrians& pedestrians, int egoIndex, int stripes) {
650 65696538 : const PState& ego = *static_cast<PState*>(pedestrians[egoIndex]);
651 65696538 : const int egoStripe = ego.stripe();
652 65696538 : Obstacles obs(stripes, Obstacle(ego.getDirection()));
653 65696538 : std::vector<bool> haveBlocker(stripes, false);
654 9968183143 : for (int index = egoIndex + 1; index < (int)pedestrians.size(); index++) {
655 9954619793 : const PState& p = *static_cast<PState*>(pedestrians[index]);
656 9954619793 : if DEBUGCOND(ego) {
657 0 : std::cout << SIMTIME << " ped=" << ego.getID() << " cur=" << egoStripe << " checking neighbor " << p.getID()
658 0 : << " nCur=" << p.stripe() << " nOth=" << p.otherStripe();
659 : }
660 9954619793 : if (!p.isWaitingToEnter() && !p.isJammed()) {
661 132532950 : const Obstacle o(p);
662 132532950 : if DEBUGCOND(ego) {
663 0 : std::cout << " dist=" << ego.distanceTo(o) << std::endl;
664 : }
665 132532950 : if (ego.distanceTo(o) == DIST_BEHIND) {
666 : break;
667 : }
668 80399762 : if (ego.distanceTo(o) == DIST_OVERLAP) {
669 80399762 : if (p.stripe() != egoStripe || p.getDirection() != ego.getDirection()) {
670 59814418 : obs[p.stripe()] = o;
671 59814418 : haveBlocker[p.stripe()] = true;
672 : } else {
673 : //std::cout << SIMTIME << " ignoring overlap between " << ego.getID() << " and " << p.getID() << " on stripe=" << egoStripe << "\n";
674 : }
675 80399762 : if (p.otherStripe() != egoStripe || p.getDirection() != ego.getDirection()) {
676 59814418 : obs[p.otherStripe()] = o;
677 59814418 : haveBlocker[p.otherStripe()] = true;
678 : } else {
679 : //std::cout << SIMTIME << " ignoring overlap between " << ego.getID() << " and " << p.getID() << " on stripe2=" << egoStripe << "\n";
680 : }
681 : } else {
682 0 : if (!haveBlocker[p.stripe()]) {
683 0 : obs[p.stripe()] = o;
684 : }
685 0 : if (!haveBlocker[p.otherStripe()]) {
686 0 : obs[p.otherStripe()] = o;
687 : }
688 : }
689 : }
690 : }
691 65696538 : if DEBUGCOND(ego) {
692 0 : std::cout << SIMTIME << " ped=" << ego.getPerson()->getID() << " neighObs=";
693 0 : DEBUG_PRINT(obs);
694 : }
695 65696538 : return obs;
696 0 : }
697 :
698 :
699 : int
700 3148224 : MSPModel_Striping::getStripeOffset(int origStripes, int destStripes, bool addRemainder) {
701 3148224 : int offset = (destStripes - origStripes) / 2;
702 3148224 : if (addRemainder) {
703 13195 : offset += (destStripes - origStripes) % 2;
704 : }
705 3148224 : return offset;
706 : }
707 :
708 :
709 : const MSPModel_Striping::Obstacles&
710 16717124 : MSPModel_Striping::getNextLaneObstacles(NextLanesObstacles& nextLanesObs, const
711 : MSLane* lane, const MSLane* nextLane, int stripes, int nextDir,
712 : double currentLength, int currentDir) {
713 : if (nextLanesObs.count(nextLane) == 0) {
714 2410964 : const double nextLength = nextLane->getEdge().isWalkingArea() ? myMinNextLengths[nextLane] : nextLane->getLength();
715 : // figure out the which pedestrians are ahead on the next lane
716 2410964 : const int nextStripes = numStripes(nextLane);
717 : // do not move past the end of the next lane in a single step
718 4821928 : Obstacles obs(stripes, Obstacle(nextDir == FORWARD ? nextLength : 0, 0, OBSTACLE_NEXTEND, "nextEnd", 0));
719 :
720 2410964 : const int offset = getStripeOffset(nextStripes, stripes, currentDir != nextDir && nextStripes > stripes);
721 : //std::cout << SIMTIME << " getNextLaneObstacles"
722 : // << " nextLane=" << nextLane->getID()
723 : // << " nextLength=" << nextLength
724 : // << " nextDir=" << nextDir
725 : // << " currentLength=" << currentLength
726 : // << " currentDir=" << currentDir
727 : // << " stripes=" << stripes
728 : // << " nextStripes=" << nextStripes
729 : // << " offset=" << offset
730 : // << "\n";
731 2410964 : if (nextStripes < stripes) {
732 : // some stripes do not continue
733 2097010 : for (int ii = 0; ii < stripes; ++ii) {
734 1809632 : if (ii < offset || ii >= nextStripes + offset) {
735 2490414 : obs[ii] = Obstacle(nextDir == FORWARD ? 0 : nextLength, 0, OBSTACLE_END, "stripeEnd", 0);
736 : }
737 : }
738 : }
739 2410964 : Pedestrians& pedestrians = getPedestrians(nextLane);
740 2410964 : if (nextLane->getEdge().isWalkingArea()) {
741 1233526 : transformToCurrentLanePositions(obs, currentDir, nextDir, currentLength, nextLength);
742 : // complex transformation into the coordinate system of the current lane
743 : // (pedestrians on next lane may walk at arbitrary angles relative to the current lane)
744 1233526 : double lateral_offset = (lane->getWidth() - stripeWidth) * 0.5;
745 1233526 : if ((stripes - nextStripes) % 2 != 0) {
746 480409 : lateral_offset += 0.5 * stripeWidth;
747 : }
748 : nextDir = currentDir;
749 : // transform pedestrians into the current coordinate system
750 8220585 : for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
751 6987059 : const PState& p = *static_cast<PState*>(pedestrians[ii]);
752 6987059 : if (p.isWaitingToEnter() || p.isJammed()) {
753 2149763 : continue;
754 : }
755 4837296 : Position pPos = p.getPosition(*p.getStage(), -1);
756 4837296 : Position relPos = lane->getShape().transformToVectorCoordinates(pPos, true);
757 : if (relPos == Position::INVALID) {
758 0 : WRITE_WARNINGF("Could not map position % onto lane '%'", pPos, lane->getID());
759 : }
760 4837296 : const double newY = relPos.y() + lateral_offset;
761 : //if (p.getPerson()->getID() == "ped200") std::cout << " ped=" << p.getPerson()->getID() << " relX=" << relPos.x() << " relY=" << newY << " latOff=" << lateral_offset << " s=" << p.stripe(newY) << " os=" << p.otherStripe(newY) << "\n";
762 4837296 : if ((currentDir == FORWARD && relPos.x() >= lane->getLength()) || (currentDir == BACKWARD && relPos.x() < 0)) {
763 4835921 : addCloserObstacle(obs, relPos.x(), p.stripe(newY), stripes, p.getPerson()->getID(), p.getPerson()->getVehicleType().getWidth(), currentDir, OBSTACLE_PED);
764 4835921 : addCloserObstacle(obs, relPos.x(), p.otherStripe(newY), stripes, p.getPerson()->getID(), p.getPerson()->getVehicleType().getWidth(), currentDir, OBSTACLE_PED);
765 : }
766 : }
767 : } else {
768 : // simple transformation into the coordinate system of the current lane
769 : // (only need to worry about currentDir and nextDir)
770 : // XXX consider waitingToEnter on nextLane
771 1177438 : sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(nextDir));
772 33082270 : for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
773 31904832 : const PState& p = *static_cast<PState*>(pedestrians[ii]);
774 31904832 : if (p.isWaitingToEnter() || p.isJammed()) {
775 21018778 : continue;
776 : }
777 : double newY = p.getPosLat();
778 10886054 : Obstacle pObs(p);
779 10886054 : if (nextDir != currentDir) {
780 289123 : newY = (nextStripes - 1) * stripeWidth - newY;
781 289123 : pObs.speed *= -1;
782 : }
783 10886054 : newY += offset * stripeWidth;
784 10886054 : const int stripe = p.stripe(newY);
785 10886054 : if (stripe >= 0 && stripe < stripes) {
786 10886051 : obs[stripe] = pObs;
787 : }
788 10886054 : const int otherStripe = p.otherStripe(newY);
789 10886054 : if (otherStripe >= 0 && otherStripe < stripes) {
790 10886051 : obs[otherStripe] = pObs;
791 : }
792 : }
793 1177438 : if (nextLane->getEdge().isCrossing()) {
794 : // add vehicle obstacles
795 373390 : const MSLink* crossingEntryLink = nextLane->getIncomingLanes().front().viaLink;
796 373390 : const bool prio = crossingEntryLink->havePriority() || crossingEntryLink->getTLLogic() != nullptr;
797 373390 : addCrossingVehs(nextLane, stripes, offset, nextDir, obs, prio);
798 : }
799 1177438 : if (nextLane->getVehicleNumberWithPartials() > 0) {
800 18622 : Obstacles vehObs = getVehicleObstacles(nextLane, nextDir);
801 18622 : PState::mergeObstacles(obs, vehObs, nextDir, offset);
802 18622 : }
803 1177438 : transformToCurrentLanePositions(obs, currentDir, nextDir, currentLength, nextLength);
804 : }
805 2410964 : nextLanesObs[nextLane] = obs;
806 2410964 : }
807 16717124 : return nextLanesObs[nextLane];
808 : }
809 :
810 : void
811 2410964 : MSPModel_Striping::transformToCurrentLanePositions(Obstacles& obs, int currentDir, int nextDir, double currentLength, double nextLength) {
812 13746856 : for (Obstacle& o : obs) {
813 11335892 : if (currentDir == FORWARD) {
814 7673353 : if (nextDir == FORWARD) {
815 7329241 : o.xFwd += currentLength;
816 7329241 : o.xBack += currentLength;
817 : } else {
818 344112 : const double tmp = o.xFwd;
819 344112 : o.xFwd = currentLength + nextLength - o.xBack;
820 344112 : o.xBack = currentLength + nextLength - tmp;
821 : }
822 : } else {
823 3662539 : if (nextDir == FORWARD) {
824 845734 : const double tmp = o.xFwd;
825 845734 : o.xFwd = -o.xBack;
826 845734 : o.xBack = -tmp;
827 : } else {
828 2816805 : o.xFwd -= nextLength;
829 2816805 : o.xBack -= nextLength;
830 : }
831 : }
832 : }
833 2410964 : }
834 :
835 :
836 : void
837 9671842 : MSPModel_Striping::addCloserObstacle(Obstacles& obs, double x, int stripe, int numStripes, const std::string& id, double width, int dir, ObstacleType type) {
838 9671842 : if (stripe >= 0 && stripe < numStripes) {
839 2277315 : if ((dir == FORWARD && x - width / 2. < obs[stripe].xBack) || (dir == BACKWARD && x + width / 2. > obs[stripe].xFwd)) {
840 1982136 : obs[stripe] = Obstacle(x, 0, type, id, width);
841 : }
842 : }
843 9671842 : }
844 :
845 : void
846 18625906 : MSPModel_Striping::moveInDirection(SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir) {
847 145144308 : for (ActiveLanes::iterator it_lane = myActiveLanes.begin(); it_lane != myActiveLanes.end(); ++it_lane) {
848 126518406 : const MSLane* lane = it_lane->first;
849 126518406 : Pedestrians& pedestrians = it_lane->second;
850 126518406 : if (pedestrians.size() == 0) {
851 117063648 : continue;
852 : }
853 : //std::cout << SIMTIME << ">>> lane=" << lane->getID() << " numPeds=" << pedestrians.size() << "\n";
854 9454758 : if (lane->getEdge().isWalkingArea()) {
855 2175307 : const double lateral_offset = (lane->getWidth() - stripeWidth) * 0.5;
856 2175307 : const double minY = stripeWidth * - 0.5 + NUMERICAL_EPS;
857 2175307 : const double maxY = stripeWidth * (numStripes(lane) - 0.5) - NUMERICAL_EPS;
858 : const WalkingAreaPath* debugPath = nullptr;
859 : // need to handle each walkingAreaPath separately and transform
860 : // coordinates beforehand
861 : std::set<const WalkingAreaPath*, walkingarea_path_sorter> paths;
862 17583599 : for (Pedestrians::iterator it = pedestrians.begin(); it != pedestrians.end(); ++it) {
863 15408292 : const PState* p = static_cast<PState*>(*it);
864 : assert(p->myWalkingAreaPath != 0);
865 15408292 : if (p->getDirection() == dir) {
866 7720898 : paths.insert(p->myWalkingAreaPath);
867 7720898 : if DEBUGCOND(*p) {
868 0 : debugPath = p->myWalkingAreaPath;
869 0 : std::cout << SIMTIME << " debugging WalkingAreaPath from=" << debugPath->from->getID() << " to=" << debugPath->to->getID() << " minY=" << minY << " maxY=" << maxY << " latOffset=" << lateral_offset << "\n";
870 : }
871 : }
872 : }
873 2175307 : const double usableWidth = (numStripes(lane) - 1) * stripeWidth;
874 3342856 : for (std::set<const WalkingAreaPath*, walkingarea_path_sorter>::iterator it = paths.begin(); it != paths.end(); ++it) {
875 1167549 : const WalkingAreaPath* path = *it;
876 : Pedestrians toDelete;
877 : Pedestrians transformedPeds;
878 1167549 : transformedPeds.reserve(pedestrians.size());
879 9833729 : for (Pedestrians::iterator it_p = pedestrians.begin(); it_p != pedestrians.end(); ++it_p) {
880 8666180 : PState* p = static_cast<PState*>(*it_p);
881 8666180 : if (p->myWalkingAreaPath == path) {
882 7720898 : transformedPeds.push_back(p);
883 7720898 : if (path == debugPath) std::cout << " ped=" << p->getPerson()->getID() << " relX=" << p->getEdgePos(0) << " relY=" << p->getPosLat() << " (untransformed), vecCoord="
884 0 : << path->shape.transformToVectorCoordinates(p->getPosition(*p->getStage(), -1)) << "\n";
885 945282 : } else if (p->myWalkingAreaPath->from == path->to && p->myWalkingAreaPath->to == path->from) {
886 42973 : if (p->myWalkingAreaPath->dir != path->dir) {
887 : // opposite direction is already in the correct coordinate system
888 39548 : transformedPeds.push_back(p);
889 39548 : if (path == debugPath) std::cout << " ped=" << p->getPerson()->getID() << " relX=" << p->getEdgePos(0) << " relY=" << p->getPosLat() << " (untransformed), vecCoord="
890 0 : << path->shape.transformToVectorCoordinates(p->getPosition(*p->getStage(), -1)) << "\n";
891 : } else {
892 : // x position must be reversed
893 3425 : PState* tp = new PState(*p);
894 3425 : tp->reverse(path->length, usableWidth);
895 3425 : toDelete.push_back(tp);
896 3425 : transformedPeds.push_back(tp);
897 3425 : if (path == debugPath) std::cout << " ped=" << p->getPerson()->getID() << " relX=" << p->getEdgePos(0) << " relY=" << p->getPosLat() << " (semi-transformed), vecCoord="
898 0 : << path->shape.transformToVectorCoordinates(p->getPosition(*p->getStage(), -1)) << "\n";
899 : }
900 : } else {
901 902309 : const Position relPos = path->shape.transformToVectorCoordinates(p->getPosition(*p->getStage(), -1));
902 902309 : const double newY = relPos.y() + lateral_offset;
903 901627 : if (relPos != Position::INVALID && newY >= minY && newY <= maxY) {
904 243636 : PState* tp = new PState(*p);
905 243636 : tp->reset(relPos.x(), newY);
906 243636 : toDelete.push_back(tp);
907 243636 : transformedPeds.push_back(tp);
908 243636 : if (path == debugPath) {
909 0 : std::cout << " ped=" << p->getPerson()->getID() << " relX=" << relPos.x() << " relY=" << newY << " (transformed), vecCoord=" << relPos << "\n";
910 : }
911 : } else {
912 658673 : if (path == debugPath) {
913 0 : std::cout << " ped=" << p->getPerson()->getID() << " relX=" << relPos.x() << " relY=" << newY << " (invalid), vecCoord=" << relPos << "\n";
914 : }
915 : }
916 : }
917 : }
918 : auto itFoe = myWalkingAreaFoes.find(&lane->getEdge());
919 1167549 : if (itFoe != myWalkingAreaFoes.end()) {
920 : // add vehicle foes on paths which cross this walkingarea
921 : // translate the vehicle into a number of dummy-pedestrians
922 : // that occupy the same space
923 2261480 : for (const MSLane* foeLane : itFoe->second) {
924 180937 : for (auto itVeh = foeLane->anyVehiclesBegin(); itVeh != foeLane->anyVehiclesEnd(); ++itVeh) {
925 180937 : const MSVehicle* veh = *itVeh;
926 180937 : const double vehWidth = veh->getVehicleType().getWidth();
927 180937 : Boundary relCorners;
928 180937 : Position relFront = path->shape.transformToVectorCoordinates(veh->getPosition(), true);
929 180937 : Position relBack = path->shape.transformToVectorCoordinates(veh->getBackPosition(), true);
930 : if (relFront == Position::INVALID) {
931 0 : WRITE_WARNINGF("Could not vehicle '%' front position % onto walkingarea '%' path=%, time=%.",
932 : veh->getID(), veh->getPosition(), lane->getID(), path->shape, time2string(SIMSTEP));
933 : }
934 : if (relBack == Position::INVALID) {
935 0 : WRITE_WARNINGF("Could not vehicle '%' back position % onto walkingarea '%' path=%, time=%.",
936 : veh->getID(), veh->getBackPosition(), lane->getID(), path->shape, time2string(SIMSTEP));
937 : }
938 180937 : PositionVector relCenter;
939 180937 : relCenter.push_back(relFront);
940 180937 : relCenter.push_back(relBack);
941 180937 : relCenter.move2side(vehWidth / 2);
942 180937 : relCorners.add(relCenter[0]);
943 180937 : relCorners.add(relCenter[1]);
944 180937 : relCenter.move2side(-vehWidth);
945 180937 : relCorners.add(relCenter[0]);
946 180937 : relCorners.add(relCenter[1]);
947 : // persons should require less gap than the vehicles to prevent getting stuck
948 : // when a vehicles moves towards them
949 180937 : relCorners.growWidth(SAFETY_GAP / 2);
950 180937 : const double xWidth = relCorners.getWidth();
951 180937 : const double vehYmin = MAX2(minY - lateral_offset, relCorners.ymin());
952 180937 : const double vehYmax = MIN2(maxY - lateral_offset, relCorners.ymax());
953 180937 : const double xCenter = relCorners.getCenter().x();
954 : Position yMinPos(xCenter, vehYmin);
955 : Position yMaxPos(xCenter, vehYmax);
956 180937 : const bool addFront = addVehicleFoe(veh, lane, yMinPos, dir * xWidth, 0, lateral_offset, minY, maxY, toDelete, transformedPeds);
957 180937 : const bool addBack = addVehicleFoe(veh, lane, yMaxPos, dir * xWidth, 0, lateral_offset, minY, maxY, toDelete, transformedPeds);
958 180937 : if (path == debugPath) {
959 : std::cout << " veh=" << veh->getID()
960 0 : << " corners=" << relCorners
961 : << " xWidth=" << xWidth
962 0 : << " ymin=" << relCorners.ymin()
963 0 : << " ymax=" << relCorners.ymax()
964 : << " vehYmin=" << vehYmin
965 : << " vehYmax=" << vehYmax
966 0 : << "\n";
967 : }
968 180937 : if (addFront && addBack) {
969 : // add in-between positions
970 180937 : const double yDist = vehYmax - vehYmin;
971 413586 : for (double dist = stripeWidth; dist < yDist; dist += stripeWidth) {
972 232649 : const double relDist = dist / yDist;
973 232649 : Position between = (yMinPos * relDist) + (yMaxPos * (1 - relDist));
974 232649 : if (path == debugPath) {
975 0 : std::cout << " vehBetween=" << veh->getID() << " pos=" << between << "\n";
976 : }
977 232649 : addVehicleFoe(veh, lane, between, dir * xWidth, stripeWidth, lateral_offset, minY, maxY, toDelete, transformedPeds);
978 : }
979 : }
980 180937 : }
981 : }
982 : }
983 1167549 : moveInDirectionOnLane(transformedPeds, lane, currentTime, changedLane, dir, path == debugPath);
984 1167549 : arriveAndAdvance(pedestrians, currentTime, changedLane, dir);
985 : // clean up
986 1910008 : for (Pedestrians::iterator it_p = toDelete.begin(); it_p != toDelete.end(); ++it_p) {
987 742459 : delete *it_p;
988 : }
989 1167549 : }
990 : } else {
991 7279451 : moveInDirectionOnLane(pedestrians, lane, currentTime, changedLane, dir, false);
992 7279451 : arriveAndAdvance(pedestrians, currentTime, changedLane, dir);
993 : }
994 : }
995 18625902 : }
996 :
997 :
998 : bool
999 594523 : MSPModel_Striping::addVehicleFoe(const MSVehicle* veh, const MSLane* walkingarea, const Position& relPos, double xWidth, double yWidth, double lateral_offset,
1000 : double minY, double maxY, Pedestrians& toDelete, Pedestrians& transformedPeds) {
1001 : if (relPos != Position::INVALID) {
1002 594523 : const double newY = relPos.y() + lateral_offset;
1003 594523 : if (newY >= minY && newY <= maxY) {
1004 495398 : PState* tp = new PStateVehicle(veh, walkingarea, relPos.x(), newY, xWidth, yWidth);
1005 : //std::cout << SIMTIME << " addVehicleFoe=" << veh->getID() << " rx=" << relPos.x() << " ry=" << newY << " s=" << tp->stripe() << " o=" << tp->otherStripe() << "\n";
1006 495398 : toDelete.push_back(tp);
1007 495398 : transformedPeds.push_back(tp);
1008 : }
1009 594523 : return true;
1010 : } else {
1011 : return false;
1012 : }
1013 : }
1014 :
1015 : void
1016 8447000 : MSPModel_Striping::arriveAndAdvance(Pedestrians& pedestrians, SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir) {
1017 : // advance to the next lane / arrive at destination
1018 8447000 : sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(dir));
1019 : // can't use iterators because we do concurrent modification
1020 133290518 : for (int i = 0; i < (int)pedestrians.size(); i++) {
1021 124843522 : PState* const p = static_cast<PState*>(pedestrians[i]);
1022 124843522 : if (p->isRemoteControlled()) {
1023 27878 : continue;
1024 : }
1025 124815644 : if (p->getDirection() == dir && p->distToLaneEnd() < 0) {
1026 : // moveToNextLane may trigger re-insertion (for consecutive
1027 : // walks) so erase must be called first
1028 : pedestrians.erase(pedestrians.begin() + i);
1029 957498 : i--;
1030 957498 : p->moveToNextLane(currentTime);
1031 957494 : if (p->getLane() != nullptr) {
1032 737260 : changedLane.insert(p->getPerson());
1033 737260 : myActiveLanes[p->getLane()].push_back(p);
1034 : } else {
1035 : // end walking stage and destroy PState
1036 220234 : p->getStage()->moveToNextEdge(p->getPerson(), currentTime, dir);
1037 220234 : myNumActivePedestrians--;
1038 : }
1039 : }
1040 : }
1041 8446996 : }
1042 :
1043 :
1044 : void
1045 8447000 : MSPModel_Striping::moveInDirectionOnLane(Pedestrians& pedestrians, const MSLane* lane, SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir, bool debug) {
1046 8447000 : const int stripes = numStripes(lane);
1047 : //std::cout << " laneWidth=" << lane->getWidth() << " stripeWidth=" << stripeWidth << " stripes=" << stripes << "\n";
1048 16894000 : Obstacles obs(stripes, Obstacle(dir)); // continuously updated
1049 : NextLanesObstacles nextLanesObs; // continuously updated
1050 8447000 : sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(dir));
1051 :
1052 16894000 : Obstacles crossingVehs(stripes, Obstacle(dir));
1053 : bool hasCrossingVehObs = false;
1054 8447000 : if (lane->getEdge().isCrossing()) {
1055 : // assume that vehicles will brake when already on the crossing
1056 636634 : hasCrossingVehObs = addCrossingVehs(lane, stripes, 0, dir, crossingVehs, true);
1057 : }
1058 :
1059 133125971 : for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
1060 124678971 : PState& p = *static_cast<PState*>(pedestrians[ii]);
1061 : UNUSED_PARAMETER(debug);
1062 : //if (debug) {
1063 : // std::cout << SIMTIME << " CHECKING d=" << dir << " p=" << p.getID() << " relX=" << p.myRelX << " xmin=" << p.getMinX() << " xmax=" << p.getMaxX() << " pdir=" << p.getDirection() << "\n";
1064 : //}
1065 124678971 : Obstacles currentObs = obs;
1066 124678971 : if (p.getDirection() != dir || changedLane.count(p.getPerson()) != 0 || p.getRemotePosition() != Position::INVALID) {
1067 58982433 : if (!p.isWaitingToEnter() && !p.isJammed()) {
1068 : //if DEBUGCOND(p) {
1069 : // std::cout << " obs=" << p.getPerson()->getID() << " y=" << p.getPosLat() << " stripe=" << p.stripe() << " oStripe=" << p.otherStripe() << "\n";
1070 : //}
1071 29782055 : Obstacle o(p);
1072 29782055 : if (p.getDirection() != dir && p.getSpeed(*p.getStage()) == 0.) {
1073 : // ensure recognition of oncoming
1074 7503737 : o.speed = (p.getDirection() == FORWARD ? 0.1 : -0.1);
1075 : }
1076 29782055 : if (o.closer(obs[p.stripe()], dir)) {
1077 29777832 : obs[p.stripe()] = o;
1078 : }
1079 29782055 : if (o.closer(obs[p.otherStripe()], dir)) {
1080 29777671 : obs[p.otherStripe()] = o;
1081 : }
1082 : }
1083 : continue;
1084 58982433 : }
1085 65696538 : if DEBUGCOND(p) {
1086 0 : std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << " currentObs=";
1087 0 : gDebugFlag1 = true;
1088 0 : DEBUG_PRINT(currentObs);
1089 : }
1090 65696538 : const MSLane* nextLane = p.myNLI.lane;
1091 65696538 : const MSLink* link = p.myNLI.link;
1092 65696538 : const double dist = p.distToLaneEnd();
1093 : const double speed(p.getStage()->getConfiguredSpeed() >= 0
1094 65696538 : ? p.getStage()->getConfiguredSpeed()
1095 53934562 : : ((nextLane != nullptr && (USE_NET_SPEEDS || nextLane->isNormal() || nextLane->isInternal()))
1096 118223476 : ? nextLane->getVehicleMaxSpeed(p.getPerson())
1097 13059474 : : p.getStage()->getMaxSpeed(p.getPerson())));
1098 :
1099 :
1100 65696538 : if (nextLane != nullptr && dist <= LOOKAHEAD_ONCOMING_DIST) {
1101 16717124 : const double currentLength = (p.myWalkingAreaPath == nullptr ? lane->getLength() : p.myWalkingAreaPath->length);
1102 16717124 : const Obstacles& nextObs = getNextLaneObstacles(
1103 : nextLanesObs, lane, nextLane, stripes,
1104 : p.myNLI.dir, currentLength, dir);
1105 :
1106 16717124 : if DEBUGCOND(p) {
1107 0 : std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << " nextObs=";
1108 0 : DEBUG_PRINT(nextObs);
1109 : }
1110 16717124 : p.mergeObstacles(currentObs, nextObs);
1111 : }
1112 65696538 : if DEBUGCOND(p) {
1113 0 : std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << " obsWithNext=";
1114 0 : DEBUG_PRINT(currentObs);
1115 : }
1116 65696538 : p.mergeObstacles(currentObs, getNeighboringObstacles(pedestrians, ii, stripes));
1117 65696538 : if DEBUGCOND(p) {
1118 0 : std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << " obsWithNeigh=";
1119 0 : DEBUG_PRINT(currentObs);
1120 : }
1121 : // time gap to pass the intersection ahead of a vehicle.
1122 65696538 : const double passingLength = p.getLength() + p.getPerson()->getTimegapCrossing() * speed;
1123 : // check link state
1124 65696538 : if DEBUGCOND(p) {
1125 0 : gDebugFlag1 = true; // get debug output from MSLink
1126 0 : std::cout << " link=" << (link == nullptr ? "NULL" : link->getViaLaneOrLane()->getID())
1127 0 : << " dist=" << dist << " d2=" << dist - p.getMinGap() << " la=" << LOOKAHEAD_SAMEDIR* speed << "\n";
1128 : }
1129 : if (link != nullptr
1130 : // only check close before junction, @todo we should take deceleration into account here
1131 53036647 : && dist - p.getMinGap() < LOOKAHEAD_SAMEDIR * speed
1132 : // persons move before vehicles so we subtract DELTA_TO because they cannot rely on vehicles having passed the intersection in the current time step
1133 76954604 : && (!link->opened(currentTime - DELTA_T, speed, speed, passingLength, p.getImpatience(), speed, 0, 0, nullptr, p.ignoreRed(link), p.getPerson())
1134 10564135 : || p.stopForYellow(link))) {
1135 : // prevent movement passed a closed link
1136 694713 : Obstacles closedLink(stripes, Obstacle(p.getEdgePos(0) + dir * (dist - NUMERICAL_EPS), 0, OBSTACLE_LINKCLOSED, "closedLink_" + link->getViaLaneOrLane()->getID(), 0));
1137 694713 : p.mergeObstacles(currentObs, closedLink);
1138 694713 : if DEBUGCOND(p) {
1139 0 : std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << " obsWithTLS=";
1140 0 : DEBUG_PRINT(currentObs);
1141 : }
1142 : // consider rerouting over another crossing
1143 694713 : if (p.myWalkingAreaPath != nullptr) {
1144 : // @todo actually another path would be needed starting at the current position
1145 539546 : const MSLane* oldNext = p.myNLI.lane;
1146 539546 : p.myNLI = getNextLane(p, p.getLane(), p.myWalkingAreaPath->from);
1147 539546 : if (p.myNLI.lane != oldNext) {
1148 154 : unregisterCrossingApproach(p, oldNext);
1149 : }
1150 : }
1151 694713 : }
1152 65696538 : if DEBUGCOND(p) {
1153 0 : gDebugFlag1 = false;
1154 : }
1155 65696538 : if (&lane->getEdge() == p.getStage()->getDestination() && p.getStage()->getDestinationStop() != nullptr) {
1156 : Obstacles arrival;
1157 1345288 : if (p.getStage()->getDestinationStop()->getWaitingCapacity() > p.getStage()->getDestinationStop()->getNumWaitingPersons() ||
1158 : (!p.getStage()->getDestinationStop()->checkPersonCapacity())) {
1159 2487194 : arrival = Obstacles(stripes, Obstacle(p.getStage()->getArrivalPos() + dir * p.getMinGap(), 0, OBSTACLE_ARRIVALPOS, "arrival", 0));
1160 : } else {
1161 203382 : arrival = Obstacles(stripes, Obstacle(p.getStage()->getArrivalPos() - dir * p.getMinGap(), 0, OBSTACLE_ARRIVALPOS, "arrival_blocked", 0));
1162 : }
1163 1345288 : p.mergeObstacles(currentObs, arrival);
1164 1345288 : }
1165 :
1166 65696538 : if (lane->getVehicleNumberWithPartials() > 0) {
1167 : // react to vehicles on the same lane
1168 : // @todo: improve efficiency by using the same iterator for all pedestrians on this lane
1169 270919 : Obstacles vehObs = getVehicleObstacles(lane, dir, &p);
1170 270919 : p.mergeObstacles(currentObs, vehObs);
1171 270919 : if DEBUGCOND(p) {
1172 0 : std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << " obsWithVehs=";
1173 0 : DEBUG_PRINT(currentObs);
1174 : }
1175 270919 : }
1176 65696538 : if (hasCrossingVehObs) {
1177 23875 : p.mergeObstacles(currentObs, crossingVehs);
1178 23875 : if DEBUGCOND(p) {
1179 0 : std::cout << SIMTIME << " ped=" << p.getPerson()->getID() << " obsWithVehs2=";
1180 0 : DEBUG_PRINT(currentObs);
1181 : }
1182 : }
1183 :
1184 : // walk, taking into account all obstacles
1185 65696538 : p.walk(currentObs);
1186 65696538 : gDebugFlag1 = false;
1187 65696538 : if (!p.isWaitingToEnter() && !p.isJammed()) {
1188 34375856 : Obstacle o(p);
1189 34375856 : if (o.closer(obs[p.stripe()], dir)) {
1190 34374322 : obs[p.stripe()] = o;
1191 : }
1192 34375856 : if (o.closer(obs[p.otherStripe()], dir)) {
1193 34374317 : obs[p.otherStripe()] = o;
1194 : }
1195 34375856 : if (MSGlobals::gCheck4Accidents && p.myWalkingAreaPath == nullptr && !p.isJammed()) {
1196 2553107496 : for (int coll = 0; coll < ii; ++coll) {
1197 2524252805 : PState& c = *static_cast<PState*>(pedestrians[coll]);
1198 2524252805 : if (!c.isWaitingToEnter() && c.myWalkingAreaPath == nullptr && !c.isJammed()) {
1199 1148477443 : if (c.stripe() == p.stripe() || p.stripe() == c.otherStripe() || p.otherStripe() == c.stripe() || p.otherStripe() == c.otherStripe()) {
1200 441870846 : Obstacle cObs(c);
1201 : // we check only for real collisions, no min gap violations
1202 441870846 : if (p.distanceTo(cObs, false) == DIST_OVERLAP) {
1203 1024504 : WRITE_WARNING("Collision of person '" + p.getPerson()->getID() + "' and person '" + c.getPerson()->getID()
1204 : + "', lane='" + lane->getID() + "', time=" + time2string(currentTime) + ".");
1205 : }
1206 : }
1207 : }
1208 : }
1209 : }
1210 : }
1211 : //std::cout << SIMTIME << p.getPerson()->getID() << " lane=" << lane->getID() << " x=" << p.myRelX << "\n";
1212 124678971 : }
1213 16894000 : }
1214 :
1215 :
1216 : void
1217 575759 : MSPModel_Striping::registerCrossingApproach(const PState& ped, const MSLane* crossing, const MSLane* beforeWA) {
1218 : // person has entered the walkingarea
1219 575759 : SUMOTime arrivalTime = SIMSTEP;
1220 : assert(ped.getLane()->isWalkingArea());
1221 575759 : const WalkingAreaPath* wa = getWalkingAreaPath(&ped.getLane()->getEdge(), beforeWA, crossing);
1222 575759 : const double speed = ped.getStage()->getMaxSpeed(ped.getPerson()) * (1 - dawdling / 2);
1223 575759 : arrivalTime += TIME2STEPS(wa->length / speed);
1224 575759 : SUMOTime leavingTime = arrivalTime + TIME2STEPS(crossing->getLength() / speed);
1225 575759 : crossing->getIncomingLanes()[0].viaLink->setApproachingPerson(ped.getPerson(), arrivalTime, leavingTime);
1226 575759 : if DEBUGCOND(ped) {
1227 0 : std::cout << SIMTIME << " register " << ped.getPerson()->getID() << " at crossing " << crossing->getID() << "\n";
1228 : }
1229 575759 : }
1230 :
1231 :
1232 : bool
1233 1010024 : MSPModel_Striping::addCrossingVehs(const MSLane* crossing, int stripes, double lateral_offset, int dir, Obstacles& obs, bool prio) {
1234 : bool hasCrossingVehObs = false;
1235 1010024 : const MSLink* crossingExitLink = crossing->getLinkCont().front();
1236 1010024 : gDebugFlag1 = DEBUGCOND2(crossing);
1237 1010024 : const MSLink::LinkLeaders linkLeaders = crossingExitLink->getLeaderInfo(nullptr, crossing->getLength());
1238 1010024 : gDebugFlag1 = false;
1239 1010024 : if (linkLeaders.size() > 0) {
1240 211598 : for (MSLink::LinkLeaders::const_iterator it = linkLeaders.begin(); it != linkLeaders.end(); ++it) {
1241 : // the vehicle to enter the junction first has priority
1242 107400 : const MSVehicle* veh = (*it).vehAndGap.first;
1243 107400 : if (veh != nullptr) {
1244 107400 : Obstacle vo((*it).distToCrossing, 0, OBSTACLE_VEHICLE, veh->getID(), veh->getVehicleType().getWidth() + 2 * minGapToVehicle, veh);
1245 : // block entry to the crossing in walking direction but allow leaving it
1246 107400 : Obstacle voBlock = vo;
1247 107400 : if (dir == FORWARD) {
1248 59703 : voBlock.xBack = NUMERICAL_EPS;
1249 : } else {
1250 47697 : voBlock.xFwd = crossing->getLength() - NUMERICAL_EPS;
1251 : }
1252 : // when approaching a priority crossings, vehicles must be able
1253 : // to brake, otherwise the person must be able to cross in time
1254 107400 : const double distToCrossBeforeVeh = (dir == FORWARD ? vo.xFwd : crossing->getLength() - vo.xBack);
1255 : const double bGap = (prio
1256 109031 : ? veh->getCarFollowModel().brakeGap(veh->getSpeed(), veh->getCarFollowModel().getMaxDecel(), 0)
1257 1631 : : veh->getSpeed() * distToCrossBeforeVeh); // walking 1m/s
1258 : double vehYmin;
1259 : double vehYmax;
1260 : // relY increases from left to right (the other way around from vehicles)
1261 107400 : if ((*it).fromLeft()) {
1262 70021 : vehYmin = -(*it).vehAndGap.second + lateral_offset; // vehicle back
1263 70021 : vehYmax = vehYmin + veh->getVehicleType().getLength() + bGap + minGapToVehicle;
1264 70021 : vehYmin -= minGapToVehicle;
1265 : } else {
1266 37379 : vehYmax = crossing->getWidth() + (*it).vehAndGap.second - lateral_offset; // vehicle back
1267 37379 : vehYmin = vehYmax - veh->getVehicleType().getLength() - bGap - minGapToVehicle;
1268 37379 : vehYmax += minGapToVehicle;
1269 :
1270 : }
1271 429641 : for (int s = MAX2(0, PState::stripe(vehYmin)); s < MIN2(PState::stripe(vehYmax), stripes); ++s) {
1272 179218 : if ((dir == FORWARD && obs[s].xBack > vo.xBack)
1273 322499 : || (dir == BACKWARD && obs[s].xFwd < vo.xFwd)) {
1274 321600 : if (!prio && veh->getSpeed() > SUMO_const_haltingSpeed) {
1275 : // do not enter the crossing
1276 9220 : obs[s] = voBlock;
1277 : } else {
1278 312380 : obs[s] = vo;
1279 : }
1280 : hasCrossingVehObs = true;
1281 : }
1282 : }
1283 107400 : if (DEBUGCOND2(crossing)) {
1284 0 : std::cout << SIMTIME
1285 : << " crossingVeh=" << veh->getID()
1286 : << " lane=" << crossing->getID()
1287 : << " prio=" << prio
1288 : << " latOffset=" << lateral_offset
1289 : << " dir=" << dir
1290 0 : << " stripes=" << stripes
1291 0 : << " dist=" << (*it).distToCrossing
1292 0 : << " gap=" << (*it).vehAndGap.second
1293 : << " brakeGap=" << bGap
1294 : << " fromLeft=" << (*it).fromLeft()
1295 : << " distToCrossBefore=" << distToCrossBeforeVeh
1296 : << " ymin=" << vehYmin
1297 : << " ymax=" << vehYmax
1298 : << " smin=" << PState::stripe(vehYmin)
1299 0 : << " smax=" << PState::stripe(vehYmax)
1300 0 : << "\n";
1301 0 : DEBUG_PRINT(obs);
1302 : }
1303 : }
1304 : }
1305 104198 : if (hasCrossingVehObs) {
1306 : // check whether the crossing is fully blocked
1307 69614 : const int reserved = getReserved((int)obs.size(), RESERVE_FOR_ONCOMING_FACTOR_JUNCTIONS);
1308 : bool allBlocked = true;
1309 :
1310 385592 : for (int i = 0; i < (int)obs.size(); i++) {
1311 342679 : const Obstacle& o = obs[i];
1312 342679 : if (o.type != OBSTACLE_VEHICLE && (
1313 49720 : (dir == FORWARD && i >= reserved) ||
1314 34430 : (dir == BACKWARD && i < (int)obs.size() - reserved))) {
1315 : allBlocked = false;
1316 : break;
1317 : }
1318 : }
1319 69614 : if (allBlocked) {
1320 42913 : if (DEBUGCOND2(crossing)) {
1321 0 : std::cout << SIMTIME << " crossing=" << crossing->getID() << " allBlocked\n";
1322 : }
1323 304790 : for (Obstacle& o : obs) {
1324 261877 : if (dir == FORWARD) {
1325 142329 : o.xBack = NUMERICAL_EPS;
1326 : } else {
1327 119548 : o.xFwd = crossing->getLength() - NUMERICAL_EPS;
1328 : }
1329 : }
1330 : }
1331 : }
1332 : }
1333 1010024 : return hasCrossingVehObs;
1334 1010024 : }
1335 :
1336 :
1337 : MSPModel_Striping::Obstacles
1338 289541 : MSPModel_Striping::getVehicleObstacles(const MSLane* lane, int dir, PState* ped) {
1339 289541 : const int stripes = numStripes(lane);
1340 579082 : Obstacles vehObs(stripes, Obstacle(dir));
1341 : int current = -1;
1342 : double minX = 0.;
1343 : double maxX = 0.;
1344 : double pRelY = -1.;
1345 : double pWidth = 0.;
1346 : std::string pID;
1347 289541 : bool debug = DEBUGCOND2(lane);
1348 289541 : if (ped != nullptr) {
1349 270919 : current = ped->stripe();
1350 270919 : minX = ped->getMinX();
1351 270919 : maxX = ped->getMaxX();
1352 : pRelY = ped->getPosLat();
1353 270919 : pWidth = ped->getPerson()->getVehicleType().getWidth();
1354 : pID = ped->getPerson()->getID();
1355 270919 : debug = DEBUGCOND(*ped);
1356 18622 : } else if (dir == BACKWARD) {
1357 : // checking vehicles on the next lane. Use entry point as reference
1358 : minX = lane->getLength();
1359 : maxX = lane->getLength();
1360 : }
1361 289541 : MSLane::AnyVehicleIterator begin = (dir == FORWARD ? lane->anyVehiclesUpstreamBegin() : lane->anyVehiclesBegin());
1362 : MSLane::AnyVehicleIterator end = (dir == FORWARD ? lane->anyVehiclesUpstreamEnd() : lane->anyVehiclesEnd());
1363 756046 : for (MSLane::AnyVehicleIterator it = begin; it != end; ++it) {
1364 466505 : const MSVehicle* veh = *it;
1365 466505 : const bool bidi = veh->getLane() == lane->getBidiLane();
1366 466505 : const double vehBack = veh->getBackPositionOnLane(lane);
1367 466505 : double vehFront = vehBack + veh->getVehicleType().getLength();
1368 : // ensure that vehicles are not blocked
1369 466505 : const double vehNextSpeed = veh->getWaitingTime() > DELTA_T ? 0 : MAX2(veh->getSpeed(), 1.0);
1370 466505 : const double clearance = SAFETY_GAP + vehNextSpeed * LOOKAHEAD_SAMEDIR;
1371 : // boundaries for range checking
1372 : double vehXMax;
1373 : double vehXMin;
1374 : double vehXMaxCheck;
1375 : double vehXMinCheck;
1376 466505 : if (bidi) {
1377 1617 : vehFront = vehBack - veh->getVehicleType().getLength();
1378 1617 : vehXMax = vehBack + SAFETY_GAP;
1379 1617 : vehXMin = vehFront - clearance;
1380 1617 : if (dir == FORWARD) {
1381 1617 : vehXMaxCheck = vehBack + NUMERICAL_EPS;
1382 1617 : vehXMinCheck = vehFront - LOOKAROUND_VEHICLES;
1383 : } else {
1384 0 : vehXMaxCheck = vehBack + LOOKAHEAD_SAMEDIR;
1385 : vehXMinCheck = vehFront - clearance;
1386 : }
1387 : } else {
1388 464888 : vehXMax = vehFront + clearance;
1389 464888 : vehXMin = vehBack - SAFETY_GAP;
1390 464888 : if (dir == FORWARD) {
1391 : vehXMaxCheck = vehFront + clearance;
1392 342689 : vehXMinCheck = vehBack - LOOKAHEAD_SAMEDIR;
1393 : } else {
1394 122199 : vehXMaxCheck = vehFront + LOOKAROUND_VEHICLES;
1395 122199 : vehXMinCheck = vehBack - NUMERICAL_EPS;
1396 : }
1397 : }
1398 466505 : if (debug) {
1399 0 : std::cout << SIMTIME << " ped=" << pID << " veh=" << veh->getID() << " check obstacle on lane=" << lane->getID()
1400 : << "\n"
1401 : << " vehXMin=" << vehXMin
1402 : << " vehXMax=" << vehXMax
1403 : << " vehXMinC=" << vehXMinCheck
1404 : << " vehXMaxC=" << vehXMaxCheck
1405 : << " minX=" << minX
1406 : << " maxX=" << maxX
1407 : << " bidi=" << bidi
1408 : << " vFront=" << vehFront
1409 : << " vBack=" << vehBack
1410 0 : << "\n";
1411 : }
1412 466505 : if (vehXMaxCheck > minX && vehXMinCheck && vehXMinCheck <= maxX) {
1413 350914 : Obstacle vo(vehBack, veh->getSpeed() * (bidi ? -1 : 1), OBSTACLE_VEHICLE, veh->getID(), 0, veh);
1414 : // moving vehicles block space along their path
1415 175811 : vo.xFwd = vehXMax;
1416 175811 : vo.xBack = vehXMin;
1417 : // relY increases from left to right (the other way around from vehicles)
1418 : // XXX lateral offset for partial vehicles
1419 175811 : const double posLat = veh->getLateralPositionOnLane() * (bidi ? -1 : 1);
1420 175811 : const double vehYmax = 0.5 * (lane->getWidth() + veh->getVehicleType().getWidth() - stripeWidth) - posLat;
1421 175811 : const double vehYmin = vehYmax - veh->getVehicleType().getWidth();
1422 755469 : for (int s = MAX2(0, PState::stripe(vehYmin)); s < MIN2(PState::stripe(vehYmax) + 1, stripes); ++s) {
1423 579658 : Obstacle prior = vehObs[s];
1424 : vehObs[s] = vo;
1425 579658 : if (s == current && vehFront + SAFETY_GAP < minX) {
1426 : // ignore if already overlapping while vehicle is still behind
1427 16724 : if (pRelY - pWidth < vehYmax &&
1428 16336 : pRelY + pWidth > vehYmin && dir == FORWARD) {
1429 10033 : if (debug) {
1430 0 : std::cout << " ignoring vehicle '" << veh->getID() << " on stripe " << s << " vehFrontSG=" << vehFront + SAFETY_GAP << " minX=" << minX << "\n";
1431 : }
1432 : if (dir == FORWARD) {
1433 : vehObs[s] = prior;
1434 : } else {
1435 : vehObs[s].xFwd = MIN2(vo.xFwd, vehFront + SAFETY_GAP);
1436 : }
1437 : }
1438 : }
1439 : }
1440 175811 : if (debug) {
1441 0 : std::cout << SIMTIME << " ped=" << pID << " veh=" << veh->getID() << " obstacle on lane=" << lane->getID()
1442 : << "\n"
1443 : << " ymin=" << vehYmin
1444 : << " ymax=" << vehYmax
1445 : << " smin=" << PState::stripe(vehYmin)
1446 0 : << " smax=" << PState::stripe(vehYmax)
1447 0 : << " relY=" << pRelY
1448 : << " current=" << current
1449 0 : << " vo.xFwd=" << vo.xFwd
1450 0 : << " vo.xBack=" << vo.xBack
1451 : << " vFront=" << vehFront
1452 : << " vBack=" << vehBack
1453 0 : << "\n";
1454 : }
1455 : }
1456 : }
1457 289541 : return vehObs;
1458 0 : }
1459 :
1460 :
1461 : // ===========================================================================
1462 : // MSPModel_Striping::Obstacle method definitions
1463 : // ===========================================================================
1464 82880079 : MSPModel_Striping::Obstacle::Obstacle(int dir, double dist) :
1465 82880079 : xFwd(dir * dist), // by default, far away when seen in dir
1466 82880079 : xBack(dir * dist), // by default, far away when seen in dir
1467 82880079 : speed(0),
1468 82880079 : type(OBSTACLE_NONE),
1469 82880079 : description("") {
1470 82880079 : }
1471 :
1472 :
1473 649447761 : MSPModel_Striping::Obstacle::Obstacle(const PState& ped) :
1474 649447761 : xFwd(ped.getMaxX()),
1475 649447761 : xBack(ped.getMinX()),
1476 649447761 : speed(ped.getDirection() * ped.getSpeed(*ped.getStage())),
1477 649447761 : type(ped.getOType()),
1478 649447761 : description(ped.getID()) {
1479 : assert(!ped.isWaitingToEnter());
1480 649447761 : if (type == OBSTACLE_VEHICLE) {
1481 529902 : vehicle = static_cast<const PStateVehicle&>(ped).getVehicle();
1482 : }
1483 649447761 : }
1484 :
1485 :
1486 : bool
1487 128315822 : MSPModel_Striping::Obstacle::closer(const Obstacle& o, int dir) {
1488 128315822 : if (dir == FORWARD) {
1489 69227236 : return xBack <= o.xBack;
1490 : } else {
1491 59088586 : return xFwd >= o.xFwd;
1492 : }
1493 : }
1494 :
1495 :
1496 : // ===========================================================================
1497 : // MSPModel_Striping::PState method definitions
1498 : // ===========================================================================
1499 243016 : MSPModel_Striping::PState::PState(MSPerson* person, MSStageMoving* stage, const MSLane* lane):
1500 : MSPModel_InteractingState(person, stage, lane),
1501 243016 : myWalkingAreaPath(nullptr) {
1502 : const MSEdge* currentEdge = &lane->getEdge();
1503 : const ConstMSEdgeVector& route = myStage->getRoute();
1504 : assert(!route.empty());
1505 243016 : myDir = FORWARD;
1506 243016 : if (route.size() == 1) {
1507 : // only a single edge, move towards end pos
1508 13270 : myDir = (myEdgePos <= myStage->getArrivalPos()) ? FORWARD : BACKWARD;
1509 229746 : } else if (route.front()->getFunction() != SumoXMLEdgeFunc::NORMAL) {
1510 : // start on an intersection
1511 8 : if (route.front()->isWalkingArea()) {
1512 8 : myWalkingAreaPath = getArbitraryPath(route.front());
1513 : }
1514 : } else {
1515 229738 : int passedFwd = 0;
1516 229738 : int passedBwd = 0;
1517 229738 : const bool mayStartForward = canTraverse(FORWARD, route, passedFwd) != UNDEFINED_DIRECTION;
1518 229738 : const bool mayStartBackward = canTraverse(BACKWARD, route, passedBwd) != UNDEFINED_DIRECTION;
1519 229738 : if DEBUGCOND(*this) {
1520 0 : std::cout << " initialize dir for " << myPerson->getID() << " forward=" << mayStartForward << " backward=" << mayStartBackward << "\n";
1521 : }
1522 229738 : if (mayStartForward && mayStartBackward) {
1523 : // figure out the best direction via routing
1524 : ConstMSEdgeVector crossingRoute;
1525 7380 : MSNet::getInstance()->getPedestrianRouter(0).compute(currentEdge, route.back(), myEdgePos, myStage->getArrivalPos(),
1526 2460 : myStage->getMaxSpeed(person), 0, nullptr, person->getVTypeParameter(), crossingRoute, true);
1527 2460 : if (crossingRoute.size() > 1) {
1528 : // route found
1529 2452 : const MSEdge* nextEdge = crossingRoute[1];
1530 2452 : if (nextEdge->getFromJunction() == currentEdge->getFromJunction() || nextEdge->getToJunction() == currentEdge->getFromJunction()) {
1531 1866 : myDir = BACKWARD;
1532 : }
1533 : }
1534 2460 : if DEBUGCOND(*this) {
1535 0 : std::cout << " crossingRoute=" << toString(crossingRoute) << "\n";
1536 : }
1537 229738 : } else if (!mayStartForward && !mayStartBackward) {
1538 157 : int lastDisconnect = passedFwd >= passedBwd ? passedFwd : passedBwd;
1539 : std::string dLoc;
1540 157 : if (route.size() > 2) {
1541 260 : dLoc = TLF(" between edge '%' and edge '%'", route[lastDisconnect - 1]->getID(), route[lastDisconnect]->getID());
1542 : }
1543 942 : WRITE_WARNINGF(TL("Person '%' walking from edge '%' to edge '%' has a disconnect%, time=%."),
1544 : myPerson->getID(), route.front()->getID(), route.back()->getID(), dLoc, SIMTIME);
1545 162 : myDir = passedFwd >= passedBwd ? FORWARD : BACKWARD;
1546 : } else {
1547 424847 : myDir = !mayStartBackward ? FORWARD : BACKWARD;
1548 : }
1549 : }
1550 243016 : if (myPosLat == UNSPECIFIED_POS_LAT || myLegacyPosLat) {
1551 242732 : if (myPosLat == UNSPECIFIED_POS_LAT) {
1552 242546 : myPosLat = 0;
1553 : }
1554 242732 : if (lane->getVehicleNumberWithPartials() > 0 && myPosLat == 0) {
1555 : // better start next to the road if nothing was specified
1556 2155 : myPosLat -= stripeWidth;
1557 : }
1558 242732 : if (myDir == FORWARD || lane->getPermissions() != SVC_PEDESTRIAN) {
1559 : // start at the right side of the sidewalk on shared roads
1560 207202 : myPosLat = stripeWidth * (numStripes(lane) - 1) - myPosLat;
1561 : }
1562 284 : } else if (myPosLat == RANDOM_POS_LAT) {
1563 20 : myPosLat = RandHelper::rand() * stripeWidth * (numStripes(lane) - 1);
1564 : } else {
1565 : // vehicle to striping coordinate system
1566 264 : myPosLat = posLatConversion(myPosLat, lane->getWidth());
1567 : }
1568 243016 : if DEBUGCOND(*this) {
1569 0 : std::cout << " added new pedestrian " << myPerson->getID() << " on " << lane->getID() << " myEdgePos=" << myEdgePos << " myPosLat=" << myPosLat << " dir=" << myDir << " route=" << toString(myStage->getRoute()) << "\n";
1570 : }
1571 :
1572 243016 : myNLI = getNextLane(*this, lane, nullptr);
1573 243012 : }
1574 :
1575 :
1576 14 : MSPModel_Striping::PState::PState(MSPerson* person, MSStageMoving* stage, std::istringstream* in) :
1577 : MSPModel_InteractingState(person, stage, nullptr),
1578 14 : myWalkingAreaPath(nullptr) {
1579 14 : if (in != nullptr) {
1580 : std::string laneID;
1581 : std::string wapLaneFrom;
1582 : std::string wapLaneTo;
1583 : std::string nextLaneID;
1584 : std::string nextLinkFrom;
1585 : std::string nextLinkTo;
1586 : int nextDir;
1587 :
1588 14 : (*in) >> laneID
1589 14 : >> myEdgePos >> myPosLat >> myDir >> mySpeed >> mySpeedLat >> myWaitingToEnter >> myWaitingTime
1590 14 : >> wapLaneFrom >> wapLaneTo
1591 14 : >> myAmJammed
1592 : >> nextLaneID
1593 : >> nextLinkFrom
1594 14 : >> nextLinkTo
1595 14 : >> nextDir;
1596 :
1597 :
1598 14 : myLane = MSLane::dictionary(laneID);
1599 14 : if (myLane == nullptr) {
1600 0 : throw ProcessError("Unknown lane '" + laneID + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1601 : }
1602 :
1603 : MSLane* nextLane = nullptr;
1604 14 : if (nextLaneID != "null") {
1605 13 : nextLane = MSLane::dictionary(nextLaneID);
1606 13 : if (nextLane == nullptr) {
1607 0 : throw ProcessError("Unknown next lane '" + nextLaneID + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1608 : }
1609 : }
1610 : const MSLink* link = nullptr;
1611 14 : if (nextLinkFrom != "null") {
1612 2 : MSLane* from = MSLane::dictionary(nextLinkFrom);
1613 2 : MSLane* to = MSLane::dictionary(nextLinkTo);
1614 2 : if (from == nullptr) {
1615 0 : throw ProcessError("Unknown link origin lane '" + nextLinkFrom + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1616 : }
1617 2 : if (to == nullptr) {
1618 0 : throw ProcessError("Unknown link destination lane '" + nextLinkTo + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1619 : }
1620 2 : link = from->getLinkTo(to);
1621 : }
1622 14 : myNLI = NextLaneInfo(nextLane, link, nextDir);
1623 :
1624 14 : if (wapLaneFrom != "null") {
1625 1 : MSLane* from = MSLane::dictionary(wapLaneFrom);
1626 1 : MSLane* to = MSLane::dictionary(wapLaneTo);
1627 1 : if (from == nullptr) {
1628 0 : throw ProcessError("Unknown walkingAreaPath origin lane '" + wapLaneFrom + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1629 : }
1630 1 : if (to == nullptr) {
1631 0 : throw ProcessError("Unknown walkingAreaPath destination lane '" + wapLaneTo + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1632 : }
1633 1 : const auto pathIt = myWalkingAreaPaths.find(std::make_pair(from, to));
1634 1 : if (pathIt != myWalkingAreaPaths.end()) {
1635 1 : myWalkingAreaPath = &pathIt->second;
1636 : } else {
1637 0 : throw ProcessError("Unknown walkingAreaPath from lane '" + wapLaneFrom + "' to lane '" + wapLaneTo + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1638 : }
1639 : }
1640 : }
1641 14 : }
1642 :
1643 :
1644 495398 : MSPModel_Striping::PState::PState() :
1645 495398 : MSPModel_InteractingState(nullptr, nullptr, nullptr) {}
1646 :
1647 :
1648 : void
1649 9 : MSPModel_Striping::PState::saveState(std::ostringstream& out) {
1650 9 : std::string wapLaneFrom = "null";
1651 9 : std::string wapLaneTo = "null";
1652 9 : if (myWalkingAreaPath != nullptr) {
1653 1 : wapLaneFrom = myWalkingAreaPath->from->getID();
1654 1 : wapLaneTo = myWalkingAreaPath->to->getID();
1655 : }
1656 9 : std::string nextLaneID = "null";
1657 9 : std::string nextLinkFrom = "null";
1658 9 : std::string nextLinkTo = "null";
1659 9 : if (myNLI.lane != nullptr) {
1660 : nextLaneID = myNLI.lane->getID();
1661 : }
1662 9 : if (myNLI.link != nullptr) {
1663 : nextLinkFrom = myNLI.link->getLaneBefore()->getID();
1664 2 : nextLinkTo = myNLI.link->getViaLaneOrLane()->getID();
1665 : }
1666 9 : out << " " << myLane->getID()
1667 18 : << " " << myEdgePos
1668 9 : << " " << myPosLat
1669 : << " " << myDir
1670 18 : << " " << mySpeed
1671 9 : << " " << mySpeedLat
1672 9 : << " " << myWaitingToEnter
1673 9 : << " " << myWaitingTime
1674 : << " " << wapLaneFrom
1675 : << " " << wapLaneTo
1676 9 : << " " << myAmJammed
1677 : << " " << nextLaneID
1678 : << " " << nextLinkFrom
1679 : << " " << nextLinkTo
1680 9 : << " " << myNLI.dir;
1681 9 : }
1682 :
1683 : double
1684 2102052808 : MSPModel_Striping::PState::getMinX(const bool includeMinGap) const {
1685 : // @todo speed should have an influence here because faster persons need more space
1686 2102052808 : if (myDir == FORWARD) {
1687 1934783667 : return myEdgePos - getLength();
1688 : }
1689 167269141 : return myEdgePos - (includeMinGap ? getMinGap() : 0.);
1690 : }
1691 :
1692 :
1693 : double
1694 2102052808 : MSPModel_Striping::PState::getMaxX(const bool includeMinGap) const {
1695 : // @todo speed should have an influence here because faster persons need more space
1696 2102052808 : if (myDir == FORWARD) {
1697 1934783667 : return myEdgePos + (includeMinGap ? getMinGap() : 0.);
1698 : }
1699 167269141 : return myEdgePos + getLength();
1700 : }
1701 :
1702 :
1703 : double
1704 2167749346 : MSPModel_Striping::PState::getLength() const {
1705 2167749346 : return myPerson->getVehicleType().getLength();
1706 : }
1707 :
1708 :
1709 : double
1710 1408423499 : MSPModel_Striping::PState::getMinGap() const {
1711 1408423499 : return myPerson->getVehicleType().getMinGap();
1712 : }
1713 :
1714 :
1715 : int
1716 7450206720 : MSPModel_Striping::PState::stripe(double relY) {
1717 7450206720 : return (int)floor(relY / stripeWidth + 0.5);
1718 : }
1719 :
1720 :
1721 : int
1722 3236341412 : MSPModel_Striping::PState::otherStripe(double relY) const {
1723 3236341412 : const int s = stripe(relY);
1724 3236341412 : const double offset = relY - s * stripeWidth;
1725 3236341412 : const double threshold = MAX2(NUMERICAL_EPS, stripeWidth - SQUEEZE * getWidth());
1726 : int result;
1727 3236341412 : if (offset > threshold) {
1728 496421 : result = s + 1;
1729 3235844991 : } else if (offset < -threshold) {
1730 419816 : result = s - 1;
1731 : } else {
1732 : result = s;
1733 : }
1734 : //std::cout.setf(std::ios::fixed , std::ios::floatfield);
1735 : //std::cout << std::setprecision(5);
1736 : //if DEBUGCOND(*this) std::cout << " otherStripe " << myPerson->getID() << " offset=" << offset << " threshold=" << threshold << " rawResult=" << result << "\n";
1737 3236341412 : return result;
1738 : }
1739 :
1740 : int
1741 4181474070 : MSPModel_Striping::PState::stripe() const {
1742 4181474070 : return MIN2(MAX2(0, stripe(myPosLat)), numStripes(myLane) - 1);
1743 : }
1744 :
1745 :
1746 : int
1747 3220619437 : MSPModel_Striping::PState::otherStripe() const {
1748 3220619437 : return MIN2(MAX2(0, otherStripe(myPosLat)), numStripes(myLane) - 1);
1749 : }
1750 :
1751 :
1752 : double
1753 133579469 : MSPModel_Striping::PState::distToLaneEnd() const {
1754 133579469 : if (myStage->getNextRouteEdge() == nullptr) {
1755 23925083 : return myDir * (myStage->getArrivalPos() - myEdgePos) - POSITION_EPS - (
1756 2021241 : (myWaitingTime > DELTA_T && (myStage->getDestinationStop() == nullptr ||
1757 : myStage->getDestinationStop()->getWaitingCapacity() > myStage->getDestinationStop()->getNumWaitingPersons()))
1758 25946324 : ? getMinGap() : 0);
1759 : } else {
1760 109654386 : const double length = myWalkingAreaPath == nullptr ? myLane->getLength() : myWalkingAreaPath->length;
1761 109654386 : return myDir == FORWARD ? length - myEdgePos : myEdgePos;
1762 : }
1763 : }
1764 :
1765 :
1766 : bool
1767 957498 : MSPModel_Striping::PState::moveToNextLane(SUMOTime currentTime) {
1768 957498 : double dist = distToLaneEnd();
1769 957498 : if (DEBUGCOND(*this)) {
1770 0 : std::cout << SIMTIME << " ped=" << myPerson->getID() << " myEdgePos=" << myEdgePos << " dist=" << dist << "\n";
1771 : }
1772 957498 : if (dist <= 0) {
1773 : //if (ped.getPerson()->getID() == DEBUG1) {
1774 : // std::cout << SIMTIME << " addToLane x=" << ped.myEdgePos << " newDir=" << newDir << " newLane=" << newLane->getID() << " walkingAreaShape=" << walkingAreaShape << "\n";
1775 : //}
1776 : //std::cout << " changing to " << newLane->getID() << " myPosLat=" << ped.myPosLat << " oldStripes=" << numStripes(myLane) << " newStripes=" << numStripes(newLane);
1777 : //std::cout << " newY=" << ped.myPosLat << " myDir=" << ped.getDirection() << " newDir=" << newDir;
1778 957498 : const int oldDir = myDir;
1779 957498 : const MSLane* oldLane = myLane;
1780 957498 : myLane = myNLI.lane;
1781 957498 : myDir = myNLI.dir;
1782 957498 : const bool normalLane = (myLane == nullptr || myLane->getEdge().getFunction() == SumoXMLEdgeFunc::NORMAL || &myLane->getEdge() == myStage->getNextRouteEdge());
1783 957498 : if DEBUGCOND(*this) {
1784 0 : std::cout << SIMTIME
1785 0 : << " ped=" << myPerson->getID()
1786 : << " moveToNextLane old=" << oldLane->getID()
1787 0 : << " new=" << (myLane == nullptr ? "NULL" : myLane->getID())
1788 : << " oldDir=" << oldDir
1789 0 : << " newDir=" << myDir
1790 0 : << " myEdgePos=" << myEdgePos
1791 : << " dist=" << dist
1792 0 : << "\n";
1793 : }
1794 957498 : if (myLane == nullptr) {
1795 220234 : myEdgePos = myStage->getArrivalPos();
1796 : }
1797 : // moveToNextEdge might destroy the person and thus mess up the heap. Better check first
1798 957498 : if (myStage->getRouteStep() == myStage->getRoute().end() - 1) {
1799 220234 : myLane = nullptr;
1800 : } else {
1801 737264 : const bool arrived = myStage->moveToNextEdge(myPerson, currentTime, oldDir, normalLane ? nullptr : &myLane->getEdge());
1802 : UNUSED_PARAMETER(arrived);
1803 : assert(!arrived);
1804 : assert(myDir != UNDEFINED_DIRECTION);
1805 737264 : myNLI = getNextLane(*this, myLane, oldLane);
1806 : // reminders must be called after updated myNLI (to ensure that getNextEdgePtr returns the correct edge)
1807 737264 : myStage->activateEntryReminders(myPerson);
1808 : assert(myNLI.lane != oldLane); // do not turn around
1809 737264 : if DEBUGCOND(*this) {
1810 0 : std::cout << " nextLane=" << (myNLI.lane == nullptr ? "NULL" : myNLI.lane->getID()) << "\n";
1811 : }
1812 737264 : if (myLane->getEdge().isWalkingArea()) {
1813 339059 : if (myNLI.dir != UNDEFINED_DIRECTION) {
1814 339022 : myWalkingAreaPath = getWalkingAreaPath(&myLane->getEdge(), oldLane, myNLI.lane);
1815 : assert(myWalkingAreaPath->shape.size() >= 2);
1816 339022 : if DEBUGCOND(*this) {
1817 0 : std::cout << " mWAPath shape=" << myWalkingAreaPath->shape << " length=" << myWalkingAreaPath->length << "\n";
1818 : }
1819 37 : } else if (myNLI.link != nullptr) {
1820 : // using direct connection (instead of using walkingarea)
1821 17 : myLane = myNLI.lane;
1822 : assert(!myLane->getEdge().isWalkingArea());
1823 17 : myStage->moveToNextEdge(myPerson, currentTime, myDir, &myLane->getEdge());
1824 17 : myWalkingAreaPath = nullptr;
1825 17 : myNLI = getNextLane(*this, myLane, oldLane);
1826 : } else {
1827 : // disconnected route. move to the next edge
1828 40 : if (OptionsCont::getOptions().getBool("ignore-route-errors")) {
1829 : // try to determine direction from topology, otherwise maintain current direction
1830 16 : const MSEdge* currRouteEdge = *myStage->getRouteStep();
1831 16 : const MSEdge* nextRouteEdge = myStage->getNextRouteEdge();
1832 : if ((nextRouteEdge->getToJunction() == currRouteEdge->getFromJunction())
1833 16 : || nextRouteEdge->getToJunction() == currRouteEdge->getToJunction()) {
1834 4 : myDir = BACKWARD;
1835 : } else if ((nextRouteEdge->getFromJunction() == currRouteEdge->getFromJunction())
1836 12 : || nextRouteEdge->getFromJunction() == currRouteEdge->getToJunction()) {
1837 12 : myDir = FORWARD;
1838 : }
1839 16 : myStage->moveToNextEdge(myPerson, currentTime, oldDir, nullptr);
1840 16 : myLane = myNLI.lane;
1841 : assert(myLane != 0);
1842 : assert(myLane->getEdge().getFunction() == SumoXMLEdgeFunc::NORMAL);
1843 16 : myNLI = getNextLane(*this, myLane, oldLane);
1844 16 : myWalkingAreaPath = nullptr;
1845 : } else {
1846 12 : throw ProcessError(TLF("Disconnected walk for person '%'.", myPerson->getID()));
1847 : }
1848 : }
1849 : } else {
1850 398205 : myWalkingAreaPath = nullptr;
1851 : }
1852 : // adapt x to fit onto the new lane
1853 : // (make sure we do not move past the end of the new lane since that
1854 : // lane was not checked for obstacles)
1855 737260 : const double newLength = (myWalkingAreaPath == nullptr ? myLane->getLength() : myWalkingAreaPath->length);
1856 737260 : if (-dist > newLength) {
1857 : assert(OptionsCont::getOptions().getBool("ignore-route-errors"));
1858 : // should not happen because the end of myLane should have been an obstacle as well
1859 : // (only when the route is broken)
1860 1 : dist = -newLength;
1861 : }
1862 737260 : if (myDir == BACKWARD) {
1863 143763 : myEdgePos = newLength + dist;
1864 : } else {
1865 593497 : myEdgePos = -dist;
1866 : }
1867 737260 : if DEBUGCOND(*this) {
1868 0 : std::cout << SIMTIME << " update myEdgePos ped=" << myPerson->getID()
1869 : << " newLength=" << newLength
1870 : << " dist=" << dist
1871 0 : << " myEdgePos=" << myEdgePos
1872 0 : << "\n";
1873 : }
1874 : // adjust to change in direction
1875 737260 : if (myDir != oldDir) {
1876 32394 : myPosLat = (numStripes(oldLane) - 1) * stripeWidth - myPosLat;
1877 : }
1878 : // adjust to differences in sidewalk width
1879 1461453 : const int offset = getStripeOffset(numStripes(oldLane), numStripes(myLane), oldDir != myDir && numStripes(myLane) < numStripes(oldLane));
1880 737260 : myPosLat += offset * stripeWidth;
1881 737260 : if DEBUGCOND(*this) {
1882 0 : std::cout << SIMTIME << " transformY ped=" << myPerson->getID()
1883 0 : << " newLane=" << Named::getIDSecure(myLane)
1884 0 : << " newY=" << myPosLat
1885 0 : << " os=" << numStripes(oldLane) << " ns=" << numStripes(myLane)
1886 0 : << " od=" << oldDir << " nd=" << myDir
1887 0 : << " offset=" << offset << "\n";
1888 : }
1889 : }
1890 957494 : myAngle = std::numeric_limits<double>::max(); // see #9014
1891 957494 : return true;
1892 : } else {
1893 : return false;
1894 : }
1895 : }
1896 :
1897 :
1898 : int
1899 65766152 : MSPModel_Striping::getReserved(int stripes, double factor) {
1900 65766152 : return MIN2(
1901 65766152 : (int)floor(stripes * factor),
1902 65766152 : (int)floor(RESERVE_FOR_ONCOMING_MAX / stripeWidth));
1903 : }
1904 :
1905 : void
1906 65696538 : MSPModel_Striping::PState::walk(const Obstacles& obs) {
1907 65696538 : const int stripes = (int)obs.size();
1908 65696538 : const int sMax = stripes - 1;
1909 : assert(stripes == numStripes(myLane));
1910 : // account stage-specific max speed but also for normal lane speed limit
1911 : // (speed limits on crossings and walkingareas ignored due to #11527)
1912 65696538 : const double vMax = (myStage->getConfiguredSpeed() >= 0
1913 65696538 : ? myStage->getConfiguredSpeed()
1914 9158486 : : (USE_NET_SPEEDS || myLane->isNormal() || myLane->isInternal()
1915 74074811 : ? myLane->getVehicleMaxSpeed(myPerson)
1916 725150 : : myStage->getMaxSpeed(myPerson)));
1917 : // ultimate goal is to choose the preferred stripe (chosen)
1918 65696538 : const int current = stripe();
1919 65696538 : const int other = otherStripe();
1920 : // compute distances
1921 65696538 : std::vector<double> distance(stripes);
1922 285895166 : for (int i = 0; i < stripes; ++i) {
1923 220198628 : distance[i] = distanceTo(obs[i], obs[i].type == OBSTACLE_PED);
1924 : }
1925 : // compute utility for all stripes
1926 65696538 : std::vector<double> utility(stripes, 0);
1927 : // forbid stripes which are blocked and also all stripes behind them
1928 285895166 : for (int i = 0; i < stripes; ++i) {
1929 220198628 : if (distance[i] == DIST_OVERLAP) {
1930 96162606 : if (i == current && (!isWaitingToEnter() || stripe() != stripe(myPosLat))) {
1931 6771754 : utility[i] += OBSTRUCTED_PENALTY;
1932 : }
1933 96162606 : if (i < current) {
1934 135137877 : for (int j = 0; j <= i; ++j) {
1935 82420108 : utility[j] += OBSTRUCTED_PENALTY;
1936 : }
1937 : }
1938 96162606 : if (i > current) {
1939 60565293 : for (int j = i; j < stripes; ++j) {
1940 35024328 : utility[j] += OBSTRUCTED_PENALTY;
1941 : }
1942 : }
1943 : }
1944 : }
1945 : // forbid a portion of the leftmost stripes (in walking direction).
1946 : // lanes with stripes less than 1 / RESERVE_FOR_ONCOMING_FACTOR
1947 : // may still deadlock in heavy pedestrian traffic
1948 65696538 : const bool onJunction = myLane->getEdge().isWalkingArea() || myLane->getEdge().isCrossing();
1949 65696538 : const int reserved = getReserved(stripes, (onJunction ? RESERVE_FOR_ONCOMING_FACTOR_JUNCTIONS : RESERVE_FOR_ONCOMING_FACTOR));
1950 65696538 : if (myDir == FORWARD) {
1951 69001042 : for (int i = 0; i < reserved; ++i) {
1952 16604935 : utility[i] += INAPPROPRIATE_PENALTY * (i == current ? 0.5 : 1);
1953 : }
1954 : } else {
1955 6816111 : for (int i = sMax; i > sMax - reserved; --i) {
1956 2787080 : utility[i] += INAPPROPRIATE_PENALTY * (i == current ? 0.5 : 1);
1957 : }
1958 : }
1959 : // adapt utility based on obstacles
1960 285895166 : for (int i = 0; i < stripes; ++i) {
1961 220198628 : if (obs[i].speed * myDir < 0) {
1962 : // penalize evasion to the left unless the obstacle is a vehicle
1963 4449301 : if ((myDir == FORWARD || obs[i].type == OBSTACLE_VEHICLE) && i > 0) {
1964 858214 : utility[i - 1] -= 0.5;
1965 3591087 : } else if (myDir == BACKWARD && i < sMax) {
1966 595240 : utility[i + 1] -= 0.5;
1967 : }
1968 : }
1969 : // compute expected distance achievable by staying on this stripe for a time horizon
1970 220198628 : const double walkDist = MAX2(0., distance[i]); // disregard special distance flags
1971 220198628 : const double lookAhead = obs[i].speed * myDir >= 0 ? LOOKAHEAD_SAMEDIR : LOOKAHEAD_ONCOMING;
1972 220198628 : const double expectedDist = MIN2(vMax * LOOKAHEAD_SAMEDIR, walkDist + obs[i].speed * myDir * lookAhead);
1973 220198628 : if (expectedDist >= 0) {
1974 217077449 : utility[i] += expectedDist;
1975 : } else {
1976 : // let only the distance count
1977 3121179 : utility[i] += ONCOMING_CONFLICT_PENALTY + distance[i];
1978 : }
1979 : }
1980 : // discourage use of the leftmost stripe (in walking direction) if there are oncoming
1981 65696538 : if (myDir == FORWARD && obs[0].speed < 0) {
1982 1494670 : utility[0] += ONCOMING_CONFLICT_PENALTY;
1983 64201868 : } else if (myDir == BACKWARD && obs[sMax].speed > 0) {
1984 1505439 : utility[sMax] += ONCOMING_CONFLICT_PENALTY;
1985 : }
1986 : // penalize lateral movement (if the current stripe permits walking)
1987 65696538 : if (distance[current] > 0 && myWaitingTime == 0) {
1988 128329956 : for (int i = 0; i < stripes; ++i) {
1989 100617393 : utility[i] += abs(i - current) * LATERAL_PENALTY;
1990 : }
1991 : }
1992 : // walk on the right side on shared space
1993 65696538 : if (myLane->getPermissions() != SVC_PEDESTRIAN && myDir == BACKWARD) {
1994 2334042 : for (int i = 0; i < stripes; ++i) {
1995 1945230 : if (i <= current) {
1996 1857452 : utility[i] += (sMax - i + 1) * LATERAL_PENALTY;
1997 : }
1998 : }
1999 : }
2000 :
2001 : // select best stripe
2002 : int chosen = current;
2003 285895166 : for (int i = 0; i < stripes; ++i) {
2004 220198628 : if (utility[i] > utility[chosen] && utility[i] >= INAPPROPRIATE_PENALTY * 0.5) {
2005 : chosen = i;
2006 : }
2007 : }
2008 : // compute speed components along both axes
2009 65696538 : const int next = (chosen == current ? current : (chosen < current ? current - 1 : current + 1));
2010 65696538 : double xDist = MIN3(distance[current], distance[other], distance[next]);
2011 65696538 : if (next != chosen) {
2012 : // ensure that we do not collide with an obstacle in the stripe beyond
2013 : // next as this might become the 'other' stripe in the next step
2014 477282 : const int nextOther = chosen < current ? current - 2 : current + 2;
2015 477282 : xDist = MIN2(xDist, distance[nextOther]);
2016 : }
2017 : // XXX preferred gap differs between approaching a standing obstacle or a moving obstacle
2018 : const double preferredGap = NUMERICAL_EPS;
2019 65696538 : double xSpeed = MIN2(vMax, MAX2(0., DIST2SPEED(xDist - preferredGap)));
2020 65696538 : if (xSpeed < NUMERICAL_EPS) {
2021 : xSpeed = 0.;
2022 : }
2023 65696538 : if (DEBUGCOND(*this)) {
2024 0 : std::cout << " xSpeedPotential=" << xSpeed << "\n";
2025 : }
2026 : // avoid tiny steps
2027 : // XXX pressure from behind?
2028 65696538 : if (mySpeed == 0 && xDist < MIN_STARTUP_DIST &&
2029 : // unless walking towards a short lane
2030 29636695 : !(
2031 29949512 : (xDist == distance[current] && obs[current].type >= OBSTACLE_END)
2032 29636711 : || (xDist == distance[other] && obs[other].type >= OBSTACLE_END)
2033 29636695 : || (xDist == distance[next] && obs[next].type >= OBSTACLE_END))
2034 : ) {
2035 : xSpeed = 0;
2036 : }
2037 36059845 : if (xSpeed == 0) {
2038 36226342 : if (DEBUGCOND(*this)) {
2039 0 : std::cout << " sharedWA=" << (myWalkingAreaFoes.find(&myLane->getEdge()) != myWalkingAreaFoes.end())
2040 0 : << " vehObs=" << Named::getIDSecure(obs[current].vehicle)
2041 0 : << " vehWait=" << STEPS2TIME(obs[current].vehicle ? obs[current].vehicle->getWaitingTime() : 0)
2042 0 : << "\n";
2043 : }
2044 36226342 : if (myWaitingTime > ((myLane->getEdge().isCrossing()
2045 : // treat shared walkingarea like a crossing to avoid deadlocking vehicles
2046 36222390 : || (myLane->getEdge().isWalkingArea() && obs[current].vehicle != nullptr && obs[current].vehicle->getWaitingTime() > jamTimeCrossing
2047 72449032 : && myWalkingAreaFoes.find(&myLane->getEdge()) != myWalkingAreaFoes.end())) ? jamTimeCrossing : jamTime)
2048 36150185 : || (sMax == 0 && obs[0].speed * myDir < 0 && myWaitingTime > jamTimeNarrow)
2049 72347942 : || myAmJammed) {
2050 : // squeeze slowly through the crowd ignoring others
2051 7829680 : if (!myAmJammed) {
2052 87888 : MSNet::getInstance()->getPersonControl().registerJammed();
2053 351552 : WRITE_WARNINGF(TL("Person '%' is jammed on edge '%', time=%."),
2054 : myPerson->getID(), myStage->getEdge()->getID(), time2string(SIMSTEP));
2055 87888 : myAmJammed = true;
2056 : }
2057 7829680 : xSpeed = vMax * jamFactor;
2058 : }
2059 29470196 : } else if (myAmJammed && stripe(myPosLat) >= 0 && stripe(myPosLat) <= sMax && xDist >= MIN_STARTUP_DIST) {
2060 78933 : myAmJammed = false;
2061 : }
2062 : // dawdling
2063 65696538 : const double dawdle = MIN2(xSpeed, RandHelper::rand() * vMax * dawdling);
2064 65696538 : xSpeed -= dawdle;
2065 :
2066 : // XXX ensure that diagonal speed <= vMax
2067 : // avoid deadlocks on narrow sidewalks
2068 : //if (oncoming && xSpeed == 0 && myStage->getWaitingTime(currentTime) > TIME2STEPS(ONCOMIN_PATIENCE)) {
2069 : // if DEBUGCOND(*this) std::cout << " stepping aside to resolve oncoming deadlock\n";
2070 : // xSpeed = POSITION_EPS; // reset myWaitingTime
2071 : // if (myDir == FORWARD && chosen < sMax) {
2072 : // chosen += 1;
2073 : // } else if (myDir == BACKWARD && chosen > 0) {
2074 : // chosen -= 1;
2075 : // }
2076 : //}
2077 65696538 : const double maxYSpeed = MIN2(MAX2(vMax * LATERAL_SPEED_FACTOR, vMax - xSpeed), stripeWidth);
2078 : double ySpeed = 0;
2079 : double yDist = 0;
2080 65696538 : if (utility[next] > OBSTRUCTION_THRESHOLD && utility[chosen] > OBSTRUCTION_THRESHOLD) {
2081 : // don't move laterally if the stripes are blocked
2082 58968318 : yDist = (chosen * stripeWidth) - myPosLat;
2083 58968318 : if (fabs(yDist) > NUMERICAL_EPS) {
2084 2685148 : ySpeed = (yDist > 0 ?
2085 1232532 : MIN2(maxYSpeed, DIST2SPEED(yDist)) :
2086 1452616 : MAX2(-maxYSpeed, DIST2SPEED(yDist)));
2087 : }
2088 6728220 : } else if (utility[next] <= OBSTRUCTION_THRESHOLD && obs[next].type == OBSTACLE_VEHICLE
2089 : // still on the road
2090 10585 : && stripe() == stripe(myPosLat)
2091 : // only when the vehicle is moving on the same lane
2092 6734061 : && !(myLane->getEdge().isCrossing() || myLane->getEdge().isWalkingArea())) {
2093 : // step aside to let the vehicle pass
2094 3457 : int stepAsideDir = myDir;
2095 3457 : if (myLane->getEdge().getLanes().size() > 1 || current > sMax / 2) {
2096 : // always step to the right on multi-lane edges or when closer to
2097 : // the right side
2098 : stepAsideDir = FORWARD;
2099 : }
2100 3457 : myAmJammed = true; // ignore pedestrian-pedestrian collisions
2101 3457 : ySpeed = stepAsideDir * vMax;
2102 : }
2103 :
2104 : // DEBUG
2105 65696538 : if DEBUGCOND(*this) {
2106 0 : std::cout << SIMTIME
2107 0 : << " ped=" << myPerson->getID()
2108 0 : << " edge=" << myStage->getEdge()->getID()
2109 0 : << " x=" << myEdgePos
2110 0 : << " y=" << myPosLat
2111 : << " d=" << myDir
2112 0 : << " pvx=" << mySpeed
2113 : << " cur=" << current
2114 0 : << " cho=" << chosen
2115 0 : << " oth=" << other
2116 0 : << " nxt=" << next
2117 0 : << " vx=" << xSpeed
2118 : << " dawdle=" << dawdle
2119 : << " vy=" << ySpeed
2120 : << " xd=" << xDist
2121 : << " yd=" << yDist
2122 : << " vMax=" << vMax
2123 0 : << " wTime=" << myStage->getWaitingTime()
2124 0 : << " jammed=" << myAmJammed
2125 0 : << "\n";
2126 0 : if (DEBUGCOND(*this)) {
2127 0 : for (int i = 0; i < stripes; ++i) {
2128 0 : const Obstacle& o = obs[i];
2129 0 : std::cout << " util=" << utility[i] << " dist=" << distance[i] << " o=" << o.description;
2130 0 : if (o.description != "") {
2131 0 : std::cout << " xF=" << o.xFwd << " xB=" << o.xBack << " v=" << o.speed;
2132 : }
2133 0 : if (i == current) {
2134 0 : std::cout << " current";
2135 : }
2136 0 : if (i == other && i != current) {
2137 0 : std::cout << " other";
2138 : }
2139 0 : if (i == chosen) {
2140 0 : std::cout << " chosen";
2141 : }
2142 0 : if (i == next) {
2143 0 : std::cout << " next";
2144 : }
2145 0 : std::cout << "\n";
2146 : }
2147 : }
2148 : }
2149 65696538 : myEdgePos += SPEED2DIST(xSpeed * myDir);
2150 65696538 : myPosLat += SPEED2DIST(ySpeed);
2151 65696538 : mySpeedLat = ySpeed;
2152 65696538 : mySpeed = xSpeed;
2153 65696538 : if (xSpeed >= SUMO_const_haltingSpeed) {
2154 33457812 : myWaitingToEnter = false;
2155 33457812 : myWaitingTime = 0;
2156 : } else {
2157 32238726 : myWaitingTime += DELTA_T;
2158 32238726 : myTotalWaitingTime += DELTA_T;
2159 : }
2160 65696538 : myAngle = std::numeric_limits<double>::max(); // set on first access or via remote control
2161 65696538 : }
2162 :
2163 :
2164 : double
2165 11258066 : MSPModel_Striping::PState::getImpatience() const {
2166 11258066 : return MAX2(0., MIN2(1., myPerson->getVehicleType().getImpatience()
2167 11258066 : + STEPS2TIME(myStage->getWaitingTime()) / MAX_WAIT_TOLERANCE));
2168 : }
2169 :
2170 :
2171 : Position
2172 10420211 : MSPModel_Striping::PState::getPosition(const MSStageMoving& stage, SUMOTime) const {
2173 : if (myRemoteXYPos != Position::INVALID) {
2174 6293 : return myRemoteXYPos;
2175 : }
2176 10413918 : if (myLane == nullptr) {
2177 : // pedestrian has already finished
2178 223 : return Position::INVALID;
2179 : }
2180 10413695 : const double lateral_offset = -getLatOffset(); // the minus is hunting me in my dreams but seems to be here for historical reasons
2181 10413695 : if (myWalkingAreaPath == nullptr) {
2182 1016932 : return stage.getLanePosition(myLane, myEdgePos, lateral_offset);
2183 : } else {
2184 : //if DEBUGCOND(*this) {
2185 : // std::cout << SIMTIME
2186 : // << " getPosition (walkingArea)"
2187 : // << " p=" << myPerson->getID()
2188 : // << " x=" << myEdgePos
2189 : // << " y=" << myPosLat
2190 : // << " latOffset=" << lateral_offset
2191 : // << " shape=" << myWalkingAreaPath->shape
2192 : // << " pos=" << myWalkingAreaPath->shape.positionAtOffset(myEdgePos, lateral_offset)
2193 : // << "\n";
2194 : //}
2195 9396763 : if (myWalkingAreaPath->angleOverride == INVALID_DOUBLE) {
2196 9209857 : return myWalkingAreaPath->shape.positionAtOffset(myEdgePos, lateral_offset);
2197 : } else {
2198 186906 : const double rotationOffset = myDir == FORWARD ? 0 : DEG2RAD(180);
2199 186906 : return myWalkingAreaPath->shape.sidePositionAtAngle(myEdgePos, lateral_offset, myWalkingAreaPath->angleOverride + rotationOffset);
2200 : }
2201 : }
2202 : }
2203 :
2204 :
2205 : double
2206 2709258 : MSPModel_Striping::PState::getAngle(const MSStageMoving&, SUMOTime) const {
2207 2709258 : if (myAngle != std::numeric_limits<double>::max()) {
2208 : return myAngle;
2209 : }
2210 825541 : if (myLane == nullptr) {
2211 : // pedestrian has already finished
2212 : return 0;
2213 : }
2214 825318 : if (myWalkingAreaPath != nullptr && myWalkingAreaPath->angleOverride != INVALID_DOUBLE) {
2215 : return myWalkingAreaPath->angleOverride;
2216 : }
2217 775673 : const PositionVector& shp = myWalkingAreaPath == nullptr ? myLane->getShape() : myWalkingAreaPath->shape;
2218 775673 : double geomX = myWalkingAreaPath == nullptr ? myLane->interpolateLanePosToGeometryPos(myEdgePos) : myEdgePos;
2219 775673 : double angle = shp.rotationAtOffset(geomX) + (myDir == MSPModel::BACKWARD ? M_PI : 0);
2220 775673 : if (myDir == MSPModel::BACKWARD) {
2221 238551 : angle += atan2(mySpeedLat, MAX2(mySpeed, NUMERICAL_EPS));
2222 : } else { // myDir == MSPModel::FORWARD
2223 679820 : angle -= atan2(mySpeedLat, MAX2(mySpeed, NUMERICAL_EPS));
2224 : }
2225 775673 : if (angle > M_PI) {
2226 171716 : angle -= 2 * M_PI;
2227 : }
2228 775673 : myAngle = angle;
2229 775673 : return angle;
2230 : }
2231 :
2232 :
2233 : const MSEdge*
2234 166320 : MSPModel_Striping::PState::getNextEdge(const MSStageMoving&) const {
2235 166320 : return myNLI.lane == nullptr ? nullptr : &myNLI.lane->getEdge();
2236 : }
2237 :
2238 :
2239 : const MSLane*
2240 1688 : MSPModel_Striping::PState::getNextCrossing() const {
2241 1688 : return myNLI.lane != nullptr && myNLI.lane->isCrossing() ? myNLI.lane : nullptr;
2242 : }
2243 :
2244 :
2245 : void
2246 3425 : MSPModel_Striping::PState::reverse(const double pathLength, const double usableWidth) {
2247 3425 : myEdgePos = pathLength - myEdgePos;
2248 3425 : myPosLat = usableWidth - myPosLat;
2249 3425 : myDir = -myWalkingAreaPath->dir;
2250 3425 : mySpeedLat = -mySpeedLat;
2251 3425 : }
2252 :
2253 :
2254 : void
2255 243636 : MSPModel_Striping::PState::reset(const double edgePos, const double latPos) {
2256 243636 : myEdgePos = edgePos;
2257 243636 : myPosLat = latPos;
2258 243636 : myDir = UNDEFINED_DIRECTION; // only an obstacle, speed may be orthogonal to dir
2259 243636 : mySpeed = 0.;
2260 243636 : mySpeedLat = 0.;
2261 243636 : }
2262 :
2263 :
2264 : void
2265 18 : MSPModel_Striping::PState::moveTo(MSPerson* p, MSLane* lane, double lanePos, double lanePosLat, SUMOTime t) {
2266 : ConstMSEdgeVector newEdges; // keep route
2267 : int routeOffset = 0;
2268 : bool laneOnRoute = false;
2269 18 : const MSJunction* laneOnJunction = lane->isNormal() ? nullptr : lane->getEdge().getToJunction();
2270 18 : for (const MSEdge* edge : myStage->getRoute()) {
2271 : if (edge == &lane->getEdge()
2272 14 : || edge->getToJunction() == laneOnJunction
2273 18 : || edge->getFromJunction() == laneOnJunction) {
2274 : laneOnRoute = true;
2275 : break;
2276 : }
2277 0 : routeOffset++;
2278 : }
2279 18 : if (!laneOnRoute) {
2280 0 : throw ProcessError("Lane '" + lane->getID() + "' is not on the route of person '" + getID() + "'.");
2281 : }
2282 18 : Position pos = lane->geometryPositionAtOffset(lanePos, lanePosLat);
2283 18 : if (lane->getEdge().isWalkingArea() && (myWalkingAreaPath == nullptr || myWalkingAreaPath->lane != lane)) {
2284 : // entered new walkingarea. Determine path to guess position
2285 10 : const MSEdge* prevEdge = myStage->getRoute()[routeOffset];
2286 10 : const MSEdge* nextEdge = routeOffset + 1 < (int)myStage->getRoute().size() ? myStage->getRoute()[routeOffset + 1] : nullptr;
2287 10 : const WalkingAreaPath* guessed = guessPath(&lane->getEdge(), prevEdge, nextEdge);
2288 10 : const double maxPos = guessed->shape.length() - NUMERICAL_EPS;
2289 10 : if (lanePos > maxPos + POSITION_EPS || lanePos < -POSITION_EPS) {
2290 0 : throw ProcessError("Lane position " + toString(lanePos) + " cannot be mapped onto walkingarea '" + lane->getID()
2291 0 : + "' (fromLane='" + guessed->from->getID()
2292 0 : + "' toLane='" + guessed->to->getID() + "') for person '" + getID() + "' time=" + time2string(t) + ".");
2293 : }
2294 : // give some slack
2295 10 : lanePos = MIN2(maxPos, MAX2(NUMERICAL_EPS, lanePos));
2296 10 : pos = guessed->shape.positionAtOffset(lanePos, lanePosLat);
2297 : }
2298 18 : const double angle = GeomHelper::naviDegree(p->getPosition().angleTo2D(pos));
2299 18 : moveToXY(p, pos, lane, lanePos, lanePosLat, angle, routeOffset, newEdges, t);
2300 18 : }
2301 :
2302 :
2303 : void
2304 14094 : MSPModel_Striping::PState::moveToXY(MSPerson* p, Position pos, MSLane* lane, double lanePos,
2305 : double lanePosLat, double angle, int routeOffset,
2306 : const ConstMSEdgeVector& edges, SUMOTime t) {
2307 14094 : MSPModel_Striping* pm = dynamic_cast<MSPModel_Striping*>(MSNet::getInstance()->getPersonControl().getMovementModel());
2308 : assert(p == myPerson);
2309 : assert(pm != nullptr);
2310 14094 : const double oldAngle = GeomHelper::naviDegree(getAngle(*myStage, t));
2311 : // person already walking in this step. undo this to obtain the previous position
2312 14094 : const double oldX = myEdgePos - SPEED2DIST(mySpeed * myDir);
2313 : const double tmp = myEdgePos;
2314 14094 : myEdgePos = oldX;
2315 14094 : Position oldPos = getPosition(*myStage, t);
2316 14094 : myEdgePos = tmp;
2317 : //if (oldPos == Position::INVALID) {
2318 : // oldPos = pos
2319 : //}
2320 14094 : myAngle = GeomHelper::fromNaviDegree(angle);
2321 : #ifdef DEBUG_MOVETOXY
2322 : std::cout << SIMTIME << " ped=" << p->getID()
2323 : << " moveToXY"
2324 : << " pos=" << pos
2325 : << " lane=" << lane->getID()
2326 : << " lanePos=" << lanePos
2327 : << " lanePosLat=" << lanePosLat
2328 : << " angle=" << angle
2329 : << " routeOffset=" << routeOffset
2330 : << " edges=" << toString(edges)
2331 : << " oldLane=" << Named::getIDSecure(myLane)
2332 : << " path=" << (myWalkingAreaPath == nullptr ? "null" : (myWalkingAreaPath->from->getID() + "->" + myWalkingAreaPath->to->getID())) << "\n";
2333 : #endif
2334 :
2335 14094 : if (lane != myLane && myLane != nullptr && lane != nullptr) {
2336 355 : pm->remove(this);
2337 : pm->registerActive();
2338 : }
2339 14094 : if (lane != nullptr &&
2340 10962 : fabs(lanePosLat) < (0.5 * (lane->getWidth() + p->getVehicleType().getWidth()) + SIDEWALK_OFFSET)) {
2341 10957 : myRemoteXYPos = Position::INVALID;
2342 10957 : const MSEdge* old = myStage->getEdge();
2343 10957 : const MSLane* oldLane = myLane;
2344 10957 : if (lane != myLane) {
2345 : // implicitly adds new active lane if necessary
2346 355 : pm->myActiveLanes[lane].push_back(this);
2347 : }
2348 10957 : if (edges.empty()) {
2349 : // map within route
2350 10908 : myStage->setRouteIndex(myPerson, routeOffset);
2351 : } else {
2352 49 : myStage->replaceRoute(myPerson, edges, routeOffset);
2353 : }
2354 10957 : if (!lane->getEdge().isNormal()) {
2355 524 : myStage->moveToNextEdge(myPerson, t, myDir, &lane->getEdge());
2356 : }
2357 :
2358 10957 : myLane = lane;
2359 10957 : const double lateral_offset = (lane->getWidth() - stripeWidth) * 0.5;
2360 10957 : if (lane->getEdge().isWalkingArea()) {
2361 255 : if (myWalkingAreaPath == nullptr || myWalkingAreaPath->lane != lane) {
2362 : // entered new walkingarea. Determine path
2363 55 : myWalkingAreaPath = guessPath(&lane->getEdge(), old, myStage->getNextRouteEdge());
2364 : #ifdef DEBUG_MOVETOXY
2365 : std::cout << " guessPath old=" << old->getID() << " next=" << Named::getIDSecure(myStage->getNextRouteEdge())
2366 : << " path=" << myWalkingAreaPath->from->getID() << "->" << myWalkingAreaPath->to->getID() << "\n";
2367 : #endif
2368 : }
2369 : // lanePos and lanePosLat are matched onto the circumference of the
2370 : // walkingarea. Use pos instead
2371 255 : const Position relPos = myWalkingAreaPath->shape.transformToVectorCoordinates(pos);
2372 : if (relPos == Position::INVALID) {
2373 0 : WRITE_WARNING("Could not map position " + toString(pos) + " onto lane '" + myLane->getID()
2374 : + "' (fromLane='" + myWalkingAreaPath->from->getID()
2375 : + "' toLane='" + myWalkingAreaPath->to->getID() + "') for person '" + getID() + "' time=" + time2string(t) + ".");
2376 0 : myRemoteXYPos = pos;
2377 : } else {
2378 255 : myEdgePos = relPos.x();
2379 255 : myPosLat = lateral_offset + relPos.y();
2380 : }
2381 : } else {
2382 10702 : myWalkingAreaPath = nullptr;
2383 10702 : myEdgePos = lanePos;
2384 10702 : myPosLat = lateral_offset - lanePosLat;
2385 : lane->requireCollisionCheck();
2386 : }
2387 : // guess direction
2388 10957 : const double angleDiff = GeomHelper::getMinAngleDiff(angle, oldAngle);
2389 10957 : if (myStage->getNextRouteEdge() != nullptr) {
2390 6498 : if (myStage->getEdge()->getToJunction() == myStage->getNextRouteEdge()->getFromJunction() ||
2391 939 : myStage->getEdge()->getToJunction() == myStage->getNextRouteEdge()->getToJunction()) {
2392 4730 : myDir = FORWARD;
2393 : } else {
2394 829 : myDir = BACKWARD;
2395 : }
2396 : } else {
2397 : // guess from angle
2398 5398 : if (angleDiff <= 90) {
2399 : // keep direction
2400 5334 : if (myDir == UNDEFINED_DIRECTION) {
2401 0 : myDir = FORWARD;
2402 : }
2403 : } else {
2404 : // change direction
2405 64 : myDir = (myDir == BACKWARD) ? FORWARD : BACKWARD;
2406 : }
2407 : }
2408 : // update next lane info (after guessing direction)
2409 10957 : if (oldLane == nullptr || &oldLane->getEdge() != &myLane->getEdge()) {
2410 313 : const MSLane* sidewalk = getSidewalk<MSEdge, MSLane>(&myLane->getEdge(), p->getVClass());
2411 : // assume that we will eventually move back onto the sidewalk if
2412 : // there is one
2413 313 : myNLI = getNextLane(*this, sidewalk == nullptr ? myLane : sidewalk, nullptr);
2414 313 : myStage->activateEntryReminders(myPerson);
2415 : #ifdef DEBUG_MOVETOXY
2416 : std::cout << " myNLI=" << Named::getIDSecure(myNLI.lane) << " link=" << (myNLI.link == nullptr ? "NULL" : myNLI.link->getDescription()) << " dir=" << myNLI.dir << "\n";
2417 : #endif
2418 : }
2419 : #ifdef DEBUG_MOVETOXY
2420 : std::cout << " newRelPos=" << Position(myEdgePos, myPosLat) << " edge=" << myPerson->getEdge()->getID()
2421 : << " newPos=" << myPerson->getPosition()
2422 : << " latOffset=" << getLatOffset()
2423 : << " oldAngle=" << oldAngle << " angleDiff=" << angleDiff << " newDir=" << myDir << "\n";
2424 : #endif
2425 10957 : if (oldLane == myLane) {
2426 10602 : mySpeed = DIST2SPEED(fabs(oldX - myEdgePos));
2427 : } else {
2428 : //std::cout << SIMTIME << " oldX=" << oldX << " oldSpeed=" << mySpeed << " oldPos=" << oldPos << " pos=" << pos << " dist=" << oldPos.distanceTo2D(pos) << " oldLane=" << Named::getIDSecure(oldLane) << " lane=" << lane->getID() << "\n";
2429 355 : mySpeed = DIST2SPEED(oldPos.distanceTo2D(pos));
2430 : }
2431 : } else {
2432 : // map outside the network
2433 3137 : myRemoteXYPos = pos;
2434 3137 : mySpeed = DIST2SPEED(oldPos.distanceTo2D(pos));
2435 : }
2436 :
2437 14094 : }
2438 :
2439 :
2440 : double
2441 731245 : MSPModel_Striping::PState::getPathLength() const {
2442 731245 : if (myWalkingAreaPath != nullptr) {
2443 673162 : return myWalkingAreaPath->length;
2444 : } else {
2445 : return 0;
2446 : }
2447 : }
2448 :
2449 : double
2450 1452864030 : MSPModel_Striping::PState::distanceTo(const Obstacle& obs, const bool includeMinGap) const {
2451 : // check for overlap
2452 1452864030 : const double maxX = getMaxX(includeMinGap);
2453 1452864030 : const double minX = getMinX(includeMinGap);
2454 : //if (DEBUGCOND(*this)) {
2455 : // std::cout << std::setprecision(2) << " distanceTo=" << obs.description << " maxX=" << maxX << " minX=" << minX << " obs.xFwd=" << obs.xFwd << " obs.xBack=" << obs.xBack << "\n";
2456 : //}
2457 1452864030 : if ((obs.xFwd >= maxX && obs.xBack <= maxX) || (obs.xFwd <= maxX && obs.xFwd >= minX)) {
2458 : // avoid blocking by itself on looped route
2459 409605148 : return (obs.type == OBSTACLE_PED && obs.description == myPerson->getID()) ? DIST_FAR_AWAY : DIST_OVERLAP;
2460 : }
2461 1043258882 : if (myDir == FORWARD) {
2462 933469983 : return obs.xFwd < minX ? DIST_BEHIND : obs.xBack - maxX;
2463 : } else {
2464 109788899 : return obs.xBack > maxX ? DIST_BEHIND : minX - obs.xFwd;
2465 : }
2466 : }
2467 :
2468 :
2469 : void
2470 84748457 : MSPModel_Striping::PState::mergeObstacles(Obstacles& into, const Obstacles& obs2) {
2471 373679379 : for (int i = 0; i < (int)into.size(); ++i) {
2472 288930922 : if (gDebugFlag1) {
2473 0 : std::cout << " i=" << i << " maxX=" << getMaxX(true) << " minX=" << getMinX(true)
2474 0 : << " into=" << into[i].description << " iDist=" << distanceTo(into[i], into[i].type == OBSTACLE_PED)
2475 0 : << " obs2=" << obs2[i].description << " oDist=" << distanceTo(obs2[i], obs2[i].type == OBSTACLE_PED) << "\n";
2476 : }
2477 288930922 : const double dO = distanceTo(obs2[i], obs2[i].type == OBSTACLE_PED);
2478 288930922 : const double dI = distanceTo(into[i], into[i].type == OBSTACLE_PED);
2479 288930922 : if (dO < dI) {
2480 : into[i] = obs2[i];
2481 : } else if (dO == dI
2482 71307550 : && into[i].type != OBSTACLE_PED
2483 33781685 : && into[i].type != OBSTACLE_VEHICLE
2484 278061630 : && (obs2[i].type == OBSTACLE_PED ||
2485 : obs2[i].type == OBSTACLE_VEHICLE)) {
2486 : into[i] = obs2[i];
2487 : }
2488 : }
2489 84748457 : }
2490 :
2491 : void
2492 18622 : MSPModel_Striping::PState::mergeObstacles(Obstacles& into, const Obstacles& obs2, int dir, int offset) {
2493 146338 : for (int i = 0; i < (int)into.size(); ++i) {
2494 127716 : int i2 = i + offset;
2495 127716 : if (i2 >= 0 && i2 < (int)obs2.size()) {
2496 126440 : if (dir == FORWARD) {
2497 91522 : if (obs2[i2].xBack < into[i].xBack) {
2498 : into[i] = obs2[i2];
2499 : }
2500 : } else {
2501 34918 : if (obs2[i2].xFwd > into[i].xFwd) {
2502 : into[i] = obs2[i2];
2503 : }
2504 : }
2505 : }
2506 : }
2507 18622 : }
2508 :
2509 :
2510 : bool
2511 11258066 : MSPModel_Striping::PState::ignoreRed(const MSLink* link) const {
2512 11258066 : if (link->haveRed()) {
2513 648950 : const double ignoreRedTime = myPerson->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_AFTER_RED_TIME, -1);
2514 648950 : if (ignoreRedTime >= 0) {
2515 40 : const double redDuration = STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - link->getLastStateChange());
2516 40 : if (DEBUGCOND(*this)) {
2517 0 : std::cout << SIMTIME << " ignoreRedTime=" << ignoreRedTime << " redDuration=" << redDuration << "\n";
2518 : }
2519 40 : return ignoreRedTime > redDuration;
2520 : } else {
2521 : return false;
2522 : }
2523 : } else {
2524 : return false;
2525 : }
2526 : }
2527 :
2528 :
2529 : bool
2530 10564135 : MSPModel_Striping::PState::stopForYellow(const MSLink* link) const {
2531 : // main use case is at rail_crossing
2532 10564135 : if (link->haveYellow()) {
2533 782 : const double ignoreYellowTime = myPerson->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_AFTER_YELLOW_TIME, -1);
2534 782 : if (ignoreYellowTime >= 0) {
2535 0 : const double yellowDuration = STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - link->getLastStateChange());
2536 0 : if (DEBUGCOND(*this)) {
2537 0 : std::cout << SIMTIME << " ignoreYellowTime=" << ignoreYellowTime << " yellowDuration=" << yellowDuration << "\n";
2538 : }
2539 0 : return ignoreYellowTime < yellowDuration;
2540 : } else {
2541 : return true;
2542 : }
2543 : } else {
2544 : return false;
2545 : }
2546 : }
2547 :
2548 : double
2549 3235341582 : MSPModel_Striping::PState::getWidth() const {
2550 3235341582 : return myPerson->getVehicleType().getWidth();
2551 : }
2552 :
2553 :
2554 : bool
2555 124843522 : MSPModel_Striping::PState::isRemoteControlled() const {
2556 124843522 : return myPerson->hasInfluencer() && myPerson->getInfluencer().isRemoteControlled();
2557 : }
2558 :
2559 : // ===========================================================================
2560 : // MSPModel_Striping::PStateVehicle method definitions
2561 : // ===========================================================================
2562 :
2563 495398 : MSPModel_Striping::PStateVehicle::PStateVehicle(const MSVehicle* veh, const MSLane* walkingarea, double relX, double relY, double xWidth, double yWidth):
2564 495398 : myVehicle(veh), myXWidth(xWidth), myYWidth(yWidth) {
2565 495398 : myLane = walkingarea; // to ensure correct limits when calling otherStripe()
2566 : // relX is the center but we want it to be the max value if the movement direction is forward
2567 : // and the min value otherwise (indicated by xWidth sign)
2568 495398 : myEdgePos = relX + xWidth / 2;
2569 495398 : myPosLat = relY;
2570 495398 : }
2571 :
2572 : const std::string&
2573 1933972 : MSPModel_Striping::PStateVehicle::getID() const {
2574 1933972 : return myVehicle->getID();
2575 : }
2576 :
2577 : double
2578 999830 : MSPModel_Striping::PStateVehicle::getWidth() const {
2579 999830 : return myYWidth;
2580 : }
2581 :
2582 : double
2583 529902 : MSPModel_Striping::PStateVehicle::getMinX(const bool /*includeMinGap*/) const {
2584 529902 : return myXWidth > 0 ? myEdgePos - myXWidth : myEdgePos;
2585 : }
2586 :
2587 : double
2588 529902 : MSPModel_Striping::PStateVehicle::getMaxX(const bool /*includeMinGap*/) const {
2589 529902 : return myXWidth > 0 ? myEdgePos : myEdgePos - myXWidth;
2590 : }
2591 :
2592 : // ===========================================================================
2593 : // MSPModel_Striping::MovePedestrians method definitions
2594 : // ===========================================================================
2595 :
2596 : SUMOTime
2597 9312955 : MSPModel_Striping::MovePedestrians::execute(SUMOTime currentTime) {
2598 : std::set<MSPerson*> changedLane;
2599 9312955 : myModel->moveInDirection(currentTime, changedLane, FORWARD);
2600 9312951 : myModel->moveInDirection(currentTime, changedLane, BACKWARD);
2601 : // DEBUG
2602 : #ifdef LOG_ALL
2603 : for (ActiveLanes::const_iterator it_lane = myModel->getActiveLanes().begin(); it_lane != myModel->getActiveLanes().end(); ++it_lane) {
2604 : const MSLane* lane = it_lane->first;
2605 : Pedestrians pedestrians = it_lane->second;
2606 : if (pedestrians.size() == 0) {
2607 : continue;
2608 : }
2609 : sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(FORWARD));
2610 : std::cout << SIMTIME << " lane=" << lane->getID();
2611 : for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
2612 : const PState& p = *pedestrians[ii];
2613 : std::cout << " (" << p.getPerson()->getID() << " " << p.myEdgePos << "," << p.myPosLat << " " << p.getDirection() << ")";
2614 : }
2615 : std::cout << "\n";
2616 : }
2617 : #endif
2618 9312951 : return DELTA_T;
2619 : }
|
