test_fcl_signed_distance.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2016, Open Source Robotics Foundation
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Open Source Robotics Foundation nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 #include <gtest/gtest.h>
36 
37 #include "eigen_matrix_compare.h"
38 #include "fcl/narrowphase/distance.h"
39 #include "fcl/narrowphase/detail/traversal/collision_node.h"
40 #include "fcl/narrowphase/detail/gjk_solver_libccd.h"
41 #include "test_fcl_utility.h"
42 #include "fcl_resources/config.h"
43 
44 using namespace fcl;
45 using std::abs;
46 
47 bool verbose = false;
48 
49 //==============================================================================
50 template <typename S>
51 void test_distance_spheresphere(GJKSolverType solver_type)
52 {
53  const S radius_1 = 20;
54  const S radius_2 = 10;
55  Sphere<S> s1{radius_1};
56  Sphere<S> s2{radius_2};
57 
58  Transform3<S> tf1{Transform3<S>::Identity()};
59  Transform3<S> tf2{Transform3<S>::Identity()};
60 
61  DistanceRequest<S> request;
62  request.enable_signed_distance = true;
63  request.enable_nearest_points = true;
64  request.gjk_solver_type = solver_type;
65 
66  DistanceResult<S> result;
67 
68  // Expecting distance to be 10
69  result.clear();
70  tf2.translation() = Vector3<S>(40, 0, 0);
71  distance(&s1, tf1, &s2, tf2, request, result);
72  EXPECT_NEAR(result.min_distance, 10, 1e-6);
73  EXPECT_TRUE(CompareMatrices(result.nearest_points[0], Vector3<S>(20, 0, 0),
74  request.distance_tolerance));
75  EXPECT_TRUE(CompareMatrices(result.nearest_points[1], Vector3<S>(30, 0, 0),
76  request.distance_tolerance));
77 
78  // request.distance_tolerance is actually the square of the distance
79  // tolerance, namely the difference between distance returned from FCL's EPA
80  // implementation and the actual distance, is less than
81  // sqrt(request.distance_tolerance).
82  const S distance_tolerance = std::sqrt(request.distance_tolerance);
83 
84  // Expecting distance to be -5
85  result.clear();
86  tf2.translation() = Vector3<S>(25, 0, 0);
87  distance(&s1, tf1, &s2, tf2, request, result);
88  EXPECT_NEAR(result.min_distance, -5, request.distance_tolerance);
89 
90  // TODO(JS): Only GST_LIBCCD can compute the pair of nearest points on the
91  // surface of the penetrating spheres.
92  if (solver_type == GST_LIBCCD)
93  {
94  EXPECT_TRUE(CompareMatrices(result.nearest_points[0], Vector3<S>(20, 0, 0),
95  distance_tolerance));
96  EXPECT_TRUE(CompareMatrices(result.nearest_points[1], Vector3<S>(15, 0, 0),
97  distance_tolerance));
98  }
99 
100  result.clear();
101  tf2.translation() = Vector3<S>(20, 0, 20);
102  distance(&s1, tf1, &s2, tf2, request, result);
103 
104  S expected_dist =
105  (tf1.translation() - tf2.translation()).norm() - radius_1 - radius_2;
106  EXPECT_NEAR(result.min_distance, expected_dist, distance_tolerance);
107  // TODO(JS): Only GST_LIBCCD can compute the pair of nearest points on the
108  // surface of the spheres.
109  if (solver_type == GST_LIBCCD)
110  {
111  Vector3<S> dir = (tf2.translation() - tf1.translation()).normalized();
112  Vector3<S> p0_expected = dir * radius_1;
113  EXPECT_TRUE(CompareMatrices(result.nearest_points[0], p0_expected,
114  distance_tolerance));
115  Vector3<S> p1_expected = tf2.translation() - dir * radius_2;
116  EXPECT_TRUE(CompareMatrices(result.nearest_points[1], p1_expected,
117  distance_tolerance));
118  }
119 }
120 
121 template <typename S>
122 void test_distance_spherecapsule(GJKSolverType solver_type)
123 {
124  Sphere<S> s1{20};
125  Capsule<S> s2{10, 20};
126 
127  Transform3<S> tf1{Transform3<S>::Identity()};
128  Transform3<S> tf2{Transform3<S>::Identity()};
129 
130  DistanceRequest<S> request;
131  request.enable_signed_distance = true;
132  request.enable_nearest_points = true;
133  request.gjk_solver_type = solver_type;
134 
135  DistanceResult<S> result;
136 
137  // Expecting distance to be 10
138  result.clear();
139  tf2.translation() = Vector3<S>(40, 0, 0);
140  distance(&s1, tf1, &s2, tf2, request, result);
141  EXPECT_NEAR(result.min_distance, 10, request.distance_tolerance);
142  EXPECT_TRUE(CompareMatrices(result.nearest_points[0], Vector3<S>(20, 0, 0),
143  request.distance_tolerance));
144  EXPECT_TRUE(CompareMatrices(result.nearest_points[1], Vector3<S>(30, 0, 0),
145  request.distance_tolerance));
146 
147  // Expecting distance to be -5
148  result.clear();
149  tf2.translation() = Vector3<S>(25, 0, 0);
150  distance(&s1, tf1, &s2, tf2, request, result);
151 
152  // request.distance_tolerance is actually the square of the distance
153  // tolerance, namely the difference between distance returned from FCL's EPA
154  // implementation and the actual distance, is less than
155  // sqrt(request.distance_tolerance).
156  const S distance_tolerance = std::sqrt(request.distance_tolerance);
157  ASSERT_NEAR(result.min_distance, -5, distance_tolerance);
158  if (solver_type == GST_LIBCCD)
159  {
160  // NOTE: Currently, only GST_LIBCCD computes the pair of nearest points.
161  EXPECT_TRUE(CompareMatrices(result.nearest_points[0], Vector3<S>(20, 0, 0),
162  distance_tolerance * 100));
163  EXPECT_TRUE(CompareMatrices(result.nearest_points[1], Vector3<S>(15, 0, 0),
164  distance_tolerance * 100));
165  }
166 }
167 
168 
169 template <typename S>
170 void test_distance_cylinder_sphere1() {
171  // This is a specific case that has cropped up in the wild that reaches the
172  // unexpected `expandPolytope()` error, where the nearest point and the new
173  // vertex both lie on an edge. It turns out that libccd incorrectly thinks
174  // that the edge is the nearest feature, while actually one of the
175  // neighbouring faces is. This test confirms that the bug is fixed, by the
176  // function validateNearestFeatureOfPolytopeBeingEdge().
177  // This case was reported in
178  // https://github.com/flexible-collision-library/fcl/issues/391.
179  using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometry<S>>;
180  const S cylinder_radius = 0.03;
181  const S cylinder_length = 0.65;
182  CollisionGeometryPtr_t cylinder_geo(
183  new fcl::Cylinder<S>(cylinder_radius, cylinder_length));
184  Transform3<S> X_WC = Transform3<S>::Identity();
185  X_WC.translation() << 0.6, 0, 0.325;
186  fcl::CollisionObject<S> cylinder(cylinder_geo, X_WC);
187 
188  const S sphere_radius = 0.055;
189  CollisionGeometryPtr_t sphere_geo(new fcl::Sphere<S>(sphere_radius));
190  Transform3<S> X_WS = Transform3<S>::Identity();
191  // clang-format off
192  X_WS.matrix() << -0.9954758066, -0.0295866301, 0.0902914702, 0.5419794018,
193  -0.0851034786, -0.1449450565, -0.9857729599, -0.0621175025,
194  0.0422530022, -0.9889972506, 0.1417713719, 0.6016236576,
195  0, 0, 0, 1;
196  // clang-format on
197  fcl::CollisionObject<S> sphere(sphere_geo, X_WS);
198 
199  fcl::DistanceRequest<S> request;
200  request.gjk_solver_type = GJKSolverType::GST_LIBCCD;
201  request.distance_tolerance = 1e-6;
202  request.enable_signed_distance = true;
203  fcl::DistanceResult<S> result;
204 
205  ASSERT_NO_THROW(fcl::distance(&cylinder, &sphere, request, result));
206  // The two objects are penetrating.
207  EXPECT_NEAR(-(result.nearest_points[0] - result.nearest_points[1]).norm(),
208  result.min_distance, request.distance_tolerance);
209  // p_CPc is the position of the witness point Pc on the cylinder, measured
210  // and expressed in the cylinder frame C.
211  const Vector3<S> p_CPc = X_WC.inverse() * result.nearest_points[0];
212  EXPECT_LE(abs(p_CPc(2)), cylinder_length / 2);
213  EXPECT_LE(p_CPc.template head<2>().norm(), cylinder_radius);
214  // p_SPs is the position of the witness point Ps on the sphere, measured and
215  // expressed in the sphere frame S.
216  const Vector3<S> p_SPs = X_WS.inverse() * result.nearest_points[1];
217  EXPECT_LE(p_SPs.norm(), sphere_radius);
218 }
219 
220 template <typename S>
221 void test_distance_cylinder_box_helper(S cylinder_radius, S cylinder_length,
222  const Transform3<S>& X_WC,
223  const Vector3<S>& box_size,
224  const Transform3<S>& X_WB) {
225  using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometryd>;
226  CollisionGeometryPtr_t cylinder_geo(
227  new fcl::Cylinder<S>(cylinder_radius, cylinder_length));
228  fcl::CollisionObject<S> cylinder(cylinder_geo, X_WC);
229 
230  CollisionGeometryPtr_t box_geo(
231  new fcl::Box<S>(box_size(0), box_size(1), box_size(2)));
232  fcl::CollisionObject<S> box(box_geo, X_WB);
233 
234  fcl::DistanceRequest<S> request;
235  request.gjk_solver_type = GJKSolverType::GST_LIBCCD;
236  request.distance_tolerance = 1e-6;
237  request.enable_signed_distance = true;
238  fcl::DistanceResult<S> result;
239 
240  ASSERT_NO_THROW(fcl::distance(&cylinder, &box, request, result));
241  EXPECT_NEAR(abs(result.min_distance),
242  (result.nearest_points[0] - result.nearest_points[1]).norm(),
243  request.distance_tolerance);
244  // p_CPc is the position of the witness point Pc on the cylinder, measured
245  // and expressed in the cylinder frame C.
246  const Vector3<S> p_CPc = X_WC.inverse() * result.nearest_points[0];
247  const S tol = 10 * std::numeric_limits<S>::epsilon();
248  EXPECT_LE(abs(p_CPc(2)), cylinder_length / 2 + tol);
249  EXPECT_LE(p_CPc.template head<2>().norm(), cylinder_radius);
250  // p_BPb is the position of the witness point Pb on the box, measured and
251  // expressed in the box frame B.
252  const Vector3<S> p_BPb = X_WB.inverse() * result.nearest_points[1];
253  EXPECT_TRUE((p_BPb.array().abs() <= box_size.array() / 2 + tol).all());
254 }
255 
256 template <typename S>
257 void test_distance_cylinder_box() {
258  // This is a *specific* case that has cropped up in the wild that reaches the
259  // unexpected `expandPolytope()` error. This error was reported in
260  // https://github.com/flexible-collision-library/fcl/issues/319
261  // This test would fail in Debug mode without the function
262  // validateNearestFeatureOfPolytopeBeingEdge in PR #388.
263  S cylinder_radius = 0.05;
264  S cylinder_length = 0.06;
265 
266  Transform3<S> X_WC = Transform3<S>::Identity();
267  X_WC.matrix() << -0.99999999997999022838, 6.2572835802045040178e-10,
268  6.3260669852976095481e-06, 0.57500009756757608503,
269  6.3260669851683709551e-06, -6.3943303429958554955e-10,
270  0.99999999997999056145, -0.42711963046787942977,
271  6.2573180158128459924e-10, 1, 6.3942912945996747041e-10,
272  1.1867093358746836351, 0, 0, 0, 1;
273  Vector3<S> box_size(0.025, 0.35, 1.845);
274  Transform3<S> X_WB = Transform3<S>::Identity();
275  // clang-format off
276  X_WB.matrix() << 0, -1, 0, 0.8,
277  1, 0, 0, -0.4575,
278  0, 0, 1, 1.0225,
279  0, 0, 0, 1;
280  // clang-format on
281  test_distance_cylinder_box_helper(cylinder_radius, cylinder_length, X_WC,
282  box_size, X_WB);
283  // This is a specific case reported in
284  // https://github.com/flexible-collision-library/fcl/issues/390#issuecomment-481634606
285  X_WC.matrix() << -0.97313010759279283679, -0.12202804064972551379,
286  0.19526123781136842106, 0.87472781461138560122, 0.20950801135757171623,
287  -0.11745920593569325607, 0.97072639199619581429, -0.4038687881347159947,
288  -0.095520609678929752073, 0.98555187191549953329, 0.13986894183635001365,
289  1.5871328698116491385, 0, 0, 0, 1;
290  // clang-format off
291  X_WB.matrix() << 0, -1, 0, 0.8,
292  1, 0, 0, -0.4575,
293  0, 0, 1, 1.0225,
294  0, 0, 0, 1;
295  // clang-format on
296  test_distance_cylinder_box_helper(cylinder_radius, cylinder_length, X_WC,
297  box_size, X_WB);
298 }
299 
300 template <typename S>
301 void test_distance_box_box_helper(const Vector3<S>& box1_size,
302  const Transform3<S>& X_WB1,
303  const Vector3<S>& box2_size,
304  const Transform3<S>& X_WB2,
305  const S* expected_distance = nullptr) {
306  using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometryd>;
307  CollisionGeometryPtr_t box1_geo(
308  new fcl::Box<S>(box1_size(0), box1_size(1), box1_size(2)));
309  fcl::CollisionObject<S> box1(box1_geo, X_WB1);
310 
311  CollisionGeometryPtr_t box2_geo(
312  new fcl::Box<S>(box2_size(0), box2_size(1), box2_size(2)));
313  fcl::CollisionObject<S> box2(box2_geo, X_WB2);
314 
315  fcl::DistanceRequest<S> request;
316  request.gjk_solver_type = GJKSolverType::GST_LIBCCD;
317  request.distance_tolerance = 1e-6;
318  request.enable_signed_distance = true;
319  fcl::DistanceResult<S> result;
320 
321  ASSERT_NO_THROW(fcl::distance(&box1, &box2, request, result));
322  EXPECT_NEAR(abs(result.min_distance),
323  (result.nearest_points[0] - result.nearest_points[1]).norm(),
324  request.distance_tolerance);
325  // p_B1P1 is the position of the witness point P1 on box 1, measured
326  // and expressed in the box 1 frame B1.
327  const Vector3<S> p_B1P1 = X_WB1.inverse() * result.nearest_points[0];
328  constexpr double tol = 10 * constants<S>::eps();
329  const double tol_1 = tol * std::max(S(1), (box1_size / 2).maxCoeff());
330  EXPECT_TRUE(
331  (p_B1P1.array().abs() <= (box1_size / 2).array() + tol_1).all())
332  << "\n p_B1P1: " << p_B1P1.transpose()
333  << "\n box1_size / 2: " << (box1_size / 2).transpose()
334  << "\n tol: " << tol_1;
335  // p_B2P2 is the position of the witness point P2 on box 2, measured
336  // and expressed in the box 2 frame B2.
337  const double tol_2 = tol * std::max(S(1), (box2_size / 2).maxCoeff());
338  const Vector3<S> p_B2P2 = X_WB2.inverse() * result.nearest_points[1];
339  EXPECT_TRUE(
340  (p_B2P2.array().abs() <= (box2_size / 2).array() + tol_2).all())
341  << "\n p_B2P2: " << p_B2P2.transpose()
342  << "\n box2_size / 2: " << (box2_size / 2).transpose()
343  << "\n tol: " << tol_2;
344 
345  // An expected distance has been provided; let's test that the value is as
346  // expected.
347  if (expected_distance) {
348  EXPECT_NEAR(result.min_distance, *expected_distance,
349  constants<S>::eps_12());
350  }
351 }
352 
353 // This is a *specific* case that has cropped up in the wild that reaches the
354 // unexpected `validateNearestFeatureOfPolytopeBeingEdge` error. This error was
355 // reported in https://github.com/flexible-collision-library/fcl/issues/388
356 template <typename S>
357 void test_distance_box_box_regression1() {
358  SCOPED_TRACE("test_distance_box_box_regression1");
359  const Vector3<S> box1_size(0.03, 0.12, 0.1);
360  Transform3<S> X_WB1 = Transform3<S>::Identity();
361  X_WB1.matrix() << -3.0627937852578681533e-08, -0.99999999999999888978,
362  -2.8893865161583314238e-08, 0.63499979627350811029, 0.9999999999999980016,
363  -3.0627939739957803544e-08, 6.4729926918527511769e-08,
364  -0.48500002215636439651, -6.4729927722963847085e-08,
365  -2.8893863029448751323e-08, 0.99999999999999711342, 1.0778146458339641356,
366  0, 0, 0, 1;
367 
368  const Vector3<S> box2_size(0.025, 0.35, 1.845);
369  Transform3<S> X_WB2 = Transform3<S>::Identity();
370  // clang-format off
371  X_WB2.matrix() << 0, -1, 0, 0.8,
372  1, 0, 0, -0.4575,
373  0, 0, 1, 1.0225,
374  0, 0, 0, 1;
375  // clang-format on
376  test_distance_box_box_helper(box1_size, X_WB1, box2_size, X_WB2);
377 }
378 
379 // This is a *specific* case that has cropped up in the wild that reaches the
380 // unexpected `triangle_size_is_zero` error. This error was
381 // reported in https://github.com/flexible-collision-library/fcl/issues/395
382 template <typename S>
383 void test_distance_box_box_regression2() {
384  SCOPED_TRACE("test_distance_box_box_regression2");
385  const Vector3<S> box1_size(0.46, 0.48, 0.01);
386  Transform3<S> X_WB1 = Transform3<S>::Identity();
387  X_WB1.matrix() << 1,0,0, -0.72099999999999997424,
388  0,1,0, -0.77200000000000001954,
389  0,0,1, 0.81000000000000005329,
390  0,0,0,1;
391 
392  const Vector3<S> box2_size(0.049521, 0.146, 0.0725);
393  Transform3<S> X_WB2 = Transform3<S>::Identity();
394  // clang-format off
395  X_WB2.matrix() << 0.10758262492983036718, -0.6624881850015212903, -0.74130653817877356637, -0.42677133002999478872,
396  0.22682184885125472595, -0.709614040775253474, 0.6670830248314786326, -0.76596851247746788882,
397 -0.96797615037608542021, -0.23991106241273435495, 0.07392465377049164954, 0.80746731400091054098,
398  0, 0, 0, 1;
399  // clang-format on
400  test_distance_box_box_helper(box1_size, X_WB1, box2_size, X_WB2);
401 }
402 
403 // This is a *specific* case that has cropped up in the wild that reaches the
404 // unexpected `query point colinear with the edge` error. This error was
405 // reported in https://github.com/flexible-collision-library/fcl/issues/415
406 template <typename S>
407 void test_distance_box_box_regression3() {
408  SCOPED_TRACE("test_distance_box_box_regression3");
409  const Vector3<S> box1_size(0.49, 0.05, 0.21);
410  Transform3<S> X_WB1 = Transform3<S>::Identity();
411  // clang-format off
412  X_WB1.matrix() << 4.8966386501092529215e-12, -1,0,-0.43999999999999994671,
413  1, 4.8966386501092529215e-12,0,-0.61499999999858001587,
414  0,0,1,0.35499999999999998224,
415  0,0,0,1;
416  // clang-format on
417  const Vector3<S> box2_size(0.035, 0.12, 0.03);
418  Transform3<S> X_WB2 = Transform3<S>::Identity();
419  // clang-format off
420  X_WB2.matrix() << 0.83512153565236335595, -0.55006546945762568868, -9.4542360608233572896e-16, -0.40653441507331000704,
421  0.55006546945762568868, 0.83512153565236313391, 1.1787444236552387666e-15, -0.69166166923735727945,
422 1.2902271444330665572e-16, -1.4878153530113264589e-15, 1, 0.43057093858718892276,
423  0, 0, 0, 1;
424  // clang-format on
425  test_distance_box_box_helper(box1_size, X_WB1, box2_size, X_WB2);
426 }
427 
428 // This is a *specific* case that has cropped up in the wild. This error was
429 // reported in https://github.com/flexible-collision-library/fcl/issues/398
430 template <typename S>
431 void test_distance_box_box_regression4() {
432  SCOPED_TRACE("test_distance_box_box_regression4");
433  const Vector3<S> box1_size(0.614, 3, 0.37);
434  Transform3<S> X_WB1 = Transform3<S>::Identity();
435  X_WB1.translation() << -0.675, 0, 0.9115;
436  const Vector3<S> box2_size(0.494, 0.552, 0.01);
437  Transform3<S> X_WB2 = Transform3<S>::Identity();
438  X_WB2.translation() << -0.692, 0, 0.935;
439  test_distance_box_box_helper(box1_size, X_WB1, box2_size, X_WB2);
440 }
441 
442 // This is a *specific* case that has cropped up in the wild. This error was
443 // reported in https://github.com/flexible-collision-library/fcl/issues/428
444 template <typename S>
445 void test_distance_box_box_regression5() {
446  SCOPED_TRACE("test_distance_box_box_regression5");
447  const Vector3<S> box1_size(0.2, 0.33, 0.1);
448  Transform3<S> X_WB1 = Transform3<S>::Identity();
449  X_WB1.translation() << -0.071000000000000035305, -0.77200000000000001954, 0.79999999999999993339;
450  const Vector3<S> box2_size(0.452, 0.27, 0.6);
451  Transform3<S> X_WB2 = Transform3<S>::Identity();
452  X_WB2.translation() << 0.12099999999999999645, -0.78769605692727695523, 0.53422044196125151316;
453  test_distance_box_box_helper(box1_size, X_WB1, box2_size, X_WB2);
454 }
455 
456 template <typename S>
457 void test_distance_box_box_regression6() {
458  SCOPED_TRACE("test_distance_box_box_regression6");
459  const Vector3<S> box1_size(0.31650000000000000355, 0.22759999999999999676, 0.1768000000000000127);
460  Transform3<S> X_WB1 = Transform3<S>::Identity();
461  // clang-format off
462  X_WB1.matrix() << 0.44540578475530234748, 0.89532881496493399442, -8.8937407685638678971e-09, 1.2652949075960071568,
463  -0.89532881496493377238, 0.44540578475530190339, -2.8948680226084145336e-08, 1.4551012423210101243,
464  -2.1957263975186326105e-08, 2.0856732016652919226e-08, 0.99999999999999955591, 0.49480006232932938204,
465  0, 0, 0, 1;
466  // clang-format on
467  const Vector3<S> box2_size(0.49430000000000001714, 0.35460000000000002629, 0.075200000000000002953);
468  Transform3<S> X_WB2 = Transform3<S>::Identity();
469  // clang-format off
470  X_WB2.matrix() << 0.44171122913485860728, 0.8971572827861190591, -1.622764514865468214e-09, 1.1304016226141906376,
471  -0.8971572827861190591, 0.44171122913485860728, -5.1621053952306079594e-09, 1.8410802645284281009,
472  -3.9144271413829990148e-09, 3.7360349218094348098e-09, 1, 0.44400006232932492933,
473  0, 0, 0, 1;
474  // clang-format on;
475  const double expected_distance{0};
476  test_distance_box_box_helper(box1_size, X_WB1, box2_size, X_WB2, &expected_distance);
477 }
478 
479 
480 template <typename S>
481 void test_distance_box_box_regression7() {
482  // This is reported in https://github.com/RobotLocomotion/drake/issues/21673#issuecomment-2373031038
483  SCOPED_TRACE("test_distance_box_box_regression7");
484  const Vector3<S> box1_size(0.050000000000000002776, 0.55000000000000004441, 0.2999999999999999889);
485  Transform3<S> X_WB1 = Transform3<S>::Identity();
486  // clang-format off
487  X_WB1.matrix() << 0.00079632671073326442932, -0.99999968293183538748, 0, 0.75000000000000055511,
488  0.99999968293183538748, 0.00079632671073326442932, 0, -2.4083553715553102684e-15,
489  0, 0, 1, 0.14999999999999999445,
490  0, 0, 0, 1;
491  // clang-format on
492  const Vector3<S> box2_size(0.25, 0.2000000000000000111,
493  0.14999999999999999445);
494  Transform3<S> X_WB2 = Transform3<S>::Identity();
495  // clang-format off
496  X_WB2.matrix() << 6.1232339957367660359e-17, 0, 1, 0.75,
497  -1, 6.1232339957367660359e-17, 6.1232339957367660359e-17, 0,
498  -6.1232339957367660359e-17, -1, 3.7493994566546440196e-33, 0.14999999999999999445,
499  0, 0, 0, 1;
500  // clang-format on;
501  test_distance_box_box_helper(box1_size, X_WB1, box2_size, X_WB2);
502 }
503 
504 // Issue #493 outlines a number of scenarios that caused signed distance
505 // failure. They consisted of two identical, stacked boxes. The boxes are
506 // slightly tilted. The boxes were essentially touching but were separated by
507 // infinitesimally small distances. The issue outlines three different examples.
508 // Rather than reproducing each of them verbatim, this test attempts to
509 // generalize those cases by testing the stacked scenario across various box
510 // sizes and separation amounts (ranging from slightly penetrating to slightly
511 // separated). These should essentially cover the variations described in the
512 // issue.
513 template <typename S>
514 void test_distance_box_box_regression_tilted_kissing_contact() {
515  SCOPED_TRACE("test_distance_box_box_regression_tilted_kissing_contact");
516  // The boxes are posed relative to each other in a common frame F (such that
517  // it is easy to reason about their separation). The stack is rotated around
518  // box A's origin and translated into the world frame.
519  Matrix3<S> R_WF;
520  R_WF <<
521  0.94096063217417758029, 0.29296840037289501035, 0.16959541586174811667,
522  -0.23569836841299879326, 0.92661523595848427348, -0.29296840037289506586,
523  -0.2429801799032638987, 0.23569836841299884878, 0.94096063217417758029;
524 
525  for (const S dim : {S(0.01), S(0.25), S(0.5), S(10), S(1000)}) {
526  const Vector3<S> box_size(dim, dim, dim);
527 
528  const Vector3<S> p_WA(0, 0, 5 * dim);
529  Transform3<S> X_WA;
530  X_WA.linear() = R_WF;
531  X_WA.translation() = p_WA;
532  Transform3<S> X_WB;
533  X_WB.linear() = R_WF;
534 
535  // Both boxes always have the same orientation and the *stack* is always
536  // located at p_WA. Only the translational component of X_WB changes with
537  // varying separation distance.
538 
539  // By design, the distances are all near epsilon. We'll scale them up for
540  // larger boxes to make sure the distance doesn't simply disappear in
541  // the rounding noise.
542  for (const S distance : {S(-1e-15), S(-2.5e-16), S(-1e-16), S(0), S(1e-16),
543  S(2.5e-16), S(1e-15)}) {
544  const S scaled_distance = distance * std::max(S(1), dim);
545  const Vector3<S> p_AB_F = Vector3<S>(0, dim + scaled_distance, 0);
546 
547  X_WB.translation() = p_WA + R_WF * p_AB_F;
548  SCOPED_TRACE("dim: " + std::to_string(dim) +
549  ", distance: " + std::to_string(distance));
550  test_distance_box_box_helper(box_size, X_WA, box_size, X_WB,
551  &scaled_distance);
552  }
553  }
554 }
555 
556 // This is a *specific* case that has cropped up in the wild that reaches the
557 // unexpected `validateNearestFeatureOfPolytopeBeingEdge` error. This error was
558 // reported in https://github.com/flexible-collision-library/fcl/issues/408
559 template <typename S>
560 void test_distance_sphere_box_regression1() {
561  SCOPED_TRACE("test_distance_sphere_box_regression1");
562  using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometry<S>>;
563  const S sphere_radius = 0.06;
564  CollisionGeometryPtr_t sphere_geo(new fcl::Sphere<S>(sphere_radius));
565  Transform3<S> X_WS = Transform3<S>::Identity();
566  // clang-format off
567  X_WS.matrix() << -0.99999999999999955591, -4.4637642593504144998e-09, 0, 1.7855056639081962376e-10,
568  4.4637642593504144998e-09, -0.99999999999999955591, 0, 0.039999999999999993894,
569  0, 0, 1.0000000000000008882, 0.33000000000000012657,
570  0, 0, 0, 1;
571  // clang-format on
572  fcl::CollisionObject<S> sphere(sphere_geo, X_WS);
573 
574  CollisionGeometryPtr_t box_geo(new fcl::Box<S>(0.1, 0.1, 0.1));
575  Transform3<S> X_WB = Transform3<S>::Identity();
576  // clang-format off
577  X_WB.matrix() << 1, 0, 0, 0.05,
578  0, 1, 0, 0.15,
579  0, 0, 1, 0.35,
580  0, 0, 0, 1;
581  // clang-format on
582  fcl::CollisionObject<S> box(box_geo, X_WB);
583  fcl::DistanceRequest<S> request;
584  request.gjk_solver_type = GJKSolverType::GST_LIBCCD;
585  request.distance_tolerance = 1e-6;
586  request.enable_signed_distance = true;
587  fcl::DistanceResult<S> result;
588 
589  ASSERT_NO_THROW(fcl::distance(&sphere, &box, request, result));
590  const S expected_distance = 0.06 - sphere_radius;
591  EXPECT_NEAR(result.min_distance, expected_distance,
592  request.distance_tolerance);
593 }
594 
595 template <typename S>
596 void test_distance_convex_to_convex1() {
597  // This is a real world failure reported in
598  // https://github.com/RobotLocomotion/drake/issues/21673#issuecomment-2679026288.
599  // The two convex shapes are almost in penetration.
600  using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometryd>;
601  CollisionGeometryPtr_t convex_geo1(new fcl::Convex<S>(
602  std::make_shared<const std::vector<Vector3<S>>>(
603  std::initializer_list<Vector3<S>>{
604  Vector3<S>(0.5, 0.70710700000000004106, -0.70710700000000004106),
605  Vector3<S>(0.5, -0, -1),
606  Vector3<S>(0.5, 1, 0),
607  Vector3<S>(0.5, -1, 0),
608  Vector3<S>(0.5, -0.70710700000000004106, -0.70710700000000004106),
609  Vector3<S>(-0.5, 0, 1),
610  Vector3<S>(-0.5, -0.70710700000000004106, 0.70710700000000004106),
611  Vector3<S>(-0.5, 0.70710700000000004106, 0.70710700000000004106),
612  Vector3<S>(0.5, 0, 1),
613  Vector3<S>(-0.5, -1, 0),
614  Vector3<S>(-0.5, 0.70710700000000004106, -0.70710700000000004106),
615  Vector3<S>(-0.5, -0, -1),
616  Vector3<S>(-0.5, -0.70710700000000004106,
617  -0.70710700000000004106),
618  Vector3<S>(-0.5, 1, 0),
619  Vector3<S>(0.5, 0.70710700000000004106, 0.70710700000000004106),
620  Vector3<S>(0.5, -0.70710700000000004106, 0.70710700000000004106),
621  }),
622  10,
623  std::make_shared<std::vector<int>>(std::initializer_list<int>{
624  4, 4, 12, 11, 1, 4, 13, 7, 14, 2, 4, 2, 0, 10, 13,
625  4, 11, 10, 0, 1, 8, 2, 14, 8, 15, 3, 4, 1, 0, 4,
626  3, 9, 12, 4, 4, 15, 6, 9, 3, 8, 12, 9, 6, 5, 7,
627  13, 10, 11, 4, 14, 7, 5, 8, 4, 8, 5, 6, 15,
628  }),
629  false));
630  Transform3<S> X_WG1 = Transform3<S>::Identity();
631  // clang-format off
632  X_WG1.matrix() << -2.0399676677885372849e-09, 0.77129744817973977522, -0.63647485922966484662, 11.477445682202462862,
633  2.4720879917217940548e-09, 0.63647485922966484662, 0.77129744817973977522, 9.7785056920756936449,
634  1, 0, -3.2051032938795742666e-09, 0,
635  0, 0, 0, 1;
636  // clang-format on
637  fcl::CollisionObject<S> convex1(convex_geo1, X_WG1);
638 
639  CollisionGeometryPtr_t convex_geo2(new fcl::Convex<S>(
640  std::make_shared<std::vector<Vector3<S>>>(
641  std::initializer_list<Vector3<S>>{
642  Vector3<S>(10.294759000000000881, 8.9321570000000001244, -0.5),
643  Vector3<S>(11.618370999999999782, 9.4565629999999991639, -0.5),
644  Vector3<S>(11.452372999999999692, 8.2276720000000000965, -0.5),
645  Vector3<S>(10.294759000000000881, 8.9321570000000001244, 0.5),
646  Vector3<S>(11.452372999999999692, 8.2276720000000000965, 0.5),
647  Vector3<S>(11.618370999999999782, 9.4565629999999991639, 0.5),
648  }),
649  5,
650  std::make_shared<std::vector<int>>(std::initializer_list<int>{
651  3, 3, 4, 5, 4, 4, 2, 1, 5, 3, 2, 0, 1, 4, 3, 0, 2, 4, 4, 1, 0, 3, 5,
652  }),
653  false));
654  const Transform3<S> X_WS2 = Transform3<S>::Identity();
655  fcl::CollisionObject<S> convex2(convex_geo2, X_WS2);
656  fcl::DistanceRequest<S> request;
657  request.gjk_solver_type = GJKSolverType::GST_LIBCCD;
658  request.distance_tolerance = 1e-6;
659  request.enable_signed_distance = true;
660  fcl::DistanceResult<S> result;
661  ASSERT_NO_THROW(fcl::distance(&convex1, &convex2, request, result));
662  EXPECT_NEAR(abs(result.min_distance),
663  (result.nearest_points[0] - result.nearest_points[1]).norm(),
664  request.distance_tolerance);
665 }
666 
667 template <typename S>
668 void test_distance_convex_to_convex2() {
669  // This is a real world failure reported in
670  // https://github.com/RobotLocomotion/drake/issues/21673#issuecomment-2688753008
671  using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometryd>;
672  CollisionGeometryPtr_t convex_geo1(new fcl::Convex<S>(
673  std::make_shared<const std::vector<Vector3<S>>>(
674  std::initializer_list<Vector3<S>>{
675  Vector3<S>(0.5, 0.5, -0.86602500000000004476),
676  Vector3<S>(0.5, -0.5, -0.86602500000000004476),
677  Vector3<S>(0.5, 1, 0),
678  Vector3<S>(-0.5, -0.5, 0.86602500000000004476),
679  Vector3<S>(-0.5, 0.5, 0.86602500000000004476),
680  Vector3<S>(-0.5, -1, 0),
681  Vector3<S>(0.5, -0.5, 0.86602500000000004476),
682  Vector3<S>(-0.5, 0.5, -0.86602500000000004476),
683  Vector3<S>(-0.5, -0.5, -0.86602500000000004476),
684  Vector3<S>(-0.5, 1, 0),
685  Vector3<S>(0.5, -1, 0),
686  Vector3<S>(0.5, 0.5, 0.86602500000000004476),
687  }),
688  8,
689  std::make_shared<std::vector<int>>(std::initializer_list<int>{
690  4, 10, 5, 8, 1, 6, 2, 11, 6, 10, 1, 0, 4, 0, 7,
691  9, 2, 4, 1, 8, 7, 0, 4, 2, 9, 4, 11, 6, 8, 5,
692  3, 4, 9, 7, 4, 6, 3, 5, 10, 4, 4, 3, 6, 11,
693  }),
694  false));
695  Transform3<S> X_WG1 = Transform3<S>::Identity();
696  // clang-format off
697  X_WG1.matrix() <<
698  -3.0140487242790098936e-09, 0.34009658459984087875, -0.94039051098122172778, 4.2864836941313040342,
699  1.090044683538143324e-09, 0.94039051098122172778, 0.34009658459984087875, 7.3869576872253679412,
700  1, 0, -3.2051032938795742666e-09, 0,
701  0, 0, 0, 1;
702  // clang-format on
703  fcl::CollisionObject<S> convex1(convex_geo1, X_WG1);
704 
705  CollisionGeometryPtr_t convex_geo2(new fcl::Convex<S>(
706  std::make_shared<std::vector<Vector3<S>>>(
707  std::initializer_list<Vector3<S>>{
708  Vector3<S>(4.1494020000000002568, 8.4937090000000008416, -0.5),
709  Vector3<S>(5.1884509999999997021, 7.476263000000000325, -0.5),
710  Vector3<S>(3.9316469999999998919, 7.3745029999999998083, -0.5),
711  Vector3<S>(4.1494020000000002568, 8.4937090000000008416, 0.5),
712  Vector3<S>(3.9316469999999998919, 7.3745029999999998083, 0.5),
713  Vector3<S>(5.1884509999999997021, 7.476263000000000325, 0.5),
714  }),
715  5,
716  std::make_shared<std::vector<int>>(std::initializer_list<int>{
717  3, 5, 3, 4, 4, 2, 1, 5, 4, 3, 2, 0, 1, 4, 3, 0, 2, 4, 4, 1, 0, 3, 5,
718  }),
719  false));
720  const Transform3<S> X_WS2 = Transform3<S>::Identity();
721  fcl::CollisionObject<S> convex2(convex_geo2, X_WS2);
722  fcl::DistanceRequest<S> request;
723  request.gjk_solver_type = GJKSolverType::GST_LIBCCD;
724  request.distance_tolerance = 1e-6;
725  request.enable_signed_distance = true;
726  fcl::DistanceResult<S> result;
727  ASSERT_NO_THROW(fcl::distance(&convex1, &convex2, request, result));
728  EXPECT_NEAR(abs(result.min_distance),
729  (result.nearest_points[0] - result.nearest_points[1]).norm(),
730  request.distance_tolerance);
731 }
732 
733 template <typename S>
734 void test_distance_convex_to_convex3() {
735  // This is a real world failure reported in
736  // https://github.com/RobotLocomotion/drake/issues/21673#issuecomment-2689035046
737  using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometryd>;
738  CollisionGeometryPtr_t convex_geo1(new fcl::Convex<S>(
739  std::make_shared<const std::vector<Vector3<S>>>(
740  std::initializer_list<Vector3<S>>{
741  Vector3<S>(0.5, 1, 0),
742  Vector3<S>(0.5, 0.62348999999999998867, -0.7818310000000000537),
743  Vector3<S>(0.5, -0.22252099999999999658, -0.97492800000000001681),
744  Vector3<S>(0.5, 0.62348999999999998867, 0.7818310000000000537),
745  Vector3<S>(-0.5, -0.90096900000000001985, 0.4338839999999999919),
746  Vector3<S>(-0.5, -0.22252099999999999658, 0.97492800000000001681),
747  Vector3<S>(-0.5, -0.90096900000000001985, -0.4338839999999999919),
748  Vector3<S>(0.5, -0.90096900000000001985, 0.4338839999999999919),
749  Vector3<S>(-0.5, 0.62348999999999998867, -0.7818310000000000537),
750  Vector3<S>(-0.5, 0.62348999999999998867, 0.7818310000000000537),
751  Vector3<S>(-0.5, -0.22252099999999999658,
752  -0.97492800000000001681),
753  Vector3<S>(-0.5, 1, 0),
754  Vector3<S>(0.5, -0.90096900000000001985, -0.4338839999999999919),
755  Vector3<S>(0.5, -0.22252099999999999658, 0.97492800000000001681),
756  }),
757  9,
758  std::make_shared<std::vector<int>>(std::initializer_list<int>{
759  4, 10, 2, 12, 6, 4, 1, 8, 11, 0, 4, 10, 8, 1, 2, 7, 2,
760  1, 0, 3, 13, 7, 12, 4, 9, 3, 0, 11, 4, 9, 5, 13, 3, 7,
761  8, 10, 6, 4, 5, 9, 11, 4, 12, 7, 4, 6, 4, 13, 5, 4, 7,
762  }),
763  false));
764  Transform3<S> X_WG1 = Transform3<S>::Identity();
765  // clang-format off
766  X_WG1.matrix() <<
767  -2.0479437360480804916e-09, -0.76923712747984118732, -0.63896341186844385351, 13.670417904867957049,
768  -2.4654844510601008403e-09, 0.63896341186844385351, -0.76923712747984118732, 13.169816779836704512,
769  1, 0, -3.2051032938795742666e-09, 0,
770  0, 0, 0, 1;
771  // clang-format on
772  fcl::CollisionObject<S> convex1(convex_geo1, X_WG1);
773 
774  CollisionGeometryPtr_t convex_geo2(new fcl::Convex<S>(
775  std::make_shared<std::vector<Vector3<S>>>(
776  std::initializer_list<Vector3<S>>{
777  Vector3<S>(14.835100999999999871, 13.060409999999999187, -0.5),
778  Vector3<S>(13.785037000000000873, 12.890523999999999205, -0.5),
779  Vector3<S>(14.767995000000000871, 13.150247999999999493, -0.5),
780  Vector3<S>(14.835100999999999871, 13.060409999999999187, 0.5),
781  Vector3<S>(14.767995000000000871, 13.150247999999999493, 0.5),
782  Vector3<S>(13.785037000000000873, 12.890523999999999205, 0.5),
783  }),
784  5,
785  std::make_shared<std::vector<int>>(std::initializer_list<int>{
786  3, 3, 4, 5, 4, 2, 1, 5, 4, 3, 2, 0, 1, 4, 3, 0, 2, 4, 4, 5, 1, 0, 3,
787  }),
788  false));
789  const Transform3<S> X_WS2 = Transform3<S>::Identity();
790  fcl::CollisionObject<S> convex2(convex_geo2, X_WS2);
791  fcl::DistanceRequest<S> request;
792  request.gjk_solver_type = GJKSolverType::GST_LIBCCD;
793  request.distance_tolerance = 1e-6;
794  request.enable_signed_distance = true;
795  fcl::DistanceResult<S> result;
796  ASSERT_NO_THROW(fcl::distance(&convex1, &convex2, request, result));
797  EXPECT_NEAR(abs(result.min_distance),
798  (result.nearest_points[0] - result.nearest_points[1]).norm(),
799  request.distance_tolerance);
800 }
801 
802 template <typename S>
803 void test_distance_convex_to_convex4() {
804  // This is a real world failure reported in
805  // https://github.com/RobotLocomotion/drake/issues/21673#issuecomment-2689248075
806  using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometryd>;
807  CollisionGeometryPtr_t convex_geo1(new fcl::Convex<S>(
808  std::make_shared<const std::vector<Vector3<S>>>(
809  std::initializer_list<Vector3<S>>{
810  Vector3<S>(0.5, -0.92388000000000003453, 0.38268299999999999539),
811  Vector3<S>(-0.5, 0.92388000000000003453, -0.38268299999999999539),
812  Vector3<S>(0.5, 0.92388000000000003453, -0.38268299999999999539),
813  Vector3<S>(-0.5, 0.92388000000000003453, 0.38268299999999999539),
814  Vector3<S>(-0.5, -0.92388000000000003453,
815  -0.38268299999999999539),
816  Vector3<S>(0.5, 0.92388000000000003453, 0.38268299999999999539),
817  Vector3<S>(-0.5, -0.92388000000000003453, 0.38268299999999999539),
818  Vector3<S>(0.5, -0.92388000000000003453, -0.38268299999999999539),
819  }),
820  6,
821  std::make_shared<std::vector<int>>(std::initializer_list<int>{
822  4, 3, 6, 0, 5, 4, 7, 2, 5, 0, 4, 6, 4, 7, 0,
823  4, 2, 1, 3, 5, 4, 3, 1, 4, 6, 4, 7, 4, 1, 2,
824  }),
825  false));
826  Transform3<S> X_WG1 = Transform3<S>::Identity();
827  // clang-format off
828  X_WG1.matrix() <<
829  -2.6487935924268804787e-09, -0.56303944378188575115, -0.82643002410717436579, 15.712812998638135298,
830  -1.8045995758494454423e-09, 0.82643002410717436579, -0.56303944378188575115, 10.219809745800642276,
831  1, 0, -3.2051032938795742666e-09, 0,
832  0, 0, 0, 1;
833  // clang-format on
834  fcl::CollisionObject<S> convex1(convex_geo1, X_WG1);
835 
836  CollisionGeometryPtr_t convex_geo2(new fcl::Convex<S>(
837  std::make_shared<std::vector<Vector3<S>>>(
838  std::initializer_list<Vector3<S>>{
839  Vector3<S>(15.417082000000000619, 9.6986209999999992704, -0.5),
840  Vector3<S>(16.236605000000000842, 9.2332309999999999661, -0.5),
841  Vector3<S>(16.224319999999998743, 8.9158489999999996911, -0.5),
842  Vector3<S>(15.417082000000000619, 9.6986209999999992704, 0.5),
843  Vector3<S>(16.224319999999998743, 8.9158489999999996911, 0.5),
844  Vector3<S>(16.236605000000000842, 9.2332309999999999661, 0.5),
845  }),
846  5,
847  std::make_shared<std::vector<int>>(std::initializer_list<int>{
848  3, 3, 4, 5, 4, 2, 1, 5, 4, 3, 2, 0, 1, 4, 4, 3, 0, 2, 4, 5, 1, 0, 3,
849  }),
850  false));
851  const Transform3<S> X_WS2 = Transform3<S>::Identity();
852  fcl::CollisionObject<S> convex2(convex_geo2, X_WS2);
853  fcl::DistanceRequest<S> request;
854  request.gjk_solver_type = GJKSolverType::GST_LIBCCD;
855  request.distance_tolerance = 1e-6;
856  request.enable_signed_distance = true;
857  fcl::DistanceResult<S> result;
858  ASSERT_NO_THROW(fcl::distance(&convex1, &convex2, request, result));
859  EXPECT_NEAR(abs(result.min_distance),
860  (result.nearest_points[0] - result.nearest_points[1]).norm(),
861  request.distance_tolerance);
862 }
863 
864 template <typename S>
865 void test_distance_convex_to_convex5() {
866  // This is a real world failure reported in
867  // https://github.com/RobotLocomotion/drake/issues/21673#issuecomment-2689297095
868  using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometryd>;
869  CollisionGeometryPtr_t convex_geo1(new fcl::Convex<S>(
870  std::make_shared<const std::vector<Vector3<S>>>(
871  std::initializer_list<Vector3<S>>{
872  Vector3<S>(0.5, 0.5, -0.86602500000000004476),
873  Vector3<S>(0.5, -0.5, -0.86602500000000004476),
874  Vector3<S>(0.5, 1, 0),
875  Vector3<S>(-0.5, -0.5, 0.86602500000000004476),
876  Vector3<S>(-0.5, 0.5, 0.86602500000000004476),
877  Vector3<S>(-0.5, -1, 0),
878  Vector3<S>(0.5, -0.5, 0.86602500000000004476),
879  Vector3<S>(-0.5, 0.5, -0.86602500000000004476),
880  Vector3<S>(-0.5, -0.5, -0.86602500000000004476),
881  Vector3<S>(-0.5, 1, 0),
882  Vector3<S>(0.5, -1, 0),
883  Vector3<S>(0.5, 0.5, 0.86602500000000004476),
884  }),
885  5,
886  std::make_shared<std::vector<int>>(std::initializer_list<int>{
887  4, 10, 5, 8, 1, 6, 2, 11, 6, 10, 1, 0, 4, 0, 7,
888  9, 2, 4, 1, 8, 7, 0, 4, 2, 9, 4, 11, 6, 8, 5,
889  3, 4, 9, 7, 4, 6, 3, 5, 10, 4, 4, 3, 6, 11,
890  }),
891  false));
892  Transform3<S> X_WG1 = Transform3<S>::Identity();
893  // clang-format off
894  X_WG1.matrix() <<
895  -2.2411796174427631456e-09, 0.7148738189791493669, -0.69925347545662319693, 6.3335747278141161232,
896  2.2912444319183419853e-09, 0.69925347545662319693, 0.7148738189791493669, 8.4631576303276716544,
897  1, 0, -3.2051032938795742666e-09, 0,
898  0, 0, 0, 1;
899  // clang-format on
900  fcl::CollisionObject<S> convex1(convex_geo1, X_WG1);
901 
902  CollisionGeometryPtr_t convex_geo2(new fcl::Convex<S>(
903  std::make_shared<std::vector<Vector3<S>>>(
904  std::initializer_list<Vector3<S>>{
905  Vector3<S>(5.2423630000000001061, 9.1228110000000004476, -0.5),
906  Vector3<S>(6.3596640000000004278, 8.2852599999999991809, -0.5),
907  Vector3<S>(5.6703770000000002227, 7.5985389999999997102, -0.5),
908  Vector3<S>(5.2423630000000001061, 9.1228110000000004476, 0.5),
909  Vector3<S>(5.6703770000000002227, 7.5985389999999997102, 0.5),
910  Vector3<S>(6.3596640000000004278, 8.2852599999999991809, 0.5),
911  }),
912  5,
913  std::make_shared<std::vector<int>>(std::initializer_list<int>{
914  3, 3, 4, 5, 4, 4, 2, 1, 5, 3, 2, 0, 1, 4, 3, 0, 2, 4, 4, 1, 0, 3, 5,
915  }),
916  false));
917  const Transform3<S> X_WS2 = Transform3<S>::Identity();
918  fcl::CollisionObject<S> convex2(convex_geo2, X_WS2);
919  fcl::DistanceRequest<S> request;
920  request.gjk_solver_type = GJKSolverType::GST_LIBCCD;
921  request.distance_tolerance = 1e-6;
922  request.enable_signed_distance = true;
923  fcl::DistanceResult<S> result;
924  ASSERT_NO_THROW(fcl::distance(&convex1, &convex2, request, result));
925  EXPECT_NEAR(abs(result.min_distance),
926  (result.nearest_points[0] - result.nearest_points[1]).norm(),
927  request.distance_tolerance);
928 }
929 
930 template <typename S>
931 void test_distance_convex_to_convex6() {
932  // This is a real world failure reported in
933  // https://github.com/RobotLocomotion/drake/issues/21673#issuecomment-2689297095
934  using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometryd>;
935  CollisionGeometryPtr_t convex_geo1(new fcl::Convex<S>(
936  std::make_shared<const std::vector<Vector3<S>>>(
937  std::initializer_list<Vector3<S>>{
938  Vector3<S>(0.5, 0.30901699999999998614, -0.95105700000000004124),
939  Vector3<S>(0.5, 1, 0),
940  Vector3<S>(-0.5, -0.80901699999999998614, 0.58778500000000000192),
941  Vector3<S>(-0.5, 0.30901699999999998614, 0.95105700000000004124),
942  Vector3<S>(-0.5, -0.80901699999999998614,
943  -0.58778500000000000192),
944  Vector3<S>(0.5, -0.80901699999999998614, 0.58778500000000000192),
945  Vector3<S>(-0.5, 0.30901699999999998614, -0.95105700000000004124),
946  Vector3<S>(-0.5, 1, 0),
947  Vector3<S>(0.5, -0.80901699999999998614, -0.58778500000000000192),
948  Vector3<S>(0.5, 0.30901699999999998614, 0.95105700000000004124),
949  }),
950  7,
951  std::make_shared<std::vector<int>>(std::initializer_list<int>{
952  4, 0, 6, 7, 1, 5, 9, 5, 8, 0, 1, 4, 0, 8, 4, 6, 4, 1, 7,
953  3, 9, 5, 6, 4, 2, 3, 7, 4, 8, 5, 2, 4, 4, 3, 2, 5, 9,
954  }),
955  false));
956  Transform3<S> X_WG1 = Transform3<S>::Identity();
957  // clang-format off
958  X_WG1.matrix() <<
959  -3.1934130613594990691e-09, -0.085331461972016672823, -0.99635261910516315087, 11.261494468063983021,
960  -2.7349614983807029573e-10, 0.99635261910516315087, -0.085331461972016672823, 5.7102314848013024928,
961  1, 0, -3.2051032938795742666e-09, 0,
962  0, 0, 0, 1;
963  // clang-format on
964  fcl::CollisionObject<S> convex1(convex_geo1, X_WG1);
965 
966  CollisionGeometryPtr_t convex_geo2(new fcl::Convex<S>(
967  std::make_shared<std::vector<Vector3<S>>>(
968  std::initializer_list<Vector3<S>>{
969  Vector3<S>(10.924768999999999508, 5.6701019999999999754, -0.5),
970  Vector3<S>(10.83925699999999992, 4.8329899999999996751, -0.5),
971  Vector3<S>(11.452507000000000659, 5.5209809999999999164, -0.5),
972  Vector3<S>(10.924768999999999508, 5.6701019999999999754, 0.5),
973  Vector3<S>(11.452507000000000659, 5.5209809999999999164, 0.5),
974  Vector3<S>(10.83925699999999992, 4.8329899999999996751, 0.5),
975  }),
976  5,
977  std::make_shared<std::vector<int>>(std::initializer_list<int>{
978  3, 4, 3, 5, 4, 5, 1, 2, 4, 3, 1, 0, 2, 4, 2, 0, 3, 4, 4, 5, 3, 0, 1,
979  }),
980  false));
981  const Transform3<S> X_WS2 = Transform3<S>::Identity();
982  fcl::CollisionObject<S> convex2(convex_geo2, X_WS2);
983  fcl::DistanceRequest<S> request;
984  request.gjk_solver_type = GJKSolverType::GST_LIBCCD;
985  request.distance_tolerance = 1e-6;
986  request.enable_signed_distance = true;
987  fcl::DistanceResult<S> result;
988  ASSERT_NO_THROW(fcl::distance(&convex1, &convex2, request, result));
989  EXPECT_NEAR(abs(result.min_distance),
990  (result.nearest_points[0] - result.nearest_points[1]).norm(),
991  request.distance_tolerance);
992 }
993 
994 template <typename S>
995 void test_distance_convex_to_convex7() {
996  // This is a real world failure reported in
997  // https://github.com/RobotLocomotion/drake/issues/21673#issuecomment-2689678480
998  using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometryd>;
999  CollisionGeometryPtr_t convex_geo1(new fcl::Convex<S>(
1000  std::make_shared<const std::vector<Vector3<S>>>(
1001  std::initializer_list<Vector3<S>>{
1002  Vector3<S>(-0.5, -0.5, -0.86602500000000004476),
1003  Vector3<S>(-0.5, -0.5, 0.86602500000000004476),
1004  Vector3<S>(-0.5, 1, 0),
1005  Vector3<S>(0.5, -0.5, -0.86602500000000004476),
1006  Vector3<S>(0.5, 1, 0),
1007  Vector3<S>(0.5, -0.5, 0.86602500000000004476),
1008  }),
1009  5,
1010  std::make_shared<std::vector<int>>(std::initializer_list<int>{
1011  3, 5, 3, 4, 4, 2, 1, 5, 4, 3, 2, 0, 1, 4, 3, 0, 2, 4, 4, 1, 0, 3, 5,
1012  }),
1013  false));
1014  Transform3<S> X_WG1 = Transform3<S>::Identity();
1015  // clang-format off
1016  X_WG1.matrix() <<
1017  -3.155367699156126597e-09, -0.17548349096519452739, -0.98448237383849002136, 9.4277544959429171456,
1018  -5.6244271491403150214e-10, 0.98448237383849002136, -0.17548349096519452739, 4.3549730982604870633,
1019  1, 0, -3.2051032938795742666e-09, 0,
1020  0, 0, 0, 1;
1021  // clang-format on
1022  fcl::CollisionObject<S> convex1(convex_geo1, X_WG1);
1023 
1024  CollisionGeometryPtr_t convex_geo2(new fcl::Convex<S>(
1025  std::make_shared<std::vector<Vector3<S>>>(
1026  std::initializer_list<Vector3<S>>{
1027  Vector3<S>(7.502455000000000318, 3.9926669999999999661, -0.5),
1028  Vector3<S>(7.9127499999999999503, 2.7334600000000000009, -0.5),
1029  Vector3<S>(8.6743819999999995929, 3.7108469999999997846, -0.5),
1030  Vector3<S>(7.502455000000000318, 3.9926669999999999661, 0.5),
1031  Vector3<S>(8.6743819999999995929, 3.7108469999999997846, 0.5),
1032  Vector3<S>(7.9127499999999999503, 2.7334600000000000009, 0.5),
1033  }),
1034  5,
1035  std::make_shared<std::vector<int>>(std::initializer_list<int>{
1036  3, 3, 5, 4, 4, 5, 1, 2, 4, 3, 1, 0, 2, 4, 4, 2, 0, 3, 4, 3, 0, 1, 5,
1037  }),
1038  false));
1039  const Transform3<S> X_WS2 = Transform3<S>::Identity();
1040  fcl::CollisionObject<S> convex2(convex_geo2, X_WS2);
1041  fcl::DistanceRequest<S> request;
1042  request.gjk_solver_type = GJKSolverType::GST_LIBCCD;
1043  request.distance_tolerance = 1e-6;
1044  request.enable_signed_distance = true;
1045  fcl::DistanceResult<S> result;
1046  ASSERT_NO_THROW(fcl::distance(&convex1, &convex2, request, result));
1047  EXPECT_NEAR(abs(result.min_distance),
1048  (result.nearest_points[0] - result.nearest_points[1]).norm(),
1049  request.distance_tolerance);
1050 }
1051 
1052 //==============================================================================
1053 
1054 GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere_ccd) {
1055  test_distance_spheresphere<double>(GST_LIBCCD);
1056 }
1057 
1058 GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere_indep) {
1059  test_distance_spheresphere<double>(GST_INDEP);
1060 }
1061 
1062 GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_capsule_ccd) {
1063  test_distance_spherecapsule<double>(GST_LIBCCD);
1064 }
1065 
1066 GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_capsule_indep) {
1067  test_distance_spherecapsule<double>(GST_INDEP);
1068 }
1069 
1070 GTEST_TEST(FCL_SIGNED_DISTANCE, cylinder_sphere1_ccd) {
1071  test_distance_cylinder_sphere1<double>();
1072 }
1073 
1074 GTEST_TEST(FCL_SIGNED_DISTANCE, cylinder_box_ccd) {
1075  test_distance_cylinder_box<double>();
1076 }
1077 
1078 
1079 // A collection of scenarios observed in practice that have created error
1080 // conditions in previous commits of the code. Each test is a unique instance.
1081 GTEST_TEST(FCL_SIGNED_DISTANCE, RealWorldRegression1) {
1082  test_distance_box_box_regression1<double>();
1083 }
1084 
1085 GTEST_TEST(FCL_SIGNED_DISTANCE, RealWorldRegression2) {
1086  test_distance_box_box_regression2<double>();
1087 }
1088 
1089 GTEST_TEST(FCL_SIGNED_DISTANCE, RealWorldRegression3) {
1090  test_distance_box_box_regression3<double>();
1091 }
1092 
1093 GTEST_TEST(FCL_SIGNED_DISTANCE, RealWorldRegression4) {
1094  test_distance_box_box_regression4<double>();
1095 }
1096 
1097 GTEST_TEST(FCL_SIGNED_DISTANCE, RealWorldRegression5) {
1098  test_distance_box_box_regression5<double>();
1099 }
1100 
1101 GTEST_TEST(FCL_SIGNED_DISTANCE, RealWorldRegression6) {
1102  test_distance_box_box_regression6<double>();
1103 }
1104 
1105 GTEST_TEST(FCL_SIGNED_DISTANCE, RealWorldRegression7) {
1106  test_distance_box_box_regression7<double>();
1107 }
1108 
1109 GTEST_TEST(FCL_SIGNED_DISTANCE, RealWorldRegression8) {
1110  test_distance_box_box_regression_tilted_kissing_contact<double>();
1111 }
1112 
1113 GTEST_TEST(FCL_SIGNED_DISTANCE, RealWorldRegression9) {
1114  test_distance_sphere_box_regression1<double>();
1115 }
1116 
1117 GTEST_TEST(FCL_SIGNED_DISTANCE, RealWorldRegression10) {
1118  test_distance_convex_to_convex1<double>();
1119 }
1120 
1121 GTEST_TEST(FCL_SIGNED_DISTANCE, RealWorldRegression11) {
1122  test_distance_convex_to_convex2<double>();
1123 }
1124 
1125 GTEST_TEST(FCL_SIGNED_DISTANCE, RealWorldRegression12) {
1126  test_distance_convex_to_convex3<double>();
1127 }
1128 
1129 GTEST_TEST(FCL_SIGNED_DISTANCE, RealWorldRegression13) {
1130  test_distance_convex_to_convex4<double>();
1131 }
1132 
1133 GTEST_TEST(FCL_SIGNED_DISTANCE, RealWorldRegression14) {
1134  test_distance_convex_to_convex5<double>();
1135 }
1136 
1137 GTEST_TEST(FCL_SIGNED_DISTANCE, RealWorldRegression15) {
1138  test_distance_convex_to_convex6<double>();
1139 }
1140 
1141 GTEST_TEST(FCL_SIGNED_DISTANCE, RealWorldRegression16) {
1142  test_distance_convex_to_convex7<double>();
1143 }
1144 //==============================================================================
1145 int main(int argc, char* argv[])
1146 {
1147  ::testing::InitGoogleTest(&argc, argv);
1148  return RUN_ALL_TESTS();
1149 }
test_distance_convex_to_convex7
void test_distance_convex_to_convex7()
Definition: test_fcl_signed_distance.cpp:995
fcl::DistanceRequest
request to the distance computation
Definition: distance_request.h:52
test_distance_box_box_regression5
void test_distance_box_box_regression5()
Definition: test_fcl_signed_distance.cpp:445
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:92
fcl::GJKSolverType
GJKSolverType
Type of narrow phase GJK solver.
Definition: gjk_solver_type.h:45
test_distance_box_box_regression4
void test_distance_box_box_regression4()
Definition: test_fcl_signed_distance.cpp:431
test_distance_convex_to_convex2
void test_distance_convex_to_convex2()
Definition: test_fcl_signed_distance.cpp:668
test_distance_spherecapsule
void test_distance_spherecapsule(GJKSolverType solver_type)
Definition: test_fcl_signed_distance.cpp:122
eigen_matrix_compare.h
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
test_distance_convex_to_convex6
void test_distance_convex_to_convex6()
Definition: test_fcl_signed_distance.cpp:931
epsilon
S epsilon()
Definition: test_fcl_simple.cpp:56
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::Convex< S >
fcl::constants::eps
static constexpr Real eps()
Returns ε for the precision of the underlying scalar type.
Definition: constants.h:151
fcl::DistanceResult::nearest_points
Vector3< S > nearest_points[2]
Nearest points in the world coordinates.
Definition: distance_result.h:69
max
static T max(T x, T y)
Definition: svm.cpp:52
test_fcl_utility.h
EXPECT_TRUE
#define EXPECT_TRUE(args)
fcl::DistanceResult
distance result
Definition: distance_request.h:48
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:71
verbose
bool verbose
Definition: test_fcl_signed_distance.cpp:47
test_distance_box_box_regression_tilted_kissing_contact
void test_distance_box_box_regression_tilted_kissing_contact()
Definition: test_fcl_signed_distance.cpp:514
test_distance_cylinder_sphere1
void test_distance_cylinder_sphere1()
Definition: test_fcl_signed_distance.cpp:170
distance.h
test_distance_spheresphere
void test_distance_spheresphere(GJKSolverType solver_type)
Definition: test_fcl_signed_distance.cpp:51
fcl::Matrix3
Eigen::Matrix< S, 3, 3 > Matrix3
Definition: types.h:86
test_distance_convex_to_convex4
void test_distance_convex_to_convex4()
Definition: test_fcl_signed_distance.cpp:803
gjk_solver_libccd.h
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
fcl::DistanceRequest::gjk_solver_type
GJKSolverType gjk_solver_type
narrow phase solver type
Definition: distance_request.h:102
test_distance_convex_to_convex1
void test_distance_convex_to_convex1()
Definition: test_fcl_signed_distance.cpp:596
fcl::GST_LIBCCD
@ GST_LIBCCD
Definition: gjk_solver_type.h:45
test_distance_box_box_regression2
void test_distance_box_box_regression2()
Definition: test_fcl_signed_distance.cpp:383
test_distance_convex_to_convex5
void test_distance_convex_to_convex5()
Definition: test_fcl_signed_distance.cpp:865
fcl::DistanceRequest::enable_nearest_points
bool enable_nearest_points
whether to return the nearest points
Definition: distance_request.h:55
fcl::DistanceResult::min_distance
S min_distance
Minimum distance between two objects.
Definition: distance_result.h:64
test_distance_box_box_regression1
void test_distance_box_box_regression1()
Definition: test_fcl_signed_distance.cpp:357
fcl::Cylinder
Center at zero cylinder.
Definition: cylinder.h:51
test_distance_cylinder_box_helper
void test_distance_cylinder_box_helper(S cylinder_radius, S cylinder_length, const Transform3< S > &X_WC, const Vector3< S > &box_size, const Transform3< S > &X_WB)
Definition: test_fcl_signed_distance.cpp:221
test_distance_box_box_regression6
void test_distance_box_box_regression6()
Definition: test_fcl_signed_distance.cpp:457
test_distance_sphere_box_regression1
void test_distance_sphere_box_regression1()
Definition: test_fcl_signed_distance.cpp:560
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:51
fcl::DistanceRequest::enable_signed_distance
bool enable_signed_distance
Whether to compute exact negative distance.
Definition: distance_request.h:92
fcl::Sphere
Center at zero point sphere.
Definition: sphere.h:51
fcl::DistanceRequest::distance_tolerance
S distance_tolerance
the threshold used in GJK algorithm to stop distance iteration
Definition: distance_request.h:99
test_distance_box_box_regression3
void test_distance_box_box_regression3()
Definition: test_fcl_signed_distance.cpp:407
main
int main(int argc, char *argv[])
Definition: test_fcl_signed_distance.cpp:1145
test_distance_box_box_helper
void test_distance_box_box_helper(const Vector3< S > &box1_size, const Transform3< S > &X_WB1, const Vector3< S > &box2_size, const Transform3< S > &X_WB2, const S *expected_distance=nullptr)
Definition: test_fcl_signed_distance.cpp:301
fcl::constants
Definition: constants.h:129
GTEST_TEST
GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere_ccd)
Definition: test_fcl_signed_distance.cpp:1054
collision_node.h
fcl::GST_INDEP
@ GST_INDEP
Definition: gjk_solver_type.h:45
expected_distance
S expected_distance
Definition: test_sphere_box.cpp:184
test_distance_box_box_regression7
void test_distance_box_box_regression7()
Definition: test_fcl_signed_distance.cpp:481
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
test_distance_convex_to_convex3
void test_distance_convex_to_convex3()
Definition: test_fcl_signed_distance.cpp:734
fcl::Capsule< S >
fcl::DistanceResult::clear
void clear()
clear the result
Definition: distance_result-inl.h:127
test_distance_cylinder_box
void test_distance_cylinder_box()
Definition: test_fcl_signed_distance.cpp:257

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