tian/NavisworksTransport

Fork 0

tian 1a3d1e7f49 清理多余文件

2025-10-13 16:53:40 +08:00

44 KiB

Raw Blame History

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

1. 背景与动机

1.1 当前方案的局限性

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

数据结构：

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

局限性：

高度层离散化
- 只能表示有限数量的通行高度
- 相邻高度层之间的连接需要特殊处理
- 难以精确表示连续变化的高度
复杂空间的表达受限
- 管道、桥梁下方的多层通道难以准确建模
- 斜坡、楼梯的连续高度变化需要大量高度层
- 悬空结构（如天桥）的下方空间处理复杂
路径规划的限制
- 垂直方向的移动需要显式的"层间连接"
- 真正的 3D 路径（如无人机、爬楼机器人）无法支持
- 算法复杂度高（需要处理层间跳转逻辑）
可扩展性问题
- 增加高度层数会显著增加内存消耗
- 多层网格的生成和维护成本高
- 难以支持动态障碍物的实时更新

1.2 体素网格的优势

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

统一的 3D 表示：

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

优势：

✅ 连续的 3D 空间：任意高度都可以表示
✅ 简化的路径算法：标准 3D A* 算法，无需特殊层间逻辑
✅ 精确的障碍物表示：管道、横梁等任意 3D 形状
✅ 更好的可视化：直观的 3D 体素显示
✅ 支持真 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.体素化与距离场

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

功能说明：

MeshSignedDistanceGrid:
- 从三角网格生成签名距离场 (SDF)
- 使用快速行进法 (Fast Marching Method)
- 支持窄带构建（仅计算障碍物附近区域，节省内存）
- 基于 Christopher Batty 的 SDFGen C++ 代码移植
Bitmap3:
- 密集 3D 布尔数组
- 高效的内存布局
- 适合小到中等规模网格（< 1000³）
DSparseGrid3:
- 稀疏 3D 网格，按需分配内存
- 适合大规模场景（仅存储非空体素）
- 基于字典或哈希表实现

2.空间查询与距离计算

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

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

3.网格操作与处理

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

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

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

4.数学与几何工具

// 向量数学（完整的 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 提供路径算法：

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

3. 架构设计

3.1 整体架构

┌──────────────────────────────────────────────────────────┐
│                     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 - 体素单元

/// <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 体素网格

/// <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 - 体素网格生成器

/// <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 - 体素路径规划器

/// <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 天）

任务：

安装 geometry4Sharp
- NuGet 添加 geometry4Sharp 1.0.0
- 验证与 .NET Framework 4.8 兼容性
- 运行示例代码测试
创建简单原型
- 从 Navisworks 提取简单场景（如单个房间）
- 生成体素网格（使用 Bitmap3 或 DSparseGrid3）
- 可视化体素网格（在 Navisworks 中显示为小方块）
距离场测试
- 提取障碍物网格转为 DMesh3
- 使用 MeshSignedDistanceGrid 计算 SDF
- 验证距离场的正确性

验收标准：

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

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

任务：

实现 VoxelGrid 类
- 完整的体素网格数据结构
- 坐标转换方法
- 邻居查询（6 邻域、26 邻域）
- 统计和查询方法
实现 VoxelGridGenerator
- Navisworks Geometry → geometry4Sharp DMesh3 转换
- 距离场计算集成
- 体素标记逻辑（障碍物、通道、门等）
- 特殊元素处理（门、楼梯）
实现 VoxelPathFinder
- 3D A* 算法实现
- 或集成 Roy-T.AStar 的 Graph 模式
- 路径坐标转换
- 成本计算（距离 + 速度限制）
集成到现有系统
- 替换 GridMapGenerator 为 VoxelGridGenerator
- 替换 AutoPathFinder 为 VoxelPathFinder
- 保持 PathOptimizer 不变（适配 3D 路径）

验收标准：

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

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

任务：

性能优化
- 使用 BiGrid3 稀疏网格减少内存
- 窄带 SDF（仅计算障碍物附近区域）
- A算法优化（JPS-3D、Theta 等）
- 多线程并行化（网格生成、路径规划）
内存优化
- 大规模场景的内存占用评估
- 分块生成和加载
- 按需计算距离场
测试场景
- 简单场景（单层房间）
- 中等场景（多层建筑）
- 复杂场景（大型工厂、管道密集）
- 边界测试（超大规模、极小体素）
可视化增强
- 体素网格的 3D 渲染（彩色方块）
- 路径的 3D 动画展示
- 距离场的热图可视化

验收标准：

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

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

任务：

代码文档
- XML 注释完善
- 关键类和方法的使用说明
- 示例代码
用户文档
- 体素网格参数配置说明
- 与 2.5D 方案的对比
- 性能调优建议
配置文件更新
- 添加体素网格相关配置项
- 向后兼容 2.5D 模式（可选）
单元测试
- 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 中添加体素网格相关配置：

[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

理由：

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

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

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

9. 总结与建议

9.1 核心结论

体素网格是 3D 路径规划的理想方案
- 统一的 3D 空间表示
- 简化的路径算法
- 更精确的障碍物表示
geometry4Sharp 是最佳的 C# 体素库
- 功能完整、代码质量高
- 商业友好、平台兼容
- 可满足项目需求
实施是可行的
- 清晰的技术路线
- 合理的时间估算（17-25 天）
- 风险可控

9.2 建议行动

立即行动：

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

后续行动：

根据原型结果决定是否全面实施
如果可行，按阶段 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 开发团队

44 KiB Raw Blame History Unescape Escape

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

1. 背景与动机

1.1 当前方案的局限性

1.2 体素网格的优势

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

2.1 候选库对比

2.2 推荐方案：geometry4Sharp

基本信息

核心功能模块

为什么选择 geometry4Sharp？

与 Roy-T.AStar 的集成策略

3. 架构设计

3.1 整体架构

3.2 核心类设计

3.2.1 VoxelCell - 体素单元

3.2.2 VoxelGrid - 3D 体素网格

3.2.3 VoxelGridGenerator - 体素网格生成器

3.2.4 VoxelPathFinder - 体素路径规划器

4. 实施计划

4.1 阶段划分

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

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

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

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

4.2 时间与资源估算

5. 风险与缓解

5.1 技术风险

风险 1：geometry4Sharp 维护状况

风险 2：Navisworks Geometry 提取复杂性

风险 3：内存占用

风险 4：路径规划性能

5.2 项目风险

风险 5：时间超期

风险 6：与现有代码冲突

6. 配置文件扩展

6.1 新增配置项

7. 性能预期

7.1 性能对比（预估）

7.2 优化目标

8. 替代方案评估

8.1 方案对比

8.2 决策建议

9. 总结与建议

9.1 核心结论

9.2 建议行动

9.3 成功标准

10. 参考资料

10.1 geometry4Sharp 资源

10.2 理论基础

10.3 相关项目

44 KiB

Raw Blame History