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;
84 loadOBJFile((path /
"env.obj").
string().c_str(), p1, t1);
85 loadOBJFile((path /
"rob.obj").
string().c_str(), p2, 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]);
321 m1.beginReplaceModel();
322 m1.replaceSubModel(vertices1_new);
323 m1.endReplaceModel(
true, refit_bottomup);
325 m2.beginReplaceModel();
326 m2.replaceSubModel(vertices2);
327 m2.endReplaceModel(
true, refit_bottomup);
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);
void loadOBJFile(const char *filename, std::vector< Vec3f > &points, std::vector< Triangle > &triangles)
Load an obj mesh file.
bool initialize(MeshCollisionTraversalNode< BV, RelativeTransformationIsIdentity > &node, BVHModel< BV > &model1, Transform3f &tf1, BVHModel< BV > &model2, Transform3f &tf2, CollisionResult &result, bool use_refit=false, bool refit_bottomup=false)
Initialize traversal node for collision between two meshes, given the current transforms.
int addSubModel(const std::vector< Vec3f > &ps, const std::vector< Triangle > &ts)
Add a set of triangles in the new BVH model.
size_t numContacts() const
number of contacts found
void generateRandomTransforms(FCL_REAL extents[6], std::vector< Transform3f > &transforms, std::size_t n)
Generate n random transforms whose translations are constrained by extents.
bool collide_Test(const Transform3f &tf, const std::vector< Vec3f > &vertices1, const std::vector< Triangle > &triangles1, const std::vector< Vec3f > &vertices2, const std::vector< Triangle > &triangles2, SplitMethodType split_method, bool verbose)
bool collide_front_list_Test(const Transform3f &tf1, const Transform3f &tf2, const std::vector< Vec3f > &vertices1, const std::vector< Triangle > &triangles1, const std::vector< Vec3f > &vertices2, const std::vector< Triangle > &triangles2, SplitMethodType split_method, bool refit_bottomup, bool verbose)
std::size_t getNbRun(const int &argc, char const *const *argv, std::size_t defaultValue)
Get the argument –nb-run from argv.
request to the collision algorithm
void clear()
clear the results obtained
int beginModel(unsigned int num_tris=0, unsigned int num_vertices=0)
Begin a new BVH model.
BOOST_AUTO_TEST_CASE(front_list)
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
SplitMethodType
Three types of split algorithms are provided in FCL as default.
HPP_FCL_DLLAPI std::size_t collide(const CollisionObject *o1, const CollisionObject *o2, const CollisionRequest &request, CollisionResult &result)
Main collision interface: given two collision objects, and the requirements for contacts, including num of max contacts, whether perform exhaustive collision (i.e., returning returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function performs the collision between them. Return value is the number of contacts generated between the two objects.
shared_ptr< BVSplitter< BV > > bv_splitter
Split rule to split one BV node into two children.
A class describing the split rule that splits each BV node.
std::list< BVHFrontNode > BVHFrontList
BVH front list is a list of front nodes.
bool collide_front_list_Test_Oriented(const Transform3f &tf1, const Transform3f &tf2, const std::vector< Vec3f > &vertices1, const std::vector< Triangle > &triangles1, const std::vector< Vec3f > &vertices2, const std::vector< Triangle > &triangles2, SplitMethodType split_method, bool verbose)
int endModel()
End BVH model construction, will build the bounding volume hierarchy.