security_margin and collision_distance_threshold of DynamicAABBTreeCollisionManager

[XinyuWuu]

Does DynamicAABBTreeCollisionManager respect security_margin and collision_distance_threshold ? I couldn't get DynamicAABBTreeCollisionManager to work but NaiveCollisionManager and hpp::fcl::collide work well. Is it a bug or I did't do it the right way?

My test code:

#include <iostream>
#include <hpp/fcl/broadphase/broadphase.h>

int main(int argc, char const *argv[])
{
    auto colOs = std::vector<hpp::fcl::CollisionObject *>();
    auto s1 = std::make_shared<hpp::fcl::Sphere>(0.1);
    auto o1 = new hpp::fcl::CollisionObject(
        s1, hpp::fcl::Quaternion3f().toRotationMatrix(), hpp::fcl::Vec3f(2, 0, 0));
    o1->computeAABB();
    colOs.push_back(o1);
    auto s2 = std::make_shared<hpp::fcl::Box>(0.1, 0.1, 0.1);
    auto o2 = new hpp::fcl::CollisionObject(
        s2, hpp::fcl::Quaternion3f().toRotationMatrix(), hpp::fcl::Vec3f(0, 2, 0));
    o2->computeAABB();
    colOs.push_back(o2);
    auto s3 = std::make_shared<hpp::fcl::Cylinder>(0.1, 0.1);
    auto o3 = new hpp::fcl::CollisionObject(
        s3, hpp::fcl::Quaternion3f().toRotationMatrix(), hpp::fcl::Vec3f(0, 0, 2));
    o3->computeAABB();
    colOs.push_back(o3);
    auto s4 = std::make_shared<hpp::fcl::Ellipsoid>(0.1, 0.1, 0.2);
    auto o4 = new hpp::fcl::CollisionObject(
        s4, hpp::fcl::Quaternion3f().toRotationMatrix(), hpp::fcl::Vec3f(0, 0, 0.2));
    o4->computeAABB();
    colOs.push_back(o4);

    auto srobot = std::make_shared<hpp::fcl::Sphere>(0.1);
    auto orobot = new hpp::fcl::CollisionObject(
        srobot, hpp::fcl::Quaternion3f().toRotationMatrix(), hpp::fcl::Vec3f(0, 0, 0));
    orobot->computeAABB();

    double security_margin = 3;

    auto NcolMag = hpp::fcl::NaiveCollisionManager();
    NcolMag.registerObjects(colOs);
    NcolMag.setup();
    auto NcolCB = hpp::fcl::CollisionCallBackDefault();
    NcolCB.data.request.num_max_contacts = 1000;
    NcolCB.data.request.security_margin = security_margin;
    NcolCB.data.request.collision_distance_threshold = security_margin;
    NcolMag.collide(orobot, &NcolCB);
    std::cout << "isCollision from NaiveCollisionManager: " << NcolCB.data.result.isCollision() << std::endl;
    std::cout << "numContacts from NaiveCollisionManager: " << NcolCB.data.result.numContacts() << std::endl;
    NcolCB.data.result.clear();

    auto DcolMag = hpp::fcl::DynamicAABBTreeCollisionManager();
    DcolMag.registerObjects(colOs);
    DcolMag.setup();
    auto DcolCB = hpp::fcl::CollisionCallBackDefault();
    DcolCB.data.request.num_max_contacts = 1000;
    DcolCB.data.request.security_margin = security_margin;
    DcolCB.data.request.collision_distance_threshold = security_margin;
    DcolMag.collide(orobot, &DcolCB);
    std::cout << "isCollision from DynamicAABBTreeCollisionManager: " << DcolCB.data.result.isCollision() << std::endl;
    std::cout << "numContacts from DynamicAABBTreeCollisionManager: " << DcolCB.data.result.numContacts() << std::endl;
    DcolCB.data.result.clear();

    auto col_req = hpp::fcl::CollisionRequest();
    col_req.num_max_contacts = 1000;
    col_req.security_margin = security_margin;
    col_req.collision_distance_threshold = security_margin;
    auto col_res = hpp::fcl::CollisionResult();
    bool isCollision = false;
    int numContacts = 0;
    for (auto o : colOs)
    {
        hpp::fcl::collide(orobot, o, col_req, col_res);
        isCollision = isCollision || col_res.isCollision();
        numContacts = numContacts + col_res.numContacts();
        col_res.clear();
    }
    std::cout << "isCollision from hpp::fcl::collide: " << isCollision << std::endl;
    std::cout << "numContacts from hpp::fcl::collide: " << numContacts << std::endl;

    for (auto o : colOs)
    {
        delete o;
    }
    delete orobot;
    return 0;
}

return with security_margin=3:

isCollision from NaiveCollisionManager: 1
numContacts from NaiveCollisionManager: 4
isCollision from DynamicAABBTreeCollisionManager: 1
numContacts from DynamicAABBTreeCollisionManager: 1
isCollision from hpp::fcl::collide: 1
numContacts from hpp::fcl::collide: 4

return with security_margin=0.1:

isCollision from NaiveCollisionManager: 1
numContacts from NaiveCollisionManager: 1
isCollision from DynamicAABBTreeCollisionManager: 1
numContacts from DynamicAABBTreeCollisionManager: 1
isCollision from hpp::fcl::collide: 1
numContacts from hpp::fcl::collide: 1