diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransition.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransition.cs new file mode 100644 index 0000000..8cb2bb6 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransition.cs @@ -0,0 +1,51 @@ +using System; +using MultiWheelC.TrajectoryPlanning.Utils; + +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2; + +/// 同一方向原语边界两侧车辆曲率的离散跳变。 +internal sealed class CurvatureTransition +{ + internal CurvatureTransition( + int segmentIndex, + int leftCoarsePathIndex, + int rightCoarsePathIndex, + double localArcLengthMeters, + double x, + double y, + double vehicleHeadingRadians, + double leftVehicleCurvaturePerMeter, + double rightVehicleCurvaturePerMeter) + { + if (segmentIndex < 0 || leftCoarsePathIndex < 0 || rightCoarsePathIndex != leftCoarsePathIndex + 1) + throw new ArgumentOutOfRangeException(nameof(leftCoarsePathIndex)); + if (!NumericGuard.IsFinite(localArcLengthMeters) || localArcLengthMeters < 0d || + !NumericGuard.IsFinite(x) || !NumericGuard.IsFinite(y) || !NumericGuard.IsFinite(vehicleHeadingRadians) || + !NumericGuard.IsFinite(leftVehicleCurvaturePerMeter) || !NumericGuard.IsFinite(rightVehicleCurvaturePerMeter)) + { + throw new ArgumentOutOfRangeException(nameof(localArcLengthMeters)); + } + + SegmentIndex = segmentIndex; + LeftCoarsePathIndex = leftCoarsePathIndex; + RightCoarsePathIndex = rightCoarsePathIndex; + LocalArcLengthMeters = localArcLengthMeters; + X = x; + Y = y; + VehicleHeadingRadians = vehicleHeadingRadians; + LeftVehicleCurvaturePerMeter = leftVehicleCurvaturePerMeter; + RightVehicleCurvaturePerMeter = rightVehicleCurvaturePerMeter; + CurvatureJumpPerMeter = Math.Abs(rightVehicleCurvaturePerMeter - leftVehicleCurvaturePerMeter); + } + + internal int SegmentIndex { get; } + internal int LeftCoarsePathIndex { get; } + internal int RightCoarsePathIndex { get; } + internal double LocalArcLengthMeters { get; } + internal double X { get; } + internal double Y { get; } + internal double VehicleHeadingRadians { get; } + internal double LeftVehicleCurvaturePerMeter { get; } + internal double RightVehicleCurvaturePerMeter { get; } + internal double CurvatureJumpPerMeter { get; } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransitionDetector.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransitionDetector.cs new file mode 100644 index 0000000..b2a10ef --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransitionDetector.cs @@ -0,0 +1,255 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; +using MultiWheelC.TrajectoryPlanning.CoarsePath; +using MultiWheelC.TrajectoryPlanning.Utils; + +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2; + +/// 从原始粗路径的同向相邻点中识别恒曲率原语边界。 +internal sealed class CurvatureTransitionDetector +{ + internal bool TryDetect( + PathSmoothingRequest request, + double maximumVehicleCurvaturePerMeter, + LocalG2OptionsSnapshot options, + out IReadOnlyList transitions, + out string reason) + { + transitions = Empty(); + reason = string.Empty; + if (request == null || request.CoarsePath == null || request.Segments == null || options == null || + !NumericGuard.IsPositiveFinite(maximumVehicleCurvaturePerMeter)) + { + reason = "局部 G2 曲率事件检测输入无效。"; + return false; + } + if (!ValidatePath(request.CoarsePath, out reason) || !ValidateSegments(request.CoarsePath, request.Segments, out reason)) + return false; + + double threshold = Math.Max( + options.AbsoluteCurvatureJumpFloorPerMeter, + options.CurvatureJumpRatioOfMaximum * maximumVehicleCurvaturePerMeter); + if (!NumericGuard.IsPositiveFinite(threshold)) + { + reason = "局部 G2 曲率事件阈值无效。"; + return false; + } + + var detected = new List(); + for (int segmentPosition = 0; segmentPosition < request.Segments.Count; segmentPosition++) + { + PathSegment segment = request.Segments[segmentPosition]; + double segmentStartArc = request.CoarsePath[segment.StartIndex].ArcLength; + for (int leftIndex = segment.StartIndex; leftIndex < segment.EndIndex; leftIndex++) + { + CoarsePathPoint left = request.CoarsePath[leftIndex]; + CoarsePathPoint right = request.CoarsePath[leftIndex + 1]; + double delta = right.VehicleCurvature - left.VehicleCurvature; + if (Math.Abs(delta) >= threshold) + { + detected.Add(new CurvatureTransition( + segment.SegmentIndex, + leftIndex, + leftIndex + 1, + left.ArcLength - segmentStartArc, + left.X, + left.Y, + left.Heading, + left.VehicleCurvature, + right.VehicleCurvature)); + } + } + } + transitions = new ReadOnlyCollection(detected); + return true; + } + + /// 反射脚本使用的确定性检测与窗口规划接缝。 + public static class TestHooks + { + public static DetectionTestSnapshot Execute(string scenario) + { + if (string.IsNullOrWhiteSpace(scenario)) throw new ArgumentException("A scenario is required.", nameof(scenario)); + switch (scenario) + { + case "SingleTransition": return DetectSingleTransition(); + case "GearSwitch": return DetectGearSwitch(); + case "Noise": return DetectNoise(); + case "Overlap": return PlanOverlap(); + case "NearStart": return PlanNearStart(); + default: throw new ArgumentOutOfRangeException(nameof(scenario)); + } + } + + public sealed class DetectionTestSnapshot + { + internal DetectionTestSnapshot(int transitionCount, double maximumJump, int regionCount, + int transitionCountInFirstRegion, double startArcLength, double leftWindowLength, double rightWindowLength) + { + TransitionCount = transitionCount; + MaximumJump = maximumJump; + RegionCount = regionCount; + TransitionCountInFirstRegion = transitionCountInFirstRegion; + StartArcLength = startArcLength; + LeftWindowLength = leftWindowLength; + RightWindowLength = rightWindowLength; + } + + public int TransitionCount { get; } + public double MaximumJump { get; } + public int RegionCount { get; } + public int TransitionCountInFirstRegion { get; } + public double StartArcLength { get; } + public double LeftWindowLength { get; } + public double RightWindowLength { get; } + } + + private static DetectionTestSnapshot DetectSingleTransition() + { + IReadOnlyList transitions = Detect(CreateRequest( + new[] { Point(0d, 0d), Point(0.1d, 0.4167d), Point(0.2d, 0.4167d) }, + new[] { new PathSegment(0, TravelDirection.Forward, 0, 2, false, false) })); + return Snapshot(transitions); + } + + private static DetectionTestSnapshot DetectGearSwitch() + { + IReadOnlyList transitions = Detect(CreateRequest( + new[] + { + Point(0d, 0d, TravelDirection.Forward), Point(0.2d, 0d, TravelDirection.Forward), + Point(0.2d, 0.4167d, TravelDirection.Reverse, true), Point(0.4d, 0.4167d, TravelDirection.Reverse), + }, + new[] + { + new PathSegment(0, TravelDirection.Forward, 0, 1, false, true), + new PathSegment(1, TravelDirection.Reverse, 2, 3, true, false), + })); + return Snapshot(transitions); + } + + private static DetectionTestSnapshot DetectNoise() + { + IReadOnlyList transitions = Detect(CreateRequest( + new[] { Point(0d, 0d), Point(0.1d, 0.01d), Point(0.2d, 0.01d) }, + new[] { new PathSegment(0, TravelDirection.Forward, 0, 2, false, false) })); + return Snapshot(transitions); + } + + private static DetectionTestSnapshot PlanOverlap() + { + LocalG2OptionsSnapshot options = CreateOptions(); + var transitions = new[] + { + new CurvatureTransition(0, 1, 2, 0.4d, 0.4d, 0d, 0d, 0d, 0.5d), + new CurvatureTransition(0, 2, 3, 0.7d, 0.7d, 0d, 0d, 0.5d, 0d), + }; + var planner = new LocalG2WindowPlanner(); + if (!planner.TryPlan(CreatePreparedPath(1.4d), transitions, options, out IReadOnlyList regions, out string reason)) + throw new InvalidOperationException(reason); + return new DetectionTestSnapshot(2, 0.5d, regions.Count, regions[0].Transitions.Count, 0d, 0d, 0d); + } + + private static DetectionTestSnapshot PlanNearStart() + { + LocalG2OptionsSnapshot options = CreateOptions(); + var transitions = new[] { new CurvatureTransition(0, 0, 1, 0.1d, 0.1d, 0d, 0d, 0d, 0.5d) }; + var planner = new LocalG2WindowPlanner(); + if (!planner.TryPlan(CreatePreparedPath(1d), transitions, options, out IReadOnlyList regions, out string reason)) + throw new InvalidOperationException(reason); + LocalG2WindowVariant first = regions[0].WindowVariants[0]; + return new DetectionTestSnapshot(1, 0.5d, regions.Count, 1, + first.StartArcLengthMeters, first.LeftWindowLengthMeters, first.RightWindowLengthMeters); + } + + private static IReadOnlyList Detect(PathSmoothingRequest request) + { + var detector = new CurvatureTransitionDetector(); + if (!detector.TryDetect(request, 0.8333d, CreateOptions(), out IReadOnlyList transitions, out string reason)) + throw new InvalidOperationException(reason); + return transitions; + } + + private static DetectionTestSnapshot Snapshot(IReadOnlyList transitions) + { + double maximum = 0d; + for (int index = 0; index < transitions.Count; index++) maximum = Math.Max(maximum, transitions[index].CurvatureJumpPerMeter); + return new DetectionTestSnapshot(transitions.Count, maximum, 0, 0, 0d, 0d, 0d); + } + + private static PathSmoothingRequest CreateRequest(IReadOnlyList points, IReadOnlyList segments) + { + return new PathSmoothingRequest(points, segments, null, null, new PathSmoothingConfiguration()); + } + + private static CoarsePathPoint Point(double arcLength, double curvature, TravelDirection direction = TravelDirection.Forward, bool gearSwitch = false) + { + return new CoarsePathPoint(arcLength, 0d, 0d, 0d, arcLength, direction, curvature, 1d, gearSwitch, + CoarsePathPointSource.MotionPrimitive); + } + + private static LocalG2OptionsSnapshot CreateOptions() => new LocalG2OptionsSnapshot(new PathSmoothingConfiguration()); + + private static Processing.PreparedPath CreatePreparedPath(double length) + { + var points = new[] + { + new Processing.SmoothingPoint2D(0d, 0d, 0d, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor), + new Processing.SmoothingPoint2D(length, 0d, length, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor), + }; + var segment = new Processing.PreparedDirectionSegment(0, TravelDirection.Forward, points, false, false); + return new Processing.PreparedPath(new[] { segment }); + } + } + + private static bool ValidatePath(IReadOnlyList path, out string reason) + { + reason = string.Empty; + if (path.Count == 0) + { + reason = "局部 G2 曲率事件检测需要粗路径点。"; + return false; + } + double previousArc = -1d; + for (int index = 0; index < path.Count; index++) + { + CoarsePathPoint point = path[index]; + if (point == null || !NumericGuard.IsFinite(point.X) || !NumericGuard.IsFinite(point.Y) || + !NumericGuard.IsFinite(point.Heading) || !NumericGuard.IsFinite(point.ArcLength) || point.ArcLength < 0d || + !NumericGuard.IsFinite(point.VehicleCurvature) || point.ArcLength < previousArc) + { + reason = "局部 G2 曲率事件检测要求有限且非递减的粗路径。"; + return false; + } + previousArc = point.ArcLength; + } + return true; + } + + private static bool ValidateSegments(IReadOnlyList path, IReadOnlyList segments, out string reason) + { + reason = string.Empty; + for (int position = 0; position < segments.Count; position++) + { + PathSegment segment = segments[position]; + if (segment == null || segment.SegmentIndex != position || segment.StartIndex < 0 || + segment.EndIndex < segment.StartIndex || segment.EndIndex >= path.Count) + { + reason = "局部 G2 曲率事件检测的方向分段无效。"; + return false; + } + for (int index = segment.StartIndex; index <= segment.EndIndex; index++) + { + if (path[index].Direction != segment.Direction) + { + reason = "局部 G2 曲率事件检测的方向分段包含换向点。"; + return false; + } + } + } + return true; + } + + private static IReadOnlyList Empty() => new ReadOnlyCollection(new List()); +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2OptionsSnapshot.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2OptionsSnapshot.cs new file mode 100644 index 0000000..3fddd10 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2OptionsSnapshot.cs @@ -0,0 +1,57 @@ +using System; +using MultiWheelC.TrajectoryPlanning.Utils; + +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2; + +/// 局部 G2 预平滑一次运行使用的已校验不可变选项。 +internal sealed class LocalG2OptionsSnapshot +{ + internal LocalG2OptionsSnapshot(PathSmoothingConfiguration configuration) + { + if (configuration == null) throw new ArgumentNullException(nameof(configuration)); + LocalG2QuinticOptions source = configuration.LocalG2Quintic; + if (source == null) throw new ArgumentOutOfRangeException(nameof(configuration)); + + MinimumWindowLengthMeters = source.MinimumWindowLengthMeters; + PreferredWindowLengthMeters = source.PreferredWindowLengthMeters; + MaximumWindowLengthMeters = source.MaximumWindowLengthMeters; + MaximumDeviationMeters = source.MaximumDeviationMeters; + AbsoluteCurvatureJumpFloorPerMeter = source.AbsoluteCurvatureJumpFloorPerMeter; + CurvatureJumpRatioOfMaximum = source.CurvatureJumpRatioOfMaximum; + MinimumPeakGradientImprovementRatio = source.MinimumPeakGradientImprovementRatio; + MaximumVariationCostRegressionRatio = source.MaximumVariationCostRegressionRatio; + MaximumCandidatesPerRegion = source.MaximumCandidatesPerRegion; + + if (!NumericGuard.IsPositiveFinite(MinimumWindowLengthMeters)) + throw new ArgumentOutOfRangeException(nameof(MinimumWindowLengthMeters)); + if (!NumericGuard.IsFinite(PreferredWindowLengthMeters) || PreferredWindowLengthMeters < MinimumWindowLengthMeters) + throw new ArgumentOutOfRangeException(nameof(PreferredWindowLengthMeters)); + if (!NumericGuard.IsFinite(MaximumWindowLengthMeters) || MaximumWindowLengthMeters < PreferredWindowLengthMeters) + throw new ArgumentOutOfRangeException(nameof(MaximumWindowLengthMeters)); + if (!NumericGuard.IsPositiveFinite(MaximumDeviationMeters)) + throw new ArgumentOutOfRangeException(nameof(MaximumDeviationMeters)); + if (!NumericGuard.IsPositiveFinite(AbsoluteCurvatureJumpFloorPerMeter)) + throw new ArgumentOutOfRangeException(nameof(AbsoluteCurvatureJumpFloorPerMeter)); + if (!NumericGuard.IsFinite(CurvatureJumpRatioOfMaximum) || CurvatureJumpRatioOfMaximum <= 0d || CurvatureJumpRatioOfMaximum > 1d) + throw new ArgumentOutOfRangeException(nameof(CurvatureJumpRatioOfMaximum)); + if (!NumericGuard.IsFinite(MinimumPeakGradientImprovementRatio) || + MinimumPeakGradientImprovementRatio <= 0d || MinimumPeakGradientImprovementRatio >= 1d) + { + throw new ArgumentOutOfRangeException(nameof(MinimumPeakGradientImprovementRatio)); + } + if (!NumericGuard.IsFinite(MaximumVariationCostRegressionRatio) || MaximumVariationCostRegressionRatio < 0d) + throw new ArgumentOutOfRangeException(nameof(MaximumVariationCostRegressionRatio)); + if (MaximumCandidatesPerRegion < 1) + throw new ArgumentOutOfRangeException(nameof(MaximumCandidatesPerRegion)); + } + + internal double MinimumWindowLengthMeters { get; } + internal double PreferredWindowLengthMeters { get; } + internal double MaximumWindowLengthMeters { get; } + internal double MaximumDeviationMeters { get; } + internal double AbsoluteCurvatureJumpFloorPerMeter { get; } + internal double CurvatureJumpRatioOfMaximum { get; } + internal double MinimumPeakGradientImprovementRatio { get; } + internal double MaximumVariationCostRegressionRatio { get; } + internal int MaximumCandidatesPerRegion { get; } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2SmoothingRegion.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2SmoothingRegion.cs new file mode 100644 index 0000000..710da97 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2SmoothingRegion.cs @@ -0,0 +1,67 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; + +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2; + +/// 一个局部 G2 候选可替换的弧长窗口。 +internal sealed class LocalG2WindowVariant +{ + internal LocalG2WindowVariant(int candidateIndex, double startArcLengthMeters, double endArcLengthMeters, + double leftWindowLengthMeters, double rightWindowLengthMeters) + { + if (candidateIndex < 0 || startArcLengthMeters < 0d || endArcLengthMeters < startArcLengthMeters || + leftWindowLengthMeters < 0d || rightWindowLengthMeters < 0d) + { + throw new ArgumentOutOfRangeException(nameof(candidateIndex)); + } + CandidateIndex = candidateIndex; + StartArcLengthMeters = startArcLengthMeters; + EndArcLengthMeters = endArcLengthMeters; + LeftWindowLengthMeters = leftWindowLengthMeters; + RightWindowLengthMeters = rightWindowLengthMeters; + } + + internal int CandidateIndex { get; } + internal double StartArcLengthMeters { get; } + internal double EndArcLengthMeters { get; } + internal double LeftWindowLengthMeters { get; } + internal double RightWindowLengthMeters { get; } +} + +/// 因最大合法窗口相交而合并的一组曲率事件。 +internal sealed class LocalG2SmoothingRegion +{ + internal LocalG2SmoothingRegion( + int segmentIndex, + IReadOnlyList transitions, + double maximumStartArcLengthMeters, + double maximumEndArcLengthMeters, + IReadOnlyList windowVariants) + { + if (segmentIndex < 0 || transitions == null || transitions.Count == 0 || + maximumStartArcLengthMeters < 0d || maximumEndArcLengthMeters < maximumStartArcLengthMeters || + windowVariants == null) + { + throw new ArgumentOutOfRangeException(nameof(transitions)); + } + SegmentIndex = segmentIndex; + Transitions = Copy(transitions); + MaximumStartArcLengthMeters = maximumStartArcLengthMeters; + MaximumEndArcLengthMeters = maximumEndArcLengthMeters; + WindowVariants = Copy(windowVariants); + } + + internal int SegmentIndex { get; } + internal IReadOnlyList Transitions { get; } + internal double MaximumStartArcLengthMeters { get; } + internal double MaximumEndArcLengthMeters { get; } + internal IReadOnlyList WindowVariants { get; } + + private static IReadOnlyList Copy(IReadOnlyList source) + { + var copy = new List(source.Count); + for (int index = 0; index < source.Count; index++) copy.Add(source[index]); + return new ReadOnlyCollection(copy); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2WindowPlanner.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2WindowPlanner.cs new file mode 100644 index 0000000..b3e9694 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2WindowPlanner.cs @@ -0,0 +1,212 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; +using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing; +using MultiWheelC.TrajectoryPlanning.Utils; + +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2; + +/// 按硬方向边界生成并合并局部 G2 曲率事件的候选窗口。 +internal sealed class LocalG2WindowPlanner +{ + private const double MergeToleranceMeters = 1e-9d; + + internal bool TryPlan( + PreparedPath originalPath, + IReadOnlyList transitions, + LocalG2OptionsSnapshot options, + out IReadOnlyList regions, + out string reason) + { + regions = Empty(); + reason = string.Empty; + if (originalPath == null || transitions == null || options == null) + { + reason = "局部 G2 窗口规划输入无效。"; + return false; + } + if (!TryGetSegmentLengths(originalPath, out Dictionary segmentLengths, out reason)) return false; + + var ordered = new List(transitions.Count); + for (int index = 0; index < transitions.Count; index++) + { + CurvatureTransition transition = transitions[index]; + if (transition == null || !segmentLengths.TryGetValue(transition.SegmentIndex, out double length) || + !NumericGuard.IsFinite(transition.LocalArcLengthMeters) || transition.LocalArcLengthMeters < 0d || + transition.LocalArcLengthMeters > length + MergeToleranceMeters) + { + reason = "局部 G2 曲率事件不属于有效方向分段。"; + return false; + } + ordered.Add(transition); + } + ordered.Sort(CompareTransitions); + + var planned = new List(); + int cursor = 0; + while (cursor < ordered.Count) + { + CurvatureTransition first = ordered[cursor]; + double segmentLength = segmentLengths[first.SegmentIndex]; + WindowRange merged = MaximumLegalRange(first.LocalArcLengthMeters, segmentLength, options.MaximumWindowLengthMeters); + var group = new List { first }; + cursor++; + + 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)); + cursor++; + } + + planned.Add(new LocalG2SmoothingRegion( + first.SegmentIndex, + group, + merged.StartArcLengthMeters, + merged.EndArcLengthMeters, + BuildVariants(group, segmentLength, options))); + } + regions = new ReadOnlyCollection(planned); + return true; + } + + private static IReadOnlyList BuildVariants( + IReadOnlyList transitions, + double segmentLength, + LocalG2OptionsSnapshot options) + { + var variants = new List(); + double firstEvent = transitions[0].LocalArcLengthMeters; + double lastEvent = transitions[transitions.Count - 1].LocalArcLengthMeters; + double anchor = (firstEvent + lastEvent) / 2d; + 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); + } + return new ReadOnlyCollection(variants); + } + + private static void AddIfLegal(List variants, double target, double leftRatio, + double anchor, double firstEvent, double lastEvent, double segmentLength, bool permitBoundaryShift, int maximumCount) + { + if (variants.Count >= maximumCount) return; + double left = target * leftRatio; + double right = target - left; + double availableLeft = anchor; + double availableRight = segmentLength - anchor; + if (permitBoundaryShift) + { + left = Math.Min(left, availableLeft); + right = Math.Min(right, availableRight); + double missing = target - left - right; + double addRight = Math.Min(missing, availableRight - right); + right += addRight; + left += Math.Min(missing - addRight, availableLeft - left); + } + else if (left > availableLeft + MergeToleranceMeters || right > availableRight + MergeToleranceMeters) + { + return; + } + + double start = anchor - left; + double end = anchor + right; + if (start > firstEvent + MergeToleranceMeters || end + MergeToleranceMeters < lastEvent || end - start + MergeToleranceMeters < target) + return; + variants.Add(new LocalG2WindowVariant(variants.Count, start, end, left, right)); + } + + private static IReadOnlyList BuildTargets(LocalG2OptionsSnapshot options, double segmentLength) + { + double[] requested = + { + options.PreferredWindowLengthMeters, + 0.75d * options.PreferredWindowLengthMeters, + 1.25d * options.PreferredWindowLengthMeters, + options.MinimumWindowLengthMeters, + options.MaximumWindowLengthMeters, + }; + var targets = new List(requested.Length); + for (int index = 0; index < requested.Length; index++) + { + double target = Math.Min(segmentLength, + Math.Max(options.MinimumWindowLengthMeters, Math.Min(options.MaximumWindowLengthMeters, requested[index]))); + bool duplicate = false; + for (int prior = 0; prior < targets.Count; prior++) + { + if (Math.Abs(targets[prior] - target) <= MergeToleranceMeters) + { + duplicate = true; + break; + } + } + if (!duplicate) targets.Add(target); + } + 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 lengths, out string reason) + { + lengths = new Dictionary(); + reason = string.Empty; + for (int position = 0; position < originalPath.Segments.Count; position++) + { + PreparedDirectionSegment segment = originalPath.Segments[position]; + if (segment == null || segment.SegmentIndex != position || segment.Points == null || segment.Points.Count == 0) + { + reason = "局部 G2 窗口规划的预处理方向分段无效。"; + return false; + } + double previousArc = -1d; + for (int pointIndex = 0; pointIndex < segment.Points.Count; pointIndex++) + { + double arc = segment.Points[pointIndex].ArcLength; + if (!NumericGuard.IsFinite(arc) || arc < 0d || arc < previousArc) + { + reason = "局部 G2 窗口规划要求分段弧长有限且非递减。"; + return false; + } + previousArc = arc; + } + lengths.Add(segment.SegmentIndex, previousArc); + } + return true; + } + + private static int CompareTransitions(CurvatureTransition left, CurvatureTransition right) + { + int segment = left.SegmentIndex.CompareTo(right.SegmentIndex); + if (segment != 0) return segment; + int arc = left.LocalArcLengthMeters.CompareTo(right.LocalArcLengthMeters); + return arc != 0 ? arc : left.LeftCoarsePathIndex.CompareTo(right.LeftCoarsePathIndex); + } + + private static IReadOnlyList Empty() => new ReadOnlyCollection(new List()); + + private readonly struct WindowRange + { + internal WindowRange(double startArcLengthMeters, double endArcLengthMeters) + { + StartArcLengthMeters = startArcLengthMeters; + EndArcLengthMeters = endArcLengthMeters; + } + + internal double StartArcLengthMeters { get; } + internal double EndArcLengthMeters { get; } + } +} diff --git a/ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1 b/ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1 new file mode 100644 index 0000000..d35eff4 --- /dev/null +++ b/ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1 @@ -0,0 +1,58 @@ +param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll')) + +$ErrorActionPreference = 'Stop' +$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath)) + +function Assert-True($Actual, [string]$Message) { + if (-not $Actual) { throw $Message } +} + +function Assert-Equal($Expected, $Actual, [string]$Message) { + if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" } +} + +function Assert-Near([double]$Expected, [double]$Actual, [double]$Tolerance, [string]$Message) { + if ([Math]::Abs($Expected - $Actual) -gt $Tolerance) { + throw "$Message Expected=$Expected Actual=$Actual Tolerance=$Tolerance" + } +} + +function Get-RequiredType([string]$Name) { + return $assembly.GetType($Name, $true) +} + +$detectorType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.CurvatureTransitionDetector' +$hooksType = $detectorType.GetNestedType('TestHooks', [Reflection.BindingFlags]'Public,NonPublic') +Assert-True ($null -ne $hooksType) 'CurvatureTransitionDetector must expose its narrowly scoped nested TestHooks helper.' +$executeMethod = $hooksType.GetMethod('Execute', [Reflection.BindingFlags]'Public,Static') +Assert-True ($null -ne $executeMethod) 'TestHooks must expose deterministic scenario execution for reflection tests.' + +function Invoke-Scenario([string]$Scenario) { + return $executeMethod.Invoke($null, @($Scenario)) +} + +# Same direction: 0 -> 0.4167 is detected once. +$singleTransition = Invoke-Scenario 'SingleTransition' +Assert-Equal 1 $singleTransition.TransitionCount 'One primitive curvature jump must be detected.' +Assert-Near 0.4167 $singleTransition.MaximumJump 0.0001 'The jump magnitude must be retained.' + +# Same pose at a forward/reverse boundary: no event crosses the stop. +$gearSwitch = Invoke-Scenario 'GearSwitch' +Assert-Equal 0 $gearSwitch.TransitionCount 'A stopped gear switch must not be a smoothing event.' + +# A 0.01 1/m numerical change is below max(0.001, 5% of 0.8333). +$noise = Invoke-Scenario 'Noise' +Assert-Equal 0 $noise.TransitionCount 'Sub-threshold curvature noise must be ignored.' + +# Two 0.50 m windows whose ranges overlap are merged. +$overlap = Invoke-Scenario 'Overlap' +Assert-Equal 1 $overlap.RegionCount 'Overlapping windows must form one joint region.' +Assert-Equal 2 $overlap.TransitionCountInFirstRegion 'The merged region must retain both events.' + +# Near a segment start, the window becomes asymmetric without crossing the hard boundary. +$nearStart = Invoke-Scenario 'NearStart' +Assert-Near 0.0 $nearStart.StartArcLength 0.000000001 'A start window must be clamped to the segment.' +Assert-True ($nearStart.RightWindowLength -gt $nearStart.LeftWindowLength) ` + 'Unavailable left length must be shifted to the right.' + +Write-Output 'Path smoothing Local G2 detection checks passed.'