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)
const std::vector< IncomingLaneInfo > & getIncomingLanes() const
Definition: MSLane.h:708
bool gDebugFlag1
global utility flags for debugging
Definition: StdDefs.cpp:91
SUMOReal getWidth() const
Returns the lane&#39;s width.
Definition: MSLane.h:496
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
MSLane * getLogicalPredecessorLane() const
get the most likely precedecessor lane (sorted using by_connections_to_sorter). The result is cached ...
Definition: MSLane.cpp:1954
SUMOReal getLength() const
Returns the lane&#39;s length.
Definition: MSLane.h:480
void moveInDirection(SUMOTime currentTime, std::set< MSPerson *> &changedLane, int dir)
move all pedestrians forward and advance to the next lane if applicable
MSEdge & getEdge() const
Returns the lane&#39;s edge.
Definition: MSLane.h:571
static const SUMOReal OBSTRUCTED_PENALTY
long long int SUMOTime
Definition: SUMOTime.h:43
static const SUMOReal SAFETY_GAP
Definition: MSPModel.h:78
#define SPEED2DIST(x)
Definition: SUMOTime.h:55
MSPerson::MSPersonStage_Walking * myStage
void moveInDirectionOnLane(Pedestrians &pedestrians, const MSLane *lane, SUMOTime currentTime, std::set< MSPerson *> &changedLane, int dir)
move pedestrians forward on one lane
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)
const MSEdge * getEdge() const
Returns the current edge.
sorts the persons by position on the lane. If dir is forward, higher x positions come first...
SUMOReal mySpeed
the current walking speed
static const MSLane * getNextWalkingArea(const MSLane *currentLane, const int dir, MSLink *&link)
return the next walkingArea in the given direction
#define DEBUG1
static int connectedDirection(const MSLane *from, const MSLane *to)
returns the direction in which these lanes are connectioned or 0 if they are not
WalkingAreaPath * myWalkingAreaPath
the current walkingAreaPath or 0
#define M_PI
Definition: angles.h:37
information regarding surround Pedestrians (and potentially other things)
The base class for an intersection.
Definition: MSJunction.h:64
PState(MSPerson *person, MSPerson::MSPersonStage_Walking *stage, const MSLane *lane)
const MSEdge * getEdge() const
Returns the current edge.
Definition: MSPerson.cpp:83
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].
Position positionAtOffset(SUMOReal pos, SUMOReal lateralOffset=0) const
Returns the position at the given length.
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
MSPedestrianRouterDijkstra & getPedestrianRouter(const MSEdgeVector &prohibited=MSEdgeVector()) const
Definition: MSNet.cpp:894
SUMOTime DELTA_T
Definition: SUMOTime.cpp:39
const PositionVector & getShape() const
Returns this lane&#39;s shape.
Definition: MSLane.h:427
PositionVector reverse() const
reverse position vector
std::vector< const MSEdge * > ConstMSEdgeVector
Definition: MSEdge.h:78
const MSEdgeVector & getIncomingEdges() const
Returns the list of edges from which this edge may be reached.
Definition: MSEdge.h:332
std::map< std::pair< const MSLane *, const MSLane * >, WalkingAreaPath > WalkingAreaPaths
bool getBool(const std::string &name) const
Returns the boolean-value of the named option (only for Option_Bool)
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
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)
SUMOReal getSpeed(const MSPerson::MSPersonStage_Walking &stage) const
return the current speed of the person
static MSPModel * myModel
Definition: MSPModel.h:87
static WalkingAreaPaths myWalkingAreaPaths
store for walkinArea elements
const MSJunction * getToJunction() const
Definition: MSEdge.h:387
#define abs(a)
Definition: polyfonts.c:67
Position getLanePosition(const MSLane *lane, SUMOReal at, SUMOReal offset) const
get position on lane at length at with orthogonal offset
SUMOReal rotationAtOffset(SUMOReal pos) const
Returns the rotation at the given length.
#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
SUMOReal distToLaneEnd() const
the absolute distance to the end of the lane in walking direction (or to the arrivalPos) ...
static Pedestrians noPedestrians
empty pedestrian vector
static bool gCheck4Accidents
Definition: MSGlobals.h:80
static const SUMOReal RESERVE_FOR_ONCOMING_FACTOR
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
SUMOReal distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:232
static const SUMOReal SQUEEZE
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)
SUMOReal getWidth() const
Get the width which vehicles of this class shall have when being drawn.
static const SUMOReal LOOKAHEAD_SAMEDIR
static const int UNDEFINED_DIRECTION
Definition: MSPModel.h:75
SUMOReal getMaxSpeed() const
accessors to be used by MSPModel
Definition: MSPerson.h:139
bool moveToNextEdge(MSPerson *person, SUMOTime currentTime, MSEdge *nextInternal=0)
move forward and return whether the person arrived
Definition: MSPerson.cpp:194
static const SUMOReal LATERAL_SPEED_FACTOR
SUMOTime execute(SUMOTime currentTime)
Executes the command.
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
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 SUMOReal INAPPROPRIATE_PENALTY
SUMOReal xFwd
maximal position on the current lane in forward direction
static const SUMOReal OBSTRUCTION_THRESHOLD
A list of positions.
MSEventControl * getBeginOfTimestepEvents()
Returns the event control for events executed at the begin of a time step.
Definition: MSNet.h:400
std::string getString(const std::string &name) const
Returns the string-value of the named option (only for Option_String)
#define DIST_FAR_AWAY
#define STEPS2TIME(x)
Definition: SUMOTime.h:65
Position getPosition(const MSPerson::MSPersonStage_Walking &stage, SUMOTime now) const
return the network coordinate of the person
SUMOTime getCurrentTimeStep() const
Returns the current simulation step.
Definition: MSNet.h:254
SUMOTime string2time(const std::string &r)
Definition: SUMOTime.cpp:46
T MIN2(T a, T b)
Definition: StdDefs.h:69
SUMOReal x() const
Returns the x-position.
Definition: Position.h:63
#define POSITION_EPS
Definition: config.h:187
const std::string & getID() const
returns the id of the transportable
std::map< const MSLane *, Obstacles, lane_by_numid_sorter > NextLanesObstacles
EdgeBasicFunction getPurpose() const
Returns the edge type (EdgeBasicFunction)
Definition: MSEdge.h:249
bool isCrossing() const
return whether this edge is a pedestrian crossing
Definition: MSEdge.h:259
std::string toString(const T &t, std::streamsize accuracy=OUTPUT_ACCURACY)
Definition: ToString.h:55
bool isInternal() const
return whether this edge is an internal edge
Definition: MSEdge.h:254
SUMOTime myWaitingTime
the consecutive time spent at speed 0
const ConstMSEdgeVector & getRoute() const
Definition: MSPerson.h:155
int myDir
the walking direction on the current lane (1 forward, -1 backward)
#define DIST_BEHIND
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
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
SUMOReal getAngle(const MSPerson::MSPersonStage_Walking &stage, SUMOTime now) const
return the direction in which the person faces in degrees
abstract base class for managing callbacks to retrieve various state information from the model ...
Definition: MSPModel.h:93
SUMOReal getMaxX(const bool includeMinGap=true) const
return the maximum position on the lane
const MSEdge * getNextEdge(const MSPerson::MSPersonStage_Walking &stage) const
return the list of internal edges if the pedestrian is on an intersection
std::vector< PState * > Pedestrians
SUMOTime getWaitingTime(SUMOTime now) const
the time this transportable spent waiting
Definition: MSPerson.cpp:117
#define SUMOTime_MAX
Definition: SUMOTime.h:44
const MSStoppingPlace * getDestinationStop() const
returns the destination stop (if any)
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:706
The edge is a normal street.
Definition: MSEdge.h:93
const MSJunction * getFromJunction() const
Definition: MSEdge.h:383
SUMOReal getArrivalPos() const
Definition: MSPerson.h:145
SUMOReal getLength() const
return the length of the edge
Definition: MSEdge.h:591
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
const MSEdgeVector & getSuccessors() const
Returns the following edges.
Definition: MSEdge.h:353
std::vector< Obstacle > Obstacles
static const SUMOReal BLOCKER_LOOKAHEAD
const MSEdge & getDestination() const
returns the destination edge
SUMOReal xBack
maximal position on the current lane in backward direction
static const int BACKWARD
Definition: MSPModel.h:74
#define DIST_OVERLAP
SUMOReal getMinGap() const
Get the free space in front of vehicles of this class.
const MSVehicleType & getVehicleType() const
bool isWalkingArea() const
return whether this edge is walking area
Definition: MSEdge.h:264
Patch the time in a way that it is at least as high as the simulation begin time. ...
#define SUMOReal
Definition: config.h:213
SUMOReal myRelY
the orthogonal shift on the current lane
static const SUMOReal MAX_WAIT_TOLERANCE
SUMOReal myRelX
the advancement along the current lane
T MIN3(T a, T b, T c)
Definition: StdDefs.h:82
MovePedestrians * myCommand
the MovePedestrians command that is registered
bool blockedAtDist(const MSLane *lane, SUMOReal distToCrossing, std::vector< const MSPerson *> *collectBlockers)
whether a pedestrian is blocking the crossing of lane at offset distToCrossing
#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)
static SUMOReal dawdling
SUMOReal getMinGap() const
return the minimum gap of the pedestrian
const MSEdge * getNextRouteEdge() const
Definition: MSPerson.h:152
std::vector< MSEdge * > MSEdgeVector
Definition: MSEdge.h:77
static SUMOTime jamTime
const MSLinkCont & getLinkCont() const
returns the container with all links !!!
Definition: MSLane.cpp:1512
int myNumActivePedestrians
the total number of active pedestrians
SUMOReal speed
speed relative to lane direction (positive means in the same direction)
SUMOReal distanceTo(const Obstacle &obs, const bool includeMinGap=true) const
std::string description
the id / description of the obstacle
SUMOTime getWaitingTime(const MSPerson::MSPersonStage_Walking &stage, SUMOTime now) const
return the time the person spent standing
SUMOReal y() const
Returns the y-position.
Definition: Position.h:68
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)
SUMOReal getLength() const
return the length of the pedestrian
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
Position transformToVectorCoordinates(const Position &p, bool extend=false) const
return position p within the length-wise coordinate system defined by this position vector...
static const SUMOReal LATERAL_PENALTY
Obstacle(int dir)
create No-Obstacle
SUMOReal getImpatience() const
Returns this type&#39;s impatience.
void extrapolate(const SUMOReal val, const bool onlyFirst=false)
extrapolate position vector
SUMOReal getFloat(const std::string &name) const
Returns the SUMOReal-value of the named option (only for Option_Float)
static const Position INVALID
Definition: Position.h:261
StageType getCurrentStageType() const
the current stage type of the transportable
static bool canTraverse(int dir, const ConstMSEdgeVector &route)
return whether the route may traversed with the given starting direction
Definition: MSPModel.cpp:91
SUMOReal getImpatience(SUMOTime now) const
returns the impatience