void octomap_distance_test(double env_scale, std::size_t env_size, bool use_mesh, bool use_mesh_octomap)
{
// srand(1);
std::vector<CollisionObject*> env;
if(use_mesh)
generateEnvironmentsMesh(env, env_scale, env_size);
else
generateEnvironments(env, env_scale, env_size);
OcTree* tree = new OcTree(std::shared_ptr<const octomap::OcTree>(generateOcTree()));
CollisionObject tree_obj((std::shared_ptr<CollisionGeometry>(tree)));
DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager();
manager->registerObjects(env);
manager->setup();
DistanceData cdata;
TStruct t1;
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());
DistanceData cdata3;
TStruct t3;
Timer timer3;
timer3.start();
manager->octree_as_geometry_collide = true;
manager->octree_as_geometry_distance = true;
manager->distance(&tree_obj, &cdata3, defaultDistanceFunction);
timer3.stop();
t3.push_back(timer3.getElapsedTime());
TStruct t2;
Timer timer2;
timer2.start();
std::vector<CollisionObject*> boxes;
if(use_mesh_octomap)
generateBoxesFromOctomapMesh(boxes, *tree);
else
generateBoxesFromOctomap(boxes, *tree);
timer2.stop();
t2.push_back(timer2.getElapsedTime());
timer2.start();
DynamicAABBTreeCollisionManager* manager2 = new DynamicAABBTreeCollisionManager();
manager2->registerObjects(boxes);
manager2->setup();
timer2.stop();
t2.push_back(timer2.getElapsedTime());
DistanceData cdata2;
timer2.start();
manager->distance(manager2, &cdata2, 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)
BOOST_CHECK(cdata2.result.min_distance <= 0);
else
BOOST_CHECK(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;
}
void octomap_collision_test_BVH(std::size_t n, bool exhaustive)
{
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<const 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);
CollisionData cdata;
if(exhaustive) cdata.request.num_max_contacts = 100000;
defaultCollisionFunction(&obj1, &obj2, &cdata);
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();
CollisionData cdata2;
if(exhaustive) cdata2.request.num_max_contacts = 100000;
manager->collide(&obj1, &cdata2, defaultCollisionFunction);
for(std::size_t j = 0; j < boxes.size(); ++j)
delete boxes[j];
delete manager;
if(exhaustive)
{
std::cout << cdata.result.numContacts() << " " << cdata2.result.numContacts() << std::endl;
BOOST_CHECK(cdata.result.numContacts() == cdata2.result.numContacts());
}
else
{
std::cout << (cdata.result.numContacts() > 0) << " " << (cdata2.result.numContacts() > 0) << std::endl;
BOOST_CHECK((cdata.result.numContacts() > 0) == (cdata2.result.numContacts() > 0));
}
}
}
void octomap_collision_test(double env_scale, std::size_t env_size, bool exhaustive, std::size_t num_max_contacts, bool use_mesh, bool use_mesh_octomap)
{
// srand(1);
std::vector<CollisionObject*> env;
if(use_mesh)
generateEnvironmentsMesh(env, env_scale, env_size);
else
generateEnvironments(env, env_scale, env_size);
OcTree* tree = new OcTree(std::shared_ptr<const octomap::OcTree>(generateOcTree()));
CollisionObject tree_obj((std::shared_ptr<CollisionGeometry>(tree)));
DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager();
manager->registerObjects(env);
manager->setup();
CollisionData cdata;
if(exhaustive) cdata.request.num_max_contacts = 100000;
else cdata.request.num_max_contacts = num_max_contacts;
TStruct t1;
Timer timer1;
timer1.start();
manager->octree_as_geometry_collide = false;
manager->octree_as_geometry_distance = false;
manager->collide(&tree_obj, &cdata, defaultCollisionFunction);
timer1.stop();
t1.push_back(timer1.getElapsedTime());
CollisionData cdata3;
if(exhaustive) cdata3.request.num_max_contacts = 100000;
else cdata3.request.num_max_contacts = num_max_contacts;
TStruct t3;
Timer timer3;
timer3.start();
manager->octree_as_geometry_collide = true;
manager->octree_as_geometry_distance = true;
manager->collide(&tree_obj, &cdata3, defaultCollisionFunction);
timer3.stop();
t3.push_back(timer3.getElapsedTime());
TStruct t2;
Timer timer2;
timer2.start();
std::vector<CollisionObject*> boxes;
if(use_mesh_octomap)
generateBoxesFromOctomapMesh(boxes, *tree);
else
generateBoxesFromOctomap(boxes, *tree);
timer2.stop();
t2.push_back(timer2.getElapsedTime());
timer2.start();
DynamicAABBTreeCollisionManager* manager2 = new DynamicAABBTreeCollisionManager();
manager2->registerObjects(boxes);
manager2->setup();
timer2.stop();
t2.push_back(timer2.getElapsedTime());
CollisionData cdata2;
if(exhaustive) cdata2.request.num_max_contacts = 100000;
else cdata2.request.num_max_contacts = num_max_contacts;
timer2.start();
manager->collide(manager2, &cdata2, defaultCollisionFunction);
timer2.stop();
t2.push_back(timer2.getElapsedTime());
std::cout << cdata.result.numContacts() << " " << cdata3.result.numContacts() << " " << cdata2.result.numContacts() << std::endl;
if(exhaustive)
{
if(use_mesh) BOOST_CHECK((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0));
else BOOST_CHECK(cdata.result.numContacts() == cdata2.result.numContacts());
}
else
{
if(use_mesh) BOOST_CHECK((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0));
else BOOST_CHECK((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0)); // because AABB return collision when two boxes contact
}
delete manager;
delete manager2;
for(size_t i = 0; i < boxes.size(); ++i)
delete boxes[i];
if(exhaustive) std::cout << "exhaustive collision" << std::endl;
else std::cout << "non exhaustive collision" << std::endl;
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) collision: " << 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;
}
