aghuahu

src/Oct.cs

@@ -0,0 +1,144 @@
+#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;
+    }
+}