diff --git a/src/microsim/MSVehicle.cpp b/src/microsim/MSVehicle.cpp index 7c09fc2ad6d..d737a9455ec 100644 --- a/src/microsim/MSVehicle.cpp +++ b/src/microsim/MSVehicle.cpp @@ -2155,6 +2155,30 @@ MSVehicle::planMoveInternal(const SUMOTime t, MSLeaderInfo ahead, DriveItemVecto #endif adaptToLeaderDistance(oppositeLeaders, 0, seen, lastLink, v, vLinkPass); } else { + if (MSGlobals::gLateralResolution > 0 + && (getLeftSideOnLane(lane) < 0 || getRightSideOnLane(lane) > lane->getWidth())) { + // if ego is driving outside lane bounds we must consider + // potential leaders that are also outside lane bounds + const bool outsideLeft = getLeftSideOnLane(lane) < 0; + if (outsideLeft) { + ahead.setSublaneOffset(ceil(-getLeftSideOnLane(lane) / MSGlobals::gLateralResolution) * MSGlobals::gLateralResolution); + } else if (getRightSideOnLane(lane) > lane->getWidth()) { + ahead.setSublaneOffset(-ceil((getRightSideOnLane(lane) - getWidth()) / MSGlobals::gLateralResolution) * MSGlobals::gLateralResolution); + } + for (const MSVehicle* cand : lane->getVehiclesSecure()) { + if ((lane != myLane || cand->getPositionOnLane() > getPositionOnLane()) + && ((outsideLeft && cand->getLeftSideOnLane() < 0) + || (!outsideLeft && cand->getRightSideOnEdge() > lane->getWidth()))) { + ahead.addLeader(cand, true); +#ifdef DEBUG_PLAN_MOVE + if (DEBUG_COND) { + std::cout << SIMTIME << " veh=" << getID() << " offset=" << ahead.getSublaneOffset() << " outsideLeft=" << outsideLeft << " outsideLeader=" << cand->getID() << "\n"; + } +#endif + } + } + lane->releaseVehicles(); + } adaptToLeaders(ahead, lateralShift, seen, lastLink, leaderLane, v, vLinkPass); } if (lastLink != nullptr) { @@ -2742,6 +2766,7 @@ MSVehicle::adaptToLeaders(const MSLeaderInfo& ahead, double latOffset, << " latOffset=" << latOffset << " rm=" << rightmost << " lm=" << leftmost + << " shift=" << ahead.getSublaneOffset() << " ahead=" << ahead.toString() << "\n"; #endif @@ -6021,6 +6046,18 @@ MSVehicle::getLeftSideOnLane() const { } +double +MSVehicle::getRightSideOnLane(const MSLane* lane) const { + return myState.myPosLat + 0.5 *lane->getWidth() - 0.5 * getVehicleType().getWidth(); +} + + +double +MSVehicle::getLeftSideOnLane(const MSLane* lane) const { + return myState.myPosLat + 0.5 * lane->getWidth() + 0.5 * getVehicleType().getWidth(); +} + + double MSVehicle::getRightSideOnEdge(const MSLane* lane) const { return getCenterOnEdge(lane) - 0.5 * getVehicleType().getWidth(); diff --git a/src/microsim/MSVehicle.h b/src/microsim/MSVehicle.h index 010342fbdd8..22036379915 100644 --- a/src/microsim/MSVehicle.h +++ b/src/microsim/MSVehicle.h @@ -432,6 +432,18 @@ class MSVehicle : public MSBaseVehicle { */ double getLeftSideOnLane() const; + /** @brief Get the lateral position of the vehicles right side on the lane: + * @return The lateral position of the vehicle (in m distance between right + * side of vehicle and right side of the lane it is on + */ + double getRightSideOnLane(const MSLane* lane) const; + + /** @brief Get the lateral position of the vehicles left side on the lane: + * @return The lateral position of the vehicle (in m distance between left + * side of vehicle and right side of the lane it is on + */ + double getLeftSideOnLane(const MSLane* lane) const; + /** @brief Get the minimal lateral distance required to move fully onto the lane at given offset * @return The lateral distance to be covered to move the vehicle fully onto the lane (in m) */