DynamicAABBTree: properly deal with plane and halfspaces

CHANGELOG.md

@@ -5,6 +5,7 @@ All notable changes to this project will be documented in this file.
 The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
 ## [Unreleased]
+- Enhance Broadphase DynamicAABBTree to better handle planes and halfspace ([#570](https://github.com/humanoid-path-planner/hpp-fcl/pull/570))
 - [#558](https://github.com/humanoid-path-planner/hpp-fcl/pull/558):
   - [internal] Removed dead code in `narrowphase/details.h` ([#558](https://github.com/humanoid-path-planner/hpp-fcl/pull/558))
   - [internal] Removed specializations of methods of `GJKSolver`. Now the specializations are all handled by `ShapeShapeDistance` in `shape_shape_func.h`.

include/hpp/fcl/BV/AABB.h

@@ -44,6 +44,8 @@ namespace hpp {
 namespace fcl {
 
 struct CollisionRequest;
+class Plane;
+class Halfspace;
 
 /// @defgroup Bounding_Volume Bounding volumes
 /// Classes of different types of bounding volume.
@@ -119,6 +121,12 @@ class HPP_FCL_DLLAPI AABB {
     return true;
   }
 
+  /// @brief Check whether AABB overlaps a plane
+  bool overlap(const Plane& p) const;
+
+  /// @brief Check whether AABB overlaps a halfspace
+  bool overlap(const Halfspace& hs) const;
+
   /// @brief Check whether two AABB are overlap
   bool overlap(const AABB& other, const CollisionRequest& request,
                FCL_REAL& sqrDistLowerBound) const;

include/hpp/fcl/broadphase/default_broadphase_callbacks.h

@@ -203,7 +203,7 @@ struct HPP_FCL_DLLAPI CollisionCallBackDefault : CollisionCallBackBase {
 
   CollisionData data;
 
-  virtual ~CollisionCallBackDefault(){};
+  virtual ~CollisionCallBackDefault() {};
 };
 
 /// @brief Default distance callback to check collision between collision
@@ -217,7 +217,7 @@ struct HPP_FCL_DLLAPI DistanceCallBackDefault : DistanceCallBackBase {
 
   DistanceData data;
 
-  virtual ~DistanceCallBackDefault(){};
+  virtual ~DistanceCallBackDefault() {};
 };
 
 /// @brief Collision callback to collect collision pairs potentially in contacts
@@ -244,7 +244,7 @@ struct HPP_FCL_DLLAPI CollisionCallBackCollect : CollisionCallBackBase {
   /// @brief Check whether a collision pair exists
   bool exist(CollisionObject* o1, CollisionObject* o2) const;
 
-  virtual ~CollisionCallBackCollect(){};
+  virtual ~CollisionCallBackCollect() {};
 
  protected:
   std::vector<CollisionPair> collision_pairs;

include/hpp/fcl/broadphase/detail/sparse_hash_table.h

@@ -53,7 +53,7 @@ class unordered_map_hash_table : public std::unordered_map<U, V> {
   typedef std::unordered_map<U, V> Base;
 
  public:
-  unordered_map_hash_table() : Base(){};
+  unordered_map_hash_table() : Base() {};
 };
 
 /// @brief A hash table implemented using unordered_map

include/hpp/fcl/collision.h

@@ -126,7 +126,7 @@ class HPP_FCL_DLLAPI ComputeCollision {
     return !(*this == other);
   }
 
-  virtual ~ComputeCollision(){};
+  virtual ~ComputeCollision() {};
 
  protected:
   // These pointers are made mutable to let the derived classes to update

include/hpp/fcl/distance.h

@@ -121,7 +121,7 @@ class HPP_FCL_DLLAPI ComputeDistance {
     return !(*this == other);
   }
 
-  virtual ~ComputeDistance(){};
+  virtual ~ComputeDistance() {};
 
  protected:
   // These pointers are made mutable to let the derived classes to update

include/hpp/fcl/narrowphase/gjk.h

@@ -420,7 +420,7 @@ struct HPP_FCL_DLLAPI EPA {
                               // (with 0 <= i <= 2).
     size_t pass;
 
-    SimplexFace() : n(Vec3f::Zero()), ignore(false){};
+    SimplexFace() : n(Vec3f::Zero()), ignore(false) {};
   };
 
   /// @brief The simplex list of EPA is a linked list of faces.

include/hpp/fcl/shape/geometric_shapes.h

@@ -67,7 +67,7 @@ class HPP_FCL_DLLAPI ShapeBase : public CollisionGeometry {
 
   ShapeBase& operator=(const ShapeBase& other) = default;
 
-  virtual ~ShapeBase(){};
+  virtual ~ShapeBase() {};
 
   /// @brief Get object type: a geometric shape
   OBJECT_TYPE getObjectType() const { return OT_GEOM; }
@@ -108,7 +108,7 @@ class HPP_FCL_DLLAPI ShapeBase : public CollisionGeometry {
 /// @brief Triangle stores the points instead of only indices of points
 class HPP_FCL_DLLAPI TriangleP : public ShapeBase {
  public:
-  TriangleP(){};
+  TriangleP() {};
 
   TriangleP(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_)
       : ShapeBase(), a(a_), b(b_), c(c_) {}

src/BV/AABB.cpp

@@ -36,6 +36,7 @@
 /** \author Jia Pan */
 
 #include <hpp/fcl/BV/AABB.h>
+#include <hpp/fcl/shape/geometric_shapes.h>
 
 #include <limits>
 #include <hpp/fcl/collision_data.h>
@@ -144,6 +145,53 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
   return bb1.overlap(b2, request, sqrDistLowerBound);
 }
 
+bool AABB::overlap(const Plane& p) const {
+  // Convert AABB to a (box, transform) representation and compute the support
+  // points in the directions normal and -normal.
+  // If both points lie on different sides of the plane, there is an overlap
+  // between the AABB and the plane. Otherwise, there is no overlap.
+  const Vec3f halfside = (this->max_ - this->min_) / 2;
+  const Vec3f center = (this->max_ + this->min_) / 2;
+
+  const Vec3f support1 = (p.n.array() > 0).select(halfside, -halfside) + center;
+  const Vec3f support2 =
+      ((-p.n).array() > 0).select(halfside, -halfside) + center;
+
+  const FCL_REAL dist1 = p.n.dot(support1) - p.d;
+  const FCL_REAL dist2 = p.n.dot(support2) - p.d;
+  const int sign1 = (dist1 > 0) ? 1 : -1;
+  const int sign2 = (dist2 > 0) ? 1 : -1;
+
+  if (p.getSweptSphereRadius() > 0) {
+    if (sign1 != sign2) {
+      // Supports are on different sides of the plane. There is an overlap.
+      return true;
+    }
+    // Both supports are on the same side of the plane.
+    // We now need to check if they are on the same side of the plane inflated
+    // by the swept-sphere radius.
+    const FCL_REAL ssr_dist1 = std::abs(dist1) - p.getSweptSphereRadius();
+    const FCL_REAL ssr_dist2 = std::abs(dist2) - p.getSweptSphereRadius();
+    const int ssr_sign1 = (ssr_dist1 > 0) ? 1 : -1;
+    const int ssr_sign2 = (ssr_dist2 > 0) ? 1 : -1;
+    return ssr_sign1 != ssr_sign2;
+  }
+
+  return (sign1 != sign2);
+}
+
+bool AABB::overlap(const Halfspace& hs) const {
+  // Convert AABB to a (box, transform) representation and compute the support
+  // points in the direction -normal.
+  // If the support is below the plane defined by the halfspace, there is an
+  // overlap between the AABB and the halfspace. Otherwise, there is no
+  // overlap.
+  Vec3f halfside = (this->max_ - this->min_) / 2;
+  Vec3f center = (this->max_ + this->min_) / 2;
+  Vec3f support = ((-hs.n).array() > 0).select(halfside, -halfside) + center;
+  return (hs.signedDistance(support) < 0);
+}
+
 }  // namespace fcl
 
 }  // namespace hpp

src/broadphase/broadphase_dynamic_AABB_tree.cpp

@@ -248,17 +248,103 @@ bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
 
 #endif
 
+//==============================================================================
+bool leafCollide(CollisionObject* o1, CollisionObject* o2,
+                 CollisionCallBackBase* callback) {
+  if ((o1->getNodeType() == GEOM_HALFSPACE ||
+       o1->getNodeType() == GEOM_PLANE) &&
+      (o2->getNodeType() == GEOM_HALFSPACE ||
+       o2->getNodeType() == GEOM_PLANE)) {
+    // Halfspace-plane / Halfspace-plane collision, there is no need to check
+    // AABBs. We can directly use the callback.
+    return (*callback)(o1, o2);
+  }
+
+  bool overlap = false;
+  if (o1->getNodeType() == GEOM_HALFSPACE || o1->getNodeType() == GEOM_PLANE) {
+    if (o1->getNodeType() == GEOM_HALFSPACE) {
+      const auto& halfspace =
+          static_cast<const Halfspace&>(*(o1->collisionGeometryPtr()));
+      overlap = o2->getAABB().overlap(transform(halfspace, o1->getTransform()));
+    } else {
+      const auto& plane =
+          static_cast<const Plane&>(*(o1->collisionGeometryPtr()));
+      overlap = o2->getAABB().overlap(transform(plane, o1->getTransform()));
+    }
+  }  //
+  else if (o2->getNodeType() == GEOM_HALFSPACE ||
+           o2->getNodeType() == GEOM_PLANE) {
+    if (o2->getNodeType() == GEOM_HALFSPACE) {
+      const auto& halfspace =
+          static_cast<const Halfspace&>(*(o2->collisionGeometryPtr()));
+      overlap = o1->getAABB().overlap(transform(halfspace, o2->getTransform()));
+    } else {
+      const auto& plane =
+          static_cast<const Plane&>(*(o2->collisionGeometryPtr()));
+      overlap = o1->getAABB().overlap(transform(plane, o2->getTransform()));
+    }
+  }  //
+  else {
+    overlap = o1->getAABB().overlap(o2->getAABB());
+  }
+
+  if (overlap) {
+    return (*callback)(o1, o2);
+  }
+  return false;
+}
+
+//==============================================================================
+bool nodeCollide(DynamicAABBTreeCollisionManager::DynamicAABBNode* node1,
+                 DynamicAABBTreeCollisionManager::DynamicAABBNode* node2) {
+  // This function assumes that at least node1 or node2 is not a leaf of the
+  // tree.
+  if (node1->isLeaf()) {
+    CollisionObject* o1 = static_cast<CollisionObject*>(node1->data);
+    if (o1->getNodeType() == GEOM_HALFSPACE ||
+        o1->getNodeType() == GEOM_PLANE) {
+      if (o1->getNodeType() == GEOM_HALFSPACE) {
+        const auto& halfspace =
+            static_cast<const Halfspace&>(*(o1->collisionGeometryPtr()));
+        return node2->bv.overlap(transform(halfspace, o1->getTransform()));
+      }
+      const auto& plane =
+          static_cast<const Plane&>(*(o1->collisionGeometryPtr()));
+      return node2->bv.overlap(transform(plane, o1->getTransform()));
+    }
+  }
+
+  if (node2->isLeaf()) {
+    CollisionObject* o2 = static_cast<CollisionObject*>(node2->data);
+    if (o2->getNodeType() == GEOM_HALFSPACE ||
+        o2->getNodeType() == GEOM_PLANE) {
+      if (o2->getNodeType() == GEOM_HALFSPACE) {
+        const auto& halfspace =
+            static_cast<const Halfspace&>(*(o2->collisionGeometryPtr()));
+        return node1->bv.overlap(transform(halfspace, o2->getTransform()));
+      }
+      const auto& plane =
+          static_cast<const Plane&>(*(o2->collisionGeometryPtr()));
+      return node1->bv.overlap(transform(plane, o2->getTransform()));
+    }
+  }
+
+  return node1->bv.overlap(node2->bv);
+}
+
 //==============================================================================
 bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
                       DynamicAABBTreeCollisionManager::DynamicAABBNode* root2,
                       CollisionCallBackBase* callback) {
   if (root1->isLeaf() && root2->isLeaf()) {
-    if (!root1->bv.overlap(root2->bv)) return false;
-    return (*callback)(static_cast<CollisionObject*>(root1->data),
-                       static_cast<CollisionObject*>(root2->data));
+    CollisionObject* o1 = static_cast<CollisionObject*>(root1->data);
+    CollisionObject* o2 = static_cast<CollisionObject*>(root2->data);
+    return leafCollide(o1, o2, callback);
   }
 
-  if (!root1->bv.overlap(root2->bv)) return false;
+  if (!nodeCollide(root1, root2)) {
+    return false;
+  }
 
   if (root2->isLeaf() ||
       (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) {
@@ -275,11 +361,21 @@ bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
 bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root,
                       CollisionObject* query, CollisionCallBackBase* callback) {
   if (root->isLeaf()) {
-    if (!root->bv.overlap(query->getAABB())) return false;
-    return (*callback)(static_cast<CollisionObject*>(root->data), query);
+    CollisionObject* leaf = static_cast<CollisionObject*>(root->data);
+    return leafCollide(leaf, query, callback);
   }
 
-  if (!root->bv.overlap(query->getAABB())) return false;
+  // Create a temporary node, attached to no tree.
+  // This allows to reuse the `nodeCollide` function, which only checks for
+  // overlap between the AABBs of two nodes.
+  DynamicAABBTreeCollisionManager::DynamicAABBNode query_node;
+  query_node.data = query;
+  query_node.bv = query->getAABB();
+  query_node.parent = nullptr;
+  query_node.children[1] = nullptr;
+  if (!nodeCollide(root, &query_node)) {
+    return false;
+  }
 
   size_t select_res =
       select(query->getAABB(), *(root->children[0]), *(root->children[1]));