Open Li-zhuoran opened 5 months ago
When detecting the distance of obj, the example uses the following method::
OcTree<S>* tree = new OcTree<S>(std::shared_ptr<const octomap::OcTree>(test::generateOcTree(resolution))); CollisionObject<S> tree_obj(std::shared_ptr<CollisionGeometry<S>>(tree)); DynamicAABBTreeCollisionManager<S>* manager = new DynamicAABBTreeCollisionManager<S>(); manager->registerObjects(env); manager->setup(); DefaultDistanceData<S> cdata; test::TStruct t1; test::Timer timer1; timer1.start(); manager->octree_as_geometry_collide = false; manager->octree_as_geometry_distance = false; manager->distance(&tree_obj, &cdata, DefaultDistanceFunction); timer1.stop(); t1.push_back(timer1.getElapsedTime());
The specific error I encountered is in the call to the distance function. The relevant function implementation is as follows:
distance
template <typename S> FCL_EXPORT void DynamicAABBTreeCollisionManager<S>::distance(BroadPhaseCollisionManager<S>* other_manager_, void* cdata, DistanceCallBack<S> callback) const { DynamicAABBTreeCollisionManager* other_manager = static_cast<DynamicAABBTreeCollisionManager*>(other_manager_); if((size() == 0) || (other_manager->size() == 0)) return; S min_dist = std::numeric_limits<S>::max(); detail::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), other_manager->dtree.getRoot(), cdata, callback, min_dist); }
I noticed that S min_dist is initialized as std::numeric_limits<S>::max(). I tried setting this value to 0, and the program seems to run fine.
std::numeric_limits<S>::max()
it looks like the calculated distance is still wrong.
When detecting the distance of obj, the example uses the following method::
The specific error I encountered is in the call to the
distance
function. The relevant function implementation is as follows:I noticed that S min_dist is initialized as
std::numeric_limits<S>::max()
. I tried setting this value to 0, and the program seems to run fine.