From 0b8f8befc7a15eafc2fecf3527b57a97edfc62d7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?R=C3=B4mulo=20Cerqueira?= <rcerqueira@krakenrobotics.com>
Date: Fri, 2 Aug 2024 11:00:50 -0300
Subject: [PATCH] computes ray intersections on update loop
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Rômulo Cerqueira <rcerqueira@krakenrobotics.com>
---
 src/systems/physics/Physics.cc | 72 ++++++++++++++++++++++++++++++++++
 1 file changed, 72 insertions(+)

diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
index 1d7ca23c157..b065753743e 100644
--- a/src/systems/physics/Physics.cc
+++ b/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<
@@ -643,6 +654,7 @@ class gz::sim::systems::PhysicsPrivate
           MinimumFeatureList,
           CollisionFeatureList,
           ContactFeatureList,
+          RayIntersectionFeatureList,
           SetContactPropertiesCallbackFeatureList,
           NestedModelFeatureList,
           CollisionDetectorFeatureList,
@@ -3737,6 +3749,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
 
@@ -3906,6 +3920,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