Skip to content

Commit

Permalink
fix linter offenses
Browse files Browse the repository at this point in the history
  • Loading branch information
romulogcerqueira committed Jan 17, 2025
1 parent 1cdde99 commit 07a158f
Show file tree
Hide file tree
Showing 3 changed files with 36 additions and 22 deletions.
6 changes: 4 additions & 2 deletions include/gz/sim/components/MultiRay.hh
Original file line number Diff line number Diff line change
Expand Up @@ -65,9 +65,11 @@ 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::components::Component<std::vector<RayIntersectionInfo>,
class MultiRayIntersectionsTag>;

GZ_SIM_REGISTER_COMPONENT("gz_sim_components.MultiRayIntersections", MultiRayIntersections)
GZ_SIM_REGISTER_COMPONENT(
"gz_sim_components.MultiRayIntersections", MultiRayIntersections)
}
}
}
Expand Down
18 changes: 10 additions & 8 deletions src/systems/physics/Physics.cc
Original file line number Diff line number Diff line change
Expand Up @@ -4183,8 +4183,8 @@ 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
// 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;

Expand All @@ -4200,9 +4200,9 @@ void PhysicsPrivate::UpdateRayIntersections(EntityComponentManager &_ecm)
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
// 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*/,
Expand All @@ -4212,7 +4212,7 @@ void PhysicsPrivate::UpdateRayIntersections(EntityComponentManager &_ecm)
// Retrieve the rays from the MultiRay component
const auto &rays = _multiRay->Data();

// Retrieve and clear the results from the MultiRayIntersections component
// Retrieve and clear results from MultiRayIntersections component
auto &rayIntersections = _multiRayIntersections->Data();
rayIntersections.clear();

Expand All @@ -4221,10 +4221,12 @@ void PhysicsPrivate::UpdateRayIntersections(EntityComponentManager &_ecm)
// Compute the ray intersections
auto rayIntersection =
worldRayIntersectionFeature->GetRayIntersectionFromLastStep(
math::eigen3::convert(ray.start), math::eigen3::convert(ray.end));
math::eigen3::convert(ray.start),
math::eigen3::convert(ray.end));

const auto result =
rayIntersection.Get<physics::World3d<RayIntersectionFeatureList>::RayIntersection>();
rayIntersection.Get<
physics::World3d<RayIntersectionFeatureList>::RayIntersection>();

// Store the results into the MultiRayIntersections component
components::RayIntersectionInfo info;
Expand Down
34 changes: 22 additions & 12 deletions test/integration/physics_system.cc
Original file line number Diff line number Diff line change
Expand Up @@ -2909,7 +2909,7 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(JointsInWorld))
}

//////////////////////////////////////////////////
/// This test verifies that ray intersections are computed by physics system during Update loop.
/// Test ray intersections computed by physics system during Update loop.
TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(RayIntersections))
{
ServerConfig serverConfig;
Expand All @@ -2930,9 +2930,10 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(RayIntersections))
testSystem.OnPreUpdate(
[&](const UpdateInfo &/*_info*/, EntityComponentManager &_ecm)
{
// Set the physics collision detector to bullet (that supports ray intersections).
// Set collision detector to bullet (supports ray intersections).
auto worldEntity = _ecm.EntityByComponents(components::World());
_ecm.CreateComponent(worldEntity, components::PhysicsCollisionDetector("bullet"));
_ecm.CreateComponent(
worldEntity, components::PhysicsCollisionDetector("bullet"));

// Create MultiRay and MultiRayIntersections components for testEntity1
testEntity1 = _ecm.CreateEntity();
Expand All @@ -2945,7 +2946,8 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(RayIntersections))
_ecm.CreateComponent(testEntity2, components::MultiRayIntersections());

// Add 5 rays to testEntity1 that intersect with the ground plane
auto &rays1 = _ecm.Component<components::MultiRay>(testEntity1)->Data();
auto &rays1 =
_ecm.Component<components::MultiRay>(testEntity1)->Data();
for (size_t i = 0; i < 5; ++i)
{
components::RayInfo ray;
Expand All @@ -2954,7 +2956,7 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(RayIntersections))
rays1.push_back(ray);
}

// Add 2 rays to testEntity2 that does not intersect with the ground plane
// Add 2 rays to testEntity2 that don't intersect with the ground plane
auto &rays2 = _ecm.Component<components::MultiRay>(testEntity2)->Data();
for (size_t i = 0; i < 2; ++i)
{
Expand All @@ -2964,13 +2966,15 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(RayIntersections))
rays2.push_back(ray);
}
});
// During PostUpdate, check the ray intersections for testEntity1 and testEntity2
// Check 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();
auto &results1 =
_ecm.Component<components::MultiRayIntersections>(
testEntity1)->Data();
ASSERT_EQ(rays1.size(), results1.size());

for (size_t i = 0; i < results1.size(); ++i) {
Expand All @@ -2983,14 +2987,20 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(RayIntersections))
}

// check the raycasting results for testEntity2
auto &rays2 = _ecm.Component<components::MultiRay>(testEntity2)->Data();
auto &results2 = _ecm.Component<components::MultiRayIntersections>(testEntity2)->Data();
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));
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));
}
});

Expand Down

0 comments on commit 07a158f

Please sign in to comment.