# 体素网格 3D 路径规划方案

## 1. 背景与动机

### 1.1 当前方案的局限性

当前 NavisworksTransport 插件采用 **2.5D 网格 + 高度层** 的路径规划方案：

**数据结构**：

```csharp
public class GridCell
{
    public List<HeightLayer> HeightLayers { get; set; } // 多个高度层
    // (X, Y) 网格坐标 + 离散的 Z 高度层
}
```

**局限性**：

1. **高度层离散化**
   - 只能表示有限数量的通行高度
   - 相邻高度层之间的连接需要特殊处理
   - 难以精确表示连续变化的高度

2. **复杂空间的表达受限**
   - 管道、桥梁下方的多层通道难以准确建模
   - 斜坡、楼梯的连续高度变化需要大量高度层
   - 悬空结构（如天桥）的下方空间处理复杂

3. **路径规划的限制**
   - 垂直方向的移动需要显式的"层间连接"
   - 真正的 3D 路径（如无人机、爬楼机器人）无法支持
   - 算法复杂度高（需要处理层间跳转逻辑）

4. **可扩展性问题**
   - 增加高度层数会显著增加内存消耗
   - 多层网格的生成和维护成本高
   - 难以支持动态障碍物的实时更新

### 1.2 体素网格的优势

采用 **真正的 3D 体素网格**（Voxel Grid）可以解决上述问题：

**统一的 3D 表示**：

```csharp
public class VoxelGrid
{
    private VoxelCell[,,] cells; // 统一的 3D 数组 (X, Y, Z)
}
```

**优势**：

1. ✅ **连续的 3D 空间**：任意高度都可以表示
2. ✅ **简化的路径算法**：标准 3D A* 算法，无需特殊层间逻辑
3. ✅ **精确的障碍物表示**：管道、横梁等任意 3D 形状
4. ✅ **更好的可视化**：直观的 3D 体素显示
5. ✅ **支持真 3D 路径**：无人机、飞行器、爬楼设备

---

## 2. 技术调研：C# 体素网格库

### 2.1 候选库对比

经过深入的 GitHub 和 NuGet 调研，找到以下候选库：

| 库名 | Stars | 许可证 | 平台 | 活跃度 | 适用性 |
|------|-------|--------|------|--------|--------|
| **geometry4Sharp** | 273 | Boost | .NET Std 2.0 | 中等 | ⭐⭐⭐⭐⭐ |
| geometry3Sharp (原版) | 1400+ | Boost | .NET Std 2.0 | 低 | ⭐⭐⭐⭐ |
| Unity PathFinding3D | ~100 | MIT | Unity | 高 | ⭐⭐ (Unity 依赖) |
| UnityOctree | ~900 | MIT | Unity | 中 | ⭐⭐ (Unity 依赖) |
| 自行实现 | N/A | N/A | 任意 | N/A | ⭐⭐⭐ (工作量大) |

### 2.2 推荐方案：geometry4Sharp

**GitHub**: <https://github.com/NewWheelTech/geometry4Sharp>

**NuGet**: `geometry4Sharp` 1.0.0

#### 基本信息

- **许可证**: Boost Software License（商业友好，无需开源衍生作品）
- **维护者**: New Wheel Technology
- **代码来源**: Fork from geometry3Sharp，底层算法来自 WildMagic5/GTEngine (David Eberly)
- **支持平台**: .NET Standard 2.0, .NET 6.0, .NET Framework 4.8
- **语言**: 纯 C#，无 C++ 互操作

#### 核心功能模块

1.体素化与距离场

```csharp
// 核心类
MeshSignedDistanceGrid  // 网格签名距离场
MeshScalarSamplingGrid  // 标量采样网格
Bitmap3                 // 3D 位图（密集体素）
DSparseGrid3           // 稀疏 3D 网格（按需分配）
BiGrid3                // 两层稀疏网格（优化大规模场景）
```

**功能说明**：

- **MeshSignedDistanceGrid**:
  - 从三角网格生成签名距离场 (SDF)
  - 使用快速行进法 (Fast Marching Method)
  - 支持窄带构建（仅计算障碍物附近区域，节省内存）
  - 基于 Christopher Batty 的 SDFGen C++ 代码移植

- **Bitmap3**:
  - 密集 3D 布尔数组
  - 高效的内存布局
  - 适合小到中等规模网格（< 1000³）

- **DSparseGrid3**:
  - 稀疏 3D 网格，按需分配内存
  - 适合大规模场景（仅存储非空体素）
  - 基于字典或哈希表实现

2.空间查询与距离计算

```csharp
// 距离查询
bool IsInside(Point3d point)              // 点包含测试
double WindingNumber(Point3d point)       // 绕组数（判断内外）
double FastWindingNumber(Point3d point)   // 快速绕组数
double Distance(Point3d point)            // 到最近表面的距离

// 空间索引
DMeshAABBTree3          // 三角网格 AABB 树（加速相交测试）
PointHashGrid3d         // 3D 点哈希网格
PointAABBTree3          // 点云 AABB 树
```

3.网格操作与处理

```csharp
// 等值面提取
MarchingCubes          // 从体素生成三角网格
MarchingCubesPro       // 延续法行进立方体（更高效）

// 网格简化与重新网格化
Remesher               // 边分裂/翻转/坍缩网格
Reducer                // 基于 QEM 的网格简化

// 几何计算
DMesh3                 // 动态三角网格类
BoundsUtil             // 包围盒计算
MeshMeasurements       // 体积、质心、惯性张量
```

4.数学与几何工具

```csharp
// 向量数学（完整的 struct 实现）
Vector2d/3d/4d, Vector2f/3f/4f
Matrix2d/3d, Matrix2f/3f
Quaterniond/f
Frame3f                // 位置+方向表示

// 几何形状
AxisAlignedBox3d/3f    // 轴对齐包围盒
Box3d/3f               // 有向包围盒
Triangle3d/3f, Segment3d/3f, Ray3d/3f, Plane3d/3f

// 距离与相交查询
DistPoint3Triangle3, DistLine3Triangle3
IntrRay3Triangle3, IntrTriangle3Triangle3
```

#### 为什么选择 geometry4Sharp？

✅ **功能完整**：

- 提供完整的网格→体素→距离场工具链
- 丰富的空间查询和距离计算
- 优化的空间数据结构（AABB 树、哈希网格）

✅ **代码质量高**：

- 源自 David Eberly 的 WildMagic5/GTEngine（几何计算领域的经典库）
- 经过多年实战验证
- 纯 C# 实现，类型安全

✅ **商业友好**：

- Boost License 允许商业使用
- 无需开源衍生代码
- 无专利限制

✅ **平台兼容**：

- 支持 .NET Framework 4.8（Navisworks 2026 要求）
- 无 Unity 依赖，纯 .NET 库
- 可与 Navisworks API 无缝集成

✅ **性能优化**：

- 多线程支持（Marching Cubes 等）
- 内存高效（稀疏网格、窄带 SDF）
- 空间索引加速查询

⚠️ **不足之处**：

- 不包含路径规划算法（需结合 Roy-T.AStar）
- 文档以 README 为主，缺少专门的 API 文档网站
- 活跃度中等（原项目 2019 年后维护减少）

#### 与 Roy-T.AStar 的集成策略

geometry4Sharp 提供**空间表示**，Roy-T.AStar 提供**路径算法**：

```graph
┌────────────────────────────────────────┐
│ Navisworks BIM 模型                    │
│ (三角网格、包围盒)                     │
└──────────────┬─────────────────────────┘
               │
               ▼
┌────────────────────────────────────────┐
│ geometry4Sharp                         │
│ - MeshSignedDistanceGrid (体素化)     │
│ - Distance Field (距离计算)           │
│ - IsInside (可通行性判断)             │
└──────────────┬─────────────────────────┘
               │
               ▼
┌────────────────────────────────────────┐
│ VoxelGrid 适配层                       │
│ - 转换为 Roy-T.AStar Grid              │
│ - 或构建自定义 3D Graph                │
└──────────────┬─────────────────────────┘
               │
               ▼
┌────────────────────────────────────────┐
│ Roy-T.AStar                            │
│ - A* 路径规划算法                      │
│ - PathFinder.FindPath()                │
└──────────────┬─────────────────────────┘
               │
               ▼
┌────────────────────────────────────────┐
│ PathOptimizer                          │
│ - 路径平滑、简化                       │
└────────────────────────────────────────┘
```

---

## 3. 架构设计

### 3.1 整体架构

```graph
┌──────────────────────────────────────────────────────────┐
│                     Navisworks API                        │
│                (ModelItem, BoundingBox, Geometry)         │
└────────────────────────┬─────────────────────────────────┘
                         │
                         ▼
┌──────────────────────────────────────────────────────────┐
│            NavisworksGeometryExtractor                    │
│  - ExtractTriangleMesh(): DMesh3                         │
│  - ExtractBoundingBoxes(): List<AxisAlignedBox3d>       │
└────────────────────────┬─────────────────────────────────┘
                         │
                         ▼
┌──────────────────────────────────────────────────────────┐
│              VoxelGridGenerator (新)                      │
│  - GenerateFromBIM()                                     │
│  ├── MeshSignedDistanceGrid (geometry4Sharp)            │
│  ├── 体素标记 (Obstacle/Free/Door/...)                   │
│  └── 生成 VoxelGrid                                      │
└────────────────────────┬─────────────────────────────────┘
                         │
                         ▼
┌──────────────────────────────────────────────────────────┐
│                   VoxelGrid (新)                          │
│  - cells: VoxelCell[X, Y, Z]                            │
│  - IsPassable(x, y, z): bool                            │
│  - GetDistance(x, y, z): double                         │
│  - GetNeighbors(x, y, z): List<(int,int,int)>          │
└────────────────────────┬─────────────────────────────────┘
                         │
                         ▼
┌──────────────────────────────────────────────────────────┐
│           VoxelPathFinder (新，替代 AutoPathFinder)       │
│  - BuildGraph3D(): 构建 3D 节点图                        │
│  - FindPath(): 使用 Roy-T.AStar 进行 3D A* 搜索         │
│  - ConvertToWorldPath(): 体素坐标 → 世界坐标            │
└────────────────────────┬─────────────────────────────────┘
                         │
                         ▼
┌──────────────────────────────────────────────────────────┐
│                   PathOptimizer                          │
│  - SimplifyPath() (保留，适配 3D 路径)                  │
└────────────────────────┬─────────────────────────────────┘
                         │
                         ▼
┌──────────────────────────────────────────────────────────┐
│            LogisticsAnimationManager                     │
│  - AnimatePath() (无需修改)                              │
└──────────────────────────────────────────────────────────┘
```

### 3.2 核心类设计

#### 3.2.1 VoxelCell - 体素单元

```csharp
/// <summary>
/// 3D 体素单元，表示空间中的一个立方体区域
/// </summary>
public class VoxelCell
{
    /// <summary>
    /// 体素类型（障碍物、通道、门、楼梯等）
    /// </summary>
    public LogisticsElementType Type { get; set; }

    /// <summary>
    /// 是否可通行
    /// </summary>
    public bool IsPassable { get; set; }

    /// <summary>
    /// 到最近障碍物表面的距离（来自 SDF）
    /// </summary>
    public double Distance { get; set; }

    /// <summary>
    /// 速度限制（米/秒），用于门、楼梯等特殊区域
    /// </summary>
    public double SpeedLimit { get; set; }

    /// <summary>
    /// 关联的 Navisworks 模型元素（可选）
    /// </summary>
    public ModelItem SourceItem { get; set; }

    /// <summary>
    /// 通行成本（基于距离、类型、速度限制计算）
    /// </summary>
    public double Cost { get; set; }
}
```

#### 3.2.2 VoxelGrid - 3D 体素网格

```csharp
/// <summary>
/// 3D 体素网格，表示整个路径规划空间
/// </summary>
public class VoxelGrid
{
    #region 字段

    /// <summary>
    /// 体素数组 [X, Y, Z]
    /// </summary>
    private VoxelCell[,,] cells;

    /// <summary>
    /// 世界坐标原点（网格左下后角）
    /// </summary>
    public Point3D Origin { get; private set; }

    /// <summary>
    /// 体素大小（米）
    /// </summary>
    public double VoxelSize { get; private set; }

    /// <summary>
    /// 网格尺寸（体素数量）
    /// </summary>
    public int SizeX { get; private set; }
    public int SizeY { get; private set; }
    public int SizeZ { get; private set; }

    #endregion

    #region 构造函数

    public VoxelGrid(Point3D origin, double voxelSize, int sizeX, int sizeY, int sizeZ)
    {
        Origin = origin;
        VoxelSize = voxelSize;
        SizeX = sizeX;
        SizeY = sizeY;
        SizeZ = sizeZ;
        cells = new VoxelCell[sizeX, sizeY, sizeZ];

        // 初始化所有体素为空
        for (int x = 0; x < sizeX; x++)
        {
            for (int y = 0; y < sizeY; y++)
            {
                for (int z = 0; z < sizeZ; z++)
                {
                    cells[x, y, z] = new VoxelCell
                    {
                        IsPassable = true,
                        Distance = double.MaxValue,
                        Type = LogisticsElementType.通道
                    };
                }
            }
        }
    }

    #endregion

    #region 坐标转换

    /// <summary>
    /// 世界坐标 → 体素索引
    /// </summary>
    public (int x, int y, int z) WorldToVoxel(Point3D worldPos)
    {
        int x = (int)Math.Floor((worldPos.X - Origin.X) / VoxelSize);
        int y = (int)Math.Floor((worldPos.Y - Origin.Y) / VoxelSize);
        int z = (int)Math.Floor((worldPos.Z - Origin.Z) / VoxelSize);
        return (x, y, z);
    }

    /// <summary>
    /// 体素索引 → 世界坐标（体素中心）
    /// </summary>
    public Point3D VoxelToWorld(int x, int y, int z)
    {
        double worldX = Origin.X + (x + 0.5) * VoxelSize;
        double worldY = Origin.Y + (y + 0.5) * VoxelSize;
        double worldZ = Origin.Z + (z + 0.5) * VoxelSize;
        return new Point3D(worldX, worldY, worldZ);
    }

    #endregion

    #region 体素访问

    /// <summary>
    /// 检查索引是否在网格范围内
    /// </summary>
    public bool IsValidIndex(int x, int y, int z)
    {
        return x >= 0 && x < SizeX &&
               y >= 0 && y < SizeY &&
               z >= 0 && z < SizeZ;
    }

    /// <summary>
    /// 获取体素（安全访问）
    /// </summary>
    public VoxelCell GetVoxel(int x, int y, int z)
    {
        if (!IsValidIndex(x, y, z))
            return null;
        return cells[x, y, z];
    }

    /// <summary>
    /// 设置体素
    /// </summary>
    public void SetVoxel(int x, int y, int z, VoxelCell voxel)
    {
        if (IsValidIndex(x, y, z))
            cells[x, y, z] = voxel;
    }

    /// <summary>
    /// 检查体素是否可通行
    /// </summary>
    public bool IsPassable(int x, int y, int z)
    {
        var voxel = GetVoxel(x, y, z);
        return voxel != null && voxel.IsPassable;
    }

    #endregion

    #region 邻居查询

    /// <summary>
    /// 获取 26 邻域体素（6 面 + 12 边 + 8 角）
    /// </summary>
    public List<(int x, int y, int z)> GetNeighbors26(int x, int y, int z)
    {
        var neighbors = new List<(int, int, int)>();

        for (int dx = -1; dx <= 1; dx++)
        {
            for (int dy = -1; dy <= 1; dy++)
            {
                for (int dz = -1; dz <= 1; dz++)
                {
                    if (dx == 0 && dy == 0 && dz == 0)
                        continue; // 跳过自己

                    int nx = x + dx;
                    int ny = y + dy;
                    int nz = z + dz;

                    if (IsValidIndex(nx, ny, nz) && IsPassable(nx, ny, nz))
                    {
                        neighbors.Add((nx, ny, nz));
                    }
                }
            }
        }

        return neighbors;
    }

    /// <summary>
    /// 获取 6 邻域体素（仅面相邻）
    /// </summary>
    public List<(int x, int y, int z)> GetNeighbors6(int x, int y, int z)
    {
        var neighbors = new List<(int, int, int)>();
        var directions = new[]
        {
            (1, 0, 0), (-1, 0, 0),  // X 轴
            (0, 1, 0), (0, -1, 0),  // Y 轴
            (0, 0, 1), (0, 0, -1)   // Z 轴
        };

        foreach (var (dx, dy, dz) in directions)
        {
            int nx = x + dx;
            int ny = y + dy;
            int nz = z + dz;

            if (IsValidIndex(nx, ny, nz) && IsPassable(nx, ny, nz))
            {
                neighbors.Add((nx, ny, nz));
            }
        }

        return neighbors;
    }

    #endregion

    #region 统计信息

    /// <summary>
    /// 统计可通行体素数量
    /// </summary>
    public int CountPassableVoxels()
    {
        int count = 0;
        for (int x = 0; x < SizeX; x++)
        {
            for (int y = 0; y < SizeY; y++)
            {
                for (int z = 0; z < SizeZ; z++)
                {
                    if (cells[x, y, z].IsPassable)
                        count++;
                }
            }
        }
        return count;
    }

    #endregion
}
```

#### 3.2.3 VoxelGridGenerator - 体素网格生成器

```csharp
/// <summary>
/// 从 Navisworks BIM 模型生成体素网格
/// </summary>
public class VoxelGridGenerator
{
    #region 配置参数

    private readonly double voxelSize;         // 体素大小（米）
    private readonly double vehicleRadius;     // 车辆半径（米）
    private readonly double vehicleHeight;     // 车辆高度（米）
    private readonly double safetyMargin;      // 安全间隙（米）

    #endregion

    #region 构造函数

    public VoxelGridGenerator(
        double voxelSizeMeters,
        double vehicleRadiusMeters,
        double vehicleHeightMeters,
        double safetyMarginMeters)
    {
        this.voxelSize = voxelSizeMeters;
        this.vehicleRadius = vehicleRadiusMeters;
        this.vehicleHeight = vehicleHeightMeters;
        this.safetyMargin = safetyMarginMeters;
    }

    #endregion

    #region 主生成方法

    /// <summary>
    /// 从 BIM 模型生成体素网格
    /// </summary>
    public VoxelGrid GenerateFromBIM(
        BoundingBox3D worldBounds,
        IEnumerable<ModelItem> obstacles,
        IEnumerable<ModelItem> channels,
        IEnumerable<ModelItem> doors)
    {
        LogManager.Info("[体素网格生成] 开始生成...");
        var sw = Stopwatch.StartNew();

        // 1. 创建空网格
        VoxelGrid grid = CreateEmptyGrid(worldBounds);
        LogManager.Info($"[体素网格生成] 网格尺寸: {grid.SizeX} × {grid.SizeY} × {grid.SizeZ}");

        // 2. 使用 geometry4Sharp 生成距离场
        MeshSignedDistanceGrid sdf = GenerateDistanceField(obstacles);
        LogManager.Info($"[体素网格生成] 距离场计算完成");

        // 3. 标记体素类型
        MarkVoxels(grid, sdf);
        LogManager.Info($"[体素网格生成] 体素标记完成");

        // 4. 处理特殊元素（门、通道等）
        ProcessSpecialElements(grid, doors, channels);

        sw.Stop();
        LogManager.Info($"[体素网格生成] 完成，耗时: {sw.ElapsedMilliseconds}ms");
        LogManager.Info($"[体素网格生成] 可通行体素: {grid.CountPassableVoxels()}");

        return grid;
    }

    #endregion

    #region 辅助方法

    /// <summary>
    /// 创建空的体素网格
    /// </summary>
    private VoxelGrid CreateEmptyGrid(BoundingBox3D bounds)
    {
        Point3D origin = bounds.Min;

        int sizeX = (int)Math.Ceiling((bounds.Max.X - bounds.Min.X) / voxelSize);
        int sizeY = (int)Math.Ceiling((bounds.Max.Y - bounds.Min.Y) / voxelSize);
        int sizeZ = (int)Math.Ceiling((bounds.Max.Z - bounds.Min.Z) / voxelSize);

        return new VoxelGrid(origin, voxelSize, sizeX, sizeY, sizeZ);
    }

    /// <summary>
    /// 使用 geometry4Sharp 生成距离场
    /// </summary>
    private MeshSignedDistanceGrid GenerateDistanceField(IEnumerable<ModelItem> obstacles)
    {
        // 1. 提取障碍物三角网格
        DMesh3 obstacleMesh = ExtractTriangleMesh(obstacles);

        // 2. 计算网格包围盒
        AxisAlignedBox3d meshBounds = obstacleMesh.GetBounds();

        // 3. 创建距离场网格
        int numCells = (int)Math.Ceiling(meshBounds.MaxDim / voxelSize);
        MeshSignedDistanceGrid sdf = new MeshSignedDistanceGrid(
            obstacleMesh,
            cellSize: voxelSize,
            numCells: numCells
        );

        // 4. 计算距离场（可能耗时）
        sdf.Compute();

        return sdf;
    }

    /// <summary>
    /// 从 Navisworks ModelItem 提取三角网格
    /// </summary>
    private DMesh3 ExtractTriangleMesh(IEnumerable<ModelItem> items)
    {
        DMesh3 mesh = new DMesh3();

        foreach (var item in items)
        {
            // 使用 Navisworks API 获取几何体
            // 转换为 geometry4Sharp 的 DMesh3 格式
            // 这部分需要实现 Navisworks Geometry → DMesh3 的转换

            // 伪代码示例：
            // var navisGeometry = item.Geometry;
            // foreach (var triangle in navisGeometry.Triangles)
            // {
            //     var v1 = mesh.AppendVertex(triangle.V1);
            //     var v2 = mesh.AppendVertex(triangle.V2);
            //     var v3 = mesh.AppendVertex(triangle.V3);
            //     mesh.AppendTriangle(v1, v2, v3);
            // }
        }

        return mesh;
    }

    /// <summary>
    /// 根据距离场标记体素
    /// </summary>
    private void MarkVoxels(VoxelGrid grid, MeshSignedDistanceGrid sdf)
    {
        double minPassableDistance = vehicleRadius + safetyMargin;

        for (int x = 0; x < grid.SizeX; x++)
        {
            for (int y = 0; y < grid.SizeY; y++)
            {
                for (int z = 0; z < grid.SizeZ; z++)
                {
                    // 体素中心的世界坐标
                    Point3D worldPos = grid.VoxelToWorld(x, y, z);

                    // 查询距离场
                    Vector3d point = new Vector3d(worldPos.X, worldPos.Y, worldPos.Z);
                    double distance = sdf[point];

                    // 判断可通行性
                    var voxel = grid.GetVoxel(x, y, z);
                    voxel.Distance = distance;
                    voxel.IsPassable = distance >= minPassableDistance;
                    voxel.Type = voxel.IsPassable
                        ? LogisticsElementType.通道
                        : LogisticsElementType.障碍物;
                }
            }
        }
    }

    /// <summary>
    /// 处理特殊元素（门、楼梯等）
    /// </summary>
    private void ProcessSpecialElements(
        VoxelGrid grid,
        IEnumerable<ModelItem> doors,
        IEnumerable<ModelItem> channels)
    {
        // 处理门元素
        foreach (var door in doors)
        {
            BoundingBox3D doorBBox = door.BoundingBox();
            MarkRegion(grid, doorBBox, LogisticsElementType.门,
                speedLimit: 0.5, isPassable: true);
        }

        // 处理通道元素
        foreach (var channel in channels)
        {
            BoundingBox3D channelBBox = channel.BoundingBox();
            MarkRegion(grid, channelBBox, LogisticsElementType.通道,
                speedLimit: 1.0, isPassable: true);
        }
    }

    /// <summary>
    /// 标记指定区域的体素
    /// </summary>
    private void MarkRegion(
        VoxelGrid grid,
        BoundingBox3D bounds,
        LogisticsElementType type,
        double speedLimit,
        bool isPassable)
    {
        var (minX, minY, minZ) = grid.WorldToVoxel(bounds.Min);
        var (maxX, maxY, maxZ) = grid.WorldToVoxel(bounds.Max);

        for (int x = minX; x <= maxX; x++)
        {
            for (int y = minY; y <= maxY; y++)
            {
                for (int z = minZ; z <= maxZ; z++)
                {
                    var voxel = grid.GetVoxel(x, y, z);
                    if (voxel != null)
                    {
                        voxel.Type = type;
                        voxel.IsPassable = isPassable;
                        voxel.SpeedLimit = speedLimit;
                    }
                }
            }
        }
    }

    #endregion
}
```

#### 3.2.4 VoxelPathFinder - 体素路径规划器

```csharp
/// <summary>
/// 基于体素网格的 3D 路径规划
/// </summary>
public class VoxelPathFinder
{
    private readonly VoxelGrid voxelGrid;

    public VoxelPathFinder(VoxelGrid voxelGrid)
    {
        this.voxelGrid = voxelGrid;
    }

    #region 路径规划

    /// <summary>
    /// 查找 3D 路径
    /// </summary>
    public List<Point3D> FindPath(Point3D start, Point3D end)
    {
        LogManager.Info($"[体素路径规划] 开始: {start} → {end}");
        var sw = Stopwatch.StartNew();

        // 1. 世界坐标 → 体素索引
        var startVoxel = voxelGrid.WorldToVoxel(start);
        var endVoxel = voxelGrid.WorldToVoxel(end);

        // 2. 检查起终点是否可通行
        if (!voxelGrid.IsPassable(startVoxel.x, startVoxel.y, startVoxel.z))
        {
            LogManager.Error($"[体素路径规划] 起点不可通行: {startVoxel}");
            return null;
        }
        if (!voxelGrid.IsPassable(endVoxel.x, endVoxel.y, endVoxel.z))
        {
            LogManager.Error($"[体素路径规划] 终点不可通行: {endVoxel}");
            return null;
        }

        // 3. 构建 3D 图（如果使用 Roy-T.AStar 的 Graph 模式）
        //    或者直接使用自定义 A* 实现

        // 方案 A：使用 Roy-T.AStar Grid 模式（需扩展到 3D）
        // 方案 B：自定义 3D A* 实现
        var voxelPath = FindPathAStar(startVoxel, endVoxel);

        if (voxelPath == null || voxelPath.Count == 0)
        {
            LogManager.Error($"[体素路径规划] 未找到路径");
            return null;
        }

        // 4. 体素坐标 → 世界坐标
        var worldPath = ConvertToWorldPath(voxelPath);

        sw.Stop();
        LogManager.Info($"[体素路径规划] 完成，耗时: {sw.ElapsedMilliseconds}ms，路径点: {worldPath.Count}");

        return worldPath;
    }

    #endregion

    #region A* 实现

    /// <summary>
    /// 3D A* 路径搜索（简化实现）
    /// </summary>
    private List<(int x, int y, int z)> FindPathAStar(
        (int x, int y, int z) start,
        (int x, int y, int z) goal)
    {
        // 使用优先队列实现 A*
        var openSet = new PriorityQueue<(int, int, int), double>();
        var cameFrom = new Dictionary<(int, int, int), (int, int, int)>();
        var gScore = new Dictionary<(int, int, int), double>();
        var fScore = new Dictionary<(int, int, int), double>();

        openSet.Enqueue(start, 0);
        gScore[start] = 0;
        fScore[start] = Heuristic(start, goal);

        while (openSet.Count > 0)
        {
            var current = openSet.Dequeue();

            // 找到目标
            if (current.Equals(goal))
            {
                return ReconstructPath(cameFrom, current);
            }

            // 遍历邻居（26 邻域）
            foreach (var neighbor in voxelGrid.GetNeighbors26(current.x, current.y, current.z))
            {
                double tentativeGScore = gScore[current] + MoveCost(current, neighbor);

                if (!gScore.ContainsKey(neighbor) || tentativeGScore < gScore[neighbor])
                {
                    cameFrom[neighbor] = current;
                    gScore[neighbor] = tentativeGScore;
                    fScore[neighbor] = gScore[neighbor] + Heuristic(neighbor, goal);

                    if (!openSet.Contains(neighbor))
                    {
                        openSet.Enqueue(neighbor, fScore[neighbor]);
                    }
                }
            }
        }

        return null; // 未找到路径
    }

    /// <summary>
    /// A* 启发式函数（欧几里得距离）
    /// </summary>
    private double Heuristic((int x, int y, int z) a, (int x, int y, int z) b)
    {
        int dx = a.x - b.x;
        int dy = a.y - b.y;
        int dz = a.z - b.z;
        return Math.Sqrt(dx * dx + dy * dy + dz * dz) * voxelGrid.VoxelSize;
    }

    /// <summary>
    /// 移动成本（考虑距离、类型、速度限制）
    /// </summary>
    private double MoveCost((int x, int y, int z) from, (int x, int y, int z) to)
    {
        // 欧几里得距离
        int dx = to.x - from.x;
        int dy = to.y - from.y;
        int dz = to.z - from.z;
        double distance = Math.Sqrt(dx * dx + dy * dy + dz * dz) * voxelGrid.VoxelSize;

        // 考虑目标体素的速度限制
        var voxel = voxelGrid.GetVoxel(to.x, to.y, to.z);
        double speedFactor = 1.0;
        if (voxel != null && voxel.SpeedLimit > 0)
        {
            speedFactor = 1.0 / voxel.SpeedLimit; // 速度越慢，成本越高
        }

        return distance * speedFactor;
    }

    /// <summary>
    /// 重建路径
    /// </summary>
    private List<(int x, int y, int z)> ReconstructPath(
        Dictionary<(int, int, int), (int, int, int)> cameFrom,
        (int x, int y, int z) current)
    {
        var path = new List<(int, int, int)> { current };

        while (cameFrom.ContainsKey(current))
        {
            current = cameFrom[current];
            path.Insert(0, current);
        }

        return path;
    }

    #endregion

    #region 路径转换

    /// <summary>
    /// 体素路径 → 世界坐标路径
    /// </summary>
    private List<Point3D> ConvertToWorldPath(List<(int x, int y, int z)> voxelPath)
    {
        var worldPath = new List<Point3D>();

        foreach (var (x, y, z) in voxelPath)
        {
            Point3D worldPos = voxelGrid.VoxelToWorld(x, y, z);
            worldPath.Add(worldPos);
        }

        return worldPath;
    }

    #endregion
}
```

---

## 4. 实施计划

### 4.1 阶段划分

#### 阶段 1：环境搭建与原型验证（3-5 天）

**任务**：

1. **安装 geometry4Sharp**
   - NuGet 添加 `geometry4Sharp` 1.0.0
   - 验证与 .NET Framework 4.8 兼容性
   - 运行示例代码测试

2. **创建简单原型**
   - 从 Navisworks 提取简单场景（如单个房间）
   - 生成体素网格（使用 `Bitmap3` 或 `DSparseGrid3`）
   - 可视化体素网格（在 Navisworks 中显示为小方块）

3. **距离场测试**
   - 提取障碍物网格转为 `DMesh3`
   - 使用 `MeshSignedDistanceGrid` 计算 SDF
   - 验证距离场的正确性

**验收标准**：

- ✅ geometry4Sharp 成功集成
- ✅ 能从 Navisworks 模型生成简单体素网格
- ✅ 能在 Navisworks 3D 视图中可视化体素

---

#### 阶段 2：核心功能开发（7-10 天）

**任务**：

1. **实现 VoxelGrid 类**
   - 完整的体素网格数据结构
   - 坐标转换方法
   - 邻居查询（6 邻域、26 邻域）
   - 统计和查询方法

2. **实现 VoxelGridGenerator**
   - Navisworks Geometry → geometry4Sharp DMesh3 转换
   - 距离场计算集成
   - 体素标记逻辑（障碍物、通道、门等）
   - 特殊元素处理（门、楼梯）

3. **实现 VoxelPathFinder**
   - 3D A* 算法实现
   - 或集成 Roy-T.AStar 的 Graph 模式
   - 路径坐标转换
   - 成本计算（距离 + 速度限制）

4. **集成到现有系统**
   - 替换 `GridMapGenerator` 为 `VoxelGridGenerator`
   - 替换 `AutoPathFinder` 为 `VoxelPathFinder`
   - 保持 `PathOptimizer` 不变（适配 3D 路径）

**验收标准**：

- ✅ 能从复杂 BIM 模型生成体素网格
- ✅ 能找到 3D 路径（包括垂直移动）
- ✅ 路径避开障碍物并穿过门/通道
- ✅ 性能可接受（中等规模场景 < 5 秒）

---

#### 阶段 3：优化与测试（5-7 天）

**任务**：

1. **性能优化**
   - 使用 `BiGrid3` 稀疏网格减少内存
   - 窄带 SDF（仅计算障碍物附近区域）
   - A*算法优化（JPS-3D、Theta* 等）
   - 多线程并行化（网格生成、路径规划）

2. **内存优化**
   - 大规模场景的内存占用评估
   - 分块生成和加载
   - 按需计算距离场

3. **测试场景**
   - 简单场景（单层房间）
   - 中等场景（多层建筑）
   - 复杂场景（大型工厂、管道密集）
   - 边界测试（超大规模、极小体素）

4. **可视化增强**
   - 体素网格的 3D 渲染（彩色方块）
   - 路径的 3D 动画展示
   - 距离场的热图可视化

**验收标准**：

- ✅ 大规模场景（> 100m³）可在合理时间内生成（< 30 秒）
- ✅ 内存占用可控（< 2GB）
- ✅ 所有测试场景路径规划成功
- ✅ 可视化效果直观

---

#### 阶段 4：文档与集成（2-3 天）

**任务**：

1. **代码文档**
   - XML 注释完善
   - 关键类和方法的使用说明
   - 示例代码

2. **用户文档**
   - 体素网格参数配置说明
   - 与 2.5D 方案的对比
   - 性能调优建议

3. **配置文件更新**
   - 添加体素网格相关配置项
   - 向后兼容 2.5D 模式（可选）

4. **单元测试**
   - VoxelGrid 测试
   - VoxelPathFinder 测试
   - 性能基准测试

**验收标准**：

- ✅ 完整的代码和用户文档
- ✅ 配置文件支持体素网格参数
- ✅ 单元测试覆盖率 > 70%

---

### 4.2 时间与资源估算

| 阶段 | 任务 | 工作量 | 依赖 |
|------|------|--------|------|
| 阶段 1 | 环境搭建与原型 | 3-5 天 | geometry4Sharp |
| 阶段 2 | 核心功能开发 | 7-10 天 | 阶段 1 |
| 阶段 3 | 优化与测试 | 5-7 天 | 阶段 2 |
| 阶段 4 | 文档与集成 | 2-3 天 | 阶段 3 |
| **总计** | | **17-25 天** | |

**资源需求**：

- 1 名全职开发人员
- Navisworks 2026 开发环境
- 测试 BIM 模型（不同规模和复杂度）
- 性能测试硬件（推荐 16GB+ RAM）

---

## 5. 风险与缓解

### 5.1 技术风险

#### 风险 1：geometry4Sharp 维护状况

**描述**：geometry4Sharp 原项目活跃度中等，可能缺少最新功能或 bug 修复。

**影响**：可能遇到无法解决的 bug 或性能问题。

**缓解措施**：

- ✅ geometry4Sharp 是 fork，源代码可访问，可自行修复
- ✅ 代码基于成熟的 WildMagic5/GTEngine，质量高
- ✅ Boost License 允许自由修改
- ✅ 如需要可 fork 自己维护版本

---

#### 风险 2：Navisworks Geometry 提取复杂性

**描述**：Navisworks API 的几何体提取可能复杂，转换为 DMesh3 可能困难。

**影响**：体素化可能不准确，或性能不佳。

**缓解措施**：

- ✅ 可使用包围盒代替精确网格（降低精度但简化实现）
- ✅ 分阶段实现：先包围盒体素化，再精确网格体素化
- ✅ 参考现有 GeometryExtractor 代码

---

#### 风险 3：内存占用

**描述**：大规模场景的体素网格可能占用大量内存（如 1000³ 体素 = 10亿个单元格）。

**影响**：内存不足导致程序崩溃。

**缓解措施**：

- ✅ 使用 `DSparseGrid3` 稀疏网格（仅存储非空体素）
- ✅ 窄带 SDF（仅计算障碍物附近区域）
- ✅ 分块加载和生成
- ✅ 限制体素网格的最大尺寸

---

#### 风险 4：路径规划性能

**描述**：3D A* 搜索空间比 2D 大得多（N³ vs N²），可能导致性能下降。

**影响**：路径规划耗时过长（> 5 秒），用户体验差。

**缓解措施**：

- ✅ 使用更高效的算法（JPS-3D、Theta*、Lazy Theta*）
- ✅ 启发式函数优化（加权 A*）
- ✅ 多线程并行搜索（多个起终点）
- ✅ 路径缓存（重复路径查询）
- ✅ 使用更大的体素（降低分辨率）

---

### 5.2 项目风险

#### 风险 5：时间超期

**描述**：实施过程中遇到意外困难，导致时间超出预期。

**影响**：延迟其他功能开发，影响项目进度。

**缓解措施**：

- ✅ 分阶段实施，每个阶段有明确的验收标准
- ✅ 阶段 1 完成后评估，决定是否继续
- ✅ 保留 2.5D 方案作为备选（双模式切换）

---

#### 风险 6：与现有代码冲突

**描述**：新体素系统与现有代码不兼容，需要大量重构。

**影响**：工作量增加，可能引入新 bug。

**缓解措施**：

- ✅ 新旧系统并存（配置开关）
- ✅ 保持接口一致性（PathFinder、PathOptimizer）
- ✅ 充分测试确保向后兼容

---

## 6. 配置文件扩展

### 6.1 新增配置项

在 `config.toml` 中添加体素网格相关配置：

```toml
[path_planning]
# 路径规划模式：grid_2d5（当前 2.5D 网格）、voxel_3d（新体素网格）
mode = "grid_2d5"  # 默认保持向后兼容

[voxel_grid]
# 体素大小（米），越小越精确但内存和计算量越大
voxel_size_meters = 0.5

# 是否使用稀疏网格（推荐大规模场景）
use_sparse_grid = true

# 是否使用窄带距离场（仅计算障碍物附近区域）
use_narrow_band = true
narrow_band_width_meters = 2.0  # 窄带宽度（米）

# 邻域类型：6（面相邻）、26（面+边+角相邻）
neighbor_type = 26

# 是否可视化体素网格
visualize_voxels = false

# 是否可视化距离场（热图）
visualize_distance_field = false

[path_planning_3d]
# 3D A* 算法类型：astar、jps3d、theta_star
algorithm_type = "astar"

# 启发式权重（> 1.0 加速但可能不是最优路径）
heuristic_weight = 1.0

# 是否允许对角线移动
allow_diagonal = true

# 是否允许垂直移动
allow_vertical = true

# 垂直移动的额外成本系数（> 1.0 表示更倾向于水平移动）
vertical_cost_factor = 1.5
```

---

## 7. 性能预期

### 7.1 性能对比（预估）

基于类似规模的 3D 体素路径规划系统经验：

| 指标 | 2.5D 网格（当前） | 3D 体素网格（预期） | 说明 |
|------|-------------------|---------------------|------|
| **网格生成** | 1-3 秒 | 3-10 秒 | 体素化和 SDF 计算耗时 |
| **路径规划** | 50-200 ms | 100-500 ms | 3D 搜索空间更大 |
| **内存占用** | 50-200 MB | 100-500 MB | 体素数据 + SDF |
| **路径精度** | 中等 | 高 | 真 3D 路径更精确 |
| **复杂场景支持** | 有限 | 优秀 | 管道、桥梁等复杂结构 |

**场景假设**：

- 建筑规模：100m × 100m × 20m
- 体素大小：0.5m
- 体素数量：200 × 200 × 40 = 160 万个
- 稀疏网格实际占用：20-30% = 32-48 万个

### 7.2 优化目标

**短期目标**（阶段 2 完成后）：

- ✅ 网格生成 < 15 秒（中等规模场景）
- ✅ 路径规划 < 1 秒（单次查询）
- ✅ 内存占用 < 500 MB

**长期目标**（阶段 3 优化后）：

- ✅ 网格生成 < 10 秒
- ✅ 路径规划 < 500 ms
- ✅ 内存占用 < 300 MB

---

## 8. 替代方案评估

### 8.1 方案对比

| 方案 | 优点 | 缺点 | 推荐度 |
|------|------|------|--------|
| **geometry4Sharp** | 功能完整、代码质量高、商业友好 | 活跃度中等、文档一般 | ⭐⭐⭐⭐⭐ |
| **自行实现** | 完全控制、轻量级 | 工作量大、需实现 SDF | ⭐⭐⭐ |
| **Unity 相关库** | 活跃、文档好 | Unity 依赖、难以剥离 | ⭐⭐ |
| **保持 2.5D** | 无开发成本、稳定 | 功能受限、无法真 3D | ⭐⭐ |

### 8.2 决策建议

**推荐方案**：**geometry4Sharp**

**理由**：

1. ✅ 功能完整，提供完整的体素化和距离场工具链
2. ✅ 代码质量高，源自经典几何库 WildMagic5/GTEngine
3. ✅ 商业友好的 Boost License
4. ✅ 纯 C#，与 Navisworks 完美兼容
5. ✅ 即使维护减少，代码仍可用且可自行修改

**备选方案**：如果 geometry4Sharp 不满足需求，可考虑：

- 轻量级自行实现（基于简单的 3D 数组 + 基础距离计算）
- Fork geometry4Sharp 自行维护

---

## 9. 总结与建议

### 9.1 核心结论

1. **体素网格是 3D 路径规划的理想方案**
   - 统一的 3D 空间表示
   - 简化的路径算法
   - 更精确的障碍物表示

2. **geometry4Sharp 是最佳的 C# 体素库**
   - 功能完整、代码质量高
   - 商业友好、平台兼容
   - 可满足项目需求

3. **实施是可行的**
   - 清晰的技术路线
   - 合理的时间估算（17-25 天）
   - 风险可控

### 9.2 建议行动

**立即行动**：

1. ✅ 安装 geometry4Sharp NuGet 包
2. ✅ 创建原型验证可行性（阶段 1）
3. ✅ 评估性能和内存占用

**后续行动**：

- 根据原型结果决定是否全面实施
- 如果可行，按阶段 2-4 计划推进
- 保持 2.5D 方案作为备选（双模式）

### 9.3 成功标准

项目成功的标志：

- ✅ 能处理真正的 3D 路径（管道下方、桥梁下方、多层重叠）
- ✅ 路径精度优于 2.5D 方案
- ✅ 性能可接受（网格生成 < 15 秒，路径规划 < 1 秒）
- ✅ 内存占用可控（< 500 MB）
- ✅ 代码质量高、可维护性好

---

## 10. 参考资料

### 10.1 geometry4Sharp 资源

- **GitHub**: <https://github.com/NewWheelTech/geometry4Sharp>
- **NuGet**: <https://www.nuget.org/packages/geometry4Sharp>
- **原版文档**: <http://www.gradientspace.com/tutorials/> (geometry3Sharp)
- **原作者**: Ryan Schmidt (@rms80)
- **Fork 维护者**: New Wheel Technology

### 10.2 理论基础

- **Signed Distance Field (SDF)**:
  - SDFGen (Christopher Batty, Robert Bridson)
  - "Signed Distance Field Tutorial" - gradientspace.com

- **Marching Cubes**:
  - "Marching Cubes: A High Resolution 3D Surface Construction Algorithm" (Lorensen & Cline, 1987)

- **3D Pathfinding**:
  - "3D Flight Navigation Using Sparse Voxel Octrees" (Daniel Brewer, Game AI Pro 3)
  - "Pathfinding in 3D Space: A*, Theta*, Lazy Theta*" (ASCANE)

### 10.3 相关项目

- **WildMagic5**: <https://www.geometrictools.com/> (David Eberly)
- **Roy-T.AStar**: <https://github.com/roy-t/AStar>
- **Unity PathFinding3D**: <https://github.com/Mjkp/PathFinding3D>
- **Nav3D (UE5)**: <https://github.com/darbycostello/Nav3D>

---

**文档版本**: v1.0
**创建日期**: 2025-10-12
**最后更新**: 2025-10-12
**作者**: NavisworksTransport 开发团队