feat: build local G2 smoothing candidates

2026-07-30 17:26:06 +08:00
parent a3b6ea7d84
commit 1c433458bc
4 changed files with 819 additions and 0 deletions
@@ -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;
+
+/// <summary>以固定窗口和尺度顺序构造有限的局部五次 Hermite G2 候选。</summary>
+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<LocalG2CandidateGeometry> Build(
+        PreparedDirectionSegment originalSegment,
+        LocalG2SmoothingRegion region,
+        double outputSpacingMeters,
+        LocalG2OptionsSnapshot options,
+        CancellationToken cancellationToken)
+    {
+        if (!IsValidInput(originalSegment, region, outputSpacingMeters, options)) return Empty();
+
+        var candidates = new List<LocalG2CandidateGeometry>();
+        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<BoundaryNode>();
+        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<SmoothingPoint2D>();
+        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<BoundaryNode> 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<SmoothingPoint2D> 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<SmoothingPoint2D> 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<SmoothingPoint2D> 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<LocalG2CandidateGeometry> Empty() => new ReadOnlyCollection<LocalG2CandidateGeometry>(new List<LocalG2CandidateGeometry>());
+    private static IReadOnlyList<LocalG2CandidateGeometry> ReadOnly(List<LocalG2CandidateGeometry> values) =>
+        new ReadOnlyCollection<LocalG2CandidateGeometry>(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; }
+    }
+
+    /// <summary>反射脚本使用的窄范围候选与拼接场景入口。</summary>
+    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<LocalG2CandidateGeometry> 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<LocalG2SmoothingRegion> regions, out string reason))
+            {
+                throw new InvalidOperationException(reason);
+            }
+            IReadOnlyList<LocalG2CandidateGeometry> 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<LocalG2CandidateGeometry> 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<LocalG2CandidateGeometry> 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<LocalG2CandidateGeometry> 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<SmoothingPoint2D>();
+            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<CurvatureTransition> 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<SmoothingPoint2D> 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));
+    }
+}
@@ -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;
+
+/// <summary>一个尚未经过安全和质量评价的局部 G2 替换几何。</summary>
+internal sealed class LocalG2CandidateGeometry
+{
+    internal LocalG2CandidateGeometry(
+        int candidateIndex,
+        int segmentIndex,
+        double startArcLengthMeters,
+        double endArcLengthMeters,
+        double leftWindowLengthMeters,
+        double rightWindowLengthMeters,
+        IReadOnlyList<SmoothingPoint2D> 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<SmoothingPoint2D> 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<SmoothingPoint2D> Copy(IReadOnlyList<SmoothingPoint2D> source)
+    {
+        var copy = new List<SmoothingPoint2D>(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<SmoothingPoint2D>(copy);
+    }
+}
@@ -0,0 +1,129 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
+
+/// <summary>只替换一个方向段内局部窗口，并保持其余方向拓扑不变。</summary>
+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<SmoothingPoint2D>();
+        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<SmoothingPoint2D> localPoints, out reason)) return false;
+
+        var segments = new List<PreparedDirectionSegment>(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<SmoothingPoint2D> points,
+        out IReadOnlyList<SmoothingPoint2D> recalculated,
+        out string reason)
+    {
+        recalculated = null;
+        reason = string.Empty;
+        if (points == null || points.Count < 2)
+        {
+            reason = "局部 G2 拼接后方向段没有足够点。";
+            return false;
+        }
+        var output = new List<SmoothingPoint2D>(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<SmoothingPoint2D> 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);
+    }
+}