410 lines
14 KiB
C#
410 lines
14 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using Autodesk.Navisworks.Api;
|
||
|
||
namespace NavisworksTransport.PathPlanning
|
||
{
|
||
/// <summary>
|
||
/// 3D体素网格路径查找器
|
||
/// 基于A*算法的真3D路径规划实现
|
||
/// </summary>
|
||
public class VoxelPathFinder
|
||
{
|
||
private readonly VoxelGrid _voxelGrid;
|
||
|
||
/// <summary>
|
||
/// 邻域类型
|
||
/// </summary>
|
||
public enum NeighborType
|
||
{
|
||
/// <summary>6邻域（上下左右前后）</summary>
|
||
Six = 6,
|
||
|
||
/// <summary>26邻域（包括对角线）</summary>
|
||
TwentySix = 26
|
||
}
|
||
|
||
/// <summary>
|
||
/// 启发式函数类型
|
||
/// </summary>
|
||
public enum HeuristicType
|
||
{
|
||
/// <summary>欧几里得距离</summary>
|
||
Euclidean,
|
||
|
||
/// <summary>曼哈顿距离</summary>
|
||
Manhattan,
|
||
|
||
/// <summary>切比雪夫距离</summary>
|
||
Chebyshev
|
||
}
|
||
|
||
/// <summary>
|
||
/// 路径查找配置
|
||
/// </summary>
|
||
public class PathFindingConfig
|
||
{
|
||
/// <summary>邻域类型（默认26邻域）</summary>
|
||
public NeighborType NeighborType { get; set; } = NeighborType.TwentySix;
|
||
|
||
/// <summary>启发式函数类型（默认欧几里得）</summary>
|
||
public HeuristicType HeuristicType { get; set; } = HeuristicType.Euclidean;
|
||
|
||
/// <summary>启发式权重（>1加速搜索但可能不是最优路径）</summary>
|
||
public double HeuristicWeight { get; set; } = 1.0;
|
||
|
||
/// <summary>垂直移动额外成本系数（鼓励水平移动）</summary>
|
||
public double VerticalCostFactor { get; set; } = 1.5;
|
||
|
||
/// <summary>对角线移动成本系数</summary>
|
||
public double DiagonalCostFactor { get; set; } = Math.Sqrt(2);
|
||
|
||
/// <summary>最大搜索节点数（防止无限循环）</summary>
|
||
public int MaxSearchNodes { get; set; } = 100000;
|
||
}
|
||
|
||
/// <summary>
|
||
/// A*节点
|
||
/// </summary>
|
||
private class AStarNode : IComparable<AStarNode>
|
||
{
|
||
public int X { get; set; }
|
||
public int Y { get; set; }
|
||
public int Z { get; set; }
|
||
|
||
/// <summary>从起点到当前节点的实际成本</summary>
|
||
public double GCost { get; set; }
|
||
|
||
/// <summary>从当前节点到终点的启发式估计成本</summary>
|
||
public double HCost { get; set; }
|
||
|
||
/// <summary>总成本 F = G + H</summary>
|
||
public double FCost => GCost + HCost;
|
||
|
||
/// <summary>父节点</summary>
|
||
public AStarNode Parent { get; set; }
|
||
|
||
public int CompareTo(AStarNode other)
|
||
{
|
||
int fCompare = FCost.CompareTo(other.FCost);
|
||
if (fCompare != 0) return fCompare;
|
||
// F相同时，比较H（优先选择离终点近的）
|
||
return HCost.CompareTo(other.HCost);
|
||
}
|
||
|
||
public override bool Equals(object obj)
|
||
{
|
||
if (obj is AStarNode other)
|
||
{
|
||
return X == other.X && Y == other.Y && Z == other.Z;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
public override int GetHashCode()
|
||
{
|
||
// .NET Framework 4.8不支持HashCode.Combine，使用传统方式
|
||
unchecked
|
||
{
|
||
int hash = 17;
|
||
hash = hash * 31 + X.GetHashCode();
|
||
hash = hash * 31 + Y.GetHashCode();
|
||
hash = hash * 31 + Z.GetHashCode();
|
||
return hash;
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 构造函数
|
||
/// </summary>
|
||
/// <param name="voxelGrid">体素网格</param>
|
||
public VoxelPathFinder(VoxelGrid voxelGrid)
|
||
{
|
||
_voxelGrid = voxelGrid ?? throw new ArgumentNullException(nameof(voxelGrid));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 查找3D路径
|
||
/// </summary>
|
||
/// <param name="start">起点世界坐标</param>
|
||
/// <param name="end">终点世界坐标</param>
|
||
/// <param name="config">路径查找配置</param>
|
||
/// <returns>路径点列表（世界坐标），如果无路径则返回null</returns>
|
||
public List<Point3D> FindPath(Point3D start, Point3D end, PathFindingConfig config = null)
|
||
{
|
||
config = config ?? new PathFindingConfig();
|
||
|
||
// 转换为体素坐标
|
||
var startVoxel = _voxelGrid.WorldToVoxel(start);
|
||
var endVoxel = _voxelGrid.WorldToVoxel(end);
|
||
|
||
LogManager.Info($"[VoxelPathFinder] 开始路径规划");
|
||
LogManager.Info($" 起点: 世界{start} -> 体素({startVoxel.x}, {startVoxel.y}, {startVoxel.z})");
|
||
LogManager.Info($" 终点: 世界{end} -> 体素({endVoxel.x}, {endVoxel.y}, {endVoxel.z})");
|
||
|
||
// 验证起终点
|
||
if (!ValidatePoint(startVoxel.x, startVoxel.y, startVoxel.z, "起点"))
|
||
return null;
|
||
|
||
if (!ValidatePoint(endVoxel.x, endVoxel.y, endVoxel.z, "终点"))
|
||
return null;
|
||
|
||
// 执行A*搜索
|
||
var pathVoxels = FindPathAStar(startVoxel, endVoxel, config);
|
||
|
||
if (pathVoxels == null || pathVoxels.Count == 0)
|
||
{
|
||
LogManager.Warning("[VoxelPathFinder] 未找到路径");
|
||
return null;
|
||
}
|
||
|
||
// 转换为世界坐标
|
||
var pathWorld = pathVoxels.Select(v => _voxelGrid.VoxelToWorld(v.X, v.Y, v.Z)).ToList();
|
||
|
||
LogManager.Info($"[VoxelPathFinder] 路径找到！长度: {pathVoxels.Count}个体素, {CalculatePathLength(pathWorld):F2}米");
|
||
|
||
return pathWorld;
|
||
}
|
||
|
||
/// <summary>
|
||
/// A*路径搜索核心算法
|
||
/// </summary>
|
||
private List<AStarNode> FindPathAStar((int x, int y, int z) start, (int x, int y, int z) end, PathFindingConfig config)
|
||
{
|
||
var startTime = System.Diagnostics.Stopwatch.StartNew();
|
||
|
||
// 开放列表（使用SortedSet作为优先队列）
|
||
var openSet = new SortedSet<AStarNode>();
|
||
|
||
// 已访问节点集合（快速查找）
|
||
var closedSet = new HashSet<(int, int, int)>();
|
||
|
||
// 节点字典（快速访问G成本）
|
||
var nodeDict = new Dictionary<(int, int, int), AStarNode>();
|
||
|
||
// 创建起始节点
|
||
var startNode = new AStarNode
|
||
{
|
||
X = start.x,
|
||
Y = start.y,
|
||
Z = start.z,
|
||
GCost = 0,
|
||
HCost = CalculateHeuristic(start, end, config.HeuristicType)
|
||
};
|
||
|
||
openSet.Add(startNode);
|
||
nodeDict[(start.x, start.y, start.z)] = startNode;
|
||
|
||
int nodesExpanded = 0;
|
||
|
||
while (openSet.Count > 0)
|
||
{
|
||
// 检查节点数限制
|
||
if (++nodesExpanded > config.MaxSearchNodes)
|
||
{
|
||
LogManager.Warning($"[VoxelPathFinder] 搜索节点数超过限制 {config.MaxSearchNodes}");
|
||
break;
|
||
}
|
||
|
||
// 获取F值最小的节点
|
||
var current = openSet.Min;
|
||
openSet.Remove(current);
|
||
|
||
// 到达终点
|
||
if (current.X == end.x && current.Y == end.y && current.Z == end.z)
|
||
{
|
||
startTime.Stop();
|
||
LogManager.Info($"[VoxelPathFinder] A*搜索完成: 耗时{startTime.ElapsedMilliseconds}ms, 扩展节点{nodesExpanded}个");
|
||
return ReconstructPath(current);
|
||
}
|
||
|
||
closedSet.Add((current.X, current.Y, current.Z));
|
||
|
||
// 遍历邻居
|
||
var neighbors = GetNeighbors(current.X, current.Y, current.Z, config.NeighborType);
|
||
|
||
foreach (var (nx, ny, nz) in neighbors)
|
||
{
|
||
// 跳过已访问的节点
|
||
if (closedSet.Contains((nx, ny, nz)))
|
||
continue;
|
||
|
||
// 跳过不可通行的节点
|
||
if (!_voxelGrid.IsPassable(nx, ny, nz))
|
||
continue;
|
||
|
||
// 计算移动成本
|
||
double moveCost = CalculateMoveCost(current.X, current.Y, current.Z, nx, ny, nz, config);
|
||
double tentativeG = current.GCost + moveCost;
|
||
|
||
// 检查是否找到更好的路径
|
||
if (!nodeDict.TryGetValue((nx, ny, nz), out var neighbor))
|
||
{
|
||
// 新节点
|
||
neighbor = new AStarNode
|
||
{
|
||
X = nx,
|
||
Y = ny,
|
||
Z = nz,
|
||
GCost = tentativeG,
|
||
HCost = CalculateHeuristic((nx, ny, nz), end, config.HeuristicType) * config.HeuristicWeight,
|
||
Parent = current
|
||
};
|
||
|
||
nodeDict[(nx, ny, nz)] = neighbor;
|
||
openSet.Add(neighbor);
|
||
}
|
||
else if (tentativeG < neighbor.GCost)
|
||
{
|
||
// 找到更好的路径，更新节点
|
||
openSet.Remove(neighbor);
|
||
neighbor.GCost = tentativeG;
|
||
neighbor.Parent = current;
|
||
openSet.Add(neighbor);
|
||
}
|
||
}
|
||
}
|
||
|
||
startTime.Stop();
|
||
LogManager.Warning($"[VoxelPathFinder] 未找到路径: 耗时{startTime.ElapsedMilliseconds}ms, 扩展节点{nodesExpanded}个");
|
||
return null;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 重建路径（从终点回溯到起点）
|
||
/// </summary>
|
||
private List<AStarNode> ReconstructPath(AStarNode endNode)
|
||
{
|
||
var path = new List<AStarNode>();
|
||
var current = endNode;
|
||
|
||
while (current != null)
|
||
{
|
||
path.Add(current);
|
||
current = current.Parent;
|
||
}
|
||
|
||
path.Reverse();
|
||
return path;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取邻居节点
|
||
/// </summary>
|
||
private List<(int x, int y, int z)> GetNeighbors(int x, int y, int z, NeighborType neighborType)
|
||
{
|
||
if (neighborType == NeighborType.Six)
|
||
{
|
||
return _voxelGrid.GetNeighbors6(x, y, z);
|
||
}
|
||
else
|
||
{
|
||
return _voxelGrid.GetNeighbors26(x, y, z);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算移动成本
|
||
/// </summary>
|
||
private double CalculateMoveCost(int x1, int y1, int z1, int x2, int y2, int z2, PathFindingConfig config)
|
||
{
|
||
double dx = x2 - x1;
|
||
double dy = y2 - y1;
|
||
double dz = z2 - z1;
|
||
|
||
// 基础欧几里得距离
|
||
double baseCost = Math.Sqrt(dx * dx + dy * dy + dz * dz);
|
||
|
||
// 垂直移动额外成本
|
||
if (dz != 0)
|
||
{
|
||
baseCost *= config.VerticalCostFactor;
|
||
}
|
||
|
||
// 对角线移动
|
||
if (dx != 0 && dy != 0 && dz == 0)
|
||
{
|
||
baseCost *= config.DiagonalCostFactor;
|
||
}
|
||
|
||
// 考虑体素类型的速度限制
|
||
var cell = _voxelGrid.GetCell(x2, y2, z2);
|
||
if (cell.SpeedLimit > 0 && cell.SpeedLimit < 1.0)
|
||
{
|
||
baseCost /= cell.SpeedLimit; // 速度限制越低，成本越高
|
||
}
|
||
|
||
return baseCost;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算启发式估计
|
||
/// </summary>
|
||
private double CalculateHeuristic((int x, int y, int z) from, (int x, int y, int z) to, HeuristicType heuristicType)
|
||
{
|
||
int dx = Math.Abs(to.x - from.x);
|
||
int dy = Math.Abs(to.y - from.y);
|
||
int dz = Math.Abs(to.z - from.z);
|
||
|
||
switch (heuristicType)
|
||
{
|
||
case HeuristicType.Euclidean:
|
||
return Math.Sqrt(dx * dx + dy * dy + dz * dz);
|
||
|
||
case HeuristicType.Manhattan:
|
||
return dx + dy + dz;
|
||
|
||
case HeuristicType.Chebyshev:
|
||
return Math.Max(Math.Max(dx, dy), dz);
|
||
|
||
default:
|
||
return Math.Sqrt(dx * dx + dy * dy + dz * dz);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证点是否有效
|
||
/// </summary>
|
||
private bool ValidatePoint(int x, int y, int z, string pointName)
|
||
{
|
||
if (!_voxelGrid.IsValidIndex(x, y, z))
|
||
{
|
||
LogManager.Error($"[VoxelPathFinder] {pointName}超出体素网格范围: ({x}, {y}, {z})");
|
||
return false;
|
||
}
|
||
|
||
if (!_voxelGrid.IsPassable(x, y, z))
|
||
{
|
||
LogManager.Error($"[VoxelPathFinder] {pointName}不可通行: ({x}, {y}, {z})");
|
||
return false;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算路径长度
|
||
/// </summary>
|
||
private double CalculatePathLength(List<Point3D> path)
|
||
{
|
||
if (path == null || path.Count < 2)
|
||
return 0;
|
||
|
||
double length = 0;
|
||
for (int i = 1; i < path.Count; i++)
|
||
{
|
||
var p1 = path[i - 1];
|
||
var p2 = path[i];
|
||
double dx = p2.X - p1.X;
|
||
double dy = p2.Y - p1.Y;
|
||
double dz = p2.Z - p1.Z;
|
||
length += Math.Sqrt(dx * dx + dy * dy + dz * dz);
|
||
}
|
||
|
||
return length;
|
||
}
|
||
}
|
||
}
|