Eclipse SUMO - Simulation of Urban MObility
MSPModel_Striping.cpp
Go to the documentation of this file.
1/****************************************************************************/
2// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3// Copyright (C) 2014-2022 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/****************************************************************************/
19// The pedestrian following model (prototype)
20/****************************************************************************/
21#include <config.h>
22
23#include <cmath>
24#include <algorithm>
29#include <microsim/MSNet.h>
30#include <microsim/MSEdge.h>
32#include <microsim/MSLane.h>
33#include <microsim/MSLink.h>
34#include <microsim/MSJunction.h>
37#include <microsim/MSGlobals.h>
40#include "MSPModel_Striping.h"
41
42
43// ===========================================================================
44// DEBUGGING HELPERS
45// ===========================================================================
46//
47#define DEBUGID1 ""
48#define DEBUGID2 ""
49//#define DEBUGCOND(PED) (false)
50//#define DEBUGCOND(PED) ((PED).myPerson->getID() == DEBUGID1 || (PED).myPerson->getID() == DEBUGID2)
51#define DEBUGCOND(PED) ((PED).myPerson->isSelected())
52#define DEBUGCOND2(LANE) ((LANE)->isSelected())
53//#define LOG_ALL 1
54//#define DEBUG_MOVETOXY
55
57 for (int i = 0; i < (int)obs.size(); ++i) {
58 std::cout
59 << "(" << obs[i].description
60 << " x=(" << obs[i].xBack << "," << obs[i].xFwd
61 << ") s=" << obs[i].speed
62 << ") ";
63 }
64 std::cout << "\n";
65}
66
67// ===========================================================================
68// named (internal) constants
69// ===========================================================================
70
71// distances are comparable with lower values being "more important"
72const double MSPModel_Striping::DIST_FAR_AWAY(10000);
73const double MSPModel_Striping::DIST_BEHIND(1000);
74const double MSPModel_Striping::DIST_OVERLAP(-1);
75
76// ===========================================================================
77// static members
78// ===========================================================================
79
81std::map<const MSEdge*, std::vector<const MSLane*> > MSPModel_Striping::myWalkingAreaFoes;
84
85// model parameters (static to simplify access from class PState
94const double MSPModel_Striping::LOOKAHEAD_SAMEDIR(4.0); // seconds
95const double MSPModel_Striping::LOOKAHEAD_ONCOMING(10.0); // seconds
96const double MSPModel_Striping::LOOKAROUND_VEHICLES(60.0); // meters
97const double MSPModel_Striping::LATERAL_PENALTY(-1.); // meters
98const double MSPModel_Striping::OBSTRUCTED_PENALTY(-300000.); // meters
99const double MSPModel_Striping::INAPPROPRIATE_PENALTY(-20000.); // meters
100const double MSPModel_Striping::ONCOMING_CONFLICT_PENALTY(-1000.); // meters
101const double MSPModel_Striping::OBSTRUCTION_THRESHOLD(MSPModel_Striping::OBSTRUCTED_PENALTY * 0.5); // despite obstruction, additional utility may have been added
102const double MSPModel_Striping::SQUEEZE(0.7);
105const double MSPModel_Striping::MAX_WAIT_TOLERANCE(120.); // seconds
107const double MSPModel_Striping::MIN_STARTUP_DIST(0.4); // meters
108
109
110// ===========================================================================
111// MSPModel_Striping method definitions
112// ===========================================================================
113
115 myNumActivePedestrians(0),
116 myAmActive(false) {
117 myWalkingAreaDetail = oc.getInt("pedestrian.striping.walkingarea-detail");
119 // configurable parameters
120 stripeWidth = oc.getFloat("pedestrian.striping.stripe-width");
122 if (defaultPedType != nullptr && defaultPedType->getWidth() > stripeWidth) {
123 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 dawdling = oc.getFloat("pedestrian.striping.dawdling");
128 minGapToVehicle = oc.getFloat("pedestrian.striping.mingap-to-vehicle");
129 RESERVE_FOR_ONCOMING_FACTOR = oc.getFloat("pedestrian.striping.reserve-oncoming");
130 RESERVE_FOR_ONCOMING_FACTOR_JUNCTIONS = oc.getFloat("pedestrian.striping.reserve-oncoming.junctions");
131
132 jamTime = string2time(oc.getString("pedestrian.striping.jamtime"));
133 if (jamTime <= 0) {
135 }
136 jamTimeCrossing = string2time(oc.getString("pedestrian.striping.jamtime.crossing"));
137 if (jamTimeCrossing <= 0) {
139 }
140 jamTimeNarrow = string2time(oc.getString("pedestrian.striping.jamtime.narrow"));
141 if (jamTimeNarrow <= 0) {
143 }
144 myLegacyPosLat = oc.getBool("pedestrian.striping.legacy-departposlat");
145}
146
147
149 clearState();
150 myWalkingAreaPaths.clear(); // need to recompute when lane pointers change
151 myWalkingAreaFoes.clear();
152 myMinNextLengths.clear();
153}
154
155void
157 myActiveLanes.clear();
159 myAmActive = false;
160}
161
164 if (!transportable->isPerson()) {
165 // containers are not supported (TODO add a warning here?)
166 return nullptr;
167 }
168 MSPerson* person = static_cast<MSPerson*>(transportable);
169 MSNet* net = MSNet::getInstance();
170 if (!myAmActive) {
172 myAmActive = true;
173 }
174 assert(person->getCurrentStageType() == MSStageType::WALKING);
175 const MSLane* lane = stage->checkDepartLane(person->getEdge(), person->getVClass(), stage->getDepartLane(), person->getID());
176 if (lane == nullptr) {
177 std::string error = "Person '" + person->getID() + "' could not find sidewalk on edge '" + person->getEdge()->getID() + "', time="
178 + time2string(net->getCurrentTimeStep()) + ".";
179 if (OptionsCont::getOptions().getBool("ignore-route-errors")) {
180 WRITE_WARNING(error);
181 return nullptr;
182 } else {
183 throw ProcessError(error);
184 }
185 }
186 PState* ped = new PState(person, stage, lane);
187 myActiveLanes[lane].push_back(ped);
189 return ped;
190}
191
192
194MSPModel_Striping::loadState(MSTransportable* transportable, MSStageMoving* stage, std::istringstream& in) {
195 MSPerson* person = static_cast<MSPerson*>(transportable);
196 MSNet* net = MSNet::getInstance();
197 if (!myAmActive) {
199 myAmActive = true;
200 }
201 PState* ped = new PState(person, stage, &in);
202 myActiveLanes[ped->getLane()].push_back(ped);
204 return ped;
205}
206
207
208void
210 const MSLane* lane = dynamic_cast<PState*>(state)->myLane;
211 Pedestrians& pedestrians = myActiveLanes[lane];
212 for (Pedestrians::iterator it = pedestrians.begin(); it != pedestrians.end(); ++it) {
213 if (*it == state) {
214 pedestrians.erase(it);
216 return;
217 }
218 }
219}
220
221
222bool
223MSPModel_Striping::blockedAtDist(const MSLane* lane, double vehSide, double vehWidth,
224 double oncomingGap, std::vector<const MSPerson*>* collectBlockers) {
225 const Pedestrians& pedestrians = getPedestrians(lane);
226 for (Pedestrians::const_iterator it_ped = pedestrians.begin(); it_ped != pedestrians.end(); ++it_ped) {
227 const PState& ped = **it_ped;
228 const double leaderFrontDist = (ped.myDir == FORWARD ? vehSide - ped.myRelX : ped.myRelX - vehSide);
229 const double leaderBackDist = leaderFrontDist + ped.getLength();
230 if DEBUGCOND(ped) {
231 std::cout << SIMTIME << " lane=" << lane->getID() << " dir=" << ped.myDir << " pX=" << ped.myRelX << " pL=" << ped.getLength()
232 << " vehSide=" << vehSide
233 << " vehWidth=" << vehWidth
234 << " lBD=" << leaderBackDist
235 << " lFD=" << leaderFrontDist
236 << "\n";
237 }
238 if (leaderBackDist >= -vehWidth
239 && (leaderFrontDist < 0
240 // give right of way to (close) approaching pedestrians unless they are standing
241 || (leaderFrontDist <= oncomingGap && ped.myWaitingTime < TIME2STEPS(2.0)))) {
242 // found one pedestrian that is not completely past the crossing point
243 //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";
244 if (collectBlockers == nullptr) {
245 return true;
246 } else {
247 collectBlockers->push_back(ped.myPerson);
248 }
249 }
250 }
251 if (collectBlockers == nullptr) {
252 return false;
253 } else {
254 return collectBlockers->size() > 0;
255 }
256}
257
258
259bool
261 return getPedestrians(lane).size() > 0;
262}
263
264
265bool
268}
269
270bool
273}
274
276MSPModel_Striping::nextBlocking(const MSLane* lane, double minPos, double minRight, double maxLeft, double stopTime, bool bidi) {
277 PersonDist result((const MSPerson*)nullptr, std::numeric_limits<double>::max());
278 const Pedestrians& pedestrians = getPedestrians(lane);
279 for (Pedestrians::const_iterator it_ped = pedestrians.begin(); it_ped != pedestrians.end(); ++it_ped) {
280 const PState& ped = **it_ped;
281 // account for distance covered by oncoming pedestrians
282 double relX2 = ped.myRelX - (ped.myDir == FORWARD ? 0 : stopTime * ped.myPerson->getMaxSpeed());
283 double dist = ((relX2 - minPos) * (bidi ? -1 : 1)
284 - (ped.myDir == FORWARD ? ped.myPerson->getVehicleType().getLength() : 0));
285 const bool aheadOfVehicle = bidi ? ped.myRelX < minPos : ped.myRelX > minPos;
286 if (aheadOfVehicle && dist < result.second) {
287 const double center = lane->getWidth() - (ped.myRelY + stripeWidth * 0.5);
288 const double halfWidth = 0.5 * ped.myPerson->getVehicleType().getWidth();
289 const bool overlap = (center + halfWidth > minRight && center - halfWidth < maxLeft);
290 if DEBUGCOND(ped) {
291 std::cout << " nextBlocking lane=" << lane->getID() << " bidi=" << bidi
292 << " minPos=" << minPos << " minRight=" << minRight << " maxLeft=" << maxLeft
293 << " stopTime=" << stopTime
294 << " pedY=" << ped.myRelY
295 << " pedX=" << ped.myRelX
296 << " relX2=" << relX2
297 << " center=" << center
298 << " pedLeft=" << center + halfWidth
299 << " pedRight=" << center - halfWidth
300 << " overlap=" << overlap
301 << "\n";
302 }
303 if (overlap) {
304 result.first = ped.myPerson;
305 result.second = dist;
306 }
307 }
308 }
309 return result;
310}
311
312
315 ActiveLanes::iterator it = myActiveLanes.find(lane);
316 if (it != myActiveLanes.end()) {
317 //std::cout << " found lane=" << lane->getID() << " n=" << it->second.size() << "\n";
318 return (it->second);
319 } else {
320 return noPedestrians;
321 }
322}
323
324
325int
327 return MAX2(1, (int)floor(lane->getWidth() / stripeWidth));
328}
329
330int
332 if (from == nullptr || to == nullptr) {
333 return UNDEFINED_DIRECTION;
334 } else if (from->getLinkTo(to) != nullptr) {
335 return FORWARD;
336 } else if (to->getLinkTo(from) != nullptr) {
337 return BACKWARD;
338 } else {
339 return UNDEFINED_DIRECTION;
340 }
341}
342
343
344void
346 if (myWalkingAreaPaths.size() > 0) {
347 return;
348 }
349 // collect vehicle lanes that cross walkingareas
350 for (MSEdgeVector::const_iterator i = MSEdge::getAllEdges().begin(); i != MSEdge::getAllEdges().end(); ++i) {
351 const MSEdge* edge = *i;
352 if (!edge->isWalkingArea() && !edge->isCrossing()) {
353 for (MSLane* lane : edge->getLanes()) {
354 for (MSLink* link : lane->getLinkCont()) {
355 if (link->getWalkingAreaFoe() != nullptr) {
356 // link is an exit link
357 myWalkingAreaFoes[&link->getWalkingAreaFoe()->getEdge()].push_back(link->getLaneBefore());
358 //std::cout << " wa=" << link->getWalkingAreaFoe()->getEdge().getID() << " foe=" << link->getLaneBefore()->getID() << "\n";
359 }
360 if (link->getWalkingAreaFoeExit() != nullptr) {
361 // link is an exit link
362 myWalkingAreaFoes[&link->getWalkingAreaFoeExit()->getEdge()].push_back(link->getLaneBefore());
363 //std::cout << " wa=" << link->getWalkingAreaFoeExit()->getEdge().getID() << " foe=" << link->getLaneBefore()->getID() << "\n";
364 }
365 }
366 }
367 }
368 }
369
370 // build walkingareaPaths
371 for (MSEdgeVector::const_iterator i = MSEdge::getAllEdges().begin(); i != MSEdge::getAllEdges().end(); ++i) {
372 const MSEdge* edge = *i;
373 if (edge->isWalkingArea()) {
374 const MSLane* walkingArea = getSidewalk<MSEdge, MSLane>(edge);
375 myMinNextLengths[walkingArea] = walkingArea->getLength();
376 // build all possible paths across this walkingArea
377 // gather all incident lanes
378 std::vector<const MSLane*> lanes;
379 for (const MSEdge* in : edge->getPredecessors()) {
380 if (!in->isTazConnector()) {
381 lanes.push_back(getSidewalk<MSEdge, MSLane>(in));
382 if (lanes.back() == nullptr) {
383 throw ProcessError("Invalid connection from edge '" + in->getID() + "' to walkingarea edge '" + edge->getID() + "'");
384 }
385 }
386 }
387 for (const MSEdge* out : edge->getSuccessors()) {
388 if (!out->isTazConnector()) {
389 lanes.push_back(getSidewalk<MSEdge, MSLane>(out));
390 if (lanes.back() == nullptr) {
391 throw ProcessError("Invalid connection from walkingarea edge '" + edge->getID() + "' to edge '" + out->getID() + "'");
392 }
393 }
394 }
395 // build all combinations
396 for (int j = 0; j < (int)lanes.size(); ++j) {
397 for (int k = 0; k < (int)lanes.size(); ++k) {
398 if (j != k) {
399 // build the walkingArea
400 const MSLane* const from = lanes[j];
401 const MSLane* const to = lanes[k];
402 const int fromDir = from->getLinkTo(walkingArea) != nullptr ? FORWARD : BACKWARD;
403 const int toDir = walkingArea->getLinkTo(to) != nullptr ? FORWARD : BACKWARD;
404 PositionVector shape;
405 Position fromPos = from->getShape()[fromDir == FORWARD ? -1 : 0];
406 Position toPos = to->getShape()[toDir == FORWARD ? 0 : -1];
407 const double maxExtent = fromPos.distanceTo2D(toPos) / 4; // prevent sharp corners
408 const double extrapolateBy = MIN2(maxExtent, walkingArea->getWidth() / 2);
409 // assemble shape
410 shape.push_back(fromPos);
411 if (extrapolateBy > POSITION_EPS) {
412 PositionVector fromShp = from->getShape();
413 fromShp.extrapolate(extrapolateBy);
414 shape.push_back_noDoublePos(fromDir == FORWARD ? fromShp.back() : fromShp.front());
415 PositionVector nextShp = to->getShape();
416 nextShp.extrapolate(extrapolateBy);
417 shape.push_back_noDoublePos(toDir == FORWARD ? nextShp.front() : nextShp.back());
418 }
419 shape.push_back_noDoublePos(toPos);
420 if (shape.size() < 2) {
421 PositionVector fromShp = from->getShape();
422 fromShp.extrapolate(1.5 * POSITION_EPS); // noDoublePos requires a difference of POSITION_EPS in at least one coordinate
423 shape.push_back_noDoublePos(fromDir == FORWARD ? fromShp.back() : fromShp.front());
424 assert(shape.size() == 2);
425 } else if (myWalkingAreaDetail > 4) {
426 shape = shape.bezier(myWalkingAreaDetail);
427 }
428 if (fromDir == BACKWARD) {
429 // will be walking backward on walkingArea
430 shape = shape.reverse();
431 }
432 WalkingAreaPath wap = WalkingAreaPath(from, walkingArea, to, shape, fromDir);
433 myWalkingAreaPaths.insert(std::make_pair(std::make_pair(from, to), wap));
434 myMinNextLengths[walkingArea] = MIN2(myMinNextLengths[walkingArea], wap.length);
435 }
436 }
437 }
438 }
439 }
440}
441
442
445 assert(walkingArea->isWalkingArea());
446 std::vector<const MSLane*> lanes;
447 for (const MSEdge* const pred : walkingArea->getPredecessors()) {
448 lanes.push_back(getSidewalk<MSEdge, MSLane>(pred));
449 }
450 for (const MSEdge* const succ : walkingArea->getSuccessors()) {
451 lanes.push_back(getSidewalk<MSEdge, MSLane>(succ));
452 }
453 if (lanes.size() < 1) {
454 throw ProcessError("Invalid walkingarea '" + walkingArea->getID() + "' does not allow continuation.");
455 }
456 return &myWalkingAreaPaths.find(std::make_pair(lanes.front(), lanes.back()))->second;
457}
458
460MSPModel_Striping::guessPath(const MSEdge* walkingArea, const MSEdge* before, const MSEdge* after) {
461 assert(walkingArea->isWalkingArea());
462 const MSLane* swBefore = getSidewalk<MSEdge, MSLane>(before);
463 const MSLane* swAfter = getSidewalk<MSEdge, MSLane>(after);
464 const auto pathIt = myWalkingAreaPaths.find(std::make_pair(swBefore, swAfter));
465 if (pathIt != myWalkingAreaPaths.end()) {
466 return &pathIt->second;
467 }
468 const MSEdgeVector& preds = walkingArea->getPredecessors();
469 const MSEdgeVector& succs = walkingArea->getSuccessors();
470 bool useBefore = swBefore != nullptr && std::find(preds.begin(), preds.end(), before) != preds.end();
471 bool useAfter = swAfter != nullptr && std::find(succs.begin(), succs.end(), after) != succs.end();
472 if (useBefore) {
473 if (useAfter) {
474 return getWalkingAreaPath(walkingArea, swBefore, swAfter);
475 } else if (succs.size() > 0) {
476 // could also try to exploit direction
477 return getWalkingAreaPath(walkingArea, swBefore, getSidewalk<MSEdge, MSLane>(succs.front()));
478 }
479 } else if (useAfter && preds.size() > 0) {
480 // could also try to exploit direction
481 return getWalkingAreaPath(walkingArea, getSidewalk<MSEdge, MSLane>(preds.front()), swAfter);
482 }
483 return getArbitraryPath(walkingArea);
484}
485
486
488MSPModel_Striping::getWalkingAreaPath(const MSEdge* walkingArea, const MSLane* before, const MSLane* after) {
489 assert(walkingArea->isWalkingArea());
490 const auto pathIt = myWalkingAreaPaths.find(std::make_pair(before, after));
491 if (pathIt != myWalkingAreaPaths.end()) {
492 return &pathIt->second;
493 } else {
494 // this can happen in case of moveToXY where before can point anywhere
495 const MSEdgeVector& preds = walkingArea->getPredecessors();
496 if (preds.size() > 0) {
497 const MSEdge* const pred = walkingArea->getPredecessors().front();
498 const auto pathIt2 = myWalkingAreaPaths.find(std::make_pair(getSidewalk<MSEdge, MSLane>(pred), after));
499 assert(pathIt2 != myWalkingAreaPaths.end());
500 return &pathIt2->second;
501 } else {
502 return getArbitraryPath(walkingArea);
503 }
504 }
505}
506
507
508
510MSPModel_Striping::getNextLane(const PState& ped, const MSLane* currentLane, const MSLane* prevLane) {
511 const MSEdge* currentEdge = &currentLane->getEdge();
512 const MSJunction* junction = ped.myDir == FORWARD ? currentEdge->getToJunction() : currentEdge->getFromJunction();
513 const MSEdge* nextRouteEdge = ped.myStage->getNextRouteEdge();
514 const MSLane* nextRouteLane = getSidewalk<MSEdge, MSLane>(nextRouteEdge, ped.myPerson->getVClass());
515 // result values
516 const MSLane* nextLane = nextRouteLane;
517 const MSLink* link = nullptr;
518 int nextDir = UNDEFINED_DIRECTION;
519
520 //if DEBUGCOND(ped) {
521 // std::cout << " nextRouteLane=" << Named::getIDSecure(nextRouteLane) << " junction=" << junction->getID() << "\n";
522 //}
523 if (nextRouteLane == nullptr && nextRouteEdge != nullptr) {
524 std::string error = "Person '" + ped.myPerson->getID() + "' could not find sidewalk on edge '" + nextRouteEdge->getID() + "', time="
525 + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".";
526 if (OptionsCont::getOptions().getBool("ignore-route-errors")) {
527 WRITE_WARNING(error);
528 nextRouteLane = nextRouteEdge->getLanes().front();
529 } else {
530 throw ProcessError(error);
531 }
532 }
533
534 if (nextRouteLane != nullptr) {
535 if (currentEdge->isInternal()) {
536 assert(junction == currentEdge->getFromJunction());
537 nextDir = junction == nextRouteEdge->getFromJunction() ? FORWARD : BACKWARD;
538 if (nextDir == FORWARD) {
539 nextLane = currentLane->getLinkCont()[0]->getViaLaneOrLane();
540 } else {
541 nextLane = currentLane->getLogicalPredecessorLane();
542 }
543 if DEBUGCOND(ped) {
544 std::cout << " internal\n";
545 }
546 } else if (currentEdge->isCrossing()) {
547 nextDir = ped.myDir;
548 if (ped.myDir == FORWARD) {
549 nextLane = currentLane->getLinkCont()[0]->getLane();
550 } else {
551 nextLane = currentLane->getLogicalPredecessorLane();
552 }
553 if DEBUGCOND(ped) {
554 std::cout << " crossing\n";
555 }
556 } else if (currentEdge->isWalkingArea()) {
557 ConstMSEdgeVector crossingRoute;
558 // departPos can be 0 because the direction of the walkingArea does not matter
559 // for the arrivalPos, we need to make sure that the route does not deviate across other junctions
560 const int nextRouteEdgeDir = nextRouteEdge->getFromJunction() == junction ? FORWARD : BACKWARD;
561 const double arrivalPos = (nextRouteEdge == ped.myStage->getRoute().back()
562 ? ped.myStage->getArrivalPos()
563 : (nextRouteEdgeDir == FORWARD ? 0 : nextRouteEdge->getLength()));
564 MSEdgeVector prohibited;
565 if (prevLane != nullptr) {
566 prohibited.push_back(&prevLane->getEdge());
567 }
568 MSNet::getInstance()->getPedestrianRouter(0, prohibited).compute(currentEdge, nextRouteEdge, 0, arrivalPos, ped.myStage->getMaxSpeed(ped.myPerson), 0, junction, crossingRoute, true);
569 if DEBUGCOND(ped) {
570 std::cout
571 << " nre=" << nextRouteEdge->getID()
572 << " nreDir=" << nextRouteEdgeDir
573 << " aPos=" << arrivalPos
574 << " crossingRoute=" << toString(crossingRoute)
575 << "\n";
576 }
577 if (crossingRoute.size() > 1) {
578 const MSEdge* nextEdge = crossingRoute[1];
579 nextLane = getSidewalk<MSEdge, MSLane>(crossingRoute[1], ped.myPerson->getVClass());
580 assert((nextEdge->getFromJunction() == junction || nextEdge->getToJunction() == junction));
581 assert(nextLane != prevLane);
582 nextDir = connectedDirection(currentLane, nextLane);
583 if DEBUGCOND(ped) {
584 std::cout << " nextDir=" << nextDir << "\n";
585 }
586 assert(nextDir != UNDEFINED_DIRECTION);
587 if (nextDir == FORWARD) {
588 link = currentLane->getLinkTo(nextLane);
589 } else {
590 link = nextLane->getLinkTo(currentLane);
591 if (nextEdge->isCrossing() && link->getTLLogic() == nullptr) {
592 const MSLane* oppositeWalkingArea = nextLane->getLogicalPredecessorLane();
593 link = oppositeWalkingArea->getLinkTo(nextLane);
594 }
595 }
596 assert(link != nullptr);
597 } else {
598 if DEBUGCOND(ped) {
599 std::cout << SIMTIME
600 << " no route from '" << (currentEdge == nullptr ? "NULL" : currentEdge->getID())
601 << "' to '" << (nextRouteEdge == nullptr ? "NULL" : nextRouteEdge->getID())
602 << "\n";
603 }
604 WRITE_WARNING("Person '" + ped.myPerson->getID() + "' could not find route across junction '" + junction->getID()
605 + "' from walkingArea '" + currentEdge->getID()
606 + "' to edge '" + nextRouteEdge->getID() + "', time=" +
607 time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
608 // error indicated by nextDir == UNDEFINED_DIRECTION
609 nextLane = nextRouteLane;
610 }
611 } else if (currentEdge == nextRouteEdge) {
612 // strange loop in this route. No need to use walkingArea
613 nextDir = -ped.myDir;
614 } else {
615 // normal edge. by default use next / previous walking area
616 nextDir = ped.myDir;
617 nextLane = getNextWalkingArea(currentLane, ped.myDir, link);
618 if (nextLane != nullptr) {
619 // walking area found
620 if DEBUGCOND(ped) {
621 std::cout << " next walkingArea " << (nextDir == FORWARD ? "forward" : "backward") << "\n";
622 }
623 } else {
624 // walk forward by default
625 nextDir = junction == nextRouteEdge->getToJunction() ? BACKWARD : FORWARD;
626 // try to use a direct link as fallback
627 // direct links only exist if built explicitly. They are used to model tl-controlled links if there are no crossings
628 if (ped.myDir == FORWARD) {
629 link = currentLane->getLinkTo(nextRouteLane);
630 if (link != nullptr) {
631 if DEBUGCOND(ped) {
632 std::cout << " direct forward\n";
633 }
634 nextLane = currentLane->getInternalFollowingLane(nextRouteLane);
635 }
636 } else {
637 link = nextRouteLane->getLinkTo(currentLane);
638 if (link != nullptr) {
639 if DEBUGCOND(ped) {
640 std::cout << " direct backward\n";
641 }
642 nextLane = nextRouteLane->getInternalFollowingLane(currentLane);
643 if (nextLane != nullptr) {
644 // advance to the end of consecutive internal lanes
645 while (nextLane->getLinkCont()[0]->getViaLaneOrLane()->isInternal()) {
646 nextLane = nextLane->getLinkCont()[0]->getViaLaneOrLane();
647 }
648 }
649 }
650 }
651 }
652 if (nextLane == nullptr) {
653 // no internal lane found
654 nextLane = nextRouteLane;
655 if DEBUGCOND(ped) {
656 std::cout << SIMTIME << " no next lane found for " << currentLane->getID() << " dir=" << ped.myDir << "\n";
657 }
658 if (usingInternalLanesStatic() && currentLane->getLinkCont().size() > 0) {
659 WRITE_WARNING("Person '" + ped.myPerson->getID() + "' could not find route across junction '" + junction->getID()
660 + "' from edge '" + currentEdge->getID()
661 + "' to edge '" + nextRouteEdge->getID() + "', time=" +
662 time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
663 }
664 } else if (nextLane->getLength() <= POSITION_EPS) {
665 // internal lane too short
666 // most often this is due to a zero-size junction. However, if
667 // the person needs to pass a crossing we cannot skip ahead
668 if ((nextLane->getCanonicalSuccessorLane() == nullptr
669 || !nextLane->getCanonicalSuccessorLane()->getEdge().isCrossing())
670 && (nextLane->getLogicalPredecessorLane() == nullptr ||
671 !nextLane->getLogicalPredecessorLane()->getEdge().isCrossing())) {
672 //WRITE_WARNING("Person '" + ped.getID()
673 // + "' skips short lane '" + nextLane->getID()
674 // + "' length=" + toString(nextLane->getLength())
675 // + " time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
676 nextLane = nextRouteLane;
677 nextDir = nextRouteEdge->getFromJunction() == junction ? FORWARD : BACKWARD;
678 }
679 }
680 }
681 }
682 if DEBUGCOND(ped) {
683 std::cout << SIMTIME
684 << " p=" << ped.myPerson->getID()
685 << " l=" << currentLane->getID()
686 << " nl=" << (nextLane == nullptr ? "NULL" : nextLane->getID())
687 << " nrl=" << (nextRouteLane == nullptr ? "NULL" : nextRouteLane->getID())
688 << " d=" << nextDir
689 << " link=" << (link == nullptr ? "NULL" : link->getViaLaneOrLane()->getID())
690 << " pedDir=" << ped.myDir
691 << "\n";
692 }
693 assert(nextLane != 0 || nextRouteLane == 0);
694 return NextLaneInfo(nextLane, link, nextDir);
695}
696
697
698const MSLane*
699MSPModel_Striping::getNextWalkingArea(const MSLane* currentLane, const int dir, const MSLink*& link) {
700 if (dir == FORWARD) {
701 for (const MSLink* const l : currentLane->getLinkCont()) {
702 if (l->getLane()->getEdge().isWalkingArea()) {
703 link = l;
704 return l->getLane();
705 }
706 }
707 } else {
708 const std::vector<MSLane::IncomingLaneInfo>& laneInfos = currentLane->getIncomingLanes();
709 for (std::vector<MSLane::IncomingLaneInfo>::const_iterator it = laneInfos.begin(); it != laneInfos.end(); ++it) {
710 if ((*it).lane->getEdge().isWalkingArea()) {
711 link = (*it).viaLink;
712 return (*it).lane;
713 }
714 }
715 }
716 return nullptr;
717}
718
719
721MSPModel_Striping::getNeighboringObstacles(const Pedestrians& pedestrians, int egoIndex, int stripes) {
722 const PState& ego = *pedestrians[egoIndex];
723 const int egoStripe = ego.stripe();
724 Obstacles obs(stripes, Obstacle(ego.myDir));
725 std::vector<bool> haveBlocker(stripes, false);
726 for (int index = egoIndex + 1; index < (int)pedestrians.size(); index++) {
727 const PState& p = *pedestrians[index];
728 if DEBUGCOND(ego) {
729 std::cout << SIMTIME << " ped=" << ego.getID() << " cur=" << egoStripe << " checking neighbor " << p.getID()
730 << " nCur=" << p.stripe() << " nOth=" << p.otherStripe();
731 }
732 if (!p.myWaitingToEnter && !p.myAmJammed) {
733 const Obstacle o(p);
734 if DEBUGCOND(ego) {
735 std::cout << " dist=" << ego.distanceTo(o) << std::endl;
736 }
737 if (ego.distanceTo(o) == DIST_BEHIND) {
738 break;
739 }
740 if (ego.distanceTo(o) == DIST_OVERLAP) {
741 if (p.stripe() != egoStripe || p.myDir != ego.myDir) {
742 obs[p.stripe()] = o;
743 haveBlocker[p.stripe()] = true;
744 } else {
745 //std::cout << SIMTIME << " ignoring overlap between " << ego.getID() << " and " << p.getID() << " on stripe=" << egoStripe << "\n";
746 }
747 if (p.otherStripe() != egoStripe || p.myDir != ego.myDir) {
748 obs[p.otherStripe()] = o;
749 haveBlocker[p.otherStripe()] = true;
750 } else {
751 //std::cout << SIMTIME << " ignoring overlap between " << ego.getID() << " and " << p.getID() << " on stripe2=" << egoStripe << "\n";
752 }
753 } else {
754 if (!haveBlocker[p.stripe()]) {
755 obs[p.stripe()] = o;
756 }
757 if (!haveBlocker[p.otherStripe()]) {
758 obs[p.otherStripe()] = o;
759 }
760 }
761 }
762 }
763 if DEBUGCOND(ego) {
764 std::cout << SIMTIME << " ped=" << ego.myPerson->getID() << " neighObs=";
765 DEBUG_PRINT(obs);
766 }
767 return obs;
768}
769
770
771int
772MSPModel_Striping::getStripeOffset(int origStripes, int destStripes, bool addRemainder) {
773 int offset = (destStripes - origStripes) / 2;
774 if (addRemainder) {
775 offset += (destStripes - origStripes) % 2;
776 }
777 return offset;
778}
779
780
783 MSLane* lane, const MSLane* nextLane, int stripes, int nextDir,
784 double currentLength, int currentDir) {
785 if (nextLanesObs.count(nextLane) == 0) {
786 const double nextLength = nextLane->getEdge().isWalkingArea() ? myMinNextLengths[nextLane] : nextLane->getLength();
787 // figure out the which pedestrians are ahead on the next lane
788 const int nextStripes = numStripes(nextLane);
789 // do not move past the end of the next lane in a single step
790 Obstacles obs(stripes, Obstacle(nextDir == FORWARD ? nextLength : 0, 0, OBSTACLE_NEXTEND, "nextEnd", 0));
791
792 const int offset = getStripeOffset(nextStripes, stripes, currentDir != nextDir && nextStripes > stripes);
793 //std::cout << SIMTIME << " getNextLaneObstacles"
794 // << " nextLane=" << nextLane->getID()
795 // << " nextLength=" << nextLength
796 // << " nextDir=" << nextDir
797 // << " currentLength=" << currentLength
798 // << " currentDir=" << currentDir
799 // << " stripes=" << stripes
800 // << " nextStripes=" << nextStripes
801 // << " offset=" << offset
802 // << "\n";
803 if (nextStripes < stripes) {
804 // some stripes do not continue
805 for (int ii = 0; ii < stripes; ++ii) {
806 if (ii < offset || ii >= nextStripes + offset) {
807 obs[ii] = Obstacle(nextDir == FORWARD ? 0 : nextLength, 0, OBSTACLE_END, "stripeEnd", 0);
808 }
809 }
810 }
811 Pedestrians& pedestrians = getPedestrians(nextLane);
812 if (nextLane->getEdge().isWalkingArea()) {
813 transformToCurrentLanePositions(obs, currentDir, nextDir, currentLength, nextLength);
814 // complex transformation into the coordinate system of the current lane
815 // (pedestrians on next lane may walk at arbitrary angles relative to the current lane)
816 double lateral_offset = (lane->getWidth() - stripeWidth) * 0.5;
817 if ((stripes - nextStripes) % 2 != 0) {
818 lateral_offset += 0.5 * stripeWidth;
819 }
820 nextDir = currentDir;
821 // transform pedestrians into the current coordinate system
822 for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
823 PState& p = *pedestrians[ii];
824 if (p.myWaitingToEnter || p.myAmJammed) {
825 continue;
826 }
827 Position relPos = lane->getShape().transformToVectorCoordinates(p.getPosition(*p.myStage, -1), true);
828 const double newY = relPos.y() + lateral_offset;
829 //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";
830 if ((currentDir == FORWARD && relPos.x() >= lane->getLength()) || (currentDir == BACKWARD && relPos.x() < 0)) {
831 addCloserObstacle(obs, relPos.x(), p.stripe(newY), stripes, p.myPerson->getID(), p.myPerson->getVehicleType().getWidth(), currentDir, OBSTACLE_PED);
832 addCloserObstacle(obs, relPos.x(), p.otherStripe(newY), stripes, p.myPerson->getID(), p.myPerson->getVehicleType().getWidth(), currentDir, OBSTACLE_PED);
833 }
834 }
835 } else {
836 // simple transformation into the coordinate system of the current lane
837 // (only need to worry about currentDir and nextDir)
838 // XXX consider waitingToEnter on nextLane
839 sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(nextDir));
840 for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
841 const PState& p = *pedestrians[ii];
842 if (p.myWaitingToEnter || p.myAmJammed) {
843 continue;
844 }
845 double newY = p.myRelY;
846 Obstacle pObs(p);
847 if (nextDir != currentDir) {
848 newY = (nextStripes - 1) * stripeWidth - newY;
849 pObs.speed *= -1;
850 }
851 newY += offset * stripeWidth;
852 const int stripe = p.stripe(newY);
853 if (stripe >= 0 && stripe < stripes) {
854 obs[stripe] = pObs;
855 }
856 const int otherStripe = p.otherStripe(newY);
857 if (otherStripe >= 0 && otherStripe < stripes) {
858 obs[otherStripe] = pObs;
859 }
860 }
861 if (nextLane->getEdge().isCrossing()) {
862 // add vehicle obstacles
863 const MSLink* crossingEntryLink = nextLane->getIncomingLanes().front().viaLink;
864 const bool prio = crossingEntryLink->havePriority() || crossingEntryLink->getTLLogic() != nullptr;
865 addCrossingVehs(nextLane, stripes, offset, nextDir, obs, prio);
866 }
867 if (nextLane->getVehicleNumberWithPartials() > 0) {
868 Obstacles vehObs = getVehicleObstacles(nextLane, nextDir);
869 PState::mergeObstacles(obs, vehObs, nextDir, offset);
870 }
871 transformToCurrentLanePositions(obs, currentDir, nextDir, currentLength, nextLength);
872 }
873 nextLanesObs[nextLane] = obs;
874 }
875 return nextLanesObs[nextLane];
876}
877
878void
879MSPModel_Striping::transformToCurrentLanePositions(Obstacles& obs, int currentDir, int nextDir, double currentLength, double nextLength) {
880 for (int ii = 0; ii < (int)obs.size(); ++ii) {
881 Obstacle& o = obs[ii];
882 if (currentDir == FORWARD) {
883 if (nextDir == FORWARD) {
884 o.xFwd += currentLength;
885 o.xBack += currentLength;
886 } else {
887 const double tmp = o.xFwd;
888 o.xFwd = currentLength + nextLength - o.xBack;
889 o.xBack = currentLength + nextLength - tmp;
890 }
891 } else {
892 if (nextDir == FORWARD) {
893 const double tmp = o.xFwd;
894 o.xFwd = -o.xBack;
895 o.xBack = -tmp;
896 } else {
897 o.xFwd -= nextLength;
898 o.xBack -= nextLength;
899 }
900 }
901 }
902}
903
904
905void
906MSPModel_Striping::addCloserObstacle(Obstacles& obs, double x, int stripe, int numStripes, const std::string& id, double width, int dir, ObstacleType type) {
907 if (stripe >= 0 && stripe < numStripes) {
908 if ((dir == FORWARD && x - width / 2. < obs[stripe].xBack) || (dir == BACKWARD && x + width / 2. > obs[stripe].xFwd)) {
909 obs[stripe] = Obstacle(x, 0, type, id, width);
910 }
911 }
912}
913
914void
915MSPModel_Striping::moveInDirection(SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir) {
916 for (ActiveLanes::iterator it_lane = myActiveLanes.begin(); it_lane != myActiveLanes.end(); ++it_lane) {
917 const MSLane* lane = it_lane->first;
918 Pedestrians& pedestrians = it_lane->second;
919 if (pedestrians.size() == 0) {
920 continue;
921 }
922 //std::cout << SIMTIME << ">>> lane=" << lane->getID() << " numPeds=" << pedestrians.size() << "\n";
923 if (lane->getEdge().isWalkingArea()) {
924 const double lateral_offset = (lane->getWidth() - stripeWidth) * 0.5;
925 const double minY = stripeWidth * - 0.5 + NUMERICAL_EPS;
926 const double maxY = stripeWidth * (numStripes(lane) - 0.5) - NUMERICAL_EPS;
927 const WalkingAreaPath* debugPath = nullptr;
928 // need to handle each walkingAreaPath separately and transform
929 // coordinates beforehand
930 std::set<const WalkingAreaPath*, walkingarea_path_sorter> paths;
931 for (Pedestrians::iterator it = pedestrians.begin(); it != pedestrians.end(); ++it) {
932 const PState* p = *it;
933 assert(p->myWalkingAreaPath != 0);
934 if (p->myDir == dir) {
935 paths.insert(p->myWalkingAreaPath);
936 if DEBUGCOND(*p) {
937 debugPath = p->myWalkingAreaPath;
938 std::cout << SIMTIME << " debugging WalkingAreaPath from=" << debugPath->from->getID() << " to=" << debugPath->to->getID() << " minY=" << minY << " maxY=" << maxY << " latOffset=" << lateral_offset << "\n";
939 }
940 }
941 }
942 const double usableWidth = (numStripes(lane) - 1) * stripeWidth;
943 for (std::set<const WalkingAreaPath*, walkingarea_path_sorter>::iterator it = paths.begin(); it != paths.end(); ++it) {
944 const WalkingAreaPath* path = *it;
945 Pedestrians toDelete;
946 Pedestrians transformedPeds;
947 transformedPeds.reserve(pedestrians.size());
948 for (Pedestrians::iterator it_p = pedestrians.begin(); it_p != pedestrians.end(); ++it_p) {
949 PState* p = *it_p;
950 if (p->myWalkingAreaPath == path) {
951 transformedPeds.push_back(p);
952 if (path == debugPath) std::cout << " ped=" << p->myPerson->getID() << " relX=" << p->myRelX << " relY=" << p->myRelY << " (untransformed), vecCoord="
953 << path->shape.transformToVectorCoordinates(p->getPosition(*p->myStage, -1)) << "\n";
954 } else if (p->myWalkingAreaPath->from == path->to && p->myWalkingAreaPath->to == path->from) {
955 if (p->myWalkingAreaPath->dir != path->dir) {
956 // opposite direction is already in the correct coordinate system
957 transformedPeds.push_back(p);
958 if (path == debugPath) std::cout << " ped=" << p->myPerson->getID() << " relX=" << p->myRelX << " relY=" << p->myRelY << " (untransformed), vecCoord="
959 << path->shape.transformToVectorCoordinates(p->getPosition(*p->myStage, -1)) << "\n";
960 } else {
961 // x position must be reversed
962 PState* tp = new PState(*p);
963 tp->myRelX = path->length - p->myRelX;
964 tp->myRelY = usableWidth - p->myRelY;
965 tp->myDir = !path->dir;
966 tp->mySpeed = -p->mySpeed;
967 tp->mySpeedLat = -p->mySpeedLat;
968 toDelete.push_back(tp);
969 transformedPeds.push_back(tp);
970 if (path == debugPath) std::cout << " ped=" << p->myPerson->getID() << " relX=" << p->myRelX << " relY=" << p->myRelY << " (semi-transformed), vecCoord="
971 << path->shape.transformToVectorCoordinates(p->getPosition(*p->myStage, -1)) << "\n";
972 }
973 } else {
974 const Position relPos = path->shape.transformToVectorCoordinates(p->getPosition(*p->myStage, -1));
975 const double newY = relPos.y() + lateral_offset;
976 if (relPos != Position::INVALID && newY >= minY && newY <= maxY) {
977 PState* tp = new PState(*p);
978 tp->myRelX = relPos.x();
979 tp->myRelY = newY;
980 // only an obstacle, speed may be orthogonal to dir
981 tp->myDir = !dir;
982 tp->mySpeed = 0;
983 tp->mySpeedLat = 0;
984 toDelete.push_back(tp);
985 transformedPeds.push_back(tp);
986 if (path == debugPath) {
987 std::cout << " ped=" << p->myPerson->getID() << " relX=" << relPos.x() << " relY=" << newY << " (transformed), vecCoord=" << relPos << "\n";
988 }
989 } else {
990 if (path == debugPath) {
991 std::cout << " ped=" << p->myPerson->getID() << " relX=" << relPos.x() << " relY=" << newY << " (invalid), vecCoord=" << relPos << "\n";
992 }
993 }
994 }
995 }
996 auto itFoe = myWalkingAreaFoes.find(&lane->getEdge());
997 if (itFoe != myWalkingAreaFoes.end()) {
998 // add vehicle foes on paths which cross this walkingarea
999 // translate the vehicle into a number of dummy-pedestrians
1000 // that occupy the same space
1001 for (const MSLane* foeLane : itFoe->second) {
1002 for (auto itVeh = foeLane->anyVehiclesBegin(); itVeh != foeLane->anyVehiclesEnd(); ++itVeh) {
1003 const MSVehicle* veh = *itVeh;
1006
1007 const double centerX = (relPos.x() + relPos2.x()) / 2;
1008 const double shapeAngle = path->shape.rotationAtOffset(centerX);
1009 // 0 means car is at 90 degree angle
1010 double angleDiff = fabs(GeomHelper::angleDiff(veh->getAngle(), shapeAngle + M_PI / 2));
1011 if (angleDiff > M_PI / 2) {
1012 angleDiff = M_PI - angleDiff;
1013 }
1014 double xWidth; // extent of car in x direction (= carWidth at 90 degrees)
1015 const double b = tan(angleDiff) * veh->getVehicleType().getWidth();
1016 if (b <= veh->getVehicleType().getLength()) {
1017 const double a = veh->getVehicleType().getWidth();
1018 xWidth = sqrt(a * a + b * b);
1019 } else {
1020 // XXX actually xWidth shrinks towards car length while angleDiff approachess 90
1021 xWidth = veh->getVehicleType().getLength();
1022 }
1023 // distort y to account for angleDiff
1024 const double correctY = sin(angleDiff) * veh->getVehicleType().getWidth() / 2; // + 0.5 * SAFETY_GAP;
1025 relPos.setx(centerX);
1026 relPos2.setx(centerX);
1027 if (relPos.y() < relPos2.y()) {
1028 relPos.sety(relPos.y() - correctY);
1029 relPos2.sety(relPos2.y() + correctY);
1030 } else {
1031 relPos.sety(relPos.y() + correctY);
1032 relPos2.sety(relPos2.y() - correctY);
1033 }
1034 if (path == debugPath) {
1035 std::cout << " veh=" << veh->getID()
1036 << " pos=" << veh->getPosition() << " back=" << veh->getBackPosition()
1037 << " vehAngle=" << RAD2DEG(veh->getAngle())
1038 << " shapeAngle=" << RAD2DEG(shapeAngle)
1039 << " angleDiff=" << RAD2DEG(angleDiff) << " b=" << b << " xWidth=" << xWidth
1040 << " correctY=" << correctY
1041 << " vecCoord=" << relPos << " vecCoordBack=" << relPos2 << "\n";
1042 }
1043 const bool addFront = addVehicleFoe(veh, lane, relPos, xWidth, 0, lateral_offset, minY, maxY, toDelete, transformedPeds);
1044 const bool addBack = addVehicleFoe(veh, lane, relPos2, xWidth, 0, lateral_offset, minY, maxY, toDelete, transformedPeds);
1045 if (addFront && addBack) {
1046 // add in-between positions
1047 const double length = veh->getVehicleType().getLength();
1048 for (double dist = stripeWidth; dist < length; dist += stripeWidth) {
1049 const double relDist = dist / length;
1050 Position between = (relPos * relDist) + (relPos2 * (1 - relDist));
1051 if (path == debugPath) {
1052 std::cout << " vehBetween=" << veh->getID() << " pos=" << between << "\n";
1053 }
1054 addVehicleFoe(veh, lane, between, xWidth, stripeWidth, lateral_offset, minY, maxY, toDelete, transformedPeds);
1055 }
1056 }
1057 }
1058 }
1059 }
1060 moveInDirectionOnLane(transformedPeds, lane, currentTime, changedLane, dir);
1061 arriveAndAdvance(pedestrians, currentTime, changedLane, dir);
1062 // clean up
1063 for (Pedestrians::iterator it_p = toDelete.begin(); it_p != toDelete.end(); ++it_p) {
1064 delete *it_p;
1065 }
1066 }
1067 } else {
1068 moveInDirectionOnLane(pedestrians, lane, currentTime, changedLane, dir);
1069 arriveAndAdvance(pedestrians, currentTime, changedLane, dir);
1070 }
1071 }
1072}
1073
1074
1075bool
1076MSPModel_Striping::addVehicleFoe(const MSVehicle* veh, const MSLane* walkingarea, const Position& relPos, double xWidth, double yWidth, double lateral_offset,
1077 double minY, double maxY, Pedestrians& toDelete, Pedestrians& transformedPeds) {
1078 if (relPos != Position::INVALID) {
1079 const double newY = relPos.y() + lateral_offset;
1080 if (newY >= minY && newY <= maxY) {
1081 PState* tp = new PStateVehicle(veh, walkingarea, relPos.x(), newY, xWidth, yWidth);
1082 //std::cout << SIMTIME << " addVehicleFoe=" << veh->getID() << " rx=" << relPos.x() << " ry=" << newY << " s=" << tp->stripe() << " o=" << tp->otherStripe() << "\n";
1083 toDelete.push_back(tp);
1084 transformedPeds.push_back(tp);
1085 }
1086 return true;
1087 } else {
1088 return false;
1089 }
1090}
1091
1092void
1093MSPModel_Striping::arriveAndAdvance(Pedestrians& pedestrians, SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir) {
1094 // advance to the next lane / arrive at destination
1095 sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(dir));
1096 // can't use iterators because we do concurrent modification
1097 for (int i = 0; i < (int)pedestrians.size(); i++) {
1098 PState* const p = pedestrians[i];
1099 if (p->isRemoteControlled()) {
1100 continue;
1101 }
1102 if (p->myDir == dir && p->distToLaneEnd() < 0) {
1103 // moveToNextLane may trigger re-insertion (for consecutive
1104 // walks) so erase must be called first
1105 pedestrians.erase(pedestrians.begin() + i);
1106 i--;
1107 p->moveToNextLane(currentTime);
1108 if (p->myLane != nullptr) {
1109 changedLane.insert(p->myPerson);
1110 myActiveLanes[p->myLane].push_back(p);
1111 } else {
1112 // end walking stage and destroy PState
1113 p->myStage->moveToNextEdge(p->myPerson, currentTime, dir);
1115 }
1116 }
1117 }
1118}
1119
1120
1121void
1122MSPModel_Striping::moveInDirectionOnLane(Pedestrians& pedestrians, const MSLane* lane, SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir) {
1123 const int stripes = numStripes(lane);
1124 //std::cout << " laneWidth=" << lane->getWidth() << " stripeWidth=" << stripeWidth << " stripes=" << stripes << "\n";
1125 Obstacles obs(stripes, Obstacle(dir)); // continously updated
1126 NextLanesObstacles nextLanesObs; // continously updated
1127 sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(dir));
1128
1129 Obstacles crossingVehs(stripes, Obstacle(dir));
1130 bool hasCrossingVehObs = false;
1131 if (lane->getEdge().isCrossing()) {
1132 // assume that vehicles will brake when already on the crossing
1133 hasCrossingVehObs = addCrossingVehs(lane, stripes, 0, dir, crossingVehs, true);
1134 }
1135
1136 for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
1137 PState& p = *pedestrians[ii];
1138 //std::cout << SIMTIME << "CHECKING" << p.myPerson->getID() << "\n";
1139 Obstacles currentObs = obs;
1140 if (p.myDir != dir || changedLane.count(p.myPerson) != 0 || p.myRemoteXYPos != Position::INVALID) {
1141 if (!p.myWaitingToEnter && !p.myAmJammed) {
1142 //if DEBUGCOND(p) {
1143 // std::cout << " obs=" << p.myPerson->getID() << " y=" << p.myRelY << " stripe=" << p.stripe() << " oStripe=" << p.otherStripe() << "\n";
1144 //}
1145 Obstacle o(p);
1146 if (p.myDir != dir && p.mySpeed == 0) {
1147 // ensure recognition of oncoming
1148 o.speed = (p.myDir == FORWARD ? 0.1 : -0.1);
1149 }
1150 obs[p.stripe()] = o;
1151 obs[p.otherStripe()] = o;
1152 }
1153 continue;
1154 }
1155 if DEBUGCOND(p) {
1156 std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " currentObs=";
1157 gDebugFlag1 = true;
1158 DEBUG_PRINT(currentObs);
1159 }
1160 const MSLane* nextLane = p.myNLI.lane;
1161 const MSLink* link = p.myNLI.link;
1162 const double dist = p.distToLaneEnd();
1163 const double speed = p.myStage->getMaxSpeed(p.myPerson);
1164 if (nextLane != nullptr && dist <= LOOKAHEAD_ONCOMING) {
1165 const double currentLength = (p.myWalkingAreaPath == nullptr ? lane->getLength() : p.myWalkingAreaPath->length);
1166 const Obstacles& nextObs = getNextLaneObstacles(
1167 nextLanesObs, lane, nextLane, stripes,
1168 p.myNLI.dir, currentLength, dir);
1169
1170 if DEBUGCOND(p) {
1171 std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " nextObs=";
1172 DEBUG_PRINT(nextObs);
1173 }
1174 p.mergeObstacles(currentObs, nextObs);
1175 }
1176 if DEBUGCOND(p) {
1177 std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWithNext=";
1178 DEBUG_PRINT(currentObs);
1179 }
1180 p.mergeObstacles(currentObs, getNeighboringObstacles(pedestrians, ii, stripes));
1181 if DEBUGCOND(p) {
1182 std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWithNeigh=";
1183 DEBUG_PRINT(currentObs);
1184 }
1185 // time gap to pass the intersection ahead of a vehicle.
1186 const double passingClearanceTime = 2;
1187 const double passingLength = p.getLength() + passingClearanceTime * speed;
1188 // check link state
1189 if DEBUGCOND(p) {
1190 gDebugFlag1 = true; // get debug output from MSLink
1191 std::cout << " link=" << (link == nullptr ? "NULL" : link->getViaLaneOrLane()->getID())
1192 << " dist=" << dist << " d2=" << dist - p.getMinGap() << " la=" << LOOKAHEAD_SAMEDIR* speed << "\n";
1193 }
1194 if (link != nullptr
1195 // only check close before junction, @todo we should take deceleration into account here
1196 && dist - p.getMinGap() < LOOKAHEAD_SAMEDIR * speed
1197 // persons move before vehicles so we subtract DELTA_TO because they cannot rely on vehicles having passed the intersection in the current time step
1198 && !link->opened(currentTime - DELTA_T, speed, speed, passingLength, p.getImpatience(currentTime), speed, 0, 0, nullptr, p.ignoreRed(link), p.myPerson)) {
1199 // prevent movement passed a closed link
1200 Obstacles closedLink(stripes, Obstacle(p.myRelX + dir * (dist - NUMERICAL_EPS), 0, OBSTACLE_LINKCLOSED, "closedLink_" + link->getViaLaneOrLane()->getID(), 0));
1201 p.mergeObstacles(currentObs, closedLink);
1202 if DEBUGCOND(p) {
1203 std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWithTLS=";
1204 DEBUG_PRINT(currentObs);
1205 }
1206 // consider rerouting over another crossing
1207 if (p.myWalkingAreaPath != nullptr) {
1208 // @todo actually another path would be needed starting at the current position
1210 }
1211 }
1212 if DEBUGCOND(p) {
1213 gDebugFlag1 = false;
1214 }
1215 if (&lane->getEdge() == p.myStage->getDestination() && p.myStage->getDestinationStop() != nullptr) {
1216 Obstacles arrival;
1218 arrival = Obstacles(stripes, Obstacle(p.myStage->getArrivalPos() + dir * p.getMinGap(), 0, OBSTACLE_ARRIVALPOS, "arrival", 0));
1219 } else {
1220 arrival = Obstacles(stripes, Obstacle(p.myStage->getArrivalPos() - dir * p.getMinGap(), 0, OBSTACLE_ARRIVALPOS, "arrival_blocked", 0));
1221 }
1222 p.mergeObstacles(currentObs, arrival);
1223 }
1224
1225 if (lane->getVehicleNumberWithPartials() > 0) {
1226 // react to vehicles on the same lane
1227 // @todo: improve efficiency by using the same iterator for all pedestrians on this lane
1228 Obstacles vehObs = getVehicleObstacles(lane, dir, &p);
1229 p.mergeObstacles(currentObs, vehObs);
1230 if DEBUGCOND(p) {
1231 std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWithVehs=";
1232 DEBUG_PRINT(currentObs);
1233 }
1234 }
1235 if (hasCrossingVehObs) {
1236 p.mergeObstacles(currentObs, crossingVehs);
1237 if DEBUGCOND(p) {
1238 std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWithVehs2=";
1239 DEBUG_PRINT(currentObs);
1240 }
1241 }
1242
1243 // walk, taking into account all obstacles
1244 p.walk(currentObs, currentTime);
1245 gDebugFlag1 = false;
1246 if (!p.myWaitingToEnter && !p.myAmJammed) {
1247 Obstacle o(p);
1248 obs[p.stripe()] = o;
1249 obs[p.otherStripe()] = o;
1250 if (MSGlobals::gCheck4Accidents && p.myWalkingAreaPath == nullptr && !p.myAmJammed) {
1251 for (int coll = 0; coll < ii; ++coll) {
1252 PState& c = *pedestrians[coll];
1253 if (!c.myWaitingToEnter && c.myWalkingAreaPath == nullptr && !c.myAmJammed) {
1254 if (c.stripe() == p.stripe() || p.stripe() == c.otherStripe() || p.otherStripe() == c.stripe() || p.otherStripe() == c.otherStripe()) {
1255 Obstacle cObs(c);
1256 // we check only for real collisions, no min gap violations
1257 if (p.distanceTo(cObs, false) == DIST_OVERLAP) {
1258 WRITE_WARNING("Collision of person '" + p.myPerson->getID() + "' and person '" + c.myPerson->getID()
1259 + "', lane='" + lane->getID() + "', time=" + time2string(currentTime) + ".");
1260 }
1261 }
1262 }
1263 }
1264 }
1265 }
1266 //std::cout << SIMTIME << p.myPerson->getID() << " lane=" << lane->getID() << " x=" << p.myRelX << "\n";
1267 }
1268}
1269
1270bool
1271MSPModel_Striping::addCrossingVehs(const MSLane* crossing, int stripes, double lateral_offset, int dir, Obstacles& obs, bool prio) {
1272 bool hasCrossingVehObs = false;
1273 const MSLink* crossingExitLink = crossing->getLinkCont().front();
1274 gDebugFlag1 = DEBUGCOND2(crossing);
1275 const MSLink::LinkLeaders linkLeaders = crossingExitLink->getLeaderInfo(nullptr, crossing->getLength());
1276 gDebugFlag1 = false;
1277 if (linkLeaders.size() > 0) {
1278 for (MSLink::LinkLeaders::const_iterator it = linkLeaders.begin(); it != linkLeaders.end(); ++it) {
1279 // the vehicle to enter the junction first has priority
1280 const MSVehicle* veh = (*it).vehAndGap.first;
1281 if (veh != nullptr) {
1282 Obstacle vo((*it).distToCrossing, 0, OBSTACLE_VEHICLE, veh->getID(), veh->getVehicleType().getWidth() + 2 * minGapToVehicle);
1283 // block entry to the crossing in walking direction but allow leaving it
1284 Obstacle voBlock = vo;
1285 if (dir == FORWARD) {
1286 voBlock.xBack = NUMERICAL_EPS;
1287 } else {
1288 voBlock.xFwd = crossing->getLength() - NUMERICAL_EPS;
1289 }
1290 // when approaching a priority crossings, vehicles must be able
1291 // to brake, otherwise the person must be able to cross in time
1292 const double distToCrossBeforeVeh = (dir == FORWARD ? vo.xFwd : crossing->getLength() - vo.xBack);
1293 const double bGap = (prio
1295 : veh->getSpeed() * distToCrossBeforeVeh); // walking 1m/s
1296 double vehYmin;
1297 double vehYmax;
1298 // relY increases from left to right (the other way around from vehicles)
1299 if ((*it).fromLeft) {
1300 vehYmin = -(*it).vehAndGap.second + lateral_offset; // vehicle back
1301 vehYmax = vehYmin + veh->getVehicleType().getLength() + bGap + minGapToVehicle;
1302 vehYmin -= minGapToVehicle;
1303 } else {
1304 vehYmax = crossing->getWidth() + (*it).vehAndGap.second - lateral_offset; // vehicle back
1305 vehYmin = vehYmax - veh->getVehicleType().getLength() - bGap - minGapToVehicle;
1306 vehYmax += minGapToVehicle;
1307
1308 }
1309 for (int s = MAX2(0, PState::stripe(vehYmin)); s < MIN2(PState::stripe(vehYmax), stripes); ++s) {
1310 if ((dir == FORWARD && obs[s].xBack > vo.xBack)
1311 || (dir == BACKWARD && obs[s].xFwd < vo.xFwd)) {
1312 if (!prio && veh->getSpeed() > SUMO_const_haltingSpeed) {
1313 // do not enter the crossing
1314 obs[s] = voBlock;
1315 } else {
1316 obs[s] = vo;
1317 }
1318 hasCrossingVehObs = true;
1319 }
1320 }
1321 if (DEBUGCOND2(crossing)) {
1322 std::cout << SIMTIME
1323 << " crossingVeh=" << veh->getID()
1324 << " lane=" << crossing->getID()
1325 << " prio=" << prio
1326 << " latOffset=" << lateral_offset
1327 << " dir=" << dir
1328 << " stripes=" << stripes
1329 << " dist=" << (*it).distToCrossing
1330 << " gap=" << (*it).vehAndGap.second
1331 << " brakeGap=" << bGap
1332 << " fromLeft=" << (*it).fromLeft
1333 << " distToCrossBefore=" << distToCrossBeforeVeh
1334 << " ymin=" << vehYmin
1335 << " ymax=" << vehYmax
1336 << " smin=" << PState::stripe(vehYmin)
1337 << " smax=" << PState::stripe(vehYmax)
1338 << "\n";
1339 DEBUG_PRINT(obs);
1340 }
1341 }
1342 }
1343 }
1344 return hasCrossingVehObs;
1345}
1346
1347
1350 const int stripes = numStripes(lane);
1351 Obstacles vehObs(stripes, Obstacle(dir));
1352 int current = -1;
1353 double minX = 0.;
1354 double maxX = 0.;
1355 double pRelY = -1.;
1356 double pWidth = 0.;
1357 std::string pID;
1358 bool debug = DEBUGCOND2(lane);
1359 if (ped != nullptr) {
1360 current = ped->stripe();
1361 minX = ped->getMinX();
1362 maxX = ped->getMaxX();
1363 pRelY = ped->myRelY;
1364 pWidth = ped->myPerson->getVehicleType().getWidth();
1365 pID = ped->myPerson->getID();
1366 debug = DEBUGCOND(*ped);
1367 } else if (dir == BACKWARD) {
1368 // checking vehicles on the next lane. Use entry point as reference
1369 minX = lane->getLength();
1370 maxX = lane->getLength();
1371 }
1374 for (MSLane::AnyVehicleIterator it = begin; it != end; ++it) {
1375 const MSVehicle* veh = *it;
1376 const bool bidi = veh->getLane() == lane->getBidiLane();
1377 const double vehBack = veh->getBackPositionOnLane(lane);
1378 double vehFront = vehBack + veh->getVehicleType().getLength();
1379 // ensure that vehicles are not blocked
1380 const double vehNextSpeed = MAX2(veh->getSpeed(), 1.0);
1381 const double clearance = SAFETY_GAP + vehNextSpeed * LOOKAHEAD_SAMEDIR;
1382 // boundaries for range checking
1383 double vehXMax;
1384 double vehXMin;
1385 double vehXMaxCheck;
1386 double vehXMinCheck;
1387 if (bidi) {
1388 vehFront = vehBack - veh->getVehicleType().getLength();
1389 vehXMax = vehBack + SAFETY_GAP;
1390 vehXMin = vehFront - clearance;
1391 if (dir == FORWARD) {
1392 vehXMaxCheck = vehBack + NUMERICAL_EPS;
1393 vehXMinCheck = vehFront - LOOKAROUND_VEHICLES;
1394 } else {
1395 vehXMaxCheck = vehBack + LOOKAHEAD_SAMEDIR;
1396 vehXMinCheck = vehFront - clearance;
1397 }
1398 } else {
1399 vehXMax = vehFront + clearance;
1400 vehXMin = vehBack - SAFETY_GAP;
1401 if (dir == FORWARD) {
1402 vehXMaxCheck = vehFront + clearance;
1403 vehXMinCheck = vehBack - LOOKAHEAD_SAMEDIR;
1404 } else {
1405 vehXMaxCheck = vehFront + LOOKAROUND_VEHICLES;
1406 vehXMinCheck = vehBack - NUMERICAL_EPS;
1407 }
1408 }
1409 if (debug) {
1410 std::cout << SIMTIME << " ped=" << pID << " veh=" << veh->getID() << " check obstacle on lane=" << lane->getID()
1411 << "\n"
1412 << " vehXMin=" << vehXMin
1413 << " vehXMax=" << vehXMax
1414 << " vehXMinC=" << vehXMinCheck
1415 << " vehXMaxC=" << vehXMaxCheck
1416 << " minX=" << minX
1417 << " maxX=" << maxX
1418 << " bidi=" << bidi
1419 << " vFront=" << vehFront
1420 << " vBack=" << vehBack
1421 << "\n";
1422 }
1423 if (vehXMaxCheck > minX && vehXMinCheck && vehXMinCheck <= maxX) {
1424 Obstacle vo(vehBack, veh->getSpeed() * (bidi ? -1 : 1), OBSTACLE_VEHICLE, veh->getID(), 0);
1425 // moving vehicles block space along their path
1426 vo.xFwd = vehXMax;
1427 vo.xBack = vehXMin;
1428 // relY increases from left to right (the other way around from vehicles)
1429 // XXX lateral offset for partial vehicles
1430 const double posLat = veh->getLateralPositionOnLane() * (bidi ? -1 : 1);
1431 const double vehYmax = 0.5 * (lane->getWidth() + veh->getVehicleType().getWidth() - stripeWidth) - posLat;
1432 const double vehYmin = vehYmax - veh->getVehicleType().getWidth();
1433 for (int s = MAX2(0, PState::stripe(vehYmin)); s < MIN2(PState::stripe(vehYmax) + 1, stripes); ++s) {
1434 Obstacle prior = vehObs[s];
1435 vehObs[s] = vo;
1436 if (s == current && vehFront + SAFETY_GAP < minX) {
1437 // ignore if aleady overlapping while vehicle is still behind
1438 if (pRelY - pWidth < vehYmax &&
1439 pRelY + pWidth > vehYmin && dir == FORWARD) {
1440 if (debug) {
1441 std::cout << " ignoring vehicle '" << veh->getID() << " on stripe " << s << " vehFrontSG=" << vehFront + SAFETY_GAP << " minX=" << minX << "\n";
1442 }
1443 if (dir == FORWARD) {
1444 vehObs[s] = prior;
1445 } else {
1446 vehObs[s].xFwd = MIN2(vo.xFwd, vehFront + SAFETY_GAP);
1447 }
1448 }
1449 }
1450 }
1451 if (debug) {
1452 std::cout << SIMTIME << " ped=" << pID << " veh=" << veh->getID() << " obstacle on lane=" << lane->getID()
1453 << "\n"
1454 << " ymin=" << vehYmin
1455 << " ymax=" << vehYmax
1456 << " smin=" << PState::stripe(vehYmin)
1457 << " smax=" << PState::stripe(vehYmax)
1458 << " relY=" << pRelY
1459 << " current=" << current
1460 << " vo.xFwd=" << vo.xFwd
1461 << " vo.xBack=" << vo.xBack
1462 << " vFront=" << vehFront
1463 << " vBack=" << vehBack
1464 << "\n";
1465 }
1466 }
1467 }
1468 return vehObs;
1469}
1470
1471
1472// ===========================================================================
1473// MSPModel_Striping::Obstacle method definitions
1474// ===========================================================================
1476 xFwd(dir * dist), // by default, far away when seen in dir
1477 xBack(dir * dist), // by default, far away when seen in dir
1478 speed(0),
1479 type(OBSTACLE_NONE),
1480 description("") {
1481}
1482
1483
1485 xFwd(ped.getMaxX()),
1486 xBack(ped.getMinX()),
1487 speed(ped.myDir * ped.mySpeed),
1488 type(ped.getOType()),
1489 description(ped.getID()) {
1490 assert(!ped.myWaitingToEnter);
1491}
1492
1493
1494// ===========================================================================
1495// MSPModel_Striping::PState method definitions
1496// ===========================================================================
1498 myPerson(person),
1499 myStage(stage),
1500 myLane(lane),
1501 myRelX(stage->getDepartPos()),
1502 myRelY(stage->getDepartPosLat()),
1503 myDir(FORWARD),
1504 mySpeed(0),
1505 mySpeedLat(0),
1506 myWaitingToEnter(true),
1507 myWaitingTime(0),
1508 myWalkingAreaPath(nullptr),
1509 myAmJammed(false),
1510 myRemoteXYPos(Position::INVALID),
1511 myAngle(std::numeric_limits<double>::max()) {
1512 const MSEdge* currentEdge = &lane->getEdge();
1513 const ConstMSEdgeVector& route = myStage->getRoute();
1514 assert(!route.empty());
1515 if (route.size() == 1) {
1516 // only a single edge, move towards end pos
1518 } else if (route.front()->getFunction() != SumoXMLEdgeFunc::NORMAL) {
1519 // start on an intersection
1520 myDir = FORWARD;
1521 if (route.front()->isWalkingArea()) {
1522 myWalkingAreaPath = getArbitraryPath(route.front());
1523 }
1524 } else {
1525 const bool mayStartForward = canTraverse(FORWARD, route) != UNDEFINED_DIRECTION;
1526 const bool mayStartBackward = canTraverse(BACKWARD, route) != UNDEFINED_DIRECTION;
1527 if DEBUGCOND(*this) {
1528 std::cout << " initialize dir for " << myPerson->getID() << " forward=" << mayStartForward << " backward=" << mayStartBackward << "\n";
1529 }
1530 if (mayStartForward && mayStartBackward) {
1531 // figure out the best direction via routing
1532 ConstMSEdgeVector crossingRoute;
1533 MSNet::getInstance()->getPedestrianRouter(0).compute(currentEdge, route.back(), myRelX, myStage->getArrivalPos(), myStage->getMaxSpeed(person), 0, nullptr, crossingRoute, true);
1534 if (crossingRoute.size() > 1) {
1535 // route found
1536 const MSEdge* nextEdge = crossingRoute[1];
1537 if (nextEdge->getFromJunction() == currentEdge->getFromJunction() || nextEdge->getToJunction() == currentEdge->getFromJunction()) {
1538 myDir = BACKWARD;
1539 }
1540 }
1541 if DEBUGCOND(*this) {
1542 std::cout << " crossingRoute=" << toString(crossingRoute) << "\n";
1543 }
1544 } else {
1545 myDir = !mayStartBackward ? FORWARD : BACKWARD;
1546 }
1547 }
1549 if (myRelY == UNSPECIFIED_POS_LAT) {
1550 myRelY = 0;
1551 }
1552 if (lane->getVehicleNumberWithPartials() > 0 && myRelY == 0) {
1553 // better start next to the road if nothing was specified
1555 }
1556 if (myDir == FORWARD || lane->getPermissions() != SVC_PEDESTRIAN) {
1557 // start at the right side of the sidewalk on shared roads
1558 myRelY = stripeWidth * (numStripes(lane) - 1) - myRelY;
1559 }
1560 } else if (myRelY == RANDOM_POS_LAT) {
1561 myRelY = RandHelper::rand() * stripeWidth * (numStripes(lane) - 1);
1562 } else {
1563 // convert vehicle-style posLat (0 is center, left is larger)
1564 // into striping coordinates (0 is on the leftmost stripe, right is larger)
1565 myRelY = lane->getWidth() / 2 - myRelY - stripeWidth / 2;
1566 }
1567 if DEBUGCOND(*this) {
1568 std::cout << " added new pedestrian " << myPerson->getID() << " on " << lane->getID() << " myRelX=" << myRelX << " myRelY=" << myRelY << " dir=" << myDir << " route=" << toString(myStage->getRoute()) << "\n";
1569 }
1570
1571 myNLI = getNextLane(*this, lane, nullptr);
1572}
1573
1574
1576 myPerson(nullptr),
1577 myStage(nullptr),
1578 myLane(nullptr),
1579 myRelX(0),
1580 myRelY(0),
1581 myDir(UNDEFINED_DIRECTION),
1582 mySpeed(0),
1583 mySpeedLat(0),
1584 myWaitingToEnter(false),
1585 myWaitingTime(0),
1586 myWalkingAreaPath(nullptr),
1587 myAmJammed(false),
1588 myRemoteXYPos(Position::INVALID),
1589 myAngle(std::numeric_limits<double>::max()) {
1590}
1591
1592
1593MSPModel_Striping::PState::PState(MSPerson* person, MSStageMoving* stage, std::istringstream* in):
1594 myPerson(person),
1595 myStage(stage),
1596 myLane(nullptr),
1597 myWalkingAreaPath(nullptr),
1598 myRemoteXYPos(Position::INVALID),
1599 myAngle(std::numeric_limits<double>::max()) {
1600 if (in != nullptr) {
1601 std::string laneID;
1602 std::string wapLaneFrom;
1603 std::string wapLaneTo;
1604 std::string nextLaneID;
1605 std::string nextLinkFrom;
1606 std::string nextLinkTo;
1607 int nextDir;
1608
1609 (*in) >> laneID
1611 >> wapLaneFrom >> wapLaneTo
1612 >> myAmJammed
1613 >> nextLaneID
1614 >> nextLinkFrom
1615 >> nextLinkTo
1616 >> nextDir;
1617
1618
1619 myLane = MSLane::dictionary(laneID);
1620 if (myLane == nullptr) {
1621 throw ProcessError("Unknown lane '" + laneID + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1622 }
1623
1624 MSLane* nextLane = nullptr;
1625 if (nextLaneID != "null") {
1626 nextLane = MSLane::dictionary(nextLaneID);
1627 if (nextLane == nullptr) {
1628 throw ProcessError("Unknown next lane '" + nextLaneID + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1629 }
1630 }
1631 const MSLink* link = nullptr;
1632 if (nextLinkFrom != "null") {
1633 MSLane* from = MSLane::dictionary(nextLinkFrom);
1634 MSLane* to = MSLane::dictionary(nextLinkTo);
1635 if (from == nullptr) {
1636 throw ProcessError("Unknown link origin lane '" + nextLinkFrom + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1637 }
1638 if (to == nullptr) {
1639 throw ProcessError("Unknown link destination lane '" + nextLinkTo + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1640 }
1641 link = from->getLinkTo(to);
1642 }
1643 myNLI = NextLaneInfo(nextLane, link, nextDir);
1644
1645 if (wapLaneFrom != "null") {
1646 MSLane* from = MSLane::dictionary(wapLaneFrom);
1647 MSLane* to = MSLane::dictionary(wapLaneTo);
1648 if (from == nullptr) {
1649 throw ProcessError("Unknown walkingAreaPath origin lane '" + wapLaneFrom + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1650 }
1651 if (to == nullptr) {
1652 throw ProcessError("Unknown walkingAreaPath destination lane '" + wapLaneTo + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1653 }
1654 const auto pathIt = myWalkingAreaPaths.find(std::make_pair(from, to));
1655 if (pathIt != myWalkingAreaPaths.end()) {
1656 myWalkingAreaPath = &pathIt->second;
1657 } else {
1658 throw ProcessError("Unknown walkingAreaPath from lane '" + wapLaneFrom + "' to lane '" + wapLaneTo + "' when loading walk for person '" + myPerson->getID() + "' from state.");
1659 }
1660 }
1661 }
1662}
1663
1664void
1666 std::string wapLaneFrom = "null";
1667 std::string wapLaneTo = "null";
1668 if (myWalkingAreaPath != nullptr) {
1669 wapLaneFrom = myWalkingAreaPath->from->getID();
1670 wapLaneTo = myWalkingAreaPath->to->getID();
1671 }
1672 std::string nextLaneID = "null";
1673 std::string nextLinkFrom = "null";
1674 std::string nextLinkTo = "null";
1675 if (myNLI.lane != nullptr) {
1676 nextLaneID = myNLI.lane->getID();
1677 }
1678 if (myNLI.link != nullptr) {
1679 nextLinkFrom = myNLI.link->getLaneBefore()->getID();
1680 nextLinkTo = myNLI.link->getViaLaneOrLane()->getID();
1681 }
1682 out << " " << myLane->getID()
1683 << " " << myRelX
1684 << " " << myRelY
1685 << " " << myDir
1686 << " " << mySpeed
1687 << " " << mySpeedLat
1688 << " " << myWaitingToEnter
1689 << " " << myWaitingTime
1690 << " " << wapLaneFrom
1691 << " " << wapLaneTo
1692 << " " << myAmJammed
1693 << " " << nextLaneID
1694 << " " << nextLinkFrom
1695 << " " << nextLinkTo
1696 << " " << myNLI.dir;
1697}
1698
1699double
1700MSPModel_Striping::PState::getMinX(const bool includeMinGap) const {
1701 // @todo speed should have an influence here because faster persons need more space
1702 if (myDir == FORWARD) {
1703 return myRelX - getLength();
1704 }
1705 return myRelX - (includeMinGap ? getMinGap() : 0.);
1706}
1707
1708
1709double
1710MSPModel_Striping::PState::getMaxX(const bool includeMinGap) const {
1711 // @todo speed should have an influence here because faster persons need more space
1712 if (myDir == FORWARD) {
1713 return myRelX + (includeMinGap ? getMinGap() : 0.);
1714 }
1715 return myRelX + getLength();
1716}
1717
1718
1719double
1721 return myPerson->getVehicleType().getLength();
1722}
1723
1724
1725double
1727 return myPerson->getVehicleType().getMinGap();
1728}
1729
1730
1731int
1733 return (int)floor(relY / stripeWidth + 0.5);
1734}
1735
1736
1737int
1739 const int s = stripe(relY);
1740 const double offset = relY - s * stripeWidth;
1741 const double threshold = MAX2(NUMERICAL_EPS, stripeWidth - SQUEEZE * getWidth());
1742 int result;
1743 if (offset > threshold) {
1744 result = s + 1;
1745 } else if (offset < -threshold) {
1746 result = s - 1;
1747 } else {
1748 result = s;
1749 }
1750 //std::cout.setf(std::ios::fixed , std::ios::floatfield);
1751 //std::cout << std::setprecision(5);
1752 //if DEBUGCOND(*this) std::cout << " otherStripe " << myPerson->getID() << " offset=" << offset << " threshold=" << threshold << " rawResult=" << result << "\n";
1753 return result;
1754}
1755
1756int
1758 return MIN2(MAX2(0, stripe(myRelY)), numStripes(myLane) - 1);
1759}
1760
1761
1762int
1764 return MIN2(MAX2(0, otherStripe(myRelY)), numStripes(myLane) - 1);
1765}
1766
1767
1768double
1770 if (myStage->getNextRouteEdge() == nullptr) {
1771 return myDir * (myStage->getArrivalPos() - myRelX) - POSITION_EPS - (
1772 (myWaitingTime > DELTA_T && (myStage->getDestinationStop() == nullptr ||
1773 myStage->getDestinationStop()->getWaitingCapacity() > myStage->getDestinationStop()->getNumWaitingPersons()))
1774 ? getMinGap() : 0);
1775 } else {
1776 const double length = myWalkingAreaPath == nullptr ? myLane->getLength() : myWalkingAreaPath->length;
1777 return myDir == FORWARD ? length - myRelX : myRelX;
1778 }
1779}
1780
1781
1782bool
1784 double dist = distToLaneEnd();
1785 if (DEBUGCOND(*this)) {
1786 std::cout << SIMTIME << " ped=" << myPerson->getID() << " myRelX=" << myRelX << " dist=" << dist << "\n";
1787 }
1788 if (dist <= 0) {
1789 //if (ped.myPerson->getID() == DEBUG1) {
1790 // std::cout << SIMTIME << " addToLane x=" << ped.myRelX << " newDir=" << newDir << " newLane=" << newLane->getID() << " walkingAreaShape=" << walkingAreaShape << "\n";
1791 //}
1792 //std::cout << " changing to " << newLane->getID() << " myRelY=" << ped.myRelY << " oldStripes=" << numStripes(myLane) << " newStripes=" << numStripes(newLane);
1793 //std::cout << " newY=" << ped.myRelY << " myDir=" << ped.myDir << " newDir=" << newDir;
1794 const int oldDir = myDir;
1795 const MSLane* oldLane = myLane;
1796 myLane = myNLI.lane;
1797 myDir = myNLI.dir;
1798 const bool normalLane = (myLane == nullptr || myLane->getEdge().getFunction() == SumoXMLEdgeFunc::NORMAL || &myLane->getEdge() == myStage->getNextRouteEdge());
1799 if DEBUGCOND(*this) {
1800 std::cout << SIMTIME
1801 << " ped=" << myPerson->getID()
1802 << " moveToNextLane old=" << oldLane->getID()
1803 << " new=" << (myLane == nullptr ? "NULL" : myLane->getID())
1804 << " oldDir=" << oldDir
1805 << " newDir=" << myDir
1806 << " myRelX=" << myRelX
1807 << " dist=" << dist
1808 << "\n";
1809 }
1810 if (myLane == nullptr) {
1811 myRelX = myStage->getArrivalPos();
1812 }
1813 // moveToNextEdge might destroy the person and thus mess up the heap. Better check first
1814 if (myStage->getRouteStep() == myStage->getRoute().end() - 1) {
1815 myLane = nullptr;
1816 } else {
1817 const bool arrived = myStage->moveToNextEdge(myPerson, currentTime, oldDir, normalLane ? nullptr : &myLane->getEdge());
1818 UNUSED_PARAMETER(arrived);
1819 assert(!arrived);
1820 assert(myDir != UNDEFINED_DIRECTION);
1821 myNLI = getNextLane(*this, myLane, oldLane);
1822 // reminders must be called after updated myNLI (to ensure that getNextEdgePtr returns the correct edge)
1823 myStage->activateEntryReminders(myPerson);
1824 assert(myNLI.lane != oldLane); // do not turn around
1825 if DEBUGCOND(*this) {
1826 std::cout << " nextLane=" << (myNLI.lane == nullptr ? "NULL" : myNLI.lane->getID()) << "\n";
1827 }
1828 if (myLane->getEdge().isWalkingArea()) {
1829 if (myNLI.dir != UNDEFINED_DIRECTION) {
1830 myWalkingAreaPath = getWalkingAreaPath(&myLane->getEdge(), oldLane, myNLI.lane);
1831 assert(myWalkingAreaPath->shape.size() >= 2);
1832 if DEBUGCOND(*this) {
1833 std::cout << " mWAPath shape=" << myWalkingAreaPath->shape << " length=" << myWalkingAreaPath->length << "\n";
1834 }
1835 } else {
1836 // disconnnected route. move to the next edge
1837 if (OptionsCont::getOptions().getBool("ignore-route-errors")) {
1838 // try to determine direction from topology, otherwise maintain current direction
1839 const MSEdge* currRouteEdge = *myStage->getRouteStep();
1840 const MSEdge* nextRouteEdge = myStage->getNextRouteEdge();
1841 if ((nextRouteEdge->getToJunction() == currRouteEdge->getFromJunction())
1842 || nextRouteEdge->getToJunction() == currRouteEdge->getToJunction()) {
1843 myDir = BACKWARD;
1844 } else if ((nextRouteEdge->getFromJunction() == currRouteEdge->getFromJunction())
1845 || nextRouteEdge->getFromJunction() == currRouteEdge->getToJunction()) {
1846 myDir = FORWARD;
1847 }
1848 myStage->moveToNextEdge(myPerson, currentTime, oldDir, nullptr);
1849 myLane = myNLI.lane;
1850 assert(myLane != 0);
1851 assert(myLane->getEdge().getFunction() == SumoXMLEdgeFunc::NORMAL);
1852 myNLI = getNextLane(*this, myLane, oldLane);
1853 myWalkingAreaPath = nullptr;
1854 } else {
1855 throw ProcessError("Disconnected walk for person '" + myPerson->getID() + "'.");
1856 }
1857 }
1858 } else {
1859 myWalkingAreaPath = nullptr;
1860 }
1861 // adapt x to fit onto the new lane
1862 // (make sure we do not move past the end of the new lane since that
1863 // lane was not checked for obstacles)
1864 const double newLength = (myWalkingAreaPath == nullptr ? myLane->getLength() : myWalkingAreaPath->length);
1865 if (-dist > newLength) {
1866 assert(OptionsCont::getOptions().getBool("ignore-route-errors"));
1867 // should not happen because the end of myLane should have been an obstacle as well
1868 // (only when the route is broken)
1869 dist = -newLength;
1870 }
1871 if (myDir == BACKWARD) {
1872 myRelX = newLength + dist;
1873 } else {
1874 myRelX = -dist;
1875 }
1876 if DEBUGCOND(*this) {
1877 std::cout << SIMTIME << " update myRelX ped=" << myPerson->getID()
1878 << " newLength=" << newLength
1879 << " dist=" << dist
1880 << " myRelX=" << myRelX
1881 << "\n";
1882 }
1883 // adjust to change in direction
1884 if (myDir != oldDir) {
1885 myRelY = (numStripes(oldLane) - 1) * stripeWidth - myRelY;
1886 }
1887 // adjust to differences in sidewalk width
1888 const int offset = getStripeOffset(numStripes(oldLane), numStripes(myLane), oldDir != myDir && numStripes(myLane) < numStripes(oldLane));
1889 myRelY += offset * stripeWidth;
1890 if DEBUGCOND(*this) {
1891 std::cout << SIMTIME << " transformY ped=" << myPerson->getID()
1892 << " newLane=" << Named::getIDSecure(myLane)
1893 << " newY=" << myRelY
1894 << " os=" << numStripes(oldLane) << " ns=" << numStripes(myLane)
1895 << " od=" << oldDir << " nd=" << myDir
1896 << " offset=" << offset << "\n";
1897 }
1898 }
1899 myAngle = std::numeric_limits<double>::max(); // see #9014
1900 return true;
1901 } else {
1902 return false;
1903 }
1904}
1905
1906void
1908 const int stripes = (int)obs.size();
1909 const int sMax = stripes - 1;
1910 assert(stripes == numStripes(myLane));
1911 // account stage-specific max speed but also for normal lane speed limit
1912 // (speed limits on crossings and walkingareas ignored due to #11527)
1913 const double vMax = (myStage->getConfiguredSpeed() >= 0
1914 ? myStage->getConfiguredSpeed()
1915 : (myLane->isNormal() || myLane->isInternal()
1916 ? myLane->getVehicleMaxSpeed(myPerson)
1917 : myStage->getMaxSpeed(myPerson)));
1918 // ultimate goal is to choose the prefered stripe (chosen)
1919 const int current = stripe();
1920 const int other = otherStripe();
1921 // compute distances
1922 std::vector<double> distance(stripes);
1923 for (int i = 0; i < stripes; ++i) {
1924 distance[i] = distanceTo(obs[i], obs[i].type == OBSTACLE_PED);
1925 }
1926 // compute utility for all stripes
1927 std::vector<double> utility(stripes, 0);
1928 // forbid stripes which are blocked and also all stripes behind them
1929 for (int i = 0; i < stripes; ++i) {
1930 if (distance[i] == DIST_OVERLAP) {
1931 if (i == current && (!myWaitingToEnter || stripe() != stripe(myRelY))) {
1932 utility[i] += OBSTRUCTED_PENALTY;
1933 }
1934 if (i < current) {
1935 for (int j = 0; j <= i; ++j) {
1936 utility[j] += OBSTRUCTED_PENALTY;
1937 }
1938 }
1939 if (i > current) {
1940 for (int j = i; j < stripes; ++j) {
1941 utility[j] += OBSTRUCTED_PENALTY;
1942 }
1943 }
1944 }
1945 }
1946 // forbid a portion of the leftmost stripes (in walking direction).
1947 // lanes with stripes less than 1 / RESERVE_FOR_ONCOMING_FACTOR
1948 // may still deadlock in heavy pedestrian traffic
1949 const bool onJunction = myLane->getEdge().isWalkingArea() || myLane->getEdge().isCrossing();
1950 const int reserved = (int)floor(stripes * (onJunction ? RESERVE_FOR_ONCOMING_FACTOR_JUNCTIONS : RESERVE_FOR_ONCOMING_FACTOR));
1951 if (myDir == FORWARD) {
1952 for (int i = 0; i < reserved; ++i) {
1953 utility[i] += INAPPROPRIATE_PENALTY * (i == current ? 0.5 : 1);
1954 }
1955 } else {
1956 for (int i = sMax; i > sMax - reserved; --i) {
1957 utility[i] += INAPPROPRIATE_PENALTY * (i == current ? 0.5 : 1);
1958 }
1959 }
1960 // adapt utility based on obstacles
1961 for (int i = 0; i < stripes; ++i) {
1962 if (obs[i].speed * myDir < 0) {
1963 // penalize evasion to the left unless the obstacle is a vehicle
1964 if ((myDir == FORWARD || obs[i].type == OBSTACLE_VEHICLE) && i > 0) {
1965 utility[i - 1] -= 0.5;
1966 } else if (myDir == BACKWARD && i < sMax) {
1967 utility[i + 1] -= 0.5;
1968 }
1969 }
1970 // compute expected distance achievable by staying on this stripe for a time horizon
1971 const double walkDist = MAX2(0., distance[i]); // disregard special distance flags
1972 const double lookAhead = obs[i].speed * myDir >= 0 ? LOOKAHEAD_SAMEDIR : LOOKAHEAD_ONCOMING;
1973 const double expectedDist = MIN2(vMax * LOOKAHEAD_SAMEDIR, walkDist + obs[i].speed * myDir * lookAhead);
1974 if (expectedDist >= 0) {
1975 utility[i] += expectedDist;
1976 } else {
1977 // let only the distance count
1978 utility[i] += ONCOMING_CONFLICT_PENALTY + distance[i];
1979 }
1980 }
1981 // discourage use of the leftmost stripe (in walking direction) if there are oncoming
1982 if (myDir == FORWARD && obs[0].speed < 0) {
1983 utility[0] += ONCOMING_CONFLICT_PENALTY;
1984 } else if (myDir == BACKWARD && obs[sMax].speed > 0) {
1985 utility[sMax] += ONCOMING_CONFLICT_PENALTY;
1986 }
1987 // penalize lateral movement (if the current stripe permits walking)
1988 if (distance[current] > 0 && myWaitingTime == 0) {
1989 for (int i = 0; i < stripes; ++i) {
1990 utility[i] += abs(i - current) * LATERAL_PENALTY;
1991 }
1992 }
1993 // walk on the right side on shared space
1994 if (myLane->getPermissions() != SVC_PEDESTRIAN && myDir == BACKWARD) {
1995 for (int i = 0; i < stripes; ++i) {
1996 if (i <= current) {
1997 utility[i] += (sMax - i + 1) * LATERAL_PENALTY;
1998 }
1999 }
2000 }
2001
2002 // select best stripe
2003 int chosen = current;
2004 for (int i = 0; i < stripes; ++i) {
2005 if (utility[i] > utility[chosen] && utility[i] >= INAPPROPRIATE_PENALTY * 0.5) {
2006 chosen = i;
2007 }
2008 }
2009 // compute speed components along both axes
2010 const int next = (chosen == current ? current : (chosen < current ? current - 1 : current + 1));
2011 double xDist = MIN3(distance[current], distance[other], distance[next]);
2012 if (next != chosen) {
2013 // ensure that we do not collide with an obstacle in the stripe beyond
2014 // next as this might become the 'other' stripe in the next step
2015 const int nextOther = chosen < current ? current - 2 : current + 2;
2016 xDist = MIN2(xDist, distance[nextOther]);
2017 }
2018 // XXX preferred gap differs between approaching a standing obstacle or a moving obstacle
2019 const double preferredGap = NUMERICAL_EPS;
2020 double xSpeed = MIN2(vMax, MAX2(0., DIST2SPEED(xDist - preferredGap)));
2021 if (xSpeed < NUMERICAL_EPS) {
2022 xSpeed = 0.;
2023 }
2024 if (DEBUGCOND(*this)) {
2025 std::cout << " xSpeedPotential=" << xSpeed << "\n";
2026 }
2027 // avoid tiny steps
2028 // XXX pressure from behind?
2029 if (mySpeed == 0 && xDist < MIN_STARTUP_DIST &&
2030 // unless walking towards a short lane
2031 !(
2032 (xDist == distance[current] && obs[current].type >= OBSTACLE_END)
2033 || (xDist == distance[other] && obs[other].type >= OBSTACLE_END)
2034 || (xDist == distance[next] && obs[next].type >= OBSTACLE_END))
2035 ) {
2036 xSpeed = 0;
2037 }
2038 if (xSpeed == 0) {
2039 if (myWaitingTime > ((myLane->getEdge().isCrossing()
2040 // treat shared walkingarea like a crossing to avoid deadlocking vehicles
2041 || (myLane->getEdge().isWalkingArea() && obs[current].type == OBSTACLE_VEHICLE
2042 && myWalkingAreaFoes.find(&myLane->getEdge()) != myWalkingAreaFoes.end())) ? jamTimeCrossing : jamTime)
2043 || (sMax == 0 && obs[0].speed * myDir < 0 && myWaitingTime > jamTimeNarrow)
2044 || myAmJammed) {
2045 // squeeze slowly through the crowd ignoring others
2046 if (!myAmJammed) {
2048 WRITE_WARNINGF(TL("Person '%' is jammed on edge '%', time=%."),
2049 myPerson->getID(), myStage->getEdge()->getID(), time2string(SIMSTEP));
2050 myAmJammed = true;
2051 }
2052 xSpeed = vMax / 4;
2053 }
2054 } else if (myAmJammed && stripe(myRelY) >= 0 && stripe(myRelY) <= sMax && xDist >= MIN_STARTUP_DIST) {
2055 myAmJammed = false;
2056 }
2057 // dawdling
2058 const double dawdle = MIN2(xSpeed, RandHelper::rand() * vMax * dawdling);
2059 xSpeed -= dawdle;
2060
2061 // XXX ensure that diagonal speed <= vMax
2062 // avoid deadlocks on narrow sidewalks
2063 //if (oncoming && xSpeed == 0 && myStage->getWaitingTime(currentTime) > TIME2STEPS(ONCOMIN_PATIENCE)) {
2064 // if DEBUGCOND(*this) std::cout << " stepping asside to resolve oncoming deadlock\n";
2065 // xSpeed = POSITION_EPS; // reset myWaitingTime
2066 // if (myDir == FORWARD && chosen < sMax) {
2067 // chosen += 1;
2068 // } else if (myDir == BACKWARD && chosen > 0) {
2069 // chosen -= 1;
2070 // }
2071 //}
2072 const double maxYSpeed = MIN2(MAX2(vMax * LATERAL_SPEED_FACTOR, vMax - xSpeed), stripeWidth);
2073 double ySpeed = 0;
2074 double yDist = 0;
2075 if (utility[next] > OBSTRUCTION_THRESHOLD && utility[chosen] > OBSTRUCTION_THRESHOLD) {
2076 // don't move laterally if the stripes are blocked
2077 yDist = (chosen * stripeWidth) - myRelY;
2078 if (fabs(yDist) > NUMERICAL_EPS) {
2079 ySpeed = (yDist > 0 ?
2080 MIN2(maxYSpeed, DIST2SPEED(yDist)) :
2081 MAX2(-maxYSpeed, DIST2SPEED(yDist)));
2082 }
2083 } else if (utility[next] <= OBSTRUCTION_THRESHOLD && obs[next].type == OBSTACLE_VEHICLE
2084 // still on the road
2085 && stripe() == stripe(myRelY)
2086 // only when the vehicle is moving on the same lane
2087 && !(myLane->getEdge().isCrossing() || myLane->getEdge().isWalkingArea())) {
2088 // step aside to let the vehicle pass
2089 int stepAsideDir = myDir;
2090 if (myLane->getEdge().getLanes().size() > 1 || current > sMax / 2) {
2091 // always step to the right on multi-lane edges or when closer to
2092 // the right side
2093 stepAsideDir = FORWARD;
2094 }
2095 myAmJammed = true; // ignore pedestrian-pedestrian collisions
2096 ySpeed = stepAsideDir * vMax;
2097 }
2098
2099 // DEBUG
2100 if DEBUGCOND(*this) {
2101 std::cout << SIMTIME
2102 << " ped=" << myPerson->getID()
2103 << " edge=" << myStage->getEdge()->getID()
2104 << " x=" << myRelX
2105 << " y=" << myRelY
2106 << " d=" << myDir
2107 << " pvx=" << mySpeed
2108 << " cur=" << current
2109 << " cho=" << chosen
2110 << " oth=" << other
2111 << " nxt=" << next
2112 << " vx=" << xSpeed
2113 << " dawdle=" << dawdle
2114 << " vy=" << ySpeed
2115 << " xd=" << xDist
2116 << " yd=" << yDist
2117 << " vMax=" << vMax
2118 << " wTime=" << myStage->getWaitingTime(currentTime)
2119 << " jammed=" << myAmJammed
2120 << "\n";
2121 if (DEBUGCOND(*this)) {
2122 for (int i = 0; i < stripes; ++i) {
2123 const Obstacle& o = obs[i];
2124 std::cout << " util=" << utility[i] << " dist=" << distance[i] << " o=" << o.description;
2125 if (o.description != "") {
2126 std::cout << " xF=" << o.xFwd << " xB=" << o.xBack << " v=" << o.speed;
2127 }
2128 if (i == current) {
2129 std::cout << " current";
2130 }
2131 if (i == other && i != current) {
2132 std::cout << " other";
2133 }
2134 if (i == chosen) {
2135 std::cout << " chosen";
2136 }
2137 if (i == next) {
2138 std::cout << " next";
2139 }
2140 std::cout << "\n";
2141 }
2142 }
2143 }
2144 myRelX += SPEED2DIST(xSpeed * myDir);
2145 myRelY += SPEED2DIST(ySpeed);
2146 mySpeedLat = ySpeed;
2147 mySpeed = xSpeed;
2148 if (xSpeed >= SUMO_const_haltingSpeed) {
2149 myWaitingToEnter = false;
2150 myWaitingTime = 0;
2151 } else {
2152 myWaitingTime += DELTA_T;
2153 }
2154 myAngle = std::numeric_limits<double>::max(); // set on first access or via remote control
2155}
2156
2157
2158double
2160 return MAX2(0., MIN2(1., myPerson->getVehicleType().getImpatience()
2161 + STEPS2TIME(myStage->getWaitingTime(now)) / MAX_WAIT_TOLERANCE));
2162}
2163
2164
2165double
2167 return myRelX;
2168}
2169
2170
2171int
2173 return myDir;
2174}
2175
2176
2179 if (myRemoteXYPos != Position::INVALID) {
2180 return myRemoteXYPos;
2181 }
2182 if (myLane == nullptr) {
2183 // pedestrian has already finished
2184 return Position::INVALID;
2185 }
2186 const double lateral_offset = myRelY + (stripeWidth - myLane->getWidth()) * 0.5;
2187 if (myWalkingAreaPath == nullptr) {
2188 return stage.getLanePosition(myLane, myRelX, lateral_offset);
2189 } else {
2190 //if DEBUGCOND(*this) {
2191 // std::cout << SIMTIME
2192 // << " getPosition (walkingArea)"
2193 // << " p=" << myPerson->getID()
2194 // << " x=" << myRelX
2195 // << " y=" << myRelY
2196 // << " latOffset=" << lateral_offset
2197 // << " shape=" << myWalkingAreaPath->shape
2198 // << " pos=" << myWalkingAreaPath->shape.positionAtOffset(myRelX, lateral_offset)
2199 // << "\n";
2200 //}
2201 return myWalkingAreaPath->shape.positionAtOffset(myRelX, lateral_offset);
2202 }
2203}
2204
2205
2206double
2208 if (myAngle != std::numeric_limits<double>::max()) {
2209 return myAngle;
2210 }
2211 if (myLane == nullptr) {
2212 // pedestrian has already finished
2213 return 0;
2214 }
2215 const PositionVector& shp = myWalkingAreaPath == nullptr ? myLane->getShape() : myWalkingAreaPath->shape;
2216 double geomX = myWalkingAreaPath == nullptr ? myLane->interpolateLanePosToGeometryPos(myRelX) : myRelX;
2217 double angle = shp.rotationAtOffset(geomX) + (myDir == MSPModel::BACKWARD ? M_PI : 0);
2218 if (myDir == MSPModel::BACKWARD) {
2219 angle += atan2(mySpeedLat, MAX2(mySpeed, NUMERICAL_EPS));
2220 } else { // myDir == MSPModel::FORWARD
2221 angle -= atan2(mySpeedLat, MAX2(mySpeed, NUMERICAL_EPS));
2222 }
2223 if (angle > M_PI) {
2224 angle -= 2 * M_PI;
2225 }
2226 myAngle = angle;
2227 return angle;
2228}
2229
2230
2233 return myWaitingTime;
2234}
2235
2236
2237double
2239 return mySpeed;
2240}
2241
2242
2243const MSEdge*
2245 return myNLI.lane == nullptr ? nullptr : &myNLI.lane->getEdge();
2246}
2247
2248
2249void
2250MSPModel_Striping::PState::moveTo(MSPerson* p, MSLane* lane, double lanePos, double lanePosLat, SUMOTime t) {
2251 ConstMSEdgeVector newEdges; // keep route
2252 int routeOffset = 0;
2253 bool laneOnRoute = false;
2254 const MSJunction* laneOnJunction = lane->isNormal() ? nullptr : lane->getEdge().getToJunction();
2255 for (const MSEdge* edge : myStage->getRoute()) {
2256 if (edge == &lane->getEdge()
2257 || edge->getToJunction() == laneOnJunction
2258 || edge->getFromJunction() == laneOnJunction) {
2259 laneOnRoute = true;
2260 break;
2261 }
2262 routeOffset++;
2263 }
2264 if (!laneOnRoute) {
2265 throw ProcessError("Lane '" + lane->getID() + "' is not on the route of person '" + getID() + "'.");
2266 }
2267 Position pos = lane->geometryPositionAtOffset(lanePos, lanePosLat);
2268 if (lane->getEdge().isWalkingArea() && (myWalkingAreaPath == nullptr || myWalkingAreaPath->lane != lane)) {
2269 // entered new walkingarea. Determine path to guess position
2270 const MSEdge* prevEdge = myStage->getRoute()[routeOffset];
2271 const MSEdge* nextEdge = routeOffset + 1 < (int)myStage->getRoute().size() ? myStage->getRoute()[routeOffset + 1] : nullptr;
2272 const WalkingAreaPath* guessed = guessPath(&lane->getEdge(), prevEdge, nextEdge);
2273 const double maxPos = guessed->shape.length() - NUMERICAL_EPS;
2274 if (lanePos > maxPos + POSITION_EPS || lanePos < -POSITION_EPS) {
2275 throw ProcessError("Lane position " + toString(lanePos) + " cannot be mapped onto walkingarea '" + lane->getID()
2276 + "' (fromLane='" + guessed->from->getID()
2277 + "' toLane='" + guessed->to->getID() + "') for person '" + getID() + "' time=" + time2string(t) + ".");
2278 }
2279 // give some slack
2280 lanePos = MIN2(maxPos, MAX2(NUMERICAL_EPS, lanePos));
2281 pos = guessed->shape.positionAtOffset(lanePos, lanePosLat);
2282 }
2283 const double angle = GeomHelper::naviDegree(p->getPosition().angleTo2D(pos));
2284 moveToXY(p, pos, lane, lanePos, lanePosLat, angle, routeOffset, newEdges, t);
2285}
2286
2287
2288void
2290 double lanePosLat, double angle, int routeOffset,
2291 const ConstMSEdgeVector& edges, SUMOTime t) {
2293 assert(p == myPerson);
2294 assert(pm != nullptr);
2295 const double oldAngle = GeomHelper::naviDegree(getAngle(*myStage, t));
2296 // person already walking in this step. undo this to obtain the previous position
2297 const double oldX = myRelX - SPEED2DIST(mySpeed * myDir);
2298 const double tmp = myRelX;
2299 myRelX = oldX;
2300 Position oldPos = getPosition(*myStage, t);
2301 myRelX = tmp;
2302 //if (oldPos == Position::INVALID) {
2303 // oldPos = pos
2304 //}
2305 myAngle = GeomHelper::fromNaviDegree(angle);
2306#ifdef DEBUG_MOVETOXY
2307 std::cout << SIMTIME << " ped=" << p->getID()
2308 << " moveToXY"
2309 << " pos=" << pos
2310 << " lane=" << lane->getID()
2311 << " lanePos=" << lanePos
2312 << " lanePosLat=" << lanePosLat
2313 << " angle=" << angle
2314 << " routeOffset=" << routeOffset
2315 << " edges=" << toString(edges)
2316 << " oldLane=" << Named::getIDSecure(myLane)
2317 << " path=" << (myWalkingAreaPath == nullptr ? "null" : (myWalkingAreaPath->from->getID() + "->" + myWalkingAreaPath->to->getID())) << "\n";
2318#endif
2319
2320 if (lane != myLane && myLane != nullptr) {
2321 pm->remove(this);
2322 pm->registerActive();
2323 }
2324 if (lane != nullptr &&
2325 fabs(lanePosLat) < (0.5 * (lane->getWidth() + p->getVehicleType().getWidth()) + SIDEWALK_OFFSET)) {
2326 myRemoteXYPos = Position::INVALID;
2327 const MSEdge* old = myStage->getEdge();
2328 const MSLane* oldLane = myLane;
2329 if (lane != myLane) {
2330 // implicitly adds new active lane if necessary
2331 pm->myActiveLanes[lane].push_back(this);
2332 }
2333 if (edges.empty()) {
2334 // map within route
2335 myStage->setRouteIndex(myPerson, routeOffset);
2336 } else {
2337 myStage->replaceRoute(myPerson, edges, routeOffset);
2338 }
2339 if (!lane->getEdge().isNormal()) {
2340 myStage->moveToNextEdge(myPerson, t, myDir, &lane->getEdge());
2341 }
2342
2343 myLane = lane;
2344 const double lateral_offset = (lane->getWidth() - stripeWidth) * 0.5;
2345 if (lane->getEdge().isWalkingArea()) {
2346 if (myWalkingAreaPath == nullptr || myWalkingAreaPath->lane != lane) {
2347 // entered new walkingarea. Determine path
2348 myWalkingAreaPath = guessPath(&lane->getEdge(), old, myStage->getNextRouteEdge());
2349#ifdef DEBUG_MOVETOXY
2350 std::cout << " guessPath old=" << old->getID() << " next=" << Named::getIDSecure(myStage->getNextRouteEdge())
2351 << " path=" << myWalkingAreaPath->from->getID() << "->" << myWalkingAreaPath->to->getID() << "\n";
2352#endif
2353 }
2354 // lanePos and lanePosLat are matched onto the circumference of the
2355 // walkingarea. Use pos instead
2356 const Position relPos = myWalkingAreaPath->shape.transformToVectorCoordinates(pos);
2357 if (relPos == Position::INVALID) {
2358 WRITE_WARNING("Could not map position " + toString(pos) + " onto lane '" + myLane->getID()
2359 + "' (fromLane='" + myWalkingAreaPath->from->getID()
2360 + "' toLane='" + myWalkingAreaPath->to->getID() + "') for person '" + getID() + "' time=" + time2string(t) + ".");
2361 myRemoteXYPos = pos;
2362 } else {
2363 myRelX = relPos.x();
2364 myRelY = lateral_offset + relPos.y();
2365 }
2366 } else {
2367 myWalkingAreaPath = nullptr;
2368 myRelX = lanePos;
2369 myRelY = lateral_offset - lanePosLat;
2370 }
2371 // guess direction
2372 const double angleDiff = GeomHelper::getMinAngleDiff(angle, oldAngle);
2373 if (myStage->getNextRouteEdge() != nullptr) {
2374 if (myStage->getEdge()->getToJunction() == myStage->getNextRouteEdge()->getFromJunction() ||
2375 myStage->getEdge()->getToJunction() == myStage->getNextRouteEdge()->getToJunction()) {
2376 myDir = FORWARD;
2377 } else {
2378 myDir = BACKWARD;
2379 }
2380 } else {
2381 // guess from angle
2382 if (angleDiff <= 90) {
2383 // keep direction
2384 if (myDir == UNDEFINED_DIRECTION) {
2385 myDir = FORWARD;
2386 }
2387 } else {
2388 // change direction
2389 myDir = (myDir == BACKWARD) ? FORWARD : BACKWARD;
2390 }
2391 }
2392 // update next lane info (after guessing direction)
2393 if (oldLane == nullptr || &oldLane->getEdge() != &myLane->getEdge()) {
2394 const MSLane* sidewalk = getSidewalk<MSEdge, MSLane>(&myLane->getEdge(), p->getVClass());
2395 // assume that we will eventually move back onto the sidewalk if
2396 // there is one
2397 myNLI = getNextLane(*this, sidewalk == nullptr ? myLane : sidewalk, nullptr);
2398 myStage->activateEntryReminders(myPerson);
2399#ifdef DEBUG_MOVETOXY
2400 std::cout << " myNLI=" << Named::getIDSecure(myNLI.lane) << " link=" << (myNLI.link == nullptr ? "NULL" : myNLI.link->getDescription()) << " dir=" << myNLI.dir << "\n";
2401#endif
2402 }
2403#ifdef DEBUG_MOVETOXY
2404 std::cout << " newRelPos=" << Position(myRelX, myRelY) << " edge=" << myPerson->getEdge()->getID() << " newPos=" << myPerson->getPosition()
2405 << " oldAngle=" << oldAngle << " angleDiff=" << angleDiff << " newDir=" << myDir << "\n";
2406#endif
2407 if (oldLane == myLane) {
2408 mySpeed = DIST2SPEED(fabs(oldX - myRelX));
2409 } else {
2410 //std::cout << SIMTIME << " oldX=" << oldX << " oldSpeed=" << mySpeed << " oldPos=" << oldPos << " pos=" << pos << " dist=" << oldPos.distanceTo2D(pos) << " oldLane=" << Named::getIDSecure(oldLane) << " lane=" << lane->getID() << "\n";
2411 mySpeed = DIST2SPEED(oldPos.distanceTo2D(pos));
2412 }
2413 } else {
2414 // map outside the network
2415 myRemoteXYPos = pos;
2416 mySpeed = DIST2SPEED(oldPos.distanceTo2D(pos));
2417 }
2418
2419}
2420
2421
2422bool
2424 return myAmJammed;
2425}
2426
2427const MSLane*
2429 return myLane;
2430}
2431
2432double
2433MSPModel_Striping::PState::distanceTo(const Obstacle& obs, const bool includeMinGap) const {
2434 // check for overlap
2435 const double maxX = getMaxX(includeMinGap);
2436 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 if ((obs.xFwd >= maxX && obs.xBack <= maxX) || (obs.xFwd <= maxX && obs.xFwd >= minX)) {
2441 // avoid blocking by itself on looped route
2442 return (obs.type == OBSTACLE_PED && obs.description == myPerson->getID()) ? DIST_FAR_AWAY : DIST_OVERLAP;
2443 }
2444 if (myDir == FORWARD) {
2445 return obs.xFwd < minX ? DIST_BEHIND : obs.xBack - maxX;
2446 } else {
2447 return obs.xBack > maxX ? DIST_BEHIND : minX - obs.xFwd;
2448 }
2449}
2450
2451
2452void
2454 for (int i = 0; i < (int)into.size(); ++i) {
2455 if (gDebugFlag1) {
2456 std::cout << " i=" << i << " maxX=" << getMaxX(true) << " minX=" << getMinX(true)
2457 << " into=" << into[i].description << " iDist=" << distanceTo(into[i], into[i].type == OBSTACLE_PED)
2458 << " obs2=" << obs2[i].description << " oDist=" << distanceTo(obs2[i], obs2[i].type == OBSTACLE_PED) << "\n";
2459 }
2460 const double dO = distanceTo(obs2[i], obs2[i].type == OBSTACLE_PED);
2461 const double dI = distanceTo(into[i], into[i].type == OBSTACLE_PED);
2462 if (dO < dI) {
2463 into[i] = obs2[i];
2464 } else if (dO == dI
2465 && into[i].type != OBSTACLE_PED
2466 && into[i].type != OBSTACLE_VEHICLE
2467 && (obs2[i].type == OBSTACLE_PED ||
2468 obs2[i].type == OBSTACLE_VEHICLE)) {
2469 into[i] = obs2[i];
2470 }
2471 }
2472}
2473
2474void
2475MSPModel_Striping::PState::mergeObstacles(Obstacles& into, const Obstacles& obs2, int dir, int offset) {
2476 for (int i = 0; i < (int)into.size(); ++i) {
2477 int i2 = i + offset;
2478 if (i2 >= 0 && i2 < (int)obs2.size()) {
2479 if (dir == FORWARD) {
2480 if (obs2[i2].xBack < into[i].xBack) {
2481 into[i] = obs2[i2];
2482 }
2483 } else {
2484 if (obs2[i2].xFwd > into[i].xFwd) {
2485 into[i] = obs2[i2];
2486 }
2487 }
2488 }
2489 }
2490}
2491
2492
2493bool
2495 if (link->haveRed()) {
2496 const double ignoreRedTime = myPerson->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_AFTER_RED_TIME, -1);
2497 if (ignoreRedTime >= 0) {
2498 const double redDuration = STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - link->getLastStateChange());
2499 if (DEBUGCOND(*this)) {
2500 std::cout << SIMTIME << " ignoreRedTime=" << ignoreRedTime << " redDuration=" << redDuration << "\n";
2501 }
2502 return ignoreRedTime > redDuration;
2503 } else {
2504 return false;
2505 }
2506 } else {
2507 return false;
2508 }
2509}
2510
2511const std::string&
2513 return myPerson->getID();
2514}
2515
2516double
2518 return myPerson->getVehicleType().getWidth();
2519}
2520
2521
2522bool
2524 return myPerson->hasInfluencer() && myPerson->getInfluencer().isRemoteControlled();
2525}
2526
2527// ===========================================================================
2528// MSPModel_Striping::PStateVehicle method definitions
2529// ===========================================================================
2530
2531MSPModel_Striping::PStateVehicle::PStateVehicle(const MSVehicle* veh, const MSLane* walkingarea, double relX, double relY, double xWidth, double yWidth):
2532 myVehicle(veh), myXWidth(xWidth), myYWidth(yWidth) {
2533 myLane = walkingarea; // to ensure correct limits when calling otherStripe()
2534 myRelX = relX;
2535 myRelY = relY;
2536}
2537
2538const std::string&
2540 return myVehicle->getID();
2541}
2542
2543double
2545 return myYWidth;
2546}
2547
2548double
2549MSPModel_Striping::PStateVehicle::getMinX(const bool /*includeMinGap*/) const {
2550 return myRelX - myXWidth / 2 - SAFETY_GAP ;
2551}
2552
2553double
2554MSPModel_Striping::PStateVehicle::getMaxX(const bool /*includeMinGap*/) const {
2555 return myRelX + myXWidth / 2 + SAFETY_GAP;
2556}
2557
2558// ===========================================================================
2559// MSPModel_Striping::MovePedestrians method definitions
2560// ===========================================================================
2561
2564 std::set<MSPerson*> changedLane;
2565 myModel->moveInDirection(currentTime, changedLane, FORWARD);
2566 myModel->moveInDirection(currentTime, changedLane, BACKWARD);
2567 // DEBUG
2568#ifdef LOG_ALL
2569 for (ActiveLanes::const_iterator it_lane = myModel->getActiveLanes().begin(); it_lane != myModel->getActiveLanes().end(); ++it_lane) {
2570 const MSLane* lane = it_lane->first;
2571 Pedestrians pedestrians = it_lane->second;
2572 if (pedestrians.size() == 0) {
2573 continue;
2574 }
2575 sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(FORWARD));
2576 std::cout << SIMTIME << " lane=" << lane->getID();
2577 for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
2578 const PState& p = *pedestrians[ii];
2579 std::cout << " (" << p.myPerson->getID() << " " << p.myRelX << "," << p.myRelY << " " << p.myDir << ")";
2580 }
2581 std::cout << "\n";
2582 }
2583#endif
2584 return DELTA_T;
2585}
2586
long long int SUMOTime
Definition: GUI.h:36
#define RAD2DEG(x)
Definition: GeomHelper.h:36
std::vector< const MSEdge * > ConstMSEdgeVector
Definition: MSEdge.h:74
std::vector< MSEdge * > MSEdgeVector
Definition: MSEdge.h:73
std::pair< const MSPerson *, double > PersonDist
Definition: MSPModel.h:41
#define DEBUGCOND2(LANE)
#define DEBUGCOND(PED)
#define WRITE_WARNINGF(...)
Definition: MsgHandler.h:266
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:265
#define TL(string)
Definition: MsgHandler.h:282
SUMOTime DELTA_T
Definition: SUMOTime.cpp:37
std::string time2string(SUMOTime t)
convert SUMOTime to string
Definition: SUMOTime.cpp:68
SUMOTime string2time(const std::string &r)
convert string to SUMOTime
Definition: SUMOTime.cpp:45
#define STEPS2TIME(x)
Definition: SUMOTime.h:54
#define SPEED2DIST(x)
Definition: SUMOTime.h:44
#define SIMSTEP
Definition: SUMOTime.h:60
#define SUMOTime_MAX
Definition: SUMOTime.h:33
#define SIMTIME
Definition: SUMOTime.h:61
#define TIME2STEPS(x)
Definition: SUMOTime.h:56
#define DIST2SPEED(x)
Definition: SUMOTime.h:46
@ SVC_PEDESTRIAN
pedestrian
const std::string DEFAULT_PEDTYPE_ID
@ SUMO_ATTR_JM_DRIVE_AFTER_RED_TIME
bool gDebugFlag1
global utility flags for debugging
Definition: StdDefs.cpp:33
#define UNUSED_PARAMETER(x)
Definition: StdDefs.h:30
T MIN3(T a, T b, T c)
Definition: StdDefs.h:84
T MIN2(T a, T b)
Definition: StdDefs.h:71
const double SUMO_const_haltingSpeed
the speed threshold at which vehicles are considered as halting
Definition: StdDefs.h:58
T MAX2(T a, T b)
Definition: StdDefs.h:77
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:46
static double naviDegree(const double angle)
Definition: GeomHelper.cpp:192
static double fromNaviDegree(const double angle)
Definition: GeomHelper.cpp:209
static double angleDiff(const double angle1, const double angle2)
Returns the difference of the second angle to the first angle in radiants.
Definition: GeomHelper.cpp:179
static double getMinAngleDiff(double angle1, double angle2)
Returns the minimum distance (clockwise/counter-clockwise) between both angles.
Definition: GeomHelper.cpp:173
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver's reaction time tau (i....
Definition: MSCFModel.h:373
double getMaxDecel() const
Get the vehicle type's maximal comfortable deceleration [m/s^2].
Definition: MSCFModel.h:262
A road/street connecting two junctions.
Definition: MSEdge.h:77
static const MSEdgeVector & getAllEdges()
Returns all edges with a numerical id.
Definition: MSEdge.cpp:984
bool isCrossing() const
return whether this edge is a pedestrian crossing
Definition: MSEdge.h:270
bool isWalkingArea() const
return whether this edge is walking area
Definition: MSEdge.h:284
const std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
Definition: MSEdge.h:168
bool isNormal() const
return whether this edge is an internal edge
Definition: MSEdge.h:260
const MSJunction * getToJunction() const
Definition: MSEdge.h:415
double getLength() const
return the length of the edge
Definition: MSEdge.h:658
const MSJunction * getFromJunction() const
Definition: MSEdge.h:411
bool isInternal() const
return whether this edge is an internal edge
Definition: MSEdge.h:265
const MSEdgeVector & getPredecessors() const
Definition: MSEdge.h:406
const MSEdgeVector & getSuccessors(SUMOVehicleClass vClass=SVC_IGNORING) const
Returns the following edges, restricted by vClass.
Definition: MSEdge.cpp:1155
virtual void addEvent(Command *operation, SUMOTime execTimeStep=-1)
Adds an Event.
static bool gCheck4Accidents
Definition: MSGlobals.h:85
static bool gUsingInternalLanes
Information whether the simulation regards internal lanes.
Definition: MSGlobals.h:78
The base class for an intersection.
Definition: MSJunction.h:58
AnyVehicleIterator is a structure, which manages the iteration through all vehicles on the lane,...
Definition: MSLane.h:129
Representation of a lane in the micro simulation.
Definition: MSLane.h:84
AnyVehicleIterator anyVehiclesEnd() const
end iterator for iterating over all vehicles touching this lane in downstream direction
Definition: MSLane.h:468
int getVehicleNumberWithPartials() const
Returns the number of vehicles on this lane (including partial occupators)
Definition: MSLane.h:437
const MSLink * getLinkTo(const MSLane *const) const
returns the link to the given lane or nullptr, if it is not connected
Definition: MSLane.cpp:2428
SVCPermissions getPermissions() const
Returns the vehicle class permissions for this lane.
Definition: MSLane.h:583
const std::vector< IncomingLaneInfo > & getIncomingLanes() const
Definition: MSLane.h:879
double getLength() const
Returns the lane's length.
Definition: MSLane.h:575
const PositionVector & getShape() const
Returns this lane's shape.
Definition: MSLane.h:506
const MSLane * getInternalFollowingLane(const MSLane *const) const
returns the internal lane leading to the given lane or nullptr, if there is none
Definition: MSLane.cpp:2440
MSLane * getCanonicalSuccessorLane() const
Definition: MSLane.cpp:2936
MSLane * getLogicalPredecessorLane() const
get the most likely precedecessor lane (sorted using by_connections_to_sorter). The result is cached ...
Definition: MSLane.cpp:2866
AnyVehicleIterator anyVehiclesUpstreamEnd() const
end iterator for iterating over all vehicles touching this lane in upstream direction
Definition: MSLane.h:480
bool isNormal() const
Definition: MSLane.cpp:2336
static bool dictionary(const std::string &id, MSLane *lane)
Static (sic!) container methods {.
Definition: MSLane.cpp:2199
AnyVehicleIterator anyVehiclesUpstreamBegin() const
begin iterator for iterating over all vehicles touching this lane in upstream direction
Definition: MSLane.h:474
AnyVehicleIterator anyVehiclesBegin() const
begin iterator for iterating over all vehicles touching this lane in downstream direction
Definition: MSLane.h:462
MSLane * getBidiLane() const
retrieve bidirectional lane or nullptr
Definition: MSLane.cpp:4252
MSEdge & getEdge() const
Returns the lane's edge.
Definition: MSLane.h:713
double getWidth() const
Returns the lane's width.
Definition: MSLane.h:590
const std::vector< MSLink * > & getLinkCont() const
returns the container with all links !!!
Definition: MSLane.h:675
const Position geometryPositionAtOffset(double offset, double lateralOffset=0) const
Definition: MSLane.h:533
The simulated network and simulation perfomer.
Definition: MSNet.h:88
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:183
MSEventControl * getBeginOfTimestepEvents()
Returns the event control for events executed at the begin of a time step.
Definition: MSNet.h:472
SUMOTime getCurrentTimeStep() const
Returns the current simulation step.
Definition: MSNet.h:321
bool hasPedestrianNetwork() const
return whether the network contains walkingareas and crossings
Definition: MSNet.h:792
MSPedestrianRouter & getPedestrianRouter(const int rngIndex, const MSEdgeVector &prohibited=MSEdgeVector()) const
Definition: MSNet.cpp:1417
MSVehicleControl & getVehicleControl()
Returns the vehicle control.
Definition: MSNet.h:379
virtual MSTransportableControl & getPersonControl()
Returns the person control.
Definition: MSNet.cpp:1096
bool hasInternalLinks() const
return whether the network contains internal links
Definition: MSNet.h:777
SUMOTime execute(SUMOTime currentTime)
Executes the command.
Container for pedestrian state and individual position update function.
virtual double getMaxX(const bool includeMinGap=true) const
return the maximum position on the lane
bool isJammed() const
whether the transportable is jammed
bool myAmJammed
whether the person is jammed
Position myRemoteXYPos
remote-controlled position
bool myWaitingToEnter
whether the pedestrian is waiting to start its walk
const WalkingAreaPath * myWalkingAreaPath
the current walkingAreaPath or 0
SUMOTime getWaitingTime(const MSStageMoving &stage, SUMOTime now) const
return the time the transportable spent standing
PState()
constructor for PStateVehicle
double myRelX
the advancement along the current lane
double distToLaneEnd() const
the absolute distance to the end of the lane in walking direction (or to the arrivalPos)
int myDir
the walking direction on the current lane (1 forward, -1 backward)
double myRelY
the orthogonal shift on the current lane
void mergeObstacles(Obstacles &into, const Obstacles &obs2)
replace obstacles in the first vector with obstacles from the second if they are closer to me
bool isRemoteControlled() const
whether the person is currently being controlled via TraCI
const MSEdge * getNextEdge(const MSStageMoving &stage) const
return the list of internal edges if the transportable is on an intersection
void walk(const Obstacles &obs, SUMOTime currentTime)
perform position update
virtual double getWidth() const
return the person width
double mySpeed
the current walking speed
void saveState(std::ostringstream &out)
Saves the current state into the given stream.
int getDirection(const MSStageMoving &stage, SUMOTime now) const
return the walking direction (FORWARD, BACKWARD)
bool ignoreRed(const MSLink *link) const
whether the pedestrian may ignore a red light
virtual double getMinX(const bool includeMinGap=true) const
return the minimum position on the lane
bool moveToNextLane(SUMOTime currentTime)
return whether this pedestrian has passed the end of the current lane and update myRelX if so
double mySpeedLat
the current lateral walking speed
double getMinGap() const
return the minimum gap of the pedestrian
void moveToXY(MSPerson *p, Position pos, MSLane *lane, double lanePos, double lanePosLat, double angle, int routeOffset, const ConstMSEdgeVector &edges, SUMOTime t)
try to move transportable to the given position
void moveTo(MSPerson *p, MSLane *lane, double lanePos, double lanePosLat, SUMOTime t)
try to move transportable to the given position
double getSpeed(const MSStageMoving &stage) const
return the current speed of the transportable
Position getPosition(const MSStageMoving &stage, SUMOTime now) const
return the network coordinate of the transportable
NextLaneInfo myNLI
information about the upcoming lane
const MSLane * myLane
the current lane of this pedestrian
double getAngle(const MSStageMoving &stage, SUMOTime now) const
return the direction in which the transportable faces in degrees
virtual const std::string & getID() const
return the person id
double getImpatience(SUMOTime now) const
returns the impatience
SUMOTime myWaitingTime
the consecutive time spent at speed 0
double distanceTo(const Obstacle &obs, const bool includeMinGap=true) const
const MSLane * getLane() const
whether the transportable is jammed
double getEdgePos(const MSStageMoving &stage, SUMOTime now) const
abstract methods inherited from PedestrianState
double getLength() const
return the length of the pedestrian
double getWidth() const
return the person width
double getMaxX(const bool includeMinGap=true) const
return the maximum position on the lane
double getMinX(const bool includeMinGap=true) const
return the minimum position on the lane
const std::string & getID() const
return the person id
PStateVehicle(const MSVehicle *veh, const MSLane *walkingarea, double relX, double relY, double xWidth, double yWidth)
sorts the persons by position on the lane. If dir is forward, higher x positions come first.
The pedestrian following model.
static const double MIN_STARTUP_DIST
static double RESERVE_FOR_ONCOMING_FACTOR
static MinNextLengths myMinNextLengths
static bool addVehicleFoe(const MSVehicle *veh, const MSLane *walkingarea, const Position &relPos, double xWidth, double yWidth, double lateral_offset, double minY, double maxY, Pedestrians &toDelete, Pedestrians &transformedPeds)
MSTransportableStateAdapter * loadState(MSTransportable *transportable, MSStageMoving *stage, std::istringstream &in)
load the state of the given transportable
static SUMOTime jamTimeCrossing
bool hasPedestrians(const MSLane *lane)
whether the given lane has pedestrians on it
void moveInDirection(SUMOTime currentTime, std::set< MSPerson * > &changedLane, int dir)
move all pedestrians forward and advance to the next lane if applicable
static void transformToCurrentLanePositions(Obstacles &o, int currentDir, int nextDir, double currentLength, double nextLength)
static int myWalkingAreaDetail
static const double LOOKAHEAD_SAMEDIR
static double minGapToVehicle
static NextLaneInfo getNextLane(const PState &ped, const MSLane *currentLane, const MSLane *prevLane)
computes the successor lane for the given pedestrian and sets the link as well as the direction to us...
static void initWalkingAreaPaths(const MSNet *net)
std::vector< PState * > Pedestrians
ActiveLanes myActiveLanes
store of all lanes which have pedestrians on them
static const double LOOKAROUND_VEHICLES
static const double SQUEEZE
static SUMOTime jamTimeNarrow
static const WalkingAreaPath * getWalkingAreaPath(const MSEdge *walkingArea, const MSLane *before, const MSLane *after)
void arriveAndAdvance(Pedestrians &pedestrians, SUMOTime currentTime, std::set< MSPerson * > &changedLane, int dir)
handle arrivals and lane advancement
std::map< const MSLane *, double > MinNextLengths
static double RESERVE_FOR_ONCOMING_FACTOR_JUNCTIONS
static int getStripeOffset(int origStripes, int destStripes, bool addRemainder)
bool myAmActive
whether an event for pedestrian processing was added
static const WalkingAreaPath * guessPath(const MSEdge *walkingArea, const MSEdge *before, const MSEdge *after)
static SUMOTime jamTime
static double stripeWidth
model parameters
bool blockedAtDist(const MSLane *lane, double vehSide, double vehWidth, double oncomingGap, std::vector< const MSPerson * > *collectBlockers)
whether a pedestrian is blocking the crossing of lane for the given vehicle bondaries
static const double MAX_WAIT_TOLERANCE
static Obstacles getVehicleObstacles(const MSLane *lane, int dir, PState *ped=0)
retrieve vehicle obstacles on the given lane
static const double OBSTRUCTED_PENALTY
std::map< const MSLane *, Obstacles, lane_by_numid_sorter > NextLanesObstacles
static const MSLane * getNextWalkingArea(const MSLane *currentLane, const int dir, const MSLink *&link)
return the next walkingArea in the given direction
PersonDist nextBlocking(const MSLane *lane, double minPos, double minRight, double maxLeft, double stopTime=0, bool bidi=false)
returns the next pedestrian beyond minPos that is laterally between minRight and maxLeft or 0
MSTransportableStateAdapter * add(MSTransportable *transportable, MSStageMoving *stage, SUMOTime now)
register the given person as a pedestrian
static const double DIST_OVERLAP
static const WalkingAreaPath * getArbitraryPath(const MSEdge *walkingArea)
return an arbitrary path across the given walkingArea
static const double LATERAL_PENALTY
std::vector< Obstacle > Obstacles
void remove(MSTransportableStateAdapter *state)
remove the specified person from the pedestrian simulation
static const double DIST_BEHIND
bool usingInternalLanes()
whether movements on intersections are modelled /
void moveInDirectionOnLane(Pedestrians &pedestrians, const MSLane *lane, SUMOTime currentTime, std::set< MSPerson * > &changedLane, int dir)
move pedestrians forward on one lane
MSPModel_Striping(const OptionsCont &oc, MSNet *net)
Constructor (it should not be necessary to construct more than one instance)
static bool usingInternalLanesStatic()
static Obstacles getNeighboringObstacles(const Pedestrians &pedestrians, int egoIndex, int stripes)
static Pedestrians noPedestrians
empty pedestrian vector
static bool myLegacyPosLat
static void addCloserObstacle(Obstacles &obs, double x, int stripe, int numStripes, const std::string &id, double width, int dir, ObstacleType type)
Pedestrians & getPedestrians(const MSLane *lane)
retrieves the pedestian vector for the given lane (may be empty)
static double dawdling
static int numStripes(const MSLane *lane)
return the maximum number of pedestrians walking side by side
static const double OBSTRUCTION_THRESHOLD
static bool addCrossingVehs(const MSLane *crossing, int stripes, double lateral_offset, int dir, Obstacles &crossingVehs, bool prio)
add vehicles driving across
static int connectedDirection(const MSLane *from, const MSLane *to)
returns the direction in which these lanes are connectioned or 0 if they are not
static void DEBUG_PRINT(const Obstacles &obs)
static const double LATERAL_SPEED_FACTOR
static const double INAPPROPRIATE_PENALTY
void clearState()
Resets pedestrians when quick-loading state.
static const double ONCOMING_CONFLICT_PENALTY
int myNumActivePedestrians
the total number of active pedestrians
static const double LOOKAHEAD_ONCOMING
static std::map< const MSEdge *, std::vector< const MSLane * > > myWalkingAreaFoes
const Obstacles & getNextLaneObstacles(NextLanesObstacles &nextLanesObs, const MSLane *lane, const MSLane *nextLane, int stripes, int nextDir, double currentLength, int currentDir)
static const double DIST_FAR_AWAY
std::map< std::pair< const MSLane *, const MSLane * >, const WalkingAreaPath > WalkingAreaPaths
static WalkingAreaPaths myWalkingAreaPaths
store for walkinArea elements
static const int BACKWARD
Definition: MSPModel.h:118
static int canTraverse(int dir, const ConstMSEdgeVector &route)
Definition: MSPModel.cpp:54
static const int FORWARD
Definition: MSPModel.h:117
static const double RANDOM_POS_LAT
magic value to encode randomized lateral offset for persons when starting a walk
Definition: MSPModel.h:131
static const double SIDEWALK_OFFSET
the offset for computing person positions when walking on edges without a sidewalk
Definition: MSPModel.h:125
static const int UNDEFINED_DIRECTION
Definition: MSPModel.h:119
static const double UNSPECIFIED_POS_LAT
the default lateral offset for persons when starting a walk
Definition: MSPModel.h:128
static const double SAFETY_GAP
Definition: MSPModel.h:122
const MSEdge * getDestination() const
returns the destination edge
Definition: MSStage.cpp:61
virtual double getArrivalPos() const
Definition: MSStage.h:89
MSStoppingPlace * getDestinationStop() const
returns the destination stop (if any)
Definition: MSStage.h:80
Position getLanePosition(const MSLane *lane, double at, double offset) const
get position on lane at length at with orthogonal offset
Definition: MSStage.cpp:143
virtual const MSEdge * getNextRouteEdge() const =0
static const MSLane * checkDepartLane(const MSEdge *edge, SUMOVehicleClass svc, int laneIndex, const std::string &id)
interpret custom depart lane
virtual bool moveToNextEdge(MSTransportable *transportable, SUMOTime currentTime, int prevDir, MSEdge *nextInternal=0)=0
move forward and return whether the transportable arrived
const std::vector< const MSEdge * > & getRoute() const
int getDepartLane() const
virtual double getMaxSpeed(const MSTransportable *const transportable=nullptr) const =0
the maximum speed of the transportable
int getNumWaitingPersons() const
get number of persons waiting at this stop
int getWaitingCapacity() const
get number of persons that can wait at this stop
MSPModel * getMovementModel()
Returns the default movement model for this kind of transportables.
void registerJammed()
register a jammed transportable
SUMOVehicleClass getVClass() const
Returns the object's access class.
bool isPerson() const
Whether it is a person.
Position getPosition(const double) const
Return current position (x/y, cartesian)
const MSVehicleType & getVehicleType() const
Returns the object's "vehicle" type.
MSStageType getCurrentStageType() const
the current stage type of the transportable
const MSEdge * getEdge() const
Returns the current edge.
double getMaxSpeed() const
Returns the maximum speed (the minimum of desired and physical maximum speed)
abstract base class for managing callbacks to retrieve various state information from the model
Definition: MSPModel.h:148
MSVehicleType * getVType(const std::string &id=DEFAULT_VTYPE_ID, SumoRNG *rng=nullptr, bool readOnly=false)
Returns the named vehicle type or a sample from the named distribution.
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:77
Position getPosition(const double offset=0) const
Return current position (x/y, cartesian)
Definition: MSVehicle.cpp:1208
double getBackPositionOnLane(const MSLane *lane) const
Get the vehicle's position relative to the given lane.
Definition: MSVehicle.h:401
const MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:577
double getLateralPositionOnLane() const
Get the vehicle's lateral position on the lane.
Definition: MSVehicle.h:416
const Position getBackPosition() const
Definition: MSVehicle.cpp:1456
double getSpeed() const
Returns the vehicle's current speed.
Definition: MSVehicle.h:486
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
Definition: MSVehicle.h:966
double getAngle() const
Returns the vehicle's direction in radians.
Definition: MSVehicle.h:729
The car-following model and parameter.
Definition: MSVehicleType.h:63
double getWidth() const
Get the width which vehicles of this class shall have when being drawn.
double getLength() const
Get vehicle's length [m].
static std::string getIDSecure(const T *obj, const std::string &fallBack="NULL")
get an identifier for Named-like object which may be Null
Definition: Named.h:67
const std::string & getID() const
Returns the id.
Definition: Named.h:74
A storage for options typed value containers)
Definition: OptionsCont.h:89
double getFloat(const std::string &name) const
Returns the double-value of the named option (only for Option_Float)
int getInt(const std::string &name) const
Returns the int-value of the named option (only for Option_Integer)
std::string getString(const std::string &name) const
Returns the string-value of the named option (only for Option_String)
bool getBool(const std::string &name) const
Returns the boolean-value of the named option (only for Option_Bool)
static OptionsCont & getOptions()
Retrieves the options.
Definition: OptionsCont.cpp:59
double compute(const E *from, const E *to, double departPos, double arrivalPos, double speed, SUMOTime msTime, const N *onlyNode, std::vector< const E * > &into, bool allEdges=false)
Builds the route between the given edges using the minimum effort at the given time The definition of...
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:37
void setx(double x)
set position x
Definition: Position.h:70
static const Position INVALID
used to indicate that a position is valid
Definition: Position.h:298
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:252
double x() const
Returns the x-position.
Definition: Position.h:55
double angleTo2D(const Position &other) const
returns the angle in the plane of the vector pointing from here to the other position
Definition: Position.h:262
void sety(double y)
set position y
Definition: Position.h:75
double y() const
Returns the y-position.
Definition: Position.h:60
A list of positions.
double length() const
Returns the length.
double rotationAtOffset(double pos) const
Returns the rotation at the given length.
Position positionAtOffset(double pos, double lateralOffset=0) const
Returns the position at the given length.
void extrapolate(const double val, const bool onlyFirst=false, const bool onlyLast=false)
extrapolate position vector
PositionVector bezier(int numPoints)
return a bezier interpolation
void push_back_noDoublePos(const Position &p)
insert in back a non double position
PositionVector reverse() const
reverse position vector
Position transformToVectorCoordinates(const Position &p, bool extend=false) const
return position p within the length-wise coordinate system defined by this position vector....
static double rand(SumoRNG *rng=nullptr)
Returns a random real number in [0, 1)
Definition: RandHelper.cpp:94
Definition: json.hpp:4471
#define M_PI
Definition: odrSpiral.cpp:45
information regarding surround Pedestrians (and potentially other things)
double speed
speed relative to lane direction (positive means in the same direction)
double xFwd
maximal position on the current lane in forward direction
Obstacle(int dir, double dist=DIST_FAR_AWAY)
create No-Obstacle
std::string description
the id / description of the obstacle
ObstacleType type
whether this obstacle denotes a border or a pedestrian
double xBack
maximal position on the current lane in backward direction