Class NavMap
Defined in File NavMap.hpp
Nested Relationships
Nested Types
Class Documentation
-
class NavMap
Main container for navigable surfaces, geometry, and layers.
A navmap::NavMap aggregates vertex positions, a list of navmap::NavCel triangles (global indexing), one or more navmap::Surface partitions, and a navmap::LayerRegistry with arbitrary per-NavCel scalar attributes.
Typical workflow
Call rebuild_geometry_accels() to compute normals, areas, adjacency, and build per-surface BVHs.
Create layers via LayerRegistry::add_or_get() sized to navcels.size().
Query with locate_navcel(), raycast(), or closest_navcel().
Public Functions
-
NavMap()
Copy constructor: deep copy of geometry, surfaces, layers and metadata.
-
NavMap(const NavMap &other)
Copy constructor: deep copy of geometry, surfaces, layers and metadata.
-
void rebuild_geometry_accels()
Recompute derived geometry and acceleration structures.
Computes triangle normals and areas, builds adjacency between neighbors, and builds per-surface BVHs. Layer sizes are not changed automatically; call LayerRegistry::resize_all() if you modified navcels.
-
NavMap &operator=(const NavMap &other)
Copy assignment optimized to avoid geometry duplication.
If both maps share identical geometry (same vertex arrays and NavCel indices), layers are synchronized by name/type (copy-on-difference) and destination-only layers are removed. Otherwise, a full deep copy is performed.
-
NavMap &operator=(NavMap &&other) noexcept
Move assignment. Transfers ownership of geometry, surfaces, layers and metadata.
-
bool has_same_geometry(const NavMap &other) const
Fast check for identical geometry (vertices and NavCel indices).
-
void build_adjacency()
Build topological adjacency between neighboring NavCels.
Two triangles are neighbors if they share an undirected edge. Called by rebuild_geometry_accels().
-
inline void mark_vertex_updated(PointId)
Mark a vertex as updated (reserved for future cache invalidation).
- Parameters:
pid – Vertex id (unused for now).
-
inline std::array<NavCelId, 3> get_neighbors(NavCelId cid) const
Return the three neighbor NavCel ids of triangle
cid.- Parameters:
cid – NavCel id.
- Returns:
Array with neighbor ids or max uint32_t if boundary.
-
bool raycast(const Eigen::Vector3f &o, const Eigen::Vector3f &d, NavCelId &hit_cid, float &t, Eigen::Vector3f &hit_pt) const
Raycast against all surfaces to find the closest hit.
-
void raycast_many(const std::vector<Ray> &rays, std::vector<RayHit> &out, bool first_hit_only = true) const
Batched raycast.
- Parameters:
rays – Input rays.
out – [out] Output hits (parallel to
rays).first_hit_only – If true, stop at the first surface that hits.
-
template<typename T>
inline T navcel_value(NavCelId cid, const LayerView<T> &layer) const Read the value of a typed per-NavCel layer at triangle
cid.- Template Parameters:
T – Layer storage type (must match the layer).
- Parameters:
cid – NavCel id.
layer – Typed layer view.
- Returns:
The value for triangle
cid.
-
inline bool locate_navcel(const Eigen::Vector3f &p_world, size_t &surface_idx, NavCelId &cid, Eigen::Vector3f &bary, Eigen::Vector3f *hit_pt) const
Locate the triangle under / near a world point (convenience).
Uses default LocateOpts. See the full overload for details.
- Parameters:
- Returns:
true if a triangle has been located.
-
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
Locate the triangle under / near a world point.
Strategy: 1) If LocateOpts::hint_cid is provided, try walking neighbors. 2) Else, try a per-surface 2D seed grid near (x,y) with planar test. 3) If still not found, vertical raycast (downward or upward).
- Parameters:
p_world – Query point in world coordinates.
surface_idx – [out] Surface index owning the located triangle.
cid – [out] Located NavCel id.
bary – [out] Barycentric coordinates of the hit.
hit_pt – [out] Optional: projected point on the surface.
opts – Tuning options (see LocateOpts).
- Returns:
true if a triangle has been located.
-
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
Find the closest triangle to a point.
Traverses per-surface BVHs with distance lower-bounds; returns the closest triangle, the closest point on it, and the squared distance.
- Parameters:
p_world – Query point in world coordinates.
surface_idx – [out] Surface index of the closest triangle.
cid – [out] NavCel id of the closest triangle.
closest_point – [out] Closest point on that triangle.
sqdist – [out] Squared distance to
p_world.restrict_surface – If >= 0, restrict search to this surface.
- Returns:
true if any triangle was considered.
-
std::size_t create_surface(std::string frame_id)
Create a new empty Surface and append it to the map.
- Parameters:
frame_id – TF frame id for the surface.
- Returns:
Index of the created surface in surfaces.
-
Surface create_surface_obj(const std::string &frame_id) const
Create a standalone Surface object (not yet added to the map).
- Parameters:
frame_id – Frame id to assign.
- Returns:
A Surface with no navcels, ready to be customized and then added.
-
std::size_t add_surface(const Surface &s)
Append an existing Surface (by value).
- Returns:
New index in surfaces.
-
std::size_t add_surface(Surface &&s)
Add a Surface by move (avoids copy).
- Returns:
New index in surfaces.
-
bool remove_surface(std::size_t surface_index)
Remove a Surface by index. Does NOT touch navcels / positions.
Note
The caller must ensure there are no dangling indices.
- Returns:
true if the surface existed and was removed.
-
uint32_t add_vertex(const Eigen::Vector3f &p)
Append a vertex and return its index.
-
NavCelId add_navcel(uint32_t v0, uint32_t v1, uint32_t v2)
Append a triangle (NavCel) and return its id.
- Parameters:
v0 – Index of vertex 0.
v1 – Index of vertex 1.
v2 – Index of vertex 2.
-
void add_navcel_to_surface(std::size_t surface_index, NavCelId cid)
Add an existing nav cell id to a surface.
-
std::vector<NavCelId> navcel_neighbors(NavCelId cid) const
Return up to 3 neighbor cell ids (skips invalid entries).
-
template<typename T>
inline std::shared_ptr<LayerView<T>> add_layer(const std::string &name, const std::string &description = {}, const std::string &unit = {}, T default_value = T{}) Create (or get) a typed per-NavCel layer with a default value.
If a layer with
nameexists, it is returned (size preserved). If the size differs from navcels.size(), values are reinitialized todefault_value.- Template Parameters:
T – Storage type.
- Parameters:
name – Layer name.
description – Human-readable description (optional).
unit – Unit (e.g., “m”, “deg”, “%”), optional.
default_value – Initial value for all triangles.
- Returns:
Shared view of the created/updated layer.
-
bool has_layer(const std::string &name) const
Check whether a layer named
nameexists.
-
std::size_t layer_size(const std::string &name) const
Number of entries in a layer (should equal navcels.size()).
-
std::string layer_type_name(const std::string &name) const
Human-readable type name for a layer (“float”, “double”, “uint8”, …).
-
template<typename T>
inline void layer_set(const std::string &name, NavCelId cid, T value) Set a per-cell value (creates/retypes the layer if needed).
- Template Parameters:
T – Storage type.
- Parameters:
name – Layer name.
cid – Triangle id.
value – Value to write.
-
template<typename T>
inline T layer_get(const std::string &name, NavCelId cid, T def = T{}) const Get a per-cell value as the requested type.
If the stored type matches
T, returns directly. Otherwise falls back to conversion vía double: floating → static_cast<T>, integral → clamp [0,max(T)] y redondeo (llround).Si la capa no existe o
cidestá fuera de rango, devuelvedef.- Template Parameters:
T – uint8_t, float, or double
- Parameters:
name – Layer name
cid – Triangle id
def – Default value on failure
- Returns:
Value converted to T, or
def
-
std::optional<LayerMeta> get_layer_meta(const std::string &name) const
Return metadata for a layer if present (implementation-defined).
- Parameters:
name – Layer name.
- Returns:
Optional metadata if available.
-
std::vector<std::string> list_layers() const
List all layer names currently stored.
- Returns:
Vector of layer names.
-
double sample_layer_at(const std::string &name, const Eigen::Vector3f &p_world, double def = std::numeric_limits<double>::quiet_NaN()) const
Sample a per-NavCel layer at a world position.
Locates the containing triangle and returns its layer value.
- Parameters:
name – Layer name.
p_world – World-space query.
def – Default value if not found.
- Returns:
Value or
defif missing.
-
template<typename T>
inline void layer_clear(const std::string &name, T value = T{}) Reset all values in a layer to a given value.
- Parameters:
T – Expected storage type of the layer.
name – Layer name.
value – Value to assign to all entries.
-
template<typename T>
inline bool layer_copy(const std::string &src, const std::string &dst) Copy values from one layer into another.
Both layers must have the same type and size.
- Parameters:
T – Storage type (e.g. float, uint8_t).
src – Name of source layer.
dst – Name of destination layer.
- Returns:
true if copy succeeded, false if type/size mismatch.
-
template<typename T>
inline std::shared_ptr<LayerView<T>> add_layer(const std::string &name, const std::string &description, const std::string &unit, const std::string &src_layer) Create (or get) a typed per-NavCel layer, initialized from another layer.
If a layer with
nameexists, it is returned (size preserved). If the size differs from navcels.size(), it is resized. If the source layersrc_layerexists, its values are copied into the destination using layer_copy(), performing type conversion if necessary. If the source layer does not exist, the destination is left with defaults.- Template Parameters:
T – Storage type.
- Parameters:
name – Layer name.
description – Human-readable description (optional).
unit – Unit (e.g., “m”, “deg”, “%”), optional.
src_layer – Source layer name to initialize from.
- Returns:
Shared view of the created/updated layer.
-
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
Forward to the existing low-level locator (implemented in .cpp).
This hook should call your original, detailed locator implementation. The robust overload above will fall back to vertical rays if needed.
-
template<typename T>
inline bool set_area(const Eigen::Vector3f &p_world, T value, const std::string &layer_name, AreaShape shape, float size) Set a per-NavCel layer to a constant value over a 2D area.
The center is the ground projection of
p_world. If the projected NavCel cannot be determined, returns false. Supports U8/F32/F64 layers.Efficiency: BFS from the seed NavCel with XY AABB pruning against the circumscribed square of the area. Inclusion test by triangle centroid.
- Template Parameters:
T – uint8_t, float, or double
- Parameters:
p_world – 3D point in world coordinates (projected to ground)
value – Value to set in the layer
layer_name – Target layer name
shape – CIRCULAR or RECTANGULAR
size – Radius (CIRCULAR) or side length (RECTANGULAR)
- Returns:
true if the seed NavCel was located and the layer updated
Public Members
-
LayerRegistry layers
Per-NavCel layers (runtime registry)
-
struct LocateOpts
Options for the locate functions.
hint_cid : starting triangle for walking (if provided).
hint_surface : optional surface restriction.
planar_eps : in-triangle barycentric tolerance.
height_eps : vertical tolerance when gating by AABB / fallback.
use_downward_ray : select downward or upward vertical ray fallback.