How can we use margin in broad phase?

[Everest-Zhao]

Hi I noticed that we have a margin parameter to determine if objects are colliding based on the margin.
However, I am using the DynamicAABBTreeCollisionManager to calculate collisions, and I found that setting the margin not works. This happens because the broad phase's leafCollide and nodeCollide do not consider the margin. Therefore, I added a function to address this issue. So far, it seems to work effectively with both mesh and convex objects. Do you think this is the right approach, or is there a better method?

//==============================================================================
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 leafCollideWithMargin(CollisionObject* o1, CollisionObject* o2,
                           CollisionCallBackDefault* 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 {
    auto request = callback->data.request;
    FCL_REAL sqrDistLowerBound;
    overlap = o1->getAABB().overlap(o2->getAABB(), request, sqrDistLowerBound);
  }

  if (overlap) {
    return (*callback)(o1, o2);
  }
  return false;
}

[Everest-Zhao]

It's the same issue as this topic security_margin and collision_distance_threshold of DynamicAABBTreeCollisionManager