68 lines
2.6 KiB
C#
68 lines
2.6 KiB
C#
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;
|
|
}
|
|
}
|
|
}
|