fix: order Local G2 regions back to front

2026-07-31 16:01:37 +08:00
parent 1ac3dbda8d
commit e8c4704d87
3 changed files with 366 additions and 1 deletions
@@ -0,0 +1,107 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
+
+/// <summary>把稳定报告顺序转换为不会使后续原始局部弧长错位的工作顺序。</summary>
+internal sealed class LocalG2RegionWorkOrder
+{
+    internal bool TryCreate(
+        IReadOnlyList<LocalG2SmoothingRegion> reportOrder,
+        out IReadOnlyList<LocalG2SmoothingRegion> workOrder,
+        out string reason)
+    {
+        workOrder = Empty();
+        reason = string.Empty;
+        if (reportOrder == null)
+        {
+            reason = "局部 G2 区域工作顺序输入无效。";
+            return false;
+        }
+
+        var indexed = new List<IndexedRegion>(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<LocalG2SmoothingRegion>(indexed.Count);
+        for (int index = 0; index < indexed.Count; index++)
+            ordered.Add(indexed[index].Region);
+        workOrder = new ReadOnlyCollection<LocalG2SmoothingRegion>(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<LocalG2SmoothingRegion> Empty()
+    {
+        return new ReadOnlyCollection<LocalG2SmoothingRegion>(
+            new List<LocalG2SmoothingRegion>());
+    }
+
+    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; }
+    }
+}