chore: save current workspace progress

2026-08-09 22:13:18 +08:00
parent 650c2ab0e3
commit 2f4fd15e52
449 changed files with 76593 additions and 971 deletions
@@ -0,0 +1,70 @@
+using System;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.Mapping;
+
+/// <summary>以 m 表示的障碍物净距离保守下界。</summary>
+internal sealed class ObstacleDistanceField
+{
+    private readonly double[] _conservativeDistances;
+    private ObstacleDistanceField(double[] conservativeDistances) { _conservativeDistances = conservativeDistances; }
+    /// <summary>
+    /// 从占据栅格创建距离场。
+    ///
+    /// 参数：occupied 为行主序占据数组；rows、cols 为其尺寸；resolutionMeters 为格边长，单位 m。
+    /// 返回：每个格到最近障碍物的保守净距离下界，单位 m；全空地图中的每项为正无穷。
+    /// </summary>
+    public static ObstacleDistanceField Create(byte[] occupied, int rows, int cols, double resolutionMeters)
+    {
+        if (!TryCreate(occupied, rows, cols, resolutionMeters, PlanningOperationBudget.Unlimited(CancellationToken.None),
+            out ObstacleDistanceField field, out _))
+            throw new InvalidOperationException("Unbounded distance-field creation unexpectedly stopped.");
+        return field;
+    }
+
+    /// <summary>使用共享预算创建距离场；停止时不返回部分距离数据。</summary>
+    internal static bool TryCreate(byte[] occupied, int rows, int cols, double resolutionMeters,
+        PlanningOperationBudget budget, out ObstacleDistanceField field, out PlanningOperationStopReason stopReason)
+    {
+        if (occupied == null) throw new ArgumentNullException(nameof(occupied));
+        if (budget == null) throw new ArgumentNullException(nameof(budget));
+        field = null;
+        stopReason = budget.GetStopReason();
+        if (stopReason != PlanningOperationStopReason.None) return false;
+        bool hasObstacle = false;
+        int workItemCount = 0;
+        for (int i = 0; i < occupied.Length; i++)
+        {
+            stopReason = budget.CheckEvery(ref workItemCount);
+            if (stopReason != PlanningOperationStopReason.None) return false;
+            if (occupied[i] != 0) { hasObstacle = true; break; }
+        }
+        var distances = new double[occupied.Length];
+        if (!hasObstacle)
+        {
+            for (int i = 0; i < distances.Length; i++)
+            {
+                stopReason = budget.CheckEvery(ref workItemCount);
+                if (stopReason != PlanningOperationStopReason.None) return false;
+                distances[i] = double.PositiveInfinity;
+            }
+            field = new ObstacleDistanceField(distances);
+            stopReason = PlanningOperationStopReason.None;
+            return true;
+        }
+        if (!EuclideanDistanceTransform.TryComputeSquaredDistances(occupied, rows, cols, budget, out double[] squared, out stopReason))
+            return false;
+        double conservativeOffset = Math.Sqrt(2d) * resolutionMeters;
+        for (int i = 0; i < distances.Length; i++)
+        {
+            stopReason = budget.CheckEvery(ref workItemCount);
+            if (stopReason != PlanningOperationStopReason.None) return false;
+            distances[i] = Math.Max(0d, Math.Sqrt(squared[i]) * resolutionMeters - conservativeOffset);
+        }
+        field = new ObstacleDistanceField(distances);
+        stopReason = PlanningOperationStopReason.None;
+        return true;
+    }
+    internal double[] CopyDistances() { return (double[])_conservativeDistances.Clone(); }
+}