diff --git a/include/vde/spatial/kd_tree.h b/include/vde/spatial/kd_tree.h index 087c671..ddcf526 100644 --- a/include/vde/spatial/kd_tree.h +++ b/include/vde/spatial/kd_tree.h @@ -1,10 +1,13 @@ #pragma once #include "vde/spatial/spatial_index.h" +#include namespace vde::spatial { using core::AABB3D; using core::Point3D; +using core::Vector3D; using core::Ray3Dd; +using core::Triangle3D; template class KDTree : public SpatialIndex { @@ -17,8 +20,34 @@ public: std::vector query_ray(const Ray3Dd& ray) const override; void clear() override; size_t size() const override { return items_.size(); } + private: + struct Node { + AABB3D bounds; + int item_idx = -1; // -1 for internal node; index into items_ for leaf + int split_axis = 0; + int left = -1; + int right = -1; + }; + std::vector items_; + std::vector nodes_; + int root_ = -1; + + /// Return a representative 3D position for the item (used for spatial ordering) + static Point3D get_position(const T& item); + + /// Return an axis-aligned bounding box for the item + static AABB3D get_aabb(const T& item); + + /// Test whether the item intersects a ray + static bool ray_hits_item(const T& item, const Ray3Dd& ray); + + int build_recursive(int start, int end); + void query_range_recursive(int node_idx, const AABB3D& range, + std::vector& result) const; + void query_ray_recursive(int node_idx, const Ray3Dd& ray, + std::vector& result) const; }; } // namespace vde::spatial diff --git a/include/vde/spatial/octree.h b/include/vde/spatial/octree.h index 9fcbf68..6ae87ab 100644 --- a/include/vde/spatial/octree.h +++ b/include/vde/spatial/octree.h @@ -24,10 +24,18 @@ public: std::vector query_ray(const Ray3Dd& ray) const override; void clear() override; size_t size() const override; + private: void insert_recursive(int node_idx, const T& item, int depth); void subdivide_recursive(int node_idx, int depth); - void query_range_recursive(int node_idx, const AABB3D& range, std::vector& result) const; + void query_range_recursive(int node_idx, const AABB3D& range, + std::vector& result) const; + void query_ray_recursive(int node_idx, const Ray3Dd& ray, + std::vector& result) const; + + /// Item position helper (for non-Point3D types) + static Point3D get_position(const T& item); + std::shared_ptr> data_; int max_depth_, max_items_; }; diff --git a/src/mesh/mesh_simplify.cpp b/src/mesh/mesh_simplify.cpp index 7d88379..fe053a1 100644 --- a/src/mesh/mesh_simplify.cpp +++ b/src/mesh/mesh_simplify.cpp @@ -2,70 +2,100 @@ #include #include #include +#include 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(a) << 32) | static_cast(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(mesh.num_faces() * opts.target_ratio); + size_t target = static_cast(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 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(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, std::greater<>> heap; + std::priority_queue, + std::greater<>> heap; std::unordered_set edge_set; - auto edge_key = [](int a, int b) -> uint64_t { - if (a > b) std::swap(a, b); - return (static_cast(a) << 32) | static_cast(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(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 new_verts(mesh.num_vertices()); - for (size_t i = 0; i < mesh.num_vertices(); ++i) new_verts[i] = mesh.vertex(i); - std::vector removed(mesh.num_vertices(), false); - std::vector> vertex_to_tris(mesh.num_vertices()); + // ── 3. Greedy edge collapse with union-find vertex tracking ────────── + const int nv = static_cast(mesh.num_vertices()); - for (size_t fi = 0; fi < mesh.num_faces(); ++fi) { + // Union-find parent pointers for collapsed vertices + std::vector parent(nv); + std::iota(parent.begin(), parent.end(), 0); + std::function find = [&](int v) -> int { + if (parent[v] != v) + parent[v] = find(parent[v]); + return parent[v]; + }; + + // New vertex positions after collapse + std::vector new_verts(nv); + for (int i = 0; i < nv; ++i) new_verts[i] = mesh.vertex(i); + + // Build vertex→face incidence for counting + std::vector> vertex_faces(nv); + for (size_t fi = 0; fi < original_faces; ++fi) { auto vis = mesh.face_vertices(static_cast(fi)); - for (int vi : vis) vertex_to_tris[vi].push_back(static_cast(fi)); + for (int vi : vis) vertex_faces[vi].push_back(static_cast(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 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 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 vert_map(nv, -1); + std::vector 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(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(fi)); - bool valid = true; - for (int vi : vis) if (removed[vi]) { valid = false; break; } - if (valid) { - std::vector 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 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 diff --git a/src/spatial/kd_tree.cpp b/src/spatial/kd_tree.cpp index 53b6605..d1f81db 100644 --- a/src/spatial/kd_tree.cpp +++ b/src/spatial/kd_tree.cpp @@ -1,67 +1,342 @@ #include "vde/spatial/kd_tree.h" #include +#include +#include +#include namespace vde::spatial { +// ── Helpers: position / AABB / ray-hit per item type ──────────────────────── + template -static void build_recursive(std::vector& items, int start, int end, int depth) { - if (end - start <= 1) return; - int axis = depth % 3; +Point3D KDTree::get_position(const T& item) { + if constexpr (std::is_same_v) { + return item; + } else if constexpr (std::is_same_v) { + return item.centroid(); + } else { + // Generic: expect bounds().center() + return item.bounds().center(); + } +} + +template +AABB3D KDTree::get_aabb(const T& item) { + if constexpr (std::is_same_v) { + return AABB3D(item, item); + } else if constexpr (std::is_same_v) { + AABB3D box; + box.expand(item.v(0)); + box.expand(item.v(1)); + box.expand(item.v(2)); + return box; + } else { + return item.bounds(); + } +} + +template +bool KDTree::ray_hits_item(const T& item, const Ray3Dd& ray) { + if constexpr (std::is_same_v) { + // 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::infinity(); + tmax = std::numeric_limits::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::infinity(); + double tmax = std::numeric_limits::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 +int KDTree::build_recursive(int start, int end) { + if (end <= start) return -1; + + int node_idx = static_cast(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) return a(axis) < b(axis); - else return true; - }); - build_recursive(items, start, mid, depth + 1); - build_recursive(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 void KDTree::build(const std::vector& items) { items_ = items; - if (!items_.empty()) build_recursive(items_, 0, static_cast(items_.size()), 0); + nodes_.clear(); + root_ = -1; + if (!items_.empty()) { + root_ = build_recursive(0, static_cast(items_.size())); + } +} + +// ── Insert / Remove ──────────────────────────────────────────────────────── + +template +void KDTree::insert(const T& item) { + items_.push_back(item); + build(items_); } template -void KDTree::insert(const T& item) { items_.push_back(item); build(items_); } +bool KDTree::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 -bool KDTree::remove(const T&) { return false; } +void KDTree::query_range_recursive(int node_idx, const AABB3D& range, + std::vector& 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 std::vector KDTree::query_range(const AABB3D& range) const { std::vector result; - for (const auto& item : items_) { - if constexpr (std::is_same_v) { - 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 std::vector KDTree::query_knn(const Point3D& point, size_t k) const { - std::vector> dists; - for (const auto& item : items_) { - double d = 0; - if constexpr (std::is_same_v) 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; + auto worst_first = [](const ScoredItem& a, const ScoredItem& b) { + return a.first < b.first; + }; + std::priority_queue, + decltype(worst_first)> best(worst_first); + + // Min-heap for nodes to visit: smallest mindist first + using NodePriority = std::pair; // (mindist, node_idx) + auto nearest_first = [](const NodePriority& a, const NodePriority& b) { + return a.first > b.first; + }; + std::priority_queue, + 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::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> sorted; + sorted.reserve(best.size()); + while (!best.empty()) { sorted.push_back(best.top()); best.pop(); } + std::reverse(sorted.begin(), sorted.end()); + std::vector 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 -std::vector KDTree::query_ray(const Ray3Dd&) const { return {}; } +// ── Query: Ray ───────────────────────────────────────────────────────────── template -void KDTree::clear() { items_.clear(); } +void KDTree::query_ray_recursive(int node_idx, const Ray3Dd& ray, + std::vector& 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 +std::vector KDTree::query_ray(const Ray3Dd& ray) const { + std::vector result; + if (root_ >= 0) query_ray_recursive(root_, ray, result); + return result; +} + +// ── Clear ───────────────────────────────────────────────────────────────── + +template +void KDTree::clear() { + items_.clear(); + nodes_.clear(); + root_ = -1; +} + +// ── Explicit instantiations ──────────────────────────────────────────────── -// Explicit instantiations for common types template class KDTree; } // namespace vde::spatial diff --git a/src/spatial/octree.cpp b/src/spatial/octree.cpp index 4be6f49..7a7a4ca 100644 --- a/src/spatial/octree.cpp +++ b/src/spatial/octree.cpp @@ -1,5 +1,7 @@ #include "vde/spatial/octree.h" #include +#include +#include namespace vde::spatial { @@ -7,7 +9,7 @@ template struct OctreeNode { AABB3D bounds; std::vector items; - std::array children{-1,-1,-1,-1,-1,-1,-1,-1}; + std::array 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 +Point3D Octree::get_position(const T& item) { + if constexpr (std::is_same_v) { + return item; + } else if constexpr (std::is_same_v) { + return item.centroid(); + } else { + return item.bounds().center(); + } +} + +namespace { + +/// Expand an AABB by an item +template +void expand_by(AABB3D& box, const T& item) { + if constexpr (std::is_same_v) { + box.expand(item); + } else if constexpr (std::is_same_v) { + 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 +bool item_in_bounds(const T& item, const AABB3D& box) { + if constexpr (std::is_same_v) { + return box.contains(item); + } else if constexpr (std::is_same_v) { + 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::infinity(); + double tmax = std::numeric_limits::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 +bool ray_hits_item(const T& item, const Ray3Dd& ray) { + if constexpr (std::is_same_v) { + 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 Octree::Octree(int max_depth, int max_items) : max_depth_(max_depth), max_items_(max_items) { @@ -27,8 +122,10 @@ Octree::Octree(int max_depth, int max_items) data_->max_items = max_items; } +// ── Subdivision ────────────────────────────────────────────────────────── + template -static void subdivide(OctreeData& d, int node_idx, int depth) { +static void subdivide(OctreeData& 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& d, int node_idx, int depth) { node.children[i] = child; for (const auto& item : node.items) { - if constexpr (std::is_same_v) { - if (d.nodes[child].bounds.contains(item)) - d.nodes[child].items.push_back(item); - } + if (item_in_bounds(item, d.nodes[child].bounds)) + d.nodes[child].items.push_back(item); } } node.items.clear(); } +// ── Build ──────────────────────────────────────────────────────────────── + template void Octree::build(const std::vector& items) { data_->nodes.clear(); @@ -61,8 +158,7 @@ void Octree::build(const std::vector& items) { OctreeNode root; for (const auto& item : items) { - if constexpr (std::is_same_v) - root.bounds.expand(item); + expand_by(root.bounds, item); } root.items = items; data_->nodes.push_back(root); @@ -75,47 +171,62 @@ void Octree::subdivide_recursive(int node_idx, int depth) { if (depth >= data_->max_depth) return; auto& node = data_->nodes[node_idx]; if (static_cast(node.items.size()) <= data_->max_items) return; - subdivide(*data_, node_idx, depth); + subdivide(*data_, node_idx, depth); for (int i = 0; i < 8; ++i) subdivide_recursive(node.children[i], depth + 1); } +// ── Insert ─────────────────────────────────────────────────────────────── + template void Octree::insert(const T& item) { data_->count++; if (data_->nodes.empty()) { OctreeNode root; - if constexpr (std::is_same_v) root.bounds.expand(item); + expand_by(root.bounds, item); root.items.push_back(item); data_->nodes.push_back(root); return; } + // Expand root bounds if needed + expand_by(data_->nodes[0].bounds, item); insert_recursive(0, item, 0); } template void Octree::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(node.items.size()) < data_->max_items || depth >= data_->max_depth)) { + if (node.leaf() && + (static_cast(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(*data_, node_idx, depth); + } for (int i = 0; i < 8; ++i) { int child = node.children[i]; - if constexpr (std::is_same_v) { - if (data_->nodes[child].bounds.contains(item)) { - insert_recursive(child, item, depth + 1); - return; - } + if (item_in_bounds(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 -bool Octree::remove(const T&) { return false; } +bool Octree::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 std::vector Octree::query_range(const AABB3D& range) const { @@ -126,33 +237,109 @@ std::vector Octree::query_range(const AABB3D& range) const { } template -void Octree::query_range_recursive(int node_idx, const AABB3D& range, std::vector& result) const { +void Octree::query_range_recursive(int node_idx, const AABB3D& range, + std::vector& 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) { - if (range.contains(item)) result.push_back(item); + if (item_in_bounds(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 std::vector Octree::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> 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(std::min(k, candidates.size())), + candidates.end(), + [](const auto& a, const auto& b) { + return a.first < b.first; + }); + std::vector result; + for (size_t i = 0; i < std::min(k, candidates.size()); ++i) + result.push_back(candidates[i].second); + return result; +} + +// ── Query: Ray ─────────────────────────────────────────────────────────── + +template +std::vector Octree::query_ray(const Ray3Dd& ray) const { + std::vector result; + if (data_->nodes.empty()) return result; + query_ray_recursive(0, ray, result); + return result; } template -std::vector Octree::query_ray(const Ray3Dd&) const { return {}; } +void Octree::query_ray_recursive(int node_idx, const Ray3Dd& ray, + std::vector& 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(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 -void Octree::clear() { data_->nodes.clear(); data_->count = 0; } +void Octree::clear() { + data_->nodes.clear(); + data_->count = 0; +} template size_t Octree::size() const { return data_->count; } +// ── Explicit instantiations ────────────────────────────────────────────── + template class Octree; } // namespace vde::spatial