diff --git a/src/esp/bindings/SimBindings.cpp b/src/esp/bindings/SimBindings.cpp index 4e8b7c01ba..41e990e57b 100644 --- a/src/esp/bindings/SimBindings.cpp +++ b/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, diff --git a/src/esp/physics/PhysicsManager.h b/src/esp/physics/PhysicsManager.h index e1250c053b..29296c449d 100644 --- a/src/esp/physics/PhysicsManager.h +++ b/src/esp/physics/PhysicsManager.h @@ -792,10 +792,14 @@ class PhysicsManager : public std::enable_shared_from_this { * 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; diff --git a/src/esp/physics/bullet/BulletPhysicsManager.cpp b/src/esp/physics/bullet/BulletPhysicsManager.cpp index 7770d0aa8c..e31f7867d8 100644 --- a/src/esp/physics/bullet/BulletPhysicsManager.cpp +++ b/src/esp/physics/bullet/BulletPhysicsManager.cpp @@ -484,7 +484,8 @@ 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(ray.direction.length()); @@ -492,8 +493,10 @@ RaycastResults BulletPhysicsManager::castRay(const esp::geo::Ray& ray, 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(allResults.m_hitFractions[i]) * maxDistance); + (static_cast((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; diff --git a/src/esp/physics/bullet/BulletPhysicsManager.h b/src/esp/physics/bullet/BulletPhysicsManager.h index 1aa0ee55ac..bd41a98bf4 100644 --- a/src/esp/physics/bullet/BulletPhysicsManager.h +++ b/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 diff --git a/src/esp/sim/Simulator.h b/src/esp/sim/Simulator.h index 3759500c56..db435c1fba 100644 --- a/src/esp/sim/Simulator.h +++ b/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(); } diff --git a/tests/test_physics.py b/tests/test_physics.py index 4ccdac8ea8..d39d4f0805 100644 --- a/tests/test_physics.py +++ b/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