298 lines
8.9 KiB
C#
298 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;
|
|
using kcp2k;
|
|
#if UNITY_5_3_OR_NEWER
|
|
using Unity.Burst;
|
|
#endif
|
|
using Unity.Mathematics;
|
|
|
|
namespace Net.BITKit.Quadtree
|
|
{
|
|
public class Quadtree
|
|
{
|
|
public ref QuadtreeNode Root => ref _root;
|
|
private QuadtreeNode _root;
|
|
|
|
public IDictionary<int, float2> Positions
|
|
{
|
|
get
|
|
{
|
|
Expansion();
|
|
return _positions;
|
|
}
|
|
}
|
|
|
|
public readonly int MaxDepth;
|
|
|
|
public IDictionary<int, float2> Sizes => _sizes;
|
|
private readonly Dictionary<int, float2> _positions;
|
|
private readonly ConcurrentQueue<(int, float2)> _queue;
|
|
private readonly Dictionary<int, float2> _sizes;
|
|
private readonly ArrayPool<int> _pool=ArrayPool<int>.Create();
|
|
//internal readonly Pool<HashSet<int>> HashSetPool=new(Activator.CreateInstance<HashSet<int>>,null,64);
|
|
private int _queryCount;
|
|
|
|
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);
|
|
|
|
MaxDepth = (int)math.log2(math.max(size.x, size.y));
|
|
}
|
|
|
|
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
|
|
{
|
|
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 is { Objects : {Count: < 4}, IsLeaf: true } || node.Depth>= MaxDepth) // 是叶子节点,并且数量小于4
|
|
{
|
|
node.Objects.Add(objectId);
|
|
}
|
|
else
|
|
{
|
|
if (node.IsLeaf)
|
|
node.Split();
|
|
|
|
for (var i = 0; i < 4; i++)
|
|
{
|
|
ref var child = ref node.Children[i];
|
|
InsertRecursive(ref child, objectId, position);
|
|
}
|
|
}
|
|
}
|
|
|
|
public Span<int> Query(float2 position, float radius)
|
|
{
|
|
_queryCount++;
|
|
|
|
Expansion();
|
|
|
|
var index = 0;
|
|
|
|
var array = _pool.Rent(math.ceilpow2(_positions.Count * 2));
|
|
|
|
QueryRecursive(ref _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++;
|
|
}
|
|
}
|
|
|
|
/*
|
|
if (index > _positions.Count+1)
|
|
{
|
|
throw new InvalidOperationException($"Query Count:{_queryCount},size:{_positions.Count},Query result length is {index}");
|
|
}
|
|
*/
|
|
_pool.Return(array);
|
|
return array.AsSpan()[..index];
|
|
}
|
|
#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(ref QuadtreeNode node, in float2 position, in float radius, Span<int> result,
|
|
ref int index)
|
|
{
|
|
if (!Intersects(node.Center, node.Size, position, radius))
|
|
return;
|
|
|
|
if (node.IsLeaf)
|
|
{
|
|
foreach (var obj in node.Objects)
|
|
{
|
|
var objPos = _positions[obj];
|
|
|
|
// 计算平方距离，避免额外变量
|
|
if (math.dot(objPos - position, objPos - position) > radius * radius) continue;
|
|
|
|
result[index] = obj;
|
|
|
|
index++;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for (var i = 0; i < 4; i++)
|
|
{
|
|
QueryRecursive(ref 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;
|
|
|
|
if (RemoveRecursive(ref _root, objectId, position) is false) return false;
|
|
|
|
_positions.Remove(objectId);
|
|
_sizes.TryRemove(objectId);
|
|
return true;
|
|
}
|
|
private bool RemoveRecursive(ref QuadtreeNode node, int objectId, float2 position)
|
|
{
|
|
if (!node.Contains(position))
|
|
return false;
|
|
|
|
// 尝试从当前节点删除
|
|
var 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)) continue;
|
|
removed = true;
|
|
break;
|
|
}
|
|
|
|
// 如果对象被移除，尝试合并子节点
|
|
if (removed)
|
|
{
|
|
TryMerge(ref node);
|
|
}
|
|
|
|
return removed;
|
|
}
|
|
|
|
public void Clear()
|
|
{
|
|
_positions.Clear();
|
|
_sizes.Clear();
|
|
_root = new QuadtreeNode(this,default,_root.Size);
|
|
}
|
|
|
|
public int Depth
|
|
{
|
|
get
|
|
{
|
|
var depth = 0;
|
|
Expansion();
|
|
For(ref _root);
|
|
return depth;
|
|
void For(ref QuadtreeNode node)
|
|
{
|
|
if (node.IsLeaf)
|
|
{
|
|
depth = math.max(node.Depth, depth);
|
|
return;
|
|
}
|
|
|
|
for (var index = 0; index < node.Children.Length; index++)
|
|
{
|
|
ref var child =ref node.Children[index];
|
|
For(ref child);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
private void TryMerge(ref QuadtreeNode node)
|
|
{
|
|
//Merge has some issue,just disable is
|
|
return;
|
|
if (node.IsLeaf) return;
|
|
|
|
var totalObjects = 0;
|
|
for (var i = 0; i < node.Children.Length; i++)
|
|
{
|
|
totalObjects += node.Children[i].Objects.Count;
|
|
}
|
|
// 如果当前节点和所有子节点的对象数量小于等于阈值，则合并
|
|
if (totalObjects <= 4) return;
|
|
// 把所有子节点的对象都合并到当前节点
|
|
for (var i = 0; i < node.Children.Length; i++)
|
|
{
|
|
ref var child = ref node.Children[i];
|
|
node.Objects.UnionWith(child.Objects);
|
|
node.Children[i] = default;
|
|
}
|
|
node.IsLeaf = true;
|
|
}
|
|
}
|
|
}
|