fix: deduplicate Local G2 evaluator windows

2026-08-01 00:05:41 +08:00
parent b8b4f00427
commit 144a0883b2
2 changed files with 92 additions and 2 deletions
@@ -16,6 +16,7 @@ namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
 internal sealed class LocalG2CandidateEvaluator
 {
    private const double CurvatureRangeTolerance = 1e-6d;
+    private const double WindowPointToleranceMeters = 1e-10d;
    private static readonly IReadOnlyList<SmoothedPathPoint> EmptyPath =
        new ReadOnlyCollection<SmoothedPathPoint>(new List<SmoothedPathPoint>());
    private static readonly IReadOnlyList<SmoothedPathSegment> EmptySegments =
@@ -192,13 +193,28 @@ internal sealed class LocalG2CandidateEvaluator
        for (int index = 0; index < segment.Points.Count; index++)
        {
            SmoothingPoint2D point = segment.Points[index];
-            if (point.ArcLength > startArcLength && point.ArcLength < endArcLength) points.Add(point);
+            if (point.ArcLength > startArcLength && point.ArcLength < endArcLength &&
+                !SamePosition(points[points.Count - 1], point))
+            {
+                points.Add(point);
+            }
        }
-        points.Add(end);
+        if (SamePosition(points[points.Count - 1], end))
+            points[points.Count - 1] = end;
+        else
+            points.Add(end);
        window = new ReadOnlyCollection<SmoothingPoint2D>(points);
        return true;
    }

+    private static bool SamePosition(SmoothingPoint2D left, SmoothingPoint2D right)
+    {
+        if (left == null || right == null) return false;
+        double x = right.X - left.X;
+        double y = right.Y - left.Y;
+        return x * x + y * y <= WindowPointToleranceMeters * WindowPointToleranceMeters;
+    }
+
    private bool TryAnalyzeWindow(PreparedDirectionSegment source, IReadOnlyList<SmoothingPoint2D> points, double spacing,
        out PathGeometryAnalysis analysis, out string reason)
    {