[bugfix] - raycast buffer distance (#2466)

* add buffer distance to raycasting API

* add tests and adjust the ray scaling with the new buffer

src/esp/bindings/SimBindings.cpp

@@ -296,6 +296,7 @@ void initSimBindings(py::module& m) {
           R"(Perform discrete collision detection for the scene. Physics must be enabled. Warning: may break simulation determinism.)")
       .def(
           "cast_ray", &Simulator::castRay, "ray"_a, "max_distance"_a = 100.0,
+          "buffer_distance"_a = 0.08,
           R"(Cast a ray into the collidable scene and return hit results. Physics must be enabled. max_distance in units of ray length.)")
       .def("set_object_bb_draw", &Simulator::setObjectBBDraw, "draw_bb"_a,
            "object_id"_a,

src/esp/physics/PhysicsManager.h

@@ -792,10 +792,14 @@ class PhysicsManager : public std::enable_shared_from_this<PhysicsManager> {
    * distances will be in units of ray length.
    * @param maxDistance The maximum distance along the ray direction to
    * search. In units of ray length.
+   * @param bufferDistance The casts the ray from this distance behind the
+   * origin in the inverted ray direction to avoid errors related to casting
+   * rays inside a collision shape's margin.
    * @return The raycast results sorted by distance.
    */
   virtual RaycastResults castRay(const esp::geo::Ray& ray,
-                                 CORRADE_UNUSED double maxDistance = 100.0) {
+                                 CORRADE_UNUSED double maxDistance = 100.0,
+                                 CORRADE_UNUSED double bufferDistance = 0.08) {
     ESP_ERROR() << "Not implemented in base PhysicsManager. Install with "
                    "--bullet to use this feature.";
     RaycastResults results;

src/esp/physics/bullet/BulletPhysicsManager.cpp

@@ -484,16 +484,19 @@ void BulletPhysicsManager::debugDraw(const Magnum::Matrix4& projTrans) const {
 }
 
 RaycastResults BulletPhysicsManager::castRay(const esp::geo::Ray& ray,
-                                             double maxDistance) {
+                                             double maxDistance,
+                                             double bufferDistance) {
   RaycastResults results;
   results.ray = ray;
   double rayLength = static_cast<double>(ray.direction.length());
   if (rayLength == 0) {
     ESP_ERROR() << "Cannot cast ray with zero length, aborting.";
     return results;
   }
-  btVector3 from(ray.origin);
+  btVector3 from(ray.origin - ((ray.direction / rayLength) * bufferDistance));
   btVector3 to(ray.origin + ray.direction * maxDistance);
+  double totalLength = (rayLength * maxDistance) + bufferDistance;
+  double scaledBuffer = bufferDistance / (totalLength);
 
   btCollisionWorld::AllHitsRayResultCallback allResults(from, to);
   bWorld_->rayTest(from, to, allResults);
@@ -505,7 +508,13 @@ RaycastResults BulletPhysicsManager::castRay(const esp::geo::Ray& ray,
     hit.normal = Magnum::Vector3{allResults.m_hitNormalWorld[i]};
     hit.point = Magnum::Vector3{allResults.m_hitPointWorld[i]};
     hit.rayDistance =
-        (static_cast<double>(allResults.m_hitFractions[i]) * maxDistance);
+        (static_cast<double>((allResults.m_hitFractions[i])) - scaledBuffer) *
+        totalLength / rayLength;
+    if (hit.rayDistance < 0) {
+      // We cast the the ray from bufferDistance behind the origin, so we'll
+      // throw away hits in the intermediate space.
+      continue;
+    }
     // default to RIGID_STAGE_ID for "scene collision" if we don't know which
     // object was involved
     hit.objectId = RIGID_STAGE_ID;

src/esp/physics/bullet/BulletPhysicsManager.h

@@ -193,10 +193,14 @@ class BulletPhysicsManager : public PhysicsManager {
    * distances will be in units of ray length.
    * @param maxDistance The maximum distance along the ray direction to search.
    * In units of ray length.
+   * @param bufferDistance The casts the ray from this distance behind the
+   * origin in the inverted ray direction to avoid errors related to casting
+   * rays inside a collision shape's margin.
    * @return The raycast results sorted by distance.
    */
   RaycastResults castRay(const esp::geo::Ray& ray,
-                         double maxDistance = 100.0) override;
+                         double maxDistance = 100.0,
+                         double bufferDistance = 0.08) override;
 
   /**
    * @brief Query the number of contact points that were active during the

src/esp/sim/Simulator.h

@@ -419,12 +419,16 @@ class Simulator {
    * distances will be in units of ray length.
    * @param maxDistance The maximum distance along the ray direction to search.
    * In units of ray length.
+   * @param bufferDistance The casts the ray from this distance behind the
+   * origin in the inverted ray direction to avoid errors related to casting
+   * rays inside a collision shape's margin.
    * @return Raycast results sorted by distance.
    */
   esp::physics::RaycastResults castRay(const esp::geo::Ray& ray,
-                                       double maxDistance = 100.0) {
+                                       double maxDistance = 100.0,
+                                       double bufferDistance = 0.08) {
     if (sceneHasPhysics()) {
-      return physicsManager_->castRay(ray, maxDistance);
+      return physicsManager_->castRay(ray, maxDistance, bufferDistance);
     }
     return esp::physics::RaycastResults();
   }

tests/test_physics.py

@@ -555,7 +555,7 @@ def test_raycast():
                 np.array([-0.999587, 0.0222882, -0.0181424]),
                 atol=0.07,
             )
-            assert abs(raycast_results.hits[0].ray_distance - 6.831) < 0.001
+            assert abs(raycast_results.hits[0].ray_distance - 6.8306) < 0.001
             # hit stage
             assert raycast_results.hits[0].object_id == habitat_sim.stage_id
 
@@ -584,6 +584,46 @@ def test_raycast():
             assert abs(raycast_results.hits[0].ray_distance - 1.89) < 0.001
             assert raycast_results.hits[0].object_id == cube_obj.object_id
 
+            #############################################
+            # test raycasting with buffer:
+            # - test raycasting from within the cube's collision margin with and without buffering
+            # - NOTE: primitive collision margin is much smaller than a convex shape, so we need to get very close to
+            test_ray_2 = habitat_sim.geo.Ray()
+            test_ray_2.origin = cube_obj.translation + mn.Vector3(
+                cube_obj.collision_shape_aabb.size_x(), 0, 0
+            )
+            test_ray_2.direction = mn.Vector3(-1, 0, 0)
+            raycast_results = sim.cast_ray(test_ray_2)
+            assert raycast_results.has_hits()
+            assert raycast_results.hits[0].object_id == cube_obj.object_id
+            # move the ray to just within 1cm of the hit point
+            test_ray_2.origin = raycast_results.hits[0].point + mn.Vector3(0.009, 0, 0)
+            # cast ray with no buffer
+            raycast_results_no_buffer = sim.cast_ray(test_ray_2, buffer_distance=0)
+            # cast ray with buffer
+            raycast_results_buffer = sim.cast_ray(
+                test_ray_2
+            )  # default buffer_distance==0.08
+
+            assert raycast_results_no_buffer.has_hits()
+            assert raycast_results_buffer.has_hits()
+            # the unbuffered raycast misses from within 1cm of the object surface
+            assert raycast_results_no_buffer.hits[0].object_id != cube_obj.object_id
+            # the buffered raycast correctly detects the hit point
+            assert raycast_results_buffer.hits[0].object_id == cube_obj.object_id
+            assert (
+                raycast_results_buffer.hits[0].point - raycast_results.hits[0].point
+            ).length() < 0.0001
+
+            # test another point within the object, but with buffer outside and guarantee no negative hits are registered
+            test_ray_2.origin = raycast_results.hits[0].point - mn.Vector3(0.01, 0, 0)
+            raycast_results_buffer = sim.cast_ray(
+                test_ray_2
+            )  # default buffer_distance==0.08
+            # the ray cast was cast from
+            assert raycast_results_buffer.hits[0].ray_distance > 0
+            assert raycast_results_buffer.hits[0].object_id != cube_obj.object_id
+
             # test raycast against a non-collidable object.
             # should not register a hit with the object.
             cube_obj.collidable = False