test_gjk_libccd-inl_signed_distance.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018. Toyota Research Institute
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 CNRS-LAAS and AIST 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 
41 
42 #include <gtest/gtest.h>
43 #include <Eigen/Dense>
44 
46 
47 namespace fcl {
48 namespace detail {
49 // Given two spheres, sphere 1 has radius1, and centered at point A, whose
50 // position is p_F1 measured and expressed in frame F; sphere 2 has radius2,
51 // and centered at point B, whose position is p_F2 measured and expressed in
52 // frame F. Computes the signed distance between the two spheres.
53 // We use the monogram notation on spatial vectors. The monogram notation is
54 // explained in
55 // http://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html
56 template <typename S>
57 S ComputeSphereSphereDistance(S radius1, S radius2, const Vector3<S>& p_F1,
58  const Vector3<S>& p_F2) {
59  S min_distance = (p_F1 - p_F2).norm() - radius1 - radius2;
60  return min_distance;
61 }
62 
63 template <typename S>
64 void TestSphereToSphereGJKSignedDistance(S radius1, S radius2,
65  const Vector3<S>& p_F1,
66  const Vector3<S>& p_F2,
67  S solver_tolerance, S test_tol,
68  S min_distance_expected) {
69  // Test if GJKSignedDistance computes the right distance. Here we used sphere
70  // to sphere as the geometries. The distance between sphere and sphere should
71  // be computed using distance between primitives, instead of the GJK
72  // algorithm. But here we choose spheres for simplicity.
73  //
74  // There are two tolerances: the solver_tolerance, and the test_tol.
75  // solver_tolerance is used to determine when the algorithm terminates. If
76  // the objects are separated, the GJK algorithm terminates when the change of
77  // distance is below solver_tolerance, which does NOT mean that the separation
78  // distance computed by GJK is within solver_tolerance to the true distance,
79  // so we use test_tol as a separate tolerance to check the accuracy of the
80  // computed distance.
81  // If the objects penetrate, the EPA algorithm terminates when the difference
82  // between the upper bound of penetration depth and the lower bound
83  // is below solver_tolerance, which means that the EPA computed penetration
84  // depth is within solver_tolerance to the true depth.
85  // The tolerance is explained in GJKSignedDistance() in gjk_libccd-inl.h
86  fcl::Sphere<S> s1(radius1);
87  fcl::Sphere<S> s2(radius2);
88  fcl::Transform3<S> tf1, tf2;
89  tf1.setIdentity();
90  tf2.setIdentity();
91  tf1.translation() = p_F1;
92  tf2.translation() = p_F2;
93  void* o1 = GJKInitializer<S, fcl::Sphere<S>>::createGJKObject(s1, tf1);
94  void* o2 = GJKInitializer<S, fcl::Sphere<S>>::createGJKObject(s2, tf2);
95 
96  S dist;
97  // N1 and N2 are the witness points on sphere 1 and 2 respectively.
98  Vector3<S> p_FN1, p_FN2;
99  GJKSolver_libccd<S> gjkSolver;
100  gjkSolver.distance_tolerance = solver_tolerance;
101  bool res = GJKSignedDistance(
102  o1, detail::GJKInitializer<S, Sphere<S>>::getSupportFunction(), o2,
103  detail::GJKInitializer<S, Sphere<S>>::getSupportFunction(),
104  gjkSolver.max_distance_iterations, gjkSolver.distance_tolerance, &dist,
105  &p_FN1, &p_FN2);
106 
107  EXPECT_EQ(res, min_distance_expected >= 0);
108 
109  EXPECT_NEAR(dist, min_distance_expected, test_tol);
110  // Now check if the distance between N1 and N2 matches with dist, they should
111  // match independent of what solver_tolerance we choose.
112  EXPECT_NEAR((p_FN1 - p_FN2).norm(), std::abs(dist),
114  // Check if p1 is on the boundary of sphere 1, and p2 is on the boundary of
115  // sphere 2.
116  EXPECT_NEAR((p_FN1 - p_F1).norm(), radius1, test_tol);
117  EXPECT_NEAR((p_FN2 - p_F2).norm(), radius2, test_tol);
118  // The witness points N1 and N2 are defined as by shifting geometry B with
119  // the vector N1 - N2, the shifted geometry B' and A are touching. Hence
120  // if we shift sphere 1 by p_FN2 - p_FN1, then the two spheres should be
121  // touching. The shifted sphere is centered at p_F1 + p_FN2 - p_FN1.
122  EXPECT_NEAR((p_F1 + p_FN2 - p_FN1 - p_F2).norm(), radius1 + radius2,
123  test_tol);
124  // Note that we do not check the computed witness points to the true witness
125  // points. There are two reasons
126  // 1. Generally, the witness points are NOT guaranteed to be unique (consider
127  // plane-to-plane contact).
128  // 2. Even if there are unique witness points, it is hard to infer the bounds
129  // on the computed witness points, based on the tolerance of the solver. This
130  // bounds depend on the curvature of the geometries near the contact region,
131  // and we do not have a general approach to compute the bound for generic
132  // geometries.
133  // On the other hand, for sphere-sphere contact, it is possible to compute
134  // the bounds, since the witness points are unique, and the curvature is
135  // constant. For the moment, we are satisfied with the test above. If the
136  // future maintainer wants to improve this test, he/she might compute these
137  // bounds for the sphere-sphere case.
138 
139  GJKInitializer<S, fcl::Sphere<S>>::deleteGJKObject(o1);
140  GJKInitializer<S, fcl::Sphere<S>>::deleteGJKObject(o2);
141 }
142 
143 template <typename S>
145  SphereSpecification<S>(S radius_, const Vector3<S>& center_)
146  : radius{radius_}, center{center_} {}
149 };
150 
151 template <typename S>
153  std::vector<SphereSpecification<S>> spheres;
154  spheres.emplace_back(0.5, Vector3<S>(0, 0, -1.2));
155  spheres.emplace_back(0.5, Vector3<S>(1.25, 0, 0));
156  spheres.emplace_back(0.3, Vector3<S>(-0.2, 0, 0));
157  spheres.emplace_back(0.4, Vector3<S>(-0.2, 0, 1.1));
158  for (int i = 0; i < static_cast<int>(spheres.size()); ++i) {
159  for (int j = i + 1; j < static_cast<int>(spheres.size()); ++j) {
160  if ((spheres[i].center - spheres[j].center).norm() >
161  spheres[i].radius + spheres[j].radius) {
162  // Not in collision.
163  for (const S solver_tolerance : {S(1e-4), S(1e-5), S(1e-6)}) {
164  const S min_distance_expected =
165  ComputeSphereSphereDistance(spheres[i].radius, spheres[j].radius,
166  spheres[i].center, spheres[j].center);
167  // When the change of distance is below solver_tolerances[k], it does
168  // not mean that the error in separating distance is below
169  // solver_tolerances[k]. Empirically we find the error is less than 10
170  // * solver_tolerances[k], but there is no proof.
171  TestSphereToSphereGJKSignedDistance<S>(
172  spheres[i].radius, spheres[j].radius, spheres[i].center,
173  spheres[j].center, solver_tolerance, 10 * solver_tolerance,
174  min_distance_expected);
175  }
176  } else {
177  GTEST_FAIL() << "The two spheres collide."
178  << "\nSpheres[" << i << "] with radius "
179  << spheres[i].radius << ", centered at "
180  << spheres[i].center.transpose() << "\nSpheres[" << j
181  << "] with radius " << spheres[j].radius
182  << ", centered at " << spheres[j].center.transpose()
183  << "\n";
184  }
185  }
186  }
187 }
188 
189 template <typename S>
191  std::vector<SphereSpecification<S>> spheres;
192  spheres.emplace_back(0.5, Vector3<S>(0, 0, 0));
193  spheres.emplace_back(0.5, Vector3<S>(0.75, 0, 0));
194  spheres.emplace_back(0.3, Vector3<S>(0.2, 0, 0));
195  spheres.emplace_back(0.4, Vector3<S>(0.2, 0, 0.4));
196  for (int i = 0; i < static_cast<int>(spheres.size()); ++i) {
197  for (int j = i + 1; j < static_cast<int>(spheres.size()); ++j) {
198  if ((spheres[i].center - spheres[j].center).norm() <
199  spheres[i].radius + spheres[j].radius) {
200  // colliding
201  const S min_distance_expected =
202  ComputeSphereSphereDistance(spheres[i].radius, spheres[j].radius,
203  spheres[i].center, spheres[j].center);
204  for (const S solver_tolerance : {S(1E-4), S(1E-5), S(1E-6)}) {
205  // For colliding spheres, the solver_tolerance is the bound on the
206  // error relative to the true answer.
207  TestSphereToSphereGJKSignedDistance<S>(
208  spheres[i].radius, spheres[j].radius, spheres[i].center,
209  spheres[j].center, solver_tolerance, solver_tolerance,
210  min_distance_expected);
211  }
212  } else {
213  GTEST_FAIL() << "The two spheres failed to collide."
214  << "\nSpheres[" << i << "] with radius "
215  << spheres[i].radius << ", centered at "
216  << spheres[i].center.transpose() << "\nSpheres[" << j
217  << "] with radius " << spheres[j].radius
218  << ", centered at " << spheres[j].center.transpose()
219  << "\n";
220  }
221  }
222  }
223 }
224 
225 GTEST_TEST(FCL_GJKSignedDistance, sphere_sphere) {
226  TestNonCollidingSphereGJKSignedDistance<double>();
227  TestNonCollidingSphereGJKSignedDistance<float>();
228  TestCollidingSphereGJKSignedDistance<double>();
229  TestCollidingSphereGJKSignedDistance<float>();
230 }
231 
232 //----------------------------------------------------------------------------
233 // Box test
234 // Given two boxes, we can perturb the pose of one box so the boxes are
235 // penetrating, touching or separated.
236 template <typename S>
238  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
239  BoxSpecification(const fcl::Vector3<S>& m_size) : size(m_size) {
240  X_FB.setIdentity();
241  }
244 };
245 
246 template <typename S>
247 void TestBoxesInFrameF(const Transform3<S>& X_WF) {
248  const fcl::Vector3<S> box1_size(1, 1, 1);
249  const fcl::Vector3<S> box2_size(0.6, 0.8, 1);
250  // Put the two boxes on the xy plane of frame F.
251  // B1 is the frame rigidly attached to box 1, B2 is the frame rigidly
252  // attached to box 2. W is the world frame. F is a frame fixed to the world.
253  // X_FB1 is the pose of box 1 expressed and measured in frame F, X_FB2 is the
254  // pose of box 2 expressed and measured in frame F.
255  fcl::Transform3<S> X_FB1, X_FB2;
256  // Box 1 is fixed.
257  X_FB1.setIdentity();
258  X_FB1.translation() << 0, 0, 0.5;
259 
260  // First fix the orientation of box 2, such that one of its diagonal (the
261  // one connecting the vertex (0.3, -0.4, 1) and (-0.3, 0.4, 1) is parallel to
262  // the x axis in frame F. If we move the position of box 2, we get different
263  // signed distance.
264  X_FB2.setIdentity();
265  X_FB2.linear() << 0.6, -0.8, 0, 0.8, 0.6, 0, 0, 0, 1;
266 
267  // p_xy_FN1 is the xy position of point N1 (the deepest penetration point on
268  // box 1) measured and expressed in the frame F.
269  // p_xy_FN2 is the xy position of point N2 (the deepest penetration point on
270  // box 2) measured and expressed in the frame F.
271  auto CheckDistance = [&box1_size, &box2_size, &X_FB1, &X_WF](
272  const Transform3<S>& X_FB2, S distance_expected,
273  const Vector2<S>& p_xy_FN1_expected, const Vector2<S>& p_xy_FN2_expected,
274  S solver_distance_tolerance, S test_distance_tolerance,
275  S test_witness_tolerance) {
276  const fcl::Transform3<S> X_WB1 = X_WF * X_FB1;
277  const fcl::Transform3<S> X_WB2 = X_WF * X_FB2;
278  fcl::Box<S> box1(box1_size);
279  fcl::Box<S> box2(box2_size);
280  void* o1 = GJKInitializer<S, fcl::Box<S>>::createGJKObject(box1, X_WB1);
281  void* o2 = GJKInitializer<S, fcl::Box<S>>::createGJKObject(box2, X_WB2);
282  S dist;
283  Vector3<S> p_WN1, p_WN2;
284  GJKSolver_libccd<S> gjkSolver;
285  gjkSolver.distance_tolerance = solver_distance_tolerance;
286  bool res = GJKSignedDistance(
287  o1, detail::GJKInitializer<S, Box<S>>::getSupportFunction(), o2,
288  detail::GJKInitializer<S, Box<S>>::getSupportFunction(),
289  gjkSolver.max_distance_iterations, gjkSolver.distance_tolerance, &dist,
290  &p_WN1, &p_WN2);
291 
292  // It is unclear how FCL handles touching contact. It could return either
293  // true or false for touching contact. So, we ignore the condition where
294  // expected distance is zero.
295  if (distance_expected < 0) {
296  EXPECT_FALSE(res);
297  } else if (distance_expected > 0) {
298  EXPECT_TRUE(res);
299  }
300 
301  EXPECT_NEAR(dist, distance_expected, test_distance_tolerance);
302  const Vector3<S> p_FN1 =
303  X_WF.linear().transpose() * (p_WN1 - X_WF.translation());
304  const Vector3<S> p_FN2 =
305  X_WF.linear().transpose() * (p_WN2 - X_WF.translation());
306 
307  EXPECT_TRUE(p_FN1.template head<2>().isApprox(p_xy_FN1_expected,
308  test_witness_tolerance));
309  EXPECT_TRUE(p_FN2.template head<2>().isApprox(p_xy_FN2_expected,
310  test_witness_tolerance));
311  // The z height of the closest points should be the same.
312  EXPECT_NEAR(p_FN1(2), p_FN2(2), test_witness_tolerance);
313  // The closest point is within object A/B, so the z height should be
314  // within [0, 1]
315  EXPECT_GE(p_FN1(2), 0);
316  EXPECT_GE(p_FN2(2), 0);
317  EXPECT_LE(p_FN1(2), 1);
318  EXPECT_LE(p_FN2(2), 1);
319 
320  GJKInitializer<S, fcl::Sphere<S>>::deleteGJKObject(o1);
321  GJKInitializer<S, fcl::Sphere<S>>::deleteGJKObject(o2);
322  };
323 
324  auto CheckBoxEdgeBoxFaceDistance = [&CheckDistance](
325  const Transform3<S>& X_FB2, S solver_distance_tolerance,
326  S test_distance_tolerance, S test_witness_tolerance) {
327  const double expected_distance = -X_FB2.translation()(0) - 1;
328  CheckDistance(
329  X_FB2, expected_distance, Vector2<S>(-0.5, X_FB2.translation()(1)),
330  Vector2<S>(X_FB2.translation()(0) + 0.5, X_FB2.translation()(1)),
331  solver_distance_tolerance, test_distance_tolerance,
332  test_witness_tolerance);
333  };
334 
335  //---------------------------------------------------------------
336  // Touching contact
337  // Test with different solver distance tolerances.
338  std::vector<S> solver_tolerances = {S(1E-4), fcl::constants<S>::eps_12(),
341  for (int i = 0; i < static_cast<int>(solver_tolerances.size()); ++i) {
342  const S solver_distance_tolerance = solver_tolerances[i];
343  // For touching contact, FCL might call either GJK or EPA algorithm. When it
344  // calls GJK algorithm, there is no theoretical guarantee, on how the
345  // distance error is related to solver's distance_tolerance.
346  // Empirically we find 10x is reasonable.
347  const S test_distance_tolerance = 10 * solver_distance_tolerance;
348  const S test_witness_tolerance = test_distance_tolerance;
349  // An edge of box 2 is touching a face of box 1
350  X_FB2.translation() << -1, 0, 0.5;
351  CheckBoxEdgeBoxFaceDistance(X_FB2, solver_distance_tolerance,
352  test_distance_tolerance,
353  test_witness_tolerance);
354 
355  // The touching face on box 1 is parallel to the y axis, so shifting box 2
356  // on y axis still gives touching contact. Shift box 2 on y axis by 0.1m.
357  X_FB2.translation() << -1, 0.1, 0.5;
358  CheckBoxEdgeBoxFaceDistance(X_FB2, solver_distance_tolerance,
359  test_distance_tolerance,
360  test_witness_tolerance);
361 
362  // Shift box 2 on y axis by -0.1m.
363  X_FB2.translation() << -1, -0.1, 0.5;
364  CheckBoxEdgeBoxFaceDistance(X_FB2, solver_distance_tolerance,
365  test_distance_tolerance,
366  test_witness_tolerance);
367  // TODO(hongkai.dai@tri.global): Add other touching contact cases, including
368  // face-face, face-vertex, edge-edge, edge-vertex and vertex-vertex.
369  }
370 
371  //--------------------------------------------------------------
372  // Penetrating contact
373  // An edge of box 2 penetrates into a face of box 1
374  for (int i = 0; i < static_cast<int>(solver_tolerances.size()); ++i) {
375  const S solver_distance_tolerance = solver_tolerances[i];
376  // For penetrating contact, FCL calls EPA algorithm. When the solver
377  // terminates, the computed distance should be within
378  // solver.distance_tolerance to the actual distance.
379  const S test_distance_tolerance = solver_distance_tolerance;
380  const S test_witness_tolerance = test_distance_tolerance;
381 
382  X_FB2.translation() << -0.9, 0, 0.5;
383  CheckBoxEdgeBoxFaceDistance(X_FB2, solver_distance_tolerance,
384  test_distance_tolerance,
385  test_witness_tolerance);
386 
387  // Shift box 2 on y axis by 0.1m.
388  X_FB2.translation() << -0.9, 0.1, 0.5;
389  CheckBoxEdgeBoxFaceDistance(X_FB2, solver_distance_tolerance,
390  test_distance_tolerance,
391  test_witness_tolerance);
392 
393  // Shift box 2 on y axis by -0.05m.
394  X_FB2.translation() << -0.9, -0.05, 0.5;
395  CheckBoxEdgeBoxFaceDistance(X_FB2, solver_distance_tolerance,
396  test_distance_tolerance,
397  test_witness_tolerance);
398 
399  // Shift box 2 on y axis by -0.1m.
400  X_FB2.translation() << -0.9, -0.1, 0.5;
401  CheckBoxEdgeBoxFaceDistance(X_FB2, solver_distance_tolerance,
402  test_distance_tolerance,
403  test_witness_tolerance);
404  }
405 }
406 
407 template <typename S>
408 void TestBoxes() {
409  // Frame F coincides with the world frame W.
410  Transform3<S> X_WF;
411  X_WF.setIdentity();
412  TestBoxesInFrameF(X_WF);
413 
414  // Frame F is shifted from the world frame W.
415  X_WF.translation() << 0, 0, 1;
416  TestBoxesInFrameF(X_WF);
417 
418  X_WF.translation() << 0, 1, 0;
419  TestBoxesInFrameF(X_WF);
420 
421  X_WF.translation() << 1, 0, 0;
422  TestBoxesInFrameF(X_WF);
423 
424  // Frame F is rotated from the world frame W.
425  X_WF.setIdentity();
426  const S kPi = fcl::constants<S>::pi();
427  X_WF.linear() =
428  Eigen::AngleAxis<S>(0.1 * kPi, Vector3<S>::UnitZ()).toRotationMatrix();
429  TestBoxesInFrameF(X_WF);
430 
431  // TODO(hongkai.dai): This test exposes an error in simplexToPolytope4, that
432  // the initial simplex can be degenerate. Should add the special case on
433  // degenerate simplex in simplexToPolytope4.
434  /*X_WF.translation() << 0, 0, 0;
435  X_WF.linear() =
436  Eigen::AngleAxis<S>(0.1 * kPi, Vector3<S>(1.0 / 3, 2.0 / 3, -2.0 / 3))
437  .toRotationMatrix();
438  TestBoxesInFrameF(X_WF);*/
439 
440  // Frame F is rotated and shifted from the world frame W.
441  X_WF.translation() << 0.1, 0.2, 0.3;
442  TestBoxesInFrameF(X_WF);
443 }
444 
445 GTEST_TEST(FCL_GJKSignedDistance, box_box) {
446  TestBoxes<double>();
447  TestBoxes<float>();
448 }
449 } // namespace detail
450 } // namespace fcl
451 
452 //==============================================================================
453 int main(int argc, char* argv[]) {
454  ::testing::InitGoogleTest(&argc, argv);
455  return RUN_ALL_TESTS();
456 }
fcl::detail::SphereSpecification
Definition: test_gjk_libccd-inl_signed_distance.cpp:144
EXPECT_GE
#define EXPECT_GE(val1, val2)
Definition: gtest.h:1987
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::constants::eps_34
static Real eps_34()
Returns ε^(3/4) for the precision of the underlying scalar type.
Definition: constants.h:170
fcl::detail::BoxSpecification
Definition: test_gjk_libccd-inl_signed_distance.cpp:237
EXPECT_LE
#define EXPECT_LE(val1, val2)
Definition: gtest.h:1983
fcl::detail::GJKSolver_libccd
collision and distance solver based on libccd library.
Definition: gjk_solver_libccd.h:54
gtest.h
fcl::constants::eps_78
static Real eps_78()
Returns ε^(7/8) for the precision of the underlying scalar type.
Definition: constants.h:164
fcl::detail::TestSphereToSphereGJKSignedDistance
void TestSphereToSphereGJKSignedDistance(S radius1, S radius2, const Vector3< S > &p_F1, const Vector3< S > &p_F2, S solver_tolerance, S test_tol, S min_distance_expected)
Definition: test_gjk_libccd-inl_signed_distance.cpp:64
EXPECT_NEAR
#define EXPECT_NEAR(val1, val2, abs_error)
Definition: gtest.h:2098
fcl::detail::BoxSpecification::BoxSpecification
EIGEN_MAKE_ALIGNED_OPERATOR_NEW BoxSpecification(const fcl::Vector3< S > &m_size)
Definition: test_gjk_libccd-inl_signed_distance.cpp:239
fcl::detail::SphereSpecification::radius
S radius
Definition: test_gjk_libccd-inl_signed_distance.cpp:147
fcl::detail::GJKInitializer
initialize GJK stuffs
Definition: gjk_libccd.h:76
EXPECT_TRUE
#define EXPECT_TRUE(condition)
Definition: gtest.h:1914
fcl::detail::TestBoxesInFrameF
void TestBoxesInFrameF(const Transform3< S > &X_WF)
Definition: test_gjk_libccd-inl_signed_distance.cpp:247
fcl::Vector2
Eigen::Matrix< S, 2, 1 > Vector2
Definition: types.h:67
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
EXPECT_EQ
#define EXPECT_EQ(expected, actual)
Definition: gtest.h:1977
GTEST_FAIL
#define GTEST_FAIL()
Definition: gtest.h:1872
gjk_solver_libccd.h
fcl::detail::ComputeSphereSphereDistance
S ComputeSphereSphereDistance(S radius1, S radius2, const Vector3< S > &p_F1, const Vector3< S > &p_F2)
Definition: test_gjk_libccd-inl_signed_distance.cpp:57
fcl::detail::BoxSpecification::X_FB
fcl::Transform3< S > X_FB
Definition: test_gjk_libccd-inl_signed_distance.cpp:243
gjk_libccd-inl.h
EXPECT_FALSE
#define EXPECT_FALSE(condition)
Definition: gtest.h:1917
RUN_ALL_TESTS
int RUN_ALL_TESTS() GTEST_MUST_USE_RESULT_
Definition: gtest.h:2287
fcl::detail::GJKSolver_libccd::distance_tolerance
S distance_tolerance
the threshold used in GJK algorithm to stop distance iteration
Definition: gjk_solver_libccd.h:171
fcl::detail::BoxSpecification::size
fcl::Vector3< S > size
Definition: test_gjk_libccd-inl_signed_distance.cpp:242
testing::InitGoogleTest
GTEST_API_ void InitGoogleTest(int *argc, char **argv)
Definition: gtest.cc:5008
fcl::detail::GTEST_TEST
GTEST_TEST(FCL_GJK_EPA, faceNormalPointingOutward)
Definition: test_gjk_libccd-inl_epa.cpp:205
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:50
fcl::Sphere
Center at zero point sphere.
Definition: sphere.h:50
fcl::constants::pi
static constexpr S pi()
The mathematical constant pi.
Definition: constants.h:134
main
int main(int argc, char *argv[])
Definition: test_gjk_libccd-inl_signed_distance.cpp:453
fcl::constants::eps_12
static Real eps_12()
Returns ε^(1/2) for the precision of the underlying scalar type.
Definition: constants.h:176
fcl::constants
Definition: constants.h:129
expected_distance
S expected_distance
Definition: test_sphere_box.cpp:184
fcl::detail::GJKSignedDistance
template bool GJKSignedDistance(void *obj1, ccd_support_fn supp1, void *obj2, ccd_support_fn supp2, unsigned int max_iterations, double tolerance, double *dist, Vector3d *p1, Vector3d *p2)
fcl::detail::TestCollidingSphereGJKSignedDistance
void TestCollidingSphereGJKSignedDistance()
Definition: test_gjk_libccd-inl_signed_distance.cpp:190
fcl::detail::GJKSolver_libccd::max_distance_iterations
unsigned int max_distance_iterations
maximum number of iterations used in GJK algorithm for distance
Definition: gjk_solver_libccd.h:165
fcl::detail::TestNonCollidingSphereGJKSignedDistance
void TestNonCollidingSphereGJKSignedDistance()
Definition: test_gjk_libccd-inl_signed_distance.cpp:152
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::detail::SphereSpecification::center
Vector3< S > center
Definition: test_gjk_libccd-inl_signed_distance.cpp:148
fcl::detail::TestBoxes
void TestBoxes()
Definition: test_gjk_libccd-inl_signed_distance.cpp:408


fcl
Author(s):
autogenerated on Fri Apr 2 2021 02:38:01