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,67 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+
+/// <summary>
+/// Hybrid A* 闭集使用的离散状态键。
+/// 位置使用地图行列索引；航向、行驶方向和曲率等级共同保留车辆运动学状态，避免把同一栅格中的不同可达姿态错误合并。
+/// </summary>
+public sealed class HybridAStarNodeKey : IEquatable<HybridAStarNodeKey>
+{
+    /// <summary>
+    /// 创建一个离散 Hybrid A* 状态键。
+    /// 参数：row、column 为零开始的地图行列；headingIndex 为航向桶；direction 为末段行驶方向；curvatureLevelIndex 为末段曲率等级。
+    /// </summary>
+    public HybridAStarNodeKey(int row, int column, int headingIndex, TravelDirection direction, int curvatureLevelIndex)
+    {
+        Row = row;
+        Column = column;
+        HeadingIndex = headingIndex;
+        Direction = direction;
+        CurvatureLevelIndex = curvatureLevelIndex;
+    }
+
+    /// <summary>车辆中心所在的零开始地图行索引。</summary>
+    public int Row { get; }
+
+    /// <summary>车辆中心所在的零开始地图列索引。</summary>
+    public int Column { get; }
+
+    /// <summary>与 <see cref="Column"/> 含义相同的列索引别名。</summary>
+    public int Col { get { return Column; } }
+
+    /// <summary>根据配置航向分辨率量化后的航向桶索引。</summary>
+    public int HeadingIndex { get; }
+
+    /// <summary>到达当前节点的最后一段行驶方向。</summary>
+    public TravelDirection Direction { get; }
+
+    /// <summary>到达当前节点的最后一段曲率等级索引。</summary>
+    public int CurvatureLevelIndex { get; }
+
+    /// <summary>判断另一个键是否表示完全相同的离散搜索状态。</summary>
+    public bool Equals(HybridAStarNodeKey other)
+    {
+        return other != null && Row == other.Row && Column == other.Column && HeadingIndex == other.HeadingIndex &&
+            Direction == other.Direction && CurvatureLevelIndex == other.CurvatureLevelIndex;
+    }
+
+    /// <summary>判断另一个对象是否表示完全相同的离散搜索状态。</summary>
+    public override bool Equals(object obj)
+    {
+        return Equals(obj as HybridAStarNodeKey);
+    }
+
+    /// <summary>返回用于闭集字典的稳定哈希值。</summary>
+    public override int GetHashCode()
+    {
+        unchecked
+        {
+            int hashCode = Row;
+            hashCode = hashCode * 397 ^ Column;
+            hashCode = hashCode * 397 ^ HeadingIndex;
+            hashCode = hashCode * 397 ^ (int)Direction;
+            return hashCode * 397 ^ CurvatureLevelIndex;
+        }
+    }
+}