chore: save current workspace progress

2026-08-09 22:13:18 +08:00
parent 650c2ab0e3
commit 2f4fd15e52
449 changed files with 76593 additions and 971 deletions
@@ -0,0 +1,145 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
+
+/// <summary>按单一方向段的弧长线性插值并保留精确锚点的确定性重采样器。</summary>
+public sealed class ArcLengthResampler
+{
+    private const double Tolerance = 1e-10d;
+
+    /// <summary>以目标间距重采样一个方向段；段末锚点始终原样保留。</summary>
+    public bool TryResample(
+        IReadOnlyList<SmoothingPoint2D> points,
+        double spacingMeters,
+        out IReadOnlyList<SmoothingPoint2D> resampled,
+        out string reason)
+    {
+        resampled = EmptyPoints();
+        reason = string.Empty;
+        if (points == null || points.Count == 0 || !NumericGuard.IsPositiveFinite(spacingMeters))
+        {
+            reason = "重采样点集或采样间距无效。";
+            return false;
+        }
+
+        for (int index = 0; index < points.Count; index++)
+        {
+            if (!IsValidPoint(points[index]))
+            {
+                reason = "重采样输入包含非法数值。";
+                return false;
+            }
+            if (index == 0) continue;
+
+            SmoothingPoint2D previous = points[index - 1];
+            SmoothingPoint2D current = points[index];
+            if (current.ArcLength <= previous.ArcLength + Tolerance)
+            {
+                reason = "同一方向段的弧长必须严格增加。";
+                return false;
+            }
+            double distance = Distance(previous, current);
+            if (!NumericGuard.IsFinite(distance) || distance <= Tolerance)
+            {
+                reason = "同一方向段中不允许重复位姿或数值溢出的距离。";
+                return false;
+            }
+        }
+
+        if (points.Count == 1)
+        {
+            resampled = CopyReadOnly(points);
+            return true;
+        }
+
+        var output = new List<SmoothingPoint2D> { points[0] };
+        double firstArc = points[0].ArcLength;
+        double finalArc = points[points.Count - 1].ArcLength;
+        int rightIndex = 1;
+        for (double targetArc = firstArc + spacingMeters;
+             targetArc < finalArc - Tolerance;
+             targetArc += spacingMeters)
+        {
+            while (rightIndex < points.Count - 1 && points[rightIndex].ArcLength < targetArc)
+                rightIndex++;
+
+            SmoothingPoint2D left = points[rightIndex - 1];
+            SmoothingPoint2D right = points[rightIndex];
+            double ratio = (targetArc - left.ArcLength) / (right.ArcLength - left.ArcLength);
+            double unwrappedHeading = left.UnwrappedHeading +
+                ratio * (right.UnwrappedHeading - left.UnwrappedHeading);
+            output.Add(new SmoothingPoint2D(
+                left.X + ratio * (right.X - left.X),
+                left.Y + ratio * (right.Y - left.Y),
+                targetArc,
+                AngleMath.NormalizeRadians(unwrappedHeading),
+                unwrappedHeading,
+                Math.Min(left.BodyClearance, right.BodyClearance),
+                false,
+                SmoothedPathPointSource.Interpolated));
+        }
+
+        // Appending the original object, rather than interpolating at the final arc, preserves the exact endpoint.
+        output.Add(points[points.Count - 1]);
+        resampled = new ReadOnlyCollection<SmoothingPoint2D>(output);
+        return true;
+    }
+
+    /// <summary>重采样一个完整方向段并保留其换向拓扑标记。</summary>
+    public bool TryResample(
+        PreparedDirectionSegment segment,
+        double spacingMeters,
+        out PreparedDirectionSegment resampled,
+        out string reason)
+    {
+        resampled = null;
+        reason = string.Empty;
+        if (segment == null)
+        {
+            reason = "待重采样方向段为空。";
+            return false;
+        }
+        if (!TryResample(segment.Points, spacingMeters, out IReadOnlyList<SmoothingPoint2D> points, out reason))
+            return false;
+
+        resampled = new PreparedDirectionSegment(
+            segment.SegmentIndex,
+            segment.Direction,
+            points,
+            segment.StartsAtGearSwitch,
+            segment.EndsAtGearSwitch,
+            segment.StartVehicleCurvaturePerMeter);
+        return true;
+    }
+
+    private static bool IsValidPoint(SmoothingPoint2D point)
+    {
+        return point != null && NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y) &&
+            NumericGuard.IsFinite(point.ArcLength) && NumericGuard.IsFinite(point.Heading) &&
+            NumericGuard.IsFinite(point.UnwrappedHeading) && NumericGuard.IsFinite(point.BodyClearance) &&
+            point.BodyClearance >= 0d;
+    }
+
+    private static double Distance(SmoothingPoint2D left, SmoothingPoint2D right)
+    {
+        double x = right.X - left.X;
+        double y = right.Y - left.Y;
+        return Math.Sqrt(x * x + y * y);
+    }
+
+    private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
+    {
+        var copy = new List<T>(source.Count);
+        for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
+        return new ReadOnlyCollection<T>(copy);
+    }
+
+    private static IReadOnlyList<SmoothingPoint2D> EmptyPoints()
+    {
+        return new ReadOnlyCollection<SmoothingPoint2D>(new List<SmoothingPoint2D>());
+    }
+}