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 GNU General Public License v3.0.
// See <http://www.gnu.org/licenses/> for details.
//
// Easy Navigation program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3.0, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
 
 
#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 <stack>
#include <cmath>
#include <cstdint>
#include <span>
#include <type_traits>
#include <deque>
#include <algorithm>
 
#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 };

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;}
 
protected:
  mutable bool hash_dirty_{true};
  mutable std::uint64_t hash_cache_{0};
};

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; hash_dirty_ = true;}
  std::uint64_t content_hash() const override;
};

class LayerRegistry {
public:
  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_dirty();
        continue;
      }
      if (auto v = std::dynamic_pointer_cast<LayerView<float>>(kv.second)) {
        v->data_.resize(nitems);
        kv.second->mark_dirty();
        continue;
      }
      if (auto v = std::dynamic_pointer_cast<LayerView<double>>(kv.second)) {
        v->data_.resize(nitems);
        kv.second->mark_dirty();
        continue;
      }
    }
  }
 
private:
  std::unordered_map<std::string, std::shared_ptr<LayerViewBase>> layers_;
};

struct LayerMeta
{
  std::string description;   
  std::string unit;          
  bool per_cell{true};       
};
 
// -----------------------------------------------------------------------------
// 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;}

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

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;

  void build_adjacency();

  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_triangle(
    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;
 
  // ---- 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{})
  {
    auto v = layers.add_or_get<T>(name, navcels.size(), layer_type_tag<T>());
    if (v->data().size() != navcels.size()) {
      v->data().assign(navcels.size(), default_value);
    }
    layer_meta[name] = LayerMeta{description, unit, 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_dirty();
    }
  }

  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 {
        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>) {
      double lo = 0.0;
      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;

  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_dirty();
  }

  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{});
    }
    layer_meta[name] = LayerMeta{description, unit, 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>) {
        double lo = 0.0;
        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};
 
  // 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) 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_triangle(
    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