test_sphere_box.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 
37 // Tests the custom sphere-box tests: distance and collision.
38 
40 
41 #include <string>
42 
43 #include <gtest/gtest.h>
44 
45 #include "eigen_matrix_compare.h"
46 #include "fcl/geometry/shape/box.h"
48 
49 namespace fcl {
50 namespace detail {
51 namespace {
52 
53 // In the worst case (with arbitrary frame orientations) it seems like I'm
54 // losing about 4 bits of precision in the solution (compared to performing
55 // the equivalent query without any rotations). This encodes that bit loss to
56 // an epsilon value appropriate to the scalar type.
57 template <typename S>
58 struct Eps {
59  using Real = typename constants<S>::Real;
60  static Real value() { return 18 * constants<S>::eps(); }
61 };
62 
63 // NOTE: The version of Eigen in travis CI seems to be using code that when
64 // evaluating: X_FB.inverse() * X_FS ends up doing the equivalent of multiplying
65 // two 4x4 matrices (rather than exploiting the compact affine representation).
66 // As such, it leaks a slightly more error into the computation and this extra
67 // padding accounts for CI peculiarity.
68 template <>
69 struct Eps<float> {
70  using Real = constants<float>::Real;
71  static Real value() { return 20 * constants<float>::eps(); }
72 };
73 
74 // Utility function for evaluating points inside boxes. Tests various
75 // configurations of points and boxes.
76 template <typename S> void NearestPointInBox() {
77  // Picking sizes that are *not* powers of two and *not* uniform in size.
78  Box<S> box{S(0.6), S(1.2), S(3.6)};
79  Vector3<S> p_BN;
80  Vector3<S> p_BQ;
81 
82  // Case: query point at origin.
83  p_BQ << 0, 0, 0;
84  bool N_is_not_C = nearestPointInBox(box.side, p_BQ, &p_BN);
85  EXPECT_FALSE(N_is_not_C);
87 
88  Vector3<S> half_size = box.side * 0.5;
89  // Per-octant tests:
90  for (S x : {-1, 1}) {
91  for (S y : {-1, 1}) {
92  for (S z : {-1, 1}) {
93  Vector3<S> quadrant{x, y, z};
94  // Case: point inside (no clamped values).
95  p_BQ = quadrant.cwiseProduct(half_size * 0.5);
96  N_is_not_C = nearestPointInBox(box.side, p_BQ, &p_BN);
97  EXPECT_FALSE(N_is_not_C);
100 
101  // For each axis:
102  for (int axis : {0, 1, 2}) {
103  // Case: one direction clamped.
104  Vector3<S> scale{0.5, 0.5, 0.5};
105  scale(axis) = 1.5;
106  p_BQ = quadrant.cwiseProduct(half_size.cwiseProduct(scale));
107  N_is_not_C = nearestPointInBox(box.side, p_BQ, &p_BN);
108  EXPECT_TRUE(N_is_not_C);
109  for (int i : {0, 1, 2}) {
110  if (i == axis)
111  EXPECT_EQ(p_BN(i), quadrant(i) * half_size(i));
112  else
113  EXPECT_EQ(p_BN(i), p_BQ(i));
114  }
115 
116  // Case: One direction unclamped.
117  scale << 1.5, 1.5, 1.5;
118  scale(axis) = 0.5;
119  p_BQ = quadrant.cwiseProduct(half_size.cwiseProduct(scale));
120  N_is_not_C = nearestPointInBox(box.side, p_BQ, &p_BN);
121  EXPECT_TRUE(N_is_not_C);
122  for (int i : {0, 1, 2}) {
123  if (i == axis)
124  EXPECT_EQ(p_BN(i), p_BQ(i));
125  else
126  EXPECT_EQ(p_BN(i), quadrant(i) * half_size(i));
127  }
128 
129  // Case: query point on face in axis direction -- unclamped.
130  scale << 0.5, 0.5, 0.5;
131  scale(axis) = 1.0;
132  p_BQ = quadrant.cwiseProduct(half_size.cwiseProduct(scale));
133  N_is_not_C = nearestPointInBox(box.side, p_BQ, &p_BN);
134  EXPECT_FALSE(N_is_not_C);
135  EXPECT_TRUE(
137  }
138 
139  // Case: external point in Voronoi region of corner (all axes clamped).
140  p_BQ = quadrant.cwiseProduct(half_size * 1.5);
141  N_is_not_C = nearestPointInBox(box.side, p_BQ, &p_BN);
142  EXPECT_TRUE(N_is_not_C);
143  for (int i : {0, 1, 2})
144  EXPECT_EQ(p_BN(i), quadrant(i) * half_size(i));
145  }
146  }
147  }
148 }
149 
150 // Defines the test configuration for a single test. It includes the geometry
151 // and the pose of the sphere in the box's frame B. It also includes the
152 // expected answers in that same frame. It does not include those quantities
153 // that vary from test invocation to invocation (e.g., the pose of the box in
154 // the world frame or the *orientation* of the sphere).
155 //
156 // Collision and distance are complementary queries -- two objects in collision
157 // have no defined distance because they are *not* separated and vice versa.
158 // These configurations allow for the test of the complementarity property.
159 template <typename S>
160 struct TestConfiguration {
161  TestConfiguration(const std::string& name_in, const Vector3<S>& half_size_in,
162  S radius, const Vector3<S>& p_BSo_in, bool colliding)
163  : name(name_in), half_size(half_size_in), r(radius), p_BSo(p_BSo_in),
164  expected_colliding(colliding) {}
165 
166  // Descriptive name of the test configuration.
168  // The half size of the axis-aligned, origin-centered box.
169  Vector3<S> half_size;
170  // Radius of the sphere.
171  S r;
172  // Position of the sphere's center in the box frame.
173  Vector3<S> p_BSo;
174 
175  // Indicates if this test configuration is expected to be in collision.
176  bool expected_colliding{false};
177 
178  // Collision values; only valid if expected_colliding is true.
180  Vector3<S> expected_normal;
181  Vector3<S> expected_pos;
182 
183  // Distance values; only valid if expected_colliding is false.
185  // The points on sphere and box, respectively, closest to the others measured
186  // and expressed in the box frame B. Only defined if separated.
187  Vector3<S> expected_p_BSb;
188  Vector3<S> expected_p_BBs;
189 };
190 
191 // Utility for creating a copy of the input configurations and appending more
192 // labels to the configuration name -- aids in debugging.
193 template <typename S>
194 std::vector<TestConfiguration<S>> AppendLabel(
195  const std::vector<TestConfiguration<S>>& configurations,
196  const std::string& label) {
197  std::vector<TestConfiguration<S>> configs;
198  for (const auto& config : configurations) {
199  configs.push_back(config);
200  configs.back().name += " - " + label;
201  }
202  return configs;
203 }
204 
205 // Returns a collection of configurations where sphere and box are uniformly
206 // scaled.
207 template <typename S>
208 std::vector<TestConfiguration<S>> GetUniformConfigurations() {
209  // Common configuration values
210  // Box and sphere dimensions.
211  const S w = 0.6;
212  const S d = 1.2;
213  const S h = 3.6;
214  const S r = 0.7;
215  const Vector3<S> half_size{w / 2, d / 2, h / 2};
216  const bool collides = true;
217 
218  std::vector<TestConfiguration<S>> configurations;
219 
220  {
221  // Case: Completely separated. Nearest point on the +z face.
222  const Vector3<S> p_BS{half_size(0) * S(0.5), half_size(1) * S(0.5),
223  half_size(2) + r * S(1.1)};
224  configurations.emplace_back(
225  "Separated; nearest face +z", half_size, r, p_BS, !collides);
226  // Not colliding --> no collision values.
227  TestConfiguration<S>& config = configurations.back();
228  config.expected_distance = p_BS(2) - half_size(2) - r;
229  config.expected_p_BBs = Vector3<S>{p_BS(0), p_BS(1), half_size(2)};
230  config.expected_p_BSb = Vector3<S>{p_BS(0), p_BS(1), p_BS(2) - r};
231  }
232 
233  {
234  // Case: Sphere completely separated with center in vertex Voronoi region.
235  const Vector3<S> p_BS = half_size + Vector3<S>{r, r, r} * S(1.25);
236  configurations.emplace_back(
237  "Separated; nearest +x, +y, +z corner", half_size, r, p_BS, !collides);
238  // Not colliding --> no collision values.
239  TestConfiguration<S>& config = configurations.back();
240  // position vector from sphere center (S) to nearest point on box (N).
241  const Vector3<S> r_SN = half_size - p_BS;
242  const S len_r_SN = r_SN.norm();
243  config.expected_distance = len_r_SN - r;
244  config.expected_p_BBs = half_size;
245  config.expected_p_BSb = p_BS + r_SN * (r / len_r_SN);
246  }
247 
248  // Case: Intersection with the sphere center *outside* the box.
249  // Subcase: sphere in face voronoi region -- normal should be in face
250  // direction.
251  // Intersects the z+ face with a depth of half the radius and a normal in the
252  // -z direction.
253  {
254  const S target_depth = r * 0.5;
255  const Vector3<S> p_BS{half_size + Vector3<S>{0, 0, r - target_depth}};
256  configurations.emplace_back(
257  "Colliding: center outside, center projects onto +z face", half_size, r,
258  p_BS, collides);
259  TestConfiguration<S>& config = configurations.back();
260  config.expected_depth = target_depth;
261  config.expected_normal = -Vector3<S>::UnitZ();
262  config.expected_pos = Vector3<S>{p_BS(0), p_BS(1), (h - target_depth) / 2};
263 
264  // Colliding; no distance values required.
265  }
266 
267  // Subcase: sphere in vertex Voronoi region -- normal should be in the
268  // direction from sphere center to box corner.
269  {
270  const S target_depth = r * 0.5;
271  const Vector3<S> n_SB_B = Vector3<S>(-1, -2, -3).normalized();
272  const Vector3<S> p_BS = half_size - n_SB_B * (r - target_depth);
273  configurations.emplace_back(
274  "Colliding: center outside, center nearest +x, +y, +z vertex",
275  half_size, r, p_BS, collides);
276  TestConfiguration<S>& config = configurations.back();
277  config.expected_depth = target_depth;
278  config.expected_normal = n_SB_B;
279  config.expected_pos = half_size + n_SB_B * (target_depth * 0.5);
280 
281  // Colliding; no distance values required.
282  }
283 
284  // Case: Intersection with the sphere center *inside* the box. We create six
285  // tests; one for each face of the box. The center will be closest to the
286  // target face. For the target face f, the normal should be in the -fₙ
287  // direction (fₙ = normal of face f), the penetration depth is
288  // radius plus distance to face, and the position is half the penetration
289  // depth from the face in the -fₙ direction.
290  // The distance to the face f will be some value less than the smallest half
291  // size to guarantee no confusion regarding different dimensions.
292  const std::string axis_name[] = {"x", "y", "z"};
293  const S center_inset = half_size.minCoeff() * 0.5;
294  for (int axis = 0; axis < 3; ++axis) {
295  for (int sign : {-1, 1}) {
296  const Vector3<S> dir = sign * Vector3<S>::Unit(axis);
297  const Vector3<S> p_BS = dir * (center_inset - half_size(axis));
298  configurations.emplace_back(
299  "Colliding: center inside, center nearest " +
300  std::string(sign > 0 ? "+" : "-") + axis_name[axis] + " face",
301  half_size, r, p_BS, collides);
302  TestConfiguration<S>& config = configurations.back();
303  config.expected_depth = center_inset + r;
304  config.expected_normal = dir;
305  config.expected_pos = dir * ((r + center_inset) / 2 - half_size(axis));
306 
307  // Colliding; no distance values required.
308  }
309  }
310 
311  // TODO(SeanCurtis-TRI): Consider pushing the point off the face by less than
312  // epsilon.
313 
314  // Sub-case: Sphere center lies on face.
315  {
316  const Vector3<S> p_BS{S(0.), S(0.), half_size(2)};
317  configurations.emplace_back("Sphere center lies on +z face", half_size, r,
318  p_BS, collides);
319  TestConfiguration<S>& config = configurations.back();
320  config.expected_depth = r;
321  config.expected_normal = -Vector3<S>::UnitZ();
322  config.expected_pos << p_BS(0), p_BS(1), p_BS(2) - r / 2;
323  }
324 
325  // Sub-case: Sphere center lies on corner.
326  {
327  // Alias the half_size as the coordinate of the +x, +y, +z corner.
328  const Vector3<S>& p_BS = half_size;
329  configurations.emplace_back("Sphere center lies on +x, +y, +z corner",
330  half_size, r, p_BS, collides);
331  TestConfiguration<S>& config = configurations.back();
332  config.expected_depth = r;
333  // Sphere center is equidistant to three faces (+x, +y, +z). As documented,
334  // in this case, the +x face is selected (by aribtrary priority) and the
335  // normal points *into* that face.
336  config.expected_normal = -Vector3<S>::UnitX();
337  config.expected_pos << p_BS(0) - r / 2, p_BS(1), p_BS(2);
338  }
339 
340  // Case: Sphere and box origins are coincident.
341 
342  // Coincident centers subcase: The box is a cube, so the all directions
343  // produce the same minimum dimension; normal should point in the -x
344  // direction.
345  {
346  configurations.emplace_back(
347  "Sphere and cube origins coincident", Vector3<S>{10, 10, 10}, 5,
348  Vector3<S>::Zero(), collides);
349  TestConfiguration<S>& config = configurations.back();
350  config.expected_depth = 15;
351  config.expected_normal = -Vector3<S>::UnitX();
352  config.expected_pos << 2.5, 0, 0;
353  }
354 
355  // Coincident centers subcase: Box height and depth are equal and smaller than
356  // width; the normal should point in the negative x-direction.
357  {
358  configurations.emplace_back(
359  "Sphere and box coincident - x & z are minimum dimension",
360  Vector3<S>{10, 15, 10}, 5, Vector3<S>::Zero(), collides);
361  TestConfiguration<S>& config = configurations.back();
362  config.expected_depth = 15;
363  config.expected_normal = -Vector3<S>::UnitX();
364  config.expected_pos << 2.5, 0, 0;
365  }
366 
367  // Coincident centers subcase: Box width is the smallest dimension; the normal
368  // should point in the negative x-direction.
369  {
370  configurations.emplace_back(
371  "Sphere and box coincident - x is minimum dimension",
372  Vector3<S>{10, 12, 14}, 5, Vector3<S>::Zero(), collides);
373  TestConfiguration<S>& config = configurations.back();
374  config.expected_depth = 15;
375  config.expected_normal = -Vector3<S>::UnitX();
376  config.expected_pos << 2.5, 0, 0;
377  }
378 
379  // Coincident centers subcase: Box height and depth are equal and smaller than
380  // width; the normal should point in the negative y-direction.
381  {
382  configurations.emplace_back(
383  "Sphere and box coincident - y & z are minimum dimension",
384  Vector3<S>{15, 10, 10}, 5, Vector3<S>::Zero(), collides);
385  TestConfiguration<S>& config = configurations.back();
386  config.expected_depth = 15;
387  config.expected_normal = -Vector3<S>::UnitY();
388  config.expected_pos << 0, 2.5, 0;
389  }
390 
391  // Coincident centers subcase: Box depth is the smallest dimension; the normal
392  // should point in the negative y-direction.
393  {
394  configurations.emplace_back(
395  "Sphere and box coincident - y is minimum dimension",
396  Vector3<S>{15, 10, 14}, 5, Vector3<S>::Zero(), collides);
397  TestConfiguration<S>& config = configurations.back();
398  config.expected_depth = 15;
399  config.expected_normal = -Vector3<S>::UnitY();
400  config.expected_pos << 0, 2.5, 0;
401  }
402 
403  // Coincident centers subcase: Box height is the smallest dimension; the
404  // normal should point in the negative z-direction.
405  {
406  configurations.emplace_back(
407  "Sphere and box coincident - z is minimum dimension",
408  Vector3<S>{15, 12, 10}, 5, Vector3<S>::Zero(), collides);
409  TestConfiguration<S>& config = configurations.back();
410  config.expected_depth = 15;
411  config.expected_normal = -Vector3<S>::UnitZ();
412  config.expected_pos << 0, 0, 2.5;
413  }
414 
415  return configurations;
416 }
417 
418 // Returns a collection of configurations where sphere and box are scaled
419 // very differently.
420 template <typename S>
421 std::vector<TestConfiguration<S>> GetNonUniformConfigurations() {
422  std::vector<TestConfiguration<S>> configurations;
423 
424  {
425  // Case: long "skinny" box and tiny sphere. Nearest feature is the +z face.
426  const Vector3<S> half_size(15, 1, 1);
427  const S r = 0.01;
428  {
429  // Subcase: colliding.
430  const Vector3<S> p_BS{half_size(0) * S(0.95), S(0.),
431  half_size(2) + r * S(0.5)};
432  configurations.emplace_back("Long, skinny box collides with small sphere",
433  half_size, r, p_BS, true /* colliding */);
434  TestConfiguration<S>& config = configurations.back();
435  config.expected_normal = -Vector3<S>::UnitZ();
436  config.expected_depth = r - (p_BS(2) - half_size(2));
437  config.expected_pos =
438  Vector3<S>{p_BS(0), p_BS(1),
439  half_size(2) - config.expected_depth / 2};
440  }
441  {
442  // Subcase: not-colliding.
443  const S distance = r * 0.1;
444  const Vector3<S> p_BS{half_size(0) * S(0.95), S(0.),
445  half_size(2) + r + distance};
446  configurations.emplace_back(
447  "Long, skinny box *not* colliding with small sphere", half_size,
448  r, p_BS, false /* not colliding */);
449  TestConfiguration<S>& config = configurations.back();
450  config.expected_distance = distance;
451  config.expected_p_BSb = p_BS - Vector3<S>{0, 0, r};
452  config.expected_p_BBs << p_BS(0), p_BS(1), half_size(2);
453  }
454  }
455 
456  {
457  // Case: Large sphere collides with small box. Nearest feature is the +x,
458  // +y, +z corner.
459  const Vector3<S> half_size(0.1, 0.15, 0.2);
460  const S r = 10;
461  const Vector3<S> n_SB = Vector3<S>{-1, -2, -3}.normalized();
462  {
463  // Subcase: colliding.
464  S target_depth = half_size.minCoeff() * 0.5;
465  const Vector3<S> p_BS = half_size - n_SB * (r - target_depth);
466  configurations.emplace_back("Large sphere colliding with tiny box",
467  half_size, r, p_BS, true /* colliding */);
468  TestConfiguration<S>& config = configurations.back();
469  config.expected_normal = n_SB;
470  config.expected_depth = target_depth;
471  config.expected_pos = half_size + n_SB * (target_depth * 0.5);
472  }
473  {
474  // Subcase: not colliding.
475  S distance = half_size.minCoeff() * 0.1;
476  const Vector3<S> p_BS = half_size - n_SB * (r + distance);
477  configurations.emplace_back(
478  "Large sphere *not* colliding with tiny box", half_size,
479  r, p_BS, false /* not colliding */);
480  TestConfiguration<S>& config = configurations.back();
481  config.expected_distance = distance;
482  config.expected_p_BSb = p_BS + n_SB * r;
483  config.expected_p_BBs = half_size;
484  }
485  }
486 
487  return configurations;
488 }
489 
490 template <typename S>
491 using EvalFunc =
492  std::function<void(const TestConfiguration<S> &, const Transform3<S> &,
493  const Matrix3<S> &, S)>;
494 
495 // This evaluates an instance of a test configuration and confirms the results
496 // match the expected data. The test configuration is defined in the box's
497 // frame with an unrotated sphere. The parameters provide the test
498 // configuration, an pose of the box's frame in the world frame.
499 //
500 // Evaluates the collision query twice. Once as the boolean "is colliding" test
501 // and once with the collision characterized with depth, normal, and position.
502 template <typename S>
503 void EvalCollisionForTestConfiguration(const TestConfiguration<S>& config,
504  const Transform3<S>& X_WB,
505  const Matrix3<S>& R_SB,
506  S eps) {
507  // Set up the experiment from input parameters and test configuration.
508  Box<S> box{config.half_size * 2};
509  Sphere<S> sphere{config.r};
510  Transform3<S> X_BS = Transform3<S>::Identity();
511  X_BS.translation() = config.p_BSo;
512  X_BS.linear() = R_SB;
513  Transform3<S> X_WS = X_WB * X_BS;
514 
515  bool colliding = sphereBoxIntersect<S>(sphere, X_WS, box, X_WB, nullptr);
516  EXPECT_EQ(colliding, config.expected_colliding) << config.name;
517 
518  std::vector<ContactPoint<S>> contacts;
519  colliding = sphereBoxIntersect<S>(sphere, X_WS, box, X_WB, &contacts);
520  EXPECT_EQ(colliding, config.expected_colliding) << config.name;
521  if (config.expected_colliding) {
522  EXPECT_EQ(contacts.size(), 1u) << config.name;
523  const ContactPoint<S>& contact = contacts[0];
524  EXPECT_NEAR(contact.penetration_depth, config.expected_depth, eps)
525  << config.name;
526  EXPECT_TRUE(CompareMatrices(contact.normal,
527  X_WB.linear() * config.expected_normal, eps,
529  << config.name;
530  EXPECT_TRUE(CompareMatrices(contact.pos, X_WB * config.expected_pos, eps,
532  << config.name;
533  } else {
534  EXPECT_EQ(contacts.size(), 0u) << config.name;
535  }
536 }
537 
538 // This evaluates an instance of a test configuration and confirms the results
539 // match the expected data. The test configuration is defined in the box's
540 // frame with an unrotated sphere. The parameters provide the test
541 // configuration.
542 //
543 // Evaluates the distance query twice. Once as the boolean "is separated" test
544 // and once with the separation characterized with distance and surface points.
545 template <typename S>
546 void EvalDistanceForTestConfiguration(const TestConfiguration<S>& config,
547  const Transform3<S>& X_WB,
548  const Matrix3<S>& R_SB,
549  S eps) {
550  // Set up the experiment from input parameters and test configuration.
551  Box<S> box{config.half_size * 2};
552  Sphere<S> sphere{config.r};
553  Transform3<S> X_BS = Transform3<S>::Identity();
554  X_BS.translation() = config.p_BSo;
555  X_BS.linear() = R_SB;
556  Transform3<S> X_WS = X_WB * X_BS;
557 
558  bool separated = sphereBoxDistance<S>(sphere, X_WS, box, X_WB, nullptr,
559  nullptr, nullptr);
560  EXPECT_NE(separated, config.expected_colliding) << config.name;
561 
562  // Initializing this to -2, to confirm that a non-colliding scenario sets
563  // distance to -1.
564  S distance{-2};
565  Vector3<S> p_WSb{0, 0, 0};
566  Vector3<S> p_WBs{0, 0, 0};
567 
568  separated =
569  sphereBoxDistance<S>(sphere, X_WS, box, X_WB, &distance, &p_WSb, &p_WBs);
570  EXPECT_NE(separated, config.expected_colliding) << config.name;
571  if (!config.expected_colliding) {
572  EXPECT_NEAR(distance, config.expected_distance, eps)
573  << config.name;
575  X_WB * config.expected_p_BSb, eps,
577  << config.name;
579  X_WB * config.expected_p_BBs, eps,
581  << config.name;
582  } else {
583  EXPECT_EQ(distance, S(-1)) << config.name;
584  EXPECT_TRUE(CompareMatrices(p_WSb, Vector3<S>::Zero(), 0,
586  EXPECT_TRUE(CompareMatrices(p_WBs, Vector3<S>::Zero(), 0,
588  }
589 }
590 
591 // This test defines the transforms for performing the single collision test.
592 template <typename S>
593 void QueryWithVaryingWorldFrames(
594  const std::vector<TestConfiguration<S>>& configurations,
595  EvalFunc<S> query_eval, const Matrix3<S>& R_BS = Matrix3<S>::Identity()) {
596  // Evaluate all the configurations with the given box pose in frame F.
597  auto evaluate_all = [&R_BS, query_eval](
598  const std::vector<TestConfiguration<S>>& configs,
599  const Transform3<S>& X_FB) {
600  for (const auto config : configs) {
601  query_eval(config, X_FB, R_BS, Eps<S>::value());
602  }
603  };
604 
605  // Frame F is the box frame.
606  Transform3<S> X_FB = Transform3<S>::Identity();
607  evaluate_all(AppendLabel(configurations, "X_FB = I"), X_FB);
608 
609  // Simple arbitrary translation away from the origin.
610  X_FB.translation() << 1.3, 2.7, 6.5;
611  evaluate_all(AppendLabel(configurations, "X_FB is translation"), X_FB);
612 
613  std::string axis_name[] = {"x", "y", "z"};
614  // 90 degree rotation around each axis.
615  for (int axis = 0; axis < 3; ++axis) {
616  std::string label = "X_FB is 90-degree rotation around " + axis_name[axis];
617  AngleAxis<S> angle_axis{constants<S>::pi() / 2, Vector3<S>::Unit(axis)};
618  X_FB.linear() << angle_axis.matrix();
619  evaluate_all(AppendLabel(configurations, label), X_FB);
620  }
621 
622  // Arbitrary orientation.
623  {
624  AngleAxis<S> angle_axis{constants<S>::pi() / 3,
625  Vector3<S>{1, 2, 3}.normalized()};
626  X_FB.linear() << angle_axis.matrix();
627  evaluate_all(AppendLabel(configurations, "X_FB is arbitrary rotation"),
628  X_FB);
629  }
630 
631  // Near axis aligned.
632  {
633  AngleAxis<S> angle_axis{constants<S>::eps_12(), Vector3<S>::UnitX()};
634  X_FB.linear() << angle_axis.matrix();
635  evaluate_all(AppendLabel(configurations, "X_FB is near identity"),
636  X_FB);
637  }
638 }
639 
640 // Runs all test configurations across multiple poses in the world frame --
641 // changing the orientation of the sphere -- should have no affect on the
642 // results.
643 template <typename S>
644 void QueryWithOrientedSphere(
645  const std::vector<TestConfiguration<S>>& configurations,
646  EvalFunc<S> query_eval) {
647 
648  std::string axis_name[] = {"x", "y", "z"};
649 
650  // 90 degree rotation around each axis.
651  for (int axis = 0; axis < 3; ++axis) {
652  AngleAxis<S> angle_axis{constants<S>::pi() / 2, Vector3<S>::Unit(axis)};
653  std::string label = "sphere rotate 90-degrees around " + axis_name[axis];
654  QueryWithVaryingWorldFrames<S>(AppendLabel(configurations, label),
655  query_eval, angle_axis.matrix());
656  }
657 
658  // Arbitrary orientation.
659  {
660  AngleAxis<S> angle_axis{constants<S>::pi() / 3,
661  Vector3<S>{1, 2, 3}.normalized()};
662  std::string label = "sphere rotated arbitrarily";
663  QueryWithVaryingWorldFrames<S>(AppendLabel(configurations, label),
664  query_eval, angle_axis.matrix());
665  }
666 
667  // Near axis aligned.
668  {
669  AngleAxis<S> angle_axis{constants<S>::eps_12(), Vector3<S>::UnitX()};
670  std::string label = "sphere rotated near axes";
671  QueryWithVaryingWorldFrames<S>(AppendLabel(configurations, label),
672  query_eval, angle_axis.matrix());
673  }
674 }
675 
676 //======================================================================
677 
678 // Tests the helper function that finds the closest point in the box.
679 GTEST_TEST(SphereBoxPrimitiveTest, NearestPointInBox) {
680  NearestPointInBox<float>();
681  NearestPointInBox<double>();
682 }
683 
684 // Evaluates collision on all test configurations across multiple poses in the
685 // world frame - but the sphere rotation is always the identity.
686 GTEST_TEST(SphereBoxPrimitiveTest, CollisionAcrossVaryingWorldFrames) {
687  QueryWithVaryingWorldFrames<float>(GetUniformConfigurations<float>(),
688  EvalCollisionForTestConfiguration<float>);
689  QueryWithVaryingWorldFrames<double>(GetUniformConfigurations<double>(),
690  EvalCollisionForTestConfiguration<double>);
691 }
692 
693 // Evaluates collision on all test configurations across multiple poses in the
694 // world frame - the sphere is rotated arbitrarily.
695 GTEST_TEST(SphereBoxPrimitiveTest, CollisionWithSphereRotations) {
696  QueryWithOrientedSphere<float>(GetUniformConfigurations<float>(),
697  EvalCollisionForTestConfiguration<float>);
698  QueryWithOrientedSphere<double>(GetUniformConfigurations<double>(),
699  EvalCollisionForTestConfiguration<double>);
700 }
701 
702 // Evaluates collision on a small set of configurations where the box and scale
703 // are of radically different scales - evaluation across multiple poses in the
704 // world frame.
705 GTEST_TEST(SphereBoxPrimitiveTest, CollisionIncompatibleScales) {
706  QueryWithVaryingWorldFrames<float>(GetNonUniformConfigurations<float>(),
707  EvalCollisionForTestConfiguration<float>);
708  QueryWithVaryingWorldFrames<double>(
709  GetNonUniformConfigurations<double>(),
710  EvalCollisionForTestConfiguration<double>);
711 }
712 
713 // Evaluates distance on all test configurations across multiple poses in the
714 // world frame - but the sphere rotation is always the identity.
715 GTEST_TEST(SphereBoxPrimitiveTest, DistanceAcrossVaryingWorldFrames) {
716  QueryWithVaryingWorldFrames<float>(GetUniformConfigurations<float>(),
717  EvalDistanceForTestConfiguration<float>);
718  QueryWithVaryingWorldFrames<double>(GetUniformConfigurations<double>(),
719  EvalDistanceForTestConfiguration<double>);
720 }
721 
722 // Evaluates distance on all test configurations across multiple poses in the
723 // world frame - the sphere is rotated arbitrarily.
724 GTEST_TEST(SphereBoxPrimitiveTest, DistanceWithSphereRotations) {
725  QueryWithOrientedSphere<float>(GetUniformConfigurations<float>(),
726  EvalDistanceForTestConfiguration<float>);
727  QueryWithOrientedSphere<double>(GetUniformConfigurations<double>(),
728  EvalDistanceForTestConfiguration<double>);
729 }
730 
731 // Evaluates distance on a small set of configurations where the box and scale
732 // are of radically different scales - evaluation across multiple poses in the
733 // world frame.
734 GTEST_TEST(SphereBoxPrimitiveTest, DistanceIncompatibleScales) {
735  QueryWithVaryingWorldFrames<float>(GetNonUniformConfigurations<float>(),
736  EvalDistanceForTestConfiguration<float>);
737  QueryWithVaryingWorldFrames<double>(GetNonUniformConfigurations<double>(),
738  EvalDistanceForTestConfiguration<double>);
739 }
740 
741 } // namespace
742 } // namespace detail
743 } // namespace fcl
744 
745 //==============================================================================
746 int main(int argc, char *argv[]) {
747  ::testing::InitGoogleTest(&argc, argv);
748  return RUN_ALL_TESTS();
749 }
eigen_matrix_compare.h
gtest.h
sphere.h
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::constants::eps
static constexpr Real eps()
Returns ε for the precision of the underlying scalar type.
Definition: constants.h:151
EXPECT_NEAR
#define EXPECT_NEAR(val1, val2, abs_error)
Definition: gtest.h:2098
EXPECT_NE
#define EXPECT_NE(expected, actual)
Definition: gtest.h:1981
expected_depth
S expected_depth
Definition: test_sphere_box.cpp:179
label
string label
Definition: test_half_space_convex.cpp:143
expected_pos
Vector3< S > expected_pos
Definition: test_sphere_box.cpp:181
EXPECT_TRUE
#define EXPECT_TRUE(condition)
Definition: gtest.h:1914
expected_p_BSb
Vector3< S > expected_p_BSb
Definition: test_sphere_box.cpp:187
testing::internal::string
::std::string string
Definition: gtest-port.h:875
expected_p_BBs
Vector3< S > expected_p_BBs
Definition: test_sphere_box.cpp:188
box.h
EXPECT_EQ
#define EXPECT_EQ(expected, actual)
Definition: gtest.h:1977
name
std::string name
Definition: test_sphere_box.cpp:167
sphere_box-inl.h
expected_colliding
bool expected_colliding
Definition: test_sphere_box.cpp:176
fcl::constants::Real
detail::ScalarTrait< S >::type Real
Definition: constants.h:131
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::MatrixCompareType::absolute
@ absolute
r
S r
Definition: test_sphere_box.cpp:171
fcl::detail::distance
template void distance(DistanceTraversalNodeBase< double > *node, BVHFrontList *front_list, int qsize)
expected_normal
Vector3< S > expected_normal
Definition: test_sphere_box.cpp:180
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
p_BSo
Vector3< S > p_BSo
Definition: test_sphere_box.cpp:173
main
int main(int argc, char *argv[])
Definition: test_sphere_box.cpp:746
half_size
Vector3< S > half_size
Definition: test_sphere_box.cpp:169
fcl::constants::pi
static constexpr S pi()
The mathematical constant pi.
Definition: constants.h:134
fcl::constants::eps_12
static Real eps_12()
Returns ε^(1/2) for the precision of the underlying scalar type.
Definition: constants.h:176
fcl::detail::nearestPointInBox
bool nearestPointInBox(const Vector3< S > &size, const Vector3< S > &p_BQ, Vector3< S > *p_BN_ptr)
Definition: sphere_box-inl.h:73
expected_distance
S expected_distance
Definition: test_sphere_box.cpp:184
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45


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