77 #include <mesosim/MESegment.h>
78 #include <mesosim/MELoop.h>
82 #ifdef CHECK_MEMORY_LEAKS
84 #endif // CHECK_MEMORY_LEAKS
87 #ifdef DEBUG_VEHICLE_GUI_SELECTION
94 #define BUS_STOP_OFFSET 0.5
125 return (myPos != state.
myPos ||
137 myPos(pos), mySpeed(speed) {}
145 : mySpeedAdaptationStarted(true), myConsiderSafeVelocity(true),
146 myConsiderMaxAcceleration(true), myConsiderMaxDeceleration(true),
147 myAmVTDControlled(false) {}
155 mySpeedAdaptationStarted =
true;
156 mySpeedTimeLine = speedTimeLine;
162 myLaneTimeLine = laneTimeLine;
169 myOriginalSpeed = speed;
171 while (mySpeedTimeLine.size() == 1 || (mySpeedTimeLine.size() > 1 && currentTime > mySpeedTimeLine[1].first)) {
172 mySpeedTimeLine.erase(mySpeedTimeLine.begin());
175 if (mySpeedTimeLine.size() < 2 || currentTime < mySpeedTimeLine[0].first) {
179 if (!mySpeedAdaptationStarted) {
180 mySpeedTimeLine[0].second = speed;
181 mySpeedAdaptationStarted =
true;
185 speed = mySpeedTimeLine[0].second - (mySpeedTimeLine[0].second - mySpeedTimeLine[1].second) * td;
186 if (myConsiderSafeVelocity) {
187 speed =
MIN2(speed, vSafe);
189 if (myConsiderMaxAcceleration) {
190 speed =
MIN2(speed, vMax);
192 if (myConsiderMaxDeceleration) {
193 speed =
MAX2(speed, vMin);
202 while (myLaneTimeLine.size() == 1 || (myLaneTimeLine.size() > 1 && currentTime > myLaneTimeLine[1].first)) {
203 myLaneTimeLine.erase(myLaneTimeLine.begin());
206 if (myLaneTimeLine.size() < 2 || currentTime < myLaneTimeLine[0].first) {
209 unsigned int destinationLaneIndex = myLaneTimeLine[1].second;
210 if ((
unsigned int)currentEdge.
getLanes().size() <= destinationLaneIndex) {
213 if (currentLaneIndex > destinationLaneIndex) {
215 }
else if (currentLaneIndex < destinationLaneIndex) {
225 myConsiderSafeVelocity = value;
231 myConsiderMaxAcceleration = value;
237 myConsiderMaxDeceleration = value;
245 if (myVTDRoute.size() != 0) {
249 if (myVTDPos > myVTDLane->getLength()) {
250 myVTDPos = myVTDLane->getLength();
252 myVTDLane->forceVehicleInsertion(v, myVTDPos);
254 myAmVTDControlled =
false;
268 (*i)->resetPartialOccupation(
this);
272 if ((*i).myLink != 0) {
273 (*i).myLink->removeApproaching(
this);
307 for (std::vector<SUMOVehicleParameter::Stop>::iterator i = pars->
stops.begin(); i != pars->
stops.end(); ++i) {
310 "' on lane '" + i->lane +
"' is not downstream the current route.");
313 for (std::vector<SUMOVehicleParameter::Stop>::const_iterator i = route->
getStops().begin(); i != route->
getStops().end(); ++i) {
316 "' on lane '" + i->lane +
"' is not downstream the current route.");
319 const MSLane*
const depLane = (*myCurrEdge)->getDepartLane(*
this);
321 throw ProcessError(
"Invalid departlane definition for vehicle '" + pars->
id +
"'.");
325 "' is too high for the departure lane '" + depLane->
getID() +
"'.");
329 "' is too high for the vehicle type '" + type->
getID() +
"'.");
340 if ((*i).myLink != 0) {
341 (*i).myLink->removeApproaching(
this);
384 for (std::list<Stop>::iterator iter =
myStops.begin(); iter !=
myStops.end();) {
385 if (find(edges.begin(), edges.end(), &iter->lane->getEdge()) == edges.end()) {
388 iter->edge = find(edges.begin(), edges.end(), &iter->lane->getEdge());
444 if (!rem->first->notifyMove(*
this, oldPos + rem->second, newPos + rem->second, newSpeed)) {
446 if (myTraceMoveReminders) {
447 traceMoveReminder(
"notifyMove", rem->first, rem->second,
false);
453 if (myTraceMoveReminders) {
454 traceMoveReminder(
"notifyMove", rem->first, rem->second,
true);
469 rem->second += oldLaneLength;
471 if (myTraceMoveReminders) {
472 traceMoveReminder(
"adaptedPos", rem->first, rem->second,
true);
510 atan2(p1.
x() - p2.
x(), p2.
y() - p1.
y()) * 180. /
PI :
531 if (stop.
until != -1) {
532 stop.
until += untilOffset;
544 std::list<Stop>::iterator iter =
myStops.begin();
547 prevStopEdge =
myStops.back().edge;
548 prevStopPos =
myStops.back().endPos;
554 while (iter !=
myStops.end() && (iter->edge < stop.
edge ||
555 (iter->endPos < stop.
endPos && iter->edge == stop.
edge))) {
556 prevStopEdge = iter->edge;
557 prevStopPos = iter->endPos;
561 int index = stopPar.
index;
563 prevStopEdge = iter->edge;
564 prevStopPos = iter->endPos;
572 (prevStopEdge == stop.
edge && prevStopPos > stop.
endPos)) {
607 return currentVelocity;
618 for (std::vector<MSPerson*>::const_iterator i = persons.begin(); i != persons.end(); ++i) {
659 bool busStopsMustHaveSpace =
true;
664 busStopsMustHaveSpace =
false;
672 if (stop.
until >= 0) {
688 return currentVelocity;
702 #ifdef DEBUG_VEHICLE_GUI_SELECTION
708 for (DriveItemVector::iterator i = lfLinks.begin(); i != lfLinks.end(); ++i) {
709 if ((*i).myLink != 0) {
710 (*i).myLink->removeApproaching(
this);
734 assert(bestLaneConts.size() > 0);
735 #ifdef HAVE_INTERNAL_LANES
736 bool hadNonInternal =
false;
738 bool hadNonInternal =
true;
743 unsigned int view = 0;
753 std::pair<const MSVehicle*, SUMOReal> leaderInfo = std::make_pair(pred, gap);
759 adaptToLeader(leaderInfo, seen, lastLink, lane, v, vLinkPass);
770 v =
MIN2(v, stopSpeed);
777 MSLinkCont::const_iterator link =
myLane->
succLinkSec(*
this, view + 1, *lane, bestLaneConts);
816 (*link)->getViaLaneOrLane()->getParallelLane(-
getLaneChangeModel().getLaneChangeDirection()) == 0)) {
818 const SUMOReal va = seen / timeRemaining;
826 const bool setRequest = v > 0;
836 #ifdef HAVE_INTERNAL_LANES
839 for (MSLink::LinkLeaders::const_iterator it = linkLeaders.begin(); it != linkLeaders.end(); ++it) {
842 if (leader->myLinkLeaders.count(
getID()) == 0) {
844 myLeaderForLink[*link] = leader->
getID();
845 myLinkLeaders.insert(leader->
getID());
850 v =
MAX2(v, vLinkWait);
862 if (!(*link)->havePriority() && stopDist > cfModel.
getMaxDecel()) {
868 + (seen - (v1 * v1 - arrivalSpeed * arrivalSpeed) * 0.5 / cfModel.
getMaxDecel()) / vLinkWait);
871 const SUMOReal accelTime = (vLinkPass - v) / accel;
872 const SUMOReal accelWay = accelTime * (vLinkPass + v) * 0.5;
884 arrivalSpeedBraking = arrivalSpeed;
889 arrivalSpeedBraking =
MIN2(arrivalSpeedBraking, arrivalSpeed);
890 arrivalTimeBraking =
MAX2(arrivalTime, t +
TIME2STEPS(seen / ((v + arrivalSpeedBraking) * 0.5)));
893 arrivalTime, arrivalSpeed,
894 arrivalTimeBraking, arrivalSpeedBraking,
897 #ifdef HAVE_INTERNAL_LANES
898 if ((*link)->getViaLane() == 0) {
899 hadNonInternal =
true;
905 if (!setRequest || ((v <= 0 || seen > dist) && hadNonInternal && seenNonInternal > vehicleLength * 2)) {
909 lane = (*link)->getViaLaneOrLane();
921 lastLink = &lfLinks.back();
930 if (leaderInfo.first != 0) {
933 if (leaderInfo.second >= 0) {
934 vsafeLeader = cfModel.
followSpeed(
this,
getSpeed(), leaderInfo.second, leaderInfo.first->getSpeed(), leaderInfo.first->getCarFollowModel().getMaxDecel());
943 v =
MIN2(v, vsafeLeader);
944 vLinkPass =
MIN2(vLinkPass, vsafeLeader);
951 #ifdef DEBUG_VEHICLE_GUI_SELECTION
968 DriveItemVector::iterator i;
969 bool braking =
false;
970 bool lastWasGreenCont =
false;
972 MSLink* link = (*i).myLink;
974 if (link != 0 && (*i).mySetRequest) {
980 vSafe = (*i).myVLinkWait;
982 lastWasGreenCont =
false;
987 const bool opened = yellow || link->
opened((*i).myArrivalTime, (*i).myArrivalSpeed, (*i).getLeaveSpeed(),
992 vSafe = (*i).myVLinkWait;
994 lastWasGreenCont =
false;
1002 vSafe = (*i).myVLinkPass;
1005 lastWasGreenCont =
false;
1006 vSafe = (*i).myVLinkWait;
1014 vSafe = (*i).myVLinkWait;
1062 std::vector<MSLane*> passedLanes;
1064 passedLanes.push_back(*i);
1066 if (passedLanes.size() == 0 || passedLanes.back() !=
myLane) {
1067 passedLanes.push_back(
myLane);
1082 MSLink* link = (*i).myLink;
1092 if (approachedLane !=
myLane && approachedLane != 0) {
1100 WRITE_WARNING(
"Vehicle '" +
getID() +
"' could not finish continuous lane change (turn lane) time=" +
1105 #ifdef HAVE_INTERNAL_LANES
1107 if (myLeaderForLink.find(link) != myLeaderForLink.end()) {
1108 myLinkLeaders.erase(myLeaderForLink[link]);
1109 myLeaderForLink.erase(link);
1118 passedLanes.push_back(approachedLane);
1124 (*i)->resetPartialOccupation(
this);
1144 std::vector<MSLane*>::reverse_iterator i = passedLanes.rbegin() + 1;
1145 while (leftLength > 0 && i != passedLanes.rend()) {
1147 leftLength -= (*i)->setPartialOccupation(
this, leftLength);
1164 for (MSLane::VehCont::const_iterator i = vehs.begin(); i != vehs.end(); ++i) {
1166 foundStopped =
true;
1167 const SUMOReal ret = (*i)->getPositionOnLane() - (*i)->getVehicleType().getLengthWithGap() - lengths;
1171 lengths += (*i)->getVehicleType().getLengthWithGap();
1180 #ifdef DEBUG_VEHICLE_GUI_SELECTION
1188 #ifdef HAVE_INTERNAL_LANES
1190 bool hadVehicle =
false;
1191 SUMOReal seenSpace = -lengthsInFront;
1193 bool foundStopped =
false;
1196 for (
unsigned int i = 0; i < lfLinks.size(); ++i) {
1199 if (item.
myLink == 0 || foundStopped) {
1205 const MSLane* approachedLane = item.
myLink->getViaLane();
1206 if (approachedLane != 0) {
1208 seenSpace = seenSpace - approachedLane->
getVehLenSum();
1230 foundStopped =
true;
1233 seenSpace += approachedLane->
getLength();
1247 foundStopped =
true;
1255 #ifdef DEBUG_VEHICLE_GUI_SELECTION
1261 for (
int i = (
int)(lfLinks.size() - 1); i > 0; --i) {
1267 bool allowsContinuation = item.
myLink == 0 || item.
myLink->
isCont() || !lfLinks[i].hadVehicle || opened;
1268 if (!opened && item.
myLink != 0) {
1272 allowsContinuation =
true;
1276 if (allowsContinuation) {
1282 int removalBegin = -1;
1283 for (
unsigned int i = 0; hadVehicle && i < lfLinks.size() && removalBegin < 0; ++i) {
1297 if (leftSpace < 0/* && item.myLink->willHaveBlockedFoe()*/) {
1304 if (leftSpace < -impatienceCorrection / 10. && item.myLink->isCrossing()) {
1312 while (removalBegin < (
int)(lfLinks.size())) {
1314 lfLinks[removalBegin].myVLinkPass = lfLinks[removalBegin].myVLinkWait;
1316 lfLinks[removalBegin].mySetRequest =
false;
1325 for (DriveItemVector::iterator i = lfLinks.begin(); i != lfLinks.end(); ++i) {
1326 if ((*i).myLink != 0) {
1330 (*i).myLink->setApproaching(
this, (*i).myArrivalTime, (*i).myArrivalSpeed, (*i).getLeaveSpeed(),
1331 (*i).mySetRequest, (*i).myArrivalTimeBraking, (*i).myArrivalSpeedBraking,
getWaitingTime());
1340 if (rem->first->getLane() != 0 && rem->first->getLane() !=
getLane()) {
1342 if (myTraceMoveReminders) {
1343 traceMoveReminder(
"notifyEnter_skipped", rem->first, rem->second,
true);
1348 if (rem->first->notifyEnter(*
this, reason)) {
1350 if (myTraceMoveReminders) {
1351 traceMoveReminder(
"notifyEnter", rem->first, rem->second,
true);
1357 if (myTraceMoveReminders) {
1358 traceMoveReminder(
"notifyEnter", rem->first, rem->second,
false);
1388 if (!onTeleporting) {
1430 while (i != route.
begin() && leftLength > 0) {
1437 leftLength -= (lane)->setPartialOccupation(
this, leftLength);
1483 MSLane* clane = enteredLane;
1484 while (leftLength > 0) {
1490 leftLength -= (clane)->setPartialOccupation(
this, leftLength);
1498 if (rem->first->notifyLeave(*
this,
myState.
myPos + rem->second, reason)) {
1500 if (myTraceMoveReminders) {
1501 traceMoveReminder(
"notifyLeave", rem->first, rem->second,
true);
1507 if (myTraceMoveReminders) {
1508 traceMoveReminder(
"notifyLeave", rem->first, rem->second,
false);
1516 (*i)->resetPartialOccupation(
this);
1538 const std::vector<MSVehicle::LaneQ>&
1540 #ifdef DEBUG_VEHICLE_GUI_SELECTION
1547 if (startLane == 0) {
1550 assert(startLane != 0);
1556 std::vector<LaneQ>::iterator i;
1557 for (i = lanes.begin(); i != lanes.end(); ++i) {
1559 for (std::vector<MSLane*>::const_iterator j = (*i).bestContinuations.begin() + 1; j != (*i).bestContinuations.end(); ++j) {
1560 nextOccupation += (*j)->getVehLenSum();
1562 (*i).nextOccupation = nextOccupation;
1563 if ((*i).lane == startLane) {
1574 const MSEdge* nextStopEdge = 0;
1575 const MSLane* nextStopLane = 0;
1579 nextStopLane = nextStop.
lane;
1580 nextStopEdge = &nextStopLane->
getEdge();
1588 if (nextStopEdge != 0) {
1597 bool progress =
true;
1599 std::vector<LaneQ> currentLanes;
1600 const std::vector<MSLane*>* allowed = 0;
1601 const MSEdge* nextEdge = 0;
1603 nextEdge = *(ce + 1);
1606 const std::vector<MSLane*>& lanes = (*ce)->getLanes();
1607 for (std::vector<MSLane*>::const_iterator i = lanes.begin(); i != lanes.end(); ++i) {
1614 q.
allowsContinuation = allowed == 0 || find(allowed->begin(), allowed->end(), cl) != allowed->end();
1615 currentLanes.push_back(q);
1618 if (nextStopEdge == *ce) {
1620 for (std::vector<LaneQ>::iterator q = currentLanes.begin(); q != currentLanes.end(); ++q) {
1621 if (nextStopLane != 0 && nextStopLane != (*q).lane) {
1622 (*q).allowsContinuation =
false;
1623 (*q).length = nextStopPos;
1630 seenLength += currentLanes[0].lane->getLength();
1632 progress &= (seen <= 4 || seenLength < 3000);
1633 progress &= seen <= 8;
1645 int bestThisIndex = 0;
1648 for (std::vector<LaneQ>::iterator j = last.begin(); j != last.end(); ++j, ++index) {
1649 if ((*j).length > bestLength) {
1650 bestLength = (*j).length;
1651 bestThisIndex = index;
1655 for (std::vector<LaneQ>::iterator j = last.begin(); j != last.end(); ++j, ++index) {
1656 if ((*j).length < bestLength) {
1657 (*j).bestLaneOffset = bestThisIndex - index;
1664 for (std::vector<std::vector<LaneQ> >::reverse_iterator i =
myBestLanes.rbegin() + 1; i !=
myBestLanes.rend(); ++i) {
1665 std::vector<LaneQ>& nextLanes = (*(i - 1));
1666 std::vector<LaneQ>& clanes = (*i);
1667 MSEdge& cE = clanes[0].lane->getEdge();
1671 for (std::vector<LaneQ>::iterator j = nextLanes.begin(); j != nextLanes.end(); ++j, ++index) {
1672 if ((*j).lane->isApproachedFrom(&cE) && bestConnectedLength < (*j).length) {
1673 bestConnectedLength = (*j).length;
1675 if (bestLength < (*j).length) {
1676 bestLength = (*j).length;
1679 if (bestConnectedLength > 0) {
1680 int bestThisIndex = 0;
1682 for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
1683 LaneQ bestConnectedNext;
1684 bestConnectedNext.
length = -1;
1685 if ((*j).allowsContinuation) {
1686 for (std::vector<LaneQ>::const_iterator m = nextLanes.begin(); m != nextLanes.end(); ++m) {
1687 if ((*m).lane->isApproachedFrom(&cE, (*j).lane)) {
1688 if (bestConnectedNext.
length < (*m).length || (bestConnectedNext.
length == (*m).length &&
abs(bestConnectedNext.
bestLaneOffset) >
abs((*m).bestLaneOffset))) {
1689 bestConnectedNext = *m;
1694 (*j).
length += bestLength;
1696 (*j).length += bestConnectedNext.
length;
1699 if (clanes[bestThisIndex].length < (*j).length || (clanes[bestThisIndex].length == (*j).length &&
abs(
abs(clanes[bestThisIndex].bestLaneOffset > (*j).bestLaneOffset)))) {
1700 bestThisIndex = index;
1706 for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
1707 if ((*j).length < clanes[bestThisIndex].length || ((*j).length == clanes[bestThisIndex].length &&
abs((*j).bestLaneOffset) <
abs(clanes[bestThisIndex].bestLaneOffset))) {
1708 (*j).bestLaneOffset = bestThisIndex - index;
1710 (*j).bestLaneOffset = 0;
1716 int bestThisIndex = 0;
1717 int bestNextIndex = 0;
1718 int bestDistToNeeded = (
int) clanes.size();
1720 for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
1721 if ((*j).allowsContinuation) {
1723 for (std::vector<LaneQ>::const_iterator m = nextLanes.begin(); m != nextLanes.end(); ++m, ++nextIndex) {
1724 if ((*m).lane->isApproachedFrom(&cE, (*j).lane)) {
1725 if (bestDistToNeeded >
abs((*m).bestLaneOffset)) {
1726 bestDistToNeeded =
abs((*m).bestLaneOffset);
1727 bestThisIndex = index;
1728 bestNextIndex = nextIndex;
1734 clanes[bestThisIndex].length += nextLanes[bestNextIndex].length;
1735 copy(nextLanes[bestNextIndex].bestContinuations.begin(), nextLanes[bestNextIndex].bestContinuations.end(), back_inserter(clanes[bestThisIndex].bestContinuations));
1737 for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
1738 if ((*j).length < clanes[bestThisIndex].length || ((*j).length == clanes[bestThisIndex].length &&
abs((*j).bestLaneOffset) <
abs(clanes[bestThisIndex].bestLaneOffset))) {
1739 (*j).bestLaneOffset = bestThisIndex - index;
1741 (*j).bestLaneOffset = 0;
1750 std::vector<LaneQ>& currLanes = *
myBestLanes.begin();
1751 std::vector<LaneQ>::iterator i;
1752 for (i = currLanes.begin(); i != currLanes.end(); ++i) {
1754 for (std::vector<MSLane*>::const_iterator j = (*i).bestContinuations.begin() + 1; j != (*i).bestContinuations.end(); ++j) {
1755 nextOccupation += (*j)->getVehLenSum();
1757 (*i).nextOccupation = nextOccupation;
1758 if ((*i).lane == startLane) {
1766 const std::vector<MSLane*>&
1771 return (*myCurrentLaneInBestLanes).bestContinuations;
1775 const std::vector<MSLane*>&
1781 if ((*i).lane == l) {
1782 return (*i).bestContinuations;
1791 std::vector<MSLane*>& bestLaneConts = (*myCurrentLaneInBestLanes).bestContinuations;
1793 bestLaneConts.erase(bestLaneConts.begin() + 1, bestLaneConts.end());
1812 #ifdef DEBUG_VEHICLE_GUI_SELECTION
1817 if (
isOnRoad() && destEdge != NULL) {
1881 unsigned int numExpected = (
unsigned int)
myStops.front().awaitedPersons.size();
1882 if (numExpected != 0) {
1887 numExpected = (
unsigned int)
myStops.front().awaitedPersons.size();
1889 if (numExpected == 0) {
1915 switch ((*link)->getDirection()) {
1944 std::vector<MSLane*>::const_iterator laneP = std::find((*myCurrEdge)->getLanes().begin(), (*myCurrEdge)->getLanes().end(),
myLane);
1945 return (
unsigned int) std::distance((*myCurrEdge)->getLanes().begin(), laneP);
1960 for (std::list<Stop>::iterator iter =
myStops.begin(); iter !=
myStops.end(); iter++) {
1961 if (iter->lane == lane && fabs(iter->endPos - pos) <
POSITION_EPS) {
1962 if (duration == 0 && !iter->reached) {
1965 iter->duration = duration;
2024 std::vector<int> internals;
2036 unsigned int routeOffset;
2041 myDeparture -= offset;