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;
}
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(boost::shared_ptr<const octomap::OcTree>(generateOcTree()));
CollisionObject tree_obj((boost::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;
}
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_cost_test(double env_scale, std::size_t env_size, std::size_t num_max_cost_sources, bool use_mesh, bool use_mesh_octomap)
{
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;
cdata.request.enable_cost = true;
cdata.request.num_max_cost_sources = num_max_cost_sources;
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;
cdata3.request.enable_cost = true;
cdata3.request.num_max_cost_sources = num_max_cost_sources;
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;
cdata2.request.enable_cost = true;
cdata3.request.num_max_cost_sources = num_max_cost_sources;
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;
std::cout << cdata.result.numCostSources() << " " << cdata3.result.numCostSources() << " " << cdata2.result.numCostSources() << std::endl;
{
std::vector<CostSource> cost_sources;
cdata.result.getCostSources(cost_sources);
for(std::size_t i = 0; i < cost_sources.size(); ++i)
{
std::cout << cost_sources[i].aabb_min << " " << cost_sources[i].aabb_max << " " << cost_sources[i].cost_density << std::endl;
}
std::cout << std::endl;
cdata3.result.getCostSources(cost_sources);
for(std::size_t i = 0; i < cost_sources.size(); ++i)
{
std::cout << cost_sources[i].aabb_min << " " << cost_sources[i].aabb_max << " " << cost_sources[i].cost_density << std::endl;
}
std::cout << std::endl;
cdata2.result.getCostSources(cost_sources);
for(std::size_t i = 0; i < cost_sources.size(); ++i)
{
std::cout << cost_sources[i].aabb_min << " " << cost_sources[i].aabb_max << " " << cost_sources[i].cost_density << std::endl;
}
std::cout << std::endl;
}
if(use_mesh) EXPECT_TRUE((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0));
else EXPECT_TRUE(cdata.result.numContacts() >= cdata2.result.numContacts());
delete manager;
delete manager2;
for(std::size_t i = 0; i < boxes.size(); ++i)
delete boxes[i];
std::cout << "collision cost" << 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;
}
