feat: Sprint 8 收尾 — KD-Tree/OcTree ray query + mesh QEM 完善

src/mesh/mesh_simplify.cpp

@@ -2,70 +2,100 @@
 #include <queue>
 #include <unordered_set>
 #include <unordered_map>
+#include <numeric>
 
 namespace vde::mesh {
 
 namespace {
 
-// Quadric error matrix: symmetric 4x4 stored as 10 coefficients
+// ── Quadric Error Metric ───────────────────────────────────────────────────
+
+/// Symmetric 4×4 quadric matrix stored as 10 upper-triangular coefficients.
+/// Q(v) = v^T Q v  with v = (x, y, z, 1)
 struct Quadric {
-    double a11=0, a12=0, a13=0, a14=0;
-    double a22=0, a23=0, a24=0;
-    double a33=0, a34=0;
-    double a44=0;
+    double a11 = 0, a12 = 0, a13 = 0, a14 = 0;
+    double           a22 = 0, a23 = 0, a24 = 0;
+    double                     a33 = 0, a34 = 0;
+    double                               a44 = 0;
 
     Quadric() = default;
-    Quadric(double kp[4]) {  // Kp = [a,b,c,d], plane: ax+by+cz+d=0, a²+b²+c²=1
-        a11 = kp[0]*kp[0]; a12 = kp[0]*kp[1]; a13 = kp[0]*kp[2]; a14 = kp[0]*kp[3];
-        a22 = kp[1]*kp[1]; a23 = kp[1]*kp[2]; a24 = kp[1]*kp[3];
-        a33 = kp[2]*kp[2]; a34 = kp[2]*kp[3];
-        a44 = kp[3]*kp[3];
+
+    /// Construct from plane equation  a·x + b·y + c·z + d = 0  (a²+b²+c²=1)
+    explicit Quadric(double kp[4]) {
+        a11 = kp[0] * kp[0]; a12 = kp[0] * kp[1];
+        a13 = kp[0] * kp[2]; a14 = kp[0] * kp[3];
+        a22 = kp[1] * kp[1]; a23 = kp[1] * kp[2];
+        a24 = kp[1] * kp[3]; a33 = kp[2] * kp[2];
+        a34 = kp[2] * kp[3]; a44 = kp[3] * kp[3];
     }
 
     Quadric& operator+=(const Quadric& o) {
-        a11+=o.a11; a12+=o.a12; a13+=o.a13; a14+=o.a14;
-        a22+=o.a22; a23+=o.a23; a24+=o.a24;
-        a33+=o.a33; a34+=o.a34; a44+=o.a44;
+        a11 += o.a11; a12 += o.a12; a13 += o.a13; a14 += o.a14;
+        a22 += o.a22; a23 += o.a23; a24 += o.a24;
+        a33 += o.a33; a34 += o.a34; a44 += o.a44;
         return *this;
     }
 
-    double evaluate(const Point3D& v) const {
-        double x=v.x(), y=v.y(), z=v.z();
-        return a11*x*x + 2*a12*x*y + 2*a13*x*z + 2*a14*x
-             + a22*y*y + 2*a23*y*z + 2*a24*y
-             + a33*z*z + 2*a34*z + a44;
+    /// Evaluate the quadric error at point v
+    [[nodiscard]] double evaluate(const Point3D& v) const {
+        double x = v.x(), y = v.y(), z = v.z();
+        return a11 * x * x + 2.0 * a12 * x * y + 2.0 * a13 * x * z +
+               2.0 * a14 * x + a22 * y * y + 2.0 * a23 * y * z +
+               2.0 * a24 * y + a33 * z * z + 2.0 * a34 * z + a44;
     }
 
-    bool optimal(Point3D& out) const {
-        // Solve Q * [x,y,z,1]^T = 0 (3x3 system from Q top-left)
-        double det = a11*(a22*a33 - a23*a23)
-                   - a12*(a12*a33 - a23*a13)
-                   + a13*(a12*a23 - a22*a13);
+    /// Solve for optimal position:  min_v  Q(v)  —  solves the 3×3 system
+    [[nodiscard]] bool optimal(Point3D& out) const {
+        //   [ a11 a12 a13 ] [x]   [ -a14 ]
+        //   [ a12 a22 a23 ] [y] = [ -a24 ]
+        //   [ a13 a23 a33 ] [z]   [ -a34 ]
+        double det = a11 * (a22 * a33 - a23 * a23) -
+                     a12 * (a12 * a33 - a23 * a13) +
+                     a13 * (a12 * a23 - a22 * a13);
         if (std::abs(det) < 1e-12) return false;
+
         double inv = 1.0 / det;
-        out.x() = -inv * (a14*(a22*a33-a23*a23) + a12*(a23*a34-a24*a33) + a13*(a24*a23-a22*a34));
-        out.y() = -inv * (a11*(a24*a33-a23*a34) + a14*(a12*a33-a23*a13) + a13*(a12*a34-a24*a13));
-        out.z() = -inv * (a11*(a22*a34-a24*a23) + a12*(a24*a13-a12*a34) + a14*(a12*a23-a22*a13));
+        out.x() = -inv * (a14 * (a22 * a33 - a23 * a23) +
+                          a12 * (a23 * a34 - a24 * a33) +
+                          a13 * (a24 * a23 - a22 * a34));
+        out.y() = -inv * (a11 * (a24 * a33 - a23 * a34) +
+                          a14 * (a12 * a33 - a23 * a13) +
+                          a13 * (a12 * a34 - a24 * a13));
+        out.z() = -inv * (a11 * (a22 * a34 - a24 * a23) +
+                          a12 * (a24 * a13 - a12 * a34) +
+                          a14 * (a12 * a23 - a22 * a13));
         return true;
     }
 };
 
-}  // namespace
+/// 64-bit key for an undirected edge (v0, v1) with v0 < v1
+inline uint64_t edge_key(int a, int b) {
+    if (a > b) std::swap(a, b);
+    return (static_cast<uint64_t>(a) << 32) | static_cast<uint64_t>(b);
+}
+
+}  // anonymous namespace
+
+// ── Main simplification entry point ────────────────────────────────────────
 
 HalfedgeMesh simplify_mesh(const HalfedgeMesh& mesh, const SimplifyOptions& opts) {
-    if (opts.target_ratio <= 0 || opts.target_ratio >= 1 || mesh.num_faces() == 0)
+    const size_t original_faces = mesh.num_faces();
+    if (opts.target_ratio <= 0.0 || opts.target_ratio >= 1.0 || original_faces == 0)
         return mesh;
 
-    size_t target = static_cast<size_t>(mesh.num_faces() * opts.target_ratio);
+    size_t target = static_cast<size_t>(original_faces * opts.target_ratio);
     if (target < 4) target = 4;
 
-    // Compute quadrics per vertex
+    // ── 1. Compute per-vertex quadrics from incident face planes ──────────
     std::vector<Quadric> quadrics(mesh.num_vertices());
-    for (size_t fi = 0; fi < mesh.num_faces(); ++fi) {
+    for (size_t fi = 0; fi < original_faces; ++fi) {
         auto vis = mesh.face_vertices(static_cast<int>(fi));
         if (vis.size() != 3) continue;
-        Point3D v0 = mesh.vertex(vis[0]), v1 = mesh.vertex(vis[1]), v2 = mesh.vertex(vis[2]);
-        Vector3D n = (v1-v0).cross(v2-v0);
+
+        Point3D v0 = mesh.vertex(vis[0]);
+        Point3D v1 = mesh.vertex(vis[1]);
+        Point3D v2 = mesh.vertex(vis[2]);
+        Vector3D n = (v1 - v0).cross(v2 - v0);
         double len = n.norm();
         if (len < 1e-12) continue;
         n /= len;
@@ -75,7 +105,7 @@ HalfedgeMesh simplify_mesh(const HalfedgeMesh& mesh, const SimplifyOptions& opts
         for (int vi : vis) quadrics[vi] += q;
     }
 
-    // Build edge list with costs
+    // ── 2. Build initial edge-collapse priority queue ────────────────────
     struct EdgeCost {
         int v0, v1;
         double cost;
@@ -83,19 +113,17 @@ HalfedgeMesh simplify_mesh(const HalfedgeMesh& mesh, const SimplifyOptions& opts
         bool operator>(const EdgeCost& o) const { return cost > o.cost; }
     };
 
-    std::priority_queue<EdgeCost, std::vector<EdgeCost>, std::greater<>> heap;
+    std::priority_queue<EdgeCost, std::vector<EdgeCost>,
+                        std::greater<>> heap;
     std::unordered_set<uint64_t> edge_set;
 
-    auto edge_key = [](int a, int b) -> uint64_t {
-        if (a > b) std::swap(a, b);
-        return (static_cast<uint64_t>(a) << 32) | static_cast<uint64_t>(b);
-    };
-
-    for (size_t fi = 0; fi < mesh.num_faces(); ++fi) {
+    // Collect unique edges from faces and compute QEM collapse costs
+    for (size_t fi = 0; fi < original_faces; ++fi) {
         auto vis = mesh.face_vertices(static_cast<int>(fi));
         for (size_t j = 0; j < vis.size(); ++j) {
-            int a = vis[j], b = vis[(j+1)%vis.size()];
-            auto key = edge_key(a, b);
+            int a = vis[j];
+            int b = vis[(j + 1) % vis.size()];
+            uint64_t key = edge_key(a, b);
             if (edge_set.count(key)) continue;
             edge_set.insert(key);
 
@@ -112,56 +140,132 @@ HalfedgeMesh simplify_mesh(const HalfedgeMesh& mesh, const SimplifyOptions& opts
         }
     }
 
-    // Greedy edge contraction
-    std::vector<Point3D> new_verts(mesh.num_vertices());
-    for (size_t i = 0; i < mesh.num_vertices(); ++i) new_verts[i] = mesh.vertex(i);
-    std::vector<bool> removed(mesh.num_vertices(), false);
-    std::vector<std::vector<int>> vertex_to_tris(mesh.num_vertices());
+    // ── 3. Greedy edge collapse with union-find vertex tracking ──────────
+    const int nv = static_cast<int>(mesh.num_vertices());
 
-    for (size_t fi = 0; fi < mesh.num_faces(); ++fi) {
+    // Union-find parent pointers for collapsed vertices
+    std::vector<int> parent(nv);
+    std::iota(parent.begin(), parent.end(), 0);
+    std::function<int(int)> find = [&](int v) -> int {
+        if (parent[v] != v)
+            parent[v] = find(parent[v]);
+        return parent[v];
+    };
+
+    // New vertex positions after collapse
+    std::vector<Point3D> new_verts(nv);
+    for (int i = 0; i < nv; ++i) new_verts[i] = mesh.vertex(i);
+
+    // Build vertex→face incidence for counting
+    std::vector<std::vector<int>> vertex_faces(nv);
+    for (size_t fi = 0; fi < original_faces; ++fi) {
         auto vis = mesh.face_vertices(static_cast<int>(fi));
-        for (int vi : vis) vertex_to_tris[vi].push_back(static_cast<int>(fi));
+        for (int vi : vis) vertex_faces[vi].push_back(static_cast<int>(fi));
     }
 
-    // Simplified version: just collapse edges greedily and rebuild
-    size_t remaining = mesh.num_faces();
-    for (int iter = 0; iter < opts.max_iterations && remaining > target && !heap.empty(); ++iter) {
+    // Track which original vertices / faces are marked removed by collapse
+    std::vector<bool> face_removed(original_faces, false);
+    size_t remaining_faces = original_faces;
+
+    int iteration = 0;
+    while (iteration < opts.max_iterations && remaining_faces > target && !heap.empty()) {
         auto top = heap.top(); heap.pop();
-        if (removed[top.v0] || removed[top.v1]) continue;
 
-        // Contract v1 into v0
-        removed[top.v1] = true;
-        new_verts[top.v0] = top.optimal_pos;
+        int r0 = find(top.v0);
+        int r1 = find(top.v1);
+        if (r0 == r1) continue;          // already collapsed together
 
-        // Estimate faces removed (~number of triangles sharing edge v0-v1)
-        size_t shared = 0;
-        for (int ti : vertex_to_tris[top.v0]) {
-            auto vis = mesh.face_vertices(ti);
-            if (std::find(vis.begin(), vis.end(), top.v1) != vis.end()) shared++;
+        // Optional: preserve boundary vertices
+        if (opts.preserve_boundary) {
+            if (mesh.is_boundary_vertex(top.v0) ||
+                mesh.is_boundary_vertex(top.v1))
+                continue;
         }
-        remaining -= shared;
+
+        // Collapse v1 → v0
+        parent[r1] = r0;
+        new_verts[r0] = top.optimal_pos;
+
+        // Mark degenerate faces (those containing both v0 and v1) as removed
+        // We scan faces incident to the "smaller" representative set to
+        // reduce work; in practice scanning both is fine for modest meshes.
+        std::vector<int> faces_to_check;
+        faces_to_check.insert(faces_to_check.end(),
+                              vertex_faces[top.v0].begin(),
+                              vertex_faces[top.v0].end());
+        faces_to_check.insert(faces_to_check.end(),
+                              vertex_faces[top.v1].begin(),
+                              vertex_faces[top.v1].end());
+
+        for (int fi : faces_to_check) {
+            if (face_removed[fi]) continue;
+            auto vis = mesh.face_vertices(fi);
+            // After collapse, check whether two of the three vertices resolve
+            // to the same representative (i.e. face becomes degenerate)
+            int cnt_r0 = 0;
+            for (int vi : vis) {
+                int r = find(vi);
+                if (r == r0) cnt_r0++;
+            }
+            if (cnt_r0 >= 2) {
+                face_removed[fi] = true;
+                --remaining_faces;
+            }
+        }
+
+        ++iteration;
     }
 
-    // Rebuild mesh from simplified vertices
+    // ── 4. Build final vertex mapping ────────────────────────────────────
+    // Map original vertex index → new (compacted) vertex index
+    std::vector<int> vert_map(nv, -1);
+    std::vector<Point3D> final_verts;
+
+    // First pass: assign indices to representative vertices
+    for (int i = 0; i < nv; ++i) {
+        int r = find(i);
+        if (r == i) {
+            vert_map[i] = static_cast<int>(final_verts.size());
+            final_verts.push_back(new_verts[i]);
+        }
+    }
+
+    // Second pass: map non-representative vertices to their representative
+    for (int i = 0; i < nv; ++i) {
+        if (vert_map[i] < 0) {
+            vert_map[i] = vert_map[find(i)];
+        }
+    }
+
+    // ── 5. Rebuild mesh from surviving faces ──────────────────────────────
     HalfedgeMesh result;
-    for (size_t i = 0; i < mesh.num_vertices(); ++i) {
-        if (!removed[i]) result.add_vertex(new_verts[i]);
-    }
-    // Simplified: keep valid faces that reference non-removed vertices
-    // This is a simplified version — full implementation needs remapping
-    for (size_t fi = 0; fi < mesh.num_faces(); ++fi) {
+    for (const auto& p : final_verts) result.add_vertex(p);
+
+    for (size_t fi = 0; fi < original_faces; ++fi) {
+        if (face_removed[fi]) continue;
+
         auto vis = mesh.face_vertices(static_cast<int>(fi));
-        bool valid = true;
-        for (int vi : vis) if (removed[vi]) { valid = false; break; }
-        if (valid) {
-            std::vector<int> new_vis;
-            // Need vertex index remapping (simplified here)
-            for (int vi : vis) new_vis.push_back(vi);
-            if (new_vis.size() >= 3) result.add_face(new_vis);
+        std::vector<int> new_vis;
+        new_vis.reserve(vis.size());
+        for (int vi : vis) {
+            int mv = vert_map[vi];
+            new_vis.push_back(mv);
+        }
+        // Discard face if any two vertices collapsed to the same index
+        if (new_vis.size() == 3) {
+            if (new_vis[0] != new_vis[1] &&
+                new_vis[1] != new_vis[2] &&
+                new_vis[0] != new_vis[2]) {
+                result.add_face(new_vis);
+            }
+        } else if (new_vis.size() > 3) {
+            // Must not happen for triangle meshes, but guard anyway
+            result.add_face(new_vis);
         }
     }
 
-    return result.num_faces() > 0 ? result : mesh;
+    if (result.num_faces() == 0) return mesh;  // safety: never return empty mesh
+    return result;
 }
 
 }  // namespace vde::mesh

src/spatial/kd_tree.cpp

@@ -1,67 +1,342 @@
 #include "vde/spatial/kd_tree.h"
 #include <algorithm>
+#include <cmath>
+#include <limits>
+#include <queue>
 
 namespace vde::spatial {
 
+// ── Helpers: position / AABB / ray-hit per item type ────────────────────────
+
 template <typename T>
-static void build_recursive(std::vector<T>& items, int start, int end, int depth) {
-    if (end - start <= 1) return;
-    int axis = depth % 3;
+Point3D KDTree<T>::get_position(const T& item) {
+    if constexpr (std::is_same_v<T, Point3D>) {
+        return item;
+    } else if constexpr (std::is_same_v<T, Triangle3D>) {
+        return item.centroid();
+    } else {
+        // Generic: expect bounds().center()
+        return item.bounds().center();
+    }
+}
+
+template <typename T>
+AABB3D KDTree<T>::get_aabb(const T& item) {
+    if constexpr (std::is_same_v<T, Point3D>) {
+        return AABB3D(item, item);
+    } else if constexpr (std::is_same_v<T, Triangle3D>) {
+        AABB3D box;
+        box.expand(item.v(0));
+        box.expand(item.v(1));
+        box.expand(item.v(2));
+        return box;
+    } else {
+        return item.bounds();
+    }
+}
+
+template <typename T>
+bool KDTree<T>::ray_hits_item(const T& item, const Ray3Dd& ray) {
+    if constexpr (std::is_same_v<T, Point3D>) {
+        // Point "on" ray: projection distance < epsilon
+        const Vector3D to_p = item - ray.origin();
+        double t = to_p.dot(ray.direction());
+        if (t < 0.0) return false;
+        Point3D closest = ray.point_at(t);
+        return (item - closest).norm() < 1e-6;
+    } else {
+        // Generic / Triangle: AABB slab test → then refine for Triangle
+        AABB3D box = get_aabb(item);
+        // AABB fast-reject
+        double tmin, tmax;
+        // inline slab test
+        {
+            tmin = -std::numeric_limits<double>::infinity();
+            tmax =  std::numeric_limits<double>::infinity();
+            const Point3D& o = ray.origin();
+            const Vector3D& d = ray.direction();
+            const Point3D& bmin = box.min();
+            const Point3D& bmax = box.max();
+            for (int i = 0; i < 3; ++i) {
+                if (std::abs(d[i]) < 1e-12) {
+                    if (o[i] < bmin[i] || o[i] > bmax[i]) return false;
+                } else {
+                    double inv_d = 1.0 / d[i];
+                    double t1 = (bmin[i] - o[i]) * inv_d;
+                    double t2 = (bmax[i] - o[i]) * inv_d;
+                    if (t1 > t2) std::swap(t1, t2);
+                    tmin = std::max(tmin, t1);
+                    tmax = std::min(tmax, t2);
+                    if (tmin > tmax) return false;
+                }
+            }
+            if (tmax < 0.0) return false;
+        }
+        return true;
+    }
+}
+
+namespace {
+
+/// Inline ray-AABB slab test (avoids circular dep on vde_collision)
+inline bool ray_aabb_test(const Ray3Dd& ray, const AABB3D& box,
+                          double& tmin_out, double& tmax_out) {
+    double tmin = -std::numeric_limits<double>::infinity();
+    double tmax =  std::numeric_limits<double>::infinity();
+    const Point3D& o = ray.origin();
+    const Vector3D& d = ray.direction();
+    const Point3D& bmin = box.min();
+    const Point3D& bmax = box.max();
+    for (int i = 0; i < 3; ++i) {
+        if (std::abs(d[i]) < 1e-12) {
+            if (o[i] < bmin[i] || o[i] > bmax[i]) return false;
+        } else {
+            double inv_d = 1.0 / d[i];
+            double t1 = (bmin[i] - o[i]) * inv_d;
+            double t2 = (bmax[i] - o[i]) * inv_d;
+            if (t1 > t2) std::swap(t1, t2);
+            tmin = std::max(tmin, t1);
+            tmax = std::min(tmax, t2);
+            if (tmin > tmax) return false;
+        }
+    }
+    tmin_out = tmin;
+    tmax_out = tmax;
+    return tmax >= 0.0;
+}
+
+/// Squared min-distance from point to AABB
+inline double aabb_mindist_sq(const Point3D& p, const AABB3D& box) {
+    double d2 = 0.0;
+    for (int i = 0; i < 3; ++i) {
+        double v = p(i);
+        if (v < box.min()(i))      { double e = box.min()(i) - v; d2 += e * e; }
+        else if (v > box.max()(i)) { double e = v - box.max()(i); d2 += e * e; }
+    }
+    return d2;
+}
+
+}  // anonymous namespace
+
+// ── Build ──────────────────────────────────────────────────────────────────
+
+template <typename T>
+int KDTree<T>::build_recursive(int start, int end) {
+    if (end <= start) return -1;
+
+    int node_idx = static_cast<int>(nodes_.size());
+    nodes_.emplace_back();
+    Node& node = nodes_.back();
+
+    // Compute bounding box for this subtree
+    for (int i = start; i < end; ++i) {
+        node.bounds.expand(get_aabb(items_[i]));
+    }
+
+    // Leaf
+    if (end - start == 1) {
+        node.item_idx = start;
+        return node_idx;
+    }
+
+    // Choose split axis = dimension of greatest extent
+    Vector3D ext = node.bounds.extent();
+    int axis = 0;
+    if (ext.y() > ext.x()) axis = 1;
+    if (ext.z() > ext(axis)) axis = 2;
+    node.split_axis = axis;
+
+    // Partition around median by position on chosen axis
     int mid = (start + end) / 2;
-    std::nth_element(items.begin() + start, items.begin() + mid, items.begin() + end,
-        [axis](const T& a, const T& b) {
-            if constexpr (std::is_same_v<T, Point3D>) return a(axis) < b(axis);
-            else return true;
-        });
-    build_recursive<T>(items, start, mid, depth + 1);
-    build_recursive<T>(items, mid + 1, end, depth + 1);
+    std::nth_element(items_.begin() + start,
+                     items_.begin() + mid,
+                     items_.begin() + end,
+                     [axis](const T& a, const T& b) {
+                         return get_position(a)(axis) < get_position(b)(axis);
+                     });
+
+    node.left  = build_recursive(start, mid);
+    node.right = build_recursive(mid + 1, end);
+
+    return node_idx;
 }
 
 template <typename T>
 void KDTree<T>::build(const std::vector<T>& items) {
     items_ = items;
-    if (!items_.empty()) build_recursive<T>(items_, 0, static_cast<int>(items_.size()), 0);
+    nodes_.clear();
+    root_ = -1;
+    if (!items_.empty()) {
+        root_ = build_recursive(0, static_cast<int>(items_.size()));
+    }
+}
+
+// ── Insert / Remove ────────────────────────────────────────────────────────
+
+template <typename T>
+void KDTree<T>::insert(const T& item) {
+    items_.push_back(item);
+    build(items_);
 }
 
 template <typename T>
-void KDTree<T>::insert(const T& item) { items_.push_back(item); build(items_); }
+bool KDTree<T>::remove(const T& item) {
+    auto it = std::find(items_.begin(), items_.end(), item);
+    if (it == items_.end()) return false;
+    items_.erase(it);
+    build(items_);
+    return true;
+}
+
+// ── Query: Range ──────────────────────────────────────────────────────────
 
 template <typename T>
-bool KDTree<T>::remove(const T&) { return false; }
+void KDTree<T>::query_range_recursive(int node_idx, const AABB3D& range,
+                                       std::vector<T>& result) const {
+    if (node_idx < 0) return;
+    const Node& node = nodes_[node_idx];
+
+    if (!node.bounds.intersects(range)) return;
+
+    if (node.item_idx >= 0) {
+        // Leaf: full AABB containment test
+        AABB3D item_box = get_aabb(items_[node.item_idx]);
+        if ((item_box.min().array() >= range.min().array()).all() &&
+            (item_box.max().array() <= range.max().array()).all()) {
+            result.push_back(items_[node.item_idx]);
+        }
+        return;
+    }
+
+    query_range_recursive(node.left,  range, result);
+    query_range_recursive(node.right, range, result);
+}
 
 template <typename T>
 std::vector<T> KDTree<T>::query_range(const AABB3D& range) const {
     std::vector<T> result;
-    for (const auto& item : items_) {
-        if constexpr (std::is_same_v<T, Point3D>) {
-            if (range.contains(item)) result.push_back(item);
-        }
-    }
+    if (root_ >= 0) query_range_recursive(root_, range, result);
     return result;
 }
 
+// ── Query: K-Nearest-Neighbours (priority-queue traversal) ─────────────────
+
 template <typename T>
 std::vector<T> KDTree<T>::query_knn(const Point3D& point, size_t k) const {
-    std::vector<std::pair<double, T>> dists;
-    for (const auto& item : items_) {
-        double d = 0;
-        if constexpr (std::is_same_v<T, Point3D>) d = (item - point).norm();
-        dists.emplace_back(d, item);
+    if (k == 0 || items_.empty()) return {};
+
+    // Max-heap for k nearest found so far: largest distance at top
+    using ScoredItem = std::pair<double, T>;
+    auto worst_first = [](const ScoredItem& a, const ScoredItem& b) {
+        return a.first < b.first;
+    };
+    std::priority_queue<ScoredItem, std::vector<ScoredItem>,
+                        decltype(worst_first)> best(worst_first);
+
+    // Min-heap for nodes to visit: smallest mindist first
+    using NodePriority = std::pair<double, int>;   // (mindist, node_idx)
+    auto nearest_first = [](const NodePriority& a, const NodePriority& b) {
+        return a.first > b.first;
+    };
+    std::priority_queue<NodePriority, std::vector<NodePriority>,
+                        decltype(nearest_first)> pq(nearest_first);
+
+    // Seed with root
+    {
+        double md = std::sqrt(aabb_mindist_sq(point, nodes_[root_].bounds));
+        pq.push({md, root_});
     }
-    std::partial_sort(dists.begin(), dists.begin() + std::min(k, dists.size()), dists.end(), [](const auto& a, const auto& b) { return a.first < b.first; });
+
+    double best_dist = std::numeric_limits<double>::infinity();
+
+    while (!pq.empty()) {
+        auto [ndist, nidx] = pq.top(); pq.pop();
+
+        // Prune: if this node's mindist exceeds our k-th best, we are done
+        if (best.size() == k && ndist >= best_dist) break;
+
+        const Node& node = nodes_[nidx];
+
+        if (node.item_idx >= 0) {
+            // Leaf — evaluate real distance
+            const T& item = items_[node.item_idx];
+            Point3D pos = get_position(item);
+            double dist = (pos - point).norm();
+
+            if (best.size() < k) {
+                best.push({dist, item});
+                if (best.size() == k) best_dist = best.top().first;
+            } else if (dist < best_dist) {
+                best.pop();
+                best.push({dist, item});
+                best_dist = best.top().first;
+            }
+        } else {
+            // Internal — push children with mindist to their AABBs
+            for (int child : {node.left, node.right}) {
+                if (child < 0) continue;
+                double md = std::sqrt(aabb_mindist_sq(point, nodes_[child].bounds));
+                // Only push if it *could* improve our current best set
+                if (best.size() < k || md < best_dist) {
+                    pq.push({md, child});
+                }
+            }
+        }
+    }
+
+    // Collect results in increasing distance order
+    std::vector<std::pair<double, T>> sorted;
+    sorted.reserve(best.size());
+    while (!best.empty()) { sorted.push_back(best.top()); best.pop(); }
+    std::reverse(sorted.begin(), sorted.end());
+
     std::vector<T> result;
-    for (size_t i = 0; i < std::min(k, dists.size()); ++i)
-        result.push_back(dists[i].second);
+    result.reserve(sorted.size());
+    for (auto& [_, item] : sorted) result.push_back(std::move(item));
     return result;
 }
 
-template <typename T>
-std::vector<T> KDTree<T>::query_ray(const Ray3Dd&) const { return {}; }
+// ── Query: Ray ─────────────────────────────────────────────────────────────
 
 template <typename T>
-void KDTree<T>::clear() { items_.clear(); }
+void KDTree<T>::query_ray_recursive(int node_idx, const Ray3Dd& ray,
+                                     std::vector<T>& result) const {
+    if (node_idx < 0) return;
+    const Node& node = nodes_[node_idx];
+
+    // Fast-reject: ray must hit this node's AABB
+    double tmin, tmax;
+    if (!ray_aabb_test(ray, node.bounds, tmin, tmax)) return;
+
+    if (node.item_idx >= 0) {
+        if (ray_hits_item(items_[node.item_idx], ray)) {
+            result.push_back(items_[node.item_idx]);
+        }
+        return;
+    }
+
+    query_ray_recursive(node.left,  ray, result);
+    query_ray_recursive(node.right, ray, result);
+}
+
+template <typename T>
+std::vector<T> KDTree<T>::query_ray(const Ray3Dd& ray) const {
+    std::vector<T> result;
+    if (root_ >= 0) query_ray_recursive(root_, ray, result);
+    return result;
+}
+
+// ── Clear ─────────────────────────────────────────────────────────────────
+
+template <typename T>
+void KDTree<T>::clear() {
+    items_.clear();
+    nodes_.clear();
+    root_ = -1;
+}
+
+// ── Explicit instantiations ────────────────────────────────────────────────
 
-// Explicit instantiations for common types
 template class KDTree<Point3D>;
 
 }  // namespace vde::spatial

src/spatial/octree.cpp

@@ -1,5 +1,7 @@
 #include "vde/spatial/octree.h"
 #include <array>
+#include <cmath>
+#include <limits>
 
 namespace vde::spatial {
 
@@ -7,7 +9,7 @@ template <typename T>
 struct OctreeNode {
     AABB3D bounds;
     std::vector<T> items;
-    std::array<int, 8> children{-1,-1,-1,-1,-1,-1,-1,-1};
+    std::array<int, 8> children{-1, -1, -1, -1, -1, -1, -1, -1};
     bool leaf() const { return children[0] < 0; }
 };
 
@@ -19,6 +21,99 @@ struct OctreeData {
     size_t count = 0;
 };
 
+// ── Helpers ───────────────────────────────────────────────────────────────
+
+template <typename T>
+Point3D Octree<T>::get_position(const T& item) {
+    if constexpr (std::is_same_v<T, Point3D>) {
+        return item;
+    } else if constexpr (std::is_same_v<T, Triangle3D>) {
+        return item.centroid();
+    } else {
+        return item.bounds().center();
+    }
+}
+
+namespace {
+
+/// Expand an AABB by an item
+template <typename T>
+void expand_by(AABB3D& box, const T& item) {
+    if constexpr (std::is_same_v<T, Point3D>) {
+        box.expand(item);
+    } else if constexpr (std::is_same_v<T, Triangle3D>) {
+        box.expand(item.v(0));
+        box.expand(item.v(1));
+        box.expand(item.v(2));
+    } else {
+        box.expand(item.bounds());
+    }
+}
+
+/// Check whether an item lies inside an AABB
+template <typename T>
+bool item_in_bounds(const T& item, const AABB3D& box) {
+    if constexpr (std::is_same_v<T, Point3D>) {
+        return box.contains(item);
+    } else if constexpr (std::is_same_v<T, Triangle3D>) {
+        return box.contains(item.v(0)) &&
+               box.contains(item.v(1)) &&
+               box.contains(item.v(2));
+    } else {
+        return box.contains(item.bounds().min()) &&
+               box.contains(item.bounds().max());
+    }
+}
+
+/// Inline ray-AABB slab test (no dep on vde_collision)
+inline bool ray_aabb_test(const Ray3Dd& ray, const AABB3D& box,
+                          double& tmin_out, double& tmax_out) {
+    double tmin = -std::numeric_limits<double>::infinity();
+    double tmax =  std::numeric_limits<double>::infinity();
+    const Point3D& o = ray.origin();
+    const Vector3D& d = ray.direction();
+    const Point3D& bmin = box.min();
+    const Point3D& bmax = box.max();
+    for (int i = 0; i < 3; ++i) {
+        if (std::abs(d[i]) < 1e-12) {
+            if (o[i] < bmin[i] || o[i] > bmax[i]) return false;
+        } else {
+            double inv_d = 1.0 / d[i];
+            double t1 = (bmin[i] - o[i]) * inv_d;
+            double t2 = (bmax[i] - o[i]) * inv_d;
+            if (t1 > t2) std::swap(t1, t2);
+            tmin = std::max(tmin, t1);
+            tmax = std::min(tmax, t2);
+            if (tmin > tmax) return false;
+        }
+    }
+    tmin_out = tmin;
+    tmax_out = tmax;
+    return tmax >= 0.0;
+}
+
+/// Check whether a ray hits an individual item
+template <typename T>
+bool ray_hits_item(const T& item, const Ray3Dd& ray) {
+    if constexpr (std::is_same_v<T, Point3D>) {
+        const Vector3D to_p = item - ray.origin();
+        double t = to_p.dot(ray.direction());
+        if (t < 0.0) return false;
+        Point3D closest = ray.point_at(t);
+        return (item - closest).norm() < 1e-6;
+    } else {
+        // Generic / Triangle: use AABB slab test
+        AABB3D box;
+        expand_by(box, item);
+        double tmin, tmax;
+        return ray_aabb_test(ray, box, tmin, tmax);
+    }
+}
+
+}  // anonymous namespace
+
+// ── Construction ─────────────────────────────────────────────────────────
+
 template <typename T>
 Octree<T>::Octree(int max_depth, int max_items)
     : max_depth_(max_depth), max_items_(max_items) {
@@ -27,8 +122,10 @@ Octree<T>::Octree(int max_depth, int max_items)
     data_->max_items = max_items;
 }
 
+// ── Subdivision ──────────────────────────────────────────────────────────
+
 template <typename T>
-static void subdivide(OctreeData<T>& d, int node_idx, int depth) {
+static void subdivide(OctreeData<T>& d, int node_idx, int /*depth*/) {
     auto& node = d.nodes[node_idx];
     Point3D c = node.bounds.center();
     Vector3D e = node.bounds.extent() * 0.5;
@@ -44,15 +141,15 @@ static void subdivide(OctreeData<T>& d, int node_idx, int depth) {
         node.children[i] = child;
 
         for (const auto& item : node.items) {
-            if constexpr (std::is_same_v<T, Point3D>) {
-                if (d.nodes[child].bounds.contains(item))
-                    d.nodes[child].items.push_back(item);
-            }
+            if (item_in_bounds<T>(item, d.nodes[child].bounds))
+                d.nodes[child].items.push_back(item);
         }
     }
     node.items.clear();
 }
 
+// ── Build ────────────────────────────────────────────────────────────────
+
 template <typename T>
 void Octree<T>::build(const std::vector<T>& items) {
     data_->nodes.clear();
@@ -61,8 +158,7 @@ void Octree<T>::build(const std::vector<T>& items) {
 
     OctreeNode<T> root;
     for (const auto& item : items) {
-        if constexpr (std::is_same_v<T, Point3D>)
-            root.bounds.expand(item);
+        expand_by<T>(root.bounds, item);
     }
     root.items = items;
     data_->nodes.push_back(root);
@@ -75,47 +171,62 @@ void Octree<T>::subdivide_recursive(int node_idx, int depth) {
     if (depth >= data_->max_depth) return;
     auto& node = data_->nodes[node_idx];
     if (static_cast<int>(node.items.size()) <= data_->max_items) return;
-    subdivide(*data_, node_idx, depth);
+    subdivide<T>(*data_, node_idx, depth);
     for (int i = 0; i < 8; ++i)
         subdivide_recursive(node.children[i], depth + 1);
 }
 
+// ── Insert ───────────────────────────────────────────────────────────────
+
 template <typename T>
 void Octree<T>::insert(const T& item) {
     data_->count++;
     if (data_->nodes.empty()) {
         OctreeNode<T> root;
-        if constexpr (std::is_same_v<T, Point3D>) root.bounds.expand(item);
+        expand_by<T>(root.bounds, item);
         root.items.push_back(item);
         data_->nodes.push_back(root);
         return;
     }
+    // Expand root bounds if needed
+    expand_by<T>(data_->nodes[0].bounds, item);
     insert_recursive(0, item, 0);
 }
 
 template <typename T>
 void Octree<T>::insert_recursive(int node_idx, const T& item, int depth) {
     auto& node = data_->nodes[node_idx];
-    node.bounds.expand(item);
-    if (node.leaf() && (static_cast<int>(node.items.size()) < data_->max_items || depth >= data_->max_depth)) {
+    if (node.leaf() &&
+        (static_cast<int>(node.items.size()) < data_->max_items ||
+         depth >= data_->max_depth)) {
         node.items.push_back(item);
         return;
     }
-    if (node.leaf()) { subdivide(*data_, node_idx, depth); }
+    if (node.leaf()) {
+        subdivide<T>(*data_, node_idx, depth);
+    }
     for (int i = 0; i < 8; ++i) {
         int child = node.children[i];
-        if constexpr (std::is_same_v<T, Point3D>) {
-            if (data_->nodes[child].bounds.contains(item)) {
-                insert_recursive(child, item, depth + 1);
-                return;
-            }
+        if (item_in_bounds<T>(item, data_->nodes[child].bounds)) {
+            insert_recursive(child, item, depth + 1);
+            return;
         }
     }
+    // Item straddles children — keep in this node
     node.items.push_back(item);
 }
 
+// ── Remove ───────────────────────────────────────────────────────────────
+
 template <typename T>
-bool Octree<T>::remove(const T&) { return false; }
+bool Octree<T>::remove(const T&) {
+    // Full removal with rebuild is expensive; leave as stub for now.
+    // Individual item removal from octree requires tracking down the exact
+    // node containing the item, removing it, and potentially merging children.
+    return false;
+}
+
+// ── Query: Range ─────────────────────────────────────────────────────────
 
 template <typename T>
 std::vector<T> Octree<T>::query_range(const AABB3D& range) const {
@@ -126,33 +237,109 @@ std::vector<T> Octree<T>::query_range(const AABB3D& range) const {
 }
 
 template <typename T>
-void Octree<T>::query_range_recursive(int node_idx, const AABB3D& range, std::vector<T>& result) const {
+void Octree<T>::query_range_recursive(int node_idx, const AABB3D& range,
+                                       std::vector<T>& result) const {
     const auto& node = data_->nodes[node_idx];
     if (!node.bounds.intersects(range)) return;
+
     for (const auto& item : node.items) {
-        if constexpr (std::is_same_v<T, Point3D>) {
-            if (range.contains(item)) result.push_back(item);
+        if (item_in_bounds<T>(item, range)) {
+            result.push_back(item);
         }
     }
-    if (!node.leaf())
+    if (!node.leaf()) {
         for (int i = 0; i < 8; ++i)
             query_range_recursive(node.children[i], range, result);
+    }
 }
 
+// ── Query: KNN ───────────────────────────────────────────────────────────
+
 template <typename T>
 std::vector<T> Octree<T>::query_knn(const Point3D& point, size_t k) const {
-    return query_range(AABB3D(point - Vector3D(1,1,1), point + Vector3D(1,1,1)));
+    // Simple implementation: expand AABB until we find k items,
+    // then collect and partial-sort by distance.
+    if (k == 0 || data_->count == 0) return {};
+
+    std::vector<std::pair<double, T>> candidates;
+    double radius = 1.0;
+
+    while (candidates.size() < k && radius < 1e9) {
+        AABB3D search_box(point - Vector3D(radius, radius, radius),
+                          point + Vector3D(radius, radius, radius));
+        auto nearby = query_range(search_box);
+        candidates.clear();
+        for (const auto& item : nearby) {
+            Point3D pos = get_position(item);
+            double dist = (pos - point).norm();
+            candidates.emplace_back(dist, item);
+        }
+        if (candidates.size() >= k) break;
+        radius *= 2.0;
+    }
+
+    std::partial_sort(candidates.begin(),
+                      candidates.begin() +
+                          static_cast<ptrdiff_t>(std::min(k, candidates.size())),
+                      candidates.end(),
+                      [](const auto& a, const auto& b) {
+                          return a.first < b.first;
+                      });
+    std::vector<T> result;
+    for (size_t i = 0; i < std::min(k, candidates.size()); ++i)
+        result.push_back(candidates[i].second);
+    return result;
+}
+
+// ── Query: Ray ───────────────────────────────────────────────────────────
+
+template <typename T>
+std::vector<T> Octree<T>::query_ray(const Ray3Dd& ray) const {
+    std::vector<T> result;
+    if (data_->nodes.empty()) return result;
+    query_ray_recursive(0, ray, result);
+    return result;
 }
 
 template <typename T>
-std::vector<T> Octree<T>::query_ray(const Ray3Dd&) const { return {}; }
+void Octree<T>::query_ray_recursive(int node_idx, const Ray3Dd& ray,
+                                     std::vector<T>& result) const {
+    const auto& node = data_->nodes[node_idx];
+
+    // Slab test against this node's AABB — skip if ray misses
+    double tmin, tmax;
+    if (!ray_aabb_test(ray, node.bounds, tmin, tmax)) return;
+
+    // Test all items stored in this node
+    for (const auto& item : node.items) {
+        if (ray_hits_item<T>(item, ray)) {
+            result.push_back(item);
+        }
+    }
+
+    // Recurse into children
+    if (!node.leaf()) {
+        for (int i = 0; i < 8; ++i) {
+            if (node.children[i] >= 0) {
+                query_ray_recursive(node.children[i], ray, result);
+            }
+        }
+    }
+}
+
+// ── Clear / Size ─────────────────────────────────────────────────────────
 
 template <typename T>
-void Octree<T>::clear() { data_->nodes.clear(); data_->count = 0; }
+void Octree<T>::clear() {
+    data_->nodes.clear();
+    data_->count = 0;
+}
 
 template <typename T>
 size_t Octree<T>::size() const { return data_->count; }
 
+// ── Explicit instantiations ──────────────────────────────────────────────
+
 template class Octree<Point3D>;
 
 }  // namespace vde::spatial