# 多高度层GridMap解决方案

**日期**: 2025-01-08
**问题**: 楼梯与楼板在同一(x,y)坐标重叠时，2D网格投影导致互相覆盖
**目标**: 支持多高度层，同时保留RoyT.AStar高性能算法

---

## 问题分析

### 场景描述
- **楼梯**: 倾斜通道，在(x,y)位置有不同Z高度
- **楼梯下方楼板**: 水平通道，在相同(x,y)位置也可通行
- **当前问题**: 两者投影到同一2D网格坐标(x,y)，后处理的覆盖前处理的

### 核心矛盾
- 2D网格投影系统无法处理垂直空间层叠
- 起点在楼面(Z=3.0m)，终点在楼梯(Z=5.0m)的路径无法正确规划

---

## 解决方案

### 核心思路
1. **保留RoyT.AStar**: 2D A*算法不变，继续产生最优2D路径
2. **扩展数据结构**: GridCell支持多个HeightLayer
3. **投影改为追加**: 通道投影不覆盖，而是添加新层
4. **路径后处理选层**: A*产生2D路径后，为每个路径点智能选择高度层

### 关键洞察
- **2D路径已经最优**: RoyT.AStar产生的2D网格序列`[(x1,y1), (x2,y2), ..., (xn,yn)]`不需要改变
- **只需选Z坐标**: 为每个2D点选择正确的高度层，形成3D路径
- **基于高度趋势**: 根据当前位置到终点的剩余高度变化，选择最合适的层

---

## 技术设计

### 1. 数据结构扩展

#### HeightLayer结构
```csharp
public struct HeightLayer
{
    public double Z;                      // 该层的Z坐标
    public HeightInterval PassableHeight; // 可通行高度范围（用于物体检查）
    public ModelItem SourceItem;          // 来源模型项
    public double SpeedLimit;             // 限速
    public CategoryAttributeManager.LogisticsElementType Type; // 层类型
}
```

#### GridCell扩展
```csharp
public struct GridCell
{
    // ... 保留所有现有字段 ...

    // 新增：多高度层支持
    public List<HeightLayer> HeightLayers { get; set; }
}
```

### 2. 通道投影改为追加模式

**文件**: `src/PathPlanning/ChannelBasedGridBuilder.cs`
**方法**: `RasterizeTriangleToGrid()`

**修改前**:
```csharp
var cell = GridCellBuilder.Channel(preciseWorldPosition, channel, channelSpeedLimit);
gridMap.PlaceCell(gridPos, cell); // 覆盖
```

**修改后**:
```csharp
var layer = new HeightLayer
{
    Z = gridCenterHeight,
    PassableHeight = new HeightInterval(0, 0), // 初始化，后续设置
    SourceItem = channel,
    SpeedLimit = channelSpeedLimit,
    Type = CategoryAttributeManager.GetLogisticsElementType(channel)
};
gridMap.AddHeightLayer(gridPos, layer); // 追加
```

### 3. PassableHeight计算适配

**保留现有逻辑**: `SetChannelPassableHeights()` 已经很完善
**修改点**: 从设置单层GridCell改为设置多个HeightLayer

```csharp
// 为每个高度层设置PassableHeight
if (cell.HeightLayers != null && cell.HeightLayers.Any())
{
    for (int i = 0; i < cell.HeightLayers.Count; i++)
    {
        var layer = cell.HeightLayers[i];
        layer.PassableHeight = new HeightInterval(0, scanHeightInModelUnits);
        cell.HeightLayers[i] = layer;
    }
}
```

### 4. 路径后处理智能选层

**核心算法**: `AssignLayerBased3DCoordinates()`

**选层策略**:
```
1. 起点锁定: 选择包含start.Z的层
2. 终点锁定: 选择包含end.Z的层
3. 中间点选层:
   - 计算剩余高度变化: remainingHeight = end.Z - current.Z
   - 计算期望每步变化: desiredChange = remainingHeight / remainingSteps
   - 目标Z: targetZ = prevZ + desiredChange
   - 选择最接近targetZ且净空足够的层
```

**示例执行**:
```
场景: 起点楼面(3.0m) → 终点楼梯(5.0m)
2D路径: [(0,0), (1,1), (2,2), (3,3)]

赋值过程:
1. (0,0): Z=3.0 (起点锁定楼面层)
2. (1,1): remainingHeight=2.0, steps=2, desiredChange=1.0
         targetZ=4.0, 选择楼梯层3.5m (更接近4.0)
3. (2,2): remainingHeight=1.5, steps=1, desiredChange=1.5
         targetZ=5.0, 选择楼梯层4.5m
4. (3,3): Z=5.0 (终点锁定楼梯层)

最终路径: [(0,0,3.0), (1,1,3.5), (2,2,4.5), (3,3,5.0)]
✅ 平滑爬升！
```

---

## 实施计划

### 步骤1: 扩展GridCell支持多层
- **文件**: `src/PathPlanning/GridMap.cs`
- **内容**: 新增HeightLayer结构、扩展GridCell、添加AddHeightLayer()和FindLayerContainingZ()方法

### 步骤2: 通道投影改为追加模式
- **文件**: `src/PathPlanning/ChannelBasedGridBuilder.cs`
- **内容**: 修改RasterizeTriangleToGrid()，从PlaceCell改为AddHeightLayer

### 步骤3: 适配PassableHeight计算
- **文件**: `src/PathPlanning/GridMapGenerator.cs`
- **内容**: 修改SetChannelPassableHeights()支持多层

### 步骤4: 实现智能选层算法
- **文件**: `src/PathPlanning/AutoPathFinder.cs`
- **内容**: 新增AssignLayerBased3DCoordinates()和SelectBestLayer()方法，替换现有路径后处理

### 步骤5: 测试验证
- 起点楼面 → 终点楼梯（验证爬升）
- 起点楼梯 → 终点楼面（验证下降）
- 楼梯下方通行（验证多层并存）

---

## 关键约束

1. **保留RoyT.AStar**: 不修改A*算法，保持高性能
2. **不考虑向后兼容**: 直接改为多层架构
3. **保留现有PassableHeight逻辑**: 只适配到多层结构
4. **最小化改动**: 只修改必要的数据结构和投影逻辑

---

## 预期效果

- ✅ 解决楼梯-楼板重叠问题
- ✅ 自动选择最优高度层
- ✅ 平滑的高度过渡
- ✅ 保持RoyT.AStar高性能
- ✅ 代码改动最小化

---

## 文件修改清单

1. `src/PathPlanning/GridMap.cs` - 数据结构扩展
2. `src/PathPlanning/ChannelBasedGridBuilder.cs` - 投影逻辑修改
3. `src/PathPlanning/GridMapGenerator.cs` - PassableHeight计算适配
4. `src/PathPlanning/AutoPathFinder.cs` - 路径后处理智能选层

---

**状态**: 设计完成，待实施
**预计工作量**: 4个文件，约200行代码修改