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