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(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_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; }