1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2014-2019 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials
5 // are made available under the terms of the Eclipse Public License v2.0
6 // which accompanies this distribution, and is available at
7 // http://www.eclipse.org/legal/epl-v20.html
8 // SPDX-License-Identifier: EPL-2.0
9 /****************************************************************************/
10 /// @file MSPModel_Striping.cpp
11 /// @author Jakob Erdmann
12 /// @author Michael Behrisch
13 /// @date Mon, 13 Jan 2014
14 /// @version $Id$
15 ///
16 // The pedestrian following model (prototype)
17 /****************************************************************************/
18
19 // ===========================================================================
20 // included modules
21 // ===========================================================================
22 #include <config.h>
23
24 #include <cmath>
25 #include <algorithm>
26 #include <utils/common/RandHelper.h>
27 #include <utils/geom/GeomHelper.h>
28 #include <utils/options/OptionsCont.h>
29 #include <utils/router/PedestrianRouter.h>
30 #include <microsim/MSNet.h>
31 #include <microsim/MSEdge.h>
32 #include <microsim/MSEventControl.h>
33 #include <microsim/MSLane.h>
34 #include <microsim/MSJunction.h>
35 #include <microsim/MSGlobals.h>
36 #include <microsim/MSTransportableControl.h>
37 #include "MSPModel_Striping.h"
38
39
40 // ===========================================================================
41 // DEBUGGING HELPERS
42 // ===========================================================================
43 //
44 #define DEBUGID1 ""
45 #define DEBUGID2 ""
46 //#define DEBUGCOND(PED) (false)
47 //#define DEBUGCOND(PED) ((PED).myPerson->getID() == DEBUGID1 || (PED).myPerson->getID() == DEBUGID2)
48 #define DEBUGCOND(PED) ((PED).myPerson->isSelected())
49 #define DEBUGCOND2(LANE) ((LANE)->isSelected())
50 //#define LOG_ALL 1
51
DEBUG_PRINT(const Obstacles & obs)52 void MSPModel_Striping::DEBUG_PRINT(const Obstacles& obs) {
53 for (int i = 0; i < (int)obs.size(); ++i) {
54 std::cout
55 << "(" << obs[i].description
56 << " x=(" << obs[i].xBack << "," << obs[i].xFwd
57 << ") s=" << obs[i].speed
58 << ") ";
59 }
60 std::cout << "\n";
61 }
62
63 // ===========================================================================
64 // named (internal) constants
65 // ===========================================================================
66
67 // distances are comparable with lower values being "more important"
68 const double MSPModel_Striping::DIST_FAR_AWAY(10000);
69 const double MSPModel_Striping::DIST_BEHIND(1000);
70 const double MSPModel_Striping::DIST_OVERLAP(-1);
71
72 // ===========================================================================
73 // static members
74 // ===========================================================================
75
76 MSPModel_Striping::WalkingAreaPaths MSPModel_Striping::myWalkingAreaPaths;
77 std::map<const MSEdge*, std::vector<const MSLane*> > MSPModel_Striping::myWalkingAreaFoes;
78 MSPModel_Striping::MinNextLengths MSPModel_Striping::myMinNextLengths;
79 MSPModel_Striping::Pedestrians MSPModel_Striping::noPedestrians;
80
81 // model parameters (static to simplify access from class PState
82 double MSPModel_Striping::stripeWidth;
83 double MSPModel_Striping::dawdling;
84 SUMOTime MSPModel_Striping::jamTime;
85 const double MSPModel_Striping::LOOKAHEAD_SAMEDIR(4.0); // seconds
86 const double MSPModel_Striping::LOOKAHEAD_ONCOMING(10.0); // seconds
87 const double MSPModel_Striping::LOOKAROUND_VEHICLES(60.0); // meters
88 const double MSPModel_Striping::LATERAL_PENALTY(-1.); // meters
89 const double MSPModel_Striping::OBSTRUCTED_PENALTY(-300000.); // meters
90 const double MSPModel_Striping::INAPPROPRIATE_PENALTY(-20000.); // meters
91 const double MSPModel_Striping::ONCOMING_CONFLICT_PENALTY(-1000.); // meters
92 const double MSPModel_Striping::OBSTRUCTION_THRESHOLD(MSPModel_Striping::OBSTRUCTED_PENALTY * 0.5); // despite obstruction, additional utility may have been added
93 const double MSPModel_Striping::SQUEEZE(0.7);
94 const double MSPModel_Striping::RESERVE_FOR_ONCOMING_FACTOR(0.0);
95 const double MSPModel_Striping::RESERVE_FOR_ONCOMING_FACTOR_JUNCTIONS(0.34);
96 const double MSPModel_Striping::MAX_WAIT_TOLERANCE(120.); // seconds
97 const double MSPModel_Striping::LATERAL_SPEED_FACTOR(0.4);
98 const double MSPModel_Striping::MIN_STARTUP_DIST(0.4); // meters
99
100 #define MINGAP_TO_VEHICLE 0.25
101
102
103 // ===========================================================================
104 // MSPModel_Striping method definitions
105 // ===========================================================================
106
MSPModel_Striping(const OptionsCont & oc,MSNet * net)107 MSPModel_Striping::MSPModel_Striping(const OptionsCont& oc, MSNet* net) :
108 myNumActivePedestrians(0),
109 myAmActive(false) {
110 initWalkingAreaPaths(net);
111 // configurable parameters
112 stripeWidth = oc.getFloat("pedestrian.striping.stripe-width");
113 dawdling = oc.getFloat("pedestrian.striping.dawdling");
114
115 jamTime = string2time(oc.getString("pedestrian.striping.jamtime"));
116 if (jamTime <= 0) {
117 jamTime = SUMOTime_MAX;
118 }
119 }
120
121
~MSPModel_Striping()122 MSPModel_Striping::~MSPModel_Striping() {
123 }
124
125
126 PedestrianState*
add(MSPerson * person,MSPerson::MSPersonStage_Walking * stage,SUMOTime)127 MSPModel_Striping::add(MSPerson* person, MSPerson::MSPersonStage_Walking* stage, SUMOTime) {
128 MSNet* net = MSNet::getInstance();
129 if (!myAmActive) {
130 net->getBeginOfTimestepEvents()->addEvent(new MovePedestrians(this), net->getCurrentTimeStep() + DELTA_T);
131 myAmActive = true;
132 }
133 assert(person->getCurrentStageType() == MSTransportable::MOVING_WITHOUT_VEHICLE);
134 const MSLane* lane = getSidewalk<MSEdge, MSLane>(person->getEdge());
135 if (lane == nullptr) {
136 std::string error = "Person '" + person->getID() + "' could not find sidewalk on edge '" + person->getEdge()->getID() + "', time="
137 + time2string(net->getCurrentTimeStep()) + ".";
138 if (OptionsCont::getOptions().getBool("ignore-route-errors")) {
139 WRITE_WARNING(error);
140 return nullptr;
141 } else {
142 throw ProcessError(error);
143 }
144 }
145 PState* ped = new PState(person, stage, lane);
146 myActiveLanes[lane].push_back(ped);
147 myNumActivePedestrians++;
148 return ped;
149 }
150
151
152 void
add(PedestrianState * pState,const MSLane * lane)153 MSPModel_Striping::add(PedestrianState* pState, const MSLane* lane) {
154 PState* ped = dynamic_cast<PState*>(pState);
155 assert(ped != 0);
156 myActiveLanes[lane].push_back(ped);
157 }
158
159
160 void
remove(PedestrianState * state)161 MSPModel_Striping::remove(PedestrianState* state) {
162 const MSLane* lane = dynamic_cast<PState*>(state)->myLane;
163 Pedestrians& pedestrians = myActiveLanes[lane];
164 for (Pedestrians::iterator it = pedestrians.begin(); it != pedestrians.end(); ++it) {
165 if (*it == state) {
166 pedestrians.erase(it);
167 return;
168 }
169 }
170 }
171
172
173 bool
blockedAtDist(const MSLane * lane,double vehSide,double vehWidth,double oncomingGap,std::vector<const MSPerson * > * collectBlockers)174 MSPModel_Striping::blockedAtDist(const MSLane* lane, double vehSide, double vehWidth,
175 double oncomingGap, std::vector<const MSPerson*>* collectBlockers) {
176 const Pedestrians& pedestrians = getPedestrians(lane);
177 for (Pedestrians::const_iterator it_ped = pedestrians.begin(); it_ped != pedestrians.end(); ++it_ped) {
178 const PState& ped = **it_ped;
179 const double leaderFrontDist = (ped.myDir == FORWARD ? vehSide - ped.myRelX : ped.myRelX - vehSide);
180 const double leaderBackDist = leaderFrontDist + ped.getLength();
181 if DEBUGCOND(ped) {
182 std::cout << SIMTIME << " lane=" << lane->getID() << " dir=" << ped.myDir << " pX=" << ped.myRelX << " pL=" << ped.getLength()
183 << " vehSide=" << vehSide
184 << " vehWidth=" << vehWidth
185 << " lBD=" << leaderBackDist
186 << " lFD=" << leaderFrontDist
187 << "\n";
188 }
189 if (leaderBackDist >= -vehWidth
190 && (leaderFrontDist < 0
191 // give right of way to (close) approaching pedestrians unless they are standing
192 || (leaderFrontDist <= oncomingGap && ped.myWaitingTime < TIME2STEPS(2.0)))) {
193 // found one pedestrian that is not completely past the crossing point
194 //std::cout << SIMTIME << " blocking pedestrian foeLane=" << lane->getID() << " ped=" << ped.myPerson->getID() << " dir=" << ped.myDir << " pX=" << ped.myRelX << " pL=" << ped.getLength() << " fDTC=" << distToCrossing << " lBD=" << leaderBackDist << "\n";
195 if (collectBlockers == nullptr) {
196 return true;
197 } else {
198 collectBlockers->push_back(ped.myPerson);
199 }
200 }
201 }
202 if (collectBlockers == nullptr) {
203 return false;
204 } else {
205 return collectBlockers->size() > 0;
206 }
207 }
208
209
210 bool
hasPedestrians(const MSLane * lane)211 MSPModel_Striping::hasPedestrians(const MSLane* lane) {
212 return getPedestrians(lane).size() > 0;
213 }
214
215
216 bool
usingInternalLanes()217 MSPModel_Striping::usingInternalLanes() {
218 return usingInternalLanesStatic();
219 }
220
221 bool
usingInternalLanesStatic()222 MSPModel_Striping::usingInternalLanesStatic() {
223 return MSGlobals::gUsingInternalLanes && MSNet::getInstance()->hasInternalLinks();
224 }
225
226 PersonDist
nextBlocking(const MSLane * lane,double minPos,double minRight,double maxLeft,double stopTime)227 MSPModel_Striping::nextBlocking(const MSLane* lane, double minPos, double minRight, double maxLeft, double stopTime) {
228 PersonDist result((const MSPerson*)nullptr, -1);
229 double closest = std::numeric_limits<double>::max();
230 const Pedestrians& pedestrians = getPedestrians(lane);
231 for (Pedestrians::const_iterator it_ped = pedestrians.begin(); it_ped != pedestrians.end(); ++it_ped) {
232 const PState& ped = **it_ped;
233 // account for distance covered by oncoming pedestrians
234 double relX2 = ped.myRelX - (ped.myDir == FORWARD ? 0 : stopTime * ped.myPerson->getVehicleType().getMaxSpeed());
235 if (ped.myRelX > minPos && (result.first == 0 || closest > relX2)) {
236 const double center = lane->getWidth() - (ped.myRelY + stripeWidth * 0.5);
237 const double halfWidth = 0.5 * ped.myPerson->getVehicleType().getWidth();
238 const bool overlap = (center + halfWidth > minRight && center - halfWidth < maxLeft);
239 if DEBUGCOND(ped) {
240 std::cout << " nextBlocking lane=" << lane->getID()
241 << " minPos=" << minPos << " minRight=" << minRight << " maxLeft=" << maxLeft
242 << " stopTime=" << stopTime
243 << " pedY=" << ped.myRelY
244 << " pedX=" << ped.myRelX
245 << " relX2=" << relX2
246 << " center=" << center
247 << " pedLeft=" << center + halfWidth
248 << " pedRight=" << center - halfWidth
249 << " overlap=" << overlap
250 << "\n";
251 }
252 if (overlap) {
253 closest = relX2;
254 result.first = ped.myPerson;
255 result.second = relX2 - minPos - (ped.myDir == FORWARD ? ped.myPerson->getVehicleType().getLength() : 0);
256 }
257 }
258 }
259 return result;
260 }
261
262
263 MSPModel_Striping::Pedestrians&
getPedestrians(const MSLane * lane)264 MSPModel_Striping::getPedestrians(const MSLane* lane) {
265 ActiveLanes::iterator it = myActiveLanes.find(lane);
266 if (it != myActiveLanes.end()) {
267 //std::cout << " found lane=" << lane->getID() << " n=" << it->second.size() << "\n";
268 return (it->second);
269 } else {
270 return noPedestrians;
271 }
272 }
273
274
275 void
cleanupHelper()276 MSPModel_Striping::cleanupHelper() {
277 myActiveLanes.clear();
278 myNumActivePedestrians = 0;
279 myWalkingAreaPaths.clear(); // need to recompute when lane pointers change
280 myWalkingAreaFoes.clear();
281 myMinNextLengths.clear();
282 }
283
284
285 int
numStripes(const MSLane * lane)286 MSPModel_Striping::numStripes(const MSLane* lane) {
287 return MAX2(1, (int)floor(lane->getWidth() / stripeWidth));
288 }
289
290 int
connectedDirection(const MSLane * from,const MSLane * to)291 MSPModel_Striping::connectedDirection(const MSLane* from, const MSLane* to) {
292 if (from == nullptr || to == nullptr) {
293 return UNDEFINED_DIRECTION;
294 } else if (MSLinkContHelper::getConnectingLink(*from, *to)) {
295 return FORWARD;
296 } else if (MSLinkContHelper::getConnectingLink(*to, *from)) {
297 return BACKWARD;
298 } else {
299 return UNDEFINED_DIRECTION;
300 }
301 }
302
303
304 void
initWalkingAreaPaths(const MSNet *)305 MSPModel_Striping::initWalkingAreaPaths(const MSNet*) {
306 if (myWalkingAreaPaths.size() > 0) {
307 return;
308 }
309 // collect vehicle lanes that cross walkingareas
310 for (MSEdgeVector::const_iterator i = MSEdge::getAllEdges().begin(); i != MSEdge::getAllEdges().end(); ++i) {
311 const MSEdge* edge = *i;
312 if (!edge->isWalkingArea() && !edge->isCrossing()) {
313 for (MSLane* lane : edge->getLanes()) {
314 for (MSLink* link : lane->getLinkCont()) {
315 if (link->getWalkingAreaFoe() != nullptr) {
316 // link is an exit link
317 myWalkingAreaFoes[&link->getWalkingAreaFoe()->getEdge()].push_back(link->getLaneBefore());
318 //std::cout << " wa=" << link->getWalkingAreaFoe()->getEdge().getID() << " foe=" << link->getLaneBefore()->getID() << "\n";
319 }
320 if (link->getWalkingAreaFoeExit() != nullptr) {
321 // link is an exit link
322 myWalkingAreaFoes[&link->getWalkingAreaFoeExit()->getEdge()].push_back(link->getLaneBefore());
323 //std::cout << " wa=" << link->getWalkingAreaFoeExit()->getEdge().getID() << " foe=" << link->getLaneBefore()->getID() << "\n";
324 }
325 }
326 }
327 }
328 }
329
330 // build walkingareaPaths
331 for (MSEdgeVector::const_iterator i = MSEdge::getAllEdges().begin(); i != MSEdge::getAllEdges().end(); ++i) {
332 const MSEdge* edge = *i;
333 if (edge->isWalkingArea()) {
334 const MSLane* walkingArea = getSidewalk<MSEdge, MSLane>(edge);
335 myMinNextLengths[walkingArea] = walkingArea->getLength();
336 // build all possible paths across this walkingArea
337 // gather all incident lanes
338 std::vector<const MSLane*> lanes;
339 const MSEdgeVector& incoming = edge->getPredecessors();
340 for (int j = 0; j < (int)incoming.size(); ++j) {
341 lanes.push_back(getSidewalk<MSEdge, MSLane>(incoming[j]));
342 }
343 const MSEdgeVector& outgoing = edge->getSuccessors();
344 for (int j = 0; j < (int)outgoing.size(); ++j) {
345 lanes.push_back(getSidewalk<MSEdge, MSLane>(outgoing[j]));
346 }
347 // build all combinations
348 for (int j = 0; j < (int)lanes.size(); ++j) {
349 for (int k = 0; k < (int)lanes.size(); ++k) {
350 if (j != k) {
351 // build the walkingArea
352 const MSLane* from = lanes[j];
353 const MSLane* to = lanes[k];
354 const int fromDir = MSLinkContHelper::getConnectingLink(*from, *walkingArea) != nullptr ? FORWARD : BACKWARD;
355 const int toDir = MSLinkContHelper::getConnectingLink(*walkingArea, *to) != nullptr ? FORWARD : BACKWARD;
356 PositionVector shape;
357 Position fromPos = from->getShape()[fromDir == FORWARD ? -1 : 0];
358 Position toPos = to->getShape()[toDir == FORWARD ? 0 : -1];
359 const double maxExtent = fromPos.distanceTo2D(toPos) / 4; // prevent sharp corners
360 const double extrapolateBy = MIN2(maxExtent, walkingArea->getWidth() / 2);
361 // assemble shape
362 shape.push_back(fromPos);
363 if (extrapolateBy > POSITION_EPS) {
364 PositionVector fromShp = from->getShape();
365 fromShp.extrapolate(extrapolateBy);
366 shape.push_back_noDoublePos(fromDir == FORWARD ? fromShp.back() : fromShp.front());
367 PositionVector nextShp = to->getShape();
368 nextShp.extrapolate(extrapolateBy);
369 shape.push_back_noDoublePos(toDir == FORWARD ? nextShp.front() : nextShp.back());
370 }
371 shape.push_back_noDoublePos(toPos);
372 if (shape.size() < 2) {
373 PositionVector fromShp = from->getShape();
374 fromShp.extrapolate(1.5 * POSITION_EPS); // noDoublePos requires a difference of POSITION_EPS in at least one coordinate
375 shape.push_back_noDoublePos(fromDir == FORWARD ? fromShp.back() : fromShp.front());
376 assert(shape.size() == 2);
377 }
378 if (fromDir == BACKWARD) {
379 // will be walking backward on walkingArea
380 shape = shape.reverse();
381 }
382 WalkingAreaPath wap = WalkingAreaPath(from, walkingArea, to, shape);
383 myWalkingAreaPaths[std::make_pair(from, to)] = wap;
384 myMinNextLengths[walkingArea] = MIN2(myMinNextLengths[walkingArea], wap.length);
385 }
386 }
387 }
388 }
389 }
390 }
391
392
393 MSPModel_Striping::WalkingAreaPath*
getArbitraryPath(const MSEdge * walkingArea)394 MSPModel_Striping::getArbitraryPath(const MSEdge* walkingArea) {
395 assert(walkingArea->isWalkingArea());
396 std::vector<const MSLane*> lanes;
397 const MSEdgeVector& incoming = walkingArea->getPredecessors();
398 for (int j = 0; j < (int)incoming.size(); ++j) {
399 lanes.push_back(getSidewalk<MSEdge, MSLane>(incoming[j]));
400 }
401 const MSEdgeVector& outgoing = walkingArea->getSuccessors();
402 for (int j = 0; j < (int)outgoing.size(); ++j) {
403 lanes.push_back(getSidewalk<MSEdge, MSLane>(outgoing[j]));
404 }
405 if (lanes.size() < 1) {
406 throw ProcessError("Invalid walkingarea '" + walkingArea->getID() + "' does not allow continuation.");
407 }
408 return &myWalkingAreaPaths[std::make_pair(lanes.front(), lanes.back())];
409 }
410
411
412 MSPModel_Striping::NextLaneInfo
getNextLane(const PState & ped,const MSLane * currentLane,const MSLane * prevLane)413 MSPModel_Striping::getNextLane(const PState& ped, const MSLane* currentLane, const MSLane* prevLane) {
414 const MSEdge* currentEdge = ¤tLane->getEdge();
415 const MSJunction* junction = ped.myDir == FORWARD ? currentEdge->getToJunction() : currentEdge->getFromJunction();
416 const MSEdge* nextRouteEdge = ped.myStage->getNextRouteEdge();
417 const MSLane* nextRouteLane = getSidewalk<MSEdge, MSLane>(nextRouteEdge);
418 // result values
419 const MSLane* nextLane = nextRouteLane;
420 MSLink* link = nullptr;
421 int nextDir = UNDEFINED_DIRECTION;
422
423 if (nextRouteLane == nullptr && nextRouteEdge != nullptr) {
424 std::string error = "Person '" + ped.myPerson->getID() + "' could not find sidewalk on edge '" + nextRouteEdge->getID() + "', time="
425 + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".";
426 if (OptionsCont::getOptions().getBool("ignore-route-errors")) {
427 WRITE_WARNING(error);
428 nextRouteLane = nextRouteEdge->getLanes().front();
429 } else {
430 throw ProcessError(error);
431 }
432 }
433
434 if (nextRouteLane != nullptr) {
435 if (currentEdge->isInternal()) {
436 assert(junction == currentEdge->getFromJunction());
437 nextDir = junction == nextRouteEdge->getFromJunction() ? FORWARD : BACKWARD;
438 if (nextDir == FORWARD) {
439 nextLane = currentLane->getLinkCont()[0]->getViaLaneOrLane();
440 } else {
441 nextLane = currentLane->getLogicalPredecessorLane();
442 }
443 if DEBUGCOND(ped) {
444 std::cout << " internal\n";
445 }
446 } else if (currentEdge->isCrossing()) {
447 nextDir = ped.myDir;
448 if (ped.myDir == FORWARD) {
449 nextLane = currentLane->getLinkCont()[0]->getLane();
450 } else {
451 nextLane = currentLane->getLogicalPredecessorLane();
452 }
453 if DEBUGCOND(ped) {
454 std::cout << " crossing\n";
455 }
456 } else if (currentEdge->isWalkingArea()) {
457 ConstMSEdgeVector crossingRoute;
458 // departPos can be 0 because the direction of the walkingArea does not matter
459 // for the arrivalPos, we need to make sure that the route does not deviate across other junctions
460 const int nextRouteEdgeDir = nextRouteEdge->getFromJunction() == junction ? FORWARD : BACKWARD;
461 const double arrivalPos = (nextRouteEdge == ped.myStage->getRoute().back()
462 ? ped.myStage->getArrivalPos()
463 : (nextRouteEdgeDir == FORWARD ? 0 : nextRouteEdge->getLength()));
464 MSEdgeVector prohibited;
465 if (prevLane != nullptr) {
466 prohibited.push_back(&prevLane->getEdge());
467 }
468 MSNet::getInstance()->getPedestrianRouter(prohibited).compute(currentEdge, nextRouteEdge, 0, arrivalPos, ped.myStage->getMaxSpeed(ped.myPerson), 0, junction, crossingRoute, true);
469 if DEBUGCOND(ped) {
470 std::cout
471 << " nre=" << nextRouteEdge->getID()
472 << " nreDir=" << nextRouteEdgeDir
473 << " aPos=" << arrivalPos
474 << " crossingRoute=" << toString(crossingRoute)
475 << "\n";
476 }
477 if (crossingRoute.size() > 1) {
478 const MSEdge* nextEdge = crossingRoute[1];
479 nextLane = getSidewalk<MSEdge, MSLane>(crossingRoute[1]);
480 assert((nextEdge->getFromJunction() == junction || nextEdge->getToJunction() == junction));
481 assert(nextLane != prevLane);
482 nextDir = connectedDirection(currentLane, nextLane);
483 if DEBUGCOND(ped) {
484 std::cout << " nextDir=" << nextDir << "\n";
485 }
486 assert(nextDir != UNDEFINED_DIRECTION);
487 if (nextDir == FORWARD) {
488 link = MSLinkContHelper::getConnectingLink(*currentLane, *nextLane);
489 } else {
490 link = MSLinkContHelper::getConnectingLink(*nextLane, *currentLane);
491 if (nextEdge->isCrossing() && link->getTLLogic() == nullptr) {
492 const MSLane* oppositeWalkingArea = nextLane->getLogicalPredecessorLane();
493 link = MSLinkContHelper::getConnectingLink(*oppositeWalkingArea, *nextLane);
494 }
495 }
496 assert(link != 0);
497 } else {
498 if DEBUGCOND(ped) {
499 std::cout << SIMTIME
500 << " no route from '" << (currentEdge == nullptr ? "NULL" : currentEdge->getID())
501 << "' to '" << (nextRouteEdge == nullptr ? "NULL" : nextRouteEdge->getID())
502 << "\n";
503 }
504 WRITE_WARNING("Person '" + ped.myPerson->getID() + "' could not find route across junction '" + junction->getID()
505 + "' from walkingArea '" + currentEdge->getID()
506 + "' to edge '" + nextRouteEdge->getID() + "', time=" +
507 time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
508 // error indicated by nextDir == UNDEFINED_DIRECTION
509 nextLane = nextRouteLane;
510 }
511 } else if (currentEdge == nextRouteEdge) {
512 // strange loop in this route. No need to use walkingArea
513 nextDir = -ped.myDir;
514 } else {
515 // normal edge. by default use next / previous walking area
516 nextDir = ped.myDir;
517 nextLane = getNextWalkingArea(currentLane, ped.myDir, link);
518 if (nextLane != nullptr) {
519 // walking area found
520 if DEBUGCOND(ped) {
521 std::cout << " next walkingArea " << (nextDir == FORWARD ? "forward" : "backward") << "\n";
522 }
523 } else {
524 // walk forward by default
525 nextDir = junction == nextRouteEdge->getToJunction() ? BACKWARD : FORWARD;
526 // try to use a direct link as fallback
527 // direct links only exist if built explicitly. They are used to model tl-controlled links if there are no crossings
528 if (ped.myDir == FORWARD) {
529 link = MSLinkContHelper::getConnectingLink(*currentLane, *nextRouteLane);
530 if (link != nullptr) {
531 if DEBUGCOND(ped) {
532 std::cout << " direct forward\n";
533 }
534 nextLane = MSLinkContHelper::getInternalFollowingLane(currentLane, nextRouteLane);
535 }
536 } else {
537 link = MSLinkContHelper::getConnectingLink(*nextRouteLane, *currentLane);
538 if (link != nullptr) {
539 if DEBUGCOND(ped) {
540 std::cout << " direct backward\n";
541 }
542 nextLane = MSLinkContHelper::getInternalFollowingLane(nextRouteLane, currentLane);
543 if (nextLane != nullptr) {
544 // advance to the end of consecutive internal lanes
545 while (nextLane->getLinkCont()[0]->getViaLaneOrLane()->isInternal()) {
546 nextLane = nextLane->getLinkCont()[0]->getViaLaneOrLane();
547 }
548 }
549 }
550 }
551 }
552 if (nextLane == nullptr) {
553 // no internal lane found
554 nextLane = nextRouteLane;
555 if DEBUGCOND(ped) {
556 std::cout << SIMTIME << " no next lane found for " << currentLane->getID() << " dir=" << ped.myDir << "\n";
557 }
558 if (usingInternalLanesStatic() && currentLane->getLinkCont().size() > 0 && MSNet::getInstance()->hasPedestrianNetwork()) {
559 WRITE_WARNING("Person '" + ped.myPerson->getID() + "' could not find route across junction '" + junction->getID()
560 + "' from edge '" + currentEdge->getID()
561 + "' to edge '" + nextRouteEdge->getID() + "', time=" +
562 time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
563 }
564 } else if (nextLane->getLength() <= POSITION_EPS) {
565 // internal lane too short
566 nextLane = nextRouteLane;
567 }
568 }
569 }
570 if DEBUGCOND(ped) {
571 std::cout << SIMTIME
572 << " p=" << ped.myPerson->getID()
573 << " l=" << currentLane->getID()
574 << " nl=" << (nextLane == nullptr ? "NULL" : nextLane->getID())
575 << " nrl=" << (nextRouteLane == nullptr ? "NULL" : nextRouteLane->getID())
576 << " d=" << nextDir
577 << " link=" << (link == nullptr ? "NULL" : link->getViaLaneOrLane()->getID())
578 << " pedDir=" << ped.myDir
579 << "\n";
580 }
581 assert(nextLane != 0 || nextRouteLane == 0);
582 return NextLaneInfo(nextLane, link, nextDir);
583 }
584
585
586 const MSLane*
getNextWalkingArea(const MSLane * currentLane,const int dir,MSLink * & link)587 MSPModel_Striping::getNextWalkingArea(const MSLane* currentLane, const int dir, MSLink*& link) {
588 if (dir == FORWARD) {
589 const MSLinkCont& links = currentLane->getLinkCont();
590 for (MSLinkCont::const_iterator it = links.begin(); it != links.end(); ++it) {
591 if ((*it)->getLane()->getEdge().isWalkingArea()) {
592 link = *it;
593 return (*it)->getLane();
594 }
595 }
596 } else {
597 const std::vector<MSLane::IncomingLaneInfo>& laneInfos = currentLane->getIncomingLanes();
598 for (std::vector<MSLane::IncomingLaneInfo>::const_iterator it = laneInfos.begin(); it != laneInfos.end(); ++it) {
599 if ((*it).lane->getEdge().isWalkingArea()) {
600 link = (*it).viaLink;
601 return (*it).lane;
602 }
603 }
604 }
605 return nullptr;
606 }
607
608
609 MSPModel_Striping::Obstacles
getNeighboringObstacles(const Pedestrians & pedestrians,int egoIndex,int stripes)610 MSPModel_Striping::getNeighboringObstacles(const Pedestrians& pedestrians, int egoIndex, int stripes) {
611 const PState& ego = *pedestrians[egoIndex];
612 Obstacles obs(stripes, Obstacle(ego.myDir));
613 std::vector<bool> haveBlocker(stripes, false);
614 for (int index = egoIndex + 1; index < (int)pedestrians.size(); index++) {
615 const PState& p = *pedestrians[index];
616 if DEBUGCOND(ego) {
617 std::cout << SIMTIME << " ped=" << ego.myPerson->getID() << " checking neighbor " << p.myPerson->getID();
618 }
619 if (!p.myWaitingToEnter) {
620 const Obstacle o(p);
621 if DEBUGCOND(ego) {
622 std::cout << " dist=" << ego.distanceTo(o) << std::endl;
623 }
624 if (ego.distanceTo(o) == DIST_BEHIND) {
625 break;
626 }
627 if (ego.distanceTo(o) == DIST_OVERLAP) {
628 obs[p.stripe()] = o;
629 obs[p.otherStripe()] = o;
630 haveBlocker[p.stripe()] = true;
631 haveBlocker[p.otherStripe()] = true;
632 }
633 if (!haveBlocker[p.stripe()]) {
634 obs[p.stripe()] = o;
635 }
636 if (!haveBlocker[p.otherStripe()]) {
637 obs[p.otherStripe()] = o;
638 }
639 }
640 }
641 if DEBUGCOND(ego) {
642 std::cout << SIMTIME << " ped=" << ego.myPerson->getID() << " neighObs=";
643 DEBUG_PRINT(obs);
644 }
645 return obs;
646 }
647
648
649 int
getStripeOffset(int origStripes,int destStripes,bool addRemainder)650 MSPModel_Striping::getStripeOffset(int origStripes, int destStripes, bool addRemainder) {
651 int offset = (destStripes - origStripes) / 2;
652 if (addRemainder) {
653 offset += (destStripes - origStripes) % 2;
654 }
655 return offset;
656 }
657
658
659 const MSPModel_Striping::Obstacles&
getNextLaneObstacles(NextLanesObstacles & nextLanesObs,const MSLane * lane,const MSLane * nextLane,int stripes,int nextDir,double currentLength,int currentDir)660 MSPModel_Striping::getNextLaneObstacles(NextLanesObstacles& nextLanesObs, const
661 MSLane* lane, const MSLane* nextLane, int stripes, int nextDir,
662 double currentLength, int currentDir) {
663 if (nextLanesObs.count(nextLane) == 0) {
664 const double nextLength = nextLane->getEdge().isWalkingArea() ? myMinNextLengths[nextLane] : nextLane->getLength();
665 // figure out the which pedestrians are ahead on the next lane
666 const int nextStripes = numStripes(nextLane);
667 // do not move past the end of the next lane in a single step
668 Obstacles obs(stripes, Obstacle(nextDir == FORWARD ? nextLength : 0, 0, OBSTACLE_NEXTEND, "nextEnd", 0));
669
670 const int offset = getStripeOffset(nextStripes, stripes, currentDir != nextDir && nextStripes > stripes);
671 //std::cout << SIMTIME << " getNextLaneObstacles"
672 // << " nextLane=" << nextLane->getID()
673 // << " nextLength=" << nextLength
674 // << " nextDir=" << nextDir
675 // << " currentLength=" << currentLength
676 // << " currentDir=" << currentDir
677 // << " stripes=" << stripes
678 // << " nextStripes=" << nextStripes
679 // << " offset=" << offset
680 // << "\n";
681 if (nextStripes < stripes) {
682 // some stripes do not continue
683 for (int ii = 0; ii < stripes; ++ii) {
684 if (ii < offset || ii >= nextStripes + offset) {
685 obs[ii] = Obstacle(nextDir == FORWARD ? 0 : nextLength, 0, OBSTACLE_END, "stripeEnd", 0);
686 }
687 }
688 }
689 Pedestrians& pedestrians = getPedestrians(nextLane);
690 if (nextLane->getEdge().isWalkingArea()) {
691 transformToCurrentLanePositions(obs, currentDir, nextDir, currentLength, nextLength);
692 // complex transformation into the coordinate system of the current lane
693 // (pedestrians on next lane may walk at arbitrary angles relative to the current lane)
694 double lateral_offset = (lane->getWidth() - stripeWidth) * 0.5;
695 if ((stripes - nextStripes) % 2 != 0) {
696 lateral_offset += 0.5 * stripeWidth;
697 }
698 nextDir = currentDir;
699 // transform pedestrians into the current coordinate system
700 for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
701 PState& p = *pedestrians[ii];
702 if (p.myWaitingToEnter || p.myAmJammed) {
703 continue;
704 }
705 Position relPos = lane->getShape().transformToVectorCoordinates(p.getPosition(*p.myStage, -1), true);
706 const double newY = relPos.y() + lateral_offset;
707 //if (p.myPerson->getID() == "ped200") std::cout << " ped=" << p.myPerson->getID() << " relX=" << relPos.x() << " relY=" << newY << " latOff=" << lateral_offset << " s=" << p.stripe(newY) << " os=" << p.otherStripe(newY) << "\n";
708 if ((currentDir == FORWARD && relPos.x() >= lane->getLength()) || (currentDir == BACKWARD && relPos.x() < 0)) {
709 addCloserObstacle(obs, relPos.x(), p.stripe(newY), stripes, p.myPerson->getID(), p.myPerson->getVehicleType().getWidth(), currentDir, OBSTACLE_PED);
710 addCloserObstacle(obs, relPos.x(), p.otherStripe(newY), stripes, p.myPerson->getID(), p.myPerson->getVehicleType().getWidth(), currentDir, OBSTACLE_PED);
711 }
712 }
713 } else {
714 // simple transformation into the coordinate system of the current lane
715 // (only need to worry about currentDir and nextDir)
716 // XXX consider waitingToEnter on nextLane
717 sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(nextDir));
718 for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
719 const PState& p = *pedestrians[ii];
720 if (p.myWaitingToEnter || p.myAmJammed) {
721 continue;
722 }
723 double newY = p.myRelY;
724 Obstacle pObs(p);
725 if (nextDir != currentDir) {
726 newY = (nextStripes - 1) * stripeWidth - newY;
727 pObs.speed *= -1;
728 }
729 newY += offset * stripeWidth;
730 const int stripe = p.stripe(newY);
731 if (stripe >= 0 && stripe < stripes) {
732 obs[stripe] = pObs;
733 }
734 const int otherStripe = p.otherStripe(newY);
735 if (otherStripe >= 0 && otherStripe < stripes) {
736 obs[otherStripe] = pObs;
737 }
738 }
739 if (nextLane->getEdge().isCrossing()) {
740 // add vehicle obstacles
741 const MSLink* crossingEntryLink = nextLane->getIncomingLanes().front().viaLink;
742 const bool prio = crossingEntryLink->havePriority() || crossingEntryLink->getTLLogic() != nullptr;
743 addCrossingVehs(nextLane, stripes, offset, nextDir, obs, prio);
744 }
745 if (nextLane->getVehicleNumberWithPartials() > 0) {
746 Obstacles vehObs = getVehicleObstacles(nextLane, nextDir);
747 PState::mergeObstacles(obs, vehObs, nextDir, offset);
748 }
749 transformToCurrentLanePositions(obs, currentDir, nextDir, currentLength, nextLength);
750 }
751 nextLanesObs[nextLane] = obs;
752 }
753 return nextLanesObs[nextLane];
754 }
755
756 void
transformToCurrentLanePositions(Obstacles & obs,int currentDir,int nextDir,double currentLength,double nextLength)757 MSPModel_Striping::transformToCurrentLanePositions(Obstacles& obs, int currentDir, int nextDir, double currentLength, double nextLength) {
758 for (int ii = 0; ii < (int)obs.size(); ++ii) {
759 Obstacle& o = obs[ii];
760 if (currentDir == FORWARD) {
761 if (nextDir == FORWARD) {
762 o.xFwd += currentLength;
763 o.xBack += currentLength;
764 } else {
765 const double tmp = o.xFwd;
766 o.xFwd = currentLength + nextLength - o.xBack;
767 o.xBack = currentLength + nextLength - tmp;
768 }
769 } else {
770 if (nextDir == FORWARD) {
771 const double tmp = o.xFwd;
772 o.xFwd = -o.xBack;
773 o.xBack = -tmp;
774 } else {
775 o.xFwd -= nextLength;
776 o.xBack -= nextLength;
777 }
778 }
779 }
780 }
781
782
783 void
addCloserObstacle(Obstacles & obs,double x,int stripe,int numStripes,const std::string & id,double width,int dir,ObstacleType type)784 MSPModel_Striping::addCloserObstacle(Obstacles& obs, double x, int stripe, int numStripes, const std::string& id, double width, int dir, ObstacleType type) {
785 if (stripe >= 0 && stripe < numStripes) {
786 if ((dir == FORWARD && x - width / 2. < obs[stripe].xBack) || (dir == BACKWARD && x + width / 2. > obs[stripe].xFwd)) {
787 obs[stripe] = Obstacle(x, 0, type, id, width);
788 }
789 }
790 }
791
792 void
moveInDirection(SUMOTime currentTime,std::set<MSPerson * > & changedLane,int dir)793 MSPModel_Striping::moveInDirection(SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir) {
794 for (ActiveLanes::iterator it_lane = myActiveLanes.begin(); it_lane != myActiveLanes.end(); ++it_lane) {
795 const MSLane* lane = it_lane->first;
796 Pedestrians& pedestrians = it_lane->second;
797 if (pedestrians.size() == 0) {
798 continue;
799 }
800 //std::cout << SIMTIME << ">>> lane=" << lane->getID() << " numPeds=" << pedestrians.size() << "\n";
801 if (lane->getEdge().isWalkingArea()) {
802 const double lateral_offset = (lane->getWidth() - stripeWidth) * 0.5;
803 const double minY = stripeWidth * - 0.5 + NUMERICAL_EPS;
804 const double maxY = stripeWidth * (numStripes(lane) - 0.5) - NUMERICAL_EPS;
805 const WalkingAreaPath* debugPath = nullptr;
806 // need to handle each walkingAreaPath separately and transform
807 // coordinates beforehand
808 std::set<const WalkingAreaPath*, walkingarea_path_sorter> paths;
809 for (Pedestrians::iterator it = pedestrians.begin(); it != pedestrians.end(); ++it) {
810 const PState* p = *it;
811 assert(p->myWalkingAreaPath != 0);
812 if (p->myDir == dir) {
813 paths.insert(p->myWalkingAreaPath);
814 if DEBUGCOND(*p) {
815 debugPath = p->myWalkingAreaPath;
816 std::cout << SIMTIME << " debugging WalkingAreaPath from=" << debugPath->from->getID() << " to=" << debugPath->to->getID() << "\n";
817 }
818 }
819 }
820 for (std::set<const WalkingAreaPath*, walkingarea_path_sorter>::iterator it = paths.begin(); it != paths.end(); ++it) {
821 const WalkingAreaPath* path = *it;
822 Pedestrians toDelete;
823 Pedestrians transformedPeds;
824 transformedPeds.reserve(pedestrians.size());
825 for (Pedestrians::iterator it_p = pedestrians.begin(); it_p != pedestrians.end(); ++it_p) {
826 PState* p = *it_p;
827 if (p->myWalkingAreaPath == path
828 // opposite direction is already in the correct coordinate system
829 || (p->myWalkingAreaPath->from == path->to && p->myWalkingAreaPath->to == path->from)) {
830 transformedPeds.push_back(p);
831 if (path == debugPath) std::cout << " ped=" << p->myPerson->getID() << " relX=" << p->myRelX << " relY=" << p->myRelY << " (untransformed), vecCoord="
832 << path->shape.transformToVectorCoordinates(p->getPosition(*p->myStage, -1)) << "\n";
833 } else {
834 const Position relPos = path->shape.transformToVectorCoordinates(p->getPosition(*p->myStage, -1));
835 const double newY = relPos.y() + lateral_offset;
836 if (relPos != Position::INVALID && newY >= minY && newY <= maxY) {
837 PState* tp = new PState(*p);
838 tp->myRelX = relPos.x();
839 tp->myRelY = newY;
840 // only an obstacle, speed may be orthogonal to dir
841 tp->myDir = !dir;
842 tp->mySpeed = 0;
843 toDelete.push_back(tp);
844 transformedPeds.push_back(tp);
845 if (path == debugPath) {
846 std::cout << " ped=" << p->myPerson->getID() << " relX=" << relPos.x() << " relY=" << newY << " (transformed), vecCoord=" << relPos << "\n";
847 }
848 } else {
849 if (path == debugPath) {
850 std::cout << " ped=" << p->myPerson->getID() << " relX=" << relPos.x() << " relY=" << newY << " (invalid), vecCoord=" << relPos << "\n";
851 }
852 }
853 }
854 }
855 auto itFoe = myWalkingAreaFoes.find(&lane->getEdge());
856 if (itFoe != myWalkingAreaFoes.end()) {
857 // add vehicle foes on paths which cross this walkingarea
858 // translate the vehicle into a number of dummy-pedestrians
859 // that occupy the same space
860 for (const MSLane* foeLane : itFoe->second) {
861 for (auto itVeh = foeLane->anyVehiclesBegin(); itVeh != foeLane->anyVehiclesEnd(); ++itVeh) {
862 const MSVehicle* veh = *itVeh;
863 const Position relPos = path->shape.transformToVectorCoordinates(veh->getPosition());
864 const Position relPos2 = path->shape.transformToVectorCoordinates(veh->getBackPosition());
865 //std::cout << " pos=" << veh->getPosition() << " back=" << veh->getBackPosition() << " relPos=" << relPos << " relPos2=" << relPos2 << " shape=" << path->shape << "\n";
866 if (addVehicleFoe(veh, lane, relPos, lateral_offset, minY, maxY, toDelete, transformedPeds)
867 && addVehicleFoe(veh, lane, relPos2, lateral_offset, minY, maxY, toDelete, transformedPeds)) {
868 // add in-between positions
869 const double length = veh->getVehicleType().getLength();
870 for (double dist = stripeWidth; dist < length; dist += stripeWidth) {
871 const double relDist = dist / length;
872 Position between = (relPos * relDist) + (relPos2 * (1 - relDist));
873 addVehicleFoe(veh, lane, between, lateral_offset, minY, maxY, toDelete, transformedPeds);
874 }
875 }
876 }
877 }
878 }
879 moveInDirectionOnLane(transformedPeds, lane, currentTime, changedLane, dir);
880 arriveAndAdvance(pedestrians, currentTime, changedLane, dir);
881 // clean up
882 for (Pedestrians::iterator it_p = toDelete.begin(); it_p != toDelete.end(); ++it_p) {
883 delete *it_p;
884 }
885 }
886 } else {
887 moveInDirectionOnLane(pedestrians, lane, currentTime, changedLane, dir);
888 arriveAndAdvance(pedestrians, currentTime, changedLane, dir);
889 }
890 }
891 }
892
893
894 bool
addVehicleFoe(const MSVehicle * veh,const MSLane * walkingarea,const Position & relPos,double lateral_offset,double minY,double maxY,Pedestrians & toDelete,Pedestrians & transformedPeds)895 MSPModel_Striping::addVehicleFoe(const MSVehicle* veh, const MSLane* walkingarea, const Position& relPos, double lateral_offset,
896 double minY, double maxY, Pedestrians& toDelete, Pedestrians& transformedPeds) {
897 if (relPos != Position::INVALID) {
898 const double newY = relPos.y() + lateral_offset;
899 if (newY >= minY && newY <= maxY) {
900 PState* tp = new PStateVehicle(veh, walkingarea, relPos.x(), newY);
901 //std::cout << SIMTIME << " addVehicleFoe=" << veh->getID() << " rx=" << relPos.x() << " ry=" << newY << " s=" << tp->stripe() << " o=" << tp->otherStripe() << "\n";
902 toDelete.push_back(tp);
903 transformedPeds.push_back(tp);
904 }
905 return true;
906 } else {
907 return false;
908 }
909 }
910
911 void
arriveAndAdvance(Pedestrians & pedestrians,SUMOTime currentTime,std::set<MSPerson * > & changedLane,int dir)912 MSPModel_Striping::arriveAndAdvance(Pedestrians& pedestrians, SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir) {
913 // advance to the next lane / arrive at destination
914 sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(dir));
915 // can't use iterators because we do concurrent modification
916 for (int i = 0; i < (int)pedestrians.size(); i++) {
917 PState* const p = pedestrians[i];
918 if (p->myDir == dir && p->distToLaneEnd() < 0) {
919 // moveToNextLane may trigger re-insertion (for consecutive
920 // walks) so erase must be called first
921 pedestrians.erase(pedestrians.begin() + i);
922 i--;
923 p->moveToNextLane(currentTime);
924 if (p->myLane != nullptr) {
925 changedLane.insert(p->myPerson);
926 myActiveLanes[p->myLane].push_back(p);
927 } else {
928 // end walking stage and destroy PState
929 p->myStage->moveToNextEdge(p->myPerson, currentTime);
930 myNumActivePedestrians--;
931 }
932 }
933 }
934 }
935
936
937 void
moveInDirectionOnLane(Pedestrians & pedestrians,const MSLane * lane,SUMOTime currentTime,std::set<MSPerson * > & changedLane,int dir)938 MSPModel_Striping::moveInDirectionOnLane(Pedestrians& pedestrians, const MSLane* lane, SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir) {
939 const int stripes = numStripes(lane);
940 //std::cout << " laneWidth=" << lane->getWidth() << " stripeWidth=" << stripeWidth << " stripes=" << stripes << "\n";
941 Obstacles obs(stripes, Obstacle(dir)); // continously updated
942 NextLanesObstacles nextLanesObs; // continously updated
943 sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(dir));
944
945 Obstacles crossingVehs(stripes, Obstacle(dir));
946 bool hasCrossingVehObs = false;
947 if (lane->getEdge().isCrossing()) {
948 // assume that vehicles will brake when already on the crossing
949 hasCrossingVehObs = addCrossingVehs(lane, stripes, 0, dir, crossingVehs, true);
950 }
951
952 for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
953 PState& p = *pedestrians[ii];
954 //std::cout << SIMTIME << "CHECKING" << p.myPerson->getID() << "\n";
955 Obstacles currentObs = obs;
956 if (p.myDir != dir || changedLane.count(p.myPerson) != 0) {
957 if (!p.myWaitingToEnter) {
958 //if DEBUGCOND(p) {
959 // std::cout << " obs=" << p.myPerson->getID() << " y=" << p.myRelY << " stripe=" << p.stripe() << " oStripe=" << p.otherStripe() << "\n";
960 //}
961 Obstacle o(p);
962 if (p.myDir != dir && p.mySpeed == 0) {
963 // ensure recognition of oncoming
964 o.speed = (p.myDir == FORWARD ? 0.1 : -0.1);
965 }
966 obs[p.stripe()] = o;
967 obs[p.otherStripe()] = o;
968 }
969 continue;
970 }
971 if DEBUGCOND(p) {
972 std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " currentObs=";
973 gDebugFlag1 = true;
974 DEBUG_PRINT(currentObs);
975 }
976 const MSLane* nextLane = p.myNLI.lane;
977 const MSLink* link = p.myNLI.link;
978 const double dist = p.distToLaneEnd();
979 const double speed = p.myStage->getMaxSpeed(p.myPerson);
980 if (nextLane != nullptr && dist <= LOOKAHEAD_ONCOMING) {
981 const double currentLength = (p.myWalkingAreaPath == nullptr ? lane->getLength() : p.myWalkingAreaPath->length);
982 const Obstacles& nextObs = getNextLaneObstacles(
983 nextLanesObs, lane, nextLane, stripes,
984 p.myNLI.dir, currentLength, dir);
985
986 if DEBUGCOND(p) {
987 std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " nextObs=";
988 DEBUG_PRINT(nextObs);
989 }
990 p.mergeObstacles(currentObs, nextObs);
991 }
992 if DEBUGCOND(p) {
993 std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWithNext=";
994 DEBUG_PRINT(currentObs);
995 }
996 p.mergeObstacles(currentObs, getNeighboringObstacles(pedestrians, ii, stripes));
997 if DEBUGCOND(p) {
998 std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWithNeigh=";
999 DEBUG_PRINT(currentObs);
1000 }
1001 // time gap to pass the intersection ahead of a vehicle.
1002 const double passingClearanceTime = 2;
1003 const double passingLength = p.getLength() + passingClearanceTime * speed;
1004 // check link state
1005 if DEBUGCOND(p) {
1006 gDebugFlag1 = true;
1007 std::cout << " link=" << (link == nullptr ? "NULL" : link->getViaLaneOrLane()->getID())
1008 << " dist=" << dist << " d2=" << dist - p.getMinGap() << " la=" << LOOKAHEAD_SAMEDIR* speed
1009 << " opened=" << (link == nullptr ? "NULL" : toString(link->opened(currentTime - DELTA_T, speed, speed, passingLength, p.getImpatience(currentTime), speed, 0, 0, nullptr, p.ignoreRed(link)))) << "\n";
1010 gDebugFlag1 = false;
1011 }
1012 if (link != nullptr
1013 // only check close before junction, @todo we should take deceleration into account here
1014 && dist - p.getMinGap() < LOOKAHEAD_SAMEDIR * speed
1015 // persons move before vehicles so we subtract DELTA_TO because they cannot rely on vehicles having passed the intersection in the current time step
1016 && !link->opened(currentTime - DELTA_T, speed, speed, passingLength, p.getImpatience(currentTime), speed, 0, 0, nullptr, p.ignoreRed(link))) {
1017 // prevent movement passed a closed link
1018 Obstacles closedLink(stripes, Obstacle(p.myRelX + dir * (dist + NUMERICAL_EPS), 0, OBSTACLE_LINKCLOSED, "closedLink_" + link->getViaLaneOrLane()->getID(), 0));
1019 p.mergeObstacles(currentObs, closedLink);
1020 if DEBUGCOND(p) {
1021 std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWithTLS=";
1022 DEBUG_PRINT(currentObs);
1023 }
1024 // consider rerouting over another crossing
1025 if (p.myWalkingAreaPath != nullptr) {
1026 // @todo actually another path would be needed starting at the current position
1027 p.myNLI = getNextLane(p, p.myLane, p.myWalkingAreaPath->from);
1028 }
1029 }
1030 if (&lane->getEdge() == p.myStage->getDestination() && p.myStage->getDestinationStop() != nullptr) {
1031 Obstacles arrival(stripes, Obstacle(p.myStage->getArrivalPos() + dir * p.getMinGap(), 0, OBSTACLE_ARRIVALPOS, "arrival", 0));
1032 p.mergeObstacles(currentObs, arrival);
1033 }
1034
1035 if (lane->getVehicleNumberWithPartials() > 0) {
1036 // react to vehicles on the same lane
1037 // @todo: improve efficiency by using the same iterator for all pedestrians on this lane
1038 Obstacles vehObs = getVehicleObstacles(lane, dir, &p);
1039 p.mergeObstacles(currentObs, vehObs);
1040 if DEBUGCOND(p) {
1041 std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWithVehs=";
1042 DEBUG_PRINT(currentObs);
1043 }
1044 }
1045 if (hasCrossingVehObs) {
1046 p.mergeObstacles(currentObs, crossingVehs);
1047 if DEBUGCOND(p) {
1048 std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWithVehs2=";
1049 DEBUG_PRINT(currentObs);
1050 }
1051 }
1052
1053 // walk, taking into account all obstacles
1054 p.walk(currentObs, currentTime);
1055 gDebugFlag1 = false;
1056 if (!p.myWaitingToEnter && !p.myAmJammed) {
1057 Obstacle o(p);
1058 obs[p.stripe()] = o;
1059 obs[p.otherStripe()] = o;
1060 if (MSGlobals::gCheck4Accidents && p.myWalkingAreaPath == nullptr && !p.myAmJammed) {
1061 for (int coll = 0; coll < ii; ++coll) {
1062 PState& c = *pedestrians[coll];
1063 if (!c.myWaitingToEnter && c.myWalkingAreaPath == nullptr && !c.myAmJammed) {
1064 if (c.stripe() == p.stripe() || p.stripe() == c.otherStripe() || p.otherStripe() == c.stripe() || p.otherStripe() == c.otherStripe()) {
1065 Obstacle cObs(c);
1066 // we check only for real collisions, no min gap violations
1067 if (p.distanceTo(cObs, false) == DIST_OVERLAP) {
1068 WRITE_WARNING("Collision of person '" + p.myPerson->getID() + "' and person '" + c.myPerson->getID()
1069 + "', lane='" + lane->getID() + "', time=" + time2string(currentTime) + ".");
1070 }
1071 }
1072 }
1073 }
1074 }
1075 }
1076 //std::cout << SIMTIME << p.myPerson->getID() << " lane=" << lane->getID() << " x=" << p.myRelX << "\n";
1077 }
1078 }
1079
1080 bool
addCrossingVehs(const MSLane * crossing,int stripes,double lateral_offset,int dir,Obstacles & obs,bool prio)1081 MSPModel_Striping::addCrossingVehs(const MSLane* crossing, int stripes, double lateral_offset, int dir, Obstacles& obs, bool prio) {
1082 bool hasCrossingVehObs = false;
1083 const MSLink* crossingExitLink = crossing->getLinkCont().front();
1084 gDebugFlag1 = DEBUGCOND2(crossing);
1085 const MSLink::LinkLeaders linkLeaders = crossingExitLink->getLeaderInfo(nullptr, crossing->getLength());
1086 gDebugFlag1 = false;
1087 if (linkLeaders.size() > 0) {
1088 for (MSLink::LinkLeaders::const_iterator it = linkLeaders.begin(); it != linkLeaders.end(); ++it) {
1089 // the vehicle to enter the junction first has priority
1090 const MSVehicle* veh = (*it).vehAndGap.first;
1091 if (veh != nullptr) {
1092 Obstacle vo((*it).distToCrossing, 0, OBSTACLE_VEHICLE, veh->getID(), veh->getVehicleType().getWidth() + 2 * MINGAP_TO_VEHICLE);
1093 // block entry to the crossing in walking direction but allow leaving it
1094 Obstacle voBlock = vo;
1095 if (dir == FORWARD) {
1096 voBlock.xBack = NUMERICAL_EPS;
1097 } else {
1098 voBlock.xFwd = crossing->getLength() - NUMERICAL_EPS;
1099 }
1100 // when approaching a priority crossings, vehicles must be able
1101 // to brake, otherwise the person must be able to cross in time
1102 const double distToCrossBeforeVeh = (dir == FORWARD ? vo.xFwd : crossing->getLength() - vo.xBack);
1103 const double bGap = (prio
1104 ? veh->getCarFollowModel().brakeGap(veh->getSpeed(), veh->getCarFollowModel().getMaxDecel(), 0)
1105 : veh->getSpeed() * distToCrossBeforeVeh); // walking 1m/s
1106 double vehYmin;
1107 double vehYmax;
1108 // relY increases from left to right (the other way around from vehicles)
1109 if ((*it).fromLeft) {
1110 vehYmin = -(*it).vehAndGap.second + lateral_offset; // vehicle back
1111 vehYmax = vehYmin + veh->getVehicleType().getLength() + bGap + MINGAP_TO_VEHICLE;
1112 vehYmin -= MINGAP_TO_VEHICLE;
1113 } else {
1114 vehYmax = crossing->getWidth() + (*it).vehAndGap.second - lateral_offset; // vehicle back
1115 vehYmin = vehYmax - veh->getVehicleType().getLength() - bGap - MINGAP_TO_VEHICLE;
1116 vehYmax += MINGAP_TO_VEHICLE;
1117
1118 }
1119 for (int s = MAX2(0, PState::stripe(vehYmin)); s < MIN2(PState::stripe(vehYmax), stripes); ++s) {
1120 if ((dir == FORWARD && obs[s].xBack > vo.xBack)
1121 || (dir == BACKWARD && obs[s].xFwd < vo.xFwd)) {
1122 if (!prio && veh->getSpeed() > SUMO_const_haltingSpeed) {
1123 // do not enter the crossing
1124 obs[s] = voBlock;
1125 } else {
1126 obs[s] = vo;
1127 }
1128 hasCrossingVehObs = true;
1129 }
1130 }
1131 if (DEBUGCOND2(crossing)) {
1132 std::cout << SIMTIME
1133 << " crossingVeh=" << veh->getID()
1134 << " lane=" << crossing->getID()
1135 << " prio=" << prio
1136 << " latOffset=" << lateral_offset
1137 << " dir=" << dir
1138 << " stripes=" << stripes
1139 << " dist=" << (*it).distToCrossing
1140 << " gap=" << (*it).vehAndGap.second
1141 << " brakeGap=" << bGap
1142 << " fromLeft=" << (*it).fromLeft
1143 << " distToCrossBefore=" << distToCrossBeforeVeh
1144 << " ymin=" << vehYmin
1145 << " ymax=" << vehYmax
1146 << " smin=" << PState::stripe(vehYmin)
1147 << " smax=" << PState::stripe(vehYmax)
1148 << "\n";
1149 DEBUG_PRINT(obs);
1150 }
1151 }
1152 }
1153 }
1154 return hasCrossingVehObs;
1155 }
1156
1157
1158 MSPModel_Striping::Obstacles
getVehicleObstacles(const MSLane * lane,int dir,PState * ped)1159 MSPModel_Striping::getVehicleObstacles(const MSLane* lane, int dir, PState* ped) {
1160 const int stripes = numStripes(lane);
1161 Obstacles vehObs(stripes, Obstacle(dir));
1162 int current = -1;
1163 double minX = 0.;
1164 double maxX = 0.;
1165 double pRelY = -1.;
1166 double pWidth = 0.;
1167 std::string pID;
1168 bool debug = DEBUGCOND2(lane);
1169 if (ped != nullptr) {
1170 current = ped->stripe();
1171 minX = ped->getMinX();
1172 maxX = ped->getMaxX();
1173 pRelY = ped->myRelY;
1174 pWidth = ped->myPerson->getVehicleType().getWidth();
1175 pID = ped->myPerson->getID();
1176 debug = DEBUGCOND(*ped);
1177 } else if (dir == BACKWARD) {
1178 // checking vehicles on the next lane. Use entry point as reference
1179 minX = lane->getLength();
1180 maxX = lane->getLength();
1181 }
1182 MSLane::AnyVehicleIterator begin = (dir == FORWARD ? lane->anyVehiclesUpstreamBegin() : lane->anyVehiclesBegin());
1183 MSLane::AnyVehicleIterator end = (dir == FORWARD ? lane->anyVehiclesUpstreamEnd() : lane->anyVehiclesEnd());
1184 for (MSLane::AnyVehicleIterator it = begin; it != end; ++it) {
1185 const MSVehicle* veh = *it;
1186 const double vehBack = veh->getBackPositionOnLane(lane);
1187 const double vehFront = vehBack + veh->getVehicleType().getLength();
1188 // ensure that vehicles are not blocked
1189 const double vehNextSpeed = MAX2(veh->getSpeed(), 1.0);
1190 const double clearance = SAFETY_GAP + vehNextSpeed * LOOKAHEAD_SAMEDIR;
1191 if ((dir == FORWARD && vehFront + clearance > minX && vehBack <= maxX + LOOKAHEAD_SAMEDIR)
1192 || (dir == BACKWARD && vehBack < maxX && vehFront >= minX - LOOKAROUND_VEHICLES)) {
1193 Obstacle vo(vehBack, veh->getSpeed(), OBSTACLE_VEHICLE, veh->getID(), 0);
1194 // moving vehicles block space along their path
1195 vo.xFwd += veh->getVehicleType().getLength() + clearance;
1196 vo.xBack -= SAFETY_GAP;
1197 // relY increases from left to right (the other way around from vehicles)
1198 // XXX lateral offset for partial vehicles
1199 const double vehYmax = 0.5 * (lane->getWidth() + veh->getVehicleType().getWidth() - stripeWidth) - veh->getLateralPositionOnLane();
1200 const double vehYmin = vehYmax - veh->getVehicleType().getWidth();
1201 for (int s = MAX2(0, PState::stripe(vehYmin)); s < MIN2(PState::stripe(vehYmax) + 1, stripes); ++s) {
1202 vehObs[s] = vo;
1203 if (s == current && vehFront + SAFETY_GAP < minX) {
1204 // ignore if aleady overlapping while vehicle is still behind
1205 if (pRelY - pWidth < vehYmax &&
1206 pRelY + pWidth > vehYmin && dir == FORWARD) {
1207 if (debug) {
1208 std::cout << " ignoring vehicle on stripe " << s << "\n";
1209 }
1210 if (dir == FORWARD) {
1211 vehObs[s] = Obstacle(dir);
1212 } else {
1213 vehObs[s].xFwd = MIN2(vo.xFwd, vehFront + SAFETY_GAP);
1214 }
1215 }
1216 }
1217 }
1218 if (debug) {
1219 std::cout << SIMTIME << " ped=" << pID << " veh=" << veh->getID() << " obstacle on lane=" << lane->getID()
1220 << "\n"
1221 << " ymin=" << vehYmin
1222 << " ymax=" << vehYmax
1223 << " smin=" << PState::stripe(vehYmin)
1224 << " smax=" << PState::stripe(vehYmax)
1225 << " relY=" << pRelY
1226 << " current=" << current
1227 << "\n";
1228 }
1229 }
1230 }
1231 return vehObs;
1232 }
1233
1234
1235 // ===========================================================================
1236 // MSPModel_Striping::Obstacle method definitions
1237 // ===========================================================================
Obstacle(int dir,double dist)1238 MSPModel_Striping::Obstacle::Obstacle(int dir, double dist) :
1239 xFwd(dir * dist), // by default, far away when seen in dir
1240 xBack(dir * dist), // by default, far away when seen in dir
1241 speed(0),
1242 type(OBSTACLE_NONE),
1243 description("") {
1244 }
1245
1246
Obstacle(const PState & ped)1247 MSPModel_Striping::Obstacle::Obstacle(const PState& ped) :
1248 xFwd(ped.getMaxX()),
1249 xBack(ped.getMinX()),
1250 speed(ped.myDir * ped.mySpeed),
1251 type(OBSTACLE_PED),
1252 description(ped.getID()) {
1253 assert(!ped.myWaitingToEnter);
1254 }
1255
1256
1257 // ===========================================================================
1258 // MSPModel_Striping::PState method definitions
1259 // ===========================================================================
1260
1261
PState(MSPerson * person,MSPerson::MSPersonStage_Walking * stage,const MSLane * lane)1262 MSPModel_Striping::PState::PState(MSPerson* person, MSPerson::MSPersonStage_Walking* stage, const MSLane* lane):
1263 myPerson(person),
1264 myStage(stage),
1265 myLane(lane),
1266 myRelX(stage->getDepartPos()),
1267 myRelY(stage->getDepartPosLat()),
1268 myDir(FORWARD),
1269 mySpeed(0),
1270 myWaitingToEnter(true),
1271 myWaitingTime(0),
1272 myWalkingAreaPath(nullptr),
1273 myAmJammed(false),
1274 myRemoteXYPos(Position::INVALID),
1275 myAngle(std::numeric_limits<double>::max()) {
1276 const MSEdge* currentEdge = &lane->getEdge();
1277 const ConstMSEdgeVector& route = myStage->getRoute();
1278 assert(!route.empty());
1279 if (route.size() == 1) {
1280 // only a single edge, move towards end pos
1281 myDir = (myRelX <= myStage->getArrivalPos()) ? FORWARD : BACKWARD;
1282 } else if (route.front()->getFunction() != EDGEFUNC_NORMAL) {
1283 // start on an intersection
1284 myDir = FORWARD;
1285 if (route.front()->isWalkingArea()) {
1286 myWalkingAreaPath = getArbitraryPath(route.front());
1287 }
1288 } else {
1289 const bool mayStartForward = canTraverse(FORWARD, route) != UNDEFINED_DIRECTION;
1290 const bool mayStartBackward = canTraverse(BACKWARD, route) != UNDEFINED_DIRECTION;
1291 if DEBUGCOND(*this) {
1292 std::cout << " initialize dir for " << myPerson->getID() << " forward=" << mayStartForward << " backward=" << mayStartBackward << "\n";
1293 }
1294 if (mayStartForward && mayStartBackward) {
1295 // figure out the best direction via routing
1296 ConstMSEdgeVector crossingRoute;
1297 MSNet::getInstance()->getPedestrianRouter().compute(currentEdge, route.back(), myRelX, myStage->getArrivalPos(), myStage->getMaxSpeed(person), 0, nullptr, crossingRoute, true);
1298 if (crossingRoute.size() > 1) {
1299 // route found
1300 const MSEdge* nextEdge = crossingRoute[1];
1301 if (nextEdge->getFromJunction() == currentEdge->getFromJunction() || nextEdge->getToJunction() == currentEdge->getFromJunction()) {
1302 myDir = BACKWARD;
1303 }
1304 }
1305 if DEBUGCOND(*this) {
1306 std::cout << " crossingRoute=" << toString(crossingRoute) << "\n";
1307 }
1308 } else {
1309 myDir = !mayStartBackward ? FORWARD : BACKWARD;
1310 }
1311 }
1312 if (lane->getVehicleNumberWithPartials() > 0 && myRelY == 0) {
1313 // better start next to the road if nothing was specified
1314 myRelY -= stripeWidth;
1315 }
1316 if (myDir == FORWARD) {
1317 // start at the right side of the sidewalk
1318 myRelY = stripeWidth * (numStripes(lane) - 1) - myRelY;
1319 }
1320 if DEBUGCOND(*this) {
1321 std::cout << " added new pedestrian " << myPerson->getID() << " on " << lane->getID() << " myRelX=" << myRelX << " myRelY=" << myRelY << " dir=" << myDir << " route=" << toString(myStage->getRoute()) << "\n";
1322 }
1323
1324 myNLI = getNextLane(*this, lane, nullptr);
1325 }
1326
PState()1327 MSPModel_Striping::PState::PState():
1328 myPerson(nullptr),
1329 myStage(nullptr),
1330 myLane(nullptr),
1331 myRelX(0),
1332 myRelY(0),
1333 myDir(UNDEFINED_DIRECTION),
1334 mySpeed(0),
1335 myWaitingToEnter(false),
1336 myWaitingTime(0),
1337 myWalkingAreaPath(nullptr),
1338 myAmJammed(false),
1339 myRemoteXYPos(Position::INVALID),
1340 myAngle(std::numeric_limits<double>::max()) {
1341 }
1342
1343
1344 double
getMinX(const bool includeMinGap) const1345 MSPModel_Striping::PState::getMinX(const bool includeMinGap) const {
1346 // @todo speed should have an influence here because faster persons need more space
1347 if (myDir == FORWARD) {
1348 return myRelX - getLength();
1349 }
1350 return myRelX - (includeMinGap ? getMinGap() : 0.);
1351 }
1352
1353
1354 double
getMaxX(const bool includeMinGap) const1355 MSPModel_Striping::PState::getMaxX(const bool includeMinGap) const {
1356 // @todo speed should have an influence here because faster persons need more space
1357 if (myDir == FORWARD) {
1358 return myRelX + (includeMinGap ? getMinGap() : 0.);
1359 }
1360 return myRelX + getLength();
1361 }
1362
1363
1364 double
getLength() const1365 MSPModel_Striping::PState::getLength() const {
1366 return myPerson->getVehicleType().getLength();
1367 }
1368
1369
1370 double
getMinGap() const1371 MSPModel_Striping::PState::getMinGap() const {
1372 return myPerson->getVehicleType().getMinGap();
1373 }
1374
1375
1376 int
stripe(double relY)1377 MSPModel_Striping::PState::stripe(double relY) {
1378 return (int)floor(relY / stripeWidth + 0.5);
1379 }
1380
1381
1382 int
otherStripe(double relY) const1383 MSPModel_Striping::PState::otherStripe(double relY) const {
1384 const int s = stripe(relY);
1385 const double offset = relY - s * stripeWidth;
1386 const double threshold = MAX2(NUMERICAL_EPS, stripeWidth - SQUEEZE * getWidth());
1387 int result;
1388 if (offset > threshold) {
1389 result = s + 1;
1390 } else if (offset < -threshold) {
1391 result = s - 1;
1392 } else {
1393 result = s;
1394 }
1395 //std::cout.setf(std::ios::fixed , std::ios::floatfield);
1396 //std::cout << std::setprecision(5);
1397 //if DEBUGCOND(*this) std::cout << " otherStripe " << myPerson->getID() << " offset=" << offset << " threshold=" << threshold << " rawResult=" << result << "\n";
1398 return result;
1399 }
1400
1401 int
stripe() const1402 MSPModel_Striping::PState::stripe() const {
1403 return MIN2(MAX2(0, stripe(myRelY)), numStripes(myLane) - 1);
1404 }
1405
1406
1407 int
otherStripe() const1408 MSPModel_Striping::PState::otherStripe() const {
1409 return MIN2(MAX2(0, otherStripe(myRelY)), numStripes(myLane) - 1);
1410 }
1411
1412
1413 double
distToLaneEnd() const1414 MSPModel_Striping::PState::distToLaneEnd() const {
1415 if (myStage->getNextRouteEdge() == nullptr) {
1416 return myDir * (myStage->getArrivalPos() - myRelX) - POSITION_EPS;
1417 } else {
1418 const double length = myWalkingAreaPath == nullptr ? myLane->getLength() : myWalkingAreaPath->length;
1419 return myDir == FORWARD ? length - myRelX : myRelX;
1420 }
1421 }
1422
1423
1424 bool
moveToNextLane(SUMOTime currentTime)1425 MSPModel_Striping::PState::moveToNextLane(SUMOTime currentTime) {
1426 double dist = distToLaneEnd();
1427 if (DEBUGCOND(*this)) {
1428 std::cout << SIMTIME << " ped=" << myPerson->getID() << " myRelX=" << myRelX << " dist=" << dist << "\n";
1429 }
1430 if (dist <= 0) {
1431 //if (ped.myPerson->getID() == DEBUG1) {
1432 // std::cout << SIMTIME << " addToLane x=" << ped.myRelX << " newDir=" << newDir << " newLane=" << newLane->getID() << " walkingAreaShape=" << walkingAreaShape << "\n";
1433 //}
1434 //std::cout << " changing to " << newLane->getID() << " myRelY=" << ped.myRelY << " oldStripes=" << numStripes(myLane) << " newStripes=" << numStripes(newLane);
1435 //std::cout << " newY=" << ped.myRelY << " myDir=" << ped.myDir << " newDir=" << newDir;
1436 const int oldDir = myDir;
1437 const MSLane* oldLane = myLane;
1438 myLane = myNLI.lane;
1439 myDir = myNLI.dir;
1440 const bool normalLane = (myLane == nullptr || myLane->getEdge().getFunction() == EDGEFUNC_NORMAL || &myLane->getEdge() == myStage->getNextRouteEdge());
1441 if DEBUGCOND(*this) {
1442 std::cout << SIMTIME
1443 << " ped=" << myPerson->getID()
1444 << " moveToNextLane old=" << oldLane->getID()
1445 << " new=" << (myLane == nullptr ? "NULL" : myLane->getID())
1446 << " oldDir=" << oldDir
1447 << " newDir=" << myDir
1448 << " myRelX=" << myRelX
1449 << " dist=" << dist
1450 << "\n";
1451 }
1452 if (myLane == nullptr) {
1453 myRelX = myStage->getArrivalPos();
1454 }
1455 // moveToNextEdge might destroy the person and thus mess up the heap. Better check first
1456 if (myStage->getRouteStep() == myStage->getRoute().end() - 1) {
1457 myLane = nullptr;
1458 } else {
1459 const bool arrived = myStage->moveToNextEdge(myPerson, currentTime, normalLane ? nullptr : &myLane->getEdge());
1460 UNUSED_PARAMETER(arrived);
1461 assert(!arrived);
1462 assert(myDir != UNDEFINED_DIRECTION);
1463 myNLI = getNextLane(*this, myLane, oldLane);
1464 assert(myNLI.lane != oldLane); // do not turn around
1465 if DEBUGCOND(*this) {
1466 std::cout << " nextLane=" << (myNLI.lane == nullptr ? "NULL" : myNLI.lane->getID()) << "\n";
1467 }
1468 if (myLane->getEdge().isWalkingArea()) {
1469 if (myNLI.dir != UNDEFINED_DIRECTION) {
1470 myWalkingAreaPath = &myWalkingAreaPaths[std::make_pair(oldLane, myNLI.lane)];
1471 assert(myWalkingAreaPath->from != 0);
1472 assert(myWalkingAreaPath->to != 0);
1473 assert(myWalkingAreaPath->shape.size() >= 2);
1474 if DEBUGCOND(*this) {
1475 std::cout << " mWAPath shape=" << myWalkingAreaPath->shape << " length=" << myWalkingAreaPath->length << "\n";
1476 }
1477 } else {
1478 // disconnnected route. move to the next edge
1479 if (OptionsCont::getOptions().getBool("ignore-route-errors")) {
1480 // try to determine direction from topology, otherwise maintain current direction
1481 const MSEdge* currRouteEdge = myStage->getRouteEdge();
1482 const MSEdge* nextRouteEdge = myStage->getNextRouteEdge();
1483 if ((nextRouteEdge->getToJunction() == currRouteEdge->getFromJunction())
1484 || nextRouteEdge->getToJunction() == currRouteEdge->getToJunction()) {
1485 myDir = BACKWARD;
1486 } else if ((nextRouteEdge->getFromJunction() == currRouteEdge->getFromJunction())
1487 || nextRouteEdge->getFromJunction() == currRouteEdge->getToJunction()) {
1488 myDir = FORWARD;
1489 }
1490 myStage->moveToNextEdge(myPerson, currentTime, nullptr);
1491 myLane = myNLI.lane;
1492 assert(myLane != 0);
1493 assert(myLane->getEdge().getFunction() == EDGEFUNC_NORMAL);
1494 myNLI = getNextLane(*this, myLane, oldLane);
1495 myWalkingAreaPath = nullptr;
1496 } else {
1497 throw ProcessError("Disconnected walk for person '" + myPerson->getID() + "'.");
1498 }
1499 }
1500 } else {
1501 myWalkingAreaPath = nullptr;
1502 }
1503 // adapt x to fit onto the new lane
1504 // (make sure we do not move past the end of the new lane since that
1505 // lane was not checked for obstacles)
1506 const double newLength = (myWalkingAreaPath == nullptr ? myLane->getLength() : myWalkingAreaPath->length);
1507 if (-dist > newLength) {
1508 assert(OptionsCont::getOptions().getBool("ignore-route-errors"));
1509 // should not happen because the end of myLane should have been an obstacle as well
1510 // (only when the route is broken)
1511 dist = -newLength;
1512 }
1513 if (myDir == BACKWARD) {
1514 myRelX = newLength + dist;
1515 } else {
1516 myRelX = -dist;
1517 }
1518 if DEBUGCOND(*this) {
1519 std::cout << SIMTIME << " update myRelX ped=" << myPerson->getID()
1520 << " newLength=" << newLength
1521 << " dist=" << dist
1522 << " myRelX=" << myRelX
1523 << "\n";
1524 }
1525 // adjust to change in direction
1526 if (myDir != oldDir) {
1527 myRelY = (numStripes(oldLane) - 1) * stripeWidth - myRelY;
1528 }
1529 // adjust to differences in sidewalk width
1530 const int offset = getStripeOffset(numStripes(oldLane), numStripes(myLane), oldDir != myDir && numStripes(myLane) < numStripes(oldLane));
1531 myRelY += offset * stripeWidth;
1532 if DEBUGCOND(*this) {
1533 std::cout << SIMTIME << " transformY ped=" << myPerson->getID()
1534 << " newLane=" << Named::getIDSecure(myLane)
1535 << " newY=" << myRelY
1536 << " os=" << numStripes(oldLane) << " ns=" << numStripes(myLane)
1537 << " od=" << oldDir << " nd=" << myDir
1538 << " offset=" << offset << "\n";
1539 }
1540 }
1541 return true;
1542 } else {
1543 return false;
1544 }
1545 }
1546
1547
1548 void
walk(const Obstacles & obs,SUMOTime currentTime)1549 MSPModel_Striping::PState::walk(const Obstacles& obs, SUMOTime currentTime) {
1550 myAngle = std::numeric_limits<double>::max(); // set on first access or via remote control
1551 const int stripes = (int)obs.size();
1552 const int sMax = stripes - 1;
1553 assert(stripes == numStripes(myLane));
1554 const double vMax = myStage->getMaxSpeed(myPerson);
1555 // ultimate goal is to choose the prefered stripe (chosen)
1556 const int current = stripe();
1557 const int other = otherStripe();
1558 // compute distances
1559 std::vector<double> distance(stripes);
1560 for (int i = 0; i < stripes; ++i) {
1561 distance[i] = distanceTo(obs[i], obs[i].type == OBSTACLE_PED);
1562 }
1563 // compute utility for all stripes
1564 std::vector<double> utility(stripes, 0);
1565 // forbid stripes which are blocked and also all stripes behind them
1566 for (int i = 0; i < stripes; ++i) {
1567 if (distance[i] == DIST_OVERLAP) {
1568 if (i == current && (!myWaitingToEnter || stripe() != stripe(myRelY))) {
1569 utility[i] += OBSTRUCTED_PENALTY;
1570 }
1571 if (i < current) {
1572 for (int j = 0; j <= i; ++j) {
1573 utility[j] += OBSTRUCTED_PENALTY;
1574 }
1575 }
1576 if (i > current) {
1577 for (int j = i; j < stripes; ++j) {
1578 utility[j] += OBSTRUCTED_PENALTY;
1579 }
1580 }
1581 }
1582 }
1583 // forbid a portion of the leftmost stripes (in walking direction).
1584 // lanes with stripes less than 1 / RESERVE_FOR_ONCOMING_FACTOR
1585 // may still deadlock in heavy pedestrian traffic
1586 const bool onJunction = myLane->getEdge().isWalkingArea() || myLane->getEdge().isCrossing();
1587 const int reserved = (int)floor(stripes * (onJunction ? RESERVE_FOR_ONCOMING_FACTOR_JUNCTIONS : RESERVE_FOR_ONCOMING_FACTOR));
1588 if (myDir == FORWARD) {
1589 for (int i = 0; i < reserved; ++i) {
1590 utility[i] += INAPPROPRIATE_PENALTY * (i == current ? 0.5 : 1);
1591 }
1592 } else {
1593 for (int i = sMax; i > sMax - reserved; --i) {
1594 utility[i] += INAPPROPRIATE_PENALTY * (i == current ? 0.5 : 1);
1595 }
1596 }
1597 // adapt utility based on obstacles
1598 for (int i = 0; i < stripes; ++i) {
1599 if (obs[i].speed * myDir < 0) {
1600 // penalize evasion to the left
1601 if (myDir == FORWARD && i > 0) {
1602 utility[i - 1] -= 0.5;
1603 } else if (myDir == BACKWARD && i < sMax) {
1604 utility[i + 1] -= 0.5;
1605 }
1606 }
1607 // compute expected distance achievable by staying on this stripe for a time horizon
1608 const double walkDist = MAX2(0., distance[i]); // disregard special distance flags
1609 const double lookAhead = obs[i].speed * myDir >= 0 ? LOOKAHEAD_SAMEDIR : LOOKAHEAD_ONCOMING;
1610 const double expectedDist = MIN2(vMax * LOOKAHEAD_SAMEDIR, walkDist + obs[i].speed * myDir * lookAhead);
1611 if (DEBUGCOND(*this)) {
1612 std::cout << " util=" << utility[i] << " exp=" << expectedDist << " dist=" << distance[i] << "\n";
1613 }
1614 if (expectedDist >= 0) {
1615 utility[i] += expectedDist;
1616 } else {
1617 // let only the distance count
1618 utility[i] += ONCOMING_CONFLICT_PENALTY + distance[i];
1619 }
1620 }
1621 // discourage use of the leftmost lane (in walking direction) if there are oncoming
1622 if (myDir == FORWARD && obs[0].speed < 0) {
1623 utility[0] += ONCOMING_CONFLICT_PENALTY;
1624 } else if (myDir == BACKWARD && obs[sMax].speed > 0) {
1625 utility[sMax] += ONCOMING_CONFLICT_PENALTY;
1626 }
1627 // penalize lateral movement (if the current stripe permits walking)
1628 if (distance[current] > 0 && myWaitingTime == 0) {
1629 for (int i = 0; i < stripes; ++i) {
1630 utility[i] += abs(i - current) * LATERAL_PENALTY;
1631 }
1632 }
1633
1634 // select best stripe
1635 int chosen = current;
1636 for (int i = 0; i < stripes; ++i) {
1637 if (utility[chosen] < utility[i]) {
1638 chosen = i;
1639 }
1640 }
1641 // compute speed components along both axes
1642 const int next = (chosen == current ? current : (chosen < current ? current - 1 : current + 1));
1643 double xDist = MIN3(distance[current], distance[other], distance[next]);
1644 if (next != chosen) {
1645 // ensure that we do not collide with an obstacle in the stripe beyond
1646 // next as this might become the 'other' stripe in the next step
1647 const int nextOther = chosen < current ? current - 2 : current + 2;
1648 xDist = MIN2(xDist, distance[nextOther]);
1649 }
1650 // XXX preferred gap differs between approaching a standing obstacle or a moving obstacle
1651 const double preferredGap = NUMERICAL_EPS;
1652 double xSpeed = MIN2(vMax, MAX2(0., DIST2SPEED(xDist - preferredGap)));
1653 if (xSpeed < NUMERICAL_EPS) {
1654 xSpeed = 0.;
1655 }
1656 if (DEBUGCOND(*this)) {
1657 std::cout << " xSpeedPotential=" << xSpeed << "\n";
1658 }
1659 // avoid tiny steps
1660 // XXX pressure from behind?
1661 if (mySpeed == 0 && xDist < MIN_STARTUP_DIST &&
1662 // unless walking towards a short lane
1663 !(
1664 (xDist == distance[current] && obs[current].type >= OBSTACLE_END)
1665 || (xDist == distance[other] && obs[other].type >= OBSTACLE_END)
1666 || (xDist == distance[next] && obs[next].type >= OBSTACLE_END))
1667 ) {
1668 xSpeed = 0;
1669 }
1670 if (xSpeed == 0) {
1671 if (myWaitingTime > jamTime || myAmJammed) {
1672 // squeeze slowly through the crowd ignoring others
1673 if (!myAmJammed) {
1674 MSNet::getInstance()->getPersonControl().registerJammed();
1675 WRITE_WARNING("Person '" + myPerson->getID()
1676 + "' is jammed on edge '" + myStage->getEdge()->getID()
1677 + "', time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
1678 myAmJammed = true;
1679 }
1680 xSpeed = vMax / 4;
1681 }
1682 } else if (stripe(myRelY) >= 0 && stripe(myRelY) <= sMax) {
1683 myAmJammed = false;
1684 }
1685 // dawdling
1686 const double dawdle = MIN2(xSpeed, RandHelper::rand() * vMax * dawdling);
1687 xSpeed -= dawdle;
1688
1689 // XXX ensure that diagonal speed <= vMax
1690 // avoid deadlocks on narrow sidewalks
1691 //if (oncoming && xSpeed == 0 && myStage->getWaitingTime(currentTime) > TIME2STEPS(ONCOMIN_PATIENCE)) {
1692 // if DEBUGCOND(*this) std::cout << " stepping asside to resolve oncoming deadlock\n";
1693 // xSpeed = POSITION_EPS; // reset myWaitingTime
1694 // if (myDir == FORWARD && chosen < sMax) {
1695 // chosen += 1;
1696 // } else if (myDir == BACKWARD && chosen > 0) {
1697 // chosen -= 1;
1698 // }
1699 //}
1700 const double maxYSpeed = MIN2(MAX2(vMax * LATERAL_SPEED_FACTOR, vMax - xSpeed), stripeWidth);
1701 double ySpeed = 0;
1702 double yDist = 0;
1703 if (utility[next] > OBSTRUCTION_THRESHOLD && utility[chosen] > OBSTRUCTION_THRESHOLD) {
1704 // don't move laterally if the stripes are blocked
1705 yDist = (chosen * stripeWidth) - myRelY;
1706 if (fabs(yDist) > NUMERICAL_EPS) {
1707 ySpeed = (yDist > 0 ?
1708 MIN2(maxYSpeed, DIST2SPEED(yDist)) :
1709 MAX2(-maxYSpeed, DIST2SPEED(yDist)));
1710 }
1711 } else if (utility[next] <= OBSTRUCTION_THRESHOLD && obs[next].type == OBSTACLE_VEHICLE
1712 // still on the road
1713 && stripe() == stripe(myRelY)
1714 // only when the vehicle is moving on the same lane
1715 && !myLane->getEdge().isCrossing()) {
1716 // step aside to let the vehicle pass
1717 myRelY += myDir * vMax;
1718 }
1719 // DEBUG
1720 if DEBUGCOND(*this) {
1721 std::cout << SIMTIME
1722 << " ped=" << myPerson->getID()
1723 << " edge=" << myStage->getEdge()->getID()
1724 << " x=" << myRelX
1725 << " y=" << myRelY
1726 << " d=" << myDir
1727 << " pvx=" << mySpeed
1728 << " cur=" << current
1729 << " cho=" << chosen
1730 << " oth=" << other
1731 << " nxt=" << next
1732 << " vx=" << xSpeed
1733 << " dawdle=" << dawdle
1734 << " vy=" << ySpeed
1735 << " xd=" << xDist
1736 << " yd=" << yDist
1737 << " vMax=" << myStage->getMaxSpeed(myPerson)
1738 << " wTime=" << myStage->getWaitingTime(currentTime)
1739 << " jammed=" << myAmJammed
1740 << "\n distance=" << toString(distance)
1741 << "\n utility=" << toString(utility)
1742 << "\n";
1743 DEBUG_PRINT(obs);
1744 }
1745 myRelX += SPEED2DIST(xSpeed * myDir);
1746 myRelY += SPEED2DIST(ySpeed);
1747 mySpeed = xSpeed;
1748 if (xSpeed >= SUMO_const_haltingSpeed) {
1749 myWaitingToEnter = false;
1750 myWaitingTime = 0;
1751 } else {
1752 myWaitingTime += DELTA_T;
1753 }
1754 }
1755
1756
1757 double
getImpatience(SUMOTime now) const1758 MSPModel_Striping::PState::getImpatience(SUMOTime now) const {
1759 return MAX2(0., MIN2(1., myPerson->getVehicleType().getImpatience()
1760 + STEPS2TIME(myStage->getWaitingTime(now)) / MAX_WAIT_TOLERANCE));
1761 }
1762
1763
1764 double
getEdgePos(const MSPerson::MSPersonStage_Walking &,SUMOTime) const1765 MSPModel_Striping::PState::getEdgePos(const MSPerson::MSPersonStage_Walking&, SUMOTime) const {
1766 return myRelX;
1767 }
1768
1769
1770 Position
getPosition(const MSPerson::MSPersonStage_Walking & stage,SUMOTime) const1771 MSPModel_Striping::PState::getPosition(const MSPerson::MSPersonStage_Walking& stage, SUMOTime) const {
1772 if (myLane == nullptr) {
1773 // pedestrian has already finished
1774 return Position::INVALID;
1775 }
1776 const double lateral_offset = myRelY + (stripeWidth - myLane->getWidth()) * 0.5;
1777 if (myWalkingAreaPath == nullptr) {
1778 return stage.getLanePosition(myLane, myRelX, lateral_offset);
1779 } else {
1780 //if DEBUGCOND(*this) {
1781 // std::cout << SIMTIME
1782 // << " getPosition (walkingArea)"
1783 // << " p=" << myPerson->getID()
1784 // << " x=" << myRelX
1785 // << " y=" << myRelY
1786 // << " latOffset=" << lateral_offset
1787 // << " shape=" << myWalkingAreaPath->shape
1788 // << " pos=" << myWalkingAreaPath->shape.positionAtOffset(myRelX, lateral_offset)
1789 // << "\n";
1790 //}
1791 return myWalkingAreaPath->shape.positionAtOffset(myRelX, lateral_offset);
1792 }
1793 }
1794
1795
1796 double
getAngle(const MSPerson::MSPersonStage_Walking &,SUMOTime) const1797 MSPModel_Striping::PState::getAngle(const MSPerson::MSPersonStage_Walking&, SUMOTime) const {
1798 if (myAngle != std::numeric_limits<double>::max()) {
1799 return myAngle;
1800 }
1801 if (myLane == nullptr) {
1802 // pedestrian has already finished
1803 return 0;
1804 }
1805 const PositionVector& shp = myWalkingAreaPath == nullptr ? myLane->getShape() : myWalkingAreaPath->shape;
1806 double angle = shp.rotationAtOffset(myRelX) + (myDir == MSPModel::BACKWARD ? M_PI : 0);
1807 if (angle > M_PI) {
1808 angle -= 2 * M_PI;
1809 }
1810 myAngle = angle;
1811 return angle;
1812 }
1813
1814
1815 SUMOTime
getWaitingTime(const MSPerson::MSPersonStage_Walking &,SUMOTime) const1816 MSPModel_Striping::PState::getWaitingTime(const MSPerson::MSPersonStage_Walking&, SUMOTime) const {
1817 return myWaitingTime;
1818 }
1819
1820
1821 double
getSpeed(const MSPerson::MSPersonStage_Walking &) const1822 MSPModel_Striping::PState::getSpeed(const MSPerson::MSPersonStage_Walking&) const {
1823 return mySpeed;
1824 }
1825
1826
1827 const MSEdge*
getNextEdge(const MSPerson::MSPersonStage_Walking &) const1828 MSPModel_Striping::PState::getNextEdge(const MSPerson::MSPersonStage_Walking&) const {
1829 return myNLI.lane == nullptr ? nullptr : &myNLI.lane->getEdge();
1830 }
1831
1832 void
moveToXY(MSPerson * p,Position pos,MSLane * lane,double lanePos,double lanePosLat,double angle,int routeOffset,const ConstMSEdgeVector & edges,SUMOTime t)1833 MSPModel_Striping::PState::moveToXY(MSPerson* p, Position pos, MSLane* lane, double lanePos,
1834 double lanePosLat, double angle, int routeOffset,
1835 const ConstMSEdgeVector& edges, SUMOTime t) {
1836 UNUSED_PARAMETER(p);
1837 assert(p == myPerson);
1838 myAngle = angle;
1839 myAngle = GeomHelper::fromNaviDegree(angle);
1840 /*
1841 std::cout << " MSPModel_Striping::PState::moveToXY"
1842 << " pos=" << pos
1843 << " lane=" << lane->getID()
1844 << " lanePos=" << lanePos
1845 << " lanePosLat=" << lanePosLat
1846 << " angle=" << angle
1847 << " routeOffset=" << routeOffset
1848 << " myRelX=" << myRelX << " myRelY=" << myRelY;
1849 */
1850 //std::cout << " newX=" << myRelX << " newY=" << myRelY << "\n";
1851 if (lane != nullptr) {
1852 myRemoteXYPos = Position::INVALID;
1853 const MSLane* sidewalk = getSidewalk<MSEdge, MSLane>(&lane->getEdge());
1854 if (lane != sidewalk) {
1855 MSPModel_Striping* pm = dynamic_cast<MSPModel_Striping*>(MSPModel::getModel());
1856 assert(pm != 0);
1857 // add a new active lane
1858 pm->remove(this);
1859 pm->add(this, lane);
1860 }
1861 if (edges.empty()) {
1862 // map within route
1863 myStage->setRouteIndex(myPerson, routeOffset);
1864 if (lane->getEdge().isInternal()) {
1865 myStage->moveToNextEdge(myPerson, t, &lane->getEdge());
1866 }
1867 } else {
1868 // map to new edge
1869 }
1870 myLane = lane;
1871 myRelX = lanePos;
1872 myRelY = (myLane->getWidth() - stripeWidth) * 0.5 - lanePosLat;
1873 } else {
1874 // map outside the network
1875 myRemoteXYPos = pos;
1876 }
1877
1878 }
1879
1880
1881
1882 double
distanceTo(const Obstacle & obs,const bool includeMinGap) const1883 MSPModel_Striping::PState::distanceTo(const Obstacle& obs, const bool includeMinGap) const {
1884 // check for overlap
1885 const double maxX = getMaxX(includeMinGap);
1886 const double minX = getMinX(includeMinGap);
1887 //if (DEBUGCOND(*this)) {
1888 // std::cout << std::setprecision(2) << " distanceTo=" << obs.description << " maxX=" << maxX << " minX=" << minX << " obs.xFwd=" << obs.xFwd << " obs.xBack=" << obs.xBack << "\n";
1889 //}
1890 if ((obs.xFwd >= maxX && obs.xBack <= maxX) || (obs.xFwd <= maxX && obs.xFwd >= minX)) {
1891 // avoid blocking by itself on looped route
1892 return (obs.type == OBSTACLE_PED && obs.description == myPerson->getID()) ? DIST_FAR_AWAY : DIST_OVERLAP;
1893 }
1894 if (myDir == FORWARD) {
1895 return obs.xFwd < minX ? DIST_BEHIND : obs.xBack - maxX;
1896 } else {
1897 return obs.xBack > maxX ? DIST_BEHIND : minX - obs.xFwd;
1898 }
1899 }
1900
1901
1902 void
mergeObstacles(Obstacles & into,const Obstacles & obs2)1903 MSPModel_Striping::PState::mergeObstacles(Obstacles& into, const Obstacles& obs2) {
1904 for (int i = 0; i < (int)into.size(); ++i) {
1905 if (gDebugFlag1) {
1906 std::cout << " i=" << i << " maxX=" << getMaxX(true) << " minX=" << getMinX(true)
1907 << " into=" << into[i].description << " iDist=" << distanceTo(into[i], into[i].type == OBSTACLE_PED)
1908 << " obs2=" << obs2[i].description << " oDist=" << distanceTo(obs2[i], obs2[i].type == OBSTACLE_PED) << "\n";
1909 }
1910 const double dO = distanceTo(obs2[i], obs2[i].type == OBSTACLE_PED);
1911 const double dI = distanceTo(into[i], into[i].type == OBSTACLE_PED);
1912 if (dO < dI) {
1913 into[i] = obs2[i];
1914 } else if (dO == dI
1915 && into[i].type != OBSTACLE_PED
1916 && into[i].type != OBSTACLE_VEHICLE
1917 && (obs2[i].type == OBSTACLE_PED ||
1918 obs2[i].type == OBSTACLE_VEHICLE)) {
1919 into[i] = obs2[i];
1920 }
1921 }
1922 }
1923
1924 void
mergeObstacles(Obstacles & into,const Obstacles & obs2,int dir,int offset)1925 MSPModel_Striping::PState::mergeObstacles(Obstacles& into, const Obstacles& obs2, int dir, int offset) {
1926 for (int i = 0; i < (int)into.size(); ++i) {
1927 int i2 = i + offset;
1928 if (i2 >= 0 && i2 < (int)obs2.size()) {
1929 if (dir == FORWARD) {
1930 if (obs2[i2].xBack < into[i].xBack) {
1931 into[i] = obs2[i2];
1932 }
1933 } else {
1934 if (obs2[i2].xFwd > into[i].xFwd) {
1935 into[i] = obs2[i2];
1936 }
1937 }
1938 }
1939 }
1940 }
1941
1942
1943 bool
ignoreRed(const MSLink * link) const1944 MSPModel_Striping::PState::ignoreRed(const MSLink* link) const {
1945 if (link->haveRed()) {
1946 const double ignoreRedTime = myPerson->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_AFTER_RED_TIME, -1);
1947 if (ignoreRedTime >= 0) {
1948 const double redDuration = STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - link->getLastStateChange());
1949 if (DEBUGCOND(*this)) {
1950 std::cout << SIMTIME << " ignoreRedTime=" << ignoreRedTime << " redDuration=" << redDuration << "\n";
1951 }
1952 return ignoreRedTime > redDuration;
1953 } else {
1954 return false;
1955 }
1956 } else {
1957 return false;
1958 }
1959 }
1960
1961 const std::string&
getID() const1962 MSPModel_Striping::PState::getID() const {
1963 return myPerson->getID();
1964 }
1965
1966 double
getWidth() const1967 MSPModel_Striping::PState::getWidth() const {
1968 return myPerson->getVehicleType().getWidth();
1969 }
1970
1971 // ===========================================================================
1972 // MSPModel_Striping::PStateVehicle method definitions
1973 // ===========================================================================
1974
PStateVehicle(const MSVehicle * veh,const MSLane * walkingarea,double relX,double relY)1975 MSPModel_Striping::PStateVehicle::PStateVehicle(const MSVehicle* veh, const MSLane* walkingarea, double relX, double relY):
1976 myVehicle(veh) {
1977 myLane = walkingarea; // to ensure correct limits when calling otherStripe()
1978 myRelX = relX;
1979 myRelY = relY;
1980 }
1981
1982 const std::string&
getID() const1983 MSPModel_Striping::PStateVehicle::getID() const {
1984 return myVehicle->getID();
1985 }
1986
1987 double
getWidth() const1988 MSPModel_Striping::PStateVehicle::getWidth() const {
1989 return myVehicle->getVehicleType().getWidth();
1990 }
1991
1992 double
getMinX(const bool) const1993 MSPModel_Striping::PStateVehicle::getMinX(const bool /*includeMinGap*/) const {
1994 return myRelX - myVehicle->getVehicleType().getWidth() / 2 - SAFETY_GAP ;
1995 }
1996
1997 double
getMaxX(const bool) const1998 MSPModel_Striping::PStateVehicle::getMaxX(const bool /*includeMinGap*/) const {
1999 return myRelX + myVehicle->getVehicleType().getWidth() / 2 + SAFETY_GAP;
2000 }
2001
2002 // ===========================================================================
2003 // MSPModel_Striping::MovePedestrians method definitions
2004 // ===========================================================================
2005
2006 SUMOTime
execute(SUMOTime currentTime)2007 MSPModel_Striping::MovePedestrians::execute(SUMOTime currentTime) {
2008 std::set<MSPerson*> changedLane;
2009 myModel->moveInDirection(currentTime, changedLane, FORWARD);
2010 myModel->moveInDirection(currentTime, changedLane, BACKWARD);
2011 // DEBUG
2012 #ifdef LOG_ALL
2013 for (ActiveLanes::const_iterator it_lane = myModel->getActiveLanes().begin(); it_lane != myModel->getActiveLanes().end(); ++it_lane) {
2014 const MSLane* lane = it_lane->first;
2015 Pedestrians pedestrians = it_lane->second;
2016 if (pedestrians.size() == 0) {
2017 continue;
2018 }
2019 sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(FORWARD));
2020 std::cout << SIMTIME << " lane=" << lane->getID();
2021 for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
2022 const PState& p = *pedestrians[ii];
2023 std::cout << " (" << p.myPerson->getID() << " " << p.myRelX << "," << p.myRelY << " " << p.myDir << ")";
2024 }
2025 std::cout << "\n";
2026 }
2027 #endif
2028 return DELTA_T;
2029 }
2030
2031