306 lines
8.9 KiB
C#
306 lines
8.9 KiB
C#
|
using System;
|
|||
|
|
using System.Buffers;
|
|||
|
|
using System.Collections;
|
|||
|
|
using System.Collections.Concurrent;
|
|||
|
|
using System.Collections.Generic;
|
|||
|
|
using System.Linq;
|
|||
|
|
using System.Net.Http.Headers;
|
|||
|
|
using System.Security.Policy;
|
|||
|
|
using System.Threading;
|
|||
|
|
using System.Threading.Tasks;
|
|||
|
|
using BITKit;
|
|||
|
|
#if UNITY_5_3_OR_NEWER
|
|||
|
|
using Unity.Burst;
|
|||
|
|
#endif
|
|||
|
|
using Unity.Mathematics;
|
|||
|
|
|
|||
|
|
namespace Net.BITKit.Quadtree
|
|||
|
|
{
|
|||
|
|
public class Quadtree
|
|||
|
|
{
|
|||
|
|
public QuadtreeNode Root { get; private set; }
|
|||
|
|
|
|||
|
|
public IDictionary<int, float2> Positions
|
|||
|
|
{
|
|||
|
|
get
|
|||
|
|
{
|
|||
|
|
Expansion();
|
|||
|
|
return _positions;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public IDictionary<int, float2> Sizes => _sizes;
|
|||
|
|
private readonly Dictionary<int, float2> _positions;
|
|||
|
|
private readonly ConcurrentQueue<(int, float2)> _queue;
|
|||
|
|
private readonly Dictionary<int, float2> _sizes;
|
|||
|
|
|
|||
|
|
public Quadtree(float2 center, float2 size)
|
|||
|
|
{
|
|||
|
|
_sizes = new Dictionary<int, float2>();
|
|||
|
|
_positions = new Dictionary<int, float2>();
|
|||
|
|
_queue = new ConcurrentQueue<(int, float2)>();
|
|||
|
|
Root = new QuadtreeNode(this, center, size);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public void Insert(in int objectId, in float2 position, in float2 size = default)
|
|||
|
|
{
|
|||
|
|
_queue.Enqueue((objectId, position));
|
|||
|
|
|
|||
|
|
if (size.x is not 0)
|
|||
|
|
{
|
|||
|
|
_sizes.TryAdd(objectId, size);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
var root = Root;
|
|||
|
|
|
|||
|
|
InsertRecursive(ref root, in objectId, in position);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private void InsertRecursive(ref QuadtreeNode node, in int objectId, in float2 position)
|
|||
|
|
{
|
|||
|
|
if (!node.Contains(position))
|
|||
|
|
return;
|
|||
|
|
if (node.Objects.Count < 4 || node.Size.x <= 1f || node.Depth > Root.Size.x / 8) // 假设最小节点大小为1
|
|||
|
|
{
|
|||
|
|
node.Objects.Add(objectId);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
if (node.Children[0].Size.x == 0) // 如果子节点未初始化
|
|||
|
|
{
|
|||
|
|
node.Split();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
for (var i = 0; i < 4; i++)
|
|||
|
|
{
|
|||
|
|
InsertRecursive(ref node.Children[i], objectId, position);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public Span<int> Query(float2 position, float radius)
|
|||
|
|
{
|
|||
|
|
Expansion();
|
|||
|
|
|
|||
|
|
var index = 0;
|
|||
|
|
|
|||
|
|
var root = Root;
|
|||
|
|
|
|||
|
|
var pool = ArrayPool<int>.Shared;
|
|||
|
|
|
|||
|
|
var array = pool.Rent(math.ceilpow2(_positions.Count * 2));
|
|||
|
|
|
|||
|
|
pool.Return(array);
|
|||
|
|
|
|||
|
|
QueryRecursive(in root, in position, in radius, array.AsSpan(), ref index);
|
|||
|
|
foreach (var (key, size) in _sizes)
|
|||
|
|
{
|
|||
|
|
var pos = _positions[key];
|
|||
|
|
|
|||
|
|
if (IsCircleInRect(pos, radius, position, size))
|
|||
|
|
{
|
|||
|
|
array[index] = key;
|
|||
|
|
index++;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
return array.AsSpan()[..index];
|
|||
|
|
}
|
|||
|
|
finally
|
|||
|
|
{
|
|||
|
|
pool.Return(array);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
#if UNITY_5_3_OR_NEWER
|
|||
|
|
[BurstCompile]
|
|||
|
|
#endif
|
|||
|
|
private static bool IsCircleInRect(float2 point, float radius, float2 pos, float2 size)
|
|||
|
|
{
|
|||
|
|
var halfSize = size * 0.5f;
|
|||
|
|
var min = pos - halfSize; // 矩形左下角
|
|||
|
|
var max = pos + halfSize; // 矩形右上角
|
|||
|
|
|
|||
|
|
// 计算扩展后的包围盒
|
|||
|
|
var expandedMin = min - radius;
|
|||
|
|
var expandedMax = max + radius;
|
|||
|
|
|
|||
|
|
// 检查点是否在扩展的矩形内
|
|||
|
|
return point.x >= expandedMin.x && point.x <= expandedMax.x &&
|
|||
|
|
point.y >= expandedMin.y && point.y <= expandedMax.y;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private void Expansion()
|
|||
|
|
{
|
|||
|
|
while (_queue.TryDequeue(out var item))
|
|||
|
|
{
|
|||
|
|
_positions.TryAdd(item.Item1, item.Item2);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
#if UNITY_5_3_OR_NEWER
|
|||
|
|
[BurstCompile]
|
|||
|
|
#endif
|
|||
|
|
private void QueryRecursive(in QuadtreeNode node, in float2 position, in float radius, Span<int> result,
|
|||
|
|
ref int index)
|
|||
|
|
{
|
|||
|
|
if (!Intersects(node.Center, node.Size, position, radius))
|
|||
|
|
return;
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
foreach (var obj in node.Objects)
|
|||
|
|
{
|
|||
|
|
// 直接 TryGetValue，避免 `Positions[obj]` 可能的重复哈希查找
|
|||
|
|
if (!_positions.TryGetValue(obj, out var objPos)) continue;
|
|||
|
|
|
|||
|
|
// 计算平方距离，避免额外变量
|
|||
|
|
if (math.dot(objPos - position, objPos - position) > radius * radius) continue;
|
|||
|
|
|
|||
|
|
// 直接写入 result
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
result[index] = obj;
|
|||
|
|
}
|
|||
|
|
catch (Exception)
|
|||
|
|
{
|
|||
|
|
#if UNITY_EDITOR
|
|||
|
|
var unions = result.ToArray().Distinct();
|
|||
|
|
var hashSet = new HashSet<int>();
|
|||
|
|
For(Root);
|
|||
|
|
void For(in QuadtreeNode node)
|
|||
|
|
{
|
|||
|
|
foreach (var child in node.Children)
|
|||
|
|
{
|
|||
|
|
For(child);
|
|||
|
|
}
|
|||
|
|
foreach (var x in node.Objects)
|
|||
|
|
{
|
|||
|
|
if (hashSet.Add(x) is false)
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
throw;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
index++;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
for (var i = 0; i < 4; i++)
|
|||
|
|
{
|
|||
|
|
if (node.Children[i].Size.x > 0)
|
|||
|
|
{
|
|||
|
|
QueryRecursive(in node.Children[i], in position, in radius, result, ref index);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
#if UNITY_5_3_OR_NEWER
|
|||
|
|
[BurstCompile]
|
|||
|
|
#endif
|
|||
|
|
private static bool Intersects(in float2 center, in float2 size, in float2 position, in float radius)
|
|||
|
|
{
|
|||
|
|
// 计算 AABB 的最小/最大点
|
|||
|
|
var min = center - size * 0.5f;
|
|||
|
|
var max = center + size * 0.5f;
|
|||
|
|
|
|||
|
|
// 找到圆心到 AABB 的最近点
|
|||
|
|
var closest = math.clamp(position, min, max);
|
|||
|
|
|
|||
|
|
// 计算圆心到最近点的距离
|
|||
|
|
var delta = position - closest;
|
|||
|
|
var distanceSq = math.dot(delta, delta);
|
|||
|
|
|
|||
|
|
// 相交条件：如果距离平方 <= 半径平方
|
|||
|
|
return distanceSq <= (radius * radius);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
// 删除对象
|
|||
|
|
public bool Remove(int objectId)
|
|||
|
|
{
|
|||
|
|
Expansion();
|
|||
|
|
|
|||
|
|
if (_positions.TryGetValue(objectId, out var position) is false) return false;
|
|||
|
|
|
|||
|
|
_sizes.TryRemove(objectId);
|
|||
|
|
|
|||
|
|
var root = Root;
|
|||
|
|
|
|||
|
|
if (RemoveRecursive(ref root, objectId, position) is false) return false;
|
|||
|
|
_positions.Remove(objectId);
|
|||
|
|
|
|||
|
|
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
private static bool RemoveRecursive(ref QuadtreeNode node, int objectId, float2 position)
|
|||
|
|
{
|
|||
|
|
if (!node.Contains(position))
|
|||
|
|
return false;
|
|||
|
|
|
|||
|
|
// 尝试从当前节点删除
|
|||
|
|
bool removed = node.Objects.Remove(objectId);
|
|||
|
|
|
|||
|
|
// 如果当前节点是叶子节点，返回是否成功删除
|
|||
|
|
if (node.IsLeaf())
|
|||
|
|
{
|
|||
|
|
return removed;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 递归从子节点删除
|
|||
|
|
for (var i = 0; i < 4; i++)
|
|||
|
|
{
|
|||
|
|
if (RemoveRecursive(ref node.Children[i], objectId, position))
|
|||
|
|
{
|
|||
|
|
removed = true;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 如果对象被移除，尝试合并子节点
|
|||
|
|
if (removed)
|
|||
|
|
{
|
|||
|
|
TryMerge(ref node);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return removed;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private static void TryMerge(ref QuadtreeNode node)
|
|||
|
|
{
|
|||
|
|
if (node.IsLeaf()) return;
|
|||
|
|
|
|||
|
|
// 检查是否可以合并子节点
|
|||
|
|
var totalObjects = node.Objects.Count;
|
|||
|
|
for (var i = 0; i < 4; i++)
|
|||
|
|
{
|
|||
|
|
totalObjects += node.Children[i].Objects.Count;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 如果当前节点和所有子节点的对象数量小于等于阈值，则合并
|
|||
|
|
if (totalObjects <= 4)
|
|||
|
|
{
|
|||
|
|
// 把所有子节点的对象都合并到当前节点
|
|||
|
|
foreach (var child in node.Children)
|
|||
|
|
{
|
|||
|
|
node.Objects.UnionWith(child.Objects);
|
|||
|
|
child.Objects.Clear(); // 清空子节点的对象集合
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 清空子节点，重新初始化
|
|||
|
|
for (var i = 0; i < 4; i++)
|
|||
|
|
{
|
|||
|
|
node.Children[i] = new QuadtreeNode()
|
|||
|
|
{
|
|||
|
|
Depth = node.Depth + 1
|
|||
|
|
};
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|