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