38 #include <gtest/gtest.h>
40 #include "fcl/config.h"
55 #include "fcl_resources/config.h"
61 void octomap_distance_test(S env_scale, std::size_t env_size,
bool use_mesh,
bool use_mesh_octomap,
double resolution = 0.1);
70 octomap_distance_test<S>(200, 100,
false,
false);
71 octomap_distance_test<S>(200, 1000,
false,
false);
73 octomap_distance_test<S>(200, 2,
false,
false, 1.0);
74 octomap_distance_test<S>(200, 10,
false,
false, 1.0);
81 test_octomap_distance<double>();
88 octomap_distance_test<S>(200, 100,
true,
true);
89 octomap_distance_test<S>(200, 1000,
true,
true);
91 octomap_distance_test<S>(200, 2,
true,
true, 1.0);
92 octomap_distance_test<S>(200, 5,
true,
true, 1.0);
99 test_octomap_distance_mesh<double>();
102 template <
typename S>
106 octomap_distance_test<S>(200, 100,
true,
false);
107 octomap_distance_test<S>(200, 1000,
true,
false);
109 octomap_distance_test<S>(200, 2,
true,
false, 1.0);
110 octomap_distance_test<S>(200, 5,
true,
false, 1.0);
117 test_octomap_distance_mesh_octomap_box<double>();
120 template <
typename S>
124 octomap_distance_test_BVH<RSS<S>>(5);
126 octomap_distance_test_BVH<RSS<S>>(5, 1.0);
133 test_octomap_bvh_rss_d_distance_rss<double>();
136 template <
typename S>
140 octomap_distance_test_BVH<OBBRSS<S>>(5);
142 octomap_distance_test_BVH<OBBRSS<S>>(5, 1.0);
149 test_octomap_bvh_obb_d_distance_obb<double>();
152 template <
typename S>
156 octomap_distance_test_BVH<kIOS<S>>(5);
158 octomap_distance_test_BVH<kIOS<S>>(5, 1.0);
165 test_octomap_bvh_kios_d_distance_kios<double>();
168 template<
typename BV>
171 using S =
typename BV::S;
173 std::vector<Vector3<S>> p1;
174 std::vector<Triangle> t1;
179 std::shared_ptr<CollisionGeometry<S>> m1_ptr(m1);
185 OcTree<S>* tree =
new OcTree<S>(
186 std::shared_ptr<octomap::OcTree>(test::generateOcTree(resolution)));
187 std::shared_ptr<CollisionGeometry<S>> tree_ptr(tree);
190 S
extents[] = {-5, -5, -5, 5, 5, 5};
194 for(std::size_t i = 0; i < n; ++i)
203 cdata.
request.enable_nearest_points =
true;
204 cdata1b.
request.enable_nearest_points =
true;
213 std::vector<CollisionObject<S>*> boxes;
214 test::generateBoxesFromOctomap(boxes, *tree);
215 for(std::size_t j = 0; j < boxes.size(); ++j)
216 boxes[j]->setTransform(tf2 * boxes[j]->getTransform());
225 S dist2 = cdata2.
result.min_distance;
227 for(std::size_t j = 0; j < boxes.size(); ++j)
238 cdata.
result.nearest_points[0] - cdata.
result.nearest_points[1];
244 EXPECT_NEAR(nearestPointDistance.norm(), dist1, 0.001);
249 template <
typename S>
250 void octomap_distance_test(S env_scale, std::size_t env_size,
bool use_mesh,
bool use_mesh_octomap,
double resolution)
253 std::vector<CollisionObject<S>*> env;
259 OcTree<S>* tree =
new OcTree<S>(std::shared_ptr<const octomap::OcTree>(test::generateOcTree(resolution)));
291 std::vector<CollisionObject<S>*> boxes;
293 test::generateBoxesFromOctomapMesh(boxes, *tree);
295 test::generateBoxesFromOctomap(boxes, *tree);
314 std::cout << cdata.
result.min_distance <<
" " << cdata3.
result.min_distance <<
" " << cdata2.
result.min_distance << std::endl;
316 if(cdata.
result.min_distance < 0)
317 EXPECT_LE(cdata2.
result.min_distance, 0);
323 for(
size_t i = 0; i < boxes.size(); ++i)
327 std::cout <<
"1) octomap overall time: " << t1.
overall_time << std::endl;
328 std::cout <<
"1') octomap overall time (as geometry): " << t3.
overall_time << std::endl;
329 std::cout <<
"2) boxes overall time: " << t2.
overall_time << std::endl;
330 std::cout <<
" a) to boxes: " << t2.
records[0] << std::endl;
331 std::cout <<
" b) structure init: " << t2.
records[1] << std::endl;
332 std::cout <<
" c) distance: " << t2.
records[2] << std::endl;
333 std::cout <<
"Note: octomap may need more collides when using mesh, because octomap collision uses box primitive inside" << std::endl;
337 int main(
int argc,
char* argv[])
339 ::testing::InitGoogleTest(&argc, argv);
340 return RUN_ALL_TESTS();