Turning angles with continuous lane changes and lcSigma

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();

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;

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();
     }
 

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;
 }
 
 

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;
 

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(),