diff --git a/geometry/proximity/BUILD.bazel b/geometry/proximity/BUILD.bazel index 3562b1ac28f5..f61b07abbecc 100644 --- a/geometry/proximity/BUILD.bazel +++ b/geometry/proximity/BUILD.bazel @@ -972,6 +972,10 @@ drake_cc_googletest( name = "boxes_overlap_test", deps = [ ":boxes_overlap", + "//common/test_utilities:maybe_pause_for_user", + "//geometry:meshcat", + "//geometry:rgba", + "//geometry:shape_specification", ], ) diff --git a/geometry/proximity/test/boxes_overlap_test.cc b/geometry/proximity/test/boxes_overlap_test.cc index 802b2ce0f86b..10d1121d89bc 100644 --- a/geometry/proximity/test/boxes_overlap_test.cc +++ b/geometry/proximity/test/boxes_overlap_test.cc @@ -1,7 +1,23 @@ #include "drake/geometry/proximity/boxes_overlap.h" +#include + +#include +#include #include +#include "drake/common/test_utilities/maybe_pause_for_user.h" +#include "drake/common/text_logging.h" +#include "drake/geometry/meshcat.h" +#include "drake/geometry/rgba.h" +#include "drake/geometry/shape_specification.h" + +DEFINE_bool( + visualize, false, + "If true, visualizes the test cases. To use this, run the test as " + "./bazel-bin/geometry/proximity/boxes_overlap_test --visualize. " + "You will have the opportunity to open a meshcat browser session."); + namespace drake { namespace geometry { namespace internal { @@ -126,18 +142,52 @@ RigidTransformd CalcEdgeTransform(const Vector3d& a_half, return RigidTransformd(R_AB, p_AoBo_A); } -// Tests to see if the two oriented bounding boxes overlap. The boxes are -// represented with vectors containing their half sizes as measured in their -// own frames. This tests A against B and B against A. If the two queries return -// different results, that is an error condition. Otherwise, reports the result -// of BoxesOverlap(). -bool InvokeBoxesOverlap(const Vector3d& a_half, const Vector3d& b_half, - const RigidTransformd& X_AB) { - const bool a_to_b = BoxesOverlap(a_half, b_half, X_AB); - const bool b_to_a = BoxesOverlap(b_half, a_half, X_AB.inverse()); - DRAKE_DEMAND(a_to_b == b_to_a); - return a_to_b; -} +class BoxesOverlapTest_ : public ::testing::Test { + public: + static void SetUpTestSuite() { + if (FLAGS_visualize) { + meshcat_ = std::make_unique(); + drake::log()->info( + "Please open a browser to the indicated URL before proceeding. When " + "the test is complete, the browser will appear empty, but the path " + "drake/BoxesOverlapTest/ should contain all of the test cases."); + common::MaybePauseForUser(); + } + } + + protected: + // Tests to see if the two oriented bounding boxes overlap. The boxes are + // represented with vectors containing their half sizes as measured in their + // own frames. This tests A against B and B against A. If the two queries + // return different results, that is an error condition. Otherwise, reports + // the result of BoxesOverlap(). + static bool InvokeBoxesOverlap(const Vector3d& a_half, const Vector3d& b_half, + const RigidTransformd& X_AB, + const std::string& label) { + const bool a_to_b = BoxesOverlap(a_half, b_half, X_AB); + const bool b_to_a = BoxesOverlap(b_half, a_half, X_AB.inverse()); + DrawCase(a_half, b_half, X_AB, "BoxesOverlapTest/" + label); + DRAKE_DEMAND(a_to_b == b_to_a); + return a_to_b; + } + + static void DrawCase(const Vector3d& a_half, const Vector3d& b_half, + const RigidTransformd& X_AB, const std::string& path) { + if (FLAGS_visualize) { + DRAKE_DEMAND(meshcat_ != nullptr); + const std::string a_path = path + "/a"; + meshcat_->SetObject(a_path, Box(a_half * 2), Rgba(0.2, 0.2, 0.8, 0.8)); + const std::string b_path = path + "/b"; + meshcat_->SetObject(b_path, Box(b_half * 2), Rgba(0.8, 0.5, 0.2, 0.8)); + meshcat_->SetTransform(b_path, X_AB); + meshcat_->SetProperty(path, "visible", false); + } + } + + static std::unique_ptr meshcat_; +}; + +std::unique_ptr BoxesOverlapTest_::meshcat_{nullptr}; // Tests whether OBBs overlap. There are 15 unique possibilities for // non-overlap. Therefore, there are 15 cases to test, each covering a different @@ -145,15 +195,16 @@ bool InvokeBoxesOverlap(const Vector3d& a_half, const Vector3d& b_half, // axes of Frame A, the next 3 cases use the axes of Frame B, and the remaining // 9 cases use the axes defined by the cross product of axes from Frame A and // Frame B. We also test that it is robust for the case of parallel boxes. -GTEST_TEST(BoxesOverlapTest, AllCases) { +TEST_F(BoxesOverlapTest_, AllCases) { // Each box simply gets posed in a common frame, G. + const char* axes[] = {"x", "y", "z"}; // One box is fully contained in the other and they are parallel. // (This transform will get repeatedly overwritten below.) RigidTransformd X_AB(Vector3d(0.2, 0.4, 0.2)); Vector3d a(1, 2, 1); Vector3d b(0.5, 1, 0.5); - EXPECT_TRUE(InvokeBoxesOverlap(a, b, X_AB)); + EXPECT_TRUE(InvokeBoxesOverlap(a, b, X_AB, "parallel_and_contained")); // The first cases are where the separating plane is perpendicular to an axis // of frame A. So, we pose box B along each axis. For example, in the case of @@ -181,9 +232,11 @@ GTEST_TEST(BoxesOverlapTest, AllCases) { b = Vector3d(3.5, 2, 1.5); for (int axis = 0; axis < 3; ++axis) { X_AB = CalcCornerTransform(a, b, axis, false /* expect_overlap */); - EXPECT_FALSE(InvokeBoxesOverlap(a, b, X_AB)); + EXPECT_FALSE(InvokeBoxesOverlap(a, b, X_AB, + fmt::format("A{} separated", axes[axis]))); X_AB = CalcCornerTransform(a, b, axis, true /* expect_overlap */); - EXPECT_TRUE(InvokeBoxesOverlap(a, b, X_AB)); + EXPECT_TRUE(InvokeBoxesOverlap(a, b, X_AB, + fmt::format("A{} colliding", axes[axis]))); } // To cover the local axes out of B, we can use the same method by swapping @@ -191,9 +244,11 @@ GTEST_TEST(BoxesOverlapTest, AllCases) { for (int axis = 0; axis < 3; ++axis) { X_AB = CalcCornerTransform(b, a, axis, false /* expect_overlap */).inverse(); - EXPECT_FALSE(InvokeBoxesOverlap(a, b, X_AB)); + EXPECT_FALSE(InvokeBoxesOverlap(a, b, X_AB, + fmt::format("B{} separated", axes[axis]))); X_AB = CalcCornerTransform(b, a, axis, true /* expect_overlap */).inverse(); - EXPECT_TRUE(InvokeBoxesOverlap(a, b, X_AB)); + EXPECT_TRUE(InvokeBoxesOverlap(a, b, X_AB, + fmt::format("B{} colliding", axes[axis]))); } // To cover the remaining 9 cases, we need to pose an edge from box B along @@ -225,10 +280,14 @@ GTEST_TEST(BoxesOverlapTest, AllCases) { X_AB = CalcEdgeTransform(a, b, a_axis, b_axis, false /* expect_overlap */); // Separate along a's y-axis and b's x-axis. - EXPECT_FALSE(InvokeBoxesOverlap(a, b, X_AB)); + EXPECT_FALSE(InvokeBoxesOverlap( + a, b, X_AB, + fmt::format("A{}-B{} separated", axes[a_axis], axes[b_axis]))); X_AB = CalcEdgeTransform(a, b, a_axis, b_axis, true /* expect_overlap */); // Separate along a's y-axis and b's x-axis. - EXPECT_TRUE(InvokeBoxesOverlap(a, b, X_AB)); + EXPECT_TRUE(InvokeBoxesOverlap( + a, b, X_AB, + fmt::format("A{}-B{} colliding", axes[a_axis], axes[b_axis]))); } } }