test_fcl_capsule_capsule.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2020, Toyota Research Institute, Inc.
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 
37 #include <gtest/gtest.h>
38 
39 #include "fcl/math/constants.h"
41 #include "eigen_matrix_compare.h"
42 
43 #include <cmath>
44 using namespace fcl;
45 
46 // Test harness for exercising the closestPtSegmentSegment() function.
47 template <typename S>
48 class SegmentSegmentNearestPtTest : public ::testing::Test {
49  protected:
50  // Calls the closestPtSegmentSegment with the two segments defined by
51  // endpoint pairs (p0, q0) and (p1, q1). The values s_, t_, n0_, and n1_ will
52  // be updated as a result of this call.
53  // Returns the squared distance between the two segments.
54  S ComputeNearestPoint(const Vector3<S>& p0, const Vector3<S>& q0,
55  const Vector3<S>& p1, const Vector3<S>& q1) {
56  return detail::closestPtSegmentSegment(p0, q0, p1, q1, &s_, &t_, &n0_,
57  &n1_);
58  }
59  // s_ and t_ will contain the lengths of the two segments after calling
60  // the nearest point function.
61  S s_;
62  S t_;
63  // n0_ and n1_ will contain the points on the segments nearest each other
64  // after calling the nearest point function.
67 };
68 
69 using ScalarTypes = ::testing::Types<double, float>;
70 
72 
74  using S = TypeParam;
75 
76  // Keep the points near the unit sphere so that the epsilon value doesn't
77  // need to be scaled.
78  const Vector3<S> p0{S(0.25), S(0.5), S(1.0)};
79  const Vector3<S> p1{S(0.6), S(1.0), S(-0.3)};
80 
81  // Case: Exactly zero.
82  {
83  const S squared_dist = this->ComputeNearestPoint(p0, p0, p1, p1);
84  EXPECT_EQ(squared_dist, (p1 - p0).squaredNorm());
85  EXPECT_TRUE(CompareMatrices(this->n0_, p0));
86  EXPECT_TRUE(CompareMatrices(this->n1_, p1));
87  }
88 
89  // We exploit the knowledge that the segment-segment function uses an
90  // epsilon of eps_78()² on *squared distance*. This test will fail if we
91  // change the epsilon in the algorithm and these brackets should move
92  // accordingly.
93  using std::sqrt;
94  const S kEps = constants<S>::eps_78();
95 
96  // Case: Zero to within epsilon.
97  {
98  // Just below the epsilon for squared distance.
99  const S eps = kEps * S(0.99);
100  const Vector3<S> q0(p0(0) + eps, p0(1), p0(2));
101  const Vector3<S> q1(p1(0) + eps, p1(1), p1(2));
102  const S squared_dist = this->ComputeNearestPoint(p0, q0, p1, q1);
103  EXPECT_EQ(squared_dist, (p1 - p0).squaredNorm());
104  EXPECT_TRUE(CompareMatrices(this->n0_, p0));
105  EXPECT_TRUE(CompareMatrices(this->n1_, p1));
106  }
107 
108  // Case: Just outside epsilon length will no longer return the distance
109  // between points p0 and p1. All that we care is that the distance is no
110  // longer *exactly* the distance between p0 and p1. Thus it shows that we've
111  // crossed the algorithm's threshold.
112  {
113  // Just above the epsilon for squared distance.
114  const S eps = kEps * S(1.5);
115  // Make sure the line segments are *not* parallel so we don't exercise any
116  // degenerate cases.
117  const Vector3<S> q0(p0(0) + eps, p0(1), p0(2));
118  const Vector3<S> q1(p1(0), p1(1) + eps, p1(2));
119  const S squared_dist = this->ComputeNearestPoint(p0, q0, p1, q1);
120  EXPECT_NE(squared_dist, (p1 - p0).squaredNorm());
121  }
122 }
123 
124 // Tests the case where the one segment is zero length.
126  using S = TypeParam;
127 
128  // Segment 2 passes through the origin.
129  Vector3<S> p1{S(-1), S(-2), S(-3)};
130  Vector3<S> q1{S(1), S(2), S(3)};
131 
132  // Case: First zero-length segment is nearest p1.
133  {
134  const Vector3<S> p0 = p1 + p1;
135  const S squared_dist = this->ComputeNearestPoint(p0, p0, p1, q1);
136  EXPECT_EQ(squared_dist, (p1 - p0).squaredNorm());
137  EXPECT_TRUE(CompareMatrices(this->n0_, p0));
138  EXPECT_TRUE(CompareMatrices(this->n1_, p1));
139  }
140 
141  // Case: First zero-length segment is nearest q1.
142  {
143  const Vector3<S> p0 = q1 + q1;
144  const S squared_dist = this->ComputeNearestPoint(p0, p0, p1, q1);
145  EXPECT_EQ(squared_dist, (q1 - p0).squaredNorm());
146  EXPECT_TRUE(CompareMatrices(this->n0_, p0));
147  EXPECT_TRUE(CompareMatrices(this->n1_, q1));
148  }
149 
150  // Case: First zero-length segment is nearest interior of second segment.
151  {
152  const Vector3<S> dir1 = q1 - p1;
153  // Create a vector perpendicular to the second segment, closest to the
154  // origin.
155  const Vector3<S> p0(dir1(1), -dir1(0), 0);
156  const S squared_dist = this->ComputeNearestPoint(p0, p0, p1, q1);
157  EXPECT_EQ(squared_dist, p0.squaredNorm());
158  EXPECT_TRUE(CompareMatrices(this->n0_, p0));
159  EXPECT_TRUE(
161  }
162 
163  // Now we want to reverse the roles (such that the second segment has zero
164  // length) but otherwise perform the same tests. So, we'll alias the old
165  // p1, q1 as p0, q0 and then modify p1, q1 to support the equivalent tests.
166 
167  const Vector3<S> p0(p1);
168  const Vector3<S> q0(q1);
169 
170  // Case: Second zero-length segment is nearest p0.
171  {
172  const Vector3<S> p1 = p0 + p0;
173  const S squared_dist = this->ComputeNearestPoint(p0, q0, p1, p1);
174  EXPECT_EQ(squared_dist, (p1 - p0).squaredNorm());
175  EXPECT_TRUE(CompareMatrices(this->n0_, p0));
176  EXPECT_TRUE(CompareMatrices(this->n1_, p1));
177  }
178 
179  // Case: Second zero-length segment is nearest q0.
180  {
181  const Vector3<S> p1 = q0 + q0;
182  const S squared_dist = this->ComputeNearestPoint(p0, q0, p1, p1);
183  EXPECT_EQ(squared_dist, (q0 - p1).squaredNorm());
184  EXPECT_TRUE(CompareMatrices(this->n0_, q0));
185  EXPECT_TRUE(CompareMatrices(this->n1_, p1));
186  }
187 
188  // Case: Second zero-length segment is nearest interior of first segment.
189  {
190  const Vector3<S> dir1 = q0 - p0;
191  // Create a vector perpendicular to the second segment, closest to the
192  // origin.
193  const Vector3<S> p1(dir1(1), -dir1(0), S(0));
194  const S squared_dist = this->ComputeNearestPoint(p0, q0, p1, p1);
195  EXPECT_EQ(squared_dist, p1.squaredNorm());
196  EXPECT_TRUE(CompareMatrices(this->n1_, p1));
197  EXPECT_TRUE(
199  }
200 }
201 
202 // Tests the case where the line segments have length and are _parallel_.
204  using S = TypeParam;
205 
206  const S eps = constants<S>::eps_78();
207 
208  // A line to which all of the segments will be parallel.
209  const Vector3<S> dir = Vector3<S>{S(1), S(2), S(3)}.normalized();
210  // A direction perpendicular to the line given by `dir`.
211  const Vector3<S> dir_perp = Vector3<S>{dir(1), -dir(0), S(0)}.normalized();
212  ASSERT_NEAR(dir.dot(dir_perp), 0, eps);
213 
214  // In the case where there are multiple possible pairs (because the solution
215  // is not unique). While we could directly express these tests based on known
216  // details of the implementation, we decouple the test from those details by
217  // equivalently testing for:
218  // 1. The distance between the pairs should be the expected distance.
219  // 2. We can determine that the nearest point lies in the correct region
220  // of one of the two segments.
221  // Then the implementation can use any policy it chooses to pick one point
222  // from the set and the test won't have to change.
223 
224  // Case: Multiple nearest pairs: co-linear and overlapping.
225  {
226  // Segment 0 is symmetric and passes through the origin.
227  const Vector3<S> p0(-dir);
228  const Vector3<S> q0(dir);
229  // Segment 1 includes the origin and passes through q1 extending a similar
230  // distance beyond.
231  const Vector3<S> p1(Vector3<S>::Zero());
232  const Vector3<S> q1(S(2) * dir);
233  const S squared_dist = this->ComputeNearestPoint(p0, q0, p1, q1);
234 
235  EXPECT_NEAR(squared_dist, S(0), eps);
236 
237  // Test pair: distance between them and that n0 lies between the origin and
238  // q0.
239  EXPECT_NEAR((this->n0_ - this->n1_).norm(), S(0), eps);
240  const S nearest_dist = this->n0_.norm();
241  EXPECT_NEAR(nearest_dist, dir.dot(this->n0_), eps);
242  EXPECT_LE(nearest_dist, q0.norm());
243  }
244 
245  // Case: Multiple nearest pairs: separated parallel lines but with overlapping
246  // projections.
247  {
248  // Same configuration as in the co-linear case, but the second segment is
249  // moved perpendicularly to the first segment.
250  const Vector3<S> p0(-dir);
251  const Vector3<S> q0(dir);
252  const S expected_dist{0.5};
253  const Vector3<S> offset = dir_perp * expected_dist;
254  const Vector3<S> p1 = Vector3<S>::Zero() + offset;
255  const Vector3<S> q1 = S(2) * dir + offset;
256  const S squared_dist = this->ComputeNearestPoint(p0, q0, p1, q1);
257 
258  EXPECT_NEAR(squared_dist, expected_dist * expected_dist, eps);
259  // Test pair: distance between them and n0 lies between origin and q0.
260  EXPECT_NEAR((this->n0_ - this->n1_).norm(), expected_dist, eps);
261  const S nearest_dist = this->n0_.norm();
262  EXPECT_NEAR(nearest_dist, dir.dot(this->n0_), eps);
263  EXPECT_LE(nearest_dist, q0.norm());
264  }
265 
266  // Case: Single solution: shared end point.
267  {
268  const Vector3<S> p0(-dir);
269  const Vector3<S> q0(dir); // also serves as p1.
270  const Vector3<S> q1(S(2) * dir);
271 
272  const S squared_dist = this->ComputeNearestPoint(p0, q0, q0, q1);
273 
274  EXPECT_NEAR(squared_dist, S(0), eps);
275  EXPECT_TRUE(CompareMatrices(this->n0_, q0));
276  EXPECT_TRUE(CompareMatrices(this->n1_, q0));
277  }
278 
279  // Case: Single solution: co-linear but non-overlapping such that (p1, q0)
280  // are the nearest points.
281  {
282  const Vector3<S> p0(-dir);
283  const Vector3<S> q0(dir);
284  const Vector3<S> p1(S(2) * dir);
285  const Vector3<S> q1(S(3) * dir);
286 
287  const S squared_dist = this->ComputeNearestPoint(p0, q0, p1, q1);
288 
289  EXPECT_NEAR(squared_dist, dir.norm(), eps);
290  EXPECT_TRUE(CompareMatrices(this->n0_, q0));
291  EXPECT_TRUE(CompareMatrices(this->n1_, p1));
292  }
293 
294  // Case: Single solution: not co-linear and non-overlapping such that (p1, q0)
295  // are the nearest points.
296  {
297  const Vector3<S> p0(-dir);
298  const Vector3<S> q0(dir);
299  const Vector3<S> p1(S(2) * dir + dir_perp);
300  const Vector3<S> q1(S(3) * dir + dir_perp);
301 
302  const S squared_dist = this->ComputeNearestPoint(p0, q0, p1, q1);
303 
304  EXPECT_NEAR(squared_dist, (p1 - q0).squaredNorm(), eps);
305  EXPECT_TRUE(CompareMatrices(this->n0_, q0));
306  EXPECT_TRUE(CompareMatrices(this->n1_, p1));
307  }
308 
309  // Case: Single solution: co-linear but non-overlapping such that (p0, q1)
310  // are the nearest points.
311  {
312  const Vector3<S> p0(-dir);
313  const Vector3<S> q0(dir);
314  const Vector3<S> p1(S(-3) * dir);
315  const Vector3<S> q1(S(-2) * dir);
316 
317  const S squared_dist = this->ComputeNearestPoint(p0, q0, p1, q1);
318 
319  EXPECT_NEAR(squared_dist, dir.norm(), eps);
320  EXPECT_TRUE(CompareMatrices(this->n0_, p0));
321  EXPECT_TRUE(CompareMatrices(this->n1_, q1));
322  }
323 
324  // Case: Single solution: not co-linear and non-overlapping such that (p0, q1)
325  // are the nearest points.
326  {
327  const Vector3<S> p0(-dir);
328  const Vector3<S> q0(dir);
329  const Vector3<S> p1(S(-3) * dir + dir_perp);
330  const Vector3<S> q1(S(-2) * dir + dir_perp);
331 
332  const S squared_dist = this->ComputeNearestPoint(p0, q0, p1, q1);
333 
334  EXPECT_NEAR(squared_dist, (p0 - q1).squaredNorm(), eps);
335  EXPECT_TRUE(CompareMatrices(this->n0_, p0));
336  EXPECT_TRUE(CompareMatrices(this->n1_, q1));
337  }
338 }
339 
340 // Tests the nominal case where the segments have length and are *not* parallel.
342  using S = TypeParam;
343 
344  const S eps = constants<S>::eps_78();
345 
346  // An arbitrary direction for segment A.
347  const Vector3<S> dir_A = Vector3<S>{S(1), S(2), S(3)}.normalized();
348  // An arbitrary direction for segment B such that it is neither parallel nor
349  // perpendicular to dir_A. We're excluding perpendicular as that represents
350  // the _best_ conditioning for the math and we want to characterize the
351  // accuracy when the numbers _aren't_ that great.
352  const Vector3<S> dir_B = Vector3<S>{S(-2), S(1), S(-0.5)}.normalized();
353  using std::abs;
354  GTEST_ASSERT_GE(abs(dir_A.dot(dir_B)), eps);
355  GTEST_ASSERT_LT(abs(dir_A.dot(dir_B)), S(1) - eps);
356 
357  // A direction perpendicular to both A and B.
358  const Vector3<S> perp = dir_A.cross(dir_B).normalized();
359 
360  // Configure the two segments so that they perfectly intersect at expected_n0.
361  const Vector3<S> expected_n0 = Vector3<S>{S(-0.5), S(1.25), S(0.75)};
362  const Vector3<S> p0 = expected_n0 + dir_A;
363  const Vector3<S> q0 = expected_n0 - S(2) * dir_A;
364  const Vector3<S> p1 = expected_n0 + dir_B;
365  const Vector3<S> q1 = expected_n0 - S(2.5) * dir_B;
366  const S expected_s = 1 / S(3);
367  const S t_expected = 1 / S(3.5);
368 
369  // Case: literally intersecting (intersection point is *not* at the origin).
370  {
371  const S squared_dist = this->ComputeNearestPoint(p0, q0, p1, q1);
372 
373  EXPECT_NEAR(squared_dist, S(0), eps);
374  EXPECT_NEAR(this->s_, expected_s, eps);
375  EXPECT_NEAR(this->t_, t_expected, eps);
376  EXPECT_TRUE(CompareMatrices(this->n0_, expected_n0, eps));
377  EXPECT_TRUE(CompareMatrices(this->n1_, expected_n0, eps));
378  }
379 
380  // Case: nearest points on interior of both. We simply separate the
381  // intersecting lines in a direction perpendicular to segment A; n0 will still
382  // be the same. And s and t will be unchanged.
383  {
384  const S expected_dist{1.25};
385  const Vector3<S> offset = expected_dist * perp;
386  const S squared_dist =
387  this->ComputeNearestPoint(p0, q0, p1 + offset, q1 + offset);
388 
389  EXPECT_NEAR(squared_dist, expected_dist * expected_dist, eps);
390  EXPECT_NEAR(this->s_, expected_s, eps);
391  EXPECT_NEAR(this->t_, t_expected, eps);
392  EXPECT_TRUE(CompareMatrices(this->n0_, expected_n0, eps));
393  EXPECT_TRUE(CompareMatrices(this->n1_, this->n0_ + offset, eps));
394  }
395 
396  // Projects the point Q onto the line segment AB and reports the projection
397  // point N and the parameter u such that N = A + u(B - A)
398  auto project_onto_segment = [](const Vector3<S>& A, const Vector3<S>& B,
399  const Vector3<S>& Q, Vector3<S>* N, S* u) {
400  const Vector3<S> d = B - A;
401  *u = d.dot(Q - A) / d.squaredNorm();
402  *N = A + *u * d;
403  };
404 
405  // Case: p0 nearest to interior of B. We'll take the intersecting
406  // configuration and slide segment B off of the p0 end of segment A, but
407  // because they aren't perpendicular, the point on B that is nearest (and the
408  // value of t), will depend on the distance.
409  {
410  const Vector3<S> offset = S(1.1) * (p0 - expected_n0);
411  const Vector3<S> p1_shift = p1 + offset;
412  const Vector3<S> q1_shift = q1 + offset;
413  Vector3<S> expected_N1;
414  S t_expected;
415  project_onto_segment(p1_shift, q1_shift, p0, &expected_N1, &t_expected);
416  const S expected_squared_dist = (p0 - expected_N1).squaredNorm();
417 
418  // Test p0 against interior of B.
419  S squared_dist =
420  this->ComputeNearestPoint(p0, q0, p1_shift, q1_shift);
421 
422  EXPECT_NEAR(squared_dist, expected_squared_dist, eps);
423  EXPECT_NEAR(this->s_, S(0), eps);
424  EXPECT_NEAR(this->t_, t_expected, eps);
425  EXPECT_TRUE(CompareMatrices(this->n0_, p0, eps));
426  EXPECT_TRUE(CompareMatrices(this->n1_, expected_N1, eps));
427 
428  // Test p1 against interior of A (by swapping parameter arguments). This
429  // requires testing s against expected t, n0 against expected n1, and n1
430  // against p0.
431  squared_dist = this->ComputeNearestPoint(p1_shift, q1_shift, p0, q0);
432 
433  EXPECT_NEAR(squared_dist, expected_squared_dist, eps);
434  EXPECT_NEAR(this->s_, t_expected, eps);
435  EXPECT_NEAR(this->t_, S(0), eps);
436  EXPECT_TRUE(CompareMatrices(this->n0_, expected_N1, eps));
437  EXPECT_TRUE(CompareMatrices(this->n1_, p0, eps));
438  }
439 
440  // Case: q0 nearest to center of B. Now slide B the other direction.
441  {
442  const Vector3<S> offset = S(1.1) * (q0 - expected_n0);
443  const Vector3<S> p1_shift = p1 + offset;
444  const Vector3<S> q1_shift = q1 + offset;
445  Vector3<S> expected_N1;
446  S t_expected;
447  project_onto_segment(p1_shift, q1_shift, q0, &expected_N1, &t_expected);
448  const S expected_squared_dist = (q0 - expected_N1).squaredNorm();
449 
450  // Test q0 against interior of B.
451  S squared_dist =
452  this->ComputeNearestPoint(p0, q0, p1_shift, q1_shift);
453 
454  EXPECT_NEAR(squared_dist, expected_squared_dist, eps);
455  EXPECT_NEAR(this->s_, S(1), eps);
456  EXPECT_NEAR(this->t_, t_expected, eps);
457  EXPECT_TRUE(CompareMatrices(this->n0_, q0, eps));
458  EXPECT_TRUE(CompareMatrices(this->n1_, expected_N1, eps));
459 
460  // Test q1 against interior of A (by swapping parameter arguments). This
461  // requires testing s against expected t, n0 against expected n1, and n1
462  // against q0.
463  squared_dist = this->ComputeNearestPoint(p1_shift, q1_shift, p0, q0);
464 
465  EXPECT_NEAR(squared_dist, expected_squared_dist, eps);
466  EXPECT_NEAR(this->s_, t_expected, eps);
467  EXPECT_NEAR(this->t_, S(1), eps);
468  EXPECT_TRUE(CompareMatrices(this->n0_, expected_N1, eps));
469  EXPECT_TRUE(CompareMatrices(this->n1_, q0, eps));
470  }
471 
472  // Case: p0 nearest end point of segment B (first p1, then q1). However, the
473  // point on B cannot be co-linear with A. We won't reverse these roles
474  // because the problem is already perfectly symmetric.
475  {
476  // Offset is guaranteed to have a positive dot product with dir the
477  // direction along segment A towards p0, but _not_ be parallel (which
478  // prevents co-linearity).
479  const Vector3<S> p0_dir = p0 - expected_n0;
480  const Vector3<S> offset =
481  p0_dir.cwiseProduct(Vector3<S>{S(1.75), S(1.5), S(1.25)});
482  const Vector3<S> p1_shift = p0 + offset;
483  // q1 needs to be positioned such that it's always farther from segment A
484  // than p1. We know the direction from segment A to p1 (offset). If
485  // offset⋅dir_B is positive, then we extend in the dir_B direction,
486  // otherwise the -dir_B direction.
487  const Vector3<S> q1_shift =
488  p1_shift + (offset.dot(dir_B) > S(0) ? dir_B : Vector3<S>{-dir_B});
489 
490  // p0 is nearest p1.
491  S squared_dist = this->ComputeNearestPoint(p0, q0, p1_shift, q1_shift);
492 
493  EXPECT_NEAR(squared_dist, offset.squaredNorm(), eps);
494  EXPECT_NEAR(this->s_, S(0), eps);
495  EXPECT_NEAR(this->t_, S(0), eps);
496  EXPECT_TRUE(CompareMatrices(this->n0_, p0, eps));
497  EXPECT_TRUE(CompareMatrices(this->n1_, p1_shift, eps));
498 
499  // p0 is nearest q1 -- simply reverse p1_shift and q1_shift.
500  squared_dist = this->ComputeNearestPoint(p0, q0, q1_shift, p1_shift);
501 
502  EXPECT_NEAR(squared_dist, offset.squaredNorm(), eps);
503  EXPECT_NEAR(this->s_, S(0), eps);
504  EXPECT_NEAR(this->t_, S(1), eps);
505  EXPECT_TRUE(CompareMatrices(this->n0_, p0, eps));
506  EXPECT_TRUE(CompareMatrices(this->n1_, p1_shift, eps));
507 
508  // q0 is nearest p1 -- reverse p0 and q0.
509  squared_dist = this->ComputeNearestPoint(q0, p0, p1_shift, q1_shift);
510 
511  EXPECT_NEAR(squared_dist, offset.squaredNorm(), eps);
512  EXPECT_NEAR(this->s_, S(1), eps);
513  EXPECT_NEAR(this->t_, S(0), eps);
514  EXPECT_TRUE(CompareMatrices(this->n0_, p0, eps));
515  EXPECT_TRUE(CompareMatrices(this->n1_, p1_shift, eps));
516 
517  // q0 is nearest q1 -- reverse all points.
518  squared_dist = this->ComputeNearestPoint(q0, p0, q1_shift, p1_shift);
519 
520  EXPECT_NEAR(squared_dist, offset.squaredNorm(), eps);
521  EXPECT_NEAR(this->s_, S(1), eps);
522  EXPECT_NEAR(this->t_, S(1), eps);
523  EXPECT_TRUE(CompareMatrices(this->n0_, p0, eps));
524  EXPECT_TRUE(CompareMatrices(this->n1_, p1_shift, eps));
525  }
526 }
527 
528 // Test harness for exercising the capsuleCapsuleDistance() function. Most of
529 // the functionality of this function relies on closestPtSegmentSegment() for
530 // correctness. That has been rigorously tested above. capsuleCapsuleDistance()
531 // simply has the responsibility of creating the correct segments out of
532 // arbitrarily posed capsules and then reporting the distance and nearest
533 // points correctly (to account for capsule radii).
534 template <typename S>
535 class CapsuleCapsuleSegmentTest : public ::testing::Test {
536  public:
537  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
538 
540  : ::testing::Test(), c1_(S(1.5), S(2.5)), c2_(S(2), S(3)) {}
541 
542  protected:
543  // See the note below for the explanation of these four cases.
548  kMultipleOverlap
549  };
550  /* Configure the scene such that neither transform is ever near an identity
551  matrix. But the scenario will essentially be one of:
552 
553  ●●●●●●●●●
554  ● _______ ● ●●●
555  ● ● ● ●
556  ●●●●●●●●● ●●●●●●●●● ○○○ ● ○○○ ●
557  ● _______ ● ●●●●●●●●● ●○ ○●
558  ○○○ ● ○○○ ● ● _○ | ○_ ● ●○ | ○●
559  ○ ○ ●●●●●●●●● ● ○ | ○ ● ●○ | ○●
560  ○ | ○ ○ | ○ ●●●●●●●●● ○●●●○
561  ○ | ○ ○ | ○ ○ | ○ ○ | ○
562  ○ | ○ ○ | ○ ○ ○ ○ ○
563  ○ | ○ ○ | ○ ○○○ ○○○
564  ○ ○ ○ ○
565  ○○○ ○○○
566 
567  Separated Intersecting Single overlap Multiple overlap
568 
569  Separated: A single, well-defined pair of witness points to the
570  positive signed distance.
571  Intersecting: A single, well-defined pair of witness points to the
572  negative signed distance.
573  Single overlap: The capsule center lines intersect so there is no unique
574  witness pair to the negative signed distance.
575  Multiple overlap: The capsule center lines overlap over an interval, so there
576  is no unique witness pair to the negative signed distance.
577 
578  We prevent the transforms from being "boring" by expressing the configuration
579  above in a test frame T, but then evaluate the test in frame F, such that X_FT
580  is arbitrarily ugly.
581  */
582  void Configure(Configuration config) {
583  switch (config) {
584  case kSeparated:
585  ConfigureSeparated();
586  break;
587  case kIntersecting:
588  ConfigureIntersecting();
589  break;
590  case kSingleOverlap:
591  ConfigureSingleOverlap();
592  break;
593  case kMultipleOverlap:
594  ConfigureMultipleOverlap();
595  break;
596  }
597  }
598 
599  ::testing::AssertionResult RunTestConfiguration(Configuration config) {
600  S distance;
601  Vector3<S> p_FW1, p_FW2;
602 
603  this->Configure(config);
604 
605  detail::capsuleCapsuleDistance(this->c1_, this->X_FC1_, this->c2_,
606  this->X_FC2_, &distance, &p_FW1, &p_FW2);
607  const auto eps = constants<S>::eps_78();
608  using std::abs;
609  S error = abs(distance - this->expected_distance_);
610  if (error > eps) {
611  return ::testing::AssertionFailure()
612  << "Error in reported distances"
613  << "\n Actual: " << distance
614  << "\n Expected: " << this->expected_distance_
615  << "\n Error: " << error
616  << "\n Tolerance: " << eps;
617  }
618  auto result1 = CompareMatrices(p_FW1, this->expected_p_FW1_, eps);
619  if (!result1) return result1;
620  return CompareMatrices(p_FW2, this->expected_p_FW2_, eps);
621  }
622 
624  this->ConfigureViableT(0.25);
625  }
626 
628  this->ConfigureViableT(-0.25);
629  }
630 
631  // Configures separated, instance, and single overlap. What they have in
632  // common is that the the geometries form a "T" (which means that between
633  // the *center lines* there will always be a unique witness point pair.
635  const auto pi = constants<S>::pi();
636 
637  // The reported distance cannot be less than -(r1 + r2); that is the deepest
638  // penetration possible.
639  using std::max;
640  expected_distance_ = max(distance, -(c1_.radius + c2_.radius));
641 
643  Transform3<S> X_TC2(AngleAxis<S>(pi / 2, Vector3<S>::UnitY()));
644  X_TC2.translation() << S(0), S(0),
645  c1_.lz / 2 + c1_.radius + c2_.radius + distance;
646 
647  Vector3<S> p_TW1{S(0), S(0), c1_.lz / S(2) + c1_.radius};
648  expected_p_FW1_ = X_FT() * p_TW1;
649  Vector3<S> p_TW2{p_TW1(0), p_TW1(1), p_TW1(2) + distance};
650  expected_p_FW2_ = X_FT() * p_TW2;
651 
652  X_FC1_ = X_FT() * X_TC1;
653  X_FC2_ = X_FT() * X_TC2;
654  }
655 
657  expected_distance_ = -(c1_.radius + c2_.radius);
658 
659  const auto pi = constants<S>::pi();
661  Transform3<S> X_TC2(AngleAxis<S>(pi / S(2), Vector3<S>::UnitY()));
662  // Position C2 halfway up the upper length of C1.
663  X_TC2.translation() << S(0), S(0), c1_.lz / S(4);
664 
665  // The witness points lie on a line parallel with the Ty direction that
666  // passes through the point p_TC2. For each capsule there are *two* points
667  // that lie on that line. This test has been specifically formulated with
668  // knowledge of how the capsuleCapsuleDistance() function is formulated
669  // to select *one* of those two. This allows the final test to simply
670  // compare witness points directly. A change in this special case in the
671  // function under test will cause this configuration to fail.
672  Vector3<S> p_TW1{S(0), c1_.radius, c1_.lz / S(4)};
673  expected_p_FW1_ = X_FT() * p_TW1;
674  Vector3<S> p_TW2{S(0), -c2_.radius, c1_.lz / S(4)};
675  expected_p_FW2_ = X_FT() * p_TW2;
676 
677  X_FC1_ = X_FT() * X_TC1;
678  X_FC2_ = X_FT() * X_TC2;
679  }
680 
682  expected_distance_ = -(c1_.radius + c2_.radius);
683 
686  // Position C2 so that the lower end of its center lines is at the origin
687  // and overlaps with the upper end of C1's center line.
688  X_TC2.translation() << S(0), S(0), c2_.lz / S(2);
689 
690  // When the two center lines are aligned and overlap, there is an infinite
691  // set of witness points. We exploit the knowledge of how the function under
692  // test resolves this to create *specific* witness points. We *know* it will
693  // pick points that line up with the Tx axis. The biggest unknown is where
694  // in the range [0, L1/2] along Tz the witness point is placed. Empirically,
695  // it is shown to be at p0 (the top of C1's center line).
696  Vector3<S> p_TW1{c1_.radius, S(0), c1_.lz / S(2)};
697  expected_p_FW1_ = X_FT() * p_TW1;
698  Vector3<S> p_TW2{-c2_.radius, S(0), c1_.lz / S(2)};
699  expected_p_FW2_ = X_FT() * p_TW2;
700 
701  X_FC1_ = X_FT() * X_TC1;
702  X_FC2_ = X_FT() * X_TC2;
703  }
704 
705  Transform3<S> X_FT() const {
706  // Create some arbitrarily ugly transform; the important bit that there
707  // be no identities (zeros and ones).
709  X_FT_.translation() << S(10.5), S(12.75), S(-2.5);
710  X_FT_.linear() = AngleAxis<S>(constants<S>::pi() / S(7),
711  Vector3<S>{S(1), S(2), S(3)}.normalized())
712  .matrix();
713  return X_FT_;
714  }
715 
720 
721  // The expected values: signed distance and witness points on capsules 1 and
722  // 2, respectively.
726 };
727 
729 
730 // The nominal case will create two unique capsules with a straight forward
731 // relationship. Making sure the transforms are *not* identity will sufficiently
732 // exercise the functions logic. We assume that if we can correctly construct
733 // one pair of line segments, that we'll construct all line segments properly.
734 TYPED_TEST(CapsuleCapsuleSegmentTest, NominalSeparatedCase) {
735  EXPECT_TRUE(this->RunTestConfiguration(
737 }
738 
739 // Similar to the nominal separated case, but the capsules will be intersecting.
740 TYPED_TEST(CapsuleCapsuleSegmentTest, NominalIntersectingCase) {
741  EXPECT_TRUE(this->RunTestConfiguration(
743 }
744 
745 // Tests the case where the capsules are positioned such their center lines
746 // intersect at a single point.
747 TYPED_TEST(CapsuleCapsuleSegmentTest, SingleIntersectionCenterLines) {
748  EXPECT_TRUE(this->RunTestConfiguration(
750 }
751 
752 // Tests the case where the capsules are positioned such that their center lines
753 // overlap.
754 TYPED_TEST(CapsuleCapsuleSegmentTest, OverlappingCenterLines) {
755  EXPECT_TRUE(this->RunTestConfiguration(
757 }
758 
759 //==============================================================================
760 int main(int argc, char* argv[])
761 {
762  ::testing::InitGoogleTest(&argc, argv);
763  return RUN_ALL_TESTS();
764 }
kEps
static const double kEps
Definition: test_gjk_libccd-inl_gjk_doSimplex2.cpp:128
CapsuleCapsuleSegmentTest::c1_
Capsule< S > c1_
Definition: test_fcl_capsule_capsule.cpp:716
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
CapsuleCapsuleSegmentTest::ConfigureIntersecting
void ConfigureIntersecting()
Definition: test_fcl_capsule_capsule.cpp:627
CapsuleCapsuleSegmentTest::RunTestConfiguration
::testing::AssertionResult RunTestConfiguration(Configuration config)
Definition: test_fcl_capsule_capsule.cpp:599
CapsuleCapsuleSegmentTest::expected_p_FW1_
Vector3< S > expected_p_FW1_
Definition: test_fcl_capsule_capsule.cpp:724
capsule_capsule-inl.h
CapsuleCapsuleSegmentTest::ConfigureMultipleOverlap
void ConfigureMultipleOverlap()
Definition: test_fcl_capsule_capsule.cpp:681
eigen_matrix_compare.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::distance
S distance(const Eigen::MatrixBase< DerivedA > &R0, const Eigen::MatrixBase< DerivedB > &T0, const kIOS< S > &b1, const kIOS< S > &b2, Vector3< S > *P, Vector3< S > *Q)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS-inl.h:266
fcl::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
CapsuleCapsuleSegmentTest::ConfigureViableT
void ConfigureViableT(S distance)
Definition: test_fcl_capsule_capsule.cpp:634
CapsuleCapsuleSegmentTest
Definition: test_fcl_capsule_capsule.cpp:535
CapsuleCapsuleSegmentTest::c2_
Capsule< S > c2_
Definition: test_fcl_capsule_capsule.cpp:717
max
static T max(T x, T y)
Definition: svm.cpp:52
TYPED_TEST_SUITE
TYPED_TEST_SUITE(SegmentSegmentNearestPtTest, ScalarTypes)
SegmentSegmentNearestPtTest::n0_
Vector3< S > n0_
Definition: test_fcl_capsule_capsule.cpp:65
EXPECT_TRUE
#define EXPECT_TRUE(args)
CapsuleCapsuleSegmentTest::ConfigureSingleOverlap
void ConfigureSingleOverlap()
Definition: test_fcl_capsule_capsule.cpp:656
SegmentSegmentNearestPtTest::n1_
Vector3< S > n1_
Definition: test_fcl_capsule_capsule.cpp:66
fcl::AngleAxis
Eigen::AngleAxis< S > AngleAxis
Definition: types.h:97
CapsuleCapsuleSegmentTest::ConfigureSeparated
void ConfigureSeparated()
Definition: test_fcl_capsule_capsule.cpp:623
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::detail::closestPtSegmentSegment
template double closestPtSegmentSegment(const Vector3d &p_FP1, const Vector3d &p_FQ1, const Vector3d &p_FP2, const Vector3d &p_FQ2, double *s, double *t, Vector3d *p_FC1, Vector3d *p_FC2)
ScalarTypes
::testing::Types< double, float > ScalarTypes
Definition: test_fcl_capsule_capsule.cpp:69
CapsuleCapsuleSegmentTest::kSeparated
@ kSeparated
Definition: test_fcl_capsule_capsule.cpp:545
CapsuleCapsuleSegmentTest::expected_distance_
S expected_distance_
Definition: test_fcl_capsule_capsule.cpp:723
TYPED_TEST
TYPED_TEST(SegmentSegmentNearestPtTest, BothZeroLength)
Definition: test_fcl_capsule_capsule.cpp:73
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
CapsuleCapsuleSegmentTest::CapsuleCapsuleSegmentTest
EIGEN_MAKE_ALIGNED_OPERATOR_NEW CapsuleCapsuleSegmentTest()
Definition: test_fcl_capsule_capsule.cpp:539
CapsuleCapsuleSegmentTest::kIntersecting
@ kIntersecting
Definition: test_fcl_capsule_capsule.cpp:546
CapsuleCapsuleSegmentTest::X_FT
Transform3< S > X_FT() const
Definition: test_fcl_capsule_capsule.cpp:705
fcl::detail::capsuleCapsuleDistance
template bool capsuleCapsuleDistance(const Capsule< double > &s1, const Transform3< double > &tf1, const Capsule< double > &s2, const Transform3< double > &tf2, double *dist, Vector3d *p1_res, Vector3d *p2_res)
CapsuleCapsuleSegmentTest::expected_p_FW2_
Vector3< S > expected_p_FW2_
Definition: test_fcl_capsule_capsule.cpp:725
SegmentSegmentNearestPtTest::ComputeNearestPoint
S ComputeNearestPoint(const Vector3< S > &p0, const Vector3< S > &q0, const Vector3< S > &p1, const Vector3< S > &q1)
Definition: test_fcl_capsule_capsule.cpp:54
CapsuleCapsuleSegmentTest::Configure
void Configure(Configuration config)
Definition: test_fcl_capsule_capsule.cpp:582
constants.h
fcl::constants::pi
static constexpr S pi()
The mathematical constant pi.
Definition: constants.h:134
CapsuleCapsuleSegmentTest::X_FC2_
Transform3< S > X_FC2_
Definition: test_fcl_capsule_capsule.cpp:719
SegmentSegmentNearestPtTest
Definition: test_fcl_capsule_capsule.cpp:48
fcl::constants
Definition: constants.h:129
SegmentSegmentNearestPtTest::t_
S t_
Definition: test_fcl_capsule_capsule.cpp:62
CapsuleCapsuleSegmentTest::Configuration
Configuration
Definition: test_fcl_capsule_capsule.cpp:544
CapsuleCapsuleSegmentTest::kSingleOverlap
@ kSingleOverlap
Definition: test_fcl_capsule_capsule.cpp:547
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
main
int main(int argc, char *argv[])
Definition: test_fcl_capsule_capsule.cpp:760
EXPECT_EQ
#define EXPECT_EQ(a, b)
CapsuleCapsuleSegmentTest::X_FC1_
Transform3< S > X_FC1_
Definition: test_fcl_capsule_capsule.cpp:718
fcl::Capsule< S >
SegmentSegmentNearestPtTest::s_
S s_
Definition: test_fcl_capsule_capsule.cpp:61


fcl
Author(s):
autogenerated on Tue Dec 5 2023 03:40:49