fix: harden local G2 candidate splicing

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs

@@ -12,6 +12,7 @@ namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
 internal sealed class LocalG2CandidateBuilder
 {
     private const double MinimumDerivativeNorm = 1e-10d;
+    private const int MinimumCurveEvaluationIntervals = 32;
     private const int MaximumSubdivisionDepth = 32;
     private static readonly double[] DerivativeScaleMultipliers = { 1d, 0.85d, 1.15d };
 
@@ -194,6 +195,13 @@ internal sealed class LocalG2CandidateBuilder
         List<SmoothingPoint2D> output,
         CancellationToken cancellationToken)
     {
+        // A short endpoint chord does not prove that a fifth-degree curve has no interior cusp.
+        // Check a deterministic interior grid before the chord-driven subdivision below; every
+        // subsequently emitted subdivision parameter is checked again by TryEvaluate.
+        for (int index = 0; index <= MinimumCurveEvaluationIntervals; index++)
+        {
+            if (!TryEvaluate(curve, (double)index / MinimumCurveEvaluationIntervals, out _)) return false;
+        }
         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);
@@ -361,6 +369,9 @@ internal sealed class LocalG2CandidateBuilder
                 case "Reverse": return BuildReverse();
                 case "Spliced": return BuildSpliced();
                 case "StartBoundary": return BuildStartBoundary();
+                case "InteriorStationaryCurve": return BuildInteriorStationaryCurve();
+                case "ExactSpliceEndpoints": return BuildExactSpliceEndpoints();
+                case "GearBoundary": return BuildGearBoundary();
                 default: throw new ArgumentOutOfRangeException(nameof(scenario));
             }
         }
@@ -371,7 +382,8 @@ internal sealed class LocalG2CandidateBuilder
                 double startCurvatureError, double endCurvatureError, bool containsLocalG2Source,
                 int outputRegionCount, bool internalConnectionsAreG2, string direction,
                 bool vehicleAndGeometricCurvatureSignsAreOpposite, bool noDuplicateNonGearPoints,
-                bool endpointsUnchanged)
+                bool endpointsUnchanged, bool rejected = false, bool endpointsAreExact = false,
+                bool gearBoundaryMarkerPreserved = false)
             {
                 CandidateCount = candidateCount;
                 StartPositionError = startPositionError;
@@ -385,6 +397,9 @@ internal sealed class LocalG2CandidateBuilder
                 VehicleAndGeometricCurvatureSignsAreOpposite = vehicleAndGeometricCurvatureSignsAreOpposite;
                 NoDuplicateNonGearPoints = noDuplicateNonGearPoints;
                 EndpointsUnchanged = endpointsUnchanged;
+                Rejected = rejected;
+                EndpointsAreExact = endpointsAreExact;
+                GearBoundaryMarkerPreserved = gearBoundaryMarkerPreserved;
             }
             public int CandidateCount { get; }
             public double StartPositionError { get; }
@@ -398,6 +413,9 @@ internal sealed class LocalG2CandidateBuilder
             public bool VehicleAndGeometricCurvatureSignsAreOpposite { get; }
             public bool NoDuplicateNonGearPoints { get; }
             public bool EndpointsUnchanged { get; }
+            public bool Rejected { get; }
+            public bool EndpointsAreExact { get; }
+            public bool GearBoundaryMarkerPreserved { get; }
         }
 
         private static CandidateTestSnapshot BuildIsolated()
@@ -517,6 +535,73 @@ internal sealed class LocalG2CandidateBuilder
                 ContainsLocalG2(candidate.RegionPoints), 0, false, string.Empty, false, false, false);
         }
 
+        private static CandidateTestSnapshot BuildInteriorStationaryCurve()
+        {
+            const double endpointX = 7d / 1500d;
+            if (!QuinticHermiteCurve2D.TryCreate(0d, 0d, 0.01d, 0d, 0d, 0d,
+                    endpointX, 0d, 0.01d, 0d, 0d, 0d, out QuinticHermiteCurve2D curve, out string reason))
+            {
+                throw new InvalidOperationException(reason);
+            }
+            var points = new[]
+            {
+                new SmoothingPoint2D(0d, 0d, 0d, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
+                new SmoothingPoint2D(endpointX, 0d, endpointX, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
+            };
+            var segment = new PreparedDirectionSegment(0, TravelDirection.Forward, points, false, false);
+            var sampled = new List<SmoothingPoint2D>();
+            bool accepted = TryAppendCurveSamples(curve,
+                new BoundaryNode(0d, 0d, 0d, 0d, 0d),
+                new BoundaryNode(endpointX, endpointX, 0d, 0d, 0d),
+                segment, 0.025d, sampled, CancellationToken.None);
+            return new CandidateTestSnapshot(0, 0d, 0d, 0d, 0d, false, 0, false, string.Empty,
+                false, false, false, !accepted);
+        }
+
+        private static CandidateTestSnapshot BuildExactSpliceEndpoints()
+        {
+            PreparedDirectionSegment segment = CreateSegment(TravelDirection.Forward, null);
+            var candidate = new LocalG2CandidateGeometry(0, 0, 0.25d, 0.75d, 0.25d, 0.25d,
+                new[]
+                {
+                    Point(0.2500000005d, 0d, 0.25d, false),
+                    Point(0.5d, 0d, 0.5d, false),
+                    Point(0.7499999995d, 0d, 0.75d, false),
+                }, 0d, 0d, 0d, 0d, true);
+            if (!new LocalG2PathSplicer().TryReplace(new PreparedPath(new[] { segment }), candidate,
+                    out PreparedPath replaced, out string reason))
+            {
+                throw new InvalidOperationException(reason);
+            }
+            PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, 0.25d, out SmoothingPoint2D start, out _);
+            PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, 0.75d, out SmoothingPoint2D end, out _);
+            IReadOnlyList<SmoothingPoint2D> result = replaced.Segments[0].Points;
+            bool exact = result[1].X == start.X && result[1].Y == start.Y &&
+                result[result.Count - 2].X == end.X && result[result.Count - 2].Y == end.Y;
+            return new CandidateTestSnapshot(0, 0d, 0d, 0d, 0d, false, 0, false, string.Empty,
+                false, false, false, false, exact);
+        }
+
+        private static CandidateTestSnapshot BuildGearBoundary()
+        {
+            var sourcePoints = new[]
+            {
+                Point(0d, 0d, 0d, true), Point(0.25d, 0d, 0.25d, false), Point(0.5d, 0d, 0.5d, false),
+            };
+            var segment = new PreparedDirectionSegment(0, TravelDirection.Forward, sourcePoints, true, false);
+            var candidate = new LocalG2CandidateGeometry(0, 0, 0d, 0.5d, 0d, 0.5d,
+                new[] { Point(0d, 0d, 0d, false), Point(0.25d, 0d, 0.25d, false), Point(0.5d, 0d, 0.5d, false) },
+                0d, 0d, 0d, 0d, true);
+            if (!new LocalG2PathSplicer().TryReplace(new PreparedPath(new[] { segment }), candidate,
+                    out PreparedPath replaced, out string reason))
+            {
+                throw new InvalidOperationException(reason);
+            }
+            bool preserved = replaced.Segments[0].StartsAtGearSwitch && replaced.Segments[0].Points[0].IsGearSwitchPoint;
+            return new CandidateTestSnapshot(0, 0d, 0d, 0d, 0d, false, 0, false, string.Empty,
+                false, false, false, false, false, preserved);
+        }
+
         private static PreparedDirectionSegment CreateSegment(TravelDirection direction, double? startCurvature)
         {
             double heading = direction == TravelDirection.Forward ? 0d : Math.PI;
@@ -549,5 +634,8 @@ internal sealed class LocalG2CandidateBuilder
         }
         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));
+        private static SmoothingPoint2D Point(double x, double y, double arcLength, bool isGearSwitch) =>
+            new SmoothingPoint2D(x, y, arcLength, 0d, 0d, 1d, isGearSwitch,
+                isGearSwitch ? SmoothedPathPointSource.GearSwitch : SmoothedPathPointSource.LocalG2Transition);
     }
 }

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2PathSplicer.cs

@@ -40,7 +40,12 @@ internal sealed class LocalG2PathSplicer
             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]);
+        // Candidate endpoints are only validated within a numerical tolerance.  The replacement
+        // itself must use the exact source-window endpoints, including a possible gear marker.
+        AddWithoutNonGearDuplicates(combined, start);
+        for (int index = 1; index < candidate.RegionPoints.Count - 1; index++)
+            AddWithoutNonGearDuplicates(combined, candidate.RegionPoints[index]);
+        AddWithoutNonGearDuplicates(combined, end);
         for (int index = 0; index < source.Points.Count; index++)
         {
             SmoothingPoint2D point = source.Points[index];

ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1

@@ -58,4 +58,16 @@ Assert-True ($startBoundary.CandidateCount -gt 0) 'A start-boundary event must b
 Assert-Near 0.0 $startBoundary.StartCurvatureError 1e-8 `
     'A start-boundary candidate must use the preserved physical start vehicle curvature.'
 
+$interiorDerivative = Invoke-Scenario 'InteriorStationaryCurve'
+Assert-True $interiorDerivative.Rejected `
+    'A curve with a stationary interior derivative must be rejected even when its endpoint chord is short.'
+
+$exactSplice = Invoke-Scenario 'ExactSpliceEndpoints'
+Assert-True $exactSplice.EndpointsAreExact `
+    'Splicing must write interpolated exact endpoints instead of accepting approximate candidate endpoints.'
+
+$gearBoundary = Invoke-Scenario 'GearBoundary'
+Assert-True $gearBoundary.GearBoundaryMarkerPreserved `
+    'A window touching a gear-switch segment boundary must preserve its point-level gear marker.'
+
 Write-Output 'Path smoothing Local G2 candidate checks passed.'