feat: detect local curvature transition regions

2026-07-30 16:52:01 +08:00
parent a267a6210e
commit 0fc64f7693
6 changed files with 700 additions and 0 deletions
@@ -0,0 +1,67 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
+
+/// <summary>一个局部 G2 候选可替换的弧长窗口。</summary>
+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; }
+}
+
+/// <summary>因最大合法窗口相交而合并的一组曲率事件。</summary>
+internal sealed class LocalG2SmoothingRegion
+{
+    internal LocalG2SmoothingRegion(
+        int segmentIndex,
+        IReadOnlyList<CurvatureTransition> transitions,
+        double maximumStartArcLengthMeters,
+        double maximumEndArcLengthMeters,
+        IReadOnlyList<LocalG2WindowVariant> 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<CurvatureTransition> Transitions { get; }
+    internal double MaximumStartArcLengthMeters { get; }
+    internal double MaximumEndArcLengthMeters { get; }
+    internal IReadOnlyList<LocalG2WindowVariant> WindowVariants { get; }
+
+    private static IReadOnlyList<T> Copy<T>(IReadOnlyList<T> source)
+    {
+        var copy = new List<T>(source.Count);
+        for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
+        return new ReadOnlyCollection<T>(copy);
+    }
+}