diff --git a/dartsim/src/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc index 3623abe3c..a6708dbbf 100644 --- a/dartsim/src/SimulationFeatures.cc +++ b/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,6 +217,8 @@ 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 @@ -224,6 +226,19 @@ SimulationFeatures::GetRayIntersectionFromLastStep( 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(); + extra.collisionShapeId = + this->shapes.IdentityOf(dtShapeFrame->asShapeNode()); + } + } } else { @@ -235,7 +250,7 @@ SimulationFeatures::GetRayIntersectionFromLastStep( intersection.fraction = std::numeric_limits::quiet_NaN(); } - return intersection; + return {intersection, extraData}; } std::vector diff --git a/dartsim/src/SimulationFeatures.hh b/dartsim/src/SimulationFeatures.hh index c55c2d34f..f2411fbba 100644 --- a/dartsim/src/SimulationFeatures.hh +++ b/dartsim/src/SimulationFeatures.hh @@ -118,7 +118,7 @@ class SimulationFeatures : public: std::vector GetContactsFromLastStep( const Identity &_worldID) const override; - public: RayIntersection GetRayIntersectionFromLastStep( + public: RayIntersectionInternal GetRayIntersectionFromLastStep( const Identity &_worldID, const LinearVector3d &_from, const LinearVector3d &_end) const override; diff --git a/include/gz/physics/GetRayIntersection.hh b/include/gz/physics/GetRayIntersection.hh index 1c9e31223..68adcd7e5 100644 --- a/include/gz/physics/GetRayIntersection.hh +++ b/include/gz/physics/GetRayIntersection.hh @@ -22,6 +22,7 @@ #include #include #include +#include namespace gz { @@ -32,6 +33,13 @@ namespace physics class GZ_PHYSICS_VISIBLE GetRayIntersectionFromLastStepFeature : public virtual FeatureWithRequirements { + public: template + struct ExtraRayIntersectionDataT + { + /// \brief The identity of the collision shape that was hit. + std::size_t collisionShapeId{INVALID_ENTITY_ID}; + }; + public: template struct RayIntersectionT { @@ -52,11 +60,14 @@ class GZ_PHYSICS_VISIBLE GetRayIntersectionFromLastStepFeature public: template class World : public virtual Feature::World { + public: using ExtraRayIntersectionData = ExtraRayIntersectionDataT; public: using VectorType = typename FromPolicy::template Use; public: using RayIntersection = RayIntersectionT; + public: using RayIntersectionData = - SpecifyData>; + SpecifyData, + ExpectData>; /// \brief Get ray intersection generated in the previous simulation step /// \param[in] _from The start point of the ray in world coordinates @@ -68,11 +79,18 @@ class GZ_PHYSICS_VISIBLE GetRayIntersectionFromLastStepFeature public: template class Implementation : public virtual Feature::Implementation { + public: using ExtraRayIntersectionData = ExtraRayIntersectionDataT; public: using RayIntersection = RayIntersectionT; public: using VectorType = typename FromPolicy::template Use; - 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; diff --git a/include/gz/physics/detail/GetRayIntersection.hh b/include/gz/physics/detail/GetRayIntersection.hh index 8448ea608..2ea73dfd5 100644 --- a/include/gz/physics/detail/GetRayIntersection.hh +++ b/include/gz/physics/detail/GetRayIntersection.hh @@ -36,10 +36,18 @@ auto GetRayIntersectionFromLastStepFeature::World< this->template Interface() ->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() = std::move(intersection); + + auto *extraData = result.extraData.template Query(); + if(extraData) + { + output.template Get() = std::move(*extraData); + } return output; } diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index 049de9006..5e95cdc31 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -2211,6 +2211,13 @@ TYPED_TEST(SimulationFeaturesRayIntersectionTest, SupportedRayIntersections) rayIntersection.normal.isApprox(Eigen::Vector3d(-1, 0, 0), epsilon)); EXPECT_DOUBLE_EQ(rayIntersection.fraction, 0.25); + // Check that the extra data contains a valid collision shape ID + auto *extraData = result.template + Query::ExtraRayIntersectionData>(); + ASSERT_NE(nullptr, extraData); + EXPECT_NE(0u, extraData->collisionShapeId); + + // ray does not hit the sphere result = world->GetRayIntersectionFromLastStep( Eigen::Vector3d(2, 0, 10), Eigen::Vector3d(-2, 0, 10)); @@ -2221,6 +2228,11 @@ TYPED_TEST(SimulationFeaturesRayIntersectionTest, SupportedRayIntersections) ASSERT_TRUE(rayIntersection.point.array().isNaN().any()); ASSERT_TRUE(rayIntersection.normal.array().isNaN().any()); ASSERT_TRUE(std::isnan(rayIntersection.fraction)); + + // Check that extra data has no valid collision shape ID for a miss + auto *extraDataMiss = result.template + Query::ExtraRayIntersectionData>(); + EXPECT_EQ(nullptr, extraDataMiss); } } } @@ -2254,6 +2266,11 @@ TYPED_TEST(SimulationFeaturesRayIntersectionTest, UnsupportedRayIntersections) ASSERT_TRUE(rayIntersection.point.array().isNaN().any()); ASSERT_TRUE(rayIntersection.normal.array().isNaN().any()); ASSERT_TRUE(std::isnan(rayIntersection.fraction)); + + auto *extraData = result.template + Query::ExtraRayIntersectionData>(); + EXPECT_EQ(nullptr, extraData); + } } }