tian
27908540c2
本次提交包含三个主要改进： 1. XY平面膨胀算法（VoxelGrid.cs） - 实现简单迭代形态学膨胀 - 只在水平方向（XY平面）的4邻域膨胀 - 不在Z方向（垂直方向）膨胀 - 符合车辆物流场景：车辆只侧面/顶部碰撞障碍物 2. 3D体素路径规划（VoxelPathFinder.cs） - 集成RoyT.AStar库进行3D A*路径规划 - 支持体素网格上的路径搜索 - 添加VoxelPathFindingTestCommand测试命令 3. UI和测试改进 - 删除旧的包围盒测试命令（VoxelGridTestCommand.cs） - 更新SystemManagementView UI - 添加体素路径规划测试功能 核心设计原则： - 门模型在SDF生成前被排除（留出通道空洞） - SDF阶段只标记几何体内部为障碍物 - 安全间隙仅在XY平面膨胀阶段应用 - 避免Z方向的错误膨胀 🤖 Generated with [Claude Code](https://claude.com/claude-code) Co-Authored-By: Claude <noreply@anthropic.com>
644 lines
25 KiB
C#
644 lines
25 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using Autodesk.Navisworks.Api;
|
||
using NavisworksTransport.Utils;
|
||
|
||
namespace NavisworksTransport.PathPlanning
|
||
{
|
||
/// <summary>
|
||
/// 3D体素网格类 - 用于真3D路径规划
|
||
/// 提供完整的3D空间离散化表示，支持任意高度的物体和通道
|
||
/// </summary>
|
||
public class VoxelGrid
|
||
{
|
||
/// <summary>
|
||
/// 3D体素数组 [x, y, z]
|
||
/// 注意：索引顺序为 [宽度, 深度, 高度]
|
||
/// </summary>
|
||
private VoxelCell[,,] cells;
|
||
|
||
/// <summary>
|
||
/// 网格原点（世界坐标，模型单位）
|
||
/// 对应体素索引 (0, 0, 0) 的世界坐标位置
|
||
/// </summary>
|
||
public Point3D Origin { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 单个体素的尺寸（模型单位）
|
||
/// 所有三个维度使用相同的体素尺寸（立方体体素）
|
||
/// </summary>
|
||
public double VoxelSize { get; private set; }
|
||
|
||
/// <summary>
|
||
/// X方向（宽度）的体素数量
|
||
/// </summary>
|
||
public int SizeX { get; private set; }
|
||
|
||
/// <summary>
|
||
/// Y方向（深度）的体素数量
|
||
/// </summary>
|
||
public int SizeY { get; private set; }
|
||
|
||
/// <summary>
|
||
/// Z方向（高度）的体素数量
|
||
/// </summary>
|
||
public int SizeZ { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 网格边界框（世界坐标，模型单位）
|
||
/// </summary>
|
||
public BoundingBox3D Bounds { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 总体素数量
|
||
/// </summary>
|
||
public int TotalVoxels => SizeX * SizeY * SizeZ;
|
||
|
||
/// <summary>
|
||
/// 构造函数 - 创建指定尺寸的体素网格
|
||
/// </summary>
|
||
/// <param name="bounds">网格边界框（世界坐标，模型单位）</param>
|
||
/// <param name="voxelSize">体素尺寸（模型单位）</param>
|
||
public VoxelGrid(BoundingBox3D bounds, double voxelSize)
|
||
{
|
||
if (voxelSize <= 0)
|
||
throw new ArgumentException("体素尺寸必须大于0", nameof(voxelSize));
|
||
|
||
VoxelSize = voxelSize;
|
||
Bounds = bounds;
|
||
Origin = bounds.Min;
|
||
|
||
// 计算每个维度需要的体素数量（向上取整以覆盖整个边界框）
|
||
double width = bounds.Max.X - bounds.Min.X;
|
||
double depth = bounds.Max.Y - bounds.Min.Y;
|
||
double height = bounds.Max.Z - bounds.Min.Z;
|
||
|
||
SizeX = (int)Math.Ceiling(width / voxelSize);
|
||
SizeY = (int)Math.Ceiling(depth / voxelSize);
|
||
SizeZ = (int)Math.Ceiling(height / voxelSize);
|
||
|
||
// 初始化体素数组（所有体素默认为可通行）
|
||
cells = new VoxelCell[SizeX, SizeY, SizeZ];
|
||
InitializeCells();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 初始化所有体素单元
|
||
/// </summary>
|
||
private void InitializeCells()
|
||
{
|
||
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();
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取指定索引的体素单元
|
||
/// </summary>
|
||
/// <param name="x">X方向索引</param>
|
||
/// <param name="y">Y方向索引</param>
|
||
/// <param name="z">Z方向索引</param>
|
||
/// <returns>体素单元，如果索引越界返回null</returns>
|
||
public VoxelCell GetCell(int x, int y, int z)
|
||
{
|
||
if (!IsValidIndex(x, y, z))
|
||
return null;
|
||
|
||
return cells[x, y, z];
|
||
}
|
||
|
||
/// <summary>
|
||
/// 设置指定索引的体素单元
|
||
/// </summary>
|
||
/// <param name="x">X方向索引</param>
|
||
/// <param name="y">Y方向索引</param>
|
||
/// <param name="z">Z方向索引</param>
|
||
/// <param name="cell">体素单元</param>
|
||
/// <returns>是否设置成功</returns>
|
||
public bool SetCell(int x, int y, int z, VoxelCell cell)
|
||
{
|
||
if (!IsValidIndex(x, y, z))
|
||
return false;
|
||
|
||
cells[x, y, z] = cell;
|
||
return true;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查体素索引是否有效（在网格范围内）
|
||
/// </summary>
|
||
/// <param name="x">X方向索引</param>
|
||
/// <param name="y">Y方向索引</param>
|
||
/// <param name="z">Z方向索引</param>
|
||
/// <returns>索引是否有效</returns>
|
||
public bool IsValidIndex(int x, int y, int z)
|
||
{
|
||
return x >= 0 && x < SizeX &&
|
||
y >= 0 && y < SizeY &&
|
||
z >= 0 && z < SizeZ;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 世界坐标转换为体素索引
|
||
/// 注意：体素索引代表体素的左下角，而不是中心点
|
||
/// </summary>
|
||
/// <param name="worldPos">世界坐标（模型单位）</param>
|
||
/// <returns>体素索引 (x, y, z)</returns>
|
||
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>
|
||
/// <param name="x">X方向索引</param>
|
||
/// <param name="y">Y方向索引</param>
|
||
/// <param name="z">Z方向索引</param>
|
||
/// <returns>世界坐标（模型单位）</returns>
|
||
public Point3D VoxelToWorld(int x, int y, int z)
|
||
{
|
||
double worldX = Origin.X + x * VoxelSize;
|
||
double worldY = Origin.Y + y * VoxelSize;
|
||
double worldZ = Origin.Z + z * VoxelSize;
|
||
|
||
return new Point3D(worldX, worldY, worldZ);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 体素索引转换为世界坐标（体素的中心点坐标）
|
||
/// </summary>
|
||
/// <param name="x">X方向索引</param>
|
||
/// <param name="y">Y方向索引</param>
|
||
/// <param name="z">Z方向索引</param>
|
||
/// <returns>世界坐标（模型单位）</returns>
|
||
public Point3D VoxelToWorldCenter(int x, int y, int z)
|
||
{
|
||
double halfVoxel = VoxelSize / 2.0;
|
||
double worldX = Origin.X + x * VoxelSize + halfVoxel;
|
||
double worldY = Origin.Y + y * VoxelSize + halfVoxel;
|
||
double worldZ = Origin.Z + z * VoxelSize + halfVoxel;
|
||
|
||
return new Point3D(worldX, worldY, worldZ);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取体素的6邻域（上下左右前后）
|
||
/// </summary>
|
||
/// <param name="x">X方向索引</param>
|
||
/// <param name="y">Y方向索引</param>
|
||
/// <param name="z">Z方向索引</param>
|
||
/// <returns>邻居体素索引列表</returns>
|
||
public List<(int x, int y, int z)> GetNeighbors6(int x, int y, int z)
|
||
{
|
||
var neighbors = new List<(int, int, int)>();
|
||
|
||
// 6个方向：右、左、后、前、上、下
|
||
int[,] directions = {
|
||
{ 1, 0, 0 }, // +X 右
|
||
{ -1, 0, 0 }, // -X 左
|
||
{ 0, 1, 0 }, // +Y 后
|
||
{ 0, -1, 0 }, // -Y 前
|
||
{ 0, 0, 1 }, // +Z 上
|
||
{ 0, 0, -1 } // -Z 下
|
||
};
|
||
|
||
for (int i = 0; i < 6; i++)
|
||
{
|
||
int nx = x + directions[i, 0];
|
||
int ny = y + directions[i, 1];
|
||
int nz = z + directions[i, 2];
|
||
|
||
if (IsValidIndex(nx, ny, nz))
|
||
{
|
||
neighbors.Add((nx, ny, nz));
|
||
}
|
||
}
|
||
|
||
return neighbors;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取体素的26邻域（包括对角线方向）
|
||
/// </summary>
|
||
/// <param name="x">X方向索引</param>
|
||
/// <param name="y">Y方向索引</param>
|
||
/// <param name="z">Z方向索引</param>
|
||
/// <returns>邻居体素索引列表</returns>
|
||
public List<(int x, int y, int z)> GetNeighbors26(int x, int y, int z)
|
||
{
|
||
var neighbors = new List<(int, int, int)>();
|
||
|
||
// 26个方向：3x3x3立方体去掉中心点
|
||
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))
|
||
{
|
||
neighbors.Add((nx, ny, nz));
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
return neighbors;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取两个体素之间的欧几里得距离（体素单位）
|
||
/// </summary>
|
||
/// <param name="x1">第一个体素的X索引</param>
|
||
/// <param name="y1">第一个体素的Y索引</param>
|
||
/// <param name="z1">第一个体素的Z索引</param>
|
||
/// <param name="x2">第二个体素的X索引</param>
|
||
/// <param name="y2">第二个体素的Y索引</param>
|
||
/// <param name="z2">第二个体素的Z索引</param>
|
||
/// <returns>欧几里得距离（体素单位）</returns>
|
||
public double GetDistance(int x1, int y1, int z1, int x2, int y2, int z2)
|
||
{
|
||
int dx = x2 - x1;
|
||
int dy = y2 - y1;
|
||
int dz = z2 - z1;
|
||
|
||
return Math.Sqrt(dx * dx + dy * dy + dz * dz);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取两个体素之间的曼哈顿距离（体素单位）
|
||
/// </summary>
|
||
/// <param name="x1">第一个体素的X索引</param>
|
||
/// <param name="y1">第一个体素的Y索引</param>
|
||
/// <param name="z1">第一个体素的Z索引</param>
|
||
/// <param name="x2">第二个体素的X索引</param>
|
||
/// <param name="y2">第二个体素的Y索引</param>
|
||
/// <param name="z2">第二个体素的Z索引</param>
|
||
/// <returns>曼哈顿距离（体素单位）</returns>
|
||
public int GetManhattanDistance(int x1, int y1, int z1, int x2, int y2, int z2)
|
||
{
|
||
return Math.Abs(x2 - x1) + Math.Abs(y2 - y1) + Math.Abs(z2 - z1);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查指定索引的体素是否可通行
|
||
/// </summary>
|
||
/// <param name="x">X方向索引</param>
|
||
/// <param name="y">Y方向索引</param>
|
||
/// <param name="z">Z方向索引</param>
|
||
/// <returns>是否可通行（索引越界返回false）</returns>
|
||
public bool IsPassable(int x, int y, int z)
|
||
{
|
||
var cell = GetCell(x, y, z);
|
||
return cell != null && cell.IsPassable;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查世界坐标位置是否可通行
|
||
/// </summary>
|
||
/// <param name="worldPos">世界坐标（模型单位）</param>
|
||
/// <returns>是否可通行</returns>
|
||
public bool IsPassable(Point3D worldPos)
|
||
{
|
||
var (x, y, z) = WorldToVoxel(worldPos);
|
||
return IsPassable(x, y, z);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取网格的统计信息
|
||
/// </summary>
|
||
/// <returns>(总体素数, 可通行体素数, 障碍物体素数)</returns>
|
||
public (int total, int passable, int obstacle) GetStatistics()
|
||
{
|
||
int total = TotalVoxels;
|
||
int passable = 0;
|
||
int obstacle = 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)
|
||
passable++;
|
||
else
|
||
obstacle++;
|
||
}
|
||
}
|
||
}
|
||
|
||
return (total, passable, obstacle);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 清除所有体素（重置为可通行状态）
|
||
/// </summary>
|
||
public void Clear()
|
||
{
|
||
InitializeCells();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 返回网格的字符串表示
|
||
/// </summary>
|
||
public override string ToString()
|
||
{
|
||
var (total, passable, obstacle) = GetStatistics();
|
||
double passableRatio = (double)passable / total * 100.0;
|
||
return $"VoxelGrid[{SizeX}x{SizeY}x{SizeZ}={total}体素, 可通行:{passableRatio:F1}%, 体素尺寸:{VoxelSize:F2}模型单位]";
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取与包围盒相交的所有体素索引
|
||
/// 用于标记障碍物：找出被模型对象占据的所有体素
|
||
/// </summary>
|
||
/// <param name="bounds">包围盒（世界坐标，模型单位）</param>
|
||
/// <returns>相交的体素索引列表</returns>
|
||
public List<(int x, int y, int z)> GetVoxelsInBounds(BoundingBox3D bounds)
|
||
{
|
||
var result = new List<(int, int, int)>();
|
||
|
||
// 将包围盒的世界坐标转换为体素索引
|
||
var minIndices = WorldToVoxel(bounds.Min);
|
||
var maxIndices = WorldToVoxel(bounds.Max);
|
||
|
||
// 遍历包围盒覆盖的所有体素
|
||
// 注意：maxIndices 可能超出网格范围，需要裁剪
|
||
int startX = Math.Max(0, minIndices.x);
|
||
int startY = Math.Max(0, minIndices.y);
|
||
int startZ = Math.Max(0, minIndices.z);
|
||
|
||
int endX = Math.Min(SizeX - 1, maxIndices.x);
|
||
int endY = Math.Min(SizeY - 1, maxIndices.y);
|
||
int endZ = Math.Min(SizeZ - 1, maxIndices.z);
|
||
|
||
for (int x = startX; x <= endX; x++)
|
||
{
|
||
for (int y = startY; y <= endY; y++)
|
||
{
|
||
for (int z = startZ; z <= endZ; z++)
|
||
{
|
||
result.Add((x, y, z));
|
||
}
|
||
}
|
||
}
|
||
|
||
return result;
|
||
}
|
||
|
||
#region 障碍物膨胀算法
|
||
|
||
/// <summary>
|
||
/// 障碍物膨胀 - 基于Fast Sweeping算法的3D距离变换
|
||
/// 为障碍物体素添加安全缓冲区，确保路径规划时保持足够的安全距离
|
||
/// </summary>
|
||
/// <param name="inflationRadiusMeters">膨胀半径（米）</param>
|
||
/// <param name="metersToModelUnits">米到模型单位的转换系数</param>
|
||
/// <returns>膨胀的体素数量</returns>
|
||
public int InflateObstacles(double inflationRadiusMeters, double metersToModelUnits)
|
||
{
|
||
LogManager.Info($"[体素膨胀] 开始膨胀，半径: {inflationRadiusMeters}米");
|
||
|
||
// 1. 计算膨胀半径（模型单位）
|
||
double inflationRadiusInModelUnits = inflationRadiusMeters * metersToModelUnits;
|
||
|
||
if (inflationRadiusInModelUnits <= 0)
|
||
{
|
||
LogManager.Info($"[体素膨胀] 膨胀半径为0，跳过膨胀");
|
||
return 0;
|
||
}
|
||
|
||
LogManager.Info($"[体素膨胀] 膨胀半径: {inflationRadiusInModelUnits:F2}模型单位");
|
||
|
||
// 2. 创建距离网格（使用g4的DenseGrid3f）
|
||
float upperBound = (SizeX + SizeY + SizeZ) * (float)VoxelSize;
|
||
var distanceGrid = new g4.DenseGrid3f(SizeX, SizeY, SizeZ, upperBound);
|
||
|
||
// 3. 初始化距离图：障碍物=0, 可通行区域=upperBound
|
||
int obstacleCount = 0;
|
||
|
||
for (int x = 0; x < SizeX; x++)
|
||
{
|
||
for (int y = 0; y < SizeY; y++)
|
||
{
|
||
for (int z = 0; z < SizeZ; z++)
|
||
{
|
||
var cell = cells[x, y, z];
|
||
|
||
// 简单逻辑：只有障碍物（IsPassable=false）才设置为膨胀源（distance=0）
|
||
// 所有可通行区域（门、楼梯、通道等IsPassable=true）都不膨胀
|
||
if (!cell.IsPassable)
|
||
{
|
||
distanceGrid[x, y, z] = 0f;
|
||
obstacleCount++;
|
||
}
|
||
else
|
||
{
|
||
distanceGrid[x, y, z] = upperBound;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
LogManager.Info($"[体素膨胀] 初始化完成 - 障碍物体素: {obstacleCount}");
|
||
|
||
// 4. 迭代膨胀：只在XY平面（水平方向）膨胀，不在Z方向膨胀
|
||
// 使用简单的形态学膨胀，每次迭代向XY方向扩展1个体素
|
||
LogManager.Info($"[体素膨胀] 开始迭代膨胀（只在XY平面）...");
|
||
var inflationStopwatch = System.Diagnostics.Stopwatch.StartNew();
|
||
|
||
int inflationRadius = (int)Math.Ceiling(inflationRadiusInModelUnits / VoxelSize);
|
||
LogManager.Info($"[体素膨胀] 膨胀半径: {inflationRadius} 个体素");
|
||
|
||
int totalInflated = 0;
|
||
|
||
// 迭代膨胀 inflationRadius 次
|
||
for (int iteration = 0; iteration < inflationRadius; iteration++)
|
||
{
|
||
// 记录本次迭代要膨胀的体素（避免在遍历时修改）
|
||
var toInflate = new List<(int x, int y, int z)>();
|
||
|
||
// 遍历所有体素，找到障碍物的XY邻域
|
||
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)
|
||
{
|
||
// 检查XY平面的4个邻居（上下左右），不检查Z方向
|
||
int[] dx = { -1, 1, 0, 0 };
|
||
int[] dy = { 0, 0, -1, 1 };
|
||
|
||
for (int dir = 0; dir < 4; dir++)
|
||
{
|
||
int nx = x + dx[dir];
|
||
int ny = y + dy[dir];
|
||
int nz = z; // Z保持不变，不向上下膨胀
|
||
|
||
// 检查边界
|
||
if (nx >= 0 && nx < SizeX && ny >= 0 && ny < SizeY)
|
||
{
|
||
// 如果邻居是可通行的，标记为待膨胀
|
||
if (cells[nx, ny, nz].IsPassable)
|
||
{
|
||
toInflate.Add((nx, ny, nz));
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// 应用膨胀
|
||
int iterationCount = 0;
|
||
foreach (var (x, y, z) in toInflate)
|
||
{
|
||
// 再次检查，避免重复标记
|
||
if (cells[x, y, z].IsPassable)
|
||
{
|
||
cells[x, y, z].SetAsObstacle();
|
||
cells[x, y, z].Type = CategoryAttributeManager.LogisticsElementType.障碍物;
|
||
iterationCount++;
|
||
}
|
||
}
|
||
|
||
totalInflated += iterationCount;
|
||
LogManager.Info($"[体素膨胀] 第 {iteration + 1}/{inflationRadius} 次迭代：膨胀了 {iterationCount} 个体素");
|
||
|
||
// 如果本次没有膨胀任何体素，提前结束
|
||
if (iterationCount == 0)
|
||
{
|
||
LogManager.Info($"[体素膨胀] 第 {iteration + 1} 次迭代无新增膨胀，提前结束");
|
||
break;
|
||
}
|
||
}
|
||
|
||
inflationStopwatch.Stop();
|
||
LogManager.Info($"[体素膨胀] 迭代膨胀完成，耗时: {inflationStopwatch.ElapsedMilliseconds}ms");
|
||
LogManager.Info($"[体素膨胀] 总膨胀体素数: {totalInflated}");
|
||
return totalInflated;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 执行Fast Sweeping算法（8方向扫描传播距离）
|
||
/// 参考geometry4Sharp的sweep_pass实现
|
||
/// </summary>
|
||
/// <param name="distanceGrid">距离网格</param>
|
||
/// <param name="cellSize">体素尺寸</param>
|
||
private void PerformFastSweeping(g4.DenseGrid3f distanceGrid, float cellSize)
|
||
{
|
||
// 8方向扫描（与g4的sweep_pass相同）
|
||
Sweep(distanceGrid, cellSize, +1, +1, +1);
|
||
Sweep(distanceGrid, cellSize, -1, -1, -1);
|
||
Sweep(distanceGrid, cellSize, +1, +1, -1);
|
||
Sweep(distanceGrid, cellSize, -1, -1, +1);
|
||
Sweep(distanceGrid, cellSize, +1, -1, +1);
|
||
Sweep(distanceGrid, cellSize, -1, +1, -1);
|
||
Sweep(distanceGrid, cellSize, +1, -1, -1);
|
||
Sweep(distanceGrid, cellSize, -1, +1, +1);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 单向扫描 - 从g4的sweep方法移植
|
||
/// 沿指定方向扫描网格，传播距离值
|
||
/// </summary>
|
||
/// <param name="grid">距离网格</param>
|
||
/// <param name="cellSize">体素尺寸</param>
|
||
/// <param name="di">X方向增量（+1或-1）</param>
|
||
/// <param name="dj">Y方向增量（+1或-1）</param>
|
||
/// <param name="dk">Z方向增量（+1或-1）</param>
|
||
private void Sweep(g4.DenseGrid3f grid, float cellSize, int di, int dj, int dk)
|
||
{
|
||
// 确定扫描范围和方向
|
||
int i0 = di > 0 ? 1 : grid.ni - 2;
|
||
int i1 = di > 0 ? grid.ni : -1;
|
||
int j0 = dj > 0 ? 1 : grid.nj - 2;
|
||
int j1 = dj > 0 ? grid.nj : -1;
|
||
int k0 = dk > 0 ? 1 : grid.nk - 2;
|
||
int k1 = dk > 0 ? grid.nk : -1;
|
||
|
||
// 三层循环扫描
|
||
for (int k = k0; k != k1; k += dk)
|
||
{
|
||
for (int j = j0; j != j1; j += dj)
|
||
{
|
||
for (int i = i0; i != i1; i += di)
|
||
{
|
||
// 检查7个邻居并更新当前点的距离
|
||
// 邻居方向：与扫描方向相反的7个方向
|
||
CheckAndUpdate(grid, cellSize, i, j, k, i - di, j, k);
|
||
CheckAndUpdate(grid, cellSize, i, j, k, i, j - dj, k);
|
||
CheckAndUpdate(grid, cellSize, i, j, k, i - di, j - dj, k);
|
||
CheckAndUpdate(grid, cellSize, i, j, k, i, j, k - dk);
|
||
CheckAndUpdate(grid, cellSize, i, j, k, i - di, j, k - dk);
|
||
CheckAndUpdate(grid, cellSize, i, j, k, i, j - dj, k - dk);
|
||
CheckAndUpdate(grid, cellSize, i, j, k, i - di, j - dj, k - dk);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查邻居并更新距离 - 从g4的check_neighbour移植
|
||
/// 如果通过邻居到达当前点的距离更短，则更新当前点的距离
|
||
/// </summary>
|
||
/// <param name="grid">距离网格</param>
|
||
/// <param name="cellSize">体素尺寸</param>
|
||
/// <param name="i0">当前点X索引</param>
|
||
/// <param name="j0">当前点Y索引</param>
|
||
/// <param name="k0">当前点Z索引</param>
|
||
/// <param name="i1">邻居X索引</param>
|
||
/// <param name="j1">邻居Y索引</param>
|
||
/// <param name="k1">邻居Z索引</param>
|
||
private void CheckAndUpdate(g4.DenseGrid3f grid, float cellSize,
|
||
int i0, int j0, int k0, int i1, int j1, int k1)
|
||
{
|
||
// 边界检查
|
||
if (i1 < 0 || i1 >= grid.ni || j1 < 0 || j1 >= grid.nj || k1 < 0 || k1 >= grid.nk)
|
||
return;
|
||
|
||
// 计算从邻居到当前点的边长
|
||
float dx = (i0 - i1) * cellSize;
|
||
float dy = (j0 - j1) * cellSize;
|
||
float dz = (k0 - k1) * cellSize;
|
||
float edgeDistance = (float)Math.Sqrt(dx * dx + dy * dy + dz * dz);
|
||
|
||
// 计算通过邻居到达当前点的新距离
|
||
float newDistance = grid[i1, j1, k1] + edgeDistance;
|
||
|
||
// 如果新距离更短，更新
|
||
if (newDistance < grid[i0, j0, k0])
|
||
{
|
||
grid[i0, j0, k0] = newDistance;
|
||
}
|
||
}
|
||
|
||
#endregion
|
||
}
|
||
}
|