Adetonics/src/Oct.cs

#nullable enable
using System.Collections.Generic;
using Godot;

namespace adatonic;

public class Node
{
    public Node()
    {
        
    }

    public Node(int id, Vector3 pos)
    {
        Id = id;
        Position = pos;
    }
    public int Id;
    public Vector3 Position;
}

public class Oct
{
    private Vector3 Start;
    private Vector3 Extent;
    
    Oct?[]? Trees = null;
    private Node? Node = null;
    public Oct()
    {
        Start = -Vector3.One;
        Extent = Vector3.One * 2f;
    }
    public Oct(Vector3 start, Vector3 extent)
    {
        Start = start;
        Extent = extent;
    }

    public void Insert(Node node)
    {
        if (!IsInside(node.Position))
        {
            GD.Print($"Failed to insert to Octree - Point out of bounds!");
            return;
        }

        if (Node == null && Trees == null)
        {
            Node = node;
            return;
        }
        Trees ??= new Oct?[8];
        if (Node != null)
        {
            int octOld = WhichSubOct(Node.Position);

            Trees[octOld] ??= new Oct(GetSubStart(octOld), Extent * 0.5f);
            Trees[octOld]?.Insert(Node);
            Node = null;
        }
        
        int oct = WhichSubOct(node.Position);
        Trees[oct] ??= new Oct(GetSubStart(oct), Extent * 0.5f);
        Trees[oct]?.Insert(node);
    }
    public Node? SearchNearest(Vector3 position)
    {
        Node? best = null;
        float bestDist = float.MaxValue;

        SearchNearest(position, ref best, ref bestDist);
        return best;
    }
    private void SearchNearest(Vector3 position, ref Node? best, ref float bestDist)
    {
        if (Node != null)
        {
            float dist = position.DistanceSquaredTo(Node.Position);
            if (dist < bestDist)
            {
                bestDist = dist;
                best = Node;
            }
        }

        if (Trees == null)
            return;

        int first = WhichSubOct(position);
        
        Trees[first]?.SearchNearest(position, ref best, ref bestDist);
        
        for (int i = 0; i < 8; i++)
        {
            if (i == first || Trees[i] == null)
                continue;

            float boxDist = Trees[i]!.DistanceToBox(position);

            if (boxDist < bestDist)
            {
                Trees[i]!.SearchNearest(position, ref best, ref bestDist);
            }
        }
    }
    private float DistanceToBox(Vector3 p)
    {
        Vector3 min = Start;
        Vector3 max = Start + Extent;

        float dx = Mathf.Max(Mathf.Max(min.X - p.X, 0), p.X - max.X);
        float dy = Mathf.Max(Mathf.Max(min.Y - p.Y, 0), p.Y - max.Y);
        float dz = Mathf.Max(Mathf.Max(min.Z - p.Z, 0), p.Z - max.Z);

        return dx * dx + dy * dy + dz * dz;
    }

    public int WhichSubOct(Vector3 position)
    {
        bool left = position.X < Start.X + Extent.X * 0.5f;
        bool bottom = position.Y < Start.Y + Extent.Y * 0.5f;
        bool near = position.Z < Start.Z + Extent.Z * 0.5f;
        
        return (left ? 1 : 0) + (bottom ? 2 : 0) + (near ? 4 : 0);
    }

    public Vector3 GetSubStart(int oct)
    {
        Vector3 start = Vector3.Zero;
        bool left = (oct & (1 << 0)) != 0;
        bool bottom = (oct & (1 << 1)) != 0;
        bool near = (oct & (1 << 2)) != 0;
        start.X += left ? Start.X : Start.X + Extent.X * 0.5f;
        start.Y += bottom ? Start.Y : Start.Y + Extent.Y * 0.5f;
        start.Z += near ? Start.Z : Start.Z + Extent.Z * 0.5f;
        return start;
    }
    public bool IsInside(Vector3 position)
    {
        return position > Start && position < Start + Extent;
    }
}