NavMap.hpp

Go to the documentation of this file.

// Copyright 2025 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.
 
#ifndef NAVMAP_CORE__NAVMAP_HPP_
#define NAVMAP_CORE__NAVMAP_HPP_

 
#include <vector>
#include <memory>
#include <unordered_map>
#include <optional>
#include <string>
#include <array>
#include <limits>
#include <cmath>
#include <cstdint>
#include <type_traits>
#include <deque>
#include <algorithm>
#include <utility>
 
#include "Eigen/Core"
#include "navmap_core/Geometry.hpp"
 
namespace navmap
{

using PointId = uint32_t;
using NavCelId = uint32_t;
 
// -----------------------------------------------------------------------------
// Core data arrays
// -----------------------------------------------------------------------------

struct Positions
{
  std::vector<float> x;  
  std::vector<float> y;  
  std::vector<float> z;  

  inline size_t size() const {return x.size();}

  inline Eigen::Vector3f at(PointId id) const
  {
    return {x[id], y[id], z[id]};
  }
};

struct Colors
{
  std::vector<uint8_t> r;  
  std::vector<uint8_t> g;  
  std::vector<uint8_t> b;  
  std::vector<uint8_t> a;  
};
 
// -----------------------------------------------------------------------------
// Layers (dynamic per-NavCel attributes)
// -----------------------------------------------------------------------------

enum class LayerType : uint8_t { U8 = 0, F32 = 1, F64 = 2, I16 = 3 };

struct LayerViewBase
{
  virtual ~LayerViewBase() = default;

  virtual LayerType type() const = 0;

  virtual const std::string & name() const = 0;

  virtual size_t size() const = 0;

  virtual std::uint64_t content_hash() const = 0;

  void mark_dirty() const noexcept {hash_dirty_ = true;}

  virtual std::shared_ptr<LayerViewBase> clone() const = 0;
 
  // ---- Change tracking (for incremental consumers: inflation, publication...) ----
  // Cells written since the last clear_changes(): recorded by NavMap::layer_set(), OccupancyLayer
  // and mark_changed(). Bulk writes (set_data, layer_clear, copies) mark the whole layer, and so
  // does creating it. Direct writes through data() must call mark_changed() themselves.

  void mark_changed(NavCelId cid) const
  {
    hash_dirty_ = true;
    if (all_changed_) {return;}
    const std::size_t word = cid / 64;
    if (changed_bits_.size() <= word) {
      changed_bits_.resize(std::max(word + 1, (size() + 63) / 64));
    }
    const std::uint64_t bit = std::uint64_t{1} << (cid % 64);
    if (!(changed_bits_[word] & bit)) {
      changed_bits_[word] |= bit;
      changed_.push_back(cid);
    }
  }

  void mark_all_changed() const noexcept {hash_dirty_ = true; all_changed_ = true;}

  bool all_changed() const noexcept {return all_changed_;}

  const std::vector<NavCelId> & changed() const noexcept {return changed_;}
 
  bool has_changes() const noexcept {return all_changed_ || !changed_.empty();}

  void clear_changes() const
  {
    for (const NavCelId c : changed_) {changed_bits_[c / 64] &= ~(std::uint64_t{1} << (c % 64));}
    changed_.clear();
    all_changed_ = false;
  }
 
protected:
  mutable bool hash_dirty_{true};
  mutable std::uint64_t hash_cache_{0};
  mutable bool all_changed_{true};               // a new layer is all new
  mutable std::vector<NavCelId> changed_;
  mutable std::vector<std::uint64_t> changed_bits_;  // allocated on the first single change
};

template<typename T>
struct LayerView : LayerViewBase
{
  std::string name_;     
  std::vector<T> data_;  
  LayerType type_;       

  LayerView(std::string name, size_t nitems, LayerType t)
  : name_(std::move(name)), data_(nitems), type_(t) {}
 
  LayerType type() const override {return type_;}
  const std::string & name() const override {return name_;}
  size_t size() const override {return data_.size();}

  T & operator[](NavCelId cid) {return data_[cid];}
  const T & operator[](NavCelId cid) const {return data_[cid];}

  std::vector<T> & data() {return data_;}
  const std::vector<T> & data() const {return data_;}

  std::vector<T> & mutable_data() const
  {
    hash_dirty_ = true; return const_cast<std::vector<T> &>(data_);
  }
  void set_data(const std::vector<T> & v) {data_ = v; mark_all_changed();}
  std::uint64_t content_hash() const override;
 
  std::shared_ptr<LayerViewBase> clone() const override
  {
    return std::make_shared<LayerView<T>>(*this);
  }

};

namespace detail
{
inline std::uint64_t fnv1a64_bytes(
  const void * data, std::size_t n,
  std::uint64_t seed = 1469598103934665603ULL)
{
  const auto * p = static_cast<const std::uint8_t *>(data);
  std::uint64_t h = seed;
  for (std::size_t i = 0; i < n; ++i) {
    h ^= p[i]; h *= 1099511628211ULL;
  }
  return h;
}
}  // namespace detail
 
template<typename T>
std::uint64_t LayerView<T>::content_hash() const
{
  if (!hash_dirty_) {return hash_cache_;}
  const std::size_t n = data_.size();
  std::uint64_t h = navmap::detail::fnv1a64_bytes(&n, sizeof(n));
  if (n) {
    static_assert(
      std::is_trivially_copyable<T>::value,
      "LayerView<T> requires trivially copyable T.");
    h = navmap::detail::fnv1a64_bytes(data_.data(), n * sizeof(T), h);
  }
  hash_cache_ = h;
  hash_dirty_ = false;
  return hash_cache_;
}

class LayerRegistry
{
public:
  LayerRegistry() = default;
  LayerRegistry(LayerRegistry &&) noexcept = default;
  LayerRegistry & operator=(LayerRegistry &&) noexcept = default;

  LayerRegistry(const LayerRegistry & other)
  {
    for (const auto & [name, view] : other.layers_) {
      layers_.emplace(name, view ? view->clone() : nullptr);
    }
  }
 
  LayerRegistry & operator=(const LayerRegistry & other)
  {
    if (this != &other) {
      LayerRegistry tmp(other);
      layers_.swap(tmp.layers_);
    }
    return *this;
  }

  template<typename T>
  std::shared_ptr<LayerView<T>> add_or_get(
    const std::string & name,
    size_t nitems,
    LayerType type)
  {
    auto it = layers_.find(name);
    if (it != layers_.end()) {
      return std::dynamic_pointer_cast<LayerView<T>>(it->second);
    }
    auto view = std::make_shared<LayerView<T>>(name, nitems, type);
    layers_[name] = view;
    return view;
  }

  std::shared_ptr<LayerViewBase> get(const std::string & name) const
  {
    auto it = layers_.find(name);
    return it == layers_.end() ? nullptr : it->second;
  }

  std::vector<std::string> list() const
  {
    std::vector<std::string> out;
    out.reserve(layers_.size());
    for (const auto & kv : layers_) {
      out.push_back(kv.first);
    }
    return out;
  }

  bool remove(const std::string & name)
  {
    return layers_.erase(name) > 0;
  }

  void resize_all(size_t nitems)
  {
    for (auto & kv : layers_) {
      if (auto v = std::dynamic_pointer_cast<LayerView<uint8_t>>(kv.second)) {
        v->data_.resize(nitems);
        kv.second->mark_all_changed();
        continue;
      }
      if (auto v = std::dynamic_pointer_cast<LayerView<float>>(kv.second)) {
        v->data_.resize(nitems);
        kv.second->mark_all_changed();
        continue;
      }
      if (auto v = std::dynamic_pointer_cast<LayerView<double>>(kv.second)) {
        v->data_.resize(nitems);
        kv.second->mark_all_changed();
        continue;
      }
      if (auto v = std::dynamic_pointer_cast<LayerView<int16_t>>(kv.second)) {
        v->data_.resize(nitems);
        kv.second->mark_all_changed();
        continue;
      }
    }
  }
 
private:
  std::unordered_map<std::string, std::shared_ptr<LayerViewBase>> layers_;
};

struct LayerMeta
{
  std::string description;   
  std::string unit;          
  bool per_cell{true};       
  std::optional<double> unknown;  
};
 
// -----------------------------------------------------------------------------
// Mesh cells and acceleration
// -----------------------------------------------------------------------------

struct NavCel
{
  PointId v[3]{0, 0, 0};                      
  Eigen::Vector3f normal{0.0f, 0.0f, 1.0f};   
  float area{0.0f};                            
  NavCelId neighbor[3]{                        
    std::numeric_limits<uint32_t>::max(),
    std::numeric_limits<uint32_t>::max(),
    std::numeric_limits<uint32_t>::max()
  };
  uint32_t layer_dirty_mask{0};               
};

struct BVHNode
{
  AABB box;          
  int left{-1};      
  int right{-1};     
  int start{0};      
  int count{0};      
  bool is_leaf() const {return count > 0;}
};

struct Surface
{
  std::string frame_id;           
  std::vector<NavCelId> navcels;  
  AABB aabb;                      
  std::vector<int> prim_indices;  
  std::vector<BVHNode> bvh;       
};
 
// -----------------------------------------------------------------------------
// Rays
// -----------------------------------------------------------------------------

struct Ray
{
  Eigen::Vector3f o;  
  Eigen::Vector3f d;  
};

struct RayHit
{
  bool hit{false};         
  size_t surface{0};       
  NavCelId cid{0};         
  float t{0.0f};           
  Eigen::Vector3f p;       
};
 
// -----------------------------------------------------------------------------
// Helpers (type → LayerType mapping)
// -----------------------------------------------------------------------------

template<typename T> inline constexpr LayerType layer_type_tag();
template<> inline constexpr LayerType layer_type_tag<uint8_t>() {return LayerType::U8;}
template<> inline constexpr LayerType layer_type_tag<float>() {return LayerType::F32;}
template<> inline constexpr LayerType layer_type_tag<double>() {return LayerType::F64;}
template<> inline constexpr LayerType layer_type_tag<int16_t>() {return LayerType::I16;}

enum class AreaShape { CIRCULAR, RECTANGULAR };
 
// -----------------------------------------------------------------------------
// NavMap: public API
// -----------------------------------------------------------------------------

enum class LinkType : uint8_t { GENERIC = 0, RAMP = 1, STAIRS = 2, ELEVATOR = 3, DOOR = 4 };

struct SurfaceLink
{
  NavCelId from{0};
  NavCelId to{0};
  LinkType type{LinkType::GENERIC};
  float length{0.0f};          
  bool bidirectional{true};    
};
 
class NavMap
{
public:
  Positions positions;                 
  std::optional<Colors> colors;        
  std::vector<NavCel> navcels;         
  std::vector<Surface> surfaces;       
  LayerRegistry layers;                
  std::unordered_map<std::string, LayerMeta> layer_meta;  

  NavMap();

  NavMap(const NavMap & other);

  void rebuild_geometry_accels();

  NavMap & operator=(const NavMap & other);

  NavMap & operator=(NavMap && other) noexcept;

  bool has_same_geometry(const NavMap & other) const;

  std::uint64_t geometry_id() const
  {
    ensure_geometry_fingerprint_();
    return geometry_fp_;
  }

  void build_adjacency();

  std::size_t nonmanifold_edges() const {return nonmanifold_edges_;}

  void mark_vertex_updated(PointId /*pid*/) {}

  std::array<NavCelId, 3> get_neighbors(NavCelId cid) const
  {
    return {navcels[cid].neighbor[0],
      navcels[cid].neighbor[1],
      navcels[cid].neighbor[2]};
  }

  bool raycast(
    const Eigen::Vector3f & o,
    const Eigen::Vector3f & d,
    NavCelId & hit_cid,
    float & t,
    Eigen::Vector3f & hit_pt) const;

  void raycast_many(
    const std::vector<Ray> & rays,
    std::vector<RayHit> & out,
    bool first_hit_only = true) const;
 
  // --- Per-NavCel layer value access ---

  template<typename T>
  T navcel_value(NavCelId cid, const LayerView<T> & layer) const
  {
    return layer[cid];
  }

  struct LocateOpts
  {
    std::optional<NavCelId> hint_cid;    
    std::optional<size_t> hint_surface;  
    float planar_eps = 1e-4f;            
    float height_eps = 0.50f;            
    bool use_downward_ray = true;        
  };

  bool locate_navcel(
    const Eigen::Vector3f & p_world,
    size_t & surface_idx,
    NavCelId & cid,
    Eigen::Vector3f & bary,
    Eigen::Vector3f * hit_pt) const;

  bool locate_navcel(
    const Eigen::Vector3f & p_world,
    size_t & surface_idx,
    NavCelId & cid,
    Eigen::Vector3f & bary,
    Eigen::Vector3f * hit_pt,
    const LocateOpts & opts) const;

  bool closest_navcel(
    const Eigen::Vector3f & p_world,
    size_t & surface_idx,
    NavCelId & cid,
    Eigen::Vector3f & closest_point,
    float & sqdist,
    int restrict_surface = -1) const;
 
  // ---- Convenience: construction & editing (additive, backward-compatible) ----

  std::size_t create_surface(std::string frame_id);

  Surface create_surface_obj(const std::string & frame_id) const;

  std::size_t add_surface(const Surface & s);

  std::size_t add_surface(Surface && s);

  bool remove_surface(std::size_t surface_index);

  uint32_t add_vertex(const Eigen::Vector3f & p);

  NavCelId add_navcel(uint32_t v0, uint32_t v1, uint32_t v2);

  void add_navcel_to_surface(std::size_t surface_index, NavCelId cid);

  Eigen::Vector3f navcel_centroid(NavCelId cid) const;

  std::vector<NavCelId> navcel_neighbors(NavCelId cid) const;
 
  // ---- Links between NavCels that are not adjacent (SurfaceLink) ----

  static constexpr std::size_t kInvalidLink = std::numeric_limits<std::size_t>::max();

  std::size_t add_link(
    NavCelId from, NavCelId to, LinkType type = LinkType::GENERIC, float length = -1.0f,
    bool bidirectional = true);

  bool remove_link(std::size_t index);
 
  void clear_links();
 
  const std::vector<SurfaceLink> & links() const {return links_;}

  const std::vector<std::uint32_t> & links_from(NavCelId cid) const;

  std::vector<float> distance_field(
    const std::vector<NavCelId> & seed_cells, bool from_boundary,
    float max_distance = std::numeric_limits<float>::infinity()) const;

  static constexpr std::size_t kNoSurface = std::numeric_limits<std::size_t>::max();

  std::size_t surface_of(NavCelId cid) const;
 
  // ---- Layers (per-NavCel) convenience ----

  template<typename T>
  std::shared_ptr<LayerView<T>> add_layer(
    const std::string & name,
    const std::string & description = {},
    const std::string & unit = {},
    T default_value = T{})
  {
    const bool existed = has_layer(name);
    auto v = layers.add_or_get<T>(name, navcels.size(), layer_type_tag<T>());
    if (!existed || v->data().size() != navcels.size()) {  // new layers start at default_value
      v->data().assign(navcels.size(), default_value);
      v->mark_all_changed();
    }
    auto & meta = layer_meta[name];
    meta.description = description;
    meta.unit = unit;
    meta.per_cell = true;
    return v;
  }

  bool has_layer(const std::string & name) const;

  std::size_t layer_size(const std::string & name) const;

  std::string layer_type_name(const std::string & name) const;

  template<typename T>
  void layer_set(const std::string & name, NavCelId cid, T value)
  {
    auto view = layers.add_or_get<T>(name, navcels.size(), layer_type_tag<T>());
    if (static_cast<size_t>(cid) < view->data().size()) {
      view->data()[cid] = value;
      view->mark_changed(cid);
    }
  }

  template<typename T>
  T layer_get(const std::string & name, NavCelId cid, T def = T{}) const
  {
    if (auto base = layers.get(name)) {
      if (auto view = std::dynamic_pointer_cast<LayerView<T>>(base)) {
        const auto & buf = view->data();
        if (static_cast<size_t>(cid) < buf.size()) {
          return buf[cid];
        }
        return def;
      }
    }
    // Fallback: try to fetch as double from any stored type and convert
    double v;
    if (auto base = layers.get(name)) {
      if (auto v8 = std::dynamic_pointer_cast<LayerView<uint8_t>>(base)) {
        const auto & buf = v8->data();
        if (static_cast<size_t>(cid) >= buf.size()) {return def;}
        v = static_cast<double>(buf[cid]);
      } else if (auto vf = std::dynamic_pointer_cast<LayerView<float>>(base)) {
        const auto & buf = vf->data();
        if (static_cast<size_t>(cid) >= buf.size()) {return def;}
        v = static_cast<double>(buf[cid]);
      } else if (auto vd = std::dynamic_pointer_cast<LayerView<double>>(base)) {
        const auto & buf = vd->data();
        if (static_cast<size_t>(cid) >= buf.size()) {return def;}
        v = buf[cid];
      } else if (auto vi = std::dynamic_pointer_cast<LayerView<int16_t>>(base)) {
        const auto & buf = vi->data();
        if (static_cast<size_t>(cid) >= buf.size()) {return def;}
        v = static_cast<double>(buf[cid]);
      } else {
        return def;
      }
    } else {
      return def;
    }
 
    if constexpr (std::is_floating_point_v<T>) {
      return static_cast<T>(v);
    } else if constexpr (std::is_integral_v<T>) {
      const double lo = static_cast<double>(std::numeric_limits<T>::lowest());
      const double hi = static_cast<double>(std::numeric_limits<T>::max());
      double vv = v < lo ? lo : (v > hi ? hi : v);
      return static_cast<T>(std::llround(vv));
    } else {
      static_assert(sizeof(T) == 0, "Unsupported layer type");
    }
  }

  std::optional<LayerMeta> get_layer_meta(const std::string & name) const;

  void set_layer_unknown(const std::string & name, double value);

  bool is_unknown(const std::string & name, NavCelId cid) const;

  std::vector<std::string> list_layers() const;

  double sample_layer_at(
    const std::string & name,
    const Eigen::Vector3f & p_world,
    double def = std::numeric_limits<double>::quiet_NaN()) const;

  template<typename T>
  void layer_clear(const std::string & name, T value = T{})
  {
    auto view = layers.add_or_get<T>(name, navcels.size(), layer_type_tag<T>());
    std::fill(view->data().begin(), view->data().end(), value);
    view->mark_all_changed();
  }

  template<typename T>
  bool layer_copy(const std::string & src, const std::string & dst)
  {
    // No-op if names are equal
    if (src == dst) {
      return true;
    }
 
    // Get source
    auto src_base = layers.get(src);
    auto src_view = std::dynamic_pointer_cast<LayerView<T>>(src_base);
    if (!src_view) {
      return false;  // source layer missing or wrong type
    }
 
    // Ensure destination exists with correct length (navcels.size())
    auto dst_view = layers.add_or_get<T>(dst, navcels.size(), layer_type_tag<T>());
    if (!dst_view) {
      return false;
    }
 
    // If sizes mismatch, something is wrong with geometry/layer size.
    // You can choose to resize here if your semantics allow it. For safety, fail:
    if (src_view->data().size() != dst_view->data().size()) {
      return false;
    }
 
    // If both views already refer to the same underlying buffer, no work to do
    if (!src_view->data().empty() &&
      src_view->data().data() == dst_view->data().data())
    {
      return true;
    }
 
    // Hash-based skip: O(1) if cached, O(n) only on first compute (then cached)
    const bool same_hash = (src_view->content_hash() == dst_view->content_hash());
    if (same_hash) {
      return true;  // identical content → avoid copy
    }
 
    // Copy only when different. set_data() marks hash as dirty for dst.
    dst_view->set_data(src_view->data());
    return true;
  }

  template<typename T>
  std::shared_ptr<LayerView<T>> add_layer(
    const std::string & name,
    const std::string & description,
    const std::string & unit,
    const std::string & src_layer)
  {
    auto dst = layers.add_or_get<T>(name, navcels.size(), layer_type_tag<T>());
    if (dst->data().size() != navcels.size()) {
      dst->data().assign(navcels.size(), T{});
    }
    auto & meta = layer_meta[name];
    meta.description = description;
    meta.unit = unit;
    meta.per_cell = true;
 
    if (!has_layer(src_layer)) {
      return dst;
    }
 
    if (layer_copy<T>(src_layer, name)) {
      return dst;
    }
 
    const std::size_t n = dst->size();
    for (NavCelId cid = 0; cid < n; ++cid) {
      double v = layer_get<double>(src_layer, cid, std::numeric_limits<double>::quiet_NaN());
      if (std::isnan(v)) {v = 0.0;}
 
      if constexpr (std::is_floating_point_v<T>) {
        dst->data()[cid] = static_cast<T>(v);
      } else if constexpr (std::is_integral_v<T>) {
        const double lo = static_cast<double>(std::numeric_limits<T>::lowest());
        const double hi = static_cast<double>(std::numeric_limits<T>::max());
        if (v < lo) {v = lo;}
        if (v > hi) {v = hi;}
        dst->data()[cid] = static_cast<T>(std::llround(v));
      } else {
        static_assert(sizeof(T) == 0, "Unsupported layer type");
      }
    }
 
    return dst;
  }

  bool locate_navcel_core(
    const Eigen::Vector3f & p_world,
    std::size_t & surface_idx,
    NavCelId & cid,
    Eigen::Vector3f & bary,
    Eigen::Vector3f * hit_pt,
    const LocateOpts & opts) const;

  template<typename T>
  bool set_area(
    const Eigen::Vector3f & p_world,
    T value,
    const std::string & layer_name,
    AreaShape shape,
    float size)
  {
    if (navcels.empty() || surfaces.empty() || size <= 0.0f) {
      return false;
    }
 
    std::size_t surface_idx = 0;
    NavCelId start_cid{};
    Eigen::Vector3f bary{};
    Eigen::Vector3f hit_pt{};
    if (!locate_navcel(p_world, surface_idx, start_cid, bary, &hit_pt)) {
      return false;
    }
 
    const float cx = hit_pt.x();
    const float cy = hit_pt.y();
 
    {
      auto existing = layers.get(layer_name);
      if (existing && existing->type() != layer_type_tag<T>()) {
        return false;  // type mismatch
      }
    }
    auto layer = layers.add_or_get<T>(layer_name, navcels.size(), layer_type_tag<T>());
    if (!layer) {
      return false;
    }
    if (layer->data().size() != navcels.size()) {
      const_cast<std::vector<T> &>(layer->data()).resize(navcels.size(), T{});
    }
 
    const float half = (shape == AreaShape::CIRCULAR) ? size : (0.5f * size);
    const float minx = cx - half, maxx = cx + half;
    const float miny = cy - half, maxy = cy + half;
    const float r2 = (shape == AreaShape::CIRCULAR) ? (size * size) : 0.0f;
 
    auto tri_aabb_intersects_box = [&](NavCelId cid) -> bool {
        const auto & tri = navcels[static_cast<std::size_t>(cid)];
        const Eigen::Vector3f p0{positions.x[tri.v[0]], positions.y[tri.v[0]],
          positions.z[tri.v[0]]};
        const Eigen::Vector3f p1{positions.x[tri.v[1]], positions.y[tri.v[1]],
          positions.z[tri.v[1]]};
        const Eigen::Vector3f p2{positions.x[tri.v[2]], positions.y[tri.v[2]],
          positions.z[tri.v[2]]};
        const float tminx = std::min({p0.x(), p1.x(), p2.x()});
        const float tmaxx = std::max({p0.x(), p1.x(), p2.x()});
        const float tminy = std::min({p0.y(), p1.y(), p2.y()});
        const float tmaxy = std::max({p0.y(), p1.y(), p2.y()});
        if (tmaxx < minx || tminx > maxx || tmaxy < miny || tminy > maxy) {
          return false;
        }
        return true;
      };
 
    auto inside_area = [&](const Eigen::Vector3f & c) -> bool {
        const float dx = c.x() - cx;
        const float dy = c.y() - cy;
        if (shape == AreaShape::CIRCULAR) {
          return (dx * dx + dy * dy) <= r2;
        } else {
          return (std::abs(dx) <= half) && (std::abs(dy) <= half);
        }
      };
 
    std::vector<char> visited(navcels.size(), 0);
    std::deque<NavCelId> q;
    q.push_back(start_cid);
    visited[static_cast<std::size_t>(start_cid)] = 1;
 
    auto & data = layer->mutable_data();
 
    while (!q.empty()) {
      const NavCelId cid = q.front(); q.pop_front();
 
      if (!tri_aabb_intersects_box(cid)) {
        continue;
      }
 
      const Eigen::Vector3f c = navcel_centroid(cid);
      if (inside_area(c)) {
        data[static_cast<std::size_t>(cid)] = value;
      }
 
      for (const auto ncid : navcel_neighbors(cid)) {
        const std::size_t nidx = static_cast<std::size_t>(ncid);
        if (nidx < navcels.size() && !visited[nidx] && tri_aabb_intersects_box(ncid)) {
          visited[nidx] = 1;
          q.push_back(ncid);
        }
      }
    }
 
    return true;
  }
 
private:
  // Geometry fingerprint cache (lazy recompute)
  void ensure_geometry_fingerprint_() const;
  static std::uint64_t hash_geometry_bytes_(
    const float * x, std::size_t nx,
    const float * y, std::size_t ny,
    const float * z, std::size_t nz,
    const std::uint32_t * v0, std::size_t nv0,
    const std::uint32_t * v1, std::size_t nv1,
    const std::uint32_t * v2, std::size_t nv2);
 
  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;
 
  std::size_t nonmanifold_edges_{0};
 
  // Owning surface per NavCel, built in rebuild_geometry_accels().
  std::vector<std::uint32_t> navcel_surface_;
 
  // BVH over the surfaces' boxes (leaves index surface_tree_prims_), built in
  // rebuild_geometry_accels(); queries visit only the surfaces it reaches. Stale (surfaces added
  // since) means the queries go through every surface, as without it.
  std::vector<BVHNode> surface_tree_;
  std::vector<std::uint32_t> surface_tree_prims_;
  std::size_t surface_tree_surfaces_{0};
  void build_surface_tree_();
  bool surface_tree_valid_() const
  {
    return !surface_tree_.empty() && surface_tree_surfaces_ == surfaces.size();
  }
  std::vector<SurfaceLink> links_;
  mutable std::vector<std::vector<std::uint32_t>> link_index_;
  mutable bool link_index_dirty_{true};
  void build_navcel_surface_index_();
 
  // Builders and traversal helpers.

  void build_surface_bvh(Surface & s);

  bool surface_raycast(
    const Surface & s,
    const Eigen::Vector3f & o,
    const Eigen::Vector3f & d,
    NavCelId & hit_cid,
    float & t_out,
    Eigen::Vector3f & hit_pt,
    float t_max = std::numeric_limits<float>::infinity()) const;

  bool locate_by_walking(
    NavCelId start_cid,
    const Eigen::Vector3f & p,
    NavCelId & cid_out,
    Eigen::Vector3f & bary,
    Eigen::Vector3f * hit_pt,
    float planar_eps) const;

  bool surface_closest_navcel(
    const Surface & s,
    const Eigen::Vector3f & p,
    NavCelId & cid,
    Eigen::Vector3f & q,
    float & best_sq) const;
};
 
// Inline convenience overload.
inline bool NavMap::locate_navcel(
  const Eigen::Vector3f & p_world,
  size_t & surface_idx,
  NavCelId & cid,
  Eigen::Vector3f & bary,
  Eigen::Vector3f * hit_pt) const
{
  return locate_navcel(p_world, surface_idx, cid, bary, hit_pt, LocateOpts{});
}
 
}  // namespace navmap
 
#endif  // NAVMAP_CORE__NAVMAP_HPP_