38 #define BOOST_TEST_MODULE FCL_FRONT_LIST
39 #include <boost/test/included/unit_test.hpp>
43 #include <../src/collision_node.h>
47 #include "fcl_resources/config.h"
48 #include <boost/filesystem.hpp>
51 namespace utf = boost::unit_test::framework;
53 template <
typename BV>
55 const std::vector<Vec3f>& vertices1,
56 const std::vector<Triangle>& triangles1,
57 const std::vector<Vec3f>& vertices2,
58 const std::vector<Triangle>& triangles2,
62 template <
typename BV,
typename TraversalNode>
65 const std::vector<Vec3f>& vertices1,
66 const std::vector<Triangle>& triangles1,
67 const std::vector<Vec3f>& vertices2,
68 const std::vector<Triangle>& triangles2,
72 template <
typename BV>
74 const std::vector<Triangle>& triangles1,
75 const std::vector<Vec3f>& vertices2,
76 const std::vector<Triangle>& triangles2,
81 std::vector<Vec3f>
p1, p2;
82 std::vector<Triangle> t1, t2;
87 std::vector<Transform3f> transforms;
88 std::vector<Transform3f> transforms2;
91 #ifndef NDEBUG // if debug mode
96 n =
getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, n);
104 for (std::size_t i = 0; i < transforms.size(); ++i) {
105 res = collide_Test<AABB>(transforms2[i],
p1, t1, p2, t2,
108 collide_front_list_Test<AABB>(transforms[i], transforms2[i],
p1, t1, p2,
110 BOOST_CHECK(
res == res2);
114 collide_front_list_Test<AABB>(transforms[i], transforms2[i],
p1, t1, p2,
116 BOOST_CHECK(
res == res2);
117 res = collide_Test<AABB>(transforms2[i],
p1, t1, p2, t2,
119 res2 = collide_front_list_Test<AABB>(transforms[i], transforms2[i],
p1, t1,
122 BOOST_CHECK(
res == res2);
125 for (std::size_t i = 0; i < transforms.size(); ++i) {
129 collide_front_list_Test<OBB>(transforms[i], transforms2[i],
p1, t1, p2,
131 BOOST_CHECK(
res == res2);
135 collide_front_list_Test<OBB>(transforms[i], transforms2[i],
p1, t1, p2,
137 BOOST_CHECK(
res == res2);
138 res = collide_Test<OBB>(transforms2[i],
p1, t1, p2, t2,
140 res2 = collide_front_list_Test<OBB>(transforms[i], transforms2[i],
p1, t1,
143 BOOST_CHECK(
res == res2);
146 for (std::size_t i = 0; i < transforms.size(); ++i) {
155 collide_front_list_Test<RSS>(transforms[i], transforms2[i],
p1, t1, p2,
157 BOOST_CHECK(
res == res2);
158 res = collide_Test<RSS>(transforms2[i],
p1, t1, p2, t2,
160 res2 = collide_front_list_Test<RSS>(transforms[i], transforms2[i],
p1, t1,
163 BOOST_CHECK(
res == res2);
166 for (std::size_t i = 0; i < transforms.size(); ++i) {
167 res = collide_Test<KDOP<16> >(transforms2[i],
p1, t1, p2, t2,
169 res2 = collide_front_list_Test<KDOP<16> >(transforms[i], transforms2[i],
p1,
172 BOOST_CHECK(
res == res2);
173 res = collide_Test<KDOP<16> >(transforms2[i],
p1, t1, p2, t2,
175 res2 = collide_front_list_Test<KDOP<16> >(transforms[i], transforms2[i],
p1,
178 BOOST_CHECK(
res == res2);
179 res = collide_Test<KDOP<16> >(transforms2[i],
p1, t1, p2, t2,
181 res2 = collide_front_list_Test<KDOP<16> >(
184 BOOST_CHECK(
res == res2);
187 for (std::size_t i = 0; i < transforms.size(); ++i) {
188 res = collide_Test<KDOP<18> >(transforms2[i],
p1, t1, p2, t2,
190 res2 = collide_front_list_Test<KDOP<18> >(transforms[i], transforms2[i],
p1,
193 BOOST_CHECK(
res == res2);
194 res = collide_Test<KDOP<18> >(transforms2[i],
p1, t1, p2, t2,
196 res2 = collide_front_list_Test<KDOP<18> >(transforms[i], transforms2[i],
p1,
199 BOOST_CHECK(
res == res2);
200 res = collide_Test<KDOP<18> >(transforms2[i],
p1, t1, p2, t2,
202 res2 = collide_front_list_Test<KDOP<18> >(
205 BOOST_CHECK(
res == res2);
208 for (std::size_t i = 0; i < transforms.size(); ++i) {
209 res = collide_Test<KDOP<24> >(transforms2[i],
p1, t1, p2, t2,
211 res2 = collide_front_list_Test<KDOP<24> >(transforms[i], transforms2[i],
p1,
214 BOOST_CHECK(
res == res2);
215 res = collide_Test<KDOP<24> >(transforms2[i],
p1, t1, p2, t2,
217 res2 = collide_front_list_Test<KDOP<24> >(transforms[i], transforms2[i],
p1,
220 BOOST_CHECK(
res == res2);
221 res = collide_Test<KDOP<24> >(transforms2[i],
p1, t1, p2, t2,
223 res2 = collide_front_list_Test<KDOP<24> >(
226 BOOST_CHECK(
res == res2);
229 for (std::size_t i = 0; i < transforms.size(); ++i) {
232 res2 = collide_front_list_Test_Oriented<RSS, MeshCollisionTraversalNodeRSS>(
235 BOOST_CHECK(
res == res2);
238 res2 = collide_front_list_Test_Oriented<RSS, MeshCollisionTraversalNodeRSS>(
241 BOOST_CHECK(
res == res2);
242 res = collide_Test<RSS>(transforms2[i],
p1, t1, p2, t2,
244 res2 = collide_front_list_Test_Oriented<RSS, MeshCollisionTraversalNodeRSS>(
247 BOOST_CHECK(
res == res2);
250 for (std::size_t i = 0; i < transforms.size(); ++i) {
253 res2 = collide_front_list_Test_Oriented<OBB, MeshCollisionTraversalNodeOBB>(
256 BOOST_CHECK(
res == res2);
259 res2 = collide_front_list_Test_Oriented<OBB, MeshCollisionTraversalNodeOBB>(
262 BOOST_CHECK(
res == res2);
263 res = collide_Test<OBB>(transforms2[i],
p1, t1, p2, t2,
265 res2 = collide_front_list_Test_Oriented<OBB, MeshCollisionTraversalNodeOBB>(
268 BOOST_CHECK(
res == res2);
272 template <
typename BV>
274 const std::vector<Vec3f>& vertices1,
275 const std::vector<Triangle>& triangles1,
276 const std::vector<Vec3f>& vertices2,
277 const std::vector<Triangle>& triangles2,
287 std::vector<Vec3f> vertices1_new(vertices1.size());
288 for (std::size_t i = 0; i < vertices1_new.size(); ++i) {
289 vertices1_new[i] =
tf1.transform(vertices1[i]);
304 MeshCollisionTraversalNode<BV> node(request);
306 bool success = initialize<BV>(node, m1, pose1, m2, pose2, local_result);
307 BOOST_REQUIRE(success);
309 node.enable_statistics =
verbose;
311 collide(&node, request, local_result, &front_list);
314 std::cout <<
"front list size " << front_list.size() << std::endl;
317 for (std::size_t i = 0; i < vertices1.size(); ++i) {
318 vertices1_new[i] =
tf2.transform(vertices1[i]);
329 local_result.
clear();
330 collide(&node, request, local_result, &front_list);
338 template <
typename BV,
typename TraversalNode>
341 const std::vector<Vec3f>& vertices1,
342 const std::vector<Triangle>& triangles1,
343 const std::vector<Vec3f>& vertices2,
344 const std::vector<Triangle>& triangles2,
366 TraversalNode node(request);
370 BOOST_REQUIRE(success);
372 node.enable_statistics =
verbose;
374 collide(&node, request, local_result, &front_list);
377 std::cout <<
"front list size " << front_list.size() << std::endl;
383 BOOST_REQUIRE(success);
385 local_result.
clear();
386 collide(&node, request, local_result, &front_list);
394 template <
typename BV>
396 const std::vector<Triangle>& triangles1,
397 const std::vector<Vec3f>& vertices2,
398 const std::vector<Triangle>& triangles2,
417 MeshCollisionTraversalNode<BV> node(request);
419 bool success = initialize<BV>(node, m1, pose1, m2, pose2, local_result);
420 BOOST_REQUIRE(success);
422 node.enable_statistics =
verbose;
424 collide(&node, request, local_result);