feat: add path smoothing geometry foundation

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/ArcLengthResampler.cs

@@ -0,0 +1,144 @@
+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);
+        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>());
+    }
+}

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalysis.cs

@@ -0,0 +1,64 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
+
+/// <summary>同一几何分析器产生的路径、方向段和未验证质量统计。</summary>
+public sealed class PathGeometryAnalysis
+{
+    internal PathGeometryAnalysis(
+        IReadOnlyList<SmoothedPathPoint> path,
+        IReadOnlyList<SmoothedPathSegment> segments,
+        double pathLengthMeters,
+        double maximumAbsoluteVehicleCurvaturePerMeter,
+        double rootMeanSquareVehicleCurvaturePerMeter,
+        double totalAbsoluteCurvatureVariationPerMeter,
+        double curvatureVariationEnergy,
+        double minimumBodyClearanceMeters)
+    {
+        Path = CopyReadOnly(path);
+        Segments = CopyReadOnly(segments);
+        PathLengthMeters = pathLengthMeters;
+        MaximumAbsoluteVehicleCurvaturePerMeter = maximumAbsoluteVehicleCurvaturePerMeter;
+        RootMeanSquareVehicleCurvaturePerMeter = rootMeanSquareVehicleCurvaturePerMeter;
+        TotalAbsoluteCurvatureVariationPerMeter = totalAbsoluteCurvatureVariationPerMeter;
+        CurvatureVariationEnergy = curvatureVariationEnergy;
+        MinimumBodyClearanceMeters = minimumBodyClearanceMeters;
+    }
+
+    /// <summary>完成几何重计算的不可变路径。</summary>
+    public IReadOnlyList<SmoothedPathPoint> Path { get; }
+
+    /// <summary>完整覆盖 <see cref="Path"/> 的不可变方向段。</summary>
+    public IReadOnlyList<SmoothedPathSegment> Segments { get; }
+
+    /// <summary>路径总长度，单位 m。</summary>
+    public double PathLengthMeters { get; }
+
+    /// <summary>车辆曲率绝对值峰值，单位 1/m。</summary>
+    public double MaximumAbsoluteVehicleCurvaturePerMeter { get; }
+
+    /// <summary>车辆曲率均方根，单位 1/m。</summary>
+    public double RootMeanSquareVehicleCurvaturePerMeter { get; }
+
+    /// <summary>不跨换向点累计的绝对曲率变化，单位 1/m。</summary>
+    public double TotalAbsoluteCurvatureVariationPerMeter { get; }
+
+    /// <summary>不跨换向点累计的曲率变化能量。</summary>
+    public double CurvatureVariationEnergy { get; }
+
+    /// <summary>输入点携带的最小保守净空，单位 m。</summary>
+    public double MinimumBodyClearanceMeters { get; }
+
+    private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
+    {
+        var copy = new List<T>(source == null ? 0 : source.Count);
+        if (source != null)
+        {
+            for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
+        }
+        return new ReadOnlyCollection<T>(copy);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs

@@ -0,0 +1,401 @@
+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 PathGeometryAnalyzer
+{
+    private const double MinimumDistanceMeters = 1e-10d;
+    private const double BoundaryToleranceMeters = 1e-8d;
+
+    /// <summary>
+    /// 对候选方向段进行确定性几何分析。换向点两侧永不参与同一次差分。
+    /// </summary>
+    public bool TryAnalyze(
+        IReadOnlyList<PreparedDirectionSegment> candidateSegments,
+        double spacingMeters,
+        out PathGeometryAnalysis analysis,
+        out string reason)
+    {
+        analysis = null;
+        reason = string.Empty;
+        if (candidateSegments == null || candidateSegments.Count == 0 ||
+            !NumericGuard.IsPositiveFinite(spacingMeters))
+        {
+            reason = "候选方向段或输出采样间距无效。";
+            return false;
+        }
+
+        var outputPath = new List<SmoothedPathPoint>();
+        var outputSegments = new List<SmoothedPathSegment>();
+        double cumulativeArcLength = 0d;
+        double previousOutputHeading = 0d;
+        double previousOutputUnwrappedHeading = 0d;
+        bool hasPreviousOutputHeading = false;
+        double maximumAbsoluteVehicleCurvature = 0d;
+        double curvatureSquareSum = 0d;
+        int curvatureSampleCount = 0;
+        double totalCurvatureVariation = 0d;
+        double curvatureVariationEnergy = 0d;
+        double minimumClearance = double.PositiveInfinity;
+
+        for (int segmentIndex = 0; segmentIndex < candidateSegments.Count; segmentIndex++)
+        {
+            PreparedDirectionSegment segment = candidateSegments[segmentIndex];
+            if (!IsValidSegment(segment, segmentIndex, out reason)) return false;
+            if (!TryValidateBoundary(candidateSegments, segmentIndex, out reason)) return false;
+
+            if (!TryResampleByGeometry(segment.Points, spacingMeters, out IReadOnlyList<SmoothingPoint2D> samples, out reason))
+                return false;
+
+            if (!TryAnalyzeSegment(
+                    segment,
+                    samples,
+                    ref cumulativeArcLength,
+                    ref previousOutputHeading,
+                    ref previousOutputUnwrappedHeading,
+                    ref hasPreviousOutputHeading,
+                    outputPath,
+                    out double segmentMaximumCurvature,
+                    out double segmentCurvatureSquareSum,
+                    out int segmentCurvatureSampleCount,
+                    out double segmentVariation,
+                    out double segmentVariationEnergy,
+                    out double segmentMinimumClearance,
+                    out reason))
+            {
+                return false;
+            }
+
+            maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, segmentMaximumCurvature);
+            curvatureSquareSum += segmentCurvatureSquareSum;
+            curvatureSampleCount += segmentCurvatureSampleCount;
+            totalCurvatureVariation += segmentVariation;
+            curvatureVariationEnergy += segmentVariationEnergy;
+            minimumClearance = Math.Min(minimumClearance, segmentMinimumClearance);
+            int endIndex = outputPath.Count - 1;
+            int startIndex = endIndex - samples.Count + 1;
+            outputSegments.Add(new SmoothedPathSegment(
+                segment.SegmentIndex,
+                segment.Direction,
+                startIndex,
+                endIndex,
+                segment.StartsAtGearSwitch,
+                segment.EndsAtGearSwitch));
+        }
+
+        double rmsCurvature = curvatureSampleCount == 0 ? 0d : Math.Sqrt(curvatureSquareSum / curvatureSampleCount);
+        analysis = new PathGeometryAnalysis(
+            outputPath,
+            outputSegments,
+            cumulativeArcLength,
+            maximumAbsoluteVehicleCurvature,
+            rmsCurvature,
+            totalCurvatureVariation,
+            curvatureVariationEnergy,
+            minimumClearance);
+        return true;
+    }
+
+    private static bool TryAnalyzeSegment(
+        PreparedDirectionSegment segment,
+        IReadOnlyList<SmoothingPoint2D> samples,
+        ref double cumulativeArcLength,
+        ref double previousOutputHeading,
+        ref double previousOutputUnwrappedHeading,
+        ref bool hasPreviousOutputHeading,
+        List<SmoothedPathPoint> output,
+        out double maximumAbsoluteVehicleCurvature,
+        out double curvatureSquareSum,
+        out int curvatureSampleCount,
+        out double totalCurvatureVariation,
+        out double curvatureVariationEnergy,
+        out double minimumClearance,
+        out string reason)
+    {
+        maximumAbsoluteVehicleCurvature = 0d;
+        curvatureSquareSum = 0d;
+        curvatureSampleCount = 0;
+        totalCurvatureVariation = 0d;
+        curvatureVariationEnergy = 0d;
+        minimumClearance = double.PositiveInfinity;
+        reason = string.Empty;
+        int count = samples.Count;
+        var localArcLengths = new double[count];
+        var headings = new double[count];
+        var unwrappedHeadings = new double[count];
+        var geometricCurvatures = new double[count];
+
+        for (int index = 1; index < count; index++)
+        {
+            double distance = Distance(samples[index - 1], samples[index]);
+            if (!NumericGuard.IsFinite(distance) || distance <= MinimumDistanceMeters)
+            {
+                reason = "同一方向段中包含重复或退化的路径点。";
+                return false;
+            }
+            localArcLengths[index] = localArcLengths[index - 1] + distance;
+        }
+
+        for (int index = 0; index < count; index++)
+        {
+            double travelHeading;
+            if (count == 1)
+            {
+                travelHeading = segment.Direction == TravelDirection.Forward
+                    ? samples[index].Heading
+                    : samples[index].Heading - Math.PI;
+            }
+            else if (index == 0)
+            {
+                travelHeading = Math.Atan2(samples[1].Y - samples[0].Y, samples[1].X - samples[0].X);
+            }
+            else if (index == count - 1)
+            {
+                travelHeading = Math.Atan2(samples[index].Y - samples[index - 1].Y,
+                    samples[index].X - samples[index - 1].X);
+            }
+            else
+            {
+                travelHeading = Math.Atan2(samples[index + 1].Y - samples[index - 1].Y,
+                    samples[index + 1].X - samples[index - 1].X);
+            }
+
+            double heading = AngleMath.NormalizeRadians(
+                segment.Direction == TravelDirection.Forward ? travelHeading : travelHeading + Math.PI);
+            if (!NumericGuard.IsFinite(heading))
+            {
+                reason = "候选路径航向无法归一化。";
+                return false;
+            }
+
+            headings[index] = heading;
+            if (index == 0 && !hasPreviousOutputHeading)
+            {
+                unwrappedHeadings[index] = heading;
+            }
+            else if (index == 0)
+            {
+                unwrappedHeadings[index] = previousOutputUnwrappedHeading +
+                    AngleMath.ShortestSignedDifference(previousOutputHeading, heading);
+            }
+            else
+            {
+                unwrappedHeadings[index] = unwrappedHeadings[index - 1] +
+                    AngleMath.ShortestSignedDifference(headings[index - 1], heading);
+            }
+        }
+
+        for (int index = 0; index < count; index++)
+        {
+            if (count == 1)
+            {
+                geometricCurvatures[index] = 0d;
+            }
+            else if (index == 0)
+            {
+                geometricCurvatures[index] = (unwrappedHeadings[1] - unwrappedHeadings[0]) /
+                    (localArcLengths[1] - localArcLengths[0]);
+            }
+            else if (index == count - 1)
+            {
+                geometricCurvatures[index] = (unwrappedHeadings[index] - unwrappedHeadings[index - 1]) /
+                    (localArcLengths[index] - localArcLengths[index - 1]);
+            }
+            else
+            {
+                geometricCurvatures[index] = (unwrappedHeadings[index + 1] - unwrappedHeadings[index - 1]) /
+                    (localArcLengths[index + 1] - localArcLengths[index - 1]);
+            }
+
+            if (!NumericGuard.IsFinite(geometricCurvatures[index]))
+            {
+                reason = "候选路径曲率计算产生了非法数值。";
+                return false;
+            }
+        }
+
+        for (int index = 0; index < count; index++)
+        {
+            SmoothingPoint2D sample = samples[index];
+            double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d;
+            double vehicleCurvature = directionSign * geometricCurvatures[index];
+            double arcLength = cumulativeArcLength + localArcLengths[index];
+            bool isGearSwitch = index == 0 && segment.StartsAtGearSwitch;
+            SmoothedPathPointSource source = isGearSwitch ? SmoothedPathPointSource.GearSwitch : sample.Source;
+            output.Add(new SmoothedPathPoint(
+                sample.X,
+                sample.Y,
+                headings[index],
+                unwrappedHeadings[index],
+                arcLength,
+                segment.Direction,
+                geometricCurvatures[index],
+                vehicleCurvature,
+                sample.BodyClearance,
+                isGearSwitch,
+                source));
+
+            maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, Math.Abs(vehicleCurvature));
+            curvatureSquareSum += vehicleCurvature * vehicleCurvature;
+            curvatureSampleCount++;
+            minimumClearance = Math.Min(minimumClearance, sample.BodyClearance);
+            if (index > 0)
+            {
+                double deltaCurvature = geometricCurvatures[index] - geometricCurvatures[index - 1];
+                double deltaArc = localArcLengths[index] - localArcLengths[index - 1];
+                totalCurvatureVariation += Math.Abs(deltaCurvature);
+                curvatureVariationEnergy += (deltaCurvature / deltaArc) * (deltaCurvature / deltaArc) * deltaArc;
+            }
+        }
+
+        cumulativeArcLength += localArcLengths[count - 1];
+        previousOutputHeading = headings[count - 1];
+        previousOutputUnwrappedHeading = unwrappedHeadings[count - 1];
+        hasPreviousOutputHeading = true;
+        return true;
+    }
+
+    private static bool TryResampleByGeometry(
+        IReadOnlyList<SmoothingPoint2D> input,
+        double spacingMeters,
+        out IReadOnlyList<SmoothingPoint2D> samples,
+        out string reason)
+    {
+        samples = null;
+        reason = string.Empty;
+        var normalized = new List<SmoothingPoint2D>(input.Count);
+        double localArcLength = 0d;
+        for (int index = 0; index < input.Count; index++)
+        {
+            SmoothingPoint2D point = input[index];
+            if (!IsValidPoint(point))
+            {
+                reason = "候选路径点包含非法数值。";
+                return false;
+            }
+
+            if (index > 0)
+            {
+                double distance = Distance(input[index - 1], point);
+                if (!NumericGuard.IsFinite(distance) || distance <= MinimumDistanceMeters)
+                {
+                    reason = "同一方向段中包含重复或退化的路径点。";
+                    return false;
+                }
+                localArcLength += distance;
+            }
+
+            normalized.Add(new SmoothingPoint2D(
+                point.X, point.Y, localArcLength, point.Heading, point.UnwrappedHeading,
+                point.BodyClearance, point.IsGearSwitchPoint, point.Source));
+        }
+
+        if (normalized.Count == 1)
+        {
+            samples = normalized;
+            return true;
+        }
+
+        var result = new List<SmoothingPoint2D> { normalized[0] };
+        double finalArc = normalized[normalized.Count - 1].ArcLength;
+        int rightIndex = 1;
+        for (double targetArc = spacingMeters; targetArc < finalArc - MinimumDistanceMeters; targetArc += spacingMeters)
+        {
+            while (rightIndex < normalized.Count - 1 && normalized[rightIndex].ArcLength < targetArc)
+                rightIndex++;
+            SmoothingPoint2D left = normalized[rightIndex - 1];
+            SmoothingPoint2D right = normalized[rightIndex];
+            double ratio = (targetArc - left.ArcLength) / (right.ArcLength - left.ArcLength);
+            double unwrappedHeading = left.UnwrappedHeading + ratio * (right.UnwrappedHeading - left.UnwrappedHeading);
+            result.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));
+        }
+
+        result.Add(normalized[normalized.Count - 1]);
+        samples = result;
+        return true;
+    }
+
+    private static bool IsValidSegment(PreparedDirectionSegment segment, int expectedIndex, out string reason)
+    {
+        reason = string.Empty;
+        if (segment == null || segment.SegmentIndex != expectedIndex ||
+            (segment.Direction != TravelDirection.Forward && segment.Direction != TravelDirection.Reverse) ||
+            segment.Points == null || segment.Points.Count == 0)
+        {
+            reason = "候选方向段索引、方向或点集无效。";
+            return false;
+        }
+        return true;
+    }
+
+    private static bool TryValidateBoundary(
+        IReadOnlyList<PreparedDirectionSegment> segments,
+        int segmentIndex,
+        out string reason)
+    {
+        reason = string.Empty;
+        PreparedDirectionSegment current = segments[segmentIndex];
+        if (segmentIndex == segments.Count - 1 && current.EndsAtGearSwitch)
+        {
+            reason = "末个方向段不得声明不存在的后续换向点。";
+            return false;
+        }
+        if (segmentIndex == 0)
+        {
+            if (current.StartsAtGearSwitch)
+            {
+                reason = "首个方向段不得从换向点开始。";
+                return false;
+            }
+            return true;
+        }
+
+        PreparedDirectionSegment previous = segments[segmentIndex - 1];
+        if (previous == null || !previous.EndsAtGearSwitch || !current.StartsAtGearSwitch ||
+            previous.Direction == current.Direction)
+        {
+            reason = "方向段边界必须是前后成对且方向相反的换向点。";
+            return false;
+        }
+
+        SmoothingPoint2D previousEnd = previous.Points[previous.Points.Count - 1];
+        SmoothingPoint2D currentStart = current.Points[0];
+        if (!currentStart.IsGearSwitchPoint ||
+            Math.Abs(previousEnd.X - currentStart.X) > BoundaryToleranceMeters ||
+            Math.Abs(previousEnd.Y - currentStart.Y) > BoundaryToleranceMeters ||
+            Math.Abs(AngleMath.ShortestSignedDifference(previousEnd.Heading, currentStart.Heading)) > BoundaryToleranceMeters)
+        {
+            reason = "换向点两侧必须保留同一位姿和航向。";
+            return false;
+        }
+        return true;
+    }
+
+    private static bool IsValidPoint(SmoothingPoint2D point)
+    {
+        return point != null && NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y) &&
+            NumericGuard.IsFinite(point.Heading) && NumericGuard.IsFinite(point.UnwrappedHeading) &&
+            NumericGuard.IsFinite(point.ArcLength) && NumericGuard.IsFinite(point.BodyClearance) &&
+            point.BodyClearance >= 0d;
+    }
+
+    private static double Distance(SmoothingPoint2D left, SmoothingPoint2D right)
+    {
+        double deltaX = right.X - left.X;
+        double deltaY = right.Y - left.Y;
+        return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathSmoothingPreprocessor.cs

@@ -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;
+    }
+}

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PreparedDirectionSegment.cs

@@ -0,0 +1,50 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
+
+/// <summary>已校验、已按单一行驶方向分割并重采样的路径段。</summary>
+public sealed class PreparedDirectionSegment
+{
+    /// <summary>创建不可变方向段。</summary>
+    public PreparedDirectionSegment(
+        int segmentIndex,
+        TravelDirection direction,
+        IReadOnlyList<SmoothingPoint2D> points,
+        bool startsAtGearSwitch,
+        bool endsAtGearSwitch)
+    {
+        if (segmentIndex < 0) throw new ArgumentOutOfRangeException(nameof(segmentIndex));
+        if (points == null || points.Count == 0) throw new ArgumentException("A prepared segment requires points.", nameof(points));
+
+        SegmentIndex = segmentIndex;
+        Direction = direction;
+        Points = CopyReadOnly(points);
+        StartsAtGearSwitch = startsAtGearSwitch;
+        EndsAtGearSwitch = endsAtGearSwitch;
+    }
+
+    /// <summary>从零开始的分段序号；在 <see cref="PreparedPath.Segments"/> 中必须与其位置一致。</summary>
+    public int SegmentIndex { get; }
+
+    /// <summary>该段的唯一行驶方向。</summary>
+    public TravelDirection Direction { get; }
+
+    /// <summary>不包含相邻段点的本段不可变采样点。</summary>
+    public IReadOnlyList<SmoothingPoint2D> Points { get; }
+
+    /// <summary>本段首点是否为换向后保留的新方向点。</summary>
+    public bool StartsAtGearSwitch { get; }
+
+    /// <summary>本段末点之后是否紧邻换向点。</summary>
+    public bool EndsAtGearSwitch { get; }
+
+    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);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PreparedPath.cs

@@ -0,0 +1,49 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
+
+/// <summary>已校验并按方向拆分的粗路径输入快照。</summary>
+public sealed class PreparedPath
+{
+    /// <summary>创建不可变预处理路径。</summary>
+    public PreparedPath(IReadOnlyList<PreparedDirectionSegment> segments)
+    {
+        if (segments == null || segments.Count == 0)
+            throw new ArgumentException("A prepared path requires direction segments.", nameof(segments));
+        for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
+        {
+            if (segments[segmentIndex] == null)
+                throw new ArgumentException("A prepared path cannot contain null direction segments.", nameof(segments));
+        }
+
+        Segments = CopyReadOnly(segments);
+        Points = Flatten(Segments);
+    }
+
+    /// <summary>按原始前进/倒车拓扑排列的方向段。</summary>
+    public IReadOnlyList<PreparedDirectionSegment> Segments { get; }
+
+    /// <summary>将所有方向段顺序拼接后的点；换向重复点保留两次。</summary>
+    public IReadOnlyList<SmoothingPoint2D> Points { get; }
+
+    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> Flatten(IReadOnlyList<PreparedDirectionSegment> segments)
+    {
+        var points = new List<SmoothingPoint2D>();
+        for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
+        {
+            PreparedDirectionSegment segment = segments[segmentIndex];
+            for (int pointIndex = 0; pointIndex < segment.Points.Count; pointIndex++)
+                points.Add(segment.Points[pointIndex]);
+        }
+        return new ReadOnlyCollection<SmoothingPoint2D>(points);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/SmoothingPoint2D.cs

@@ -0,0 +1,53 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
+
+/// <summary>供平滑算法处理的二维路径采样点；所有长度单位均为 m，航向单位为 rad。</summary>
+public sealed class SmoothingPoint2D
+{
+    /// <summary>创建不可变二维路径点。</summary>
+    public SmoothingPoint2D(
+        double xMeters,
+        double yMeters,
+        double arcLengthMeters,
+        double headingRadians,
+        double unwrappedHeadingRadians,
+        double bodyClearanceMeters,
+        bool isGearSwitchPoint,
+        SmoothedPathPointSource source)
+    {
+        X = xMeters;
+        Y = yMeters;
+        ArcLength = arcLengthMeters;
+        Heading = headingRadians;
+        UnwrappedHeading = unwrappedHeadingRadians;
+        BodyClearance = bodyClearanceMeters;
+        IsGearSwitchPoint = isGearSwitchPoint;
+        Source = source;
+    }
+
+    /// <summary>世界 X 坐标，单位 m。</summary>
+    public double X { get; }
+
+    /// <summary>世界 Y 坐标，单位 m。</summary>
+    public double Y { get; }
+
+    /// <summary>本方向段中的累计弧长，单位 m。</summary>
+    public double ArcLength { get; }
+
+    /// <summary>归一化的车辆航向，单位 rad。</summary>
+    public double Heading { get; }
+
+    /// <summary>连续展开的车辆航向，单位 rad。</summary>
+    public double UnwrappedHeading { get; }
+
+    /// <summary>输入路径携带的保守净空，单位 m。</summary>
+    public double BodyClearance { get; }
+
+    /// <summary>该点是否为新方向段开始处的换向点。</summary>
+    public bool IsGearSwitchPoint { get; }
+
+    /// <summary>该点在平滑流程中的来源。</summary>
+    public SmoothedPathPointSource Source { get; }
+}

ClumsyPilot/tests/verify_path_smoothing_geometry.ps1

@@ -0,0 +1,377 @@
+param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
+
+$ErrorActionPreference = 'Stop'
+$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
+
+function Assert-True($Actual, [string]$Message) {
+    if (-not $Actual) { throw $Message }
+}
+
+function Assert-Equal($Expected, $Actual, [string]$Message) {
+    if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
+}
+
+function Assert-Near([double]$Expected, [double]$Actual, [double]$Tolerance, [string]$Message) {
+    if ([Math]::Abs($Expected - $Actual) -gt $Tolerance) {
+        throw "$Message Expected=$Expected Actual=$Actual Tolerance=$Tolerance"
+    }
+}
+
+function Assert-False($Actual, [string]$Message) {
+    if ($Actual) { throw $Message }
+}
+
+function Assert-Throws([scriptblock]$Action, [string]$Message) {
+    try {
+        & $Action
+    }
+    catch {
+        return
+    }
+    throw $Message
+}
+
+function Get-RequiredType([string]$Name) {
+    return $assembly.GetType($Name, $true)
+}
+
+function New-GeometryPoint(
+    [double]$X,
+    [double]$Y,
+    [double]$ArcLength,
+    [double]$Heading,
+    [double]$UnwrappedHeading,
+    [bool]$IsGearSwitch = $false) {
+    return [Activator]::CreateInstance($pointType, @(
+        $X, $Y, $ArcLength, $Heading, $UnwrappedHeading,
+        [double]1.0, $IsGearSwitch, $anchor))
+}
+
+function New-DirectionSegment(
+    [int]$Index,
+    $Direction,
+    [object[]]$Points,
+    [bool]$StartsAtGearSwitch = $false,
+    [bool]$EndsAtGearSwitch = $false) {
+    $typedPoints = [Array]::CreateInstance($pointType, $Points.Count)
+    for ($pointIndex = 0; $pointIndex -lt $Points.Count; $pointIndex++) {
+        $typedPoints.SetValue($Points[$pointIndex], $pointIndex)
+    }
+
+    return [Activator]::CreateInstance($segmentType, @(
+        $Index, $Direction, $typedPoints, $StartsAtGearSwitch, $EndsAtGearSwitch))
+}
+
+function New-CoarsePoint(
+    [double]$X,
+    [double]$Y,
+    [double]$ArcLength,
+    $Direction,
+    [bool]$IsGearSwitch = $false) {
+    return [Activator]::CreateInstance($coarsePointType, @(
+        $X, $Y, [double]0.0, [double]0.0, $ArcLength, $Direction,
+        [double]0.0, [double]1.0, $IsGearSwitch, $coarseAnchor))
+}
+
+function Invoke-Analysis([object[]]$Segments, [double]$Spacing = 0.05) {
+    $typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
+    for ($index = 0; $index -lt $Segments.Count; $index++) {
+        $typedSegments.SetValue($Segments[$index], $index)
+    }
+
+    $arguments = [object[]]@($typedSegments, $Spacing, $null, $null)
+    $accepted = $analyzeMethod.Invoke($analyzer, $arguments)
+    $description = [string]::Join(',', @($Segments | ForEach-Object {
+        "index=$($_.SegmentIndex);direction=$($_.Direction);points=$($_.Points.Count)"
+    }))
+    Assert-True $accepted ("Geometry analysis must accept the analytic candidate. Reason=" + $arguments[3] + '; Segments=' + $description)
+    Assert-True ($null -ne $arguments[2]) 'Successful geometry analysis must return PathGeometryAnalysis.'
+    return $arguments[2]
+}
+
+function Assert-AnalysisRejected([object[]]$Segments, [string]$Message) {
+    $typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
+    for ($segmentIndex = 0; $segmentIndex -lt $Segments.Count; $segmentIndex++) {
+        $typedSegments.SetValue($Segments[$segmentIndex], $segmentIndex)
+    }
+
+    $arguments = [object[]]@($typedSegments, [double]0.05, $null, $null)
+    $accepted = $analyzeMethod.Invoke($analyzer, $arguments)
+    Assert-True (-not $accepted) ($Message + '; Reason=' + $arguments[3])
+}
+
+function Invoke-RejectedAnalysis([object[]]$Segments, [string]$Message) {
+    $typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
+    for ($index = 0; $index -lt $Segments.Count; $index++) {
+        $typedSegments.SetValue($Segments[$index], $index)
+    }
+
+    $arguments = [object[]]@($typedSegments, [double]0.05, $null, $null)
+    $accepted = $analyzeMethod.Invoke($analyzer, $arguments)
+    Assert-False $accepted ($Message + '; Reason=' + $arguments[3])
+}
+
+function New-CoarsePathPoint(
+    [double]$X,
+    [double]$Y,
+    [double]$ArcLength,
+    $Direction,
+    [bool]$IsGearSwitch = $false,
+    [string]$SourceName = 'MotionPrimitive') {
+    return [Activator]::CreateInstance($coarsePointType, @(
+        $X, $Y, [double]0.0, [double]0.0, $ArcLength,
+        $Direction, [double]0.0, [double]1.0, $IsGearSwitch,
+        [Enum]::Parse($coarsePointSourceType, $SourceName)))
+}
+
+function New-EmptyGeometryMap {
+    $mapRequest = [Activator]::CreateInstance($mapRequestType)
+    $mapRequest.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
+    $mapRequest.ResolutionMm = [single]50
+    $mapRequest.AllowExplicitEmptyMap = $true
+    $map = [Activator]::CreateInstance($mapFactoryType).Create($mapRequest).Map
+    Assert-True ($null -ne $map) 'Geometry test must create an explicit empty planning map.'
+    return $map
+}
+
+$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
+$processing = $root + 'Processing.'
+$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
+
+$analyzerType = Get-RequiredType ($processing + 'PathGeometryAnalyzer')
+$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
+$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
+$pointType = Get-RequiredType ($processing + 'SmoothingPoint2D')
+$segmentType = Get-RequiredType ($processing + 'PreparedDirectionSegment')
+$preparedPathType = Get-RequiredType ($processing + 'PreparedPath')
+$analysisType = Get-RequiredType ($processing + 'PathGeometryAnalysis')
+$preprocessorType = Get-RequiredType ($processing + 'PathSmoothingPreprocessor')
+$resamplerType = Get-RequiredType ($processing + 'ArcLengthResampler')
+$requestType = Get-RequiredType ($root + 'PathSmoothingRequest')
+$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
+$coarsePointType = Get-RequiredType ($coarsePath + 'CoarsePathPoint')
+$coarseSegmentType = Get-RequiredType ($coarsePath + 'PathSegment')
+$coarseSourceType = Get-RequiredType ($coarsePath + 'CoarsePathPointSource')
+$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
+$mapType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningGridMap'
+$coarsePointType = Get-RequiredType ($coarsePath + 'CoarsePathPoint')
+$coarseSegmentType = Get-RequiredType ($coarsePath + 'PathSegment')
+$coarsePointSourceType = Get-RequiredType ($coarsePath + 'CoarsePathPointSource')
+$smoothingRequestType = Get-RequiredType ($root + 'PathSmoothingRequest')
+$smoothingConfigurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
+$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
+$boundsType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm'
+$mapRequestType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapRequest'
+$mapFactoryType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapFactory'
+
+Assert-True ($null -ne $preparedPathType) 'PreparedPath must be discoverable for smoothing algorithms.'
+Assert-True ($null -ne $preprocessorType) 'PathSmoothingPreprocessor must be discoverable for request preparation.'
+Assert-True ($null -ne $resamplerType) 'ArcLengthResampler must be discoverable for deterministic resampling.'
+
+$analyzer = [Activator]::CreateInstance($analyzerType)
+$analyzeMethod = $analyzerType.GetMethod('TryAnalyze')
+Assert-True ($null -ne $analyzeMethod) 'PathGeometryAnalyzer must expose TryAnalyze.'
+Assert-Equal 4 $analyzeMethod.GetParameters().Length 'TryAnalyze must accept segments, spacing, analysis, and reason.'
+
+$forward = [Enum]::Parse($directionType, 'Forward')
+$reverse = [Enum]::Parse($directionType, 'Reverse')
+$anchor = [Enum]::Parse($sourceType, 'Anchor')
+$coarseAnchor = [Enum]::Parse($coarseSourceType, 'Start')
+
+# Forward straight: resampling is exactly 0.05 m, preserves the exact endpoint, and has zero curvature.
+$straight = New-DirectionSegment 0 $forward @(
+    (New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
+    (New-GeometryPoint 1.0 0.0 1.0 0.0 0.0))
+$straightAnalysis = Invoke-Analysis @($straight)
+Assert-Equal 21 $straightAnalysis.Path.Count 'A one-metre straight must produce twenty 0.05 m intervals plus the initial point.'
+for ($index = 1; $index -lt $straightAnalysis.Path.Count; $index++) {
+    $left = $straightAnalysis.Path[$index - 1]
+    $right = $straightAnalysis.Path[$index]
+    $distance = [Math]::Sqrt(($right.X - $left.X) * ($right.X - $left.X) + ($right.Y - $left.Y) * ($right.Y - $left.Y))
+    Assert-Near 0.05 $distance 0.000000001 'Straight resampling intervals must be exactly 0.05 m.'
+    Assert-Near 0.0 $right.GeometricCurvature 0.000000001 'A forward straight must have zero geometric curvature.'
+    Assert-Near 0.0 $right.VehicleCurvature 0.000000001 'A forward straight must have zero vehicle curvature.'
+}
+$straightEnd = $straightAnalysis.Path[$straightAnalysis.Path.Count - 1]
+Assert-Near 1.0 $straightEnd.X 0.0 'Resampling must retain the exact final X coordinate.'
+Assert-Near 0.0 $straightEnd.Y 0.0 'Resampling must retain the exact final Y coordinate.'
+
+# A forward R=2 quarter circle has positive +0.5 1/m vehicle curvature.
+$forwardArcPoints = New-Object System.Collections.Generic.List[object]
+for ($index = 0; $index -le 32; $index++) {
+    $theta = ([Math]::PI / 2.0) * $index / 32.0
+    $x = 2.0 * [Math]::Sin($theta)
+    $y = 2.0 * (1.0 - [Math]::Cos($theta))
+    $arcLength = 2.0 * $theta
+    [void]$forwardArcPoints.Add((New-GeometryPoint $x $y $arcLength $theta $theta))
+}
+$forwardArc = New-DirectionSegment 0 $forward $forwardArcPoints.ToArray()
+$forwardArcAnalysis = Invoke-Analysis @($forwardArc)
+$forwardArcMidpoint = $forwardArcAnalysis.Path[[int]($forwardArcAnalysis.Path.Count / 2)]
+Assert-Near 0.5 $forwardArcMidpoint.GeometricCurvature 0.01 'An R=2 quarter circle must have geometric curvature +0.5 1/m.'
+Assert-Near 0.5 $forwardArcMidpoint.VehicleCurvature 0.01 'A forward R=2 quarter circle must have vehicle curvature +0.5 1/m.'
+
+# The same spatial R=2 circle in reverse retains geometric curvature but negates vehicle curvature.
+$reverseArc = New-DirectionSegment 0 $reverse $forwardArcPoints.ToArray()
+$reverseArcAnalysis = Invoke-Analysis @($reverseArc)
+$reverseArcMidpoint = $reverseArcAnalysis.Path[[int]($reverseArcAnalysis.Path.Count / 2)]
+Assert-Near 0.5 $reverseArcMidpoint.GeometricCurvature 0.01 'Reverse travel must not change geometric curvature.'
+Assert-Near -0.5 $reverseArcMidpoint.VehicleCurvature 0.01 'A reverse R=2 quarter circle must have vehicle curvature -0.5 1/m.'
+
+# Gear-switch poses are intentionally duplicated: they keep equal arc length and never enter a derivative denominator.
+$forwardBeforeSwitch = New-DirectionSegment 0 $forward @(
+    (New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
+    (New-GeometryPoint 1.0 0.0 1.0 0.0 0.0 $true)) $false $true
+$reverseAfterSwitch = New-DirectionSegment 1 $reverse @(
+    (New-GeometryPoint 1.0 0.0 1.0 0.0 0.0 $true),
+    (New-GeometryPoint 0.0 0.0 2.0 0.0 0.0)) $true $false
+$switchAnalysis = Invoke-Analysis @($forwardBeforeSwitch, $reverseAfterSwitch)
+$firstSegment = $switchAnalysis.Segments[0]
+$secondSegment = $switchAnalysis.Segments[1]
+$switchLeft = $switchAnalysis.Path[$firstSegment.EndIndex]
+$switchRight = $switchAnalysis.Path[$secondSegment.StartIndex]
+Assert-Near $switchLeft.X $switchRight.X 0.0 'Gear-switch endpoints must retain duplicate X coordinates.'
+Assert-Near $switchLeft.Y $switchRight.Y 0.0 'Gear-switch endpoints must retain duplicate Y coordinates.'
+Assert-Near $switchLeft.ArcLength $switchRight.ArcLength 0.0 'Gear-switch endpoints must retain duplicate arc length.'
+Assert-Equal 'Forward' $switchLeft.Direction.ToString() 'The first gear-switch pose must retain its forward segment direction.'
+Assert-Equal 'Reverse' $switchRight.Direction.ToString() 'The second gear-switch pose must retain its reverse segment direction.'
+Assert-True (-not [double]::IsNaN($switchLeft.GeometricCurvature)) 'No derivative may cross the gear-switch duplicate point.'
+Assert-True (-not [double]::IsNaN($switchRight.GeometricCurvature)) 'No reverse derivative may cross the gear-switch duplicate point.'
+
+# Curvature at a curved segment's end and the following straight reverse segment's start must remain independently differentiated.
+$forwardArcToSwitch = New-DirectionSegment 0 $forward $forwardArcPoints.ToArray() $false $true
+$reverseStraightAfterArc = New-DirectionSegment 1 $reverse @(
+    (New-GeometryPoint 2.0 2.0 0.0 ([Math]::PI / 2.0) ([Math]::PI / 2.0) $true),
+    (New-GeometryPoint 2.0 1.0 1.0 ([Math]::PI / 2.0) ([Math]::PI / 2.0))) $true $false
+$curveSwitchAnalysis = Invoke-Analysis @($forwardArcToSwitch, $reverseStraightAfterArc)
+$reverseStraightStart = $curveSwitchAnalysis.Path[$curveSwitchAnalysis.Segments[1].StartIndex]
+Assert-Near 0.0 $reverseStraightStart.GeometricCurvature 0.000000001 'A gear-switch must not use the preceding curve to differentiate a reverse straight segment.'
+
+# A boundary that claims a gear switch must be a duplicated pose with opposite direction; discontinuities are rejected.
+$invalidSwitch = New-DirectionSegment 1 $reverse @(
+    (New-GeometryPoint 1.2 0.0 1.0 0.0 0.0 $true),
+    (New-GeometryPoint 0.2 0.0 2.0 0.0 0.0)) $true $false
+Assert-AnalysisRejected @($forwardBeforeSwitch, $invalidSwitch) 'A discontinuous gear-switch boundary must be rejected.'
+$trailingGearSwitch = New-DirectionSegment 0 $forward @(
+    (New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
+    (New-GeometryPoint 1.0 0.0 1.0 0.0 0.0)) $false $true
+Assert-AnalysisRejected @($trailingGearSwitch) 'The final direction segment must not advertise a non-existent trailing gear switch.'
+
+# The public preprocessor receives a raw coarse path and resets every prepared direction segment to local arc length zero.
+$coarsePoints = [Array]::CreateInstance($coarsePointType, 4)
+$coarsePoints.SetValue((New-CoarsePoint 0.0 0.0 0.0 $forward), 0)
+$coarsePoints.SetValue((New-CoarsePoint 1.0 0.0 1.0 $forward), 1)
+$coarsePoints.SetValue((New-CoarsePoint 1.0 0.0 1.0 $reverse $true), 2)
+$coarsePoints.SetValue((New-CoarsePoint 0.0 0.0 2.0 $reverse), 3)
+$coarseSegments = [Array]::CreateInstance($coarseSegmentType, 2)
+$coarseSegments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(0, $forward, 0, 1, $false, $true)), 0)
+$coarseSegments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(1, $reverse, 2, 3, $true, $false)), 1)
+$vehicle = [Activator]::CreateInstance($vehicleType)
+$vehicle.LengthMeters = 1.0
+$vehicle.WidthMeters = 0.5
+$vehicle.SafetyMarginMeters = 0.0
+$vehicle.MaximumCurvaturePerMeter = 1.0
+$configuration = [Activator]::CreateInstance($configurationType)
+$uninitializedMap = [System.Runtime.Serialization.FormatterServices]::GetUninitializedObject($mapType)
+$request = [Activator]::CreateInstance($requestType, @($coarsePoints, $coarseSegments, $uninitializedMap, $vehicle, $configuration))
+$preprocessor = [Activator]::CreateInstance($preprocessorType)
+$prepareMethod = $preprocessorType.GetMethod('TryPrepare')
+$prepareArguments = [object[]]@($request, $null, $null)
+$prepared = $prepareMethod.Invoke($preprocessor, $prepareArguments)
+Assert-True $prepared ('Preprocessor must accept a legal forward/reverse raw coarse path. Reason=' + $prepareArguments[2])
+Assert-Near 0.0 $prepareArguments[1].Segments[1].Points[0].ArcLength 0.0 'Every prepared direction segment must begin at local arc length zero.'
+Assert-True $prepareArguments[1].Segments[1].Points[0].IsGearSwitchPoint 'The reverse prepared segment must retain its gear-switch point.'
+
+# Finite coordinates can still overflow distance arithmetic; public resampling must reject them instead of emitting NaN/Infinity.
+$overflowSegment = New-DirectionSegment 0 $forward @(
+    (New-GeometryPoint -1.0e308 0.0 0.0 0.0 0.0),
+    (New-GeometryPoint 1.0e308 0.0 1.0 0.0 0.0))
+$resampler = [Activator]::CreateInstance($resamplerType)
+$segmentResampleMethod = @($resamplerType.GetMethods() | Where-Object {
+    $_.Name -eq 'TryResample' -and $_.GetParameters()[0].ParameterType -eq $segmentType
+})[0]
+$resampleArguments = [object[]]@($overflowSegment, [double]0.05, $null, $null)
+$resampled = $segmentResampleMethod.Invoke($resampler, $resampleArguments)
+Assert-True (-not $resampled) ('Resampling must reject a distance overflow. Reason=' + $resampleArguments[3])
+
+# A prepared path must reject null direction segments rather than silently dropping them during flattening.
+$nullSegmentArray = [Array]::CreateInstance($segmentType, 1)
+$nullSegmentRejected = $false
+try { [void][Activator]::CreateInstance($preparedPathType, @($nullSegmentArray)) } catch { $nullSegmentRejected = $true }
+Assert-True $nullSegmentRejected 'PreparedPath must reject a null direction segment.'
+
+# Unwrapped heading must not jump by 2π when tangents cross the -π/π branch cut.
+$crossing = New-DirectionSegment 0 $forward @(
+    (New-GeometryPoint 0.0 0.0 0.0 (170.0 * [Math]::PI / 180.0) (170.0 * [Math]::PI / 180.0)),
+    (New-GeometryPoint -1.0 ([Math]::Tan(10.0 * [Math]::PI / 180.0)) 1.015 (170.0 * [Math]::PI / 180.0) (170.0 * [Math]::PI / 180.0)),
+    (New-GeometryPoint -2.0 0.0 2.03 (-170.0 * [Math]::PI / 180.0) (-170.0 * [Math]::PI / 180.0)))
+$crossingAnalysis = Invoke-Analysis @($crossing)
+for ($index = 1; $index -lt $crossingAnalysis.Path.Count; $index++) {
+    $difference = [Math]::Abs($crossingAnalysis.Path[$index].UnwrappedHeading - $crossingAnalysis.Path[$index - 1].UnwrappedHeading)
+    Assert-True ($difference -lt [Math]::PI) 'Unwrapped headings must remain continuous across the ±π branch cut.'
+}
+
+# The request preprocessor must reset arc length independently for every direction segment,
+# while retaining the duplicated pose that represents a legal forward-to-reverse gear switch.
+$preprocessor = [Activator]::CreateInstance($preprocessorType)
+$prepareMethod = $preprocessorType.GetMethod('TryPrepare')
+Assert-True ($null -ne $prepareMethod) 'PathSmoothingPreprocessor must expose TryPrepare.'
+$coarsePath = [Array]::CreateInstance($coarsePointType, 5)
+$coarsePath.SetValue((New-CoarsePathPoint 0.0 0.0 0.0 $forward $false 'Start'), 0)
+$coarsePath.SetValue((New-CoarsePathPoint 1.0 0.0 1.0 $forward), 1)
+$coarsePath.SetValue((New-CoarsePathPoint 2.0 0.0 2.0 $forward), 2)
+$coarsePath.SetValue((New-CoarsePathPoint 2.0 0.0 2.0 $reverse $true), 3)
+$coarsePath.SetValue((New-CoarsePathPoint 1.0 0.0 3.0 $reverse), 4)
+$coarseSegments = [Array]::CreateInstance($coarseSegmentType, 2)
+$coarseSegments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(0, $forward, 0, 2, $false, $true)), 0)
+$coarseSegments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(1, $reverse, 3, 4, $true, $false)), 1)
+$vehicle = [Activator]::CreateInstance($vehicleType)
+$vehicle.LengthMeters = [double]0.80
+$vehicle.WidthMeters = [double]0.60
+$vehicle.SafetyMarginMeters = [double]0.05
+$vehicle.MaximumCurvaturePerMeter = [double]0.80
+$configuration = [Activator]::CreateInstance($smoothingConfigurationType)
+$smoothingRequest = [Activator]::CreateInstance($smoothingRequestType, @(
+    $coarsePath, $coarseSegments, (New-EmptyGeometryMap), $vehicle, $configuration))
+$prepareArguments = [object[]]@($smoothingRequest, $null, $null)
+Assert-True $prepareMethod.Invoke($preprocessor, $prepareArguments) ('Preprocessor must accept legal forward/reverse topology. Reason=' + $prepareArguments[2])
+$preparedPath = $prepareArguments[1]
+Assert-Equal 2 $preparedPath.Segments.Count 'Preprocessor must preserve both direction segments.'
+Assert-Near 0.0 $preparedPath.Segments[1].Points[0].ArcLength 0.0 'The reverse segment must restart local arc length at zero.'
+Assert-True $preparedPath.Segments[1].Points[0].IsGearSwitchPoint 'The duplicate reverse gear-switch point must be retained.'
+
+# Segments may meet only at a paired, coincident forward/reverse gear switch.
+$illegalGearJump = New-DirectionSegment 1 $reverse @(
+    (New-GeometryPoint 1.25 0.0 1.0 0.0 0.0 $true),
+    (New-GeometryPoint 0.25 0.0 2.0 0.0 0.0)) $true $false
+Invoke-RejectedAnalysis @($forwardBeforeSwitch, $illegalGearJump) 'A gear-switch boundary whose poses differ must be rejected.'
+$illegalNormalBoundary = New-DirectionSegment 1 $forward @(
+    (New-GeometryPoint 2.0 0.0 0.0 0.0 0.0),
+    (New-GeometryPoint 3.0 0.0 1.0 0.0 0.0)) $false $false
+Invoke-RejectedAnalysis @($straight, $illegalNormalBoundary) 'A non-gear segment boundary must be rejected.'
+
+# Finite endpoint coordinates can still overflow while computing their separation; reject before interpolation.
+$resampler = [Activator]::CreateInstance($resamplerType)
+$resamplePointsMethod = $resamplerType.GetMethods() | Where-Object {
+    $_.Name -eq 'TryResample' -and $_.GetParameters().Length -eq 4 -and
+    $_.GetParameters()[0].ParameterType -eq [System.Collections.Generic.IReadOnlyList``1].MakeGenericType($pointType)
+} | Select-Object -First 1
+Assert-True ($null -ne $resamplePointsMethod) 'ArcLengthResampler must expose point-list TryResample.'
+$hugePoints = [Array]::CreateInstance($pointType, 2)
+$hugeCoordinate = [double]::MaxValue / 2.0
+$hugePoints.SetValue((New-GeometryPoint (-$hugeCoordinate) 0.0 0.0 0.0 0.0), 0)
+$hugePoints.SetValue((New-GeometryPoint $hugeCoordinate 0.0 1.0 0.0 0.0), 1)
+$resampleArguments = [object[]]@($hugePoints, [double]0.05, $null, $null)
+Assert-False $resamplePointsMethod.Invoke($resampler, $resampleArguments) 'Resampling must reject an infinite geometric distance caused by finite coordinates.'
+
+# PreparedPath is an all-or-nothing immutable topology snapshot: null direction segments are invalid.
+$nullPreparedSegments = [Array]::CreateInstance($segmentType, 1)
+Assert-Throws { [Activator]::CreateInstance($preparedPathType, @($nullPreparedSegments)) } 'PreparedPath must reject null direction segments.'
+
+# Segment indices are deliberately dense and equal to their position in the candidate array.
+$sparseSegment = New-DirectionSegment 2 $forward @(
+    (New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
+    (New-GeometryPoint 1.0 0.0 1.0 0.0 0.0))
+Invoke-RejectedAnalysis @($sparseSegment) 'Prepared direction-segment indices must match their dense array position.'
+
+Write-Output 'Path smoothing geometry checks passed.'