Physics: Ray intersections

include/gz/sim/components/MultiRay.hh

@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) 2024 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_SIM_COMPONENTS_MULTIRAY_HH_
+#define GZ_SIM_COMPONENTS_MULTIRAY_HH_
+
+#include <gz/math/Vector3.hh>
+#include <gz/sim/components/Factory.hh>
+#include <gz/sim/components/Component.hh>
+#include <gz/sim/config.hh>
+
+#include <vector>
+
+namespace gz
+{
+namespace sim
+{
+// Inline bracket to help doxygen filtering.
+inline namespace GZ_SIM_VERSION_NAMESPACE {
+namespace components
+{
+/// \brief A struct that holds the information of a ray.
+struct RayInfo
+{
+  /// \brief Starting point of the ray in world coordinates
+  gz::math::Vector3d start;
+
+  /// \brief Ending point of the ray in world coordinates
+  gz::math::Vector3d end;
+};
+
+/// \brief A struct that holds the results of raycasting.
+struct RayIntersectionInfo
+{
+  /// \brief The hit point in the world coordinates
+  gz::math::Vector3d point;
+
+  /// \brief The fraction of the ray length at the intersection/hit point.
+  double fraction;
+
+  /// \brief The normal at the hit point in the world coordinates
+  gz::math::Vector3d normal;
+};
+
+/// \brief A component type that contains multiple rays from an entity.
+using MultiRay =
+  gz::sim::components::Component<std::vector<RayInfo>, class MultiRayTag>;
+
+GZ_SIM_REGISTER_COMPONENT("gz_sim_components.MultiRay", MultiRay)
+
+/// \brief A component type that contains the raycasting results from multiple
+// rays from an entity into a physics world.
+using MultiRayIntersections =
+  gz::sim::components::Component<std::vector<RayIntersectionInfo>, class MultiRayIntersectionsTag>;
+
+GZ_SIM_REGISTER_COMPONENT("gz_sim_components.MultiRayIntersections", MultiRayIntersections)
+}
+}
+}
+}
+#endif  // GZ_SIM_COMPONENTS_MULTIRAY_HH_

src/systems/physics/Physics.cc

@@ -63,6 +63,7 @@
 #include <gz/physics/GetContacts.hh>
 #include <gz/physics/GetBoundingBox.hh>
 #include <gz/physics/GetEntities.hh>
+#include <gz/physics/GetRayIntersection.hh>
 #include <gz/physics/Joint.hh>
 #include <gz/physics/Link.hh>
 #include <gz/physics/RemoveEntities.hh>
@@ -126,6 +127,7 @@
 #include "gz/sim/components/LinearVelocityCmd.hh"
 #include "gz/sim/components/Link.hh"
 #include "gz/sim/components/Model.hh"
+#include "gz/sim/components/MultiRay.hh"
 #include "gz/sim/components/Name.hh"
 #include "gz/sim/components/ParentEntity.hh"
 #include "gz/sim/components/ParentLinkName.hh"
@@ -311,6 +313,10 @@ class gz::sim::systems::PhysicsPrivate
   /// \param[in] _ecm Mutable reference to ECM.
   public: void UpdateCollisions(EntityComponentManager &_ecm);
 
+  /// \brief Update ray intersection components from physics simulation
+  /// \param[in] _ecm Mutable reference to ECM.
+  public: void UpdateRayIntersections(EntityComponentManager &_ecm);
+
   /// \brief FrameData relative to world at a given offset pose
   /// \param[in] _link gz-physics link
   /// \param[in] _pose Offset pose in which to compute the frame data
@@ -513,6 +519,11 @@ class gz::sim::systems::PhysicsPrivate
             CollisionFeatureList,
             physics::GetContactsFromLastStepFeature>{};
 
+  /// \brief Feature list to handle ray intersection information.
+  public: struct RayIntersectionFeatureList : physics::FeatureList<
+            MinimumFeatureList,
+            physics::GetRayIntersectionFromLastStepFeature>{};
+
   /// \brief Feature list to change contacts before they are applied to physics.
   public: struct SetContactPropertiesCallbackFeatureList :
             physics::FeatureList<
@@ -651,6 +662,7 @@ class gz::sim::systems::PhysicsPrivate
           MinimumFeatureList,
           CollisionFeatureList,
           ContactFeatureList,
+          RayIntersectionFeatureList,
           SetContactPropertiesCallbackFeatureList,
           NestedModelFeatureList,
           CollisionDetectorFeatureList,
@@ -3782,6 +3794,8 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm,
 
   // TODO(louise) Skip this if there are no collision features
   this->UpdateCollisions(_ecm);
+
+  this->UpdateRayIntersections(_ecm);
 }  // NOLINT readability/fn_size
 // TODO (azeey) Reduce size of function and remove the NOLINT above
 
@@ -3963,6 +3977,64 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm)
       });
 }
 
+//////////////////////////////////////////////////
+void PhysicsPrivate::UpdateRayIntersections(EntityComponentManager &_ecm)
+{
+  GZ_PROFILE("PhysicsPrivate::UpdateRayIntersections");
+  // Quit early if the MultiRayIntersections component hasn't been created. This means
+  // there are no systems that need contact information
+  if (!_ecm.HasComponentType(components::MultiRayIntersections::typeId))
+    return;
+
+  // Assumes that there is only one world entity
+  Entity worldEntity = _ecm.EntityByComponents(components::World());
+
+  if (!this->entityWorldMap.HasEntity(worldEntity))
+  {
+    gzwarn << "Failed to find world [" << worldEntity << "]." << std::endl;
+    return;
+  }
+
+  auto worldRayIntersectionFeature =
+      this->entityWorldMap.EntityCast<RayIntersectionFeatureList>(worldEntity);
+
+  // Go through each entity that has a MultiRay and MultiRayIntersections components,
+  // trace the rays and set the MultiRayIntersections component value to the list of
+  // intersections that correspond to the ray entity
+  _ecm.Each<components::MultiRay,
+            components::MultiRayIntersections>(
+      [&](const Entity &/*_entity*/,
+          components::MultiRay *_multiRay,
+          components::MultiRayIntersections *_multiRayIntersections) -> bool
+      {
+        // Retrieve the rays from the MultiRay component
+        const auto &rays = _multiRay->Data();
+
+        // Retrieve and clear the results from the MultiRayIntersections component
+        auto &rayIntersections = _multiRayIntersections->Data();
+        rayIntersections.clear();
+
+        for (const auto &ray : rays)
+        {
+          // Compute the ray intersections
+          auto rayIntersection =
+            worldRayIntersectionFeature->GetRayIntersectionFromLastStep(
+              math::eigen3::convert(ray.start), math::eigen3::convert(ray.end));
+
+          const auto result =
+            rayIntersection.Get<physics::World3d<RayIntersectionFeatureList>::RayIntersection>();
+
+          // Store the results into the MultiRayIntersections component
+          components::RayIntersectionInfo info;
+          info.point = math::eigen3::convert(result.point);
+          info.fraction = result.fraction;
+          info.normal = math::eigen3::convert(result.normal);
+          rayIntersections.push_back(info);
+        }
+        return true;
+      });
+}
+
 //////////////////////////////////////////////////
 physics::FrameData3d PhysicsPrivate::LinkFrameDataAtOffset(
       const LinkPtrType &_link, const math::Pose3d &_pose) const

test/integration/physics_system.cc

@@ -37,6 +37,8 @@
 #include <sdf/Sphere.hh>
 #include <sdf/World.hh>
 
+#include <gz/math/eigen3/Conversions.hh>
+
 #include "gz/sim/Entity.hh"
 #include "gz/sim/EntityComponentManager.hh"
 #include "gz/sim/Link.hh"
@@ -69,6 +71,7 @@
 #include "gz/sim/components/LinearVelocity.hh"
 #include "gz/sim/components/Material.hh"
 #include "gz/sim/components/Model.hh"
+#include "gz/sim/components/MultiRay.hh"
 #include "gz/sim/components/Name.hh"
 #include "gz/sim/components/ParentEntity.hh"
 #include "gz/sim/components/Physics.hh"
@@ -2621,3 +2624,93 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(JointsInWorld))
   server.AddSystem(testSystem.systemPtr);
   server.Run(true, 1000, false);
 }
+
+//////////////////////////////////////////////////
+/// Tests that ray intersections are computed by physics system during Update loop.
+TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(RayIntersections))
+{
+  ServerConfig serverConfig;
+
+  const auto sdfFile =
+    common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "empty.sdf");
+
+  serverConfig.SetSdfFile(sdfFile);
+  Server server(serverConfig);
+  server.SetUpdatePeriod(1ns);
+
+  Entity testEntity1;
+  Entity testEntity2;
+
+  test::Relay testSystem;
+
+  // During PreUpdate, add rays to testEntity1 and testEntity2
+  testSystem.OnPreUpdate(
+      [&](const UpdateInfo &/*_info*/, EntityComponentManager &_ecm)
+      {
+        // Set the physics collision detector to bullet (that supports ray intersections).
+        auto worldEntity = _ecm.EntityByComponents(components::World());
+        _ecm.CreateComponent(worldEntity, components::PhysicsCollisionDetector("bullet"));
+
+        // Create MultiRay and MultiRayIntersections components for testEntity1
+        testEntity1 = _ecm.CreateEntity();
+        _ecm.CreateComponent(testEntity1, components::MultiRay());
+        _ecm.CreateComponent(testEntity1, components::MultiRayIntersections());
+
+        // Create MultiRay and MultiRayIntersections components for testEntity2
+        testEntity2 = _ecm.CreateEntity();
+        _ecm.CreateComponent(testEntity2, components::MultiRay());
+        _ecm.CreateComponent(testEntity2, components::MultiRayIntersections());
+
+        // Add 5 rays to testEntity1 that intersect with the ground plane
+        auto &rays1 = _ecm.Component<components::MultiRay>(testEntity1)->Data();
+        for (size_t i = 0; i < 5; ++i)
+        {
+          components::RayInfo ray;
+          ray.start = math::Vector3d(0, 0, 10 - i);
+          ray.end = math::Vector3d(0, 0, -10);
+          rays1.push_back(ray);
+        }
+
+        // Add 2 rays to testEntity2 that does not intersect with the ground plane
+        auto &rays2 = _ecm.Component<components::MultiRay>(testEntity2)->Data();
+        for (size_t i = 0; i < 2; ++i)
+        {
+          components::RayInfo ray;
+          ray.start = math::Vector3d(0, 0, 10 - i);
+          ray.end = math::Vector3d(0, 0, 5);
+          rays2.push_back(ray);
+        }
+      });
+  // During PostUpdate, check the ray intersections for testEntity1 and testEntity2
+  testSystem.OnPostUpdate(
+      [&](const UpdateInfo &/*_info*/, const EntityComponentManager &_ecm)
+      {
+        // check the raycasting results for testEntity1
+        auto &rays1 = _ecm.Component<components::MultiRay>(testEntity1)->Data();
+        auto &results1 = _ecm.Component<components::MultiRayIntersections>(testEntity1)->Data();
+        ASSERT_EQ(rays1.size(), results1.size());
+
+        for (size_t i = 0; i < results1.size(); ++i) {
+          ASSERT_EQ(results1[i].point, math::Vector3d::Zero);
+          ASSERT_EQ(results1[i].normal, math::Vector3d(0, 0, 1));
+          double exp_fraction =
+            (rays1[i].start - results1[i].point).Length() /
+              (rays1[i].start - rays1[i].end).Length();
+          ASSERT_NEAR(results1[i].fraction, exp_fraction, 1e-6);
+        }
+
+        // check the raycasting results for testEntity2
+        auto &rays2 = _ecm.Component<components::MultiRay>(testEntity2)->Data();
+        auto &results2 = _ecm.Component<components::MultiRayIntersections>(testEntity2)->Data();
+        ASSERT_EQ(rays2.size(), results2.size());
+
+        for (size_t i = 0; i < results2.size(); ++i) {
+          ASSERT_TRUE(math::eigen3::convert(results2[i].point).array().isNaN().all());
+          ASSERT_TRUE(math::eigen3::convert(results2[i].normal).array().isNaN().all());
+          ASSERT_TRUE(std::isnan(results2[i].fraction));
+        }
+      });
+
+  server.AddSystem(testSystem.systemPtr);
+  server.Run(true, 1, false);
+}