diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs index 8597d96..edab501 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs +++ b/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(); + 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 workOrder, + out string orderReason)) + { + throw new InvalidOperationException(orderReason); + } + if (!orderer.TryCreate( + reportOrder, + out IReadOnlyList 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; diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2RegionWorkOrder.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2RegionWorkOrder.cs new file mode 100644 index 0000000..dde7845 --- /dev/null +++ b/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; + +/// 把稳定报告顺序转换为不会使后续原始局部弧长错位的工作顺序。 +internal sealed class LocalG2RegionWorkOrder +{ + internal bool TryCreate( + IReadOnlyList reportOrder, + out IReadOnlyList workOrder, + out string reason) + { + workOrder = Empty(); + reason = string.Empty; + if (reportOrder == null) + { + reason = "局部 G2 区域工作顺序输入无效。"; + return false; + } + + var indexed = new List(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(indexed.Count); + for (int index = 0; index < indexed.Count; index++) + ordered.Add(indexed[index].Region); + workOrder = new ReadOnlyCollection(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 Empty() + { + return new ReadOnlyCollection( + new List()); + } + + 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; } + } +} diff --git a/ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 b/ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 index 336630a..2f7625b 100644 --- a/ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 +++ b/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.'