From 1710da37aa0352af97eaad43660b0aa9b992fc24 Mon Sep 17 00:00:00 2001
From: Leo <leonhard.luecken@dlr.de>
Date: Thu, 26 Apr 2018 11:22:27 +0200
Subject: [PATCH] Added MSLCM_SL2015::myTurnAlignmentDistance and dynamic
 alignment adaption to upcoming turns.

---
 src/microsim/lcmodels/MSLCM_SL2015.cpp | 30 +++++++++++++++++++++++++-
 src/microsim/lcmodels/MSLCM_SL2015.h   |  2 ++
 2 files changed, 31 insertions(+), 1 deletion(-)

diff --git a/src/microsim/lcmodels/MSLCM_SL2015.cpp b/src/microsim/lcmodels/MSLCM_SL2015.cpp
index 63ca5722f82d..3b689a83277b 100644
--- a/src/microsim/lcmodels/MSLCM_SL2015.cpp
+++ b/src/microsim/lcmodels/MSLCM_SL2015.cpp
@@ -143,6 +143,7 @@ MSLCM_SL2015::MSLCM_SL2015(MSVehicle& v) :
     myMinImpatience(myImpatience),
     myTimeToImpatience(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_TIME_TO_IMPATIENCE, std::numeric_limits<double>::max())),
     myAccelLat(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_ACCEL_LAT, 1.0)),
+    myTurnAlignmentDist(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_TURN_ALIGNMENT_DISTANCE, 0.0)),
     myLookaheadLeft(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_LOOKAHEADLEFT, 2.0)),
     mySpeedGainRight(v.getVehicleType().getParameter().getLCParam(SUMO_ATTR_LCA_SPEEDGAINRIGHT, 0.1))
 {
@@ -1653,7 +1654,32 @@ MSLCM_SL2015::_wantsChangeSublane(
 #endif
 
         } else {
-            switch (myVehicle.getVehicleType().getPreferredLateralAlignment()) {
+
+            LateralAlignment align = myVehicle.getVehicleType().getPreferredLateralAlignment();
+            // Check whether the vehicle should adapt its alignment to an upcoming turn
+            if (myTurnAlignmentDist > 0) {
+                const std::pair<double, LinkDirection>& turnInfo = myVehicle.getNextTurn();
+                if (turnInfo.first < myTurnAlignmentDist) {
+                    // Vehicle is close enough to the link to change its default alignment
+                    switch (turnInfo.second) {
+                    case LINKDIR_TURN:
+                    case LINKDIR_LEFT:
+                    case LINKDIR_PARTLEFT:
+                        align = LATALIGN_LEFT;
+                        break;
+                    case LINKDIR_TURN_LEFTHAND:
+                    case LINKDIR_RIGHT:
+                    case LINKDIR_PARTRIGHT:
+                        align = LATALIGN_RIGHT;
+                        break;
+                    case LINKDIR_STRAIGHT:
+                    case LINKDIR_NODIR:
+                    default:
+                        break;
+                    }
+                }
+            }
+            switch (align) {
                 case LATALIGN_RIGHT:
                     latDistSublane = -halfLaneWidth + halfVehWidth - myVehicle.getLateralPositionOnLane();
                     break;
@@ -3233,6 +3259,8 @@ MSLCM_SL2015::setParameter(const std::string& key, const std::string& value) {
         myTimeToImpatience = doubleValue;
     } else if (key == toString(SUMO_ATTR_LCA_ACCEL_LAT)) {
         myAccelLat = doubleValue;
+    } else if (key == toString(SUMO_ATTR_LCA_TURN_ALIGNMENT_DISTANCE)) {
+        myTurnAlignmentDist = doubleValue;
     } else if (key == toString(SUMO_ATTR_LCA_LOOKAHEADLEFT)) {
         myLookaheadLeft = doubleValue;
     } else if (key == toString(SUMO_ATTR_LCA_SPEEDGAINRIGHT)) {
diff --git a/src/microsim/lcmodels/MSLCM_SL2015.h b/src/microsim/lcmodels/MSLCM_SL2015.h
index 5f4736711e6a..c98ff259fab9 100644
--- a/src/microsim/lcmodels/MSLCM_SL2015.h
+++ b/src/microsim/lcmodels/MSLCM_SL2015.h
@@ -399,6 +399,8 @@ class MSLCM_SL2015 : public MSAbstractLaneChangeModel {
     double myTimeToImpatience;
     // @brief lateral acceleration
     double myAccelLat;
+    // @brief distance to turn at which alignment should be adjusted to the turn direction
+    double myTurnAlignmentDist;
     // @brief the factor by which the lookahead distance to the left differs from the lookahead to the right
     double myLookaheadLeft;
     // @brief the factor by which the speedGain-threshold for the leftdiffers from the threshold for the right