diff --git a/src/Core/Spatial/SpatialHashGrid.cs b/src/Core/Spatial/SpatialHashGrid.cs
new file mode 100644
index 0000000..415ca40
--- /dev/null
+++ b/src/Core/Spatial/SpatialHashGrid.cs
@@ -0,0 +1,242 @@
+using System;
+using System.Collections.Generic;
+using g4; // geometry4Sharp
+
+namespace NavisworksTransport.Core.Spatial
+{
+ ///
+ /// 自定义3D空间哈希网格
+ /// 用于高效的空间范围查询,复杂度 O(1) 平均
+ ///
+ /// 存储的对象类型
+ public class SpatialHashGrid
+ {
+ private readonly Dictionary> _grid;
+ private readonly double _cellSize;
+
+ ///
+ /// 创建空间哈希网格
+ ///
+ /// 格子大小(模型单位)
+ public SpatialHashGrid(double cellSize)
+ {
+ if (cellSize <= 0)
+ throw new ArgumentException("格子大小必须大于0", nameof(cellSize));
+
+ _cellSize = cellSize;
+ _grid = new Dictionary>();
+ }
+
+ ///
+ /// 格子大小(模型单位)
+ ///
+ public double CellSize => _cellSize;
+
+ ///
+ /// 格子总数
+ ///
+ public int CellCount => _grid.Count;
+
+ ///
+ /// 对象总数
+ ///
+ public int ObjectCount
+ {
+ get
+ {
+ int count = 0;
+ foreach (var list in _grid.Values)
+ {
+ count += list.Count;
+ }
+ return count;
+ }
+ }
+
+ ///
+ /// 插入对象到空间哈希网格
+ ///
+ /// 要插入的对象
+ /// 对象的3D位置
+ public void Insert(T item, Vector3d position)
+ {
+ var gridCell = ToGridCoordinates(position);
+
+ if (!_grid.ContainsKey(gridCell))
+ {
+ _grid[gridCell] = new List();
+ }
+
+ _grid[gridCell].Add(item);
+ }
+
+ ///
+ /// 批量插入对象
+ ///
+ /// 对象和位置的键值对
+ public void InsertRange(IEnumerable> items)
+ {
+ foreach (var kvp in items)
+ {
+ Insert(kvp.Key, kvp.Value);
+ }
+ }
+
+ ///
+ /// 获取指定格子中的所有对象
+ ///
+ /// 格子坐标
+ /// 该格子中的对象列表
+ public List GetObjectsInCell(Vector3i gridCell)
+ {
+ if (_grid.TryGetValue(gridCell, out var list))
+ {
+ return new List(list); // 返回副本,避免外部修改
+ }
+ return new List();
+ }
+
+ ///
+ /// 范围查询:查找指定位置附近指定半径内的所有对象
+ ///
+ /// 查询中心位置
+ /// 查询半径(模型单位)
+ /// 可选的距离计算函数(用于精确过滤)
+ /// 范围内的对象列表(去重)
+ public List FindInRadius(
+ Vector3d center,
+ double radius,
+ Func distanceFunc = null)
+ {
+ var results = new HashSet();
+
+ // 计算需要检查的格子范围
+ int gridRadius = (int)Math.Ceiling(radius / _cellSize);
+ var centerGrid = ToGridCoordinates(center);
+
+ // 遍历查询范围内的所有格子
+ for (int dx = -gridRadius; dx <= gridRadius; dx++)
+ {
+ for (int dy = -gridRadius; dy <= gridRadius; dy++)
+ {
+ for (int dz = -gridRadius; dz <= gridRadius; dz++)
+ {
+ var gridCell = new Vector3i(
+ centerGrid.x + dx,
+ centerGrid.y + dy,
+ centerGrid.z + dz
+ );
+
+ if (_grid.TryGetValue(gridCell, out var cellObjects))
+ {
+ foreach (var obj in cellObjects)
+ {
+ // 如果提供了距离函数,进行精确距离检查
+ if (distanceFunc != null)
+ {
+ double distance = distanceFunc(obj);
+ if (distance <= radius)
+ {
+ results.Add(obj);
+ }
+ }
+ else
+ {
+ // 否则直接添加(假设格子内的对象都在范围内)
+ results.Add(obj);
+ }
+ }
+ }
+ }
+ }
+ }
+
+ return new List(results);
+ }
+
+ ///
+ /// 范围查询(带精确距离检查)
+ ///
+ /// 查询中心
+ /// 查询半径
+ /// 获取对象位置的函数
+ /// 范围内的对象列表
+ public List FindInRadiusExact(
+ Vector3d center,
+ double radius,
+ Func getPositionFunc)
+ {
+ if (getPositionFunc == null)
+ throw new ArgumentNullException(nameof(getPositionFunc));
+
+ return FindInRadius(center, radius, obj =>
+ {
+ var objPos = getPositionFunc(obj);
+ return Vector3d.Distance(center, objPos);
+ });
+ }
+
+ ///
+ /// 清空所有数据
+ ///
+ public void Clear()
+ {
+ _grid.Clear();
+ }
+
+ ///
+ /// 将世界坐标转换为网格坐标
+ ///
+ /// 世界坐标
+ /// 网格坐标
+ private Vector3i ToGridCoordinates(Vector3d position)
+ {
+ return new Vector3i(
+ (int)Math.Floor(position.x / _cellSize),
+ (int)Math.Floor(position.y / _cellSize),
+ (int)Math.Floor(position.z / _cellSize)
+ );
+ }
+
+ ///
+ /// 获取网格统计信息(用于调试)
+ ///
+ /// 统计信息字符串
+ public string GetStatistics()
+ {
+ int totalObjects = 0;
+ int maxObjectsPerCell = 0;
+ int minObjectsPerCell = int.MaxValue;
+ int emptyCount = 0;
+
+ foreach (var list in _grid.Values)
+ {
+ int count = list.Count;
+ totalObjects += count;
+
+ if (count == 0)
+ emptyCount++;
+
+ if (count > maxObjectsPerCell)
+ maxObjectsPerCell = count;
+
+ if (count < minObjectsPerCell && count > 0)
+ minObjectsPerCell = count;
+ }
+
+ double avgObjectsPerCell = _grid.Count > 0 ? (double)totalObjects / _grid.Count : 0;
+
+ if (minObjectsPerCell == int.MaxValue)
+ minObjectsPerCell = 0;
+
+ return $"空间哈希网格统计:\n" +
+ $" 格子大小: {_cellSize:F2} 模型单位\n" +
+ $" 总格子数: {_grid.Count}\n" +
+ $" 总对象数: {totalObjects}\n" +
+ $" 平均每格对象数: {avgObjectsPerCell:F2}\n" +
+ $" 最大每格对象数: {maxObjectsPerCell}\n" +
+ $" 最小每格对象数: {minObjectsPerCell}\n" +
+ $" 空格子数: {emptyCount}";
+ }
+ }
+}
diff --git a/src/Core/Spatial/SpatialIndexManager.cs b/src/Core/Spatial/SpatialIndexManager.cs
new file mode 100644
index 0000000..f5a5968
--- /dev/null
+++ b/src/Core/Spatial/SpatialIndexManager.cs
@@ -0,0 +1,293 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Linq;
+using Autodesk.Navisworks.Api;
+using NavisworksTransport.Utils;
+using g4; // geometry4Sharp
+
+namespace NavisworksTransport.Core.Spatial
+{
+ ///
+ /// 全局空间索引管理器
+ /// 单例模式,为所有动画提供高效的空间查询服务
+ ///
+ public class SpatialIndexManager
+ {
+ private static SpatialIndexManager _instance;
+ private static readonly object _lockObj = new object();
+
+ ///
+ /// 单例实例
+ ///
+ public static SpatialIndexManager Instance
+ {
+ get
+ {
+ if (_instance == null)
+ {
+ lock (_lockObj)
+ {
+ if (_instance == null)
+ {
+ _instance = new SpatialIndexManager();
+ }
+ }
+ }
+ return _instance;
+ }
+ }
+
+ private SpatialHashGrid _globalSpatialIndex;
+ private double _cellSize = 1.0; // 默认格子大小(模型单位)
+ private bool _isInitialized = false;
+ private int _indexedObjectCount = 0;
+
+ // 缓存对象位置,避免重复计算
+ private readonly Dictionary _objectPositions = new Dictionary();
+
+ private SpatialIndexManager()
+ {
+ }
+
+ ///
+ /// 空间索引是否已初始化
+ ///
+ public bool IsInitialized => _isInitialized;
+
+ ///
+ /// 格子大小(模型单位)
+ ///
+ public double CellSize => _cellSize;
+
+ ///
+ /// 已索引的对象数量
+ ///
+ public int IndexedObjectCount => _indexedObjectCount;
+
+ ///
+ /// 构建全局空间索引
+ ///
+ /// 格子大小(模型单位),建议设置为车辆半径的2倍
+ public void BuildGlobalIndex(double cellSizeInModelUnits = 1.0)
+ {
+ var sw = Stopwatch.StartNew();
+ LogManager.Info("[空间索引] 开始构建全局空间索引");
+ LogManager.Info($"[空间索引] 格子大小: {cellSizeInModelUnits:F2} 模型单位");
+
+ try
+ {
+ _cellSize = cellSizeInModelUnits;
+
+ // 1. 获取所有几何对象
+ var allItems = Application.ActiveDocument.Models.RootItemDescendantsAndSelf
+ .Where(item => item.HasGeometry)
+ .ToList();
+
+ LogManager.Info($"[空间索引] 找到 {allItems.Count} 个几何对象");
+
+ if (allItems.Count == 0)
+ {
+ LogManager.Warning("[空间索引] 场景中没有几何对象");
+ return;
+ }
+
+ // 2. 创建空间哈希网格
+ _globalSpatialIndex = new SpatialHashGrid(cellSizeInModelUnits);
+ _objectPositions.Clear();
+
+ // 3. 索引所有对象
+ int indexedCount = 0;
+ int failedCount = 0;
+
+ foreach (var item in allItems)
+ {
+ try
+ {
+ // 获取包围盒中心作为对象位置
+ var bbox = item.BoundingBox();
+ var center = new Vector3d(
+ (bbox.Min.X + bbox.Max.X) / 2.0,
+ (bbox.Min.Y + bbox.Max.Y) / 2.0,
+ (bbox.Min.Z + bbox.Max.Z) / 2.0
+ );
+
+ // 插入到空间索引
+ _globalSpatialIndex.Insert(item, center);
+
+ // 缓存位置
+ _objectPositions[item] = center;
+
+ indexedCount++;
+ }
+ catch (Exception ex)
+ {
+ LogManager.Warning($"[空间索引] 索引对象失败: {item.DisplayName}, 错误: {ex.Message}");
+ failedCount++;
+ }
+ }
+
+ _indexedObjectCount = indexedCount;
+ _isInitialized = true;
+
+ sw.Stop();
+
+ LogManager.Info("[空间索引] 构建完成");
+ LogManager.Info($" - 成功索引: {indexedCount} 个对象");
+ LogManager.Info($" - 失败: {failedCount} 个对象");
+ LogManager.Info($" - 格子数量: {_globalSpatialIndex.CellCount} 个");
+ LogManager.Info($" - 耗时: {sw.ElapsedMilliseconds} ms");
+
+ // 输出统计信息
+ LogManager.Debug("[空间索引] " + _globalSpatialIndex.GetStatistics());
+ }
+ catch (Exception ex)
+ {
+ LogManager.Error($"[空间索引] 构建失败: {ex.Message}");
+ LogManager.Error($"[空间索引] 堆栈跟踪: {ex.StackTrace}");
+ _isInitialized = false;
+ throw;
+ }
+ }
+
+ ///
+ /// 范围查询:查找指定位置附近的对象
+ ///
+ /// 查询中心位置(Navisworks Point3D)
+ /// 查询半径(模型单位)
+ /// 要排除的对象(通常是移动物体本身)
+ /// 范围内的对象列表
+ public List FindNearbyObjects(
+ Point3D position,
+ double searchRadiusInModelUnits,
+ ModelItem excludeObject = null)
+ {
+ if (!_isInitialized)
+ {
+ throw new InvalidOperationException(
+ "[空间索引] 空间索引未初始化,请先调用 BuildGlobalIndex()");
+ }
+
+ var sw = Stopwatch.StartNew();
+
+ try
+ {
+ // 转换为 geometry4Sharp 的 Vector3d
+ var centerVec = new Vector3d(position.X, position.Y, position.Z);
+
+ // 使用空间哈希网格进行范围查询(带精确距离检查)
+ var nearbyObjects = _globalSpatialIndex.FindInRadiusExact(
+ centerVec,
+ searchRadiusInModelUnits,
+ getPositionFunc: item =>
+ {
+ // 从缓存中获取对象位置
+ if (_objectPositions.TryGetValue(item, out var pos))
+ {
+ return pos;
+ }
+
+ // 缓存未命中,重新计算(理论上不应该发生)
+ var bbox = item.BoundingBox();
+ return new Vector3d(
+ (bbox.Min.X + bbox.Max.X) / 2.0,
+ (bbox.Min.Y + bbox.Max.Y) / 2.0,
+ (bbox.Min.Z + bbox.Max.Z) / 2.0
+ );
+ }
+ );
+
+ // 排除指定对象
+ if (excludeObject != null)
+ {
+ nearbyObjects = nearbyObjects.Where(obj => !obj.Equals(excludeObject)).ToList();
+ }
+
+ sw.Stop();
+
+ LogManager.Debug($"[空间索引] 范围查询完成: " +
+ $"位置=({position.X:F2}, {position.Y:F2}, {position.Z:F2}), " +
+ $"半径={searchRadiusInModelUnits:F2}, " +
+ $"结果={nearbyObjects.Count} 个对象, " +
+ $"耗时={sw.Elapsed.TotalMilliseconds:F2}ms");
+
+ return nearbyObjects;
+ }
+ catch (Exception ex)
+ {
+ LogManager.Error($"[空间索引] 范围查询失败: {ex.Message}");
+ throw;
+ }
+ }
+
+ ///
+ /// 范围查询(带排除列表)
+ ///
+ /// 查询中心位置
+ /// 查询半径
+ /// 要排除的对象列表
+ /// 范围内的对象列表
+ public List FindNearbyObjects(
+ Point3D position,
+ double searchRadiusInModelUnits,
+ IEnumerable excludeObjects)
+ {
+ var results = FindNearbyObjects(position, searchRadiusInModelUnits, excludeObject: null);
+
+ if (excludeObjects != null)
+ {
+ var excludeSet = new HashSet(excludeObjects);
+ results = results.Where(obj => !excludeSet.Contains(obj)).ToList();
+ }
+
+ return results;
+ }
+
+ ///
+ /// 获取对象的缓存位置
+ ///
+ /// 模型对象
+ /// 对象的3D位置(Vector3d)
+ public Vector3d? GetObjectPosition(ModelItem item)
+ {
+ if (_objectPositions.TryGetValue(item, out var pos))
+ {
+ return pos;
+ }
+ return null;
+ }
+
+ ///
+ /// 清除空间索引
+ ///
+ public void Clear()
+ {
+ LogManager.Info("[空间索引] 清除空间索引");
+
+ _globalSpatialIndex?.Clear();
+ _objectPositions.Clear();
+ _isInitialized = false;
+ _indexedObjectCount = 0;
+
+ LogManager.Info("[空间索引] 空间索引已清除");
+ }
+
+ ///
+ /// 获取空间索引统计信息
+ ///
+ /// 统计信息字符串
+ public string GetStatistics()
+ {
+ if (!_isInitialized)
+ {
+ return "[空间索引] 未初始化";
+ }
+
+ return $"[空间索引] 统计信息:\n" +
+ $" - 已索引对象: {_indexedObjectCount} 个\n" +
+ $" - 格子大小: {_cellSize:F2} 模型单位\n" +
+ $" - 格子数量: {_globalSpatialIndex.CellCount} 个\n" +
+ $"{_globalSpatialIndex.GetStatistics()}";
+ }
+ }
+}