Fix FCL mesh contact regression and add test (#2258)

Author: jslee02

dart/collision/fcl/FCLCollisionDetector.cpp

@@ -1100,9 +1100,21 @@ bool collisionCallback(
   ::fcl::collide(o1, o2, fclRequest, fclResult);
 
   if (result) {
+    const bool usingMeshContacts
+        = (collData->primitiveShapeType == FCLCollisionDetector::MESH);
+    const bool forcingMeshFallback = usingMeshContacts
+                                     && collData->contactPointComputationMethod
+                                            == FCLCollisionDetector::FCL;
+
     // Post processing -- converting fcl contact information to ours if needed
-    if (FCLCollisionDetector::DART == collData->contactPointComputationMethod
-        && FCLCollisionDetector::MESH == collData->primitiveShapeType) {
+    if (usingMeshContacts) {
+      if (forcingMeshFallback) {
+        DART_WARN_ONCE(
+            "FCL mesh contact-point computation is known to be inaccurate "
+            "(see flexible-collision-library/fcl#106); falling back to DART's "
+            "mesh contact handler.");
+      }
+
       postProcessDART(fclResult, o1, o2, option, *result);
     } else {
       postProcessFCL(fclResult, o1, o2, option, *result);

tests/integration/CMakeLists.txt

@@ -10,6 +10,13 @@ if(TARGET dart-utils)
   target_link_libraries(INTEGRATION_collision_Collision dart-utils)
 endif()
 
+if(DART_BUILD_COLLISION_FCL)
+  dart_add_test(
+    "integration"
+    INTEGRATION_collision_MeshContactRegression
+    collision/test_MeshContactRegression.cpp)
+endif()
+
 # Additional collision tests
 if(TARGET dart-utils-urdf)
   dart_add_test("integration" INTEGRATION_collision_SelfCollisionFiltering collision/test_SelfCollisionFiltering.cpp)

tests/integration/collision/test_MeshContactRegression.cpp

@@ -0,0 +1,93 @@
+/*
+ * Copyright (c) 2011-2025, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/main/LICENSE
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <dart/collision/CollisionResult.hpp>
+#include <dart/collision/fcl/FCLCollisionDetector.hpp>
+
+#include <dart/dynamics/BoxShape.hpp>
+#include <dart/dynamics/Frame.hpp>
+#include <dart/dynamics/SimpleFrame.hpp>
+
+#include <Eigen/Core>
+#include <cmath>
+#include <gtest/gtest.h>
+
+using dart::collision::FCLCollisionDetector;
+
+// Reproduces the face-to-face mesh box contact from DART issue #860.
+// The detector is intentionally configured to use FCL's contact computation,
+// and the test asserts that DART still returns sensible contact points on the
+// touching face.
+TEST(CollisionRegression, MeshMeshContactPointsStayOnContactPlane)
+{
+  auto detector = FCLCollisionDetector::create();
+  detector->setPrimitiveShapeType(FCLCollisionDetector::MESH);
+  detector->setContactPointComputationMethod(FCLCollisionDetector::FCL);
+
+  auto shape = std::make_shared<dart::dynamics::BoxShape>(
+      Eigen::Vector3d::Ones());
+
+  auto bottom = dart::dynamics::SimpleFrame::createShared(
+      dart::dynamics::Frame::World());
+  auto top = dart::dynamics::SimpleFrame::createShared(
+      dart::dynamics::Frame::World());
+
+  bottom->setShape(shape);
+  top->setShape(shape);
+
+  bottom->setTranslation(Eigen::Vector3d::Zero());
+  top->setTranslation(Eigen::Vector3d(0.0, 0.0, 1.0));
+
+  auto group = detector->createCollisionGroup(bottom.get(), top.get());
+
+  dart::collision::CollisionOption option;
+  option.enableContact = true;
+  option.maxNumContacts = 20u;
+
+  dart::collision::CollisionResult result;
+  const bool collided = group->collide(option, &result);
+  ASSERT_TRUE(collided);
+  ASSERT_GT(result.getNumContacts(), 0u);
+
+  const double half = 0.5;
+  const double tol = 1e-3;
+
+  for (std::size_t i = 0; i < result.getNumContacts(); ++i) {
+    const auto& contact = result.getContact(i);
+
+    EXPECT_NEAR(contact.point[2], half, tol) << "Contact index: " << i;
+    EXPECT_LE(contact.point.head<2>().cwiseAbs().maxCoeff(), half + tol);
+
+    EXPECT_NEAR(std::abs(contact.normal[2]), 1.0, 1e-6);
+    EXPECT_LT(std::abs(contact.normal[0]) + std::abs(contact.normal[1]), 1e-3);
+  }
+}

tests/integration/dynamics/test_DynamicsHybrid.cpp

@@ -1,6 +1,6 @@
-#include "DynamicsTestFixture.hpp"
 #include "helpers/dynamics_helpers.hpp"
 
+#include "DynamicsTestFixture.hpp"
 #include "dart/dynamics/Inertia.hpp"
 #include "dart/math/Random.hpp"
 #include "dart/simulation/World.hpp"