frontlist.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2015, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
38 #define BOOST_TEST_MODULE FCL_FRONT_LIST
39 #include <boost/test/included/unit_test.hpp>
40 
43 #include <../src/collision_node.h>
45 #include "utility.h"
46 
47 #include "fcl_resources/config.h"
48 #include <boost/filesystem.hpp>
49 
50 using namespace hpp::fcl;
51 namespace utf = boost::unit_test::framework;
52 
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,
59  SplitMethodType split_method, bool refit_bottomup,
60  bool verbose);
61 
62 template <typename BV, typename TraversalNode>
64  const Transform3f& tf2,
65  const std::vector<Vec3f>& vertices1,
66  const std::vector<Triangle>& triangles1,
67  const std::vector<Vec3f>& vertices2,
68  const std::vector<Triangle>& triangles2,
69  SplitMethodType split_method,
70  bool verbose);
71 
72 template <typename BV>
73 bool collide_Test(const Transform3f& tf, const std::vector<Vec3f>& vertices1,
74  const std::vector<Triangle>& triangles1,
75  const std::vector<Vec3f>& vertices2,
76  const std::vector<Triangle>& triangles2,
77  SplitMethodType split_method, bool verbose);
78 
79 // TODO: randomly still have some runtime error
80 BOOST_AUTO_TEST_CASE(front_list) {
81  std::vector<Vec3f> p1, p2;
82  std::vector<Triangle> t1, t2;
83  boost::filesystem::path path(TEST_RESOURCES_DIR);
84  loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
85  loadOBJFile((path / "rob.obj").string().c_str(), p2, t2);
86 
87  std::vector<Transform3f> transforms; // t0
88  std::vector<Transform3f> transforms2; // t1
89  FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
90  FCL_REAL delta_trans[] = {1, 1, 1};
91 #ifndef NDEBUG // if debug mode
92  std::size_t n = 2;
93 #else
94  std::size_t n = 20;
95 #endif
96  n = getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, n);
97  bool verbose = false;
98 
99  generateRandomTransforms(extents, delta_trans, 0.005 * 2 * 3.1415, transforms,
100  transforms2, n);
101 
102  bool res, res2;
103 
104  for (std::size_t i = 0; i < transforms.size(); ++i) {
105  res = collide_Test<AABB>(transforms2[i], p1, t1, p2, t2,
107  res2 =
108  collide_front_list_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2,
109  t2, SPLIT_METHOD_MEDIAN, false, verbose);
110  BOOST_CHECK(res == res2);
111  res = collide_Test<AABB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN,
112  verbose);
113  res2 =
114  collide_front_list_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2,
115  t2, SPLIT_METHOD_MEAN, false, verbose);
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,
120  p2, t2, SPLIT_METHOD_BV_CENTER, false,
121  verbose);
122  BOOST_CHECK(res == res2);
123  }
124 
125  for (std::size_t i = 0; i < transforms.size(); ++i) {
126  res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN,
127  verbose);
128  res2 =
129  collide_front_list_Test<OBB>(transforms[i], transforms2[i], p1, t1, p2,
130  t2, SPLIT_METHOD_MEDIAN, false, verbose);
131  BOOST_CHECK(res == res2);
132  res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN,
133  verbose);
134  res2 =
135  collide_front_list_Test<OBB>(transforms[i], transforms2[i], p1, t1, p2,
136  t2, SPLIT_METHOD_MEAN, false, verbose);
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,
141  p2, t2, SPLIT_METHOD_BV_CENTER, false,
142  verbose);
143  BOOST_CHECK(res == res2);
144  }
145 
146  for (std::size_t i = 0; i < transforms.size(); ++i) {
147  // Disabled broken test lines. Please see #25.
148  // res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2,
149  // SPLIT_METHOD_MEDIAN, verbose); res2 =
150  // collide_front_list_Test<RSS>(transforms[i], transforms2[i], p1, t1, p2,
151  // t2, SPLIT_METHOD_MEDIAN, false, verbose); BOOST_CHECK(res == res2);
152  res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN,
153  verbose);
154  res2 =
155  collide_front_list_Test<RSS>(transforms[i], transforms2[i], p1, t1, p2,
156  t2, SPLIT_METHOD_MEAN, false, verbose);
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,
161  p2, t2, SPLIT_METHOD_BV_CENTER, false,
162  verbose);
163  BOOST_CHECK(res == res2);
164  }
165 
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,
170  t1, p2, t2, SPLIT_METHOD_MEDIAN,
171  false, verbose);
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,
176  t1, p2, t2, SPLIT_METHOD_MEAN,
177  false, verbose);
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> >(
182  transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER,
183  false, verbose);
184  BOOST_CHECK(res == res2);
185  }
186 
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,
191  t1, p2, t2, SPLIT_METHOD_MEDIAN,
192  false, verbose);
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,
197  t1, p2, t2, SPLIT_METHOD_MEAN,
198  false, verbose);
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> >(
203  transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER,
204  false, verbose);
205  BOOST_CHECK(res == res2);
206  }
207 
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,
212  t1, p2, t2, SPLIT_METHOD_MEDIAN,
213  false, verbose);
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,
218  t1, p2, t2, SPLIT_METHOD_MEAN,
219  false, verbose);
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> >(
224  transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER,
225  false, verbose);
226  BOOST_CHECK(res == res2);
227  }
228 
229  for (std::size_t i = 0; i < transforms.size(); ++i) {
230  res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN,
231  verbose);
232  res2 = collide_front_list_Test_Oriented<RSS, MeshCollisionTraversalNodeRSS>(
233  transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN,
234  verbose);
235  BOOST_CHECK(res == res2);
236  res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN,
237  verbose);
238  res2 = collide_front_list_Test_Oriented<RSS, MeshCollisionTraversalNodeRSS>(
239  transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN,
240  verbose);
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>(
245  transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER,
246  verbose);
247  BOOST_CHECK(res == res2);
248  }
249 
250  for (std::size_t i = 0; i < transforms.size(); ++i) {
251  res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN,
252  verbose);
253  res2 = collide_front_list_Test_Oriented<OBB, MeshCollisionTraversalNodeOBB>(
254  transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN,
255  verbose);
256  BOOST_CHECK(res == res2);
257  res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN,
258  verbose);
259  res2 = collide_front_list_Test_Oriented<OBB, MeshCollisionTraversalNodeOBB>(
260  transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN,
261  verbose);
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>(
266  transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER,
267  verbose);
268  BOOST_CHECK(res == res2);
269  }
270 }
271 
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,
278  SplitMethodType split_method, bool refit_bottomup,
279  bool verbose) {
280  BVHModel<BV> m1;
281  BVHModel<BV> m2;
282  m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
283  m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
284 
285  BVHFrontList front_list;
286 
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]);
290  }
291 
292  m1.beginModel();
293  m1.addSubModel(vertices1_new, triangles1);
294  m1.endModel();
295 
296  m2.beginModel();
297  m2.addSubModel(vertices2, triangles2);
298  m2.endModel();
299 
300  Transform3f pose1, pose2;
301 
302  CollisionResult local_result;
303  CollisionRequest request(NO_REQUEST, (std::numeric_limits<int>::max)());
304  MeshCollisionTraversalNode<BV> node(request);
305 
306  bool success = initialize<BV>(node, m1, pose1, m2, pose2, local_result);
307  BOOST_REQUIRE(success);
308 
309  node.enable_statistics = verbose;
310 
311  collide(&node, request, local_result, &front_list);
312 
313  if (verbose)
314  std::cout << "front list size " << front_list.size() << std::endl;
315 
316  // update the mesh
317  for (std::size_t i = 0; i < vertices1.size(); ++i) {
318  vertices1_new[i] = tf2.transform(vertices1[i]);
319  }
320 
321  m1.beginReplaceModel();
322  m1.replaceSubModel(vertices1_new);
323  m1.endReplaceModel(true, refit_bottomup);
324 
325  m2.beginReplaceModel();
326  m2.replaceSubModel(vertices2);
327  m2.endReplaceModel(true, refit_bottomup);
328 
329  local_result.clear();
330  collide(&node, request, local_result, &front_list);
331 
332  if (local_result.numContacts() > 0)
333  return true;
334  else
335  return false;
336 }
337 
338 template <typename BV, typename TraversalNode>
340  const Transform3f& tf2,
341  const std::vector<Vec3f>& vertices1,
342  const std::vector<Triangle>& triangles1,
343  const std::vector<Vec3f>& vertices2,
344  const std::vector<Triangle>& triangles2,
345  SplitMethodType split_method,
346  bool verbose) {
347  BVHModel<BV> m1;
348  BVHModel<BV> m2;
349  m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
350  m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
351 
352  BVHFrontList front_list;
353 
354  m1.beginModel();
355  m1.addSubModel(vertices1, triangles1);
356  m1.endModel();
357 
358  m2.beginModel();
359  m2.addSubModel(vertices2, triangles2);
360  m2.endModel();
361 
362  Transform3f pose1(tf1), pose2;
363 
364  CollisionResult local_result;
365  CollisionRequest request(NO_REQUEST, (std::numeric_limits<int>::max)());
366  TraversalNode node(request);
367 
368  bool success = initialize(node, (const BVHModel<BV>&)m1, pose1,
369  (const BVHModel<BV>&)m2, pose2, local_result);
370  BOOST_REQUIRE(success);
371 
372  node.enable_statistics = verbose;
373 
374  collide(&node, request, local_result, &front_list);
375 
376  if (verbose)
377  std::cout << "front list size " << front_list.size() << std::endl;
378 
379  // update the mesh
380  pose1 = tf2;
381  success = initialize(node, (const BVHModel<BV>&)m1, pose1,
382  (const BVHModel<BV>&)m2, pose2, local_result);
383  BOOST_REQUIRE(success);
384 
385  local_result.clear();
386  collide(&node, request, local_result, &front_list);
387 
388  if (local_result.numContacts() > 0)
389  return true;
390  else
391  return false;
392 }
393 
394 template <typename BV>
395 bool collide_Test(const Transform3f& tf, const std::vector<Vec3f>& vertices1,
396  const std::vector<Triangle>& triangles1,
397  const std::vector<Vec3f>& vertices2,
398  const std::vector<Triangle>& triangles2,
399  SplitMethodType split_method, bool verbose) {
400  BVHModel<BV> m1;
401  BVHModel<BV> m2;
402  m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
403  m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
404 
405  m1.beginModel();
406  m1.addSubModel(vertices1, triangles1);
407  m1.endModel();
408 
409  m2.beginModel();
410  m2.addSubModel(vertices2, triangles2);
411  m2.endModel();
412 
413  Transform3f pose1(tf), pose2;
414 
415  CollisionResult local_result;
416  CollisionRequest request(NO_REQUEST, (std::numeric_limits<int>::max)());
417  MeshCollisionTraversalNode<BV> node(request);
418 
419  bool success = initialize<BV>(node, m1, pose1, m2, pose2, local_result);
420  BOOST_REQUIRE(success);
421 
422  node.enable_statistics = verbose;
423 
424  collide(&node, request, local_result);
425 
426  if (local_result.numContacts() > 0)
427  return true;
428  else
429  return false;
430 }
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
tuple p1
Definition: octree.py:55
void loadOBJFile(const char *filename, std::vector< Vec3f > &points, std::vector< Triangle > &triangles)
Load an obj mesh file.
Definition: utility.cpp:96
tuple tf2
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.
Definition: BVH_model.cpp:410
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.
Definition: utility.cpp:224
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)
Definition: frontlist.cpp:395
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)
Definition: frontlist.cpp:273
std::size_t getNbRun(const int &argc, char const *const *argv, std::size_t defaultValue)
Get the argument –nb-run from argv.
Definition: utility.cpp:382
request to the collision algorithm
void clear()
clear the results obtained
Vec3f transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:153
double FCL_REAL
Definition: data_types.h:65
int beginModel(unsigned int num_tris=0, unsigned int num_vertices=0)
Begin a new BVH model.
Definition: BVH_model.cpp:170
FCL_REAL extents[6]
BOOST_AUTO_TEST_CASE(front_list)
Definition: frontlist.cpp:80
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.
res
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.
Definition: BVH/BVH_model.h:59
bool verbose
Definition: benchmark.cpp:32
std::list< BVHFrontNode > BVHFrontList
BVH front list is a list of front nodes.
Definition: BVH_front.h:67
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)
Definition: frontlist.cpp:339
tuple tf1
int endModel()
End BVH model construction, will build the bounding volume hierarchy.
Definition: BVH_model.cpp:473


hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:01