fix: harden Local G2 stabilization regressions

2026-07-31 16:38:21 +08:00
parent 49c7e5b0c0
commit 24af39de74
5 changed files with 234 additions and 15 deletions
@@ -562,6 +562,7 @@ internal sealed class LocalG2CandidateBuilder
                case "ExactSpliceEndpoints": return BuildExactSpliceEndpoints();
                case "GearBoundary": return BuildGearBoundary();
                case "TwoRegionWorkOrder": return BuildTwoRegionWorkOrder();
+                case "MultiSegmentWorkOrder": return BuildMultiSegmentWorkOrder();
                default: throw new ArgumentOutOfRangeException(nameof(scenario));
            }
        }
@@ -576,7 +577,9 @@ internal sealed class LocalG2CandidateBuilder
                bool gearBoundaryMarkerPreserved = false, bool accepted = false,
                bool workOrderDescending = false, bool frontArcPreservedAfterBackReplacement = false,
                bool bothReplacementsRetained = false, bool forwardOrderRejected = false,
-                bool deterministicWorkOrder = false, bool invalidWorkOrderRejected = false)
+                bool deterministicWorkOrder = false, bool invalidWorkOrderRejected = false,
+                bool deterministicReplacementGeometry = false, bool segmentOrderAscending = false,
+                bool equalArcUsesReportOrder = false)
            {
                CandidateCount = candidateCount;
                StartPositionError = startPositionError;
@@ -600,6 +603,9 @@ internal sealed class LocalG2CandidateBuilder
                ForwardOrderRejected = forwardOrderRejected;
                DeterministicWorkOrder = deterministicWorkOrder;
                InvalidWorkOrderRejected = invalidWorkOrderRejected;
+                DeterministicReplacementGeometry = deterministicReplacementGeometry;
+                SegmentOrderAscending = segmentOrderAscending;
+                EqualArcUsesReportOrder = equalArcUsesReportOrder;
            }
            public int CandidateCount { get; }
            public double StartPositionError { get; }
@@ -623,6 +629,9 @@ internal sealed class LocalG2CandidateBuilder
            public bool ForwardOrderRejected { get; }
            public bool DeterministicWorkOrder { get; }
            public bool InvalidWorkOrderRejected { get; }
+            public bool DeterministicReplacementGeometry { get; }
+            public bool SegmentOrderAscending { get; }
+            public bool EqualArcUsesReportOrder { get; }
        }

        private static CandidateTestSnapshot BuildIsolated()
@@ -951,15 +960,50 @@ internal sealed class LocalG2CandidateBuilder
                throw new InvalidOperationException(frontReason);
            }

-            int localG2PointCount = 0;
+            if (!splicer.TryReplace(
+                    original,
+                    backCandidate,
+                    out PreparedPath repeatedAfterBack,
+                    out string repeatedBackReason))
+            {
+                throw new InvalidOperationException(repeatedBackReason);
+            }
+            if (!splicer.TryReplace(
+                    repeatedAfterBack,
+                    frontCandidate,
+                    out PreparedPath repeatedAfterBoth,
+                    out string repeatedFrontReason))
+            {
+                throw new InvalidOperationException(repeatedFrontReason);
+            }
+
+            bool frontInteriorRetained = false;
+            bool backInteriorRetained = false;
+            int frontInteriorCount = 0;
+            int backInteriorCount = 0;
            for (int index = 0; index < afterBoth.Segments[0].Points.Count; index++)
            {
-                if (afterBoth.Segments[0].Points[index].Source ==
-                    SmoothedPathPointSource.LocalG2Transition)
+                SmoothingPoint2D point = afterBoth.Segments[0].Points[index];
+                if (point.Source != SmoothedPathPointSource.LocalG2Transition)
+                    continue;
+                if (point.X == 0.75d && point.Y == 0.20d)
                {
-                    localG2PointCount++;
+                    frontInteriorCount++;
+                    frontInteriorRetained = true;
+                }
+                if (point.X == 2.25d && point.Y == 0.20d)
+                {
+                    backInteriorCount++;
+                    backInteriorRetained = true;
                }
            }
+            bool exactInteriorsRetained =
+                frontInteriorRetained &&
+                backInteriorRetained &&
+                frontInteriorCount == 1 &&
+                backInteriorCount == 1;
+            bool deterministicReplacementGeometry =
+                HasSameReplacementGeometry(afterBoth, repeatedAfterBoth);

            if (!splicer.TryReplace(
                    original,
@@ -994,20 +1038,101 @@ internal sealed class LocalG2CandidateBuilder
                false,
                descending,
                frontArcPreserved,
-                localG2PointCount >= 2,
+                exactInteriorsRetained,
                forwardOrderRejected,
                deterministic,
-                invalidWorkOrderRejected);
+                invalidWorkOrderRejected,
+                deterministicReplacementGeometry);
+        }
+
+        private static CandidateTestSnapshot BuildMultiSegmentWorkOrder()
+        {
+            LocalG2SmoothingRegion laterSegment =
+                CreateOrderedRegion(0.25d, 0d, 0.5d, 0, 1);
+            LocalG2SmoothingRegion firstTie =
+                CreateOrderedRegion(0.75d, 0.5d, 1.0d, 0);
+            LocalG2SmoothingRegion secondTie =
+                CreateOrderedRegion(0.75d, 0.5d, 1.0d, 1);
+            var reportOrder = new[] { laterSegment, firstTie, secondTie };
+            if (!new LocalG2RegionWorkOrder().TryCreate(
+                    reportOrder,
+                    out IReadOnlyList<LocalG2SmoothingRegion> workOrder,
+                    out string reason))
+            {
+                throw new InvalidOperationException(reason);
+            }
+
+            bool segmentOrderAscending =
+                workOrder.Count == 3 &&
+                workOrder[0].SegmentIndex == 0 &&
+                workOrder[1].SegmentIndex == 0 &&
+                workOrder[2].SegmentIndex == 1;
+            bool equalArcUsesReportOrder =
+                ReferenceEquals(firstTie, workOrder[0]) &&
+                ReferenceEquals(secondTie, workOrder[1]);
+            return new CandidateTestSnapshot(
+                0,
+                0d,
+                0d,
+                0d,
+                0d,
+                false,
+                0,
+                false,
+                string.Empty,
+                false,
+                false,
+                false,
+                segmentOrderAscending: segmentOrderAscending,
+                equalArcUsesReportOrder: equalArcUsesReportOrder);
+        }
+
+        private static bool HasSameReplacementGeometry(
+            PreparedPath first,
+            PreparedPath second)
+        {
+            if (first == null || second == null ||
+                first.Points.Count != second.Points.Count ||
+                first.Segments.Count != second.Segments.Count)
+            {
+                return false;
+            }
+
+            for (int segmentIndex = 0; segmentIndex < first.Segments.Count; segmentIndex++)
+            {
+                PreparedDirectionSegment firstSegment = first.Segments[segmentIndex];
+                PreparedDirectionSegment secondSegment = second.Segments[segmentIndex];
+                if (firstSegment.Direction != secondSegment.Direction ||
+                    firstSegment.Points.Count != secondSegment.Points.Count)
+                {
+                    return false;
+                }
+
+                for (int pointIndex = 0; pointIndex < firstSegment.Points.Count; pointIndex++)
+                {
+                    SmoothingPoint2D firstPoint = firstSegment.Points[pointIndex];
+                    SmoothingPoint2D secondPoint = secondSegment.Points[pointIndex];
+                    if (firstPoint.X != secondPoint.X ||
+                        firstPoint.Y != secondPoint.Y ||
+                        firstPoint.Source != secondPoint.Source ||
+                        firstSegment.Direction != secondSegment.Direction)
+                    {
+                        return false;
+                    }
+                }
+            }
+            return true;
        }

        private static LocalG2SmoothingRegion CreateOrderedRegion(
            double eventArc,
            double startArc,
            double endArc,
-            int index)
+            int index,
+            int segmentIndex = 0)
        {
            var transition = new CurvatureTransition(
-                0,
+                segmentIndex,
                index,
                index + 1,
                eventArc,
@@ -1017,7 +1142,7 @@ internal sealed class LocalG2CandidateBuilder
                0d,
                0.4d);
            return new LocalG2SmoothingRegion(
-                0,
+                segmentIndex,
                new[] { transition },
                startArc,
                endArc,
@@ -30,7 +30,7 @@ internal sealed class LocalG2WindowVariant
    internal double RightWindowLengthMeters { get; }
 }

-/// <summary>因最大合法窗口相交而合并的一组曲率事件。</summary>
+/// <summary>至少存在一个联合合法生成窗口变体的一组曲率过渡。</summary>
 internal sealed class LocalG2SmoothingRegion
 {
    internal LocalG2SmoothingRegion(
@@ -1,6 +1,7 @@
 using System;
 using System.Collections.Generic;
 using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
 using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;

 namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
@@ -45,6 +46,7 @@ internal static class RawPathBaselineBuilder
        }

        if (reason != "平滑路径包含非法数值或超限车辆曲率。" ||
+            !HasTrustedVehicleCurvaturesWithinLimit(request) ||
            !TryCreateTrustedRawAnalysis(request, out analysis, out reason) ||
            !validator.TryValidate(
                analysis.Path,
@@ -64,6 +66,28 @@ internal static class RawPathBaselineBuilder
        return true;
    }

+    private static bool HasTrustedVehicleCurvaturesWithinLimit(PathSmoothingRequest request)
+    {
+        if (request?.CoarsePath == null || request.CoarsePath.Count == 0 ||
+            !VehicleKinematics.TryGetMaximumCurvaturePerMeter(
+                request.Vehicle,
+                out double maximumCurvaturePerMeter))
+        {
+            return false;
+        }
+
+        for (int index = 0; index < request.CoarsePath.Count; index++)
+        {
+            CoarsePathPoint point = request.CoarsePath[index];
+            if (point == null || !IsFinite(point.VehicleCurvature) ||
+                Math.Abs(point.VehicleCurvature) > maximumCurvaturePerMeter)
+            {
+                return false;
+            }
+        }
+        return true;
+    }
+
    private static bool TryCreateTrustedRawAnalysis(
        PathSmoothingRequest request,
        out PathGeometryAnalysis analysis,