diff --git a/dartsim/src/SimulationFeatures_TEST.cc b/dartsim/src/SimulationFeatures_TEST.cc deleted file mode 100644 index 49f3e51dc..000000000 --- a/dartsim/src/SimulationFeatures_TEST.cc +++ /dev/null @@ -1,142 +0,0 @@ -/* - * 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. - * - */ - -#include - -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include "test/Utils.hh" -#include "test/common_test/Worlds.hh" - -struct TestFeatureList : gz::physics::FeatureList< - gz::physics::CollisionDetector, - gz::physics::ForwardStep, - gz::physics::sdf::ConstructSdfWorld, - gz::physics::GetRayIntersectionFromLastStepFeature -> { }; - -using namespace gz; - -using TestEnginePtr = physics::Engine3dPtr; -using TestWorldPtr = physics::World3dPtr; - -class SimulationFeaturesFixture : public ::testing::TestWithParam -{ - protected: void SetUp() override - { - gz::plugin::Loader loader; - loader.LoadLib(dartsim_plugin_LIB); - - gz::plugin::PluginPtr dartsim = - loader.Instantiate("gz::physics::dartsim::Plugin"); - - this->engine = - gz::physics::RequestEngine3d::From(dartsim); - ASSERT_NE(nullptr, this->engine); - } - protected: TestEnginePtr engine; -}; - -TestWorldPtr LoadWorld( - const TestEnginePtr &_engine, - const std::string &_sdfFile) -{ - sdf::Root root; - const sdf::Errors errors = root.Load(_sdfFile); - EXPECT_TRUE(errors.empty()); - const sdf::World *sdfWorld = root.WorldByIndex(0); - return _engine->ConstructWorld(*sdfWorld); -} - -// Test suite for raycasting with supported collision detectors -class RayIntersectionSupportedFixture : public SimulationFeaturesFixture {}; - -// Test suite for raycasting with unsupported collision detectors -class RayIntersectionNotSupportedFixture : public SimulationFeaturesFixture {}; - -TEST_P(RayIntersectionSupportedFixture, RayIntersection) -{ - const auto world = LoadWorld(this->engine, common_test::worlds::kSphereSdf); - world->SetCollisionDetector(GetParam()); - - // ray hits the sphere - auto result = world->GetRayIntersectionFromLastStep( - Eigen::Vector3d(-2, 0, 2), Eigen::Vector3d(2, 0, 2)); - - auto rayIntersection = - result.template - Get::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::RayIntersection>(); - - ASSERT_TRUE(rayIntersection.point.array().isNaN().any()); - ASSERT_TRUE(rayIntersection.normal.array().isNaN().any()); - ASSERT_TRUE(std::isnan(rayIntersection.fraction)); -} - -TEST_P(RayIntersectionNotSupportedFixture, RayIntersection) -{ - const auto world = LoadWorld(this->engine, common_test::worlds::kSphereSdf); - world->SetCollisionDetector(GetParam()); - - // 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::RayIntersection>(); - - ASSERT_TRUE(rayIntersection.point.array().isNaN().any()); - ASSERT_TRUE(rayIntersection.normal.array().isNaN().any()); - ASSERT_TRUE(std::isnan(rayIntersection.fraction)); -} - -// Parameterized instantiation of test suites -INSTANTIATE_TEST_SUITE_P(CollisionDetectorsSupported, - RayIntersectionSupportedFixture, - ::testing::Values("bullet")); - -INSTANTIATE_TEST_SUITE_P(CollisionDetectorsNotSupported, - RayIntersectionNotSupportedFixture, - ::testing::Values("ode", "dart", "fcl", "banana")); diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index d92e474f9..83afc7bdd 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -37,6 +37,7 @@ #include "gz/physics/BoxShape.hh" #include +#include #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 SimulationFeaturesRayIntersectionTest : + public SimulationFeaturesTest{}; +using SimulationFeaturesRayIntersectionTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(SimulationFeaturesRayIntersectionTest, + SimulationFeaturesRayIntersectionTestTypes); + +TYPED_TEST(SimulationFeaturesRayIntersectionTest, SupportedRayIntersections) +{ + std::vector 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( + this->loader, + name, + common_test::worlds::kSphereSdf); + world->SetCollisionDetector(collisionDetector); + auto checkedOutput = StepWorld(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::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::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 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( + this->loader, + name, + common_test::worlds::kSphereSdf); + world->SetCollisionDetector(collisionDetector); + auto checkedOutput = StepWorld(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::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);