79 #include <mesosim/MESegment.h>
80 #include <mesosim/MELoop.h>
84 #ifdef CHECK_MEMORY_LEAKS
86 #endif // CHECK_MEMORY_LEAKS
89 #ifdef DEBUG_VEHICLE_GUI_SELECTION
96 #define BUS_STOP_OFFSET 0.5
127 return (myPos != state.
myPos ||
139 myPos(pos), mySpeed(speed) {}
147 : mySpeedAdaptationStarted(true), myConsiderSafeVelocity(true),
148 myConsiderMaxAcceleration(true), myConsiderMaxDeceleration(true),
149 myAmVTDControlled(false) {}
157 mySpeedAdaptationStarted =
true;
158 mySpeedTimeLine = speedTimeLine;
164 myLaneTimeLine = laneTimeLine;
171 myOriginalSpeed = speed;
173 while (mySpeedTimeLine.size() == 1 || (mySpeedTimeLine.size() > 1 && currentTime > mySpeedTimeLine[1].first)) {
174 mySpeedTimeLine.erase(mySpeedTimeLine.begin());
177 if (mySpeedTimeLine.size() < 2 || currentTime < mySpeedTimeLine[0].first) {
181 if (!mySpeedAdaptationStarted) {
182 mySpeedTimeLine[0].second = speed;
183 mySpeedAdaptationStarted =
true;
187 speed = mySpeedTimeLine[0].second - (mySpeedTimeLine[0].second - mySpeedTimeLine[1].second) * td;
188 if (myConsiderSafeVelocity) {
189 speed =
MIN2(speed, vSafe);
191 if (myConsiderMaxAcceleration) {
192 speed =
MIN2(speed, vMax);
194 if (myConsiderMaxDeceleration) {
195 speed =
MAX2(speed, vMin);
204 while (myLaneTimeLine.size() == 1 || (myLaneTimeLine.size() > 1 && currentTime > myLaneTimeLine[1].first)) {
205 myLaneTimeLine.erase(myLaneTimeLine.begin());
208 if (myLaneTimeLine.size() < 2 || currentTime < myLaneTimeLine[0].first) {
211 unsigned int destinationLaneIndex = myLaneTimeLine[1].second;
212 if ((
unsigned int)currentEdge.
getLanes().size() <= destinationLaneIndex) {
215 if (currentLaneIndex > destinationLaneIndex) {
217 }
else if (currentLaneIndex < destinationLaneIndex) {
227 myConsiderSafeVelocity = value;
233 myConsiderMaxAcceleration = value;
239 myConsiderMaxDeceleration = value;
247 if (myVTDRoute.size() != 0) {
251 if (myVTDPos > myVTDLane->getLength()) {
252 myVTDPos = myVTDLane->getLength();
254 myVTDLane->forceVehicleInsertion(v, myVTDPos);
256 myAmVTDControlled =
false;
270 (*i)->resetPartialOccupation(
this);
274 if ((*i).myLink != 0) {
275 (*i).myLink->removeApproaching(
this);
310 for (std::vector<SUMOVehicleParameter::Stop>::iterator i = pars->
stops.begin(); i != pars->
stops.end(); ++i) {
313 "' on lane '" + i->lane +
"' is not downstream the current route.");
316 for (std::vector<SUMOVehicleParameter::Stop>::const_iterator i = route->
getStops().begin(); i != route->
getStops().end(); ++i) {
319 "' on lane '" + i->lane +
"' is not downstream the current route.");
322 const MSLane*
const depLane = (*myCurrEdge)->getDepartLane(*
this);
324 throw ProcessError(
"Invalid departlane definition for vehicle '" + pars->
id +
"'.");
328 "' is too high for the departure lane '" + depLane->
getID() +
"'.");
332 "' is too high for the vehicle type '" + type->
getID() +
"'.");
348 if ((*i).myLink != 0) {
349 (*i).myLink->removeApproaching(
this);
392 for (std::list<Stop>::iterator iter =
myStops.begin(); iter !=
myStops.end();) {
393 if (find(edges.begin(), edges.end(), &iter->lane->getEdge()) == edges.end()) {
396 iter->edge = find(edges.begin(), edges.end(), &iter->lane->getEdge());
452 if (!rem->first->notifyMove(*
this, oldPos + rem->second, newPos + rem->second, newSpeed)) {
467 rem->second += oldLaneLength;
492 return atan2(p1.
x() - p2.
x(), p2.
y() - p1.
y()) * 180. /
PI;
509 if (stop.
until != -1) {
510 stop.
until += untilOffset;
522 std::list<Stop>::iterator iter =
myStops.begin();
525 prevStopEdge =
myStops.back().edge;
526 prevStopPos =
myStops.back().endPos;
532 while (iter !=
myStops.end() && (iter->edge < stop.
edge ||
533 (iter->endPos < stop.
endPos && iter->edge == stop.
edge))) {
534 prevStopEdge = iter->edge;
535 prevStopPos = iter->endPos;
539 int index = stopPar.
index;
541 prevStopEdge = iter->edge;
542 prevStopPos = iter->endPos;
550 (prevStopEdge == stop.
edge && prevStopPos > stop.
endPos)) {
577 return currentVelocity;
587 for (std::vector<MSPerson*>::const_iterator i = persons.begin(); i != persons.end(); ++i) {
628 bool busStopsMustHaveSpace =
true;
633 busStopsMustHaveSpace =
false;
641 if (stop.
until >= 0) {
657 return currentVelocity;
664 if (myHBMsgEmitter != 0) {
671 #ifdef DEBUG_VEHICLE_GUI_SELECTION
678 if ((*i).myLink != 0) {
679 (*i).myLink->removeApproaching(
this);
703 assert(bestLaneConts.size() > 0);
704 #ifdef HAVE_INTERNAL_LANES
705 bool hadNonInternal =
false;
707 bool hadNonInternal =
true;
713 unsigned int view = 0;
714 bool firstLane =
true;
716 std::pair<MSVehicle*, SUMOReal> leaderInfo = pred != 0 ? std::pair<MSVehicle*, SUMOReal>(pred,
gap2pred(*pred)) : std::pair<MSVehicle*, SUMOReal>((
MSVehicle*) 0, 0);
725 leaderInfo = lane->getLastVehicleInformation();
728 }
else if (leaderInfo.first == 0) {
734 adaptToLeader(leaderInfo, seen, lastLink, lane, v, vLinkPass);
737 if (!
myStops.empty() && &
myStops.begin()->lane->getEdge() == &lane->getEdge()) {
745 v =
MIN2(v, stopSpeed);
752 MSLinkCont::const_iterator link =
myLane->
succLinkSec(*
this, view + 1, *lane, bestLaneConts);
754 bool onAppropriateLane = !lane->isLinkEnd(link);
770 if (!onAppropriateLane) {
784 const bool setRequest = v > 0;
792 #ifdef HAVE_INTERNAL_LANES
794 std::pair<MSVehicle*, SUMOReal> linkLeaderInfo = (*link)->getLeaderInfo(myLeaderForLink, seen -
getVehicleType().getMinGap());
795 if (linkLeaderInfo.first != 0) {
797 if (!linkLeaderInfo.first->hasLinkLeader(
this)) {
798 myLeaderForLink[*link] = linkLeaderInfo.first->getID();
799 myLinkLeaders.insert(linkLeaderInfo.first->getID());
800 adaptToLeader(linkLeaderInfo, seen, lastLink, lane, v, vLinkPass);
805 SUMOReal va = firstLane ? v : lane->getVehicleMaxSpeed(
this);
817 + (seen - (v1 * v1 - arrivalSpeed * arrivalSpeed) * 0.5 /
getCarFollowModel().getMaxDecel()) / vLinkWait);
820 if (vLinkPass >= v) {
822 const SUMOReal accelWay = accelTime * (vLinkPass + v) * 0.5;
827 const SUMOReal decelWay = decelTime * (vLinkPass + v) * 0.5;
833 lane = (*link)->getViaLaneOrLane();
834 #ifdef HAVE_INTERNAL_LANES
835 if ((*link)->getViaLane() == 0) {
836 hadNonInternal =
true;
846 if (!setRequest || ((v <= 0 || seen > dist) && hadNonInternal && seenNonInternal > vehicleLength * 2)) {
852 va =
MAX2(lane->getVehicleMaxSpeed(
this), cfModel.
freeSpeed(
this,
getSpeed(), seen, lane->getVehicleMaxSpeed(
this)));
855 seen += lane->getLength();
863 if (leaderInfo.first != 0) {
866 if (leaderInfo.second >= 0) {
867 vsafeLeader = cfModel.
followSpeed(
this,
getSpeed(), leaderInfo.second, leaderInfo.first->getSpeed(), leaderInfo.first->getCarFollowModel().getMaxDecel());
876 v =
MIN2(v, vsafeLeader);
877 vLinkPass =
MIN2(vLinkPass, vsafeLeader);
902 #ifdef DEBUG_VEHICLE_GUI_SELECTION
919 DriveItemVector::iterator i;
920 bool braking =
false;
921 bool lastWasGreenCont =
false;
923 MSLink* link = (*i).myLink;
925 if (link != 0 && (*i).mySetRequest) {
931 vSafe = (*i).myVLinkWait;
933 lastWasGreenCont =
false;
942 vSafe = (*i).myVLinkWait;
944 lastWasGreenCont =
false;
952 vSafe = (*i).myVLinkPass;
955 lastWasGreenCont =
false;
956 vSafe = (*i).myVLinkWait;
964 vSafe = (*i).myVLinkWait;
1009 if (myHBMsgEmitter != 0) {
1015 if (myBMsgEmitter != 0) {
1026 std::vector<MSLane*> passedLanes;
1028 passedLanes.push_back(*i);
1030 if (passedLanes.size() == 0 || passedLanes.back() !=
myLane) {
1031 passedLanes.push_back(
myLane);
1046 MSLink* link = (*i).myLink;
1056 if (approachedLane !=
myLane && approachedLane != 0) {
1061 #ifdef HAVE_INTERNAL_LANES
1063 if (myLeaderForLink.find(link) != myLeaderForLink.end()) {
1064 myLinkLeaders.erase(myLeaderForLink[link]);
1065 myLeaderForLink.erase(link);
1073 passedLanes.push_back(approachedLane);
1079 (*i)->resetPartialOccupation(
this);
1099 std::vector<MSLane*>::reverse_iterator i = passedLanes.rbegin() + 1;
1100 while (leftLength > 0 && i != passedLanes.rend()) {
1102 leftLength -= (*i)->setPartialOccupation(
this, leftLength);
1116 for (MSLane::VehCont::const_iterator i = vehs.begin(); i != vehs.end(); ++i) {
1117 if ((*i)->getSpeed() < .1) {
1118 foundStopped =
true;
1119 SUMOReal ret = (*i)->getPositionOnLane() - (*i)->getVehicleType().getLengthWithGap() - lengths;
1123 lengths += (*i)->getVehicleType().getLengthWithGap();
1132 #ifdef DEBUG_VEHICLE_GUI_SELECTION
1140 #ifdef HAVE_INTERNAL_LANES
1142 int removalBegin = -1;
1143 bool hadVehicle =
false;
1144 SUMOReal seenSpace = -lengthsInFront;
1146 std::vector<SUMOReal> availableSpace;
1147 std::vector<bool> hadVehicles;
1148 bool foundStopped =
false;
1153 if (item.
myLink == 0 || foundStopped) {
1154 availableSpace.push_back(seenSpace);
1155 hadVehicles.push_back(hadVehicle);
1160 if (approachedLane != 0) {
1162 seenSpace = seenSpace - approachedLane->
getVehLenSum();
1168 availableSpace.push_back(seenSpace);
1169 hadVehicles.push_back(hadVehicle);
1178 availableSpace.push_back(m);
1182 foundStopped =
true;
1188 if (!foundStopped) {
1191 seenSpace = availableSpace.back();
1199 availableSpace.push_back(m);
1205 if (!foundStopped) {
1208 seenSpace = availableSpace.back();
1212 foundStopped =
true;
1216 hadVehicles.push_back(hadVehicle);
1218 #ifdef DEBUG_VEHICLE_GUI_SELECTION
1228 bool allowsContinuation = item.
myLink == 0 || item.
myLink->
isCont() || !hadVehicles[i] || opened;
1229 if (!opened && item.
myLink != 0) {
1233 allowsContinuation =
true;
1237 if (allowsContinuation) {
1238 availableSpace[i - 1] = availableSpace[i];
1242 for (
unsigned int i = 0; hadVehicle && i <
myLFLinkLanes.size() && removalBegin < 0; ++i) {
1256 if (leftSpace < 0/* && item.myLink->willHaveBlockedFoe()*/) {
1263 if (leftSpace < -impatienceCorrection / 10.) {
1284 if ((*i).myLink != 0) {
1286 (*i).myLink->setApproaching(
this, (*i).myArrivalTime, (*i).myArrivalSpeed, leaveSpeed, (*i).mySetRequest);
1295 if (rem->first->getLane() != 0 && rem->first->getLane() !=
getLane()) {
1298 if (rem->first->notifyEnter(*
this, reason)) {
1328 if (!onTeleporting) {
1350 if (myLCMsgEmitter != 0) {
1376 while (i != route.
begin() && leftLength > 0) {
1383 leftLength -= (lane)->setPartialOccupation(
this, leftLength);
1429 MSLane* clane = enteredLane;
1430 while (leftLength > 0) {
1436 leftLength -= (clane)->setPartialOccupation(
this, leftLength);
1444 if (rem->first->notifyLeave(*
this,
myState.
myPos + rem->second, reason)) {
1452 (*i)->resetPartialOccupation(
this);
1474 const std::vector<MSVehicle::LaneQ>&
1476 #ifdef DEBUG_VEHICLE_GUI_SELECTION
1483 if (startLane == 0) {
1486 assert(startLane != 0);
1492 std::vector<LaneQ>::iterator i;
1493 for (i = lanes.begin(); i != lanes.end(); ++i) {
1495 for (std::vector<MSLane*>::const_iterator j = (*i).bestContinuations.begin() + 1; j != (*i).bestContinuations.end(); ++j) {
1496 nextOccupation += (*j)->getVehLenSum();
1498 (*i).nextOccupation = nextOccupation;
1499 if ((*i).lane == startLane) {
1510 const MSEdge* nextStopEdge = 0;
1511 const MSLane* nextStopLane = 0;
1515 nextStopLane = nextStop.
lane;
1516 nextStopEdge = &nextStopLane->
getEdge();
1524 if (nextStopEdge != 0) {
1533 bool progress =
true;
1535 std::vector<LaneQ> currentLanes;
1536 const std::vector<MSLane*>* allowed = 0;
1537 const MSEdge* nextEdge = 0;
1539 nextEdge = *(ce + 1);
1542 const std::vector<MSLane*>& lanes = (*ce)->getLanes();
1543 for (std::vector<MSLane*>::const_iterator i = lanes.begin(); i != lanes.end(); ++i) {
1550 q.
allowsContinuation = allowed == 0 || find(allowed->begin(), allowed->end(), cl) != allowed->end();
1551 currentLanes.push_back(q);
1554 if (nextStopEdge == *ce) {
1556 for (std::vector<LaneQ>::iterator q = currentLanes.begin(); q != currentLanes.end(); ++q) {
1557 if (nextStopLane != 0 && nextStopLane != (*q).lane) {
1558 (*q).allowsContinuation =
false;
1559 (*q).length = nextStopPos;
1566 seenLength += currentLanes[0].lane->getLength();
1568 progress &= (seen <= 4 || seenLength < 3000);
1569 progress &= seen <= 8;
1581 int bestThisIndex = 0;
1584 for (std::vector<LaneQ>::iterator j = last.begin(); j != last.end(); ++j, ++index) {
1585 if ((*j).length > bestLength) {
1586 bestLength = (*j).length;
1587 bestThisIndex = index;
1591 for (std::vector<LaneQ>::iterator j = last.begin(); j != last.end(); ++j, ++index) {
1592 if ((*j).length < bestLength) {
1593 (*j).bestLaneOffset = bestThisIndex - index;
1600 for (std::vector<std::vector<LaneQ> >::reverse_iterator i =
myBestLanes.rbegin() + 1; i !=
myBestLanes.rend(); ++i) {
1601 std::vector<LaneQ>& nextLanes = (*(i - 1));
1602 std::vector<LaneQ>& clanes = (*i);
1603 MSEdge& cE = clanes[0].lane->getEdge();
1607 for (std::vector<LaneQ>::iterator j = nextLanes.begin(); j != nextLanes.end(); ++j, ++index) {
1608 if ((*j).lane->isApproachedFrom(&cE) && bestConnectedLength < (*j).length) {
1609 bestConnectedLength = (*j).length;
1611 if (bestLength < (*j).length) {
1612 bestLength = (*j).length;
1615 if (bestConnectedLength > 0) {
1616 int bestThisIndex = 0;
1618 for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
1619 LaneQ bestConnectedNext;
1620 bestConnectedNext.
length = -1;
1621 if ((*j).allowsContinuation) {
1622 for (std::vector<LaneQ>::const_iterator m = nextLanes.begin(); m != nextLanes.end(); ++m) {
1623 if ((*m).lane->isApproachedFrom(&cE, (*j).lane)) {
1624 if (bestConnectedNext.
length < (*m).length || (bestConnectedNext.
length == (*m).length &&
abs(bestConnectedNext.
bestLaneOffset) >
abs((*m).bestLaneOffset))) {
1625 bestConnectedNext = *m;
1630 (*j).
length += bestLength;
1632 (*j).length += bestConnectedNext.
length;
1635 if (clanes[bestThisIndex].length < (*j).length || (clanes[bestThisIndex].length == (*j).length &&
abs(
abs(clanes[bestThisIndex].bestLaneOffset > (*j).bestLaneOffset)))) {
1636 bestThisIndex = index;
1642 for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
1643 if ((*j).length < clanes[bestThisIndex].length || ((*j).length == clanes[bestThisIndex].length &&
abs((*j).bestLaneOffset) <
abs(clanes[bestThisIndex].bestLaneOffset))) {
1644 (*j).bestLaneOffset = bestThisIndex - index;
1646 (*j).bestLaneOffset = 0;
1652 int bestThisIndex = 0;
1653 int bestNextIndex = 0;
1654 int bestDistToNeeded = (
int) clanes.size();
1656 for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
1657 if ((*j).allowsContinuation) {
1659 for (std::vector<LaneQ>::const_iterator m = nextLanes.begin(); m != nextLanes.end(); ++m, ++nextIndex) {
1660 if ((*m).lane->isApproachedFrom(&cE, (*j).lane)) {
1661 if (bestDistToNeeded >
abs((*m).bestLaneOffset)) {
1662 bestDistToNeeded =
abs((*m).bestLaneOffset);
1663 bestThisIndex = index;
1664 bestNextIndex = nextIndex;
1670 clanes[bestThisIndex].length += nextLanes[bestNextIndex].length;
1671 copy(nextLanes[bestNextIndex].bestContinuations.begin(), nextLanes[bestNextIndex].bestContinuations.end(), back_inserter(clanes[bestThisIndex].bestContinuations));
1673 for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
1674 if ((*j).length < clanes[bestThisIndex].length || ((*j).length == clanes[bestThisIndex].length &&
abs((*j).bestLaneOffset) <
abs(clanes[bestThisIndex].bestLaneOffset))) {
1675 (*j).bestLaneOffset = bestThisIndex - index;
1677 (*j).bestLaneOffset = 0;
1686 std::vector<LaneQ>& currLanes = *
myBestLanes.begin();
1687 std::vector<LaneQ>::iterator i;
1688 for (i = currLanes.begin(); i != currLanes.end(); ++i) {
1690 for (std::vector<MSLane*>::const_iterator j = (*i).bestContinuations.begin() + 1; j != (*i).bestContinuations.end(); ++j) {
1691 nextOccupation += (*j)->getVehLenSum();
1693 (*i).nextOccupation = nextOccupation;
1694 if ((*i).lane == startLane) {
1702 const std::vector<MSLane*>&
1707 return (*myCurrentLaneInBestLanes).bestContinuations;
1711 const std::vector<MSLane*>&
1717 if ((*i).lane == l) {
1718 return (*i).bestContinuations;
1728 #ifdef DEBUG_VEHICLE_GUI_SELECTION
1733 if (
isOnRoad() && destEdge != NULL) {
1821 switch ((*link)->getDirection()) {
1850 std::vector<MSLane*>::const_iterator laneP = std::find((*myCurrEdge)->getLanes().begin(), (*myCurrEdge)->getLanes().end(),
myLane);
1851 return (
unsigned int) std::distance((*myCurrEdge)->getLanes().begin(), laneP);
1859 for (std::list<Stop>::iterator iter =
myStops.begin(); iter !=
myStops.end(); iter++) {
1860 if (iter->lane == lane && fabs(iter->endPos - pos) <
POSITION_EPS) {
1861 if (duration == 0 && !iter->reached) {
1864 iter->duration = duration;