test_convex.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 implementation of a convex polytope geometry.
38 
40 
41 #include <vector>
42 
43 #include <Eigen/StdVector>
44 #include <gtest/gtest.h>
45 
46 #include "eigen_matrix_compare.h"
47 #include "expect_throws_message.h"
48 #include "fcl/common/types.h"
50 
51 namespace fcl {
52 class ConvexTester {
53  public:
54  template <typename S>
55  static bool find_extreme_via_neighbors(const Convex<S>& convex) {
56  return convex.find_extreme_via_neighbors_;
57  }
58 
59  // Override the built-in logic for disabling find_extreme_via_neighbors.
60  template <typename S>
62  convex->find_extreme_via_neighbors_ = true;
63  }
64 };
65 namespace {
66 
67 using std::max;
68 
69 // Necessary to satisfy Eigen's alignment requirements. See
70 // http://eigen.tuxfamily.org/dox-devel/group__TopicStlContainers.html#StlContainers_vector
71 template <typename S>
72 using PoseVector = std::vector<Transform3<S>,
73  Eigen::aligned_allocator<Transform3<S>>>;
74 
75 // Utilities to print scalar type in error messages.
76 template <typename S>
77 struct ScalarString {
78  static std::string value() { return "unknown"; }
79 };
80 
81 template <>
82 struct ScalarString<double> {
83  static std::string value() { return "double"; }
84 };
85 
86 template <>
87 struct ScalarString<float> {
88  static std::string value() { return "float"; }
89 };
90 
91 // Base definition of a "unit" convex polytope. Specific instances should define
92 // faces, vertices, and quantities such as volume, center of mass, and moment of
93 // inertia in terms of a scale factor.
94 template <typename S>
95 class Polytope {
96  public:
97  explicit Polytope(S scale)
98  : vertices_(std::make_shared<std::vector<Vector3<S>>>()),
99  polygons_(std::make_shared<std::vector<int>>()), scale_(scale) {}
100 
101  Polytope(const Polytope &other)
102  : vertices_(std::make_shared<std::vector<Vector3<S>>>(*(other.vertices_))),
103  polygons_(std::make_shared<std::vector<int>>(*(other.polygons_))),
104  scale_(other.scale_) {}
105 
106  virtual int face_count() const = 0;
107  virtual int vertex_count() const = 0;
108  virtual S volume() const { throw std::logic_error("Not implemented yet"); }
109  virtual Vector3<S> com() const {
110  throw std::logic_error("Not implemented yet");
111  }
112  virtual Matrix3<S> principal_inertia_tensor() const {
113  throw std::logic_error("Not implemented yet");
114  }
115  virtual std::string description() const {
116  throw std::logic_error("Not implemented yet");
117  }
118 
119  // The scale of the polytope to use with test tolerances.
120  S scale() const { return scale_; }
121  std::shared_ptr<const std::vector<Vector3<S>>> points() const {
122  return vertices_;
123  }
124  std::shared_ptr<const std::vector<int>> polygons() const {
125  return polygons_;
126  }
127  Convex<S> MakeConvex(bool throw_if_invalid = true) const {
128  return Convex<S>(points(), face_count(), polygons(), throw_if_invalid);
129  }
130  Vector3<S> min_point() const {
131  Vector3<S> m;
132  m.setConstant(std::numeric_limits<S>::max());
133  for (const auto& v : *vertices_) {
134  for (int i = 0; i < 3; ++i) {
135  if (v(i) < m(i)) m(i) = v(i);
136  }
137  }
138  return m;
139  }
140  Vector3<S> max_point() const {
141  Vector3<S> m;
142  m.setConstant(-std::numeric_limits<S>::max());
143  for (const auto& v : *vertices_) {
144  for (int i = 0; i < 3; ++i) {
145  if (v(i) > m(i)) m(i) = v(i);
146  }
147  }
148  return m;
149  }
150  Vector3<S> aabb_center() const {
151  return (max_point() + min_point()) / 2;
152  }
153  S aabb_radius() const { return (min_point() - aabb_center()).norm(); }
154  void SetPose(const Transform3<S>& X_WP) {
155  for (auto& v : *vertices_) {
156  v = X_WP * v;
157  }
158  }
159 
160  protected:
161  void add_vertex(const Vector3<S>& vertex) { vertices_->push_back(vertex); }
162  void add_face(std::initializer_list<int> indices) {
163  polygons_->push_back(static_cast<int>(indices.size()));
164  polygons_->insert(polygons_->end(), indices);
165  }
166  // Confirms the number of vertices and number of polygons matches the counts
167  // implied by vertex_count() and face_count(), respectively.
168  void confirm_data() {
169  // Confirm point count.
170  GTEST_ASSERT_EQ(vertex_count(), static_cast<int>(vertices_->size()));
171 
172  // Confirm face count.
173  // Count the number of faces encoded in polygons_;
174  int count = 0;
175  int i = 0;
176  while (i < static_cast<int>(polygons_->size())) {
177  ++count;
178  i += (*polygons_)[i] + 1;
179  }
180  GTEST_ASSERT_EQ(i, static_cast<int>(polygons_->size()))
181  << "Badly defined polygons";
182  GTEST_ASSERT_EQ(face_count(), count);
183  }
184 
185  protected:
186  std::shared_ptr<std::vector<Vector3<S>>> vertices_;
187  std::shared_ptr<std::vector<int>> polygons_;
188  S scale_{};
189 };
190 
191 // A simple regular tetrahedron with edges of length `scale` centered on the
192 // origin.
193 template <typename S>
194 class EquilateralTetrahedron : public Polytope<S> {
195  public:
196  // Constructs the tetrahedron (of edge length `s`).
197  explicit EquilateralTetrahedron(S scale) : Polytope<S>(scale) {
198  // Tetrahedron vertices in the tet's canonical frame T. The tet is
199  // "centered" on the origin so that it's center of mass is simple [0, 0, 0].
200  const S z_base = -1 / S(2 * sqrt(6.));
201  Vector3<S> points_T[] = {{S(0.5), S(-0.5 / sqrt(3.)), z_base},
202  {S(-0.5), S(-0.5 / sqrt(3.)), z_base},
203  {S(0), S(1. / sqrt(3.)), z_base},
204  {S(0), S(0), S(sqrt(3. / 8))}};
205  for (const auto& v : points_T) {
206  this->add_vertex(scale * v);
207  };
208 
209  // Now add the polygons
210  this->add_face({0, 1, 2});
211  this->add_face({1, 0, 3});
212  this->add_face({0, 2, 3});
213  this->add_face({2, 1, 3});
214 
215  this->confirm_data();
216  }
217  // Properties of the polytope.
218  int face_count() const final { return 4; }
219  int vertex_count() const final { return 4; }
220  S volume() const final {
221  // This assumes unit mass.
222  const S s = this->scale_;
223  return S(sqrt(2) / 12) * s * s * s;
224  }
225  Vector3<S> com() const final { return Vector3<S>::Zero(); }
226  std::string description() const final {
227  return "Tetrahedron with scale: " + std::to_string(this->scale());
228  }
229 };
230 
231 // A simple cube with sides of length `scale`.
232 template <typename S>
233 class Cube : public Polytope<S> {
234  public:
235  Cube(S scale) : Polytope<S>(scale) {
236  // Cube vertices in the cube's canonical frame C.
237  Vector3<S> points_C[] = {{S(-0.5), S(-0.5), S(-0.5)}, // v0
238  {S(0.5), S(-0.5), S(-0.5)}, // v1
239  {S(-0.5), S(0.5), S(-0.5)}, // v2
240  {S(0.5), S(0.5), S(-0.5)}, // v3
241  {S(-0.5), S(-0.5), S(0.5)}, // v4
242  {S(0.5), S(-0.5), S(0.5)}, // v5
243  {S(-0.5), S(0.5), S(0.5)}, // v6
244  {S(0.5), S(0.5), S(0.5)}}; // v7
245  for (const auto& v : points_C) {
246  this->add_vertex(scale * v);
247  }
248 
249  // Now add the polygons
250  this->add_face({1, 3, 7, 5}); // +x
251  this->add_face({0, 4, 6, 2}); // -x
252  this->add_face({4, 5, 7, 6}); // +y
253  this->add_face({0, 2, 3, 1}); // -y
254  this->add_face({6, 7, 3, 2}); // +z
255  this->add_face({0, 1, 5, 4}); // -z
256 
257  this->confirm_data();
258  }
259 
260  // Polytope properties
261  int face_count() const final { return 6; }
262  int vertex_count() const final { return 8; }
263  virtual S volume() const final {
264  S s = this->scale();
265  return s * s * s;
266  }
267  virtual Vector3<S> com() const final { return Vector3<S>::Zero(); }
268  virtual Matrix3<S> principal_inertia_tensor() const {
269  S scale_sqd = this->scale() * this->scale();
270  // This assumes unit mass.
271  return Eigen::DiagonalMatrix<S, 3>(1. / 6., 1. / 6., 1. / 6.) * scale_sqd;
272  };
273  std::string description() const final {
274  return "Cube with scale: " + std::to_string(this->scale());
275  }
276 };
277 
278 void testConvexConstruction() {
279  Cube<double> cube{1};
280  // Set the cube at some other location to make sure that the interior point
281  // test/ doesn't pass just because it initialized to zero.
282  Vector3<double> p_WB(1, 2, 3);
283  cube.SetPose(Transform3<double>(Eigen::Translation3d(p_WB)));
284  Convex<double> convex = cube.MakeConvex();
285 
286  // This doesn't depend on the correct logic in the constructor. But this is
287  // as convenient a time as any to test that it reports the right node type.
288  EXPECT_EQ(convex.getNodeType(), GEOM_CONVEX);
289 
290  // The constructor computes the interior point.
291  EXPECT_TRUE(CompareMatrices(convex.getInteriorPoint(), p_WB));
292 }
293 
294 template <template <typename> class Shape, typename S>
295 void testAABBComputation(const Shape<S>& model, const Transform3<S>& X_WS) {
296  Shape<S> shape(model);
297  shape.SetPose(X_WS);
298  Convex<S> convex = shape.MakeConvex();
299  convex.computeLocalAABB();
300 
301  typename constants<S>::Real eps = constants<S>::eps();
302  EXPECT_NEAR(shape.aabb_radius(), convex.aabb_radius, eps);
303  EXPECT_TRUE(CompareMatrices(shape.aabb_center(), convex.aabb_center, eps));
304  EXPECT_TRUE(CompareMatrices(shape.min_point(), convex.aabb_local.min_, eps));
305  EXPECT_TRUE(CompareMatrices(shape.max_point(), convex.aabb_local.max_, eps));
306 }
307 
308 template <template <typename> class Shape, typename S>
309 void testVolume(const Shape<S>& model, const Transform3<S>& X_WS,
310  int bits_lost) {
311  // If we're losing more than 10 bits, then we have a major problem.
312  GTEST_ASSERT_LE(bits_lost, 10);
313 
314  Shape<S> shape(model);
315  shape.SetPose(X_WS);
316  Convex<S> convex = shape.MakeConvex();
317 
318  // We want the basic tolerance to be near machine precision. The invocation
319  // of this function indicates how many bits of precision are expected to be
320  // lost and the machine epsilon is modified to account for this.
321  typename constants<S>::Real eps = (1 << bits_lost) * constants<S>::eps();
322 
323  // We want to do a *relative* comparison. We scale our eps by the volume so
324  // that large volumes have tolerances proportional to the actual true value.
325  S scale = max(shape.volume(), S(1));
326  EXPECT_NEAR(shape.volume(), convex.computeVolume(), eps * scale)
327  << shape.description() << " at\n" << X_WS.matrix()
328  << "\nusing scalar: " << ScalarString<S>::value();
329 }
330 
331 template <template <typename> class Shape, typename S>
332 void testCenterOfMass(const Shape<S>& model, const Transform3<S>& X_WS,
333  int bits_lost) {
334  // If we're losing more than 10 bits, then we have a major problem.
335  GTEST_ASSERT_LE(bits_lost, 10);
336 
337  Shape<S> shape(model);
338  shape.SetPose(X_WS);
339  Convex<S> convex = shape.MakeConvex();
340 
341  // We want the basic tolerance to be near machine precision. The invocation
342  // of this function indicates how many bits of precision are expected to be
343  // lost and the machine epsilon is modified to account for this.
344  typename constants<S>::Real eps = (1 << bits_lost) * constants<S>::eps();
345 
346  // We want to do a *relative* comparison. The center-of-mass calculation is a
347  // volume-weighted calculation. So, the relative tolerance should scale with
348  // volume.
349  S scale = max(shape.volume(), S(1));
350  EXPECT_TRUE(
351  CompareMatrices(X_WS * shape.com(), convex.computeCOM(), eps * scale))
352  << shape.description() << " at\n" << X_WS.matrix()
353  << "\nusing scalar: " << ScalarString<S>::value();
354 }
355 
356 template <template <typename> class Shape, typename S>
357 void testMomentOfInertia(const Shape<S>& model, const Transform3<S>& X_WS,
358  int bits_lost) {
359  // If we're losing more than 10 bits, then we have a major problem.
360  GTEST_ASSERT_LE(bits_lost, 10);
361 
362  Shape<S> shape(model);
363  shape.SetPose(X_WS);
364  Convex<S> convex = shape.MakeConvex();
365 
366  // We want the basic tolerance to be near machine precision. The invocation
367  // of this function indicates how many bits of precision are expected to be
368  // lost and the machine epsilon is modified to account for this.
369  typename constants<S>::Real eps = (1 << bits_lost) * constants<S>::eps();
370 
371  // We want to do a *relative* comparison. The inertia calculation is a
372  // volume-weighted calculation. So, the relative tolerance should scale with
373  // volume.
374  S scale = max(shape.volume(), S(1));
375  EXPECT_TRUE(
376  CompareMatrices(X_WS.linear().transpose() *
377  shape.principal_inertia_tensor() * X_WS.linear(),
378  convex.computeMomentofInertiaRelatedToCOM(), eps * scale))
379  << shape.description() << " at\n" << X_WS.matrix()
380  << "\nusing scalar: " << ScalarString<S>::value();
381 }
382 
383 // The definition of a support vertex test configuration.
384 template <typename S>
385 struct SupportVertexTest {
386  // The direction for which we find the support vertex, expressed in the
387  // shape's frame S.
388  Vector3<S> v_S;
389  // The position of the *expected* support vertex measured and expressed in the
390  // shape's frame S.
391  Vector3<S> p_SE;
392 };
393 
394 template <template <typename> class Shape, typename S>
395 void testSupportVertex(const Shape<S>& model, const Transform3<S>& X_WS,
396  const std::vector<SupportVertexTest<S>>& tests) {
397  Shape<S> shape_W(model);
398  shape_W.SetPose(X_WS);
399  Convex<S> convex_W = shape_W.MakeConvex();
400  for (const auto& test : tests) {
401  const Vector3<S> v_W = X_WS.linear() * test.v_S;
402  const Vector3<S> p_WE = X_WS * test.p_SE;
403  // As long as we don't have directions parallel with face normals, the
404  // answer should be unique and precise down to the last bit.
405  EXPECT_TRUE(CompareMatrices(convex_W.findExtremeVertex(v_W), p_WE))
406  << shape_W.description() << " at\n"
407  << X_WS.matrix() << "\nusing scalar: " << ScalarString<S>::value()
408  << "\n v_W = " << v_W.transpose();
409  }
410 }
411 
412 template <typename S>
413 PoseVector<S> GetPoses() {
414  PoseVector<S> poses;
415  // Identity.
416  poses.push_back(Transform3<S>::Identity());
417 
418  Transform3<S> X_WS;
419  // 90-degree rotation around each axis, in turn.
420  for (int i = 0; i < 3; ++i) {
421  X_WS = Transform3<S>::Identity();
422  X_WS.linear() = AngleAxis<S>(constants<S>::pi() / 2,
423  Vector3<S>::Unit(i)).matrix();
424  poses.push_back(X_WS);
425  }
426 
427  // Small angle away from identity.
428  X_WS.linear() = AngleAxis<S>(S(1e-5), Vector3<S>{1, 2, 3}.normalized())
429  .matrix();
430  poses.push_back(X_WS);
431 
432  // 45-degree angle to move away from axis-aligned as much as possible.
433  X_WS.linear() = AngleAxis<S>(constants<S>::pi() / 4,
434  Vector3<S>{1, 2, 3}.normalized()).matrix();
435  poses.push_back(X_WS);
436 
437  // We don't test translation because that would imply the geometry is
438  // defined away from its own frame's origin. And that's just a recklessly
439  // stupid thing to do. Given the *current* algorithms, this will degrade
440  // the answers based on the *distance* to the origin.
441  // TODO(SeanCurtis-TRI): When the algorithms are no longer sensitive to vertex
442  // position relative to the origin, add tests that show that.
443 
444  return poses;
445 }
446 
447 std::vector<double> get_test_scales() {
448  return std::vector<double>{0.001, 1, 1000.};
449 }
450 
451 template <template <typename> class Shape, typename S>
452 void testLocalAABBComputation(const Shape<S>& shape) {
453  for (const auto& X_WP : GetPoses<S>()) {
454  testAABBComputation<Shape>(shape, X_WP);
455  }
456 }
457 
458 template <template <typename> class Shape, typename S>
459 void testVolumeComputation(const Shape<S>& shape, int bits_lost) {
460  for (const auto& X_WP : GetPoses<S>()) {
461  testVolume<Shape>(shape, X_WP, bits_lost);
462  }
463 }
464 
465 template <template <typename> class Shape, typename S>
466 void testCenterOfMassComputation(const Shape<S>& shape, int bits_lost) {
467  for (const auto& X_WP : GetPoses<S>()) {
468  testCenterOfMass<Shape>(shape, X_WP, bits_lost);
469  }
470 }
471 
472 template <template <typename> class Shape, typename S>
473 void testMomentOfInertiaComputation(const Shape<S>& shape, int bits_lost) {
474  for (const auto& X_WP : GetPoses<S>()) {
475  testMomentOfInertia<Shape>(shape, X_WP, bits_lost);
476  }
477 }
478 
479 template <template <typename> class Shape, typename S>
480 void testSupportVertexComputation(
481  const Shape<S>& shape, const std::vector<SupportVertexTest<S>>& tests) {
482  for (const auto& X_WP : GetPoses<S>()) {
483  testSupportVertex<Shape>(shape, X_WP, tests);
484  }
485 }
486 
487 GTEST_TEST(ConvexGeometry, Constructor) {
488  testConvexConstruction();
489 }
490 
491 GTEST_TEST(ConvexGeometry, LocalAABBComputation_Cube) {
492  for (double scale : get_test_scales()) {
493  Cube<double> cube_d(scale);
494  testLocalAABBComputation(cube_d);
495  Cube<float> cube_f(static_cast<float>(scale));
496  testLocalAABBComputation(cube_f);
497  }
498 }
499 
500 GTEST_TEST(ConvexGeometry, Volume_Cube) {
501  for (double scale : get_test_scales()) {
502  Cube<double> cube_d(scale);
503  testVolumeComputation(cube_d, 0);
504  Cube<float> cube_f(static_cast<float>(scale));
505  // Apparently, no bits of precision are lost (relative to machine precision)
506  // on the cube volume *except* for the *large* cube in single precision.
507  // The reason for this isn't obvious, but probably a coincidental artifact
508  // of the particular configuration.
509  const int bits_lost = scale > 1 ? 2 : 0;
510  testVolumeComputation(cube_f, bits_lost);
511  }
512 }
513 
514 GTEST_TEST(ConvexGeometry, CenterOfMass_Cube) {
515  for (double scale : get_test_scales()) {
516  Cube<double> cube_d(scale);
517  testCenterOfMassComputation(cube_d, 0);
518  Cube<float> cube_f(static_cast<float>(scale));
519  testCenterOfMassComputation(cube_f, 0);
520  }
521 }
522 
523 GTEST_TEST(ConvexGeometry, MomentOfInertia_Cube) {
524  for (double scale : get_test_scales()) {
525  Cube<double> cube_d(scale);
526  testMomentOfInertiaComputation(cube_d, 0);
527  Cube<float> cube_f(static_cast<float>(scale));
528  testMomentOfInertiaComputation(cube_f, 0);
529  }
530 }
531 
532 GTEST_TEST(ConvexGeometry, LocalAABBComputation_Tetrahedron) {
533  for (double scale : get_test_scales()) {
534  EquilateralTetrahedron<double> tet_d(scale);
535  testLocalAABBComputation(tet_d);
536  EquilateralTetrahedron<float> tet_f(static_cast<float>(scale));
537  testLocalAABBComputation(tet_f);
538  }
539 }
540 
541 GTEST_TEST(ConvexGeometry, Volume_Tetrahedron) {
542  for (double scale : get_test_scales()) {
543  EquilateralTetrahedron<double> tet_d(scale);
544  // Apparently, no bits of precision are lost (relative to machine precision)
545  // on the tet volume *except* for the *large* test in double precision.
546  // The reason for this isn't obvious, but probably a coincidental artifact
547  // of the particular configuration.
548  const int bits_lost = scale > 1 ? 1 : 0;
549  testVolumeComputation(tet_d, bits_lost);
550  EquilateralTetrahedron<float> tet_f(static_cast<float>(scale));
551  testVolumeComputation(tet_f, 0);
552  }
553 }
554 
555 GTEST_TEST(ConvexGeometry, CenterOfMass_Tetrahedron) {
556  for (double scale : get_test_scales()) {
557  EquilateralTetrahedron<double> tet_d(scale);
558  testCenterOfMassComputation(tet_d, 0);
559  EquilateralTetrahedron<float> tet_f(static_cast<float>(scale));
560  testCenterOfMassComputation(tet_f, 0);
561  }
562 }
563 
564 // Defines a collection of support vertex directions and expected vertex
565 // results. We assume that the given polytope contains the origin so that the
566 // direction to every polytope vertex is unique. So, the direction to the vertex
567 // should uniquely produce that vertex as the support vertex.
568 template <typename S>
569 std::vector<SupportVertexTest<S>> BuildSupportVertexTests(
570  const Polytope<S>& polytope) {
571  std::vector<SupportVertexTest<S>> tests;
572  for (const auto& p_SV : *polytope.points()) {
573  tests.push_back({p_SV, p_SV});
574  }
575  return tests;
576 }
577 
578 GTEST_TEST(ConvexGeometry, SupportVertex_Tetrahedron) {
579  for (double scale : get_test_scales()) {
580  EquilateralTetrahedron<double> tet_d(scale);
581  std::vector<SupportVertexTest<double>> tests_d =
582  BuildSupportVertexTests(tet_d);
583  testSupportVertexComputation(tet_d, tests_d);
584 
585  EquilateralTetrahedron<float> tet_f(scale);
586  std::vector<SupportVertexTest<float>> tests_f =
587  BuildSupportVertexTests(tet_f);
588  testSupportVertexComputation(tet_f, tests_f);
589  }
590 }
591 
592 // A tetrahedron whose bottom triangle consists of three co-planar faces. (In
593 // other words, we've injected a new vertex into the center of the bottom face.)
594 // That new vertex is vertex 0. Used for the SupportVertexCoPlanarFaces test.
595 class CoPlanarTetrahedron final : public Polytope<double> {
596  public:
597  CoPlanarTetrahedron() : Polytope<double>(1.0) {
598  // Tetrahedron vertices in the tet's canonical frame T. The tet is placed
599  // so that it's bottom face lies on the z = 0 plane.
600  Vector3d points_T[] = {{0.5, -0.5 / sqrt(3.), 0},
601  {-0.5, -0.5 / sqrt(3.), 0},
602  {0, 1. / sqrt(3.), 0},
603  {0, 0, sqrt(3. / 8)}};
604  const Vector3d center = (points_T[0] + points_T[1] + points_T[2]) / 3.0;
605  this->add_vertex(center);
606  for (const auto& v : points_T) {
607  this->add_vertex(v);
608  };
609 
610  // Now add the polygons
611  this->add_face({0, 1, 2});
612  this->add_face({0, 2, 3});
613  this->add_face({0, 3, 1});
614  this->add_face({1, 3, 4});
615  this->add_face({3, 2, 4});
616  this->add_face({1, 4, 2});
617 
618  this->confirm_data();
619  }
620  // Properties of the polytope.
621  int face_count() const final { return 6; }
622  int vertex_count() const final { return 5; }
623 };
624 
625 // Test for special condition in findExtremeVertex which can arise iff the
626 // Convex shape has a vertex whose adjacent vertices are all co-planar with it,
627 // that vertex is the *starting* vertex of the search, *and* the query direction
628 // is perpendicular to the plane that those vertices all lie on.
629 GTEST_TEST(ConvexGeometry, SupportVertexCoPlanarFaces) {
630  CoPlanarTetrahedron tet;
631  Convex<double> convex_W = tet.MakeConvex();
632  // Query direction is perpendicular to the bottom face.
633  const Vector3d v_W{0, 0, 1};
634  const Vector3d p_WE_expected = tet.points()->at(4);
635  // With only five vertices, findExtremeVertex would default to a linear
636  // search. For this test, we want to use the edge graph. So, we force it to
637  // be enabled.
639 
640  // We can expect an exact answer down to the last bit.
641  EXPECT_TRUE(CompareMatrices(convex_W.findExtremeVertex(v_W), p_WE_expected));
642 }
643 
644 // A tetrahedron with a missing face.
645 class HoleTetrahedron final : public Polytope<double> {
646  public:
647  HoleTetrahedron() : Polytope<double>(1.0) {
648  // We'll start with a good tetrahedron, copy all vertices and simply omit
649  // its first face.
650  EquilateralTetrahedron<double> tet(1.0);
651  vertices_->insert(vertices_->begin(), tet.points()->begin(),
652  tet.points()->end());
653 
654  // Add the faces of the tet (skipping the first).
655  const std::vector<int>& polys = *tet.polygons();
656  // polys[0] is the number of vertices in face 0. So, face 1 starts at
657  // that number plus one.
658  const int face1 = polys[0] + 1;
659  polygons_->insert(polygons_->end(), polys.begin() + face1, polys.end());
660 
661  this->confirm_data();
662  }
663  // Properties of the polytope.
664  int face_count() const final { return 3; }
665  int vertex_count() const final { return 4; }
666 };
667 
668 // A tetrahedron with a structural crack; a vertex is duplicated (but left in
669 // the same location). It forms a break in the topology over its adjacent edges.
670 class CrackTetrahedron final : public Polytope<double> {
671  public:
672  CrackTetrahedron() : Polytope<double>(1.0) {
673  EquilateralTetrahedron<double> tet(1.0);
674  vertices_->insert(vertices_->begin(), tet.points()->begin(),
675  tet.points()->end());
676  vertices_->push_back((*vertices_)[0]);
677 
678  // Now add the polygons by hand (copied and modified from
679  // EquilateralTetrahedron).
680  this->add_face({4, 1, 2}); // Vertex 4 swapped for vertex 0.
681  this->add_face({1, 0, 3});
682  this->add_face({0, 2, 3});
683  this->add_face({2, 1, 3});
684 
685  this->confirm_data();
686  }
687  // Properties of the polytope.
688  int face_count() const final { return 4; }
689  int vertex_count() const final { return 5; }
690 };
691 
692 // A tetrahedron with a "stray" vertex; a vertex not connected to any face.
693 class StrayVertexTetrahedron final : public Polytope<double> {
694  public:
695  StrayVertexTetrahedron() : Polytope<double>(1.0) {
696  EquilateralTetrahedron<double> tet(1.0);
697  vertices_->insert(vertices_->begin(), tet.points()->begin(),
698  tet.points()->end());
699  // Add the stray.
700  vertices_->push_back({-1, -1, -1});
701 
702  // Add the faces of the tet.
703  const std::vector<int>& polys = *tet.polygons();
704  polygons_->insert(polygons_->end(), polys.begin(), polys.end());
705 
706  this->confirm_data();
707  }
708  // Properties of the polytope.
709  int face_count() const final { return 4; }
710  int vertex_count() const final { return 5; }
711 };
712 
713 // A tetrahedron with an extra face built off of one of the edges.
714 class NonManifoldTetrahedron final : public Polytope<double> {
715  public:
716  NonManifoldTetrahedron() : Polytope<double>(1.0) {
717  EquilateralTetrahedron<double> tet(1.0);
718  vertices_->insert(vertices_->begin(), tet.points()->begin(),
719  tet.points()->end());
720  vertices_->push_back({0, 0, -5});
721 
722  polygons_->insert(polygons_->end(), tet.polygons()->begin(),
723  tet.polygons()->end());
724  // The (0, 1) edge is now shared by 3 faces.
725  this->add_face({0, 1, 4});
726 
727  this->confirm_data();
728  }
729  // Properties of the polytope.
730  int face_count() const final { return 5; }
731  int vertex_count() const final { return 5; }
732 };
733 
734 // The test for the "watertight" validation conditions with several variations
735 // of invalid topologies. We don't *explicitly* test the *valid* case because it
736 // is implicitly tested every time a Convex is created in these tests. We also
737 // don't care about the scalar types because this test is purely about topology.
738 GTEST_TEST(ConvexGeometry, WaterTightValidation) {
739  {
740  // Hole in the convex mesh.
741  HoleTetrahedron bad_tet;
743  bad_tet.MakeConvex(), std::runtime_error,
744  "Found errors in the Convex mesh[^]+ Edge between vertices \\d+ and "
745  "\\d+ is shared by 1 faces .+");
746  }
747 
748  {
749  // Crack in an otherwise closed convex mesh due to duplicate vertices.
750  StrayVertexTetrahedron bad_tet;
752  bad_tet.MakeConvex(), std::runtime_error,
753  "Found errors in the Convex mesh[^]+ Not all vertices are connected[^]+"
754  " Vertex \\d+ is not included in any faces[^]*");
755  }
756 
757  {
758  // Crack in an otherwise closed convex mesh due to duplicate vertices.
759  CrackTetrahedron bad_tet;
761  bad_tet.MakeConvex(), std::runtime_error,
762  "Found errors in the Convex mesh[^]+ Edge between vertices \\d+ and "
763  "\\d+ is shared by 1 faces .+");
764  }
765 
766  {
767  // Non-manifold mesh (an edge is shared by three faces).
768  NonManifoldTetrahedron bad_tet;
770  bad_tet.MakeConvex(), std::runtime_error,
771  "Found errors in the Convex mesh[^]+ Edge between vertices 0 and 1 is "
772  "shared by 3 faces [^]+");
773  }
774 }
775 
776 // A tessellated unit sphere; 8 longitudinal wedges and 8 latitudinal bands.
777 class TessellatedSphere final : public Polytope<double> {
778  public:
779  TessellatedSphere() : Polytope<double>(1.0) {
780  // The angle between the latitude lines measured along the prime meridian.
781  const double dphi = M_PI / 8;
782  auto slice_height = [dphi](int slice_index) {
783  // Assumes 1 <= slice_index < 8.
784  return std::cos(slice_index * dphi);
785  };
786  auto slice_radius = [dphi](int slice_index) {
787  // Assumes 1 <= slice_index < 8.
788  return std::sin(slice_index * dphi);
789  };
790  // North pole is top of slice 1.
791  vertices_->push_back({0, 0, 1});
792  // Now create the bands of vertices between slices 1 & 2, 2 & 3, etc.
793  // The angle between the longitude lines measured along the equator.
794  const double dtheta = 2 * M_PI / 8;
795  for (int slice = 1; slice < 8; ++slice) {
796  double z = slice_height(slice);
797  double r = slice_radius(slice);
798  for (int i = 0; i < 8; ++i) {
799  const double theta = dtheta * i;
800  vertices_->emplace_back(std::cos(theta) * r,
801  std::sin(theta) * r,
802  z);
803  }
804  }
805  // South pole is slice bottom of slice 8.
806  vertices_->push_back({0, 0, -1});
807 
808  // North pole triangle fan: slice 1.
809  // [0, 8, 1], [0, 1, 2], [0, 2, 3], ..., [0, 7, 8].
810  // The "previous" index is: 8, 1, 2, 3, ... 7 and i is 1, 2, 3, ..., 8.
811  int prev = 8;
812  int next = 1;
813  for (; next <= 8; prev = next, ++next) {
814  this->add_face({0, prev, next});
815  prev = next;
816  }
817  // The rectangular facets for each latitude band. For slice 2, the quads
818  // would be: [upper prev, prev, next, upper next]. I.e., [8, 16, 9, 1],
819  // [1, 9, 10, 2], [2, 10, 11, 3], ..., [7, 15, 16, 8]. Such that
820  // upper_prev = prev - 8, and upper_next = next - 8. So, we track
821  // prev and next and compute the upper versions.
822  for (int slice = 2; slice < 8; ++slice) {
823  prev = slice * 8;
824  next = prev - 7;
825  for (int i = 0; i < 8; ++i, prev = next, ++next) {
826  this->add_face({prev - 8, prev, next, next - 8});
827  }
828  }
829  // South pole triangle fan: slice 8.
830  prev = 56; // slice 7 * 8.
831  next = 49; // The index of the first vertex on slice 8.
832  for (int i = 0; i < 8; ++i, prev = next, ++next) {
833  this->add_face({57, next, prev});
834  }
835 
836  this->confirm_data();
837  }
838  // Properties of the polytope.
839  int face_count() const final { return 64; }
840  int vertex_count() const final { return 58; }
841 };
842 
843 // Confirm that edge walking gets disabled in expected cases.
844 GTEST_TEST(ConvexGeometry, UseEdgeWalkingConditions) {
845  const bool throw_if_invalid{true};
846  {
847  // Too few triangles.
848  EquilateralTetrahedron<double> poly(1.0);
849  Convex<double> convex = poly.MakeConvex(throw_if_invalid);
851  }
852  {
853  // Hole in an unvalidated convex mesh.
854  HoleTetrahedron poly;
855  Convex<double> convex = poly.MakeConvex(!throw_if_invalid);
857  }
858  {
859  // A *valid* mesh with sufficient number of vertices will enable edge
860  // walking. Simply create a tessellated sphere.
861  TessellatedSphere poly;
862  Convex<double> convex = poly.MakeConvex(throw_if_invalid);
864  }
865 }
866 
867 // TODO(SeanCurtis-TRI): Add Tetrahedron inertia unit test.
868 
869 // TODO(SeanCurtis-TRI): Extend the moment of inertia test.
870 // Tesselate smooth geometries (sphere, ellipsoid, cone, etc) which have
871 // well-known closed-form values for the tensor product. Confirm that as
872 // the tesselation gets finer, that the answer converges to the reference
873 // solution.
874 
875 GTEST_TEST(ConvexGeometry, Representation) {
876  // This defines the `shape` and `code_string` variables used in the test.
879  std::make_shared<std::vector<Vector3<double>>>(
880  std::initializer_list<Vector3<double>>{
881  Vector3<double>(0, 0, 0), Vector3<double>(1.5, 0, 0),
882  Vector3<double>(0, 1.5, 0), Vector3<double>(1, 1.5, 0),}),
883  2,
884  std::make_shared<std::vector<int>>(
885  std::initializer_list<int>{ 3, 0, 1, 2, 3, 1, 3, 2,}),
886  false);)
887 
888  EXPECT_TRUE(detail::ValidateRepresentation(shape, code_string));
889 }
890 
891 } // namespace
892 } // namespace fcl
893 
894 //==============================================================================
895 int main(int argc, char *argv[]) {
896  ::testing::InitGoogleTest(&argc, argv);
897  return RUN_ALL_TESTS();
898 }
899 
fcl::ConvexTester::find_extreme_via_neighbors
static bool find_extreme_via_neighbors(const Convex< S > &convex)
Definition: test_convex.cpp:55
FCL_EXPECT_THROWS_MESSAGE
#define FCL_EXPECT_THROWS_MESSAGE(expression, exception, regexp)
Definition: expect_throws_message.h:111
types.h
eigen_matrix_compare.h
fcl::Convex< double >
template class FCL_EXPORT Convex< double >
fcl::CompareMatrices
::testing::AssertionResult CompareMatrices(const Eigen::MatrixBase< DerivedA > &m1, const Eigen::MatrixBase< DerivedB > &m2, double tolerance=0.0, MatrixCompareType compare_type=MatrixCompareType::absolute)
Definition: eigen_matrix_compare.h:63
fcl::Convex< S >
fcl::constants::eps
static constexpr Real eps()
Returns ε for the precision of the underlying scalar type.
Definition: constants.h:151
max
static T max(T x, T y)
Definition: svm.cpp:52
fcl::detail::ValidateRepresentation
::testing::AssertionResult ValidateRepresentation(const Shape &shape, const std::string &code_string)
Definition: representation_util.h:74
EXPECT_TRUE
#define EXPECT_TRUE(args)
expect_throws_message.h
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
INSTANTIATE_AND_SAVE_STRING
#define INSTANTIATE_AND_SAVE_STRING(code)
Definition: representation_util.h:58
fcl::ConvexTester
Definition: test_convex.cpp:52
representation_util.h
fcl::Translation3d
Translation3< double > Translation3d
Definition: types.h:118
fcl::constants::Real
detail::ScalarTrait< S >::type Real
Definition: constants.h:131
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
convex.h
fcl::ConvexTester::force_find_extreme_via_neighbors
static void force_find_extreme_via_neighbors(Convex< S > *convex)
Definition: test_convex.cpp:61
prev
EndPoint * prev[3]
the previous end point in the end point list
Definition: broadphase_SaP.h:187
next
EndPoint * next[3]
the next end point in the end point list
Definition: broadphase_SaP.h:190
fcl::Convex::find_extreme_via_neighbors_
bool find_extreme_via_neighbors_
Definition: convex.h:267
r
S r
Definition: test_sphere_box.cpp:171
fcl::GEOM_CONVEX
@ GEOM_CONVEX
Definition: collision_geometry.h:54
GTEST_TEST
GTEST_TEST(DynamicAABBTreeCollisionManager, update)
Definition: test_broadphase_dynamic_AABB_tree.cpp:61
Vector3d
fcl::Vector3d Vector3d
Definition: test_broadphase_dynamic_AABB_tree.cpp:48
main
int main(int argc, char *argv[])
Definition: test_convex.cpp:895
M_PI
#define M_PI
fcl::constants::pi
static constexpr S pi()
The mathematical constant pi.
Definition: constants.h:134
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
EXPECT_EQ
#define EXPECT_EQ(a, b)
EXPECT_FALSE
#define EXPECT_FALSE(args)


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