a

1 files changed, 59 insertions, 0 deletions

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
+    }
+}