SUMO - Simulation of Urban MObility
MSPModel_Striping.cpp
Go to the documentation of this file.
1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2014-2017 German Aerospace Center (DLR) and others.
4 /****************************************************************************/
5 //
6 // This program and the accompanying materials
7 // are made available under the terms of the Eclipse Public License v2.0
8 // which accompanies this distribution, and is available at
9 // http://www.eclipse.org/legal/epl-v20.html
10 //
11 /****************************************************************************/
18 // The pedestrian following model (prototype)
19 /****************************************************************************/
20 
21 // ===========================================================================
22 // included modules
23 // ===========================================================================
24 #ifdef _MSC_VER
25 #include <windows_config.h>
26 #else
27 #include <config.h>
28 #endif
29 
30 #include <cmath>
31 #include <algorithm>
33 #include <utils/geom/GeomHelper.h>
35 #include <microsim/MSNet.h>
36 #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 DEBUGID1 ""
50 #define DEBUGID2 ""
51 //#define DEBUGCOND(PED) (false)
52 //#define DEBUGCOND(PED) ((PED).myPerson->getID() == DEBUGID1 || (PED).myPerson->getID() == DEBUGID2)
53 #define DEBUGCOND(PED) ((PED).myPerson->isSelected())
54 #define DEBUGCOND2(LANE) ((LANE)->isSelected())
55 //#define LOG_ALL 1
56 
58  for (int i = 0; i < (int)obs.size(); ++i) {
59  std::cout
60  << "(" << obs[i].description
61  << " x=(" << obs[i].xBack << "," << obs[i].xFwd
62  << ") s=" << obs[i].speed
63  << ") ";
64  }
65  std::cout << "\n";
66 }
67 
68 // ===========================================================================
69 // named (internal) constants
70 // ===========================================================================
71 
72 // distances are comparable with lower values being "more important"
73 const double MSPModel_Striping::DIST_FAR_AWAY(10000);
74 const double MSPModel_Striping::DIST_BEHIND(1000);
75 const double MSPModel_Striping::DIST_OVERLAP(-1);
76 
77 // ===========================================================================
78 // static members
79 // ===========================================================================
80 
84 
85 
86 // model parameters (static to simplify access from class PState
90 const double MSPModel_Striping::LOOKAHEAD_SAMEDIR(4.0); // seconds
91 const double MSPModel_Striping::LOOKAHEAD_ONCOMING(10.0); // seconds
92 const double MSPModel_Striping::LOOKAROUND_VEHICLES(60.0); // meters
93 const double MSPModel_Striping::LATERAL_PENALTY(-1.); // meters
94 const double MSPModel_Striping::OBSTRUCTED_PENALTY(-300000.); // meters
95 const double MSPModel_Striping::INAPPROPRIATE_PENALTY(-20000.); // meters
96 const double MSPModel_Striping::ONCOMING_CONFLICT_PENALTY(-1000.); // meters
97 const double MSPModel_Striping::OBSTRUCTION_THRESHOLD(MSPModel_Striping::OBSTRUCTED_PENALTY * 0.5); // despite obstruction, additional utility may have been added
98 const double MSPModel_Striping::SQUEEZE(0.7);
101 const double MSPModel_Striping::MAX_WAIT_TOLERANCE(120.); // seconds
103 const double MSPModel_Striping::MIN_STARTUP_DIST(0.4); // meters
104 
105 #define MINGAP_TO_VEHICLE 0.25
106 
107 
108 // ===========================================================================
109 // MSPModel_Striping method definitions
110 // ===========================================================================
111 
114  myCommand = new MovePedestrians(this);
117  // configurable parameters
118  stripeWidth = oc.getFloat("pedestrian.striping.stripe-width");
119  dawdling = oc.getFloat("pedestrian.striping.dawdling");
120 
121  jamTime = string2time(oc.getString("pedestrian.striping.jamtime"));
122  if (jamTime <= 0) {
124  }
125 }
126 
127 
129 }
130 
131 
135  const MSLane* lane = getSidewalk<MSEdge, MSLane>(person->getEdge());
136  if (lane == 0) {
137  std::string error = "Person '" + person->getID() + "' could not find sidewalk on edge '" + person->getEdge()->getID() + "', time="
138  + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".";
139  if (OptionsCont::getOptions().getBool("ignore-route-errors")) {
140  WRITE_WARNING(error);
141  return 0;
142  } else {
143  throw ProcessError(error);
144  }
145  }
146  PState* ped = new PState(person, stage, lane);
147  myActiveLanes[lane].push_back(ped);
149  return ped;
150 }
151 
152 
153 void
155  const MSLane* lane = dynamic_cast<PState*>(state)->myLane;
156  Pedestrians& pedestrians = myActiveLanes[lane];
157  for (Pedestrians::iterator it = pedestrians.begin(); it != pedestrians.end(); ++it) {
158  if (*it == state) {
159  delete state;
160  pedestrians.erase(it);
161  return;
162  }
163  }
164 }
165 
166 
167 bool
168 MSPModel_Striping::blockedAtDist(const MSLane* lane, double vehSide, double vehWidth,
169  double oncomingGap, std::vector<const MSPerson*>* collectBlockers) {
170  const Pedestrians& pedestrians = getPedestrians(lane);
171  for (Pedestrians::const_iterator it_ped = pedestrians.begin(); it_ped != pedestrians.end(); ++it_ped) {
172  const PState& ped = **it_ped;
173  const double leaderFrontDist = (ped.myDir == FORWARD ? vehSide - ped.myRelX : ped.myRelX - vehSide);
174  const double leaderBackDist = leaderFrontDist + ped.getLength();
175  if DEBUGCOND(ped) {
176  std::cout << SIMTIME << " lane=" << lane->getID() << " dir=" << ped.myDir << " pX=" << ped.myRelX << " pL=" << ped.getLength()
177  << " vehSide=" << vehSide
178  << " vehWidth=" << vehWidth
179  << " lBD=" << leaderBackDist
180  << " lFD=" << leaderFrontDist
181  << "\n";
182  }
183  if (leaderBackDist >= -vehWidth
184  && (leaderFrontDist < 0
185  // give right of way to (close) approaching pedestrians unless they are standing
186  || (leaderFrontDist <= oncomingGap && ped.myWaitingTime < TIME2STEPS(2.0)))) {
187  // found one pedestrian that is not completely past the crossing point
188  //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";
189  if (collectBlockers == 0) {
190  return true;
191  } else {
192  collectBlockers->push_back(ped.myPerson);
193  }
194  }
195  }
196  if (collectBlockers == 0) {
197  return false;
198  } else {
199  return collectBlockers->size() > 0;
200  }
201 }
202 
203 
204 bool
206  return getPedestrians(lane).size() > 0;
207 }
208 
209 
210 bool
213 }
214 
216 MSPModel_Striping::nextBlocking(const MSLane* lane, double minPos, double minRight, double maxLeft, double stopTime) {
217  PersonDist result((const MSPerson*)0, -1);
218  double closest = std::numeric_limits<double>::max();
219  const Pedestrians& pedestrians = getPedestrians(lane);
220  for (Pedestrians::const_iterator it_ped = pedestrians.begin(); it_ped != pedestrians.end(); ++it_ped) {
221  const PState& ped = **it_ped;
222  // account for distance covered by oncoming pedestrians
223  double relX2 = ped.myRelX - (ped.myDir == FORWARD ? 0 : stopTime * ped.myPerson->getVehicleType().getMaxSpeed());
224  if (ped.myRelX > minPos && (result.first == 0 || closest > relX2)) {
225  const double center = lane->getWidth() - (ped.myRelY + stripeWidth * 0.5);
226  const double halfWidth = 0.5 * ped.myPerson->getVehicleType().getWidth();
227  const bool overlap = (center + halfWidth > minRight && center - halfWidth < maxLeft);
228  if DEBUGCOND(ped) {
229  std::cout << " nextBlocking lane=" << lane->getID()
230  << " minPos=" << minPos << " minRight=" << minRight << " maxLeft=" << maxLeft
231  << " stopTime=" << stopTime
232  << " pedY=" << ped.myRelY
233  << " pedX=" << ped.myRelX
234  << " relX2=" << relX2
235  << " center=" << center
236  << " pedLeft=" << center + halfWidth
237  << " pedRight=" << center - halfWidth
238  << " overlap=" << overlap
239  << "\n";
240  }
241  if (overlap) {
242  closest = relX2;
243  result.first = ped.myPerson;
244  result.second = relX2 - minPos - (ped.myDir == FORWARD ? ped.myPerson->getVehicleType().getLength() : 0);
245  }
246  }
247  }
248  return result;
249 }
250 
251 
254  ActiveLanes::iterator it = myActiveLanes.find(lane);
255  if (it != myActiveLanes.end()) {
256  //std::cout << " found lane=" << lane->getID() << " n=" << it->second.size() << "\n";
257  return (it->second);
258  } else {
259  return noPedestrians;
260  }
261 }
262 
263 
264 void
266  for (ActiveLanes::iterator it_lane = myActiveLanes.begin(); it_lane != myActiveLanes.end(); ++it_lane) {
267  for (Pedestrians::iterator it_p = it_lane->second.begin(); it_p != it_lane->second.end(); ++it_p) {
268  delete *it_p;
269  }
270  }
271  myActiveLanes.clear();
273  myWalkingAreaPaths.clear(); // need to recompute when lane pointers change
274  myMinNextLengths.clear();
275 }
276 
277 
278 int
280  return (int)floor(lane->getWidth() / stripeWidth);
281 }
282 
283 int
285  if (from == 0 || to == 0) {
286  return UNDEFINED_DIRECTION;
287  } else if (MSLinkContHelper::getConnectingLink(*from, *to)) {
288  return FORWARD;
289  } else if (MSLinkContHelper::getConnectingLink(*to, *from)) {
290  return BACKWARD;
291  } else {
292  return UNDEFINED_DIRECTION;
293  }
294 }
295 
296 
297 void
299  if (myWalkingAreaPaths.size() > 0) {
300  return;
301  }
302  for (MSEdgeVector::const_iterator i = MSEdge::getAllEdges().begin(); i != MSEdge::getAllEdges().end(); ++i) {
303  const MSEdge* edge = *i;
304  if (edge->isWalkingArea()) {
305  const MSLane* walkingArea = getSidewalk<MSEdge, MSLane>(edge);
306  myMinNextLengths[walkingArea] = walkingArea->getLength();
307  // build all possible paths across this walkingArea
308  // gather all incident lanes
309  std::vector<const MSLane*> lanes;
310  const MSEdgeVector& incoming = edge->getPredecessors();
311  for (int j = 0; j < (int)incoming.size(); ++j) {
312  lanes.push_back(getSidewalk<MSEdge, MSLane>(incoming[j]));
313  }
314  const MSEdgeVector& outgoing = edge->getSuccessors();
315  for (int j = 0; j < (int)outgoing.size(); ++j) {
316  lanes.push_back(getSidewalk<MSEdge, MSLane>(outgoing[j]));
317  }
318  // build all combinations
319  for (int j = 0; j < (int)lanes.size(); ++j) {
320  for (int k = 0; k < (int)lanes.size(); ++k) {
321  if (j != k) {
322  // build the walkingArea
323  const MSLane* from = lanes[j];
324  const MSLane* to = lanes[k];
325  const int fromDir = MSLinkContHelper::getConnectingLink(*from, *walkingArea) != 0 ? FORWARD : BACKWARD;
326  const int toDir = MSLinkContHelper::getConnectingLink(*walkingArea, *to) != 0 ? FORWARD : BACKWARD;
327  PositionVector shape;
328  Position fromPos = from->getShape()[fromDir == FORWARD ? -1 : 0];
329  Position toPos = to->getShape()[toDir == FORWARD ? 0 : -1];
330  const double maxExtent = fromPos.distanceTo2D(toPos) / 4; // prevent sharp corners
331  const double extrapolateBy = MIN2(maxExtent, walkingArea->getWidth() / 2);
332  // assemble shape
333  shape.push_back(fromPos);
334  if (extrapolateBy > POSITION_EPS) {
335  PositionVector fromShp = from->getShape();
336  fromShp.extrapolate(extrapolateBy);
337  shape.push_back_noDoublePos(fromDir == FORWARD ? fromShp.back() : fromShp.front());
338  PositionVector nextShp = to->getShape();
339  nextShp.extrapolate(extrapolateBy);
340  shape.push_back_noDoublePos(toDir == FORWARD ? nextShp.front() : nextShp.back());
341  }
342  shape.push_back_noDoublePos(toPos);
343  if (shape.size() < 2) {
344  PositionVector fromShp = from->getShape();
345  fromShp.extrapolate(1.5 * POSITION_EPS); // noDoublePos requires a difference of POSITION_EPS in at least one coordinate
346  shape.push_back_noDoublePos(fromDir == FORWARD ? fromShp.back() : fromShp.front());
347  assert(shape.size() == 2);
348  }
349  if (fromDir == BACKWARD) {
350  // will be walking backward on walkingArea
351  shape = shape.reverse();
352  }
353  WalkingAreaPath wap = WalkingAreaPath(from, walkingArea, to, shape);
354  myWalkingAreaPaths[std::make_pair(from, to)] = wap;
355  myMinNextLengths[walkingArea] = MIN2(myMinNextLengths[walkingArea], wap.length);
356  }
357  }
358  }
359  }
360  }
361 }
362 
363 
366  assert(walkingArea->isWalkingArea());
367  std::vector<const MSLane*> lanes;
368  const MSEdgeVector& incoming = walkingArea->getPredecessors();
369  for (int j = 0; j < (int)incoming.size(); ++j) {
370  lanes.push_back(getSidewalk<MSEdge, MSLane>(incoming[j]));
371  }
372  const MSEdgeVector& outgoing = walkingArea->getSuccessors();
373  for (int j = 0; j < (int)outgoing.size(); ++j) {
374  lanes.push_back(getSidewalk<MSEdge, MSLane>(outgoing[j]));
375  }
376  if (lanes.size() < 1) {
377  throw ProcessError("Invalid walkingarea '" + walkingArea->getID() + "' does not allow continuation.");
378  }
379  return &myWalkingAreaPaths[std::make_pair(lanes.front(), lanes.back())];
380 }
381 
382 
384 MSPModel_Striping::getNextLane(const PState& ped, const MSLane* currentLane, const MSLane* prevLane) {
385  const MSEdge* currentEdge = &currentLane->getEdge();
386  const MSJunction* junction = ped.myDir == FORWARD ? currentEdge->getToJunction() : currentEdge->getFromJunction();
387  const MSEdge* nextRouteEdge = ped.myStage->getNextRouteEdge();
388  const MSLane* nextRouteLane = getSidewalk<MSEdge, MSLane>(nextRouteEdge);
389  // result values
390  const MSLane* nextLane = nextRouteLane;
391  MSLink* link = 0;
392  int nextDir = UNDEFINED_DIRECTION;
393 
394  if (nextRouteLane == 0 && nextRouteEdge != 0) {
395  std::string error = "Person '" + ped.myPerson->getID() + "' could not find sidewalk on edge '" + nextRouteEdge->getID() + "', time="
396  + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".";
397  if (OptionsCont::getOptions().getBool("ignore-route-errors")) {
398  WRITE_WARNING(error);
399  nextRouteLane = nextRouteEdge->getLanes().front();
400  } else {
401  throw ProcessError(error);
402  }
403  }
404 
405  if (nextRouteLane != 0) {
406  if (currentEdge->isInternal()) {
407  assert(junction == currentEdge->getFromJunction());
408  nextDir = junction == nextRouteEdge->getFromJunction() ? FORWARD : BACKWARD;
409  if DEBUGCOND(ped) {
410  std::cout << " internal\n";
411  }
412  } else if (currentEdge->isCrossing()) {
413  nextDir = ped.myDir;
414  if (ped.myDir == FORWARD) {
415  nextLane = currentLane->getLinkCont()[0]->getLane();
416  } else {
417  nextLane = currentLane->getLogicalPredecessorLane();
418  }
419  if DEBUGCOND(ped) {
420  std::cout << " crossing\n";
421  }
422  } else if (currentEdge->isWalkingArea()) {
423  ConstMSEdgeVector crossingRoute;
424  // departPos can be 0 because the direction of the walkingArea does not matter
425  // for the arrivalPos, we need to make sure that the route does not deviate across other junctions
426  const int nextRouteEdgeDir = nextRouteEdge->getFromJunction() == junction ? FORWARD : BACKWARD;
427  const double arrivalPos = (nextRouteEdge == ped.myStage->getRoute().back()
428  ? ped.myStage->getArrivalPos()
429  : (nextRouteEdgeDir == FORWARD ? 0 : nextRouteEdge->getLength()));
430  MSEdgeVector prohibited;
431  if (prevLane != 0) {
432  prohibited.push_back(&prevLane->getEdge());
433  }
434  MSNet::getInstance()->getPedestrianRouter(prohibited).compute(currentEdge, nextRouteEdge, 0, arrivalPos, ped.myStage->getMaxSpeed(ped.myPerson), 0, junction, crossingRoute, true);
435  if DEBUGCOND(ped) {
436  std::cout
437  << " nre=" << nextRouteEdge->getID()
438  << " nreDir=" << nextRouteEdgeDir
439  << " aPos=" << arrivalPos
440  << " crossingRoute=" << toString(crossingRoute)
441  << "\n";
442  }
443  if (crossingRoute.size() > 1) {
444  const MSEdge* nextEdge = crossingRoute[1];
445  nextLane = getSidewalk<MSEdge, MSLane>(crossingRoute[1]);
446  assert((nextEdge->getFromJunction() == junction || nextEdge->getToJunction() == junction));
447  assert(nextLane != prevLane);
448  nextDir = connectedDirection(currentLane, nextLane);
449  if DEBUGCOND(ped) {
450  std::cout << " nextDir=" << nextDir << "\n";
451  }
452  assert(nextDir != UNDEFINED_DIRECTION);
453  if (nextDir == FORWARD) {
454  link = MSLinkContHelper::getConnectingLink(*currentLane, *nextLane);
455  } else if (nextEdge->isCrossing()) {
456  const MSLane* oppositeWalkingArea = nextLane->getLogicalPredecessorLane();
457  link = MSLinkContHelper::getConnectingLink(*oppositeWalkingArea, *nextLane);
458  } else {
459  link = MSLinkContHelper::getConnectingLink(*nextLane, *currentLane);
460  }
461  assert(link != 0);
462  } else {
463  if DEBUGCOND(ped) {
464  std::cout << SIMTIME
465  << " no route from '" << (currentEdge == 0 ? "NULL" : currentEdge->getID())
466  << "' to '" << (nextRouteEdge == 0 ? "NULL" : nextRouteEdge->getID())
467  << "\n";
468  }
469  WRITE_WARNING("Person '" + ped.myPerson->getID() + "' could not find route across junction '" + junction->getID() + "', time=" +
470  time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
471  // error indicated by nextDir == UNDEFINED_DIRECTION
472  }
473  } else if (currentEdge == nextRouteEdge) {
474  // strange loop in this route. No need to use walkingArea
475  nextDir = -ped.myDir;
476  } else {
477  // normal edge. by default use next / previous walking area
478  nextDir = ped.myDir;
479  nextLane = getNextWalkingArea(currentLane, ped.myDir, link);
480  if (nextLane != 0) {
481  // walking area found
482  if DEBUGCOND(ped) {
483  std::cout << " next walkingArea " << (nextDir == FORWARD ? "forward" : "backward") << "\n";
484  }
485  } else {
486  // walk forward by default
487  nextDir = junction == nextRouteEdge->getToJunction() ? BACKWARD : FORWARD;
488  // try to use a direct link as fallback
489  // direct links only exist if built explicitly. They are used to model tl-controlled links if there are no crossings
490  if (ped.myDir == FORWARD) {
491  link = MSLinkContHelper::getConnectingLink(*currentLane, *nextRouteLane);
492  if (link != 0) {
493  if DEBUGCOND(ped) {
494  std::cout << " direct forward\n";
495  }
496  nextLane = MSLinkContHelper::getInternalFollowingLane(currentLane, nextRouteLane);
497  }
498  } else {
499  link = MSLinkContHelper::getConnectingLink(*nextRouteLane, *currentLane);
500  if (link != 0) {
501  if DEBUGCOND(ped) {
502  std::cout << " direct backward\n";
503  }
504  nextLane = MSLinkContHelper::getInternalFollowingLane(nextRouteLane, currentLane);
505  }
506  }
507  }
508  if (nextLane == 0) {
509  // no internal lane found
510  nextLane = nextRouteLane;
511  if DEBUGCOND(ped) {
512  std::cout << SIMTIME << " no next lane found for " << currentLane->getID() << " dir=" << ped.myDir << "\n";
513  }
514  } else if (nextLane->getLength() <= POSITION_EPS) {
515  // internal lane too short
516  nextLane = nextRouteLane;
517  }
518  }
519  }
520  if DEBUGCOND(ped) {
521  std::cout << SIMTIME
522  << " p=" << ped.myPerson->getID()
523  << " l=" << currentLane->getID()
524  << " nl=" << (nextLane == 0 ? "NULL" : nextLane->getID())
525  << " nrl=" << (nextRouteLane == 0 ? "NULL" : nextRouteLane->getID())
526  << " d=" << nextDir
527  << " link=" << (link == 0 ? "NULL" : link->getViaLaneOrLane()->getID())
528  << " pedDir=" << ped.myDir
529  << "\n";
530  }
531  return NextLaneInfo(nextLane, link, nextDir);
532 }
533 
534 
535 const MSLane*
536 MSPModel_Striping::getNextWalkingArea(const MSLane* currentLane, const int dir, MSLink*& link) {
537  if (dir == FORWARD) {
538  const MSLinkCont& links = currentLane->getLinkCont();
539  for (MSLinkCont::const_iterator it = links.begin(); it != links.end(); ++it) {
540  if ((*it)->getLane()->getEdge().isWalkingArea()) {
541  link = *it;
542  return (*it)->getLane();
543  }
544  }
545  } else {
546  const std::vector<MSLane::IncomingLaneInfo>& laneInfos = currentLane->getIncomingLanes();
547  for (std::vector<MSLane::IncomingLaneInfo>::const_iterator it = laneInfos.begin(); it != laneInfos.end(); ++it) {
548  if ((*it).lane->getEdge().isWalkingArea()) {
549  link = (*it).viaLink;
550  return (*it).lane;
551  }
552  }
553  }
554  return 0;
555 }
556 
557 
559 MSPModel_Striping::getNeighboringObstacles(const Pedestrians& pedestrians, int egoIndex, int stripes) {
560  const PState& ego = *pedestrians[egoIndex];
561  Obstacles obs(stripes, Obstacle(ego.myDir));
562  std::vector<bool> haveBlocker(stripes, false);
563  for (int index = egoIndex + 1; index < (int)pedestrians.size(); index++) {
564  const PState& p = *pedestrians[index];
565  if DEBUGCOND(ego) {
566  std::cout << SIMTIME << " ped=" << ego.myPerson->getID() << " checking neighbor " << p.myPerson->getID();
567  }
568  if (!p.myWaitingToEnter) {
569  const Obstacle o(p);
570  if DEBUGCOND(ego) {
571  std::cout << " dist=" << ego.distanceTo(o) << std::endl;
572  }
573  if (ego.distanceTo(o) == DIST_BEHIND) {
574  break;
575  }
576  if (ego.distanceTo(o) == DIST_OVERLAP) {
577  obs[p.stripe()] = o;
578  obs[p.otherStripe()] = o;
579  haveBlocker[p.stripe()] = true;
580  haveBlocker[p.otherStripe()] = true;
581  }
582  if (!haveBlocker[p.stripe()]) {
583  obs[p.stripe()] = o;
584  }
585  if (!haveBlocker[p.otherStripe()]) {
586  obs[p.otherStripe()] = o;
587  }
588  }
589  }
590  if DEBUGCOND(ego) {
591  std::cout << SIMTIME << " ped=" << ego.myPerson->getID() << " neighObs=";
592  DEBUG_PRINT(obs);
593  }
594  return obs;
595 }
596 
597 
598 int
599 MSPModel_Striping::getStripeOffset(int origStripes, int destStripes, bool addRemainder) {
600  int offset = (destStripes - origStripes) / 2;
601  if (addRemainder) {
602  offset += (destStripes - origStripes) % 2;
603  }
604  return offset;
605 }
606 
607 
610  MSLane* lane, const MSLane* nextLane, int stripes, int nextDir,
611  double currentLength, int currentDir) {
612  if (nextLanesObs.count(nextLane) == 0) {
613  const double nextLength = nextLane->getEdge().isWalkingArea() ? myMinNextLengths[nextLane] : nextLane->getLength();
614  // figure out the which pedestrians are ahead on the next lane
615  const int nextStripes = numStripes(nextLane);
616  // do not move past the end of the next lane in a single step
617  Obstacles obs(stripes, Obstacle(nextDir == FORWARD ? nextLength : 0, 0, OBSTACLE_NEXTEND, "nextEnd", 0));
618 
619  const int offset = getStripeOffset(nextStripes, stripes, currentDir != nextDir && nextStripes > stripes);
620  //std::cout << SIMTIME << " getNextLaneObstacles"
621  // << " nextLane=" << nextLane->getID()
622  // << " nextLength=" << nextLength
623  // << " nextDir=" << nextDir
624  // << " currentLength=" << currentLength
625  // << " currentDir=" << currentDir
626  // << " stripes=" << stripes
627  // << " nextStripes=" << nextStripes
628  // << " offset=" << offset
629  // << "\n";
630  if (nextStripes < stripes) {
631  // some stripes do not continue
632  for (int ii = 0; ii < stripes; ++ii) {
633  if (ii < offset || ii >= nextStripes + offset) {
634  obs[ii] = Obstacle(nextDir == FORWARD ? 0 : nextLength, 0, OBSTACLE_END, "stripeEnd", 0);
635  }
636  }
637  }
638  Pedestrians& pedestrians = getPedestrians(nextLane);
639  if (nextLane->getEdge().isWalkingArea()) {
640  transformToCurrentLanePositions(obs, currentDir, nextDir, currentLength, nextLength);
641  // complex transformation into the coordinate system of the current lane
642  // (pedestrians on next lane may walk at arbitrary angles relative to the current lane)
643  double lateral_offset = (lane->getWidth() - stripeWidth) * 0.5;
644  if ((stripes - nextStripes) % 2 != 0) {
645  lateral_offset += 0.5 * stripeWidth;
646  }
647  nextDir = currentDir;
648  // transform pedestrians into the current coordinate system
649  for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
650  PState& p = *pedestrians[ii];
651  if (p.myWaitingToEnter || p.myAmJammed) {
652  continue;
653  }
654  Position relPos = lane->getShape().transformToVectorCoordinates(p.getPosition(*p.myStage, -1), true);
655  const double newY = relPos.y() + lateral_offset;
656  //if (p.myPerson->getID() == "ped200") std::cout << " ped=" << p.myPerson->getID() << " relX=" << relPos.x() << " relY=" << newY << " latOff=" << lateral_offset << " s=" << p.stripe(newY) << " os=" << p.otherStripe(newY) << "\n";
657  if ((currentDir == FORWARD && relPos.x() >= lane->getLength()) || (currentDir == BACKWARD && relPos.x() < 0)) {
658  addCloserObstacle(obs, relPos.x(), p.stripe(newY), stripes, p.myPerson->getID(), p.myPerson->getVehicleType().getWidth(), currentDir, OBSTACLE_PED);
659  addCloserObstacle(obs, relPos.x(), p.otherStripe(newY), stripes, p.myPerson->getID(), p.myPerson->getVehicleType().getWidth(), currentDir, OBSTACLE_PED);
660  }
661  }
662  } else {
663  // simple transformation into the coordinate system of the current lane
664  // (only need to worry about currentDir and nextDir)
665  // XXX consider waitingToEnter on nextLane
666  sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(nextDir));
667  for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
668  const PState& p = *pedestrians[ii];
669  if (p.myWaitingToEnter || p.myAmJammed) {
670  continue;
671  }
672  double newY = p.myRelY;
673  Obstacle pObs(p);
674  if (nextDir != currentDir) {
675  newY = (nextStripes - 1) * stripeWidth - newY;
676  pObs.speed *= -1;
677  }
678  newY += offset * stripeWidth;
679  const int stripe = p.stripe(newY);
680  if (stripe >= 0 && stripe < stripes) {
681  obs[stripe] = pObs;
682  }
683  const int otherStripe = p.otherStripe(newY);
684  if (otherStripe >= 0 && otherStripe < stripes) {
685  obs[otherStripe] = pObs;
686  }
687  }
688  if (nextLane->getEdge().isCrossing()) {
689  // add vehicle obstacles
690  addCrossingVehs(nextLane, stripes, offset, nextDir, obs);
691  }
692  transformToCurrentLanePositions(obs, currentDir, nextDir, currentLength, nextLength);
693  }
694  nextLanesObs[nextLane] = obs;
695  }
696  return nextLanesObs[nextLane];
697 }
698 
699 void
700 MSPModel_Striping::transformToCurrentLanePositions(Obstacles& obs, int currentDir, int nextDir, double currentLength, double nextLength) {
701  for (int ii = 0; ii < (int)obs.size(); ++ii) {
702  Obstacle& o = obs[ii];
703  if (currentDir == FORWARD) {
704  if (nextDir == FORWARD) {
705  o.xFwd += currentLength;
706  o.xBack += currentLength;
707  } else {
708  const double tmp = o.xFwd;
709  o.xFwd = currentLength + nextLength - o.xBack;
710  o.xBack = currentLength + nextLength - tmp;
711  }
712  } else {
713  if (nextDir == FORWARD) {
714  const double tmp = o.xFwd;
715  o.xFwd = -o.xBack;
716  o.xBack = -tmp;
717  } else {
718  o.xFwd -= nextLength;
719  o.xBack -= nextLength;
720  }
721  }
722  }
723 }
724 
725 
726 void
727 MSPModel_Striping::addCloserObstacle(Obstacles& obs, double x, int stripe, int numStripes, const std::string& id, double width, int dir, ObstacleType type) {
728  if (stripe >= 0 && stripe < numStripes) {
729  if ((dir == FORWARD && x - width / 2. < obs[stripe].xBack) || (dir == BACKWARD && x + width / 2. > obs[stripe].xFwd)) {
730  obs[stripe] = Obstacle(x, 0, type, id, width);
731  }
732  }
733 }
734 
735 void
736 MSPModel_Striping::moveInDirection(SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir) {
737  for (ActiveLanes::iterator it_lane = myActiveLanes.begin(); it_lane != myActiveLanes.end(); ++it_lane) {
738  const MSLane* lane = it_lane->first;
739  Pedestrians& pedestrians = it_lane->second;
740  if (pedestrians.size() == 0) {
741  continue;
742  }
743  //std::cout << SIMTIME << ">>> lane=" << lane->getID() << " numPeds=" << pedestrians.size() << "\n";
744  if (lane->getEdge().isWalkingArea()) {
745  const double lateral_offset = (lane->getWidth() - stripeWidth) * 0.5;
746  const double minY = stripeWidth * - 0.5 + NUMERICAL_EPS;
747  const double maxY = stripeWidth * (numStripes(lane) - 0.5) - NUMERICAL_EPS;
748  const WalkingAreaPath* debugPath = 0;
749  // need to handle each walkingAreaPath seperately and transform
750  // coordinates beforehand
751  std::set<const WalkingAreaPath*, walkingarea_path_sorter> paths;
752  for (Pedestrians::iterator it = pedestrians.begin(); it != pedestrians.end(); ++it) {
753  const PState* p = *it;
754  assert(p->myWalkingAreaPath != 0);
755  if (p->myDir == dir) {
756  paths.insert(p->myWalkingAreaPath);
757  if DEBUGCOND(*p) {
758  debugPath = p->myWalkingAreaPath;
759  std::cout << SIMTIME << " debugging WalkingAreaPath from=" << debugPath->from->getID() << " to=" << debugPath->to->getID() << "\n";
760  }
761  }
762  }
763  for (std::set<const WalkingAreaPath*, walkingarea_path_sorter>::iterator it = paths.begin(); it != paths.end(); ++it) {
764  const WalkingAreaPath* path = *it;
765  Pedestrians toDelete;
766  Pedestrians transformedPeds;
767  transformedPeds.reserve(pedestrians.size());
768  for (Pedestrians::iterator it_p = pedestrians.begin(); it_p != pedestrians.end(); ++it_p) {
769  PState* p = *it_p;
770  if (p->myWalkingAreaPath == path
771  // opposite direction is already in the correct coordinate system
772  || (p->myWalkingAreaPath->from == path->to && p->myWalkingAreaPath->to == path->from)) {
773  transformedPeds.push_back(p);
774  if (path == debugPath) std::cout << " ped=" << p->myPerson->getID() << " relX=" << p->myRelX << " relY=" << p->myRelY << " (untransformed), vecCoord="
775  << path->shape.transformToVectorCoordinates(p->getPosition(*p->myStage, -1)) << "\n";
776  } else {
777  const Position relPos = path->shape.transformToVectorCoordinates(p->getPosition(*p->myStage, -1));
778  const double newY = relPos.y() + lateral_offset;
779  if (relPos != Position::INVALID && newY >= minY && newY <= maxY) {
780  PState* tp = new PState(*p);
781  tp->myRelX = relPos.x();
782  tp->myRelY = newY;
783  // only an obstacle, speed may be orthogonal to dir
784  tp->myDir = !dir;
785  tp->mySpeed = 0;
786  toDelete.push_back(tp);
787  transformedPeds.push_back(tp);
788  if (path == debugPath) {
789  std::cout << " ped=" << p->myPerson->getID() << " relX=" << relPos.x() << " relY=" << newY << " (transformed), vecCoord=" << relPos << "\n";
790  }
791  } else {
792  if (path == debugPath) {
793  std::cout << " ped=" << p->myPerson->getID() << " relX=" << relPos.x() << " relY=" << newY << " (invalid), vecCoord=" << relPos << "\n";
794  }
795  }
796  }
797  }
798  moveInDirectionOnLane(transformedPeds, lane, currentTime, changedLane, dir);
799  arriveAndAdvance(pedestrians, currentTime, changedLane, dir);
800  // clean up
801  for (Pedestrians::iterator it_p = toDelete.begin(); it_p != toDelete.end(); ++it_p) {
802  delete *it_p;
803  }
804  }
805  } else {
806  moveInDirectionOnLane(pedestrians, lane, currentTime, changedLane, dir);
807  arriveAndAdvance(pedestrians, currentTime, changedLane, dir);
808  }
809  }
810 }
811 
812 
813 void
814 MSPModel_Striping::arriveAndAdvance(Pedestrians& pedestrians, SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir) {
815  // advance to the next lane / arrive at destination
816  sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(dir));
817  // can't use iterators because we do concurrent modification
818  for (int i = 0; i < (int)pedestrians.size(); i++) {
819  PState* const p = pedestrians[i];
820  if (p->myDir == dir && p->distToLaneEnd() < 0) {
821  // moveToNextLane may trigger re-insertion (for consecutive
822  // walks) so erase must be called first
823  pedestrians.erase(pedestrians.begin() + i);
824  i--;
825  p->moveToNextLane(currentTime);
826  if (p->myLane != 0) {
827  changedLane.insert(p->myPerson);
828  myActiveLanes[p->myLane].push_back(p);
829  } else {
830  delete p;
832  }
833  }
834  }
835 }
836 
837 
838 void
839 MSPModel_Striping::moveInDirectionOnLane(Pedestrians& pedestrians, const MSLane* lane, SUMOTime currentTime, std::set<MSPerson*>& changedLane, int dir) {
840  const int stripes = numStripes(lane);
841  //std::cout << " laneWidth=" << lane->getWidth() << " stripeWidth=" << stripeWidth << " stripes=" << stripes << "\n";
842  Obstacles obs(stripes, Obstacle(dir)); // continously updated
843  NextLanesObstacles nextLanesObs; // continously updated
844  sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(dir));
845 
846  Obstacles crossingVehs(stripes, Obstacle(dir));
847  bool hasCrossingVehObs = false;
848  if (lane->getEdge().isCrossing()) {
849  hasCrossingVehObs = addCrossingVehs(lane, stripes, 0, dir, crossingVehs);
850  }
851 
852  for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
853  PState& p = *pedestrians[ii];
854  //std::cout << SIMTIME << "CHECKING" << p.myPerson->getID() << "\n";
855  Obstacles currentObs = obs;
856  if (p.myDir != dir || changedLane.count(p.myPerson) != 0) {
857  if (!p.myWaitingToEnter) {
858  //if DEBUGCOND(p) {
859  // std::cout << " obs=" << p.myPerson->getID() << " y=" << p.myRelY << " stripe=" << p.stripe() << " oStripe=" << p.otherStripe() << "\n";
860  //}
861  Obstacle o(p);
862  if (p.myDir != dir && p.mySpeed == 0) {
863  // ensure recognition of oncoming
864  o.speed = (p.myDir == FORWARD ? 0.1 : -0.1);
865  }
866  obs[p.stripe()] = o;
867  obs[p.otherStripe()] = o;
868  }
869  continue;
870  }
871  if DEBUGCOND(p) {
872  std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " currentObs=";
873  gDebugFlag1 = true;
874  DEBUG_PRINT(currentObs);
875  }
876  const MSLane* nextLane = p.myNLI.lane;
877  const MSLink* link = p.myNLI.link;
878  const double dist = p.distToLaneEnd();
879  const double speed = p.myStage->getMaxSpeed(p.myPerson);
880  if (nextLane != 0 && dist <= LOOKAHEAD_ONCOMING) {
881  const double currentLength = (p.myWalkingAreaPath == 0 ? lane->getLength() : p.myWalkingAreaPath->length);
882  const Obstacles& nextObs = getNextLaneObstacles(
883  nextLanesObs, lane, nextLane, stripes,
884  p.myNLI.dir, currentLength, dir);
885 
886  if DEBUGCOND(p) {
887  std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " nextObs=";
888  DEBUG_PRINT(nextObs);
889  }
890  p.mergeObstacles(currentObs, nextObs);
891  }
892  if DEBUGCOND(p) {
893  std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWithNext=";
894  DEBUG_PRINT(currentObs);
895  }
896  p.mergeObstacles(currentObs, getNeighboringObstacles(pedestrians, ii, stripes));
897  if DEBUGCOND(p) {
898  std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWithNeigh=";
899  DEBUG_PRINT(currentObs);
900  }
901  // check link state
902  gDebugFlag1 = DEBUGCOND(p); // see MSLink_DEBUG_OPENED
903  if (link != 0
904  // only check close before junction, @todo we should take deceleration into account here
905  && dist - p.getMinGap() < LOOKAHEAD_SAMEDIR * speed
906  && !link->opened(currentTime, speed, speed, p.getLength(), p.getImpatience(currentTime), speed, 0, 0, 0, p.ignoreRed(link))) {
907  // prevent movement passed a closed link
908  Obstacles closedLink(stripes, Obstacle(p.myRelX + dir * (dist + NUMERICAL_EPS), 0, OBSTACLE_LINKCLOSED, "closedLink_" + link->getViaLaneOrLane()->getID(), 0));
909  p.mergeObstacles(currentObs, closedLink);
910  if DEBUGCOND(p) {
911  std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWitTLS=";
912  DEBUG_PRINT(currentObs);
913  }
914  // consider rerouting over another crossing
915  if (p.myWalkingAreaPath != 0) {
916  // @todo actually another path would be needed starting at the current position
918  }
919  }
920  gDebugFlag1 = false;
921  if (&lane->getEdge() == &p.myStage->getDestination() && p.myStage->getDestinationStop() != 0) {
922  Obstacles arrival(stripes, Obstacle(p.myStage->getArrivalPos() + dir * p.getMinGap(), 0, OBSTACLE_ARRIVALPOS, "arrival", 0));
923  p.mergeObstacles(currentObs, arrival);
924  }
925 
926  if (lane->getVehicleNumberWithPartials() > 0) {
927  // react to vehicles on the same lane
928  // @todo: improve efficiency by using the same iterator for all pedestrians on this lane
929  Obstacles vehObs(stripes, Obstacle(dir));
930  const int current = p.stripe();
932  MSLane::AnyVehicleIterator end = (dir == FORWARD ? lane->anyVehiclesUpstreamEnd() : lane->anyVehiclesEnd());
933  for (MSLane::AnyVehicleIterator it = begin; it != end; ++it) {
934  const MSVehicle* veh = *it;
935  const double vehBack = veh->getBackPositionOnLane(lane);
936  const double vehFront = vehBack + veh->getVehicleType().getLength();
937  // ensure that vehicles are not blocked
938  const double vehNextSpeed = MAX2(veh->getSpeed(), 1.0);
939  const double clearance = SAFETY_GAP + vehNextSpeed * LOOKAHEAD_SAMEDIR;
940  if ((dir == FORWARD && vehFront + clearance > p.getMinX() && vehBack <= p.getMaxX() + LOOKAHEAD_SAMEDIR)
941  || (dir == BACKWARD && vehBack < p.getMaxX() && vehFront >= p.getMinX() - LOOKAROUND_VEHICLES)) {
942  Obstacle vo(vehBack, veh->getSpeed(), OBSTACLE_VEHICLE, veh->getID(), 0);
943  // moving vehicles block space along their path
944  vo.xFwd += veh->getVehicleType().getLength() + clearance;
945  vo.xBack -= SAFETY_GAP;
946  // relY increases from left to right (the other way around from vehicles)
947  // XXX lateral offset for partial vehicles
948  const double vehYmax = 0.5 * (lane->getWidth() + veh->getVehicleType().getWidth() - stripeWidth) - veh->getLateralPositionOnLane();
949  const double vehYmin = vehYmax - veh->getVehicleType().getWidth();
950  for (int s = MAX2(0, p.stripe(vehYmin)); s < MIN2(p.stripe(vehYmax) + 1, stripes); ++s) {
951  vehObs[s] = vo;
952  if (s == current && vehFront + SAFETY_GAP < p.getMinX()) {
953  // ignore if aleady overlapping while vehicle is still behind
954  if (p.myRelY - p.myPerson->getVehicleType().getWidth() < vehYmax &&
955  p.myRelY + p.myPerson->getVehicleType().getWidth() > vehYmin && dir == FORWARD) {
956  if DEBUGCOND(p) {
957  std::cout << " ignoring vehicle on stripe " << s << "\n";
958  }
959  if (dir == FORWARD) {
960  vehObs[s] = Obstacle(dir);
961  } else {
962  vehObs[s].xFwd = MIN2(vo.xFwd, vehFront + SAFETY_GAP);
963  }
964  }
965  }
966  }
967  if DEBUGCOND(p) {
968  std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " veh=" << veh->getID() << " obstacle on lane=" << lane->getID()
969  << "\n"
970  << " ymin=" << vehYmin
971  << " ymax=" << vehYmax
972  << " smin=" << PState::stripe(vehYmin)
973  << " smax=" << PState::stripe(vehYmax)
974  << " relY=" << p.myRelY
975  << " current=" << current
976  << "\n";
977  }
978  }
979  }
980  p.mergeObstacles(currentObs, vehObs);
981  if DEBUGCOND(p) {
982  std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWithVehs=";
983  DEBUG_PRINT(currentObs);
984  }
985  }
986  if (hasCrossingVehObs) {
987  p.mergeObstacles(currentObs, crossingVehs);
988  if DEBUGCOND(p) {
989  std::cout << SIMTIME << " ped=" << p.myPerson->getID() << " obsWithVehs2=";
990  DEBUG_PRINT(currentObs);
991  }
992  }
993 
994  // walk, taking into account all obstacles
995  p.walk(currentObs, currentTime);
996  gDebugFlag1 = false;
997  if (!p.myWaitingToEnter && !p.myAmJammed) {
998  Obstacle o(p);
999  obs[p.stripe()] = o;
1000  obs[p.otherStripe()] = o;
1002  for (int coll = 0; coll < ii; ++coll) {
1003  PState& c = *pedestrians[coll];
1004  if (!c.myWaitingToEnter && c.myWalkingAreaPath == 0 && !c.myAmJammed) {
1005  if (c.stripe() == p.stripe() || p.stripe() == c.otherStripe() || p.otherStripe() == c.stripe() || p.otherStripe() == c.otherStripe()) {
1006  Obstacle cObs(c);
1007  // we check only for real collisions, no min gap violations
1008  if (p.distanceTo(cObs, false) == DIST_OVERLAP) {
1009  WRITE_WARNING("Collision of person '" + p.myPerson->getID() + "' and person '" + c.myPerson->getID()
1010  + "', lane='" + lane->getID() + "', time=" + time2string(currentTime) + ".");
1011  }
1012  }
1013  }
1014  }
1015  }
1016  }
1017  //std::cout << SIMTIME << p.myPerson->getID() << " lane=" << lane->getID() << " x=" << p.myRelX << "\n";
1018  }
1019 }
1020 
1021 bool
1022 MSPModel_Striping::addCrossingVehs(const MSLane* crossing, int stripes, double lateral_offset, int dir, Obstacles& obs) {
1023  bool hasCrossingVehObs = false;
1024  const MSLink::LinkLeaders linkLeaders = crossing->getLinkCont().front()->getLeaderInfo(0, crossing->getLength());
1025  if (linkLeaders.size() > 0) {
1026  for (MSLink::LinkLeaders::const_iterator it = linkLeaders.begin(); it != linkLeaders.end(); ++it) {
1027  // the vehicle to enter the junction first has priority
1028  const MSVehicle* veh = (*it).vehAndGap.first;
1029  if (veh != 0) {
1030  Obstacle vo((*it).distToCrossing, 0, OBSTACLE_VEHICLE, veh->getID(), veh->getVehicleType().getWidth() + 2 * MINGAP_TO_VEHICLE);
1031  // relY increases from left to right (the other way around from vehicles)
1032  const double bGap = veh->getCarFollowModel().brakeGap(veh->getSpeed(), veh->getCarFollowModel().getMaxDecel(), 0);
1033  double vehYmin;
1034  double vehYmax;
1035  if ((*it).fromLeft) {
1036  vehYmin = -(*it).vehAndGap.second + lateral_offset; // vehicle back
1037  vehYmax = vehYmin + veh->getVehicleType().getLength() + bGap + MINGAP_TO_VEHICLE;
1038  vehYmin -= MINGAP_TO_VEHICLE;
1039  } else {
1040  vehYmax = crossing->getWidth() + (*it).vehAndGap.second - lateral_offset; // vehicle back
1041  vehYmin = vehYmax - veh->getVehicleType().getLength() - bGap - MINGAP_TO_VEHICLE;
1042  vehYmax += MINGAP_TO_VEHICLE;
1043 
1044  }
1045  for (int s = MAX2(0, PState::stripe(vehYmin)); s < MIN2(PState::stripe(vehYmax), stripes); ++s) {
1046  if ((dir == FORWARD && obs[s].xBack > vo.xBack)
1047  || (dir == BACKWARD && obs[s].xFwd < vo.xFwd)) {
1048  obs[s] = vo;
1049  hasCrossingVehObs = true;
1050  }
1051  }
1052  if (DEBUGCOND2(crossing)) {
1053  std::cout << SIMTIME
1054  << " crossingVeh=" << veh->getID()
1055  << " lane=" << crossing->getID()
1056  << " latOffset=" << lateral_offset
1057  << " dir=" << dir
1058  << " stripes=" << stripes
1059  << " dist=" << (*it).distToCrossing
1060  << " gap=" << (*it).vehAndGap.second
1061  << " brakeGap=" << bGap
1062  << " fromLeft=" << (*it).fromLeft
1063  << " ymin=" << vehYmin
1064  << " ymax=" << vehYmax
1065  << " smin=" << PState::stripe(vehYmin)
1066  << " smax=" << PState::stripe(vehYmax)
1067  << "\n";
1068  DEBUG_PRINT(obs);
1069  }
1070  }
1071  }
1072  }
1073  return hasCrossingVehObs;
1074 }
1075 
1076 // ===========================================================================
1077 // MSPModel_Striping::Obstacle method definitions
1078 // ===========================================================================
1080  xFwd(dir * dist), // by default, far away when seen in dir
1081  xBack(dir * dist), // by default, far away when seen in dir
1082  speed(0),
1083  type(OBSTACLE_NONE),
1084  description("") {
1085 }
1086 
1087 
1089  xFwd(ped.getMaxX()),
1090  xBack(ped.getMinX()),
1091  speed(ped.myDir * ped.mySpeed),
1092  type(OBSTACLE_PED),
1093  description(ped.myPerson->getID()) {
1094  assert(!ped.myWaitingToEnter);
1095 }
1096 
1097 
1098 // ===========================================================================
1099 // MSPModel_Striping::PState method definitions
1100 // ===========================================================================
1101 
1102 
1104  myPerson(person),
1105  myStage(stage),
1106  myLane(lane),
1107  myRelX(stage->getDepartPos()),
1108  myRelY(stage->getDepartPosLat()),
1109  myDir(FORWARD),
1110  mySpeed(0),
1111  myWaitingToEnter(true),
1112  myWaitingTime(0),
1113  myWalkingAreaPath(0),
1114  myAmJammed(false) {
1115  const MSEdge* currentEdge = &lane->getEdge();
1116  const ConstMSEdgeVector& route = myStage->getRoute();
1117  assert(!route.empty());
1118  if (route.size() == 1) {
1119  // only a single edge, move towards end pos
1121  } else if (route.front()->getFunction() != EDGEFUNC_NORMAL) {
1122  // start on an intersection
1123  myDir = FORWARD;
1124  if (route.front()->isWalkingArea()) {
1125  myWalkingAreaPath = getArbitraryPath(route.front());
1126  }
1127  } else {
1128  const bool mayStartForward = canTraverse(FORWARD, route);
1129  const bool mayStartBackward = canTraverse(BACKWARD, route);
1130  if DEBUGCOND(*this) {
1131  std::cout << " initialize dir for " << myPerson->getID() << " forward=" << mayStartForward << " backward=" << mayStartBackward << "\n";
1132  }
1133  if (mayStartForward && mayStartBackward) {
1134  // figure out the best direction via routing
1135  ConstMSEdgeVector crossingRoute;
1136  MSNet::getInstance()->getPedestrianRouter().compute(currentEdge, route.back(), myRelX, myStage->getArrivalPos(), myStage->getMaxSpeed(person), 0, 0, crossingRoute, true);
1137  if (crossingRoute.size() > 1) {
1138  // route found
1139  const MSEdge* nextEdge = crossingRoute[1];
1140  if (nextEdge->getFromJunction() == currentEdge->getFromJunction() || nextEdge->getToJunction() == currentEdge->getFromJunction()) {
1141  myDir = BACKWARD;
1142  }
1143  }
1144  if DEBUGCOND(*this) {
1145  std::cout << " crossingRoute=" << toString(crossingRoute) << "\n";
1146  }
1147  } else {
1148  myDir = !mayStartBackward ? FORWARD : BACKWARD;
1149  }
1150  }
1151  if (lane->getVehicleNumberWithPartials() > 0 && myRelY == 0) {
1152  // better start next to the road if nothing was specified
1153  myRelY -= stripeWidth;
1154  }
1155  if (myDir == FORWARD) {
1156  // start at the right side of the sidewalk
1157  myRelY = stripeWidth * (numStripes(lane) - 1) - myRelY;
1158  }
1159  if DEBUGCOND(*this) {
1160  std::cout << " added new pedestrian " << myPerson->getID() << " on " << lane->getID() << " myRelX=" << myRelX << " myRelY=" << myRelY << " dir=" << myDir << " route=" << toString(myStage->getRoute()) << "\n";
1161  }
1162 
1163  myNLI = getNextLane(*this, lane, 0);
1164 }
1165 
1166 
1167 double
1168 MSPModel_Striping::PState::getMinX(const bool includeMinGap) const {
1169  // @todo speed should have an influence here because faster persons need more space
1170  if (myDir == FORWARD) {
1171  return myRelX - getLength();
1172  }
1173  return myRelX - (includeMinGap ? getMinGap() : 0.);
1174 }
1175 
1176 
1177 double
1178 MSPModel_Striping::PState::getMaxX(const bool includeMinGap) const {
1179  // @todo speed should have an influence here because faster persons need more space
1180  if (myDir == FORWARD) {
1181  return myRelX + (includeMinGap ? getMinGap() : 0.);
1182  }
1183  return myRelX + getLength();
1184 }
1185 
1186 
1187 double
1189  return myPerson->getVehicleType().getLength();
1190 }
1191 
1192 
1193 double
1195  return myPerson->getVehicleType().getMinGap();
1196 }
1197 
1198 
1199 int
1201  return (int)floor(relY / stripeWidth + 0.5);
1202 }
1203 
1204 
1205 int
1207  const int s = stripe(relY);
1208  const double offset = relY - s * stripeWidth;
1209  const double threshold = MAX2(NUMERICAL_EPS, stripeWidth - SQUEEZE * myPerson->getVehicleType().getWidth());
1210  int result;
1211  if (offset > threshold) {
1212  result = s + 1;
1213  } else if (offset < -threshold) {
1214  result = s - 1;
1215  } else {
1216  result = s;
1217  }
1218  //std::cout.setf(std::ios::fixed , std::ios::floatfield);
1219  //std::cout << std::setprecision(5);
1220  //if DEBUGCOND(*this) std::cout << " otherStripe " << myPerson->getID() << " offset=" << offset << " threshold=" << threshold << " rawResult=" << result << "\n";
1221  return result;
1222 }
1223 
1224 int
1226  return MIN2(MAX2(0, stripe(myRelY)), numStripes(myLane) - 1);
1227 }
1228 
1229 
1230 int
1232  return MIN2(MAX2(0, otherStripe(myRelY)), numStripes(myLane) - 1);
1233 }
1234 
1235 
1236 double
1238  if (myStage->getNextRouteEdge() == 0) {
1239  return myDir * (myStage->getArrivalPos() - myRelX) - POSITION_EPS;
1240  } else {
1241  const double length = myWalkingAreaPath == 0 ? myLane->getLength() : myWalkingAreaPath->length;
1242  return myDir == FORWARD ? length - myRelX : myRelX;
1243  }
1244 }
1245 
1246 
1247 bool
1249  double dist = distToLaneEnd();
1250  if (DEBUGCOND(*this)) {
1251  std::cout << SIMTIME << " ped=" << myPerson->getID() << " myRelX=" << myRelX << " dist=" << dist << "\n";
1252  }
1253  if (dist <= 0) {
1254  //if (ped.myPerson->getID() == DEBUG1) {
1255  // std::cout << SIMTIME << " addToLane x=" << ped.myRelX << " newDir=" << newDir << " newLane=" << newLane->getID() << " walkingAreaShape=" << walkingAreaShape << "\n";
1256  //}
1257  //std::cout << " changing to " << newLane->getID() << " myRelY=" << ped.myRelY << " oldStripes=" << numStripes(myLane) << " newStripes=" << numStripes(newLane);
1258  //std::cout << " newY=" << ped.myRelY << " myDir=" << ped.myDir << " newDir=" << newDir;
1259  const int oldDir = myDir;
1260  const MSLane* oldLane = myLane;
1261  myLane = myNLI.lane;
1262  myDir = myNLI.dir;
1263  const bool normalLane = (myLane == 0 || myLane->getEdge().getFunction() == EDGEFUNC_NORMAL);
1264  if DEBUGCOND(*this) {
1265  std::cout << SIMTIME
1266  << " ped=" << myPerson->getID()
1267  << " moveToNextLane old=" << oldLane->getID()
1268  << " new=" << (myLane == 0 ? "NULL" : myLane->getID())
1269  << " oldDir=" << oldDir
1270  << " newDir=" << myDir
1271  << " myRelX=" << myRelX
1272  << " dist=" << dist
1273  << "\n";
1274  }
1275  if (myLane == 0) {
1277  }
1278  myStage->moveToNextEdge(myPerson, currentTime, normalLane ? 0 : &myLane->getEdge());
1279  if (myLane != 0) {
1280  assert(myDir != UNDEFINED_DIRECTION);
1281  myNLI = getNextLane(*this, myLane, oldLane);
1282  assert(myNLI.lane != oldLane); // do not turn around
1283  if DEBUGCOND(*this) {
1284  std::cout << " nextLane=" << (myNLI.lane == 0 ? "NULL" : myNLI.lane->getID()) << "\n";
1285  }
1286  if (myLane->getEdge().isWalkingArea()) {
1287  if (myNLI.dir != UNDEFINED_DIRECTION) {
1288  myWalkingAreaPath = &myWalkingAreaPaths[std::make_pair(oldLane, myNLI.lane)];
1289  assert(myWalkingAreaPath->from != 0);
1290  assert(myWalkingAreaPath->to != 0);
1291  assert(myWalkingAreaPath->shape.size() >= 2);
1292  if DEBUGCOND(*this) {
1293  std::cout << " mWAPath shape=" << myWalkingAreaPath->shape << " length=" << myWalkingAreaPath->length << "\n";
1294  }
1295  } else {
1296  // disconnnected route. move to the next edge
1297  if (OptionsCont::getOptions().getBool("ignore-route-errors")) {
1298  // try to determine direction from topology, otherwise maintain current direction
1299  const MSEdge* currRouteEdge = myStage->getRouteEdge();
1300  const MSEdge* nextRouteEdge = myStage->getNextRouteEdge();
1301  if ((nextRouteEdge->getToJunction() == currRouteEdge->getFromJunction())
1302  || nextRouteEdge->getToJunction() == currRouteEdge->getToJunction()) {
1303  myDir = BACKWARD;
1304  } else if ((nextRouteEdge->getFromJunction() == currRouteEdge->getFromJunction())
1305  || nextRouteEdge->getFromJunction() == currRouteEdge->getToJunction()) {
1306  myDir = FORWARD;
1307  }
1308  myStage->moveToNextEdge(myPerson, currentTime, 0);
1309  myLane = myNLI.lane;
1310  assert(myLane != 0);
1311  assert(myLane->getEdge().getFunction() == EDGEFUNC_NORMAL);
1312  myNLI = getNextLane(*this, myLane, oldLane);
1313  myWalkingAreaPath = 0;
1314  } else {
1315  throw ProcessError("Disconnected walk for person '" + myPerson->getID() + "'.");
1316  }
1317  }
1318  } else {
1319  myWalkingAreaPath = 0;
1320  }
1321  // adapt x to fit onto the new lane
1322  // (make sure we do not move past the end of the new lane since that
1323  // lane was not checked for obstacles)
1324  const double newLength = (myWalkingAreaPath == 0 ? myLane->getLength() : myWalkingAreaPath->length);
1325  if (-dist > newLength) {
1326  assert(false);
1327  // should not happen because the end of myLane should have been an obstacle as well
1328  dist = -newLength;
1329  }
1330  if (myDir == BACKWARD) {
1331  myRelX = newLength + dist;
1332  } else {
1333  myRelX = -dist;
1334  }
1335  if DEBUGCOND(*this) {
1336  std::cout << SIMTIME << " update myRelX ped=" << myPerson->getID()
1337  << " newLength=" << newLength
1338  << " dist=" << dist
1339  << " myRelX=" << myRelX
1340  << "\n";
1341  }
1342  // adjust to change in direction
1343  if (myDir != oldDir) {
1344  myRelY = (numStripes(oldLane) - 1) * stripeWidth - myRelY;
1345  }
1346  // adjust to differences in sidewalk width
1347  const int offset = getStripeOffset(numStripes(oldLane), numStripes(myLane), oldDir != myDir && numStripes(myLane) < numStripes(oldLane));
1348  myRelY += offset * stripeWidth;
1349  if DEBUGCOND(*this) {
1350  std::cout << SIMTIME << " transformY ped=" << myPerson->getID()
1351  << " newLane=" << Named::getIDSecure(myLane)
1352  << " newY=" << myRelY
1353  << " os=" << numStripes(oldLane) << " ns=" << numStripes(myLane)
1354  << " od=" << oldDir << " nd=" << myDir
1355  << " offset=" << offset << "\n";
1356  }
1357  }
1358  return true;
1359  } else {
1360  return false;
1361  }
1362 }
1363 
1364 
1365 void
1367  const int stripes = (int)obs.size();
1368  const int sMax = stripes - 1;
1369  assert(stripes == numStripes(myLane));
1370  const double vMax = myStage->getMaxSpeed(myPerson);
1371  // ultimate goal is to choose the prefered stripe (chosen)
1372  const int current = stripe();
1373  const int other = otherStripe();
1374  // compute distances
1375  std::vector<double> distance(stripes);
1376  for (int i = 0; i < stripes; ++i) {
1377  distance[i] = distanceTo(obs[i], obs[i].type == OBSTACLE_PED);
1378  }
1379  // compute utility for all stripes
1380  std::vector<double> utility(stripes, 0);
1381  // forbid stripes which are blocked and also all stripes behind them
1382  for (int i = 0; i < stripes; ++i) {
1383  if (distance[i] == DIST_OVERLAP) {
1384  if (i == current && (!myWaitingToEnter || stripe() != stripe(myRelY))) {
1385  utility[i] += OBSTRUCTED_PENALTY;
1386  }
1387  if (i < current) {
1388  for (int j = 0; j <= i; ++j) {
1389  utility[j] += OBSTRUCTED_PENALTY;
1390  }
1391  }
1392  if (i > current) {
1393  for (int j = i; j < stripes; ++j) {
1394  utility[j] += OBSTRUCTED_PENALTY;
1395  }
1396  }
1397  }
1398  }
1399  // forbid a portion of the leftmost stripes (in walking direction).
1400  // lanes with stripes less than 1 / RESERVE_FOR_ONCOMING_FACTOR
1401  // may still deadlock in heavy pedestrian traffic
1402  const bool onJunction = myLane->getEdge().isWalkingArea() || myLane->getEdge().isCrossing();
1403  const int reserved = (int)floor(stripes * (onJunction ? RESERVE_FOR_ONCOMING_FACTOR_JUNCTIONS : RESERVE_FOR_ONCOMING_FACTOR));
1404  if (myDir == FORWARD) {
1405  for (int i = 0; i < reserved; ++i) {
1406  utility[i] += INAPPROPRIATE_PENALTY * (i == current ? 0.5 : 1);
1407  }
1408  } else {
1409  for (int i = sMax; i > sMax - reserved; --i) {
1410  utility[i] += INAPPROPRIATE_PENALTY * (i == current ? 0.5 : 1);
1411  }
1412  }
1413  // adapt utility based on obstacles
1414  for (int i = 0; i < stripes; ++i) {
1415  if (obs[i].speed * myDir < 0) {
1416  // penalize evasion to the left
1417  if (myDir == FORWARD && i > 0) {
1418  utility[i - 1] -= 0.5;
1419  } else if (myDir == BACKWARD && i < sMax) {
1420  utility[i + 1] -= 0.5;
1421  }
1422  }
1423  // compute expected distance achievable by staying on this stripe for a time horizon
1424  const double walkDist = MAX2(0., distance[i]); // disregard special distance flags
1425  const double lookAhead = obs[i].speed * myDir >= 0 ? LOOKAHEAD_SAMEDIR : LOOKAHEAD_ONCOMING;
1426  const double expectedDist = MIN2(vMax * LOOKAHEAD_SAMEDIR, walkDist + obs[i].speed * myDir * lookAhead);
1427  if (DEBUGCOND(*this)) {
1428  std::cout << " util=" << utility[i] << " exp=" << expectedDist << " dist=" << distance[i] << "\n";
1429  }
1430  if (expectedDist >= 0) {
1431  utility[i] += expectedDist;
1432  } else {
1433  // let only the distance count
1434  utility[i] += ONCOMING_CONFLICT_PENALTY + distance[i];
1435  }
1436  }
1437  // discourage use of the leftmost lane (in walking direction) if there are oncoming
1438  if (myDir == FORWARD && obs[0].speed < 0) {
1439  utility[0] += ONCOMING_CONFLICT_PENALTY;
1440  } else if (myDir == BACKWARD && obs[sMax].speed > 0) {
1441  utility[sMax] += ONCOMING_CONFLICT_PENALTY;
1442  }
1443  // penalize lateral movement (if the current stripe permits walking)
1444  if (distance[current] > 0 && myWaitingTime == 0) {
1445  for (int i = 0; i < stripes; ++i) {
1446  utility[i] += abs(i - current) * LATERAL_PENALTY;
1447  }
1448  }
1449 
1450  // select best stripe
1451  int chosen = current;
1452  for (int i = 0; i < stripes; ++i) {
1453  if (utility[chosen] < utility[i]) {
1454  chosen = i;
1455  }
1456  }
1457  // compute speed components along both axes
1458  const int next = (chosen == current ? current : (chosen < current ? current - 1 : current + 1));
1459  const double xDist = MIN3(distance[current], distance[other], distance[next]);
1460  // XXX preferred gap differs between approaching a standing obstacle or a moving obstacle
1461  const double preferredGap = NUMERICAL_EPS;
1462  double xSpeed = MIN2(vMax, MAX2(0., DIST2SPEED(xDist - preferredGap)));
1463  if (xSpeed < NUMERICAL_EPS) {
1464  xSpeed = 0.;
1465  }
1466  if (DEBUGCOND(*this)) {
1467  std::cout << " xSpeedPotential=" << xSpeed << "\n";
1468  }
1469  // avoid tiny steps
1470  // XXX pressure from behind?
1471  if (mySpeed == 0 && xDist < MIN_STARTUP_DIST &&
1472  // unless walking towards a short lane
1473  !(
1474  (xDist == distance[current] && obs[current].type >= OBSTACLE_END)
1475  || (xDist == distance[other] && obs[other].type >= OBSTACLE_END)
1476  || (xDist == distance[next] && obs[next].type >= OBSTACLE_END))
1477  ) {
1478  xSpeed = 0;
1479  }
1480  if (xSpeed == 0) {
1481  if (myWaitingTime > jamTime || myAmJammed) {
1482  // squeeze slowly through the crowd ignoring others
1483  if (!myAmJammed) {
1485  WRITE_WARNING("Person '" + myPerson->getID()
1486  + "' is jammed on edge '" + myStage->getEdge()->getID()
1487  + "', time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
1488  myAmJammed = true;
1489  }
1490  xSpeed = vMax / 4;
1491  }
1492  } else if (stripe(myRelY) >= 0 && stripe(myRelY) <= sMax) {
1493  myAmJammed = false;
1494  }
1495  // dawdling
1496  const double dawdle = MIN2(xSpeed, RandHelper::rand() * vMax * dawdling);
1497  xSpeed -= dawdle;
1498 
1499  // XXX ensure that diagonal speed <= vMax
1500  // avoid deadlocks on narrow sidewalks
1501  //if (oncoming && xSpeed == 0 && myStage->getWaitingTime(currentTime) > TIME2STEPS(ONCOMIN_PATIENCE)) {
1502  // if DEBUGCOND(*this) std::cout << " stepping asside to resolve oncoming deadlock\n";
1503  // xSpeed = POSITION_EPS; // reset myWaitingTime
1504  // if (myDir == FORWARD && chosen < sMax) {
1505  // chosen += 1;
1506  // } else if (myDir == BACKWARD && chosen > 0) {
1507  // chosen -= 1;
1508  // }
1509  //}
1510  const double maxYSpeed = MIN2(MAX2(vMax * LATERAL_SPEED_FACTOR, vMax - xSpeed), stripeWidth);
1511  double ySpeed = 0;
1512  double yDist = 0;
1513  if (utility[next] > OBSTRUCTION_THRESHOLD && utility[chosen] > OBSTRUCTION_THRESHOLD) {
1514  // don't move laterally if the stripes are blocked
1515  yDist = (chosen * stripeWidth) - myRelY;
1516  if (fabs(yDist) > NUMERICAL_EPS) {
1517  ySpeed = (yDist > 0 ?
1518  MIN2(maxYSpeed, DIST2SPEED(yDist)) :
1519  MAX2(-maxYSpeed, DIST2SPEED(yDist)));
1520  }
1521  } else if (utility[next] <= OBSTRUCTION_THRESHOLD && obs[next].type == OBSTACLE_VEHICLE
1522  // still on the road
1523  && stripe() == stripe(myRelY)) {
1524  // step aside to let the vehicle pass
1525  myRelY += myDir * vMax;
1526  }
1527  // DEBUG
1528  if DEBUGCOND(*this) {
1529  std::cout << SIMTIME
1530  << " ped=" << myPerson->getID()
1531  << " edge=" << myStage->getEdge()->getID()
1532  << " x=" << myRelX
1533  << " y=" << myRelY
1534  << " d=" << myDir
1535  << " pvx=" << mySpeed
1536  << " cur=" << current
1537  << " cho=" << chosen
1538  << " oth=" << other
1539  << " nxt=" << next
1540  << " vx=" << xSpeed
1541  << " dawdle=" << dawdle
1542  << " vy=" << ySpeed
1543  << " xd=" << xDist
1544  << " yd=" << yDist
1545  << " vMax=" << myStage->getMaxSpeed(myPerson)
1546  << " wTime=" << myStage->getWaitingTime(currentTime)
1547  << " jammed=" << myAmJammed
1548  << "\n distance=" << toString(distance)
1549  << "\n utility=" << toString(utility)
1550  << "\n";
1551  DEBUG_PRINT(obs);
1552  }
1553  myRelX += SPEED2DIST(xSpeed * myDir);
1554  myRelY += SPEED2DIST(ySpeed);
1555  mySpeed = xSpeed;
1556  if (xSpeed >= SUMO_const_haltingSpeed) {
1557  myWaitingToEnter = false;
1558  myWaitingTime = 0;
1559  } else {
1561  }
1562 }
1563 
1564 
1565 double
1567  return MAX2(0., MIN2(1., myPerson->getVehicleType().getImpatience()
1569 }
1570 
1571 
1572 double
1574  return myRelX;
1575 }
1576 
1577 
1578 Position
1580  if (myLane == 0) {
1581  // pedestrian has already finished
1582  return Position::INVALID;
1583  }
1584  const double lateral_offset = myRelY + (stripeWidth - myLane->getWidth()) * 0.5;
1585  if (myWalkingAreaPath == 0) {
1586  return stage.getLanePosition(myLane, myRelX, lateral_offset);
1587  } else {
1588  //if DEBUGCOND(*this) {
1589  // std::cout << SIMTIME
1590  // << " getPosition (walkingArea)"
1591  // << " p=" << myPerson->getID()
1592  // << " x=" << myRelX
1593  // << " y=" << myRelY
1594  // << " latOffset=" << lateral_offset
1595  // << " shape=" << myWalkingAreaPath->shape
1596  // << " pos=" << myWalkingAreaPath->shape.positionAtOffset(myRelX, lateral_offset)
1597  // << "\n";
1598  //}
1599  return myWalkingAreaPath->shape.positionAtOffset(myRelX, lateral_offset);
1600  }
1601 }
1602 
1603 
1604 double
1606  if (myLane == 0) {
1607  // pedestrian has already finished
1608  return 0;
1609  }
1611  double angle = shp.rotationAtOffset(myRelX) + (myDir == MSPModel::BACKWARD ? M_PI : 0);
1612  if (angle > M_PI) {
1613  angle -= 2 * M_PI;
1614  }
1615  return angle;
1616 }
1617 
1618 
1619 SUMOTime
1621  return myWaitingTime;
1622 }
1623 
1624 
1625 double
1627  return mySpeed;
1628 }
1629 
1630 
1631 const MSEdge*
1633  return myNLI.lane == 0 ? 0 : &myNLI.lane->getEdge();
1634 }
1635 
1636 void
1638  double lanePosLat, double angle, int routeOffset,
1639  const ConstMSEdgeVector& edges, SUMOTime t) {
1640  /*
1641  std::cout << " MSPModel_Striping::PState::moveToXY"
1642  << " pos=" << pos
1643  << " lane=" << lane->getID()
1644  << " lanePos=" << lanePos
1645  << " lanePosLat=" << lanePosLat
1646  << " angle=" << angle
1647  << " routeOffset=" << routeOffset
1648  << " myRelX=" << myRelX << " myRelY=" << myRelY;
1649  */
1650  myRelX = lanePos,
1651  myRelY = (myLane->getWidth() - stripeWidth) * 0.5 - lanePosLat;
1652  //std::cout << " newX=" << myRelX << " newY=" << myRelY << "\n";
1653  UNUSED_PARAMETER(p);
1654  UNUSED_PARAMETER(pos);
1655  UNUSED_PARAMETER(lane);
1656  UNUSED_PARAMETER(angle);
1657  UNUSED_PARAMETER(routeOffset);
1658  UNUSED_PARAMETER(edges);
1659  UNUSED_PARAMETER(t);
1660 }
1661 
1662 
1663 
1664 double
1665 MSPModel_Striping::PState::distanceTo(const Obstacle& obs, const bool includeMinGap) const {
1666  // check for overlap
1667  const double maxX = getMaxX(includeMinGap);
1668  const double minX = getMinX(includeMinGap);
1669  //if (DEBUGCOND(*this)) {
1670  // std::cout << std::setprecision(2) << " distanceTo=" << obs.description << " maxX=" << maxX << " minX=" << minX << " obs.xFwd=" << obs.xFwd << " obs.xBack=" << obs.xBack << "\n";
1671  //}
1672  if ((obs.xFwd >= maxX && obs.xBack <= maxX) || (obs.xFwd <= maxX && obs.xFwd >= minX)) {
1673  return DIST_OVERLAP;
1674  }
1675  if (myDir == FORWARD) {
1676  return obs.xFwd < minX ? DIST_BEHIND : obs.xBack - maxX;
1677  } else {
1678  return obs.xBack > maxX ? DIST_BEHIND : minX - obs.xFwd;
1679  }
1680 }
1681 
1682 
1683 void
1685  for (int i = 0; i < (int)into.size(); ++i) {
1686  if (gDebugFlag1) {
1687  std::cout << " i=" << i << " intoDist=" << distanceTo(into[i]) << " obs2Dist=" << distanceTo(obs2[i]) << "\n";
1688  }
1689  if (distanceTo(obs2[i]) < distanceTo(into[i])) {
1690  into[i] = obs2[i];
1691  }
1692  }
1693 }
1694 
1695 
1696 bool
1698  if (link->haveRed()) {
1699  const double ignoreRedTime = myPerson->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_DRIVE_AFTER_RED_TIME, -1);
1700  if (ignoreRedTime >= 0) {
1701  const double redDuration = STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() - link->getLastStateChange());
1702  if (DEBUGCOND(*this)) {
1703  std::cout << SIMTIME << " ignoreRedTime=" << ignoreRedTime << " redDuration=" << redDuration << "\n";
1704  }
1705  return ignoreRedTime > redDuration;
1706  } else {
1707  return false;
1708  }
1709  } else {
1710  return false;
1711  }
1712 }
1713 
1714 // ===========================================================================
1715 // MSPModel_Striping::MovePedestrians method definitions
1716 // ===========================================================================
1717 //
1718 
1719 SUMOTime
1721  std::set<MSPerson*> changedLane;
1722  myModel->moveInDirection(currentTime, changedLane, FORWARD);
1723  myModel->moveInDirection(currentTime, changedLane, BACKWARD);
1724  // DEBUG
1725 #ifdef LOG_ALL
1726  for (ActiveLanes::const_iterator it_lane = myModel->getActiveLanes().begin(); it_lane != myModel->getActiveLanes().end(); ++it_lane) {
1727  const MSLane* lane = it_lane->first;
1728  Pedestrians pedestrians = it_lane->second;
1729  if (pedestrians.size() == 0) {
1730  continue;
1731  }
1732  sort(pedestrians.begin(), pedestrians.end(), by_xpos_sorter(FORWARD));
1733  std::cout << SIMTIME << " lane=" << lane->getID();
1734  for (int ii = 0; ii < (int)pedestrians.size(); ++ii) {
1735  const PState& p = *pedestrians[ii];
1736  std::cout << " (" << p.myPerson->getID() << " " << p.myRelX << "," << p.myRelY << " " << p.myDir << ")";
1737  }
1738  std::cout << "\n";
1739  }
1740 #endif
1741  return DELTA_T;
1742 }
1743 
const std::vector< IncomingLaneInfo > & getIncomingLanes() const
Definition: MSLane.h:752
bool gDebugFlag1
global utility flags for debugging
Definition: StdDefs.cpp:32
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:56
double getImpatience(SUMOTime now) const
returns the impatience
MSLane * getLogicalPredecessorLane() const
get the most likely precedecessor lane (sorted using by_connections_to_sorter). The result is cached ...
Definition: MSLane.cpp:2256
double brakeGap(const double speed) const
Returns the distance the vehicle needs to halt including driver&#39;s reaction time tau (i...
Definition: MSCFModel.h:289
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:607
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:83
double getJMParam(const SumoXMLAttr attr, const double defaultValue) const
Returns the named value from the map, or the default if it is not contained there.
static const double LATERAL_SPEED_FACTOR
static const double SQUEEZE
#define SPEED2DIST(x)
Definition: SUMOTime.h:54
MSPerson::MSPersonStage_Walking * myStage
static const double DIST_BEHIND
void moveInDirectionOnLane(Pedestrians &pedestrians, const MSLane *lane, SUMOTime currentTime, std::set< MSPerson *> &changedLane, int dir)
move pedestrians forward on one lane
static const double LOOKAHEAD_ONCOMING
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)
Obstacle(int dir, double dist=DIST_FAR_AWAY)
create No-Obstacle
const MSEdge * getEdge() const
Returns the current edge.
double getMinGap() const
return the minimum gap of the pedestrian
sorts the persons by position on the lane. If dir is forward, higher x positions come first...
static WalkingAreaPath * getArbitraryPath(const MSEdge *walkingArea)
return an arbitrary path across the given walkingArea
static const MSLane * getNextWalkingArea(const MSLane *currentLane, const int dir, MSLink *&link)
return the next walkingArea in the given direction
bool ignoreRed(const MSLink *link) const
whether the pedestrian may ignore a red light
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
static const double MIN_STARTUP_DIST
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:249
information regarding surround Pedestrians (and potentially other things)
const MSEdgeVector & getPredecessors() const
Definition: MSEdge.h:343
The base class for an intersection.
Definition: MSJunction.h:64
double y() const
Returns the y-position.
Definition: Position.h:67
static double rand(std::mt19937 *rng=0)
Returns a random real number in [0, 1)
Definition: RandHelper.h:64
double getMaxSpeed(const MSPerson *person) const
accessors to be used by MSPModel
Definition: MSPerson.cpp:313
PState(MSPerson *person, MSPerson::MSPersonStage_Walking *stage, const MSLane *lane)
const MSEdge * getEdge() const
Returns the current edge.
Definition: MSPerson.cpp:87
AnyVehicleIterator is a structure, which manages the iteration through all vehicles on the lane...
Definition: MSLane.h:103
static const int FORWARD
Definition: MSPModel.h:105
std::string time2string(SUMOTime t)
Definition: SUMOTime.cpp:59
double x() const
Returns the x-position.
Definition: Position.h:62
void registerJammed()
register a jammed transportable
const std::vector< MSLane * > & getLanes() const
Returns this edge&#39;s lanes.
Definition: MSEdge.h:167
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:167
T MAX2(T a, T b)
Definition: StdDefs.h:73
MSPedestrianRouterDijkstra & getPedestrianRouter(const MSEdgeVector &prohibited=MSEdgeVector()) const
Definition: MSNet.cpp:928
void remove(PedestrianState *state)
remove the specified person from the pedestrian simulation
SUMOTime DELTA_T
Definition: SUMOTime.cpp:39
double getLength() const
Returns the lane&#39;s length.
Definition: MSLane.h:497
const PositionVector & getShape() const
Returns this lane&#39;s shape.
Definition: MSLane.h:439
static std::string getIDSecure(const T *obj, const std::string &fallBack="NULL")
get an identifier for Named-like object which may be Null
Definition: Named.h:58
PositionVector reverse() const
reverse position vector
double rotationAtOffset(double pos) const
Returns the rotation at the given length.
std::vector< const MSEdge * > ConstMSEdgeVector
Definition: MSEdge.h:78
bool usingInternalLanes()
whether movements on intersections are modelled
std::map< std::pair< const MSLane *, const MSLane * >, WalkingAreaPath > WalkingAreaPaths
const std::string & getID() const
Returns the id.
Definition: Named.h:65
bool moveToNextLane(SUMOTime currentTime)
return whether this pedestrian has passed the end of the current lane and update myRelX if so ...
#define TIME2STEPS(x)
Definition: SUMOTime.h:66
Position getLanePosition(const MSLane *lane, double at, double offset) const
get position on lane at length at with orthogonal offset
void moveToXY(MSPerson *p, Position pos, MSLane *lane, double lanePos, double lanePosLat, double angle, int routeOffset, const ConstMSEdgeVector &edges, SUMOTime t)
try to move person to the given position
static MSPModel * myModel
Definition: MSPModel.h:125
double getLength() const
return the length of the edge
Definition: MSEdge.h:569
double getMaxX(const bool includeMinGap=true) const
return the maximum position on the lane
static WalkingAreaPaths myWalkingAreaPaths
store for walkinArea elements
const MSJunction * getToJunction() const
Definition: MSEdge.h:352
double getLength() const
return the length of the pedestrian
#define MINGAP_TO_VEHICLE
static const double LATERAL_PENALTY
double getWidth() const
Returns the lane&#39;s width.
Definition: MSLane.h:513
#define UNUSED_PARAMETER(x)
Definition: StdDefs.h:39
PersonDist nextBlocking(const MSLane *lane, double minPos, double minRight, double maxLeft, double stopTime=0)
returns the next pedestrian beyond minPos that is laterally between minRight and maxLeft or 0 ...
double mySpeed
the current walking speed
static const double OBSTRUCTION_THRESHOLD
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:199
The simulated network and simulation perfomer.
Definition: MSNet.h:90
bool myAmJammed
whether the person is jammed
int getVehicleNumberWithPartials() const
Returns the number of vehicles on this lane (including partial occupators)
Definition: MSLane.h:373
static OptionsCont & getOptions()
Retrieves the options.
Definition: OptionsCont.cpp:64
#define SIMTIME
Definition: SUMOTime.h:71
static Pedestrians noPedestrians
empty pedestrian vector
static bool gCheck4Accidents
Definition: MSGlobals.h:82
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
static const double RESERVE_FOR_ONCOMING_FACTOR_JUNCTIONS
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)
const MSCFModel & getCarFollowModel() const
Returns the vehicle&#39;s car following model definition.
Definition: MSVehicle.h:882
double xFwd
maximal position on the current lane in forward direction
static const int UNDEFINED_DIRECTION
Definition: MSPModel.h:110
bool moveToNextEdge(MSPerson *person, SUMOTime currentTime, MSEdge *nextInternal=0)
move forward and return whether the person arrived
Definition: MSPerson.cpp:287
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:55
SUMOTime execute(SUMOTime currentTime)
Executes the command.
static const double OBSTRUCTED_PENALTY
static const double DIST_OVERLAP
#define DEBUGCOND2(LANE)
virtual MSTransportableControl & getPersonControl()
Returns the person control.
Definition: MSNet.cpp:768
void walk(const Obstacles &obs, SUMOTime currentTime)
perform position update
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:45
static int getStripeOffset(int origStripes, int destStripes, bool addRemainder)
#define SUMOTime_MAX
Definition: TraCIDefs.h:52
A list of positions.
double myRelY
the orthogonal shift on the current lane
double xBack
maximal position on the current lane in backward direction
double myRelX
the advancement along the current lane
MSEventControl * getBeginOfTimestepEvents()
Returns the event control for events executed at the begin of a time step.
Definition: MSNet.h:399
std::string getString(const std::string &name) const
Returns the string-value of the named option (only for Option_String)
virtual void addEvent(Command *operation, SUMOTime execTimeStep=-1)
Adds an Event.
#define STEPS2TIME(x)
Definition: SUMOTime.h:64
double getSpeed(const MSPerson::MSPersonStage_Walking &stage) const
return the current speed of the person
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:253
double distToLaneEnd() const
the absolute distance to the end of the lane in walking direction (or to the arrivalPos) ...
SUMOTime string2time(const std::string &r)
Definition: SUMOTime.cpp:46
double getMaxSpeed() const
Get vehicle&#39;s maximum speed [m/s].
static double dawdling
T MIN2(T a, T b)
Definition: StdDefs.h:67
AnyVehicleIterator anyVehiclesEnd() const
end iterator for iterating over all vehicles touching this lane in downstream direction ...
Definition: MSLane.h:403
AnyVehicleIterator anyVehiclesBegin() const
begin iterator for iterating over all vehicles touching this lane in downstream direction ...
Definition: MSLane.h:398
#define POSITION_EPS
Definition: config.h:175
const std::string & getID() const
returns the id of the transportable
bool hasInternalLinks() const
return whether the network contains internal links
Definition: MSNet.h:587
static bool gUsingInternalLanes
Information whether the simulation regards internal lanes.
Definition: MSGlobals.h:75
double getMinGap() const
Get the free space in front of vehicles of this class.
std::map< const MSLane *, Obstacles, lane_by_numid_sorter > NextLanesObstacles
double getMaxDecel() const
Get the vehicle type&#39;s maximal comfortable deceleration [m/s^2].
Definition: MSCFModel.h:211
AnyVehicleIterator anyVehiclesUpstreamBegin() const
begin iterator for iterating over all vehicles touching this lane in upstream direction ...
Definition: MSLane.h:408
double getArrivalPos() const
Definition: MSPerson.h:154
bool isCrossing() const
return whether this edge is a pedestrian crossing
Definition: MSEdge.h:234
bool isInternal() const
return whether this edge is an internal edge
Definition: MSEdge.h:229
SUMOTime myWaitingTime
the consecutive time spent at speed 0
const ConstMSEdgeVector & getRoute() const
Definition: MSPerson.h:168
int myDir
the walking direction on the current lane (1 forward, -1 backward)
static const double LOOKAHEAD_SAMEDIR
const SUMOVTypeParameter & getParameter() const
double getFloat(const std::string &name) const
Returns the double-value of the named option (only for Option_Float)
double speed
speed relative to lane direction (positive means in the same direction)
const Obstacles & getNextLaneObstacles(NextLanesObstacles &nextLanesObs, const MSLane *lane, const MSLane *nextLane, int stripes, int nextDir, double currentLength, int currentDir)
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 double RESERVE_FOR_ONCOMING_FACTOR
double getLateralPositionOnLane() const
Get the vehicle&#39;s lateral position on the lane.
Definition: MSVehicle.h:439
void arriveAndAdvance(Pedestrians &pedestrians, SUMOTime currentTime, std::set< MSPerson *> &changedLane, int dir)
handle arrivals and lane advancement
static void transformToCurrentLanePositions(Obstacles &o, int currentDir, int nextDir, double currentLength, double nextLength)
abstract base class for managing callbacks to retrieve various state information from the model ...
Definition: MSPModel.h:131
std::pair< const MSPerson *, double > PersonDist
Definition: MSPModel.h:43
bool hasPedestrians(const MSLane *lane)
whether the given lane has pedestrians on it
double getWidth() const
Get the width which vehicles of this class shall have when being drawn.
std::map< const MSLane *, double > MinNextLengths
ObstacleType type
whether this obstacle denotes a border or a pedestrian
static const double DIST_FAR_AWAY
double getEdgePos(const MSPerson::MSPersonStage_Walking &stage, SUMOTime now) const
abstract methods inherited from PedestrianState
static const double ONCOMING_CONFLICT_PENALTY
void extrapolate(const double val, const bool onlyFirst=false, const bool onlyLast=false)
extrapolate position vector
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:121
double compute(const E *from, const E *to, double departPos, double arrivalPos, double 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...
const MSVehicleType & getVehicleType() const
Returns the vehicle&#39;s type definition.
const MSStoppingPlace * getDestinationStop() const
returns the destination stop (if any)
static double stripeWidth
model parameters
static const double INAPPROPRIATE_PENALTY
bool myWaitingToEnter
whether the pedestrian is waiting to start its walk
#define M_PI
Definition: odrSpiral.cpp:40
static const MSEdgeVector & getAllEdges()
Returns all edges with a numerical id.
Definition: MSEdge.cpp:777
const MSJunction * getFromJunction() const
Definition: MSEdge.h:348
double distanceTo(const Obstacle &obs, const bool includeMinGap=true) const
A storage for options typed value containers)
Definition: OptionsCont.h:98
Container for pedestrian state and individual position update function.
const MSEdgeVector & getSuccessors() const
Returns the following edges.
Definition: MSEdge.h:319
std::vector< Obstacle > Obstacles
double getLength() const
Get vehicle&#39;s length [m].
AnyVehicleIterator anyVehiclesUpstreamEnd() const
end iterator for iterating over all vehicles touching this lane in upstream direction ...
Definition: MSLane.h:413
const MSEdge & getDestination() const
returns the destination edge
const double SUMO_const_haltingSpeed
the speed threshold at which vehicles are considered as halting
Definition: StdDefs.h:57
#define DEBUGCOND(PED)
static const int BACKWARD
Definition: MSPModel.h:109
bool isWalkingArea() const
return whether this edge is walking area
Definition: MSEdge.h:248
const MSVehicleType & getVehicleType() const
double getBackPositionOnLane(const MSLane *lane) const
Get the vehicle&#39;s position relative to the given lane.
Definition: MSVehicle.cpp:3120
const MSEdge * getRouteEdge() const
Definition: MSPerson.h:162
static const double SAFETY_GAP
Definition: MSPModel.h:113
double getMinX(const bool includeMinGap=true) const
return the minimum position on the lane
T MIN3(T a, T b, T c)
Definition: StdDefs.h:80
MovePedestrians * myCommand
the MovePedestrians command that is registered
long long int SUMOTime
Definition: TraCIDefs.h:51
#define NUMERICAL_EPS
Definition: config.h:151
void push_back_noDoublePos(const Position &p)
insert in back a non double position
static void DEBUG_PRINT(const Obstacles &obs)
double getImpatience() const
Returns this type&#39;s impatience.
static void addCloserObstacle(Obstacles &obs, double x, int stripe, int numStripes, const std::string &id, double width, int dir, ObstacleType type)
const MSEdge * getNextRouteEdge() const
Definition: MSPerson.h:165
std::vector< MSEdge * > MSEdgeVector
Definition: MSEdge.h:77
static SUMOTime jamTime
const MSLinkCont & getLinkCont() const
returns the container with all links !!!
Definition: MSLane.cpp:1874
int myNumActivePedestrians
the total number of active pedestrians
double getAngle(const MSPerson::MSPersonStage_Walking &stage, SUMOTime now) const
return the direction in which the person faces in degrees
static MinNextLengths myMinNextLengths
double getSpeed() const
Returns the vehicle&#39;s current speed.
Definition: MSVehicle.h:482
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
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)
const std::string & getID() const
Returns the name of the vehicle.
Position positionAtOffset(double pos, double lateralOffset=0) const
Returns the position at the given length.
static const double MAX_WAIT_TOLERANCE
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
Position transformToVectorCoordinates(const Position &p, bool extend=false) const
return position p within the length-wise coordinate system defined by this position vector...
bool blockedAtDist(const MSLane *lane, double vehSide, double vehWidth, double oncomingGap, std::vector< const MSPerson *> *collectBlockers)
whether a pedestrian is blocking the crossing of lane for the given vehicle bondaries ...
SumoXMLEdgeFunc getFunction() const
Returns the edge type (SumoXMLEdgeFunc)
Definition: MSEdge.h:224
static const Position INVALID
used to indicate that a position is valid
Definition: Position.h:277
static bool addCrossingVehs(const MSLane *crossing, int stripes, double lateral_offset, int dir, Obstacles &crossingVehs)
add vehicles driving across
static const double LOOKAROUND_VEHICLES
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:98