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,84 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public static class ReferencePathSegmenter
+{
+    public static IReadOnlyList<DirectionSegmentView> Create(PathSmoothingResult referencePath)
+    {
+        if (referencePath == null || referencePath.Path == null || referencePath.Segments == null ||
+            referencePath.Path.Count == 0 || referencePath.Segments.Count == 0)
+            throw new ArgumentException("A published reference path with direction segments is required.", nameof(referencePath));
+
+        var result = new List<DirectionSegmentView>(referencePath.Segments.Count);
+        int expectedStart = 0;
+        for (int segmentListIndex = 0; segmentListIndex < referencePath.Segments.Count; segmentListIndex++)
+        {
+            SmoothedPathSegment sourceSegment = referencePath.Segments[segmentListIndex];
+            if (sourceSegment == null || sourceSegment.SegmentIndex != segmentListIndex ||
+                sourceSegment.StartIndex != expectedStart || sourceSegment.StartIndex < 0 ||
+                sourceSegment.EndIndex < sourceSegment.StartIndex || sourceSegment.EndIndex >= referencePath.Path.Count)
+                throw new ArgumentException("Reference path segment indexing is invalid.", nameof(referencePath));
+
+            SmoothedPathPoint firstSourcePoint = referencePath.Path[sourceSegment.StartIndex];
+            if (firstSourcePoint == null || firstSourcePoint.Direction != sourceSegment.Direction)
+                throw new ArgumentException("Reference path segment start is invalid.", nameof(referencePath));
+
+            double sourceStartS = firstSourcePoint.ArcLength;
+            var rebasedPoints = new List<SmoothedPathPoint>(sourceSegment.EndIndex - sourceSegment.StartIndex + 1);
+            double previousLocalS = -1d;
+            for (int pathIndex = sourceSegment.StartIndex; pathIndex <= sourceSegment.EndIndex; pathIndex++)
+            {
+                SmoothedPathPoint sourcePoint = referencePath.Path[pathIndex];
+                if (sourcePoint == null || sourcePoint.Direction != sourceSegment.Direction ||
+                    double.IsNaN(sourcePoint.ArcLength) || double.IsInfinity(sourcePoint.ArcLength))
+                    throw new ArgumentException("Reference path point is invalid.", nameof(referencePath));
+
+                double localS = sourcePoint.ArcLength - sourceStartS;
+                if (localS < 0d || (pathIndex > sourceSegment.StartIndex && localS <= previousLocalS))
+                    throw new ArgumentException("Reference path segment arc length must strictly increase.", nameof(referencePath));
+                rebasedPoints.Add(CloneAtLocalS(sourcePoint, localS));
+                previousLocalS = localS;
+            }
+
+            EmBoundaryType startType = sourceSegment.StartsAtGearSwitch
+                ? EmBoundaryType.GearSwitchDeparture
+                : EmBoundaryType.None;
+            EmBoundaryType endType = sourceSegment.EndsAtGearSwitch
+                ? EmBoundaryType.GearSwitchApproach
+                : segmentListIndex == referencePath.Segments.Count - 1
+                    ? EmBoundaryType.Goal
+                    : EmBoundaryType.None;
+            var startBoundary = new ReferenceBoundary(sourceSegment.SegmentIndex, 0d, startType, sourceStartS);
+            var endBoundary = new ReferenceBoundary(sourceSegment.SegmentIndex, previousLocalS, endType,
+                referencePath.Path[sourceSegment.EndIndex].ArcLength);
+            result.Add(new DirectionSegmentView(sourceSegment.SegmentIndex, sourceSegment.Direction, rebasedPoints,
+                startBoundary, endBoundary, sourceStartS));
+            expectedStart = sourceSegment.EndIndex + 1;
+        }
+
+        if (expectedStart != referencePath.Path.Count)
+            throw new ArgumentException("Reference path segments do not cover the full path.", nameof(referencePath));
+        return new ReadOnlyCollection<DirectionSegmentView>(result);
+    }
+
+    internal static SmoothedPathPoint CloneAtLocalS(SmoothedPathPoint source, double localS)
+    {
+        return new SmoothedPathPoint(
+            source.X,
+            source.Y,
+            source.Heading,
+            source.UnwrappedHeading,
+            localS,
+            source.Direction,
+            source.GeometricCurvature,
+            source.VehicleCurvature,
+            source.VehicleCurvatureDerivative,
+            source.BodyClearance,
+            source.IsGearSwitchPoint,
+            source.Source);
+    }
+}