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