test_fcl_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-2016, 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 #include <gtest/gtest.h>
39 
40 #include "fcl/narrowphase/detail/traversal/collision_node.h"
41 #include "test_fcl_utility.h"
42 #include "eigen_matrix_compare.h"
43 #include "fcl_resources/config.h"
44 
45 // TODO(SeanCurtis-TRI): A file called `test_fcl_distance.cpp` should *not* have
46 // collision tests.
47 
48 using namespace fcl;
49 
50 bool verbose = false;
51 
52 template <typename S>
53 S DELTA() { return 0.001; }
54 
55 template<typename BV>
56 void distance_Test(const Transform3<typename BV::S>& tf,
57  const std::vector<Vector3<typename BV::S>>& vertices1, const std::vector<Triangle>& triangles1,
58  const std::vector<Vector3<typename BV::S>>& vertices2, const std::vector<Triangle>& triangles2, detail::SplitMethodType split_method,
59  int qsize,
60  test::DistanceRes<typename BV::S>& distance_result,
61  bool verbose = true);
62 
63 template <typename S>
64 bool collide_Test_OBB(const Transform3<S>& tf,
65  const std::vector<Vector3<S>>& vertices1, const std::vector<Triangle>& triangles1,
66  const std::vector<Vector3<S>>& vertices2, const std::vector<Triangle>& triangles2, detail::SplitMethodType split_method, bool verbose);
67 
68 template<typename BV, typename TraversalNode>
69 void distance_Test_Oriented(const Transform3<typename BV::S>& tf,
70  const std::vector<Vector3<typename BV::S>>& vertices1, const std::vector<Triangle>& triangles1,
71  const std::vector<Vector3<typename BV::S>>& vertices2, const std::vector<Triangle>& triangles2, detail::SplitMethodType split_method,
72  int qsize,
73  test::DistanceRes<typename BV::S>& distance_result,
74  bool verbose = true);
75 
76 template <typename S>
77 bool nearlyEqual(const Vector3<S>& a, const Vector3<S>& b)
78 {
79  if(fabs(a[0] - b[0]) > DELTA<S>()) return false;
80  if(fabs(a[1] - b[1]) > DELTA<S>()) return false;
81  if(fabs(a[2] - b[2]) > DELTA<S>()) return false;
82  return true;
83 }
84 
85 template <typename S>
86 void test_mesh_distance()
87 {
88  std::vector<Vector3<S>> p1, p2;
89  std::vector<Triangle> t1, t2;
90 
91  test::loadOBJFile(TEST_RESOURCES_DIR"/env.obj", p1, t1);
92  test::loadOBJFile(TEST_RESOURCES_DIR"/rob.obj", p2, t2);
93 
94  aligned_vector<Transform3<S>> transforms; // t0
95  S extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
96 #ifdef NDEBUG
97  std::size_t n = 10;
98 #else
99  std::size_t n = 1;
100 #endif
101 
102  test::generateRandomTransforms(extents, transforms, n);
103 
104  double dis_time = 0;
105  double col_time = 0;
106 
107  test::DistanceRes<S> res, res_now;
108  for(std::size_t i = 0; i < transforms.size(); ++i)
109  {
110  test::Timer timer_col;
111  timer_col.start();
112  collide_Test_OBB(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, verbose);
113  timer_col.stop();
114  col_time += timer_col.getElapsedTimeInSec();
115 
116  test::Timer timer_dist;
117  timer_dist.start();
118  distance_Test_Oriented<RSS<S>, detail::MeshDistanceTraversalNodeRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, 2, res, verbose);
119  timer_dist.stop();
120  dis_time += timer_dist.getElapsedTimeInSec();
121 
122  distance_Test_Oriented<RSS<S>, detail::MeshDistanceTraversalNodeRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
123 
124  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
125  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
126 
127  distance_Test_Oriented<RSS<S>, detail::MeshDistanceTraversalNodeRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
128 
129  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
130  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
131 
132  distance_Test_Oriented<RSS<S>, detail::MeshDistanceTraversalNodeRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, 20, res_now, verbose);
133 
134  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
135  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
136 
137  distance_Test_Oriented<RSS<S>, detail::MeshDistanceTraversalNodeRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
138 
139  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
140  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
141 
142  distance_Test_Oriented<RSS<S>, detail::MeshDistanceTraversalNodeRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
143 
144  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
145  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
146 
147  distance_Test_Oriented<kIOS<S>, detail::MeshDistanceTraversalNodekIOS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, 2, res_now, verbose);
148 
149  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
150  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
151 
152  distance_Test_Oriented<kIOS<S>, detail::MeshDistanceTraversalNodekIOS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
153 
154  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
155  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
156 
157  distance_Test_Oriented<kIOS<S>, detail::MeshDistanceTraversalNodekIOS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
158 
159  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
160  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
161 
162  distance_Test_Oriented<kIOS<S>, detail::MeshDistanceTraversalNodekIOS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, 20, res_now, verbose);
163 
164  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
165  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
166 
167  distance_Test_Oriented<kIOS<S>, detail::MeshDistanceTraversalNodekIOS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
168 
169  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
170  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
171 
172  distance_Test_Oriented<kIOS<S>, detail::MeshDistanceTraversalNodekIOS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
173 
174  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
175  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
176 
177  distance_Test_Oriented<OBBRSS<S>, detail::MeshDistanceTraversalNodeOBBRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, 2, res_now, verbose);
178 
179  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
180  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
181 
182  distance_Test_Oriented<OBBRSS<S>, detail::MeshDistanceTraversalNodeOBBRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
183 
184  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
185  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
186 
187  distance_Test_Oriented<OBBRSS<S>, detail::MeshDistanceTraversalNodeOBBRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
188 
189  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
190  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
191 
192  distance_Test_Oriented<OBBRSS<S>, detail::MeshDistanceTraversalNodeOBBRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, 20, res_now, verbose);
193 
194  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
195  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
196 
197  distance_Test_Oriented<OBBRSS<S>, detail::MeshDistanceTraversalNodeOBBRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
198 
199  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
200  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
201 
202  distance_Test_Oriented<OBBRSS<S>, detail::MeshDistanceTraversalNodeOBBRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
203 
204  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
205  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
206 
207 
208 
209  distance_Test<RSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, 2, res_now, verbose);
210 
211  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
212  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
213 
214  distance_Test<RSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
215 
216  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
217  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
218 
219  distance_Test<RSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
220 
221  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
222  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
223 
224  distance_Test<RSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, 20, res_now, verbose);
225 
226  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
227  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
228 
229  distance_Test<RSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
230 
231  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
232  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
233 
234  distance_Test<RSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
235 
236  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
237  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
238 
239  distance_Test<kIOS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, 2, res_now, verbose);
240 
241  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
242  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
243 
244  distance_Test<kIOS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
245 
246  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
247  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
248 
249  distance_Test<kIOS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
250 
251  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
252  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
253 
254  distance_Test<kIOS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, 20, res_now, verbose);
255 
256  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
257  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
258 
259  distance_Test<kIOS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
260 
261  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
262  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
263 
264  distance_Test<kIOS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
265 
266  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
267  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
268 
269 
270  distance_Test<OBBRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, 2, res_now, verbose);
271 
272  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
273  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
274 
275  distance_Test<OBBRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
276 
277  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
278  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
279 
280  distance_Test<OBBRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
281 
282  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
283  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
284 
285  distance_Test<OBBRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEAN, 20, res_now, verbose);
286 
287  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
288  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
289 
290  distance_Test<OBBRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
291 
292  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
293  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
294 
295  distance_Test<OBBRSS<S>>(transforms[i], p1, t1, p2, t2, detail::SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
296 
297  EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA<S>());
298  EXPECT_TRUE(fabs(res.distance) < DELTA<S>() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
299 
300  }
301 
302  std::cout << "distance timing: " << dis_time << " sec" << std::endl;
303  std::cout << "collision timing: " << col_time << " sec" << std::endl;
304 }
305 
306 GTEST_TEST(FCL_DISTANCE, mesh_distance)
307 {
308 // test_mesh_distance<float>();
309  test_mesh_distance<double>();
310 }
311 
312 template <typename S>
313 void NearestPointFromDegenerateSimplex() {
314  // Tests a historical bug. In certain configurations, the distance query
315  // would terminate with a degenerate 3-simplex; the triangle was actually a
316  // line segment. As a result, nearest points were populated with NaN values.
317  // See https://github.com/flexible-collision-library/fcl/issues/293 for
318  // more discussion.
319  // This test is only relevant if box-box distance is computed via GJK. We
320  // intentionally short-circuit the mechanism for dispatching custom methods
321  // to guarantee GJK is evaluated on these two boxes.
322  DistanceResult<S> result;
323  DistanceRequest<S> request;
324 
325  // These values were extracted from a real-world scenario that produced NaNs.
326  std::shared_ptr<CollisionGeometry<S>> box_geometry_1(
327  new Box<S>(2.750000, 6.000000, 0.050000));
328  std::shared_ptr<CollisionGeometry<S>> box_geometry_2(
329  new Box<S>(0.424000, 0.150000, 0.168600));
330  CollisionObject<S> box_object_1(
331  box_geometry_1, Eigen::Quaterniond(1, 0, 0, 0).matrix(),
332  Eigen::Vector3d(1.625000, 0.000000, 0.500000));
333  CollisionObject<S> box_object_2(
334  box_geometry_2,
335  Eigen::Quaterniond(0.672811, 0.340674, 0.155066, 0.638138)
336  .normalized()
337  .matrix(),
338  Eigen::Vector3d(0.192074, -0.277870, 0.273546));
339 
340  // Direct invocation.
341  // NOTE: This code is basically lifted from ShapeDistanceLibccdImpl::run() in
342  // gjk_solver_libbd-inl.h.
343  Box<S>* box1 = static_cast<Box<S>*>(box_geometry_1.get());
344  Box<S>* box2 = static_cast<Box<S>*>(box_geometry_2.get());
345  detail::GJKSolver_libccd<S> solver;
346  void* o1 = detail::GJKInitializer<S, Box<S>>::createGJKObject(
347  *box1, box_object_1.getTransform());
348  void* o2 = detail::GJKInitializer<S, Box<S>>::createGJKObject(
349  *box2, box_object_2.getTransform());
350  detail::GJKDistance(
351  o1, detail::GJKInitializer<S, Box<S>>::getSupportFunction(), o2,
352  detail::GJKInitializer<S, Box<S>>::getSupportFunction(),
353  solver.max_distance_iterations, request.distance_tolerance,
354  &result.min_distance, &result.nearest_points[0],
355  &result.nearest_points[1]);
356 
357  detail::GJKInitializer<S, Box<S>>::deleteGJKObject(o1);
358  detail::GJKInitializer<S, Box<S>>::deleteGJKObject(o2);
359 
360  // These hard-coded values have been previously computed and visually
361  // inspected and considered to be the ground truth for this very specific
362  // test configuration.
363  S expected_dist{0.053516162824549};
364  // The "nearest" points (N1 and N2) measured and expressed in box 1's and
365  // box 2's frames (B1 and B2, respectively).
366  const Vector3<S> expected_p_B1N1{-1.375, -0.098881502700918666,
367  -0.025000000000000022};
368  const Vector3<S> expected_p_B2N2{0.21199965773384655, 0.074999692703297122,
369  0.084299993303443954};
370  // The nearest points in the world frame.
371  const Vector3<S> expected_p_WN1 =
372  box_object_1.getTransform() * expected_p_B1N1;
373  const Vector3<S> expected_p_WN2 =
374  box_object_2.getTransform() * expected_p_B2N2;
375  EXPECT_TRUE(CompareMatrices(result.nearest_points[0], expected_p_WN1,
376  DELTA<S>(), MatrixCompareType::absolute));
377  EXPECT_TRUE(CompareMatrices(result.nearest_points[1], expected_p_WN2,
378  DELTA<S>(), MatrixCompareType::absolute));
379  EXPECT_NEAR(expected_dist, result.min_distance,
380  constants<ccd_real_t>::eps_78());
381 }
382 
383 GTEST_TEST(FCL_DISTANCE, NearestPointFromDegenerateSimplex) {
384  NearestPointFromDegenerateSimplex<double>();
385 }
386 
387 template<typename BV, typename TraversalNode>
388 void distance_Test_Oriented(const Transform3<typename BV::S>& tf,
389  const std::vector<Vector3<typename BV::S>>& vertices1, const std::vector<Triangle>& triangles1,
390  const std::vector<Vector3<typename BV::S>>& vertices2, const std::vector<Triangle>& triangles2, detail::SplitMethodType split_method,
391  int qsize,
392  test::DistanceRes<typename BV::S>& distance_result,
393  bool verbose)
394 {
395  using S = typename BV::S;
396 
397  BVHModel<BV> m1;
398  BVHModel<BV> m2;
399  m1.bv_splitter.reset(new detail::BVSplitter<BV>(split_method));
400  m2.bv_splitter.reset(new detail::BVSplitter<BV>(split_method));
401 
402 
403  m1.beginModel();
404  m1.addSubModel(vertices1, triangles1);
405  m1.endModel();
406 
407  m2.beginModel();
408  m2.addSubModel(vertices2, triangles2);
409  m2.endModel();
410 
411  DistanceResult<S> local_result;
412  TraversalNode node;
413  if(!initialize(node, (const BVHModel<BV>&)m1, tf, (const BVHModel<BV>&)m2, Transform3<S>::Identity(), DistanceRequest<S>(true), local_result))
414  std::cout << "initialize error" << std::endl;
415 
416  node.enable_statistics = verbose;
417 
418  distance(&node, nullptr, qsize);
419 
420  // points are in local coordinate, to global coordinate
421  Vector3<S> p1 = local_result.nearest_points[0];
422  Vector3<S> p2 = local_result.nearest_points[1];
423 
424 
425  distance_result.distance = local_result.min_distance;
426  distance_result.p1 = p1;
427  distance_result.p2 = p2;
428 
429  if(verbose)
430  {
431  std::cout << "distance " << local_result.min_distance << std::endl;
432 
433  std::cout << p1[0] << " " << p1[1] << " " << p1[2] << std::endl;
434  std::cout << p2[0] << " " << p2[1] << " " << p2[2] << std::endl;
435  std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
436  }
437 }
438 
439 template<typename BV>
440 void distance_Test(const Transform3<typename BV::S>& tf,
441  const std::vector<Vector3<typename BV::S>>& vertices1, const std::vector<Triangle>& triangles1,
442  const std::vector<Vector3<typename BV::S>>& vertices2, const std::vector<Triangle>& triangles2, detail::SplitMethodType split_method,
443  int qsize,
444  test::DistanceRes<typename BV::S>& distance_result,
445  bool verbose)
446 {
447  using S = typename BV::S;
448 
449  BVHModel<BV> m1;
450  BVHModel<BV> m2;
451  m1.bv_splitter.reset(new detail::BVSplitter<BV>(split_method));
452  m2.bv_splitter.reset(new detail::BVSplitter<BV>(split_method));
453 
454 
455  m1.beginModel();
456  m1.addSubModel(vertices1, triangles1);
457  m1.endModel();
458 
459  m2.beginModel();
460  m2.addSubModel(vertices2, triangles2);
461  m2.endModel();
462 
463  Transform3<S> pose1(tf);
464  Transform3<S> pose2 = Transform3<S>::Identity();
465 
466  DistanceResult<S> local_result;
467  detail::MeshDistanceTraversalNode<BV> node;
468 
469  if(!detail::initialize<BV>(node, m1, pose1, m2, pose2, DistanceRequest<S>(true), local_result))
470  std::cout << "initialize error" << std::endl;
471 
472  node.enable_statistics = verbose;
473 
474  distance(&node, nullptr, qsize);
475 
476  distance_result.distance = local_result.min_distance;
477  distance_result.p1 = local_result.nearest_points[0];
478  distance_result.p2 = local_result.nearest_points[1];
479 
480  if(verbose)
481  {
482  std::cout << "distance " << local_result.min_distance << std::endl;
483 
484  std::cout << local_result.nearest_points[0][0] << " " << local_result.nearest_points[0][1] << " " << local_result.nearest_points[0][2] << std::endl;
485  std::cout << local_result.nearest_points[1][0] << " " << local_result.nearest_points[1][1] << " " << local_result.nearest_points[1][2] << std::endl;
486  std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
487  }
488 }
489 
490 template <typename S>
491 bool collide_Test_OBB(const Transform3<S>& tf,
492  const std::vector<Vector3<S>>& vertices1, const std::vector<Triangle>& triangles1,
493  const std::vector<Vector3<S>>& vertices2, const std::vector<Triangle>& triangles2, detail::SplitMethodType split_method, bool verbose)
494 {
495  BVHModel<OBB<S>> m1;
496  BVHModel<OBB<S>> m2;
497  m1.bv_splitter.reset(new detail::BVSplitter<OBB<S>>(split_method));
498  m2.bv_splitter.reset(new detail::BVSplitter<OBB<S>>(split_method));
499 
500  m1.beginModel();
501  m1.addSubModel(vertices1, triangles1);
502  m1.endModel();
503 
504  m2.beginModel();
505  m2.addSubModel(vertices2, triangles2);
506  m2.endModel();
507 
508  CollisionResult<S> local_result;
509  detail::MeshCollisionTraversalNodeOBB<S> node;
510  if(!detail::initialize(node, (const BVHModel<OBB<S>>&)m1, tf, (const BVHModel<OBB<S>>&)m2, Transform3<S>::Identity(),
511  CollisionRequest<S>(), local_result))
512  std::cout << "initialize error" << std::endl;
513 
514  node.enable_statistics = verbose;
515 
516  collide(&node);
517 
518  if(local_result.numContacts() > 0)
519  return true;
520  else
521  return false;
522 }
523 
524 //==============================================================================
525 int main(int argc, char* argv[])
526 {
527  ::testing::InitGoogleTest(&argc, argv);
528  return RUN_ALL_TESTS();
529 }
fcl::BVHModel::bv_splitter
std::shared_ptr< detail::BVSplitterBase< BV > > bv_splitter
Split rule to split one BV node into two children.
Definition: BVH_model.h:180
fcl::DistanceRequest
request to the distance computation
Definition: distance_request.h:52
fcl::detail::SPLIT_METHOD_MEDIAN
@ SPLIT_METHOD_MEDIAN
Definition: BV_splitter.h:59
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:92
fcl::test::Timer
Definition: test_fcl_utility.h:77
nearlyEqual
bool nearlyEqual(const Vector3< S > &a, const Vector3< S > &b)
Definition: test_fcl_distance.cpp:77
fcl::test::loadOBJFile
void loadOBJFile(const char *filename, std::vector< Vector3< S >> &points, std::vector< Triangle > &triangles)
Load an obj mesh file.
Definition: test_fcl_utility.h:194
DELTA
S DELTA()
Definition: test_fcl_distance.cpp:53
fcl::detail::GJKSolver_libccd
collision and distance solver based on libccd library.
Definition: gjk_solver_libccd.h:54
eigen_matrix_compare.h
fcl::detail::BVHDistanceTraversalNode::num_leaf_tests
int num_leaf_tests
Definition: bvh_distance_traversal_node.h:93
fcl::distance
S distance(const Eigen::MatrixBase< DerivedA > &R0, const Eigen::MatrixBase< DerivedB > &T0, const kIOS< S > &b1, const kIOS< S > &b2, Vector3< S > *P, Vector3< S > *Q)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS-inl.h:266
fcl::detail::BVHDistanceTraversalNode::num_bv_tests
int num_bv_tests
statistical information
Definition: bvh_distance_traversal_node.h:92
fcl::CompareMatrices
::testing::AssertionResult CompareMatrices(const Eigen::MatrixBase< DerivedA > &m1, const Eigen::MatrixBase< DerivedB > &m2, double tolerance=0.0, MatrixCompareType compare_type=MatrixCompareType::absolute)
Definition: eigen_matrix_compare.h:63
fcl::test::Timer::start
void start()
start timer
Definition: test_fcl_utility.cpp:75
fcl::test::DistanceRes::p2
Vector3< S > p2
Definition: test_fcl_utility.h:164
fcl::DistanceResult::nearest_points
Vector3< S > nearest_points[2]
Nearest points in the world coordinates.
Definition: distance_result.h:69
fcl::detail::SplitMethodType
SplitMethodType
Three types of split algorithms are provided in FCL as default.
Definition: BV_splitter.h:56
fcl::BVHModel::endModel
int endModel()
End BVH model construction, will build the bounding volume hierarchy.
Definition: BVH_model-inl.h:450
distance_Test
void distance_Test(const Transform3< typename BV::S > &tf, const std::vector< Vector3< typename BV::S >> &vertices1, const std::vector< Triangle > &triangles1, const std::vector< Vector3< typename BV::S >> &vertices2, const std::vector< Triangle > &triangles2, detail::SplitMethodType split_method, int qsize, test::DistanceRes< typename BV::S > &distance_result, bool verbose=true)
Definition: test_fcl_distance.cpp:440
fcl::detail::GJKInitializer
initialize GJK stuffs
Definition: gjk_libccd.h:76
test_fcl_utility.h
fcl::CollisionResult
collision result
Definition: collision_request.h:48
EXPECT_TRUE
#define EXPECT_TRUE(args)
fcl::test::Timer::getElapsedTimeInSec
double getElapsedTimeInSec()
get elapsed time in second (same as getElapsedTime)
Definition: test_fcl_utility.cpp:123
fcl::detail::CollisionTraversalNodeBase< OBB< S > ::S >::enable_statistics
bool enable_statistics
Whether stores statistics.
Definition: collision_traversal_node_base.h:78
fcl::DistanceResult
distance result
Definition: distance_request.h:48
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:71
fcl::detail::MeshDistanceTraversalNodekIOS
Definition: mesh_distance_traversal_node.h:137
test_mesh_distance
void test_mesh_distance()
Definition: test_fcl_distance.cpp:86
NearestPointFromDegenerateSimplex
void NearestPointFromDegenerateSimplex()
Definition: test_fcl_distance.cpp:313
fcl::OBB< S >
main
int main(int argc, char *argv[])
Definition: test_fcl_distance.cpp:525
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
fcl::test::DistanceRes
Structure for minimum distance between two meshes and the corresponding nearest point pair.
Definition: test_fcl_utility.h:160
fcl::detail::MeshDistanceTraversalNode
Traversal node for distance computation between two meshes.
Definition: mesh_distance_traversal_node.h:55
fcl::Quaterniond
Quaternion< double > Quaterniond
Definition: types.h:117
fcl::MatrixCompareType::absolute
@ absolute
fcl::CollisionRequest
Parameters for performing collision request.
Definition: collision_request.h:52
fcl::CollisionResult::numContacts
size_t numContacts() const
number of contacts found
Definition: collision_result-inl.h:83
fcl::detail::initialize
template bool initialize(MeshCollisionTraversalNodeOBB< double > &node, const BVHModel< OBB< double >> &model1, const Transform3< double > &tf1, const BVHModel< OBB< double >> &model2, const Transform3< double > &tf2, const CollisionRequest< double > &request, CollisionResult< double > &result)
fcl::collide
template FCL_EXPORT std::size_t collide(const CollisionObject< double > *o1, const CollisionObject< double > *o2, const CollisionRequest< double > &request, CollisionResult< double > &result)
fcl::DistanceResult::min_distance
S min_distance
Minimum distance between two objects.
Definition: distance_result.h:64
fcl::detail::SPLIT_METHOD_MEAN
@ SPLIT_METHOD_MEAN
Definition: BV_splitter.h:58
fcl::detail::SPLIT_METHOD_BV_CENTER
@ SPLIT_METHOD_BV_CENTER
Definition: BV_splitter.h:60
fcl::BVHModel::beginModel
int beginModel(int num_tris=0, int num_vertices=0)
Begin a new BVH model.
Definition: BVH_model-inl.h:207
Vector3d
fcl::Vector3d Vector3d
Definition: test_broadphase_dynamic_AABB_tree.cpp:48
fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:57
fcl::test::generateRandomTransforms
void generateRandomTransforms(S extents[6], aligned_vector< Transform3< S >> &transforms, std::size_t n)
Generate n random transforms whose translations are constrained by extents.
Definition: test_fcl_utility.h:349
fcl::CollisionObject::getTransform
const Transform3< S > & getTransform() const
get object's transform
Definition: collision_object-inl.h:170
GTEST_TEST
GTEST_TEST(FCL_DISTANCE, mesh_distance)
Definition: test_fcl_distance.cpp:306
fcl::test::Timer::stop
void stop()
stop the timer
Definition: test_fcl_utility.cpp:86
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:51
fcl::detail::MeshDistanceTraversalNodeRSS
Traversal node for distance computation between two meshes if their underlying BVH node is oriented n...
Definition: mesh_distance_traversal_node.h:96
fcl::DistanceRequest::distance_tolerance
S distance_tolerance
the threshold used in GJK algorithm to stop distance iteration
Definition: distance_request.h:99
fcl::detail::DistanceTraversalNodeBase< BV::S >::enable_statistics
bool enable_statistics
Whether stores statistics.
Definition: distance_traversal_node_base.h:79
fcl::detail::BVSplitter
A class describing the split rule that splits each BV node.
Definition: BV_splitter.h:65
fcl::constants
Definition: constants.h:129
fcl::detail::GJKDistance
template bool GJKDistance(void *obj1, ccd_support_fn supp1, void *obj2, ccd_support_fn supp2, unsigned int max_iterations, double tolerance, double *dist, Vector3d *p1, Vector3d *p2)
collide_Test_OBB
bool collide_Test_OBB(const Transform3< S > &tf, const std::vector< Vector3< S >> &vertices1, const std::vector< Triangle > &triangles1, const std::vector< Vector3< S >> &vertices2, const std::vector< Triangle > &triangles2, detail::SplitMethodType split_method, bool verbose)
Definition: test_fcl_distance.cpp:491
fcl::BVHModel::addSubModel
int addSubModel(const std::vector< Vector3< S >> &ps, const std::vector< Triangle > &ts)
Add a set of triangles in the new BVH model.
Definition: BVH_model-inl.h:383
collision_node.h
fcl::detail::MeshDistanceTraversalNodeOBBRSS
Definition: mesh_distance_traversal_node.h:178
fcl::test::DistanceRes::p1
Vector3< S > p1
Definition: test_fcl_utility.h:163
fcl::test::DistanceRes::distance
S distance
Definition: test_fcl_utility.h:162
extents
std::array< S, 6 > & extents()
Definition: test_fcl_geometric_shapes.cpp:63
fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:51
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::detail::MeshCollisionTraversalNodeOBB
Traversal node for collision between two meshes if their underlying BVH node is oriented node (OBB,...
Definition: mesh_collision_traversal_node.h:99
fcl::aligned_vector
std::vector< _Tp, Eigen::aligned_allocator< _Tp > > aligned_vector
Definition: types.h:123
distance_Test_Oriented
void distance_Test_Oriented(const Transform3< typename BV::S > &tf, const std::vector< Vector3< typename BV::S >> &vertices1, const std::vector< Triangle > &triangles1, const std::vector< Vector3< typename BV::S >> &vertices2, const std::vector< Triangle > &triangles2, detail::SplitMethodType split_method, int qsize, test::DistanceRes< typename BV::S > &distance_result, bool verbose=true)
Definition: test_fcl_distance.cpp:388
verbose
bool verbose
Definition: test_fcl_distance.cpp:50

fcl
Author(s):
autogenerated on Fri Mar 14 2025 02:38:18