Octree

Octree.cs

public sealed class Octree
{
    public Vector3 Origin { get; }
    public OctreeNode Root { get; }

    public Octree(Vector3 origin, int sideLength, ReadOnlySpan<int> span)
    {
        Origin = origin;
        Root = new OctreeNode(sideLength, span);
    }
    
    public Octree(Vector3 origin, int sideLength, Span<int> span, bool copy)
    {
        Origin = origin;
        Root = new OctreeNode(sideLength, span, copy);
    }
}

public sealed class OctreeNode
{
    public int SideLength { get; private set; }
    public ReadOnlyMemory<OctreeNode> Children { get; private set; }
    public int? Value { get; private set; }

    public OctreeNode(int sideLength, ReadOnlySpan<int> span)
    {
        var newSideLength = CeilPow2(sideLength);

        Memory<int> newMemory = new int[newSideLength * newSideLength * newSideLength];
        var newSpan = newMemory.Span;
        for (int x = 0; x < sideLength; x++)
        for (int z = 0; z < sideLength; z++)
        {
            var val = span.Slice((x * sideLength * sideLength) + (z * sideLength), sideLength);
            val.CopyTo(newSpan.Slice((x * newSideLength * newSideLength) + (z * newSideLength), sideLength));
        }

        Setup(newSideLength, newSpan);
    }
    
    public OctreeNode(int sideLength, Span<int> span, bool copy)
    {
        var newSideLength = CeilPow2(sideLength);

        if (!copy)
        {
            Debug.Assert(sideLength == newSideLength);
        }

        Span<int> newSpan;
        if (copy)
        {
            Memory<int> newMemory = new int[newSideLength * newSideLength * newSideLength];
            newSpan = newMemory.Span;
            for (int x = 0; x < sideLength; x++)
            for (int z = 0; z < sideLength; z++)
            {
                var val = span.Slice((x * sideLength * sideLength) + (z * sideLength), sideLength);
                val.CopyTo(newSpan.Slice((x * newSideLength * newSideLength) + (z * newSideLength), sideLength));
            }
        }
        else
        {
            newSpan = span;
        }

        Setup(newSideLength, newSpan);
    }

    private OctreeNode(int sideLength, Span<int> span)
    {
        Setup(sideLength, span);
    }

    private void Setup(int sideLength, Span<int> span)
    {
        SideLength = sideLength;
        
        if (sideLength == 1)
        {
            Value = span[0];
            Children = ReadOnlyMemory<OctreeNode>.Empty;
            return;
        }

        var c = span[0];
        var isSingle = true;

        var sideLengthPow2 = sideLength * sideLength;
        for (int x = 0; x < sideLength; x++)
        for (int z = 0; z < sideLength; z++)
        for (int y = 0; y < sideLength; y++)
        {
            var i = (x * sideLengthPow2) + (z * sideLength) + y;
            if (c != span[i])
            {
                isSingle = false;
                goto endEmptyDetection; // break all loops
            }
        }

        endEmptyDetection:

        if (isSingle)
        {
            Children = ReadOnlyMemory<OctreeNode>.Empty;
            Value = c;
            return;
        }

        Span<int> Slice(int sideLength, byte axis, Span<int> oldSpan,
            Span<int> newSpan)
        {
            Debug.Assert(newSpan.Length == sideLength * sideLength * sideLength);

            var xOffset = (axis & 0b001) != 0 ? sideLength : 0;
            var yOffset = (axis & 0b010) != 0 ? sideLength : 0;
            var zOffset = (axis & 0b100) != 0 ? sideLength : 0;

            var sideLengthPow2 = sideLength * sideLength;
            for (int x = 0; x < sideLength; x++)
            for (int z = 0; z < sideLength; z++)
            {
                var newSlice = newSpan.Slice((x * sideLengthPow2) + (z * sideLength),
                    sideLength);
                var oldSlice =
                    oldSpan.Slice(
                        ((x + xOffset) * sideLengthPow2) + ((z + yOffset) * sideLength) + zOffset,
                        sideLength);
                oldSlice.CopyTo(newSlice);
            }

            return newSpan;
        }

        Memory<OctreeNode> children = new OctreeNode[8];
        var childrenSpan = children.Span;
        var newSideLength = sideLength / 2;

        var newDataSize = newSideLength * newSideLength * newSideLength;
        var newData = span.Slice(0, newDataSize);
        for (byte i = 0; i < 7; i++)
        {
            childrenSpan[i] = new OctreeNode(newSideLength, Slice(newSideLength, i, span, newData));

            if (childrenSpan[i].Value == 0)
                childrenSpan[i] = null;
        }
        
        Children = children;
        Value = null;
    }

    private static int CeilPow2(int v)
    {
        v--;
        v |= v >> 1;
        v |= v >> 2;
        v |= v >> 4;
        v |= v >> 8;
        v |= v >> 16;
        v++;
        return v;
    }
}