# 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`