45 #define DEBUG1 "disabled"
46 #define DEBUG2 "disabled"
47 #define DEBUGCOND(PEDID) (PEDID == DEBUG1 || PEDID == DEBUG2)
51 for (
int i = 0; i < (
int)obs.size(); ++i) {
53 <<
"(" << obs[i].description
55 <<
" s=" << obs[i].speed
67 #define BLOCKED_STRIPE -10000
68 #define FAR_AWAY 10000
69 #define ONCOMING_CONFLICT -1000
97 myNumActivePedestrians(0) {
125 for (Pedestrians::const_iterator it_ped = pedestrians.begin(); it_ped != pedestrians.end(); ++it_ped) {
126 const PState& ped = **it_ped;
127 const SUMOReal halfVehicleWidth = 1.0;
135 if (collectBlockers == 0) {
138 collectBlockers->push_back(ped.
myPerson);
142 if (collectBlockers == 0) {
145 return collectBlockers->size() > 0;
177 if (from == 0 || to == 0) {
200 std::vector<MSLane*> lanes;
202 for (
int j = 0; j < (
int)incoming.size(); ++j) {
209 for (
int j = 0; j < (
int)lanes.size(); ++j) {
210 for (
int k = 0; k < (
int)lanes.size(); ++k) {
213 const MSLane* from = lanes[j];
214 const MSLane* to = lanes[k];
253 const MSLane* nextLane = nextRouteLane;
257 if (nextRouteLane != 0) {
262 std::cout <<
" internal\n";
267 nextLane = currentLane->
getLinkCont()[0]->getLane();
272 std::cout <<
" crossing\n";
275 std::vector<const MSEdge*> crossingRoute;
282 std::vector<MSEdge*> prohibited;
283 prohibited.push_back(&prevLane->
getEdge());
287 <<
" nreDir=" << nextRouteEdgeDir
288 <<
" aPos=" << arrivalPos
289 <<
" crossingRoute=" <<
toString(crossingRoute)
292 if (crossingRoute.size() > 1) {
293 const MSEdge* nextEdge = crossingRoute[1];
296 assert(nextLane != prevLane);
299 std::cout <<
" nextDir=" << nextDir <<
"\n";
314 <<
" no route from '" << (currentEdge == 0 ?
"NULL" : currentEdge->
getID())
315 <<
"' to '" << (nextRouteEdge == 0 ?
"NULL" : nextRouteEdge->
getID())
327 std::cout <<
" next walkingArea " << (nextDir ==
FORWARD ?
"forward" :
"backward") <<
"\n";
334 #ifdef HAVE_INTERNAL_LANES
337 std::cout <<
" direct forward\n";
339 nextLane = MSLinkContHelper::getInternalFollowingLane(currentLane, nextRouteLane);
344 #ifdef HAVE_INTERNAL_LANES
347 std::cout <<
" direct backward\n";
349 nextLane = MSLinkContHelper::getInternalFollowingLane(nextRouteLane, currentLane);
356 nextLane = nextRouteLane;
358 std::cout <<
SIMTIME <<
" no next lane found for " << currentLane->
getID() <<
" dir=" << ped.
myDir <<
"\n";
362 nextLane = nextRouteLane;
369 <<
" l=" << currentLane->
getID()
370 <<
" nl=" << (nextLane == 0 ?
"NULL" : nextLane->
getID())
371 <<
" nrl=" << (nextRouteLane == 0 ?
"NULL" : nextRouteLane->
getID())
374 <<
" pedDir=" << ped.
myDir
385 for (MSLinkCont::const_iterator it = links.begin(); it != links.end(); ++it) {
386 if ((*it)->getLane()->getEdge().isWalkingArea()) {
392 const std::vector<MSLane::IncomingLaneInfo>& laneInfos = currentLane->
getIncomingLanes();
393 for (std::vector<MSLane::IncomingLaneInfo>::const_iterator it = laneInfos.begin(); it != laneInfos.end(); ++it) {
394 if ((*it).lane->getEdge().isWalkingArea()) {
395 link = (*it).viaLink;
406 Obstacles result(obs1.begin(), obs1.end());
407 for (
int i = 0; i < (
int)obs1.size(); ++i) {
408 if ((obs2[i].x - obs1[i].x) * dir < 0) {
418 const PState& ego = *pedestrians[egoIndex];
420 int index = egoIndex + 1;
422 while (index < (
int)pedestrians.size() && ego.
myDir * (pedestrians[index]->myRelX - egoBack) > 0) {
423 const PState& p = *pedestrians[index];
441 SUMOReal currentLength,
int currentDir) {
442 if (nextLanesObs.count(nextLane) == 0) {
452 const int offset = (stripes -
numStripes(nextLane)) / 2;
456 sort(pedestrians.begin(), pedestrians.end(),
by_xpos_sorter(nextDir));
457 for (
int ii = 0; ii < (
int)pedestrians.size(); ++ii) {
458 const PState& p = *pedestrians[ii];
463 int mappedStripe = p.
stripe() + offset;
464 if (mappedStripe >= 0 && mappedStripe < stripes) {
465 obs[mappedStripe] = o;
468 if (mappedStripe >= 0 && mappedStripe < stripes) {
469 obs[mappedStripe] = o;
472 for (
int ii = 0; ii < stripes; ++ii) {
475 o.
x = nextLength - o.
x;
478 o.
x += currentLength;
483 nextLanesObs[nextLane] = obs;
485 return nextLanesObs[nextLane];
492 const MSLane* lane = it_lane->first;
500 sort(pedestrians.begin(), pedestrians.end(),
by_xpos_sorter(dir));
501 for (
int ii = 0; ii < (
int)pedestrians.size(); ++ii) {
502 PState& p = *pedestrians[ii];
525 nextLanesObs, nextLane, stripes,
563 p.
walk(currentObs, currentTime);
573 sort(pedestrians.begin(), pedestrians.end(),
by_xpos_sorter(dir));
574 bool checkAdvance =
true;
575 while (checkAdvance) {
576 checkAdvance =
false;;
577 if (pedestrians.size() > 0) {
578 PState* p = pedestrians.front();
579 if (p->
myDir != dir) {
583 pedestrians.erase(pedestrians.begin());
609 description(ped.myPerson->getID()) {
611 if (dir == ped.
myDir) {
630 myRelX(stage->getDepartPos()),
634 myBlockedByOncoming(false),
635 myWaitingToEnter(true),
637 myWalkingAreaPath(0) {
641 if (route.size() == 1) {
648 std::cout <<
" initialize dir for " <<
myPerson->
getID() <<
" forward=" << mayStartForward <<
" backward=" << mayStartBackward <<
"\n";
650 if (mayStartForward && mayStartBackward) {
652 std::vector<const MSEdge*> crossingRoute;
654 if (crossingRoute.size() > 1) {
656 const MSEdge* nextEdge = crossingRoute[1];
662 std::cout <<
" crossingRoute=" <<
toString(crossingRoute) <<
"\n";
682 return myPerson->getVehicleType().getLength();
696 const int s = stripe();
700 if (offset > threshold) {
702 }
else if (offset < -threshold) {
707 std::cout.setf(std::ios::fixed , std::ios::floatfield);
708 std::cout << std::setprecision(5);
716 if (myStage->getNextRouteEdge() == 0) {
717 return myDir * (myStage->getArrivalPos() - myRelX);
719 const SUMOReal length = myWalkingAreaPath == 0 ? myLane->getLength() : myWalkingAreaPath->length;
720 return myDir ==
FORWARD ? length - myRelX : myRelX;
727 const SUMOReal dist = distToLaneEnd();
738 const int oldDir = myDir;
739 const MSLane* oldLane = myLane;
745 <<
" ped=" << myPerson->getID()
746 <<
" moveToNextLane old=" << oldLane->
getID()
747 <<
" new=" << (myLane == 0 ?
"NULL" : myLane->getID())
750 myStage->moveToNextEdge(myPerson, currentTime, normalLane ? 0 : &myLane->getEdge());
755 std::cout <<
" nextLane=" << (myNLI.lane == 0 ?
"NULL" : myNLI.lane->getID()) <<
"\n";
757 if (myLane->getEdge().isWalkingArea()) {
759 assert(myWalkingAreaPath->from != 0);
760 assert(myWalkingAreaPath->to != 0);
761 assert(myWalkingAreaPath->shape.size() >= 2);
763 std::cout <<
" mWAPath shape=" << myWalkingAreaPath->shape <<
" length=" << myWalkingAreaPath->length <<
"\n";
766 myWalkingAreaPath = 0;
770 const SUMOReal newLength = (myWalkingAreaPath == 0 ? myLane->getLength() : myWalkingAreaPath->length);
771 myRelX = newLength + dist;
776 if (myDir != oldDir) {
791 const int stripes = (
int)obs.size();
792 const int sMax = stripes - 1;
794 const SUMOReal vMax = myStage->getMaxSpeed();
796 const int current = stripe();
797 const int other = otherStripe();
798 int chosen = current;
800 std::vector<SUMOReal> utility(stripes, 0);
803 for (
int i = 0; i < stripes; ++i) {
807 std::vector<SUMOReal> distance(stripes);
808 for (
int i = 0; i < stripes; ++i) {
809 distance[i] += myDir * (obs[i].x - myRelX);
812 for (
int i = 0; i < stripes; ++i) {
813 if (distance[i] <= 0) {
815 for (
int j = 0; j <= i; ++j) {
819 for (
int j = i; j < stripes; ++j) {
830 for (
int i = 0; i < reserved; ++i) {
834 for (
int i = sMax; i > sMax - reserved; --i) {
839 for (
int i = 0; i < stripes; ++i) {
840 if (obs[i].speed < 0) {
842 if (myDir ==
FORWARD && i > 0) {
843 utility[i - 1] -= 0.5;
844 }
else if (myDir ==
BACKWARD && i < sMax) {
845 utility[i + 1] -= 0.5;
852 std::cout <<
" util=" << utility[i] <<
" exp=" << expectedDist <<
"\n";
854 if (expectedDist >= 0) {
855 utility[i] += expectedDist;
863 if (((myDir ==
FORWARD && current == sMax)
864 || (myDir ==
BACKWARD && current == 0))
865 && obs[current].speed < 0) {
870 if (((myDir ==
BACKWARD && current == sMax)
871 || (myDir ==
FORWARD && current == 0))
872 && obs[current].speed < 0) {
877 for (
int i = 0; i < stripes; ++i) {
878 if (utility[chosen] < utility[i]) {
883 const int next = (chosen == current ? current : (chosen < current ? current - 1 : current + 1));
884 const SUMOReal xDist =
MIN3(distance[current], distance[other], distance[next]);
886 const SUMOReal preferredGap = myPerson->getVehicleType().getMinGap() + xDist * 0.5;
891 std::cout <<
" xSpeedPotential=" << xSpeed <<
"\n";
893 if (mySpeed == 0 && xSpeed < 0.5 * vMax) {
915 ySpeed = (yDist > 0 ?
916 MIN2(maxYSpeed, yDist) :
917 MAX2(-maxYSpeed, yDist));
920 if (
true &&
DEBUGCOND(myPerson->getID())) {
922 <<
" ped=" << myPerson->getID()
923 <<
" edge=" << myStage->getEdge()->getID()
927 <<
" pvx=" << mySpeed
928 <<
" cur=" << current
933 <<
" dawdle=" << dawdle
937 <<
" vMax=" << myStage->getMaxSpeed()
938 <<
" wTime=" << myStage->getWaitingTime(currentTime)
939 <<
"\n distance=" <<
toString(distance)
948 myWaitingToEnter =
false;
959 myPerson->getVehicleType().getImpatience()
977 if (myWalkingAreaPath == 0) {
980 return myWalkingAreaPath->shape.positionAtOffset(myRelX, lateral_offset);
991 const PositionVector& shp = myWalkingAreaPath == 0 ? myLane->getShape() : myWalkingAreaPath->shape;
1002 return myWaitingTime;
1018 std::set<MSPerson*> changedLane;
1023 for (ActiveLanes::const_iterator it_lane =
myModel->getActiveLanes().begin(); it_lane !=
myModel->getActiveLanes().end(); ++it_lane) {
1024 const MSLane* lane = it_lane->first;
1026 if (pedestrians.size() == 0) {
1031 for (
int ii = 0; ii < (
int)pedestrians.size(); ++ii) {
1032 const PState& p = *pedestrians[ii];