test_gjk_libccd-inl_extractClosestPoints.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 
38 
39 #include <gtest/gtest.h>
40 #include <Eigen/Dense>
41 
42 #include "fcl/common/types.h"
43 
44 namespace fcl {
45 namespace detail {
46 
48 
49 // Helper functions to work with libccd types -- the ccd_vec3_t is a nuisance.
50 // This allows working with Eigen Vector3d's instead.
51 
52 ccd_vec3_t eigen_to_ccd(const Vector3d& vector) {
53  ccd_vec3_t out;
54  out.v[0] = vector(0);
55  out.v[1] = vector(1);
56  out.v[2] = vector(2);
57  return out;
58 }
59 
60 Vector3d ccd_to_eigen(const ccd_vec3_t& vector) {
61  // TODO(SeanCurtis-TRI): When libccd is *always* double precision, this can
62  // become: `return Vector3d{vector.v};`
63  return Vector3d{vector.v[0], vector.v[1], vector.v[2]};
64 }
65 
66 // Tests that the given vector and libccd vector are the same with in a given
67 // tolerance.
69  const ccd_vec3_t& tested,
70  double tolerance) {
71  if (tolerance < 0) {
72  return ::testing::AssertionFailure() << "Invalid tolerance: "
73  << tolerance;
74  }
75 
76  Vector3d ccd = ccd_to_eigen(tested);
77  Vector3d error = (expected - ccd).cwiseAbs();
78 
79  for (int i = 0; i < 3; ++i) {
80  if (error(i) > tolerance) {
82  << "Values at index " << i
83  << " exceed tolerance; " << expected(i) << " vs "
84  << ccd(i) << ", diff = " << error(i)
85  << ", tolerance = " << tolerance << "\nexpected = "
86  << expected.transpose() << "\ntested = "
87  << ccd.transpose() << "\n|delta| = "
88  << error.transpose();
89  }
90  }
92 }
93 
94 // These are tests on the extractClosestPoints() function (and its underlying
95 // support functions).
96 //
97 // These functions map a simplex and a point of interest on the "surface" of the
98 // simplex into a pair of points. The simplex is defined by support vertices
99 // (ccd_support_t). Each support vertex contains three vectors in R3. They
100 // represent:
101 // 1. A vertex `v` on the boundary of Minkowski difference of two objects:
102 // O₁ ⊕ -O₂. It is, by definition the difference of two vertex positions
103 // on the two objects: `v = v₁ - v₂`.
104 // 2. The vertex `v₁` on O₁ that contributes to the definition of `v`.
105 // 3. The vertex `v₂` on O₂ that contributes to the definition of `v`.
106 //
107 // extractClosestPoints() supports 1-, 2- and 3-simplices.
108 //
109 // The corresponding points are written to output parameters. If the output
110 // parameters are null, the value is not computed. These tests exercise all
111 // permutations of the two output parameters.
112 
113 // Simple wrapper to facilitate working with Eigen primitives in place of libccd
114 // primitives.
115 bool are_coincident(const Vector3d& p, const Vector3d& q) {
117 }
118 
119 // Tests the `are_coincident` function.
120 GTEST_TEST(DegenerateGeometry, CoincidentPoints) {
121  // The coincidence test uses this as the threshold.
122  const ccd_real_t eps = constants<ccd_real_t>::eps();
123  const ccd_real_t almost_eps = eps * 0.75;
124  const ccd_real_t extra_eps = eps * 1.5;
125 
126  Vector3d p{1, 1, 1};
127  Vector3d q{p};
128  // Exact coincidence at unit scale (down to the last bit)
130 
131  // Coincidence within a unit-scaled epsilon.
132  EXPECT_TRUE(are_coincident(p + Vector3d{almost_eps, 0, 0}, q));
133  EXPECT_TRUE(are_coincident(p + Vector3d{0, almost_eps, 0}, q));
134  EXPECT_TRUE(are_coincident(p + Vector3d{0, 0, almost_eps}, q));
135 
136  // Distinct points just outside a unit-scaled epsilon.
137  EXPECT_FALSE(are_coincident(p + Vector3d{extra_eps, 0, 0}, q));
138  EXPECT_FALSE(are_coincident(p + Vector3d{0, extra_eps, 0}, q));
139  EXPECT_FALSE(are_coincident(p + Vector3d{0, 0, extra_eps}, q));
140 
141  // Coincidence within a larger-than-unit-scale scale factor.
142  const double scale = 100;
143  p << scale, scale, scale;
144  q = p;
145  // Exact coincidence at a larger-than-unit-scale (down to the last bit).
147 
148  // Coincidence within a larger-than-unit-scale epsilon.
149  EXPECT_TRUE(are_coincident(p + Vector3d{almost_eps * scale, 0, 0}, q));
150  EXPECT_TRUE(are_coincident(p + Vector3d{0, almost_eps * scale, 0}, q));
151  EXPECT_TRUE(are_coincident(p + Vector3d{0, 0, almost_eps * scale}, q));
152 
153  // Distinct points just outside a larger-than-unit-scaled epsilon.
154  EXPECT_FALSE(are_coincident(p + Vector3d{extra_eps * scale, 0, 0}, q));
155  EXPECT_FALSE(are_coincident(p + Vector3d{0, extra_eps * scale, 0}, q));
156  EXPECT_FALSE(are_coincident(p + Vector3d{0, 0, extra_eps * scale}, q));
157 
158  // Coincidence within a smaller-than-unit-scale scale factor. NOTE: eps
159  // stays at an *absolute* tolerance as the scale gets smaller.
160  p << 0.01, 0.01, 0.01;
161  q = p;
162  // Exact coincidence at a smaller-than-unit-scale (down to the last bit).
164 
165  // Coincidence within a smaller-than-unit-scale epsilon.
166  EXPECT_TRUE(are_coincident(p + Vector3d{almost_eps, 0, 0}, q));
167  EXPECT_TRUE(are_coincident(p + Vector3d{0, almost_eps, 0}, q));
168  EXPECT_TRUE(are_coincident(p + Vector3d{0, 0, almost_eps}, q));
169 
170  // Distinct points just outside a smaller-than-unit-scaled epsilon.
171  EXPECT_FALSE(are_coincident(p + Vector3d{extra_eps, 0, 0}, q));
172  EXPECT_FALSE(are_coincident(p + Vector3d{0, extra_eps, 0}, q));
173  EXPECT_FALSE(are_coincident(p + Vector3d{0, 0, extra_eps}, q));
174 }
175 
176 // Wrapper to allow invocation of triangle_area_is_zero with eigen primitives.
177 bool triangle_area_is_zero(const Vector3d& a, const Vector3d& b,
178  const Vector3d& c) {
180  eigen_to_ccd(b),
181  eigen_to_ccd(c));
182 }
183 
184 // Tests the `triangle_area_is_zero()` function. NOTE: This computation
185 // makes use of `are_coincident()`. Only a single test of coincident
186 // vertices is provided (to exercise the code path). The permutations for how
187 // two points can be considered coincident are done in their own tests.
188 GTEST_TEST(DegenerateGeometry, ZeroAreaTriangle) {
189  using std::asin;
190  Vector3d a{0, 0, 0};
191  Vector3d b{1, 0, 0};
192  Vector3d c{0, 1, 0};
193 
194  // Viable triangle
196 
197  // Triangle with token coincident vertices to make sure it keys on it.
199 
200  // Co-linearity tests.
201 
202  // Obvious co-linearity
203  EXPECT_TRUE(triangle_area_is_zero(a, b, 3 * b));
204 
205  // Test co-linearity
206  // The co-linear threshold is based on the angles of the triangle. If the
207  // triangle has an angle θ, such that |sin(θ)| < ε, then the triangle is
208  // considered to be degenerate. This condition implicitly defines an envelope
209  // around θ. We define θₑ such that sin(θₑ) = ε. The condition |sin(θ)| < ε
210  // implies |θ| < θₑ. So, if the smallest angle is less than θₑ, the triangle
211  // will be considered co-linear. If the smallest angle is larger than θₑ, the
212  // triangle is considered valid.
213  //
214  // The test wants to provide proof as to the boundary of the tolerance
215  // envelope. Therefore, it will construct two triangles. One whose smallest
216  // angle is θₑ - δ and the other triangle's smallest angle is θₑ + δ, for
217  // some suitably small δ.
218  //
219  // The `triangle_area_is_zero()` function has a second failure condition:
220  // coincident points. We'll make sure that the points are all far enough away
221  // from each other that this failure condition won't be triggered.
222  //
223  // The triangle consists of three vertices: A, B, C.
224  // A: [0, 0, 0] (for simplicity).
225  // B: [1, 1, 1] (again, for simplicity).
226  // C: We must define C such that the angle ∠CAB is θ = θₑ ± δ.
227  //
228  // We'll do it by construction:
229  // 1. Pick a vector v perpendicular to [1, 1, 1] (e.g., v = [1, 1, -2]).
230  // 2. C' = B + v̂ * |B|·tan(θ) (with v̂ = v / |v|). This produces a point C'
231  // that:
232  // a) forms a triangle where ∠C`AB < θₑ, if θ < θₑ, but
233  // b) the points B and C' *may* be coincident.
234  // 3. Move the point C' farther away (in the AC' direction). This preserves
235  // the angle, but distances B from C'. So, C ≙ s·C' (for some scalar
236  // s ≠ 1). This trick works because A is the origin and we generally
237  // assume that |C'| >> ε.
238  //
239  // This triangle illustrates the construction (but rotated for ascii art).
240  //
241  // A
242  // /| θ = ∠BAC = ∠BAC'
243  // / |
244  // C'/__|
245  // / / B
246  // / /
247  // //
248  // C
249 
250  const ccd_real_t eps = constants<ccd_real_t>::eps();
251  const ccd_real_t theta_e = asin(eps);
252 
253  a << 0, 0, 0;
254  b << 1, 1, 1;
255  const Vector3d v_hat = Vector3d(1, 1, -2).normalized();
256 
257  // Triangle where θ < θₑ.
258  const ccd_real_t colinear_theta = tan(theta_e * 0.75);
259  Vector3d c_prime = b + (b.norm() * colinear_theta) * v_hat;
260  c = c_prime * 2;
263 
264  // Triangle where θ > θₑ.
265  const ccd_real_t good_tan = tan(theta_e * 1.5);
266  c_prime = b + (b.norm() * good_tan) * v_hat;
267  c = c_prime * 2;
268  // Confirm the test doesn't report false because the points are coincident.
271 }
272 
273 // This class creates a single simplex. It is the Minkowski difference of
274 // one triangles and a vertex.
276  protected:
277  void SetUp() override {
278  // Configure a 3-simplex; "last" is the index of the last valid simplex.
279  // For an n-simplex, last is always n - 1.
280  simplex_.last = 2;
281  for (int i = 0; i < 3; ++i) {
282  const Vector3d minkowski_diff{v0_ - t1_[i]};
283  write_support(minkowski_diff, v0_, t1_[i], &simplex_.ps[i]);
284  }
285  }
286 
287  // Write the three Eigen vector values into a ccd support vector.
288  static void write_support(const Vector3d& minkowski_diff, const Vector3d& v0,
289  const Vector3d& v1, ccd_support_t* support) {
290  support->v = eigen_to_ccd(minkowski_diff);
291  support->v1 = eigen_to_ccd(v0);
292  support->v2 = eigen_to_ccd(v1);
293  }
294 
295  // Performs the common work of evaluating extractClosetPoint() on a
296  // permutation of parameters.
298  const Vector3d& p0_expected,
299  const Vector3d& p1_expected,
300  ccd_vec3_t* closest,
301  const char* message) {
303 
304  const Vector3d& dummy1{-1, -2, -3};
305  const Vector3d& dummy2{-2, -3, -4};
306  ccd_vec3_t p0 = eigen_to_ccd(dummy1);
307  ccd_vec3_t p1 = eigen_to_ccd(dummy2);
308 
309  // Confirm expected solution are not the dummy values.
310  EXPECT_FALSE(are_same(p0_expected, p0, kTolerance));
311  EXPECT_FALSE(are_same(p1_expected, p1, kTolerance));
312 
313  // Test extraction of neither.
314  EXPECT_NO_THROW(extractClosestPoints(simplex, nullptr, nullptr, closest))
315  << message;
316 
317  // Test extraction of p0.
318  EXPECT_NO_THROW(extractClosestPoints(simplex, &p0, nullptr, closest))
319  << message;
320  EXPECT_TRUE(are_same(p0_expected, p0, kTolerance)) << message;
321 
322  // Test extraction of p1.
323  EXPECT_NO_THROW(extractClosestPoints(simplex, nullptr, &p1, closest))
324  << message;
325  EXPECT_TRUE(are_same(p1_expected, p1, kTolerance)) << message;
326 
327  // Test extraction of both.
328  p0 = eigen_to_ccd(dummy1);
329  p1 = eigen_to_ccd(dummy2);
330  EXPECT_NO_THROW(extractClosestPoints(simplex, &p0, &p1, closest))
331  << message;
332  EXPECT_TRUE(are_same(p0_expected, p0, kTolerance)) << message;
333  EXPECT_TRUE(are_same(p1_expected, p1, kTolerance)) << message;
334  }
335 
336  // Perform linear interpolation between points a & b.
337  // @pre 0 <= s <= 1.
338  static Vector3d lerp(const Vector3d& a, const Vector3d& b, double s) {
339  return a * s + b * (1 - s);
340  };
341 
343 
344  // Vertex on object 0.
345  const Vector3d v0_{0.5, 1, 0};
346 
347  // Vertices 0, 1, & 2 for triangle 1.
348  const Vector3d t1_[3] = {{0, 0.25, 0.25}, {1, 0.25, 0.25}, {0.5, 0.8, 1.0}};
349 
350  // TODO(SeanCurtis-TRI): Change this to 1e-15 when the mac libccd
351  // single/double precision has been worked out.
353 };
354 
355 // Test extraction from a 1-simplex support method.
356 TEST_F(ExtractClosestPoint, ExtractFrom1SimplexSupport) {
357  using namespace fcl::detail::libccd_extension;
358  const Vector3d& dummy1{-1, -2, -3};
359  const Vector3d& dummy2{-2, -3, -4};
360  // Set up the single support *point*.
361  ccd_vec3_t p0 = eigen_to_ccd(dummy1);
362  ccd_vec3_t p1 = eigen_to_ccd(dummy2);
363 
364  // Test extraction of neither.
366  extractObjectPointsFromPoint(&simplex_.ps[0], nullptr, nullptr));
367 
368  // Test extraction of p1.
369  EXPECT_NO_THROW(extractObjectPointsFromPoint(&simplex_.ps[0], &p0, nullptr));
370  EXPECT_TRUE(are_same(v0_, p0, kTolerance));
371 
372  // Test extraction of p2.
373  EXPECT_NO_THROW(extractObjectPointsFromPoint(&simplex_.ps[0], nullptr, &p1));
374  EXPECT_TRUE(are_same(t1_[0], p1, kTolerance));
375 
376  // Test extraction of both.
377  p0 = eigen_to_ccd(dummy1);
378  p1 = eigen_to_ccd(dummy2);
379  extractObjectPointsFromPoint(&simplex_.ps[0], &p0, &p1);
380  EXPECT_TRUE(are_same(v0_, p0, kTolerance));
381  EXPECT_TRUE(are_same(t1_[0], p1, kTolerance));
382 }
383 
384 // Test extraction from a 1-simplex through the extractClosestPoints() method.
385 TEST_F(ExtractClosestPoint, ExtractFrom1Simplex) {
386  simplex_.last = 0;
387 
388  // NOTE: For a 1-simplex, the closest point isn't used.
389  ccd_vec3_t closest = eigen_to_ccd({0, 0, 0});
390 
391  EvaluateExtractClosestPoint(&simplex_, v0_, t1_[0], &closest,
392  "ExtractFrom1Simplex");
393 }
394 
395 // Test extraction from a 2-simplex support method.
396 TEST_F(ExtractClosestPoint, ExtractFrom2SimplexSupport) {
397  using namespace fcl::detail::libccd_extension;
398  const Vector3d& dummy1{-1, -2, -3};
399  const Vector3d& dummy2{-2, -3, -4};
400 
401  ccd_vec3_t p0 = eigen_to_ccd(dummy1);
402  ccd_vec3_t p1 = eigen_to_ccd(dummy2);
403 
404  // The query point we're going to use is a simple linear combination of the
405  // two end points.
406  const Vector3d m0 = ccd_to_eigen(simplex_.ps[0].v);
407  const Vector3d m1 = ccd_to_eigen(simplex_.ps[1].v);
408 
409  const double s = 1 / 3.0;
410  ASSERT_TRUE(s >= 0 && s <= 1);
411 
412  ccd_vec3_t closest = eigen_to_ccd(lerp(m0, m1, s));
413  const Vector3d p0_expected = v0_;
414  const Vector3d p1_expected = lerp(t1_[0], t1_[1], s);
415 
416  // Test extraction of neither.
418  &simplex_.ps[0], &simplex_.ps[1], nullptr, nullptr, &closest));
419 
420  // Test extraction of p1.
422  &simplex_.ps[0], &simplex_.ps[1], &p0, nullptr, &closest));
423  EXPECT_TRUE(are_same(p0_expected, p0, kTolerance));
424 
425  // Test extraction of p2.
427  &simplex_.ps[0], &simplex_.ps[1], nullptr, &p1, &closest));
428  EXPECT_TRUE(are_same(p1_expected, p1, kTolerance));
429 
430  // Test extraction of both.
431  p0 = eigen_to_ccd(dummy1);
432  p1 = eigen_to_ccd(dummy2);
434  &simplex_.ps[0], &simplex_.ps[1], &p0, &p1, &closest));
435  EXPECT_TRUE(are_same(p0_expected, p0, kTolerance));
436  EXPECT_TRUE(are_same(p1_expected, p1, kTolerance));
437 }
438 
439 // Test extraction from a 2-simplex through the extractClosestPoints() method.
440 TEST_F(ExtractClosestPoint, ExtractFrom2Simplex) {
441  simplex_.last = 1;
442 
443  // The query point we're going to use is a simple linear combination of the
444  // two end points.
445  const Vector3d m0 = ccd_to_eigen(simplex_.ps[0].v);
446  const Vector3d m1 = ccd_to_eigen(simplex_.ps[1].v);
447 
448  const double s = 1 / 3.0;
449  ASSERT_TRUE(s >= 0 && s <= 1);
450 
451  ccd_vec3_t closest = eigen_to_ccd(lerp(m0, m1, s));
452  const Vector3d p0_expected = v0_;
453  const Vector3d p1_expected = lerp(t1_[0], t1_[1], s);
454 
455  EvaluateExtractClosestPoint(&simplex_, p0_expected, p1_expected, &closest,
456  "ExtractFrom2Simplex");
457 }
458 
459 // Tests the case where the simplex is a degenerate simplex -- i.e., it is
460 // actually a line segment.
461 TEST_F(ExtractClosestPoint, ExtractFrom2SimplexDegenerate) {
462  simplex_.last = 1;
463  // NOTE: This exercises the knowledge that the coincidence tolerance is eps.
464  const ccd_real_t eps = 0.5 * constants<ccd_real_t>::eps();
465 
466  // Copy the first support vertex into the second support vertex and then
467  // perturb the second a small amount. We add a small amount to the
468  // x-components of the minkowski sum *and* the object1 vertex.
469  ccdSupportCopy(&simplex_.ps[1], &simplex_.ps[0]);
470  simplex_.ps[1].v.v[0] += eps;
471  simplex_.ps[1].v1.v[0] += eps;
472  // Confirm that the input and expected answer (v0_) match.
473  ASSERT_TRUE(are_same(v0_, simplex_.ps[0].v1, kTolerance));
474 
475  // The line segment is now of length 1; the answer should be essentially the
476  // same as evaluating for a single point.
477  ccd_vec3_t closest = eigen_to_ccd({0, 0, 0});
478 
479  EvaluateExtractClosestPoint(&simplex_, v0_, t1_[0], &closest,
480  "ExtractFrom2SimplexDegenerate");
481 }
482 
483 // Test extraction from a 3-simplex through the extractClosestPoints() method.
484 // Note: there is no support method for the 3-simplex like there is for the 1-
485 // and 2-simplices.
486 TEST_F(ExtractClosestPoint, ExtractFrom3Simplex) {
487  // Compute a "closest point" based on arbitrary barycentric coordinates.
488  const double alpha = 0.25;
489  const double beta = 0.33;
490  ASSERT_TRUE(alpha >= 0 && alpha <= 1 && beta >= 0 && beta <= 1 &&
491  alpha + beta <= 1);
492 
493  const Vector3d m0 = ccd_to_eigen(simplex_.ps[0].v);
494  const Vector3d m1 = ccd_to_eigen(simplex_.ps[1].v);
495  const Vector3d m2 = ccd_to_eigen(simplex_.ps[2].v);
496 
497  // Interpolate three vertices via barycentric coordinates s1 and s2.
498  auto interpolate = [](const Vector3d& a, const Vector3d& b, const Vector3d& c,
499  double s1, double s2) -> Vector3d {
500  return a * s1 + b * s2 + c * (1 - s1 - s2);
501  };
502 
503  ccd_vec3_t closest = eigen_to_ccd(interpolate(m0, m1, m2, alpha, beta));
504  const Vector3d p0_expected = v0_;
505  const Vector3d p1_expected = interpolate(t1_[0], t1_[1], t1_[2], alpha, beta);
506 
507  EvaluateExtractClosestPoint(&simplex_, p0_expected, p1_expected, &closest,
508  "ExtractFrom3Simplex");
509 }
510 
511 // Tests the case where the 3-simplex is degenerate -- the points are considered
512 // coincident.
513 TEST_F(ExtractClosestPoint, ExtractFrom3SimplesDegenerateCoincident) {
514  // This test essentially reproduces the *valid* result from
515  // ExtractFrom2Simplex(). The difference is that it *claims* to be a triangle.
516 
517  // NOTE: This exercises the knowledge that the coincidence tolerance is eps.
518  const ccd_real_t eps = 0.5 * constants<ccd_real_t>::eps();
519 
520  // Copy the first support vertex into the second and third support vertices
521  // and then perturb the copies a small amount. We add a small amount to the
522  // x-components of the minkowski sum *and* the object1 vertex.
523  for (auto i : {1, 2}) {
524  ccdSupportCopy(&simplex_.ps[i], &simplex_.ps[0]);
525  simplex_.ps[i].v.v[0] += eps;
526  simplex_.ps[i].v1.v[0] += eps;
527  }
528  // Confirm that the input and expected answer (v0_) match.
529  ASSERT_TRUE(are_same(v0_, simplex_.ps[0].v1, kTolerance));
530 
531  // The triangle has zero area because the vertices are all coincident;
532  // the answer should be essentially the same as evaluating for a single point.
533  ccd_vec3_t closest = eigen_to_ccd({0, 0, 0});
534 
535  EvaluateExtractClosestPoint(&simplex_, v0_, t1_[0], &closest,
536  "ExtractFrom3SimplexDegenerateCoincident");
537 }
538 
539 // Tests the case where the 3-simplex is degenerate -- the points are considered
540 // co-linear.
541 TEST_F(ExtractClosestPoint, ExtractFrom3SimplesDegenerateColinear) {
542  // The query point we're going to use is a simple linear combination of the
543  // v0 and v1 (from the minkowski sum). That means the points on the triangle
544  // should likewise be a combination of v0 and v1 from each shape.
545  const Vector3d m0 = ccd_to_eigen(simplex_.ps[0].v);
546  const Vector3d m1 = ccd_to_eigen(simplex_.ps[1].v);
547 
548  const double s = 1 / 3.0;
549  ASSERT_TRUE(s >= 0 && s <= 1);
550 
551  ccd_vec3_t closest = eigen_to_ccd(lerp(m0, m1, s));
552  const Vector3d p0_expected = v0_;
553  const Vector3d p1_expected = lerp(t1_[0], t1_[1], s);
554 
555  // Now set up co-linear configuration. Vertex 2 will simply be a linear
556  // combination of vertex 0 and vertex 1.
557  // v2 = 2 * (v1 - v0) + v0 = 2 * v1 - v0.
558  // Note: this puts v2 on the same line, but not inside the line segment
559  // spanned by v0 and v1. Because the closest point lies on the segment, this
560  // confirms that arbitrarily extending the degeneracy doesn't change the
561  // answer.
562  auto linearly_combine = [](const ccd_vec3_t& a, const ccd_vec3_t& b,
563  ccd_vec3_t* dst) {
564  auto A = ccd_to_eigen(a);
565  auto B = ccd_to_eigen(b);
566  auto C = 2 * B - A;
567  *dst = eigen_to_ccd(C);
568  };
569  linearly_combine(simplex_.ps[0].v, simplex_.ps[1].v, &simplex_.ps[2].v);
570  linearly_combine(simplex_.ps[0].v1, simplex_.ps[1].v1, &simplex_.ps[2].v1);
571  linearly_combine(simplex_.ps[0].v2, simplex_.ps[1].v2, &simplex_.ps[2].v2);
572 
573  EvaluateExtractClosestPoint(&simplex_, p0_expected, p1_expected, &closest,
574  "ExtractFrom3SimplexDegenerateColinear");
575 }
576 
577 } // namespace detail
578 } // namespace fcl
579 
580 //==============================================================================
581 int main(int argc, char* argv[]) {
582  ::testing::InitGoogleTest(&argc, argv);
583  return RUN_ALL_TESTS();
584 }
fcl::detail::libccd_extension::extractObjectPointsFromPoint
static void extractObjectPointsFromPoint(ccd_support_t *q, ccd_vec3_t *p1, ccd_vec3_t *p2)
Definition: gjk_libccd-inl.h:1805
ASSERT_FALSE
#define ASSERT_FALSE(condition)
Definition: gtest.h:1923
testing::AssertionFailure
GTEST_API_ AssertionResult AssertionFailure()
Definition: gtest.cc:976
fcl::detail::ExtractClosestPoint::kTolerance
const double kTolerance
Definition: test_gjk_libccd-inl_extractClosestPoints.cpp:352
fcl::detail::ExtractClosestPoint::v0_
const Vector3d v0_
Definition: test_gjk_libccd-inl_extractClosestPoints.cpp:345
fcl::detail::TEST_F
TEST_F(ExtractClosestPoint, ExtractFrom1SimplexSupport)
Definition: test_gjk_libccd-inl_extractClosestPoints.cpp:356
fcl::detail::libccd_extension::triangle_area_is_zero
static bool triangle_area_is_zero(const ccd_vec3_t &a, const ccd_vec3_t &b, const ccd_vec3_t &c)
Definition: gjk_libccd-inl.h:943
types.h
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
_ccd_support_t
Definition: support.h:27
_ccd_simplex_t::ps
ccd_support_t ps[4]
Definition: simplex.h:44
fcl::constants::eps
static constexpr Real eps()
Returns ε for the precision of the underlying scalar type.
Definition: constants.h:151
_ccd_support_t::v
ccd_vec3_t v
Support point in minkowski sum.
Definition: support.h:43
fcl::detail::are_coincident
bool are_coincident(const Vector3d &p, const Vector3d &q)
Definition: test_gjk_libccd-inl_extractClosestPoints.cpp:115
fcl::detail::eigen_to_ccd
ccd_vec3_t eigen_to_ccd(const Vector3d &vector)
Definition: test_gjk_libccd-inl_extractClosestPoints.cpp:52
fcl::detail::libccd_extension::extractClosestPoints
static void extractClosestPoints(ccd_simplex_t *simplex, ccd_vec3_t *p1, ccd_vec3_t *p2, ccd_vec3_t *p)
Definition: gjk_libccd-inl.h:1894
fcl::detail::ExtractClosestPoint::lerp
static Vector3d lerp(const Vector3d &a, const Vector3d &b, double s)
Definition: test_gjk_libccd-inl_extractClosestPoints.cpp:338
fcl::detail::libccd_extension::are_coincident
static bool are_coincident(const ccd_vec3_t &p, const ccd_vec3_t &q)
Definition: gjk_libccd-inl.h:912
testing::Test
Definition: gtest.h:347
fcl::detail::ExtractClosestPoint::EvaluateExtractClosestPoint
void EvaluateExtractClosestPoint(ccd_simplex_t *simplex, const Vector3d &p0_expected, const Vector3d &p1_expected, ccd_vec3_t *closest, const char *message)
Definition: test_gjk_libccd-inl_extractClosestPoints.cpp:297
EXPECT_TRUE
#define EXPECT_TRUE(condition)
Definition: gtest.h:1914
fcl::detail::are_same
::testing::AssertionResult are_same(const Vector3d &expected, const ccd_vec3_t &tested, double tolerance)
Definition: test_gjk_libccd-inl_extractClosestPoints.cpp:68
_ccd_support_t::v1
ccd_vec3_t v1
Support point in obj1.
Definition: support.h:44
fcl::detail::libccd_extension
Definition: gjk_libccd-inl.h:186
fcl::detail::ExtractClosestPoint
Definition: test_gjk_libccd-inl_extractClosestPoints.cpp:275
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
_ccd_support_t::v2
ccd_vec3_t v2
Support point in obj2.
Definition: support.h:45
fcl::detail::libccd_extension::extractObjectPointsFromSegment
static void extractObjectPointsFromSegment(ccd_support_t *a, ccd_support_t *b, ccd_vec3_t *p1, ccd_vec3_t *p2, ccd_vec3_t *p)
Definition: gjk_libccd-inl.h:1818
EXPECT_NO_THROW
#define EXPECT_NO_THROW(statement)
Definition: gtest.h:1900
fcl::detail::Vector3d
Vector3< double > Vector3d
Definition: test_gjk_libccd-inl_extractClosestPoints.cpp:47
gjk_libccd-inl.h
testing::AssertionResult
Definition: gtest.h:256
_ccd_simplex_t
Definition: simplex.h:28
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
testing::AssertionSuccess
GTEST_API_ AssertionResult AssertionSuccess()
Definition: gtest.cc:971
fcl::detail::ccd_to_eigen
Vector3d ccd_to_eigen(const ccd_vec3_t &vector)
Definition: test_gjk_libccd-inl_extractClosestPoints.cpp:60
ASSERT_TRUE
#define ASSERT_TRUE(condition)
Definition: gtest.h:1920
fcl::detail::ExtractClosestPoint::t1_
const Vector3d t1_[3]
Definition: test_gjk_libccd-inl_extractClosestPoints.cpp:348
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
_ccd_simplex_t::last
int last
index of last added point
Definition: simplex.h:45
ccdSupportCopy
_ccd_inline void ccdSupportCopy(ccd_support_t *, const ccd_support_t *s)
Definition: support.h:46
tolerance
S tolerance()
Definition: test_fcl_geometric_shapes.cpp:87
fcl::detail::ExtractClosestPoint::SetUp
void SetUp() override
Definition: test_gjk_libccd-inl_extractClosestPoints.cpp:277
main
int main(int argc, char *argv[])
Definition: test_gjk_libccd-inl_extractClosestPoints.cpp:581
fcl::detail::ExtractClosestPoint::write_support
static void write_support(const Vector3d &minkowski_diff, const Vector3d &v0, const Vector3d &v1, ccd_support_t *support)
Definition: test_gjk_libccd-inl_extractClosestPoints.cpp:288
fcl::detail::ExtractClosestPoint::simplex_
ccd_simplex_t simplex_
Definition: test_gjk_libccd-inl_extractClosestPoints.cpp:340
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::detail::triangle_area_is_zero
bool triangle_area_is_zero(const Vector3d &a, const Vector3d &b, const Vector3d &c)
Definition: test_gjk_libccd-inl_extractClosestPoints.cpp:177


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