fix: enforce Local G2 total window length

2026-07-31 15:47:33 +08:00
parent c623c9b529
commit 1ac3dbda8d
3 changed files with 236 additions and 33 deletions
@@ -1,6 +1,7 @@
 using System;
 using System.Collections.Generic;
 using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.Utils;

 namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;

@@ -40,8 +41,11 @@ internal sealed class LocalG2SmoothingRegion
        IReadOnlyList<LocalG2WindowVariant> windowVariants)
    {
        if (segmentIndex < 0 || transitions == null || transitions.Count == 0 ||
-            maximumStartArcLengthMeters < 0d || maximumEndArcLengthMeters < maximumStartArcLengthMeters ||
-            windowVariants == null)
+            !NumericGuard.IsFinite(maximumStartArcLengthMeters) ||
+            !NumericGuard.IsFinite(maximumEndArcLengthMeters) ||
+            maximumStartArcLengthMeters < 0d ||
+            maximumEndArcLengthMeters < maximumStartArcLengthMeters ||
+            windowVariants == null || windowVariants.Count == 0)
        {
            throw new ArgumentOutOfRangeException(nameof(transitions));
        }
@@ -1,6 +1,7 @@
 using System;
 using System.Collections.Generic;
 using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
 using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
 using MultiWheelC.TrajectoryPlanning.Utils;

@@ -48,28 +49,49 @@ internal sealed class LocalG2WindowPlanner
        {
            CurvatureTransition first = ordered[cursor];
            double segmentLength = segmentLengths[first.SegmentIndex];
-            WindowRange merged = MaximumLegalRange(first.LocalArcLengthMeters, segmentLength, options.MaximumWindowLengthMeters);
            var group = new List<CurvatureTransition> { first };
+            IReadOnlyList<LocalG2WindowVariant> variants =
+                BuildVariants(group, segmentLength, options);
+            if (variants.Count == 0)
+            {
+                reason = "局部 G2 单事件无法生成满足总长度约束的窗口。";
+                return false;
+            }
            cursor++;

-            while (cursor < ordered.Count && ordered[cursor].SegmentIndex == first.SegmentIndex)
+            while (cursor < ordered.Count &&
+                ordered[cursor].SegmentIndex == first.SegmentIndex)
            {
-                CurvatureTransition next = ordered[cursor];
-                WindowRange nextRange = MaximumLegalRange(next.LocalArcLengthMeters, segmentLength, options.MaximumWindowLengthMeters);
-                if (nextRange.StartArcLengthMeters > merged.EndArcLengthMeters + MergeToleranceMeters) break;
-                group.Add(next);
-                merged = new WindowRange(
-                    Math.Min(merged.StartArcLengthMeters, nextRange.StartArcLengthMeters),
-                    Math.Max(merged.EndArcLengthMeters, nextRange.EndArcLengthMeters));
+                var tentative = new List<CurvatureTransition>(group)
+                {
+                    ordered[cursor],
+                };
+                IReadOnlyList<LocalG2WindowVariant> tentativeVariants =
+                    BuildVariants(tentative, segmentLength, options);
+                if (tentativeVariants.Count == 0) break;
+                group = tentative;
+                variants = tentativeVariants;
                cursor++;
            }

+            double minimumStart = double.PositiveInfinity;
+            double maximumEnd = double.NegativeInfinity;
+            for (int variantIndex = 0; variantIndex < variants.Count; variantIndex++)
+            {
+                minimumStart = Math.Min(
+                    minimumStart,
+                    variants[variantIndex].StartArcLengthMeters);
+                maximumEnd = Math.Max(
+                    maximumEnd,
+                    variants[variantIndex].EndArcLengthMeters);
+            }
+
            planned.Add(new LocalG2SmoothingRegion(
                first.SegmentIndex,
                group,
-                merged.StartArcLengthMeters,
-                merged.EndArcLengthMeters,
-                BuildVariants(group, segmentLength, options)));
+                minimumStart,
+                maximumEnd,
+                variants));
        }
        regions = new ReadOnlyCollection<LocalG2SmoothingRegion>(planned);
        return true;
@@ -87,17 +109,18 @@ internal sealed class LocalG2WindowPlanner
        foreach (double target in BuildTargets(options, segmentLength))
        {
            if (variants.Count >= options.MaximumCandidatesPerRegion) break;
-            AddIfLegal(variants, target, 0.5d, anchor, firstEvent, lastEvent, segmentLength, true, options.MaximumCandidatesPerRegion);
-            AddIfLegal(variants, target, 0.4d, anchor, firstEvent, lastEvent, segmentLength, false, options.MaximumCandidatesPerRegion);
-            AddIfLegal(variants, target, 0.6d, anchor, firstEvent, lastEvent, segmentLength, false, options.MaximumCandidatesPerRegion);
+            AddIfLegal(variants, target, 0.5d, anchor, firstEvent, lastEvent, segmentLength, true, options);
+            AddIfLegal(variants, target, 0.4d, anchor, firstEvent, lastEvent, segmentLength, false, options);
+            AddIfLegal(variants, target, 0.6d, anchor, firstEvent, lastEvent, segmentLength, false, options);
        }
        return new ReadOnlyCollection<LocalG2WindowVariant>(variants);
    }

    private static void AddIfLegal(List<LocalG2WindowVariant> variants, double target, double leftRatio,
-        double anchor, double firstEvent, double lastEvent, double segmentLength, bool permitBoundaryShift, int maximumCount)
+        double anchor, double firstEvent, double lastEvent, double segmentLength, bool permitBoundaryShift,
+        LocalG2OptionsSnapshot options)
    {
-        if (variants.Count >= maximumCount) return;
+        if (variants.Count >= options.MaximumCandidatesPerRegion) return;
        double left = target * leftRatio;
        double right = target - left;
        double availableLeft = anchor;
@@ -120,11 +143,20 @@ internal sealed class LocalG2WindowPlanner
        double end = anchor + right;
        if (start > firstEvent + MergeToleranceMeters || end + MergeToleranceMeters < lastEvent || end - start + MergeToleranceMeters < target)
            return;
+        double actualLength = end - start;
+        if (actualLength + MergeToleranceMeters < options.MinimumWindowLengthMeters ||
+            actualLength > options.MaximumWindowLengthMeters + MergeToleranceMeters)
+        {
+            return;
+        }
        variants.Add(new LocalG2WindowVariant(variants.Count, start, end, left, right));
    }

    private static IReadOnlyList<double> BuildTargets(LocalG2OptionsSnapshot options, double segmentLength)
    {
+        if (segmentLength + MergeToleranceMeters < options.MinimumWindowLengthMeters)
+            return new ReadOnlyCollection<double>(new List<double>());
+
        double[] requested =
        {
            options.PreferredWindowLengthMeters,
@@ -152,14 +184,6 @@ internal sealed class LocalG2WindowPlanner
        return targets;
    }

-    private static WindowRange MaximumLegalRange(double eventArcLength, double segmentLength, double maximumWindowLength)
-    {
-        double target = Math.Min(maximumWindowLength, segmentLength);
-        return new WindowRange(
-            Math.Max(0d, eventArcLength - target),
-            Math.Min(segmentLength, eventArcLength + target));
-    }
-
    private static bool TryGetSegmentLengths(PreparedPath originalPath, out Dictionary<int, double> lengths, out string reason)
    {
        lengths = new Dictionary<int, double>();
@@ -198,15 +222,157 @@ internal sealed class LocalG2WindowPlanner

    private static IReadOnlyList<T> Empty<T>() => new ReadOnlyCollection<T>(new List<T>());

-    private readonly struct WindowRange
+    public static class TestHooks
    {
-        internal WindowRange(double startArcLengthMeters, double endArcLengthMeters)
+        public static WindowPlanningTestSnapshot Execute(string scenario)
        {
-            StartArcLengthMeters = startArcLengthMeters;
-            EndArcLengthMeters = endArcLengthMeters;
+            if (string.IsNullOrWhiteSpace(scenario))
+                throw new ArgumentException("A scenario is required.", nameof(scenario));
+
+            IReadOnlyList<CurvatureTransition> transitions;
+            double segmentLength;
+            switch (scenario)
+            {
+                case "SeparatedByOneMeter":
+                    transitions = new[]
+                    {
+                        Transition(0.2d, 0),
+                        Transition(1.2d, 1),
+                    };
+                    segmentLength = 1.4d;
+                    break;
+                case "Mergeable":
+                    transitions = new[]
+                    {
+                        Transition(0.4d, 0),
+                        Transition(0.7d, 1),
+                    };
+                    segmentLength = 1.4d;
+                    break;
+                case "ThreeEventPartition":
+                    transitions = new[]
+                    {
+                        Transition(0.2d, 0),
+                        Transition(0.6d, 1),
+                        Transition(1.2d, 2),
+                    };
+                    segmentLength = 1.4d;
+                    break;
+                case "NearBoundary":
+                    transitions = new[] { Transition(0.1d, 0) };
+                    segmentLength = 1d;
+                    break;
+                default:
+                    throw new ArgumentOutOfRangeException(nameof(scenario));
+            }
+
+            var planner = new LocalG2WindowPlanner();
+            if (!planner.TryPlan(
+                    CreatePreparedPath(segmentLength),
+                    transitions,
+                    new LocalG2OptionsSnapshot(new PathSmoothingConfiguration()),
+                    out IReadOnlyList<LocalG2SmoothingRegion> regions,
+                    out string reason))
+            {
+                throw new InvalidOperationException(reason);
+            }
+
+            double maximumLength = 0d;
+            bool exactEnvelope = true;
+            var counts = new List<string>(regions.Count);
+            var signature = new List<string>();
+            for (int regionIndex = 0; regionIndex < regions.Count; regionIndex++)
+            {
+                LocalG2SmoothingRegion region = regions[regionIndex];
+                counts.Add(region.Transitions.Count.ToString());
+                double minimumStart = double.PositiveInfinity;
+                double maximumEnd = double.NegativeInfinity;
+                for (int variantIndex = 0; variantIndex < region.WindowVariants.Count; variantIndex++)
+                {
+                    LocalG2WindowVariant variant = region.WindowVariants[variantIndex];
+                    maximumLength = Math.Max(
+                        maximumLength,
+                        variant.EndArcLengthMeters - variant.StartArcLengthMeters);
+                    minimumStart = Math.Min(minimumStart, variant.StartArcLengthMeters);
+                    maximumEnd = Math.Max(maximumEnd, variant.EndArcLengthMeters);
+                    signature.Add(
+                        region.SegmentIndex + ":" +
+                        variant.CandidateIndex + ":" +
+                        variant.StartArcLengthMeters.ToString("R") + ":" +
+                        variant.EndArcLengthMeters.ToString("R"));
+                }
+                exactEnvelope &= Math.Abs(region.MaximumStartArcLengthMeters - minimumStart) <= 1e-9d;
+                exactEnvelope &= Math.Abs(region.MaximumEndArcLengthMeters - maximumEnd) <= 1e-9d;
+            }
+
+            LocalG2WindowVariant first = regions[0].WindowVariants[0];
+            return new WindowPlanningTestSnapshot(
+                regions.Count,
+                string.Join(",", counts),
+                maximumLength,
+                exactEnvelope,
+                first.LeftWindowLengthMeters,
+                first.RightWindowLengthMeters,
+                string.Join("|", signature));
        }

-        internal double StartArcLengthMeters { get; }
-        internal double EndArcLengthMeters { get; }
+        public sealed class WindowPlanningTestSnapshot
+        {
+            internal WindowPlanningTestSnapshot(
+                int regionCount,
+                string transitionCounts,
+                double maximumWindowLength,
+                bool exactEnvelope,
+                double firstLeftLength,
+                double firstRightLength,
+                string signature)
+            {
+                RegionCount = regionCount;
+                TransitionCounts = transitionCounts;
+                MaximumWindowLength = maximumWindowLength;
+                ExactEnvelope = exactEnvelope;
+                FirstLeftLength = firstLeftLength;
+                FirstRightLength = firstRightLength;
+                Signature = signature;
+            }
+
+            public int RegionCount { get; }
+            public string TransitionCounts { get; }
+            public double MaximumWindowLength { get; }
+            public bool ExactEnvelope { get; }
+            public double FirstLeftLength { get; }
+            public double FirstRightLength { get; }
+            public string Signature { get; }
+        }
+
+        private static CurvatureTransition Transition(double arcLength, int index)
+        {
+            return new CurvatureTransition(
+                0,
+                index,
+                index + 1,
+                arcLength,
+                arcLength,
+                0d,
+                0d,
+                index % 2 == 0 ? 0d : 0.5d,
+                index % 2 == 0 ? 0.5d : 0d);
+        }
+
+        private static PreparedPath CreatePreparedPath(double length)
+        {
+            var points = new[]
+            {
+                new SmoothingPoint2D(
+                    0d, 0d, 0d, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
+                new SmoothingPoint2D(
+                    length, 0d, length, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
+            };
+            return new PreparedPath(new[]
+            {
+                new PreparedDirectionSegment(
+                    0, TravelDirection.Forward, points, false, false),
+            });
+        }
    }
 }
@@ -93,4 +93,37 @@ Assert-True (-not [string]::IsNullOrWhiteSpace([string]$overflowArguments[4])) `
    'Rejected overflow detection must provide a stable reason.'
 Assert-Equal 0 $overflowArguments[3].Count 'Rejected overflow detection must not publish a curvature event.'

+$plannerType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.LocalG2WindowPlanner'
+$plannerHooksType = $plannerType.GetNestedType('TestHooks', [Reflection.BindingFlags]'Public,NonPublic')
+Assert-True ($null -ne $plannerHooksType) 'LocalG2WindowPlanner must expose narrow deterministic TestHooks.'
+$planScenario = $plannerHooksType.GetMethod('Execute', [Reflection.BindingFlags]'Public,Static')
+
+$separated = $planScenario.Invoke($null, @('SeparatedByOneMeter'))
+Assert-Equal 2 $separated.RegionCount 'Events 1.0 m apart cannot share a 0.80 m total window.'
+Assert-Equal '1,1' $separated.TransitionCounts 'Separated events must remain one event per region.'
+
+$mergeable = $planScenario.Invoke($null, @('Mergeable'))
+Assert-Equal 1 $mergeable.RegionCount 'Events covered by one legal total window must merge.'
+Assert-Equal '2' $mergeable.TransitionCounts 'The merged region must retain both events.'
+
+$partition = $planScenario.Invoke($null, @('ThreeEventPartition'))
+Assert-Equal 2 $partition.RegionCount 'Three events must split at the first infeasible joint window.'
+Assert-Equal '2,1' $partition.TransitionCounts 'Only a feasible consecutive subgroup may merge.'
+
+$boundary = $planScenario.Invoke($null, @('NearBoundary'))
+Assert-True ($boundary.FirstRightLength -gt $boundary.FirstLeftLength) `
+    'A boundary-clamped total window must transfer missing length to the available side.'
+Assert-True ($boundary.MaximumWindowLength -le 0.80 + 1e-9) `
+    'No candidate window may exceed 0.80 m total length.'
+
+foreach ($snapshot in @($separated, $mergeable, $partition, $boundary)) {
+    Assert-True $snapshot.ExactEnvelope 'Region envelope must equal the extrema of actual legal variants.'
+    Assert-True ($snapshot.MaximumWindowLength -le 0.80 + 1e-9) `
+        'MaximumWindowLengthMeters is a total, not a per-side length.'
+}
+
+$repeat = $planScenario.Invoke($null, @('ThreeEventPartition'))
+Assert-Equal $partition.Signature $repeat.Signature `
+    'Repeated planning must preserve grouping, candidate numbering and variant order.'
+
 Write-Output 'Path smoothing Local G2 detection checks passed.'