Fix plane collision handling for Bullet and FCL (#2246)

Author: jslee02

dart/collision/bullet/BulletCollisionGroup.cpp

@@ -36,6 +36,8 @@
 #include "dart/collision/bullet/BulletCollisionObject.hpp"
 #include "dart/collision/bullet/detail/BulletCollisionDispatcher.hpp"
 
+#include <BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h>
+
 #if !defined(_WIN32)
   #include <LinearMath/btAlignedAllocator.h>
 
@@ -180,7 +182,8 @@ BulletCollisionGroup::BulletCollisionGroup(
         mBulletCollisionConfiguration.get()))
 {
   ensureBulletAllocator();
-  // Do nothing
+  btGImpactCollisionAlgorithm::registerAlgorithm(
+      static_cast<btCollisionDispatcher*>(mBulletDispatcher.get()));
 }
 
 //==============================================================================

dart/collision/fcl/FCLCollisionDetector.cpp

@@ -985,21 +985,18 @@ FCLCollisionDetector::createFCLCollisionGeometry(
     // Use mesh since FCL doesn't support pyramid shape.
     geom = createPyramid<fcl::OBBRSS>(*pyramid, fcl::getTransform3Identity());
   } else if (PlaneShape::getStaticType() == shapeType) {
-    if (FCLCollisionDetector::PRIMITIVE == type) {
-      DART_ASSERT(dynamic_cast<const PlaneShape*>(shape.get()));
-      auto plane = static_cast<const PlaneShape*>(shape.get());
-      const Eigen::Vector3d normal = plane->getNormal();
-      const double offset = plane->getOffset();
-
-      geom = new fcl::Halfspace(FCLTypes::convertVector3(normal), offset);
-    } else {
-      geom = createCube<fcl::OBBRSS>(1000.0, 0.0, 1000.0);
-
-      DART_WARN(
-          "[FCLCollisionDetector] PlaneShape is not supported by "
-          "FCLCollisionDetector. We create a thin box mesh instead, where the "
-          "size is [1000 0 1000].");
-    }
+    DART_ASSERT(dynamic_cast<const PlaneShape*>(shape.get()));
+    const auto plane = static_cast<const PlaneShape*>(shape.get());
+    const Eigen::Vector3d normal = plane->getNormal();
+    const double offset = plane->getOffset();
+
+    geom = new fcl::Halfspace(FCLTypes::convertVector3(normal), offset);
+
+    DART_WARN_ONCE_IF(
+        FCLCollisionDetector::MESH == type,
+        "[FCLCollisionDetector] PlaneShape requested with primitive shape type "
+        "MESH. Using analytic halfspace instead of the previous thin-box "
+        "fallback to keep plane collisions reliable.");
   } else if (MeshShape::getStaticType() == shapeType) {
     DART_ASSERT(dynamic_cast<const MeshShape*>(shape.get()));
 

tests/integration/collision/test_PlaneShapeCollision.cpp

@@ -37,6 +37,7 @@
 #include <dart/constraint/ConstraintSolver.hpp>
 
 #include <dart/collision/bullet/BulletCollisionDetector.hpp>
+#include <dart/collision/fcl/FCLCollisionDetector.hpp>
 #include <dart/collision/ode/OdeCollisionDetector.hpp>
 
 #include <dart/dynamics/All.hpp>
@@ -111,9 +112,6 @@ void runIssue1234Test(
   const auto box
       = std::make_shared<dart::dynamics::BoxShape>(bb.getMax() - bb.getMin());
 
-  const auto sphere = std::make_shared<dart::dynamics::SphereShape>(
-      bb.getMax()[0] - bb.getMin()[0]);
-
   std::size_t numTests = 0;
   std::size_t numPasses = 0;
 
@@ -172,3 +170,14 @@ TEST(Issue1234, Ode)
   runIssue1234Test(
       [] { return dart::collision::OdeCollisionDetector::create(); });
 }
+
+//==============================================================================
+TEST(Issue1234, Fcl)
+{
+  runIssue1234Test([] {
+    auto detector = dart::collision::FCLCollisionDetector::create();
+    detector->setPrimitiveShapeType(
+        dart::collision::FCLCollisionDetector::PRIMITIVE);
+    return detector;
+  });
+}