move the ray intersection tests to test/common_test/

gazebosim · Jun 7, 2024 · 9e16c04 · 9e16c04
1 parent add7805
commit 9e16c04
Show file tree

Hide file tree

Showing 2 changed files with 98 additions and 142 deletions.
diff --git a/dartsim/src/SimulationFeatures_TEST.cc b/dartsim/src/SimulationFeatures_TEST.cc
diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc
@@ -37,6 +37,7 @@
 
 #include "gz/physics/BoxShape.hh"
 #include <gz/physics/GetContacts.hh>
+#include <gz/physics/GetRayIntersection.hh>
 #include "gz/physics/ContactProperties.hh"
 #include "gz/physics/CylinderShape.hh"
 #include "gz/physics/CapsuleShape.hh"
@@ -1233,6 +1234,103 @@ TYPED_TEST(SimulationFeaturesTestBasic, MultipleCollisions)
   }
 }
 
+/////////////////////////////////////////////////
+// The features that an engine must have to be loaded by this loader.
+struct FeaturesRayIntersections : gz::physics::FeatureList<
+  gz::physics::sdf::ConstructSdfWorld,
+  gz::physics::GetRayIntersectionFromLastStepFeature,
+  gz::physics::CollisionDetector,
+  gz::physics::ForwardStep
+> {};
+
+template <class T>
+class SimulationFeaturesRayIntersectionTest :
+  public SimulationFeaturesTest<T>{};
+using SimulationFeaturesRayIntersectionTestTypes =
+    ::testing::Types<FeaturesRayIntersections>;
+TYPED_TEST_SUITE(SimulationFeaturesRayIntersectionTest,
+                 SimulationFeaturesRayIntersectionTestTypes);
+
+TYPED_TEST(SimulationFeaturesRayIntersectionTest, SupportedRayIntersections)
+{
+  std::vector<std::string> supportedCollisionDetectors = {"bullet"};
+
+  for (const std::string &name : this->pluginNames)
+  {
+    CHECK_UNSUPPORTED_ENGINE(name, "bullet", "bullet-featherstone", "tpe")
+
+    for (const std::string &collisionDetector : supportedCollisionDetectors) {
+      auto world = LoadPluginAndWorld<FeaturesRayIntersections>(
+          this->loader,
+          name,
+          common_test::worlds::kSphereSdf);
+      world->SetCollisionDetector(collisionDetector);
+      auto checkedOutput = StepWorld<FeaturesRayIntersections>(world, true, 1).first;
+      EXPECT_TRUE(checkedOutput);
+
+      // ray hits the sphere
+      auto result =
+        world->GetRayIntersectionFromLastStep(
+            Eigen::Vector3d(-2, 0, 2), Eigen::Vector3d(2, 0, 2));
+
+      auto rayIntersection =
+          result.template
+            Get<gz::physics::World3d<FeaturesRayIntersections>::RayIntersection>();
+
+      double epsilon = 1e-3;
+      EXPECT_TRUE(
+          rayIntersection.point.isApprox(Eigen::Vector3d(-1, 0, 2), epsilon));
+      EXPECT_TRUE(
+          rayIntersection.normal.isApprox(Eigen::Vector3d(-1, 0, 0), epsilon));
+      EXPECT_DOUBLE_EQ(rayIntersection.fraction, 0.25);
+
+      // ray does not hit the sphere
+      result = world->GetRayIntersectionFromLastStep(
+          Eigen::Vector3d(2, 0, 10), Eigen::Vector3d(-2, 0, 10));
+      rayIntersection =
+          result.template
+            Get<gz::physics::World3d<FeaturesRayIntersections>::RayIntersection>();
+
+      ASSERT_TRUE(rayIntersection.point.array().isNaN().any());
+      ASSERT_TRUE(rayIntersection.normal.array().isNaN().any());
+      ASSERT_TRUE(std::isnan(rayIntersection.fraction));
+    }
+  }
+}
+
+TYPED_TEST(SimulationFeaturesRayIntersectionTest, UnsupportedRayIntersections)
+{
+  std::vector<std::string> unsupportedCollisionDetectors = {"ode", "dart", "fcl", "banana"};
+
+  for (const std::string &name : this->pluginNames)
+  {
+    CHECK_UNSUPPORTED_ENGINE(name, "bullet", "bullet-featherstone", "tpe")
+
+    for (const std::string &collisionDetector : unsupportedCollisionDetectors) {
+      auto world = LoadPluginAndWorld<FeaturesRayIntersections>(
+          this->loader,
+          name,
+          common_test::worlds::kSphereSdf);
+      world->SetCollisionDetector(collisionDetector);
+      auto checkedOutput = StepWorld<FeaturesRayIntersections>(world, true, 1).first;
+      EXPECT_TRUE(checkedOutput);
+
+      // ray would hit the sphere, but the collision detector does
+      // not support ray intersection
+      auto result = world->GetRayIntersectionFromLastStep(
+          Eigen::Vector3d(-2, 0, 2), Eigen::Vector3d(2, 0, 2));
+
+      auto rayIntersection =
+          result.template
+            Get<gz::physics::World3d<FeaturesRayIntersections>::RayIntersection>();
+
+      ASSERT_TRUE(rayIntersection.point.array().isNaN().any());
+      ASSERT_TRUE(rayIntersection.normal.array().isNaN().any());
+      ASSERT_TRUE(std::isnan(rayIntersection.fraction));
+    }
+  }
+}
+
 int main(int argc, char *argv[])
 {
   ::testing::InitGoogleTest(&argc, argv);