feat: assemble complete EM trajectories

2026-08-04 11:35:50 +08:00
parent 591143f181
commit 1d58864908
5 changed files with 391 additions and 1 deletions
@@ -0,0 +1,99 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal sealed class LateralPathInterpolator
+{
+    private const double BoundaryTolerance = 1e-10d;
+    private readonly LateralPath path;
+
+    public LateralPathInterpolator(LateralPath path)
+    {
+        if (path == null || !path.IsIndependentlyValidated || path.Points.Count < 2)
+            throw new ArgumentException("Trajectory assembly requires an independently validated lateral path.", nameof(path));
+
+        double previousPathS = double.NegativeInfinity;
+        for (int index = 0; index < path.Points.Count; index++)
+        {
+            LateralPathPoint point = path.Points[index];
+            if (point == null || point.PathS <= previousPathS)
+                throw new ArgumentException("Lateral path points must have strictly increasing PathS.", nameof(path));
+            previousPathS = point.PathS;
+        }
+
+        this.path = path;
+    }
+
+    public InterpolatedLateralPathPoint Interpolate(double pathS)
+    {
+        if (!IsFinite(pathS))
+            throw new ArgumentOutOfRangeException(nameof(pathS));
+
+        LateralPathPoint first = path.Points[0];
+        LateralPathPoint last = path.Points[path.Points.Count - 1];
+        if (pathS < first.PathS - BoundaryTolerance || pathS > last.PathS + BoundaryTolerance)
+            throw new ArgumentOutOfRangeException(nameof(pathS));
+        if (pathS <= first.PathS + BoundaryTolerance)
+            return From(first);
+        if (pathS >= last.PathS - BoundaryTolerance)
+            return From(last);
+
+        for (int index = 1; index < path.Points.Count; index++)
+        {
+            LateralPathPoint right = path.Points[index];
+            if (pathS <= right.PathS)
+            {
+                LateralPathPoint left = path.Points[index - 1];
+                double ratio = (pathS - left.PathS) / (right.PathS - left.PathS);
+                return new InterpolatedLateralPathPoint(
+                    Linear(left.X, right.X, ratio),
+                    Linear(left.Y, right.Y, ratio),
+                    NormalizeYaw(left.VehicleYaw + ratio * NormalizeYaw(right.VehicleYaw - left.VehicleYaw)),
+                    Linear(left.VehicleCurvature, right.VehicleCurvature, ratio));
+            }
+        }
+
+        throw new InvalidOperationException("A PathS value inside the lateral path was not bracketed.");
+    }
+
+    private static InterpolatedLateralPathPoint From(LateralPathPoint point)
+    {
+        return new InterpolatedLateralPathPoint(point.X, point.Y, NormalizeYaw(point.VehicleYaw), point.VehicleCurvature);
+    }
+
+    private static double Linear(double left, double right, double ratio)
+    {
+        return left + ratio * (right - left);
+    }
+
+    internal static double NormalizeYaw(double yaw)
+    {
+        double normalized = yaw % (2d * Math.PI);
+        if (normalized >= Math.PI)
+            normalized -= 2d * Math.PI;
+        if (normalized < -Math.PI)
+            normalized += 2d * Math.PI;
+        return normalized;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}
+
+internal sealed class InterpolatedLateralPathPoint
+{
+    public InterpolatedLateralPathPoint(double x, double y, double yaw, double vehicleCurvature)
+    {
+        X = x;
+        Y = y;
+        Yaw = yaw;
+        VehicleCurvature = vehicleCurvature;
+    }
+
+    public double X { get; }
+    public double Y { get; }
+    public double Yaw { get; }
+    public double VehicleCurvature { get; }
+}