Skip to content

Commit

Permalink
computes ray intersections on update loop
Browse files Browse the repository at this point in the history
Signed-off-by: Rômulo Cerqueira <[email protected]>
  • Loading branch information
Rômulo Cerqueira committed Aug 6, 2024
1 parent e545b43 commit 2767eee
Showing 1 changed file with 72 additions and 0 deletions.
72 changes: 72 additions & 0 deletions src/systems/physics/Physics.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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"
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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<
Expand Down Expand Up @@ -651,6 +662,7 @@ class gz::sim::systems::PhysicsPrivate
MinimumFeatureList,
CollisionFeatureList,
ContactFeatureList,
RayIntersectionFeatureList,
SetContactPropertiesCallbackFeatureList,
NestedModelFeatureList,
CollisionDetectorFeatureList,
Expand Down Expand Up @@ -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

Expand Down Expand Up @@ -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
Expand Down

0 comments on commit 2767eee

Please sign in to comment.