diff --git a/src/microsim/MSLaneChanger.cpp b/src/microsim/MSLaneChanger.cpp index 1dd2be1b92ff..ef300566b764 100644 --- a/src/microsim/MSLaneChanger.cpp +++ b/src/microsim/MSLaneChanger.cpp @@ -292,8 +292,9 @@ MSLaneChanger::change() { if (vehicle->getLaneChangeModel().isChangingLanes() && !vehicle->getLaneChangeModel().alreadyChanged()) { return continueChange(vehicle, myCandi); + } else if (!(vehicle->getLaneChangeModel().getSigmaLat() > 0)) { + vehicle->getLaneChangeModel().setSpeedLat(0); } - vehicle->getLaneChangeModel().setSpeedLat(0); if (!myAllowsChanging || vehicle->getLaneChangeModel().alreadyChanged() || vehicle->isStoppedOnLane()) { registerUnchanged(vehicle); if (vehicle->isStoppedOnLane()) { @@ -489,6 +490,7 @@ MSLaneChanger::continueChange(MSVehicle* vehicle, ChangerIt& from) { } if (!lcm.isChangingLanes()) { vehicle->myState.myPosLat = 0; + lcm.setSpeedLat(0.0); lcm.endLaneChangeManeuver(); } lcm.updateShadowLane(); diff --git a/src/microsim/MSLaneChangerSublane.cpp b/src/microsim/MSLaneChangerSublane.cpp index 0b029d2d089b..9c5772f039ea 100644 --- a/src/microsim/MSLaneChangerSublane.cpp +++ b/src/microsim/MSLaneChangerSublane.cpp @@ -470,7 +470,30 @@ MSLaneChangerSublane::startChangeSublane(MSVehicle* vehicle, ChangerIt& from, do // compute new angle of the vehicle from the x- and y-distances travelled within last time step // (should happen last because primaryLaneChanged() also triggers angle computation) // this part of the angle comes from the orientation of our current lane - double laneAngle = vehicle->getLane()->getShape().rotationAtOffset(vehicle->getLane()->interpolateLanePosToGeometryPos(vehicle->getPositionOnLane())) ; + const double posLat = -vehicle->myState.myPosLat; // @todo get rid of the '-' + const double lefthandSign = (MSGlobals::gLefthand ? -1 : 1); + Position p1 = vehicle->getLane()->geometryPositionAtOffset(vehicle->myState.myPos, lefthandSign * posLat); + if (p1 == Position::INVALID && vehicle->getLane()->getShape().length2D() == 0. && vehicle->getLane()->isInternal()) { + // workaround: extrapolate the preceding lane shape + MSLane* predecessorLane = vehicle->getLane()->getCanonicalPredecessorLane(); + p1 = predecessorLane->geometryPositionAtOffset(predecessorLane->getLength() + vehicle->myState.myPos, lefthandSign * posLat); + } + Position p2 = vehicle->getBackPosition(); + if (p2 == Position::INVALID) { + // Handle special case of vehicle's back reaching out of the network + if (vehicle->getFurtherLanes().size() > 0) { + p2 = vehicle->getFurtherLanes().back()->geometryPositionAtOffset(0, -vehicle->getFurtherLanesPosLat().back()); + if (p2 == Position::INVALID) { + // unsuitable lane geometry + p2 = vehicle->getLane()->geometryPositionAtOffset(0, posLat); + } + } else { + p2 = vehicle->getLane()->geometryPositionAtOffset(0, posLat); + } + } + double laneAngle = (p1 != p2 ? p2.angleTo2D(p1) : + vehicle->getLane()->getShape().rotationAtOffset(vehicle->getLane()->interpolateLanePosToGeometryPos(vehicle->getPositionOnLane()))); + if (vehicle->getLane()->getShape().length2D() == 0) { if (vehicle->getFurtherLanes().size() == 0) { laneAngle = vehicle->getAngle(); @@ -479,15 +502,11 @@ MSLaneChangerSublane::startChangeSublane(MSVehicle* vehicle, ChangerIt& from, do } } // this part of the angle comes from the vehicle's lateral movement - double changeAngle = 0; - // avoid flicker - if (fabs(latDist) > NUMERICAL_EPS) { - // angle is between vehicle front and vehicle back (and depending on travelled distance) - changeAngle = atan2(DIST2SPEED(latDist), vehicle->getVehicleType().getLength() + vehicle->getSpeed()); - if (MSGlobals::gLefthand) { - changeAngle *= -1; - } + double changeAngle = vehicle->getLaneChangeModel().calcAngleOffset(); + if (MSGlobals::gLefthand) { + changeAngle *= -1; } + if (vehicle->getLaneChangeModel().isOpposite()) { // reverse lane angle laneAngle += M_PI; diff --git a/src/microsim/MSVehicle.cpp b/src/microsim/MSVehicle.cpp index 61fa24299598..812f2b2c1bd9 100644 --- a/src/microsim/MSVehicle.cpp +++ b/src/microsim/MSVehicle.cpp @@ -1425,9 +1425,9 @@ MSVehicle::computeAngle() const { } double result = (p1 != p2 ? p2.angleTo2D(p1) : myLane->getShape().rotationAtOffset(myLane->interpolateLanePosToGeometryPos(getPositionOnLane()))); - if (myLaneChangeModel->isChangingLanes()) { - result += lefthandSign * DEG2RAD(myLaneChangeModel->getAngleOffset()); - } + + result += lefthandSign * myLaneChangeModel->calcAngleOffset(); + #ifdef DEBUG_FURTHER if (DEBUG_COND) { std::cout << SIMTIME << " computeAngle veh=" << getID() << " p1=" << p1 << " p2=" << p2 << " angle=" << RAD2DEG(result) << " naviDegree=" << GeomHelper::naviDegree(result) << "\n"; @@ -4308,6 +4308,7 @@ MSVehicle::executeMove() { } #endif } + myLaneChangeModel->setPreviousAngleOffset(myLaneChangeModel->getAngleOffset()); myAngle = computeAngle(); } diff --git a/src/microsim/lcmodels/MSAbstractLaneChangeModel.cpp b/src/microsim/lcmodels/MSAbstractLaneChangeModel.cpp index e3cc5d05bcb1..b90a95c7bc37 100644 --- a/src/microsim/lcmodels/MSAbstractLaneChangeModel.cpp +++ b/src/microsim/lcmodels/MSAbstractLaneChangeModel.cpp @@ -107,6 +107,8 @@ MSAbstractLaneChangeModel::MSAbstractLaneChangeModel(MSVehicle& v, const LaneCha myCanceledStateLeft(LCA_NONE), mySpeedLat(0), myAccelerationLat(0), + myAngleOffset(0), + myPreviousAngleOffset(0), myCommittedSpeed(0), myLaneChangeCompletion(1.0), myLaneChangeDirection(0), @@ -723,13 +725,18 @@ MSAbstractLaneChangeModel::determineTargetLane(int& targetDir) const { double -MSAbstractLaneChangeModel::getAngleOffset() const { - const double totalDuration = (myVehicle.getVehicleType().wasSet(VTYPEPARS_MAXSPEED_LAT_SET) - ? SUMO_const_laneWidth / myVehicle.getVehicleType().getMaxSpeedLat() - : STEPS2TIME(MSGlobals::gLaneChangeDuration)); +MSAbstractLaneChangeModel::calcAngleOffset() { + double result = 0.; + if (!(fabs(mySpeedLat) < NUMERICAL_EPS && fabs(myPreviousAngleOffset * 180 / PI) < NUMERICAL_EPS)) { + if (myVehicle.getLength() < sqrt(SPEED2DIST(mySpeedLat) * SPEED2DIST(mySpeedLat) + SPEED2DIST(myVehicle.getSpeed()) * SPEED2DIST(myVehicle.getSpeed()))) { + result = atan2(mySpeedLat, myVehicle.getSpeed()); + } else { + result = myPreviousAngleOffset + asin((sin(PI / 2 - myPreviousAngleOffset) * (SPEED2DIST(mySpeedLat) - tan(myPreviousAngleOffset) * SPEED2DIST(myVehicle.getSpeed()))) / myVehicle.getLength()); + } + } - const double angleOffset = 60 / totalDuration * (pastMidpoint() ? 1 - myLaneChangeCompletion : myLaneChangeCompletion); - return myLaneChangeDirection * angleOffset; + myAngleOffset = result; + return result; } diff --git a/src/microsim/lcmodels/MSAbstractLaneChangeModel.h b/src/microsim/lcmodels/MSAbstractLaneChangeModel.h index 8f6d8fdca2b5..90c77639d463 100644 --- a/src/microsim/lcmodels/MSAbstractLaneChangeModel.h +++ b/src/microsim/lcmodels/MSAbstractLaneChangeModel.h @@ -468,7 +468,22 @@ class MSAbstractLaneChangeModel { int getShadowDirection() const; /// @brief return the angle offset during a continuous change maneuver - double getAngleOffset() const; + double calcAngleOffset(); + + /// @brief return the angle offset resulting from lane change and sigma + inline double getAngleOffset() const { + return myAngleOffset; + } + + /// @brief set the angle offset resulting from lane change and sigma + inline void setAngleOffset(const double angleOffset) { + myAngleOffset = angleOffset; + } + + /// @brief set the angle offset of the previous time step + inline void setPreviousAngleOffset(const double angleOffset) { + myPreviousAngleOffset = angleOffset; + } /// @brief reset the flag whether a vehicle already moved to false inline bool alreadyChanged() const { @@ -565,6 +580,11 @@ class MSAbstractLaneChangeModel { return myAccelerationLat; } + /// @brief return the factor for lane keeping imperfection + double getSigmaLat() const { + return mySigma; + } + /// @brief set the lateral speed and update lateral acceleraton void setSpeedLat(double speedLat); @@ -650,6 +670,12 @@ class MSAbstractLaneChangeModel { /// @brief the current lateral acceleration double myAccelerationLat; + /// @brief the current angle offset resulting from lane change and sigma + double myAngleOffset; + + /// @brief the angle offset of the previous time step resulting from lane change and sigma + double myPreviousAngleOffset; + /// @brief the speed when committing to a change maneuver double myCommittedSpeed; diff --git a/src/microsim/lcmodels/MSLCM_LC2013.cpp b/src/microsim/lcmodels/MSLCM_LC2013.cpp index 61f5d5120d7d..afb01185ad38 100644 --- a/src/microsim/lcmodels/MSLCM_LC2013.cpp +++ b/src/microsim/lcmodels/MSLCM_LC2013.cpp @@ -1076,6 +1076,7 @@ MSLCM_LC2013::prepareStep() { scaledDelta = deltaPosLat * myVehicle.getSpeed() / myVehicle.getLane()->getSpeedLimit(); } myVehicle.setLateralPositionOnLane(oldPosLat + scaledDelta); + myVehicle.getLaneChangeModel().setSpeedLat(DIST2SPEED(scaledDelta)); } } @@ -1984,7 +1985,7 @@ MSLCM_LC2013::slowDownForBlocked(MSVehicle** blocked, int state) { } #endif } /* else { - // experimental else-branch... + // experimental else-branch... state |= LCA_AMBACKBLOCKER; myVSafes.push_back(getCarFollowModel().followSpeed( &myVehicle, myVehicle.getSpeed(),