Provide the ability to visualize the configurations

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",
     ],
 )
 

geometry/proximity/test/boxes_overlap_test.cc

@@ -1,7 +1,23 @@
 #include "drake/geometry/proximity/boxes_overlap.h"
 
+#include <string>
+
+#include <fmt/format.h>
+#include <gflags/gflags.h>
 #include <gtest/gtest.h>
 
+#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,34 +142,69 @@ 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<Meshcat>();
+      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> meshcat_;
+};
+
+std::unique_ptr<Meshcat> 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
 // separating axis between the two bounding boxes. The first 3 cases use the
 // 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,19 +232,23 @@ 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
   // the order of the boxes and then using the inverse of the transform.
   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])));
     }
   }
 }