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,34 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.Utils;
+
+/// <summary>Deterministic angle helpers used by planning code.</summary>
+public static class AngleMath
+{
+    public static double NormalizeRadians(double radians)
+    {
+        if (!NumericGuard.IsFinite(radians))
+            return double.NaN;
+
+        double normalized = radians % (2d * Math.PI);
+        if (normalized >= Math.PI) normalized -= 2d * Math.PI;
+        if (normalized < -Math.PI) normalized += 2d * Math.PI;
+        return normalized;
+    }
+
+    public static double ShortestSignedDifference(double from, double to)
+    {
+        return NormalizeRadians(to - from);
+    }
+
+    public static int ToHeadingIndex(double heading, double resolution, int binCount)
+    {
+        if (!NumericGuard.IsFinite(heading) || !NumericGuard.IsPositiveFinite(resolution) || binCount <= 0)
+            throw new ArgumentOutOfRangeException();
+
+        double normalized = NormalizeRadians(heading);
+        int index = (int)Math.Floor((normalized + Math.PI) / resolution);
+        index %= binCount;
+        return index < 0 ? index + binCount : index;
+    }
+}
@@ -0,0 +1,18 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.Utils;
+
+/// <summary>Two-dimensional rigid-body coordinate transforms. Units are preserved.</summary>
+public static class CoordinateTransform
+{
+    public static void LocalToWorld(
+        double originX, double originY, double headingRadians,
+        double localX, double localY,
+        out double worldX, out double worldY)
+    {
+        double cosine = Math.Cos(headingRadians);
+        double sine = Math.Sin(headingRadians);
+        worldX = originX + cosine * localX - sine * localY;
+        worldY = originY + sine * localX + cosine * localY;
+    }
+}
@@ -0,0 +1,15 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.Utils;
+
+public readonly struct GridIndex : IEquatable<GridIndex>
+{
+    public GridIndex(int row, int col) { Row = row; Col = col; }
+    public int Row { get; }
+    public int Col { get; }
+    public bool Equals(GridIndex other) { return Row == other.Row && Col == other.Col; }
+    public override bool Equals(object obj) { return obj is GridIndex && Equals((GridIndex)obj); }
+    public override int GetHashCode() { unchecked { return Row * 397 ^ Col; } }
+    public static bool operator ==(GridIndex left, GridIndex right) { return left.Equals(right); }
+    public static bool operator !=(GridIndex left, GridIndex right) { return !left.Equals(right); }
+}
@@ -0,0 +1,14 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.Utils;
+
+public static class NumericGuard
+{
+    public static bool IsFinite(double value) { return !double.IsNaN(value) && !double.IsInfinity(value); }
+    public static bool IsFinite(float value) { return !float.IsNaN(value) && !float.IsInfinity(value); }
+    public static bool IsPositiveFinite(double value) { return IsFinite(value) && value > 0d; }
+    public static bool IsInRange(float value, float minimum, float maximum)
+    {
+        return IsFinite(value) && value >= minimum && value <= maximum;
+    }
+}
@@ -0,0 +1,81 @@
+using System;
+using System.Diagnostics;
+using System.Threading;
+
+namespace MultiWheelC.TrajectoryPlanning.Utils;
+
+/// <summary>一次规划操作的中立停止原因；Map 与 CoarsePath 分别映射为自己的结果状态。</summary>
+internal enum PlanningOperationStopReason
+{
+    /// <summary>预算仍可继续执行。</summary>
+    None,
+    /// <summary>调用方取消了本次操作。</summary>
+    Cancelled,
+    /// <summary>从预算创建起已经达到总超时。</summary>
+    TimedOut,
+}
+
+/// <summary>
+/// 以单调时钟统一控制一次规划操作的取消和总超时。
+/// 本类不依赖 Map 或 CoarsePath，供两层在长循环检查同一个停止条件。
+/// </summary>
+internal sealed class PlanningOperationBudget
+{
+    private const int CheckIntervalWorkItems = 256;
+
+    private readonly CancellationToken _cancellationToken;
+    private readonly Stopwatch _stopwatch;
+    private readonly TimeSpan _timeout;
+    private readonly bool _hasTimeout;
+
+    /// <summary>
+    /// 创建具有总超时的操作预算。
+    /// 参数：cancellationToken 为调用方停止请求；timeout 必须非负，且从构造时开始计时。
+    /// </summary>
+    internal PlanningOperationBudget(CancellationToken cancellationToken, TimeSpan timeout)
+    {
+        if (timeout < TimeSpan.Zero) throw new ArgumentOutOfRangeException(nameof(timeout));
+        _cancellationToken = cancellationToken;
+        _timeout = timeout;
+        _hasTimeout = true;
+        _stopwatch = Stopwatch.StartNew();
+    }
+
+    private PlanningOperationBudget(CancellationToken cancellationToken)
+    {
+        _cancellationToken = cancellationToken;
+        _timeout = TimeSpan.Zero;
+        _hasTimeout = false;
+        _stopwatch = Stopwatch.StartNew();
+    }
+
+    /// <summary>创建只响应取消、不限制总耗时的兼容预算。</summary>
+    internal static PlanningOperationBudget Unlimited(CancellationToken cancellationToken)
+    {
+        return new PlanningOperationBudget(cancellationToken);
+    }
+
+    /// <summary>从预算创建到当前的单调经过时间。</summary>
+    internal TimeSpan Elapsed { get { return _stopwatch.Elapsed; } }
+
+    /// <summary>立即查询当前停止原因；取消优先于超时。</summary>
+    internal PlanningOperationStopReason GetStopReason()
+    {
+        if (_cancellationToken.IsCancellationRequested) return PlanningOperationStopReason.Cancelled;
+        return _hasTimeout && _stopwatch.Elapsed >= _timeout
+            ? PlanningOperationStopReason.TimedOut
+            : PlanningOperationStopReason.None;
+    }
+
+    /// <summary>
+    /// 在循环工作单元中按固定上限检查预算。
+    /// 参数：workItemCount 为当前循环共享的已处理单元计数；首次及每 256 个单元检查一次。
+    /// </summary>
+    internal PlanningOperationStopReason CheckEvery(ref int workItemCount)
+    {
+        workItemCount++;
+        return workItemCount == 1 || workItemCount % CheckIntervalWorkItems == 0
+            ? GetStopReason()
+            : PlanningOperationStopReason.None;
+    }
+}
@@ -0,0 +1,11 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.Utils;
+
+public static class UnitConverter
+{
+    public static double MillimetersToMeters(double value) { return value / 1000d; }
+    public static double MetersToMillimeters(double value) { return value * 1000d; }
+    public static double DegreesToRadians(double value) { return value * Math.PI / 180d; }
+    public static double RadiansToDegrees(double value) { return value * 180d / Math.PI; }
+}