public void RangeQuery(AABB2D bounds, NativeList <QuadElement <T> > results)
{
#if ENABLE_UNITY_COLLECTIONS_CHECKS
AtomicSafetyHandle.CheckReadAndThrow(this.safetyHandle);
#endif
new QuadTreeRangeQuery().Query(this, bounds, results);
}
/// <summary>
/// Create a new QuadTree.
/// - Ensure the bounds are not way bigger than needed, otherwise the buckets are very off. Probably best to calculate bounds
/// - The higher the depth, the larger the overhead, it especially goes up at a depth of 7/8
/// </summary>
public NativeQuadTree(AABB2D bounds, Allocator allocator = Allocator.Temp, int maxDepth = 4, short maxLeafElements = 16, int initialElementsCapacity = 256) : this()
{
this.allocator = allocator;
this.bounds = bounds;
this.maxDepth = maxDepth;
this.maxLeafElements = maxLeafElements;
this.elementsCount = 0;
this.isCreated = true;
if (maxDepth > 8)
{
// Currently no support for higher depths, the morton code lookup tables would have to support it
throw new InvalidOperationException();
}
#if ENABLE_UNITY_COLLECTIONS_CHECKS
//CollectionHelper.CheckIsUnmanaged<T>();
DisposeSentinel.Create(out this.safetyHandle, out this.disposeSentinel, 1, allocator);
#endif
// Allocate memory for every depth, the nodes on all depths are stored in a single continuous array
var totalSize = LookupTables.DepthSizeLookup[maxDepth + 1];
this.lookup = new NativeArray <int>(
totalSize, allocator); //UnsafeList.Create(UnsafeUtility.SizeOf<int>(), UnsafeUtility.AlignOf<int>(), totalSize, allocator, NativeArrayOptions.ClearMemory);
this.nodes = new NativeArray <QuadNode>(
totalSize, allocator); //UnsafeList.Create(UnsafeUtility.SizeOf<QuadNode>(), UnsafeUtility.AlignOf<QuadNode>(), totalSize, allocator, NativeArrayOptions.ClearMemory);
this.elements = new NativeArray <QuadElement <T> >(initialElementsCapacity,
allocator); //UnsafeList.Create(UnsafeUtility.SizeOf<QuadElement<T>>(), UnsafeUtility.AlignOf<QuadElement<T>>(), initialElementsCapacity, allocator);
}
private static void DrawBounds(Color[][] texture, AABB2D bounds, NativeQuadTree <T> tree)
{
var widthMult = texture.Length / tree.bounds.Extents.x * 2 / 2 / 2;
var heightMult = texture[0].Length / tree.bounds.Extents.y * 2 / 2 / 2;
var widthAdd = tree.bounds.Center.x + tree.bounds.Extents.x;
var heightAdd = tree.bounds.Center.y + tree.bounds.Extents.y;
var top = new float2(bounds.Center.x, bounds.Center.y - bounds.Extents.y);
var left = new float2(bounds.Center.x - bounds.Extents.x, bounds.Center.y);
for (var leftToRight = 0; leftToRight < bounds.Extents.x * 2; leftToRight++)
{
var poxX = left.x + leftToRight;
texture[(int)((poxX + widthAdd) * widthMult)][(int)((bounds.Center.y + heightAdd + bounds.Extents.y) * heightMult)] = Color.blue;
texture[(int)((poxX + widthAdd) * widthMult)][(int)((bounds.Center.y + heightAdd - bounds.Extents.y) * heightMult)] = Color.blue;
}
for (var topToBottom = 0; topToBottom < bounds.Extents.y * 2; topToBottom++)
{
var posY = top.y + topToBottom;
texture[(int)((bounds.Center.x + widthAdd + bounds.Extents.x) * widthMult)][(int)((posY + heightAdd) * heightMult)] = Color.blue;
texture[(int)((bounds.Center.x + widthAdd - bounds.Extents.x) * widthMult)][(int)((posY + heightAdd) * heightMult)] = Color.blue;
}
}
public bool Intersects(AABB2D b)
{
//bool noOverlap = Min[0] > b.Max[0] ||
// b.Min[0] > Max[0]||
// Min[1] > b.Max[1] ||
// b.Min[1] > Max[1];
//
//return !noOverlap;
return(math.abs(this.Center[0] - b.Center[0]) < this.Extents[0] + b.Extents[0] &&
math.abs(this.Center[1] - b.Center[1]) < this.Extents[1] + b.Extents[1]);
}
public void Query(NativeQuadTree <T> tree, AABB2D bounds, NativeList <QuadElement <T> > results)
{
this.tree = tree;
this.bounds = bounds;
this.count = 0;
// Get pointer to inner list data for faster writing
//this.fastResults = (UnsafeList*)NativeListUnsafeUtility.GetInternalListDataPtrUnchecked(ref results);
this.RecursiveRangeQuery(results, tree.bounds, false, 1, 1);
results.Length = this.count;
//this.fastResults->Length = this.count;
}
private static AABB2D GetChildBounds(AABB2D parentBounds, int childZIndex)
{
var half = parentBounds.Extents.x * .5f;
switch (childZIndex)
{
case 0:
return(new AABB2D(new float2(parentBounds.Center.x - half, parentBounds.Center.y + half), half));
case 1:
return(new AABB2D(new float2(parentBounds.Center.x + half, parentBounds.Center.y + half), half));
case 2:
return(new AABB2D(new float2(parentBounds.Center.x - half, parentBounds.Center.y - half), half));
case 3:
return(new AABB2D(new float2(parentBounds.Center.x + half, parentBounds.Center.y - half), half));
default:
throw new Exception();
}
}
public bool Contains(AABB2D b)
{
return(this.Contains(b.Center + new float2(-b.Extents.x, -b.Extents.y)) && this.Contains(b.Center + new float2(-b.Extents.x, b.Extents.y)) &&
this.Contains(b.Center + new float2(b.Extents.x, -b.Extents.y)) && this.Contains(b.Center + new float2(b.Extents.x, b.Extents.y)));
}
public void RecursiveRangeQuery(NativeList <QuadElement <T> > results, AABB2D parentBounds, bool parentContained, int prevOffset, int depth)
{
/*if (this.count + 4 * this.tree.maxLeafElements > results.Length) {
* results.Resize(math.max(results.Length * 2, this.count + 4 * this.tree.maxLeafElements), NativeArrayOptions.ClearMemory);
* }*/
var depthSize = LookupTables.DepthSizeLookup[this.tree.maxDepth - depth + 1];
for (var l = 0; l < 4; ++l)
{
var childBounds = NativeQuadTree <T> .QuadTreeRangeQuery.GetChildBounds(parentBounds, l);
var contained = parentContained;
if (contained == false)
{
if (this.bounds.Contains(childBounds) == true)
{
contained = true;
}
else if (this.bounds.Intersects(childBounds) == false)
{
continue;
}
}
var at = prevOffset + l * depthSize;
var elementCount = this.tree.lookup[at]; //UnsafeUtility.ReadArrayElement<int>(tree.lookup->Ptr, at);
if (elementCount > this.tree.maxLeafElements && depth < this.tree.maxDepth)
{
this.RecursiveRangeQuery(results, childBounds, contained, at + 1, depth + 1);
}
else if (elementCount != 0)
{
var node = this.tree.nodes[at]; //UnsafeUtility.ReadArrayElement<QuadNode>(tree.nodes->Ptr, at);
if (contained == true)
{
var source = (void *)((IntPtr)this.tree.elements.GetUnsafePtr() + node.firstChildIndex * UnsafeUtility.SizeOf <QuadElement <T> >());
if (node.firstChildIndex < 0 || node.firstChildIndex >= this.tree.elements.Length)
{
throw new IndexOutOfRangeException($"{node.firstChildIndex} [0..{this.tree.elements.Length}]");
}
results.Resize(math.max(results.Length * 2, this.count + node.count), NativeArrayOptions.ClearMemory);
var dest = (void *)((IntPtr)results.GetUnsafePtr() + this.count * UnsafeUtility.SizeOf <QuadElement <T> >());
UnsafeUtility.MemCpy(dest, source, node.count * UnsafeUtility.SizeOf <QuadElement <T> >());
//NativeArrayUtils.Copy(this.tree.elements, node.firstChildIndex, ref results, this.count, node.count);
this.count += node.count;
}
else
{
results.Resize(math.max(results.Length * 2, this.count + node.count), NativeArrayOptions.ClearMemory);
for (var k = 0; k < node.count; k++)
{
var element = this.tree.elements[node.firstChildIndex + k];
//UnsafeUtility.ReadArrayElement<QuadElement<T>>(tree.elements->Ptr, node.firstChildIndex + k);
if (this.bounds.Contains(element.pos) == true)
{
//UnsafeUtility.WriteArrayElement(this.fastResults->Ptr, this.count++, element);
results[this.count++] = element;
}
}
}
}
}
}
public static void Draw(NativeQuadTree <T> tree, NativeList <QuadElement <T> > results, AABB2D range,
Color[][] texture)
{
var widthMult = texture.Length / tree.bounds.Extents.x * 2 / 2 / 2;
var heightMult = texture[0].Length / tree.bounds.Extents.y * 2 / 2 / 2;
var widthAdd = tree.bounds.Center.x + tree.bounds.Extents.x;
var heightAdd = tree.bounds.Center.y + tree.bounds.Extents.y;
for (var i = 0; i < tree.nodes.Length; i++)
{
var node = tree.nodes[i]; //UnsafeUtility.ReadArrayElement<QuadNode>(tree.nodes->Ptr, i);
if (node.count > 0)
{
for (var k = 0; k < node.count; k++)
{
//var element = UnsafeUtility.ReadArrayElement<QuadElement<T>>(tree.elements->Ptr, node.firstChildIndex + k);
var element = tree.elements[node.firstChildIndex + k];
texture[(int)((element.pos.x + widthAdd) * widthMult)]
[(int)((element.pos.y + heightAdd) * heightMult)] = Color.red;
}
}
}
foreach (var element in results)
{
texture[(int)((element.pos.x + widthAdd) * widthMult)]
[(int)((element.pos.y + heightAdd) * heightMult)] = Color.green;
}
NativeQuadTree <T> .DrawBounds(texture, range, tree);
}