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,64 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class DirectionSegmentView
+{
+    public DirectionSegmentView(
+        int segmentIndex,
+        TravelDirection direction,
+        IReadOnlyList<SmoothedPathPoint> points,
+        ReferenceBoundary startBoundary,
+        ReferenceBoundary endBoundary,
+        double sourceStartArcLength)
+    {
+        if (segmentIndex < 0)
+            throw new ArgumentOutOfRangeException(nameof(segmentIndex));
+        if (points == null || points.Count == 0)
+            throw new ArgumentException("A direction segment requires points.", nameof(points));
+        if (startBoundary == null || endBoundary == null)
+            throw new ArgumentNullException(startBoundary == null ? nameof(startBoundary) : nameof(endBoundary));
+        if (startBoundary.SegmentIndex != segmentIndex || endBoundary.SegmentIndex != segmentIndex)
+            throw new ArgumentException("Boundary segment identity must match the segment.");
+        if (points[0] == null || Math.Abs(points[0].ArcLength) > 1e-12d)
+            throw new ArgumentException("A direction segment must begin at local S zero.", nameof(points));
+
+        var copy = new List<SmoothedPathPoint>(points.Count);
+        double previousS = -1d;
+        for (int index = 0; index < points.Count; index++)
+        {
+            SmoothedPathPoint point = points[index];
+            if (point == null || point.Direction != direction || point.ArcLength < 0d || point.ArcLength < previousS)
+                throw new ArgumentException("Direction segment points are invalid.", nameof(points));
+            copy.Add(point);
+            previousS = point.ArcLength;
+        }
+        if (Math.Abs(endBoundary.SegmentLocalS - previousS) > 1e-12d)
+            throw new ArgumentException("End boundary must match the final local S.", nameof(endBoundary));
+
+        SegmentIndex = segmentIndex;
+        Direction = direction;
+        Points = new ReadOnlyCollection<SmoothedPathPoint>(copy);
+        StartBoundary = startBoundary;
+        EndBoundary = endBoundary;
+        SourceStartArcLength = sourceStartArcLength;
+    }
+
+    public int SegmentIndex { get; }
+
+    public TravelDirection Direction { get; }
+
+    public IReadOnlyList<SmoothedPathPoint> Points { get; }
+
+    public ReferenceBoundary StartBoundary { get; }
+
+    public ReferenceBoundary EndBoundary { get; }
+
+    public double SourceStartArcLength { get; }
+
+    public double LengthMeters { get { return EndBoundary.SegmentLocalS; } }
+}