Its pretty much Native Octree from here https://github.com/marijnz/NativeOctree, except a bit refactored and has some extras.
using System;
using System.Diagnostics;
using Unity.Burst;
using Unity.Collections;
using Unity.Collections.LowLevel.Unsafe;
using Unity.Mathematics;
namespace Octree
{
// Represents an element node in the octree.
public struct OctElement<T> where T : unmanaged
{
public float3 Pos;
public T Element;
}
internal struct OctNode
{
// Points to this node's first child index in elements
public int FirstChildIndex;
// Number of elements in the leaf
public short Count;
public bool IsLeaf;
}
/// <summary>
/// An Octree aimed to be used with Burst, supports fast bulk insertion and querying.
///
/// TODO:
/// - Better test coverage
/// - Automated depth / bounds / max leaf elements calculation
/// </summary>
[NativeContainer]
public unsafe partial struct NativeOctree<T> : IDisposable where T : unmanaged {
#region [Properties]
public bool IsCreated => (IntPtr) _elements != IntPtr.Zero;
#endregion
#region [Fields]
#if ENABLE_UNITY_COLLECTIONS_CHECKS
// Safety
private AtomicSafetyHandle _safetyHandle;
[NativeSetClassTypeToNullOnSchedule]
private DisposeSentinel _disposeSentinel;
private static int _staticSafetyId;
#endif
// Data
[NativeDisableUnsafePtrRestriction]
private UnsafeList* _elements;
[NativeDisableUnsafePtrRestriction]
private UnsafeList* _lookup;
[NativeDisableUnsafePtrRestriction]
private UnsafeList* _nodes;
private int _elementsCount;
private readonly int _maxDepth;
private readonly short _maxLeafElements;
private AABB _bounds; // NOTE: Currently assuming uniform
#endregion
/// <summary>
/// Create a new Octree.
/// - 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 NativeOctree(AABB bounds = new AABB(),
Allocator allocator = Allocator.Temp,
int maxDepth = 6,
short maxLeafElements = 16,
int initialElementsCapacity = 256) : this() {
_bounds = bounds;
_maxDepth = maxDepth;
_maxLeafElements = maxLeafElements;
_elementsCount = 0;
if(maxDepth > 8)
{
// Currently no support for higher depths, the morton code lookup tables would have to support it
throw new InvalidOperationException();
}
// Allocate memory for every depth, the nodes on all depths are stored in a single continuous array
int totalSize = LookupTables.DepthSizeLookup[maxDepth+1];
_lookup = UnsafeList.Create(UnsafeUtility.SizeOf<int>(),
UnsafeUtility.AlignOf<int>(),
totalSize,
allocator,
NativeArrayOptions.ClearMemory);
_nodes = UnsafeList.Create(UnsafeUtility.SizeOf<OctNode>(),
UnsafeUtility.AlignOf<OctNode>(),
totalSize,
allocator,
NativeArrayOptions.ClearMemory);
_elements = UnsafeList.Create(UnsafeUtility.SizeOf<OctElement<T>>(),
UnsafeUtility.AlignOf<OctElement<T>>(),
initialElementsCapacity,
allocator);
#if ENABLE_UNITY_COLLECTIONS_CHECKS
CheckIsUnmanaged<T>();
DisposeSentinel.Create(out _safetyHandle, out _disposeSentinel, 1, allocator);
AssignStaticSafetyId(ref _safetyHandle);
#endif
}
#if ENABLE_UNITY_COLLECTIONS_CHECKS
[BurstDiscard]
private static void AssignStaticSafetyId(ref AtomicSafetyHandle safetyHandle)
{
// static safety IDs are unique per-type, and should only be initialized the first time an instance of
// the type is created.
if (_staticSafetyId == 0)
{
_staticSafetyId = AtomicSafetyHandle.NewStaticSafetyId<NativeOctree<T>>();
// Each static safety ID can optionally provide custom error messages for each AtomicSafetyErrorType.
// This is rarely necessary, but can be useful if higher-level code can provide more helpful error guidance
// than the default error message.
byte[] errorMsg = System.Text.Encoding.UTF8.GetBytes("The {5} has been deallocated before being passed into a job");
fixed (byte* pointerToMsgBytes = errorMsg)
{
AtomicSafetyHandle.SetCustomErrorMessage(_staticSafetyId, AtomicSafetyErrorType.DeallocatedFromJob, pointerToMsgBytes, errorMsg.Length);
}
}
AtomicSafetyHandle.SetStaticSafetyId(ref safetyHandle, _staticSafetyId);
}
#endif
/// <summary>
/// Sets AABB bounds of the tree
/// - Ensure the bounds are not way bigger than needed, otherwise the buckets are very off. Probably best to calculate bounds
/// </summary>
public void SetBounds(AABB bounds) {
_bounds = bounds;
}
[Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")]
[BurstDiscard] // Must use BurstDiscard because UnsafeUtility.IsUnmanaged is not burstable.
[NotBurstCompatible]
private static void CheckIsUnmanaged<T1>()
{
if (!UnsafeUtility.IsValidNativeContainerElementType<T>())
{
throw new ArgumentException($"{typeof(T1)} used in native collection is not blittable, "
+ $"not primitive, or contains a type tagged as NativeContainer");
}
}
public void ClearAndBulkInsert(NativeArray<OctElement<T>> incomingElements)
{ // Always have to clear before bulk insert as otherwise the lookup and node allocations need to account
// for existing data.
Clear();
#if ENABLE_UNITY_COLLECTIONS_CHECKS
AtomicSafetyHandle.CheckWriteAndBumpSecondaryVersion(_safetyHandle);
#endif
// Resize if needed
if(_elements->Capacity < _elementsCount + incomingElements.Length)
{
_elements->Resize<OctElement<T>>(math.max(incomingElements.Length, _elements->Capacity*2));
}
// Prepare morton codes
NativeArray<int> mortonCodes = new NativeArray<int>(incomingElements.Length, Allocator.Temp);
// ReSharper disable Unity.BurstLoadingManagedType -> static readonly
float3 depthExtentsScaling = LookupTables.DepthLookup[_maxDepth] / _bounds.Extents;
for (int i = 0; i < incomingElements.Length; i++)
{
float3 incPos = incomingElements[i].Pos;
incPos -= _bounds.Center; // Offset by center
incPos.y = -incPos.y; // World -> array
float3 pos = (incPos + _bounds.Extents) * .5f; // Make positive
// Now scale into available space that belongs to the depth
pos *= depthExtentsScaling;
// And interleave the bits for the morton code
mortonCodes[i] =(int) (LookupTables.MortonLookup[(int) pos.x] | (LookupTables.MortonLookup[(int) pos.y] << 1) | (LookupTables.MortonLookup[(int) pos.z] << 2));
}
// ReSharper restore Unity.BurstLoadingManagedType
// Index total child element count per node (total, so parent's counts include those of child nodes)
for (int i = 0; i < mortonCodes.Length; i++)
{
int atIndex = 0;
for (int depth = 0; depth <= _maxDepth; depth++)
{
// Increment the node on this depth that this element is contained in
(*(int*) ((IntPtr) _lookup->Ptr + atIndex * sizeof (int)))++;
atIndex = IncrementIndex(depth, mortonCodes, i, atIndex);
}
}
// Prepare the tree leaf nodes
RecursivePrepareLeaves(1, 1);
// Add elements to leaf nodes
for (int i = 0; i < incomingElements.Length; i++)
{
int atIndex = 0;
for (int depth = 0; depth <= _maxDepth; depth++)
{
var node = UnsafeUtility.ReadArrayElement<OctNode>(_nodes->Ptr, atIndex);
if(node.IsLeaf)
{
// We found a leaf, add this element to it and move to the next element
UnsafeUtility.WriteArrayElement(_elements->Ptr, node.FirstChildIndex + node.Count, incomingElements[i]);
node.Count++;
UnsafeUtility.WriteArrayElement(_nodes->Ptr, atIndex, node);
break;
}
// No leaf found, we keep going deeper until we find one
atIndex = IncrementIndex(depth, mortonCodes, i, atIndex);
}
}
mortonCodes.Dispose();
}
private int IncrementIndex(int depth, NativeArray<int> mortonCodes, int i, int atIndex)
{
int atDepth = math.max(0, _maxDepth - depth);
// Shift to the right and only get the first three bits
int shiftedMortonCode = (mortonCodes[i] >> ((atDepth - 1) * 3)) & 0b111;
// so the index becomes that... (0,1,2,3)
atIndex += LookupTables.DepthSizeLookup[atDepth] * shiftedMortonCode;
atIndex++; // offset for self
return atIndex;
}
private void RecursivePrepareLeaves(int prevOffset, int depth)
{
for (int l = 0; l < 8; l++)
{
int at = prevOffset + l * LookupTables.DepthSizeLookup[_maxDepth - depth+1];
int elementCount = UnsafeUtility.ReadArrayElement<int>(_lookup->Ptr, at);
if(elementCount > _maxLeafElements && depth < _maxDepth)
{
// There's more elements than allowed on this node so keep going deeper
RecursivePrepareLeaves(at+1, depth+1);
}
else if(elementCount != 0)
{
// We either hit max depth or there's less than the max elements on this node, make it a leaf
OctNode node = new OctNode {FirstChildIndex = _elementsCount, Count = 0, IsLeaf = true };
UnsafeUtility.WriteArrayElement(_nodes->Ptr, at, node);
_elementsCount += elementCount;
}
}
}
public void RangeQuery(AABB bounds, NativeList<OctElement<T>> results)
{
#if ENABLE_UNITY_COLLECTIONS_CHECKS
AtomicSafetyHandle.CheckReadAndThrow(_safetyHandle);
#endif
new OctreeRangeQuery().Query(this, bounds, results);
}
public void Clear()
{
#if ENABLE_UNITY_COLLECTIONS_CHECKS
AtomicSafetyHandle.CheckWriteAndBumpSecondaryVersion(_safetyHandle);
#endif
UnsafeUtility.MemClear(_lookup->Ptr, _lookup->Capacity * UnsafeUtility.SizeOf<int>());
UnsafeUtility.MemClear(_nodes->Ptr, _nodes->Capacity * UnsafeUtility.SizeOf<OctNode>());
UnsafeUtility.MemClear(_elements->Ptr, _elements->Capacity * UnsafeUtility.SizeOf<OctElement<T>>());
_elementsCount = 0;
}
public void Dispose()
{
UnsafeList.Destroy(_elements);
_elements = null;
UnsafeList.Destroy(_lookup);
_lookup = null;
UnsafeList.Destroy(_nodes);
_nodes = null;
#if ENABLE_UNITY_COLLECTIONS_CHECKS
DisposeSentinel.Dispose(ref _safetyHandle, ref _disposeSentinel);
#endif
}
}
}
namespace Octree
{
public static class LookupTables
{
public static readonly uint[] MortonLookup = {
0x00000000,
0x00000001, 0x00000008, 0x00000009, 0x00000040, 0x00000041, 0x00000048, 0x00000049, 0x00000200,
0x00000201, 0x00000208, 0x00000209, 0x00000240, 0x00000241, 0x00000248, 0x00000249, 0x00001000,
0x00001001, 0x00001008, 0x00001009, 0x00001040, 0x00001041, 0x00001048, 0x00001049, 0x00001200,
0x00001201, 0x00001208, 0x00001209, 0x00001240, 0x00001241, 0x00001248, 0x00001249, 0x00008000,
0x00008001, 0x00008008, 0x00008009, 0x00008040, 0x00008041, 0x00008048, 0x00008049, 0x00008200,
0x00008201, 0x00008208, 0x00008209, 0x00008240, 0x00008241, 0x00008248, 0x00008249, 0x00009000,
0x00009001, 0x00009008, 0x00009009, 0x00009040, 0x00009041, 0x00009048, 0x00009049, 0x00009200,
0x00009201, 0x00009208, 0x00009209, 0x00009240, 0x00009241, 0x00009248, 0x00009249, 0x00040000,
0x00040001, 0x00040008, 0x00040009, 0x00040040, 0x00040041, 0x00040048, 0x00040049, 0x00040200,
0x00040201, 0x00040208, 0x00040209, 0x00040240, 0x00040241, 0x00040248, 0x00040249, 0x00041000,
0x00041001, 0x00041008, 0x00041009, 0x00041040, 0x00041041, 0x00041048, 0x00041049, 0x00041200,
0x00041201, 0x00041208, 0x00041209, 0x00041240, 0x00041241, 0x00041248, 0x00041249, 0x00048000,
0x00048001, 0x00048008, 0x00048009, 0x00048040, 0x00048041, 0x00048048, 0x00048049, 0x00048200,
0x00048201, 0x00048208, 0x00048209, 0x00048240, 0x00048241, 0x00048248, 0x00048249, 0x00049000,
0x00049001, 0x00049008, 0x00049009, 0x00049040, 0x00049041, 0x00049048, 0x00049049, 0x00049200,
0x00049201, 0x00049208, 0x00049209, 0x00049240, 0x00049241, 0x00049248, 0x00049249, 0x00200000,
0x00200001, 0x00200008, 0x00200009, 0x00200040, 0x00200041, 0x00200048, 0x00200049, 0x00200200,
0x00200201, 0x00200208, 0x00200209, 0x00200240, 0x00200241, 0x00200248, 0x00200249, 0x00201000,
0x00201001, 0x00201008, 0x00201009, 0x00201040, 0x00201041, 0x00201048, 0x00201049, 0x00201200,
0x00201201, 0x00201208, 0x00201209, 0x00201240, 0x00201241, 0x00201248, 0x00201249, 0x00208000,
0x00208001, 0x00208008, 0x00208009, 0x00208040, 0x00208041, 0x00208048, 0x00208049, 0x00208200,
0x00208201, 0x00208208, 0x00208209, 0x00208240, 0x00208241, 0x00208248, 0x00208249, 0x00209000,
0x00209001, 0x00209008, 0x00209009, 0x00209040, 0x00209041, 0x00209048, 0x00209049, 0x00209200,
0x00209201, 0x00209208, 0x00209209, 0x00209240, 0x00209241, 0x00209248, 0x00209249, 0x00240000,
0x00240001, 0x00240008, 0x00240009, 0x00240040, 0x00240041, 0x00240048, 0x00240049, 0x00240200,
0x00240201, 0x00240208, 0x00240209, 0x00240240, 0x00240241, 0x00240248, 0x00240249, 0x00241000,
0x00241001, 0x00241008, 0x00241009, 0x00241040, 0x00241041, 0x00241048, 0x00241049, 0x00241200,
0x00241201, 0x00241208, 0x00241209, 0x00241240, 0x00241241, 0x00241248, 0x00241249, 0x00248000,
0x00248001, 0x00248008, 0x00248009, 0x00248040, 0x00248041, 0x00248048, 0x00248049, 0x00248200,
0x00248201, 0x00248208, 0x00248209, 0x00248240, 0x00248241, 0x00248248, 0x00248249, 0x00249000,
0x00249001, 0x00249008, 0x00249009, 0x00249040, 0x00249041, 0x00249048, 0x00249049, 0x00249200,
0x00249201, 0x00249208, 0x00249209, 0x00249240, 0x00249241, 0x00249248, 0x00249249
};
public static readonly int[] DepthSizeLookup =
{
0,
1,
1+2*2*2,
1+2*2+4*4*4,
1+2*2+4*4*4+8*8*8,
1+2*2+4*4*4+8*8*8+16*16*16,
1+2*2+4*4*4+8*8*8+16*16*16+32*32*32,
1+2*2+4*4*4+8*8*8+16*16*16+32*32*32+64*64*64,
1+2*2+4*4*4+8*8*8+16*16*16+32*32*32+64*64*64+128*128*128,
1+2*2+4*4*4+8*8*8+16*16*16+32*32*32+64*64*64+128*128*128+256*256*256,
};
public static readonly int[] DepthLookup =
{
0,
2,
4,
8,
16,
32,
64,
128,
256,
};
}
}
using System;
using Unity.Collections;
using Unity.Collections.LowLevel.Unsafe;
using Unity.Mathematics;
using static Unity.Mathematics.math;
namespace Octree
{
public unsafe partial struct NativeOctree<T> where T : unmanaged
{
private struct OctreeRangeQuery
{
private NativeOctree<T> _tree;
private UnsafeList* _fastResults;
private int _count;
private AABB _bounds;
public void Query(NativeOctree<T> tree, AABB bounds, NativeList<OctElement<T>> results)
{
_tree = tree;
_bounds = bounds;
_count = 0;
// Get pointer to inner list data for faster writing
_fastResults = (UnsafeList*) NativeListUnsafeUtility.GetInternalListDataPtrUnchecked(ref results);
RecursiveRangeQuery(tree._bounds, false, 1, 1);
_fastResults->Length = _count;
}
public void RecursiveRangeQuery(AABB parentBounds, bool parentContained, int prevOffset, int depth)
{
if(_count + 8 * _tree._maxLeafElements > _fastResults->Capacity)
{
_fastResults->Resize<OctElement<T>>(max(_fastResults->Capacity * 2, _count + 8 * _tree._maxLeafElements));
}
int depthSize = LookupTables.DepthSizeLookup[_tree._maxDepth - depth+1];
for (int l = 0; l < 8; l++)
{
var childBounds = GetChildBounds(parentBounds, l);
var contained = parentContained;
if(!contained)
{
if(_bounds.Contains(childBounds))
{
contained = true;
}
else if(!Intersects(_bounds, childBounds))
{
continue;
}
}
int at = prevOffset + l * depthSize;
int elementCount = UnsafeUtility.ReadArrayElement<int>(_tree._lookup->Ptr, at);
if(elementCount > _tree._maxLeafElements && depth < _tree._maxDepth)
{
RecursiveRangeQuery(childBounds, contained, at+1, depth+1);
}
else if(elementCount != 0)
{
var node = UnsafeUtility.ReadArrayElement<OctNode>(_tree._nodes->Ptr, at);
if(contained)
{
var index = (void*) ((IntPtr) _tree._elements->Ptr + node.FirstChildIndex * UnsafeUtility.SizeOf<OctElement<T>>());
UnsafeUtility.MemCpy((void*) ((IntPtr) _fastResults->Ptr + _count * UnsafeUtility.SizeOf<OctElement<T>>()),
index, node.Count * UnsafeUtility.SizeOf<OctElement<T>>());
_count += node.Count;
}
else
{
for (int k = 0; k < node.Count; k++)
{
var element = UnsafeUtility.ReadArrayElement<OctElement<T>>(_tree._elements->Ptr, node.FirstChildIndex + k);
if(_bounds.Contains(element.Pos))
{
UnsafeUtility.WriteArrayElement(_fastResults->Ptr, _count++, element);
}
}
}
}
}
}
public bool Intersects(AABB a, AABB b)
{
return abs(a.Center[0] - b.Center[0]) < a.Extents[0] + b.Extents[0] &&
abs(a.Center[1] - b.Center[1]) < a.Extents[1] + b.Extents[1] &&
abs(a.Center[2] - b.Center[2]) < a.Extents[2] + b.Extents[2];
}
static AABB GetChildBounds(AABB parentBounds, int childZIndex)
{
var half = parentBounds.Extents.x * .5f;
switch (childZIndex)
{
case 0: return new AABB { Center = new float3(parentBounds.Center.x - half, parentBounds.Center.y + half, parentBounds.Center.z - half), Extents = half};
case 1: return new AABB { Center = new float3(parentBounds.Center.x + half, parentBounds.Center.y + half, parentBounds.Center.z - half), Extents = half};
case 2: return new AABB { Center = new float3(parentBounds.Center.x - half, parentBounds.Center.y - half, parentBounds.Center.z - half), Extents = half};
case 3: return new AABB { Center = new float3(parentBounds.Center.x + half, parentBounds.Center.y - half, parentBounds.Center.z - half), Extents = half};
case 4: return new AABB { Center = new float3(parentBounds.Center.x - half, parentBounds.Center.y + half, parentBounds.Center.z + half), Extents = half};
case 5: return new AABB { Center = new float3(parentBounds.Center.x + half, parentBounds.Center.y + half, parentBounds.Center.z + half), Extents = half};
case 6: return new AABB { Center = new float3(parentBounds.Center.x - half, parentBounds.Center.y - half, parentBounds.Center.z + half), Extents = half};
case 7: return new AABB { Center = new float3(parentBounds.Center.x + half, parentBounds.Center.y - half, parentBounds.Center.z + half), Extents = half};
default: throw new Exception();
}
}
}
}
}