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);
290 for (std::vector<SUMOVehicleParameter::Stop>::iterator i = pars->
stops.begin(); i != pars->
stops.end(); ++i) {
293 "' on lane '" + i->lane +
"' is not downstream the current route.");
296 for (std::vector<SUMOVehicleParameter::Stop>::const_iterator i = route->
getStops().begin(); i != route->
getStops().end(); ++i) {
299 "' on lane '" + i->lane +
"' is not downstream the current route.");
302 const MSLane*
const depLane = (*myCurrEdge)->getDepartLane(*
this);
304 throw ProcessError(
"Invalid departlane definition for vehicle '" + pars->
id +
"'.");
308 "' is too high for the departure lane '" + depLane->
getID() +
"'.");
312 "' is too high for the vehicle type '" + type->
getID() +
"'.");
328 if ((*i).myLink != 0) {
329 (*i).myLink->removeApproaching(
this);
369 for (std::list<Stop>::iterator iter =
myStops.begin(); iter !=
myStops.end();) {
370 if (find(edges.begin(), edges.end(), &iter->lane->getEdge()) == edges.end()) {
373 iter->edge = find(edges.begin(), edges.end(), &iter->lane->getEdge());
402 if (!rem->first->notifyMove(*
this, oldPos + rem->second, newPos + rem->second, newSpeed)) {
417 rem->second += oldLaneLength;
442 return atan2(p1.
x() - p2.
x(), p2.
y() - p1.
y()) * 180. /
PI;
459 if (stop.
until != -1) {
460 stop.
until += untilOffset;
472 std::list<Stop>::iterator iter =
myStops.begin();
475 prevStopEdge =
myStops.back().edge;
476 prevStopPos =
myStops.back().endPos;
482 while (iter !=
myStops.end() && (iter->edge < stop.
edge ||
483 (iter->endPos < stop.
endPos && iter->edge == stop.
edge))) {
484 prevStopEdge = iter->edge;
485 prevStopPos = iter->endPos;
489 int index = stopPar.
index;
491 prevStopEdge = iter->edge;
492 prevStopPos = iter->endPos;
526 return currentVelocity;
571 bool busStopsMustHaveSpace =
true;
576 busStopsMustHaveSpace =
false;
584 if (stop.
until >= 0) {
600 return currentVelocity;
610 if (myHBMsgEmitter != 0) {
617 #ifdef DEBUG_VEHICLE_GUI_SELECTION
624 if ((*i).myLink != 0) {
625 (*i).myLink->removeApproaching(
this);
634 if (!onAppropriateLane) {
658 if (predP != 0 && predP !=
this) {
675 vBeg =
MIN2(vBeg, vsafeStop);
683 if (!onAppropriateLane) {
696 #ifdef DEBUG_VEHICLE_GUI_SELECTION
706 DriveItemVector::iterator i;
707 bool braking =
false;
708 bool lastWasGreenCont =
false;
710 MSLink* link = (*i).myLink;
712 if (link != 0 && (*i).mySetRequest) {
718 vSafe = (*i).myVLinkWait;
720 lastWasGreenCont =
false;
725 const bool opened = yellow || link->
opened((*i).myArrivalTime, (*i).myArrivalSpeed,
getVehicleType().getLengthWithGap());
728 vSafe = (*i).myVLinkWait;
730 lastWasGreenCont =
false;
738 vSafe = (*i).myVLinkPass;
741 lastWasGreenCont =
false;
742 vSafe = (*i).myVLinkWait;
750 vSafe = (*i).myVLinkWait;
789 if (myHBMsgEmitter != 0) {
795 if (myBMsgEmitter != 0) {
805 std::vector<MSLane*> passedLanes;
807 passedLanes.push_back(*i);
809 if (passedLanes.size() == 0 || passedLanes.back() !=
myLane) {
810 passedLanes.push_back(
myLane);
826 MSLink* link = (*i).myLink;
834 #ifdef HAVE_INTERNAL_LANES
835 approachedLane = link->getViaLane();
836 if (approachedLane == 0) {
837 approachedLane = link->
getLane();
840 approachedLane = link->
getLane();
845 if (approachedLane !=
myLane && approachedLane != 0) {
852 passedLanes.push_back(approachedLane);
864 std::vector<MSLane*>::reverse_iterator i = passedLanes.rbegin() + 1;
865 while (leftLength > 0 && i != passedLanes.rend()) {
867 leftLength -= (*i)->setPartialOccupation(
this, leftLength);
881 for (MSLane::VehCont::const_iterator i = vehs.begin(); i != vehs.end(); ++i) {
882 if ((*i)->getSpeed() < .1) {
884 SUMOReal ret = (*i)->getPositionOnLane() - (*i)->getVehicleType().getLengthWithGap() - lengths;
888 lengths += (*i)->getVehicleType().getLengthWithGap();
897 #ifdef DEBUG_VEHICLE_GUI_SELECTION
905 #ifdef HAVE_INTERNAL_LANES
907 int removalBegin = -1;
908 bool hadVehicle =
false;
909 SUMOReal seenSpace = -lengthsInFront;
911 std::vector<SUMOReal> availableSpace;
912 std::vector<bool> hadVehicles;
913 bool foundStopped =
false;
918 if (item.
myLink == 0 || foundStopped) {
919 availableSpace.push_back(seenSpace);
920 hadVehicles.push_back(hadVehicle);
925 if (approachedLane != 0) {
933 availableSpace.push_back(seenSpace);
934 hadVehicles.push_back(hadVehicle);
943 availableSpace.push_back(m);
956 seenSpace = availableSpace.back();
964 availableSpace.push_back(m);
973 seenSpace = availableSpace.back();
981 hadVehicles.push_back(hadVehicle);
983 #ifdef DEBUG_VEHICLE_GUI_SELECTION
993 bool allowsContinuation = check1 || opened;
994 if (!opened && item.
myLink != 0) {
998 allowsContinuation =
true;
1002 if (allowsContinuation) {
1003 availableSpace[i - 1] = availableSpace[i];
1007 for (
unsigned int i = 0; hadVehicle && i <
myLFLinkLanes.size() && removalBegin < 0; ++i) {
1021 if (leftSpace < 0/* && item.myLink->willHaveBlockedFoe()*/) {
1028 if (leftSpace < -impatienceCorrection / 10.) {
1047 if ((*i).myLink != 0) {
1048 (*i).myLink->setApproaching(
this, (*i).myArrivalTime, (*i).myArrivalSpeed, (*i).mySetRequest);
1057 #ifdef DEBUG_VEHICLE_GUI_SELECTION
1090 #ifdef HAVE_INTERNAL_LANES
1091 bool hadNonInternal =
false;
1093 bool hadNonInternal =
true;
1096 unsigned int view = 1;
1105 vLinkPass =
MIN2(vLinkPass, vsafeStop);
1106 vLinkWait =
MIN2(vLinkWait, vsafeStop);
1110 MSLinkCont::const_iterator link =
myLane->
succLinkSec(*
this, view, *nextLane, bestLaneConts);
1121 laneEndVSafe = vLinkPass;
1130 vLinkWait = vLinkPass;
1134 #ifdef HAVE_INTERNAL_LANES
1135 nextLane = (*link)->getViaLane();
1136 if (nextLane == 0) {
1137 nextLane = (*link)->getLane();
1138 hadNonInternal =
true;
1142 nextLane = (*link)->getLane();
1152 SUMOReal vsafePredNextLane = 100000;
1154 if (lastOnNext.first != 0) {
1155 if (seen + lastOnNext.second >= 0) {
1158 vsafePredNextLane = cfModel.
stopSpeed(
this, seen - place);
1161 #ifdef DEBUG_VEHICLE_GUI_SELECTION
1167 vLinkPass =
MIN3(vLinkPass, vmaxNextLane, vsafePredNextLane);
1171 vLinkWait =
MIN3(vLinkPass, vLinkWait, cfModel.
stopSpeed(
this, seen - place));
1179 bool setRequest =
false;
1186 vLinkPass =
MIN2(vLinkPass, vsafeStop);
1187 vLinkWait =
MIN2(vLinkWait, vsafeStop);
1193 vLinkPass =
MIN2(vLinkPass, vsafe);
1194 vLinkWait =
MIN2(vLinkWait, vsafe);
1203 vLinkPass = vLinkWait;
1220 if (!setRequest || ((vLinkPass <= 0 || seen > dist) && hadNonInternal && seenNonInternal > 50)) {
1230 if (rem->first->getLane() != 0 && rem->first->getLane() !=
getLane()) {
1233 if (rem->first->notifyEnter(*
this, reason)) {
1254 if (!onTeleporting) {
1266 if (!onTeleporting) {
1284 if (myLCMsgEmitter != 0) {
1297 if (leftLength < 0) {
1302 while (i != route.
begin() && leftLength > 0) {
1303 const MSEdge*
const prev = *(--i);
1304 const std::vector<MSLane::IncomingLaneInfo> &incomingLanes = lane->
getIncomingLanes();
1305 for (std::vector<MSLane::IncomingLaneInfo>::const_iterator j = incomingLanes.begin(); j != incomingLanes.end(); ++j) {
1306 if (&(*j).lane->getEdge() == prev) {
1307 #ifdef HAVE_INTERNAL_LANES
1308 (*j).lane->setPartialOccupation(
this, leftLength);
1310 leftLength -= (*j).length;
1311 (*j).lane->setPartialOccupation(
this, leftLength);
1313 leftLength -= (*j).lane->getLength();
1347 MSLane* clane = enteredLane;
1348 while (leftLength > 0) {
1354 leftLength -= (clane)->setPartialOccupation(
this, leftLength);
1362 if (rem->first->notifyLeave(*
this,
myState.
myPos + rem->second, reason)) {
1370 (*i)->resetPartialOccupation(
this);
1392 const std::vector<MSVehicle::LaneQ> &
1394 #ifdef DEBUG_VEHICLE_GUI_SELECTION
1401 if (startLane == 0) {
1407 std::vector<LaneQ>::iterator i;
1408 for (i = lanes.begin(); i != lanes.end(); ++i) {
1410 for (std::vector<MSLane*>::const_iterator j = (*i).bestContinuations.begin() + 1; j != (*i).bestContinuations.end(); ++j) {
1411 nextOccupation += (*j)->getVehLenSum();
1413 (*i).nextOccupation = nextOccupation;
1414 if ((*i).lane == startLane) {
1425 const MSEdge* nextStopEdge = 0;
1426 const MSLane* nextStopLane = 0;
1430 nextStopLane = nextStop.
lane;
1431 nextStopEdge = &nextStopLane->
getEdge();
1443 bool progress =
true;
1445 std::vector<LaneQ> currentLanes;
1446 const std::vector<MSLane*> *allowed = 0;
1447 const MSEdge* nextEdge = 0;
1449 nextEdge = *(ce + 1);
1452 const std::vector<MSLane*> &lanes = (*ce)->getLanes();
1453 for (std::vector<MSLane*>::const_iterator i = lanes.begin(); i != lanes.end(); ++i) {
1460 q.
allowsContinuation = allowed == 0 || find(allowed->begin(), allowed->end(), cl) != allowed->end();
1461 currentLanes.push_back(q);
1464 if (nextStopEdge == *ce) {
1466 for (std::vector<LaneQ>::iterator q = currentLanes.begin(); q != currentLanes.end(); ++q) {
1467 if (nextStopLane != (*q).lane) {
1468 (*q).allowsContinuation =
false;
1469 (*q).length = nextStopPos;
1476 seenLength += currentLanes[0].lane->getLength();
1478 progress &= (seen <= 4 || seenLength < 3000);
1479 progress &= seen <= 8;
1491 int bestThisIndex = 0;
1494 for (std::vector<LaneQ>::iterator j = last.begin(); j != last.end(); ++j, ++index) {
1495 if ((*j).length > bestLength) {
1496 bestLength = (*j).length;
1497 bestThisIndex = index;
1501 for (std::vector<LaneQ>::iterator j = last.begin(); j != last.end(); ++j, ++index) {
1502 if ((*j).length < bestLength) {
1503 (*j).bestLaneOffset = bestThisIndex - index;
1510 for (std::vector<std::vector<LaneQ> >::reverse_iterator i =
myBestLanes.rbegin() + 1; i !=
myBestLanes.rend(); ++i) {
1511 std::vector<LaneQ> &nextLanes = (*(i - 1));
1512 std::vector<LaneQ> &clanes = (*i);
1513 MSEdge& cE = clanes[0].lane->getEdge();
1517 for (std::vector<LaneQ>::iterator j = nextLanes.begin(); j != nextLanes.end(); ++j, ++index) {
1518 if ((*j).lane->isApproachedFrom(&cE) && bestConnectedLength < (*j).length) {
1519 bestConnectedLength = (*j).length;
1521 if (bestLength < (*j).length) {
1522 bestLength = (*j).length;
1525 if (bestConnectedLength > 0) {
1526 int bestThisIndex = 0;
1528 for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
1529 LaneQ bestConnectedNext;
1530 bestConnectedNext.
length = -1;
1531 if ((*j).allowsContinuation) {
1532 for (std::vector<LaneQ>::const_iterator m = nextLanes.begin(); m != nextLanes.end(); ++m) {
1533 if ((*m).lane->isApproachedFrom(&cE, (*j).lane)) {
1534 if (bestConnectedNext.
length < (*m).length || (bestConnectedNext.
length == (*m).length &&
abs(bestConnectedNext.
bestLaneOffset) >
abs((*m).bestLaneOffset))) {
1535 bestConnectedNext = *m;
1540 (*j).
length += bestLength;
1542 (*j).length += bestConnectedNext.
length;
1545 if (clanes[bestThisIndex].length < (*j).length || (clanes[bestThisIndex].length == (*j).length &&
abs(
abs(clanes[bestThisIndex].bestLaneOffset > (*j).bestLaneOffset)))) {
1546 bestThisIndex = index;
1552 for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
1553 if ((*j).length < clanes[bestThisIndex].length || ((*j).length == clanes[bestThisIndex].length &&
abs((*j).bestLaneOffset) <
abs(clanes[bestThisIndex].bestLaneOffset))) {
1554 (*j).bestLaneOffset = bestThisIndex - index;
1556 (*j).bestLaneOffset = 0;
1562 int bestThisIndex = 0;
1563 int bestNextIndex = 0;
1564 int bestDistToNeeded = (
int) clanes.size();
1566 for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
1567 if ((*j).allowsContinuation) {
1569 for (std::vector<LaneQ>::const_iterator m = nextLanes.begin(); m != nextLanes.end(); ++m, ++nextIndex) {
1570 if ((*m).lane->isApproachedFrom(&cE, (*j).lane)) {
1571 if (bestDistToNeeded >
abs((*m).bestLaneOffset)) {
1572 bestDistToNeeded =
abs((*m).bestLaneOffset);
1573 bestThisIndex = index;
1574 bestNextIndex = nextIndex;
1580 clanes[bestThisIndex].length += nextLanes[bestNextIndex].length;
1581 copy(nextLanes[bestNextIndex].bestContinuations.begin(), nextLanes[bestNextIndex].bestContinuations.end(), back_inserter(clanes[bestThisIndex].bestContinuations));
1583 for (std::vector<LaneQ>::iterator j = clanes.begin(); j != clanes.end(); ++j, ++index) {
1584 if ((*j).length < clanes[bestThisIndex].length || ((*j).length == clanes[bestThisIndex].length &&
abs((*j).bestLaneOffset) <
abs(clanes[bestThisIndex].bestLaneOffset))) {
1585 (*j).bestLaneOffset = bestThisIndex - index;
1587 (*j).bestLaneOffset = 0;
1596 std::vector<LaneQ> &currLanes = *
myBestLanes.begin();
1597 std::vector<LaneQ>::iterator i;
1598 for (i = currLanes.begin(); i != currLanes.end(); ++i) {
1600 for (std::vector<MSLane*>::const_iterator j = (*i).bestContinuations.begin() + 1; j != (*i).bestContinuations.end(); ++j) {
1601 nextOccupation += (*j)->getVehLenSum();
1603 (*i).nextOccupation = nextOccupation;
1604 if ((*i).lane == startLane) {
1612 const std::vector<MSLane*> &
1617 return (*myCurrentLaneInBestLanes).bestContinuations;
1621 const std::vector<MSLane*> &
1623 for (std::vector<std::vector<LaneQ> >::const_iterator i =
myBestLanes.begin(); i !=
myBestLanes.end(); ++i) {
1624 if ((*i).size() != 0 && (*i)[0].lane == l) {
1625 return (*i)[0].bestContinuations;
1635 #ifdef DEBUG_VEHICLE_GUI_SELECTION
1640 if (
isOnRoad() && destEdge != NULL) {
1721 switch ((*link)->getDirection()) {
1750 std::vector<MSLane*>::const_iterator laneP = std::find((*myCurrEdge)->getLanes().begin(), (*myCurrEdge)->getLanes().end(),
myLane);
1751 return (
unsigned int) std::distance((*myCurrEdge)->getLanes().begin(), laneP);
1759 for (std::list<Stop>::iterator iter =
myStops.begin(); iter !=
myStops.end(); iter++) {
1760 if (iter->lane == lane && fabs(iter->endPos - pos) <
POSITION_EPS) {
1761 if (duration == 0 && !iter->reached) {
1764 iter->duration = duration;