From f75a3bae5ef6b88623e8fc5c8bed24f2675d2fae Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Wed, 29 Jul 2026 08:50:12 +0800 Subject: [PATCH] feat: add path smoothing geometry foundation --- .../Processing/ArcLengthResampler.cs | 144 +++++++ .../Processing/PathGeometryAnalysis.cs | 64 +++ .../Processing/PathGeometryAnalyzer.cs | 401 ++++++++++++++++++ .../Processing/PathSmoothingPreprocessor.cs | 194 +++++++++ .../Processing/PreparedDirectionSegment.cs | 50 +++ .../PathSmoothing/Processing/PreparedPath.cs | 49 +++ .../Processing/SmoothingPoint2D.cs | 53 +++ .../tests/verify_path_smoothing_geometry.ps1 | 377 ++++++++++++++++ 8 files changed, 1332 insertions(+) create mode 100644 ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/ArcLengthResampler.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalysis.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathSmoothingPreprocessor.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PreparedDirectionSegment.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PreparedPath.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/SmoothingPoint2D.cs create mode 100644 ClumsyPilot/tests/verify_path_smoothing_geometry.ps1 diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/ArcLengthResampler.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/ArcLengthResampler.cs new file mode 100644 index 0000000..52a68fb --- /dev/null +++ b/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; + +/// 按单一方向段的弧长线性插值并保留精确锚点的确定性重采样器。 +public sealed class ArcLengthResampler +{ + private const double Tolerance = 1e-10d; + + /// 以目标间距重采样一个方向段；段末锚点始终原样保留。 + public bool TryResample( + IReadOnlyList points, + double spacingMeters, + out IReadOnlyList 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 { 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(output); + return true; + } + + /// 重采样一个完整方向段并保留其换向拓扑标记。 + 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 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 CopyReadOnly(IReadOnlyList source) + { + var copy = new List(source.Count); + for (int index = 0; index < source.Count; index++) copy.Add(source[index]); + return new ReadOnlyCollection(copy); + } + + private static IReadOnlyList EmptyPoints() + { + return new ReadOnlyCollection(new List()); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalysis.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalysis.cs new file mode 100644 index 0000000..d347f02 --- /dev/null +++ b/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; + +/// 同一几何分析器产生的路径、方向段和未验证质量统计。 +public sealed class PathGeometryAnalysis +{ + internal PathGeometryAnalysis( + IReadOnlyList path, + IReadOnlyList 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; + } + + /// 完成几何重计算的不可变路径。 + public IReadOnlyList Path { get; } + + /// 完整覆盖 的不可变方向段。 + public IReadOnlyList Segments { get; } + + /// 路径总长度，单位 m。 + public double PathLengthMeters { get; } + + /// 车辆曲率绝对值峰值，单位 1/m。 + public double MaximumAbsoluteVehicleCurvaturePerMeter { get; } + + /// 车辆曲率均方根，单位 1/m。 + public double RootMeanSquareVehicleCurvaturePerMeter { get; } + + /// 不跨换向点累计的绝对曲率变化，单位 1/m。 + public double TotalAbsoluteCurvatureVariationPerMeter { get; } + + /// 不跨换向点累计的曲率变化能量。 + public double CurvatureVariationEnergy { get; } + + /// 输入点携带的最小保守净空，单位 m。 + public double MinimumBodyClearanceMeters { get; } + + private static IReadOnlyList CopyReadOnly(IReadOnlyList source) + { + var copy = new List(source == null ? 0 : source.Count); + if (source != null) + { + for (int index = 0; index < source.Count; index++) copy.Add(source[index]); + } + return new ReadOnlyCollection(copy); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs new file mode 100644 index 0000000..bb06f31 --- /dev/null +++ b/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; + +/// 在每个单独方向段内统一重采样、恢复航向并计算几何曲率。 +public sealed class PathGeometryAnalyzer +{ + private const double MinimumDistanceMeters = 1e-10d; + private const double BoundaryToleranceMeters = 1e-8d; + + /// + /// 对候选方向段进行确定性几何分析。换向点两侧永不参与同一次差分。 + /// + public bool TryAnalyze( + IReadOnlyList 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(); + var outputSegments = new List(); + 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 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 samples, + ref double cumulativeArcLength, + ref double previousOutputHeading, + ref double previousOutputUnwrappedHeading, + ref bool hasPreviousOutputHeading, + List 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 input, + double spacingMeters, + out IReadOnlyList samples, + out string reason) + { + samples = null; + reason = string.Empty; + var normalized = new List(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 { 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 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); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathSmoothingPreprocessor.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathSmoothingPreprocessor.cs new file mode 100644 index 0000000..202cf24 --- /dev/null +++ b/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; + +/// 校验粗路径契约、保护换向拓扑并产生统一间距的平滑输入。 +public sealed class PathSmoothingPreprocessor +{ + private const double Tolerance = 1e-8d; + private readonly ArcLengthResampler _resampler; + + /// 创建使用默认确定性重采样器的预处理器。 + public PathSmoothingPreprocessor() + : this(new ArcLengthResampler()) + { + } + + /// 创建使用指定重采样器的预处理器。 + public PathSmoothingPreprocessor(ArcLengthResampler resampler) + { + _resampler = resampler ?? throw new ArgumentNullException(nameof(resampler)); + } + + /// 将一条粗路径请求校验、按方向拆分并按配置间距重采样。 + 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(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(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 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 path, + IReadOnlyList 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; + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PreparedDirectionSegment.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PreparedDirectionSegment.cs new file mode 100644 index 0000000..ec6ffd5 --- /dev/null +++ b/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; + +/// 已校验、已按单一行驶方向分割并重采样的路径段。 +public sealed class PreparedDirectionSegment +{ + /// 创建不可变方向段。 + public PreparedDirectionSegment( + int segmentIndex, + TravelDirection direction, + IReadOnlyList 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; + } + + /// 从零开始的分段序号；在 中必须与其位置一致。 + public int SegmentIndex { get; } + + /// 该段的唯一行驶方向。 + public TravelDirection Direction { get; } + + /// 不包含相邻段点的本段不可变采样点。 + public IReadOnlyList Points { get; } + + /// 本段首点是否为换向后保留的新方向点。 + public bool StartsAtGearSwitch { get; } + + /// 本段末点之后是否紧邻换向点。 + public bool EndsAtGearSwitch { get; } + + private static IReadOnlyList CopyReadOnly(IReadOnlyList source) + { + var copy = new List(source.Count); + for (int index = 0; index < source.Count; index++) copy.Add(source[index]); + return new ReadOnlyCollection(copy); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PreparedPath.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PreparedPath.cs new file mode 100644 index 0000000..379c33c --- /dev/null +++ b/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; + +/// 已校验并按方向拆分的粗路径输入快照。 +public sealed class PreparedPath +{ + /// 创建不可变预处理路径。 + public PreparedPath(IReadOnlyList 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); + } + + /// 按原始前进/倒车拓扑排列的方向段。 + public IReadOnlyList Segments { get; } + + /// 将所有方向段顺序拼接后的点；换向重复点保留两次。 + public IReadOnlyList Points { get; } + + private static IReadOnlyList CopyReadOnly(IReadOnlyList source) + { + var copy = new List(source.Count); + for (int index = 0; index < source.Count; index++) copy.Add(source[index]); + return new ReadOnlyCollection(copy); + } + + private static IReadOnlyList Flatten(IReadOnlyList segments) + { + var points = new List(); + 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(points); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/SmoothingPoint2D.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/SmoothingPoint2D.cs new file mode 100644 index 0000000..1ca0e05 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/SmoothingPoint2D.cs @@ -0,0 +1,53 @@ +using System; +using MultiWheelC.TrajectoryPlanning.PathSmoothing; + +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing; + +/// 供平滑算法处理的二维路径采样点；所有长度单位均为 m，航向单位为 rad。 +public sealed class SmoothingPoint2D +{ + /// 创建不可变二维路径点。 + 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; + } + + /// 世界 X 坐标，单位 m。 + public double X { get; } + + /// 世界 Y 坐标，单位 m。 + public double Y { get; } + + /// 本方向段中的累计弧长，单位 m。 + public double ArcLength { get; } + + /// 归一化的车辆航向，单位 rad。 + public double Heading { get; } + + /// 连续展开的车辆航向，单位 rad。 + public double UnwrappedHeading { get; } + + /// 输入路径携带的保守净空，单位 m。 + public double BodyClearance { get; } + + /// 该点是否为新方向段开始处的换向点。 + public bool IsGearSwitchPoint { get; } + + /// 该点在平滑流程中的来源。 + public SmoothedPathPointSource Source { get; } +} diff --git a/ClumsyPilot/tests/verify_path_smoothing_geometry.ps1 b/ClumsyPilot/tests/verify_path_smoothing_geometry.ps1 new file mode 100644 index 0000000..4d9f8f2 --- /dev/null +++ b/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.'