feat: preserve EM planner segment boundaries

2026-08-03 22:27:00 +08:00
parent b326431d63
commit e492e1610a
8 changed files with 407 additions and 4 deletions
@@ -0,0 +1,105 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class ReferenceHorizonSlice
+{
+    public ReferenceHorizonSlice(DirectionSegmentView segment, IReadOnlyList<SmoothedPathPoint> points,
+        ReferenceBoundary terminalBoundary)
+    {
+        if (segment == null)
+            throw new ArgumentNullException(nameof(segment));
+        if (points == null || points.Count == 0)
+            throw new ArgumentException("A horizon slice requires points.", nameof(points));
+        if (terminalBoundary == null || terminalBoundary.SegmentIndex != segment.SegmentIndex)
+            throw new ArgumentException("A matching terminal boundary is required.", nameof(terminalBoundary));
+
+        var copy = new List<SmoothedPathPoint>(points.Count);
+        for (int index = 0; index < points.Count; index++) copy.Add(points[index]);
+        Segment = segment;
+        Points = new ReadOnlyCollection<SmoothedPathPoint>(copy);
+        TerminalBoundary = terminalBoundary;
+    }
+
+    public DirectionSegmentView Segment { get; }
+
+    public IReadOnlyList<SmoothedPathPoint> Points { get; }
+
+    public ReferenceBoundary TerminalBoundary { get; }
+}
+
+public static class ReferenceHorizonSlicer
+{
+    private const double Epsilon = 1e-12d;
+
+    public static ReferenceHorizonSlice Slice(DirectionSegmentView segment, double requestedEndSegmentLocalS)
+    {
+        if (segment == null)
+            throw new ArgumentNullException(nameof(segment));
+        if (double.IsNaN(requestedEndSegmentLocalS) || double.IsInfinity(requestedEndSegmentLocalS) || requestedEndSegmentLocalS < 0d)
+            throw new ArgumentOutOfRangeException(nameof(requestedEndSegmentLocalS));
+
+        double terminalS = Math.Min(requestedEndSegmentLocalS, segment.LengthMeters);
+        var points = new List<SmoothedPathPoint>();
+        for (int index = 0; index < segment.Points.Count; index++)
+        {
+            SmoothedPathPoint point = segment.Points[index];
+            if (point.ArcLength < terminalS - Epsilon)
+                points.Add(point);
+        }
+
+        points.Add(GetExactTerminalPoint(segment, terminalS));
+        ReferenceBoundary terminal = terminalS >= segment.LengthMeters - Epsilon
+            ? segment.EndBoundary
+            : new ReferenceBoundary(segment.SegmentIndex, terminalS, EmBoundaryType.RollingSafetyStop,
+                segment.SourceStartArcLength + terminalS);
+        return new ReferenceHorizonSlice(segment, points, terminal);
+    }
+
+    private static SmoothedPathPoint GetExactTerminalPoint(DirectionSegmentView segment, double terminalS)
+    {
+        for (int index = 0; index < segment.Points.Count; index++)
+        {
+            SmoothedPathPoint point = segment.Points[index];
+            if (Math.Abs(point.ArcLength - terminalS) <= Epsilon)
+                return point;
+            if (point.ArcLength > terminalS)
+            {
+                SmoothedPathPoint previous = segment.Points[index - 1];
+                return Interpolate(previous, point, terminalS);
+            }
+        }
+        return segment.Points[segment.Points.Count - 1];
+    }
+
+    private static SmoothedPathPoint Interpolate(SmoothedPathPoint lower, SmoothedPathPoint upper, double localS)
+    {
+        double interval = upper.ArcLength - lower.ArcLength;
+        if (interval <= 0d)
+            throw new ArgumentException("Reference points do not bracket a positive interval.");
+        double fraction = (localS - lower.ArcLength) / interval;
+        double unwrappedHeading = lower.UnwrappedHeading + (upper.UnwrappedHeading - lower.UnwrappedHeading) * fraction;
+        return new SmoothedPathPoint(
+            Interpolate(lower.X, upper.X, fraction),
+            Interpolate(lower.Y, upper.Y, fraction),
+            AngleMath.NormalizeRadians(unwrappedHeading),
+            unwrappedHeading,
+            localS,
+            lower.Direction,
+            Interpolate(lower.GeometricCurvature, upper.GeometricCurvature, fraction),
+            Interpolate(lower.VehicleCurvature, upper.VehicleCurvature, fraction),
+            Interpolate(lower.VehicleCurvatureDerivative, upper.VehicleCurvatureDerivative, fraction),
+            Interpolate(lower.BodyClearance, upper.BodyClearance, fraction),
+            false,
+            SmoothedPathPointSource.Interpolated);
+    }
+
+    private static double Interpolate(double lower, double upper, double fraction)
+    {
+        return lower + (upper - lower) * fraction;
+    }
+}