Failure to detect collison with mesh soup in DynamicAABBTreeCollisionManager

[velecto1]

Hello, I am having trouble with a simple program to detect collision between a mesh soup and a box.

I have written it according to the README of this repository, partially inspired by the code from fcl_examples: https://github.com/RyodoTanaka/fcl_examples/blob/master/src/broadphase/bp_dynamic_aabb_tree.cpp

CMakeLists.txt (using my fork of FCL to avoid issues with compiling Eigen3, to program itself should work with the original FCL as well):

cmake_minimum_required(VERSION 3.26)
project(fcl_example)

set(CMAKE_CXX_STANDARD 17)

include(FetchContent)

FetchContent_Declare(
        fcl
        GIT_REPOSITORY https://github.com/velecto1/fcl.git # https://github.com/flexible-collision-library/fcl.git
        GIT_TAG origin/master
        GIT_SHALLOW TRUE
        GIT_PROGRESS TRUE)
FetchContent_MakeAvailable(fcl)

add_executable(fcl_example main.cpp)
target_link_libraries(fcl_example PRIVATE fcl)

main.cpp:

#include <iostream>

#include "fcl/math/bv/utility.h"
#include "fcl/narrowphase/detail/gjk_solver_indep.h"
#include "fcl/narrowphase/detail/traversal/collision_node.h"
#include "fcl/broadphase/broadphase_dynamic_AABB_tree.h"
#include "fcl/broadphase/default_broadphase_callbacks.h"

int main() {
  ///////////////////////////////////////////// prepare variables for bigCuboid
  std::vector<fcl::Vector3<double>> bigCuboid_points = {
      {9.5,  0,  3.8},
      {9.5,  0,  7.5},
      {9.5,  20, 3.8},
      {9.5,  20, 7.5},
      {10.5, 0,  3.8},
      {10.5, 0,  7.5},
      {10.5, 20, 3.8},
      {10.5, 20, 7.5}
  };
  std::vector<fcl::Triangle> bigCuboid_triangles = {
      {0, 1, 3},
      {0, 3, 2},
      {2, 3, 7},
      {2, 7, 6},
      {6, 7, 5},
      {6, 5, 4},
      {4, 5, 1},
      {4, 1, 0},
      {2, 6, 4},
      {2, 4, 0},
      {7, 3, 1},
      {7, 1, 5}
  };

  auto bigCuboid_BVHModel = std::make_shared<fcl::BVHModel<fcl::OBBRSS<double>>>();
  bigCuboid_BVHModel->beginModel();
  bigCuboid_BVHModel->addSubModel(bigCuboid_points, bigCuboid_triangles);
  bigCuboid_BVHModel->endModel();

  auto bigCuboid_CollisionObject = fcl::CollisionObject<double>(bigCuboid_BVHModel, fcl::Transform3<double>::Identity());
  auto bigCuboid_AABB = bigCuboid_CollisionObject.getAABB();
  std::cout << "bigCuboid_AABB.width()=  " << bigCuboid_AABB.width() << std::endl;
  std::cout << "bigCuboid_AABB.height()= " << bigCuboid_AABB.height() << std::endl;
  std::cout << "bigCuboid_AABB.depth()=  " << bigCuboid_AABB.depth() << std::endl;
  std::cout << "bigCuboid_AABB.center()= [" << bigCuboid_AABB.center().x() << ", " << bigCuboid_AABB.center().y() << ", " << bigCuboid_AABB.center().z() << "]"
            << std::endl;

  auto bigCuboid_CollisionManager = std::make_shared<fcl::DynamicAABBTreeCollisionManager<double>>();
  bigCuboid_CollisionManager->registerObject(&bigCuboid_CollisionObject);
  bigCuboid_CollisionManager->setup();

  ///////////////////////////////////////////// prepare variables for twoMetersBox
  auto twoMeters_Box = std::make_shared<fcl::Box<double>>(2, 2, 2);

  auto twoMetersBox_CollisionObject = std::make_shared<fcl::CollisionObject<double>>(twoMeters_Box);
  fcl::Vector3<double> twoMetersBoxTranslation_Vector3(10, 10, 5.65);
  twoMetersBox_CollisionObject->setTranslation(twoMetersBoxTranslation_Vector3);

  auto twoMetersBox_CollisionManager = new fcl::DynamicAABBTreeCollisionManager<double>();
  twoMetersBox_CollisionManager->registerObject(twoMetersBox_CollisionObject.get());
  twoMetersBox_CollisionManager->setup();

  ///////////////////////////////////////////// run collision detection
  fcl::DefaultCollisionData<double> collision_data;
  bigCuboid_CollisionManager->collide(twoMetersBox_CollisionManager, &collision_data, fcl::DefaultCollisionFunction<double>);

  ///////////////////////////////////////////// print results
  std::cout << "collision_data.result.numContacts()=" << collision_data.result.numContacts() << std::endl;
  std::cout << "collision_data.result.isCollision()=" << collision_data.result.isCollision() << std::endl;

  ///////////////////////////////////////////// exit
  delete twoMetersBox_CollisionManager;
  return 0;
}

When I add something like the twoMetersBox_CollisionObect to bigCuboid_CollisionManager on the right location ([10, 10, 5.65] or similar), collision is detected. But the mesh soup seems to be somehow ignored... Why, please?

Update:
With CollisionObjects directly, it works:

fcl::CollisionRequest<double> request;
fcl::CollisionResult<double> result;
collide(bigCuboid_CollisionObject.get(), twoMetersBox_CollisionObject.get(), request, result);

[velecto1]

I did some more testing:

Tested 3D Objects	1-on-1, or grouped?	How is Translation Set	Works?
box × box	CollisionObject	anyhow	YES
mesh × box	CollisionObject	anyhow	YES
box × box	CollisionManager	anyhow	YES
mesh × box	CollisionManager	collisionObject.setTranslation(...)	NO
mesh × box	CollisionManager	CollisionObject(geom, ...)	YES

So, collision detection does not work only and only if

• one of the objects is a mesh soup,
• the objects are grouped in (and checked by) CollisionManager,
• and the transformations are not already set in the constructor of CollisionObject but after CollisionObjects' creation.

---

This led me to a question: Why is an AABB of a CollisionObject dependent on the way the transformation of the CollisionObject is set, and does it influence collision checking with CollisionManagers?

See -- the code

auto transformation_Transform3 = fcl::Transform3<double>(translation_Translation3);
auto twoMetersBox_CollisionObject = std::make_shared<fcl::CollisionObject<double>>(twoMeters_Box, transformation_Transform3);

produces

twoMetersBox_AABB.center()= [10, 10, 5.65],

while

auto twoMetersBox_CollisionObject = std::make_shared<fcl::CollisionObject<double>>(twoMeters_Box);
twoMetersBox_CollisionObject->setTranslation(twoMetersBoxTranslation_Vector3);

produces:

twoMetersBox_AABB.center()= [0, 0, 0].

My proposition is to make CollisionObject::setTranslation() and CollisionObject::setRotation() protected or fix the way CollisionManagers are handling them, but I still lack the full understanding of how the whole thing works.

[velecto1]

I just gave a last shot trying to find a similar issue, and here we are! This issue would have been solved long ago if the terribly old issue #40 was solved, too!!!