Add NavMap::distance_field

navmap_core/include/navmap_core/NavMap.hpp

@@ -699,6 +699,23 @@ class NavMap
   /// \brief Return up to 3 neighbor cell ids (skips invalid entries).
   std::vector<NavCelId> navcel_neighbors(NavCelId cid) const;
 
+  /**
+   * \brief Distance (m, 3D) from each NavCel centroid to the nearest seed.
+   *
+   * Seeds are the centroids of \p seed_cells and, if \p from_boundary, the open edges of the
+   * mesh (edges without a neighbor: walls, drops, unmapped space). The nearest seed is
+   * propagated over adjacency and the distance is measured straight to it, so it never jumps
+   * between disconnected surfaces (e.g. stacked floors).
+   *
+   * \param seed_cells    Seed NavCels (out-of-range ids are ignored).
+   * \param from_boundary Also seed the open edges.
+   * \param max_distance  Stop beyond this distance; farther or unreachable cells get +inf.
+   * \return One distance per NavCel.
+   */
+  std::vector<float> distance_field(
+    const std::vector<NavCelId> & seed_cells, bool from_boundary,
+    float max_distance = std::numeric_limits<float>::infinity()) const;
+
   /// \brief Value returned by \ref surface_of() for a NavCel in no surface.
   static constexpr std::size_t kNoSurface = std::numeric_limits<std::size_t>::max();
 

navmap_core/src/navmap_core/NavMap.cpp

@@ -16,6 +16,7 @@
 
 #include "navmap_core/NavMap.hpp"
 #include <array>
+#include <queue>
 #include <vector>
 #include <algorithm>
 #include <functional>
@@ -1098,6 +1099,80 @@ bool NavMap::locate_navcel(
   return true;
 }
 
+// ---- Distance field ----
+
+namespace
+{
+inline float point_segment_distance(const Vec3 & p, const Vec3 & a, const Vec3 & b)
+{
+  const Vec3 ab = b - a;
+  const float len2 = ab.squaredNorm();
+  const float t = len2 > 0.0f ? std::clamp((p - a).dot(ab) / len2, 0.0f, 1.0f) : 0.0f;
+  return (p - (a + t * ab)).norm();
+}
+}  // namespace
+
+std::vector<float> NavMap::distance_field(
+  const std::vector<NavCelId> & seed_cells, bool from_boundary, float max_distance) const
+{
+  constexpr float kInf = std::numeric_limits<float>::infinity();
+  constexpr auto kNone = std::numeric_limits<NavCelId>::max();
+  const std::size_t n = navcels.size();
+  std::vector<float> dist(n, kInf);
+  if (n == 0) {return dist;}
+
+  std::vector<Vec3> centroid(n);
+  for (NavCelId c = 0; c < n; ++c) {
+    centroid[c] = navcel_centroid(c);
+  }
+
+  // A seed is a point (a == b) or a segment.
+  struct Seed {Vec3 a, b;};
+  std::vector<Seed> seeds;
+  std::vector<std::uint32_t> nearest(n, std::numeric_limits<std::uint32_t>::max());
+  using Item = std::pair<float, NavCelId>;
+  std::priority_queue<Item, std::vector<Item>, std::greater<Item>> open;
+
+  auto offer = [&](NavCelId c, std::uint32_t seed, float d) {
+      if (d < dist[c] && d <= max_distance) {
+        dist[c] = d;
+        nearest[c] = seed;
+        open.push({d, c});
+      }
+    };
+  auto distance_to = [&](NavCelId c, std::uint32_t seed) {
+      return point_segment_distance(centroid[c], seeds[seed].a, seeds[seed].b);
+    };
+
+  for (NavCelId c : seed_cells) {
+    if (c >= n) {continue;}
+    seeds.push_back({centroid[c], centroid[c]});
+    offer(c, static_cast<std::uint32_t>(seeds.size() - 1), 0.0f);
+  }
+  if (from_boundary) {
+    for (NavCelId c = 0; c < n; ++c) {
+      for (int e = 0; e < 3; ++e) {
+        if (navcels[c].neighbor[e] != kNone) {continue;}
+        seeds.push_back({positions.at(navcels[c].v[e]), positions.at(navcels[c].v[(e + 1) % 3])});
+        const auto seed = static_cast<std::uint32_t>(seeds.size() - 1);
+        offer(c, seed, distance_to(c, seed));
+      }
+    }
+  }
+
+  while (!open.empty()) {
+    const auto [d, u] = open.top();
+    open.pop();
+    if (d > dist[u]) {continue;}
+    for (int e = 0; e < 3; ++e) {
+      const NavCelId v = navcels[u].neighbor[e];
+      if (v == kNone || v >= n) {continue;}
+      offer(v, nearest[u], distance_to(v, nearest[u]));
+    }
+  }
+  return dist;
+}
+
 // ---- Layers ----
 bool NavMap::has_layer(const std::string & name) const
 {

navmap_core/tests/CMakeLists.txt

@@ -47,3 +47,8 @@ ament_add_gtest(test_adjacency test_adjacency.cpp)
 target_link_libraries(test_adjacency
   ${PROJECT_NAME}
 )
+
+ament_add_gtest(test_distance_field test_distance_field.cpp)
+target_link_libraries(test_distance_field
+  ${PROJECT_NAME}
+)

navmap_core/tests/test_distance_field.cpp

@@ -0,0 +1,255 @@
+// 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 <gtest/gtest.h>
+
+#include <algorithm>
+#include <cmath>
+#include <cstdint>
+#include <functional>
+#include <limits>
+#include <random>
+#include <vector>
+
+#include "navmap_core/NavMap.hpp"
+
+using navmap::NavCelId;
+using navmap::NavMap;
+
+namespace
+{
+
+constexpr float kInf = std::numeric_limits<float>::infinity();
+
+// nx x ny cells of `res` m at z = f(x, y), skipping cells where skip(i, j); returns the surface.
+std::size_t add_grid(
+  NavMap & nm, int nx, int ny, float res, const std::function<float(float, float)> & f,
+  const std::function<bool(int, int)> & skip = nullptr)
+{
+  const std::size_t s = nm.create_surface("map");
+  const auto base = static_cast<uint32_t>(nm.positions.size());
+  for (int j = 0; j <= ny; ++j) {
+    for (int i = 0; i <= nx; ++i) {
+      nm.add_vertex({i * res, j * res, f(i * res, j * res)});
+    }
+  }
+  auto v = [&](int i, int j) {return base + static_cast<uint32_t>(j * (nx + 1) + i);};
+  for (int j = 0; j < ny; ++j) {
+    for (int i = 0; i < nx; ++i) {
+      if (skip && skip(i, j)) {continue;}
+      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;
+}
+
+float point_segment(const Eigen::Vector3f & p, const Eigen::Vector3f & a, const Eigen::Vector3f & b)
+{
+  const Eigen::Vector3f ab = b - a;
+  const float t = std::clamp((p - a).dot(ab) / ab.squaredNorm(), 0.0f, 1.0f);
+  return (p - (a + t * ab)).norm();
+}
+
+// Exact nearest distance over all seeds (no connectivity).
+std::vector<float> brute(const NavMap & nm, const std::vector<NavCelId> & seeds, bool boundary)
+{
+  std::vector<float> d(nm.navcels.size(), kInf);
+  for (NavCelId c = 0; c < nm.navcels.size(); ++c) {
+    const auto p = nm.navcel_centroid(c);
+    for (NavCelId s : seeds) {
+      d[c] = std::min(d[c], (p - nm.navcel_centroid(s)).norm());
+    }
+    if (!boundary) {continue;}
+    for (NavCelId b = 0; b < nm.navcels.size(); ++b) {
+      for (int e = 0; e < 3; ++e) {
+        if (nm.navcels[b].neighbor[e] != std::numeric_limits<NavCelId>::max()) {continue;}
+        d[c] = std::min(
+          d[c], point_segment(
+            p, nm.positions.at(nm.navcels[b].v[e]), nm.positions.at(nm.navcels[b].v[(e + 1) % 3])));
+      }
+    }
+  }
+  return d;
+}
+
+NavCelId cell_near(const NavMap & nm, float x, float y, float z)
+{
+  std::size_t s; NavCelId c; Eigen::Vector3f bary, hit;
+  EXPECT_TRUE(nm.locate_navcel({x, y, z}, s, c, bary, &hit));
+  return c;
+}
+
+auto flat(float h) {return [h](float, float) {return h;};}
+
+}  // namespace
+
+TEST(NavMap_DistanceField, BoundaryOfARectangleIsExact)
+{
+  NavMap nm;
+  add_grid(nm, 20, 12, 0.5f, flat(0.0f));
+  nm.rebuild_geometry_accels();
+  const auto d = nm.distance_field({}, true);
+  const auto ref = brute(nm, {}, true);
+  for (NavCelId c = 0; c < nm.navcels.size(); ++c) {
+    const auto p = nm.navcel_centroid(c);
+    const float expected = std::min({p.x(), 10.0f - p.x(), p.y(), 6.0f - p.y()});
+    EXPECT_NEAR(d[c], expected, 1e-4f) << c;
+    EXPECT_NEAR(d[c], ref[c], 1e-4f) << c;
+  }
+}
+
+TEST(NavMap_DistanceField, SeedCellsGiveStraightLineDistance)
+{
+  NavMap nm;
+  add_grid(nm, 20, 20, 0.5f, flat(0.0f));
+  nm.rebuild_geometry_accels();
+  const std::vector<NavCelId> seeds{cell_near(nm, 2.1f, 2.2f, 0.0f),
+    cell_near(nm, 7.3f, 6.9f, 0.0f)};
+  const auto d = nm.distance_field(seeds, false);
+  const auto ref = brute(nm, seeds, false);
+  for (NavCelId c = 0; c < nm.navcels.size(); ++c) {
+    EXPECT_NEAR(d[c], ref[c], 1e-4f) << c;
+  }
+  for (NavCelId s : seeds) {
+    EXPECT_FLOAT_EQ(d[s], 0.0f);
+                                                        }
+
+  // Seeds and boundary together: the nearest of both.
+  const auto both = nm.distance_field(seeds, true);
+  const auto ref_both = brute(nm, seeds, true);
+  for (NavCelId c = 0; c < nm.navcels.size(); ++c) {
+    EXPECT_NEAR(both[c], ref_both[c], 1e-4f) << c;
+  }
+}
+
+TEST(NavMap_DistanceField, MaxDistanceCutsOff)
+{
+  NavMap nm;
+  add_grid(nm, 20, 20, 0.5f, flat(0.0f));
+  nm.rebuild_geometry_accels();
+  const NavCelId seed = cell_near(nm, 5.1f, 5.2f, 0.0f);
+  const auto full = nm.distance_field({seed}, false);
+  const auto cut = nm.distance_field({seed}, false, 2.0f);
+  for (NavCelId c = 0; c < nm.navcels.size(); ++c) {
+    if (full[c] <= 2.0f) {
+      EXPECT_NEAR(cut[c], full[c], 1e-5f);
+    } else {
+      EXPECT_EQ(cut[c], kInf);
+    }
+  }
+}
+
+TEST(NavMap_DistanceField, StackedFloorsDoNotSeeEachOther)
+{
+  NavMap nm;
+  add_grid(nm, 10, 10, 0.5f, flat(0.0f));
+  add_grid(nm, 10, 10, 0.5f, flat(2.5f));
+  nm.rebuild_geometry_accels();
+  const NavCelId low = cell_near(nm, 2.6f, 2.6f, 0.0f);
+  const auto d = nm.distance_field({low}, false);
+  for (auto c : nm.surfaces[1].navcels) {
+    EXPECT_EQ(d[c], kInf);
+                                                               }
+  for (auto c : nm.surfaces[0].navcels) {
+    EXPECT_LT(d[c], kInf);
+                                                               }
+
+  // Boundary of the upper floor only reaches the upper floor's own cells.
+  const NavCelId high = cell_near(nm, 2.6f, 2.6f, 2.5f);
+  const auto b = nm.distance_field({}, true);
+  EXPECT_NEAR(b[high], b[low], 1e-4f);  // same geometry, same distance to its own border
+}
+
+TEST(NavMap_DistanceField, SlopeDistanceIs3D)
+{
+  // 30 deg ramp along x: the distance to a seed grows with the slope length, not the XY one.
+  const float k = std::tan(30.0f * static_cast<float>(M_PI) / 180.0f);
+  NavMap nm;
+  add_grid(nm, 20, 4, 0.5f, [k](float x, float) {return k * x;});
+  nm.rebuild_geometry_accels();
+  const NavCelId seed = cell_near(nm, 0.4f, 1.0f, 0.2f);
+  const auto d = nm.distance_field({seed}, false);
+  const auto ref = brute(nm, {seed}, false);
+  const NavCelId far = cell_near(nm, 8.4f, 1.0f, k * 8.4f);
+  EXPECT_NEAR(d[far], ref[far], 1e-4f);
+  const auto pf = nm.navcel_centroid(far), ps = nm.navcel_centroid(seed);
+  EXPECT_GT(d[far], (pf.head<2>() - ps.head<2>()).norm() * 1.1f);
+}
+
+TEST(NavMap_DistanceField, InteriorHoleEdgesAreBoundary)
+{
+  NavMap nm;
+  auto hole = [](int i, int j) {return i >= 9 && i <= 10 && j >= 9 && j <= 10;};
+  add_grid(nm, 20, 20, 0.5f, flat(0.0f), hole);
+  nm.rebuild_geometry_accels();
+  const auto d = nm.distance_field({}, true);
+  const auto ref = brute(nm, {}, true);
+  for (NavCelId c = 0; c < nm.navcels.size(); ++c) {
+    EXPECT_NEAR(d[c], ref[c], 1e-4f) << c;
+                                                                                          }
+  // Next to the 1x1 m hole (x, y in [4.5, 5.5]).
+  EXPECT_LT(d[cell_near(nm, 4.2f, 5.0f, 0.0f)], 0.31f);
+}
+
+TEST(NavMap_DistanceField, NeverBelowTheTrueDistanceOnIrregularTerrain)
+{
+  std::mt19937 rng(17);
+  std::uniform_real_distribution<float> U(0.0f, 1.0f);
+  for (int m = 0; m < 4; ++m) {
+    const float a = 0.3f * U(rng), b = 0.3f * U(rng);
+    NavMap nm;
+    add_grid(
+      nm, 30, 25, 0.4f, [&](float x, float y) {return a * std::sin(x) + b * std::cos(0.7f * y);},
+      [&](int, int) {return U(rng) < 0.05f;});  // scattered holes
+    nm.rebuild_geometry_accels();
+    std::vector<NavCelId> seeds;
+    for (int k = 0; k < 5; ++k) {
+      seeds.push_back(static_cast<NavCelId>(U(rng) * static_cast<float>(nm.navcels.size() - 1)));
+    }
+    const auto d = nm.distance_field(seeds, true);
+    const auto ref = brute(nm, seeds, true);
+    double sum = 0.0;
+    int n = 0;
+    for (NavCelId c = 0; c < nm.navcels.size(); ++c) {
+      if (!std::isfinite(d[c])) {continue;}
+      EXPECT_GE(d[c], ref[c] - 1e-4f) << "map " << m << " cell " << c;
+      sum += d[c] - ref[c];
+      ++n;
+    }
+    EXPECT_LT(sum / n, 0.02) << "map " << m;  // propagation is close to the exact distance
+  }
+}
+
+TEST(NavMap_DistanceField, DegenerateInputs)
+{
+  NavMap empty;
+  empty.rebuild_geometry_accels();
+  EXPECT_TRUE(empty.distance_field({0, 5}, true).empty());
+
+  NavMap nm;
+  add_grid(nm, 4, 4, 0.5f, flat(0.0f));
+  nm.rebuild_geometry_accels();
+  for (float v : nm.distance_field({}, false)) {
+    EXPECT_EQ(v, kInf);
+                                                                   }
+  const auto d = nm.distance_field({9999}, false);  // out of range: ignored
+  for (float v : d) {
+    EXPECT_EQ(v, kInf);
+                                        }
+  const auto z = nm.distance_field({0}, false, 0.0f);  // only the seed itself
+  EXPECT_FLOAT_EQ(z[0], 0.0f);
+  EXPECT_EQ(std::count_if(z.begin(), z.end(), [](float v) {return v < kInf;}), 1);
+}