SUMO - Simulation of Urban MObility
MSPModel_Striping.cpp
Go to the documentation of this file.
1 /****************************************************************************/
7 // The pedestrian following model (prototype)
8 /****************************************************************************/
9 // SUMO, Simulation of Urban MObility; see http://sumo.dlr.de/
10 // Copyright (C) 2014-2015 DLR (http://www.dlr.de/) and contributors
11 /****************************************************************************/
12 //
13 // This file is part of SUMO.
14 // SUMO is free software: you can redistribute it and/or modify
15 // it under the terms of the GNU General Public License as published by
16 // the Free Software Foundation, either version 3 of the License, or
17 // (at your option) any later version.
18 //
19 /****************************************************************************/
20 
21 // ===========================================================================
22 // included modules
23 // ===========================================================================
24 #ifdef _MSC_VER
25 #include <windows_config.h>
26 #else
27 #include <config.h>
28 #endif
29 
30 #include <math.h>
31 #include <algorithm>
34 #include <microsim/MSNet.h>
35 #include <microsim/MSEdge.h>
36 #include <microsim/MSLane.h>
37 #include <microsim/MSJunction.h>
38 #include "MSPModel_Striping.h"
39 
40 
41 // ===========================================================================
42 // DEBUGGING HELPERS
43 // ===========================================================================
44 //
45 #define DEBUG1 "disabled"
46 #define DEBUG2 "disabled"
47 #define DEBUGCOND(PEDID) (PEDID == DEBUG1 || PEDID == DEBUG2)
48 //#define LOG_ALL 1
49 
51  for (int i = 0; i < (int)obs.size(); ++i) {
52  std::cout
53  << "(" << obs[i].description
54  << " x=" << obs[i].x
55  << " s=" << obs[i].speed
56  << ") ";
57  }
58  std::cout << "\n";
59 }
60 
61 // ===========================================================================
62 // named (internal) constants
63 // ===========================================================================
64 
65 
66 // need to be able to subtract without underflow
67 #define BLOCKED_STRIPE -10000
68 #define FAR_AWAY 10000
69 #define ONCOMING_CONFLICT -1000
70 
71 // ===========================================================================
72 // static members
73 // ===========================================================================
74 
77 
78 
79 // model parameters (static to simplify access from class PState
84 const SUMOReal MSPModel_Striping::LOOKAHEAD_ONCOMING(10.0); // seconds
89 const SUMOReal MSPModel_Striping::MAX_WAIT_TOLERANCE(120.); // seconds
92 
93 
94 // ===========================================================================
95 // MSPModel_Striping method definitions
96 // ===========================================================================
97 
99  myNumActivePedestrians(0) {
100  myCommand = new MovePedestrians(this);
103  // configurable parameters
104  stripeWidth = oc.getFloat("pedestrian.striping.stripe-width");
105  dawdling = oc.getFloat("pedestrian.striping.dawdling");
106 
107  jamTime = string2time(oc.getString("pedestrian.striping.jamtime"));
108  if (jamTime <= 0) {
110  }
111 }
112 
113 
115 }
116 
117 
120  assert(person->getCurrentStageType() == MSPerson::WALKING);
121  const MSLane* lane = getSidewalk(person->getEdge());
122  PState* ped = new PState(person, stage, lane);
123  myActiveLanes[lane].push_back(ped);
125  return ped;
126 }
127 
128 
129 bool
130 MSPModel_Striping::blockedAtDist(const MSLane* lane, SUMOReal distToCrossing, std::vector<const MSPerson*>* collectBlockers) {
131  const Pedestrians& pedestrians = getPedestrians(lane);
132  for (Pedestrians::const_iterator it_ped = pedestrians.begin(); it_ped != pedestrians.end(); ++it_ped) {
133  const PState& ped = **it_ped;
134  const SUMOReal halfVehicleWidth = 1.0; // @note could get the actual value from the vehicle
135  const SUMOReal leaderBackDist = (ped.myDir == FORWARD
136  ? distToCrossing - (ped.myRelX - ped.getLength() - MSPModel::SAFETY_GAP - halfVehicleWidth)
137  : (ped.myRelX + ped.getLength() + MSPModel::SAFETY_GAP + halfVehicleWidth) - distToCrossing);
138  //std::cout << SIMTIME << " foe=" << foeLane->getID() << " dir=" << p.myDir << " pX=" << ped.myRelX << " pL=" << ped.getLength() << " fDTC=" << distToCrossing << " lBD=" << leaderBackDist << "\n";
139  if (leaderBackDist >= 0 && leaderBackDist <= BLOCKER_LOOKAHEAD) {
140  // found one pedestrian that is not completely past the crossing point
141  //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";
142  if (collectBlockers == 0) {
143  return true;
144  } else {
145  collectBlockers->push_back(ped.myPerson);
146  }
147  }
148  }
149  if (collectBlockers == 0) {
150  return false;
151  } else {
152  return collectBlockers->size() > 0;
153  }
154 }
155 
156 
159  ActiveLanes::iterator it = myActiveLanes.find(lane);
160  if (it != myActiveLanes.end()) {
161  //std::cout << " found lane=" << lane->getID() << " n=" << it->second.size() << "\n";
162  return (it->second);
163  } else {
164  return noPedestrians;
165  }
166 }
167 
168 
169 void
171  myActiveLanes.clear();
173  myWalkingAreaPaths.clear(); // need to recompute when lane pointers change
174 }
175 
176 
177 int
179  return (int)floor(lane->getWidth() / stripeWidth);
180 }
181 
182 int
184  if (from == 0 || to == 0) {
185  return UNDEFINED_DIRECTION;
186  } else if (MSLinkContHelper::getConnectingLink(*from, *to)) {
187  return FORWARD;
188  } else if (MSLinkContHelper::getConnectingLink(*to, *from)) {
189  return BACKWARD;
190  } else {
191  return UNDEFINED_DIRECTION;
192  }
193 }
194 
195 
196 void
198  if (myWalkingAreaPaths.size() > 0) {
199  return;
200  }
201  for (size_t i = 0; i < MSEdge::dictSize(); ++i) {
202  const MSEdge* edge = MSEdge::dictionary(i);
203  if (edge->isWalkingArea()) {
204  const MSLane* walkingArea = getSidewalk(edge);
205  // build all possible paths across this walkingArea
206  // gather all incident lanes
207  std::vector<MSLane*> lanes;
208  const MSEdgeVector& incoming = edge->getIncomingEdges();
209  for (int j = 0; j < (int)incoming.size(); ++j) {
210  lanes.push_back(getSidewalk(incoming[j]));
211  }
212  const MSEdgeVector& outgoing = edge->getSuccessors();
213  for (int j = 0; j < (int)outgoing.size(); ++j) {
214  lanes.push_back(getSidewalk(outgoing[j]));
215  }
216  // build all combinations
217  for (int j = 0; j < (int)lanes.size(); ++j) {
218  for (int k = 0; k < (int)lanes.size(); ++k) {
219  if (j != k) {
220  // build the walkingArea
221  const MSLane* from = lanes[j];
222  const MSLane* to = lanes[k];
223  const int fromDir = MSLinkContHelper::getConnectingLink(*from, *walkingArea) != 0 ? FORWARD : BACKWARD;
224  const int toDir = MSLinkContHelper::getConnectingLink(*walkingArea, *to) != 0 ? FORWARD : BACKWARD;
225  PositionVector shape;
226  Position fromPos = from->getShape()[fromDir == FORWARD ? -1 : 0];
227  Position toPos = to->getShape()[toDir == FORWARD ? 0 : -1];
228  const SUMOReal maxExtent = fromPos.distanceTo2D(toPos) / 4; // prevent sharp corners
229  const SUMOReal extrapolateBy = MIN2(maxExtent, walkingArea->getWidth() / 2);
230  // assemble shape
231  shape.push_back(fromPos);
232  if (extrapolateBy > POSITION_EPS) {
233  PositionVector fromShp = from->getShape();
234  fromShp.extrapolate(extrapolateBy);
235  shape.push_back_noDoublePos(fromDir == FORWARD ? fromShp.back() : fromShp.front());
236  PositionVector nextShp = to->getShape();
237  nextShp.extrapolate(extrapolateBy);
238  shape.push_back_noDoublePos(toDir == FORWARD ? nextShp.front() : nextShp.back());
239  }
240  shape.push_back_noDoublePos(toPos);
241  if (shape.size() < 2) {
242  PositionVector fromShp = from->getShape();
243  fromShp.extrapolate(1.5 * POSITION_EPS); // noDoublePos requires a difference of POSITION_EPS in at least one coordinate
244  shape.push_back_noDoublePos(fromDir == FORWARD ? fromShp.back() : fromShp.front());
245  assert(shape.size() == 2);
246  }
247  if (fromDir == BACKWARD) {
248  // will be walking backward on walkingArea
249  shape = shape.reverse();
250  }
251  myWalkingAreaPaths[std::make_pair(from, to)] = WalkingAreaPath(from, walkingArea, to, shape);
252  }
253  }
254  }
255  }
256  }
257 }
258 
259 
261 MSPModel_Striping::getNextLane(const PState& ped, const MSLane* currentLane, const MSLane* prevLane) {
262  const MSEdge* currentEdge = &currentLane->getEdge();
263  const MSJunction* junction = ped.myDir == FORWARD ? currentEdge->getToJunction() : currentEdge->getFromJunction();
264  const MSEdge* nextRouteEdge = ped.myStage->getNextRouteEdge();
265  const MSLane* nextRouteLane = getSidewalk(nextRouteEdge);
266  // result values
267  const MSLane* nextLane = nextRouteLane;
268  MSLink* link = 0;
269  int nextDir = UNDEFINED_DIRECTION;
270 
271  if (nextRouteLane != 0) {
272  if (currentEdge->isInternal()) {
273  assert(junction == currentEdge->getFromJunction());
274  nextDir = junction == nextRouteEdge->getFromJunction() ? FORWARD : BACKWARD;
275  if DEBUGCOND(ped.myPerson->getID()) {
276  std::cout << " internal\n";
277  }
278  } else if (currentEdge->isCrossing()) {
279  nextDir = ped.myDir;
280  if (ped.myDir == FORWARD) {
281  nextLane = currentLane->getLinkCont()[0]->getLane();
282  } else {
283  nextLane = currentLane->getLogicalPredecessorLane();
284  }
285  if DEBUGCOND(ped.myPerson->getID()) {
286  std::cout << " crossing\n";
287  }
288  } else if (currentEdge->isWalkingArea()) {
289  ConstMSEdgeVector crossingRoute;
290  // departPos can be 0 because the direction of the walkingArea does not matter
291  // for the arrivalPos, we need to make sure that the route does not deviate across other junctions
292  const int nextRouteEdgeDir = nextRouteEdge->getFromJunction() == junction ? FORWARD : BACKWARD;
293  const SUMOReal arrivalPos = (nextRouteEdge == ped.myStage->getRoute().back()
294  ? ped.myStage->getArrivalPos()
295  : (nextRouteEdgeDir == FORWARD ? 0 : nextRouteEdge->getLength()));
296  MSEdgeVector prohibited;
297  prohibited.push_back(&prevLane->getEdge());
298  MSNet::getInstance()->getPedestrianRouter(prohibited).compute(currentEdge, nextRouteEdge, 0, arrivalPos, ped.myStage->getMaxSpeed(), 0, junction, crossingRoute, true);
299  if DEBUGCOND(ped.myPerson->getID()) {
300  std::cout
301  << " nre=" << nextRouteEdge->getID()
302  << " nreDir=" << nextRouteEdgeDir
303  << " aPos=" << arrivalPos
304  << " crossingRoute=" << toString(crossingRoute)
305  << "\n";
306  }
307  if (crossingRoute.size() > 1) {
308  const MSEdge* nextEdge = crossingRoute[1];
309  nextLane = getSidewalk(crossingRoute[1]);
310  assert((nextEdge->getFromJunction() == junction || nextEdge->getToJunction() == junction));
311  assert(nextLane != prevLane);
312  nextDir = connectedDirection(currentLane, nextLane);
313  if DEBUGCOND(ped.myPerson->getID()) {
314  std::cout << " nextDir=" << nextDir << "\n";
315  }
316  assert(nextDir != UNDEFINED_DIRECTION);
317  if (nextDir == FORWARD) {
318  link = MSLinkContHelper::getConnectingLink(*currentLane, *nextLane);
319  } else if (nextEdge->isCrossing()) {
320  const MSLane* oppositeWalkingArea = nextLane->getLogicalPredecessorLane();
321  link = MSLinkContHelper::getConnectingLink(*oppositeWalkingArea, *nextLane);
322  } else {
323  link = MSLinkContHelper::getConnectingLink(*nextLane, *currentLane);
324  }
325  assert(link != 0);
326  } else {
327  if DEBUGCOND(ped.myPerson->getID()) {
328  std::cout << SIMTIME
329  << " no route from '" << (currentEdge == 0 ? "NULL" : currentEdge->getID())
330  << "' to '" << (nextRouteEdge == 0 ? "NULL" : nextRouteEdge->getID())
331  << "\n";
332  }
333  // error indicated by nextDir == UNDEFINED_DIRECTION
334  }
335  } else if (currentEdge == nextRouteEdge) {
336  // strange loop in this route. No need to use walkingArea
337  nextDir = -ped.myDir;
338  } else {
339  // normal edge. by default use next / previous walking area
340  nextDir = ped.myDir;
341  nextLane = getNextWalkingArea(currentLane, ped.myDir, link);
342  if (nextLane != 0) {
343  // walking area found
344  if DEBUGCOND(ped.myPerson->getID()) {
345  std::cout << " next walkingArea " << (nextDir == FORWARD ? "forward" : "backward") << "\n";
346  }
347  } else {
348  nextDir = junction == nextRouteEdge->getFromJunction() ? FORWARD : BACKWARD;
349  // try to use a direct link as fallback
350  // direct links only exist if built explicitly. They are used to model tl-controlled links if there are no crossings
351  if (ped.myDir == FORWARD) {
352  link = MSLinkContHelper::getConnectingLink(*currentLane, *nextRouteLane);
353 #ifdef HAVE_INTERNAL_LANES
354  if (link != 0) {
355  if DEBUGCOND(ped.myPerson->getID()) {
356  std::cout << " direct forward\n";
357  }
358  nextLane = MSLinkContHelper::getInternalFollowingLane(currentLane, nextRouteLane);
359  }
360 #endif
361  } else {
362  link = MSLinkContHelper::getConnectingLink(*nextRouteLane, *currentLane);
363 #ifdef HAVE_INTERNAL_LANES
364  if (link != 0) {
365  if DEBUGCOND(ped.myPerson->getID()) {
366  std::cout << " direct backward\n";
367  }
368  nextLane = MSLinkContHelper::getInternalFollowingLane(nextRouteLane, currentLane);
369  }
370 #endif
371  }
372  }
373  if (nextLane == 0) {
374  // no internal lane found
375  nextLane = nextRouteLane;
376  if DEBUGCOND(ped.myPerson->getID()) {
377  std::cout << SIMTIME << " no next lane found for " << currentLane->getID() << " dir=" << ped.myDir << "\n";
378  }
379  } else if (nextLane->getLength() <= POSITION_EPS) {
380  // internal lane too short
381  nextLane = nextRouteLane;
382  }
383  }
384  }
385  if DEBUGCOND(ped.myPerson->getID()) {
386  std::cout << SIMTIME
387  << " p=" << ped.myPerson->getID()
388  << " l=" << currentLane->getID()
389  << " nl=" << (nextLane == 0 ? "NULL" : nextLane->getID())
390  << " nrl=" << (nextRouteLane == 0 ? "NULL" : nextRouteLane->getID())
391  << " d=" << nextDir
392  << " link=" << (link == 0 ? "NULL" : link->getViaLaneOrLane()->getID())
393  << " pedDir=" << ped.myDir
394  << "\n";
395  }
396  return NextLaneInfo(nextLane, link, nextDir);
397 }
398 
399 
400 const MSLane*
401 MSPModel_Striping::getNextWalkingArea(const MSLane* currentLane, const int dir, MSLink*& link) {
402  if (dir == FORWARD) {
403  const MSLinkCont& links = currentLane->getLinkCont();
404  for (MSLinkCont::const_iterator it = links.begin(); it != links.end(); ++it) {
405  if ((*it)->getLane()->getEdge().isWalkingArea()) {
406  link = *it;
407  return (*it)->getLane();
408  }
409  }
410  } else {
411  const std::vector<MSLane::IncomingLaneInfo>& laneInfos = currentLane->getIncomingLanes();
412  for (std::vector<MSLane::IncomingLaneInfo>::const_iterator it = laneInfos.begin(); it != laneInfos.end(); ++it) {
413  if ((*it).lane->getEdge().isWalkingArea()) {
414  link = (*it).viaLink;
415  return (*it).lane;
416  }
417  }
418  }
419  return 0;
420 }
421 
422 
424 MSPModel_Striping::mergeObstacles(const Obstacles& obs1, const Obstacles& obs2, int dir) {
425  Obstacles result(obs1.begin(), obs1.end());
426  for (int i = 0; i < (int)obs1.size(); ++i) {
427  if ((obs2[i].x - obs1[i].x) * dir < 0) {
428  result[i] = obs2[i];
429  }
430  }
431  return result;
432 }
433 
434 
436 MSPModel_Striping::getNeighboringObstacles(const Pedestrians& pedestrians, int egoIndex, int stripes) {
437  const PState& ego = *pedestrians[egoIndex];
438  const SUMOReal egoBack = ego.myRelX - ego.myDir * ego.getLength();
439  int index = egoIndex + 1;
440  Obstacles obs(stripes, Obstacle(ego.myDir));
441  while (index < (int)pedestrians.size() && ego.myDir * (pedestrians[index]->myRelX - egoBack) > 0) {
442  const PState& p = *pedestrians[index];
443  if (!p.myWaitingToEnter) {
444  Obstacle o(p, ego.myDir);
445  obs[p.stripe()] = o;
446  obs[p.otherStripe()] = o;
447  }
448  index++;
449  }
450  if DEBUGCOND(ego.myPerson->getID()) {
451  std::cout << SIMTIME << " ped=" << ego.myPerson->getID() << " neighObs=";
452  DEBUG_PRINT(obs);
453  }
454  return obs;
455 }
456 
457 
460  MSLane* lane, const MSLane* nextLane, int stripes, SUMOReal nextLength, int nextDir,
461  SUMOReal currentLength, int currentDir) {
462  if (nextLanesObs.count(nextLane) == 0) {
463  //std::cout << SIMTIME << " getNextLaneObstacles"
464  // << " nextLane=" << nextLane->getID()
465  // << " nextLength=" << nextLength
466  // << " nextDir=" << nextDir
467  // << " currentLength=" << currentLength
468  // << " currentDir=" << currentDir
469  // << "\n";
470 
471  // figure out the which pedestrians are ahead on the next lane
472  const int nextStripes = numStripes(nextLane);
473  const int offset = (stripes - nextStripes) / 2;
474  Obstacles obs(stripes, Obstacle(nextDir));
475  if (nextStripes < stripes) {
476  // some stripes do not continue
477  for (int ii = 0; ii < stripes; ++ii) {
478  if (ii < offset || ii >= nextStripes + offset) {
479  obs[ii] = Obstacle(nextDir == FORWARD ? 0 : nextLength, 0, "stripeEnd");
480  }
481  }
482  }
483  Pedestrians& pedestrians = getPedestrians(nextLane);
484  if (nextLane->getEdge().isWalkingArea()) {
485  const SUMOReal maxY = stripeWidth * (stripes - 0.5) - NUMERICAL_EPS;
486  // complex transformation into the coordinate system of the current lane
487  // (pedestrians on next lane may walk at arbitrary angles relative to the current lane)
488  const SUMOReal lateral_offset = (lane->getWidth() - stripeWidth) * 0.5;
489  nextDir = currentDir;
490  // transform pedestrians into the current coordinate system
491  for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
492  PState& p = *pedestrians[ii];
493  Position relPos = lane->getShape().transformToVectorCoordinates(p.getPosition(*p.myStage, -1), true);
494  const SUMOReal newY = relPos.y() + lateral_offset;
495  if (newY >= 0 && newY <= maxY) {
496  addCloserObstacle(obs, relPos.x(), p.stripe(newY), p.myPerson->getID(), stripes, currentDir);
497  addCloserObstacle(obs, relPos.x(), p.otherStripe(newY), p.myPerson->getID(), stripes, currentDir);
498  }
499  }
500  } else {
501  // simple transformation into the coordinate system of the current lane
502  // (only need to worry about currentDir and nextDir)
503  // XXX consider waitingToEnter on nextLane
504  sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(nextDir));
505  for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
506  const PState& p = *pedestrians[ii];
507  if (p.myWaitingToEnter) {
508  continue;
509  }
510  addMappedObstacle(obs, p, p.stripe(), currentDir, nextDir, offset, stripes, nextStripes);
511  addMappedObstacle(obs, p, p.otherStripe(), currentDir, nextDir, offset, stripes, nextStripes);
512  }
513  for (int ii = 0; ii < stripes; ++ii) {
514  Obstacle& o = obs[ii];
515  if (nextDir == BACKWARD) {
516  o.x = nextLength - o.x;
517  }
518  if (currentDir == FORWARD) {
519  o.x += currentLength;
520  } else {
521  o.x *= -1;
522  }
523  }
524  }
525  nextLanesObs[nextLane] = obs;
526  }
527  return nextLanesObs[nextLane];
528 }
529 
530 
531 void
532 MSPModel_Striping::addMappedObstacle(Obstacles& obs, const PState& p, int stripe, int currentDir, int nextDir, int offset, int stripes, int nextStripes) {
533  const int mappedStripe = (nextDir == currentDir
534  ? stripe + offset
535  : nextStripes - 1 - stripe + offset);
536  if (mappedStripe >= 0 && mappedStripe < stripes) {
537  obs[mappedStripe] = Obstacle(p, nextDir);
538  }
539 }
540 
541 
542 void
543 MSPModel_Striping::addCloserObstacle(Obstacles& obs, SUMOReal x, int stripe, const std::string& id, int stripes, int dir) {
544  if (stripe < stripes && obs[stripe].x * dir > x * dir) {
545  obs[stripe] = Obstacle(x, 0, id);
546  }
547 }
548 
549 void
550 MSPModel_Striping::moveInDirection(SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir) {
551  for (ActiveLanes::iterator it_lane = myActiveLanes.begin(); it_lane != myActiveLanes.end(); ++it_lane) {
552  const MSLane* lane = it_lane->first;
553  Pedestrians& pedestrians = it_lane->second;
554  //std::cout << SIMTIME << ">>> lane=" << lane->getID() << " numPeds=" << pedestrians.size() << "\n";
555  if (lane->getEdge().isWalkingArea()) {
556  const SUMOReal lateral_offset = (lane->getWidth() - stripeWidth) * 0.5;
557  const WalkingAreaPath* debugPath = 0;
558  // need to handle each walkingAreaPath seperately and transform
559  // coordinates beforehand
560  std::set<const WalkingAreaPath*, walkingarea_path_sorter> paths;
561  for (Pedestrians::iterator it = pedestrians.begin(); it != pedestrians.end(); ++it) {
562  const PState* p = *it;
563  assert(p->myWalkingAreaPath != 0);
564  if (p->myDir == dir) {
565  paths.insert(p->myWalkingAreaPath);
566  if DEBUGCOND(p->myPerson->getID()) {
567  debugPath = p->myWalkingAreaPath;
568  std::cout << SIMTIME << " debugging WalkingAreaPath from=" << debugPath->from->getID() << " to=" << debugPath->to->getID() << "\n";
569  }
570  }
571  }
572  for (std::set<const WalkingAreaPath*, walkingarea_path_sorter>::iterator it = paths.begin(); it != paths.end(); ++it) {
573  const WalkingAreaPath* path = *it;
574  Pedestrians toDelete;
575  Pedestrians transformedPeds;
576  transformedPeds.reserve(pedestrians.size());
577  for (Pedestrians::iterator it_p = pedestrians.begin(); it_p != pedestrians.end(); ++it_p) {
578  PState* p = *it_p;
579  if (p->myWalkingAreaPath == path
580  // opposite direction is already in the correct coordinate system
581  || (p->myWalkingAreaPath->from == path->to && p->myWalkingAreaPath->to == path->from)) {
582  transformedPeds.push_back(p);
583  if (path == debugPath) std::cout << " ped=" << p->myPerson->getID() << " relX=" << p->myRelX << " relY=" << p->myRelY << " (untransformed), vecCoord="
584  << path->shape.transformToVectorCoordinates(p->getPosition(*p->myStage, -1)) << "\n";
585  } else {
586  const Position relPos = path->shape.transformToVectorCoordinates(p->getPosition(*p->myStage, -1));
587  const SUMOReal newY = relPos.y() + lateral_offset;
588  if (relPos != Position::INVALID &&
589  newY >= 0 && newY <= lane->getWidth()) {
590  PState* tp = new PState(*p);
591  tp->myRelX = relPos.x();
592  tp->myRelY = newY;
593  // only an obstacle, speed may be orthogonal to dir
594  tp->myDir = !dir;
595  tp->mySpeed = 0;
596  toDelete.push_back(tp);
597  transformedPeds.push_back(tp);
598  if (path == debugPath) {
599  std::cout << " ped=" << p->myPerson->getID() << " relX=" << relPos.x() << " relY=" << newY << " (transformed), vecCoord=" << relPos << "\n";
600  }
601  } else {
602  if (path == debugPath) {
603  std::cout << " ped=" << p->myPerson->getID() << " relX=" << relPos.x() << " relY=" << newY << " (invalid), vecCoord=" << relPos << "\n";
604  }
605  }
606  }
607  }
608  moveInDirectionOnLane(transformedPeds, lane, currentTime, changedLane, dir);
609  // clean up
610  for (Pedestrians::iterator it_p = toDelete.begin(); it_p != toDelete.end(); ++it_p) {
611  delete *it_p;
612  }
613  };
614 
615  } else {
616  moveInDirectionOnLane(pedestrians, lane, currentTime, changedLane, dir);
617  }
618  // advance to the next lane / arrive at destination
619  sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(dir));
620  for (Pedestrians::iterator it = pedestrians.begin(); it != pedestrians.end();) {
621  PState* p = *it;
622  if (p->myDir != dir) {
623  ++it;
624  } else if (p->moveToNextLane(currentTime)) {
625  it = pedestrians.erase(it);
626  if (p->myLane != 0) {
627  changedLane.insert(p->myPerson);
628  myActiveLanes[p->myLane].push_back(p);
629  } else {
630  delete p;
632  }
633  } else {
634  ++it;
635  }
636  }
637  }
638 }
639 
640 
641 void
642 MSPModel_Striping::moveInDirectionOnLane(Pedestrians& pedestrians, const MSLane* lane, SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir) {
643  const int stripes = numStripes(lane);
644  Obstacles obs(stripes, Obstacle(dir)); // continously updated
645  NextLanesObstacles nextLanesObs; // continously updated
646  sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(dir));
647  for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
648  PState& p = *pedestrians[ii];
649  //std::cout << SIMTIME << "CHECKING" << p.myPerson->getID() << "\n";
650  Obstacles currentObs = obs;
651  if (p.myDir != dir || changedLane.count(p.myPerson) != 0) {
652  if (!p.myWaitingToEnter) {
653  Obstacle o(p, dir);
654  obs[p.stripe()] = o;
655  obs[p.otherStripe()] = o;
656  }
657  continue;
658  }
659  if DEBUGCOND(p.myPerson->getID()) {
660  std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " currentObs=";
661  DEBUG_PRINT(currentObs);
662  }
663  const MSLane* nextLane = p.myNLI.lane;
664  const MSLink* link = p.myNLI.link;
665  const SUMOReal dist = p.distToLaneEnd();
666  const SUMOReal speed = p.myStage->getMaxSpeed();
667  if (nextLane != 0 && dist <= LOOKAHEAD_ONCOMING) {
668  const SUMOReal currentLength = (p.myWalkingAreaPath == 0 ? lane->getLength() : p.myWalkingAreaPath->length);
669  const SUMOReal nextLength = nextLane->getLength(); // XXX what to do if nextLane is a walkingArea?
670  const Obstacles& nextObs = getNextLaneObstacles(
671  nextLanesObs, lane, nextLane, stripes,
672  nextLength, p.myNLI.dir,
673  currentLength, dir);
674 
675  if DEBUGCOND(p.myPerson->getID()) {
676  std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " nextObs=";
677  DEBUG_PRINT(nextObs);
678  }
679  currentObs = mergeObstacles(currentObs, nextObs, dir);
680  }
681  if DEBUGCOND(p.myPerson->getID()) {
682  std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWithNext=";
683  DEBUG_PRINT(currentObs);
684  }
685  currentObs = mergeObstacles(currentObs, getNeighboringObstacles(pedestrians, ii, stripes), dir);
686  if DEBUGCOND(p.myPerson->getID()) {
687  std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWithNeigh=";
688  DEBUG_PRINT(currentObs);
689  }
690  // check link state
691  if (link != 0
692  && dist < speed // only check close before junction
693  && (!link->opened(currentTime, speed, speed, p.getLength(), p.getImpatience(currentTime), speed, 0)
694  // XXX check for presence of vehicles blocking the path
695  )) {
696  // prevent movement passed a closed link
697  Obstacles closedLink(stripes, Obstacle(p.myRelX + dir * dist - POSITION_EPS, 0, "closedLink"));
698  currentObs = mergeObstacles(currentObs, closedLink, dir);
699  if DEBUGCOND(p.myPerson->getID()) {
700  std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWitTLS=";
701  DEBUG_PRINT(currentObs);
702  }
703  // consider rerouting over another crossing
704  if (p.myWalkingAreaPath != 0) {
705  // XXX actually another path would be needed starting at the current position
707  }
708  }
709  p.walk(currentObs, currentTime);
710  if (!p.myWaitingToEnter) {
711  Obstacle o(p, dir);
712  obs[p.stripe()] = o;
713  obs[p.otherStripe()] = o;
714  }
715  //std::cout << SIMTIME << p.myPerson->getID() << " lane=" << lane->getID() << " x=" << p.myRelX << "\n";
716  }
717 }
718 
719 
720 // ===========================================================================
721 // MSPModel_Striping::Obstacle method definitions
722 // ===========================================================================
724  x(dir* FAR_AWAY), // far away when seen in dir
725  speed(0),
726  description("")
727 {}
728 
729 
731  description(ped.myPerson->getID()) {
732  assert(!ped.myWaitingToEnter);
733  if (dir == ped.myDir) {
734  speed = ped.mySpeed;
735  x = ped.myRelX - dir * ped.getLength();
736  } else {
737  speed = -ped.mySpeed;
738  x = ped.myRelX;
739  }
740 }
741 
742 
743 // ===========================================================================
744 // MSPModel_Striping::PState method definitions
745 // ===========================================================================
746 
747 
749  myPerson(person),
750  myStage(stage),
751  myLane(lane),
752  myRelX(stage->getDepartPos()),
753  myRelY(0),
754  myDir(FORWARD),
755  mySpeed(0),
756  myWaitingToEnter(true),
757  myWaitingTime(0),
758  myWalkingAreaPath(0),
759  myAmJammed(false) {
760  const MSEdge* currentEdge = &lane->getEdge();
761  assert(!currentEdge->isWalkingArea());
762  const ConstMSEdgeVector& route = myStage->getRoute();
763  if (route.size() == 1) {
764  // only a single edge, move towards end pos
766  } else {
767  const bool mayStartForward = canTraverse(FORWARD, route);
768  const bool mayStartBackward = canTraverse(BACKWARD, route);
769  if DEBUGCOND(myPerson->getID()) {
770  std::cout << " initialize dir for " << myPerson->getID() << " forward=" << mayStartForward << " backward=" << mayStartBackward << "\n";
771  }
772  if (mayStartForward && mayStartBackward) {
773  // figure out the best direction via routing
774  ConstMSEdgeVector crossingRoute;
775  MSNet::getInstance()->getPedestrianRouter().compute(currentEdge, route.back(), myRelX, myStage->getArrivalPos(), myStage->getMaxSpeed(), 0, 0, crossingRoute, true);
776  if (crossingRoute.size() > 1) {
777  // route found
778  const MSEdge* nextEdge = crossingRoute[1];
779  if (nextEdge->getFromJunction() == currentEdge->getFromJunction() || nextEdge->getToJunction() == currentEdge->getFromJunction()) {
780  myDir = BACKWARD;
781  }
782  }
783  if DEBUGCOND(myPerson->getID()) {
784  std::cout << " crossingRoute=" << toString(crossingRoute) << "\n";
785  }
786  } else {
787  myDir = !mayStartBackward ? FORWARD : BACKWARD;
788  }
789  }
790  if (myDir == FORWARD) {
791  // start at the right side of the sidewalk
792  myRelY = stripeWidth * (numStripes(lane) - 1);
793  }
794  if DEBUGCOND(myPerson->getID()) {
795  std::cout << " added new pedestrian " << myPerson->getID() << " on " << lane->getID() << " myRelX=" << myRelX << " myRelY=" << myRelY << " dir=" << myDir << " route=" << toString(myStage->getRoute()) << "\n";
796  }
797 
798  myNLI = getNextLane(*this, lane, 0);
799 }
800 
801 
802 SUMOReal
804  return myPerson->getVehicleType().getLength();
805 }
806 
807 
808 int
810  const int max = numStripes(myLane) - 1;
811  return MIN2(MAX2(0, (int)floor((relY + 0.5 * stripeWidth) / stripeWidth)), max);
812 }
813 
814 
815 int
817  const int max = numStripes(myLane) - 1;
818  const int s = stripe();
819  const SUMOReal offset = relY - s * stripeWidth;
820  const SUMOReal threshold = MAX2(NUMERICAL_EPS, stripeWidth - SQUEEZE * myPerson->getVehicleType().getWidth());
821  int result;
822  if (offset > threshold) {
823  result = s + 1;
824  } else if (offset < -threshold) {
825  result = s - 1;
826  } else {
827  result = s;
828  }
829  std::cout.setf(std::ios::fixed , std::ios::floatfield);
830  std::cout << std::setprecision(5);
831  //if DEBUGCOND(myPerson->getID()) std::cout << " otherStripe " << myPerson->getID() << " offset=" << offset << " threshold=" << threshold << " rawResult=" << result << "\n";
832  return MIN2(MAX2(0, result), max);
833 }
834 
835 int
837  return stripe(myRelY);
838 }
839 
840 
841 int
843  return otherStripe(myRelY);
844 }
845 
846 
847 SUMOReal
849  if (myStage->getNextRouteEdge() == 0) {
850  return myDir * (myStage->getArrivalPos() - myRelX);
851  } else {
852  const SUMOReal length = myWalkingAreaPath == 0 ? myLane->getLength() : myWalkingAreaPath->length;
853  return myDir == FORWARD ? length - myRelX : myRelX;
854  }
855 }
856 
857 
858 bool
860  const SUMOReal dist = distToLaneEnd();
861  if (myPerson->getID() == DEBUG1) {
862  std::cout << SIMTIME << " myRelX=" << myRelX << " dist=" << dist << "\n";
863  }
864  if (dist <= 0) {
865  //if (ped.myPerson->getID() == DEBUG1) {
866  // std::cout << SIMTIME << " addToLane x=" << ped.myRelX << " newDir=" << newDir << " newLane=" << newLane->getID() << " walkingAreaShape=" << walkingAreaShape << "\n";
867  //}
868  //std::cout << " changing to " << newLane->getID() << " myRelY=" << ped.myRelY << " oldStripes=" << oldStripes << " newStripes=" << numStripes(newLane);
869  //std::cout << " newY=" << ped.myRelY << " myDir=" << ped.myDir << " newDir=" << newDir;
870  const int oldStripes = numStripes(myLane);
871  const int oldDir = myDir;
872  const MSLane* oldLane = myLane;
873  myLane = myNLI.lane;
874  myDir = myNLI.dir;
875  const bool normalLane = (myLane == 0 || myLane->getEdge().getPurpose() == MSEdge::EDGEFUNCTION_NORMAL);
876  if DEBUGCOND(myPerson->getID()) {
877  std::cout << SIMTIME
878  << " ped=" << myPerson->getID()
879  << " moveToNextLane old=" << oldLane->getID()
880  << " new=" << (myLane == 0 ? "NULL" : myLane->getID())
881  << " oldDir=" << oldDir
882  << " newDir=" << myDir
883  << "\n";
884  }
885  myStage->moveToNextEdge(myPerson, currentTime, normalLane ? 0 : &myLane->getEdge());
886  if (myLane != 0) {
887  assert(myDir != UNDEFINED_DIRECTION);
888  myNLI = getNextLane(*this, myLane, oldLane);
889  assert(myNLI.lane != oldLane); // do not turn around
890  if DEBUGCOND(myPerson->getID()) {
891  std::cout << " nextLane=" << (myNLI.lane == 0 ? "NULL" : myNLI.lane->getID()) << "\n";
892  }
893  if (myLane->getEdge().isWalkingArea()) {
894  if (myNLI.dir != UNDEFINED_DIRECTION) {
895  myWalkingAreaPath = &myWalkingAreaPaths[std::make_pair(oldLane, myNLI.lane)];
896  assert(myWalkingAreaPath->from != 0);
897  assert(myWalkingAreaPath->to != 0);
898  assert(myWalkingAreaPath->shape.size() >= 2);
899  if DEBUGCOND(myPerson->getID()) {
900  std::cout << " mWAPath shape=" << myWalkingAreaPath->shape << " length=" << myWalkingAreaPath->length << "\n";
901  }
902  } else {
903  // disconnnected route. move to the next edge (arbitrariliy, maintaining current direction)
904  if (OptionsCont::getOptions().getBool("ignore-route-errors")) {
905  myStage->moveToNextEdge(myPerson, currentTime, 0);
906  myLane = myNLI.lane;
907  assert(myLane != 0);
908  assert(myLane->getEdge().getPurpose() == MSEdge::EDGEFUNCTION_NORMAL);
909  myNLI = getNextLane(*this, myLane, oldLane);
910  myWalkingAreaPath = 0;
911  } else {
912  throw ProcessError("Disconnected walk for person '" + myPerson->getID() + "'.");
913  }
914  }
915  } else {
916  myWalkingAreaPath = 0;
917  }
918  // adapt x to fit onto the new lane
919  if (myDir == BACKWARD) {
920  const SUMOReal newLength = (myWalkingAreaPath == 0 ? myLane->getLength() : myWalkingAreaPath->length);
921  myRelX = newLength + dist;
922  } else {
923  myRelX = -dist;
924  }
925  // adjust to change in direction
926  if (myDir != oldDir) {
927  myRelY = (numStripes(oldLane) - 1) * stripeWidth - myRelY;
928  }
929  // adjust to differences in sidewalk width
930  myRelY += 0.5 * stripeWidth * (numStripes(myLane) - oldStripes);
931  }
932  return true;
933  } else {
934  return false;
935  }
936 }
937 
938 
939 void
941  const int stripes = (int)obs.size();
942  const int sMax = stripes - 1;
943  assert(stripes == numStripes(myLane));
944  const SUMOReal vMax = myStage->getMaxSpeed();
945  // ultimate goal is to chose the prefered stripe (chosen)
946  const int current = stripe();
947  const int other = otherStripe();
948  int chosen = current;
949  // compute utility for all stripes
950  std::vector<SUMOReal> utility(stripes, 0);
951 
952  // penalize lateral movement (may increase jamming)
953  for (int i = 0; i < stripes; ++i) {
954  utility[i] += abs(i - current) * LATERAL_PENALTY;
955  }
956  // compute distances
957  std::vector<SUMOReal> distance(stripes);
958  for (int i = 0; i < stripes; ++i) {
959  distance[i] += myDir * (obs[i].x - myRelX);
960  }
961  // forbid stripes which are blocked and also all stripes behind them
962  for (int i = 0; i < stripes; ++i) {
963  if (distance[i] <= 0) {
964  if (i < current) {
965  for (int j = 0; j <= i; ++j) {
966  utility[j] = 2 * BLOCKED_STRIPE;
967  }
968  } else {
969  for (int j = i; j < stripes; ++j) {
970  utility[j] = 2 * BLOCKED_STRIPE;
971  }
972  }
973  }
974  }
975  // forbid a portion of the leftmost stripes (in walking direction).
976  // lanes with stripes less than 1 / RESERVE_FOR_ONCOMING_FACTOR
977  // may still deadlock in heavy pedestrian traffic
978  const int reserved = (int)floor(stripes * RESERVE_FOR_ONCOMING_FACTOR);
979  if (myDir == FORWARD) {
980  for (int i = 0; i < reserved; ++i) {
981  utility[i] = BLOCKED_STRIPE;
982  }
983  } else {
984  for (int i = sMax; i > sMax - reserved; --i) {
985  utility[i] = BLOCKED_STRIPE;
986  }
987  }
988  // adapt utility based on obstacles
989  for (int i = 0; i < stripes; ++i) {
990  if (obs[i].speed < 0) {
991  // penalize evasion to the left
992  if (myDir == FORWARD && i > 0) {
993  utility[i - 1] -= 0.5;
994  } else if (myDir == BACKWARD && i < sMax) {
995  utility[i + 1] -= 0.5;
996  }
997  }
998  // compute expected distance achievable by staying on this stripe for a time horizon
999  const SUMOReal lookAhead = obs[i].speed >= 0 ? LOOKAHEAD_SAMEDIR : LOOKAHEAD_ONCOMING;
1000  const SUMOReal expectedDist = MIN2(vMax * LOOKAHEAD_SAMEDIR, distance[i] + obs[i].speed * lookAhead);
1001  if (DEBUGCOND(myPerson->getID())) {
1002  std::cout << " util=" << utility[i] << " exp=" << expectedDist << "\n";
1003  }
1004  if (expectedDist >= 0) {
1005  utility[i] += expectedDist;
1006  } else {
1007  // let only the distance count
1008  utility[i] += ONCOMING_CONFLICT + distance[i];
1009  }
1010  }
1011  // bonus to remain on the rightmost lane (in walking direction) if there
1012  // are oncoming
1013  if (((myDir == FORWARD && current == sMax)
1014  || (myDir == BACKWARD && current == 0))
1015  && obs[current].speed < 0) {
1016  utility[current] -= ONCOMING_CONFLICT;
1017  }
1018  // bonus to leave the leftmost lane (in walking direction) if there
1019  // are oncoming
1020  if (((myDir == BACKWARD && current == sMax)
1021  || (myDir == FORWARD && current == 0))
1022  && obs[current].speed < 0) {
1023  utility[current] += ONCOMING_CONFLICT;
1024  }
1025 
1026  // select best stripe
1027  for (int i = 0; i < stripes; ++i) {
1028  if (utility[chosen] < utility[i]) {
1029  chosen = i;
1030  }
1031  }
1032  // compute speed components along both axes
1033  const int next = (chosen == current ? current : (chosen < current ? current - 1 : current + 1));
1034  const SUMOReal xDist = MIN3(distance[current], distance[other], distance[next]);
1035  // XXX preferred gap differs between approaching a standing obstacle or a moving obstacle
1036  const SUMOReal preferredGap = myPerson->getVehicleType().getMinGap() + xDist * 0.5;
1037  SUMOReal xSpeed = MIN2(vMax, MAX2((SUMOReal)0, xDist - preferredGap));
1038  if (DEBUGCOND(myPerson->getID())) {
1039  std::cout << " xSpeedPotential=" << xSpeed << "\n";
1040  }
1041  // avoid tiny steps
1042  // XXX pressure from behind?
1043  if (mySpeed == 0 && xSpeed < MIN_STARTUP_SPEED * vMax) {
1044  xSpeed = 0;
1045  }
1046  if (xSpeed == 0) {
1047  if (myWaitingTime > jamTime || myAmJammed) {
1048  // squeeze slowly through the crowd ignoring others
1049  myAmJammed = true;
1050  xSpeed = vMax / 4;
1051  } else {
1052  myAmJammed = false;
1053  }
1054  }
1055  // dawdling
1056  const SUMOReal dawdle = MIN2(xSpeed, RandHelper::rand() * vMax * dawdling);
1057  xSpeed -= dawdle;
1058 
1059  // XXX ensure that diagonal speed <= vMax
1060  // avoid deadlocks on narrow sidewalks
1061  //if (oncoming && xSpeed == 0 && myStage->getWaitingTime(currentTime) > TIME2STEPS(ONCOMIN_PATIENCE)) {
1062  // if DEBUGCOND(myPerson->getID()) std::cout << " stepping asside to resolve oncoming deadlock\n";
1063  // xSpeed = POSITION_EPS; // reset myWaitingTime
1064  // if (myDir == FORWARD && chosen < sMax) {
1065  // chosen += 1;
1066  // } else if (myDir == BACKWARD && chosen > 0) {
1067  // chosen -= 1;
1068  // }
1069  //}
1070  const SUMOReal maxYSpeed = MAX2(vMax * LATERAL_SPEED_FACTOR, vMax - xSpeed);
1071  SUMOReal ySpeed = 0;
1072  const SUMOReal yDist = (chosen * stripeWidth) - myRelY;
1073  if (fabs(yDist) > NUMERICAL_EPS) {
1074  ySpeed = (yDist > 0 ?
1075  MIN2(maxYSpeed, yDist) :
1076  MAX2(-maxYSpeed, yDist));
1077  }
1078  // DEBUG
1079  if DEBUGCOND(myPerson->getID()) {
1080  std::cout << SIMTIME
1081  << " ped=" << myPerson->getID()
1082  << " edge=" << myStage->getEdge()->getID()
1083  << " x=" << myRelX
1084  << " y=" << myRelY
1085  << " d=" << myDir
1086  << " pvx=" << mySpeed
1087  << " cur=" << current
1088  << " cho=" << chosen
1089  << " oth=" << other
1090  << " nxt=" << next
1091  << " vx=" << xSpeed
1092  << " dawdle=" << dawdle
1093  << " vy=" << ySpeed
1094  << " xd=" << xDist
1095  << " yd=" << yDist
1096  << " vMax=" << myStage->getMaxSpeed()
1097  << " wTime=" << myStage->getWaitingTime(currentTime)
1098  << " jammed=" << myAmJammed
1099  << "\n distance=" << toString(distance)
1100  << " utility=" << toString(utility)
1101  << "\n";
1102  DEBUG_PRINT(obs);
1103  }
1104  myRelX += SPEED2DIST(xSpeed * myDir);
1105  myRelY += SPEED2DIST(ySpeed);
1106  mySpeed = xSpeed;
1107  if (xSpeed > 0) {
1108  myWaitingToEnter = false;
1109  myWaitingTime = 0;
1110  } else {
1111  myWaitingTime += DELTA_T;
1112  }
1113 }
1114 
1115 
1116 SUMOReal
1118  return MAX2((SUMOReal)0, MIN2(SUMOReal(1),
1119  myPerson->getVehicleType().getImpatience()
1120  + STEPS2TIME(myStage->getWaitingTime(now)) / MAX_WAIT_TOLERANCE));
1121 }
1122 
1123 
1124 SUMOReal
1126  return myRelX;
1127 }
1128 
1129 
1130 Position
1132  if (myLane == 0) {
1133  // pedestrian has already finished
1134  return Position::INVALID;
1135  }
1136  const SUMOReal lateral_offset = myRelY + (stripeWidth - myLane->getWidth()) * 0.5;
1137  if (myWalkingAreaPath == 0) {
1138  return stage.getLanePosition(myLane, myRelX, lateral_offset);
1139  } else {
1140  return myWalkingAreaPath->shape.positionAtOffset(myRelX, lateral_offset);
1141  }
1142 }
1143 
1144 
1145 SUMOReal
1147  if (myLane == 0) {
1148  // pedestrian has already finished
1149  return 0;
1150  }
1151  const PositionVector& shp = myWalkingAreaPath == 0 ? myLane->getShape() : myWalkingAreaPath->shape;
1152  SUMOReal angle = -shp.rotationDegreeAtOffset(myRelX) + (myDir == MSPModel::BACKWARD ? 180 : 0);
1153  if (angle > 180) {
1154  angle -= 360;
1155  }
1156  return angle;
1157 }
1158 
1159 
1160 SUMOTime
1162  return myWaitingTime;
1163 }
1164 
1165 
1166 SUMOReal
1168  return mySpeed;
1169 }
1170 
1171 // ===========================================================================
1172 // MSPModel_Striping::MovePedestrians method definitions
1173 // ===========================================================================
1174 //
1175 
1176 SUMOTime
1178  std::set<MSPerson*> changedLane;
1179  myModel->moveInDirection(currentTime, changedLane, FORWARD);
1180  myModel->moveInDirection(currentTime, changedLane, BACKWARD);
1181  // DEBUG
1182 #ifdef LOG_ALL
1183  for (ActiveLanes::const_iterator it_lane = myModel->getActiveLanes().begin(); it_lane != myModel->getActiveLanes().end(); ++it_lane) {
1184  const MSLane* lane = it_lane->first;
1185  Pedestrians pedestrians = it_lane->second;
1186  if (pedestrians.size() == 0) {
1187  continue;
1188  }
1189  sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(FORWARD));
1190  std::cout << SIMTIME << " lane=" << lane->getID();
1191  for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
1192  const PState& p = *pedestrians[ii];
1193  std::cout << " (" << p.myPerson->getID() << " " << p.myRelX << "," << p.myRelY << " " << p.myDir << ")";
1194  }
1195  std::cout << "\n";
1196  }
1197 #endif
1198  return DELTA_T;
1199 }
1200 
const Obstacles & getNextLaneObstacles(NextLanesObstacles &nextLanesObs, const MSLane *lane, const MSLane *nextLane, int stripes, SUMOReal nextLength, int nextDir, SUMOReal currentLength, int currentDir)
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...
const std::string & getID() const
returns the person id
Definition: MSPerson.cpp:552
bool blockedAtDist(const MSLane *lane, SUMOReal distToCrossing, std::vector< const MSPerson * > *collectBlockers)
whether a pedestrian is blocking the crossing of lane at offset distToCrossing
MSEdge & getEdge() const
Returns the lane's edge.
Definition: MSLane.h:455
static const SUMOReal SAFETY_GAP
Definition: MSPModel.h:81
const MSEdge * getNextRouteEdge() const
Definition: MSPerson.h:268
#define SPEED2DIST(x)
Definition: SUMOTime.h:55
SUMOReal distToLaneEnd() const
the absolute distance to the end of the lane in walking direction (or to the arrivalPos) ...
MSPerson::MSPersonStage_Walking * myStage
Position getLanePosition(const MSLane *lane, SUMOReal at, SUMOReal offset) const
get position on lane at length at with orthogonal offset
Definition: MSPerson.cpp:102
static SUMOReal stripeWidth
model parameters
static int numStripes(const MSLane *lane)
return the maximum number of pedestrians walking side by side
static Obstacles getNeighboringObstacles(const Pedestrians &pedestrians, int egoIndex, int stripes)
SUMOReal mySpeed
the current walking speed
static const MSLane * getNextWalkingArea(const MSLane *currentLane, const int dir, MSLink *&link)
return the next walkingArea in the given direction
#define DEBUG1
static int connectedDirection(const MSLane *from, const MSLane *to)
returns the direction in which these lanes are connectioned or 0 if they are not
WalkingAreaPath * myWalkingAreaPath
the current walkingAreaPath or 0
const MSEdge * getEdge() const
Returns the current edge.
Definition: MSPerson.h:542
information regarding surround Pedestrians (and potentially other things)
SUMOReal getLength() const
Returns the lane's length.
Definition: MSLane.h:370
The base class for an intersection.
Definition: MSJunction.h:61
StageType getCurrentStageType() const
Definition: MSPerson.h:567
#define BLOCKED_STRIPE
bool getBool(const std::string &name) const
Returns the boolean-value of the named option (only for Option_Bool)
bool isWalkingArea() const
return whether this edge is walking area
Definition: MSEdge.h:250
PState(MSPerson *person, MSPerson::MSPersonStage_Walking *stage, const MSLane *lane)
static const int FORWARD
Definition: MSPModel.h:73
#define ONCOMING_CONFLICT
static SUMOReal rand()
Returns a random real number in [0, 1)
Definition: RandHelper.h:62
MSPedestrianRouterDijkstra & getPedestrianRouter(const MSEdgeVector &prohibited=MSEdgeVector()) const
Definition: MSNet.cpp:828
SUMOReal getWidth() const
Returns the lane's width.
Definition: MSLane.h:386
#define DEBUGCOND(PEDID)
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:159
T MAX2(T a, T b)
Definition: StdDefs.h:74
static MSLane * getSidewalk(const MSEdge *edge)
return the appropriate lane to walk on
Definition: MSPModel.cpp:93
static bool dictionary(const std::string &id, MSEdge *edge)
Inserts edge into the static dictionary Returns true if the key id isn't already in the dictionary...
Definition: MSEdge.cpp:531
SUMOReal getFloat(const std::string &name) const
Returns the SUMOReal-value of the named option (only for Option_Float)
std::vector< const MSEdge * > ConstMSEdgeVector
Definition: MSEdge.h:79
std::map< std::pair< const MSLane *, const MSLane * >, WalkingAreaPath > WalkingAreaPaths
bool moveToNextLane(SUMOTime currentTime)
return whether this pedestrian has passed the end of the current lane and update myRelX if so ...
#define FAR_AWAY
const MSEdgeVector & getIncomingEdges() const
Returns the list of edges from which this edge may be reached.
Definition: MSEdge.h:300
static MSPModel * myModel
Definition: MSPModel.h:90
SUMOTime getCurrentTimeStep() const
Returns the current simulation step.
Definition: MSNet.h:235
static WalkingAreaPaths myWalkingAreaPaths
store for walkinArea elements
SUMOReal x() const
Returns the x-position.
Definition: Position.h:63
#define abs(a)
Definition: polyfonts.c:67
The simulated network and simulation perfomer.
Definition: MSNet.h:94
static OptionsCont & getOptions()
Retrieves the options.
Definition: OptionsCont.cpp:67
#define SIMTIME
Definition: SUMOTime.h:96
static Pedestrians noPedestrians
empty pedestrian vector
static const SUMOReal RESERVE_FOR_ONCOMING_FACTOR
PositionVector reverse() const
void moveInDirection(SUMOTime currentTime, std::set< MSPerson * > &changedLane, int dir)
move all pedestrians forward and advance to the next lane if applicable
NextLaneInfo myNLI
information about the upcoming lane
static const SUMOReal SQUEEZE
std::string getString(const std::string &name) const
Returns the string-value of the named option (only for Option_String)
const std::string & getID() const
Returns the id.
Definition: Named.h:60
A road/street connecting two junctions.
Definition: MSEdge.h:81
Pedestrians & getPedestrians(const MSLane *lane)
retrieves the pedestian vector for the given lane (may be empty)
static const SUMOReal LOOKAHEAD_SAMEDIR
void compute(const E *from, const E *to, SUMOReal departPos, SUMOReal arrivalPos, SUMOReal 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...
static const int UNDEFINED_DIRECTION
Definition: MSPModel.h:78
SUMOReal getLength() const
return the length of the edge
Definition: MSEdge.h:535
MSLane * getLogicalPredecessorLane() const
Definition: MSLane.cpp:1357
#define max(a, b)
Definition: polyfonts.c:65
static const SUMOReal LATERAL_SPEED_FACTOR
SUMOTime execute(SUMOTime currentTime)
Executes the command.
void walk(const Obstacles &obs, SUMOTime currentTime)
perform position update
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:46
static const SUMOReal LOOKAHEAD_ONCOMING
const ConstMSEdgeVector & getRoute() const
Definition: MSPerson.h:271
A list of positions.
MSEventControl * getBeginOfTimestepEvents()
Returns the event control for events executed at the begin of a time step.
Definition: MSNet.h:369
SUMOReal getLength() const
return the length of the pedestrian
SUMOTime getWaitingTime(const MSPerson::MSPersonStage_Walking &stage, SUMOTime now) const
return the time the person spent standing
static Obstacles mergeObstacles(const Obstacles &obs1, const Obstacles &obs2, int dir)
#define STEPS2TIME(x)
Definition: SUMOTime.h:65
SUMOTime string2time(const std::string &r)
Definition: SUMOTime.cpp:48
T MIN2(T a, T b)
Definition: StdDefs.h:68
#define POSITION_EPS
Definition: config.h:189
Position getPosition(const MSPerson::MSPersonStage_Walking &stage, SUMOTime now) const
return the network coordinate of the person
std::map< const MSLane *, Obstacles, lane_by_numid_sorter > NextLanesObstacles
SUMOReal getAngle(const MSPerson::MSPersonStage_Walking &stage, SUMOTime now) const
return the direction in which the person faces in degrees
SUMOReal x
position on the current lane
std::string toString(const T &t, std::streamsize accuracy=OUTPUT_ACCURACY)
Definition: ToString.h:53
bool isInternal() const
return whether this edge is an internal edge
Definition: MSEdge.h:240
int myDir
the walking direction on the current lane (1 forward, -1 backward)
virtual SUMOTime addEvent(Command *operation, SUMOTime execTimeStep, AdaptType type)
Adds an Event.
SUMOReal getEdgePos(const MSPerson::MSPersonStage_Walking &stage, SUMOTime now) const
abstract methods inherited from PedestrianState
SUMOReal getSpeed(const MSPerson::MSPersonStage_Walking &stage) const
return the current speed of the person
void cleanupHelper()
remove state at simulation end
PedestrianState * add(MSPerson *person, MSPerson::MSPersonStage_Walking *stage, SUMOTime now)
register the given person as a pedestrian
abstract base class for managing callbacks to retrieve various state information from the model ...
Definition: MSPModel.h:96
void extrapolate(SUMOReal val)
SUMOReal getArrivalPos() const
Definition: MSPerson.h:261
SUMOReal rotationDegreeAtOffset(SUMOReal pos) const
Returns the rotation at the given length.
void push_back(const PositionVector &p)
Appends all positions from the given vector.
SUMOReal getImpatience(SUMOTime now) const
returns the impatience
std::vector< PState * > Pedestrians
#define SUMOTime_MAX
Definition: SUMOTime.h:44
bool myWaitingToEnter
whether the pedestrian is waiting to start its walk
The edge is a normal street.
Definition: MSEdge.h:94
SUMOReal y() const
Returns the y-position.
Definition: Position.h:68
static size_t dictSize()
Returns the number of edges.
Definition: MSEdge.cpp:565
A storage for options typed value containers)
Definition: OptionsCont.h:108
Container for pedestrian state and individual position update function.
std::vector< Obstacle > Obstacles
int SUMOTime
Definition: SUMOTime.h:43
static const SUMOReal BLOCKER_LOOKAHEAD
static const int BACKWARD
Definition: MSPModel.h:77
const PositionVector & getShape() const
Returns this lane's shape.
Definition: MSLane.h:323
SUMOReal distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:240
static void addMappedObstacle(Obstacles &obs, const PState &p, int stripe, int currentDir, int nextDir, int offset, int stripes, int nextStripes)
Patch the time in a way that it is at least as high as the simulation begin time. ...
#define SUMOReal
Definition: config.h:218
SUMOReal myRelY
the orthogonal shift on the current lane
static const SUMOReal MAX_WAIT_TOLERANCE
bool isCrossing() const
return whether this edge is a pedestrian crossing
Definition: MSEdge.h:245
SUMOReal myRelX
the advancement along the current lane
const MSJunction * getFromJunction() const
Definition: MSEdge.h:345
const MSEdgeVector & getSuccessors() const
Returns the following edges.
Definition: MSEdge.h:315
T MIN3(T a, T b, T c)
Definition: StdDefs.h:81
MovePedestrians * myCommand
the MovePedestrians command that is registered
#define NUMERICAL_EPS
Definition: config.h:162
void push_back_noDoublePos(const Position &p)
static void DEBUG_PRINT(const Obstacles &obs)
#define DELTA_T
Definition: SUMOTime.h:50
const MSLinkCont & getLinkCont() const
returns the container with all links !!!
Definition: MSLane.cpp:1026
static SUMOReal dawdling
std::vector< MSEdge * > MSEdgeVector
Definition: MSEdge.h:78
static SUMOTime jamTime
int myNumActivePedestrians
the total number of active pedestrians
SUMOReal speed
speed relative to ego direction (positive means in the same direction)
const std::vector< IncomingLaneInfo > & getIncomingLanes() const
Definition: MSLane.h:584
SUMOReal getMaxSpeed() const
accessors to be used by MSPModel
Definition: MSPerson.h:255
static void initWalkingAreaPaths(const MSNet *net)
MSPModel_Striping(const OptionsCont &oc, MSNet *net)
Constructor (it should not be necessary to construct more than one instance)
Representation of a lane in the micro simulation.
Definition: MSLane.h:77
ActiveLanes myActiveLanes
store of all lanes which have pedestrians on them
static const SUMOReal MIN_STARTUP_SPEED
const MSLane * myLane
the current lane of this pedestrian
static const SUMOReal LATERAL_PENALTY
static void addCloserObstacle(Obstacles &obs, SUMOReal x, int stripe, const std::string &id, int stripes, int dir)
Obstacle(int dir)
create No-Obstacle
void moveInDirectionOnLane(Pedestrians &pedestrians, const MSLane *lane, SUMOTime currentTime, std::set< MSPerson * > &changedLane, int dir)
move pedestrians forward on one lane
static const Position INVALID
Definition: Position.h:262
Position transformToVectorCoordinates(const Position &p, bool extend=false) const
return position p within the length-wise coordinate system defined by this position vector...
const MSJunction * getToJunction() const
Definition: MSEdge.h:349
static bool canTraverse(int dir, const ConstMSEdgeVector &route)
return whether the route may traversed with the given starting direction
Definition: MSPModel.cpp:109