2021-11-05 fcl collisiondata 的重复使用？重分配内存

// 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 object
std::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 objects
std::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 input
manager1->setup();
manager2->setup();
// Examples for various queries
// 1. Collision query between two object groups and get collision numbers
manager2->collide(manager1, &collision_data, DefaultCollisionFunction);
int n_contact_num = collision_data.result.numContacts(); 
// 2. Distance query between two object groups and get the minimum distance
manager2->distance(manager1, &distance_data, DefaultDistanceFunction);
double min_distance = distance_data.result.min_distance;
// 3. Self collision query for group 1
manager1->collide(&collision_data, DefaultCollisionFunction);
// 4. Self distance query for group 1
manager1->distance(&distance_data, DefaultDistanceFunction);
// 5. Collision query between one object in group 1 and the entire group 2
manager2->collide(objects1[0], &collision_data, DefaultCollisionFunction);
// 6. Distance query between one object in group 1 and the entire group 2
manager2->distance(objects1[0], &distance_data, DefaultDistanceFunction);

fcl碰撞检测函数

// Given two objects o1 and o2
CollisionObjectf* o1 = ...
CollisionObjectf* o2 = ...
// set the collision request structure, here we just use the default setting
CollisionRequest request;
// result will be returned via the collision result structure
CollisionResult result;
// perform collision test
collide(o1, o2, request, result);

object 间直接碰撞可见已碰撞
多个object 添加到manager
manager之间检测碰撞发现无碰撞？

问题分析：
1 object transform 之后manager里面的object transform没有更新？
测试，重新赋值更新manager transform

结果无区别，不是transform的问题。

改变末端位姿，更改潜在碰撞障碍物为obstacle1，测试是否碰撞

发现问题：循环检测碰撞的collisiondata类自带的clear函数没有用。。。clear一次之后就不能再存新的碰撞结果了？

解决方案：在循环内部每次定义变量分配新的内存地址，保存碰撞检测数据。
测试：


连杆12未碰撞，3456发生碰撞