a

1 files changed, 66 insertions, 0 deletions

diff --git a/src/spatial/octtree.rs b/src/spatial/octtree.rs
new file mode 100644
index 0000000..f8919f5
--- /dev/null
+++ b/src/spatial/octtree.rs
@@ -0,0 +1,66 @@
+use super::SpatialTree;
+use crate::NodeID;
+use glam::{DVec3, dvec3};
+use std::mem::swap;
+
+pub struct Octtree {
+    pub center: DVec3,
+    pub size: DVec3,
+    pub elems: Vec<(DVec3, NodeID)>,
+    pub children: Option<Box<[[[Octtree; 2]; 2]; 2]>>,
+}
+impl Octtree {
+    pub fn new(size: DVec3) -> Self {
+        Self {
+            center: DVec3::ZERO,
+            size,
+            children: None,
+            elems: Vec::new(),
+        }
+    }
+}
+impl SpatialTree for Octtree {
+    fn insert(&mut self, pos: DVec3, id: NodeID) {
+        if let Some(c) = &mut self.children {
+            let [x, y, z] = (pos - self.center)
+                .to_array()
+                .map(|e| if e > 0. { 1 } else { 0 });
+            c[x][y][z].insert(pos, id);
+        } else {
+            self.elems.push((pos, id));
+            if self.elems.len() > 64 {
+                self.children = Some(Box::new([-1., 1.].map(|x| {
+                    [-1., 1.].map(|y| {
+                        [-1., 1.].map(|z| Octtree {
+                            center: self.center + (self.size / 4.) * dvec3(x, y, z),
+                            size: self.size / 2.,
+                            elems: Vec::new(),
+                            children: None,
+                        })
+                    })
+                })));
+                let mut elems = Vec::new();
+                swap(&mut elems, &mut self.elems);
+                for (pos, id) in elems {
+                    self.insert(pos, id);
+                }
+            }
+        }
+    }
+
+    fn depth(&self) -> usize {
+        if let Some(c) = &self.children {
+            1 + c[0][0][0]
+                .depth()
+                .max(c[0][0][1].depth())
+                .max(c[0][1][0].depth())
+                .max(c[0][1][1].depth())
+                .max(c[1][0][0].depth())
+                .max(c[1][0][1].depth())
+                .max(c[1][1][0].depth())
+                .max(c[1][1][1].depth())
+        } else {
+            0
+        }
+    }
+}