fix: order Local G2 regions back to front

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs

@@ -561,6 +561,7 @@ internal sealed class LocalG2CandidateBuilder
                 case "ConstantVelocityCurve": return BuildConstantVelocityCurve();
                 case "ExactSpliceEndpoints": return BuildExactSpliceEndpoints();
                 case "GearBoundary": return BuildGearBoundary();
+                case "TwoRegionWorkOrder": return BuildTwoRegionWorkOrder();
                 default: throw new ArgumentOutOfRangeException(nameof(scenario));
             }
         }
@@ -572,7 +573,10 @@ internal sealed class LocalG2CandidateBuilder
                 int outputRegionCount, bool internalConnectionsAreG2, string direction,
                 bool vehicleAndGeometricCurvatureSignsAreOpposite, bool noDuplicateNonGearPoints,
                 bool endpointsUnchanged, bool rejected = false, bool endpointsAreExact = false,
-                bool gearBoundaryMarkerPreserved = false, bool accepted = false)
+                bool gearBoundaryMarkerPreserved = false, bool accepted = false,
+                bool workOrderDescending = false, bool frontArcPreservedAfterBackReplacement = false,
+                bool bothReplacementsRetained = false, bool forwardOrderRejected = false,
+                bool deterministicWorkOrder = false, bool invalidWorkOrderRejected = false)
             {
                 CandidateCount = candidateCount;
                 StartPositionError = startPositionError;
@@ -590,6 +594,12 @@ internal sealed class LocalG2CandidateBuilder
                 EndpointsAreExact = endpointsAreExact;
                 GearBoundaryMarkerPreserved = gearBoundaryMarkerPreserved;
                 Accepted = accepted;
+                WorkOrderDescending = workOrderDescending;
+                FrontArcPreservedAfterBackReplacement = frontArcPreservedAfterBackReplacement;
+                BothReplacementsRetained = bothReplacementsRetained;
+                ForwardOrderRejected = forwardOrderRejected;
+                DeterministicWorkOrder = deterministicWorkOrder;
+                InvalidWorkOrderRejected = invalidWorkOrderRejected;
             }
             public int CandidateCount { get; }
             public double StartPositionError { get; }
@@ -607,6 +617,12 @@ internal sealed class LocalG2CandidateBuilder
             public bool EndpointsAreExact { get; }
             public bool GearBoundaryMarkerPreserved { get; }
             public bool Accepted { get; }
+            public bool WorkOrderDescending { get; }
+            public bool FrontArcPreservedAfterBackReplacement { get; }
+            public bool BothReplacementsRetained { get; }
+            public bool ForwardOrderRejected { get; }
+            public bool DeterministicWorkOrder { get; }
+            public bool InvalidWorkOrderRejected { get; }
         }
 
         private static CandidateTestSnapshot BuildIsolated()
@@ -815,6 +831,234 @@ internal sealed class LocalG2CandidateBuilder
                 false, false, false, false, false, preserved);
         }
 
+        private static CandidateTestSnapshot BuildTwoRegionWorkOrder()
+        {
+            var points = new List<SmoothingPoint2D>();
+            for (int index = 0; index <= 12; index++)
+            {
+                double arc = index * 0.25d;
+                points.Add(new SmoothingPoint2D(
+                    arc,
+                    0d,
+                    arc,
+                    0d,
+                    0d,
+                    1d,
+                    false,
+                    SmoothedPathPointSource.Anchor));
+            }
+
+            var segment = new PreparedDirectionSegment(
+                0,
+                TravelDirection.Forward,
+                points,
+                false,
+                false);
+            var original = new PreparedPath(new[] { segment });
+            LocalG2SmoothingRegion frontRegion = CreateOrderedRegion(0.75d, 0.5d, 1.0d, 0);
+            LocalG2SmoothingRegion backRegion = CreateOrderedRegion(2.25d, 2.0d, 2.5d, 1);
+            var reportOrder = new[] { frontRegion, backRegion };
+
+            var orderer = new LocalG2RegionWorkOrder();
+            if (!orderer.TryCreate(
+                    reportOrder,
+                    out IReadOnlyList<LocalG2SmoothingRegion> workOrder,
+                    out string orderReason))
+            {
+                throw new InvalidOperationException(orderReason);
+            }
+            if (!orderer.TryCreate(
+                    reportOrder,
+                    out IReadOnlyList<LocalG2SmoothingRegion> repeatedOrder,
+                    out string repeatedReason))
+            {
+                throw new InvalidOperationException(repeatedReason);
+            }
+
+            bool descending =
+                ReferenceEquals(backRegion, workOrder[0]) &&
+                ReferenceEquals(frontRegion, workOrder[1]);
+            bool deterministic =
+                ReferenceEquals(workOrder[0], repeatedOrder[0]) &&
+                ReferenceEquals(workOrder[1], repeatedOrder[1]) &&
+                ReferenceEquals(frontRegion, reportOrder[0]) &&
+                ReferenceEquals(backRegion, reportOrder[1]);
+
+            var wrongSegmentTransition = new CurvatureTransition(
+                1,
+                0,
+                1,
+                0.75d,
+                0.75d,
+                0d,
+                0d,
+                0d,
+                0.4d);
+            var invalidRegion = new LocalG2SmoothingRegion(
+                0,
+                new[] { wrongSegmentTransition },
+                0.5d,
+                1.0d,
+                new[]
+                {
+                    new LocalG2WindowVariant(0, 0.5d, 1.0d, 0.25d, 0.25d),
+                });
+            bool invalidWorkOrderRejected =
+                !orderer.TryCreate(
+                    new[] { invalidRegion },
+                    out _,
+                    out _) &&
+                !orderer.TryCreate(
+                    new LocalG2SmoothingRegion[] { null },
+                    out _,
+                    out _);
+
+            LocalG2CandidateGeometry frontCandidate =
+                CreateLengthChangingCandidate(0, 0.5d, 1.0d);
+            LocalG2CandidateGeometry backCandidate =
+                CreateLengthChangingCandidate(1, 2.0d, 2.5d);
+            var splicer = new LocalG2PathSplicer();
+
+            if (!splicer.TryReplace(
+                    original,
+                    backCandidate,
+                    out PreparedPath afterBack,
+                    out string backReason))
+            {
+                throw new InvalidOperationException(backReason);
+            }
+
+            bool frontArcPreserved =
+                PathReferenceInterpolator.TryInterpolateByArcLength(
+                    afterBack.Segments[0].Points,
+                    0.5d,
+                    out SmoothingPoint2D frontStart,
+                    out _) &&
+                PathReferenceInterpolator.TryInterpolateByArcLength(
+                    afterBack.Segments[0].Points,
+                    1.0d,
+                    out SmoothingPoint2D frontEnd,
+                    out _) &&
+                Math.Abs(frontStart.X - 0.5d) <= 1e-12d &&
+                Math.Abs(frontEnd.X - 1.0d) <= 1e-12d;
+
+            if (!splicer.TryReplace(
+                    afterBack,
+                    frontCandidate,
+                    out PreparedPath afterBoth,
+                    out string frontReason))
+            {
+                throw new InvalidOperationException(frontReason);
+            }
+
+            int localG2PointCount = 0;
+            for (int index = 0; index < afterBoth.Segments[0].Points.Count; index++)
+            {
+                if (afterBoth.Segments[0].Points[index].Source ==
+                    SmoothedPathPointSource.LocalG2Transition)
+                {
+                    localG2PointCount++;
+                }
+            }
+
+            if (!splicer.TryReplace(
+                    original,
+                    frontCandidate,
+                    out PreparedPath afterFront,
+                    out string firstReason))
+            {
+                throw new InvalidOperationException(firstReason);
+            }
+            bool forwardOrderRejected = !splicer.TryReplace(
+                afterFront,
+                backCandidate,
+                out _,
+                out _);
+
+            return new CandidateTestSnapshot(
+                0,
+                0d,
+                0d,
+                0d,
+                0d,
+                false,
+                0,
+                false,
+                string.Empty,
+                false,
+                false,
+                false,
+                false,
+                false,
+                false,
+                false,
+                descending,
+                frontArcPreserved,
+                localG2PointCount >= 2,
+                forwardOrderRejected,
+                deterministic,
+                invalidWorkOrderRejected);
+        }
+
+        private static LocalG2SmoothingRegion CreateOrderedRegion(
+            double eventArc,
+            double startArc,
+            double endArc,
+            int index)
+        {
+            var transition = new CurvatureTransition(
+                0,
+                index,
+                index + 1,
+                eventArc,
+                eventArc,
+                0d,
+                0d,
+                0d,
+                0.4d);
+            return new LocalG2SmoothingRegion(
+                0,
+                new[] { transition },
+                startArc,
+                endArc,
+                new[]
+                {
+                    new LocalG2WindowVariant(
+                        0,
+                        startArc,
+                        endArc,
+                        eventArc - startArc,
+                        endArc - eventArc),
+                });
+        }
+
+        private static LocalG2CandidateGeometry CreateLengthChangingCandidate(
+            int candidateIndex,
+            double startArc,
+            double endArc)
+        {
+            double middleArc = 0.5d * (startArc + endArc);
+            var points = new[]
+            {
+                Point(startArc, 0d, startArc, false),
+                Point(middleArc, 0.20d, middleArc, false),
+                Point(endArc, 0d, endArc, false),
+            };
+            return new LocalG2CandidateGeometry(
+                candidateIndex,
+                0,
+                startArc,
+                endArc,
+                middleArc - startArc,
+                endArc - middleArc,
+                points,
+                0d,
+                0d,
+                0d,
+                0d,
+                true);
+        }
+
         private static PreparedDirectionSegment CreateSegment(TravelDirection direction, double? startCurvature)
         {
             double heading = direction == TravelDirection.Forward ? 0d : Math.PI;

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2RegionWorkOrder.cs

@@ -0,0 +1,107 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
+
+/// <summary>把稳定报告顺序转换为不会使后续原始局部弧长错位的工作顺序。</summary>
+internal sealed class LocalG2RegionWorkOrder
+{
+    internal bool TryCreate(
+        IReadOnlyList<LocalG2SmoothingRegion> reportOrder,
+        out IReadOnlyList<LocalG2SmoothingRegion> workOrder,
+        out string reason)
+    {
+        workOrder = Empty();
+        reason = string.Empty;
+        if (reportOrder == null)
+        {
+            reason = "局部 G2 区域工作顺序输入无效。";
+            return false;
+        }
+
+        var indexed = new List<IndexedRegion>(reportOrder.Count);
+        for (int index = 0; index < reportOrder.Count; index++)
+        {
+            LocalG2SmoothingRegion region = reportOrder[index];
+            if (!TryValidate(region, out double firstArc, out reason))
+                return false;
+            indexed.Add(new IndexedRegion(region, index, firstArc));
+        }
+
+        indexed.Sort(Compare);
+        var ordered = new List<LocalG2SmoothingRegion>(indexed.Count);
+        for (int index = 0; index < indexed.Count; index++)
+            ordered.Add(indexed[index].Region);
+        workOrder = new ReadOnlyCollection<LocalG2SmoothingRegion>(ordered);
+        return true;
+    }
+
+    private static bool TryValidate(
+        LocalG2SmoothingRegion region,
+        out double firstArc,
+        out string reason)
+    {
+        firstArc = 0d;
+        reason = string.Empty;
+        if (region == null || region.SegmentIndex < 0 ||
+            region.Transitions == null || region.Transitions.Count == 0 ||
+            region.WindowVariants == null || region.WindowVariants.Count == 0)
+        {
+            reason = "局部 G2 工作顺序包含空区域或空窗口集。";
+            return false;
+        }
+
+        double previousArc = -1d;
+        for (int index = 0; index < region.Transitions.Count; index++)
+        {
+            CurvatureTransition transition = region.Transitions[index];
+            if (transition == null ||
+                transition.SegmentIndex != region.SegmentIndex ||
+                !NumericGuard.IsFinite(transition.LocalArcLengthMeters) ||
+                transition.LocalArcLengthMeters < previousArc)
+            {
+                reason = "局部 G2 工作顺序要求区域事件有限、同段且按弧长升序。";
+                return false;
+            }
+            previousArc = transition.LocalArcLengthMeters;
+        }
+
+        firstArc = region.Transitions[0].LocalArcLengthMeters;
+        return true;
+    }
+
+    private static int Compare(IndexedRegion left, IndexedRegion right)
+    {
+        int segment = left.Region.SegmentIndex.CompareTo(right.Region.SegmentIndex);
+        if (segment != 0) return segment;
+        int descendingArc = right.FirstArc.CompareTo(left.FirstArc);
+        return descendingArc != 0
+            ? descendingArc
+            : left.ReportIndex.CompareTo(right.ReportIndex);
+    }
+
+    private static IReadOnlyList<LocalG2SmoothingRegion> Empty()
+    {
+        return new ReadOnlyCollection<LocalG2SmoothingRegion>(
+            new List<LocalG2SmoothingRegion>());
+    }
+
+    private sealed class IndexedRegion
+    {
+        internal IndexedRegion(
+            LocalG2SmoothingRegion region,
+            int reportIndex,
+            double firstArc)
+        {
+            Region = region;
+            ReportIndex = reportIndex;
+            FirstArc = firstArc;
+        }
+
+        internal LocalG2SmoothingRegion Region { get; }
+        internal int ReportIndex { get; }
+        internal double FirstArc { get; }
+    }
+}

ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1

@@ -74,6 +74,20 @@ $gearBoundary = Invoke-Scenario 'GearBoundary'
 Assert-True $gearBoundary.GearBoundaryMarkerPreserved `
     'A window touching a gear-switch segment boundary must preserve its point-level gear marker.'
 
+$twoRegions = Invoke-Scenario 'TwoRegionWorkOrder'
+Assert-True $twoRegions.WorkOrderDescending `
+    'Same-segment regions must be processed from larger original local arc to smaller local arc.'
+Assert-True $twoRegions.FrontArcPreservedAfterBackReplacement `
+    'Replacing the back region must preserve the front region original arc coordinates.'
+Assert-True $twoRegions.BothReplacementsRetained `
+    'Back-then-front replacement must retain both length-changing local replacements.'
+Assert-True $twoRegions.ForwardOrderRejected `
+    'The regression fixture must prove that front-first invalidates the original back absolute arc.'
+Assert-True $twoRegions.DeterministicWorkOrder `
+    'Work ordering must repeat exactly without mutating report order.'
+Assert-True $twoRegions.InvalidWorkOrderRejected `
+    'Work ordering must reject null regions and cross-segment event contents.'
+
 $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.'