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];
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...
const std::string & getID() const
returns the person id
bool blockedAtDist(const MSLane *lane, SUMOReal distToCrossing, std::vector< const MSPerson * > *collectBlockers)
whether a pedestrian is blocking the crossing of lane at offset distToCrossing
MSEdge & getEdge() const
Returns the lane's edge.
static const SUMOReal SAFETY_GAP
const MSEdge * getNextRouteEdge() const
SUMOReal distToLaneEnd() const
the absolute distance to the end of the lane in walking direction (or to the arrivalPos) ...
MSPerson::MSPersonStage_Walking * myStage
Position getLanePosition(const MSLane *lane, SUMOReal at, SUMOReal offset) const
get position on lane at length at with orthogonal offset
static SUMOReal stripeWidth
model parameters
static int numStripes(const MSLane *lane)
return the maximum number of pedestrians walking side by side
static Obstacles getNeighboringObstacles(const Pedestrians &pedestrians, int egoIndex, int stripes)
SUMOReal mySpeed
the current walking speed
static const MSLane * getNextWalkingArea(const MSLane *currentLane, const int dir, MSLink *&link)
return the next walkingArea in the given direction
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
const MSEdge * getEdge() const
Returns the current edge.
information regarding surround Pedestrians (and potentially other things)
SUMOReal getLength() const
Returns the lane's length.
The base class for an intersection.
StageType getCurrentStageType() const
bool isWalkingArea() const
return whether this edge is walking area
PState(MSPerson *person, MSPerson::MSPersonStage_Walking *stage, const MSLane *lane)
#define ONCOMING_CONFLICT
static SUMOReal rand()
Returns a random real number in [0, 1)
SUMOReal getWidth() const
Returns the lane's width.
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
static MSLane * getSidewalk(const MSEdge *edge)
return the appropriate lane to walk on
static bool dictionary(const std::string &id, MSEdge *edge)
Inserts edge into the static dictionary Returns true if the key id isn't already in the dictionary...
SUMOReal getFloat(const std::string &name) const
Returns the SUMOReal-value of the named option (only for Option_Float)
std::map< std::pair< const MSLane *, const MSLane * >, WalkingAreaPath > WalkingAreaPaths
bool moveToNextLane(SUMOTime currentTime)
return whether this pedestrian has passed the end of the current lane and update myRelX if so ...
static MSPModel * myModel
SUMOTime getCurrentTimeStep() const
Returns the current simulation step.
static WalkingAreaPaths myWalkingAreaPaths
store for walkinArea elements
The simulated network and simulation perfomer.
static Pedestrians noPedestrians
empty pedestrian vector
static const SUMOReal RESERVE_FOR_ONCOMING_FACTOR
PositionVector reverse() const
void moveInDirection(SUMOTime currentTime, std::set< MSPerson * > &changedLane, int dir)
move all pedestrians forward and advance to the next lane if applicable
static bool canTraverse(int dir, const std::vector< const MSEdge * > &route)
return whether the route may traversed with the given starting direction
NextLaneInfo myNLI
information about the upcoming lane
const std::vector< const MSEdge * > & getRoute() const
static const SUMOReal SQUEEZE
const std::string & getID() const
Returns the id.
A road/street connecting two junctions.
Pedestrians & getPedestrians(const MSLane *lane)
retrieves the pedestian vector for the given lane (may be empty)
static const SUMOReal LOOKAHEAD_SAMEDIR
void compute(const E *from, const E *to, SUMOReal departPos, SUMOReal arrivalPos, SUMOReal speed, SUMOTime msTime, const N *onlyNode, std::vector< const E * > &into, bool allEdges=false)
Builds the route between the given edges using the minimum effort at the given time The definition of...
static const int UNDEFINED_DIRECTION
SUMOReal getLength() const
return the length of the edge
MSLane * getLogicalPredecessorLane() const
const Obstacles & getNextLaneObstacles(NextLanesObstacles &nextLanesObs, const MSLane *nextLane, int stripes, SUMOReal nextLength, int nextDir, SUMOReal currentLength, int currentDir)
static const SUMOReal LATERAL_SPEED_FACTOR
SUMOTime execute(SUMOTime currentTime)
Executes the command.
void walk(const Obstacles &obs, SUMOTime currentTime)
perform position update
A point in 2D or 3D with translation and scaling methods.
static const SUMOReal LOOKAHEAD_ONCOMING
MSLane * getLane() const
Returns the connected lane.
SUMOReal getLength() const
return the length of the pedestrian
unsigned int getNoFollowing() const
Returns the number of edges that may be reached from this edge.
SUMOTime getWaitingTime(const MSPerson::MSPersonStage_Walking &stage, SUMOTime now) const
return the time the person spent standing
static Obstacles mergeObstacles(const Obstacles &obs1, const Obstacles &obs2, int dir)
Position getPosition(const MSPerson::MSPersonStage_Walking &stage, SUMOTime now) const
return the network coordinate of the person
SUMOReal getAngle(const MSPerson::MSPersonStage_Walking &stage, SUMOTime now) const
return the direction in which the person faces in degrees
SUMOReal x
position on the current lane
std::string toString(const T &t, std::streamsize accuracy=OUTPUT_ACCURACY)
bool isInternal() const
return whether this edge is an internal edge
int myDir
the walking direction on the current lane (1 forward, -1 backward)
virtual SUMOTime addEvent(Command *operation, SUMOTime execTimeStep, AdaptType type)
Adds an Event.
SUMOReal getEdgePos(const MSPerson::MSPersonStage_Walking &stage, SUMOTime now) const
abstract methods inherited from PedestrianState
SUMOReal getSpeed(const MSPerson::MSPersonStage_Walking &stage) const
return the current speed of the person
void cleanupHelper()
remove state at simulation end
PedestrianState * add(MSPerson *person, MSPerson::MSPersonStage_Walking *stage, SUMOTime now)
register the given person as a pedestrian
abstract base class for managing callbacks to retrieve various state information from the model ...
void extrapolate(SUMOReal val)
SUMOReal getArrivalPos() const
SUMOReal rotationDegreeAtOffset(SUMOReal pos) const
Returns the rotation at the given length.
void push_back(const PositionVector &p)
Appends all positions from the given vector.
const std::vector< MSEdge * > & getIncomingEdges() const
Returns the list of edges from which this edge may be reached.
SUMOReal getImpatience(SUMOTime now) const
returns the impatience
std::vector< PState * > Pedestrians
bool myWaitingToEnter
whether the pedestrian is waiting to start its walk
static MSLink * getConnectingLink(const MSLane &from, const MSLane &to)
Returns the link connecting both lanes Both lanes have to be non-internal; 0 may be returned if no co...
The edge is a normal street.
MSLane * getViaLaneOrLane() const
return the via lane if it exists and the lane otherwise
static size_t dictSize()
Returns the number of edges.
A storage for options typed value containers)
Container for pedestrian state and individual position update function.
std::vector< Obstacle > Obstacles
static const SUMOReal BLOCKER_LOOKAHEAD
const MSEdge * getFollower(unsigned int n) const
Returns the n-th of the following edges.
static const int BACKWARD
const PositionVector & getShape() const
Returns this lane's shape.
SUMOReal distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Patch the time in a way that it is at least as high as the simulation begin time. ...
SUMOReal myRelY
the orthogonal shift on the current lane
std::map< const MSLane *, Obstacles > NextLanesObstacles
static const SUMOReal MAX_WAIT_TOLERANCE
bool isCrossing() const
return whether this edge is a pedestrian crossing
SUMOReal myRelX
the advancement along the current lane
const MSJunction * getFromJunction() const
MovePedestrians * myCommand
the MovePedestrians command that is registered
static void DEBUG_PRINT(const Obstacles &obs)
const MSLinkCont & getLinkCont() const
returns the container with all links !!!
int myNumActivePedestrians
the total number of active pedestrians
SUMOReal speed
speed relative to ego direction (positive means in the same direction)
MSEventControl & getBeginOfTimestepEvents()
Returns the event control for events executed at the begin of a time step.
const std::vector< IncomingLaneInfo > & getIncomingLanes() const
SUMOReal getMaxSpeed() const
accessors to be used by MSPModel
static void initWalkingAreaPaths(const MSNet *net)
MSPedestrianRouterDijkstra & getPedestrianRouter(const std::vector< MSEdge * > &prohibited=std::vector< MSEdge * >()) const
MSPModel_Striping(const OptionsCont &oc, MSNet *net)
Constructor (it should not be necessary to construct more than one instance)
Representation of a lane in the micro simulation.
ActiveLanes myActiveLanes
store of all lanes which have pedestrians on them
const MSLane * myLane
the current lane of this pedestrian
bool opened(SUMOTime arrivalTime, SUMOReal arrivalSpeed, SUMOReal leaveSpeed, SUMOReal vehicleLength, SUMOReal impatience, SUMOReal decel, SUMOTime waitingTime, std::vector< const SUMOVehicle * > *collectFoes=0) const
Returns the information whether the link may be passed.
static const SUMOReal LATERAL_PENALTY
Obstacle(int dir)
create No-Obstacle
static const Position INVALID
const MSJunction * getToJunction() const