More accurate get_random_navigable_point_near

[aclegg3]

Motivation and Context

TL;DR: This PR adds a new implementation for sampling random points within a circle.

Context:

Currently, we use a Detour function findRandomPointAroundCircle for these queries. This function is implemented with a breadth-first search over navmesh polygons, excluding polygons outside the circle, starting from the closest snap point.
The problem with this function is that it excludes regions of the navmesh which are inside the circle, but separated from the region containing the start point.

For example, this image shows points resulting from the query within the circle centered on the red object. Note that only bed points are considered because the "ramp" off the bed is outside the circle, cutting it off from the rest of that room and the other rooms.

To fix this behavior and accurately sample all points within the circle, two options are considered:

1. Change Detour to allow the breadth-first search to continue outside the circle by removing this check.
2. (proposed) Implement our own filter function to remove polygons outside the circle and then use a standard sample query with rejection for distance.

This PR implements option 2 for better efficiency. See performance notes below for details.

Performance Notes:

The previous method was very fast because it incorrectly only touches polygons connected to the start point and within the circle. Any fix will be inherently slower because it must consider the entire navmesh (or island) to correctly filter candidate polygons.

Option (1) above is empirically about 100x slower than the original approach and often fails to find a point within the iteration limit. 🙁 Likely due to the overhead of a search based sampling and high rejection rate.

Option (2) is about 10x slower than the original approach and does not appear to fail, offering better stability. There is overhead in the initial filtering phase, but then sampling is very fast. Future advantage: multi-point sampling could be easily batched for additional efficiency as the filter would only need to be set once for the batch.

Profile for the pictured scenario on my machine:

• current: 5.588531494140625e-06
• option 1: 2.3991942405700683e-04
• option 2: 7.155656814575195e-05

Other Notes:

This is breaking change because the behavior of this function will change for downstream use. It will now more accurately sample points in a circle which could be considered a bug fix.

How Has This Been Tested

Results with new implementation:

Navmesh with path between rooms visualized for reference:

No CI tests presently. Could invent a contrived example to ensure this works.

Types of changes

• Docs change / refactoring / dependency upgrade
• Bug fix (non-breaking change which fixes an issue)
• New feature (non-breaking change which adds functionality)
• [x Breaking change (fix or feature that would cause existing functionality to change)

Checklist

• My code follows the code style of this project.
• My change requires a change to the documentation.
• I have updated the documentation accordingly.
• I have read the CONTRIBUTING document.
• I have completed my CLA (see CONTRIBUTING)
• I have added tests to cover my changes.
• All new and existing tests passed.

[jimmytyyang]

Hi Alex, thank you so much for this!

This new function allows us to sample a wide range of points homogeneously, but we will still sample a point that is on the other room. So that robot still has a problem picking up the object. Do we consider that we fix that in the habitat-lab side or habitat-sim side? If it is the former, then we need to change the corresponding function there.

[aclegg3]

we will still sample a point that is on the other room. Do we consider that we fix that in the habitat-lab side or habitat-sim side?

I will put up a lab PR today which depends on this PR to add a function which handles the occlusion snapping problem.

[jimmytyyang]

LGTM! Thanks for working on this! this is great!

[0mdc]

LGTM! Thanks for the detailed description.

I left some very minor comments.