Add GetBatchRayIntersectionFromLastStepFeature for CPU lidar

dartsim/src/GzCollisionDetector.cc

@@ -22,6 +22,8 @@
 
 #include <dart/collision/CollisionObject.hpp>
 
+#include <BulletCollision/CollisionDispatch/btCollisionWorld.h>
+
 #include "GzCollisionDetector.hh"
 
 using namespace dart;
@@ -99,6 +101,16 @@ void GzCollisionDetector::LimitCollisionPairMaxContacts(
   }
 }
 
+/////////////////////////////////////////////////
+bool GzCollisionDetector::BatchRaycast(
+    CollisionGroup */*_group*/,
+    const std::vector<GzRay> &/*_rays*/,
+    std::vector<GzRayResult> &_results) const
+{
+  _results.clear();
+  return false;
+}
+
 /////////////////////////////////////////////////
 GzOdeCollisionDetector::GzOdeCollisionDetector()
   : OdeCollisionDetector(), GzCollisionDetector()
@@ -149,12 +161,33 @@ bool GzOdeCollisionDetector::collide(
   return ret;
 }
 
+/////////////////////////////////////////////////
+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<dart::collision::CollisionGroup>
+GzBulletCollisionDetector::createCollisionGroup()
+{
+  return std::make_unique<GzBulletCollisionGroup>(this->shared_from_this());
+}
+
 /////////////////////////////////////////////////
 GzBulletCollisionDetector::Registrar<GzBulletCollisionDetector>
     GzBulletCollisionDetector::mRegistrar{
@@ -193,3 +226,56 @@ bool GzBulletCollisionDetector::collide(
   this->LimitCollisionPairMaxContacts(_result);
   return ret;
 }
+
+/////////////////////////////////////////////////
+bool GzBulletCollisionDetector::BatchRaycast(
+    CollisionGroup *_group,
+    const std::vector<GzRay> &_rays,
+    std::vector<GzRayResult> &_results) const
+{
+  _results.clear();
+  _results.reserve(_rays.size());
+
+  auto *gzGroup = dynamic_cast<GzBulletCollisionGroup *>(_group);
+  if (!gzGroup)
+  {
+    _results.resize(_rays.size());
+    return true;
+  }
+
+  const btCollisionWorld *btWorld = gzGroup->getCollisionWorld();
+  if (!btWorld)
+  {
+    _results.resize(_rays.size());
+    return true;
+  }
+
+  for (const auto &ray : _rays)
+  {
+    const btVector3 btFrom(
+      static_cast<btScalar>(ray.from.x()),
+      static_cast<btScalar>(ray.from.y()),
+      static_cast<btScalar>(ray.from.z()));
+    const btVector3 btTo(
+      static_cast<btScalar>(ray.to.x()),
+      static_cast<btScalar>(ray.to.y()),
+      static_cast<btScalar>(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<double>(rayCallback.m_closestHitFraction);
+    }
+    _results.push_back(std::move(result));
+  }
+
+  return true;
+}

dartsim/src/GzCollisionDetector.hh

@@ -21,14 +21,36 @@
 #include <cstdio>
 #include <limits>
 #include <memory>
+#include <vector>
+
+#include <Eigen/Core>
 
 #include <dart/collision/CollisionResult.hpp>
 #include <dart/collision/bullet/BulletCollisionDetector.hpp>
+#include <dart/collision/bullet/BulletCollisionGroup.hpp>
 #include <dart/collision/ode/OdeCollisionDetector.hpp>
 
 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.
+struct GzRayResult
+{
+  bool hit{false};
+  Eigen::Vector3d point{
+    Eigen::Vector3d::Constant(std::numeric_limits<double>::quiet_NaN())};
+  double fraction{std::numeric_limits<double>::quiet_NaN()};
+  Eigen::Vector3d normal{
+    Eigen::Vector3d::Constant(std::numeric_limits<double>::quiet_NaN())};
+};
+
 class GzCollisionDetector
 {
   /// \brief Set the maximum number of contacts between a pair of collision
@@ -42,6 +64,20 @@ 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.
+  /// \param[out] _results One result per input ray, in the same order.
+  /// \return True if the detector supports batch raycasting, false otherwise.
+  ///   When false, _results is left empty and the caller should fall back.
+  public: virtual bool BatchRaycast(
+      CollisionGroup *_group,
+      const std::vector<GzRay> &_rays,
+      std::vector<GzRayResult> &_results) const;
+
+  /// Destructor
+  public: virtual ~GzCollisionDetector() = default;
+
   /// Constructor
   protected: GzCollisionDetector();
 
@@ -83,6 +119,17 @@ class GzOdeCollisionDetector :
   private: static Registrar<GzOdeCollisionDetector> mRegistrar;
 };
 
+/// \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;
+};
+
 class GzBulletCollisionDetector :
     public dart::collision::BulletCollisionDetector,
     public dart::collision::GzCollisionDetector
@@ -100,6 +147,15 @@ class GzBulletCollisionDetector :
       const CollisionOption& option = CollisionOption(false, 1u, nullptr),
       CollisionResult* result = nullptr) override;
 
+  // Documentation inherited
+  public: std::unique_ptr<CollisionGroup> createCollisionGroup() override;
+
+  // Documentation inherited
+  public: bool BatchRaycast(
+      CollisionGroup *_group,
+      const std::vector<GzRay> &_rays,
+      std::vector<GzRayResult> &_results) const override;
+
   /// \brief Create the GzBulletCollisionDetector
   public: static std::shared_ptr<GzBulletCollisionDetector> create();
 

dartsim/src/SimulationFeatures.cc

@@ -21,7 +21,9 @@
 #include <sstream>
 #include <string>
 #include <unordered_map>
+#include <unordered_set>
 #include <utility>
+#include <vector>
 
 
 #include <dart/collision/CollisionObject.hpp>
@@ -38,8 +40,10 @@
 #include <gz/math/Pose3.hh>
 #include <gz/math/eigen3/Conversions.hh>
 
+#include "gz/physics/GetBatchRayIntersection.hh"
 #include "gz/physics/GetContacts.hh"
 
+#include "GzCollisionDetector.hh"
 #include "SimulationFeatures.hh"
 
 #if DART_VERSION_AT_LEAST(6, 13, 0)
@@ -238,6 +242,68 @@ SimulationFeatures::GetRayIntersectionFromLastStep(
   return intersection;
 }
 
+/////////////////////////////////////////////////
+std::vector<SimulationFeatures::BatchRayIntersection>
+SimulationFeatures::GetBatchRayIntersectionFromLastStep(
+  const Identity &_worldID,
+  const std::vector<SimulationFeatures::BatchRayQuery> &_rays) const
+{
+  std::vector<SimulationFeatures::BatchRayIntersection> results;
+  results.reserve(_rays.size());
+
+  if (_rays.empty())
+    return results;
+
+  constexpr double kNaN = std::numeric_limits<double>::quiet_NaN();
+  const Eigen::Vector3d kNaNVec = Eigen::Vector3d::Constant(kNaN);
+
+  auto *const world = this->ReferenceInterface<DartWorld>(_worldID);
+  auto *const solver = world->getConstraintSolver();
+
+  auto detector = solver->getCollisionDetector();
+  auto *gzDetector =
+    dynamic_cast<dart::collision::GzCollisionDetector *>(detector.get());
+
+  if (gzDetector)
+  {
+    std::vector<dart::collision::GzRay> gzRays;
+    gzRays.reserve(_rays.size());
+    for (const auto &ray : _rays)
+      gzRays.push_back({ray.origin, ray.target});
+
+    std::vector<dart::collision::GzRayResult> gzResults;
+    if (gzDetector->BatchRaycast(
+          solver->getCollisionGroup().get(), gzRays, gzResults))
+    {
+      for (const auto &r : gzResults)
+      {
+        SimulationFeatures::BatchRayIntersection intersection;
+        intersection.hit = r.hit;
+        intersection.point = r.point;
+        intersection.fraction = r.fraction;
+        intersection.normal = r.normal;
+        results.push_back(std::move(intersection));
+      }
+      return results;
+    }
+  }
+
+  // Collision detector does not support batch raycasting.
+  // Warn once per detector type and return NaN for all rays.
+  static std::unordered_set<std::string> warnedDetectors;
+  const std::string detectorType = detector->getType();
+  if (warnedDetectors.find(detectorType) == warnedDetectors.end())
+  {
+    warnedDetectors.insert(detectorType);
+    gzwarn << "GetBatchRayIntersectionFromLastStep: collision detector ["
+           << detectorType << "] does not support batch raycasting. "
+           << "All ray results will be NaN.\n";
+  }
+
+  results.assign(_rays.size(), {false, kNaNVec, kNaN, kNaNVec});
+  return results;
+}
+
 std::vector<SimulationFeatures::ContactInternal>
 SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const
 {

dartsim/src/SimulationFeatures.hh

@@ -33,6 +33,7 @@
 #include <gz/physics/CanWriteData.hh>
 
 #include <gz/physics/ForwardStep.hh>
+#include <gz/physics/GetBatchRayIntersection.hh>
 #include <gz/physics/GetContacts.hh>
 #include <gz/physics/GetRayIntersection.hh>
 #include <gz/physics/ContactProperties.hh>
@@ -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<BatchRayIntersection> GetBatchRayIntersectionFromLastStep(
+      const Identity &_worldID,
+      const std::vector<BatchRayQuery> &_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<std::size_t, math::Pose3d> prevLinkPoses;