void octomap_distance_test_BVH(std::size_t n, double resolution)
{
using S = typename BV::S;
std::vector<Vector3<S>> p1;
std::vector<Triangle> t1;
test::loadOBJFile(TEST_RESOURCES_DIR"/env.obj", p1, t1);
BVHModel<BV>* m1 = new BVHModel<BV>();
std::shared_ptr<CollisionGeometry<S>> m1_ptr(m1);
m1->beginModel();
m1->addSubModel(p1, t1);
m1->endModel();
OcTree<S>* tree = new OcTree<S>(std::shared_ptr<octomap::OcTree>(test::generateOcTree(resolution)));
std::shared_ptr<CollisionGeometry<S>> tree_ptr(tree);
Eigen::aligned_vector<Transform3<S>> transforms;
S extents[] = {-10, -10, 10, 10, 10, 10};
test::generateRandomTransforms(extents, transforms, n);
for(std::size_t i = 0; i < n; ++i)
{
Transform3<S> tf(transforms[i]);
CollisionObject<S> obj1(m1_ptr, tf);
CollisionObject<S> obj2(tree_ptr, tf);
test::DistanceData<S> cdata;
S dist1 = std::numeric_limits<S>::max();
test::defaultDistanceFunction(&obj1, &obj2, &cdata, dist1);
std::vector<CollisionObject<S>*> boxes;
test::generateBoxesFromOctomap(boxes, *tree);
for(std::size_t j = 0; j < boxes.size(); ++j)
boxes[j]->setTransform(tf * boxes[j]->getTransform());
DynamicAABBTreeCollisionManager<S>* manager = new DynamicAABBTreeCollisionManager<S>();
manager->registerObjects(boxes);
manager->setup();
test::DistanceData<S> cdata2;
manager->distance(&obj1, &cdata2, test::defaultDistanceFunction);
S dist2 = cdata2.result.min_distance;
for(std::size_t j = 0; j < boxes.size(); ++j)
delete boxes[j];
delete manager;
std::cout << dist1 << " " << dist2 << std::endl;
EXPECT_TRUE(std::abs(dist1 - dist2) < 0.001);
}
}
void octomap_distance_test_BVH(std::size_t n)
{
std::vector<Vec3f> p1;
std::vector<Triangle> t1;
boost::filesystem::path path(TEST_RESOURCES_DIR);
loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
BVHModel<BV>* m1 = new BVHModel<BV>();
boost::shared_ptr<CollisionGeometry> m1_ptr(m1);
m1->beginModel();
m1->addSubModel(p1, t1);
m1->endModel();
OcTree* tree = new OcTree(boost::shared_ptr<octomap::OcTree>(generateOcTree()));
boost::shared_ptr<CollisionGeometry> tree_ptr(tree);
std::vector<Transform3f> transforms;
FCL_REAL extents[] = {-10, -10, 10, 10, 10, 10};
generateRandomTransforms(extents, transforms, n);
for(std::size_t i = 0; i < n; ++i)
{
Transform3f tf(transforms[i]);
CollisionObject obj1(m1_ptr, tf);
CollisionObject obj2(tree_ptr, tf);
DistanceData cdata;
FCL_REAL dist1 = std::numeric_limits<FCL_REAL>::max();
defaultDistanceFunction(&obj1, &obj2, &cdata, dist1);
std::vector<CollisionObject*> boxes;
generateBoxesFromOctomap(boxes, *tree);
for(std::size_t j = 0; j < boxes.size(); ++j)
boxes[j]->setTransform(tf * boxes[j]->getTransform());
DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager();
manager->registerObjects(boxes);
manager->setup();
DistanceData cdata2;
manager->distance(&obj1, &cdata2, defaultDistanceFunction);
FCL_REAL dist2 = cdata2.result.min_distance;
for(std::size_t j = 0; j < boxes.size(); ++j)
delete boxes[j];
delete manager;
std::cout << dist1 << " " << dist2 << std::endl;
BOOST_CHECK(std::abs(dist1 - dist2) < 0.001);
}
}
void octomap_distance_test(S env_scale, std::size_t env_size, bool use_mesh, bool use_mesh_octomap, double resolution)
{
// srand(1);
std::vector<CollisionObject<S>*> env;
if(use_mesh)
test::generateEnvironmentsMesh(env, env_scale, env_size);
else
test::generateEnvironments(env, env_scale, env_size);
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();
test::DistanceData<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, test::defaultDistanceFunction);
timer1.stop();
t1.push_back(timer1.getElapsedTime());
test::DistanceData<S> cdata3;
test::TStruct t3;
test::Timer timer3;
timer3.start();
manager->octree_as_geometry_collide = true;
manager->octree_as_geometry_distance = true;
manager->distance(&tree_obj, &cdata3, test::defaultDistanceFunction);
timer3.stop();
t3.push_back(timer3.getElapsedTime());
test::TStruct t2;
test::Timer timer2;
timer2.start();
std::vector<CollisionObject<S>*> boxes;
if(use_mesh_octomap)
test::generateBoxesFromOctomapMesh(boxes, *tree);
else
test::generateBoxesFromOctomap(boxes, *tree);
timer2.stop();
t2.push_back(timer2.getElapsedTime());
timer2.start();
DynamicAABBTreeCollisionManager<S>* manager2 = new DynamicAABBTreeCollisionManager<S>();
manager2->registerObjects(boxes);
manager2->setup();
timer2.stop();
t2.push_back(timer2.getElapsedTime());
test::DistanceData<S> cdata2;
timer2.start();
manager->distance(manager2, &cdata2, test::defaultDistanceFunction);
timer2.stop();
t2.push_back(timer2.getElapsedTime());
std::cout << cdata.result.min_distance << " " << cdata3.result.min_distance << " " << cdata2.result.min_distance << std::endl;
if(cdata.result.min_distance < 0)
EXPECT_TRUE(cdata2.result.min_distance <= 0);
else
EXPECT_TRUE(std::abs(cdata.result.min_distance - cdata2.result.min_distance) < 1e-3);
delete manager;
delete manager2;
for(size_t i = 0; i < boxes.size(); ++i)
delete boxes[i];
std::cout << "1) octomap overall time: " << t1.overall_time << std::endl;
std::cout << "1') octomap overall time (as geometry): " << t3.overall_time << std::endl;
std::cout << "2) boxes overall time: " << t2.overall_time << std::endl;
std::cout << " a) to boxes: " << t2.records[0] << std::endl;
std::cout << " b) structure init: " << t2.records[1] << std::endl;
std::cout << " c) distance: " << t2.records[2] << std::endl;
std::cout << "Note: octomap may need more collides when using mesh, because octomap collision uses box primitive inside" << std::endl;
}
