38 #include <gtest/gtest.h>
40 #include "fcl/config.h"
53 #include "fcl_resources/config.h"
68 double resolution = 0.1);
77 octomap_collision_test<S>(200, 100,
false, 10,
false,
false);
78 octomap_collision_test<S>(200, 1000,
false, 10,
false,
false);
79 octomap_collision_test<S>(200, 100,
true, 1,
false,
false);
80 octomap_collision_test<S>(200, 1000,
true, 1,
false,
false);
82 octomap_collision_test<S>(200, 10,
false, 10,
false,
false, 0.1);
83 octomap_collision_test<S>(200, 100,
false, 10,
false,
false, 0.1);
84 octomap_collision_test<S>(200, 10,
true, 1,
false,
false, 0.1);
85 octomap_collision_test<S>(200, 100,
true, 1,
false,
false, 0.1);
92 test_octomap_collision<double>();
99 octomap_collision_test<S>(200, 100,
false, 10,
true,
true);
100 octomap_collision_test<S>(200, 1000,
false, 10,
true,
true);
101 octomap_collision_test<S>(200, 100,
true, 1,
true,
true);
102 octomap_collision_test<S>(200, 1000,
true, 1,
true,
true);
104 octomap_collision_test<S>(200, 4,
false, 1,
true,
true, 1.0);
105 octomap_collision_test<S>(200, 4,
true, 1,
true,
true, 1.0);
112 test_octomap_collision_mesh<double>();
115 template <
typename S>
119 octomap_collision_test_contact_primitive_id<S>(1, 30, 100000);
121 octomap_collision_test_contact_primitive_id<S>(1, 10, 10000, 1.0);
128 test_octomap_collision_contact_primitive_id<double>();
131 template <
typename S>
135 octomap_collision_test<S>(200, 100,
false, 10,
true,
false);
136 octomap_collision_test<S>(200, 1000,
false, 10,
true,
false);
137 octomap_collision_test<S>(200, 100,
true, 1,
true,
false);
138 octomap_collision_test<S>(200, 1000,
true, 1,
true,
false);
140 octomap_collision_test<S>(200, 4,
false, 4,
true,
false, 1.0);
141 octomap_collision_test<S>(200, 4,
true, 1,
true,
false, 1.0);
148 test_octomap_collision_mesh_octomap_box<double>();
151 template <
typename S>
155 octomap_collision_test_BVH<OBB<S>>(5,
false);
156 octomap_collision_test_BVH<OBB<S>>(5,
true);
158 octomap_collision_test_BVH<OBB<S>>(1,
false, 1.0);
159 octomap_collision_test_BVH<OBB<S>>(1,
true, 1.0);
166 test_octomap_bvh_obb_collision_obb<double>();
169 template<
typename BV>
172 using S =
typename BV::S;
174 std::vector<Vector3<S>> p1;
175 std::vector<Triangle> t1;
180 std::shared_ptr<CollisionGeometry<S>> m1_ptr(m1);
186 auto octree = std::shared_ptr<const octomap::OcTree>(
187 test::generateOcTree(resolution));
188 OcTree<S>* tree =
new OcTree<S>(octree);
189 std::shared_ptr<CollisionGeometry<S>> tree_ptr(tree);
195 size_t free_nodes = 0;
196 size_t occupied_nodes = 0;
197 for (
auto it = octree->begin(), end = octree->end(); it != end; ++it)
199 if (tree->isNodeFree(&*it))
201 if (tree->isNodeOccupied(&*it))
204 EXPECT_GT(free_nodes, 0UL);
205 EXPECT_GT(occupied_nodes, 0UL);
208 S
extents[] = {-10, -10, 10, 10, 10, 10};
212 for(std::size_t i = 0; i < n; ++i)
219 if(exhaustive) cdata.
request.num_max_contacts = 100000;
224 std::vector<CollisionObject<S>*> boxes;
225 test::generateBoxesFromOctomap(boxes, *tree);
226 for(std::size_t j = 0; j < boxes.size(); ++j)
227 boxes[j]->setTransform(tf * boxes[j]->getTransform());
234 if(exhaustive) cdata2.
request.num_max_contacts = 100000;
237 for(std::size_t j = 0; j < boxes.size(); ++j)
243 std::cout << cdata.
result.numContacts() <<
" " << cdata2.
result.numContacts() << std::endl;
248 std::cout << (cdata.
result.numContacts() > 0) <<
" " << (cdata2.
result.numContacts() > 0) << std::endl;
254 template <
typename S>
258 std::vector<CollisionObject<S>*> env;
264 OcTree<S>* tree =
new OcTree<S>(std::shared_ptr<const octomap::OcTree>(test::generateOcTree(resolution)));
272 if(exhaustive) cdata.
request.num_max_contacts = 100000;
285 if(exhaustive) cdata3.
request.num_max_contacts = 100000;
300 std::vector<CollisionObject<S>*> boxes;
302 test::generateBoxesFromOctomapMesh(boxes, *tree);
304 test::generateBoxesFromOctomap(boxes, *tree);
317 if(exhaustive) cdata2.
request.num_max_contacts = 100000;
325 std::cout << cdata.
result.numContacts() <<
" " << cdata3.
result.numContacts() <<
" " << cdata2.
result.numContacts() << std::endl;
339 for(
size_t i = 0; i < boxes.size(); ++i)
342 if(exhaustive) std::cout <<
"exhaustive collision" << std::endl;
343 else std::cout <<
"non exhaustive collision" << std::endl;
344 std::cout <<
"1) octomap overall time: " << t1.
overall_time << std::endl;
345 std::cout <<
"1') octomap overall time (as geometry): " << t3.
overall_time << std::endl;
346 std::cout <<
"2) boxes overall time: " << t2.
overall_time << std::endl;
347 std::cout <<
" a) to boxes: " << t2.
records[0] << std::endl;
348 std::cout <<
" b) structure init: " << t2.
records[1] << std::endl;
349 std::cout <<
" c) collision: " << t2.
records[2] << std::endl;
350 std::cout <<
"Note: octomap may need more collides when using mesh, because octomap collision uses box primitive inside" << std::endl;
353 template <
typename S>
356 std::size_t env_size,
360 std::vector<CollisionObject<S>*> env;
368 std::shared_ptr<const octomap::OcTree> octree(
369 test::generateOcTree(resolution));
370 OcTree<S>* tree =
new OcTree<S>(octree);
373 std::vector<CollisionObject<S>*> boxes;
374 test::generateBoxesFromOctomap(boxes, *tree);
376 cit != env.end(); ++cit)
381 for(std::size_t index=0; index<cResult.
numContacts(); ++index)
385 const fcl::OcTree<S>* contact_tree =
static_cast<const fcl::OcTree<S>*
>(
390 auto get_node_rv = contact_tree->getNodeByQueryCellId(
397 auto center_octomap_point = octree->keyToCoord(key);
398 double cell_size = octree->getNodeSize(depth);
399 for (
unsigned i = 0; i < 3; ++i)
406 auto octree_node = octree->search(key, depth);
416 int main(
int argc,
char* argv[])
418 ::testing::InitGoogleTest(&argc, argv);
419 return RUN_ALL_TESTS();