279 lines
7.2 KiB
Markdown
279 lines
7.2 KiB
Markdown
# Roy-T.AStar库的使用方法与经验总结
|
||
|
||
## 概述
|
||
|
||
Roy-T.AStar是一个高性能的C# A*寻路算法库，位于`C:\Users\Tellme\apps\OpenSource\AStar-master`。本文档记录了在NavisworksTransport项目中集成和使用该库的经验教训。
|
||
|
||
## 核心概念理解
|
||
|
||
### 1. 坐标系统
|
||
|
||
**关键发现**：Roy-T.AStar使用**米坐标系统**，而不是网格索引。
|
||
|
||
```csharp
|
||
// 创建网格时，传入的是物理尺寸
|
||
var cellSize = new Size(
|
||
Distance.FromMeters((float)cellSizeInMeters),
|
||
Distance.FromMeters((float)cellSizeInMeters)
|
||
);
|
||
|
||
// GridPosition构造函数接受的是网格索引
|
||
var gridPos = new GridPosition(x, y); // x,y是网格索引，如(0,0), (1,0)等
|
||
|
||
// 但Node.Position返回的是米坐标！
|
||
// 例如：网格(1,0)的Node.Position可能是(2.0, 0.0)米（假设cellSize=2米）
|
||
```
|
||
|
||
### 2. 路径数据结构
|
||
|
||
**Path对象结构**：
|
||
|
||
- `Path.Edges`: 边的列表（IReadOnlyList<IEdge>）
|
||
- 每条边包含：
|
||
- `Start`: 起始节点
|
||
- `End`: 终止节点
|
||
- `Distance`: 边的长度
|
||
- `TraversalVelocity`: 遍历速度
|
||
|
||
**重要特性**：连续边的关系
|
||
|
||
```
|
||
Edge[0]: Start=A, End=B
|
||
Edge[1]: Start=B, End=C // 注意：Edge[0].End == Edge[1].Start
|
||
Edge[2]: Start=C, End=D
|
||
```
|
||
|
||
## 常见陷阱与解决方案
|
||
|
||
### 陷阱1：坐标转换时的重复点问题
|
||
|
||
**错误做法**：
|
||
|
||
```csharp
|
||
// ❌ 错误：会产生重复点
|
||
var gridPath = new List<GridPoint2D>();
|
||
gridPath.Add(ConvertToGrid(edges[0].Start)); // 添加起点
|
||
foreach (var edge in edges) {
|
||
gridPath.Add(ConvertToGrid(edge.End)); // 每条边的终点
|
||
}
|
||
// 结果：[A, B, B, C, C, D] - 转弯点重复！
|
||
```
|
||
|
||
**正确做法**：
|
||
|
||
```csharp
|
||
// ✅ 正确：避免重复
|
||
var gridPath = new List<GridPoint2D>();
|
||
if (edges.Count > 0) {
|
||
gridPath.Add(ConvertToGrid(edges[0].Start)); // 只添加第一个起点
|
||
foreach (var edge in edges) {
|
||
var gridPoint = ConvertToGrid(edge.End);
|
||
// 检查是否与上一个点重复
|
||
if (gridPath.Count == 0 || !gridPath.Last().Equals(gridPoint)) {
|
||
gridPath.Add(gridPoint);
|
||
}
|
||
}
|
||
}
|
||
// 结果：[A, B, C, D] - 完美的连续路径
|
||
```
|
||
|
||
### 陷阱2：米坐标到网格坐标的转换
|
||
|
||
**关键代码**：
|
||
|
||
```csharp
|
||
// 从A*的米坐标转换为网格索引
|
||
double cellSizeInMeters = UnitsConverter.ConvertToMeters(gridMap.CellSize);
|
||
int gridX = (int)Math.Floor(node.Position.X / cellSizeInMeters);
|
||
int gridY = (int)Math.Floor(node.Position.Y / cellSizeInMeters);
|
||
```
|
||
|
||
### 陷阱3：网格创建时的节点连接
|
||
|
||
**默认创建方法的限制**：
|
||
|
||
```csharp
|
||
// Roy-T.AStar提供的默认方法会连接所有节点
|
||
var grid = Grid.CreateGridWithLateralConnections(gridSize, cellSize, velocity);
|
||
```
|
||
|
||
**自定义障碍物处理**：
|
||
|
||
```csharp
|
||
// 1. 先断开所有连接
|
||
for (int x = 0; x < gridMap.Width; x++) {
|
||
for (int y = 0; y < gridMap.Height; y++) {
|
||
grid.DisconnectNode(new GridPosition(x, y));
|
||
}
|
||
}
|
||
|
||
// 2. 只连接可通行的节点
|
||
for (int x = 0; x < gridMap.Width; x++) {
|
||
for (int y = 0; y < gridMap.Height; y++) {
|
||
if (IsWalkable(x, y)) {
|
||
// 连接到右侧邻居
|
||
if (x + 1 < width && IsWalkable(x + 1, y)) {
|
||
grid.AddEdge(new GridPosition(x, y),
|
||
new GridPosition(x + 1, y), velocity);
|
||
}
|
||
// 连接到下方邻居
|
||
if (y + 1 < height && IsWalkable(x, y + 1)) {
|
||
grid.AddEdge(new GridPosition(x, y),
|
||
new GridPosition(x, y + 1), velocity);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
```
|
||
|
||
## 路径优化策略
|
||
|
||
### 1. 网格路径优化算法
|
||
|
||
**问题**：A*输出的路径包含大量中间点，需要优化。
|
||
|
||
**解决方案**：基于方向变化的路径简化
|
||
|
||
```csharp
|
||
private List<GridPoint2D> SimplifyPath(List<GridPoint2D> path) {
|
||
if (path.Count < 3) return path;
|
||
|
||
var simplified = new List<GridPoint2D> { path[0] };
|
||
|
||
// 计算初始方向（归一化）
|
||
int dx = path[1].X - path[0].X;
|
||
int dy = path[1].Y - path[0].Y;
|
||
var prevDirection = new GridPoint2D(
|
||
dx == 0 ? 0 : Math.Sign(dx),
|
||
dy == 0 ? 0 : Math.Sign(dy)
|
||
);
|
||
|
||
// 检测方向变化
|
||
for (int i = 2; i < path.Count; i++) {
|
||
dx = path[i].X - path[i-1].X;
|
||
dy = path[i].Y - path[i-1].Y;
|
||
var currentDirection = new GridPoint2D(
|
||
dx == 0 ? 0 : Math.Sign(dx),
|
||
dy == 0 ? 0 : Math.Sign(dy)
|
||
);
|
||
|
||
// 方向改变时保留转弯点
|
||
if (!currentDirection.Equals(prevDirection)) {
|
||
simplified.Add(path[i - 1]);
|
||
prevDirection = currentDirection;
|
||
}
|
||
}
|
||
|
||
simplified.Add(path.Last()); // 添加终点
|
||
return simplified;
|
||
}
|
||
```
|
||
|
||
### 2. 方向归一化的重要性
|
||
|
||
**问题**：不同步长的移动被误判为转弯
|
||
|
||
- `(-1, 0)` → `(-2, 0)`：都是向左，但步长不同
|
||
- `(0, -1)` → `(0, -3)`：都是向上，但步长不同
|
||
|
||
**解决**：使用`Math.Sign()`归一化方向向量
|
||
|
||
```csharp
|
||
// 归一化为单位方向向量 (-1, 0, 1)
|
||
int dirX = dx == 0 ? 0 : Math.Sign(dx);
|
||
int dirY = dy == 0 ? 0 : Math.Sign(dy);
|
||
```
|
||
|
||
## 性能优化建议
|
||
|
||
### 1. 路径缓存
|
||
|
||
对于频繁查询的路径，考虑缓存结果：
|
||
|
||
```csharp
|
||
private Dictionary<(Point3D, Point3D), Path> _pathCache;
|
||
```
|
||
|
||
### 2. 分层寻路
|
||
|
||
对于大型地图，可以使用分层A*算法：
|
||
|
||
- 高层：粗略网格，快速找到大致路径
|
||
- 低层：精细网格，优化局部路径
|
||
|
||
### 3. 动态障碍物
|
||
|
||
Roy-T.AStar支持动态修改网格连接：
|
||
|
||
```csharp
|
||
// 添加障碍物
|
||
grid.DisconnectNode(position);
|
||
|
||
// 移除障碍物
|
||
grid.AddEdge(from, to, velocity);
|
||
```
|
||
|
||
## 2.5D路径规划扩展
|
||
|
||
### 高度约束处理
|
||
|
||
```csharp
|
||
// 检查节点是否满足高度约束
|
||
if (cell.PassableHeights != null && cell.PassableHeights.Any()) {
|
||
bool heightOk = cell.PassableHeights.Any(
|
||
interval => interval.GetSpan() >= vehicleHeight
|
||
);
|
||
if (!heightOk) {
|
||
grid.DisconnectNode(position); // 不满足高度要求，断开连接
|
||
}
|
||
}
|
||
```
|
||
|
||
## 调试技巧
|
||
|
||
### 1. 日志输出
|
||
|
||
```csharp
|
||
LogManager.Info($"[A*执行] 找到路径，包含 {path.Edges.Count} 条边");
|
||
LogManager.Debug($"[路径优化] 方向从({prev.X},{prev.Y})变为({curr.X},{curr.Y})");
|
||
```
|
||
|
||
### 2. 路径验证
|
||
|
||
```csharp
|
||
// 验证路径连续性
|
||
for (int i = 1; i < path.Count; i++) {
|
||
var dist = Math.Abs(path[i].X - path[i-1].X) +
|
||
Math.Abs(path[i].Y - path[i-1].Y);
|
||
if (dist > 1) {
|
||
LogManager.Warning($"路径不连续：从{path[i-1]}到{path[i]}");
|
||
}
|
||
}
|
||
```
|
||
|
||
## 实际优化效果
|
||
|
||
在NavisworksTransport项目中的实测结果：
|
||
|
||
- **原始A*输出**：101个路径点
|
||
- **优化后**：19个关键转弯点
|
||
- **优化率**：81.2%
|
||
- **处理时间**：约8ms
|
||
|
||
## 总结
|
||
|
||
使用Roy-T.AStar库的关键要点：
|
||
|
||
1. 理解米坐标系统，正确进行坐标转换
|
||
2. 注意Path.Edges的连续性，避免重复点
|
||
3. 使用方向归一化进行路径优化
|
||
4. 灵活运用DisconnectNode和AddEdge处理障碍物
|
||
5. 对于2.5D场景，在网格连接阶段处理高度约束
|
||
|
||
## 参考资源
|
||
|
||
- Roy-T.AStar源码：`C:\Users\Tellme\apps\OpenSource\AStar-master`
|
||
- NavisworksTransport集成代码：
|
||
- `src\PathPlanning\AutoPathFinder.cs`
|
||
- `src\PathPlanning\PathOptimizer.cs`
|