99 lines
5.6 KiB
C#
99 lines
5.6 KiB
C#
using System;
|
|
using System.Threading;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
|
|
|
/// <summary>
|
|
/// 将障碍物几何保守投影为栅格占据状态的唯一写入入口。
|
|
///
|
|
/// 注意：调用方不能直接改写 <see cref="EnvironmentGridMap"/>；相交或贴边的栅格均按占据处理。
|
|
/// </summary>
|
|
public static class MapObstacleRasterizer
|
|
{
|
|
/// <summary>
|
|
/// 将一个有效障碍物栅格化到环境地图。
|
|
///
|
|
/// 参数：map 为待写入的环境栅格；obstacle 为世界 mm 坐标的圆形或轴对齐矩形障碍物。
|
|
/// 返回：无。地图或障碍物为空、障碍物无效、几何类型不受支持时抛出异常。
|
|
/// 注意：该方法只增加占据格，不会清除已有障碍。
|
|
/// </summary>
|
|
public static void Rasterize(EnvironmentGridMap map, IMapObstacle obstacle)
|
|
{
|
|
if (!TryRasterize(map, obstacle, PlanningOperationBudget.Unlimited(CancellationToken.None), out _))
|
|
throw new InvalidOperationException("Unbounded rasterization unexpectedly stopped.");
|
|
}
|
|
|
|
/// <summary>使用共享预算将障碍物写入环境栅格；停止时返回 false 且不发布环境地图。</summary>
|
|
internal static bool TryRasterize(EnvironmentGridMap map, IMapObstacle obstacle, PlanningOperationBudget budget,
|
|
out PlanningOperationStopReason stopReason)
|
|
{
|
|
if (map == null) throw new ArgumentNullException(nameof(map));
|
|
if (obstacle == null || !obstacle.IsValid) throw new ArgumentException("Obstacle must be valid.", nameof(obstacle));
|
|
if (budget == null) throw new ArgumentNullException(nameof(budget));
|
|
stopReason = budget.GetStopReason();
|
|
if (stopReason != PlanningOperationStopReason.None) return false;
|
|
int workItemCount = 0;
|
|
var circle = obstacle as CircleObstacle;
|
|
if (circle != null) return TryRasterizeCircle(map, circle, budget, ref workItemCount, out stopReason);
|
|
var rectangle = obstacle as AxisAlignedRectangleObstacle;
|
|
if (rectangle != null) return TryRasterizeRectangle(map, rectangle, budget, ref workItemCount, out stopReason);
|
|
throw new NotSupportedException("Unsupported map obstacle geometry.");
|
|
}
|
|
|
|
private static bool TryRasterizeCircle(EnvironmentGridMap map, CircleObstacle circle, PlanningOperationBudget budget,
|
|
ref int workItemCount, out PlanningOperationStopReason stopReason)
|
|
{
|
|
GetCandidateRange(map, circle.CenterX - circle.RadiusMm, circle.CenterX + circle.RadiusMm,
|
|
circle.CenterY - circle.RadiusMm, circle.CenterY + circle.RadiusMm,
|
|
out int firstRow, out int lastRow, out int firstCol, out int lastCol);
|
|
double radiusSquared = (double)circle.RadiusMm * circle.RadiusMm;
|
|
for (int row = firstRow; row <= lastRow; row++)
|
|
for (int col = firstCol; col <= lastCol; col++)
|
|
{
|
|
stopReason = budget.CheckEvery(ref workItemCount);
|
|
if (stopReason != PlanningOperationStopReason.None) return false;
|
|
map.GetCellBounds(row, col, out float xMin, out float xMax, out float yMin, out float yMax);
|
|
double nearestX = Math.Max(xMin, Math.Min(circle.CenterX, xMax));
|
|
double nearestY = Math.Max(yMin, Math.Min(circle.CenterY, yMax));
|
|
double dx = circle.CenterX - nearestX;
|
|
double dy = circle.CenterY - nearestY;
|
|
if (dx * dx + dy * dy <= radiusSquared) map.MarkOccupied(row, col);
|
|
}
|
|
stopReason = PlanningOperationStopReason.None;
|
|
return true;
|
|
}
|
|
|
|
private static bool TryRasterizeRectangle(EnvironmentGridMap map, AxisAlignedRectangleObstacle rectangle, PlanningOperationBudget budget,
|
|
ref int workItemCount, out PlanningOperationStopReason stopReason)
|
|
{
|
|
GetCandidateRange(map, rectangle.XMin, rectangle.XMax, rectangle.YMin, rectangle.YMax,
|
|
out int firstRow, out int lastRow, out int firstCol, out int lastCol);
|
|
for (int row = firstRow; row <= lastRow; row++)
|
|
for (int col = firstCol; col <= lastCol; col++)
|
|
{
|
|
stopReason = budget.CheckEvery(ref workItemCount);
|
|
if (stopReason != PlanningOperationStopReason.None) return false;
|
|
map.GetCellBounds(row, col, out float xMin, out float xMax, out float yMin, out float yMax);
|
|
if (rectangle.XMax >= xMin && rectangle.XMin <= xMax && rectangle.YMax >= yMin && rectangle.YMin <= yMax)
|
|
map.MarkOccupied(row, col);
|
|
}
|
|
stopReason = PlanningOperationStopReason.None;
|
|
return true;
|
|
}
|
|
|
|
private static void GetCandidateRange(EnvironmentGridMap map, float xMin, float xMax, float yMin, float yMax,
|
|
out int firstRow, out int lastRow, out int firstCol, out int lastCol)
|
|
{
|
|
if (xMax < map.Bounds.XMin || xMin > map.Bounds.XMax || yMax < map.Bounds.YMin || yMin > map.Bounds.YMax)
|
|
{ firstRow = 1; lastRow = 0; firstCol = 1; lastCol = 0; return; }
|
|
// Geometry is closed for conservative rasterisation. Include the cell on
|
|
// the lower side when a boundary lies exactly on a grid line.
|
|
firstCol = Clamp((int)Math.Floor(((double)xMin - map.Bounds.XMin) / map.ResolutionMm) - 1, 0, map.Cols - 1);
|
|
lastCol = Clamp((int)Math.Floor(((double)xMax - map.Bounds.XMin) / map.ResolutionMm), 0, map.Cols - 1);
|
|
firstRow = Clamp((int)Math.Floor(((double)yMin - map.Bounds.YMin) / map.ResolutionMm) - 1, 0, map.Rows - 1);
|
|
lastRow = Clamp((int)Math.Floor(((double)yMax - map.Bounds.YMin) / map.ResolutionMm), 0, map.Rows - 1);
|
|
}
|
|
private static int Clamp(int value, int min, int max) { return value < min ? min : value > max ? max : value; }
|
|
}
|