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.'