esp::nav::PathFinder class
#include <esp/nav/PathFinder.h>

Loads and/or builds a navigation mesh and then allows point sampling, path finding, collision, and island queries on that navmesh.

Also allows export of the navmesh data.

Powered by integration with Recast Navigation | Detour.

A navigation mesh (NavMesh) is a collection of two-dimensional convex polygons (i.e., a polygon mesh) that define which areas of an environment are traversable by an agent with a particular embodiement. In other words, an agent could freely navigate around within these areas unobstructed by objects, walls, gaps, overhangs, or other barriers that are part of the environment. Adjacent polygons are connected to each other in a graph enabling efficient pathfinding algorithms to chart routes between points on the NavMesh.

Using a NavMesh approximation of navigability, an agent is embodied as a rigid cylinder aligned with the gravity direction. The NavMesh is then computed by voxelizing the static scene and generating polygons on the top surfaces of solid voxels where the cylinder would sit without intersection or overhanging and respecting configured constraints such as maximum climbable slope and step-height.

Constructors, destructors, conversion operators

PathFinder()
Constructor.
~PathFinder() defaulted

Public functions

auto build(const NavMeshSettings& bs, const float* verts, int nverts, const int* tris, int ntris, const float* bmin, const float* bmax) -> bool
Construct a NavMesh from NavMeshSettings and mesh data pointers.
auto build(const NavMeshSettings& bs, const esp::assets::MeshData& mesh) -> bool
Construct a NavMesh from NavMeshSettings and a MeshData object.
auto getRandomNavigablePoint(int maxTries = 10, int islandIndex = ID_UNDEFINED) -> vec3f
Returns a random navigable point.
auto getRandomNavigablePointAroundSphere(const vec3f& circleCenter, float radius, int maxTries = 10, int islandIndex = ID_UNDEFINED) -> vec3f
Returns a random navigable point within a specified radius about a given point.
auto findPath(ShortestPath& path) -> bool
Finds the shortest path between two points on the navigation mesh.
auto findPath(MultiGoalShortestPath& path) -> bool
Finds the shortest path from a start point to the closest (by geoddesic distance) end point.
template<typename T>
auto tryStep(const T& start, const T& end) -> T
Attempts to move from start to end and returns the navigable point closest to end that is feasibly reachable from start.
template<typename T>
auto tryStepNoSliding(const T& start, const T& end) -> T
Same as tryStep but does not allow for sliding along walls.
template<typename T>
auto snapPoint(const T& pt, int islandIndex = ID_UNDEFINED) -> T
Snaps a point to the navigation mesh.
template<typename T>
auto getIsland(const T& pt) -> int
Identifies the island closest to a point.
auto loadNavMesh(const std::string& path) -> bool
Loads a navigation meshed saved by saveNavMesh.
auto saveNavMesh(const std::string& path) -> bool
Saves a navigation mesh to later be loaded by loadNavMesh.
auto isLoaded() const -> bool
void seed(uint32_t newSeed)
Seed the pathfinder. Useful for getRandomNavigablePoint.
auto islandRadius(const vec3f& pt) const -> float
returns a heuristic for the size of the connected component that pt belongs to.
auto islandRadius(int islandIndex) const -> float
returns the size of the specified connected component.
auto numIslands() const -> int
returns the number of connected components making up the navmesh.
auto distanceToClosestObstacle(const vec3f& pt, float maxSearchRadius = 2.0) const -> float
Finds the distance to the closest non-navigable location.
auto closestObstacleSurfacePoint(const vec3f& pt, float maxSearchRadius = 2.0) const -> HitRecord
Same as distanceToClosestObstacle but returns additional information.
auto isNavigable(const vec3f& pt, float maxYDelta = 0.5) const -> bool
Query whether or not a given location is navigable.
auto getNavigableArea(int islandIndex = ID_UNDEFINED) const -> float
auto bounds() const -> std::pair<vec3f, vec3f>
auto getTopDownView(float metersPerPixel, float height, float eps = 0.5) -> Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic>
Get a 2D grid marking navigable and non-navigable cells at a specified height and resolution.
auto getTopDownIslandView(float metersPerPixel, float height, float eps = 0.5) -> Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic>
Get a 2D grid marking island index for navigable cells and -1 for non-navigable cells at a specified height and resolution.
auto getNavMeshData(int islandIndex = ID_UNDEFINED) -> std::shared_ptr<assets::MeshData>
Returns a MeshData object containing triangulated NavMesh polys.
auto getNavMeshSettings() const -> Corrade::Containers::Optional<NavMeshSettings>
Return the settings for the current NavMesh.

Function documentation

bool esp::nav::PathFinder::build(const NavMeshSettings& bs, const float* verts, int nverts, const int* tris, int ntris, const float* bmin, const float* bmax)

Construct a NavMesh from NavMeshSettings and mesh data pointers.

Parameters
bs Parameter settings for NavMesh construction.
verts Vertex array of the mesh.
nverts Number of verts in the array.
tris Index array of the mesh triangles.
ntris Number of triangle indices in the array.
bmin Navigable region bounding box min corner. Could be mesh bb or user defined override for subset of the mesh.
bmax Navigable region bounding box max corner. Could be mesh bb or user defined override for subset of the mesh.
Returns Whether or not construction was successful.

bool esp::nav::PathFinder::build(const NavMeshSettings& bs, const esp::assets::MeshData& mesh)

Construct a NavMesh from NavMeshSettings and a MeshData object.

Parameters
bs Parameter settings for NavMesh construction.
mesh A joined mesh for which to compute a navmesh.
Returns Whether or not construction was successful.

vec3f esp::nav::PathFinder::getRandomNavigablePoint(int maxTries = 10, int islandIndex = ID_UNDEFINED)

Returns a random navigable point.

Parameters
maxTries in The maximum number of tries sampling will be retried if it fails.
islandIndex in Optionally specify the island from which to sample the point. Default -1 queries the full navmesh.
Returns A random navigable point or NAN if none found.

vec3f esp::nav::PathFinder::getRandomNavigablePointAroundSphere(const vec3f& circleCenter, float radius, int maxTries = 10, int islandIndex = ID_UNDEFINED)

Returns a random navigable point within a specified radius about a given point.

Parameters
circleCenter in The center of the spherical sample region.
radius in The spherical sample radius.
maxTries in The maximum number of tries sampling will be retried if it fails.
islandIndex in Optionally specify the island from which to sample the point. Default -1 queries the full navmesh.
Returns A random navigable point or NAN if none found.

bool esp::nav::PathFinder::findPath(ShortestPath& path)

Finds the shortest path between two points on the navigation mesh.

Parameters
path in/out The ShortestPath structure contain the starting and end point. This method will populate the ShortestPath::points and ShortestPath::geodesicDistance fields.
Returns Whether or not a path exists between ShortestPath::requestedStart and ShortestPath::requestedEnd.

For this method to succeed, both end points must exist on the same navmesh island.

bool esp::nav::PathFinder::findPath(MultiGoalShortestPath& path)

Finds the shortest path from a start point to the closest (by geoddesic distance) end point.

Parameters
path in/out The MultiGoalShortestPath structure contain the start point and list of possible end points. This method will populate the MultiGoalShortestPath::points and MultiGoalShortestPath::geodesicDistance fields.
Returns Whether or not a path exists between MultiGoalShortestPath::requestedStart and any MultiGoalShortestPath.requestedEnds.

For this method to succeed, start point and one end point must exist on the same navmesh island.

template<typename T>
T esp::nav::PathFinder::tryStep(const T& start, const T& end)

Attempts to move from start to end and returns the navigable point closest to end that is feasibly reachable from start.

Parameters
start in The starting location
end out The desired end location
Returns The found end location.

template<typename T>
T esp::nav::PathFinder::snapPoint(const T& pt, int islandIndex = ID_UNDEFINED)

Snaps a point to the navigation mesh.

Parameters
pt in The point to snap to the navigation mesh
islandIndex in Optionally specify the island from which to sample the point. Default -1 queries the full navmesh.
Returns The closest navigation point to pt. Will be {NAN, NAN, NAN} if no navigable point was within a reasonable distance

template<typename T>
int esp::nav::PathFinder::getIsland(const T& pt)

Identifies the island closest to a point.

Parameters
pt in The point to check against the navigation mesh island system.
Returns The island for the point or ID_UNDEFINED (-1) if failed.

Snaps the point to the navmesh and then queries the island at the snap point.

When the input point is not assumed to be on the NavMesh, it is possible that the snapped point is quite far from the query point. In this case, also consider checking isNavigable or check distance to the snap point to validate.

bool esp::nav::PathFinder::loadNavMesh(const std::string& path)

Loads a navigation meshed saved by saveNavMesh.

Parameters
path in The saved navigation mesh file, generally has extension .navmesh
Returns Whether or not the navmesh was successfully loaded

Also imports serialized NavMeshSettings if available.

bool esp::nav::PathFinder::saveNavMesh(const std::string& path)

Saves a navigation mesh to later be loaded by loadNavMesh.

Parameters
path in The name of the file, generally has extension .navmesh
Returns Whether or not the navmesh was successfully saved

Also serializes NavMeshSettings into the file.

bool esp::nav::PathFinder::isLoaded() const

Returns If a valid navigation mesh is currently loaded or not.

void esp::nav::PathFinder::seed(uint32_t newSeed)

Seed the pathfinder. Useful for getRandomNavigablePoint.

Parameters
newSeed in The random seed

float esp::nav::PathFinder::islandRadius(const vec3f& pt) const

returns a heuristic for the size of the connected component that pt belongs to.

Parameters
pt in The point which specifies the connected component in question.
Returns Heuristic size of the connected component.

The point will be snapped to the NavMesh and the resulting island radius returned.

float esp::nav::PathFinder::islandRadius(int islandIndex) const

returns the size of the specified connected component.

Parameters
islandIndex in The index of the specified connected component
Returns Size of the connected component

int esp::nav::PathFinder::numIslands() const

returns the number of connected components making up the navmesh.

Returns Number of the connected components

float esp::nav::PathFinder::distanceToClosestObstacle(const vec3f& pt, float maxSearchRadius = 2.0) const

Finds the distance to the closest non-navigable location.

Parameters
pt in The radius to search in
maxSearchRadius
Returns The distance to the closest non-navigable location or maxSearchRadius if all locations within maxSearchRadius are navigable

bool esp::nav::PathFinder::isNavigable(const vec3f& pt, float maxYDelta = 0.5) const

Query whether or not a given location is navigable.

Parameters
pt in The location to check whether or not it is navigable
maxYDelta in The maximum y displacement. This tolerance is useful for computing a top-down occupancy grid as the floor is not perfectly level
Returns Whether or not pt is navigable

This method works by snapping pt to the navigation mesh with snapPoint and then checking to see if there was no displacement in the x-z plane and at most maxYDelta displacement in the y direction.

float esp::nav::PathFinder::getNavigableArea(int islandIndex = ID_UNDEFINED) const

Parameters
islandIndex in Optionally limit results to a specific island. Default -1 queries all islands.

Compute and return the total area of all NavMesh polygons.

Values are pre-computed and cached for constant time access.

std::pair<vec3f, vec3f> esp::nav::PathFinder::bounds() const

Returns The axis aligned bounding box containing the navigation mesh.

Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> esp::nav::PathFinder::getTopDownView(float metersPerPixel, float height, float eps = 0.5)

Get a 2D grid marking navigable and non-navigable cells at a specified height and resolution.

Parameters
metersPerPixel size of the discrete grid cells. Controls grid resolution.
height The vertical height of the 2D slice.
eps Sets allowable epsilon meter Y offsets from the configured height value.
Returns The 2D grid marking cells as navigable or not.

The size of the grid depends on the navmesh bounds and selected resolution.

Can be further processed by Habitat-lab utilities. See examples/tutorials/notebooks/ECCV_Navigation.ipynb for details.

Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic> esp::nav::PathFinder::getTopDownIslandView(float metersPerPixel, float height, float eps = 0.5)

Get a 2D grid marking island index for navigable cells and -1 for non-navigable cells at a specified height and resolution.

Parameters
metersPerPixel size of the discrete grid cells. Controls grid resolution.
height The vertical height of the 2D slice.
eps Sets allowable epsilon meter Y offsets from the configured height value.
Returns The 2D grid marking cell islands or -1 for not navigable.

The size of the grid depends on the navmesh bounds and selected resolution.

std::shared_ptr<assets::MeshData> esp::nav::PathFinder::getNavMeshData(int islandIndex = ID_UNDEFINED)

Returns a MeshData object containing triangulated NavMesh polys.

Parameters
islandIndex in Optionally limit results to a specific island. Default -1 queries all islands.
Returns The object containing triangulated NavMesh polys.

Theobject is generated and stored if this is the first query for constant time access in subsequent calls.

Does nothing if the PathFinder is not loaded.