// Initialize the collision manager for the first group of objects. // FCL provides various different implementations of CollisionManager.// Generally, the DynamicAABBTreeCollisionManager would provide the best// performance.BroadPhaseCollisionManagerf* manager1 = new DynamicAABBTreeCollisionManagerf(); // Initialize the collision manager for the second group of objects.BroadPhaseCollisionManagerf* manager2 = new DynamicAABBTreeCollisionManagerf();// To add objects into the collision manager, using// BroadPhaseCollisionManager::registerObject() function to add one objectstd::vector<CollisionObjectf*> objects1 = ...for(std::size_t i = 0; i < objects1.size(); ++i)manager1->registerObject(objects1[i]);// Another choose is to use BroadPhaseCollisionManager::registerObjects()// function to add a set of objectsstd::vector<CollisionObjectf*> objects2 = ...manager2->registerObjects(objects2);// In order to collect the information during broadphase, CollisionManager// requires two settings:// a) a callback to collision or distance; // b) an intermediate data to store the information generated during the// broadphase computation.// For convenience, FCL provides default callbacks to satisfy a) and a// corresponding call back data to satisfy b) for both collision and distance// queries. For collision use DefaultCollisionCallback and DefaultCollisionData// and for distance use DefaultDistanceCallback and DefaultDistanceData.// The default collision/distance data structs are simply containers which// include the request and distance structures for each query type as mentioned// above.DefaultCollisionData collision_data;DefaultDistanceData distance_data;// Setup the managers, which is related with initializing the broadphase// acceleration structure according to objects inputmanager1->setup();manager2->setup();// Examples for various queries// 1. Collision query between two object groups and get collision numbersmanager2->collide(manager1, &collision_data, DefaultCollisionFunction);int n_contact_num = collision_data.result.numContacts(); // 2. Distance query between two object groups and get the minimum distancemanager2->distance(manager1, &distance_data, DefaultDistanceFunction);double min_distance = distance_data.result.min_distance;// 3. Self collision query for group 1manager1->collide(&collision_data, DefaultCollisionFunction);// 4. Self distance query for group 1manager1->distance(&distance_data, DefaultDistanceFunction);// 5. Collision query between one object in group 1 and the entire group 2manager2->collide(objects1[0], &collision_data, DefaultCollisionFunction);// 6. Distance query between one object in group 1 and the entire group 2manager2->distance(objects1[0], &distance_data, DefaultDistanceFunction);
fcl碰撞检测函数
// Given two objects o1 and o2CollisionObjectf* o1 = ...CollisionObjectf* o2 = ...// set the collision request structure, here we just use the default settingCollisionRequest request;// result will be returned via the collision result structureCollisionResult result;// perform collision testcollide(o1, o2, request, result);
object 间直接碰撞可见已碰撞
多个object 添加到manager
manager之间检测碰撞发现无碰撞？
问题分析：
1 object transform 之后manager里面的object transform没有更新？
测试，重新赋值更新manager transform
结果无区别，不是transform的问题。
改变末端位姿，更改潜在碰撞障碍物为obstacle1，测试是否碰撞
发现问题：循环检测碰撞的collisiondata类自带的clear函数没有用。。。clear一次之后就不能再存新的碰撞结果了？
解决方案：在循环内部每次定义变量分配新的内存地址，保存碰撞检测数据。
测试：
连杆12未碰撞，3456发生碰撞