feat: add local cubic bezier smoother

2026-07-29 13:21:13 +08:00
parent d670a9c821
commit c893abe7d2
2 changed files with 673 additions and 0 deletions
@@ -0,0 +1,395 @@
+using System;
+using System.Collections.Generic;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
+
+/// <summary>在方向段内以局部三次 Bézier 连接替换明显转角。</summary>
+internal sealed class LocalCubicBezierSmoother : IPathSmoother
+{
+    private const double WindowToleranceMeters = 1e-9d;
+
+    /// <inheritdoc />
+    public SmoothingMethod Method => SmoothingMethod.LocalCubicBezier;
+
+    /// <inheritdoc />
+    public SmoothingCandidate Smooth(
+        SmoothingAlgorithmInput input,
+        double effectiveStrength,
+        CancellationToken cancellationToken)
+    {
+        if (input == null || input.OriginalPath == null || input.Options == null ||
+            !NumericGuard.IsPositiveFinite(effectiveStrength) ||
+            !NumericGuard.IsFinite(input.MinimumClearanceReserveMeters) ||
+            input.MinimumClearanceReserveMeters < 0d)
+        {
+            return SmoothingCandidate.Failed("Bézier 输入、强度或净空预留无效。");
+        }
+
+        var candidateSegments = new List<PreparedDirectionSegment>(input.OriginalPath.Segments.Count);
+        for (int segmentIndex = 0; segmentIndex < input.OriginalPath.Segments.Count; segmentIndex++)
+        {
+            cancellationToken.ThrowIfCancellationRequested();
+            PreparedDirectionSegment sourceSegment = input.OriginalPath.Segments[segmentIndex];
+            if (!TrySmoothSegment(
+                    sourceSegment,
+                    input.Options.BezierCornerHeadingThresholdRadians,
+                    input.Options.BezierMaximumWindowLengthMeters,
+                    input.Options.BezierHandleLengthRatio,
+                    effectiveStrength,
+                    input.MinimumClearanceReserveMeters,
+                    cancellationToken,
+                    out IReadOnlyList<SmoothingPoint2D> points,
+                    out string reason,
+                    out SmoothingCandidateStatus status))
+            {
+                return status == SmoothingCandidateStatus.RetryableInfeasible
+                    ? SmoothingCandidate.RetryableInfeasible(reason)
+                    : SmoothingCandidate.Failed(reason);
+            }
+
+            candidateSegments.Add(new PreparedDirectionSegment(
+                sourceSegment.SegmentIndex,
+                sourceSegment.Direction,
+                points,
+                sourceSegment.StartsAtGearSwitch,
+                sourceSegment.EndsAtGearSwitch));
+        }
+
+        return SmoothingCandidate.Success(candidateSegments);
+    }
+
+    private static bool TrySmoothSegment(
+        PreparedDirectionSegment sourceSegment,
+        double cornerThresholdRadians,
+        double maximumWindowLengthMeters,
+        double handleLengthRatio,
+        double strength,
+        double reserveMeters,
+        CancellationToken cancellationToken,
+        out IReadOnlyList<SmoothingPoint2D> result,
+        out string reason,
+        out SmoothingCandidateStatus status)
+    {
+        result = null;
+        reason = string.Empty;
+        status = SmoothingCandidateStatus.Failed;
+        if (sourceSegment == null || sourceSegment.Points == null || sourceSegment.Points.Count == 0)
+        {
+            reason = "Bézier 方向段为空。";
+            return false;
+        }
+
+        IReadOnlyList<SmoothingPoint2D> anchors = sourceSegment.Points;
+        if (!ValidateAnchors(anchors, cancellationToken, out reason)) return false;
+        if (anchors.Count < 3)
+        {
+            result = anchors;
+            return true;
+        }
+
+        if (!TryCreateMergedWindows(
+                anchors,
+                cornerThresholdRadians,
+                maximumWindowLengthMeters,
+                cancellationToken,
+                out List<Window> windows,
+                out reason))
+        {
+            return false;
+        }
+        if (windows.Count == 0)
+        {
+            result = anchors;
+            return true;
+        }
+
+        var output = new List<SmoothingPoint2D>(anchors.Count);
+        int anchorIndex = 0;
+        for (int windowIndex = 0; windowIndex < windows.Count; windowIndex++)
+        {
+            cancellationToken.ThrowIfCancellationRequested();
+            Window window = windows[windowIndex];
+            while (anchorIndex <= window.StartIndex)
+            {
+                output.Add(anchors[anchorIndex]);
+                anchorIndex++;
+            }
+
+            if (!TryAppendWindowInterior(
+                    anchors,
+                    window,
+                    handleLengthRatio,
+                    strength,
+                    reserveMeters,
+                    output,
+                    cancellationToken,
+                    out reason,
+                    out status))
+            {
+                return false;
+            }
+
+            output.Add(anchors[window.EndIndex]);
+            anchorIndex = window.EndIndex + 1;
+        }
+
+        while (anchorIndex < anchors.Count)
+        {
+            output.Add(anchors[anchorIndex]);
+            anchorIndex++;
+        }
+
+        result = output;
+        return true;
+    }
+
+    private static bool ValidateAnchors(
+        IReadOnlyList<SmoothingPoint2D> anchors,
+        CancellationToken cancellationToken,
+        out string reason)
+    {
+        reason = string.Empty;
+        for (int index = 0; index < anchors.Count; index++)
+        {
+            cancellationToken.ThrowIfCancellationRequested();
+            SmoothingPoint2D point = anchors[index];
+            if (point == null || !NumericGuard.IsFinite(point.X) || !NumericGuard.IsFinite(point.Y) ||
+                !NumericGuard.IsFinite(point.ArcLength) || point.ArcLength < 0d ||
+                !NumericGuard.IsFinite(point.Heading) || !NumericGuard.IsFinite(point.UnwrappedHeading) ||
+                !NumericGuard.IsFinite(point.BodyClearance) || point.BodyClearance < 0d ||
+                (index > 0 && point.ArcLength <= anchors[index - 1].ArcLength))
+            {
+                reason = "Bézier 方向段包含非有限、非递增弧长或无效净空的锚点。";
+                return false;
+            }
+        }
+        return true;
+    }
+
+    private static bool TryCreateMergedWindows(
+        IReadOnlyList<SmoothingPoint2D> anchors,
+        double cornerThresholdRadians,
+        double maximumWindowLengthMeters,
+        CancellationToken cancellationToken,
+        out List<Window> windows,
+        out string reason)
+    {
+        windows = new List<Window>();
+        reason = string.Empty;
+        for (int cornerIndex = 1; cornerIndex < anchors.Count - 1; cornerIndex++)
+        {
+            cancellationToken.ThrowIfCancellationRequested();
+            if (!TryGetTravelTangent(anchors[cornerIndex - 1], anchors[cornerIndex], out Point2D entryTangent) ||
+                !TryGetTravelTangent(anchors[cornerIndex], anchors[cornerIndex + 1], out Point2D exitTangent))
+            {
+                reason = "Bézier 转角包含零长度或非有限行进切向。";
+                return false;
+            }
+
+            double cross = entryTangent.X * exitTangent.Y - entryTangent.Y * exitTangent.X;
+            double dot = entryTangent.X * exitTangent.X + entryTangent.Y * exitTangent.Y;
+            double turnRadians = Math.Atan2(Math.Abs(cross), dot);
+            if (!NumericGuard.IsFinite(turnRadians))
+            {
+                reason = "Bézier 转角计算产生非有限数值。";
+                return false;
+            }
+            if (turnRadians < cornerThresholdRadians) continue;
+
+            int startIndex = cornerIndex - 1;
+            int endIndex = cornerIndex + 1;
+            double windowLength = anchors[endIndex].ArcLength - anchors[startIndex].ArcLength;
+            if (!NumericGuard.IsPositiveFinite(windowLength))
+            {
+                reason = "Bézier 局部窗口弧长无效。";
+                return false;
+            }
+            if (windowLength > maximumWindowLengthMeters + WindowToleranceMeters) continue;
+            if (ContainsGearSwitch(anchors, startIndex, endIndex)) continue;
+
+            var proposed = new Window(startIndex, endIndex);
+            if (windows.Count == 0 || proposed.StartIndex > windows[windows.Count - 1].EndIndex + 1)
+            {
+                windows.Add(proposed);
+            }
+            else
+            {
+                Window previous = windows[windows.Count - 1];
+                windows[windows.Count - 1] = new Window(
+                    previous.StartIndex,
+                    Math.Max(previous.EndIndex, proposed.EndIndex));
+            }
+        }
+        return true;
+    }
+
+    private static bool TryAppendWindowInterior(
+        IReadOnlyList<SmoothingPoint2D> anchors,
+        Window window,
+        double handleLengthRatio,
+        double strength,
+        double reserveMeters,
+        List<SmoothingPoint2D> output,
+        CancellationToken cancellationToken,
+        out string reason,
+        out SmoothingCandidateStatus status)
+    {
+        reason = string.Empty;
+        status = SmoothingCandidateStatus.Failed;
+        SmoothingPoint2D p0 = anchors[window.StartIndex];
+        SmoothingPoint2D p3 = anchors[window.EndIndex];
+        if (!TryGetTravelTangent(p0, anchors[window.StartIndex + 1], out Point2D entryTangent) ||
+            !TryGetTravelTangent(anchors[window.EndIndex - 1], p3, out Point2D exitTangent))
+        {
+            reason = "Bézier 窗口端点包含无效行进切向。";
+            return false;
+        }
+
+        double arcLength = p3.ArcLength - p0.ArcLength;
+        double handleLength = arcLength * handleLengthRatio * strength;
+        if (!NumericGuard.IsPositiveFinite(arcLength) || !NumericGuard.IsPositiveFinite(handleLength))
+        {
+            reason = "Bézier 窗口弧长或控制柄长度无效。";
+            return false;
+        }
+
+        Point2D control1 = new Point2D(
+            p0.X + entryTangent.X * handleLength,
+            p0.Y + entryTangent.Y * handleLength);
+        Point2D control2 = new Point2D(
+            p3.X - exitTangent.X * handleLength,
+            p3.Y - exitTangent.Y * handleLength);
+        if (!IsFinite(control1) || !IsFinite(control2))
+        {
+            reason = "Bézier 控制点产生非有限数值。";
+            return false;
+        }
+
+        for (int index = window.StartIndex + 1; index < window.EndIndex; index++)
+        {
+            cancellationToken.ThrowIfCancellationRequested();
+            SmoothingPoint2D anchor = anchors[index];
+            double parameter = (anchor.ArcLength - p0.ArcLength) / arcLength;
+            if (!NumericGuard.IsFinite(parameter) || parameter <= 0d || parameter >= 1d)
+            {
+                reason = "Bézier 窗口参数无效。";
+                return false;
+            }
+
+            Point2D evaluated = Evaluate(p0, control1, control2, p3, parameter);
+            if (!IsFinite(evaluated))
+            {
+                reason = "Bézier 评估产生非有限数值。";
+                return false;
+            }
+
+            double referenceArcLength = p0.ArcLength + parameter * arcLength;
+            if (!PathReferenceInterpolator.TryInterpolateByArcLength(
+                    anchors,
+                    referenceArcLength,
+                    out SmoothingPoint2D reference,
+                    out reason))
+            {
+                return false;
+            }
+
+            double displacement = Distance(evaluated, reference);
+            if (!NumericGuard.IsFinite(displacement))
+            {
+                reason = "Bézier 评估点位移产生非有限数值。";
+                return false;
+            }
+            double allowedDisplacement = Math.Max(0d, reference.BodyClearance - reserveMeters);
+            if (displacement > allowedDisplacement)
+            {
+                reason = "Bézier 评估点超过对应原始弧长参考点的允许移动范围。";
+                status = SmoothingCandidateStatus.RetryableInfeasible;
+                return false;
+            }
+
+            output.Add(new SmoothingPoint2D(
+                evaluated.X,
+                evaluated.Y,
+                reference.ArcLength,
+                reference.Heading,
+                reference.UnwrappedHeading,
+                reference.BodyClearance,
+                false,
+                SmoothedPathPointSource.Interpolated));
+        }
+        return true;
+    }
+
+    private static bool ContainsGearSwitch(IReadOnlyList<SmoothingPoint2D> anchors, int startIndex, int endIndex)
+    {
+        for (int index = startIndex; index <= endIndex; index++)
+        {
+            if (anchors[index].IsGearSwitchPoint) return true;
+        }
+        return false;
+    }
+
+    private static bool TryGetTravelTangent(SmoothingPoint2D start, SmoothingPoint2D end, out Point2D tangent)
+    {
+        tangent = default;
+        double deltaX = end.X - start.X;
+        double deltaY = end.Y - start.Y;
+        double length = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
+        if (!NumericGuard.IsPositiveFinite(length)) return false;
+        tangent = new Point2D(deltaX / length, deltaY / length);
+        return IsFinite(tangent);
+    }
+
+    private static Point2D Evaluate(SmoothingPoint2D p0, Point2D p1, Point2D p2, SmoothingPoint2D p3, double parameter)
+    {
+        double oneMinusParameter = 1d - parameter;
+        double p0Weight = oneMinusParameter * oneMinusParameter * oneMinusParameter;
+        double p1Weight = 3d * oneMinusParameter * oneMinusParameter * parameter;
+        double p2Weight = 3d * oneMinusParameter * parameter * parameter;
+        double p3Weight = parameter * parameter * parameter;
+        return new Point2D(
+            p0Weight * p0.X + p1Weight * p1.X + p2Weight * p2.X + p3Weight * p3.X,
+            p0Weight * p0.Y + p1Weight * p1.Y + p2Weight * p2.Y + p3Weight * p3.Y);
+    }
+
+    private static bool IsFinite(Point2D point)
+    {
+        return NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y);
+    }
+
+    private static double Distance(Point2D point, SmoothingPoint2D reference)
+    {
+        double deltaX = point.X - reference.X;
+        double deltaY = point.Y - reference.Y;
+        return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
+    }
+
+    private readonly struct Window
+    {
+        internal Window(int startIndex, int endIndex)
+        {
+            StartIndex = startIndex;
+            EndIndex = endIndex;
+        }
+
+        internal int StartIndex { get; }
+
+        internal int EndIndex { get; }
+    }
+
+    private readonly struct Point2D
+    {
+        internal Point2D(double x, double y)
+        {
+            X = x;
+            Y = y;
+        }
+
+        internal double X { get; }
+
+        internal double Y { get; }
+    }
+}