3 files changed, 134 insertions, 0 deletions
diff --git a/src/spatial/mod.rs b/src/spatial/mod.rs
new file mode 100644
index 0000000..c09824d
--- /dev/null
+++ b/src/spatial/mod.rs
@@ -0,0 +1,9 @@
+use crate::NodeID;
+use glam::DVec3;
+pub mod mtree;
+pub mod octtree;
+
+pub trait SpatialTree {
+    fn insert(&mut self, pos: DVec3, id: NodeID);
+    fn depth(&self) -> usize;
+}
diff --git a/src/spatial/mtree.rs b/src/spatial/mtree.rs
new file mode 100644
index 0000000..d8d2282
--- /dev/null
+++ b/src/spatial/mtree.rs
@@ -0,0 +1,59 @@
+use super::SpatialTree;
+use crate::NodeID;
+use glam::DVec3;
+
+pub struct MTree {
+    center: DVec3,
+    id: NodeID,
+    radius: f64,
+    children: Vec<MTree>,
+}
+impl Default for MTree {
+    fn default() -> Self {
+        Self {
+            id: 0,
+            center: DVec3::ZERO,
+            radius: 0.,
+            children: Vec::new(),
+        }
+    }
+}
+const MAX_CHILDREN: usize = 32;
+impl SpatialTree for MTree {
+    fn insert(&mut self, pos: DVec3, id: NodeID) {
+        if self.children.len() < MAX_CHILDREN {
+            self.children.push(MTree {
+                center: pos,
+                radius: 0.,
+                id,
+                children: Vec::new(),
+            });
+            if self.children.len() == MAX_CHILDREN {
+                for c in &mut self.children {
+                    c.children.push(MTree {
+                        center: c.center,
+                        id: c.id,
+                        radius: 0.,
+                        children: Vec::new(),
+                    });
+                    c.id = 0;
+                }
+            }
+        } else {
+            let mut min_dist = f64::MAX;
+            let mut min_index = 0;
+            for (i, c) in self.children.iter().enumerate() {
+                let d = c.center.distance_squared(pos);
+                if d < min_dist {
+                    min_index = i;
+                    min_dist = d;
+                }
+            }
+            self.radius = self.radius.max(min_dist.sqrt());
+            self.children[min_index].insert(pos, id);
+        }
+    }
+    fn depth(&self) -> usize {
+        self.children.iter().map(|c| c.depth()).max().unwrap_or(0) + 1
+    }
+}
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
+        }
+    }
+}