feat: add path smoothing geometry foundation

2026-07-29 08:50:12 +08:00
parent 5cea617036
commit f75a3bae5e
8 changed files with 1332 additions and 0 deletions
@@ -0,0 +1,194 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
+
+/// <summary>校验粗路径契约、保护换向拓扑并产生统一间距的平滑输入。</summary>
+public sealed class PathSmoothingPreprocessor
+{
+    private const double Tolerance = 1e-8d;
+    private readonly ArcLengthResampler _resampler;
+
+    /// <summary>创建使用默认确定性重采样器的预处理器。</summary>
+    public PathSmoothingPreprocessor()
+        : this(new ArcLengthResampler())
+    {
+    }
+
+    /// <summary>创建使用指定重采样器的预处理器。</summary>
+    public PathSmoothingPreprocessor(ArcLengthResampler resampler)
+    {
+        _resampler = resampler ?? throw new ArgumentNullException(nameof(resampler));
+    }
+
+    /// <summary>将一条粗路径请求校验、按方向拆分并按配置间距重采样。</summary>
+    public bool TryPrepare(PathSmoothingRequest request, out PreparedPath preparedPath, out string reason)
+    {
+        preparedPath = null;
+        reason = string.Empty;
+        if (request == null || request.Map == null || request.Vehicle == null || request.Configuration == null ||
+            request.CoarsePath == null || request.Segments == null || request.CoarsePath.Count == 0 || request.Segments.Count == 0)
+        {
+            reason = "平滑请求缺少粗路径、方向分段、地图、车辆或配置。";
+            return false;
+        }
+
+        PathSmoothingConfiguration configuration = request.Configuration;
+        if (!NumericGuard.IsPositiveFinite(configuration.OutputSpacingMeters))
+        {
+            reason = "平滑输出采样间距无效。";
+            return false;
+        }
+
+        if (!ValidatePathPoints(request.CoarsePath, out reason) ||
+            !ValidateSegments(request.CoarsePath, request.Segments, out reason))
+        {
+            return false;
+        }
+
+        var preparedSegments = new List<PreparedDirectionSegment>(request.Segments.Count);
+        for (int segmentIndex = 0; segmentIndex < request.Segments.Count; segmentIndex++)
+        {
+            PathSegment sourceSegment = request.Segments[segmentIndex];
+            double segmentStartArcLength = request.CoarsePath[sourceSegment.StartIndex].ArcLength;
+            var segmentPoints = new List<SmoothingPoint2D>(sourceSegment.EndIndex - sourceSegment.StartIndex + 1);
+            for (int pointIndex = sourceSegment.StartIndex; pointIndex <= sourceSegment.EndIndex; pointIndex++)
+            {
+                CoarsePathPoint point = request.CoarsePath[pointIndex];
+                bool isGearSwitch = point.IsGearSwitchPoint;
+                segmentPoints.Add(new SmoothingPoint2D(
+                    point.X,
+                    point.Y,
+                    point.ArcLength - segmentStartArcLength,
+                    point.Heading,
+                    point.UnwrappedHeading,
+                    point.BodyClearance,
+                    isGearSwitch,
+                    isGearSwitch ? SmoothedPathPointSource.GearSwitch : SmoothedPathPointSource.Anchor));
+            }
+
+            var unresampled = new PreparedDirectionSegment(
+                sourceSegment.SegmentIndex,
+                sourceSegment.Direction,
+                segmentPoints,
+                sourceSegment.StartsAtGearSwitch,
+                sourceSegment.EndsAtGearSwitch);
+            if (!_resampler.TryResample(unresampled, configuration.OutputSpacingMeters, out PreparedDirectionSegment resampled, out reason))
+                return false;
+            preparedSegments.Add(resampled);
+        }
+
+        preparedPath = new PreparedPath(preparedSegments);
+        return true;
+    }
+
+    private static bool ValidatePathPoints(IReadOnlyList<CoarsePathPoint> path, out string reason)
+    {
+        reason = string.Empty;
+        CoarsePathPoint first = path[0];
+        if (!IsValidPoint(first) || first.IsGearSwitchPoint || Math.Abs(first.ArcLength) > Tolerance)
+        {
+            reason = "粗路径首点无效。";
+            return false;
+        }
+
+        for (int index = 1; index < path.Count; index++)
+        {
+            CoarsePathPoint previous = path[index - 1];
+            CoarsePathPoint current = path[index];
+            if (!IsValidPoint(current) || current.ArcLength + Tolerance < previous.ArcLength)
+            {
+                reason = "粗路径包含非法数值或非递增弧长。";
+                return false;
+            }
+
+            double expectedHeadingDelta = AngleMath.ShortestSignedDifference(previous.Heading, current.Heading);
+            if (!NumericGuard.IsFinite(expectedHeadingDelta) ||
+                Math.Abs((current.UnwrappedHeading - previous.UnwrappedHeading) - expectedHeadingDelta) > Tolerance)
+            {
+                reason = "粗路径展开航向不连续。";
+                return false;
+            }
+
+            bool duplicatePoseAndArc = Math.Abs(current.X - previous.X) <= Tolerance &&
+                Math.Abs(current.Y - previous.Y) <= Tolerance &&
+                Math.Abs(current.ArcLength - previous.ArcLength) <= Tolerance &&
+                Math.Abs(AngleMath.ShortestSignedDifference(previous.Heading, current.Heading)) <= Tolerance;
+            if (duplicatePoseAndArc)
+            {
+                if (previous.Direction == current.Direction || !current.IsGearSwitchPoint)
+                {
+                    reason = "粗路径包含非法的重复点。";
+                    return false;
+                }
+            }
+            else if (current.IsGearSwitchPoint || current.ArcLength <= previous.ArcLength + Tolerance)
+            {
+                reason = "粗路径普通点必须有正弧长增量且不得标记为换向点。";
+                return false;
+            }
+        }
+        return true;
+    }
+
+    private static bool ValidateSegments(
+        IReadOnlyList<CoarsePathPoint> path,
+        IReadOnlyList<PathSegment> segments,
+        out string reason)
+    {
+        reason = string.Empty;
+        int expectedStartIndex = 0;
+        for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
+        {
+            PathSegment segment = segments[segmentIndex];
+            if (segment == null || segment.SegmentIndex != segmentIndex || segment.StartIndex != expectedStartIndex ||
+                segment.StartIndex < 0 || segment.EndIndex < segment.StartIndex || segment.EndIndex >= path.Count ||
+                segment.StartsAtGearSwitch != path[segment.StartIndex].IsGearSwitchPoint)
+            {
+                reason = "粗路径方向分段索引或起始换向标记无效。";
+                return false;
+            }
+
+            for (int pointIndex = segment.StartIndex; pointIndex <= segment.EndIndex; pointIndex++)
+            {
+                if (path[pointIndex].Direction != segment.Direction)
+                {
+                    reason = "粗路径方向分段包含不同方向的点。";
+                    return false;
+                }
+            }
+
+            bool hasNextSegment = segmentIndex + 1 < segments.Count;
+            bool expectedEndsAtGearSwitch = hasNextSegment && segment.EndIndex + 1 < path.Count &&
+                path[segment.EndIndex + 1].IsGearSwitchPoint;
+            if (segment.EndsAtGearSwitch != expectedEndsAtGearSwitch)
+            {
+                reason = "粗路径方向分段末尾换向标记无效。";
+                return false;
+            }
+            expectedStartIndex = segment.EndIndex + 1;
+        }
+
+        if (expectedStartIndex != path.Count)
+        {
+            reason = "粗路径方向分段未完整覆盖全部点。";
+            return false;
+        }
+        return true;
+    }
+
+    private static bool IsValidPoint(CoarsePathPoint point)
+    {
+        return point != null && NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y) &&
+            NumericGuard.IsFinite(point.Heading) && NumericGuard.IsFinite(point.UnwrappedHeading) &&
+            NumericGuard.IsFinite(point.ArcLength) && point.ArcLength >= 0d &&
+            NumericGuard.IsFinite(point.VehicleCurvature) && NumericGuard.IsFinite(point.BodyClearance) &&
+            point.BodyClearance >= 0d &&
+            (point.Direction == TravelDirection.Forward || point.Direction == TravelDirection.Reverse) &&
+            Enum.IsDefined(typeof(CoarsePathPointSource), point.Source) &&
+            Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, AngleMath.NormalizeRadians(point.Heading))) <= Tolerance;
+    }
+}