Add collisionShapeId to RayIntersectionT for raycast hit entity ident…

dartsim/src/SimulationFeatures.cc

@@ -199,7 +199,7 @@ void SimulationFeatures::Write(ChangedWorldPoses &_changedPoses) const
   this->prevLinkPoses = std::move(newPoses);
 }
 
-SimulationFeatures::RayIntersection
+SimulationFeatures::RayIntersectionInternal
 SimulationFeatures::GetRayIntersectionFromLastStep(
   const Identity &_worldID,
   const LinearVector3d &_from,
@@ -217,14 +217,27 @@ SimulationFeatures::GetRayIntersectionFromLastStep(
   // Currently, raycast supports only the Bullet collision detector.
   // For other collision detectors, the result will always be NaN.
   SimulationFeatures::RayIntersection intersection;
+  CompositeData extraData;
+
   if (result.hasHit())
   {
     // Store intersection data if there is a ray hit
     const auto &firstHit = result.mRayHits[0];
     intersection.point = firstHit.mPoint;
     intersection.normal = firstHit.mNormal;
     intersection.fraction = firstHit.mFraction;
-  }
+
+    // Map DART CollisionObject to gz-physics shape identity
+    if (firstHit.mCollisionObject)
+    {
+      auto *dtShapeFrame = firstHit.mCollisionObject->getShapeFrame();
+      if (dtShapeFrame && this->shapes.HasEntity(dtShapeFrame->asShapeNode()))
+      {
+        auto &extra = extraData.Get<SimulationFeatures::ExtraRayIntersectionData>();
+        extra.collisionShapeId = this->shapes.IdentityOf(dtShapeFrame->asShapeNode());
+      }
+    }
+}
   else
   {
     // Set invalid measurements to NaN according to REP-117
@@ -235,7 +248,7 @@ SimulationFeatures::GetRayIntersectionFromLastStep(
     intersection.fraction = std::numeric_limits<double>::quiet_NaN();
   }
 
-  return intersection;
+  return {intersection, extraData};
 }
 
 std::vector<SimulationFeatures::ContactInternal>

dartsim/src/SimulationFeatures.hh

@@ -118,7 +118,7 @@ class SimulationFeatures :
   public: std::vector<ContactInternal> GetContactsFromLastStep(
       const Identity &_worldID) const override;
 
-  public: RayIntersection GetRayIntersectionFromLastStep(
+  public: RayIntersectionInternal GetRayIntersectionFromLastStep(
       const Identity &_worldID,
       const LinearVector3d &_from,
       const LinearVector3d &_end) const override;

include/gz/physics/GetRayIntersection.hh

@@ -32,6 +32,14 @@ namespace physics
 class GZ_PHYSICS_VISIBLE GetRayIntersectionFromLastStepFeature
     : public virtual FeatureWithRequirements<ForwardStep>
 {
+  public: template <typename PolicyT>
+  struct ExtraRayIntersectionDataT
+  {
+    /// \brief The identity of the collision shape that was hit.
+    /// Set to 0 if no shape was hit.
+    std::size_t collisionShapeId{0};
+  };
+
   public: template <typename PolicyT>
   struct RayIntersectionT
   {
@@ -52,11 +60,12 @@ class GZ_PHYSICS_VISIBLE GetRayIntersectionFromLastStepFeature
   public: template <typename PolicyT, typename FeaturesT>
   class World : public virtual Feature::World<PolicyT, FeaturesT>
   {
+    public: using ExtraRayIntersectionData = ExtraRayIntersectionDataT<PolicyT>;
     public: using VectorType =
       typename FromPolicy<PolicyT>::template Use<LinearVector>;
     public: using RayIntersection = RayIntersectionT<PolicyT>;
     public: using RayIntersectionData =
-      SpecifyData<RequireData<RayIntersection>>;
+      SpecifyData<RequireData<RayIntersection>, ExpectData<ExtraRayIntersectionData> >;
 
     /// \brief Get ray intersection generated in the previous simulation step
     /// \param[in] _from The start point of the ray in world coordinates
@@ -68,11 +77,18 @@ class GZ_PHYSICS_VISIBLE GetRayIntersectionFromLastStepFeature
   public: template <typename PolicyT>
   class Implementation : public virtual Feature::Implementation<PolicyT>
   {
+    public: using ExtraRayIntersectionData = ExtraRayIntersectionDataT<PolicyT>;
     public: using RayIntersection = RayIntersectionT<PolicyT>;
     public: using VectorType =
       typename FromPolicy<PolicyT>::template Use<LinearVector>;
 
-    public: virtual RayIntersection GetRayIntersectionFromLastStep(
+    public: struct RayIntersectionInternal
+    {
+      RayIntersection intersection;
+      CompositeData extraData;
+    };  
+
+    public: virtual RayIntersectionInternal GetRayIntersectionFromLastStep(
       const Identity &_worldID,
       const VectorType &_from,
       const VectorType &_to) const = 0;

include/gz/physics/detail/GetRayIntersection.hh

@@ -36,10 +36,19 @@ auto GetRayIntersectionFromLastStepFeature::World<
     this->template Interface<GetRayIntersectionFromLastStepFeature>()
         ->GetRayIntersectionFromLastStep(this->identity, _from, _to);
 
-  RayIntersection intersection{result.point, result.fraction, result.normal};
+RayIntersection intersection{
+    result.intersection.point,
+    result.intersection.fraction,
+    result.intersection.normal};
 
   RayIntersectionData output;
   output.template Get<RayIntersection>() = std::move(intersection);
+
+  auto *extraData = result.extraData.template Query<ExtraRayIntersectionData>();
+  if(extraData)
+  {
+    output.template Get<ExtraRayIntersectionData>() = std::move(*extraData);
+  }
   return output;
 }