diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateEvaluator.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateEvaluator.cs
new file mode 100644
index 0000000..1d04d17
--- /dev/null
+++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateEvaluator.cs
@@ -0,0 +1,543 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using System.Linq;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
+
+/// 对一个局部 G2 替换候选执行区域质量门和完整路径安全复核。
+internal sealed class LocalG2CandidateEvaluator
+{
+ private const double CurvatureRangeTolerance = 1e-6d;
+ private static readonly IReadOnlyList EmptyPath =
+ new ReadOnlyCollection(new List());
+ private static readonly IReadOnlyList EmptySegments =
+ new ReadOnlyCollection(new List());
+ private readonly PathGeometryAnalyzer _analyzer;
+ private readonly LocalG2PathSplicer _splicer;
+ private readonly SmoothedPathValidator _validator;
+
+ internal LocalG2CandidateEvaluator()
+ : this(new PathGeometryAnalyzer(), new LocalG2PathSplicer(), new SmoothedPathValidator())
+ {
+ }
+
+ internal LocalG2CandidateEvaluator(
+ PathGeometryAnalyzer analyzer,
+ LocalG2PathSplicer splicer,
+ SmoothedPathValidator validator)
+ {
+ _analyzer = analyzer ?? throw new ArgumentNullException(nameof(analyzer));
+ _splicer = splicer ?? throw new ArgumentNullException(nameof(splicer));
+ _validator = validator ?? throw new ArgumentNullException(nameof(validator));
+ }
+
+ internal LocalG2CandidateEvaluation Evaluate(
+ PreparedPath rawPath,
+ PreparedPath currentPath,
+ LocalG2SmoothingRegion region,
+ LocalG2CandidateGeometry candidate,
+ PathSmoothingRequest request,
+ LocalG2OptionsSnapshot options,
+ CancellationToken cancellationToken)
+ {
+ cancellationToken.ThrowIfCancellationRequested();
+ int candidateIndex = candidate == null ? -1 : candidate.CandidateIndex;
+ if (!HasUsableInput(rawPath, currentPath, region, candidate, request, options))
+ return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed, "局部 G2 候选评价输入无效。");
+
+ PreparedDirectionSegment currentSegment = currentPath.Segments[candidate.SegmentIndex];
+ if (!TryExtractWindow(currentSegment, candidate.StartArcLengthMeters, candidate.EndArcLengthMeters,
+ out IReadOnlyList rawWindow, out string reason) ||
+ !TryAnalyzeWindow(currentSegment, rawWindow, request.Configuration.OutputSpacingMeters,
+ out PathGeometryAnalysis rawAnalysis, out reason) ||
+ !TryAnalyzeWindow(currentSegment, candidate.RegionPoints, request.Configuration.OutputSpacingMeters,
+ out PathGeometryAnalysis candidateAnalysis, out reason) ||
+ !HasFiniteMetrics(rawAnalysis) || !HasFiniteMetrics(candidateAnalysis))
+ {
+ return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed,
+ string.IsNullOrEmpty(reason) ? "局部 G2 区域几何分析失败。" : reason);
+ }
+
+ if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(request.Vehicle, out double vehicleMaximumCurvature))
+ return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed, "车辆曲率约束无效。");
+ if (candidateAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter > vehicleMaximumCurvature + CurvatureRangeTolerance)
+ return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CurvatureExceeded, "局部 G2 候选超过车辆曲率上限。");
+
+ GetCurvatureRange(rawAnalysis.Path, out double rawMinimumCurvature, out double rawMaximumCurvature);
+ if (ExceedsRawCurvatureRange(candidateAnalysis.Path, rawMinimumCurvature, rawMaximumCurvature))
+ return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CurvatureOvershoot, "局部 G2 候选超出原始区域曲率范围。");
+
+ double maximumDeviation = MaximumDistanceToPolyline(candidate.RegionPoints, rawWindow);
+ if (!NumericGuard.IsFinite(maximumDeviation))
+ return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed, "局部 G2 候选偏差计算失败。");
+ if (maximumDeviation > options.MaximumDeviationMeters)
+ return Rejected(candidateIndex, PathSmoothingRegionFailureReason.DeviationExceeded, "局部 G2 候选偏离原始窗口过远。");
+
+ if (!_splicer.TryReplace(currentPath, candidate, out PreparedPath spliced, out reason) ||
+ !_analyzer.TryAnalyze(spliced.Segments, request.Configuration.OutputSpacingMeters, out PathGeometryAnalysis fullAnalysis, out reason) ||
+ !HasFiniteMetrics(fullAnalysis))
+ {
+ return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed,
+ string.IsNullOrEmpty(reason) ? "局部 G2 完整路径几何分析失败。" : reason);
+ }
+
+ if (!_validator.TryValidate(fullAnalysis.Path, fullAnalysis.Segments, rawPath, request.Map, request.Vehicle,
+ request.Configuration.MaximumCollisionCheckStepMeters, out IReadOnlyList safePath,
+ out double minimumClearance, out reason))
+ {
+ return Rejected(candidateIndex, PathSmoothingRegionFailureReason.Collision,
+ string.IsNullOrEmpty(reason) ? "局部 G2 完整路径安全复核失败。" : reason);
+ }
+ if (!NumericGuard.IsFinite(minimumClearance) || minimumClearance < request.Configuration.MinimumClearanceReserveMeters)
+ return new LocalG2CandidateEvaluation(false, PathSmoothingRegionFailureReason.InsufficientClearance, candidateIndex,
+ null, EmptyPath, EmptySegments, rawAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
+ candidateAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter, rawAnalysis.CurvatureVariationCost,
+ candidateAnalysis.CurvatureVariationCost, maximumDeviation, minimumClearance,
+ candidateAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter, 0d, "局部 G2 候选净空不足。");
+
+ if (candidateAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter >
+ rawAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter *
+ (1d - options.MinimumPeakGradientImprovementRatio))
+ {
+ return new LocalG2CandidateEvaluation(false, PathSmoothingRegionFailureReason.InsufficientImprovement, candidateIndex,
+ null, EmptyPath, EmptySegments, rawAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
+ candidateAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter, rawAnalysis.CurvatureVariationCost,
+ candidateAnalysis.CurvatureVariationCost, maximumDeviation, minimumClearance,
+ candidateAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter, 0d, "局部 G2 候选曲率导数峰值改善不足。");
+ }
+ if (candidateAnalysis.CurvatureVariationCost > rawAnalysis.CurvatureVariationCost *
+ (1d + options.MaximumVariationCostRegressionRatio))
+ {
+ return Rejected(candidateIndex, PathSmoothingRegionFailureReason.VariationCostRegression, "局部 G2 候选曲率变化代价回退。");
+ }
+
+ if (!_analyzer.TryAnalyze(rawPath.Segments, request.Configuration.OutputSpacingMeters, out PathGeometryAnalysis rawFullAnalysis, out reason) ||
+ !HasFiniteMetrics(rawFullAnalysis))
+ {
+ return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed,
+ string.IsNullOrEmpty(reason) ? "原始完整路径几何分析失败。" : reason);
+ }
+
+ return new LocalG2CandidateEvaluation(
+ true,
+ PathSmoothingRegionFailureReason.None,
+ candidateIndex,
+ spliced,
+ safePath,
+ fullAnalysis.Segments,
+ rawAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
+ candidateAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
+ rawAnalysis.CurvatureVariationCost,
+ candidateAnalysis.CurvatureVariationCost,
+ maximumDeviation,
+ minimumClearance,
+ candidateAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter,
+ Math.Abs(fullAnalysis.PathLengthMeters - rawFullAnalysis.PathLengthMeters),
+ "Accepted");
+ }
+
+ internal static LocalG2CandidateEvaluation SelectBest(IReadOnlyList evaluations)
+ {
+ if (evaluations == null) throw new ArgumentNullException(nameof(evaluations));
+ LocalG2CandidateEvaluation best = null;
+ for (int index = 0; index < evaluations.Count; index++)
+ {
+ LocalG2CandidateEvaluation evaluation = evaluations[index];
+ if (evaluation == null || !evaluation.Accepted) continue;
+ if (best == null || Compare(evaluation, best) < 0) best = evaluation;
+ }
+ return best ?? Rejected(-1, PathSmoothingRegionFailureReason.CandidateGenerationFailed, "没有通过质量门的局部 G2 候选。");
+ }
+
+ private static int Compare(LocalG2CandidateEvaluation left, LocalG2CandidateEvaluation right)
+ {
+ int result = left.MaximumDeviationMeters.CompareTo(right.MaximumDeviationMeters);
+ if (result != 0) return result;
+ result = left.ResultPeakCurvatureDerivativePerSquareMeter.CompareTo(right.ResultPeakCurvatureDerivativePerSquareMeter);
+ if (result != 0) return result;
+ result = left.ResultCurvatureVariationCost.CompareTo(right.ResultCurvatureVariationCost);
+ if (result != 0) return result;
+ result = left.AbsolutePathLengthChangeMeters.CompareTo(right.AbsolutePathLengthChangeMeters);
+ return result != 0 ? result : left.CandidateIndex.CompareTo(right.CandidateIndex);
+ }
+
+ private static bool HasUsableInput(PreparedPath rawPath, PreparedPath currentPath, LocalG2SmoothingRegion region,
+ LocalG2CandidateGeometry candidate, PathSmoothingRequest request, LocalG2OptionsSnapshot options)
+ {
+ return rawPath != null && currentPath != null && region != null && candidate != null && request != null && options != null &&
+ request.Configuration != null && request.Map != null && request.Vehicle != null && candidate.SegmentIndex == region.SegmentIndex &&
+ candidate.SegmentIndex >= 0 && candidate.SegmentIndex < currentPath.Segments.Count &&
+ NumericGuard.IsPositiveFinite(request.Configuration.OutputSpacingMeters) &&
+ NumericGuard.IsPositiveFinite(request.Configuration.MaximumCollisionCheckStepMeters) &&
+ NumericGuard.IsFinite(request.Configuration.MinimumClearanceReserveMeters) &&
+ request.Configuration.MinimumClearanceReserveMeters >= 0d;
+ }
+
+ private static bool TryExtractWindow(PreparedDirectionSegment segment, double startArcLength, double endArcLength,
+ out IReadOnlyList window, out string reason)
+ {
+ window = null;
+ reason = string.Empty;
+ if (segment == null || !PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, startArcLength, out SmoothingPoint2D start, out reason) ||
+ !PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, endArcLength, out SmoothingPoint2D end, out reason)) return false;
+ var points = new List { start };
+ for (int index = 0; index < segment.Points.Count; index++)
+ {
+ SmoothingPoint2D point = segment.Points[index];
+ if (point.ArcLength > startArcLength && point.ArcLength < endArcLength) points.Add(point);
+ }
+ points.Add(end);
+ window = new ReadOnlyCollection(points);
+ return true;
+ }
+
+ private bool TryAnalyzeWindow(PreparedDirectionSegment source, IReadOnlyList points, double spacing,
+ out PathGeometryAnalysis analysis, out string reason)
+ {
+ var segment = new PreparedDirectionSegment(0, source.Direction, points, false, false,
+ source.Points[0].ArcLength == points[0].ArcLength ? source.StartVehicleCurvaturePerMeter : null);
+ return _analyzer.TryAnalyze(new[] { segment }, spacing, out analysis, out reason);
+ }
+
+ private static bool HasFiniteMetrics(PathGeometryAnalysis analysis)
+ {
+ return analysis != null && NumericGuard.IsFinite(analysis.PathLengthMeters) &&
+ NumericGuard.IsFinite(analysis.MaximumAbsoluteVehicleCurvaturePerMeter) &&
+ NumericGuard.IsFinite(analysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter) &&
+ NumericGuard.IsFinite(analysis.CurvatureVariationCost) && analysis.Path != null && analysis.Path.Count >= 2;
+ }
+
+ private static void GetCurvatureRange(IReadOnlyList path, out double minimum, out double maximum)
+ {
+ minimum = double.PositiveInfinity;
+ maximum = double.NegativeInfinity;
+ for (int index = 0; index < path.Count; index++)
+ {
+ minimum = Math.Min(minimum, path[index].VehicleCurvature);
+ maximum = Math.Max(maximum, path[index].VehicleCurvature);
+ }
+ }
+
+ private static bool ExceedsRawCurvatureRange(IReadOnlyList candidate, double minimum, double maximum)
+ {
+ for (int index = 0; index < candidate.Count; index++)
+ {
+ double curvature = candidate[index].VehicleCurvature;
+ if (curvature < minimum - CurvatureRangeTolerance || curvature > maximum + CurvatureRangeTolerance) return true;
+ }
+ return false;
+ }
+
+ private static double MaximumDistanceToPolyline(IReadOnlyList candidate, IReadOnlyList raw)
+ {
+ if (candidate == null || raw == null || candidate.Count == 0 || raw.Count < 2) return double.NaN;
+ double maximum = 0d;
+ for (int index = 0; index < candidate.Count; index++)
+ {
+ double nearest = double.PositiveInfinity;
+ for (int segment = 1; segment < raw.Count; segment++)
+ nearest = Math.Min(nearest, PointToSegmentDistance(candidate[index], raw[segment - 1], raw[segment]));
+ maximum = Math.Max(maximum, nearest);
+ }
+ return maximum;
+ }
+
+ private static double PointToSegmentDistance(SmoothingPoint2D point, SmoothingPoint2D start, SmoothingPoint2D end)
+ {
+ double dx = end.X - start.X;
+ double dy = end.Y - start.Y;
+ double lengthSquared = dx * dx + dy * dy;
+ if (!NumericGuard.IsPositiveFinite(lengthSquared)) return double.NaN;
+ double projection = ((point.X - start.X) * dx + (point.Y - start.Y) * dy) / lengthSquared;
+ projection = Math.Max(0d, Math.Min(1d, projection));
+ double nearestX = start.X + projection * dx;
+ double nearestY = start.Y + projection * dy;
+ double distanceX = point.X - nearestX;
+ double distanceY = point.Y - nearestY;
+ return Math.Sqrt(distanceX * distanceX + distanceY * distanceY);
+ }
+
+ private static LocalG2CandidateEvaluation Rejected(int candidateIndex, PathSmoothingRegionFailureReason failureReason, string reason)
+ {
+ return new LocalG2CandidateEvaluation(false, failureReason, candidateIndex, null, EmptyPath, EmptySegments,
+ 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, reason);
+ }
+
+ /// 反射脚本使用的窄范围确定性质量门覆盖入口。
+ public static class TestHooks
+ {
+ public static EvaluationTestSnapshot Execute(string scenario)
+ {
+ LocalG2CandidateEvaluation evaluation;
+ switch (scenario)
+ {
+ case "TooFar":
+ PreparedPath tooFarPath = CreateWavyPath(1d);
+ evaluation = Evaluate(tooFarPath, CreateCandidate(tooFarPath, 0, 1.12d), 0d);
+ break;
+ case "Overshoot":
+ PreparedPath overshootPath = CreateStraightPath(1d);
+ evaluation = Evaluate(overshootPath, CreateCandidate(overshootPath, 0, 1.15d), 0d);
+ break;
+ case "NoOp":
+ PreparedPath noOpPath = CreateWavyPath(1d);
+ evaluation = Evaluate(noOpPath, CreateCandidateFromPath(noOpPath, 0), 0d);
+ break;
+ case "Oscillating":
+ PreparedPath oscillatingPath = CreateOscillationBaseline(1d);
+ evaluation = Evaluate(oscillatingPath, CreateOscillatingCandidate(oscillatingPath), 0d);
+ break;
+ case "LowClearance":
+ PreparedPath lowClearancePath = CreateWavyPath(0.27d);
+ evaluation = Evaluate(lowClearancePath, CreateCandidateFromPath(lowClearancePath, 0), 0.02d, false, true);
+ break;
+ case "Improved":
+ PreparedPath improvedPath = CreateWavyPath(1d);
+ evaluation = Evaluate(improvedPath, CreateCandidate(improvedPath, 0, 1d), 0d);
+ break;
+ case "SmallestDeviation":
+ PreparedPath smallestDeviationPath = CreateWavyPath(1d);
+ evaluation = Evaluate(smallestDeviationPath, CreateCandidate(smallestDeviationPath, 4, 1d), 100d);
+ break;
+ case "Best": evaluation = SelectBest(CreateWavyPath(1d)); break;
+ default: throw new ArgumentOutOfRangeException(nameof(scenario));
+ }
+ return new EvaluationTestSnapshot(evaluation.Accepted ? "Accepted" : "Rejected", evaluation.FailureReason.ToString(), evaluation.CandidateIndex,
+ evaluation.MinimumBodyClearanceMeters, evaluation.Reason);
+ }
+
+ private static LocalG2CandidateEvaluation Evaluate(PreparedPath path, LocalG2CandidateGeometry candidate, double minimumClearance,
+ bool useNearObstacle = false, bool useEmptyMap = false)
+ {
+ PathSmoothingConfiguration configuration = CreateConfiguration(minimumClearance);
+ PathSmoothingRequest request = new PathSmoothingRequest(null, null,
+ useEmptyMap ? CreateEmptyMap() : CreateMap(useNearObstacle), CreateVehicle(), configuration);
+ return new LocalG2CandidateEvaluator().Evaluate(path, path, CreateRegion(), candidate, request,
+ new LocalG2OptionsSnapshot(configuration), CancellationToken.None);
+ }
+
+ private static LocalG2CandidateEvaluation SelectBest(PreparedPath path)
+ {
+ PathSmoothingConfiguration configuration = CreateConfiguration(0d);
+ PathSmoothingRequest request = new PathSmoothingRequest(null, null, CreateMap(false), CreateVehicle(), configuration);
+ var evaluator = new LocalG2CandidateEvaluator();
+ LocalG2CandidateEvaluation smallestDeviation = evaluator.Evaluate(path, path, CreateRegion(),
+ CreateCandidate(path, 4, 1d), request, new LocalG2OptionsSnapshot(configuration), CancellationToken.None);
+ LocalG2CandidateEvaluation smootherButFarther = evaluator.Evaluate(path, path, CreateRegion(),
+ CreateCandidate(path, 5, 1.01d), request, new LocalG2OptionsSnapshot(configuration), CancellationToken.None);
+ return LocalG2CandidateEvaluator.SelectBest(new[] { smootherButFarther, smallestDeviation });
+ }
+
+ private static PreparedPath CreateWavyPath(double startX)
+ {
+ return CreatePath(new[]
+ {
+ Point(startX, 1d), Point(startX + 0.10d, 1.04d), Point(startX + 0.20d, 1.08d),
+ Point(startX + 0.30d, 1.05d), Point(startX + 0.40d, 0.98d), Point(startX + 0.50d, 0.92d),
+ Point(startX + 0.60d, 0.98d), Point(startX + 0.70d, 1.05d), Point(startX + 0.80d, 1.08d),
+ Point(startX + 0.90d, 1.04d), Point(startX + 1d, 1d),
+ });
+ }
+
+ private static PreparedPath CreateStraightPath(double startX) => CreatePath(new[] { Point(startX, 1d), Point(startX + 0.5d, 1d), Point(startX + 1d, 1d) });
+
+ private static PreparedPath CreateOscillationBaseline(double startX)
+ {
+ return CreatePath(new[]
+ {
+ Point(startX, 1d), Point(startX + 0.05d, 1.022d), Point(startX + 0.10d, 1d),
+ Point(startX + 2.03d, 1d),
+ });
+ }
+
+ private static PreparedPath CreatePath(IReadOnlyList points)
+ {
+ var normalized = new List(points.Count);
+ double arcLength = 0d;
+ for (int index = 0; index < points.Count; index++)
+ {
+ if (index > 0)
+ {
+ double dx = points[index].X - points[index - 1].X;
+ double dy = points[index].Y - points[index - 1].Y;
+ arcLength += Math.Sqrt(dx * dx + dy * dy);
+ }
+ normalized.Add(new SmoothingPoint2D(points[index].X, points[index].Y, arcLength, 0d, 0d, 1d,
+ false, SmoothedPathPointSource.LocalG2Transition));
+ }
+ return new PreparedPath(new[] { new PreparedDirectionSegment(0, TravelDirection.Forward, normalized, false, false) });
+ }
+
+ private static LocalG2CandidateGeometry CreateCandidate(PreparedPath path, int index, double middleY)
+ {
+ IReadOnlyList source = path.Segments[0].Points;
+ SmoothingPoint2D start = source[0];
+ SmoothingPoint2D end = source[source.Count - 1];
+ return new LocalG2CandidateGeometry(index, 0, start.ArcLength, end.ArcLength, 0d, 0d,
+ new[] { Point(start.X, start.Y), Point((start.X + end.X) / 2d, middleY), Point(end.X, end.Y) },
+ 0d, 0d, 0d, 0d, true);
+ }
+
+ private static LocalG2CandidateGeometry CreateCandidateFromPath(PreparedPath path, int index)
+ {
+ IReadOnlyList points = path.Segments[0].Points;
+ return new LocalG2CandidateGeometry(index, 0, points[0].ArcLength, points[points.Count - 1].ArcLength, 0d, 0d, points,
+ 0d, 0d, 0d, 0d, true);
+ }
+
+ private static LocalG2CandidateGeometry CreateOscillatingCandidate(PreparedPath path)
+ {
+ IReadOnlyList source = path.Segments[0].Points;
+ SmoothingPoint2D start = source[0];
+ SmoothingPoint2D end = source[source.Count - 1];
+ var points = new List { Point(start.X, start.Y) };
+ SmoothingPoint2D straightStart = source[source.Count - 2];
+ points.Add(Point(straightStart.X, straightStart.Y));
+ for (int index = 1; index < 20; index++)
+ {
+ double x = straightStart.X + index * (end.X - straightStart.X) / 20d;
+ points.Add(Point(x, straightStart.Y + (index % 2 == 0 ? 0.004d : -0.004d)));
+ }
+ points.Add(Point(end.X, end.Y));
+ return new LocalG2CandidateGeometry(0, 0, start.ArcLength, end.ArcLength, 0d, 0d, points, 0d, 0d, 0d, 0d, true);
+ }
+
+ private static LocalG2SmoothingRegion CreateRegion()
+ {
+ return new LocalG2SmoothingRegion(0,
+ new[] { new CurvatureTransition(0, 1, 2, 0.5d, 0.5d, 0d, 0d, 0d, 0d) },
+ 0d, 1d, new[] { new LocalG2WindowVariant(0, 0d, 1d, 0d, 0d) });
+ }
+
+ private static PathSmoothingConfiguration CreateConfiguration(double minimumClearance)
+ {
+ var configuration = new PathSmoothingConfiguration
+ {
+ OutputSpacingMeters = 0.05d,
+ MaximumCollisionCheckStepMeters = 0.05d,
+ MinimumClearanceReserveMeters = minimumClearance,
+ };
+ configuration.LocalG2Quintic.MaximumDeviationMeters = 0.10d;
+ configuration.LocalG2Quintic.MinimumPeakGradientImprovementRatio = 0.20d;
+ configuration.LocalG2Quintic.MaximumVariationCostRegressionRatio = 0.02d;
+ return configuration;
+ }
+
+ private static VehicleParameters CreateVehicle() => new VehicleParameters
+ {
+ LengthMeters = 0.20d, WidthMeters = 0.20d, SafetyMarginMeters = 0d,
+ MaximumCurvaturePerMeter = 1000000d, MinimumTurningRadiusMeters = 0.000001d,
+ };
+
+ private static PlanningGridMap CreateMap(bool useNearObstacle)
+ {
+ var request = new PlanningMapRequest
+ {
+ Bounds = new MapBoundsMm(0f, 4000f, 0f, 4000f), ResolutionMm = 20f,
+ ObstacleSources = new IMapObstacleSource[]
+ {
+ new ManualObstacleSource("local-g2-evaluator", 1, true, new IMapObstacle[]
+ {
+ useNearObstacle
+ ? new AxisAlignedRectangleObstacle(100f, 150f, 900f, 1100f)
+ : new AxisAlignedRectangleObstacle(3000f, 3100f, 3000f, 3100f),
+ }),
+ },
+ };
+ PlanningMapBuildResult result = new PlanningMapFactory().Create(request);
+ if (!result.Succeeded || result.Map == null) throw new InvalidOperationException("测试地图创建失败。");
+ return result.Map;
+ }
+
+ private static PlanningGridMap CreateEmptyMap()
+ {
+ var request = new PlanningMapRequest
+ {
+ Bounds = new MapBoundsMm(0f, 4000f, 0f, 4000f), ResolutionMm = 20f,
+ ObstacleSources = Array.Empty(), AllowExplicitEmptyMap = true,
+ };
+ PlanningMapBuildResult result = new PlanningMapFactory().Create(request);
+ if (!result.Succeeded || result.Map == null) throw new InvalidOperationException("测试空地图创建失败。");
+ return result.Map;
+ }
+
+ private static SmoothingPoint2D Point(double x, double y) =>
+ new SmoothingPoint2D(x, y, 0d, 0d, 0d, 1d, false, SmoothedPathPointSource.LocalG2Transition);
+
+ public sealed class EvaluationTestSnapshot
+ {
+ internal EvaluationTestSnapshot(string status, string failureReason, int candidateIndex, double minimumClearanceMeters, string reason)
+ {
+ Status = status;
+ FailureReason = failureReason;
+ CandidateIndex = candidateIndex;
+ MinimumClearanceMeters = minimumClearanceMeters;
+ Reason = reason;
+ }
+ public string Status { get; }
+ public string FailureReason { get; }
+ public int CandidateIndex { get; }
+ public double MinimumClearanceMeters { get; }
+ public string Reason { get; }
+ }
+ }
+}
+
+/// 不可变的局部 G2 候选质量与安全评价结果。
+internal sealed class LocalG2CandidateEvaluation
+{
+ internal LocalG2CandidateEvaluation(bool accepted, PathSmoothingRegionFailureReason failureReason, int candidateIndex,
+ PreparedPath splicedPreparedPath, IReadOnlyList safePath, IReadOnlyList safeSegments,
+ double rawPeakCurvatureDerivativePerSquareMeter, double resultPeakCurvatureDerivativePerSquareMeter,
+ double rawCurvatureVariationCost, double resultCurvatureVariationCost, double maximumDeviationMeters,
+ double minimumBodyClearanceMeters, double maximumAbsoluteVehicleCurvaturePerMeter,
+ double absolutePathLengthChangeMeters, string reason)
+ {
+ Accepted = accepted;
+ FailureReason = failureReason;
+ CandidateIndex = candidateIndex;
+ SplicedPreparedPath = splicedPreparedPath;
+ SafePath = Copy(safePath);
+ SafeSegments = Copy(safeSegments);
+ RawPeakCurvatureDerivativePerSquareMeter = rawPeakCurvatureDerivativePerSquareMeter;
+ ResultPeakCurvatureDerivativePerSquareMeter = resultPeakCurvatureDerivativePerSquareMeter;
+ RawCurvatureVariationCost = rawCurvatureVariationCost;
+ ResultCurvatureVariationCost = resultCurvatureVariationCost;
+ MaximumDeviationMeters = maximumDeviationMeters;
+ MinimumBodyClearanceMeters = minimumBodyClearanceMeters;
+ MaximumAbsoluteVehicleCurvaturePerMeter = maximumAbsoluteVehicleCurvaturePerMeter;
+ AbsolutePathLengthChangeMeters = absolutePathLengthChangeMeters;
+ Reason = reason ?? string.Empty;
+ }
+
+ internal bool Accepted { get; }
+ internal PathSmoothingRegionFailureReason FailureReason { get; }
+ internal int CandidateIndex { get; }
+ internal PreparedPath SplicedPreparedPath { get; }
+ internal IReadOnlyList SafePath { get; }
+ internal IReadOnlyList SafeSegments { get; }
+ internal double RawPeakCurvatureDerivativePerSquareMeter { get; }
+ internal double ResultPeakCurvatureDerivativePerSquareMeter { get; }
+ internal double RawCurvatureVariationCost { get; }
+ internal double ResultCurvatureVariationCost { get; }
+ internal double MaximumDeviationMeters { get; }
+ internal double MinimumBodyClearanceMeters { get; }
+ internal double MaximumAbsoluteVehicleCurvaturePerMeter { get; }
+ internal double AbsolutePathLengthChangeMeters { get; }
+ internal string Reason { get; }
+
+ private static IReadOnlyList Copy(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/tests/verify_path_smoothing_local_g2_candidates.ps1 b/ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1
index 01d8edb..336630a 100644
--- a/ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1
+++ b/ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1
@@ -74,4 +74,43 @@ $gearBoundary = Invoke-Scenario 'GearBoundary'
Assert-True $gearBoundary.GearBoundaryMarkerPreserved `
'A window touching a gear-switch segment boundary must preserve its point-level gear marker.'
+$evaluatorType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.LocalG2CandidateEvaluator'
+$evaluatorHooksType = $evaluatorType.GetNestedType('TestHooks', [Reflection.BindingFlags]'Public,NonPublic')
+Assert-True ($null -ne $evaluatorHooksType) 'LocalG2CandidateEvaluator must expose narrowly scoped deterministic TestHooks.'
+$evaluateMethod = $evaluatorHooksType.GetMethod('Execute', [Reflection.BindingFlags]'Public,Static')
+Assert-True ($null -ne $evaluateMethod) 'TestHooks must execute deterministic candidate quality-gate scenarios.'
+
+function Invoke-EvaluationScenario([string]$Scenario) {
+ return $evaluateMethod.Invoke($null, @($Scenario))
+}
+
+$tooFar = Invoke-EvaluationScenario 'TooFar'
+Assert-Equal 'DeviationExceeded' $tooFar.FailureReason `
+ 'A collision-free candidate more than 0.10 m from the raw window must be rejected.'
+
+$overshoot = Invoke-EvaluationScenario 'Overshoot'
+Assert-Equal 'CurvatureOvershoot' $overshoot.FailureReason `
+ 'A candidate outside the raw regional curvature range must be rejected.'
+
+$noOp = Invoke-EvaluationScenario 'NoOp'
+Assert-Equal 'InsufficientImprovement' $noOp.FailureReason `
+ 'A safe no-op must not be accepted.'
+
+$oscillating = Invoke-EvaluationScenario 'Oscillating'
+Assert-Equal 'VariationCostRegression' $oscillating.FailureReason `
+ 'Repeated curvature oscillation must fail the variation-cost gate.'
+
+$lowClearance = Invoke-EvaluationScenario 'LowClearance'
+Assert-Equal 'InsufficientClearance' $lowClearance.FailureReason `
+ 'A path with less than 0.02 m checked clearance must be rejected.'
+
+$improved = Invoke-EvaluationScenario 'Improved'
+Assert-Equal 'Accepted' $improved.Status `
+ 'A safe candidate with at least 20 percent peak improvement must be accepted.'
+
+$smallestDeviation = Invoke-EvaluationScenario 'SmallestDeviation'
+$best = Invoke-EvaluationScenario 'Best'
+Assert-Equal $smallestDeviation.CandidateIndex $best.CandidateIndex `
+ 'Among sufficient candidates, minimum deviation must win before extra smoothness.'
+
Write-Output 'Path smoothing Local G2 candidate checks passed.'