89 lines
5.1 KiB
C#
89 lines
5.1 KiB
C#
using System;
|
||
|
||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||
|
||
/// <summary>
|
||
/// 供粗路径规划使用的不可变地图快照。
|
||
///
|
||
/// 单位：世界查询方法使用 m；<see cref="Bounds"/> 和 <see cref="ResolutionMm"/> 保留原始 mm 数据。
|
||
/// 注意：世界坐标越界一律按占据处理，净距离为零。
|
||
/// </summary>
|
||
public sealed class PlanningGridMap
|
||
{
|
||
private readonly byte[] _occupied;
|
||
private readonly double[] _conservativeDistances;
|
||
|
||
internal PlanningGridMap(MapBoundsMm bounds, float resolutionMm, int rows, int cols, byte[] occupied, double[] conservativeDistances,
|
||
long snapshotId, bool planningReady, string planningBlockReason, string inputFingerprint, string occupancyHash)
|
||
{
|
||
Bounds = bounds; ResolutionMm = resolutionMm; Rows = rows; Cols = cols;
|
||
_occupied = occupied; _conservativeDistances = conservativeDistances;
|
||
SnapshotId = snapshotId; PlanningReady = planningReady; PlanningBlockReason = planningBlockReason ?? string.Empty;
|
||
InputFingerprint = inputFingerprint ?? string.Empty; OccupancyHash = occupancyHash ?? string.Empty;
|
||
}
|
||
/// <summary>源环境图的世界边界，单位 mm，采用左闭右开规则。</summary>
|
||
public MapBoundsMm Bounds { get; }
|
||
/// <summary>源环境图的栅格边长，单位 mm。</summary>
|
||
public float ResolutionMm { get; }
|
||
/// <summary>规划世界查询对应的栅格边长，单位 m。</summary>
|
||
public double ResolutionMeters { get { return ResolutionMm / 1000d; } }
|
||
/// <summary>栅格行数。</summary>
|
||
public int Rows { get; }
|
||
/// <summary>栅格列数。</summary>
|
||
public int Cols { get; }
|
||
/// <summary>工厂为本次返回快照分配的单调编号，用于区分不同构建结果。</summary>
|
||
public long SnapshotId { get; }
|
||
/// <summary>地图是否允许进入粗路径规划。true 时可直接查询；false 时应先处理 <see cref="PlanningBlockReason"/>。</summary>
|
||
public bool PlanningReady { get; }
|
||
/// <summary>禁止规划的原因。<see cref="PlanningReady"/> 为 true 时为空字符串。</summary>
|
||
public string PlanningBlockReason { get; }
|
||
/// <summary>完整建图输入的稳定指纹，用于识别精确输入缓存命中。</summary>
|
||
public string InputFingerprint { get; }
|
||
/// <summary>占据栅格内容哈希，用于识别可复用的占据与距离数组。</summary>
|
||
public string OccupancyHash { get; }
|
||
|
||
/// <summary>查询世界位置是否占据。参数 xMeters、yMeters 单位为 m；位置越界时保守地返回 true。</summary>
|
||
public bool IsOccupiedWorld(double xMeters, double yMeters)
|
||
{
|
||
return !TryWorldToGrid(xMeters, yMeters, out int row, out int col) || _occupied[row * Cols + col] != 0;
|
||
}
|
||
/// <summary>
|
||
/// 查询到最近障碍物的保守净距离下界。
|
||
///
|
||
/// 参数：xMeters、yMeters 为世界坐标，单位 m。
|
||
/// 返回：单位 m 的非负距离下界；地图内无障碍物时为正无穷，越界时为零。
|
||
/// </summary>
|
||
public double GetConservativeObstacleDistanceMeters(double xMeters, double yMeters)
|
||
{
|
||
return !TryWorldToGrid(xMeters, yMeters, out int row, out int col) ? 0d : _conservativeDistances[row * Cols + col];
|
||
}
|
||
/// <summary>
|
||
/// 将规划世界坐标转换为栅格索引。
|
||
///
|
||
/// 参数：xMeters、yMeters 为世界坐标，单位 m；row、col 为输出索引。
|
||
/// 返回：位置在地图内时为 true 并写入索引；否则返回 false，两个输出均为 -1。
|
||
/// </summary>
|
||
public bool TryWorldToGrid(double xMeters, double yMeters, out int row, out int col)
|
||
{
|
||
row = -1; col = -1;
|
||
double xMm = xMeters * 1000d, yMm = yMeters * 1000d;
|
||
if (xMm < Bounds.XMin || xMm >= Bounds.XMax || yMm < Bounds.YMin || yMm >= Bounds.YMax) return false;
|
||
col = (int)Math.Floor((xMm - Bounds.XMin) / ResolutionMm);
|
||
row = (int)Math.Floor((yMm - Bounds.YMin) / ResolutionMm);
|
||
return row >= 0 && row < Rows && col >= 0 && col < Cols;
|
||
}
|
||
/// <summary>按行列索引查询占据状态。参数从零开始；任一索引越界时返回 true。</summary>
|
||
public bool IsOccupied(int row, int col) { return row < 0 || row >= Rows || col < 0 || col >= Cols || _occupied[row * Cols + col] != 0; }
|
||
internal byte[] CopyOccupied() { return (byte[])_occupied.Clone(); }
|
||
internal bool OccupancyEquals(PlanningGridMap other)
|
||
{
|
||
if (other == null || Rows != other.Rows || Cols != other.Cols || ResolutionMm != other.ResolutionMm || !Bounds.Equals(other.Bounds) || _occupied.Length != other._occupied.Length) return false;
|
||
for (int i = 0; i < _occupied.Length; i++) if (_occupied[i] != other._occupied[i]) return false;
|
||
return true;
|
||
}
|
||
internal PlanningGridMap WithMetadata(long snapshotId, bool planningReady, string blockReason, string inputFingerprint, string occupancyHash)
|
||
{
|
||
return new PlanningGridMap(Bounds, ResolutionMm, Rows, Cols, _occupied, _conservativeDistances, snapshotId, planningReady, blockReason, inputFingerprint, occupancyHash);
|
||
}
|
||
}
|