diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs
new file mode 100644
index 0000000..2ab322a
--- /dev/null
+++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs
@@ -0,0 +1,553 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
+
+/// 以固定窗口和尺度顺序构造有限的局部五次 Hermite G2 候选。
+internal sealed class LocalG2CandidateBuilder
+{
+ private const double MinimumDerivativeNorm = 1e-10d;
+ private const int MaximumSubdivisionDepth = 32;
+ private static readonly double[] DerivativeScaleMultipliers = { 1d, 0.85d, 1.15d };
+
+ internal IReadOnlyList Build(
+ PreparedDirectionSegment originalSegment,
+ LocalG2SmoothingRegion region,
+ double outputSpacingMeters,
+ LocalG2OptionsSnapshot options,
+ CancellationToken cancellationToken)
+ {
+ if (!IsValidInput(originalSegment, region, outputSpacingMeters, options)) return Empty();
+
+ var candidates = new List();
+ for (int windowIndex = 0; windowIndex < region.WindowVariants.Count; windowIndex++)
+ {
+ LocalG2WindowVariant window = region.WindowVariants[windowIndex];
+ for (int scaleIndex = 0; scaleIndex < DerivativeScaleMultipliers.Length; scaleIndex++)
+ {
+ cancellationToken.ThrowIfCancellationRequested();
+ if (candidates.Count >= Math.Min(options.MaximumCandidatesPerRegion, 12)) return ReadOnly(candidates);
+ if (TryBuildCandidate(
+ candidates.Count,
+ originalSegment,
+ region,
+ window,
+ DerivativeScaleMultipliers[scaleIndex],
+ outputSpacingMeters,
+ cancellationToken,
+ out LocalG2CandidateGeometry candidate))
+ {
+ candidates.Add(candidate);
+ }
+ }
+ }
+ return ReadOnly(candidates);
+ }
+
+ private static bool TryBuildCandidate(
+ int candidateIndex,
+ PreparedDirectionSegment segment,
+ LocalG2SmoothingRegion region,
+ LocalG2WindowVariant window,
+ double multiplier,
+ double outputSpacingMeters,
+ CancellationToken cancellationToken,
+ out LocalG2CandidateGeometry candidate)
+ {
+ candidate = null;
+ if (!PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, window.StartArcLengthMeters, out SmoothingPoint2D startReference, out _) ||
+ !PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, window.EndArcLengthMeters, out SmoothingPoint2D endReference, out _))
+ {
+ return false;
+ }
+
+ double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d;
+ CurvatureTransition first = region.Transitions[0];
+ CurvatureTransition last = region.Transitions[region.Transitions.Count - 1];
+ bool firstAtStart = SameArc(first.LocalArcLengthMeters, window.StartArcLengthMeters);
+ bool lastAtEnd = SameArc(last.LocalArcLengthMeters, window.EndArcLengthMeters);
+ double startVehicleCurvature = firstAtStart && segment.StartVehicleCurvaturePerMeter.HasValue
+ ? segment.StartVehicleCurvaturePerMeter.Value
+ : first.LeftVehicleCurvaturePerMeter;
+ double endVehicleCurvature = last.RightVehicleCurvaturePerMeter;
+ if (!NumericGuard.IsFinite(startVehicleCurvature) || !NumericGuard.IsFinite(endVehicleCurvature)) return false;
+
+ var nodes = new List();
+ nodes.Add(new BoundaryNode(window.StartArcLengthMeters, startReference.X, startReference.Y,
+ startReference.Heading, startVehicleCurvature));
+ for (int transitionIndex = 0; transitionIndex < region.Transitions.Count; transitionIndex++)
+ {
+ CurvatureTransition transition = region.Transitions[transitionIndex];
+ bool atStart = SameArc(transition.LocalArcLengthMeters, window.StartArcLengthMeters);
+ bool atEnd = SameArc(transition.LocalArcLengthMeters, window.EndArcLengthMeters);
+ // A one-sided transition at a hard segment boundary has no left/right zero-length piece.
+ if (atStart || atEnd) continue;
+ double leftAvailable = transition.LocalArcLengthMeters - window.StartArcLengthMeters;
+ double rightAvailable = window.EndArcLengthMeters - transition.LocalArcLengthMeters;
+ double denominator = leftAvailable + rightAvailable;
+ if (!NumericGuard.IsPositiveFinite(denominator)) return false;
+ double sharedCurvature = transition.LeftVehicleCurvaturePerMeter +
+ (transition.RightVehicleCurvaturePerMeter - transition.LeftVehicleCurvaturePerMeter) * leftAvailable / denominator;
+ if (!NumericGuard.IsFinite(sharedCurvature)) return false;
+ nodes.Add(new BoundaryNode(transition.LocalArcLengthMeters, transition.X, transition.Y,
+ transition.VehicleHeadingRadians, sharedCurvature));
+ }
+ nodes.Add(new BoundaryNode(window.EndArcLengthMeters, endReference.X, endReference.Y,
+ endReference.Heading, endVehicleCurvature));
+
+ if (!TryAssignDerivativeScales(nodes, multiplier) || nodes.Count < 2) return false;
+ var sampled = new List();
+ for (int nodeIndex = 1; nodeIndex < nodes.Count; nodeIndex++)
+ {
+ cancellationToken.ThrowIfCancellationRequested();
+ BoundaryNode left = nodes[nodeIndex - 1];
+ BoundaryNode right = nodes[nodeIndex];
+ if (!TryCreateCurve(left, right, directionSign, out QuinticHermiteCurve2D curve)) return false;
+ if (!TryAppendCurveSamples(
+ curve,
+ left,
+ right,
+ segment,
+ outputSpacingMeters,
+ sampled,
+ cancellationToken))
+ {
+ return false;
+ }
+ }
+
+ if (sampled.Count < 2) return false;
+ candidate = new LocalG2CandidateGeometry(
+ candidateIndex,
+ segment.SegmentIndex,
+ window.StartArcLengthMeters,
+ window.EndArcLengthMeters,
+ window.LeftWindowLengthMeters,
+ window.RightWindowLengthMeters,
+ sampled,
+ startVehicleCurvature,
+ endVehicleCurvature,
+ directionSign * startVehicleCurvature,
+ directionSign * endVehicleCurvature,
+ true);
+ return true;
+ }
+
+ private static bool TryAssignDerivativeScales(List nodes, double multiplier)
+ {
+ if (!NumericGuard.IsPositiveFinite(multiplier)) return false;
+ for (int index = 0; index < nodes.Count; index++)
+ {
+ double scale;
+ if (index == 0) scale = nodes[1].ArcLengthMeters - nodes[0].ArcLengthMeters;
+ else if (index == nodes.Count - 1) scale = nodes[index].ArcLengthMeters - nodes[index - 1].ArcLengthMeters;
+ else scale = ((nodes[index].ArcLengthMeters - nodes[index - 1].ArcLengthMeters) +
+ (nodes[index + 1].ArcLengthMeters - nodes[index].ArcLengthMeters)) / 2d;
+ scale *= multiplier;
+ if (!NumericGuard.IsPositiveFinite(scale)) return false;
+ nodes[index] = nodes[index].WithDerivativeScale(scale);
+ }
+ return true;
+ }
+
+ private static bool TryCreateCurve(BoundaryNode left, BoundaryNode right, double directionSign, out QuinticHermiteCurve2D curve)
+ {
+ curve = null;
+ if (!TryGetDerivatives(left, directionSign, out double ldx, out double ldy, out double lddx, out double lddy) ||
+ !TryGetDerivatives(right, directionSign, out double rdx, out double rdy, out double rddx, out double rddy))
+ {
+ return false;
+ }
+ return QuinticHermiteCurve2D.TryCreate(left.X, left.Y, ldx, ldy, lddx, lddy,
+ right.X, right.Y, rdx, rdy, rddx, rddy, out curve, out _);
+ }
+
+ private static bool TryGetDerivatives(BoundaryNode node, double directionSign,
+ out double dx, out double dy, out double ddx, out double ddy)
+ {
+ dx = dy = ddx = ddy = 0d;
+ double travelHeading = directionSign > 0d ? node.VehicleHeadingRadians : node.VehicleHeadingRadians - Math.PI;
+ double geometricCurvature = directionSign * node.VehicleCurvaturePerMeter;
+ double tx = Math.Cos(travelHeading);
+ double ty = Math.Sin(travelHeading);
+ double nx = -ty;
+ double ny = tx;
+ dx = node.DerivativeScale * tx;
+ dy = node.DerivativeScale * ty;
+ ddx = node.DerivativeScale * node.DerivativeScale * geometricCurvature * nx;
+ ddy = node.DerivativeScale * node.DerivativeScale * geometricCurvature * ny;
+ return NumericGuard.IsFinite(dx) && NumericGuard.IsFinite(dy) && NumericGuard.IsFinite(ddx) && NumericGuard.IsFinite(ddy) &&
+ Math.Sqrt(dx * dx + dy * dy) >= MinimumDerivativeNorm;
+ }
+
+ private static bool TryAppendCurveSamples(
+ QuinticHermiteCurve2D curve,
+ BoundaryNode left,
+ BoundaryNode right,
+ PreparedDirectionSegment segment,
+ double outputSpacingMeters,
+ List output,
+ CancellationToken cancellationToken)
+ {
+ if (!TryEvaluate(curve, 0d, out CurveSample start) || !TryEvaluate(curve, 1d, out CurveSample end)) return false;
+ if (output.Count == 0 && !TryAppendPoint(start, left, right, segment, output)) return false;
+ return TrySubdivide(curve, left, right, segment, outputSpacingMeters, 0d, start, 1d, end, 0, output, cancellationToken);
+ }
+
+ private static bool TrySubdivide(
+ QuinticHermiteCurve2D curve,
+ BoundaryNode left,
+ BoundaryNode right,
+ PreparedDirectionSegment segment,
+ double outputSpacingMeters,
+ double u0,
+ CurveSample sample0,
+ double u1,
+ CurveSample sample1,
+ int depth,
+ List output,
+ CancellationToken cancellationToken)
+ {
+ cancellationToken.ThrowIfCancellationRequested();
+ if (Distance(sample0, sample1) <= outputSpacingMeters)
+ {
+ return TryAppendPoint(sample1, left, right, segment, output);
+ }
+ if (depth >= MaximumSubdivisionDepth) return false;
+ double middle = (u0 + u1) / 2d;
+ if (!TryEvaluate(curve, middle, out CurveSample middleSample)) return false;
+ return TrySubdivide(curve, left, right, segment, outputSpacingMeters, u0, sample0, middle, middleSample,
+ depth + 1, output, cancellationToken) &&
+ TrySubdivide(curve, left, right, segment, outputSpacingMeters, middle, middleSample, u1, sample1,
+ depth + 1, output, cancellationToken);
+ }
+
+ private static bool TryAppendPoint(
+ CurveSample sample,
+ BoundaryNode left,
+ BoundaryNode right,
+ PreparedDirectionSegment segment,
+ List output)
+ {
+ double referenceArcLength = left.ArcLengthMeters + (right.ArcLengthMeters - left.ArcLengthMeters) * sample.Parameter;
+ if (!PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, referenceArcLength, out SmoothingPoint2D reference, out _))
+ return false;
+ double x = SameArc(sample.Parameter, 0d) ? left.X : SameArc(sample.Parameter, 1d) ? right.X : sample.X;
+ double y = SameArc(sample.Parameter, 0d) ? left.Y : SameArc(sample.Parameter, 1d) ? right.Y : sample.Y;
+ double travelHeading = Math.Atan2(sample.Dy, sample.Dx);
+ double vehicleHeading = segment.Direction == TravelDirection.Forward ? travelHeading : travelHeading + Math.PI;
+ if (!NumericGuard.IsFinite(x) || !NumericGuard.IsFinite(y) || !NumericGuard.IsFinite(travelHeading) || !NumericGuard.IsFinite(vehicleHeading)) return false;
+ if (output.Count > 0 && Distance(output[output.Count - 1], x, y) <= 1e-12d) return false;
+ output.Add(new SmoothingPoint2D(
+ x,
+ y,
+ referenceArcLength,
+ AngleMath.NormalizeRadians(vehicleHeading),
+ vehicleHeading,
+ reference.BodyClearance,
+ false,
+ SmoothedPathPointSource.LocalG2Transition));
+ return true;
+ }
+
+ private static bool TryEvaluate(QuinticHermiteCurve2D curve, double parameter, out CurveSample sample)
+ {
+ sample = default;
+ curve.Evaluate(parameter, out double x, out double y, out double dx, out double dy, out double ddx, out double ddy);
+ double derivativeNorm = Math.Sqrt(dx * dx + dy * dy);
+ if (!NumericGuard.IsFinite(x) || !NumericGuard.IsFinite(y) || !NumericGuard.IsFinite(dx) || !NumericGuard.IsFinite(dy) ||
+ !NumericGuard.IsFinite(ddx) || !NumericGuard.IsFinite(ddy) || !NumericGuard.IsFinite(derivativeNorm) ||
+ derivativeNorm < MinimumDerivativeNorm)
+ {
+ return false;
+ }
+ sample = new CurveSample(parameter, x, y, dx, dy);
+ return true;
+ }
+
+ private static bool IsValidInput(PreparedDirectionSegment segment, LocalG2SmoothingRegion region,
+ double outputSpacingMeters, LocalG2OptionsSnapshot options)
+ {
+ if (segment == null || region == null || options == null || segment.SegmentIndex != region.SegmentIndex ||
+ !NumericGuard.IsPositiveFinite(outputSpacingMeters) || segment.Points == null || segment.Points.Count < 2 ||
+ region.Transitions == null || region.Transitions.Count == 0 || region.WindowVariants == null)
+ {
+ return false;
+ }
+ for (int index = 0; index < region.Transitions.Count; index++)
+ {
+ CurvatureTransition transition = region.Transitions[index];
+ if (transition == null || transition.SegmentIndex != segment.SegmentIndex) return false;
+ }
+ return true;
+ }
+
+ private static double Distance(CurveSample left, CurveSample right) => Distance(left.X, left.Y, right.X, right.Y);
+ private static double Distance(SmoothingPoint2D point, double x, double y) => Distance(point.X, point.Y, x, y);
+ private static double Distance(double x0, double y0, double x1, double y1)
+ {
+ double dx = x1 - x0;
+ double dy = y1 - y0;
+ return Math.Sqrt(dx * dx + dy * dy);
+ }
+ private static bool SameArc(double left, double right) => Math.Abs(left - right) <= 1e-10d;
+ private static IReadOnlyList Empty() => new ReadOnlyCollection(new List());
+ private static IReadOnlyList ReadOnly(List values) =>
+ new ReadOnlyCollection(values);
+
+ private readonly struct BoundaryNode
+ {
+ internal BoundaryNode(double arcLengthMeters, double x, double y, double vehicleHeadingRadians, double vehicleCurvaturePerMeter)
+ {
+ ArcLengthMeters = arcLengthMeters;
+ X = x;
+ Y = y;
+ VehicleHeadingRadians = vehicleHeadingRadians;
+ VehicleCurvaturePerMeter = vehicleCurvaturePerMeter;
+ DerivativeScale = 0d;
+ }
+ private BoundaryNode(double arcLengthMeters, double x, double y, double vehicleHeadingRadians,
+ double vehicleCurvaturePerMeter, double derivativeScale)
+ {
+ ArcLengthMeters = arcLengthMeters;
+ X = x;
+ Y = y;
+ VehicleHeadingRadians = vehicleHeadingRadians;
+ VehicleCurvaturePerMeter = vehicleCurvaturePerMeter;
+ DerivativeScale = derivativeScale;
+ }
+ internal double ArcLengthMeters { get; }
+ internal double X { get; }
+ internal double Y { get; }
+ internal double VehicleHeadingRadians { get; }
+ internal double VehicleCurvaturePerMeter { get; }
+ internal double DerivativeScale { get; }
+ internal BoundaryNode WithDerivativeScale(double derivativeScale) => new BoundaryNode(
+ ArcLengthMeters, X, Y, VehicleHeadingRadians, VehicleCurvaturePerMeter, derivativeScale);
+ }
+
+ private readonly struct CurveSample
+ {
+ internal CurveSample(double parameter, double x, double y, double dx, double dy)
+ {
+ Parameter = parameter;
+ X = x;
+ Y = y;
+ Dx = dx;
+ Dy = dy;
+ }
+ internal double Parameter { get; }
+ internal double X { get; }
+ internal double Y { get; }
+ internal double Dx { get; }
+ internal double Dy { get; }
+ }
+
+ /// 反射脚本使用的窄范围候选与拼接场景入口。
+ public static class TestHooks
+ {
+ public static CandidateTestSnapshot Execute(string scenario)
+ {
+ if (string.IsNullOrWhiteSpace(scenario)) throw new ArgumentException("A scenario is required.", nameof(scenario));
+ switch (scenario)
+ {
+ case "Isolated": return BuildIsolated();
+ case "Cluster": return BuildCluster();
+ case "Reverse": return BuildReverse();
+ case "Spliced": return BuildSpliced();
+ case "StartBoundary": return BuildStartBoundary();
+ default: throw new ArgumentOutOfRangeException(nameof(scenario));
+ }
+ }
+
+ public sealed class CandidateTestSnapshot
+ {
+ internal CandidateTestSnapshot(int candidateCount, double startPositionError, double endPositionError,
+ double startCurvatureError, double endCurvatureError, bool containsLocalG2Source,
+ int outputRegionCount, bool internalConnectionsAreG2, string direction,
+ bool vehicleAndGeometricCurvatureSignsAreOpposite, bool noDuplicateNonGearPoints,
+ bool endpointsUnchanged)
+ {
+ CandidateCount = candidateCount;
+ StartPositionError = startPositionError;
+ EndPositionError = endPositionError;
+ StartCurvatureError = startCurvatureError;
+ EndCurvatureError = endCurvatureError;
+ ContainsLocalG2Source = containsLocalG2Source;
+ OutputRegionCount = outputRegionCount;
+ InternalConnectionsAreG2 = internalConnectionsAreG2;
+ Direction = direction;
+ VehicleAndGeometricCurvatureSignsAreOpposite = vehicleAndGeometricCurvatureSignsAreOpposite;
+ NoDuplicateNonGearPoints = noDuplicateNonGearPoints;
+ EndpointsUnchanged = endpointsUnchanged;
+ }
+ public int CandidateCount { get; }
+ public double StartPositionError { get; }
+ public double EndPositionError { get; }
+ public double StartCurvatureError { get; }
+ public double EndCurvatureError { get; }
+ public bool ContainsLocalG2Source { get; }
+ public int OutputRegionCount { get; }
+ public bool InternalConnectionsAreG2 { get; }
+ public string Direction { get; }
+ public bool VehicleAndGeometricCurvatureSignsAreOpposite { get; }
+ public bool NoDuplicateNonGearPoints { get; }
+ public bool EndpointsUnchanged { get; }
+ }
+
+ private static CandidateTestSnapshot BuildIsolated()
+ {
+ PreparedDirectionSegment segment = CreateSegment(TravelDirection.Forward, null);
+ LocalG2SmoothingRegion region = CreateRegion(new[]
+ {
+ new CurvatureTransition(0, 1, 2, 0.5d, 0.5d, 0d, 0d, 0d, 0.4d),
+ }, 0.25d, 0.75d);
+ IReadOnlyList candidates = new LocalG2CandidateBuilder().Build(
+ segment, region, 0.025d, CreateOptions(), CancellationToken.None);
+ if (candidates.Count == 0) throw new InvalidOperationException("The isolated candidate scenario produced no candidate.");
+ LocalG2CandidateGeometry candidate = candidates[0];
+ PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, 0.25d, out SmoothingPoint2D start, out _);
+ PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, 0.75d, out SmoothingPoint2D end, out _);
+ return new CandidateTestSnapshot(candidates.Count,
+ Distance(start, candidate.RegionPoints[0]), Distance(end, candidate.RegionPoints[candidate.RegionPoints.Count - 1]),
+ Math.Abs(candidate.StartVehicleCurvaturePerMeter - 0d),
+ Math.Abs(candidate.EndVehicleCurvaturePerMeter - 0.4d),
+ ContainsLocalG2(candidate.RegionPoints), 0, false, string.Empty, false, false, false);
+ }
+
+ private static CandidateTestSnapshot BuildCluster()
+ {
+ PreparedDirectionSegment segment = CreateSegment(TravelDirection.Forward, null);
+ var transitions = new[]
+ {
+ new CurvatureTransition(0, 1, 2, 0.4d, 0.4d, 0d, 0d, 0d, 0.4d),
+ new CurvatureTransition(0, 2, 3, 0.6d, 0.6d, 0d, 0d, 0.4d, 0d),
+ };
+ var planner = new LocalG2WindowPlanner();
+ if (!planner.TryPlan(new PreparedPath(new[] { segment }), transitions, CreateOptions(),
+ out IReadOnlyList regions, out string reason))
+ {
+ throw new InvalidOperationException(reason);
+ }
+ IReadOnlyList candidates = new LocalG2CandidateBuilder().Build(
+ segment, regions[0], 0.025d, CreateOptions(), CancellationToken.None);
+ if (candidates.Count == 0) throw new InvalidOperationException("The clustered candidate scenario produced no candidate.");
+ return new CandidateTestSnapshot(0, 0d, 0d, 0d, 0d, false, regions.Count,
+ candidates[0].InternalConnectionsAreG2, string.Empty, false, false, false);
+ }
+
+ private static CandidateTestSnapshot BuildReverse()
+ {
+ PreparedDirectionSegment segment = CreateSegment(TravelDirection.Reverse, null);
+ LocalG2SmoothingRegion region = CreateRegion(new[]
+ {
+ new CurvatureTransition(0, 1, 2, 0.5d, 0.5d, 0d, Math.PI, 0.2d, 0.4d),
+ }, 0.25d, 0.75d);
+ IReadOnlyList candidates = new LocalG2CandidateBuilder().Build(
+ segment, region, 0.025d, CreateOptions(), CancellationToken.None);
+ if (candidates.Count == 0) throw new InvalidOperationException("The reverse candidate scenario produced no candidate.");
+ LocalG2CandidateGeometry candidate = candidates[0];
+ bool opposite = candidate.StartVehicleCurvaturePerMeter * candidate.StartGeometricCurvaturePerMeter < 0d &&
+ candidate.EndVehicleCurvaturePerMeter * candidate.EndGeometricCurvaturePerMeter < 0d;
+ return new CandidateTestSnapshot(0, 0d, 0d, 0d, 0d, false, 0, false,
+ segment.Direction.ToString(), opposite, false, false);
+ }
+
+ private static CandidateTestSnapshot BuildSpliced()
+ {
+ PreparedDirectionSegment segment = CreateSegment(TravelDirection.Forward, 0.17d);
+ LocalG2SmoothingRegion region = CreateRegion(new[]
+ {
+ new CurvatureTransition(0, 1, 2, 0.5d, 0.5d, 0d, 0d, 0.17d, 0.4d),
+ }, 0.25d, 0.75d);
+ IReadOnlyList candidates = new LocalG2CandidateBuilder().Build(
+ segment, region, 0.025d, CreateOptions(), CancellationToken.None);
+ if (candidates.Count == 0) throw new InvalidOperationException("The splice candidate scenario produced no candidate.");
+ PreparedDirectionSegment untouched = CreateSegment(TravelDirection.Reverse, null);
+ var path = new PreparedPath(new[] { segment, untouched });
+ if (!new LocalG2PathSplicer().TryReplace(path, candidates[0], out PreparedPath replaced, out string reason))
+ throw new InvalidOperationException(reason);
+ PreparedDirectionSegment result = replaced.Segments[0];
+ bool noDuplicates = true;
+ for (int index = 1; index < result.Points.Count; index++)
+ {
+ if (!result.Points[index - 1].IsGearSwitchPoint && !result.Points[index].IsGearSwitchPoint &&
+ Distance(result.Points[index - 1], result.Points[index]) <= 1e-12d)
+ {
+ noDuplicates = false;
+ break;
+ }
+ }
+ bool monotonicallyIncreasing = true;
+ for (int index = 1; index < result.Points.Count; index++)
+ {
+ if (result.Points[index].ArcLength <= result.Points[index - 1].ArcLength)
+ {
+ monotonicallyIncreasing = false;
+ break;
+ }
+ }
+ bool endpoints = Distance(segment.Points[0], result.Points[0]) <= 1e-12d &&
+ Distance(segment.Points[segment.Points.Count - 1], result.Points[result.Points.Count - 1]) <= 1e-12d &&
+ result.StartVehicleCurvaturePerMeter == segment.StartVehicleCurvaturePerMeter && monotonicallyIncreasing &&
+ ReferenceEquals(untouched, replaced.Segments[1]);
+ return new CandidateTestSnapshot(0, 0d, 0d, 0d, 0d, false, 0, false, string.Empty,
+ false, noDuplicates, endpoints);
+ }
+
+ private static CandidateTestSnapshot BuildStartBoundary()
+ {
+ PreparedDirectionSegment segment = CreateSegment(TravelDirection.Forward, 0.17d);
+ LocalG2SmoothingRegion region = CreateRegion(new[]
+ {
+ new CurvatureTransition(0, 0, 1, 0d, 0d, 0d, 0d, 0.17d, 0.4d),
+ }, 0d, 0.5d);
+ IReadOnlyList candidates = new LocalG2CandidateBuilder().Build(
+ segment, region, 0.025d, CreateOptions(), CancellationToken.None);
+ if (candidates.Count == 0) throw new InvalidOperationException("The start-boundary transition must not create a zero-length left curve.");
+ LocalG2CandidateGeometry candidate = candidates[0];
+ return new CandidateTestSnapshot(candidates.Count,
+ Math.Abs(candidate.StartVehicleCurvaturePerMeter - 0.17d), 0d,
+ Math.Abs(candidate.StartVehicleCurvaturePerMeter - 0.17d), 0d,
+ ContainsLocalG2(candidate.RegionPoints), 0, false, string.Empty, false, false, false);
+ }
+
+ private static PreparedDirectionSegment CreateSegment(TravelDirection direction, double? startCurvature)
+ {
+ double heading = direction == TravelDirection.Forward ? 0d : Math.PI;
+ var points = new List();
+ for (int index = 0; index <= 4; index++)
+ {
+ double arc = index * 0.25d;
+ points.Add(new SmoothingPoint2D(arc, 0d, arc, heading, heading, 1d, false, SmoothedPathPointSource.Anchor));
+ }
+ return new PreparedDirectionSegment(0, direction, points, false, false, startCurvature);
+ }
+
+ private static LocalG2SmoothingRegion CreateRegion(
+ IReadOnlyList transitions,
+ double startArcLengthMeters,
+ double endArcLengthMeters)
+ {
+ return new LocalG2SmoothingRegion(0, transitions, startArcLengthMeters, endArcLengthMeters,
+ new[] { new LocalG2WindowVariant(0, startArcLengthMeters, endArcLengthMeters,
+ transitions[0].LocalArcLengthMeters - startArcLengthMeters,
+ endArcLengthMeters - transitions[transitions.Count - 1].LocalArcLengthMeters) });
+ }
+
+ private static LocalG2OptionsSnapshot CreateOptions() => new LocalG2OptionsSnapshot(new PathSmoothingConfiguration());
+ private static bool ContainsLocalG2(IReadOnlyList points)
+ {
+ for (int index = 0; index < points.Count; index++)
+ if (points[index].Source == SmoothedPathPointSource.LocalG2Transition) return true;
+ return false;
+ }
+ private static double Distance(SmoothingPoint2D left, SmoothingPoint2D right) =>
+ Math.Sqrt((right.X - left.X) * (right.X - left.X) + (right.Y - left.Y) * (right.Y - left.Y));
+ }
+}
diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateGeometry.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateGeometry.cs
new file mode 100644
index 0000000..22161f7
--- /dev/null
+++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateGeometry.cs
@@ -0,0 +1,76 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
+
+/// 一个尚未经过安全和质量评价的局部 G2 替换几何。
+internal sealed class LocalG2CandidateGeometry
+{
+ internal LocalG2CandidateGeometry(
+ int candidateIndex,
+ int segmentIndex,
+ double startArcLengthMeters,
+ double endArcLengthMeters,
+ double leftWindowLengthMeters,
+ double rightWindowLengthMeters,
+ IReadOnlyList regionPoints,
+ double startVehicleCurvaturePerMeter,
+ double endVehicleCurvaturePerMeter,
+ double startGeometricCurvaturePerMeter,
+ double endGeometricCurvaturePerMeter,
+ bool internalConnectionsAreG2)
+ {
+ if (candidateIndex < 0 || segmentIndex < 0 || !NumericGuard.IsFinite(startArcLengthMeters) ||
+ !NumericGuard.IsFinite(endArcLengthMeters) || startArcLengthMeters < 0d ||
+ endArcLengthMeters < startArcLengthMeters || !NumericGuard.IsFinite(leftWindowLengthMeters) ||
+ !NumericGuard.IsFinite(rightWindowLengthMeters) || leftWindowLengthMeters < 0d ||
+ rightWindowLengthMeters < 0d || regionPoints == null || regionPoints.Count < 2 ||
+ !NumericGuard.IsFinite(startVehicleCurvaturePerMeter) || !NumericGuard.IsFinite(endVehicleCurvaturePerMeter) ||
+ !NumericGuard.IsFinite(startGeometricCurvaturePerMeter) || !NumericGuard.IsFinite(endGeometricCurvaturePerMeter))
+ {
+ throw new ArgumentOutOfRangeException(nameof(regionPoints));
+ }
+
+ CandidateIndex = candidateIndex;
+ SegmentIndex = segmentIndex;
+ StartArcLengthMeters = startArcLengthMeters;
+ EndArcLengthMeters = endArcLengthMeters;
+ LeftWindowLengthMeters = leftWindowLengthMeters;
+ RightWindowLengthMeters = rightWindowLengthMeters;
+ RegionPoints = Copy(regionPoints);
+ StartVehicleCurvaturePerMeter = startVehicleCurvaturePerMeter;
+ EndVehicleCurvaturePerMeter = endVehicleCurvaturePerMeter;
+ StartGeometricCurvaturePerMeter = startGeometricCurvaturePerMeter;
+ EndGeometricCurvaturePerMeter = endGeometricCurvaturePerMeter;
+ InternalConnectionsAreG2 = internalConnectionsAreG2;
+ }
+
+ internal int CandidateIndex { get; }
+ internal int SegmentIndex { get; }
+ internal double StartArcLengthMeters { get; }
+ internal double EndArcLengthMeters { get; }
+ internal double LeftWindowLengthMeters { get; }
+ internal double RightWindowLengthMeters { get; }
+ internal IReadOnlyList RegionPoints { get; }
+
+ // 这些边界状态供候选评价和窄范围反射验证使用，不能替代统一几何分析器的最终统计。
+ internal double StartVehicleCurvaturePerMeter { get; }
+ internal double EndVehicleCurvaturePerMeter { get; }
+ internal double StartGeometricCurvaturePerMeter { get; }
+ internal double EndGeometricCurvaturePerMeter { get; }
+ internal bool InternalConnectionsAreG2 { get; }
+
+ private static IReadOnlyList Copy(IReadOnlyList source)
+ {
+ var copy = new List(source.Count);
+ for (int index = 0; index < source.Count; index++)
+ {
+ if (source[index] == null) throw new ArgumentOutOfRangeException(nameof(source));
+ copy.Add(source[index]);
+ }
+ return new ReadOnlyCollection(copy);
+ }
+}
diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2PathSplicer.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2PathSplicer.cs
new file mode 100644
index 0000000..55a4b02
--- /dev/null
+++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2PathSplicer.cs
@@ -0,0 +1,129 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
+
+/// 只替换一个方向段内局部窗口，并保持其余方向拓扑不变。
+internal sealed class LocalG2PathSplicer
+{
+ internal bool TryReplace(
+ PreparedPath currentPath,
+ LocalG2CandidateGeometry candidate,
+ out PreparedPath replacedPath,
+ out string reason)
+ {
+ replacedPath = null;
+ reason = string.Empty;
+ if (currentPath == null || candidate == null || candidate.SegmentIndex < 0 ||
+ candidate.SegmentIndex >= currentPath.Segments.Count || candidate.RegionPoints == null ||
+ candidate.RegionPoints.Count < 2)
+ {
+ reason = "局部 G2 拼接输入无效。";
+ return false;
+ }
+
+ PreparedDirectionSegment source = currentPath.Segments[candidate.SegmentIndex];
+ if (source == null || source.SegmentIndex != candidate.SegmentIndex ||
+ !PathReferenceInterpolator.TryInterpolateByArcLength(source.Points, candidate.StartArcLengthMeters, out SmoothingPoint2D start, out reason) ||
+ !PathReferenceInterpolator.TryInterpolateByArcLength(source.Points, candidate.EndArcLengthMeters, out SmoothingPoint2D end, out reason) ||
+ !SamePosition(candidate.RegionPoints[0], start) || !SamePosition(candidate.RegionPoints[candidate.RegionPoints.Count - 1], end))
+ {
+ if (string.IsNullOrEmpty(reason)) reason = "局部 G2 候选端点不匹配原始窗口。";
+ return false;
+ }
+
+ var combined = new List();
+ for (int index = 0; index < source.Points.Count; index++)
+ {
+ SmoothingPoint2D point = source.Points[index];
+ if (point.ArcLength < candidate.StartArcLengthMeters) AddWithoutNonGearDuplicates(combined, point);
+ }
+ for (int index = 0; index < candidate.RegionPoints.Count; index++) AddWithoutNonGearDuplicates(combined, candidate.RegionPoints[index]);
+ for (int index = 0; index < source.Points.Count; index++)
+ {
+ SmoothingPoint2D point = source.Points[index];
+ if (point.ArcLength > candidate.EndArcLengthMeters) AddWithoutNonGearDuplicates(combined, point);
+ }
+ if (!TryRecalculateArcLengths(combined, out IReadOnlyList localPoints, out reason)) return false;
+
+ var segments = new List(currentPath.Segments.Count);
+ for (int index = 0; index < currentPath.Segments.Count; index++)
+ {
+ PreparedDirectionSegment segment = currentPath.Segments[index];
+ if (index != candidate.SegmentIndex)
+ {
+ segments.Add(segment);
+ continue;
+ }
+ segments.Add(new PreparedDirectionSegment(segment.SegmentIndex, segment.Direction, localPoints,
+ segment.StartsAtGearSwitch, segment.EndsAtGearSwitch, segment.StartVehicleCurvaturePerMeter));
+ }
+ replacedPath = new PreparedPath(segments);
+ return true;
+ }
+
+ private static bool TryRecalculateArcLengths(
+ IReadOnlyList points,
+ out IReadOnlyList recalculated,
+ out string reason)
+ {
+ recalculated = null;
+ reason = string.Empty;
+ if (points == null || points.Count < 2)
+ {
+ reason = "局部 G2 拼接后方向段没有足够点。";
+ return false;
+ }
+ var output = new List(points.Count);
+ double arcLength = 0d;
+ for (int index = 0; index < points.Count; index++)
+ {
+ SmoothingPoint2D point = points[index];
+ if (!IsValid(point))
+ {
+ reason = "局部 G2 拼接点包含非法数值。";
+ return false;
+ }
+ if (index > 0)
+ {
+ double distance = Distance(points[index - 1], point);
+ if (!NumericGuard.IsPositiveFinite(distance))
+ {
+ reason = "局部 G2 拼接后存在重复非换向点。";
+ return false;
+ }
+ arcLength += distance;
+ }
+ output.Add(new SmoothingPoint2D(point.X, point.Y, arcLength, point.Heading, point.UnwrappedHeading,
+ point.BodyClearance, point.IsGearSwitchPoint, point.Source));
+ }
+ recalculated = output;
+ return true;
+ }
+
+ private static void AddWithoutNonGearDuplicates(List output, SmoothingPoint2D point)
+ {
+ if (output.Count == 0)
+ {
+ output.Add(point);
+ return;
+ }
+ SmoothingPoint2D previous = output[output.Count - 1];
+ if (!previous.IsGearSwitchPoint && !point.IsGearSwitchPoint && SamePosition(previous, point)) return;
+ output.Add(point);
+ }
+
+ private static bool IsValid(SmoothingPoint2D point) => point != null && NumericGuard.IsFinite(point.X) &&
+ NumericGuard.IsFinite(point.Y) && NumericGuard.IsFinite(point.Heading) && NumericGuard.IsFinite(point.UnwrappedHeading) &&
+ NumericGuard.IsFinite(point.BodyClearance) && point.BodyClearance >= 0d;
+ private static bool SamePosition(SmoothingPoint2D left, SmoothingPoint2D right) =>
+ left != null && right != null && Distance(left, right) <= 1e-9d;
+ private static double Distance(SmoothingPoint2D left, SmoothingPoint2D right)
+ {
+ double dx = right.X - left.X;
+ double dy = right.Y - left.Y;
+ return Math.Sqrt(dx * dx + dy * dy);
+ }
+}
diff --git a/ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 b/ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1
new file mode 100644
index 0000000..12596d3
--- /dev/null
+++ b/ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1
@@ -0,0 +1,61 @@
+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 Get-RequiredType([string]$Name) {
+ return $assembly.GetType($Name, $true)
+}
+
+$builderType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.LocalG2CandidateBuilder'
+$hooksType = $builderType.GetNestedType('TestHooks', [Reflection.BindingFlags]'Public,NonPublic')
+Assert-True ($null -ne $hooksType) 'LocalG2CandidateBuilder must expose narrowly scoped deterministic TestHooks.'
+$executeMethod = $hooksType.GetMethod('Execute', [Reflection.BindingFlags]'Public,Static')
+Assert-True ($null -ne $executeMethod) 'TestHooks must execute deterministic candidate scenarios.'
+
+function Invoke-Scenario([string]$Scenario) {
+ return $executeMethod.Invoke($null, @($Scenario))
+}
+
+$isolated = Invoke-Scenario 'Isolated'
+Assert-True ($isolated.CandidateCount -gt 0 -and $isolated.CandidateCount -le 12) `
+ 'An isolated event must produce a bounded non-empty candidate set.'
+Assert-Near 0.0 $isolated.StartPositionError 1e-9 'Window start position must match.'
+Assert-Near 0.0 $isolated.EndPositionError 1e-9 'Window end position must match.'
+Assert-Near 0.0 $isolated.StartCurvatureError 1e-8 'Window start curvature must match.'
+Assert-Near 0.0 $isolated.EndCurvatureError 1e-8 'Window end curvature must match.'
+Assert-True $isolated.ContainsLocalG2Source 'Generated samples must identify their source.'
+
+$cluster = Invoke-Scenario 'Cluster'
+Assert-Equal 1 $cluster.OutputRegionCount 'Overlapping events must be built as one region.'
+Assert-True $cluster.InternalConnectionsAreG2 'Internal anchors must share tangent and curvature.'
+
+$reverse = Invoke-Scenario 'Reverse'
+Assert-Equal 'Reverse' $reverse.Direction 'Reverse candidates must preserve segment direction.'
+Assert-True $reverse.VehicleAndGeometricCurvatureSignsAreOpposite `
+ 'Reverse candidates must convert vehicle curvature to geometric curvature exactly once.'
+
+$spliced = Invoke-Scenario 'Spliced'
+Assert-True $spliced.NoDuplicateNonGearPoints 'Splicing must remove duplicate boundary samples.'
+Assert-True $spliced.EndpointsUnchanged 'Splicing must preserve the complete segment endpoints.'
+
+$startBoundary = Invoke-Scenario 'StartBoundary'
+Assert-True ($startBoundary.CandidateCount -gt 0) 'A start-boundary event must build a one-sided candidate.'
+Assert-Near 0.0 $startBoundary.StartCurvatureError 1e-8 `
+ 'A start-boundary candidate must use the preserved physical start vehicle curvature.'
+
+Write-Output 'Path smoothing Local G2 candidate checks passed.'