test/distance.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_DISTANCE
39 #include <chrono>
40 
41 #include <boost/test/included/unit_test.hpp>
42 #include <boost/filesystem.hpp>
43 
44 #include <hpp/fcl/internal/traversal_node_bvhs.h>
45 #include <hpp/fcl/internal/traversal_node_setup.h>
46 #include "../src/collision_node.h"
47 #include <hpp/fcl/internal/BV_splitter.h>
48 
49 #include "utility.h"
50 #include "fcl_resources/config.h"
51 
52 using namespace hpp::fcl;
53 namespace utf = boost::unit_test::framework;
54 
55 bool verbose = false;
56 FCL_REAL DELTA = 0.001;
57 
58 template <typename BV>
59 void distance_Test(const Transform3f& tf, const std::vector<Vec3f>& vertices1,
60  const std::vector<Triangle>& triangles1,
61  const std::vector<Vec3f>& vertices2,
62  const std::vector<Triangle>& triangles2,
63  SplitMethodType split_method, unsigned int qsize,
64  DistanceRes& distance_result, bool verbose = true);
65 
66 bool collide_Test_OBB(const Transform3f& tf,
67  const std::vector<Vec3f>& vertices1,
68  const std::vector<Triangle>& triangles1,
69  const std::vector<Vec3f>& vertices2,
70  const std::vector<Triangle>& triangles2,
71  SplitMethodType split_method, bool verbose);
72 
73 template <typename BV, typename TraversalNode>
74 void distance_Test_Oriented(const Transform3f& tf,
75  const std::vector<Vec3f>& vertices1,
76  const std::vector<Triangle>& triangles1,
77  const std::vector<Vec3f>& vertices2,
78  const std::vector<Triangle>& triangles2,
79  SplitMethodType split_method, unsigned int qsize,
80  DistanceRes& distance_result, bool verbose = true);
81 
82 inline bool nearlyEqual(const Vec3f& a, const Vec3f& b) {
83  if (fabs(a[0] - b[0]) > DELTA) return false;
84  if (fabs(a[1] - b[1]) > DELTA) return false;
85  if (fabs(a[2] - b[2]) > DELTA) return false;
86  return true;
87 }
88 
89 BOOST_AUTO_TEST_CASE(mesh_distance) {
90  std::vector<Vec3f> p1, p2;
91  std::vector<Triangle> t1, t2;
92  boost::filesystem::path path(TEST_RESOURCES_DIR);
93  loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
94  loadOBJFile((path / "rob.obj").string().c_str(), p2, t2);
95 
96  std::vector<Transform3f> transforms; // t0
97  FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
98 #ifndef NDEBUG // if debug mode
99  std::size_t n = 1;
100 #else
101  std::size_t n = 10;
102 #endif
103  n = getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, n);
104 
105  generateRandomTransforms(extents, transforms, n);
106 
107  double dis_time = 0;
108  double col_time = 0;
109 
110  DistanceRes res, res_now;
111  for (std::size_t i = 0; i < transforms.size(); ++i) {
112  auto timer_col = std::chrono::high_resolution_clock::now();
113  collide_Test_OBB(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
114  col_time += std::chrono::duration_cast<std::chrono::duration<double>>(
115  std::chrono::high_resolution_clock::now() - timer_col)
116  .count();
117 
118  auto timer_dist = std::chrono::high_resolution_clock::now();
119  distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(
120  transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res, verbose);
121  dis_time += std::chrono::duration_cast<std::chrono::duration<double>>(
122  std::chrono::high_resolution_clock::now() - timer_dist)
123  .count();
124 
125  distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(
126  transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now,
127  verbose);
128 
129  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
130  BOOST_CHECK(fabs(res.distance) < DELTA ||
131  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
132  nearlyEqual(res.p2, res_now.p2)));
133 
134  distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(
135  transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now,
136  verbose);
137 
138  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
139  BOOST_CHECK(fabs(res.distance) < DELTA ||
140  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
141  nearlyEqual(res.p2, res_now.p2)));
142 
143  distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(
144  transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);
145 
146  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
147  BOOST_CHECK(fabs(res.distance) < DELTA ||
148  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
149  nearlyEqual(res.p2, res_now.p2)));
150 
151  distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(
152  transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now,
153  verbose);
154 
155  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
156  BOOST_CHECK(fabs(res.distance) < DELTA ||
157  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
158  nearlyEqual(res.p2, res_now.p2)));
159 
160  distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(
161  transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now,
162  verbose);
163 
164  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
165  BOOST_CHECK(fabs(res.distance) < DELTA ||
166  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
167  nearlyEqual(res.p2, res_now.p2)));
168 
169  distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(
170  transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);
171 
172  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
173  BOOST_CHECK(fabs(res.distance) < DELTA ||
174  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
175  nearlyEqual(res.p2, res_now.p2)));
176 
177  distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(
178  transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now,
179  verbose);
180 
181  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
182  BOOST_CHECK(fabs(res.distance) < DELTA ||
183  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
184  nearlyEqual(res.p2, res_now.p2)));
185 
186  distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(
187  transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now,
188  verbose);
189 
190  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
191  BOOST_CHECK(fabs(res.distance) < DELTA ||
192  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
193  nearlyEqual(res.p2, res_now.p2)));
194 
195  distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(
196  transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);
197 
198  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
199  BOOST_CHECK(fabs(res.distance) < DELTA ||
200  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
201  nearlyEqual(res.p2, res_now.p2)));
202 
203  distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(
204  transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now,
205  verbose);
206 
207  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
208  BOOST_CHECK(fabs(res.distance) < DELTA ||
209  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
210  nearlyEqual(res.p2, res_now.p2)));
211 
212  distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(
213  transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now,
214  verbose);
215 
216  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
217  BOOST_CHECK(fabs(res.distance) < DELTA ||
218  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
219  nearlyEqual(res.p2, res_now.p2)));
220 
221  distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(
222  transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);
223 
224  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
225  BOOST_CHECK(fabs(res.distance) < DELTA ||
226  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
227  nearlyEqual(res.p2, res_now.p2)));
228 
229  distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(
230  transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now,
231  verbose);
232 
233  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
234  BOOST_CHECK(fabs(res.distance) < DELTA ||
235  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
236  nearlyEqual(res.p2, res_now.p2)));
237 
238  distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(
239  transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now,
240  verbose);
241 
242  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
243  BOOST_CHECK(fabs(res.distance) < DELTA ||
244  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
245  nearlyEqual(res.p2, res_now.p2)));
246 
247  distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(
248  transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);
249 
250  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
251  BOOST_CHECK(fabs(res.distance) < DELTA ||
252  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
253  nearlyEqual(res.p2, res_now.p2)));
254 
255  distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(
256  transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now,
257  verbose);
258 
259  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
260  BOOST_CHECK(fabs(res.distance) < DELTA ||
261  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
262  nearlyEqual(res.p2, res_now.p2)));
263 
264  distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(
265  transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now,
266  verbose);
267 
268  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
269  BOOST_CHECK(fabs(res.distance) < DELTA ||
270  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
271  nearlyEqual(res.p2, res_now.p2)));
272 
273  distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2,
274  res_now, verbose);
275 
276  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
277  BOOST_CHECK(fabs(res.distance) < DELTA ||
278  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
279  nearlyEqual(res.p2, res_now.p2)));
280 
281  distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2,
282  res_now, verbose);
283 
284  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
285  BOOST_CHECK(fabs(res.distance) < DELTA ||
286  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
287  nearlyEqual(res.p2, res_now.p2)));
288 
289  distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2,
290  res_now, verbose);
291 
292  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
293  BOOST_CHECK(fabs(res.distance) < DELTA ||
294  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
295  nearlyEqual(res.p2, res_now.p2)));
296 
297  distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20,
298  res_now, verbose);
299 
300  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
301  BOOST_CHECK(fabs(res.distance) < DELTA ||
302  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
303  nearlyEqual(res.p2, res_now.p2)));
304 
305  distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER,
306  20, res_now, verbose);
307 
308  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
309  BOOST_CHECK(fabs(res.distance) < DELTA ||
310  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
311  nearlyEqual(res.p2, res_now.p2)));
312 
313  distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20,
314  res_now, verbose);
315 
316  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
317  BOOST_CHECK(fabs(res.distance) < DELTA ||
318  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
319  nearlyEqual(res.p2, res_now.p2)));
320 
321  distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2,
322  res_now, verbose);
323 
324  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
325  BOOST_CHECK(fabs(res.distance) < DELTA ||
326  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
327  nearlyEqual(res.p2, res_now.p2)));
328 
329  distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2,
330  res_now, verbose);
331 
332  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
333  BOOST_CHECK(fabs(res.distance) < DELTA ||
334  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
335  nearlyEqual(res.p2, res_now.p2)));
336 
337  distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2,
338  res_now, verbose);
339 
340  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
341  BOOST_CHECK(fabs(res.distance) < DELTA ||
342  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
343  nearlyEqual(res.p2, res_now.p2)));
344 
345  distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20,
346  res_now, verbose);
347 
348  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
349  BOOST_CHECK(fabs(res.distance) < DELTA ||
350  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
351  nearlyEqual(res.p2, res_now.p2)));
352 
353  distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER,
354  20, res_now, verbose);
355 
356  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
357  BOOST_CHECK(fabs(res.distance) < DELTA ||
358  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
359  nearlyEqual(res.p2, res_now.p2)));
360 
361  distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20,
362  res_now, verbose);
363 
364  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
365  BOOST_CHECK(fabs(res.distance) < DELTA ||
366  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
367  nearlyEqual(res.p2, res_now.p2)));
368 
369  distance_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2,
370  res_now, verbose);
371 
372  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
373  BOOST_CHECK(fabs(res.distance) < DELTA ||
374  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
375  nearlyEqual(res.p2, res_now.p2)));
376 
377  distance_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2,
378  res_now, verbose);
379 
380  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
381  BOOST_CHECK(fabs(res.distance) < DELTA ||
382  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
383  nearlyEqual(res.p2, res_now.p2)));
384 
385  distance_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER,
386  2, res_now, verbose);
387 
388  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
389  BOOST_CHECK(fabs(res.distance) < DELTA ||
390  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
391  nearlyEqual(res.p2, res_now.p2)));
392 
393  distance_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20,
394  res_now, verbose);
395 
396  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
397  BOOST_CHECK(fabs(res.distance) < DELTA ||
398  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
399  nearlyEqual(res.p2, res_now.p2)));
400 
401  distance_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20,
402  res_now, verbose);
403 
404  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
405  BOOST_CHECK(fabs(res.distance) < DELTA ||
406  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
407  nearlyEqual(res.p2, res_now.p2)));
408 
409  distance_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER,
410  20, res_now, verbose);
411 
412  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
413  BOOST_CHECK(fabs(res.distance) < DELTA ||
414  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
415  nearlyEqual(res.p2, res_now.p2)));
416 
417  distance_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2,
418  res_now, verbose);
419 
420  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
421  BOOST_CHECK(fabs(res.distance) < DELTA ||
422  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
423  nearlyEqual(res.p2, res_now.p2)));
424 
425  distance_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2,
426  res_now, verbose);
427 
428  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
429  BOOST_CHECK(fabs(res.distance) < DELTA ||
430  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
431  nearlyEqual(res.p2, res_now.p2)));
432 
433  distance_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER,
434  2, res_now, verbose);
435 
436  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
437  BOOST_CHECK(fabs(res.distance) < DELTA ||
438  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
439  nearlyEqual(res.p2, res_now.p2)));
440 
441  distance_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20,
442  res_now, verbose);
443 
444  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
445  BOOST_CHECK(fabs(res.distance) < DELTA ||
446  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
447  nearlyEqual(res.p2, res_now.p2)));
448 
449  distance_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN,
450  20, res_now, verbose);
451 
452  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
453  BOOST_CHECK(fabs(res.distance) < DELTA ||
454  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
455  nearlyEqual(res.p2, res_now.p2)));
456 
457  distance_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER,
458  20, res_now, verbose);
459 
460  BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
461  BOOST_CHECK(fabs(res.distance) < DELTA ||
462  (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) &&
463  nearlyEqual(res.p2, res_now.p2)));
464  }
465 
466  BOOST_TEST_MESSAGE("distance timing: " << dis_time << " sec");
467  BOOST_TEST_MESSAGE("collision timing: " << col_time << " sec");
468 }
469 
470 template <typename BV, typename TraversalNode>
471 void distance_Test_Oriented(const Transform3f& tf,
472  const std::vector<Vec3f>& vertices1,
473  const std::vector<Triangle>& triangles1,
474  const std::vector<Vec3f>& vertices2,
475  const std::vector<Triangle>& triangles2,
476  SplitMethodType split_method, unsigned int qsize,
477  DistanceRes& distance_result, bool verbose) {
478  BVHModel<BV> m1;
479  BVHModel<BV> m2;
480  m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
481  m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
482 
483  m1.beginModel();
484  m1.addSubModel(vertices1, triangles1);
485  m1.endModel();
486 
487  m2.beginModel();
488  m2.addSubModel(vertices2, triangles2);
489  m2.endModel();
490 
491  DistanceResult local_result;
492  TraversalNode node;
493  if (!initialize(node, (const BVHModel<BV>&)m1, tf, (const BVHModel<BV>&)m2,
494  Transform3f(), DistanceRequest(true), local_result))
495  std::cout << "initialize error" << std::endl;
496 
497  node.enable_statistics = verbose;
498 
499  distance(&node, NULL, qsize);
500 
501  // points are in local coordinate, to global coordinate
502  Vec3f p1 = local_result.nearest_points[0];
503  Vec3f p2 = local_result.nearest_points[1];
504 
505  distance_result.distance = local_result.min_distance;
506  distance_result.p1 = p1;
507  distance_result.p2 = p2;
508 
509  if (verbose) {
510  std::cout << "distance " << local_result.min_distance << std::endl;
511 
512  std::cout << p1[0] << " " << p1[1] << " " << p1[2] << std::endl;
513  std::cout << p2[0] << " " << p2[1] << " " << p2[2] << std::endl;
514  std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
515  }
516 }
517 
518 template <typename BV>
519 void distance_Test(const Transform3f& tf, const std::vector<Vec3f>& vertices1,
520  const std::vector<Triangle>& triangles1,
521  const std::vector<Vec3f>& vertices2,
522  const std::vector<Triangle>& triangles2,
523  SplitMethodType split_method, unsigned int qsize,
524  DistanceRes& distance_result, bool verbose) {
525  BVHModel<BV> m1;
526  BVHModel<BV> m2;
527  m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
528  m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
529 
530  m1.beginModel();
531  m1.addSubModel(vertices1, triangles1);
532  m1.endModel();
533 
534  m2.beginModel();
535  m2.addSubModel(vertices2, triangles2);
536  m2.endModel();
537 
538  Transform3f pose1(tf), pose2;
539 
540  DistanceResult local_result;
541  MeshDistanceTraversalNode<BV> node;
542 
543  if (!initialize<BV>(node, m1, pose1, m2, pose2, DistanceRequest(true),
544  local_result))
545  std::cout << "initialize error" << std::endl;
546 
547  node.enable_statistics = verbose;
548 
549  distance(&node, NULL, qsize);
550 
551  distance_result.distance = local_result.min_distance;
552  distance_result.p1 = local_result.nearest_points[0];
553  distance_result.p2 = local_result.nearest_points[1];
554 
555  if (verbose) {
556  std::cout << "distance " << local_result.min_distance << std::endl;
557 
558  std::cout << local_result.nearest_points[0][0] << " "
559  << local_result.nearest_points[0][1] << " "
560  << local_result.nearest_points[0][2] << std::endl;
561  std::cout << local_result.nearest_points[1][0] << " "
562  << local_result.nearest_points[1][1] << " "
563  << local_result.nearest_points[1][2] << std::endl;
564  std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
565  }
566 }
567 
568 bool collide_Test_OBB(const Transform3f& tf,
569  const std::vector<Vec3f>& vertices1,
570  const std::vector<Triangle>& triangles1,
571  const std::vector<Vec3f>& vertices2,
572  const std::vector<Triangle>& triangles2,
573  SplitMethodType split_method, bool verbose) {
574  BVHModel<OBB> m1;
575  BVHModel<OBB> m2;
576  m1.bv_splitter.reset(new BVSplitter<OBB>(split_method));
577  m2.bv_splitter.reset(new BVSplitter<OBB>(split_method));
578 
579  m1.beginModel();
580  m1.addSubModel(vertices1, triangles1);
581  m1.endModel();
582 
583  m2.beginModel();
584  m2.addSubModel(vertices2, triangles2);
585  m2.endModel();
586 
587  CollisionResult local_result;
588  CollisionRequest request(CONTACT | DISTANCE_LOWER_BOUND, 1);
589  MeshCollisionTraversalNodeOBB node(request);
590  bool success(initialize(node, (const BVHModel<OBB>&)m1, tf,
591  (const BVHModel<OBB>&)m2, Transform3f(),
592  local_result));
593  BOOST_REQUIRE(success);
594 
595  node.enable_statistics = verbose;
596 
597  collide(&node, request, local_result);
598 
599  if (local_result.numContacts() > 0)
600  return true;
601  else
602  return false;
603 }
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::loadOBJFile
void loadOBJFile(const char *filename, std::vector< Vec3f > &points, std::vector< Triangle > &triangles)
Load an obj mesh file.
Definition: utility.cpp:96
hpp::fcl::DistanceResult::nearest_points
Vec3f nearest_points[2]
nearest points
Definition: collision_data.h:427
hpp::fcl::DistanceRes::p1
Vec3f p1
Definition: utility.h:165
hpp::fcl::BVHModelBase::beginModel
int beginModel(unsigned int num_tris=0, unsigned int num_vertices=0)
Begin a new BVH model.
Definition: BVH_model.cpp:170
hpp::fcl::CONTACT
@ CONTACT
Definition: collision_data.h:229
initialize
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.
Definition: traversal_node_setup.h:423
hpp::fcl::CollisionResult::numContacts
size_t numContacts() const
number of contacts found
Definition: collision_data.h:342
hpp::fcl::DistanceRes::distance
double distance
Definition: utility.h:164
utility.h
distance_Test
void distance_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, unsigned int qsize, DistanceRes &distance_result, bool verbose=true)
Definition: test/distance.cpp:519
hpp::fcl::distance
HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS.cpp:181
nearlyEqual
bool nearlyEqual(const Vec3f &a, const Vec3f &b)
Definition: test/distance.cpp:82
hpp::fcl::SPLIT_METHOD_MEDIAN
@ SPLIT_METHOD_MEDIAN
Definition: internal/BV_splitter.h:53
distance_Test_Oriented
void distance_Test_Oriented(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, unsigned int qsize, DistanceRes &distance_result, bool verbose=true)
Definition: test/distance.cpp:471
collide_Test_OBB
bool collide_Test_OBB(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: test/distance.cpp:568
extents
FCL_REAL extents[6]
Definition: test/geometric_shapes.cpp:51
hpp::fcl::BVHModel::bv_splitter
shared_ptr< BVSplitter< BV > > bv_splitter
Split rule to split one BV node into two children.
Definition: BVH/BVH_model.h:278
a
list a
hpp::fcl::BVHModelBase::endModel
int endModel()
End BVH model construction, will build the bounding volume hierarchy.
Definition: BVH_model.cpp:473
hpp::fcl::collide
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,...
Definition: src/collision.cpp:63
res
res
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
verbose
bool verbose
Definition: test/distance.cpp:55
hpp::fcl::SPLIT_METHOD_MEAN
@ SPLIT_METHOD_MEAN
Definition: internal/BV_splitter.h:52
hpp::fcl::SplitMethodType
SplitMethodType
Three types of split algorithms are provided in FCL as default.
Definition: internal/BV_splitter.h:51
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(mesh_distance)
Definition: test/distance.cpp:89
hpp::fcl::CollisionRequest
request to the collision algorithm
Definition: collision_data.h:235
hpp::fcl::getNbRun
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
hpp::fcl::CollisionResult
collision result
Definition: collision_data.h:302
hpp::fcl::SPLIT_METHOD_BV_CENTER
@ SPLIT_METHOD_BV_CENTER
Definition: internal/BV_splitter.h:54
hpp::fcl::generateRandomTransforms
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
collision.path
path
Definition: scripts/collision.py:6
hpp::fcl::DistanceResult::min_distance
FCL_REAL min_distance
minimum distance between two objects. if two objects are in collision, min_distance <= 0.
Definition: collision_data.h:424
hpp::fcl
Definition: broadphase_bruteforce.h:45
octree.p1
tuple p1
Definition: octree.py:54
generate_distance_plot.b
float b
Definition: generate_distance_plot.py:7
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
DELTA
FCL_REAL DELTA
Definition: test/distance.cpp:56
hpp::fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH/BVH_model.h:273
hpp::fcl::DistanceRequest
request to the distance computation
Definition: collision_data.h:392
hpp::fcl::BVSplitter
A class describing the split rule that splits each BV node.
Definition: BVH/BVH_model.h:59
traversal_node_bvhs.h
BV_splitter.h
hpp::fcl::DistanceResult
distance result
Definition: collision_data.h:420
hpp::fcl::DistanceRes::p2
Vec3f p2
Definition: utility.h:166
traversal_node_setup.h
hpp::fcl::DistanceRes
Definition: utility.h:163
hpp::fcl::DISTANCE_LOWER_BOUND
@ DISTANCE_LOWER_BOUND
Definition: collision_data.h:230
hpp::fcl::BVHModelBase::addSubModel
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

hpp-fcl
Author(s):
autogenerated on Fri Jan 26 2024 03:46:13