chore: save current workspace progress
This commit is contained in:
梁薄云
@@ -0,0 +1,31 @@
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>边与世界 X/Y 轴平行的矩形障碍物，坐标单位为 mm。</summary>
|
||||
public sealed class AxisAlignedRectangleObstacle : IMapObstacle
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建轴对齐矩形障碍物。
|
||||
///
|
||||
/// 参数：xMin、xMax、yMin、yMax 分别为矩形世界坐标边界，单位 mm。
|
||||
/// 注意：构造不校验边界顺序，请通过 <see cref="IsValid"/> 判断后再使用。
|
||||
/// </summary>
|
||||
public AxisAlignedRectangleObstacle(float xMin, float xMax, float yMin, float yMax)
|
||||
{
|
||||
XMin = xMin; XMax = xMax; YMin = yMin; YMax = yMax;
|
||||
}
|
||||
/// <summary>矩形世界 X 下边界，单位 mm。</summary>
|
||||
public float XMin { get; }
|
||||
/// <summary>矩形世界 X 上边界，单位 mm。</summary>
|
||||
public float XMax { get; }
|
||||
/// <summary>矩形世界 Y 下边界，单位 mm。</summary>
|
||||
public float YMin { get; }
|
||||
/// <summary>矩形世界 Y 上边界，单位 mm。</summary>
|
||||
public float YMax { get; }
|
||||
/// <summary>四个边界均为有限数且 XMax≥XMin、YMax≥YMin 时为 true；否则为 false。</summary>
|
||||
public bool IsValid
|
||||
{
|
||||
get { return NumericGuard.IsFinite(XMin) && NumericGuard.IsFinite(XMax) && NumericGuard.IsFinite(YMin) && NumericGuard.IsFinite(YMax) && XMax >= XMin && YMax >= YMin; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>以世界 mm 坐标表示的圆形障碍物。</summary>
|
||||
public sealed class CircleObstacle : IMapObstacle
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建圆形障碍物。
|
||||
///
|
||||
/// 参数：centerX、centerY 为圆心世界坐标，单位 mm；radiusMm 为半径，单位 mm。
|
||||
/// 注意：构造不抛出几何校验异常，请通过 <see cref="IsValid"/> 判断后再使用。
|
||||
/// </summary>
|
||||
public CircleObstacle(float centerX, float centerY, float radiusMm)
|
||||
{
|
||||
CenterX = centerX; CenterY = centerY; RadiusMm = radiusMm;
|
||||
}
|
||||
/// <summary>圆心世界 X 坐标，单位 mm。</summary>
|
||||
public float CenterX { get; }
|
||||
/// <summary>圆心世界 Y 坐标，单位 mm。</summary>
|
||||
public float CenterY { get; }
|
||||
/// <summary>圆的半径，单位 mm。</summary>
|
||||
public float RadiusMm { get; }
|
||||
/// <summary>圆心和半径均为有限数且半径不小于零时为 true；否则为 false。</summary>
|
||||
public bool IsValid
|
||||
{
|
||||
get { return NumericGuard.IsFinite(CenterX) && NumericGuard.IsFinite(CenterY) && NumericGuard.IsFinite(RadiusMm) && RadiusMm >= 0f; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,12 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 世界坐标中的不可变障碍物几何。
|
||||
///
|
||||
/// 单位：所有几何坐标与尺寸均为 mm。实现类型必须能由 <see cref="MapObstacleRasterizer"/> 栅格化。
|
||||
/// </summary>
|
||||
public interface IMapObstacle
|
||||
{
|
||||
/// <summary>几何数据是否有效。true 表示数值有限且尺寸满足该几何类型的约束；false 表示不得投影到地图。</summary>
|
||||
bool IsValid { get; }
|
||||
}
|
||||
@@ -0,0 +1,98 @@
|
||||
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; }
|
||||
}
|
||||
Reference in New Issue
Block a user