chore: save current workspace progress

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/LateralPathInterpolator.cs

@@ -46,6 +46,7 @@ internal sealed class LateralPathInterpolator
                 LateralPathPoint left = path.Points[index - 1];
                 double ratio = (pathS - left.PathS) / (right.PathS - left.PathS);
                 return new InterpolatedLateralPathPoint(
+                    Linear(left.ReferenceS, right.ReferenceS, ratio),
                     Linear(left.X, right.X, ratio),
                     Linear(left.Y, right.Y, ratio),
                     NormalizeYaw(left.VehicleYaw + ratio * NormalizeYaw(right.VehicleYaw - left.VehicleYaw)),
@@ -58,7 +59,8 @@ internal sealed class LateralPathInterpolator
 
     private static InterpolatedLateralPathPoint From(LateralPathPoint point)
     {
-        return new InterpolatedLateralPathPoint(point.X, point.Y, NormalizeYaw(point.VehicleYaw), point.VehicleCurvature);
+        return new InterpolatedLateralPathPoint(point.ReferenceS, point.X, point.Y, NormalizeYaw(point.VehicleYaw),
+            point.VehicleCurvature);
     }
 
     private static double Linear(double left, double right, double ratio)
@@ -84,14 +86,16 @@ internal sealed class LateralPathInterpolator
 
 internal sealed class InterpolatedLateralPathPoint
 {
-    public InterpolatedLateralPathPoint(double x, double y, double yaw, double vehicleCurvature)
+    public InterpolatedLateralPathPoint(double referenceS, double x, double y, double yaw, double vehicleCurvature)
     {
+        ReferenceS = referenceS;
         X = x;
         Y = y;
         Yaw = yaw;
         VehicleCurvature = vehicleCurvature;
     }
 
+    public double ReferenceS { get; }
     public double X { get; }
     public double Y { get; }
     public double Yaw { get; }