diff --git a/dartsim/src/GzCollisionDetector.cc b/dartsim/src/GzCollisionDetector.cc index e32403525..a9a345b79 100644 --- a/dartsim/src/GzCollisionDetector.cc +++ b/dartsim/src/GzCollisionDetector.cc @@ -17,10 +17,16 @@ #include #include +#include #include #include #include +#include + +#include + +#include #include "GzCollisionDetector.hh" @@ -99,6 +105,21 @@ void GzCollisionDetector::LimitCollisionPairMaxContacts( } } +///////////////////////////////////////////////// +std::optional> GzCollisionDetector::BatchRaycast( + CollisionGroup */*_group*/, + const std::vector &/*_rays*/) const +{ + static bool warned = false; + if (!warned) + { + warned = true; + gzwarn << "BatchRaycast: collision detector does not support batch " + << "raycasting. All ray results will be NaN." << std::endl; + } + return std::nullopt; +} + ///////////////////////////////////////////////// GzOdeCollisionDetector::GzOdeCollisionDetector() : OdeCollisionDetector(), GzCollisionDetector() @@ -149,12 +170,44 @@ bool GzOdeCollisionDetector::collide( return ret; } +/// \brief Exposes BulletCollisionGroup::getBulletCollisionWorld() which +/// is protected in the base class. +class GzBulletCollisionGroup : public dart::collision::BulletCollisionGroup +{ + public: explicit GzBulletCollisionGroup( + const dart::collision::CollisionDetectorPtr &_detector); + + /// \brief Return the underlying btCollisionWorld + public: const btCollisionWorld *getCollisionWorld() const; +}; + +///////////////////////////////////////////////// +GzBulletCollisionGroup::GzBulletCollisionGroup( + const dart::collision::CollisionDetectorPtr &_detector) + : dart::collision::BulletCollisionGroup(_detector) +{ +} + +///////////////////////////////////////////////// +const btCollisionWorld *GzBulletCollisionGroup::getCollisionWorld() const +{ + // getBulletCollisionWorld() is protected in BulletCollisionGroup. + return this->getBulletCollisionWorld(); +} + ///////////////////////////////////////////////// GzBulletCollisionDetector::GzBulletCollisionDetector() : BulletCollisionDetector(), GzCollisionDetector() { } +///////////////////////////////////////////////// +std::unique_ptr +GzBulletCollisionDetector::createCollisionGroup() +{ + return std::make_unique(this->shared_from_this()); +} + ///////////////////////////////////////////////// GzBulletCollisionDetector::Registrar GzBulletCollisionDetector::mRegistrar{ @@ -193,3 +246,56 @@ bool GzBulletCollisionDetector::collide( this->LimitCollisionPairMaxContacts(_result); return ret; } + +///////////////////////////////////////////////// +std::optional> GzBulletCollisionDetector::BatchRaycast( + CollisionGroup *_group, + const std::vector &_rays) const +{ + std::vector results; + results.reserve(_rays.size()); + + auto *gzGroup = dynamic_cast(_group); + if (!gzGroup) + return std::nullopt; + + const btCollisionWorld *btWorld = gzGroup->getCollisionWorld(); + if (!btWorld) + return std::nullopt; + + for (const auto &ray : _rays) + { + const btVector3 btFrom( + static_cast(ray.from.x()), + static_cast(ray.from.y()), + static_cast(ray.from.z())); + const btVector3 btTo( + static_cast(ray.to.x()), + static_cast(ray.to.y()), + static_cast(ray.to.z())); + + btCollisionWorld::ClosestRayResultCallback rayCallback(btFrom, btTo); + btWorld->rayTest(btFrom, btTo, rayCallback); + + GzRayResult result; + if (rayCallback.hasHit()) + { + const btVector3 &hp = rayCallback.m_hitPointWorld; + const btVector3 &hn = rayCallback.m_hitNormalWorld; + result.hit = true; + result.point << hp.x(), hp.y(), hp.z(); + result.normal << hn.x(), hn.y(), hn.z(); + result.fraction = static_cast(rayCallback.m_closestHitFraction); + } + else + { + constexpr double kNaN = std::numeric_limits::quiet_NaN(); + result.point = Eigen::Vector3d::Constant(kNaN); + result.fraction = kNaN; + result.normal = Eigen::Vector3d::Constant(kNaN); + } + results.push_back(std::move(result)); + } + + return results; +} diff --git a/dartsim/src/GzCollisionDetector.hh b/dartsim/src/GzCollisionDetector.hh index f5c502c03..720860875 100644 --- a/dartsim/src/GzCollisionDetector.hh +++ b/dartsim/src/GzCollisionDetector.hh @@ -21,14 +21,34 @@ #include #include #include +#include +#include + +#include #include #include #include +#include +#include + namespace dart { namespace collision { +/// \brief Single ray query: origin and target in world coordinates. +struct GzRay +{ + Eigen::Vector3d from; + Eigen::Vector3d to; +}; + +/// \brief Result of a single ray query. +/// Alias for the gz-physics RayIntersection type to avoid redundant copies. +using GzRayResult = + gz::physics::GetBatchRayIntersectionFromLastStepFeature + ::RayIntersectionT; + class GzCollisionDetector { /// \brief Set the maximum number of contacts between a pair of collision @@ -42,6 +62,18 @@ class GzCollisionDetector /// \return Maximum number of contacts between a pair of collision objects. public: virtual std::size_t GetCollisionPairMaxContacts() const; + /// \brief Cast multiple rays against a collision group. + /// \param[in] _group The collision group to test against. + /// \param[in] _rays The rays to cast. + /// \return Results for each ray if supported, or std::nullopt if this + /// detector does not support batch raycasting. + public: virtual std::optional> BatchRaycast( + CollisionGroup *_group, + const std::vector &_rays) const; + + /// Destructor + public: virtual ~GzCollisionDetector() = default; + /// Constructor protected: GzCollisionDetector(); @@ -100,6 +132,14 @@ class GzBulletCollisionDetector : const CollisionOption& option = CollisionOption(false, 1u, nullptr), CollisionResult* result = nullptr) override; + // Documentation inherited + public: std::unique_ptr createCollisionGroup() override; + + // Documentation inherited + public: std::optional> BatchRaycast( + CollisionGroup *_group, + const std::vector &_rays) const override; + /// \brief Create the GzBulletCollisionDetector public: static std::shared_ptr create(); diff --git a/dartsim/src/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc index a6708dbbf..766416517 100644 --- a/dartsim/src/SimulationFeatures.cc +++ b/dartsim/src/SimulationFeatures.cc @@ -22,6 +22,7 @@ #include #include #include +#include #include @@ -38,8 +39,10 @@ #include #include +#include "gz/physics/GetBatchRayIntersection.hh" #include "gz/physics/GetContacts.hh" +#include "GzCollisionDetector.hh" #include "SimulationFeatures.hh" #if DART_VERSION_AT_LEAST(6, 13, 0) @@ -253,6 +256,47 @@ SimulationFeatures::GetRayIntersectionFromLastStep( return {intersection, extraData}; } +///////////////////////////////////////////////// +std::vector +SimulationFeatures::GetBatchRayIntersectionFromLastStep( + const Identity &_worldID, + const std::vector &_rays) const +{ + std::vector results; + results.reserve(_rays.size()); + + if (_rays.empty()) + return results; + + constexpr double kNaN = std::numeric_limits::quiet_NaN(); + const Eigen::Vector3d kNaNVec = Eigen::Vector3d::Constant(kNaN); + + auto *const world = this->ReferenceInterface(_worldID); + auto *const solver = world->getConstraintSolver(); + + auto detector = solver->getCollisionDetector(); + auto *gzDetector = + dynamic_cast(detector.get()); + + if (gzDetector) + { + std::vector gzRays; + gzRays.reserve(_rays.size()); + for (const auto &ray : _rays) + gzRays.push_back({ray.origin, ray.target}); + + auto gzResults = gzDetector->BatchRaycast( + solver->getCollisionGroup().get(), gzRays); + if (gzResults) + { + return std::move(*gzResults); + } + } + + results.assign(_rays.size(), {false, kNaNVec, kNaN, kNaNVec}); + return results; +} + std::vector SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const { diff --git a/dartsim/src/SimulationFeatures.hh b/dartsim/src/SimulationFeatures.hh index f2411fbba..7922a3b21 100644 --- a/dartsim/src/SimulationFeatures.hh +++ b/dartsim/src/SimulationFeatures.hh @@ -33,6 +33,7 @@ #include #include +#include #include #include #include @@ -58,7 +59,8 @@ struct SimulationFeatureList : FeatureList< SetContactPropertiesCallbackFeature, #endif GetContactsFromLastStepFeature, - GetRayIntersectionFromLastStepFeature + GetRayIntersectionFromLastStepFeature, + GetBatchRayIntersectionFromLastStepFeature > { }; #ifdef DART_HAS_CONTACT_SURFACE @@ -102,6 +104,14 @@ class SimulationFeatures : public: using GetRayIntersectionFromLastStepFeature::Implementation< FeaturePolicy3d>::RayIntersection; + public: using BatchRayIntersection = + GetBatchRayIntersectionFromLastStepFeature::Implementation< + FeaturePolicy3d>::RayIntersection; + + public: using BatchRayQuery = + GetBatchRayIntersectionFromLastStepFeature::Implementation< + FeaturePolicy3d>::RayQuery; + public: SimulationFeatures() = default; public: ~SimulationFeatures() override = default; @@ -123,6 +133,10 @@ class SimulationFeatures : const LinearVector3d &_from, const LinearVector3d &_end) const override; + public: std::vector GetBatchRayIntersectionFromLastStep( + const Identity &_worldID, + const std::vector &_rays) const override; + /// \brief link poses from the most recent pose change/update. /// The key is the link's ID, and the value is the link's pose private: mutable std::unordered_map prevLinkPoses; diff --git a/include/gz/physics/GetBatchRayIntersection.hh b/include/gz/physics/GetBatchRayIntersection.hh new file mode 100644 index 000000000..231d5ec0d --- /dev/null +++ b/include/gz/physics/GetBatchRayIntersection.hh @@ -0,0 +1,104 @@ +/* + * Copyright (C) 2026 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef GZ_PHYSICS_GETBATCHRAYINTERSECTION_HH_ +#define GZ_PHYSICS_GETBATCHRAYINTERSECTION_HH_ + +#include + +#include +#include +#include + +namespace gz +{ +namespace physics +{ + +/// \brief GetBatchRayIntersectionFromLastStepFeature is a feature for +/// retrieving multiple ray intersections generated in the previous simulation +/// step in a single call. +class GZ_PHYSICS_VISIBLE GetBatchRayIntersectionFromLastStepFeature + : public virtual FeatureWithRequirements +{ + public: template + struct RayIntersectionT + { + public: using Scalar = typename PolicyT::Scalar; + public: using VectorType = + typename FromPolicy::template Use; + + /// \brief True if the ray intersected an object; false on a miss. + /// When false, point, normal, and fraction carry no meaningful value. + bool hit{false}; + + /// \brief The hit point in world coordinates + VectorType point; + + /// \brief The fraction of the ray length at the intersection/hit point. + Scalar fraction; + + /// \brief The normal at the hit point in world coordinates + VectorType normal; + }; + + public: template + struct RayT + { + public: using VectorType = + typename FromPolicy::template Use; + + /// \brief Ray start point in world coordinates + VectorType origin; + + /// \brief Ray end point in world coordinates + VectorType target; + }; + + public: template + class World : public virtual Feature::World + { + public: using RayIntersection = RayIntersectionT; + public: using RayQuery = RayT; + + /// \brief Cast multiple rays and return one result per ray. + /// \param[in] _rays Ray queries (origin + target) in world coordinates. + /// \return One RayIntersection per input ray, in the same order. + public: std::vector + GetBatchRayIntersectionFromLastStep( + const std::vector &_rays) const; + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using RayIntersection = RayIntersectionT; + public: using RayQuery = RayT; + + public: virtual std::vector + GetBatchRayIntersectionFromLastStep( + const Identity &_worldID, + const std::vector &_rays) const = 0; + }; +}; + +} // namespace physics +} // namespace gz + +#include "gz/physics/detail/GetBatchRayIntersection.hh" + +#endif // GZ_PHYSICS_GETBATCHRAYINTERSECTION_HH_ diff --git a/include/gz/physics/detail/GetBatchRayIntersection.hh b/include/gz/physics/detail/GetBatchRayIntersection.hh new file mode 100644 index 000000000..266f5a77d --- /dev/null +++ b/include/gz/physics/detail/GetBatchRayIntersection.hh @@ -0,0 +1,44 @@ +/* + * Copyright (C) 2026 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef GZ_PHYSICS_DETAIL_GETBATCHRAYINTERSECTION_HH_ +#define GZ_PHYSICS_DETAIL_GETBATCHRAYINTERSECTION_HH_ + +#include + +#include + +namespace gz +{ +namespace physics +{ + +///////////////////////////////////////////////// +template +auto GetBatchRayIntersectionFromLastStepFeature::World< + PolicyT, FeaturesT>::GetBatchRayIntersectionFromLastStep( + const std::vector &_rays) const + -> std::vector +{ + return this->template Interface() + ->GetBatchRayIntersectionFromLastStep(this->identity, _rays); +} + +} // namespace physics +} // namespace gz + +#endif // GZ_PHYSICS_DETAIL_GETBATCHRAYINTERSECTION_HH_ diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index 5e95cdc31..75e3d3d1b 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -48,6 +48,7 @@ #include #include #include +#include #include #include #include "gz/physics/SphereShape.hh" @@ -2275,6 +2276,201 @@ TYPED_TEST(SimulationFeaturesRayIntersectionTest, UnsupportedRayIntersections) } } +// The features that an engine must have to be loaded by this loader. +struct FeaturesBatchRayIntersections : gz::physics::FeatureList< + gz::physics::sdf::ConstructSdfWorld, + gz::physics::GetBatchRayIntersectionFromLastStepFeature, + gz::physics::CollisionDetector, + gz::physics::ForwardStep +> {}; + +template +class SimulationFeaturesBatchRayIntersectionTest : + public SimulationFeaturesTest{}; +using SimulationFeaturesBatchRayIntersectionTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(SimulationFeaturesBatchRayIntersectionTest, + SimulationFeaturesBatchRayIntersectionTestTypes); + +///////////////////////////////////////////////// +TYPED_TEST(SimulationFeaturesBatchRayIntersectionTest, + SupportedBatchRayIntersections) +{ + for (const std::string &name : this->pluginNames) + { + auto world = LoadPluginAndWorld( + this->loader, + name, + common_test::worlds::kSphereSdf); + world->SetCollisionDetector("bullet"); + auto checkedOutput = + StepWorld(world, true, 1).first; + EXPECT_TRUE(checkedOutput); + + using World = gz::physics::World3d; + using RayQuery = World::RayQuery; + using RayIntersection = World::RayIntersection; + + // Build a batch: first ray hits the sphere, second misses + std::vector rays = { + {Eigen::Vector3d(-2, 0, 2), Eigen::Vector3d(2, 0, 2)}, // hit + {Eigen::Vector3d(2, 0, 10), Eigen::Vector3d(-2, 0, 10)}, // miss + }; + + std::vector results = + world->GetBatchRayIntersectionFromLastStep(rays); + + ASSERT_EQ(2u, results.size()); + + // Ray 0 — hits the unit sphere centred at (0,0,2) + { + const auto &hit = results[0]; + EXPECT_TRUE(hit.hit); + double epsilon = 1e-3; + EXPECT_TRUE(hit.point.isApprox(Eigen::Vector3d(-1, 0, 2), epsilon)) + << "hit point: " << hit.point.transpose(); + EXPECT_TRUE(hit.normal.isApprox(Eigen::Vector3d(-1, 0, 0), epsilon)) + << "hit normal: " << hit.normal.transpose(); + EXPECT_DOUBLE_EQ(0.25, hit.fraction); + } + + // Ray 1 — misses; hit must be false, numeric fields NaN (REP-117) + { + const auto &miss = results[1]; + EXPECT_FALSE(miss.hit); + EXPECT_TRUE(miss.point.array().isNaN().all()) + << "miss point should be NaN: " << miss.point.transpose(); + EXPECT_TRUE(miss.normal.array().isNaN().all()) + << "miss normal should be NaN: " << miss.normal.transpose(); + EXPECT_TRUE(std::isnan(miss.fraction)) + << "miss fraction should be NaN: " << miss.fraction; + } + } +} + +///////////////////////////////////////////////// +TYPED_TEST(SimulationFeaturesBatchRayIntersectionTest, + EmptyBatchRayIntersections) +{ + for (const std::string &name : this->pluginNames) + { + auto world = LoadPluginAndWorld( + this->loader, + name, + common_test::worlds::kSphereSdf); + world->SetCollisionDetector("bullet"); + auto checkedOutput = + StepWorld(world, true, 1).first; + EXPECT_TRUE(checkedOutput); + + using World = gz::physics::World3d; + using RayQuery = World::RayQuery; + + std::vector rays; // intentionally empty + auto results = world->GetBatchRayIntersectionFromLastStep(rays); + EXPECT_TRUE(results.empty()); + } +} + +///////////////////////////////////////////////// +TYPED_TEST(SimulationFeaturesBatchRayIntersectionTest, + UnsupportedBatchRayIntersections) +{ + std::vector unsupportedCollisionDetectors = + {"ode", "dart", "fcl", "banana"}; + + for (const std::string &name : this->pluginNames) + { + for (const std::string &collisionDetector : unsupportedCollisionDetectors) + { + auto world = LoadPluginAndWorld( + this->loader, + name, + common_test::worlds::kSphereSdf); + world->SetCollisionDetector(collisionDetector); + auto checkedOutput = + StepWorld(world, true, 1).first; + EXPECT_TRUE(checkedOutput); + + using World = gz::physics::World3d; + using RayQuery = World::RayQuery; + using RayIntersection = World::RayIntersection; + + // ray would hit the sphere, but the collision detector does not + // support ray intersection — results must be NaN + std::vector rays = { + {Eigen::Vector3d(-2, 0, 2), Eigen::Vector3d(2, 0, 2)}, + }; + + std::vector results = + world->GetBatchRayIntersectionFromLastStep(rays); + + ASSERT_EQ(1u, results.size()); + EXPECT_FALSE(results[0].hit); + EXPECT_TRUE(results[0].point.array().isNaN().all()); + EXPECT_TRUE(results[0].normal.array().isNaN().all()); + EXPECT_TRUE(std::isnan(results[0].fraction)); + } + } +} + +///////////////////////////////////////////////// +TYPED_TEST(SimulationFeaturesBatchRayIntersectionTest, + LargeBatchRayIntersections) +{ + for (const std::string &name : this->pluginNames) + { + auto world = LoadPluginAndWorld( + this->loader, + name, + common_test::worlds::kSphereSdf); + world->SetCollisionDetector("bullet"); + auto checkedOutput = + StepWorld(world, true, 1).first; + EXPECT_TRUE(checkedOutput); + + using World = gz::physics::World3d; + using RayQuery = World::RayQuery; + + // Sphere is centred at (0, 0, 2) with radius 1. + struct RayCase { RayQuery ray; bool expectedHit; }; + const std::vector cases = { + // equator crossings + {{Eigen::Vector3d(-2, 0, 2), Eigen::Vector3d(2, 0, 2)}, true}, + {{Eigen::Vector3d(0, -2, 2), Eigen::Vector3d(0, 2, 2)}, true}, + // chords above/below equator + {{Eigen::Vector3d(-2, 0, 2.5), Eigen::Vector3d(2, 0, 2.5)}, true}, + {{Eigen::Vector3d(-2, 0, 1.5), Eigen::Vector3d(2, 0, 1.5)}, true}, + // vertical shots + {{Eigen::Vector3d(0, 0, 5), Eigen::Vector3d(0, 0, 1)}, true}, + {{Eigen::Vector3d(0, 0, 10), Eigen::Vector3d(0, 0, 2)}, true}, + // repeat of first hit — verifies multiple hits in sequence + {{Eigen::Vector3d(0, -2, 2), Eigen::Vector3d(0, 2, 2)}, true}, + // misses + {{Eigen::Vector3d(2, 0, 10), Eigen::Vector3d(-2, 0, 10)}, false}, + {{Eigen::Vector3d(0, 2, 20), Eigen::Vector3d(0, -2, 20)}, false}, + {{Eigen::Vector3d(5, 5, 2), Eigen::Vector3d(10, 5, 2)}, false}, + {{Eigen::Vector3d(3, 3, 2), Eigen::Vector3d(5, 3, 2)}, false}, + {{Eigen::Vector3d(-2, 1.5, 2), Eigen::Vector3d(2, 1.5, 2)}, false}, + }; + + std::vector rays; + rays.reserve(cases.size()); + for (const auto &c : cases) + rays.push_back(c.ray); + + auto results = world->GetBatchRayIntersectionFromLastStep(rays); + + ASSERT_EQ(cases.size(), results.size()); + + for (std::size_t i = 0; i < cases.size(); ++i) + { + EXPECT_EQ(cases[i].expectedHit, results[i].hit) + << "ray index " << i << " hit mismatch"; + } + } +} + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv);