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::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
nearlyEqual
bool nearlyEqual(const Vec3f &a, const Vec3f &b)
Definition: test/distance.cpp:82
hpp::fcl::DistanceRequest
request to the distance computation
Definition: collision_data.h:392
traversal_node_setup.h
generate_distance_plot.b
float b
Definition: generate_distance_plot.py:8
octree.p1
tuple p1
Definition: octree.py:55
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
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::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
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
hpp::fcl::CollisionResult::numContacts
size_t numContacts() const
number of contacts found
Definition: collision_data.h:342
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
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
BV_splitter.h
DELTA
FCL_REAL DELTA
Definition: test/distance.cpp:56
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::CollisionRequest
request to the collision algorithm
Definition: collision_data.h:235
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
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
traversal_node_bvhs.h
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::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
extents
FCL_REAL extents[6]
Definition: test/geometric_shapes.cpp:51
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
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(mesh_distance)
Definition: test/distance.cpp:89
verbose
bool verbose
Definition: test/distance.cpp:55
hpp::fcl::SplitMethodType
SplitMethodType
Three types of split algorithms are provided in FCL as default.
Definition: internal/BV_splitter.h:51
res
res
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, 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.
Definition: src/collision.cpp:63
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
hpp::fcl::BVSplitter
A class describing the split rule that splits each BV node.
Definition: BVH/BVH_model.h:59
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
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::BVHModelBase::endModel
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:00