diff --git a/navmap_core/CMakeLists.txt b/navmap_core/CMakeLists.txt index c5067b8..f4f9dd8 100644 --- a/navmap_core/CMakeLists.txt +++ b/navmap_core/CMakeLists.txt @@ -28,6 +28,13 @@ install( DESTINATION include/ ) +option(NAVMAP_BUILD_BENCH "Build the navmap_core micro-benchmark" OFF) +if(NAVMAP_BUILD_BENCH) + add_executable(navmap_bench bench/navmap_bench.cpp) + target_link_libraries(navmap_bench ${PROJECT_NAME}) + install(TARGETS navmap_bench RUNTIME DESTINATION lib/${PROJECT_NAME}) +endif() + install(TARGETS ${PROJECT_NAME} EXPORT export_${PROJECT_NAME} diff --git a/navmap_core/bench/navmap_bench.cpp b/navmap_core/bench/navmap_bench.cpp new file mode 100644 index 0000000..00d3052 --- /dev/null +++ b/navmap_core/bench/navmap_bench.cpp @@ -0,0 +1,161 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Micro-benchmark of the NavMap core queries over grid maps of growing size. +// Usage: navmap_bench [max_side_cells=1000] [floors=1] + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "navmap_core/NavMap.hpp" + +using navmap::NavCelId; +using navmap::NavMap; + +namespace +{ + +using Clock = std::chrono::steady_clock; + +// side x side cells of res metres per floor, floors 3 m apart. +NavMap make_map(int side, int floors, float res) +{ + NavMap nm; + for (int f = 0; f < floors; ++f) { + const float z = 3.0f * static_cast(f); + const std::size_t s = nm.create_surface("map"); + const auto base = static_cast(nm.positions.size()); + for (int j = 0; j <= side; ++j) { + for (int i = 0; i <= side; ++i) { + nm.add_vertex({i * res, j * res, z}); + } + } + auto v = [&](int i, int j) {return base + static_cast(j * (side + 1) + i);}; + for (int j = 0; j < side; ++j) { + for (int i = 0; i < side; ++i) { + nm.add_navcel_to_surface(s, nm.add_navcel(v(i, j), v(i + 1, j), v(i + 1, j + 1))); + nm.add_navcel_to_surface(s, nm.add_navcel(v(i, j), v(i + 1, j + 1), v(i, j + 1))); + } + } + } + return nm; +} + +// Median time per call in microseconds over `reps` batches of `n` calls. +template +double median_us(int reps, int n, F && f) +{ + std::vector t; + for (int r = 0; r < reps; ++r) { + const auto t0 = Clock::now(); + for (int k = 0; k < n; ++k) { + f(k); + } + t.push_back(std::chrono::duration(Clock::now() - t0).count() / n); + } + std::nth_element(t.begin(), t.begin() + t.size() / 2, t.end()); + return t[t.size() / 2]; +} + +std::size_t geometry_bytes(const NavMap & nm) +{ + std::size_t b = nm.positions.size() * 3 * sizeof(float) + nm.navcels.size() * + sizeof(navmap::NavCel); + for (const auto & s : nm.surfaces) { + b += s.bvh.size() * sizeof(navmap::BVHNode) + (s.prim_indices.size() + s.navcels.size()) * 4; + } + return b; +} + +} // namespace + +int main(int argc, char ** argv) +{ + const int max_side = argc > 1 ? std::atoi(argv[1]) : 1000; + const int floors = argc > 2 ? std::atoi(argv[2]) : 1; + const float res = 0.05f; + const int N = 20000, R = 7; + + std::printf( + "floors=%d res=%.2f m, median us/op over %d x %d calls\n" + "%9s %9s %9s %9s %9s %9s %9s %9s %9s %9s %9s\n", + floors, res, R, N, "navcels", "MB", "build_ms", "loc_nohnt", "loc_hint", "ray_down", + "ray_rand", "closest", "area_r0.5", "copy_same", "copy_deep"); + + for (int side : {100, 300, 1000, 3000}) { + if (side > max_side) {break;} + NavMap nm = make_map(side, floors, res); + const auto t0 = Clock::now(); + nm.rebuild_geometry_accels(); + const double build_ms = std::chrono::duration(Clock::now() - t0).count(); + nm.add_layer("occupancy", "", "", 0); + + const float extent = side * res; + std::mt19937 rng(1); + std::uniform_real_distribution U(0.01f, extent - 0.01f); + std::uniform_int_distribution F(0, floors - 1); + std::normal_distribution G(0.0f, 1.0f); + std::vector pts(N), dirs(N); + for (int k = 0; k < N; ++k) { + pts[k] = {U(rng), U(rng), 3.0f * static_cast(F(rng)) + 0.1f}; + dirs[k] = Eigen::Vector3f(G(rng), G(rng), G(rng)).normalized(); + } + + std::size_t s; NavCelId c; Eigen::Vector3f bary, hit, cp; float t, sq; + const double loc_nohint = median_us( + R, N, [&](int k) {nm.locate_navcel(pts[k], s, c, bary, &hit);}); + + // Coherent trajectory: small steps reusing the previous cell as hint. + NavMap::LocateOpts opts; + nm.locate_navcel(pts[0], s, c, bary, &hit); + Eigen::Vector3f p = pts[0]; + const double loc_hint = median_us( + R, N, [&](int k) { + p.x() = std::clamp(p.x() + 0.02f * ((k % 7) - 3), 0.01f, extent - 0.01f); + p.y() = std::clamp(p.y() + 0.02f * ((k % 5) - 2), 0.01f, extent - 0.01f); + opts.hint_cid = c; + nm.locate_navcel(p, s, c, bary, &hit, opts); + }); + + const double ray_down = median_us( + R, N, [&](int k) { + nm.raycast(pts[k] + Eigen::Vector3f(0, 0, 1), {0.0f, 0.0f, -1.0f}, c, t, hit); + }); + const double ray_rand = median_us( + R, N, [&](int k) {nm.raycast(pts[k], dirs[k], c, t, hit);}); + const double closest = median_us( + R, N, [&](int k) {nm.closest_navcel(pts[k] + 0.3f * dirs[k], s, c, cp, sq);}); + const double area = median_us( + R, N / 20, [&](int k) { + nm.set_area(pts[k], 254, "occupancy", navmap::AreaShape::CIRCULAR, 0.5f); + }); + + NavMap same = nm; + const double copy_same = median_us(R, 10, [&](int) {same = nm;}); + const double copy_deep = median_us(R, 10, [&](int) {NavMap d(nm); (void)d;}); + + std::printf( + "%9zu %9.1f %9.1f %9.3f %9.3f %9.3f %9.3f %9.3f %9.1f %9.0f %9.0f\n", + nm.navcels.size(), geometry_bytes(nm) / 1e6, build_ms, loc_nohint, loc_hint, ray_down, + ray_rand, closest, area, copy_same, copy_deep); + } + return 0; +} diff --git a/navmap_core/include/navmap_core/NavMap.hpp b/navmap_core/include/navmap_core/NavMap.hpp index 340ce1d..086b59c 100644 --- a/navmap_core/include/navmap_core/NavMap.hpp +++ b/navmap_core/include/navmap_core/NavMap.hpp @@ -695,6 +695,13 @@ class NavMap /// \brief Return up to 3 neighbor cell ids (skips invalid entries). std::vector navcel_neighbors(NavCelId cid) const; + /// \brief Value returned by \ref surface_of() for a NavCel in no surface. + static constexpr std::size_t kNoSurface = std::numeric_limits::max(); + + /// \brief Index of the surface owning \p cid, or \ref kNoSurface. + /// \note O(1) after \ref rebuild_geometry_accels(). + std::size_t surface_of(NavCelId cid) const; + // ---- Layers (per-NavCel) convenience ---- /** @@ -1114,6 +1121,15 @@ class NavMap mutable bool geometry_dirty_{true}; mutable std::uint64_t geometry_fp_{0}; + // Closest hit over all surfaces with t < t_max, nearest surfaces first. + bool closest_hit_( + const Eigen::Vector3f & o, const Eigen::Vector3f & d, float t_max, + RayHit & out) const; + + // Owning surface per NavCel, built in rebuild_geometry_accels(). + std::vector navcel_surface_; + void build_navcel_surface_index_(); + // Builders and traversal helpers. /** @@ -1130,6 +1146,7 @@ class NavMap * \param[out] hit_cid Triangle id on hit. * \param[out] t_out Distance along ray. * \param[out] hit_pt Intersection point (world). + * \param t_max Only hits closer than this count (prunes the BVH). * \return true if any triangle was hit. */ bool surface_raycast( @@ -1138,7 +1155,8 @@ class NavMap const Eigen::Vector3f & d, NavCelId & hit_cid, float & t_out, - Eigen::Vector3f & hit_pt) const; + Eigen::Vector3f & hit_pt, + float t_max = std::numeric_limits::infinity()) const; /** * \brief Walk across neighbors starting from \p start_cid to locate \p p. diff --git a/navmap_core/src/navmap_core/NavMap.cpp b/navmap_core/src/navmap_core/NavMap.cpp index b18b331..b69f9cb 100644 --- a/navmap_core/src/navmap_core/NavMap.cpp +++ b/navmap_core/src/navmap_core/NavMap.cpp @@ -15,7 +15,8 @@ #include "navmap_core/NavMap.hpp" -#include +#include +#include #include #include #include @@ -56,6 +57,7 @@ NavMap::NavMap(const NavMap & other) surfaces = other.surfaces; layers = other.layers; layer_meta = other.layer_meta; + navcel_surface_ = other.navcel_surface_; geometry_dirty_ = true; geometry_fp_ = 0; @@ -179,6 +181,7 @@ NavMap & NavMap::operator=(const NavMap & other) surfaces = other.surfaces; layers = other.layers; layer_meta = other.layer_meta; + navcel_surface_ = other.navcel_surface_; geometry_dirty_ = true; return *this; @@ -195,6 +198,7 @@ NavMap & NavMap::operator=(NavMap && other) noexcept surfaces = std::move(other.surfaces); layers = std::move(other.layers); layer_meta = std::move(other.layer_meta); + navcel_surface_ = std::move(other.navcel_surface_); geometry_fp_ = other.geometry_fp_; geometry_dirty_ = other.geometry_dirty_; @@ -348,6 +352,67 @@ void NavMap::build_surface_bvh(Surface & s) } } +// ----------------------------------------------------------------------------- +// BVH traversal stack: fixed buffer, heap only if a degenerate tree overflows it +// ----------------------------------------------------------------------------- + +namespace +{ +class NodeStack +{ +public: + void push(int v) + { + if (n_ < static_cast(buf_.size())) {buf_[n_++] = v;} else {spill_.push_back(v);} + } + int pop() + { + if (!spill_.empty()) { + const int v = spill_.back(); + spill_.pop_back(); + return v; + } + return buf_[--n_]; + } + bool empty() const {return n_ == 0 && spill_.empty();} + +private: + std::array buf_; + int n_{0}; + std::vector spill_; +}; + +// Entry distance of the ray into the box, if it enters before t_max. +inline bool ray_box_entry( + const AABB & box, const Vec3 & o, const Vec3 & d, float t_max, + float & t_in) +{ + float t0 = 0.0f, t1 = t_max; + for (int i = 0; i < 3; ++i) { + if (std::fabs(d[i]) < 1e-12f) { + if (o[i] < box.min[i] || o[i] > box.max[i]) {return false;} + continue; + } + const float inv = 1.0f / d[i]; + float a = (box.min[i] - o[i]) * inv, b = (box.max[i] - o[i]) * inv; + if (a > b) {std::swap(a, b);} + t0 = std::max(t0, a); + t1 = std::min(t1, b); + if (!(t0 <= t1)) {return false;} // also rejects NaN + } + t_in = t0; + return true; +} + +inline float box_sq_dist(const AABB & box, const Vec3 & p) +{ + const float dx = std::max(std::max(box.min.x() - p.x(), 0.0f), p.x() - box.max.x()); + const float dy = std::max(std::max(box.min.y() - p.y(), 0.0f), p.y() - box.max.y()); + const float dz = std::max(std::max(box.min.z() - p.z(), 0.0f), p.z() - box.max.z()); + return dx * dx + dy * dy + dz * dz; +} +} // namespace + // ----------------------------------------------------------------------------- // Rebuild accelerations and geometry caches // ----------------------------------------------------------------------------- @@ -370,6 +435,7 @@ void NavMap::rebuild_geometry_accels() for (auto & s : surfaces) { build_surface_bvh(s); } + build_navcel_surface_index_(); // NOTE: Layers are per-NavCel; if navcels size changed, caller may // choose to call layers.resize_all(navcels.size()) externally. @@ -385,21 +451,20 @@ bool NavMap::surface_raycast( const Vec3 & d, NavCelId & hit_cid, float & t_out, - Vec3 & hit_pt) const + Vec3 & hit_pt, + float t_max) const { if (s.bvh.empty()) { return false; } bool hit = false; - float best_t = std::numeric_limits::infinity(); - std::stack st; + float best_t = t_max; + NodeStack st; st.push(0); while (!st.empty()) { - int idx = st.top(); - st.pop(); - const BVHNode & n = s.bvh[idx]; + const BVHNode & n = s.bvh[st.pop()]; if (!n.box.intersects_ray(o, d, best_t)) { continue; } @@ -439,29 +504,47 @@ bool NavMap::raycast( float & t, Vec3 & hit_pt) const { - bool any = false; - float best_t = std::numeric_limits::infinity(); - Vec3 best_p = Vec3::Zero(); - NavCelId best_cid = 0; + RayHit h; + if (!closest_hit_(o, d, std::numeric_limits::infinity(), h)) { + return false; + } + hit_cid = h.cid; + t = h.t; + hit_pt = h.p; + return true; +} - for (const auto & s : surfaces) { - NavCelId cid; - float tt; - Vec3 p; - if (surface_raycast(s, o, d, cid, tt, p)) { - if (tt < best_t) { - best_t = tt; - best_p = p; - best_cid = cid; - any = true; - } +bool NavMap::closest_hit_(const Vec3 & o, const Vec3 & d, float t_max, RayHit & out) const +{ + // Surfaces the ray enters before t_max, sorted by entry distance. + constexpr std::size_t kInline = 16; + std::array, kInline> inline_buf; + std::vector> heap_buf; + std::pair * order = inline_buf.data(); + if (surfaces.size() > kInline) { + heap_buf.resize(surfaces.size()); + order = heap_buf.data(); + } + std::size_t n = 0; + for (std::size_t s = 0; s < surfaces.size(); ++s) { + float t_in; + if (!surfaces[s].bvh.empty() && ray_box_entry(surfaces[s].aabb, o, d, t_max, t_in)) { + order[n++] = {t_in, static_cast(s)}; } } + std::sort(order, order + n); - if (any) { - hit_cid = best_cid; - t = best_t; - hit_pt = best_p; + bool any = false; + float best_t = t_max; + for (std::size_t k = 0; k < n && order[k].first < best_t; ++k) { + NavCelId cid; + float t; + Vec3 p; + if (surface_raycast(surfaces[order[k].second], o, d, cid, t, p, best_t) && t < best_t) { + best_t = t; + out = {true, order[k].second, cid, t, p}; + any = true; + } } return any; } @@ -476,36 +559,19 @@ void NavMap::raycast_many( for (size_t i = 0; i < rays.size(); ++i) { const auto & ray = rays[i]; - bool any = false; - float best_t = std::numeric_limits::infinity(); - Eigen::Vector3f best_p(0, 0, 0); - NavCelId best_cid = 0; - size_t best_surf = 0; - + if (!first_hit_only) { + closest_hit_(ray.o, ray.d, std::numeric_limits::infinity(), out[i]); + continue; + } for (size_t s = 0; s < surfaces.size(); ++s) { NavCelId cid; float t; Eigen::Vector3f p; if (surface_raycast(surfaces[s], ray.o, ray.d, cid, t, p)) { - if (first_hit_only) { - out[i] = {true, s, cid, t, p}; - any = true; - break; - } else { - if (t < best_t) { - best_t = t; - best_p = p; - best_cid = cid; - best_surf = s; - any = true; - } - } + out[i] = {true, s, cid, t, p}; + break; } } - - if (!first_hit_only && any) { - out[i] = {true, best_surf, best_cid, best_t, best_p}; - } } } @@ -513,6 +579,16 @@ void NavMap::raycast_many( // Locate helpers // ----------------------------------------------------------------------------- +// Point of the plane (a, n) under/over p; along n if the plane is vertical. +static inline Vec3 project_to_plane_vertically(const Vec3 & p, const Vec3 & a, const Vec3 & n) +{ + const float dist = n.dot(p - a); + if (std::fabs(n.z()) > 1e-6f) { + return Vec3(p.x(), p.y(), p.z() - dist / n.z()); + } + return p - dist * n; +} + static inline bool point_in_triangle_bary( const Vec3 & p, const Vec3 & a, @@ -557,8 +633,7 @@ bool NavMap::locate_by_walking( Vec3 b = positions.at(c.v[1]); Vec3 d = positions.at(c.v[2]); Vec3 n = c.normal; - float dist = n.dot(p - a); - Vec3 q = p - dist * n; + Vec3 q = project_to_plane_vertically(p, a, n); if (hit_pt) { *hit_pt = q; } @@ -578,7 +653,8 @@ bool NavMap::locate_by_walking( cid_out = cid; return true; } - NavCelId next = c.neighbor[neg_idx]; + // bary[i] < 0: p is beyond the edge opposite vertex i, i.e. edge (i+1)%3. + NavCelId next = c.neighbor[(neg_idx + 1) % 3]; if (next == std::numeric_limits::max()) { cid_out = cid; return false; @@ -612,7 +688,7 @@ bool NavMap::locate_navcel_core( const Vec3 & n = c.normal; const float dist = n.dot(p_world - a); - const Vec3 q = p_world - dist * n; + const Vec3 q = project_to_plane_vertically(p_world, a, n); Vec3 bary_hint; if (point_in_triangle_bary(q, a, b, d, bary_hint, opts.planar_eps) && @@ -624,13 +700,10 @@ bool NavMap::locate_navcel_core( *hit_pt = q; } - // Find the surface that owns this navcel. - for (size_t s = 0; s < surfaces.size(); ++s) { - const auto & surf = surfaces[s]; - if (std::find(surf.navcels.begin(), surf.navcels.end(), cid) != surf.navcels.end()) { - surface_idx = s; - return true; - } + const std::size_t s = surface_of(cid); + if (s != kNoSurface) { + surface_idx = s; + return true; } // If no surface owns the hinted navcel, fall through to the generic search. } @@ -647,21 +720,17 @@ bool NavMap::locate_navcel_core( const float dist0 = n0.dot(p_world - a0); // Do not walk if the query point is clearly off the hinted plane. + Vec3 q; if (std::fabs(dist0) <= opts.height_eps) { - if (locate_by_walking( - start, - p_world, - cid, - bary, - hit_pt, - opts.planar_eps)) + // The walk may end on a cell far from p in height: re-check height_eps there. + if (locate_by_walking(start, p_world, cid, bary, &q, opts.planar_eps) && + (p_world - q).norm() <= opts.height_eps) { - for (size_t s = 0; s < surfaces.size(); ++s) { - const auto & surf = surfaces[s]; - if (std::find(surf.navcels.begin(), surf.navcels.end(), cid) != surf.navcels.end()) { - surface_idx = s; - return true; - } + if (hit_pt) {*hit_pt = q;} + const std::size_t s = surface_of(cid); + if (s != kNoSurface) { + surface_idx = s; + return true; } // Fall through if surface not found. } @@ -669,54 +738,41 @@ bool NavMap::locate_navcel_core( } } - // 3) Vertical raycast fallback (choose by |dz| using BOTH directions) + // 3) Vertical rays down and up; keep the surface closest in z. { - // Shoot both rays: downward and upward - std::vector rays; - rays.push_back({p_world, Vec3(0.0f, 0.0f, -1.0f)}); - rays.push_back({p_world, Vec3(0.0f, 0.0f, 1.0f)}); - - std::vector hits; - raycast_many(rays, hits, /*first_hit_only=*/ true); - - bool any = false; - float best_dz = std::numeric_limits::infinity(); - size_t best_surf = 0; - NavCelId best_cid = 0; - Vec3 best_hit(0.0f, 0.0f, 0.0f); - Vec3 best_bary(0.0f, 0.0f, 0.0f); - - for (const auto & h : hits) { - if (!h.hit) {continue;} - const float dz = std::fabs(h.p.z() - p_world.z()); - // Compute barycentrics on the hit triangle to return consistent outputs - const auto & tri = navcels[h.cid]; - const Vec3 a = positions.at(tri.v[0]); - const Vec3 b = positions.at(tri.v[1]); - const Vec3 c = positions.at(tri.v[2]); - Vec3 bary_tmp; - if (!point_in_triangle_bary(h.p, a, b, c, bary_tmp, opts.planar_eps)) { - continue; // numerical guard - } - if (dz < best_dz) { - best_dz = dz; - best_surf = h.surface; - best_cid = h.cid; - best_hit = h.p; - best_bary = bary_tmp; - any = true; - } - } - - if (any) { - surface_idx = best_surf; - cid = best_cid; - bary = best_bary; - if (hit_pt) {*hit_pt = best_hit;} + // Start slightly past p so a surface through p itself is hit (t > 0). + constexpr float kRayLift = 1e-3f; + const float inf = std::numeric_limits::infinity(); + + auto valid_hit = [&](const RayHit & h, Vec3 & bary_out) { + const auto & tri = navcels[h.cid]; + return point_in_triangle_bary( + h.p, positions.at(tri.v[0]), positions.at(tri.v[1]), positions.at(tri.v[2]), + bary_out, opts.planar_eps); + }; + + RayHit down, up; + Vec3 bary_down, bary_up; + const bool has_down = + closest_hit_(p_world + Vec3(0.0f, 0.0f, kRayLift), Vec3(0.0f, 0.0f, -1.0f), inf, down) && + valid_hit(down, bary_down); + const float dz_down = has_down ? std::fabs(down.p.z() - p_world.z()) : inf; + + // Upward ray only needs surfaces strictly closer than the one below. + const float up_max = has_down ? dz_down + kRayLift : inf; + const bool has_up = + closest_hit_(p_world - Vec3(0.0f, 0.0f, kRayLift), Vec3(0.0f, 0.0f, 1.0f), up_max, up) && + valid_hit(up, bary_up) && std::fabs(up.p.z() - p_world.z()) < dz_down; + + if (has_down || has_up) { + const RayHit & best = has_up ? up : down; + surface_idx = best.surface; + cid = best.cid; + bary = has_up ? bary_up : bary_down; + if (hit_pt) {*hit_pt = best.p;} return true; } } - return false; } @@ -736,19 +792,12 @@ bool NavMap::surface_closest_navcel( } bool any = false; - std::stack st; + NodeStack st; st.push(0); while (!st.empty()) { - int idx = st.top(); - st.pop(); - const BVHNode & n = s.bvh[idx]; - - float dx = std::max(std::max(n.box.min.x() - p.x(), 0.0f), p.x() - n.box.max.x()); - float dy = std::max(std::max(n.box.min.y() - p.y(), 0.0f), p.y() - n.box.max.y()); - float dz = std::max(std::max(n.box.min.z() - p.z(), 0.0f), p.z() - n.box.max.z()); - float box_lb = dx * dx + dy * dy + dz * dz; - if (box_lb > best_sq) { + const BVHNode & n = s.bvh[st.pop()]; + if (box_sq_dist(n.box, p) >= best_sq) { continue; } @@ -768,6 +817,12 @@ bool NavMap::surface_closest_navcel( any = true; } } + } else if (n.left >= 0 && n.right >= 0) { + // Push the farther child first so the nearer one is visited first. + const bool left_near = + box_sq_dist(s.bvh[n.left].box, p) <= box_sq_dist(s.bvh[n.right].box, p); + st.push(left_near ? n.right : n.left); + st.push(left_near ? n.left : n.right); } else { if (n.left >= 0) {st.push(n.left);} if (n.right >= 0) {st.push(n.right);} @@ -913,6 +968,38 @@ std::vector NavMap::navcel_neighbors(NavCelId cid) const return out; } +void NavMap::build_navcel_surface_index_() +{ + constexpr auto kUnset = std::numeric_limits::max(); + navcel_surface_.assign(navcels.size(), kUnset); + for (std::size_t s = 0; s < surfaces.size(); ++s) { + for (auto cid : surfaces[s].navcels) { + // First owner wins, as in the linear fallback. + if (cid < navcel_surface_.size() && navcel_surface_[cid] == kUnset) { + navcel_surface_[cid] = static_cast(s); + } + } + } +} + +std::size_t NavMap::surface_of(NavCelId cid) const +{ + if (cid >= navcels.size()) {return kNoSurface;} + + // Stale index (geometry edited without a rebuild): linear fallback. + if (navcel_surface_.size() != navcels.size()) { + for (std::size_t s = 0; s < surfaces.size(); ++s) { + const auto & ids = surfaces[s].navcels; + if (std::find(ids.begin(), ids.end(), cid) != ids.end()) {return s;} + } + return kNoSurface; + } + + const auto s = navcel_surface_[cid]; + return (s == std::numeric_limits::max() || s >= surfaces.size()) ? + kNoSurface : static_cast(s); +} + bool NavMap::locate_navcel( const Eigen::Vector3f & p_world, std::size_t & surface_idx, @@ -953,13 +1040,8 @@ bool NavMap::locate_navcel( c.hit = hit; c.dz = std::fabs(hit.z() - p_world.z()); - for (std::size_t si = 0; si < surfaces.size(); ++si) { - const auto & s = surfaces[si]; - if (std::find(s.navcels.begin(), s.navcels.end(), hit_cid) != s.navcels.end()) { - c.surf = si; - break; - } - } + const std::size_t si = surface_of(hit_cid); + c.surf = (si != kNoSurface) ? si : 0; const auto & tri = navcels[c.cid]; const Eigen::Vector3f A = positions.at(tri.v[0]); diff --git a/navmap_core/tests/CMakeLists.txt b/navmap_core/tests/CMakeLists.txt index e11cf6d..3b42222 100644 --- a/navmap_core/tests/CMakeLists.txt +++ b/navmap_core/tests/CMakeLists.txt @@ -32,3 +32,13 @@ ament_add_gtest(test_navmap_assign test_navmap_assign.cpp) target_link_libraries(test_navmap_assign ${PROJECT_NAME} ) + +ament_add_gtest(test_locate_multisurface test_locate_multisurface.cpp) +target_link_libraries(test_locate_multisurface + ${PROJECT_NAME} +) + +ament_add_gtest(test_navmap_properties test_navmap_properties.cpp) +target_link_libraries(test_navmap_properties + ${PROJECT_NAME} +) diff --git a/navmap_core/tests/test_locate_multisurface.cpp b/navmap_core/tests/test_locate_multisurface.cpp new file mode 100644 index 0000000..32c4b8b --- /dev/null +++ b/navmap_core/tests/test_locate_multisurface.cpp @@ -0,0 +1,330 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "navmap_core/NavMap.hpp" + +using navmap::NavCelId; +using navmap::NavMap; + +namespace +{ + +// Adds an n x n grid floor at height z as a new surface; returns the surface index. +std::size_t add_floor(NavMap & nm, float z, int n = 10, float res = 1.0f) +{ + const std::size_t s = nm.create_surface("map"); + const auto base = static_cast(nm.positions.size()); + for (int j = 0; j <= n; ++j) { + for (int i = 0; i <= n; ++i) { + nm.add_vertex({i * res, j * res, z}); + } + } + auto v = [&](int i, int j) {return base + static_cast(j * (n + 1) + i);}; + for (int j = 0; j < n; ++j) { + for (int i = 0; i < n; ++i) { + nm.add_navcel_to_surface(s, nm.add_navcel(v(i, j), v(i + 1, j), v(i + 1, j + 1))); + nm.add_navcel_to_surface(s, nm.add_navcel(v(i, j), v(i + 1, j + 1), v(i, j + 1))); + } + } + return s; +} + +struct Located +{ + bool ok{false}; + std::size_t surface{0}; + NavCelId cid{0}; + Eigen::Vector3f hit{Eigen::Vector3f::Zero()}; +}; + +Located locate(const NavMap & nm, const Eigen::Vector3f & p, NavMap::LocateOpts opts = {}) +{ + Located r; + Eigen::Vector3f bary; + r.ok = nm.locate_navcel(p, r.surface, r.cid, bary, &r.hit, opts); + return r; +} + +// Floors at z = 0, 3, 6 listed in the given order. +NavMap build_floors(const std::vector & heights) +{ + NavMap nm; + for (float z : heights) { + add_floor(nm, z); + } + nm.rebuild_geometry_accels(); + return nm; +} + +} // namespace + +TEST(NavMap_LocateMultiSurface, PointJustAboveEachFloorLocatesThatFloor) +{ + const NavMap nm = build_floors({0.0f, 3.0f, 6.0f}); + const float heights[] = {0.0f, 3.0f, 6.0f}; + for (std::size_t k = 0; k < 3; ++k) { + const auto r = locate(nm, {5.2f, 5.3f, heights[k] + 0.1f}); + ASSERT_TRUE(r.ok) << "floor " << k; + EXPECT_EQ(r.surface, k) << "floor " << k; + EXPECT_NEAR(r.hit.z(), heights[k], 1e-5f) << "floor " << k; + EXPECT_EQ(nm.surface_of(r.cid), k); + } +} + +TEST(NavMap_LocateMultiSurface, PointExactlyOnAFloorLocatesThatFloor) +{ + // Typical robot pose: z exactly on the floor it stands on. + const NavMap nm = build_floors({0.0f, 3.0f, 6.0f}); + for (std::size_t k = 0; k < 3; ++k) { + const float z = 3.0f * static_cast(k); + const auto r = locate(nm, {5.2f, 5.3f, z}); + ASSERT_TRUE(r.ok) << "floor " << k; + EXPECT_EQ(r.surface, k) << "floor " << k; + EXPECT_NEAR(r.hit.z(), z, 1e-5f); + } + // Single flat map, as built from an OccupancyGrid. + const NavMap flat = build_floors({0.0f}); + const auto r = locate(flat, {5.2f, 5.3f, 0.0f}); + ASSERT_TRUE(r.ok); + EXPECT_EQ(r.surface, 0u); +} + +TEST(NavMap_LocateMultiSurface, ResultDoesNotDependOnSurfaceOrder) +{ + // Same building, surfaces listed top-down: the closest floor must still win. + const NavMap nm = build_floors({6.0f, 3.0f, 0.0f}); + const auto r0 = locate(nm, {5.2f, 5.3f, 0.1f}); + const auto r1 = locate(nm, {5.2f, 5.3f, 3.1f}); + const auto r2 = locate(nm, {5.2f, 5.3f, 6.1f}); + ASSERT_TRUE(r0.ok && r1.ok && r2.ok); + EXPECT_EQ(r0.surface, 2u); + EXPECT_EQ(r1.surface, 1u); + EXPECT_EQ(r2.surface, 0u); + EXPECT_NEAR(r1.hit.z(), 3.0f, 1e-5f); +} + +TEST(NavMap_LocateMultiSurface, BetweenFloorsPicksTheClosestInZ) +{ + const NavMap nm = build_floors({0.0f, 3.0f}); + EXPECT_EQ(locate(nm, {4.5f, 4.5f, 1.0f}).surface, 0u); + EXPECT_EQ(locate(nm, {4.5f, 4.5f, 2.0f}).surface, 1u); + EXPECT_EQ(locate(nm, {4.5f, 4.5f, 2.9f}).surface, 1u); +} + +TEST(NavMap_LocateMultiSurface, AboveTopAndBelowBottomFloor) +{ + const NavMap nm = build_floors({0.0f, 3.0f}); + const auto above = locate(nm, {4.5f, 4.5f, 10.0f}); + ASSERT_TRUE(above.ok); + EXPECT_EQ(above.surface, 1u); + const auto below = locate(nm, {4.5f, 4.5f, -2.0f}); + ASSERT_TRUE(below.ok); + EXPECT_EQ(below.surface, 0u); +} + +TEST(NavMap_LocateMultiSurface, OutsideFootprintFails) +{ + const NavMap nm = build_floors({0.0f, 3.0f}); + EXPECT_FALSE(locate(nm, {50.0f, 50.0f, 0.1f}).ok); +} + +TEST(NavMap_LocateMultiSurface, HintOnTheRightFloorIsUsed) +{ + const NavMap nm = build_floors({0.0f, 3.0f}); + const auto seed = locate(nm, {2.2f, 2.3f, 3.1f}); + ASSERT_TRUE(seed.ok); + ASSERT_EQ(seed.surface, 1u); + + // Sequence of nearby queries reusing the previous cell as hint. + NavMap::LocateOpts opts; + NavCelId prev = seed.cid; + for (int k = 0; k < 20; ++k) { + opts.hint_cid = prev; + const auto r = locate(nm, {2.2f + 0.3f * k, 2.3f, 3.05f}, opts); + ASSERT_TRUE(r.ok) << "step " << k; + EXPECT_EQ(r.surface, 1u) << "step " << k; + EXPECT_NEAR(r.hit.z(), 3.0f, 1e-5f); + prev = r.cid; + } +} + +TEST(NavMap_LocateMultiSurface, HintOnTheOtherFloorIsIgnored) +{ + const NavMap nm = build_floors({0.0f, 3.0f}); + const auto upper = locate(nm, {5.2f, 5.3f, 3.1f}); + ASSERT_TRUE(upper.ok); + + // Hint on the upper floor, query on the lower one, same XY. + NavMap::LocateOpts opts; + opts.hint_cid = upper.cid; + const auto r = locate(nm, {5.2f, 5.3f, 0.1f}, opts); + ASSERT_TRUE(r.ok); + EXPECT_EQ(r.surface, 0u); + EXPECT_NEAR(r.hit.z(), 0.0f, 1e-5f); +} + +TEST(NavMap_LocateMultiSurface, HintWalksAcrossNeighborsOnItsSurface) +{ + // A: floor at z=0. B: shelf at z=0.5 over the same area. Query at z=0.3 is + // closer to B, so only a working neighbor walk from a hint on A returns A. + NavMap nm; + const std::size_t a = add_floor(nm, 0.0f); + const std::size_t b = add_floor(nm, 0.5f); + nm.rebuild_geometry_accels(); + ASSERT_EQ(locate(nm, {5.5f, 5.5f, 0.3f}).surface, b); + + int checked = 0; + for (NavCelId hint : nm.surfaces[a].navcels) { + const Eigen::Vector3f c = nm.navcel_centroid(hint); + if ((c.head<2>() - Eigen::Vector2f(5.5f, 5.5f)).norm() > 3.0f) {continue;} + NavMap::LocateOpts opts; + opts.hint_cid = hint; + const auto r = locate(nm, {5.5f, 5.5f, 0.3f}, opts); + ASSERT_TRUE(r.ok); + EXPECT_EQ(r.surface, a) << "hint " << hint; + EXPECT_NEAR(r.hit.z(), 0.0f, 1e-5f); + ++checked; + } + EXPECT_GT(checked, 40); +} + +TEST(NavMap_LocateMultiSurface, InvalidHintFallsBackToSearch) +{ + const NavMap nm = build_floors({0.0f, 3.0f}); + NavMap::LocateOpts opts; + opts.hint_cid = static_cast(nm.navcels.size() + 100); + const auto r = locate(nm, {5.2f, 5.3f, 3.1f}, opts); + ASSERT_TRUE(r.ok); + EXPECT_EQ(r.surface, 1u); +} + +TEST(NavMap_LocateMultiSurface, HintOnCellWithoutSurfaceFallsBackToSearch) +{ + NavMap nm = build_floors({0.0f, 3.0f}); + // A loose triangle at z=3 that belongs to no surface. + const auto a = nm.add_vertex({20.0f, 20.0f, 3.0f}); + const auto b = nm.add_vertex({21.0f, 20.0f, 3.0f}); + const auto c = nm.add_vertex({20.0f, 21.0f, 3.0f}); + const NavCelId loose = nm.add_navcel(a, b, c); + nm.rebuild_geometry_accels(); + EXPECT_EQ(nm.surface_of(loose), NavMap::kNoSurface); + + NavMap::LocateOpts opts; + opts.hint_cid = loose; + const auto r = locate(nm, {5.2f, 5.3f, 3.1f}, opts); + ASSERT_TRUE(r.ok); + EXPECT_EQ(r.surface, 1u); +} + +TEST(NavMap_SurfaceOf, IndexMatchesSurfaceLists) +{ + const NavMap nm = build_floors({0.0f, 3.0f, 6.0f}); + for (std::size_t s = 0; s < nm.surfaces.size(); ++s) { + for (auto cid : nm.surfaces[s].navcels) { + ASSERT_EQ(nm.surface_of(cid), s); + } + } + EXPECT_EQ(nm.surface_of(static_cast(nm.navcels.size())), NavMap::kNoSurface); +} + +TEST(NavMap_SurfaceOf, EmptyMap) +{ + NavMap nm; + nm.rebuild_geometry_accels(); + EXPECT_EQ(nm.surface_of(0), NavMap::kNoSurface); +} + +TEST(NavMap_SurfaceOf, StaleIndexFallsBackToLinearSearch) +{ + NavMap nm = build_floors({0.0f}); + // Add a new surface and cell without rebuilding. + const std::size_t s = nm.create_surface("map"); + const auto a = nm.add_vertex({20.0f, 20.0f, 0.0f}); + const auto b = nm.add_vertex({21.0f, 20.0f, 0.0f}); + const auto c = nm.add_vertex({20.0f, 21.0f, 0.0f}); + const NavCelId cid = nm.add_navcel(a, b, c); + nm.add_navcel_to_surface(s, cid); + + EXPECT_EQ(nm.surface_of(cid), s); + EXPECT_EQ(nm.surface_of(0), 0u); + + nm.rebuild_geometry_accels(); + EXPECT_EQ(nm.surface_of(cid), s); +} + +TEST(NavMap_SurfaceOf, SurvivesCopyAssignAndMove) +{ + const NavMap src = build_floors({0.0f, 3.0f}); + const NavCelId upper_cid = src.surfaces[1].navcels.front(); + + const NavMap copied(src); + EXPECT_EQ(copied.surface_of(upper_cid), 1u); + + NavMap assigned; + assigned = src; + EXPECT_EQ(assigned.surface_of(upper_cid), 1u); + + // Same-geometry assignment keeps the destination index. + NavMap same = build_floors({0.0f, 3.0f}); + same = src; + EXPECT_EQ(same.surface_of(upper_cid), 1u); + + NavMap moved_from = src; + NavMap moved; + moved = std::move(moved_from); + EXPECT_EQ(moved.surface_of(upper_cid), 1u); + const auto r = locate(moved, {5.2f, 5.3f, 3.1f}); + ASSERT_TRUE(r.ok); + EXPECT_EQ(r.surface, 1u); +} + +TEST(NavMap_LocateMultiSurface, SetAreaOnUpperFloorLeavesLowerFloorUntouched) +{ + NavMap nm = build_floors({0.0f, 3.0f}); + nm.add_layer("occupancy", "", "", 0); + ASSERT_TRUE( + nm.set_area( + {5.0f, 5.0f, 3.1f}, 254, "occupancy", + navmap::AreaShape::CIRCULAR, 1.5f)); + + int marked_lower = 0, marked_upper = 0; + for (auto cid : nm.surfaces[0].navcels) { + marked_lower += nm.layer_get("occupancy", cid) == 254; + } + for (auto cid : nm.surfaces[1].navcels) { + marked_upper += nm.layer_get("occupancy", cid) == 254; + } + EXPECT_EQ(marked_lower, 0); + EXPECT_GT(marked_upper, 0); +} + +TEST(NavMap_LocateMultiSurface, SampleLayerAtReadsTheRightFloor) +{ + NavMap nm = build_floors({0.0f, 3.0f}); + auto layer = nm.add_layer("elevation", "", "m", 0.0f); + for (std::size_t s = 0; s < nm.surfaces.size(); ++s) { + for (auto cid : nm.surfaces[s].navcels) { + (*layer)[cid] = s == 0 ? 0.0f : 3.0f; + } + } + EXPECT_FLOAT_EQ(nm.sample_layer_at("elevation", {5.2f, 5.3f, 0.1f}), 0.0f); + EXPECT_FLOAT_EQ(nm.sample_layer_at("elevation", {5.2f, 5.3f, 3.1f}), 3.0f); +} diff --git a/navmap_core/tests/test_navmap_properties.cpp b/navmap_core/tests/test_navmap_properties.cpp new file mode 100644 index 0000000..77905c5 --- /dev/null +++ b/navmap_core/tests/test_navmap_properties.cpp @@ -0,0 +1,501 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Randomized checks of the geometric queries against brute-force references. + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "navmap_core/NavMap.hpp" + +using navmap::NavCelId; +using navmap::NavMap; + +namespace +{ + +constexpr float kInf = std::numeric_limits::infinity(); +constexpr float kNaN = std::numeric_limits::quiet_NaN(); +// Reference rays start low enough to keep float precision (all maps are below 20 m). +constexpr float kTopZ = 50.0f; + +struct Patch +{ + float x0, y0, res, base_z, sx, sy, bump; + int nx, ny; + float z(float x, float y) const + { + return base_z + sx * x + sy * y + bump * std::sin(1.3f * x) * std::cos(0.7f * y); + } +}; + +// Random multi-surface map: sloped, bumpy patches overlapping in XY, with the +// triangles of all surfaces interleaved in the global NavCel array. +NavMap random_map(std::mt19937 & rng, int n_surfaces) +{ + std::uniform_real_distribution U(0.0f, 1.0f); + NavMap nm; + struct Tri {uint32_t a, b, c; std::size_t s;}; + std::vector tris; + + for (int k = 0; k < n_surfaces; ++k) { + Patch p; + p.res = 0.2f + 0.8f * U(rng); + p.nx = 4 + static_cast(16 * U(rng)); + p.ny = 4 + static_cast(16 * U(rng)); + p.x0 = -2.0f + 4.0f * U(rng); + p.y0 = -2.0f + 4.0f * U(rng); + p.base_z = 2.5f * static_cast(k) + 0.3f * U(rng); + p.sx = -0.2f + 0.4f * U(rng); + p.sy = -0.2f + 0.4f * U(rng); + p.bump = 0.15f * U(rng); + + const std::size_t s = nm.create_surface("map"); + const auto base = static_cast(nm.positions.size()); + for (int j = 0; j <= p.ny; ++j) { + for (int i = 0; i <= p.nx; ++i) { + const float x = p.x0 + i * p.res, y = p.y0 + j * p.res; + nm.add_vertex({x, y, p.z(x, y)}); + } + } + auto v = [&](int i, int j) {return base + static_cast(j * (p.nx + 1) + i);}; + for (int j = 0; j < p.ny; ++j) { + for (int i = 0; i < p.nx; ++i) { + tris.push_back({v(i, j), v(i + 1, j), v(i + 1, j + 1), s}); + tris.push_back({v(i, j), v(i + 1, j + 1), v(i, j + 1), s}); + } + } + } + + std::shuffle(tris.begin(), tris.end(), rng); + for (const auto & t : tris) { + nm.add_navcel_to_surface(t.s, nm.add_navcel(t.a, t.b, t.c)); + } + nm.rebuild_geometry_accels(); + return nm; +} + +Eigen::Vector3f vtx(const NavMap & nm, NavCelId cid, int k) +{ + return nm.positions.at(nm.navcels[cid].v[k]); +} + +// Heights of the surface under (x, y), using the same ray test as the library. +std::vector heights_at(const NavMap & nm, float x, float y) +{ + std::vector out; + const Eigen::Vector3f o(x, y, kTopZ), d(0.0f, 0.0f, -1.0f); + for (NavCelId cid = 0; cid < nm.navcels.size(); ++cid) { + float t, u, v; + if (navmap::ray_triangle_intersect(o, d, vtx(nm, cid, 0), vtx(nm, cid, 1), vtx(nm, cid, 2), + t, u, v)) + { + out.push_back(o.z() - t); + } + } + return out; +} + +std::optional brute_raycast( + const NavMap & nm, const Eigen::Vector3f & o, + const Eigen::Vector3f & d) +{ + std::optional best; + for (NavCelId cid = 0; cid < nm.navcels.size(); ++cid) { + float t, u, v; + if (navmap::ray_triangle_intersect(o, d, vtx(nm, cid, 0), vtx(nm, cid, 1), vtx(nm, cid, 2), + t, u, v) && (!best || t < *best)) + { + best = t; + } + } + return best; +} + +float brute_closest_sq(const NavMap & nm, const Eigen::Vector3f & p) +{ + float best = kInf; + for (NavCelId cid = 0; cid < nm.navcels.size(); ++cid) { + best = std::min( + best, navmap::point_triangle_squared_distance( + p, vtx(nm, cid, 0), vtx(nm, cid, 1), vtx(nm, cid, 2))); + } + return best; +} + +// True if (x, y) projects inside triangle cid (vertical ray hits it). +bool covers_xy(const NavMap & nm, NavCelId cid, float x, float y) +{ + float t, u, v; + return navmap::ray_triangle_intersect( + {x, y, kTopZ}, {0.0f, 0.0f, -1.0f}, + vtx(nm, cid, 0), vtx(nm, cid, 1), vtx(nm, cid, 2), t, u, v); +} + +struct Bounds +{ + Eigen::Vector3f min{kInf, kInf, kInf}, max{-kInf, -kInf, -kInf}; +}; + +Bounds bounds_of(const NavMap & nm) +{ + Bounds b; + for (std::size_t i = 0; i < nm.positions.size(); ++i) { + b.min = b.min.cwiseMin(nm.positions.at(static_cast(i))); + b.max = b.max.cwiseMax(nm.positions.at(static_cast(i))); + } + return b; +} + +} // namespace + +TEST(NavMap_Properties, LocateMatchesBruteForceNearestSurface) +{ + std::mt19937 rng(1234); + int checked = 0, on_surface_checked = 0; + for (int map_i = 0; map_i < 12; ++map_i) { + const NavMap nm = random_map(rng, 1 + map_i % 4); + const Bounds b = bounds_of(nm); + std::uniform_real_distribution X(b.min.x() - 1.0f, b.max.x() + 1.0f); + std::uniform_real_distribution Y(b.min.y() - 1.0f, b.max.y() + 1.0f); + std::uniform_real_distribution Z(b.min.z() - 1.0f, b.max.z() + 1.0f); + std::bernoulli_distribution on_surface(0.3); + + for (int q = 0; q < 150; ++q) { + const float x = X(rng), y = Y(rng); + const auto hs = heights_at(nm, x, y); + float z = Z(rng); + const bool exact = !hs.empty() && on_surface(rng); + if (exact) { + z = hs[std::uniform_int_distribution(0, hs.size() - 1)(rng)]; + } + + std::size_t s; NavCelId cid; Eigen::Vector3f bary, hit; + const bool ok = nm.locate_navcel({x, y, z}, s, cid, bary, &hit); + if (hs.empty()) { + EXPECT_FALSE(ok) << "no surface under (" << x << "," << y << ")"; + continue; + } + + // Nearest surface in |dz|; skip ambiguous ties between distinct heights. + std::vector dz; + for (float h : hs) { + dz.push_back(std::fabs(h - z)); + } + std::sort(dz.begin(), dz.end()); + // Same height twice is a shared edge; a near-equal different one is a tie. + const auto other = std::find_if( + dz.begin(), dz.end(), [&](float v) {return v - dz[0] > 1e-5f;}); + if (other != dz.end() && *other - dz[0] < 1e-3f) {continue;} + + ASSERT_TRUE(ok) << "(" << x << "," << y << "," << z << ")"; + EXPECT_NEAR(std::fabs(hit.z() - z), dz[0], 2e-4f) << "(" << x << "," << y << "," << z << ")"; + EXPECT_EQ(nm.surface_of(cid), s); + EXPECT_TRUE(covers_xy(nm, cid, x, y)); + ++checked; + on_surface_checked += exact; + } + } + EXPECT_GT(checked, 500); + EXPECT_GT(on_surface_checked, 100); +} + +TEST(NavMap_Properties, LocateWithHintIsConsistent) +{ + std::mt19937 rng(99); + for (int map_i = 0; map_i < 6; ++map_i) { + const NavMap nm = random_map(rng, 2 + map_i % 3); + const Bounds b = bounds_of(nm); + std::uniform_real_distribution X(b.min.x(), b.max.x()); + std::uniform_real_distribution Y(b.min.y(), b.max.y()); + std::uniform_int_distribution C(0, static_cast(nm.navcels.size() - 1)); + + for (int q = 0; q < 200; ++q) { + const float x = X(rng), y = Y(rng); + const auto hs = heights_at(nm, x, y); + if (hs.empty()) {continue;} + const float z = hs[0] + 0.05f; + + std::size_t s0; NavCelId c0; Eigen::Vector3f bary, hit0; + ASSERT_TRUE(nm.locate_navcel({x, y, z}, s0, c0, bary, &hit0)); + + // Hint on the right cell: same answer. + NavMap::LocateOpts opts; + opts.hint_cid = c0; + std::size_t s1; NavCelId c1; Eigen::Vector3f hit1; + ASSERT_TRUE(nm.locate_navcel({x, y, z}, s1, c1, bary, &hit1, opts)); + EXPECT_EQ(s1, s0); + EXPECT_NEAR(hit1.z(), hit0.z(), 2e-4f); + + // Random hint: either the nearest surface or a cell within height_eps of the point. + opts.hint_cid = C(rng); + std::size_t s2; NavCelId c2; Eigen::Vector3f hit2; + ASSERT_TRUE(nm.locate_navcel({x, y, z}, s2, c2, bary, &hit2, opts)); + EXPECT_EQ(nm.surface_of(c2), s2); + const auto & n = nm.navcels[c2].normal; + const float plane_dist = std::fabs(n.dot(Eigen::Vector3f(x, y, z) - vtx(nm, c2, 0))); + EXPECT_TRUE( + std::fabs(hit2.z() - hit0.z()) < 2e-4f || plane_dist <= opts.height_eps + 1e-4f) + << "hint " << *opts.hint_cid << " plane_dist " << plane_dist; + } + } +} + +TEST(NavMap_Properties, RaycastMatchesBruteForce) +{ + std::mt19937 rng(7); + std::normal_distribution N(0.0f, 1.0f); + int hits = 0, misses = 0; + for (int map_i = 0; map_i < 8; ++map_i) { + const NavMap nm = random_map(rng, 1 + map_i % 4); + const Bounds b = bounds_of(nm); + std::uniform_real_distribution X(b.min.x() - 2.0f, b.max.x() + 2.0f); + std::uniform_real_distribution Y(b.min.y() - 2.0f, b.max.y() + 2.0f); + std::uniform_real_distribution Z(b.min.z() - 2.0f, b.max.z() + 2.0f); + + std::vector rays; + for (int q = 0; q < 150; ++q) { + Eigen::Vector3f d(N(rng), N(rng), N(rng)); + if (d.norm() < 1e-3f) {continue;} + rays.push_back({{X(rng), Y(rng), Z(rng)}, d.normalized()}); + } + + std::vector many_closest, many_first; + nm.raycast_many(rays, many_closest, false); + nm.raycast_many(rays, many_first, true); + ASSERT_EQ(many_closest.size(), rays.size()); + + for (std::size_t i = 0; i < rays.size(); ++i) { + const auto ref = brute_raycast(nm, rays[i].o, rays[i].d); + NavCelId cid; float t; Eigen::Vector3f p; + const bool hit = nm.raycast(rays[i].o, rays[i].d, cid, t, p); + ASSERT_EQ(hit, ref.has_value()) << "ray " << i; + EXPECT_EQ(many_closest[i].hit, hit); + EXPECT_EQ(many_first[i].hit, hit); + if (!hit) {++misses; continue;} + ++hits; + EXPECT_NEAR(t, *ref, 1e-4f); + EXPECT_LT((p - (rays[i].o + t * rays[i].d)).norm(), 1e-4f); + EXPECT_NEAR(many_closest[i].t, *ref, 1e-4f); + EXPECT_EQ(nm.surface_of(many_closest[i].cid), many_closest[i].surface); + EXPECT_GE(many_first[i].t, *ref - 1e-4f); + } + } + EXPECT_GT(hits, 100); + EXPECT_GT(misses, 100); +} + +TEST(NavMap_Properties, ClosestMatchesBruteForce) +{ + std::mt19937 rng(42); + for (int map_i = 0; map_i < 8; ++map_i) { + const NavMap nm = random_map(rng, 1 + map_i % 4); + const Bounds b = bounds_of(nm); + std::uniform_real_distribution X(b.min.x() - 3.0f, b.max.x() + 3.0f); + std::uniform_real_distribution Y(b.min.y() - 3.0f, b.max.y() + 3.0f); + std::uniform_real_distribution Z(b.min.z() - 3.0f, b.max.z() + 3.0f); + for (int q = 0; q < 100; ++q) { + const Eigen::Vector3f p(X(rng), Y(rng), Z(rng)); + std::size_t s; NavCelId cid; Eigen::Vector3f cp; float sq; + ASSERT_TRUE(nm.closest_navcel(p, s, cid, cp, sq)); + EXPECT_NEAR(sq, brute_closest_sq(nm, p), 1e-4f * std::max(1.0f, sq)); + EXPECT_NEAR((cp - p).squaredNorm(), sq, 1e-3f * std::max(1.0f, sq)); + EXPECT_EQ(nm.surface_of(cid), s); + + // Restricted search never beats the unrestricted one. + std::size_t s_r; NavCelId c_r; Eigen::Vector3f cp_r; float sq_r; + ASSERT_TRUE(nm.closest_navcel(p, s_r, c_r, cp_r, sq_r, static_cast(s))); + EXPECT_EQ(s_r, s); + EXPECT_NEAR(sq_r, sq, 1e-4f * std::max(1.0f, sq)); + } + } +} + +TEST(NavMap_Properties, AdjacencyIsSymmetricAndSharesAnEdge) +{ + std::mt19937 rng(5); + for (int map_i = 0; map_i < 6; ++map_i) { + const NavMap nm = random_map(rng, 1 + map_i % 4); + for (NavCelId cid = 0; cid < nm.navcels.size(); ++cid) { + for (NavCelId n : nm.navcel_neighbors(cid)) { + ASSERT_LT(n, nm.navcels.size()); + const auto back = nm.navcel_neighbors(n); + EXPECT_NE(std::find(back.begin(), back.end(), cid), back.end()); + int shared = 0; + for (auto a : nm.navcels[cid].v) { + for (auto c : nm.navcels[n].v) { + shared += a == c; + } + } + EXPECT_EQ(shared, 2); + EXPECT_EQ(nm.surface_of(n), nm.surface_of(cid)); + } + } + } +} + +TEST(NavMap_Robustness, NonFiniteQueriesFailCleanly) +{ + std::mt19937 rng(3); + const NavMap nm = random_map(rng, 2); + const Eigen::Vector3f bad[] = { + {kNaN, 0.0f, 0.0f}, {0.0f, kNaN, 0.0f}, {0.0f, 0.0f, kNaN}, + {kInf, 0.0f, 0.0f}, {0.0f, -kInf, 0.0f}, {0.0f, 0.0f, kInf}}; + for (const auto & p : bad) { + std::size_t s; NavCelId cid; Eigen::Vector3f bary, hit; + EXPECT_FALSE(nm.locate_navcel(p, s, cid, bary, &hit)); + EXPECT_TRUE(std::isnan(nm.sample_layer_at("missing", p))); + } + + NavCelId cid; float t; Eigen::Vector3f hit; + EXPECT_FALSE(nm.raycast({kNaN, 0.0f, 0.0f}, {0.0f, 0.0f, -1.0f}, cid, t, hit)); + EXPECT_FALSE(nm.raycast({0.0f, 0.0f, 10.0f}, {kNaN, kNaN, kNaN}, cid, t, hit)); + EXPECT_FALSE(nm.raycast({0.0f, 0.0f, 10.0f}, {0.0f, 0.0f, 0.0f}, cid, t, hit)); +} + +TEST(NavMap_Robustness, EmptyMapAndEmptySurfaces) +{ + NavMap empty; + empty.rebuild_geometry_accels(); + std::size_t s; NavCelId cid; Eigen::Vector3f bary, hit, cp; float t, sq; + EXPECT_FALSE(empty.locate_navcel({0.0f, 0.0f, 0.0f}, s, cid, bary, &hit)); + EXPECT_FALSE(empty.raycast({0.0f, 0.0f, 1.0f}, {0.0f, 0.0f, -1.0f}, cid, t, hit)); + EXPECT_FALSE(empty.closest_navcel({0.0f, 0.0f, 0.0f}, s, cid, cp, sq)); + + // Empty surfaces before and after a real one. + NavMap nm; + nm.create_surface("empty_a"); + const std::size_t real = nm.create_surface("map"); + nm.create_surface("empty_b"); + const auto a = nm.add_vertex({0.0f, 0.0f, 0.0f}); + const auto b = nm.add_vertex({1.0f, 0.0f, 0.0f}); + const auto c = nm.add_vertex({0.0f, 1.0f, 0.0f}); + nm.add_navcel_to_surface(real, nm.add_navcel(a, b, c)); + nm.rebuild_geometry_accels(); + ASSERT_TRUE(nm.locate_navcel({0.2f, 0.2f, 0.0f}, s, cid, bary, &hit)); + EXPECT_EQ(s, real); + ASSERT_TRUE(nm.closest_navcel({5.0f, 5.0f, 0.0f}, s, cid, cp, sq)); + EXPECT_EQ(s, real); +} + +TEST(NavMap_Robustness, DegenerateTrianglesAreIgnored) +{ + NavMap nm; + const std::size_t s0 = nm.create_surface("map"); + const auto a = nm.add_vertex({0.0f, 0.0f, 0.0f}); + const auto b = nm.add_vertex({1.0f, 0.0f, 0.0f}); + const auto c = nm.add_vertex({0.0f, 1.0f, 0.0f}); + const auto d = nm.add_vertex({2.0f, 0.0f, 0.0f}); + nm.add_navcel_to_surface(s0, nm.add_navcel(a, a, b)); // repeated vertex + nm.add_navcel_to_surface(s0, nm.add_navcel(a, b, d)); // collinear + const NavCelId good = nm.add_navcel(a, b, c); + nm.add_navcel_to_surface(s0, good); + nm.rebuild_geometry_accels(); + + for (const auto & cel : nm.navcels) { + EXPECT_TRUE(cel.normal.allFinite()); + EXPECT_TRUE(std::isfinite(cel.area)); + } + std::size_t s; NavCelId cid; Eigen::Vector3f bary, hit; + ASSERT_TRUE(nm.locate_navcel({0.2f, 0.2f, 0.5f}, s, cid, bary, &hit)); + EXPECT_EQ(cid, good); +} + +TEST(NavMap_Robustness, QueriesOnVerticesAndEdges) +{ + std::mt19937 rng(11); + const NavMap nm = random_map(rng, 3); + for (NavCelId cid = 0; cid < nm.navcels.size(); cid += 7) { + const Eigen::Vector3f pa = vtx(nm, cid, 0), pb = vtx(nm, cid, 1); + for (const Eigen::Vector3f & p : {pa, Eigen::Vector3f(0.5f * (pa + pb))}) { + std::size_t s; NavCelId c; Eigen::Vector3f bary, hit; + ASSERT_TRUE(nm.locate_navcel(p, s, c, bary, &hit)); + EXPECT_NEAR(hit.z(), p.z(), 2e-4f); + EXPECT_EQ(nm.surface_of(c), s); + } + } +} + +TEST(NavMap_Robustness, EveryCellAsHintGivesTheSameFloor) +{ + NavMap nm; + for (float z : {0.0f, 3.0f}) { + const std::size_t s = nm.create_surface("map"); + const auto b = static_cast(nm.positions.size()); + nm.add_vertex({0, 0, z}); nm.add_vertex({4, 0, z}); + nm.add_vertex({4, 4, z}); nm.add_vertex({0, 4, z}); + nm.add_navcel_to_surface(s, nm.add_navcel(b, b + 1, b + 2)); + nm.add_navcel_to_surface(s, nm.add_navcel(b, b + 2, b + 3)); + } + nm.rebuild_geometry_accels(); + + for (float z : {0.0f, 0.2f, 3.0f, 3.2f}) { + const std::size_t expected = z < 1.5f ? 0u : 1u; + for (NavCelId h = 0; h < nm.navcels.size(); ++h) { + NavMap::LocateOpts opts; + opts.hint_cid = h; + std::size_t s; NavCelId c; Eigen::Vector3f bary, hit; + ASSERT_TRUE(nm.locate_navcel({1.0f, 2.5f, z}, s, c, bary, &hit, opts)); + EXPECT_EQ(s, expected) << "z=" << z << " hint=" << h; + } + } +} + +TEST(NavMap_Robustness, CellInTwoSurfacesReportsTheFirstOwner) +{ + NavMap nm; + const std::size_t s0 = nm.create_surface("a"); + const std::size_t s1 = nm.create_surface("b"); + const auto a = nm.add_vertex({0.0f, 0.0f, 0.0f}); + const auto b = nm.add_vertex({1.0f, 0.0f, 0.0f}); + const auto c = nm.add_vertex({0.0f, 1.0f, 0.0f}); + const NavCelId cid = nm.add_navcel(a, b, c); + nm.add_navcel_to_surface(s0, cid); + nm.add_navcel_to_surface(s1, cid); + EXPECT_EQ(nm.surface_of(cid), s0); // linear fallback (no rebuild yet) + nm.rebuild_geometry_accels(); + EXPECT_EQ(nm.surface_of(cid), s0); // index +} + +TEST(NavMap_Robustness, RebuildIsIdempotentAndTracksEdits) +{ + std::mt19937 rng(21); + NavMap nm = random_map(rng, 2); + const auto before = nm.navcels.size(); + nm.rebuild_geometry_accels(); + nm.rebuild_geometry_accels(); + for (NavCelId cid = 0; cid < before; ++cid) { + for (std::size_t s = 0; s < nm.surfaces.size(); ++s) { + const auto & ids = nm.surfaces[s].navcels; + if (std::find(ids.begin(), ids.end(), cid) != ids.end()) { + ASSERT_EQ(nm.surface_of(cid), s); + } + } + } + + // Remove the first surface and rebuild: indices shift accordingly. + const auto moved = nm.surfaces[1].navcels.front(); + nm.remove_surface(0); + nm.rebuild_geometry_accels(); + EXPECT_EQ(nm.surface_of(moved), 0u); + EXPECT_EQ(nm.surface_of(nm.surfaces.size() > 0 ? nm.surfaces[0].navcels.back() : 0), 0u); +}