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  for (Pedestrians::iterator it = pedestrians.begin(); it != pedestrians.end();) {
650  PState* p = *it;
651  if (p->myDir != dir) {
652  ++it;
653  } else if (p->distToLaneEnd() < 0) {
654  // moveToNextLane may trigger re-insertion (for consecutive
655  // walks) so erase must be called first
656  it = pedestrians.erase(it);
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  } else {
666  ++it;
667  }
668  }
669  }
670 }
671 
672 
673 void
674 MSPModel_Striping::moveInDirectionOnLane(Pedestrians& pedestrians, const MSLane* lane, SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir) {
675  const int stripes = numStripes(lane);
676  Obstacles obs(stripes, Obstacle(dir)); // continously updated
677  NextLanesObstacles nextLanesObs; // continously updated
678  sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(dir));
679  for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
680  PState& p = *pedestrians[ii];
681  //std::cout << SIMTIME << "CHECKING" << p.myPerson->getID() << "\n";
682  Obstacles currentObs = obs;
683  if (p.myDir != dir || changedLane.count(p.myPerson) != 0) {
684  if (!p.myWaitingToEnter) {
685  Obstacle o(p);
686  obs[p.stripe()] = o;
687  obs[p.otherStripe()] = o;
688  }
689  continue;
690  }
691  if DEBUGCOND(p.myPerson->getID()) {
692  std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " currentObs=";
693  gDebugFlag1 = true;
694  DEBUG_PRINT(currentObs);
695  }
696  const MSLane* nextLane = p.myNLI.lane;
697  const MSLink* link = p.myNLI.link;
698  const SUMOReal dist = p.distToLaneEnd();
699  const SUMOReal speed = p.myStage->getMaxSpeed();
700  if (nextLane != 0 && dist <= LOOKAHEAD_ONCOMING) {
701  const SUMOReal currentLength = (p.myWalkingAreaPath == 0 ? lane->getLength() : p.myWalkingAreaPath->length);
702  const SUMOReal nextLength = nextLane->getLength(); // XXX what to do if nextLane is a walkingArea?
703  const Obstacles& nextObs = getNextLaneObstacles(
704  nextLanesObs, lane, nextLane, stripes,
705  nextLength, p.myNLI.dir,
706  currentLength, dir);
707 
708  if DEBUGCOND(p.myPerson->getID()) {
709  std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " nextObs=";
710  DEBUG_PRINT(nextObs);
711  }
712  p.mergeObstacles(currentObs, nextObs);
713  }
714  if DEBUGCOND(p.myPerson->getID()) {
715  std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWithNext=";
716  DEBUG_PRINT(currentObs);
717  }
718  p.mergeObstacles(currentObs, getNeighboringObstacles(pedestrians, ii, stripes));
719  if DEBUGCOND(p.myPerson->getID()) {
720  std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWithNeigh=";
721  DEBUG_PRINT(currentObs);
722  }
723  // check link state
724  if (link != 0
725  // only check close before junction, @todo we should take deceleration into account here
726  && dist - p.getMinGap() < LOOKAHEAD_SAMEDIR * speed
727  && (!link->opened(currentTime, speed, speed, p.getLength(), p.getImpatience(currentTime), speed, 0)
728  // @todo check for presence of vehicles blocking the path
729  )) {
730  // prevent movement passed a closed link
731  Obstacles closedLink(stripes, Obstacle(p.myRelX + dir * (dist + NUMERICAL_EPS), 0, "closedLink"));
732  p.mergeObstacles(currentObs, closedLink);
733  if DEBUGCOND(p.myPerson->getID()) {
734  std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWitTLS=";
735  DEBUG_PRINT(currentObs);
736  }
737  // consider rerouting over another crossing
738  if (p.myWalkingAreaPath != 0) {
739  // @todo actually another path would be needed starting at the current position
741  }
742  }
743  if (&lane->getEdge() == &p.myStage->getDestination() && p.myStage->getDestinationStop() != 0) {
744  Obstacles arrival(stripes, Obstacle(p.myStage->getArrivalPos() + dir * p.getMinGap(), 0, "arrival"));
745  p.mergeObstacles(currentObs, arrival);
746  }
747  p.walk(currentObs, currentTime);
748  gDebugFlag1 = false;
749  if (!p.myWaitingToEnter && !p.myAmJammed) {
750  Obstacle o(p);
751  obs[p.stripe()] = o;
752  obs[p.otherStripe()] = o;
754  for (int coll = 0; coll < ii; ++coll) {
755  PState& c = *pedestrians[coll];
756  if (!c.myWaitingToEnter && c.myWalkingAreaPath == 0 && !c.myAmJammed) {
757  if (c.stripe() == p.stripe() || p.stripe() == c.otherStripe() || p.otherStripe() == c.stripe() || p.otherStripe() == c.otherStripe()) {
758  Obstacle cObs(c);
759  // we check only for real collisions, no min gap violations
760  if (p.distanceTo(cObs, false) == DIST_OVERLAP) {
761  WRITE_WARNING("Collision of person '" + p.myPerson->getID() + "' and person '" + c.myPerson->getID()
762  + "', lane='" + lane->getID() + "', time=" + time2string(currentTime) + ".");
763  }
764  }
765  }
766  }
767  }
768  }
769  //std::cout << SIMTIME << p.myPerson->getID() << " lane=" << lane->getID() << " x=" << p.myRelX << "\n";
770  }
771 }
772 
773 
774 // ===========================================================================
775 // MSPModel_Striping::Obstacle method definitions
776 // ===========================================================================
778  xFwd(dir * DIST_FAR_AWAY), // far away when seen in dir
779  xBack(dir * DIST_FAR_AWAY), // far away when seen in dir
780  speed(0),
781  description("") {
782 }
783 
784 
786  description(ped.myPerson->getID()) {
787  assert(!ped.myWaitingToEnter);
788  xFwd = ped.getMaxX();
789  xBack = ped.getMinX();
790  speed = ped.myDir * ped.mySpeed;
791 }
792 
793 
794 // ===========================================================================
795 // MSPModel_Striping::PState method definitions
796 // ===========================================================================
797 
798 
800  myPerson(person),
801  myStage(stage),
802  myLane(lane),
803  myRelX(stage->getDepartPos()),
804  myRelY(0),
805  myDir(FORWARD),
806  mySpeed(0),
807  myWaitingToEnter(true),
808  myWaitingTime(0),
809  myWalkingAreaPath(0),
810  myAmJammed(false) {
811  const MSEdge* currentEdge = &lane->getEdge();
812  assert(!currentEdge->isWalkingArea());
813  const ConstMSEdgeVector& route = myStage->getRoute();
814  if (route.size() == 1) {
815  // only a single edge, move towards end pos
817  } else {
818  const bool mayStartForward = canTraverse(FORWARD, route);
819  const bool mayStartBackward = canTraverse(BACKWARD, route);
820  if DEBUGCOND(myPerson->getID()) {
821  std::cout << " initialize dir for " << myPerson->getID() << " forward=" << mayStartForward << " backward=" << mayStartBackward << "\n";
822  }
823  if (mayStartForward && mayStartBackward) {
824  // figure out the best direction via routing
825  ConstMSEdgeVector crossingRoute;
826  MSNet::getInstance()->getPedestrianRouter().compute(currentEdge, route.back(), myRelX, myStage->getArrivalPos(), myStage->getMaxSpeed(), 0, 0, crossingRoute, true);
827  if (crossingRoute.size() > 1) {
828  // route found
829  const MSEdge* nextEdge = crossingRoute[1];
830  if (nextEdge->getFromJunction() == currentEdge->getFromJunction() || nextEdge->getToJunction() == currentEdge->getFromJunction()) {
831  myDir = BACKWARD;
832  }
833  }
834  if DEBUGCOND(myPerson->getID()) {
835  std::cout << " crossingRoute=" << toString(crossingRoute) << "\n";
836  }
837  } else {
838  myDir = !mayStartBackward ? FORWARD : BACKWARD;
839  }
840  }
841  if (myDir == FORWARD) {
842  // start at the right side of the sidewalk
843  myRelY = stripeWidth * (numStripes(lane) - 1);
844  }
845  if DEBUGCOND(myPerson->getID()) {
846  std::cout << " added new pedestrian " << myPerson->getID() << " on " << lane->getID() << " myRelX=" << myRelX << " myRelY=" << myRelY << " dir=" << myDir << " route=" << toString(myStage->getRoute()) << "\n";
847  }
848 
849  myNLI = getNextLane(*this, lane, 0);
850 }
851 
852 
853 SUMOReal
854 MSPModel_Striping::PState::getMinX(const bool includeMinGap) const {
855  // @todo speed should have an influence here because faster persons need more space
856  if (myDir == FORWARD) {
857  return myRelX - getLength();
858  }
859  return myRelX - (includeMinGap ? getMinGap() : 0.);
860 }
861 
862 
863 SUMOReal
864 MSPModel_Striping::PState::getMaxX(const bool includeMinGap) const {
865  // @todo speed should have an influence here because faster persons need more space
866  if (myDir == FORWARD) {
867  return myRelX + (includeMinGap ? getMinGap() : 0.);
868  }
869  return myRelX + getLength();
870 }
871 
872 
873 SUMOReal
875  return myPerson->getVehicleType().getLength();
876 }
877 
878 
879 SUMOReal
881  return myPerson->getVehicleType().getMinGap();
882 }
883 
884 
885 int
887  return (int)floor(relY / stripeWidth + 0.5);
888 }
889 
890 
891 int
893  const int s = stripe(relY);
894  const SUMOReal offset = relY - s * stripeWidth;
895  const SUMOReal threshold = MAX2(NUMERICAL_EPS, stripeWidth - SQUEEZE * myPerson->getVehicleType().getWidth());
896  int result;
897  if (offset > threshold) {
898  result = s + 1;
899  } else if (offset < -threshold) {
900  result = s - 1;
901  } else {
902  result = s;
903  }
904  //std::cout.setf(std::ios::fixed , std::ios::floatfield);
905  //std::cout << std::setprecision(5);
906  //if DEBUGCOND(myPerson->getID()) std::cout << " otherStripe " << myPerson->getID() << " offset=" << offset << " threshold=" << threshold << " rawResult=" << result << "\n";
907  return result;
908 }
909 
910 int
912  return MIN2(MAX2(0, stripe(myRelY)), numStripes(myLane) - 1);
913 }
914 
915 
916 int
918  return MIN2(MAX2(0, otherStripe(myRelY)), numStripes(myLane) - 1);
919 }
920 
921 
922 SUMOReal
924  if (myStage->getNextRouteEdge() == 0) {
925  return myDir * (myStage->getArrivalPos() - myRelX) - POSITION_EPS;
926  } else {
928  return myDir == FORWARD ? length - myRelX : myRelX;
929  }
930 }
931 
932 
933 bool
935  const SUMOReal dist = distToLaneEnd();
936  if (myPerson->getID() == DEBUG1) {
937  std::cout << SIMTIME << " myRelX=" << myRelX << " dist=" << dist << "\n";
938  }
939  if (dist <= 0) {
940  //if (ped.myPerson->getID() == DEBUG1) {
941  // std::cout << SIMTIME << " addToLane x=" << ped.myRelX << " newDir=" << newDir << " newLane=" << newLane->getID() << " walkingAreaShape=" << walkingAreaShape << "\n";
942  //}
943  //std::cout << " changing to " << newLane->getID() << " myRelY=" << ped.myRelY << " oldStripes=" << numStripes(myLane) << " newStripes=" << numStripes(newLane);
944  //std::cout << " newY=" << ped.myRelY << " myDir=" << ped.myDir << " newDir=" << newDir;
945  const int oldDir = myDir;
946  const MSLane* oldLane = myLane;
947  myLane = myNLI.lane;
948  myDir = myNLI.dir;
949  const bool normalLane = (myLane == 0 || myLane->getEdge().getPurpose() == MSEdge::EDGEFUNCTION_NORMAL);
950  if DEBUGCOND(myPerson->getID()) {
951  std::cout << SIMTIME
952  << " ped=" << myPerson->getID()
953  << " moveToNextLane old=" << oldLane->getID()
954  << " new=" << (myLane == 0 ? "NULL" : myLane->getID())
955  << " oldDir=" << oldDir
956  << " newDir=" << myDir
957  << "\n";
958  }
959  if (myLane == 0) {
961  }
962  myStage->moveToNextEdge(myPerson, currentTime, normalLane ? 0 : &myLane->getEdge());
963  if (myLane != 0) {
964  assert(myDir != UNDEFINED_DIRECTION);
965  myNLI = getNextLane(*this, myLane, oldLane);
966  assert(myNLI.lane != oldLane); // do not turn around
967  if DEBUGCOND(myPerson->getID()) {
968  std::cout << " nextLane=" << (myNLI.lane == 0 ? "NULL" : myNLI.lane->getID()) << "\n";
969  }
970  if (myLane->getEdge().isWalkingArea()) {
971  if (myNLI.dir != UNDEFINED_DIRECTION) {
972  myWalkingAreaPath = &myWalkingAreaPaths[std::make_pair(oldLane, myNLI.lane)];
973  assert(myWalkingAreaPath->from != 0);
974  assert(myWalkingAreaPath->to != 0);
975  assert(myWalkingAreaPath->shape.size() >= 2);
976  if DEBUGCOND(myPerson->getID()) {
977  std::cout << " mWAPath shape=" << myWalkingAreaPath->shape << " length=" << myWalkingAreaPath->length << "\n";
978  }
979  } else {
980  // disconnnected route. move to the next edge (arbitrariliy, maintaining current direction)
981  if (OptionsCont::getOptions().getBool("ignore-route-errors")) {
982  myStage->moveToNextEdge(myPerson, currentTime, 0);
983  myLane = myNLI.lane;
984  assert(myLane != 0);
986  myNLI = getNextLane(*this, myLane, oldLane);
987  myWalkingAreaPath = 0;
988  } else {
989  throw ProcessError("Disconnected walk for person '" + myPerson->getID() + "'.");
990  }
991  }
992  } else {
993  myWalkingAreaPath = 0;
994  }
995  // adapt x to fit onto the new lane
996  if (myDir == BACKWARD) {
997  const SUMOReal newLength = (myWalkingAreaPath == 0 ? myLane->getLength() : myWalkingAreaPath->length);
998  myRelX = newLength + dist;
999  } else {
1000  myRelX = -dist;
1001  }
1002  // adjust to change in direction
1003  if (myDir != oldDir) {
1004  myRelY = oldLane->getWidth() - stripeWidth - myRelY;
1005  }
1006  // adjust to differences in sidewalk width
1007  myRelY += 0.5 * (myLane->getWidth() - oldLane->getWidth());
1008  }
1009  return true;
1010  } else {
1011  return false;
1012  }
1013 }
1014 
1015 
1016 void
1018  const int stripes = (int)obs.size();
1019  const int sMax = stripes - 1;
1020  assert(stripes == numStripes(myLane));
1021  const SUMOReal vMax = myStage->getMaxSpeed();
1022  // ultimate goal is to choose the prefered stripe (chosen)
1023  const int current = stripe();
1024  const int other = otherStripe();
1025  // compute utility for all stripes
1026  std::vector<SUMOReal> utility(stripes);
1027  // penalize lateral movement (may increase jamming)
1028  for (int i = 0; i < stripes; ++i) {
1029  utility[i] = abs(i - current) * LATERAL_PENALTY;
1030  }
1031  // compute distances
1032  std::vector<SUMOReal> distance(stripes);
1033  for (int i = 0; i < stripes; ++i) {
1034  distance[i] = distanceTo(obs[i]);
1035  }
1036  // forbid stripes which are blocked and also all stripes behind them
1037  for (int i = 0; i < stripes; ++i) {
1038  if (distanceTo(obs[i]) == DIST_OVERLAP) {
1039  if (i == current && !myWaitingToEnter) {
1040  utility[i] += OBSTRUCTED_PENALTY;
1041  }
1042  if (i < current) {
1043  for (int j = 0; j <= i; ++j) {
1044  utility[j] += OBSTRUCTED_PENALTY;
1045  }
1046  }
1047  if (i > current) {
1048  for (int j = i; j < stripes; ++j) {
1049  utility[j] += OBSTRUCTED_PENALTY;
1050  }
1051  }
1052  }
1053  }
1054  // forbid a portion of the leftmost stripes (in walking direction).
1055  // lanes with stripes less than 1 / RESERVE_FOR_ONCOMING_FACTOR
1056  // may still deadlock in heavy pedestrian traffic
1057  const int reserved = (int)floor(stripes * RESERVE_FOR_ONCOMING_FACTOR);
1058  if (myDir == FORWARD) {
1059  for (int i = 0; i < reserved; ++i) {
1060  utility[i] += INAPPROPRIATE_PENALTY;
1061  }
1062  } else {
1063  for (int i = sMax; i > sMax - reserved; --i) {
1064  utility[i] += INAPPROPRIATE_PENALTY;
1065  }
1066  }
1067  // adapt utility based on obstacles
1068  for (int i = 0; i < stripes; ++i) {
1069  if (obs[i].speed < 0) {
1070  // penalize evasion to the left
1071  if (myDir == FORWARD && i > 0) {
1072  utility[i - 1] -= 0.5;
1073  } else if (myDir == BACKWARD && i < sMax) {
1074  utility[i + 1] -= 0.5;
1075  }
1076  }
1077  // compute expected distance achievable by staying on this stripe for a time horizon
1078  const SUMOReal walkDist = MAX2((SUMOReal)0, distance[i]); // disregard special distance flags
1079  const SUMOReal lookAhead = obs[i].speed * myDir >= 0 ? LOOKAHEAD_SAMEDIR : LOOKAHEAD_ONCOMING;
1080  const SUMOReal expectedDist = MIN2(vMax * LOOKAHEAD_SAMEDIR, walkDist + obs[i].speed * myDir * lookAhead);
1081  if (DEBUGCOND(myPerson->getID())) {
1082  std::cout << " util=" << utility[i] << " exp=" << expectedDist << "\n";
1083  }
1084  if (expectedDist >= 0) {
1085  utility[i] += expectedDist;
1086  } else {
1087  // let only the distance count
1088  utility[i] += ONCOMING_CONFLICT_PENALTY + distance[i];
1089  }
1090  }
1091  // bonus to remain on the rightmost lane (in walking direction) if there
1092  // are oncoming
1093  if (((myDir == FORWARD && current == sMax)
1094  || (myDir == BACKWARD && current == 0))
1095  && obs[current].speed * myDir < 0) {
1096  utility[current] -= ONCOMING_CONFLICT_PENALTY;
1097  }
1098  // bonus to leave the leftmost lane (in walking direction) if there
1099  // are oncoming
1100  if (((myDir == BACKWARD && current == sMax)
1101  || (myDir == FORWARD && current == 0))
1102  && obs[current].speed * myDir < 0) {
1103  utility[current] += ONCOMING_CONFLICT_PENALTY;
1104  }
1105 
1106  // select best stripe
1107  int chosen = current;
1108  for (int i = 0; i < stripes; ++i) {
1109  if (utility[chosen] < utility[i]) {
1110  chosen = i;
1111  }
1112  }
1113  // compute speed components along both axes
1114  const int next = (chosen == current ? current : (chosen < current ? current - 1 : current + 1));
1115  const SUMOReal xDist = MIN3(distance[current], distance[other], distance[next]);
1116  // XXX preferred gap differs between approaching a standing obstacle or a moving obstacle
1117  const SUMOReal preferredGap = NUMERICAL_EPS;
1118  SUMOReal xSpeed = MIN2(vMax, MAX2((SUMOReal)0, DIST2SPEED(xDist - preferredGap)));
1119  if (xSpeed < NUMERICAL_EPS) {
1120  xSpeed = 0.;
1121  }
1122  if (DEBUGCOND(myPerson->getID())) {
1123  std::cout << " xSpeedPotential=" << xSpeed << "\n";
1124  }
1125  // avoid tiny steps
1126  // XXX pressure from behind?
1127  if (mySpeed == 0 && xDist < MIN_STARTUP_DIST) {
1128  xSpeed = 0;
1129  }
1130  if (xSpeed == 0) {
1131  if (myWaitingTime > jamTime || myAmJammed) {
1132  // squeeze slowly through the crowd ignoring others
1133  if (!myAmJammed) {
1135  WRITE_WARNING("Person '" + myPerson->getID()
1136  + "' is jammed on edge '" + myStage->getEdge()->getID()
1137  + "', time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
1138  myAmJammed = true;
1139  }
1140  xSpeed = vMax / 4;
1141  }
1142  } else {
1143  myAmJammed = false;
1144  }
1145  // dawdling
1146  const SUMOReal dawdle = MIN2(xSpeed, RandHelper::rand() * vMax * dawdling);
1147 
1148  // XXX ensure that diagonal speed <= vMax
1149  // avoid deadlocks on narrow sidewalks
1150  //if (oncoming && xSpeed == 0 && myStage->getWaitingTime(currentTime) > TIME2STEPS(ONCOMIN_PATIENCE)) {
1151  // if DEBUGCOND(myPerson->getID()) std::cout << " stepping asside to resolve oncoming deadlock\n";
1152  // xSpeed = POSITION_EPS; // reset myWaitingTime
1153  // if (myDir == FORWARD && chosen < sMax) {
1154  // chosen += 1;
1155  // } else if (myDir == BACKWARD && chosen > 0) {
1156  // chosen -= 1;
1157  // }
1158  //}
1159  const SUMOReal maxYSpeed = MAX2(vMax * LATERAL_SPEED_FACTOR, vMax - xSpeed);
1160  SUMOReal ySpeed = 0;
1161  SUMOReal yDist = 0;
1162  if (utility[next] > OBSTRUCTION_THRESHOLD && utility[chosen] > OBSTRUCTION_THRESHOLD) {
1163  // don't move laterally if the stripes are blocked
1164  yDist = (chosen * stripeWidth) - myRelY;
1165  if (fabs(yDist) > NUMERICAL_EPS) {
1166  ySpeed = (yDist > 0 ?
1167  MIN2(maxYSpeed, DIST2SPEED(yDist)) :
1168  MAX2(-maxYSpeed, DIST2SPEED(yDist)));
1169  }
1170  }
1171  // DEBUG
1172  if DEBUGCOND(myPerson->getID()) {
1173  std::cout << SIMTIME
1174  << " ped=" << myPerson->getID()
1175  << " edge=" << myStage->getEdge()->getID()
1176  << " x=" << myRelX
1177  << " y=" << myRelY
1178  << " d=" << myDir
1179  << " pvx=" << mySpeed
1180  << " cur=" << current
1181  << " cho=" << chosen
1182  << " oth=" << other
1183  << " nxt=" << next
1184  << " vx=" << xSpeed
1185  << " dawdle=" << dawdle
1186  << " vy=" << ySpeed
1187  << " xd=" << xDist
1188  << " yd=" << yDist
1189  << " vMax=" << myStage->getMaxSpeed()
1190  << " wTime=" << myStage->getWaitingTime(currentTime)
1191  << " jammed=" << myAmJammed
1192  << "\n distance=" << toString(distance)
1193  << "\n utility=" << toString(utility)
1194  << "\n";
1195  DEBUG_PRINT(obs);
1196  }
1197  myRelX += SPEED2DIST(xSpeed * myDir);
1198  myRelY += SPEED2DIST(ySpeed);
1199  mySpeed = xSpeed;
1200  if (xSpeed >= SUMO_const_haltingSpeed) {
1201  myWaitingToEnter = false;
1202  myWaitingTime = 0;
1203  } else {
1205  }
1206 }
1207 
1208 
1209 SUMOReal
1211  return MAX2((SUMOReal)0, MIN2(SUMOReal(1),
1214 }
1215 
1216 
1217 SUMOReal
1219  return myRelX;
1220 }
1221 
1222 
1223 Position
1225  if (myLane == 0) {
1226  // pedestrian has already finished
1227  return Position::INVALID;
1228  }
1229  const SUMOReal lateral_offset = myRelY + (stripeWidth - myLane->getWidth()) * 0.5;
1230  if (myWalkingAreaPath == 0) {
1231  return stage.getLanePosition(myLane, myRelX, lateral_offset);
1232  } else {
1233  return myWalkingAreaPath->shape.positionAtOffset(myRelX, lateral_offset);
1234  }
1235 }
1236 
1237 
1238 SUMOReal
1240  if (myLane == 0) {
1241  // pedestrian has already finished
1242  return 0;
1243  }
1245  SUMOReal angle = shp.rotationAtOffset(myRelX) + (myDir == MSPModel::BACKWARD ? M_PI : 0);
1246  if (angle > M_PI) {
1247  angle -= 2 * M_PI;
1248  }
1249  return angle;
1250 }
1251 
1252 
1253 SUMOTime
1255  return myWaitingTime;
1256 }
1257 
1258 
1259 SUMOReal
1261  return mySpeed;
1262 }
1263 
1264 
1265 const MSEdge*
1267  return myNLI.lane == 0 ? 0 : &myNLI.lane->getEdge();
1268 }
1269 
1270 
1271 SUMOReal
1272 MSPModel_Striping::PState::distanceTo(const Obstacle& obs, const bool includeMinGap) const {
1273  // check for overlap
1274  const SUMOReal maxX = getMaxX(includeMinGap);
1275  const SUMOReal minX = getMinX(includeMinGap);
1276  if ((obs.xFwd >= maxX && obs.xBack <= maxX) || (obs.xFwd <= maxX && obs.xFwd >= minX)) {
1277  return DIST_OVERLAP;
1278  }
1279  if (myDir == FORWARD) {
1280  return obs.xFwd < minX ? DIST_BEHIND : obs.xBack - maxX;
1281  } else {
1282  return obs.xBack > maxX ? DIST_BEHIND : minX - obs.xFwd;
1283  }
1284 }
1285 
1286 
1287 void
1289  for (int i = 0; i < (int)into.size(); ++i) {
1290  if (gDebugFlag1) {
1291  std::cout << " i=" << i << " intoDist=" << distanceTo(into[i]) << " obs2Dist=" << distanceTo(obs2[i]) << "\n";
1292  }
1293  if (distanceTo(obs2[i]) < distanceTo(into[i])) {
1294  into[i] = obs2[i];
1295  }
1296  }
1297 }
1298 
1299 
1300 
1301 // ===========================================================================
1302 // MSPModel_Striping::MovePedestrians method definitions
1303 // ===========================================================================
1304 //
1305 
1306 SUMOTime
1308  std::set<MSPerson*> changedLane;
1309  myModel->moveInDirection(currentTime, changedLane, FORWARD);
1310  myModel->moveInDirection(currentTime, changedLane, BACKWARD);
1311  // DEBUG
1312 #ifdef LOG_ALL
1313  for (ActiveLanes::const_iterator it_lane = myModel->getActiveLanes().begin(); it_lane != myModel->getActiveLanes().end(); ++it_lane) {
1314  const MSLane* lane = it_lane->first;
1315  Pedestrians pedestrians = it_lane->second;
1316  if (pedestrians.size() == 0) {
1317  continue;
1318  }
1319  sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(FORWARD));
1320  std::cout << SIMTIME << " lane=" << lane->getID();
1321  for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
1322  const PState& p = *pedestrians[ii];
1323  std::cout << " (" << p.myPerson->getID() << " " << p.myRelX << "," << p.myRelY << " " << p.myDir << ")";
1324  }
1325  std::cout << "\n";
1326  }
1327 #endif
1328  return DELTA_T;
1329 }
1330 
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:467
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:375
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:257
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:885
SUMOReal getWidth() const
Returns the lane&#39;s width.
Definition: MSLane.h:391
static const SUMOReal ONCOMING_CONFLICT_PENALTY
#define DEBUGCOND(PEDID)
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:160
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:321
static MSPModel * myModel
Definition: MSPModel.h:87
SUMOTime getCurrentTimeStep() const
Returns the current simulation step.
Definition: MSNet.h:255
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:94
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
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:65
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:562
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:1308
bool moveToNextEdge(MSPerson *person, SUMOTime currentTime, MSEdge *nextInternal=0)
move forward and return whether the person arrived
Definition: MSPerson.cpp:195
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.
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:389
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:54
SUMOTime myWaitingTime
the consecutive time spent at speed 0
bool isInternal() const
return whether this edge is an internal edge
Definition: MSEdge.h:247
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
void registerJammed()
register a jammed person
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:242
SUMOReal getImpatience(SUMOTime now) const
returns the impatience
std::vector< PState * > Pedestrians
#define SUMOTime_MAX
Definition: SUMOTime.h:44
virtual MSPersonControl & getPersonControl()
Returns the person control.
Definition: MSNet.cpp:690
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:611
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:108
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:322
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:252
SUMOReal myRelX
the advancement along the current lane
const MSJunction * getFromJunction() const
Definition: MSEdge.h:372
const MSEdgeVector & getSuccessors() const
Returns the following edges.
Definition: MSEdge.h:342
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)
static void DEBUG_PRINT(const Obstacles &obs)
const MSLinkCont & getLinkCont() const
returns the container with all links !!!
Definition: MSLane.cpp:947
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:589
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:77
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)
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:376
static bool canTraverse(int dir, const ConstMSEdgeVector &route)
return whether the route may traversed with the given starting direction
Definition: MSPModel.cpp:91