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) {}
156 mySpeedAdaptationStarted =
true;
157 mySpeedTimeLine = speedTimeLine;
163 myLaneTimeLine = laneTimeLine;
170 myOriginalSpeed = speed;
172 while (mySpeedTimeLine.size() == 1 || (mySpeedTimeLine.size() > 1 && currentTime > mySpeedTimeLine[1].first)) {
173 mySpeedTimeLine.erase(mySpeedTimeLine.begin());
176 if (mySpeedTimeLine.size() < 2 || currentTime < mySpeedTimeLine[0].first) {
180 if (!mySpeedAdaptationStarted) {
181 mySpeedTimeLine[0].second = speed;
182 mySpeedAdaptationStarted =
true;
186 speed = mySpeedTimeLine[0].second - (mySpeedTimeLine[0].second - mySpeedTimeLine[1].second) * td;
187 if (myConsiderSafeVelocity) {
188 speed =
MIN2(speed, vSafe);
190 if (myConsiderMaxAcceleration) {
191 speed =
MIN2(speed, vMax);
193 if (myConsiderMaxDeceleration) {
194 speed =
MAX2(speed, vMin);
203 while (myLaneTimeLine.size() == 1 || (myLaneTimeLine.size() > 1 && currentTime > myLaneTimeLine[1].first)) {
204 myLaneTimeLine.erase(myLaneTimeLine.begin());
207 if (myLaneTimeLine.size() < 2 || currentTime < myLaneTimeLine[0].first) {
210 unsigned int destinationLaneIndex = myLaneTimeLine[1].second;
211 if ((
unsigned int)currentEdge.
getLanes().size() <= destinationLaneIndex) {
214 if (currentLaneIndex > destinationLaneIndex) {
216 }
else if (currentLaneIndex < destinationLaneIndex) {
226 myConsiderSafeVelocity = value;
232 myConsiderMaxAcceleration = value;
238 myConsiderMaxDeceleration = value;
251 (*i)->resetPartialOccupation(
this);
254 if ((*i).myLink != 0) {
255 (*i).myLink->removeApproaching(
this);
291 for (std::vector<SUMOVehicleParameter::Stop>::iterator i = pars->
stops.begin(); i != pars->
stops.end(); ++i) {
294 "' on lane '" + i->lane +
"' is not downstream the current route.");
297 for (std::vector<SUMOVehicleParameter::Stop>::const_iterator i = route->
getStops().begin(); i != route->
getStops().end(); ++i) {
300 "' on lane '" + i->lane +
"' is not downstream the current route.");
303 const MSLane*
const depLane = (*myCurrEdge)->getDepartLane(*
this);
305 throw ProcessError(
"Invalid departlane definition for vehicle '" + pars->
id +
"'.");
309 "' is too high for the departure lane '" + depLane->
getID() +
"'.");
313 "' is too high for the vehicle type '" + type->
getID() +
"'.");
329 if ((*i).myLink != 0) {
330 (*i).myLink->removeApproaching(
this);
370 for (std::list<Stop>::iterator iter =
myStops.begin(); iter !=
myStops.end();) {
371 if (find(edges.begin(), edges.end(), &iter->lane->getEdge()) == edges.end()) {
374 iter->edge = find(edges.begin(), edges.end(), &iter->lane->getEdge());
430 if (!rem->first->notifyMove(*
this, oldPos + rem->second, newPos + rem->second, newSpeed)) {
445 rem->second += oldLaneLength;
470 return atan2(p1.
x() - p2.
x(), p2.
y() - p1.
y()) * 180. /
PI;
487 if (stop.
until != -1) {
488 stop.
until += untilOffset;
500 std::list<Stop>::iterator iter =
myStops.begin();
503 prevStopEdge =
myStops.back().edge;
504 prevStopPos =
myStops.back().endPos;
510 while (iter !=
myStops.end() && (iter->edge < stop.
edge ||
511 (iter->endPos < stop.
endPos && iter->edge == stop.
edge))) {
512 prevStopEdge = iter->edge;
513 prevStopPos = iter->endPos;
517 int index = stopPar.
index;
519 prevStopEdge = iter->edge;
520 prevStopPos = iter->endPos;
528 (prevStopEdge == stop.
edge && prevStopPos > stop.
endPos)) {
555 return currentVelocity;
565 for (std::vector<MSPerson*>::const_iterator i = persons.begin(); i != persons.end(); ++i) {
606 bool busStopsMustHaveSpace =
true;
611 busStopsMustHaveSpace =
false;
619 if (stop.
until >= 0) {
635 return currentVelocity;
642 if (myHBMsgEmitter != 0) {
649 #ifdef DEBUG_VEHICLE_GUI_SELECTION
656 if ((*i).myLink != 0) {
657 (*i).myLink->removeApproaching(
this);
678 #ifdef HAVE_INTERNAL_LANES
679 bool hadNonInternal =
false;
681 bool hadNonInternal =
true;
686 unsigned int view = 0;
687 bool firstLane =
true;
689 std::pair<MSVehicle*, SUMOReal> leaderInfo = pred != 0 ? std::pair<MSVehicle*, SUMOReal>(pred,
gap2pred(*pred)) : std::pair<MSVehicle*, SUMOReal>((
MSVehicle*) 0, 0);
698 }
else if (leaderInfo.first == 0) {
704 if (leaderInfo.first != 0) {
706 if (leaderInfo.second >= 0) {
707 vsafeLeader = cfModel.
followSpeed(
this,
getSpeed(), leaderInfo.second, leaderInfo.first->getSpeed(), leaderInfo.first->getCarFollowModel().getMaxDecel());
716 v =
MIN2(v, vsafeLeader);
728 v =
MIN2(v, stopSpeed);
735 MSLinkCont::const_iterator link =
myLane->
succLinkSec(*
this, view + 1, *lane, bestLaneConts);
737 bool onAppropriateLane = !lane->
isLinkEnd(link);
751 if (!onAppropriateLane) {
765 bool setRequest = v > 0;
781 lane = (*link)->getViaLaneOrLane();
782 #ifdef HAVE_INTERNAL_LANES
783 if ((*link)->getViaLane() == 0) {
784 hadNonInternal =
true;
796 if (!setRequest || ((vLinkPass <= 0 || seen > dist) && hadNonInternal && seenNonInternal > vehicleLength * 2)) {
803 v =
MIN2(va, vLinkPass);
813 #ifdef DEBUG_VEHICLE_GUI_SELECTION
823 DriveItemVector::iterator i;
824 bool braking =
false;
825 bool lastWasGreenCont =
false;
827 MSLink* link = (*i).myLink;
829 if (link != 0 && (*i).mySetRequest) {
835 vSafe = (*i).myVLinkWait;
837 lastWasGreenCont =
false;
845 vSafe = (*i).myVLinkWait;
847 lastWasGreenCont =
false;
855 vSafe = (*i).myVLinkPass;
858 lastWasGreenCont =
false;
859 vSafe = (*i).myVLinkWait;
867 vSafe = (*i).myVLinkWait;
905 if (myHBMsgEmitter != 0) {
911 if (myBMsgEmitter != 0) {
922 std::vector<MSLane*> passedLanes;
924 passedLanes.push_back(*i);
926 if (passedLanes.size() == 0 || passedLanes.back() !=
myLane) {
927 passedLanes.push_back(
myLane);
942 MSLink* link = (*i).myLink;
952 if (approachedLane !=
myLane && approachedLane != 0) {
962 passedLanes.push_back(approachedLane);
968 (*i)->resetPartialOccupation(
this);
981 std::vector<MSLane*>::reverse_iterator i = passedLanes.rbegin() + 1;
982 while (leftLength > 0 && i != passedLanes.rend()) {
984 leftLength -= (*i)->setPartialOccupation(
this, leftLength);
998 for (MSLane::VehCont::const_iterator i = vehs.begin(); i != vehs.end(); ++i) {
999 if ((*i)->getSpeed() < .1) {
1000 foundStopped =
true;
1001 SUMOReal ret = (*i)->getPositionOnLane() - (*i)->getVehicleType().getLengthWithGap() - lengths;
1005 lengths += (*i)->getVehicleType().getLengthWithGap();
1014 #ifdef DEBUG_VEHICLE_GUI_SELECTION
1022 #ifdef HAVE_INTERNAL_LANES
1024 int removalBegin = -1;
1025 bool hadVehicle =
false;
1026 SUMOReal seenSpace = -lengthsInFront;
1028 std::vector<SUMOReal> availableSpace;
1029 std::vector<bool> hadVehicles;
1030 bool foundStopped =
false;
1035 if (item.
myLink == 0 || foundStopped) {
1036 availableSpace.push_back(seenSpace);
1037 hadVehicles.push_back(hadVehicle);
1042 if (approachedLane != 0) {
1044 seenSpace = seenSpace - approachedLane->
getVehLenSum();
1050 availableSpace.push_back(seenSpace);
1051 hadVehicles.push_back(hadVehicle);
1060 availableSpace.push_back(m);
1064 foundStopped =
true;
1070 if (!foundStopped) {
1073 seenSpace = availableSpace.back();
1081 availableSpace.push_back(m);
1087 if (!foundStopped) {
1090 seenSpace = availableSpace.back();
1094 foundStopped =
true;
1098 hadVehicles.push_back(hadVehicle);
1100 #ifdef DEBUG_VEHICLE_GUI_SELECTION
1110 bool allowsContinuation = item.
myLink == 0 || item.
myLink->
isCont() || !hadVehicles[i] || opened;
1111 if (!opened && item.
myLink != 0) {
1115 allowsContinuation =
true;
1119 if (allowsContinuation) {
1120 availableSpace[i - 1] = availableSpace[i];
1124 for (
unsigned int i = 0; hadVehicle && i <
myLFLinkLanes.size() && removalBegin < 0; ++i) {
1138 if (leftSpace < 0/* && item.myLink->willHaveBlockedFoe()*/) {
1145 if (leftSpace < -impatienceCorrection / 10.) {
1166 if ((*i).myLink != 0) {
1167 (*i).myLink->setApproaching(
this, (*i).myArrivalTime, (*i).myArrivalSpeed, (*i).mySetRequest);
1176 if (rem->first->getLane() != 0 && rem->first->getLane() !=
getLane()) {
1179 if (rem->first->notifyEnter(*
this, reason)) {
1210 if (!onTeleporting) {
1232 if (myLCMsgEmitter != 0) {
1256 while (i != route.
begin() && leftLength > 0) {
1263 leftLength -= (lane)->setPartialOccupation(
this, leftLength);
1309 MSLane* clane = enteredLane;
1310 while (leftLength > 0) {
1316 leftLength -= (clane)->setPartialOccupation(
this, leftLength);
1324 if (rem->first->notifyLeave(*
this,
myState.
myPos + rem->second, reason)) {
1332 (*i)->resetPartialOccupation(
this);
1354 const std::vector<MSVehicle::LaneQ>&
1356 #ifdef DEBUG_VEHICLE_GUI_SELECTION
1363 if (startLane == 0) {
1369 std::vector<LaneQ>::iterator i;
1370 for (i = lanes.begin(); i != lanes.end(); ++i) {
1372 for (std::vector<MSLane*>::const_iterator j = (*i).bestContinuations.begin() + 1; j != (*i).bestContinuations.end(); ++j) {
1373 nextOccupation += (*j)->getVehLenSum();
1375 (*i).nextOccupation = nextOccupation;
1376 if ((*i).lane == startLane) {
1387 const MSEdge* nextStopEdge = 0;
1388 const MSLane* nextStopLane = 0;
1392 nextStopLane = nextStop.
lane;
1393 nextStopEdge = &nextStopLane->
getEdge();
1405 bool progress =
true;
1407 std::vector<LaneQ> currentLanes;
1408 const std::vector<MSLane*>* allowed = 0;
1409 const MSEdge* nextEdge = 0;
1411 nextEdge = *(ce + 1);
1414 const std::vector<MSLane*>& lanes = (*ce)->getLanes();
1415 for (std::vector<MSLane*>::const_iterator i = lanes.begin(); i != lanes.end(); ++i) {
1422 q.
allowsContinuation = allowed == 0 || find(allowed->begin(), allowed->end(), cl) != allowed->end();
1423 currentLanes.push_back(q);
1426 if (nextStopEdge == *ce) {
1428 for (std::vector<LaneQ>::iterator q = currentLanes.begin(); q != currentLanes.end(); ++q) {
1429 if (nextStopLane != 0 && nextStopLane != (*q).lane) {
1430 (*q).allowsContinuation =
false;
1431 (*q).length = nextStopPos;
1438 seenLength += currentLanes[0].lane->getLength();
1440 progress &= (seen <= 4 || seenLength < 3000);
1441 progress &= seen <= 8;
1453 int bestThisIndex = 0;
1456 for (std::vector<LaneQ>::iterator j = last.begin(); j != last.end(); ++j, ++index) {
1457 if ((*j).length > bestLength) {
1458 bestLength = (*j).length;
1459 bestThisIndex = index;
1463 for (std::vector<LaneQ>::iterator j = last.begin(); j != last.end(); ++j, ++index) {
1464 if ((*j).length < bestLength) {
1465 (*j).bestLaneOffset = bestThisIndex - index;
1472 for (std::vector<std::vector<LaneQ> >::reverse_iterator i =
myBestLanes.rbegin() + 1; i !=
myBestLanes.rend(); ++i) {
1473 std::vector<LaneQ>& nextLanes = (*(i - 1));
1474 std::vector<LaneQ>& clanes = (*i);
1475 MSEdge& cE = clanes[0].lane->getEdge();
1479 for (std::vector<LaneQ>::iterator j = nextLanes.begin(); j != nextLanes.end(); ++j, ++index) {
1480 if ((*j).lane->isApproachedFrom(&cE) && bestConnectedLength < (*j).length) {
1481 bestConnectedLength = (*j).length;
1483 if (bestLength < (*j).length) {
1484 bestLength = (*j).length;
1487 if (bestConnectedLength > 0) {
1488 int bestThisIndex = 0;
1490 for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
1491 LaneQ bestConnectedNext;
1492 bestConnectedNext.
length = -1;
1493 if ((*j).allowsContinuation) {
1494 for (std::vector<LaneQ>::const_iterator m = nextLanes.begin(); m != nextLanes.end(); ++m) {
1495 if ((*m).lane->isApproachedFrom(&cE, (*j).lane)) {
1496 if (bestConnectedNext.
length < (*m).length || (bestConnectedNext.
length == (*m).length &&
abs(bestConnectedNext.
bestLaneOffset) >
abs((*m).bestLaneOffset))) {
1497 bestConnectedNext = *m;
1502 (*j).
length += bestLength;
1504 (*j).length += bestConnectedNext.
length;
1507 if (clanes[bestThisIndex].length < (*j).length || (clanes[bestThisIndex].length == (*j).length &&
abs(
abs(clanes[bestThisIndex].bestLaneOffset > (*j).bestLaneOffset)))) {
1508 bestThisIndex = index;
1514 for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
1515 if ((*j).length < clanes[bestThisIndex].length || ((*j).length == clanes[bestThisIndex].length &&
abs((*j).bestLaneOffset) <
abs(clanes[bestThisIndex].bestLaneOffset))) {
1516 (*j).bestLaneOffset = bestThisIndex - index;
1518 (*j).bestLaneOffset = 0;
1524 int bestThisIndex = 0;
1525 int bestNextIndex = 0;
1526 int bestDistToNeeded = (
int) clanes.size();
1528 for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
1529 if ((*j).allowsContinuation) {
1531 for (std::vector<LaneQ>::const_iterator m = nextLanes.begin(); m != nextLanes.end(); ++m, ++nextIndex) {
1532 if ((*m).lane->isApproachedFrom(&cE, (*j).lane)) {
1533 if (bestDistToNeeded >
abs((*m).bestLaneOffset)) {
1534 bestDistToNeeded =
abs((*m).bestLaneOffset);
1535 bestThisIndex = index;
1536 bestNextIndex = nextIndex;
1542 clanes[bestThisIndex].length += nextLanes[bestNextIndex].length;
1543 copy(nextLanes[bestNextIndex].bestContinuations.begin(), nextLanes[bestNextIndex].bestContinuations.end(), back_inserter(clanes[bestThisIndex].bestContinuations));
1545 for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
1546 if ((*j).length < clanes[bestThisIndex].length || ((*j).length == clanes[bestThisIndex].length &&
abs((*j).bestLaneOffset) <
abs(clanes[bestThisIndex].bestLaneOffset))) {
1547 (*j).bestLaneOffset = bestThisIndex - index;
1549 (*j).bestLaneOffset = 0;
1558 std::vector<LaneQ>& currLanes = *
myBestLanes.begin();
1559 std::vector<LaneQ>::iterator i;
1560 for (i = currLanes.begin(); i != currLanes.end(); ++i) {
1562 for (std::vector<MSLane*>::const_iterator j = (*i).bestContinuations.begin() + 1; j != (*i).bestContinuations.end(); ++j) {
1563 nextOccupation += (*j)->getVehLenSum();
1565 (*i).nextOccupation = nextOccupation;
1566 if ((*i).lane == startLane) {
1574 const std::vector<MSLane*>&
1579 return (*myCurrentLaneInBestLanes).bestContinuations;
1583 const std::vector<MSLane*>&
1589 if ((*i).lane == l) {
1590 return (*i).bestContinuations;
1600 #ifdef DEBUG_VEHICLE_GUI_SELECTION
1605 if (
isOnRoad() && destEdge != NULL) {
1693 switch ((*link)->getDirection()) {
1722 std::vector<MSLane*>::const_iterator laneP = std::find((*myCurrEdge)->getLanes().begin(), (*myCurrEdge)->getLanes().end(),
myLane);
1723 return (
unsigned int) std::distance((*myCurrEdge)->getLanes().begin(), laneP);
1731 for (std::list<Stop>::iterator iter =
myStops.begin(); iter !=
myStops.end(); iter++) {
1732 if (iter->lane == lane && fabs(iter->endPos - pos) <
POSITION_EPS) {
1733 if (duration == 0 && !iter->reached) {
1736 iter->duration = duration;