diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2WindowPlanner.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2WindowPlanner.cs index 26143e2..6b351e0 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2WindowPlanner.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2WindowPlanner.cs @@ -106,12 +106,25 @@ internal sealed class LocalG2WindowPlanner double firstEvent = transitions[0].LocalArcLengthMeters; double lastEvent = transitions[transitions.Count - 1].LocalArcLengthMeters; double anchor = (firstEvent + lastEvent) / 2d; - foreach (double target in BuildTargets(options, segmentLength)) + IReadOnlyList targets = BuildTargets(options, segmentLength); + double[] ratios = { 0.5d, 0.4d, 0.6d }; + for (int ratioIndex = 0; ratioIndex < ratios.Length; ratioIndex++) { - if (variants.Count >= options.MaximumCandidatesPerRegion) break; - 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); + for (int targetIndex = 0; targetIndex < targets.Count; targetIndex++) + { + if (variants.Count >= options.MaximumCandidatesPerRegion) + return new ReadOnlyCollection(variants); + AddIfLegal( + variants, + targets[targetIndex], + ratios[ratioIndex], + anchor, + firstEvent, + lastEvent, + segmentLength, + ratioIndex == 0, + options); + } } return new ReadOnlyCollection(variants); } @@ -160,10 +173,10 @@ internal sealed class LocalG2WindowPlanner double[] requested = { options.PreferredWindowLengthMeters, - 0.75d * options.PreferredWindowLengthMeters, - 1.25d * options.PreferredWindowLengthMeters, options.MinimumWindowLengthMeters, options.MaximumWindowLengthMeters, + 0.75d * options.PreferredWindowLengthMeters, + 1.25d * options.PreferredWindowLengthMeters, }; var targets = new List(requested.Length); for (int index = 0; index < requested.Length; index++) @@ -262,6 +275,10 @@ internal sealed class LocalG2WindowPlanner transitions = new[] { Transition(0.1d, 0) }; segmentLength = 1d; break; + case "InteriorCoverage": + transitions = new[] { Transition(1d, 0) }; + segmentLength = 2d; + break; default: throw new ArgumentOutOfRangeException(nameof(scenario)); } @@ -305,7 +322,25 @@ internal sealed class LocalG2WindowPlanner exactEnvelope &= Math.Abs(region.MaximumEndArcLengthMeters - maximumEnd) <= 1e-9d; } - LocalG2WindowVariant first = regions[0].WindowVariants[0]; + IReadOnlyList firstVariants = regions[0].WindowVariants; + bool representativeTargetsFirst = + firstVariants.Count >= 3 && + Math.Abs(WindowLength(firstVariants[0]) - 0.50d) <= 1e-9d && + Math.Abs(WindowLength(firstVariants[1]) - 0.20d) <= 1e-9d && + Math.Abs(WindowLength(firstVariants[2]) - 0.80d) <= 1e-9d; + bool hasAsymmetricVariant = false; + for (int index = 0; index < firstVariants.Count; index++) + { + if (Math.Abs( + firstVariants[index].LeftWindowLengthMeters - + firstVariants[index].RightWindowLengthMeters) > 1e-9d) + { + hasAsymmetricVariant = true; + break; + } + } + + LocalG2WindowVariant first = firstVariants[0]; return new WindowPlanningTestSnapshot( regions.Count, string.Join(",", counts), @@ -313,7 +348,10 @@ internal sealed class LocalG2WindowPlanner exactEnvelope, first.LeftWindowLengthMeters, first.RightWindowLengthMeters, - string.Join("|", signature)); + string.Join("|", signature), + firstVariants.Count, + representativeTargetsFirst, + hasAsymmetricVariant); } public sealed class WindowPlanningTestSnapshot @@ -325,7 +363,10 @@ internal sealed class LocalG2WindowPlanner bool exactEnvelope, double firstLeftLength, double firstRightLength, - string signature) + string signature, + int firstRegionVariantCount, + bool representativeTargetsFirst, + bool hasAsymmetricVariant) { RegionCount = regionCount; TransitionCounts = transitionCounts; @@ -334,6 +375,9 @@ internal sealed class LocalG2WindowPlanner FirstLeftLength = firstLeftLength; FirstRightLength = firstRightLength; Signature = signature; + FirstRegionVariantCount = firstRegionVariantCount; + RepresentativeTargetsFirst = representativeTargetsFirst; + HasAsymmetricVariant = hasAsymmetricVariant; } public int RegionCount { get; } @@ -343,8 +387,14 @@ internal sealed class LocalG2WindowPlanner public double FirstLeftLength { get; } public double FirstRightLength { get; } public string Signature { get; } + public int FirstRegionVariantCount { get; } + public bool RepresentativeTargetsFirst { get; } + public bool HasAsymmetricVariant { get; } } + private static double WindowLength(LocalG2WindowVariant variant) => + variant.EndArcLengthMeters - variant.StartArcLengthMeters; + private static CurvatureTransition Transition(double arcLength, int index) { return new CurvatureTransition( diff --git a/ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1 b/ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1 index 7b54982..68142c9 100644 --- a/ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1 +++ b/ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1 @@ -116,7 +116,15 @@ Assert-True ($boundary.FirstRightLength -gt $boundary.FirstLeftLength) ` 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)) { +$coverage = $planScenario.Invoke($null, @('InteriorCoverage')) +Assert-True $coverage.RepresentativeTargetsFirst ` + 'Preferred, minimum, and maximum balanced targets must precede asymmetric variants.' +Assert-True $coverage.HasAsymmetricVariant ` + 'Default window planning must retain a legal asymmetric variant after balanced coverage.' +Assert-True ($coverage.FirstRegionVariantCount -le 12) ` + 'Window planning must obey the configured default cap.' + +foreach ($snapshot in @($separated, $mergeable, $partition, $boundary, $coverage)) { 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.'