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